697 files changed, 233796 insertions, 1372 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index d236aef7e59..82dc759a772 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -126,6 +126,8 @@ source "drivers/platform/Kconfig"
 
 source "drivers/clk/Kconfig"
 
+source "drivers/tee/Kconfig"
+
 source "drivers/hwspinlock/Kconfig"
 
 source "drivers/clocksource/Kconfig"
@@ -140,4 +142,9 @@ source "drivers/virt/Kconfig"
 
 source "drivers/devfreq/Kconfig"
 
+source "drivers/modem/Kconfig"
+
+source "drivers/gator/Kconfig"
+
 endmenu
+
diff --git a/drivers/Makefile b/drivers/Makefile
index 95952c82bf1..d638d85a80e 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -99,6 +99,7 @@ obj-$(CONFIG_CPU_FREQ)		+= cpufreq/
 obj-$(CONFIG_CPU_IDLE)		+= cpuidle/
 obj-y				+= mmc/
 obj-$(CONFIG_MEMSTICK)		+= memstick/
+obj-$(CONFIG_MODEM)		+= modem/
 obj-y				+= leds/
 obj-$(CONFIG_INFINIBAND)	+= infiniband/
 obj-$(CONFIG_SGI_SN)		+= sn/
@@ -122,6 +123,7 @@ obj-y				+= platform/
 obj-y				+= ieee802154/
 #common clk code
 obj-y				+= clk/
+obj-$(CONFIG_TEE_SUPPORT)	+= tee/
 
 obj-$(CONFIG_HWSPINLOCK)	+= hwspinlock/
 obj-$(CONFIG_NFC)		+= nfc/
@@ -134,3 +136,5 @@ obj-$(CONFIG_VIRT_DRIVERS)	+= virt/
 obj-$(CONFIG_HYPERV)		+= hv/
 
 obj-$(CONFIG_PM_DEVFREQ)	+= devfreq/
+
+obj-$(CONFIG_GATOR)		+= gator/
diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index cc273226dbd..e98838a060e 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -117,7 +117,7 @@ static int amba_legacy_resume(struct device *dev)
 
 #ifdef CONFIG_SUSPEND
 
-static int amba_pm_suspend(struct device *dev)
+int amba_pm_suspend(struct device *dev)
 {
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
@@ -135,7 +135,7 @@ static int amba_pm_suspend(struct device *dev)
 	return ret;
 }
 
-static int amba_pm_resume(struct device *dev)
+int amba_pm_resume(struct device *dev)
 {
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
@@ -162,7 +162,7 @@ static int amba_pm_resume(struct device *dev)
 
 #ifdef CONFIG_HIBERNATE_CALLBACKS
 
-static int amba_pm_freeze(struct device *dev)
+int amba_pm_freeze(struct device *dev)
 {
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
@@ -180,7 +180,7 @@ static int amba_pm_freeze(struct device *dev)
 	return ret;
 }
 
-static int amba_pm_thaw(struct device *dev)
+int amba_pm_thaw(struct device *dev)
 {
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
@@ -198,7 +198,7 @@ static int amba_pm_thaw(struct device *dev)
 	return ret;
 }
 
-static int amba_pm_poweroff(struct device *dev)
+int amba_pm_poweroff(struct device *dev)
 {
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
@@ -216,7 +216,7 @@ static int amba_pm_poweroff(struct device *dev)
 	return ret;
 }
 
-static int amba_pm_restore(struct device *dev)
+int amba_pm_restore(struct device *dev)
 {
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index 0dc5d7ce486..a73e2c197cd 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -49,6 +49,11 @@ obj-$(CONFIG_NSC_GPIO)		+= nsc_gpio.o
 obj-$(CONFIG_GPIO_TB0219)	+= tb0219.o
 obj-$(CONFIG_TELCLOCK)		+= tlclk.o
 
+ifdef CONFIG_PHONET
+obj-$(CONFIG_U8500_SHRM)	+= shrm_char.o
+obj-$(CONFIG_MODEM_M6718_SPI)	+= m6718_modem_char.o
+endif
+
 obj-$(CONFIG_MWAVE)		+= mwave/
 obj-$(CONFIG_AGP)		+= agp/
 obj-$(CONFIG_PCMCIA)		+= pcmcia/
diff --git a/drivers/char/m6718_modem_char.c b/drivers/char/m6718_modem_char.c
new file mode 100644
index 00000000000..34bfe98aa57
--- /dev/null
+++ b/drivers/char/m6718_modem_char.c
@@ -0,0 +1,722 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *   based on shrm_char.c
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * M6718 modem char device interface.
+ */
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/sched.h>
+#include <linux/atomic.h>
+#include <linux/modem/m6718_spi/modem_char.h>
+#include <linux/modem/m6718_spi/modem_driver.h>
+
+#define NAME "IPC_ISA"
+
+#define MAX_PDU_SIZE           (2000) /* largest frame we need to send */
+#define MAX_RX_FIFO_ENTRIES    (10)
+#define SIZE_OF_RX_FIFO        (MAX_PDU_SIZE * MAX_RX_FIFO_ENTRIES)
+#define SIZE_OF_TX_COPY_BUFFER (MAX_PDU_SIZE) /* only need 1 at a time */
+
+static u8 message_fifo[MODEM_M6718_SPI_MAX_CHANNELS][SIZE_OF_RX_FIFO];
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_LOOPBACK
+static u8 wr_mlb_msg[SIZE_OF_TX_COPY_BUFFER];
+#endif
+static u8 wr_audio_msg[SIZE_OF_TX_COPY_BUFFER];
+
+struct map_device {
+	u8 l2_header;
+	u8 idx;
+	char *name;
+};
+
+static struct map_device map_dev[] = {
+	{MODEM_M6718_SPI_CHN_ISI, 0, "isi"},
+	{MODEM_M6718_SPI_CHN_AUDIO, 1, "modemaudio"},
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_LOOPBACK
+	{MODEM_M6718_SPI_CHN_MASTER_LOOPBACK0, 2, "master_loopback0"},
+	{MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK0, 3, "slave_loopback0"},
+	{MODEM_M6718_SPI_CHN_MASTER_LOOPBACK1, 4, "master_loopback1"},
+	{MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK1, 5, "slave_loopback1"},
+#endif
+};
+
+/*
+ * major - variable exported as module_param to specify major node number
+ */
+static int major;
+module_param(major, int, 0);
+MODULE_PARM_DESC(major, "Major device number");
+
+/* global fops mutex */
+static DEFINE_MUTEX(isa_lock);
+
+/**
+ * modem_get_cdev_index() - return the index mapped to l2 header
+ * @l2_header:	L2 header
+ *
+ * struct map_device maps the index(count) with the device L2 header.
+ * This function returns the index for the provided L2 header in case
+ * of success else -ve value.
+ */
+int modem_get_cdev_index(u8 l2_header)
+{
+	u8 cnt;
+	for (cnt = 0; cnt < ARRAY_SIZE(map_dev); cnt++) {
+		if (map_dev[cnt].l2_header == l2_header)
+			return map_dev[cnt].idx;
+	}
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(modem_get_cdev_index);
+
+/**
+ * modem_get_cdev_l2header() - return l2_header mapped to the index
+ * @idx:	index
+ *
+ * struct map_device maps the index(count) with the device L2 header.
+ * This function returns the L2 header for the given index in case
+ * of success else -ve value.
+ */
+int modem_get_cdev_l2header(u8 idx)
+{
+	u8 cnt;
+	for (cnt = 0; cnt < ARRAY_SIZE(map_dev); cnt++) {
+		if (map_dev[cnt].idx == idx)
+			return map_dev[cnt].l2_header;
+	}
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(modem_get_cdev_l2header);
+
+/**
+ * modem_isa_reset() - reset device interfaces
+ * @modem_spi_dev: pointer to modem driver information structure
+ *
+ * Emptys the queue for each L2 mux channel.
+ */
+void modem_isa_reset(struct modem_spi_dev *modem_spi_dev)
+{
+	struct isa_device_context *isadev;
+	struct isa_driver_context *isa_context;
+	struct queue_element *cur_msg = NULL;
+	struct list_head *cur_msg_ptr = NULL;
+	struct list_head *msg_ptr;
+	struct message_queue *q;
+	int devidx;
+
+	dev_info(modem_spi_dev->dev, "resetting char device queues\n");
+
+	isa_context = modem_spi_dev->isa_context;
+	for (devidx = 0; devidx < ARRAY_SIZE(map_dev); devidx++) {
+		isadev = &isa_context->isadev[devidx];
+		q = &isadev->dl_queue;
+
+		spin_lock_bh(&q->update_lock);
+		/* empty out the msg queue */
+		list_for_each_safe(cur_msg_ptr, msg_ptr, &q->msg_list) {
+			cur_msg = list_entry(cur_msg_ptr,
+					struct queue_element, entry);
+			list_del(cur_msg_ptr);
+			kfree(cur_msg);
+		}
+
+		/* reset the msg queue pointers */
+		q->size = SIZE_OF_RX_FIFO;
+		q->readptr = 0;
+		q->writeptr = 0;
+		q->no = 0;
+
+		/* wake up the blocking read/select */
+		atomic_set(&q->q_rp, 1);
+		wake_up_interruptible(&q->wq_readable);
+		spin_unlock_bh(&q->update_lock);
+	}
+}
+EXPORT_SYMBOL_GPL(modem_isa_reset);
+
+static void create_queue(struct message_queue *q, u8 channel,
+	struct modem_spi_dev *modem_spi_dev)
+{
+	q->channel = channel;
+	q->fifo_base = (u8 *)&message_fifo[channel];
+	q->size = SIZE_OF_RX_FIFO;
+	q->free = q->size;
+	q->readptr = 0;
+	q->writeptr = 0;
+	q->no = 0;
+	spin_lock_init(&q->update_lock);
+	atomic_set(&q->q_rp, 0);
+	init_waitqueue_head(&q->wq_readable);
+	INIT_LIST_HEAD(&q->msg_list);
+	q->modem_spi_dev = modem_spi_dev;
+}
+
+static void delete_queue(struct message_queue *q)
+{
+	q->size = 0;
+	q->readptr = 0;
+	q->writeptr = 0;
+}
+
+/**
+ * modem_isa_queue_msg() - Add a message to a queue queue
+ * @q:		message queue
+ * @size:	size in bytes
+ *
+ * This function tries to allocate size bytes in FIFO q.
+ * It returns negative number when no memory can be allocated
+ * currently.
+ */
+int modem_isa_queue_msg(struct message_queue *q, u32 size)
+{
+	struct queue_element *new_msg = NULL;
+	struct modem_spi_dev *modem_spi_dev = q->modem_spi_dev;
+
+	new_msg = kmalloc(sizeof(struct queue_element), GFP_ATOMIC);
+	if (new_msg == NULL) {
+		dev_err(modem_spi_dev->dev,
+			"failed to allocate memory for queue item\n");
+		return -ENOMEM;
+	}
+	new_msg->offset = q->writeptr;
+	new_msg->size = size;
+	new_msg->no = q->no++;
+
+	/* check for overflow condition */
+	if (q->readptr <= q->writeptr) {
+		if (((q->writeptr - q->readptr) + size) >= q->size) {
+			dev_err(modem_spi_dev->dev, "rx q++ ch %d %d (%d)\n",
+				q->channel, size, q->free);
+			dev_err(modem_spi_dev->dev,
+				"ch%d buffer overflow, frame discarded\n",
+				q->channel);
+			return -ENOMEM;
+		}
+	} else {
+		if ((q->writeptr + size) >= q->readptr) {
+			dev_err(modem_spi_dev->dev, "rx q++ ch %d %d (%d)\n",
+				q->channel, size, q->free);
+			dev_err(modem_spi_dev->dev,
+				"ch%d buffer overflow, frame discarded\n",
+				q->channel);
+			return -ENOMEM;
+		}
+	}
+	q->free -= size;
+	q->writeptr = (q->writeptr + size) % q->size;
+	if (list_empty(&q->msg_list)) {
+		list_add_tail(&new_msg->entry, &q->msg_list);
+		/* There can be 2 blocking calls: read and another select */
+		atomic_set(&q->q_rp, 1);
+		wake_up_interruptible(&q->wq_readable);
+	} else {
+		list_add_tail(&new_msg->entry, &q->msg_list);
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(modem_isa_queue_msg);
+
+/**
+ * modem_isa_unqueue_msg() - remove a message from the msg queue
+ * @q:	message queue
+ *
+ * Deletes a message from the message list associated with message
+ * queue q and also updates read ptr. If the message list is empty
+ * then a flag is set to block the select and read calls of the paricular queue.
+ *
+ * The message list is FIFO style and message is always added to tail and
+ * removed from head.
+ */
+int modem_isa_unqueue_msg(struct message_queue *q)
+{
+	struct queue_element *old_msg = NULL;
+	struct list_head *msg_ptr = NULL;
+	struct list_head *old_msg_ptr = NULL;
+
+	list_for_each_safe(old_msg_ptr, msg_ptr, &q->msg_list) {
+		old_msg = list_entry(old_msg_ptr, struct queue_element, entry);
+		if (old_msg == NULL)
+			return -EFAULT;
+		list_del(old_msg_ptr);
+		q->readptr = (q->readptr + old_msg->size) % q->size;
+		q->free += old_msg->size;
+		kfree(old_msg);
+		break;
+	}
+	if (list_empty(&q->msg_list))
+		atomic_set(&q->q_rp, 0);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(modem_isa_unqueue_msg);
+
+/**
+ * modem_isa_msg_size() - retrieve the size of the most recent message
+ * @q:	message queue
+ */
+int modem_isa_msg_size(struct message_queue *q)
+{
+	struct queue_element *new_msg = NULL;
+	struct list_head *msg_list;
+	unsigned long flags;
+	int size = 0;
+
+	spin_lock_irqsave(&q->update_lock, flags);
+	list_for_each(msg_list, &q->msg_list) {
+		new_msg = list_entry(msg_list, struct queue_element, entry);
+		if (new_msg == NULL) {
+			spin_unlock_irqrestore(&q->update_lock, flags);
+			return -EFAULT;
+		}
+		size = new_msg->size;
+		break;
+	}
+	spin_unlock_irqrestore(&q->update_lock, flags);
+	return size;
+}
+EXPORT_SYMBOL_GPL(modem_isa_msg_size);
+
+static u32 isa_select(struct file *filp, struct poll_table_struct *wait)
+{
+	struct isa_device_context *isadev = filp->private_data;
+	struct modem_spi_dev *modem_spi_dev = isadev->dl_queue.modem_spi_dev;
+	struct message_queue *q;
+	u32 m = iminor(filp->f_path.dentry->d_inode);
+	u8 idx = modem_get_cdev_index(m);
+
+	if (modem_spi_dev->msr_flag)
+		return -ENODEV;
+	if (isadev->device_id != idx)
+		return -1;
+
+	q = &isadev->dl_queue;
+	poll_wait(filp, &q->wq_readable, wait);
+	if (atomic_read(&q->q_rp) == 1)
+		return POLLIN | POLLRDNORM;
+	return 0;
+}
+
+static ssize_t isa_read(struct file *filp, char __user *buf,
+	size_t len, loff_t *ppos)
+{
+	u32 size = 0;
+	int ret;
+	char *psrc;
+	struct isa_device_context *isadev =
+		(struct isa_device_context *)filp->private_data;
+	struct message_queue *q = &isadev->dl_queue;
+	struct modem_spi_dev *modem_spi_dev = q->modem_spi_dev;
+	u32 msgsize;
+	unsigned long flags;
+
+	if (len <= 0)
+		return -EFAULT;
+
+	if (modem_spi_dev->msr_flag) {
+		atomic_set(&q->q_rp, 0);
+		return -ENODEV;
+	}
+
+	spin_lock_irqsave(&q->update_lock, flags);
+	if (list_empty(&q->msg_list)) {
+		spin_unlock_irqrestore(&q->update_lock, flags);
+		dev_dbg(modem_spi_dev->dev, "waiting for data on device %d\n",
+			isadev->device_id);
+		if (wait_event_interruptible(q->wq_readable,
+				atomic_read(&q->q_rp) == 1))
+			return -ERESTARTSYS;
+	} else {
+		spin_unlock_irqrestore(&q->update_lock, flags);
+	}
+
+	if (modem_spi_dev->msr_flag) {
+		atomic_set(&q->q_rp, 0);
+		return -ENODEV;
+	}
+
+	msgsize = modem_isa_msg_size(q);
+	if (len < msgsize)
+		return -EINVAL;
+
+	if ((q->readptr + msgsize) >= q->size) {
+		psrc = (char *)buf;
+		size = (q->size - q->readptr);
+		/* copy first part of msg */
+		if (copy_to_user(psrc, (u8 *)(q->fifo_base + q->readptr),
+				size))
+			return -EFAULT;
+
+		psrc += size;
+		/* copy second part of msg at the top of fifo */
+		if (copy_to_user(psrc, (u8 *)(q->fifo_base),
+				(msgsize - size)))
+			return -EFAULT;
+	} else {
+		if (copy_to_user(buf, (u8 *)(q->fifo_base + q->readptr),
+				msgsize))
+			return -EFAULT;
+	}
+
+	spin_lock_irqsave(&q->update_lock, flags);
+	ret = modem_isa_unqueue_msg(q);
+	if (ret < 0)
+		msgsize = ret;
+	spin_unlock_irqrestore(&q->update_lock, flags);
+	return msgsize;
+}
+
+static ssize_t isa_write(struct file *filp, const char __user *buf,
+	size_t len, loff_t *ppos)
+{
+	struct isa_device_context *isadev = filp->private_data;
+	struct message_queue *q = &isadev->dl_queue;
+	struct modem_spi_dev *modem_spi_dev = q->modem_spi_dev;
+	struct isa_driver_context *isa_context = modem_spi_dev->isa_context;
+	void *addr = 0;
+	int err;
+	int l2_header;
+	int ret = 0;
+	unsigned long flags;
+
+	if (len <= 0 || buf == NULL)
+		return -EFAULT;
+
+	if (len > SIZE_OF_TX_COPY_BUFFER) {
+		dev_err(modem_spi_dev->dev,
+			"invalid message size %d! max is %d bytes\n",
+			len, SIZE_OF_TX_COPY_BUFFER);
+		return -EFAULT;
+	}
+
+	l2_header = modem_get_cdev_l2header(isadev->device_id);
+	if (l2_header < 0) {
+		dev_err(modem_spi_dev->dev, "invalid L2 channel!\n");
+		return l2_header;
+	}
+
+	switch (l2_header) {
+	case MODEM_M6718_SPI_CHN_AUDIO:
+		addr = (void *)wr_audio_msg;
+		break;
+	case MODEM_M6718_SPI_CHN_MASTER_LOOPBACK0:
+	case MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK0:
+	case MODEM_M6718_SPI_CHN_MASTER_LOOPBACK1:
+	case MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK1:
+		addr = (void *)wr_mlb_msg;
+		break;
+	default:
+		dev_dbg(modem_spi_dev->dev, "invalid device!\n");
+		return -EFAULT;
+	}
+
+	if (copy_from_user(addr, buf, len))
+		return -EFAULT;
+
+	/*
+	 * Special handling for audio channel:
+	 *   uses a mutext instead of a spinlock
+	 */
+	if (l2_header == MODEM_M6718_SPI_CHN_AUDIO ||
+		l2_header == MODEM_M6718_SPI_CHN_MASTER_LOOPBACK1) {
+		mutex_lock(&isa_context->audio_tx_mutex);
+		err = modem_m6718_spi_send(modem_spi_dev, l2_header, len, addr);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		mutex_unlock(&modem_spi_dev->isa_context->audio_tx_mutex);
+	} else {
+		spin_lock_irqsave(&isa_context->common_tx_lock, flags);
+		err = modem_m6718_spi_send(modem_spi_dev, l2_header, len, addr);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		spin_unlock_irqrestore(&isa_context->common_tx_lock, flags);
+	}
+	return ret;
+}
+
+static long isa_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	return -EINVAL;
+}
+
+static int isa_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	return -EINVAL;
+}
+
+static int isa_close(struct inode *inode, struct file *filp)
+{
+	struct isa_device_context *isadev = filp->private_data;
+	struct modem_spi_dev *modem_spi_dev = isadev->dl_queue.modem_spi_dev;
+	struct isa_driver_context *isa_context = modem_spi_dev->isa_context;
+	u8 m;
+	int idx;
+
+	mutex_lock(&isa_lock);
+	m = iminor(filp->f_path.dentry->d_inode);
+	idx = modem_get_cdev_index(m);
+	if (idx < 0) {
+		dev_err(modem_spi_dev->dev, "invalid L2 channel!\n");
+		return idx;
+	}
+
+	if (atomic_dec_and_test(&isa_context->is_open[idx])) {
+		atomic_inc(&isa_context->is_open[idx]);
+		dev_err(modem_spi_dev->dev, "device is not open yet!\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	atomic_set(&isa_context->is_open[idx], 1);
+
+	switch (m) {
+	case MODEM_M6718_SPI_CHN_AUDIO:
+		dev_dbg(modem_spi_dev->dev, "close channel AUDIO\n");
+		break;
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_LOOPBACK
+	case MODEM_M6718_SPI_CHN_MASTER_LOOPBACK0:
+		dev_dbg(modem_spi_dev->dev, "close channel MASTER_LOOPBACK0\n");
+		break;
+	case MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK0:
+		dev_dbg(modem_spi_dev->dev, "close channel SLAVE_LOOPBACK0\n");
+		break;
+	case MODEM_M6718_SPI_CHN_MASTER_LOOPBACK1:
+		dev_dbg(modem_spi_dev->dev, "close channel MASTER_LOOPBACK1\n");
+		break;
+	case MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK1:
+		dev_dbg(modem_spi_dev->dev, "close channel SLAVE_LOOPBACK1\n");
+		break;
+#endif
+	default:
+		dev_dbg(modem_spi_dev->dev, "invalid device\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	mutex_unlock(&isa_lock);
+	return 0;
+}
+
+static int isa_open(struct inode *inode, struct file *filp)
+{
+	int err = 0;
+	u8 m;
+	int idx;
+	struct isa_device_context *isadev;
+	struct isa_driver_context *isa_context =
+		container_of(inode->i_cdev, struct isa_driver_context, cdev);
+	struct modem_spi_dev *modem_spi_dev =
+		isa_context->isadev->dl_queue.modem_spi_dev;
+
+	if (!modem_m6718_spi_is_boot_done()) {
+		dev_dbg(modem_spi_dev->dev,
+			"failed to open device, boot is not complete\n");
+		err = -EBUSY;
+		goto out;
+	}
+
+	mutex_lock(&isa_lock);
+	m = iminor(inode);
+	idx = modem_get_cdev_index(m);
+	if (idx < 0) {
+		dev_err(modem_spi_dev->dev, "invalid device\n");
+		err = -ENODEV;
+		goto cleanup;
+	}
+
+	if (!atomic_dec_and_test(&isa_context->is_open[idx])) {
+		atomic_inc(&isa_context->is_open[idx]);
+		dev_err(modem_spi_dev->dev, "device is already open\n");
+		err = -EBUSY;
+		goto cleanup;
+	}
+
+	isadev = &isa_context->isadev[idx];
+	if (filp != NULL)
+		filp->private_data = isadev;
+
+	switch (m) {
+	case MODEM_M6718_SPI_CHN_AUDIO:
+		dev_dbg(modem_spi_dev->dev, "open channel AUDIO\n");
+		break;
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_LOOPBACK
+	case MODEM_M6718_SPI_CHN_MASTER_LOOPBACK0:
+		dev_dbg(modem_spi_dev->dev, "open channel MASTER_LOOPBACK0\n");
+		break;
+	case MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK0:
+		dev_dbg(modem_spi_dev->dev, "open channel SLAVE_LOOPBACK0\n");
+		break;
+	case MODEM_M6718_SPI_CHN_MASTER_LOOPBACK1:
+		dev_dbg(modem_spi_dev->dev, "open channel MASTER_LOOPBACK1\n");
+		break;
+	case MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK1:
+		dev_dbg(modem_spi_dev->dev, "open channel SLAVE_LOOPBACK1\n");
+		break;
+#endif
+	}
+
+cleanup:
+	mutex_unlock(&isa_lock);
+out:
+	return err;
+}
+
+const struct file_operations isa_fops = {
+	.owner = THIS_MODULE,
+	.open = isa_open,
+	.release = isa_close,
+	.unlocked_ioctl = isa_ioctl,
+	.mmap = isa_mmap,
+	.read = isa_read,
+	.write = isa_write,
+	.poll = isa_select,
+};
+
+/**
+ * modem_isa_init() - initialise the modem char device interfaces
+ * @modem_spi_dev: pointer to the modem driver information structure
+ *
+ * This function registers registers as a char device driver and creates the
+ * char device nodes supported by the modem.
+ */
+int modem_isa_init(struct modem_spi_dev *modem_spi_dev)
+{
+	dev_t dev_id;
+	int retval;
+	int devidx;
+	struct isa_device_context *isadev;
+	struct isa_driver_context *isa_context;
+
+	dev_dbg(modem_spi_dev->dev, "registering char device interfaces\n");
+
+	isa_context = kzalloc(sizeof(struct isa_driver_context), GFP_KERNEL);
+	if (isa_context == NULL) {
+		dev_err(modem_spi_dev->dev, "failed to allocate context\n");
+		retval = -ENOMEM;
+		goto rollback;
+	}
+
+	modem_spi_dev->isa_context = isa_context;
+	if (major) {
+		/* major node specified at module load */
+		dev_id = MKDEV(major, 0);
+		retval = register_chrdev_region(dev_id,
+			MODEM_M6718_SPI_MAX_CHANNELS, NAME);
+	} else {
+		retval = alloc_chrdev_region(&dev_id, 0,
+			MODEM_M6718_SPI_MAX_CHANNELS, NAME);
+		major = MAJOR(dev_id);
+	}
+
+	dev_dbg(modem_spi_dev->dev, "device major is %d\n", major);
+
+	cdev_init(&isa_context->cdev, &isa_fops);
+	isa_context->cdev.owner = THIS_MODULE;
+	retval = cdev_add(&isa_context->cdev, dev_id,
+		MODEM_M6718_SPI_MAX_CHANNELS);
+	if (retval) {
+		dev_err(modem_spi_dev->dev, "failed to add char device\n");
+		goto rollback_register;
+	}
+
+	isa_context->modem_class = class_create(THIS_MODULE, NAME);
+	if (IS_ERR(isa_context->modem_class)) {
+		dev_err(modem_spi_dev->dev, "failed to create modem class\n");
+		retval = PTR_ERR(isa_context->modem_class);
+		goto rollback_add_dev;
+	}
+	isa_context->isadev = kzalloc(sizeof(struct isa_device_context) *
+				MODEM_M6718_SPI_MAX_CHANNELS, GFP_KERNEL);
+	if (isa_context->isadev == NULL) {
+		dev_err(modem_spi_dev->dev,
+			"failed to allocate device context\n");
+		goto rollback_create_dev;
+	}
+
+	for (devidx = 0; devidx < ARRAY_SIZE(map_dev); devidx++) {
+		atomic_set(&isa_context->is_open[devidx], 1);
+		device_create(isa_context->modem_class,
+			NULL,
+			MKDEV(MAJOR(dev_id), map_dev[devidx].l2_header),
+			NULL,
+			map_dev[devidx].name);
+
+		isadev = &isa_context->isadev[devidx];
+		isadev->device_id = devidx;
+		create_queue(&isadev->dl_queue,
+			isadev->device_id, modem_spi_dev);
+
+		dev_dbg(modem_spi_dev->dev, "created device %d (%s) (%d.%d)\n",
+			devidx, map_dev[devidx].name, major,
+			map_dev[devidx].l2_header);
+	}
+
+	mutex_init(&isa_context->audio_tx_mutex);
+	spin_lock_init(&isa_context->common_tx_lock);
+
+	dev_dbg(modem_spi_dev->dev, "registered modem char devices\n");
+	return 0;
+
+rollback_create_dev:
+	for (devidx = 0; devidx < ARRAY_SIZE(map_dev); devidx++) {
+		device_destroy(isa_context->modem_class,
+			MKDEV(MAJOR(dev_id), map_dev[devidx].l2_header));
+	}
+	class_destroy(isa_context->modem_class);
+rollback_add_dev:
+	cdev_del(&isa_context->cdev);
+rollback_register:
+	unregister_chrdev_region(dev_id, MODEM_M6718_SPI_MAX_CHANNELS);
+	kfree(isa_context);
+	modem_spi_dev->isa_context = NULL;
+rollback:
+	return retval;
+}
+EXPORT_SYMBOL_GPL(modem_isa_init);
+
+/**
+ * modem_isa_exit() - remove the char device interfaces and clean up
+ * @modem_spi_dev: pointer to the modem driver information structure
+ */
+void modem_isa_exit(struct modem_spi_dev *modem_spi_dev)
+{
+	int devidx;
+	struct isa_device_context *isadev;
+	struct isa_driver_context *isa_context = modem_spi_dev->isa_context;
+	dev_t dev_id = MKDEV(major, 0);
+
+	if (!modem_spi_dev || !modem_spi_dev->isa_context)
+		return;
+
+	for (devidx = 0; devidx < ARRAY_SIZE(map_dev); devidx++)
+		device_destroy(isa_context->modem_class,
+				MKDEV(MAJOR(dev_id),
+				map_dev[devidx].l2_header));
+	for (devidx = 0; devidx < MODEM_M6718_SPI_MAX_CHANNELS; devidx++) {
+		isadev = &isa_context->isadev[devidx];
+		delete_queue(&isadev->dl_queue);
+		kfree(isadev);
+	}
+	class_destroy(isa_context->modem_class);
+	cdev_del(&isa_context->cdev);
+	unregister_chrdev_region(dev_id, MODEM_M6718_SPI_MAX_CHANNELS);
+	kfree(isa_context);
+	modem_spi_dev->isa_context = NULL;
+	dev_dbg(modem_spi_dev->dev, "removed modem char devices\n");
+}
+EXPORT_SYMBOL_GPL(modem_isa_exit);
+
+MODULE_AUTHOR("Chris Blair <chris.blair@stericsson.com>");
+MODULE_DESCRIPTION("M6718 modem IPC char device interface");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/char/shrm_char.c b/drivers/char/shrm_char.c
new file mode 100644
index 00000000000..e8f350e5da8
--- /dev/null
+++ b/drivers/char/shrm_char.c
@@ -0,0 +1,897 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghavi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/sched.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_config.h>
+#include <linux/modem/shrm/shrm.h>
+#include <asm/atomic.h>
+
+#include <mach/isa_ioctl.h>
+
+
+#define NAME "IPC_ISA"
+/* L2 header for rtc_calibration device is 0xC8 and hence 0xC8 + 1 = 201 */
+#define MAX_L2_HEADERS 201
+
+#define SIZE_OF_FIFO (512*1024)
+
+static u8 message_fifo[ISA_DEVICES][SIZE_OF_FIFO];
+
+static u8 wr_rpc_msg[10*1024];
+static u8 wr_sec_msg[10*1024];
+static u8 wr_audio_msg[10*1024];
+static u8 wr_rtc_cal_msg[100];
+
+struct map_device {
+	u8 l2_header;
+	u8 idx;
+	char *name;
+};
+
+static struct map_device map_dev[] = {
+	{ISI_MESSAGING, 0, "isi"},
+	{RPC_MESSAGING, 1, "rpc"},
+	{AUDIO_MESSAGING, 2, "modemaudio"},
+	{SECURITY_MESSAGING, 3, "sec"},
+	{COMMON_LOOPBACK_MESSAGING, 4, "common_loopback"},
+	{AUDIO_LOOPBACK_MESSAGING, 5, "audio_loopback"},
+	{CIQ_MESSAGING, 6, "ciq"},
+	{RTC_CAL_MESSAGING, 7, "rtc_calibration"},
+};
+
+/*
+ * int major:This variable is exported to user as module_param to specify
+ * major number at load time
+ */
+static int major;
+module_param(major, int, 0);
+MODULE_PARM_DESC(major, "Major device number");
+/* global fops mutex */
+static DEFINE_MUTEX(isa_lock);
+
+/**
+ * shrm_get_cdev_index() - return the index mapped to l2 header
+ * @l2_header:	L2 header
+ *
+ * struct map_device maps the index(count) with the device L2 header.
+ * This function returns the index for the provided L2 header in case
+ * of success else -ve value.
+ */
+int shrm_get_cdev_index(u8 l2_header)
+{
+	u8 cnt;
+	for (cnt = 0; cnt < ISA_DEVICES; cnt++) {
+		if (map_dev[cnt].l2_header == l2_header)
+			return map_dev[cnt].idx;
+	}
+	return -EINVAL;
+}
+
+/**
+ * shrm_get_cdev_l2header() - return l2_header mapped to the index
+ * @idx:	index
+ *
+ * struct map_device maps the index(count) with the device L2 header.
+ * This function returns the L2 header for the given index in case
+ * of success else -ve value.
+ */
+int shrm_get_cdev_l2header(u8 idx)
+{
+	u8 cnt;
+	for (cnt = 0; cnt < ISA_DEVICES; cnt++) {
+		if (map_dev[cnt].idx == idx)
+			return map_dev[cnt].l2_header;
+	}
+	return -EINVAL;
+}
+
+void shrm_char_reset_queues(struct shrm_dev *shrm)
+{
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context;
+	struct queue_element *cur_msg = NULL;
+	struct list_head *cur_msg_ptr = NULL;
+	struct list_head *msg_ptr;
+	struct message_queue *q;
+	int no_dev;
+
+	dev_info(shrm->dev, "%s: Resetting char device queues\n", __func__);
+	isa_context = shrm->isa_context;
+	for (no_dev = 0 ; no_dev < ISA_DEVICES ; no_dev++) {
+		isadev = &isa_context->isadev[no_dev];
+		q = &isadev->dl_queue;
+
+		spin_lock_bh(&q->update_lock);
+		/* empty out the msg queue */
+		list_for_each_safe(cur_msg_ptr, msg_ptr, &q->msg_list) {
+			cur_msg = list_entry(cur_msg_ptr,
+					struct queue_element, entry);
+			list_del(cur_msg_ptr);
+			kfree(cur_msg);
+		}
+
+		/* reset the msg queue pointers */
+		q->size = SIZE_OF_FIFO;
+		q->readptr = 0;
+		q->writeptr = 0;
+		q->no = 0;
+
+		/* wake up the blocking read/select */
+		atomic_set(&q->q_rp, 1);
+		wake_up_interruptible(&q->wq_readable);
+
+		spin_unlock_bh(&q->update_lock);
+	}
+}
+
+/**
+ * create_queue() - To create FIFO for Tx and Rx message buffering.
+ * @q:		message queue.
+ * @devicetype:	device type 0-isi,1-rpc,2-audio,3-security,
+ * 4-common_loopback, 5-audio_loopback.
+ * @shrm:	pointer to the shrm device information structure
+ *
+ * This function creates a FIFO buffer of n_bytes size using
+ * dma_alloc_coherent(). It also initializes all queue handling
+ * locks, queue management pointers. It also initializes message list
+ * which occupies this queue.
+ */
+static int create_queue(struct message_queue *q, u32 devicetype,
+						struct shrm_dev *shrm)
+{
+	q->fifo_base = (u8 *)&message_fifo[devicetype];
+	q->size = SIZE_OF_FIFO;
+	q->readptr = 0;
+	q->writeptr = 0;
+	q->no = 0;
+	q->shrm = shrm;
+	spin_lock_init(&q->update_lock);
+	INIT_LIST_HEAD(&q->msg_list);
+	init_waitqueue_head(&q->wq_readable);
+	atomic_set(&q->q_rp, 0);
+
+	return 0;
+}
+
+static void delete_queue(struct message_queue *q)
+{
+	q->size = 0;
+	q->readptr = 0;
+	q->writeptr = 0;
+}
+
+/**
+ * add_msg_to_queue() - Add a message inside queue
+ * @q:		message queue
+ * @size:	size in bytes
+ *
+ * This function tries to allocate n_bytes of size in FIFO q.
+ * It returns negative number when no memory can be allocated
+ * currently.
+ */
+int add_msg_to_queue(struct message_queue *q, u32 size)
+{
+	struct queue_element *new_msg = NULL;
+	struct shrm_dev *shrm = q->shrm;
+
+	dev_dbg(shrm->dev, "%s IN q->writeptr=%d\n", __func__, q->writeptr);
+	new_msg = kmalloc(sizeof(struct queue_element), GFP_ATOMIC);
+	if (new_msg == NULL) {
+		dev_err(shrm->dev, "unable to allocate memory\n");
+		return -ENOMEM;
+	}
+	new_msg->offset = q->writeptr;
+	new_msg->size = size;
+	new_msg->no = q->no++;
+
+	/* check for overflow condition */
+	if (q->readptr <= q->writeptr) {
+		if (((q->writeptr-q->readptr) + size) >= q->size) {
+			dev_err(shrm->dev, "Buffer overflow !!\n");
+			BUG_ON(((q->writeptr-q->readptr) + size) >= q->size);
+		}
+	} else {
+		if ((q->writeptr + size) >= q->readptr) {
+			dev_err(shrm->dev, "Buffer overflow !!\n");
+			BUG_ON((q->writeptr + size) >= q->readptr);
+		}
+	}
+	q->writeptr = (q->writeptr + size) % q->size;
+	if (list_empty(&q->msg_list)) {
+		list_add_tail(&new_msg->entry, &q->msg_list);
+		/* There can be 2 blocking calls read  and another select */
+		atomic_set(&q->q_rp, 1);
+		wake_up_interruptible(&q->wq_readable);
+	} else
+		list_add_tail(&new_msg->entry, &q->msg_list);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+}
+
+/**
+ * remove_msg_from_queue() - To remove a message from the msg queue.
+ * @q:	message queue
+ *
+ * This function delets a message from the message list associated with message
+ * queue q and also updates read ptr.
+ * If the message list is empty, then, event is set to block the select and
+ * read calls of the paricular queue.
+ *
+ * The message list is FIFO style and message is always added to tail and
+ * removed from head.
+ */
+int remove_msg_from_queue(struct message_queue *q)
+{
+	struct queue_element *old_msg = NULL;
+	struct shrm_dev *shrm = q->shrm;
+	struct list_head *msg_ptr = NULL;
+	struct list_head *old_msg_ptr = NULL;
+
+	dev_dbg(shrm->dev, "%s IN q->readptr %d\n", __func__, q->readptr);
+
+	list_for_each_safe(old_msg_ptr, msg_ptr, &q->msg_list) {
+		old_msg = list_entry(old_msg_ptr, struct queue_element, entry);
+		if (old_msg == NULL) {
+			dev_err(shrm->dev, "no message found\n");
+			return -EFAULT;
+		}
+		list_del(old_msg_ptr);
+		q->readptr = (q->readptr + old_msg->size)%q->size;
+		kfree(old_msg);
+		break;
+	}
+	if (list_empty(&q->msg_list)) {
+		dev_dbg(shrm->dev, "List is empty setting RP= 0\n");
+		atomic_set(&q->q_rp, 0);
+	}
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+}
+
+/**
+ * get_size_of_new_msg() - retrieve new message from message list
+ * @q:	message queue
+ *
+ * This function will retrieve most recent message from the corresponding
+ * queue list. New message is always retrieved from head side.
+ * It returns new message no, offset if FIFO and size.
+ */
+int get_size_of_new_msg(struct message_queue *q)
+{
+	struct queue_element *new_msg = NULL;
+	struct list_head *msg_list;
+	struct shrm_dev *shrm = q->shrm;
+	int size = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	spin_lock_bh(&q->update_lock);
+	list_for_each(msg_list, &q->msg_list) {
+		new_msg = list_entry(msg_list, struct queue_element, entry);
+		if (new_msg == NULL) {
+			spin_unlock_bh(&q->update_lock);
+			dev_err(shrm->dev, "no message found\n");
+			return -EFAULT;
+		}
+		size = new_msg->size;
+		break;
+	}
+	spin_unlock_bh(&q->update_lock);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return size;
+}
+
+/**
+ * isa_select() - shrm char interface driver select interface
+ * @filp:	file descriptor pointer
+ * @wait:	poll_table_struct pointer
+ *
+ * This function is used to perform non-blocking read operations. It allows
+ * a process to determine whether it can read from one or more open files
+ * without blocking. These calls can also block a process until any of a
+ * given set of file descriptors becomes available for reading.
+ * If a file is ready to read, POLLIN | POLLRDNORM bitmask is returned.
+ * The driver method is called whenever the user-space program performs a select
+ * system call involving a file descriptor associated with the driver.
+ */
+static u32 isa_select(struct file *filp,
+				struct poll_table_struct *wait)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	u32 mask = 0;
+	u32 m = iminor(filp->f_path.dentry->d_inode);
+	u8 idx = shrm_get_cdev_index(m);
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (shrm->msr_flag)
+		return -ENODEV;
+
+	if (isadev->device_id != idx)
+			return -1;
+
+	q = &isadev->dl_queue;
+	poll_wait(filp, &q->wq_readable, wait);
+	if (atomic_read(&q->q_rp) == 1)
+		mask = POLLIN | POLLRDNORM;
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return mask;
+}
+
+/**
+ * isa_read() - Read from device
+ * @filp:	file descriptor
+ * @buf:	user buffer pointer
+ * @len:	size of requested data transfer
+ * @ppos:	not used
+ *
+ * It reads a oldest message from queue and copies it into user buffer and
+ * returns its size.
+ * If there is no message present in queue, then it blocks until new data is
+ * available.
+ */
+ssize_t isa_read(struct file *filp, char __user *buf, size_t len, loff_t *ppos)
+{
+	u32 size = 0;
+	int ret;
+	char *psrc;
+	struct isadev_context *isadev = (struct isadev_context *)
+							filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	u32 msgsize;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (len <= 0)
+		return -EFAULT;
+
+	q = &isadev->dl_queue;
+
+	if (shrm->msr_flag) {
+		atomic_set(&q->q_rp, 0);
+		return -ENODEV;
+	}
+
+	spin_lock_bh(&q->update_lock);
+	if (list_empty(&q->msg_list)) {
+		spin_unlock_bh(&q->update_lock);
+		dev_dbg(shrm->dev, "Waiting for Data\n");
+		if (wait_event_interruptible(q->wq_readable,
+				atomic_read(&q->q_rp) == 1))
+			return -ERESTARTSYS;
+	} else
+		spin_unlock_bh(&q->update_lock);
+
+	if (shrm->msr_flag) {
+		atomic_set(&q->q_rp, 0);
+		return -ENODEV;
+	}
+
+	msgsize = get_size_of_new_msg(q);
+
+	if (len < msgsize)
+		return -EINVAL;
+
+	if ((q->readptr+msgsize) >= q->size) {
+		dev_dbg(shrm->dev, "Inside Loop Back\n");
+		psrc = (char *)buf;
+		size = (q->size-q->readptr);
+		/* Copy First Part of msg */
+		if (copy_to_user(psrc,
+				(u8 *)(q->fifo_base+q->readptr),
+				size)) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		if (copy_to_user(psrc,
+				(u8 *)(q->fifo_base),
+				(msgsize-size))) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+	} else {
+		if (copy_to_user(buf,
+				(u8 *)(q->fifo_base + q->readptr),
+				msgsize)) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+	}
+	spin_lock_bh(&q->update_lock);
+	ret = remove_msg_from_queue(q);
+	if (ret < 0) {
+		dev_err(shrm->dev,
+				"Remove msg from message queue failed\n");
+		msgsize = ret;
+	}
+	spin_unlock_bh(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return msgsize;
+}
+
+/**
+ * isa_write() - Write to shrm char device
+ * @filp:	file descriptor
+ * @buf:	user buffer pointer
+ * @len:	size of requested data transfer
+ * @ppos:	not used
+ *
+ * It checks if there is space available in queue, and copies the message
+ * inside queue. If there is no space, it blocks until space becomes available.
+ * It also schedules transfer thread to transmit the newly added message.
+ */
+ssize_t isa_write(struct file *filp, const char __user *buf,
+				 size_t len, loff_t *ppos)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	void *addr = 0;
+	int err, l2_header;
+	int ret = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (len <= 0 || buf == NULL)
+		return -EFAULT;
+	q = &isadev->dl_queue;
+	l2_header = shrm_get_cdev_l2header(isadev->device_id);
+	if (l2_header < 0) {
+		dev_err(shrm->dev, "failed to get L2 header\n");
+		return l2_header;
+	}
+
+	switch (l2_header) {
+	case RPC_MESSAGING:
+		dev_dbg(shrm->dev, "RPC\n");
+		addr = (void *)wr_rpc_msg;
+		break;
+	case AUDIO_MESSAGING:
+		dev_dbg(shrm->dev, "Audio\n");
+		addr = (void *)wr_audio_msg;
+		break;
+	case SECURITY_MESSAGING:
+		dev_dbg(shrm->dev, "Security\n");
+		addr = (void *)wr_sec_msg;
+		break;
+	case COMMON_LOOPBACK_MESSAGING:
+		dev_dbg(shrm->dev, "Common loopback\n");
+		addr = isadev->addr;
+		break;
+	case AUDIO_LOOPBACK_MESSAGING:
+		dev_dbg(shrm->dev, "Audio loopback\n");
+		addr = isadev->addr;
+		break;
+	case CIQ_MESSAGING:
+		dev_dbg(shrm->dev, "CIQ\n");
+		addr = isadev->addr;
+		break;
+	case RTC_CAL_MESSAGING:
+		dev_dbg(shrm->dev, "isa_write(): RTC Calibration\n");
+		addr = (void *)wr_rtc_cal_msg;
+		break;
+	default:
+		dev_dbg(shrm->dev, "Wrong device\n");
+		return -EFAULT;
+	}
+
+	if (copy_from_user(addr, buf, len)) {
+		dev_err(shrm->dev, "copy_from_user failed\n");
+		return -EFAULT;
+	}
+	/* Write msg to Fifo */
+	if ((l2_header == AUDIO_MESSAGING) ||
+			(l2_header == AUDIO_LOOPBACK_MESSAGING)) {
+		mutex_lock(&shrm->isa_context->tx_audio_mutex);
+		err = shm_write_msg(shrm, l2_header, addr, len);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		mutex_unlock(&shrm->isa_context->tx_audio_mutex);
+	} else {
+		spin_lock_bh(&shrm->isa_context->common_tx);
+		err = shm_write_msg(shrm, l2_header, addr, len);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		spin_unlock_bh(&shrm->isa_context->common_tx);
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * isa_ioctl() - To handle different ioctl commands supported by driver.
+ * @inode:	structure is used by the kernel internally to represent files
+ * @filp:	file descriptor pointer
+ * @cmd:	ioctl command
+ * @arg:	input param
+ *
+ * Following ioctls are supported by this driver.
+ * DLP_IOCTL_ALLOCATE_BUFFER - To allocate buffer for new uplink message.
+ * This ioctl is called with required message size. It returns offset for
+ * the allocates space in the queue. DLP_IOCTL_PUT_MESSAGE -  To indicate
+ * new uplink message available in queuq for  transmission. Message is copied
+ * from offset location returned by previous ioctl before calling this ioctl.
+ * DLP_IOCTL_GET_MESSAGE - To check if any downlink message is available in
+ * queue. It returns  offset for new message inside queue.
+ * DLP_IOCTL_DEALLOCATE_BUFFER - To deallocate any buffer allocate for
+ * downlink message once the message is copied. Message is copied from offset
+ * location returned by previous ioctl before calling this ioctl.
+ */
+static long isa_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	long err = 0;
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	u32 m = iminor(filp->f_path.dentry->d_inode);
+
+	isadev = (struct isadev_context *)filp->private_data;
+
+	if (isadev->device_id != m)
+		return -EINVAL;
+
+	switch (cmd) {
+	case DLP_IOC_ALLOCATE_BUFFER:
+		dev_dbg(shrm->dev, "DLP_IOC_ALLOCATE_BUFFER\n");
+		break;
+	case DLP_IOC_PUT_MESSAGE:
+		dev_dbg(shrm->dev, "DLP_IOC_PUT_MESSAGE\n");
+		break;
+	case DLP_IOC_GET_MESSAGE:
+		dev_dbg(shrm->dev, "DLP_IOC_GET_MESSAGE\n");
+		break;
+	case DLP_IOC_DEALLOCATE_BUFFER:
+		dev_dbg(shrm->dev, "DLP_IOC_DEALLOCATE_BUFFER\n");
+		break;
+	default:
+		dev_dbg(shrm->dev, "Unknown IOCTL\n");
+		err = -EFAULT;
+		break;
+	}
+	return err;
+}
+/**
+ * isa_mmap() - Maps kernel queue memory to user space.
+ * @filp:	file descriptor pointer
+ * @vma:	virtual area memory structure.
+ *
+ * This function maps kernel FIFO into user space. This function
+ * shall be called twice to map both uplink and downlink buffers.
+ */
+static int isa_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+
+	u32 m = iminor(filp->f_path.dentry->d_inode);
+	dev_dbg(shrm->dev, "%s %d\n", __func__, m);
+
+	return 0;
+}
+
+/**
+ * isa_close() - Close device file
+ * @inode:	structure is used by the kernel internally to represent files
+ * @filp:	device file descriptor
+ *
+ * This function deletes structues associated with this file, deletes
+ * queues, flushes and destroys workqueus and closes this file.
+ * It also unregisters itself from l2mux driver.
+ */
+static int isa_close(struct inode *inode, struct file *filp)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct isa_driver_context *isa_context = shrm->isa_context;
+	u8 m;
+	int idx;
+
+	mutex_lock(&isa_lock);
+	m = iminor(filp->f_path.dentry->d_inode);
+	idx = shrm_get_cdev_index(m);
+	if (idx < 0) {
+		dev_err(shrm->dev, "failed to get index\n");
+		mutex_unlock(&isa_lock);
+		return idx;
+	}
+	dev_dbg(shrm->dev, "isa_close %d", m);
+
+	if (atomic_dec_and_test(&isa_context->is_open[idx])) {
+		atomic_inc(&isa_context->is_open[idx]);
+		dev_err(shrm->dev, "Device not opened yet\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	atomic_set(&isa_context->is_open[idx], 1);
+
+	switch (m) {
+	case RPC_MESSAGING:
+		dev_info(shrm->dev, "Close RPC_MESSAGING Device\n");
+		break;
+	case AUDIO_MESSAGING:
+		dev_info(shrm->dev, "Close AUDIO_MESSAGING Device\n");
+		break;
+	case SECURITY_MESSAGING:
+		dev_info(shrm->dev, "CLose SECURITY_MESSAGING Device\n");
+		break;
+	case COMMON_LOOPBACK_MESSAGING:
+		kfree(isadev->addr);
+		dev_info(shrm->dev, "Close COMMON_LOOPBACK_MESSAGING Device\n");
+		break;
+	case AUDIO_LOOPBACK_MESSAGING:
+		kfree(isadev->addr);
+		dev_info(shrm->dev, "Close AUDIO_LOOPBACK_MESSAGING Device\n");
+		break;
+	case CIQ_MESSAGING:
+		kfree(isadev->addr);
+		dev_info(shrm->dev, "Close CIQ_MESSAGING Device\n");
+		break;
+	case RTC_CAL_MESSAGING:
+		dev_info(shrm->dev, "Close RTC_CAL_MESSAGING Device\n");
+		break;
+	default:
+		dev_info(shrm->dev, "No such device present\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	};
+	mutex_unlock(&isa_lock);
+	return 0;
+}
+/**
+ * isa_open() -  Open device file
+ * @inode:	structure is used by the kernel internally to represent files
+ * @filp:	device file descriptor
+ *
+ * This function performs initialization tasks needed to open SHM channel.
+ * Following tasks are performed.
+ * -return if device is already opened
+ * -create uplink FIFO
+ * -create downlink FIFO
+ * -init delayed workqueue thread
+ * -register to l2mux driver
+ */
+static int isa_open(struct inode *inode, struct file *filp)
+{
+	int err = 0;
+	u8 m;
+	int idx;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context = container_of(
+						inode->i_cdev,
+						struct isa_driver_context,
+						cdev);
+	struct shrm_dev *shrm = isa_context->isadev->dl_queue.shrm;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (get_boot_state() != BOOT_DONE) {
+		dev_err(shrm->dev, "Boot is not done\n");
+		return -EBUSY;
+	}
+	mutex_lock(&isa_lock);
+	m = iminor(inode);
+
+	if ((m != RPC_MESSAGING) &&
+				(m != AUDIO_LOOPBACK_MESSAGING) &&
+				(m != COMMON_LOOPBACK_MESSAGING) &&
+				(m != AUDIO_MESSAGING) &&
+				(m != SECURITY_MESSAGING) &&
+				(m != CIQ_MESSAGING) &&
+				(m != RTC_CAL_MESSAGING)) {
+		dev_err(shrm->dev, "No such device present\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	idx = shrm_get_cdev_index(m);
+	if (idx < 0) {
+		dev_err(shrm->dev, "failed to get index\n");
+		mutex_unlock(&isa_lock);
+		return idx;
+	}
+	if (!atomic_dec_and_test(&isa_context->is_open[idx])) {
+		atomic_inc(&isa_context->is_open[idx]);
+		dev_err(shrm->dev, "Device already opened\n");
+		mutex_unlock(&isa_lock);
+		return -EBUSY;
+	}
+	isadev = &isa_context->isadev[idx];
+	if (filp != NULL)
+		filp->private_data = isadev;
+
+	switch (m) {
+	case RPC_MESSAGING:
+		dev_info(shrm->dev, "Open RPC_MESSAGING Device\n");
+		break;
+	case AUDIO_MESSAGING:
+		dev_info(shrm->dev, "Open AUDIO_MESSAGING Device\n");
+		break;
+	case SECURITY_MESSAGING:
+		dev_info(shrm->dev, "Open SECURITY_MESSAGING Device\n");
+		break;
+	case COMMON_LOOPBACK_MESSAGING:
+		isadev->addr = kzalloc(10 * 1024, GFP_KERNEL);
+		if (!isadev->addr) {
+			mutex_unlock(&isa_lock);
+			return -ENOMEM;
+		}
+		dev_info(shrm->dev, "Open COMMON_LOOPBACK_MESSAGING Device\n");
+		break;
+	case AUDIO_LOOPBACK_MESSAGING:
+		isadev->addr = kzalloc(10 * 1024, GFP_KERNEL);
+		if (!isadev->addr) {
+			mutex_unlock(&isa_lock);
+			return -ENOMEM;
+		}
+		dev_info(shrm->dev, "Open AUDIO_LOOPBACK_MESSAGING Device\n");
+		break;
+	case CIQ_MESSAGING:
+		isadev->addr = kzalloc(10 * 1024, GFP_KERNEL);
+		if (!isadev->addr) {
+			mutex_unlock(&isa_lock);
+			return -ENOMEM;
+		}
+		dev_info(shrm->dev, "Open CIQ_MESSAGING Device\n");
+		break;
+	case RTC_CAL_MESSAGING:
+		dev_info(shrm->dev, "Open RTC_CAL_MESSAGING Device\n");
+		break;
+	};
+
+	mutex_unlock(&isa_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return err;
+}
+
+const struct file_operations isa_fops = {
+	.owner = THIS_MODULE,
+	.open = isa_open,
+	.release = isa_close,
+	.unlocked_ioctl = isa_ioctl,
+	.mmap = isa_mmap,
+	.read = isa_read,
+	.write = isa_write,
+	.poll = isa_select,
+};
+
+/**
+ * isa_init() - module insertion function
+ * @shrm:	pointer to the shrm device information structure
+ *
+ * This function registers module as a character driver using
+ * register_chrdev_region() or alloc_chrdev_region. It adds this
+ * driver to system using cdev_add() call. Major number is dynamically
+ * allocated using alloc_chrdev_region() by default or left to user to specify
+ * it during load time. For this variable major is used as module_param
+ * Nodes to be created using
+ * mknod /dev/isi c $major 0
+ * mknod /dev/rpc c $major 1
+ * mknod /dev/audio c $major 2
+ * mknod /dev/sec c $major 3
+ */
+int isa_init(struct shrm_dev *shrm)
+{
+	dev_t	dev_id;
+	int	retval, no_dev;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context;
+
+	isa_context = kzalloc(sizeof(struct isa_driver_context),
+								GFP_KERNEL);
+	if (isa_context == NULL) {
+		dev_err(shrm->dev, "Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+	shrm->isa_context = isa_context;
+	if (major) {
+		dev_id = MKDEV(major, MAX_L2_HEADERS);
+		retval = register_chrdev_region(dev_id, ISA_DEVICES, NAME);
+	} else {
+		/*
+		 * L2 header of loopback device is 192(0xc0). As per the shrm
+		 * protocol the minor id of the deivce is mapped to the
+		 * L2 header.
+		 */
+		retval = alloc_chrdev_region(&dev_id, 0, MAX_L2_HEADERS, NAME);
+		major = MAJOR(dev_id);
+	}
+	dev_dbg(shrm->dev, " major %d\n", major);
+
+	cdev_init(&isa_context->cdev, &isa_fops);
+	isa_context->cdev.owner = THIS_MODULE;
+	retval = cdev_add(&isa_context->cdev, dev_id, MAX_L2_HEADERS);
+	if (retval) {
+		dev_err(shrm->dev, "Failed to add char device\n");
+		return retval;
+	}
+	/* create class and device */
+	isa_context->shm_class = class_create(THIS_MODULE, NAME);
+	if (IS_ERR(isa_context->shm_class)) {
+		dev_err(shrm->dev, "Error creating shrm class\n");
+		cdev_del(&isa_context->cdev);
+		retval = PTR_ERR(isa_context->shm_class);
+		kfree(isa_context);
+		return retval;
+	}
+
+	for (no_dev = 0; no_dev < ISA_DEVICES; no_dev++) {
+		atomic_set(&isa_context->is_open[no_dev], 1);
+		device_create(isa_context->shm_class, NULL,
+				MKDEV(MAJOR(dev_id),
+				map_dev[no_dev].l2_header), NULL,
+				map_dev[no_dev].name);
+	}
+
+	isa_context->isadev = kzalloc(sizeof
+				(struct isadev_context)*ISA_DEVICES,
+				GFP_KERNEL);
+	if (isa_context->isadev == NULL) {
+		dev_err(shrm->dev, "Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+	for (no_dev = 0 ; no_dev < ISA_DEVICES ; no_dev++) {
+		isadev = &isa_context->isadev[no_dev];
+		isadev->device_id = no_dev;
+		retval = create_queue(&isadev->dl_queue,
+					isadev->device_id, shrm);
+
+		if (retval < 0) {
+			dev_err(shrm->dev, "create dl_queue failed\n");
+			delete_queue(&isadev->dl_queue);
+			kfree(isadev);
+			return retval;
+		}
+	}
+	mutex_init(&isa_context->tx_audio_mutex);
+	spin_lock_init(&isa_context->common_tx);
+	dev_dbg(shrm->dev, " SHM Char Driver added\n");
+	return retval;
+}
+
+void isa_exit(struct shrm_dev *shrm)
+{
+	int no_dev;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context = shrm->isa_context;
+	dev_t dev_id = MKDEV(major, 0);
+
+	for (no_dev = 0 ; no_dev < ISA_DEVICES ; no_dev++) {
+		device_destroy(isa_context->shm_class,
+				MKDEV(MAJOR(dev_id),
+				map_dev[no_dev].l2_header));
+		isadev = &isa_context->isadev[no_dev];
+		delete_queue(&isadev->dl_queue);
+		kfree(isadev);
+	}
+	class_destroy(isa_context->shm_class);
+	cdev_del(&isa_context->cdev);
+	unregister_chrdev_region(dev_id, ISA_DEVICES);
+	kfree(isa_context);
+	dev_dbg(shrm->dev, " SHM Char Driver removed\n");
+}
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 5138927a416..6f86f8ca9b0 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -19,13 +19,27 @@ config DW_APB_TIMER
 config CLKSRC_DBX500_PRCMU
 	bool "Clocksource PRCMU Timer"
 	depends on UX500_SOC_DB5500 || UX500_SOC_DB8500
-	default y
+	default y if UX500_SOC_DB8500
 	help
 	  Use the always on PRCMU Timer as clocksource
 
 config CLKSRC_DBX500_PRCMU_SCHED_CLOCK
-	bool "Clocksource PRCMU Timer sched_clock"
-	depends on (CLKSRC_DBX500_PRCMU && !NOMADIK_MTU_SCHED_CLOCK)
+	bool
+	depends on CLKSRC_DBX500_PRCMU
+	select HAVE_SCHED_CLOCK
+	help
+	  Use the always on PRCMU Timer as sched_clock
+
+config CLKSRC_DB5500_MTIMER
+	bool "Clocksource MTIMER"
+	depends on UX500_SOC_DB5500
 	default y
 	help
+	  Use the always on MTIMER as clocksource
+
+config CLKSRC_DB5500_MTIMER_SCHED_CLOCK
+	bool
+	depends on CLKSRC_DB5500_MTIMER
+	select HAVE_SCHED_CLOCK
+	help
 	  Use the always on PRCMU Timer as sched_clock
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 8d81a1d3265..9b10f6b7536 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -9,4 +9,5 @@ obj-$(CONFIG_SH_TIMER_TMU)	+= sh_tmu.o
 obj-$(CONFIG_CLKBLD_I8253)	+= i8253.o
 obj-$(CONFIG_CLKSRC_MMIO)	+= mmio.o
 obj-$(CONFIG_DW_APB_TIMER)	+= dw_apb_timer.o
-obj-$(CONFIG_CLKSRC_DBX500_PRCMU)	+= clksrc-dbx500-prcmu.o
\ No newline at end of file
+obj-$(CONFIG_CLKSRC_DBX500_PRCMU)	+= clksrc-dbx500-prcmu.o
+obj-$(CONFIG_CLKSRC_DB5500_MTIMER)	+= db5500-mtimer.o
diff --git a/drivers/clocksource/clksrc-dbx500-prcmu.c b/drivers/clocksource/clksrc-dbx500-prcmu.c
index c26c369eb9e..dc71e432dc5 100644
--- a/drivers/clocksource/clksrc-dbx500-prcmu.c
+++ b/drivers/clocksource/clksrc-dbx500-prcmu.c
@@ -14,6 +14,9 @@
  */
 #include <linux/clockchips.h>
 #include <linux/clksrc-dbx500-prcmu.h>
+#ifdef CONFIG_BOOTTIME
+#include <linux/boottime.h>
+#endif
 
 #include <asm/sched_clock.h>
 
@@ -68,6 +71,23 @@ static u32 notrace dbx500_prcmu_sched_clock_read(void)
 
 #endif
 
+#ifdef CONFIG_BOOTTIME
+static unsigned long __init boottime_get_time(void)
+{
+	return div_s64(clocksource_cyc2ns(clocksource_dbx500_prcmu.read(
+						  &clocksource_dbx500_prcmu),
+					  clocksource_dbx500_prcmu.mult,
+					  clocksource_dbx500_prcmu.shift),
+		       1000);
+}
+
+static struct boottime_timer __initdata boottime_timer = {
+	.init     = NULL,
+	.get_time = boottime_get_time,
+	.finalize = NULL,
+};
+#endif
+
 void __init clksrc_dbx500_prcmu_init(void __iomem *base)
 {
 	clksrc_dbx500_timer_base = base;
@@ -90,4 +110,7 @@ void __init clksrc_dbx500_prcmu_init(void __iomem *base)
 			 32, RATE_32K);
 #endif
 	clocksource_register_hz(&clocksource_dbx500_prcmu, RATE_32K);
+#ifdef CONFIG_BOOTTIME
+	boottime_activate(&boottime_timer);
+#endif
 }
diff --git a/drivers/clocksource/db5500-mtimer.c b/drivers/clocksource/db5500-mtimer.c
new file mode 100644
index 00000000000..5e64da19e66
--- /dev/null
+++ b/drivers/clocksource/db5500-mtimer.c
@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ */
+
+#include <linux/io.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/clockchips.h>
+#include <linux/clksrc-db5500-mtimer.h>
+#include <linux/boottime.h>
+
+#include <asm/sched_clock.h>
+
+#define MTIMER_PRIMARY_COUNTER		0x18
+
+static void __iomem *db5500_mtimer_base;
+
+#ifdef CONFIG_CLKSRC_DB5500_MTIMER_SCHED_CLOCK
+static DEFINE_CLOCK_DATA(cd);
+
+unsigned long long notrace sched_clock(void)
+{
+	u32 cyc;
+
+	if (unlikely(!db5500_mtimer_base))
+		return 0;
+
+	cyc = readl_relaxed(db5500_mtimer_base + MTIMER_PRIMARY_COUNTER);
+
+	return cyc_to_sched_clock(&cd, cyc, (u32)~0);
+}
+
+static void notrace db5500_mtimer_update_sched_clock(void)
+{
+	u32 cyc = readl_relaxed(db5500_mtimer_base + MTIMER_PRIMARY_COUNTER);
+	update_sched_clock(&cd, cyc, (u32)~0);
+}
+#endif
+
+#ifdef CONFIG_BOOTTIME
+static unsigned long __init boottime_get_time(void)
+{
+	return sched_clock();
+}
+
+static struct boottime_timer __initdata boottime_timer = {
+	.init     = NULL,
+	.get_time = boottime_get_time,
+	.finalize = NULL,
+};
+#endif
+
+void __init db5500_mtimer_init(void __iomem *base)
+{
+	db5500_mtimer_base = base;
+
+	clocksource_mmio_init(base + MTIMER_PRIMARY_COUNTER, "mtimer", 32768,
+			      400, 32, clocksource_mmio_readl_up);
+
+#ifdef CONFIG_CLKSRC_DB5500_MTIMER_SCHED_CLOCK
+	init_sched_clock(&cd, db5500_mtimer_update_sched_clock,
+			 32, 32768);
+#endif
+	boottime_activate(&boottime_timer);
+}
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index 9531fc2eda2..44aa7093235 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -39,7 +39,8 @@ obj-$(CONFIG_X86_CPUFREQ_NFORCE2)	+= cpufreq-nforce2.o
 
 ##################################################################################
 # ARM SoC drivers
-obj-$(CONFIG_UX500_SOC_DB8500)		+= db8500-cpufreq.o
+obj-$(CONFIG_UX500_SOC_DB8500)		+= dbx500-cpufreq.o
+obj-$(CONFIG_UX500_SOC_DB5500)		+= dbx500-cpufreq.o
 obj-$(CONFIG_ARM_S3C2416_CPUFREQ)	+= s3c2416-cpufreq.o
 obj-$(CONFIG_ARM_S3C64XX_CPUFREQ)	+= s3c64xx-cpufreq.o
 obj-$(CONFIG_ARM_S5PV210_CPUFREQ)	+= s5pv210-cpufreq.o
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 7f2f149ae40..4de1fff790f 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -376,6 +376,27 @@ show_one(scaling_cur_freq, cur);
 static int __cpufreq_set_policy(struct cpufreq_policy *data,
 				struct cpufreq_policy *policy);
 
+int cpufreq_update_freq(int cpu, unsigned int min, unsigned int max)
+{
+	int ret;
+	struct cpufreq_policy new_policy;
+	struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
+
+	ret = cpufreq_get_policy(&new_policy, cpu);
+	if (ret)
+		return -EINVAL;
+
+	new_policy.min = min;
+	new_policy.max = max;
+
+	ret = __cpufreq_set_policy(policy, &new_policy);
+	policy->user_policy.min = policy->min;
+	policy->user_policy.max = policy->max;
+
+	return ret;
+}
+EXPORT_SYMBOL(cpufreq_update_freq);
+
 /**
  * cpufreq_per_cpu_attr_write() / store_##file_name() - sysfs write access
  */
diff --git a/drivers/cpufreq/cpufreq_ondemand.c b/drivers/cpufreq/cpufreq_ondemand.c
index 836e9b062e5..765256e4904 100644
--- a/drivers/cpufreq/cpufreq_ondemand.c
+++ b/drivers/cpufreq/cpufreq_ondemand.c
@@ -79,7 +79,6 @@ struct cpu_dbs_info_s {
 	cputime64_t prev_cpu_wall;
 	cputime64_t prev_cpu_nice;
 	struct cpufreq_policy *cur_policy;
-	struct delayed_work work;
 	struct cpufreq_frequency_table *freq_table;
 	unsigned int freq_lo;
 	unsigned int freq_lo_jiffies;
@@ -95,8 +94,10 @@ struct cpu_dbs_info_s {
 	struct mutex timer_mutex;
 };
 static DEFINE_PER_CPU(struct cpu_dbs_info_s, od_cpu_dbs_info);
+static DEFINE_PER_CPU(struct delayed_work, ondemand_work);
 
 static unsigned int dbs_enable;	/* number of CPUs using this policy */
+static ktime_t time_stamp;
 
 /*
  * dbs_mutex protects dbs_enable in governor start/stop.
@@ -290,22 +291,23 @@ static void update_sampling_rate(unsigned int new_rate)
 
 		mutex_lock(&dbs_info->timer_mutex);
 
-		if (!delayed_work_pending(&dbs_info->work)) {
+		if (!delayed_work_pending(&per_cpu(ondemand_work, cpu))) {
 			mutex_unlock(&dbs_info->timer_mutex);
 			continue;
 		}
 
 		next_sampling  = jiffies + usecs_to_jiffies(new_rate);
-		appointed_at = dbs_info->work.timer.expires;
+		appointed_at = per_cpu(ondemand_work, cpu).timer.expires;
 
 
 		if (time_before(next_sampling, appointed_at)) {
 
 			mutex_unlock(&dbs_info->timer_mutex);
-			cancel_delayed_work_sync(&dbs_info->work);
+			cancel_delayed_work_sync(&per_cpu(ondemand_work, cpu));
 			mutex_lock(&dbs_info->timer_mutex);
 
-			schedule_delayed_work_on(dbs_info->cpu, &dbs_info->work,
+			schedule_delayed_work_on(dbs_info->cpu,
+						 &per_cpu(ondemand_work, cpu),
 						 usecs_to_jiffies(new_rate));
 
 		}
@@ -449,6 +451,26 @@ static struct attribute_group dbs_attr_group = {
 
 /************************** sysfs end ************************/
 
+static bool dbs_sw_coordinated_cpus(void)
+{
+	struct cpu_dbs_info_s *dbs_info;
+	struct cpufreq_policy *policy;
+	int i = 0;
+	int j;
+
+	dbs_info = &per_cpu(od_cpu_dbs_info, 0);
+	policy = dbs_info->cur_policy;
+
+	for_each_cpu(j, policy->cpus) {
+		i++;
+	}
+
+	if (i > 1)
+		return true; /* Dependant CPUs */
+	else
+		return false;
+}
+
 static void dbs_freq_increase(struct cpufreq_policy *p, unsigned int freq)
 {
 	if (dbs_tuners_ins.powersave_bias)
@@ -463,7 +485,6 @@ static void dbs_freq_increase(struct cpufreq_policy *p, unsigned int freq)
 static void dbs_check_cpu(struct cpu_dbs_info_s *this_dbs_info)
 {
 	unsigned int max_load_freq;
-
 	struct cpufreq_policy *policy;
 	unsigned int j;
 
@@ -598,20 +619,42 @@ static void dbs_check_cpu(struct cpu_dbs_info_s *this_dbs_info)
 
 static void do_dbs_timer(struct work_struct *work)
 {
-	struct cpu_dbs_info_s *dbs_info =
-		container_of(work, struct cpu_dbs_info_s, work.work);
-	unsigned int cpu = dbs_info->cpu;
-	int sample_type = dbs_info->sample_type;
-
+	struct cpu_dbs_info_s *dbs_info;
+	unsigned int cpu = smp_processor_id();
+	int sample_type;
 	int delay;
+	bool sample = true;
+
+	/* If SW dependant CPUs, use CPU 0 as leader */
+	if (dbs_sw_coordinated_cpus()) {
+
+		ktime_t time_now;
+		s64 delta_us;
 
-	mutex_lock(&dbs_info->timer_mutex);
+		dbs_info = &per_cpu(od_cpu_dbs_info, 0);
+		mutex_lock(&dbs_info->timer_mutex);
+
+		time_now = ktime_get();
+		delta_us = ktime_us_delta(time_now, time_stamp);
+
+		/* Do nothing if we recently have sampled */
+		if (delta_us < (s64)(dbs_tuners_ins.sampling_rate / 2))
+			sample = false;
+		else
+			time_stamp = time_now;
+	} else {
+		dbs_info = &per_cpu(od_cpu_dbs_info, cpu);
+		mutex_lock(&dbs_info->timer_mutex);
+	}
+
+	sample_type = dbs_info->sample_type;
 
 	/* Common NORMAL_SAMPLE setup */
 	dbs_info->sample_type = DBS_NORMAL_SAMPLE;
 	if (!dbs_tuners_ins.powersave_bias ||
 	    sample_type == DBS_NORMAL_SAMPLE) {
-		dbs_check_cpu(dbs_info);
+		if (sample)
+			dbs_check_cpu(dbs_info);
 		if (dbs_info->freq_lo) {
 			/* Setup timer for SUB_SAMPLE */
 			dbs_info->sample_type = DBS_SUB_SAMPLE;
@@ -627,15 +670,17 @@ static void do_dbs_timer(struct work_struct *work)
 				delay -= jiffies % delay;
 		}
 	} else {
-		__cpufreq_driver_target(dbs_info->cur_policy,
-			dbs_info->freq_lo, CPUFREQ_RELATION_H);
+		if (sample)
+			__cpufreq_driver_target(dbs_info->cur_policy,
+						dbs_info->freq_lo,
+						CPUFREQ_RELATION_H);
 		delay = dbs_info->freq_lo_jiffies;
 	}
-	schedule_delayed_work_on(cpu, &dbs_info->work, delay);
+	schedule_delayed_work_on(cpu, &per_cpu(ondemand_work, cpu), delay);
 	mutex_unlock(&dbs_info->timer_mutex);
 }
 
-static inline void dbs_timer_init(struct cpu_dbs_info_s *dbs_info)
+static inline void dbs_timer_init(struct cpu_dbs_info_s *dbs_info, int cpu)
 {
 	/* We want all CPUs to do sampling nearly on same jiffy */
 	int delay = usecs_to_jiffies(dbs_tuners_ins.sampling_rate);
@@ -644,13 +689,18 @@ static inline void dbs_timer_init(struct cpu_dbs_info_s *dbs_info)
 		delay -= jiffies % delay;
 
 	dbs_info->sample_type = DBS_NORMAL_SAMPLE;
-	INIT_DELAYED_WORK_DEFERRABLE(&dbs_info->work, do_dbs_timer);
-	schedule_delayed_work_on(dbs_info->cpu, &dbs_info->work, delay);
+	cancel_delayed_work_sync(&per_cpu(ondemand_work, cpu));
+	schedule_delayed_work_on(cpu, &per_cpu(ondemand_work, cpu), delay);
+}
+
+static inline void dbs_timer_exit(int cpu)
+{
+	cancel_delayed_work_sync(&per_cpu(ondemand_work, cpu));
 }
 
-static inline void dbs_timer_exit(struct cpu_dbs_info_s *dbs_info)
+static void dbs_timer_exit_per_cpu(struct work_struct *dummy)
 {
-	cancel_delayed_work_sync(&dbs_info->work);
+	dbs_timer_exit(smp_processor_id());
 }
 
 /*
@@ -676,6 +726,42 @@ static int should_io_be_busy(void)
 	return 0;
 }
 
+static int __cpuinit cpu_callback(struct notifier_block *nfb,
+				  unsigned long action, void *hcpu)
+{
+	unsigned int cpu = (unsigned long)hcpu;
+	struct device *cpu_dev;
+	struct cpu_dbs_info_s *dbs_info;
+
+	if (dbs_sw_coordinated_cpus())
+		dbs_info = &per_cpu(od_cpu_dbs_info, 0);
+	else
+		dbs_info = &per_cpu(od_cpu_dbs_info, cpu);
+
+	cpu_dev = get_cpu_device(cpu);
+	if (cpu_dev) {
+		switch (action) {
+		case CPU_ONLINE:
+		case CPU_ONLINE_FROZEN:
+			dbs_timer_init(dbs_info, cpu);
+			break;
+		case CPU_DOWN_PREPARE:
+		case CPU_DOWN_PREPARE_FROZEN:
+			dbs_timer_exit(cpu);
+			break;
+		case CPU_DOWN_FAILED:
+		case CPU_DOWN_FAILED_FROZEN:
+			dbs_timer_init(dbs_info, cpu);
+			break;
+		}
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block __refdata ondemand_cpu_notifier = {
+	.notifier_call = cpu_callback,
+};
+
 static int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 				   unsigned int event)
 {
@@ -704,9 +790,13 @@ static int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 			if (dbs_tuners_ins.ignore_nice)
 				j_dbs_info->prev_cpu_nice =
 						kcpustat_cpu(j).cpustat[CPUTIME_NICE];
+			mutex_init(&j_dbs_info->timer_mutex);
+			INIT_DELAYED_WORK_DEFERRABLE(&per_cpu(ondemand_work, j),
+						     do_dbs_timer);
+
+			j_dbs_info->rate_mult = 1;
 		}
 		this_dbs_info->cpu = cpu;
-		this_dbs_info->rate_mult = 1;
 		ondemand_powersave_bias_init_cpu(cpu);
 		/*
 		 * Start the timerschedule work, when this governor
@@ -736,21 +826,46 @@ static int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 		}
 		mutex_unlock(&dbs_mutex);
 
-		mutex_init(&this_dbs_info->timer_mutex);
-		dbs_timer_init(this_dbs_info);
+		/* If SW coordinated CPUs then register notifier */
+		if (dbs_sw_coordinated_cpus()) {
+			register_hotcpu_notifier(&ondemand_cpu_notifier);
+
+			for_each_cpu(j, policy->cpus) {
+				struct cpu_dbs_info_s *j_dbs_info;
+
+				j_dbs_info = &per_cpu(od_cpu_dbs_info, 0);
+				dbs_timer_init(j_dbs_info, j);
+			}
+
+			/* Initiate timer time stamp */
+			time_stamp = ktime_get();
+
+
+		} else
+			dbs_timer_init(this_dbs_info, cpu);
 		break;
 
 	case CPUFREQ_GOV_STOP:
-		dbs_timer_exit(this_dbs_info);
+
+		dbs_timer_exit(cpu);
 
 		mutex_lock(&dbs_mutex);
 		mutex_destroy(&this_dbs_info->timer_mutex);
 		dbs_enable--;
 		mutex_unlock(&dbs_mutex);
-		if (!dbs_enable)
+		if (!dbs_enable) {
 			sysfs_remove_group(cpufreq_global_kobject,
 					   &dbs_attr_group);
 
+			if (dbs_sw_coordinated_cpus()) {
+				/*
+				 * Make sure all pending timers/works are
+				 * stopped.
+				 */
+				schedule_on_each_cpu(dbs_timer_exit_per_cpu);
+				unregister_hotcpu_notifier(&ondemand_cpu_notifier);
+			}
+		}
 		break;
 
 	case CPUFREQ_GOV_LIMITS:
diff --git a/drivers/cpufreq/db8500-cpufreq.c b/drivers/cpufreq/db8500-cpufreq.c
deleted file mode 100644
index 0bf1b8910ee..00000000000
--- a/drivers/cpufreq/db8500-cpufreq.c
+++ /dev/null
@@ -1,170 +0,0 @@
-/*
- * Copyright (C) STMicroelectronics 2009
- * Copyright (C) ST-Ericsson SA 2010
- *
- * License Terms: GNU General Public License v2
- * Author: Sundar Iyer <sundar.iyer@stericsson.com>
- * Author: Martin Persson <martin.persson@stericsson.com>
- * Author: Jonas Aaberg <jonas.aberg@stericsson.com>
- *
- */
-#include <linux/kernel.h>
-#include <linux/cpufreq.h>
-#include <linux/delay.h>
-#include <linux/slab.h>
-#include <linux/mfd/dbx500-prcmu.h>
-#include <mach/id.h>
-
-static struct cpufreq_frequency_table freq_table[] = {
-	[0] = {
-		.index = 0,
-		.frequency = 200000,
-	},
-	[1] = {
-		.index = 1,
-		.frequency = 400000,
-	},
-	[2] = {
-		.index = 2,
-		.frequency = 800000,
-	},
-	[3] = {
-		/* Used for MAX_OPP, if available */
-		.index = 3,
-		.frequency = CPUFREQ_TABLE_END,
-	},
-	[4] = {
-		.index = 4,
-		.frequency = CPUFREQ_TABLE_END,
-	},
-};
-
-static enum arm_opp idx2opp[] = {
-	ARM_EXTCLK,
-	ARM_50_OPP,
-	ARM_100_OPP,
-	ARM_MAX_OPP
-};
-
-static struct freq_attr *db8500_cpufreq_attr[] = {
-	&cpufreq_freq_attr_scaling_available_freqs,
-	NULL,
-};
-
-static int db8500_cpufreq_verify_speed(struct cpufreq_policy *policy)
-{
-	return cpufreq_frequency_table_verify(policy, freq_table);
-}
-
-static int db8500_cpufreq_target(struct cpufreq_policy *policy,
-				unsigned int target_freq,
-				unsigned int relation)
-{
-	struct cpufreq_freqs freqs;
-	unsigned int idx;
-
-	/* scale the target frequency to one of the extremes supported */
-	if (target_freq < policy->cpuinfo.min_freq)
-		target_freq = policy->cpuinfo.min_freq;
-	if (target_freq > policy->cpuinfo.max_freq)
-		target_freq = policy->cpuinfo.max_freq;
-
-	/* Lookup the next frequency */
-	if (cpufreq_frequency_table_target
-	    (policy, freq_table, target_freq, relation, &idx)) {
-		return -EINVAL;
-	}
-
-	freqs.old = policy->cur;
-	freqs.new = freq_table[idx].frequency;
-
-	if (freqs.old == freqs.new)
-		return 0;
-
-	/* pre-change notification */
-	for_each_cpu(freqs.cpu, policy->cpus)
-		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-
-	/* request the PRCM unit for opp change */
-	if (prcmu_set_arm_opp(idx2opp[idx])) {
-		pr_err("db8500-cpufreq:  Failed to set OPP level\n");
-		return -EINVAL;
-	}
-
-	/* post change notification */
-	for_each_cpu(freqs.cpu, policy->cpus)
-		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
-
-	return 0;
-}
-
-static unsigned int db8500_cpufreq_getspeed(unsigned int cpu)
-{
-	int i;
-	/* request the prcm to get the current ARM opp */
-	for (i = 0; prcmu_get_arm_opp() != idx2opp[i]; i++)
-		;
-	return freq_table[i].frequency;
-}
-
-static int __cpuinit db8500_cpufreq_init(struct cpufreq_policy *policy)
-{
-	int i, res;
-
-	BUILD_BUG_ON(ARRAY_SIZE(idx2opp) + 1 != ARRAY_SIZE(freq_table));
-
-	if (prcmu_has_arm_maxopp())
-		freq_table[3].frequency = 1000000;
-
-	pr_info("db8500-cpufreq : Available frequencies:\n");
-	for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++)
-		pr_info("  %d Mhz\n", freq_table[i].frequency/1000);
-
-	/* get policy fields based on the table */
-	res = cpufreq_frequency_table_cpuinfo(policy, freq_table);
-	if (!res)
-		cpufreq_frequency_table_get_attr(freq_table, policy->cpu);
-	else {
-		pr_err("db8500-cpufreq : Failed to read policy table\n");
-		return res;
-	}
-
-	policy->min = policy->cpuinfo.min_freq;
-	policy->max = policy->cpuinfo.max_freq;
-	policy->cur = db8500_cpufreq_getspeed(policy->cpu);
-	policy->governor = CPUFREQ_DEFAULT_GOVERNOR;
-
-	/*
-	 * FIXME : Need to take time measurement across the target()
-	 *	   function with no/some/all drivers in the notification
-	 *	   list.
-	 */
-	policy->cpuinfo.transition_latency = 20 * 1000; /* in ns */
-
-	/* policy sharing between dual CPUs */
-	cpumask_copy(policy->cpus, cpu_present_mask);
-
-	policy->shared_type = CPUFREQ_SHARED_TYPE_ALL;
-
-	return 0;
-}
-
-static struct cpufreq_driver db8500_cpufreq_driver = {
-	.flags  = CPUFREQ_STICKY,
-	.verify = db8500_cpufreq_verify_speed,
-	.target = db8500_cpufreq_target,
-	.get    = db8500_cpufreq_getspeed,
-	.init   = db8500_cpufreq_init,
-	.name   = "DB8500",
-	.attr   = db8500_cpufreq_attr,
-};
-
-static int __init db8500_cpufreq_register(void)
-{
-	if (!cpu_is_u8500v20_or_later())
-		return -ENODEV;
-
-	pr_info("cpufreq for DB8500 started\n");
-	return cpufreq_register_driver(&db8500_cpufreq_driver);
-}
-device_initcall(db8500_cpufreq_register);
diff --git a/drivers/cpufreq/dbx500-cpufreq.c b/drivers/cpufreq/dbx500-cpufreq.c
new file mode 100644
index 00000000000..a6f991e2fb6
--- /dev/null
+++ b/drivers/cpufreq/dbx500-cpufreq.c
@@ -0,0 +1,340 @@
+/*
+ * Copyright (C) STMicroelectronics 2009
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Sundar Iyer <sundar.iyer@stericsson.com>
+ * Author: Martin Persson <martin.persson@stericsson.com>
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/cpufreq.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <mach/id.h>
+
+static struct cpufreq_frequency_table db8500_freq_table[] = {
+	[0] = {
+		.index = 0,
+		.frequency = 200000,
+	},
+	[1] = {
+		.index = 1,
+		.frequency = 400000,
+	},
+	[2] = {
+		.index = 2,
+		.frequency = 800000,
+	},
+	[3] = {
+		/* Used for MAX_OPP, if available */
+		.index = 3,
+		.frequency = CPUFREQ_TABLE_END,
+	},
+	[4] = {
+		.index = 4,
+		.frequency = CPUFREQ_TABLE_END,
+	},
+};
+
+static struct cpufreq_frequency_table db5500_freq_table[] = {
+	[0] = {
+		.index = 0,
+		.frequency = 200000,
+	},
+	[1] = {
+		.index = 1,
+		.frequency = 396500,
+	},
+	[2] = {
+		.index = 2,
+		.frequency = 793000,
+	},
+	[3] = {
+		.index = 3,
+		.frequency = CPUFREQ_TABLE_END,
+	},
+};
+
+static struct cpufreq_frequency_table *freq_table;
+static int freq_table_len;
+
+static enum arm_opp db8500_idx2opp[] = {
+	ARM_EXTCLK,
+	ARM_50_OPP,
+	ARM_100_OPP,
+	ARM_MAX_OPP
+};
+
+static enum arm_opp db5500_idx2opp[] = {
+	ARM_EXTCLK,
+	ARM_50_OPP,
+	ARM_100_OPP,
+};
+
+static enum arm_opp *idx2opp;
+
+static struct freq_attr *dbx500_cpufreq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static int dbx500_cpufreq_verify_speed(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, freq_table);
+}
+
+static int dbx500_cpufreq_target(struct cpufreq_policy *policy,
+				unsigned int target_freq,
+				unsigned int relation)
+{
+	struct cpufreq_freqs freqs;
+	unsigned int idx;
+
+	/* scale the target frequency to one of the extremes supported */
+	if (target_freq < policy->cpuinfo.min_freq)
+		target_freq = policy->cpuinfo.min_freq;
+	if (target_freq > policy->cpuinfo.max_freq)
+		target_freq = policy->cpuinfo.max_freq;
+
+	/* Lookup the next frequency */
+	if (cpufreq_frequency_table_target
+	    (policy, freq_table, target_freq, relation, &idx)) {
+		return -EINVAL;
+	}
+
+	freqs.old = policy->cur;
+	freqs.new = freq_table[idx].frequency;
+
+	if (freqs.old == freqs.new)
+		return 0;
+
+	/* pre-change notification */
+	for_each_cpu(freqs.cpu, policy->cpus)
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+
+	BUG_ON(idx >= freq_table_len);
+
+	/* request the PRCM unit for opp change */
+	if (prcmu_set_arm_opp(idx2opp[idx])) {
+		pr_err("ux500-cpufreq:  Failed to set OPP level\n");
+		return -EINVAL;
+	}
+
+	/* post change notification */
+	for_each_cpu(freqs.cpu, policy->cpus)
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+	return 0;
+}
+
+static unsigned int dbx500_cpufreq_getspeed(unsigned int cpu)
+{
+	int i;
+	enum arm_opp current_opp;
+
+	current_opp = prcmu_get_arm_opp();
+
+	/* request the prcm to get the current ARM opp */
+	for (i = 0;  i < freq_table_len; i++) {
+		if (current_opp == idx2opp[i])
+			return freq_table[i].frequency;
+	}
+
+	pr_err("cpufreq: ERROR: unknown opp %d given from prcmufw!\n",
+	       current_opp);
+	BUG_ON(1);
+
+	/*
+	 * Better to return something that might be correct than
+	 * errno or zero, since clk_get_rate() won't do well with an errno.
+	 */
+	return freq_table[0].frequency;
+}
+
+static void __init dbx500_cpufreq_init_maxopp_freq(void)
+{
+	struct prcmu_fw_version *fw_version = prcmu_get_fw_version();
+
+	if ((fw_version == NULL) || !prcmu_has_arm_maxopp())
+		return;
+
+	switch (fw_version->project) {
+	case PRCMU_FW_PROJECT_U8500:
+	case PRCMU_FW_PROJECT_U9500:
+	case PRCMU_FW_PROJECT_U8420:
+		freq_table[3].frequency = 1000000;
+		break;
+	case PRCMU_FW_PROJECT_U8500_C2:
+	case PRCMU_FW_PROJECT_U9500_C2:
+	case PRCMU_FW_PROJECT_U8520:
+		freq_table[3].frequency = 1150000;
+		break;
+	default:
+		break;
+	}
+}
+
+static bool initialized;
+
+static void __init dbx500_cpufreq_early_init(void)
+{
+	if (cpu_is_u5500()) {
+		freq_table = db5500_freq_table;
+		idx2opp = db5500_idx2opp;
+		freq_table_len = ARRAY_SIZE(db5500_freq_table);
+	} else if (cpu_is_u8500()) {
+		freq_table = db8500_freq_table;
+		idx2opp = db8500_idx2opp;
+		dbx500_cpufreq_init_maxopp_freq();
+		freq_table_len = ARRAY_SIZE(db8500_freq_table);
+		if (!prcmu_has_arm_maxopp())
+			freq_table_len--;
+	} else {
+		ux500_unknown_soc();
+	}
+	initialized = true;
+}
+
+/*
+ * This is called from localtimer initialization, via the clk_get_rate() for
+ * the smp_twd clock.  This is way before cpufreq is initialized.
+ */
+unsigned long dbx500_cpufreq_getfreq(void)
+{
+	if (!initialized)
+		dbx500_cpufreq_early_init();
+
+	return dbx500_cpufreq_getspeed(0) * 1000;
+}
+
+int dbx500_cpufreq_percent2freq(int percent)
+{
+	int op;
+	int i;
+
+	switch (percent) {
+	case 0:
+		/* Fall through */
+	case 25:
+		op = ARM_EXTCLK;
+		break;
+	case 50:
+		op = ARM_50_OPP;
+		break;
+	case 100:
+		op = ARM_100_OPP;
+		break;
+	case 125:
+		if (cpu_is_u8500() && prcmu_has_arm_maxopp())
+			op = ARM_MAX_OPP;
+		else
+			op = ARM_100_OPP;
+		break;
+	default:
+		pr_err("cpufreq-dbx500: Incorrect arm target value (%d).\n",
+		       percent);;
+		return -EINVAL;
+		break;
+	}
+
+	for (i = 0; idx2opp[i] != op && i < freq_table_len; i++)
+		;
+
+	if (freq_table[i].frequency == CPUFREQ_TABLE_END) {
+		pr_err("cpufreq-dbx500: Matching frequency does not exist!\n");
+		return -EINVAL;
+	}
+
+	return freq_table[i].frequency;
+}
+
+int dbx500_cpufreq_get_limits(int cpu, int r,
+			      unsigned int *min, unsigned int *max)
+{
+	int freq;
+	int ret;
+	static int old_freq;
+	struct cpufreq_policy p;
+
+	freq = dbx500_cpufreq_percent2freq(r);
+
+	if (freq < 0)
+		return -EINVAL;
+
+	if (freq != old_freq)
+		pr_debug("cpufreq-dbx500: set min arm freq to %d\n",
+			 freq);
+
+	(*min) = freq;
+
+	ret = cpufreq_get_policy(&p, cpu);
+	if (ret) {
+		pr_err("cpufreq-dbx500: Failed to get policy.\n");
+		return -EINVAL;
+	}
+
+	(*max) = p.max;
+	return 0;
+}
+
+static int __cpuinit dbx500_cpufreq_init(struct cpufreq_policy *policy)
+{
+	int res;
+
+	/* get policy fields based on the table */
+	res = cpufreq_frequency_table_cpuinfo(policy, freq_table);
+	if (!res)
+		cpufreq_frequency_table_get_attr(freq_table, policy->cpu);
+	else {
+		pr_err("dbx500-cpufreq : Failed to read policy table\n");
+		return res;
+	}
+
+	policy->min = policy->cpuinfo.min_freq;
+	policy->max = policy->cpuinfo.max_freq;
+	policy->cur = dbx500_cpufreq_getspeed(policy->cpu);
+	policy->governor = CPUFREQ_DEFAULT_GOVERNOR;
+
+	/*
+	 * FIXME : Need to take time measurement across the target()
+	 *	   function with no/some/all drivers in the notification
+	 *	   list.
+	 */
+	policy->cpuinfo.transition_latency = 20 * 1000; /* in ns */
+
+	/* policy sharing between dual CPUs */
+	cpumask_copy(policy->cpus, cpu_present_mask);
+
+	policy->shared_type = CPUFREQ_SHARED_TYPE_ALL;
+
+	return 0;
+}
+
+static struct cpufreq_driver dbx500_cpufreq_driver = {
+	.flags  = CPUFREQ_STICKY,
+	.verify = dbx500_cpufreq_verify_speed,
+	.target = dbx500_cpufreq_target,
+	.get    = dbx500_cpufreq_getspeed,
+	.init   = dbx500_cpufreq_init,
+	.name   = "DBX500",
+	.attr   = dbx500_cpufreq_attr,
+};
+
+static int __init dbx500_cpufreq_register(void)
+{
+	int i;
+
+	if (!initialized)
+		dbx500_cpufreq_early_init();
+
+	pr_info("dbx500-cpufreq : Available frequencies:\n");
+
+	for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++)
+		pr_info("  %d Mhz\n", freq_table[i].frequency / 1000);
+
+	return cpufreq_register_driver(&dbx500_cpufreq_driver);
+}
+device_initcall(dbx500_cpufreq_register);
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index dd414d9350e..638648816c9 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -296,4 +296,15 @@ config CRYPTO_DEV_TEGRA_AES
 	  To compile this driver as a module, choose M here: the module
 	  will be called tegra-aes.
 
+config CRYPTO_DEV_UX500
+	tristate "Driver for ST-Ericsson UX500 crypto hardware acceleration"
+	depends on ARCH_U8500
+	select CRYPTO_ALGAPI
+	help
+	  Driver for ST-Ericsson UX500 crypto engine.
+
+if CRYPTO_DEV_UX500
+	source "drivers/crypto/ux500/Kconfig"
+endif # if CRYPTO_DEV_UX500
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index f3e64eadd7a..8737ed1bdfe 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -14,3 +14,4 @@ obj-$(CONFIG_CRYPTO_DEV_OMAP_AES) += omap-aes.o
 obj-$(CONFIG_CRYPTO_DEV_PICOXCELL) += picoxcell_crypto.o
 obj-$(CONFIG_CRYPTO_DEV_S5P) += s5p-sss.o
 obj-$(CONFIG_CRYPTO_DEV_TEGRA_AES) += tegra-aes.o
+obj-$(CONFIG_CRYPTO_DEV_UX500) += ux500/
diff --git a/drivers/crypto/ux500/Kconfig b/drivers/crypto/ux500/Kconfig
new file mode 100644
index 00000000000..165a03d46c0
--- /dev/null
+++ b/drivers/crypto/ux500/Kconfig
@@ -0,0 +1,29 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Shujuan Chen (shujuan.chen@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+config CRYPTO_DEV_UX500_CRYP
+	tristate "UX500 crypto driver for CRYP block"
+	depends on CRYPTO_DEV_UX500
+	select CRYPTO_DES
+	help
+	  This is the driver for the crypto block CRYP.
+
+config CRYPTO_DEV_UX500_HASH
+	tristate "UX500 crypto driver for HASH block"
+	depends on CRYPTO_DEV_UX500
+	select CRYPTO_HASH
+	select CRYPTO_HMAC
+	help
+	  This selects the UX500 hash driver for the HASH hardware.
+	  Depends on U8500/STM DMA if running in DMA mode.
+
+config CRYPTO_DEV_UX500_DEBUG
+	bool "Activate ux500 platform debug-mode for crypto and hash block"
+	depends on CRYPTO_DEV_UX500_CRYP || CRYPTO_DEV_UX500_HASH
+	default n
+	help
+	  Say Y if you want to add debug prints to ux500_hash and
+	  ux500_cryp devices.
diff --git a/drivers/crypto/ux500/Makefile b/drivers/crypto/ux500/Makefile
new file mode 100644
index 00000000000..b9a365bade8
--- /dev/null
+++ b/drivers/crypto/ux500/Makefile
@@ -0,0 +1,8 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Shujuan Chen (shujuan.chen@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+obj-$(CONFIG_CRYPTO_DEV_UX500_HASH) += hash/
+obj-$(CONFIG_CRYPTO_DEV_UX500_CRYP) += cryp/
diff --git a/drivers/crypto/ux500/cryp/Makefile b/drivers/crypto/ux500/cryp/Makefile
new file mode 100644
index 00000000000..e5d362a6f68
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/Makefile
@@ -0,0 +1,13 @@
+#/*
+# * Copyright (C) ST-Ericsson SA 2010
+# * Author: shujuan.chen@stericsson.com for ST-Ericsson.
+# * License terms: GNU General Public License (GPL) version 2  */
+
+ifdef CONFIG_CRYPTO_DEV_UX500_DEBUG
+CFLAGS_cryp_core.o := -DDEBUG -O0
+CFLAGS_cryp.o := -DDEBUG -O0
+CFLAGS_cryp_irq.o := -DDEBUG -O0
+endif
+
+obj-$(CONFIG_CRYPTO_DEV_UX500_CRYP) += ux500_cryp.o
+ux500_cryp-objs :=  cryp.o cryp_irq.o cryp_core.o
diff --git a/drivers/crypto/ux500/cryp/cryp.c b/drivers/crypto/ux500/cryp/cryp.c
new file mode 100644
index 00000000000..cec92af2f73
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp.c
@@ -0,0 +1,418 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+
+#include <mach/hardware.h>
+
+#include "cryp_p.h"
+#include "cryp.h"
+
+/**
+ * cryp_wait_until_done - wait until the device logic is not busy
+ */
+void cryp_wait_until_done(struct cryp_device_data *device_data)
+{
+	while (cryp_is_logic_busy(device_data))
+		cpu_relax();
+}
+
+/**
+ * cryp_check - This routine checks Peripheral and PCell Id
+ * @device_data: Pointer to the device data struct for base address.
+ */
+int cryp_check(struct cryp_device_data *device_data)
+{
+	int peripheralID2 = 0;
+
+	if (NULL == device_data)
+		return -EINVAL;
+
+	if (cpu_is_u8500() || cpu_is_u9540())
+		peripheralID2 = CRYP_PERIPHERAL_ID2_DB8500;
+	else if (cpu_is_u5500())
+		peripheralID2 = CRYP_PERIPHERAL_ID2_DB5500;
+
+	/* Check Peripheral and Pcell Id Register for CRYP */
+	if ((CRYP_PERIPHERAL_ID0 ==
+		readl_relaxed(&device_data->base->periphId0))
+	    && (CRYP_PERIPHERAL_ID1 ==
+		    readl_relaxed(&device_data->base->periphId1))
+	    && (peripheralID2 ==
+		    readl_relaxed(&device_data->base->periphId2))
+	    && (CRYP_PERIPHERAL_ID3 ==
+		    readl_relaxed(&device_data->base->periphId3))
+	    && (CRYP_PCELL_ID0 ==
+		    readl_relaxed(&device_data->base->pcellId0))
+	    && (CRYP_PCELL_ID1 ==
+		    readl_relaxed(&device_data->base->pcellId1))
+	    && (CRYP_PCELL_ID2 ==
+		    readl_relaxed(&device_data->base->pcellId2))
+	    && (CRYP_PCELL_ID3 ==
+		    readl_relaxed(&device_data->base->pcellId3))) {
+		return 0;
+	}
+
+	return -EPERM;
+}
+
+/**
+ * cryp_activity - This routine enables/disable the cryptography function.
+ * @device_data: Pointer to the device data struct for base address.
+ * @cryp_activity: Enable/Disable functionality
+ */
+void cryp_activity(struct cryp_device_data *device_data,
+		   enum cryp_crypen cryp_crypen)
+{
+	CRYP_PUT_BITS(&device_data->base->cr,
+		      cryp_crypen,
+		      CRYP_CR_CRYPEN_POS,
+		      CRYP_CR_CRYPEN_MASK);
+}
+
+/**
+ * cryp_flush_inoutfifo - Resets both the input and the output FIFOs
+ * @device_data: Pointer to the device data struct for base address.
+ */
+void cryp_flush_inoutfifo(struct cryp_device_data *device_data)
+{
+	/*
+	 * We always need to disble the hardware before trying to flush the
+	 * FIFO. This is something that isn't written in the design
+	 * specification, but we have been informed by the hardware designers
+	 * that this must be done.
+	 */
+	cryp_activity(device_data, CRYP_CRYPEN_DISABLE);
+	cryp_wait_until_done(device_data);
+
+	CRYP_SET_BITS(&device_data->base->cr, CRYP_CR_FFLUSH_MASK);
+	/*
+	 * CRYP_SR_INFIFO_READY_MASK is the expected value on the status
+	 * register when starting a new calculation, which means Input FIFO is
+	 * not full and input FIFO is empty.
+	 */
+	while (readl_relaxed(&device_data->base->sr) !=
+	       CRYP_SR_INFIFO_READY_MASK)
+		cpu_relax();
+}
+
+/**
+ * cryp_set_configuration - This routine set the cr CRYP IP
+ * @device_data: Pointer to the device data struct for base address.
+ * @cryp_config: Pointer to the configuration parameter
+ * @control_register: The control register to be written later on.
+ */
+int cryp_set_configuration(struct cryp_device_data *device_data,
+			   struct cryp_config *cryp_config,
+			   u32 *control_register)
+{
+	u32 cr_for_kse;
+
+	if (NULL == device_data || NULL == cryp_config)
+		return -EINVAL;
+
+	*control_register |= (cryp_config->keysize << CRYP_CR_KEYSIZE_POS);
+
+	/* Prepare key for decryption in AES_ECB and AES_CBC mode. */
+	if ((CRYP_ALGORITHM_DECRYPT == cryp_config->algodir) &&
+	    ((CRYP_ALGO_AES_ECB == cryp_config->algomode) ||
+	     (CRYP_ALGO_AES_CBC == cryp_config->algomode))) {
+		cr_for_kse = *control_register;
+		/*
+		 * This seems a bit odd, but it is indeed needed to set this to
+		 * encrypt even though it is a decryption that we are doing. It
+		 * also mentioned in the design spec that you need to do this.
+		 * After the keyprepartion for decrypting is done you should set
+		 * algodir back to decryption, which is done outside this if
+		 * statement.
+		 *
+		 * According to design specification we should set mode ECB
+		 * during key preparation even though we might be running CBC
+		 * when enter this function.
+		 *
+		 * Writing to KSE_ENABLED will drop CRYPEN when key preparation
+		 * is done. Therefore we need to set CRYPEN again outside this
+		 * if statement when running decryption.
+		 */
+		cr_for_kse |= ((CRYP_ALGORITHM_ENCRYPT << CRYP_CR_ALGODIR_POS) |
+			       (CRYP_ALGO_AES_ECB << CRYP_CR_ALGOMODE_POS) |
+			       (CRYP_CRYPEN_ENABLE << CRYP_CR_CRYPEN_POS) |
+			       (KSE_ENABLED << CRYP_CR_KSE_POS));
+
+		writel_relaxed(cr_for_kse, &device_data->base->cr);
+		cryp_wait_until_done(device_data);
+	}
+
+	*control_register |=
+		((cryp_config->algomode << CRYP_CR_ALGOMODE_POS) |
+		 (cryp_config->algodir << CRYP_CR_ALGODIR_POS));
+
+	return 0;
+}
+
+/**
+ * cryp_configure_protection - set the protection bits in the CRYP logic.
+ * @device_data: Pointer to the device data struct for base address.
+ * @p_protect_config:	Pointer to the protection mode and
+ *			secure mode configuration
+ */
+int cryp_configure_protection(struct cryp_device_data *device_data,
+			      struct cryp_protection_config *p_protect_config)
+{
+	if (NULL == p_protect_config)
+		return -EINVAL;
+
+	CRYP_WRITE_BIT(&device_data->base->cr,
+		       (u32) p_protect_config->secure_access,
+		       CRYP_CR_SECURE_MASK);
+	CRYP_PUT_BITS(&device_data->base->cr,
+		      p_protect_config->privilege_access,
+		      CRYP_CR_PRLG_POS,
+		      CRYP_CR_PRLG_MASK);
+
+	return 0;
+}
+
+/**
+ * cryp_is_logic_busy - returns the busy status of the CRYP logic
+ * @device_data: Pointer to the device data struct for base address.
+ */
+int cryp_is_logic_busy(struct cryp_device_data *device_data)
+{
+	return CRYP_TEST_BITS(&device_data->base->sr,
+			      CRYP_SR_BUSY_MASK);
+}
+
+/**
+ * cryp_configure_for_dma - configures the CRYP IP for DMA operation
+ * @device_data: Pointer to the device data struct for base address.
+ * @dma_req: Specifies the DMA request type value.
+ */
+void cryp_configure_for_dma(struct cryp_device_data *device_data,
+			    enum cryp_dma_req_type dma_req)
+{
+	CRYP_SET_BITS(&device_data->base->dmacr,
+		      (u32) dma_req);
+}
+
+/**
+ * cryp_configure_key_values - configures the key values for CRYP operations
+ * @device_data: Pointer to the device data struct for base address.
+ * @key_reg_index: Key value index register
+ * @key_value: The key value struct
+ */
+int cryp_configure_key_values(struct cryp_device_data *device_data,
+			      enum cryp_key_reg_index key_reg_index,
+			      struct cryp_key_value key_value)
+{
+	while (cryp_is_logic_busy(device_data))
+		cpu_relax();
+
+	switch (key_reg_index) {
+	case CRYP_KEY_REG_1:
+		writel_relaxed(key_value.key_value_left,
+		       &device_data->base->key_1_l);
+		writel_relaxed(key_value.key_value_right,
+		       &device_data->base->key_1_r);
+		break;
+	case CRYP_KEY_REG_2:
+		writel_relaxed(key_value.key_value_left,
+		       &device_data->base->key_2_l);
+		writel_relaxed(key_value.key_value_right,
+		       &device_data->base->key_2_r);
+		break;
+	case CRYP_KEY_REG_3:
+		writel_relaxed(key_value.key_value_left,
+		       &device_data->base->key_3_l);
+		writel_relaxed(key_value.key_value_right,
+		       &device_data->base->key_3_r);
+		break;
+	case CRYP_KEY_REG_4:
+		writel_relaxed(key_value.key_value_left,
+		       &device_data->base->key_4_l);
+		writel_relaxed(key_value.key_value_right,
+		       &device_data->base->key_4_r);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+
+}
+
+/**
+ * cryp_configure_init_vector - configures the initialization vector register
+ * @device_data: Pointer to the device data struct for base address.
+ * @init_vector_index: Specifies the index of the init vector.
+ * @init_vector_value: Specifies the value for the init vector.
+ */
+int cryp_configure_init_vector(struct cryp_device_data *device_data,
+			       enum cryp_init_vector_index
+			       init_vector_index,
+			       struct cryp_init_vector_value
+			       init_vector_value)
+{
+	while (cryp_is_logic_busy(device_data))
+		cpu_relax();
+
+	switch (init_vector_index) {
+	case CRYP_INIT_VECTOR_INDEX_0:
+		writel_relaxed(init_vector_value.init_value_left,
+		       &device_data->base->init_vect_0_l);
+		writel_relaxed(init_vector_value.init_value_right,
+		       &device_data->base->init_vect_0_r);
+		break;
+	case CRYP_INIT_VECTOR_INDEX_1:
+		writel_relaxed(init_vector_value.init_value_left,
+		       &device_data->base->init_vect_1_l);
+		writel_relaxed(init_vector_value.init_value_right,
+		       &device_data->base->init_vect_1_r);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * cryp_save_device_context -	Store hardware registers and
+ *				other device context parameter
+ * @device_data: Pointer to the device data struct for base address.
+ * @ctx: Crypto device context
+ */
+void cryp_save_device_context(struct cryp_device_data *device_data,
+			      struct cryp_device_context *ctx,
+			      int cryp_mode)
+{
+	enum cryp_algo_mode algomode;
+	struct cryp_register *src_reg = device_data->base;
+	struct cryp_config *config =
+		(struct cryp_config *)device_data->current_ctx;
+
+	/*
+	 * Always start by disable the hardware and wait for it to finish the
+	 * ongoing calculations before trying to reprogram it.
+	 */
+	cryp_activity(device_data, CRYP_CRYPEN_DISABLE);
+	cryp_wait_until_done(device_data);
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		cryp_configure_for_dma(device_data, CRYP_DMA_DISABLE_BOTH);
+
+	if (CRYP_TEST_BITS(&src_reg->sr, CRYP_SR_IFEM_MASK) == 0)
+		ctx->din = readl_relaxed(&src_reg->din);
+
+	ctx->cr = readl_relaxed(&src_reg->cr) & CRYP_CR_CONTEXT_SAVE_MASK;
+
+	switch (config->keysize) {
+	case CRYP_KEY_SIZE_256:
+		ctx->key_4_l = readl_relaxed(&src_reg->key_4_l);
+		ctx->key_4_r = readl_relaxed(&src_reg->key_4_r);
+
+	case CRYP_KEY_SIZE_192:
+		ctx->key_3_l = readl_relaxed(&src_reg->key_3_l);
+		ctx->key_3_r = readl_relaxed(&src_reg->key_3_r);
+
+	case CRYP_KEY_SIZE_128:
+		ctx->key_2_l = readl_relaxed(&src_reg->key_2_l);
+		ctx->key_2_r = readl_relaxed(&src_reg->key_2_r);
+
+	default:
+		ctx->key_1_l = readl_relaxed(&src_reg->key_1_l);
+		ctx->key_1_r = readl_relaxed(&src_reg->key_1_r);
+	}
+
+	/* Save IV for CBC mode for both AES and DES. */
+	algomode = ((ctx->cr & CRYP_CR_ALGOMODE_MASK) >> CRYP_CR_ALGOMODE_POS);
+	if (algomode == CRYP_ALGO_TDES_CBC ||
+	    algomode == CRYP_ALGO_DES_CBC ||
+	    algomode == CRYP_ALGO_AES_CBC) {
+		ctx->init_vect_0_l = readl_relaxed(&src_reg->init_vect_0_l);
+		ctx->init_vect_0_r = readl_relaxed(&src_reg->init_vect_0_r);
+		ctx->init_vect_1_l = readl_relaxed(&src_reg->init_vect_1_l);
+		ctx->init_vect_1_r = readl_relaxed(&src_reg->init_vect_1_r);
+	}
+}
+
+/**
+ * cryp_restore_device_context -	Restore hardware registers and
+ *					other device context parameter
+ * @device_data: Pointer to the device data struct for base address.
+ * @ctx: Crypto device context
+ */
+void cryp_restore_device_context(struct cryp_device_data *device_data,
+				 struct cryp_device_context *ctx)
+{
+	struct cryp_register *reg = device_data->base;
+	struct cryp_config *config =
+		(struct cryp_config *)device_data->current_ctx;
+
+	/*
+	 * Fall through for all items in switch statement. DES is captured in
+	 * the default.
+	 */
+	switch (config->keysize) {
+	case CRYP_KEY_SIZE_256:
+		writel_relaxed(ctx->key_4_l, &reg->key_4_l);
+		writel_relaxed(ctx->key_4_r, &reg->key_4_r);
+
+	case CRYP_KEY_SIZE_192:
+		writel_relaxed(ctx->key_3_l, &reg->key_3_l);
+		writel_relaxed(ctx->key_3_r, &reg->key_3_r);
+
+	case CRYP_KEY_SIZE_128:
+		writel_relaxed(ctx->key_2_l, &reg->key_2_l);
+		writel_relaxed(ctx->key_2_r, &reg->key_2_r);
+
+	default:
+		writel_relaxed(ctx->key_1_l, &reg->key_1_l);
+		writel_relaxed(ctx->key_1_r, &reg->key_1_r);
+	}
+
+	/* Restore IV for CBC mode for AES and DES. */
+	if (config->algomode == CRYP_ALGO_TDES_CBC ||
+	    config->algomode == CRYP_ALGO_DES_CBC ||
+	    config->algomode == CRYP_ALGO_AES_CBC) {
+		writel_relaxed(ctx->init_vect_0_l, &reg->init_vect_0_l);
+		writel_relaxed(ctx->init_vect_0_r, &reg->init_vect_0_r);
+		writel_relaxed(ctx->init_vect_1_l, &reg->init_vect_1_l);
+		writel_relaxed(ctx->init_vect_1_r, &reg->init_vect_1_r);
+	}
+}
+
+/**
+ * cryp_write_indata - This routine writes 32 bit data into the data input
+ *		       register of the cryptography IP.
+ * @device_data: Pointer to the device data struct for base address.
+ * @write_data: Data word to write
+ */
+int cryp_write_indata(struct cryp_device_data *device_data, u32 write_data)
+{
+	writel_relaxed(write_data, &device_data->base->din);
+
+	return 0;
+}
+
+/**
+ * cryp_read_outdata - This routine reads the data from the data output
+ *		       register of the CRYP logic
+ * @device_data: Pointer to the device data struct for base address.
+ * @read_data: Read the data from the output FIFO.
+ */
+int cryp_read_outdata(struct cryp_device_data *device_data, u32 *read_data)
+{
+	*read_data = readl_relaxed(&device_data->base->dout);
+
+	return 0;
+}
diff --git a/drivers/crypto/ux500/cryp/cryp.h b/drivers/crypto/ux500/cryp/cryp.h
new file mode 100644
index 00000000000..df2e25d4671
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp.h
@@ -0,0 +1,308 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef _CRYP_H_
+#define _CRYP_H_
+
+#include <linux/completion.h>
+#include <linux/dmaengine.h>
+#include <linux/klist.h>
+#include <linux/mutex.h>
+
+#define DEV_DBG_NAME "crypX crypX:"
+
+/* CRYP enable/disable */
+enum cryp_crypen {
+	CRYP_CRYPEN_DISABLE = 0,
+	CRYP_CRYPEN_ENABLE = 1
+};
+
+/* CRYP Start Computation enable/disable */
+enum cryp_start {
+	CRYP_START_DISABLE = 0,
+	CRYP_START_ENABLE = 1
+};
+
+/* CRYP Init Signal enable/disable */
+enum cryp_init {
+	CRYP_INIT_DISABLE = 0,
+	CRYP_INIT_ENABLE = 1
+};
+
+/* Cryp State enable/disable */
+enum cryp_state {
+	CRYP_STATE_DISABLE = 0,
+	CRYP_STATE_ENABLE = 1
+};
+
+/* Key preparation bit enable */
+enum cryp_key_prep {
+	KSE_DISABLED = 0,
+	KSE_ENABLED = 1
+};
+
+/* Key size for AES */
+#define	CRYP_KEY_SIZE_128 (0)
+#define	CRYP_KEY_SIZE_192 (1)
+#define	CRYP_KEY_SIZE_256 (2)
+
+/* AES modes */
+enum cryp_algo_mode {
+	CRYP_ALGO_TDES_ECB,
+	CRYP_ALGO_TDES_CBC,
+	CRYP_ALGO_DES_ECB,
+	CRYP_ALGO_DES_CBC,
+	CRYP_ALGO_AES_ECB,
+	CRYP_ALGO_AES_CBC,
+	CRYP_ALGO_AES_CTR,
+	CRYP_ALGO_AES_XTS
+};
+
+/* Cryp Encryption or Decryption */
+enum cryp_algorithm_dir {
+	CRYP_ALGORITHM_ENCRYPT,
+	CRYP_ALGORITHM_DECRYPT
+};
+
+/* Hardware access method */
+enum cryp_mode {
+	CRYP_MODE_POLLING,
+	CRYP_MODE_INTERRUPT,
+	CRYP_MODE_DMA
+};
+
+/**
+ * struct cryp_config -
+ * @keysize: Key size for AES
+ * @algomode: AES modes
+ * @algodir: Cryp Encryption or Decryption
+ *
+ * CRYP configuration structure to be passed to set configuration
+ */
+struct cryp_config {
+	int keysize;
+	enum cryp_algo_mode algomode;
+	enum cryp_algorithm_dir algodir;
+};
+
+/**
+ * struct cryp_protection_config -
+ * @privilege_access: Privileged cryp state enable/disable
+ * @secure_access: Secure cryp state enable/disable
+ *
+ * Protection configuration structure for setting privilage access
+ */
+struct cryp_protection_config {
+	enum cryp_state privilege_access;
+	enum cryp_state secure_access;
+};
+
+/* Cryp status */
+enum cryp_status_id {
+	CRYP_STATUS_BUSY = 0x10,
+	CRYP_STATUS_OUTPUT_FIFO_FULL = 0x08,
+	CRYP_STATUS_OUTPUT_FIFO_NOT_EMPTY = 0x04,
+	CRYP_STATUS_INPUT_FIFO_NOT_FULL = 0x02,
+	CRYP_STATUS_INPUT_FIFO_EMPTY = 0x01
+};
+
+/* Cryp DMA interface */
+enum cryp_dma_req_type {
+	CRYP_DMA_DISABLE_BOTH,
+	CRYP_DMA_ENABLE_IN_DATA,
+	CRYP_DMA_ENABLE_OUT_DATA,
+	CRYP_DMA_ENABLE_BOTH_DIRECTIONS
+};
+
+enum cryp_dma_channel {
+	CRYP_DMA_RX = 0,
+	CRYP_DMA_TX
+};
+
+/* Key registers */
+enum cryp_key_reg_index {
+	CRYP_KEY_REG_1,
+	CRYP_KEY_REG_2,
+	CRYP_KEY_REG_3,
+	CRYP_KEY_REG_4
+};
+
+/* Key register left and right */
+struct cryp_key_value {
+	u32 key_value_left;
+	u32 key_value_right;
+};
+
+/* Cryp Initialization structure */
+enum cryp_init_vector_index {
+	CRYP_INIT_VECTOR_INDEX_0,
+	CRYP_INIT_VECTOR_INDEX_1
+};
+
+/* struct cryp_init_vector_value -
+ * @init_value_left
+ * @init_value_right
+ * */
+struct cryp_init_vector_value {
+	u32 init_value_left;
+	u32 init_value_right;
+};
+
+/**
+ * struct cryp_device_context - structure for a cryp context.
+ * @cr: control register
+ * @dmacr: DMA control register
+ * @imsc: Interrupt mask set/clear register
+ * @key_1_l: Key 1l register
+ * @key_1_r: Key 1r register
+ * @key_2_l: Key 2l register
+ * @key_2_r: Key 2r register
+ * @key_3_l: Key 3l register
+ * @key_3_r: Key 3r register
+ * @key_4_l: Key 4l register
+ * @key_4_r: Key 4r register
+ * @init_vect_0_l: Initialization vector 0l register
+ * @init_vect_0_r: Initialization vector 0r register
+ * @init_vect_1_l: Initialization vector 1l register
+ * @init_vect_1_r: Initialization vector 0r register
+ * @din: Data in register
+ * @dout: Data out register
+ *
+ * CRYP power management specifc structure.
+ */
+struct cryp_device_context {
+	u32 cr;
+	u32 dmacr;
+	u32 imsc;
+
+	u32 key_1_l;
+	u32 key_1_r;
+	u32 key_2_l;
+	u32 key_2_r;
+	u32 key_3_l;
+	u32 key_3_r;
+	u32 key_4_l;
+	u32 key_4_r;
+
+	u32 init_vect_0_l;
+	u32 init_vect_0_r;
+	u32 init_vect_1_l;
+	u32 init_vect_1_r;
+
+	u32 din;
+	u32 dout;
+};
+
+struct cryp_dma {
+	dma_cap_mask_t mask;
+	struct completion cryp_dma_complete;
+	struct dma_chan *chan_cryp2mem;
+	struct dma_chan *chan_mem2cryp;
+	struct stedma40_chan_cfg *cfg_cryp2mem;
+	struct stedma40_chan_cfg *cfg_mem2cryp;
+	int sg_src_len;
+	int sg_dst_len;
+	struct scatterlist *sg_src;
+	struct scatterlist *sg_dst;
+	int nents_src;
+	int nents_dst;
+};
+
+/**
+ * struct cryp_device_data - structure for a cryp device.
+ * @base: Pointer to the hardware base address.
+ * @dev: Pointer to the devices dev structure.
+ * @clk: Pointer to the device's clock control.
+ * @pwr_regulator: Pointer to the device's power control.
+ * @power_status: Current status of the power.
+ * @ctx_lock: Lock for current_ctx.
+ * @current_ctx: Pointer to the currently allocated context.
+ * @list_node: For inclusion into a klist.
+ * @dma: The dma structure holding channel configuration.
+ * @power_state: TRUE = power state on, FALSE = power state off.
+ * @power_state_spinlock: Spinlock for power_state.
+ * @restore_dev_ctx: TRUE = saved ctx, FALSE = no saved ctx.
+ */
+struct cryp_device_data {
+	struct cryp_register __iomem *base;
+	struct device *dev;
+	struct clk *clk;
+	struct ux500_regulator *pwr_regulator;
+	int power_status;
+	struct spinlock ctx_lock;
+	struct cryp_ctx *current_ctx;
+	struct klist_node list_node;
+	struct cryp_dma dma;
+	bool power_state;
+	struct spinlock power_state_spinlock;
+	bool restore_dev_ctx;
+};
+
+void cryp_wait_until_done(struct cryp_device_data *device_data);
+
+/* Initialization functions */
+
+int cryp_check(struct cryp_device_data *device_data);
+
+void cryp_activity(struct cryp_device_data *device_data,
+		   enum cryp_crypen cryp_crypen);
+
+void cryp_flush_inoutfifo(struct cryp_device_data *device_data);
+
+int cryp_set_configuration(struct cryp_device_data *device_data,
+			   struct cryp_config *cryp_config,
+			   u32 *control_register);
+
+void cryp_configure_for_dma(struct cryp_device_data *device_data,
+			    enum cryp_dma_req_type dma_req);
+
+int cryp_configure_key_values(struct cryp_device_data *device_data,
+			      enum cryp_key_reg_index key_reg_index,
+			      struct cryp_key_value key_value);
+
+int cryp_configure_init_vector(struct cryp_device_data *device_data,
+			       enum cryp_init_vector_index
+			       init_vector_index,
+			       struct cryp_init_vector_value
+			       init_vector_value);
+
+int cryp_configure_protection(struct cryp_device_data *device_data,
+			      struct cryp_protection_config *p_protect_config);
+
+/* Power management funtions */
+void cryp_save_device_context(struct cryp_device_data *device_data,
+			      struct cryp_device_context *ctx,
+			      int cryp_mode);
+
+void cryp_restore_device_context(struct cryp_device_data *device_data,
+				 struct cryp_device_context *ctx);
+
+/* Data transfer and status bits. */
+int cryp_is_logic_busy(struct cryp_device_data *device_data);
+
+int cryp_get_status(struct cryp_device_data *device_data);
+
+/**
+ * cryp_write_indata - This routine writes 32 bit data into the data input
+ *		       register of the cryptography IP.
+ * @device_data: Pointer to the device data struct for base address.
+ * @write_data: Data to write.
+ */
+int cryp_write_indata(struct cryp_device_data *device_data, u32 write_data);
+
+/**
+ * cryp_read_outdata - This routine reads the data from the data output
+ *		       register of the CRYP logic
+ * @device_data: Pointer to the device data struct for base address.
+ * @read_data: Read the data from the output FIFO.
+ */
+int cryp_read_outdata(struct cryp_device_data *device_data, u32 *read_data);
+
+#endif /* _CRYP_H_ */
diff --git a/drivers/crypto/ux500/cryp/cryp_core.c b/drivers/crypto/ux500/cryp/cryp_core.c
new file mode 100644
index 00000000000..5893abb57dc
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_core.c
@@ -0,0 +1,2313 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/crypto.h>
+#include <linux/dmaengine.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irqreturn.h>
+#include <linux/klist.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/dbx500-prcmu.h>
+#include <linux/semaphore.h>
+
+#include <crypto/aes.h>
+#include <crypto/algapi.h>
+#include <crypto/ctr.h>
+#include <crypto/des.h>
+#include <crypto/scatterwalk.h>
+
+#include <plat/ste_dma40.h>
+
+#include <mach/crypto-ux500.h>
+#include <mach/hardware.h>
+
+#include "cryp_p.h"
+#include "cryp.h"
+
+#define CRYP_MAX_KEY_SIZE	32
+#define BYTES_PER_WORD		4
+
+static int cryp_mode;
+static atomic_t session_id;
+
+static struct stedma40_chan_cfg *mem_to_engine;
+static struct stedma40_chan_cfg *engine_to_mem;
+
+/**
+ * struct cryp_driver_data - data specific to the driver.
+ *
+ * @device_list: A list of registered devices to choose from.
+ * @device_allocation: A semaphore initialized with number of devices.
+ */
+struct cryp_driver_data {
+	struct klist device_list;
+	struct semaphore device_allocation;
+};
+
+/**
+ * struct cryp_ctx - Crypto context
+ * @config: Crypto mode.
+ * @key[CRYP_MAX_KEY_SIZE]: Key.
+ * @keylen: Length of key.
+ * @iv: Pointer to initialization vector.
+ * @indata: Pointer to indata.
+ * @outdata: Pointer to outdata.
+ * @datalen: Length of indata.
+ * @outlen: Length of outdata.
+ * @blocksize: Size of blocks.
+ * @updated: Updated flag.
+ * @dev_ctx: Device dependent context.
+ * @device: Pointer to the device.
+ */
+struct cryp_ctx {
+	struct cryp_config config;
+	u8 key[CRYP_MAX_KEY_SIZE];
+	u32 keylen;
+	u8 *iv;
+	const u8 *indata;
+	u8 *outdata;
+	u32 datalen;
+	u32 outlen;
+	u32 blocksize;
+	u8 updated;
+	struct cryp_device_context dev_ctx;
+	struct cryp_device_data *device;
+	u32 session_id;
+};
+
+static struct cryp_driver_data driver_data;
+
+/**
+ * uint8p_to_uint32_be - 4*uint8 to uint32 big endian
+ * @in: Data to convert.
+ */
+static inline u32 uint8p_to_uint32_be(u8 *in)
+{
+	return  (u32)in[0]<<24 |
+		((u32)in[1]<<16) |
+		((u32)in[2]<<8) |
+		((u32)in[3]);
+}
+
+/**
+ * swap_bits_in_byte - mirror the bits in a byte
+ * @b: the byte to be mirrored
+ *
+ * The bits are swapped the following way:
+ *  Byte b include bits 0-7, nibble 1 (n1) include bits 0-3 and
+ *  nibble 2 (n2) bits 4-7.
+ *
+ *  Nibble 1 (n1):
+ *  (The "old" (moved) bit is replaced with a zero)
+ *  1. Move bit 6 and 7, 4 positions to the left.
+ *  2. Move bit 3 and 5, 2 positions to the left.
+ *  3. Move bit 1-4, 1 position to the left.
+ *
+ *  Nibble 2 (n2):
+ *  1. Move bit 0 and 1, 4 positions to the right.
+ *  2. Move bit 2 and 4, 2 positions to the right.
+ *  3. Move bit 3-6, 1 position to the right.
+ *
+ *  Combine the two nibbles to a complete and swapped byte.
+ */
+
+static inline u8 swap_bits_in_byte(u8 b)
+{
+#define R_SHIFT_4_MASK  (0xc0) /* Bits 6 and 7, right shift 4 */
+#define R_SHIFT_2_MASK  (0x28) /* (After right shift 4) Bits 3 and 5,
+				  right shift 2 */
+#define R_SHIFT_1_MASK  (0x1e) /* (After right shift 2) Bits 1-4,
+				  right shift 1 */
+#define L_SHIFT_4_MASK  (0x03) /* Bits 0 and 1, left shift 4 */
+#define L_SHIFT_2_MASK  (0x14) /* (After left shift 4) Bits 2 and 4,
+				  left shift 2 */
+#define L_SHIFT_1_MASK  (0x78) /* (After left shift 1) Bits 3-6,
+				  left shift 1 */
+
+	u8 n1;
+	u8 n2;
+
+	/* Swap most significant nibble */
+	/* Right shift 4, bits 6 and 7 */
+	n1 = ((b  & R_SHIFT_4_MASK) >> 4) | (b  & ~(R_SHIFT_4_MASK >> 4));
+	/* Right shift 2, bits 3 and 5 */
+	n1 = ((n1 & R_SHIFT_2_MASK) >> 2) | (n1 & ~(R_SHIFT_2_MASK >> 2));
+	/* Right shift 1, bits 1-4 */
+	n1 = (n1  & R_SHIFT_1_MASK) >> 1;
+
+	/* Swap least significant nibble */
+	/* Left shift 4, bits 0 and 1 */
+	n2 = ((b  & L_SHIFT_4_MASK) << 4) | (b  & ~(L_SHIFT_4_MASK << 4));
+	/* Left shift 2, bits 2 and 4 */
+	n2 = ((n2 & L_SHIFT_2_MASK) << 2) | (n2 & ~(L_SHIFT_2_MASK << 2));
+	/* Left shift 1, bits 3-6 */
+	n2 = (n2  & L_SHIFT_1_MASK) << 1;
+
+	return n1 | n2;
+}
+
+static inline void swap_words_in_key_and_bits_in_byte(const u8 *in,
+						      u8 *out, u32 len)
+{
+	unsigned int i = 0;
+	int j;
+	int index = 0;
+
+	j = len - BYTES_PER_WORD;
+	while (j >= 0) {
+		for (i = 0; i < BYTES_PER_WORD; i++) {
+			index = len - j - BYTES_PER_WORD + i;
+			out[j + i] =
+				swap_bits_in_byte(in[index]);
+		}
+		j -= BYTES_PER_WORD;
+	}
+}
+
+static void add_session_id(struct cryp_ctx *ctx)
+{
+	/*
+	 * We never want 0 to be a valid value, since this is the default value
+	 * for the software context.
+	 */
+	if (unlikely(atomic_inc_and_test(&session_id)))
+		atomic_inc(&session_id);
+
+	ctx->session_id = atomic_read(&session_id);
+}
+
+static irqreturn_t cryp_interrupt_handler(int irq, void *param)
+{
+	struct cryp_ctx *ctx;
+	int i;
+	struct cryp_device_data *device_data;
+
+	if (param == NULL) {
+		BUG_ON(!param);
+		return IRQ_HANDLED;
+	}
+
+	/* The device is coming from the one found in hw_crypt_noxts. */
+	device_data = (struct cryp_device_data *)param;
+
+	ctx = device_data->current_ctx;
+
+	if (ctx == NULL) {
+		BUG_ON(!ctx);
+		return IRQ_HANDLED;
+	}
+
+	dev_dbg(ctx->device->dev, "[%s] (len: %d) %s, ", __func__, ctx->outlen,
+		cryp_pending_irq_src(device_data, CRYP_IRQ_SRC_OUTPUT_FIFO) ?
+		"out" : "in");
+
+	if (cryp_pending_irq_src(device_data,
+				 CRYP_IRQ_SRC_OUTPUT_FIFO)) {
+		if (ctx->outlen / ctx->blocksize > 0) {
+			for (i = 0; i < ctx->blocksize / 4; i++) {
+				cryp_read_outdata(device_data,
+						 (u32 *)ctx->outdata);
+				ctx->outdata += 4;
+				ctx->outlen -= 4;
+			}
+
+			if (ctx->outlen == 0) {
+				cryp_disable_irq_src(device_data,
+						     CRYP_IRQ_SRC_OUTPUT_FIFO);
+			}
+		}
+	} else if (cryp_pending_irq_src(device_data,
+					CRYP_IRQ_SRC_INPUT_FIFO)) {
+		if (ctx->datalen / ctx->blocksize > 0) {
+			for (i = 0 ; i < ctx->blocksize / 4; i++) {
+				cryp_write_indata(device_data,
+						 *((u32 *)ctx->indata));
+				ctx->indata += 4;
+				ctx->datalen -= 4;
+			}
+
+			if (ctx->datalen == 0)
+				cryp_disable_irq_src(device_data,
+						   CRYP_IRQ_SRC_INPUT_FIFO);
+
+			if (ctx->config.algomode == CRYP_ALGO_AES_XTS) {
+				CRYP_PUT_BITS(&device_data->base->cr,
+					      CRYP_START_ENABLE,
+					      CRYP_CR_START_POS,
+					      CRYP_CR_START_MASK);
+
+				cryp_wait_until_done(device_data);
+			}
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int mode_is_aes(enum cryp_algo_mode mode)
+{
+	return	(CRYP_ALGO_AES_ECB == mode) ||
+		(CRYP_ALGO_AES_CBC == mode) ||
+		(CRYP_ALGO_AES_CTR == mode) ||
+		(CRYP_ALGO_AES_XTS == mode);
+}
+
+static int cfg_iv(struct cryp_device_data *device_data, u32 left, u32 right,
+		  enum cryp_init_vector_index index)
+{
+	struct cryp_init_vector_value vector_value;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	vector_value.init_value_left = left;
+	vector_value.init_value_right = right;
+
+	return cryp_configure_init_vector(device_data,
+					  index,
+					  vector_value);
+}
+
+static int cfg_ivs(struct cryp_device_data *device_data, struct cryp_ctx *ctx)
+{
+	int i;
+	int status = 0;
+	int num_of_regs = ctx->blocksize / 8;
+	u32 iv[AES_BLOCK_SIZE / 4];
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	/*
+	 * Since we loop on num_of_regs we need to have a check in case
+	 * someone provides an incorrect blocksize which would force calling
+	 * cfg_iv with i greater than 2 which is an error.
+	 */
+	if (num_of_regs > 2) {
+		dev_err(device_data->dev, "[%s] Incorrect blocksize %d",
+			__func__, ctx->blocksize);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < ctx->blocksize / 4; i++)
+		iv[i] = uint8p_to_uint32_be(ctx->iv + i*4);
+
+	for (i = 0; i < num_of_regs; i++) {
+		status = cfg_iv(device_data, iv[i*2], iv[i*2+1],
+				(enum cryp_init_vector_index) i);
+		if (status != 0)
+			return status;
+	}
+	return status;
+}
+
+static int set_key(struct cryp_device_data *device_data,
+		   u32 left_key,
+		   u32 right_key,
+		   enum cryp_key_reg_index index)
+{
+	struct cryp_key_value key_value;
+	int cryp_error;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	key_value.key_value_left = left_key;
+	key_value.key_value_right = right_key;
+
+	cryp_error = cryp_configure_key_values(device_data,
+					       index,
+					       key_value);
+	if (cryp_error != 0)
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_configure_key_values() failed!", __func__);
+
+	return cryp_error;
+}
+
+static int cfg_keys(struct cryp_ctx *ctx)
+{
+	int i;
+	int num_of_regs = ctx->keylen / 8;
+	u32 swapped_key[CRYP_MAX_KEY_SIZE / 4];
+	int cryp_error = 0;
+
+	dev_dbg(ctx->device->dev, "[%s]", __func__);
+
+	if (mode_is_aes(ctx->config.algomode)) {
+		swap_words_in_key_and_bits_in_byte((u8 *)ctx->key,
+						   (u8 *)swapped_key,
+						   ctx->keylen);
+	} else {
+		for (i = 0; i < ctx->keylen / 4; i++)
+			swapped_key[i] = uint8p_to_uint32_be(ctx->key + i*4);
+	}
+
+	for (i = 0; i < num_of_regs; i++) {
+		cryp_error = set_key(ctx->device,
+				     *(((u32 *)swapped_key)+i*2),
+				     *(((u32 *)swapped_key)+i*2+1),
+				     (enum cryp_key_reg_index) i);
+
+		if (cryp_error != 0) {
+			dev_err(ctx->device->dev, "[%s]: set_key() failed!",
+					__func__);
+			return cryp_error;
+		}
+	}
+	return cryp_error;
+}
+
+static int cryp_setup_context(struct cryp_ctx *ctx,
+			      struct cryp_device_data *device_data)
+{
+	u32 control_register = CRYP_CR_DEFAULT;
+
+	switch (cryp_mode) {
+	case CRYP_MODE_INTERRUPT:
+		writel_relaxed(CRYP_IMSC_DEFAULT, &device_data->base->imsc);
+		break;
+
+	case CRYP_MODE_DMA:
+		writel_relaxed(CRYP_DMACR_DEFAULT, &device_data->base->dmacr);
+		break;
+
+	default:
+		break;
+	}
+
+	if (ctx->updated == 0) {
+		cryp_flush_inoutfifo(device_data);
+		if (cfg_keys(ctx) != 0) {
+			dev_err(ctx->device->dev, "[%s]: cfg_keys failed!",
+				__func__);
+			return -EPERM;
+		}
+
+		if ((ctx->iv) &&
+		    (CRYP_ALGO_AES_ECB != ctx->config.algomode) &&
+		    (CRYP_ALGO_DES_ECB != ctx->config.algomode) &&
+		    (CRYP_ALGO_TDES_ECB != ctx->config.algomode)) {
+			if (cfg_ivs(device_data, ctx) != 0)
+				return -EPERM;
+		}
+
+		cryp_set_configuration(device_data, &ctx->config,
+				       &control_register);
+		add_session_id(ctx);
+	} else if (ctx->updated == 1 &&
+		   ctx->session_id != atomic_read(&session_id)) {
+		cryp_flush_inoutfifo(device_data);
+		cryp_restore_device_context(device_data, &ctx->dev_ctx);
+
+		add_session_id(ctx);
+		control_register = ctx->dev_ctx.cr;
+	} else
+		control_register = ctx->dev_ctx.cr;
+
+	writel(control_register |
+	       (CRYP_CRYPEN_ENABLE << CRYP_CR_CRYPEN_POS),
+	       &device_data->base->cr);
+
+	return 0;
+}
+
+static int cryp_get_device_data(struct cryp_ctx *ctx,
+				struct cryp_device_data **device_data)
+{
+	int ret;
+	struct klist_iter device_iterator;
+	struct klist_node *device_node;
+	struct cryp_device_data *local_device_data = NULL;
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	/* Wait until a device is available */
+	ret = down_interruptible(&driver_data.device_allocation);
+	if (ret)
+		return ret;  /* Interrupted */
+
+	/* Select a device */
+	klist_iter_init(&driver_data.device_list, &device_iterator);
+
+	device_node = klist_next(&device_iterator);
+	while (device_node) {
+		local_device_data = container_of(device_node,
+					   struct cryp_device_data, list_node);
+		spin_lock(&local_device_data->ctx_lock);
+		/* current_ctx allocates a device, NULL = unallocated */
+		if (local_device_data->current_ctx) {
+			device_node = klist_next(&device_iterator);
+		} else {
+			local_device_data->current_ctx = ctx;
+			ctx->device = local_device_data;
+			spin_unlock(&local_device_data->ctx_lock);
+			break;
+		}
+		spin_unlock(&local_device_data->ctx_lock);
+	}
+	klist_iter_exit(&device_iterator);
+
+	if (!device_node) {
+		/**
+		 * No free device found.
+		 * Since we allocated a device with down_interruptible, this
+		 * should not be able to happen.
+		 * Number of available devices, which are contained in
+		 * device_allocation, is therefore decremented by not doing
+		 * an up(device_allocation).
+		 */
+		return -EBUSY;
+	}
+
+	*device_data = local_device_data;
+
+	return 0;
+}
+
+static void cryp_dma_setup_channel(struct cryp_device_data *device_data,
+				   struct device *dev)
+{
+	dma_cap_zero(device_data->dma.mask);
+	dma_cap_set(DMA_SLAVE, device_data->dma.mask);
+
+	device_data->dma.cfg_mem2cryp = mem_to_engine;
+	device_data->dma.chan_mem2cryp =
+		dma_request_channel(device_data->dma.mask,
+				    stedma40_filter,
+				    device_data->dma.cfg_mem2cryp);
+
+	device_data->dma.cfg_cryp2mem = engine_to_mem;
+	device_data->dma.chan_cryp2mem =
+		dma_request_channel(device_data->dma.mask,
+				    stedma40_filter,
+				    device_data->dma.cfg_cryp2mem);
+
+	init_completion(&device_data->dma.cryp_dma_complete);
+}
+
+static void cryp_dma_out_callback(void *data)
+{
+	struct cryp_ctx *ctx = (struct cryp_ctx *) data;
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	complete(&ctx->device->dma.cryp_dma_complete);
+}
+
+static int cryp_set_dma_transfer(struct cryp_ctx *ctx,
+				 struct scatterlist *sg,
+				 int len,
+				 enum dma_data_direction direction)
+{
+	struct dma_async_tx_descriptor *desc;
+	struct dma_chan *channel = NULL;
+	dma_cookie_t cookie;
+
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	if (unlikely(!IS_ALIGNED((u32)sg, 4))) {
+		dev_err(ctx->device->dev, "[%s]: Data in sg list isn't "
+			"aligned! Addr: 0x%08x", __func__, (u32)sg);
+		return -EFAULT;
+	}
+
+	switch (direction) {
+	case DMA_TO_DEVICE:
+		channel = ctx->device->dma.chan_mem2cryp;
+		ctx->device->dma.sg_src = sg;
+		ctx->device->dma.sg_src_len = dma_map_sg(channel->device->dev,
+						 ctx->device->dma.sg_src,
+						 ctx->device->dma.nents_src,
+						 direction);
+
+		if (!ctx->device->dma.sg_src_len) {
+			dev_dbg(ctx->device->dev,
+				"[%s]: Could not map the sg list (TO_DEVICE)",
+				__func__);
+			return -EFAULT;
+		}
+
+		dev_dbg(ctx->device->dev, "[%s]: Setting up DMA for buffer "
+			"(TO_DEVICE)", __func__);
+
+		desc = channel->device->device_prep_slave_sg(channel,
+					     ctx->device->dma.sg_src,
+					     ctx->device->dma.sg_src_len,
+					     direction,
+					     DMA_CTRL_ACK);
+		break;
+
+	case DMA_FROM_DEVICE:
+		channel = ctx->device->dma.chan_cryp2mem;
+		ctx->device->dma.sg_dst = sg;
+		ctx->device->dma.sg_dst_len = dma_map_sg(channel->device->dev,
+						 ctx->device->dma.sg_dst,
+						 ctx->device->dma.nents_dst,
+						 direction);
+
+		if (!ctx->device->dma.sg_dst_len) {
+			dev_dbg(ctx->device->dev,
+				"[%s]: Could not map the sg list "
+				"(FROM_DEVICE)", __func__);
+			return -EFAULT;
+		}
+
+		dev_dbg(ctx->device->dev, "[%s]: Setting up DMA for buffer "
+			"(FROM_DEVICE)", __func__);
+
+		desc = channel->device->device_prep_slave_sg(channel,
+					     ctx->device->dma.sg_dst,
+					     ctx->device->dma.sg_dst_len,
+					     direction,
+					     DMA_CTRL_ACK |
+					     DMA_PREP_INTERRUPT);
+
+		desc->callback = cryp_dma_out_callback;
+		desc->callback_param = ctx;
+		break;
+
+	default:
+		dev_dbg(ctx->device->dev, "[%s]: Invalid DMA direction",
+			__func__);
+		return -EFAULT;
+	}
+
+	cookie = desc->tx_submit(desc);
+	dma_async_issue_pending(channel);
+
+	return 0;
+}
+
+static void cryp_dma_done(struct cryp_ctx *ctx)
+{
+	struct dma_chan *chan;
+
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	chan = ctx->device->dma.chan_mem2cryp;
+	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dma_unmap_sg(chan->device->dev, ctx->device->dma.sg_src,
+		     ctx->device->dma.sg_src_len, DMA_TO_DEVICE);
+
+	chan = ctx->device->dma.chan_cryp2mem;
+	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dma_unmap_sg(chan->device->dev, ctx->device->dma.sg_dst,
+		     ctx->device->dma.sg_dst_len, DMA_FROM_DEVICE);
+}
+
+static int cryp_dma_write(struct cryp_ctx *ctx, struct scatterlist *sg,
+			  int len)
+{
+	int error = cryp_set_dma_transfer(ctx, sg, len, DMA_TO_DEVICE);
+	dev_dbg(ctx->device->dev, "[%s]: ", __func__);
+
+	if (error) {
+		dev_dbg(ctx->device->dev, "[%s]: cryp_set_dma_transfer() "
+			"failed", __func__);
+		return error;
+	}
+
+	return len;
+}
+
+static int cryp_dma_read(struct cryp_ctx *ctx, struct scatterlist *sg, int len)
+{
+	int error = cryp_set_dma_transfer(ctx, sg, len, DMA_FROM_DEVICE);
+	if (error) {
+		dev_dbg(ctx->device->dev, "[%s]: cryp_set_dma_transfer() "
+			"failed", __func__);
+		return error;
+	}
+
+	return len;
+}
+
+static void cryp_polling_mode(struct cryp_ctx *ctx,
+			      struct cryp_device_data *device_data)
+{
+	int len = ctx->blocksize / BYTES_PER_WORD;
+	int remaining_length = ctx->datalen;
+	u32 *indata = (u32 *)ctx->indata;
+	u32 *outdata = (u32 *)ctx->outdata;
+
+	while (remaining_length > 0) {
+		writesl(&device_data->base->din, indata, len);
+		indata += len;
+		remaining_length -= (len * BYTES_PER_WORD);
+		cryp_wait_until_done(device_data);
+
+		readsl(&device_data->base->dout, outdata, len);
+		outdata += len;
+		cryp_wait_until_done(device_data);
+	}
+}
+
+static int cryp_disable_power(struct device *dev,
+			      struct cryp_device_data *device_data,
+			      bool save_device_context)
+{
+	int ret = 0;
+
+	dev_dbg(dev, "[%s]", __func__);
+
+	spin_lock(&device_data->power_state_spinlock);
+	if (!device_data->power_state)
+		goto out;
+
+	spin_lock(&device_data->ctx_lock);
+	if (save_device_context && device_data->current_ctx) {
+		cryp_save_device_context(device_data,
+				&device_data->current_ctx->dev_ctx,
+				cryp_mode);
+		device_data->restore_dev_ctx = true;
+	}
+	spin_unlock(&device_data->ctx_lock);
+
+	clk_disable(device_data->clk);
+	ret = ux500_regulator_atomic_disable(device_data->pwr_regulator);
+	if (ret)
+		dev_err(dev, "[%s]: "
+				"regulator_disable() failed!",
+				__func__);
+
+	device_data->power_state = false;
+
+out:
+	spin_unlock(&device_data->power_state_spinlock);
+
+	return ret;
+}
+
+static int cryp_enable_power(
+		struct device *dev,
+		struct cryp_device_data *device_data,
+		bool restore_device_context)
+{
+	int ret = 0;
+
+	dev_dbg(dev, "[%s]", __func__);
+
+	spin_lock(&device_data->power_state_spinlock);
+	if (!device_data->power_state) {
+		ret = ux500_regulator_atomic_enable(device_data->pwr_regulator);
+		if (ret) {
+			dev_err(dev, "[%s]: regulator_enable() failed!",
+					__func__);
+			goto out;
+		}
+
+		ret = clk_enable(device_data->clk);
+		if (ret) {
+			dev_err(dev, "[%s]: clk_enable() failed!",
+					__func__);
+			ux500_regulator_atomic_disable(
+					       device_data->pwr_regulator);
+			goto out;
+		}
+		device_data->power_state = true;
+	}
+
+	if (device_data->restore_dev_ctx) {
+		spin_lock(&device_data->ctx_lock);
+		if (restore_device_context && device_data->current_ctx) {
+			device_data->restore_dev_ctx = false;
+			cryp_restore_device_context(device_data,
+					&device_data->current_ctx->dev_ctx);
+		}
+		spin_unlock(&device_data->ctx_lock);
+	}
+out:
+	spin_unlock(&device_data->power_state_spinlock);
+
+	return ret;
+}
+
+static int hw_crypt_noxts(struct cryp_ctx *ctx,
+			  struct cryp_device_data *device_data)
+{
+	int ret = 0;
+
+	const u8 *indata = ctx->indata;
+	u8 *outdata = ctx->outdata;
+	u32 datalen = ctx->datalen;
+	u32 outlen = datalen;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->outlen = ctx->datalen;
+
+	if (unlikely(!IS_ALIGNED((u32)indata, 4))) {
+		pr_debug(DEV_DBG_NAME " [%s]: Data isn't aligned! Addr: "
+			 "0x%08x", __func__, (u32)indata);
+		return -EINVAL;
+	}
+
+	ret = cryp_setup_context(ctx, device_data);
+
+	if (ret)
+		goto out;
+
+	if (cryp_mode == CRYP_MODE_INTERRUPT) {
+		cryp_enable_irq_src(device_data, CRYP_IRQ_SRC_INPUT_FIFO |
+				    CRYP_IRQ_SRC_OUTPUT_FIFO);
+
+		/*
+		 * ctx->outlen is decremented in the cryp_interrupt_handler
+		 * function. We had to add cpu_relax() (barrier) to make sure
+		 * that gcc didn't optimze away this variable.
+		 */
+		while (ctx->outlen > 0)
+			cpu_relax();
+	} else if (cryp_mode == CRYP_MODE_POLLING ||
+		   cryp_mode == CRYP_MODE_DMA) {
+		/*
+		 * The reason for having DMA in this if case is that if we are
+		 * running cryp_mode = 2, then we separate DMA routines for
+		 * handling cipher/plaintext > blocksize, except when
+		 * running the normal CRYPTO_ALG_TYPE_CIPHER, then we still use
+		 * the polling mode. Overhead of doing DMA setup eats up the
+		 * benefits using it.
+		 */
+		cryp_polling_mode(ctx, device_data);
+	} else {
+		dev_err(ctx->device->dev, "[%s]: Invalid operation mode!",
+			__func__);
+		ret = -EPERM;
+		goto out;
+	}
+
+	cryp_save_device_context(device_data, &ctx->dev_ctx, cryp_mode);
+	ctx->updated = 1;
+
+out:
+	ctx->indata = indata;
+	ctx->outdata = outdata;
+	ctx->datalen = datalen;
+	ctx->outlen = outlen;
+
+	return ret;
+}
+
+static int get_nents(struct scatterlist *sg, int nbytes)
+{
+	int nents = 0;
+
+	while (nbytes > 0) {
+		nbytes -= sg->length;
+		sg = scatterwalk_sg_next(sg);
+		nents++;
+	}
+
+	return nents;
+}
+
+static int ablk_dma_crypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	struct cryp_device_data *device_data;
+
+	int bytes_written = 0;
+	int bytes_read = 0;
+	int ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->datalen = areq->nbytes;
+	ctx->outlen = areq->nbytes;
+
+	ret = cryp_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	ret = cryp_enable_power(device_data->dev, device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	ret = cryp_setup_context(ctx, device_data);
+	if (ret)
+		goto out_power;
+
+	/* We have the device now, so store the nents in the dma struct. */
+	ctx->device->dma.nents_src = get_nents(areq->src, ctx->datalen);
+	ctx->device->dma.nents_dst = get_nents(areq->dst, ctx->outlen);
+
+	/* Enable DMA in- and output. */
+	cryp_configure_for_dma(device_data, CRYP_DMA_ENABLE_BOTH_DIRECTIONS);
+
+	bytes_written = cryp_dma_write(ctx, areq->src, ctx->datalen);
+	bytes_read = cryp_dma_read(ctx, areq->dst, bytes_written);
+
+	wait_for_completion(&ctx->device->dma.cryp_dma_complete);
+	cryp_dma_done(ctx);
+
+	cryp_save_device_context(device_data, &ctx->dev_ctx, cryp_mode);
+	ctx->updated = 1;
+
+out_power:
+	if (cryp_disable_power(device_data->dev, device_data, false))
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_disable_power() failed!", __func__);
+
+out:
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx = NULL;
+	ctx->device = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+
+	if (unlikely(bytes_written != bytes_read))
+		return -EPERM;
+
+	return 0;
+}
+
+static int ablk_crypt(struct ablkcipher_request *areq)
+{
+	struct ablkcipher_walk walk;
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	struct cryp_device_data *device_data;
+	unsigned long src_paddr;
+	unsigned long dst_paddr;
+	int ret;
+	int nbytes;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ret = cryp_get_device_data(ctx, &device_data);
+	if (ret)
+		goto out;
+
+	ret = cryp_enable_power(device_data->dev, device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_enable_power() failed!", __func__);
+		goto out_power;
+	}
+
+	ablkcipher_walk_init(&walk, areq->dst, areq->src, areq->nbytes);
+	ret = ablkcipher_walk_phys(areq, &walk);
+
+	if (ret) {
+		pr_err(DEV_DBG_NAME "[%s]: ablkcipher_walk_phys() failed!",
+			__func__);
+		goto out_power;
+	}
+
+	while ((nbytes = walk.nbytes) > 0) {
+		ctx->iv = walk.iv;
+		src_paddr = (page_to_phys(walk.src.page) + walk.src.offset);
+		ctx->indata = phys_to_virt(src_paddr);
+
+		dst_paddr = (page_to_phys(walk.dst.page) + walk.dst.offset);
+		ctx->outdata = phys_to_virt(dst_paddr);
+
+		ctx->datalen = nbytes - (nbytes % ctx->blocksize);
+
+		ret = hw_crypt_noxts(ctx, device_data);
+		if (ret)
+			goto out_power;
+
+		nbytes -= ctx->datalen;
+		ret = ablkcipher_walk_done(areq, &walk, nbytes);
+		if (ret)
+			goto out_power;
+	}
+	ablkcipher_walk_complete(&walk);
+
+out_power:
+	if (cryp_disable_power(device_data->dev, device_data, false))
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_disable_power() failed!", __func__);
+out:
+	/* Release the device */
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx = NULL;
+	ctx->device = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+
+	return ret;
+}
+
+static int aes_ablkcipher_setkey(struct crypto_ablkcipher *cipher,
+				 const u8 *key, unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	switch (keylen) {
+	case AES_KEYSIZE_128:
+		ctx->config.keysize = CRYP_KEY_SIZE_128;
+		break;
+
+	case AES_KEYSIZE_192:
+		ctx->config.keysize = CRYP_KEY_SIZE_192;
+		break;
+
+	case AES_KEYSIZE_256:
+		ctx->config.keysize = CRYP_KEY_SIZE_256;
+		break;
+
+	default:
+		pr_err(DEV_DBG_NAME "[%s]: Unknown keylen!", __func__);
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+
+	return 0;
+}
+
+static int aes_setkey(struct crypto_tfm *tfm, const u8 *key,
+		      unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 *flags = &tfm->crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	if (unlikely(!IS_ALIGNED((u32)key, 4))) {
+		dev_err(ctx->device->dev, "[%s]: key isn't aligned! Addr: "
+			"0x%08x", __func__, (u32)key);
+		return -EFAULT;
+	}
+
+	/* For CTR mode */
+	if (keylen != AES_KEYSIZE_128 &&
+	    keylen != AES_KEYSIZE_192 &&
+	    keylen != AES_KEYSIZE_256) {
+
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s] invalid keylen", __func__);
+		return -EINVAL;
+	}
+
+	if (keylen == AES_KEYSIZE_128)
+		ctx->config.keysize = CRYP_KEY_SIZE_128;
+	else if (keylen == AES_KEYSIZE_192)
+		ctx->config.keysize = CRYP_KEY_SIZE_192;
+	else if (keylen == AES_KEYSIZE_256)
+		ctx->config.keysize = CRYP_KEY_SIZE_256;
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int des_ablkcipher_setkey(struct crypto_ablkcipher *cipher,
+				 const u8 *key, unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+	u32 tmp[DES_EXPKEY_WORDS];
+	int ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+	if (keylen != DES_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+				__func__);
+		return -EINVAL;
+	}
+
+	ret = des_ekey(tmp, key);
+	if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+				__func__);
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int des_setkey(struct crypto_tfm *tfm, const u8 *key,
+		      unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 *flags = &tfm->crt_flags;
+	int ret;
+	u32 tmp[DES_EXPKEY_WORDS];
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	if (keylen != DES_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+			__func__);
+		return -EINVAL;
+	}
+
+	ret = des_ekey(tmp, key);
+	if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+			__func__);
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int des3_ablkcipher_setkey(struct crypto_ablkcipher *cipher,
+				  const u8 *key, unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+	const u32 *K = (const u32 *)key;
+	u32 tmp[DES3_EDE_EXPKEY_WORDS];
+	int i, ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+	if (keylen != DES3_EDE_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+				__func__);
+		return -EINVAL;
+	}
+
+	/* Checking key interdependency for weak key detection. */
+	if (unlikely(!((K[0] ^ K[2]) | (K[1] ^ K[3])) ||
+				!((K[2] ^ K[4]) | (K[3] ^ K[5]))) &&
+			(*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+				__func__);
+		return -EINVAL;
+	}
+	for (i = 0; i < 3; i++) {
+		ret = des_ekey(tmp, key + i*DES_KEY_SIZE);
+		if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+			*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+			pr_debug(DEV_DBG_NAME " [%s]: "
+					"CRYPTO_TFM_REQ_WEAK_KEY", __func__);
+			return -EINVAL;
+		}
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int des3_setkey(struct crypto_tfm *tfm, const u8 *key,
+		       unsigned int keylen)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 *flags = &tfm->crt_flags;
+	const u32 *K = (const u32 *)key;
+	u32 tmp[DES3_EDE_EXPKEY_WORDS];
+	int i, ret;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	if (keylen != DES3_EDE_KEY_SIZE) {
+		*flags |= CRYPTO_TFM_RES_BAD_KEY_LEN;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_RES_BAD_KEY_LEN",
+				__func__);
+		return -EINVAL;
+	}
+
+	if (unlikely(!((K[0] ^ K[2]) | (K[1] ^ K[3])) ||
+		     !((K[2] ^ K[4]) | (K[3] ^ K[5]))) &&
+	    (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+		*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+		pr_debug(DEV_DBG_NAME " [%s]: CRYPTO_TFM_REQ_WEAK_KEY",
+			__func__);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < 3; i++) {
+		ret = des_ekey(tmp, key + i*DES_KEY_SIZE);
+		if (unlikely(ret == 0) && (*flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+			*flags |= CRYPTO_TFM_RES_WEAK_KEY;
+			pr_debug(DEV_DBG_NAME " [%s]: "
+				 "CRYPTO_TFM_REQ_WEAK_KEY", __func__);
+			return -EINVAL;
+		}
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	ctx->updated = 0;
+	return 0;
+}
+
+static int cryp_hw_calculate(struct cryp_ctx *ctx)
+{
+	struct cryp_device_data *device_data;
+	int ret;
+
+	ret = cryp_get_device_data(ctx, &device_data);
+	if (ret)
+		goto out;
+
+	ret = cryp_enable_power(device_data->dev, device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	if (hw_crypt_noxts(ctx, device_data))
+		dev_err(device_data->dev, "[%s]: hw_crypt_noxts() failed!",
+			__func__);
+
+out:
+	if (cryp_disable_power(device_data->dev, device_data, false))
+		dev_err(device_data->dev, "[%s]: "
+			"cryp_disable_power() failed!", __func__);
+
+	/* Release the device */
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx = NULL;
+	ctx->device = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+
+	return ret;
+}
+
+static void aes_encrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("ux500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void aes_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("ux500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void des_encrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("ux500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void des_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("ux500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void des3_encrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("ux500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static void des3_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
+{
+	struct cryp_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->blocksize = crypto_tfm_alg_blocksize(tfm);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_ECB;
+
+	ctx->indata = in;
+	ctx->outdata = out;
+	ctx->datalen = ctx->blocksize;
+
+	if (cryp_hw_calculate(ctx))
+		pr_err("ux500_cryp:crypX: [%s]: cryp_hw_calculate() failed!",
+				__func__);
+}
+
+static int aes_ecb_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_ECB;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_ecb_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_ECB;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_cbc_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_CBC;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	/* Only DMA for ablkcipher, since givcipher not yet supported */
+	if ((cryp_mode == CRYP_MODE_DMA) &&
+			(*flags & CRYPTO_ALG_TYPE_ABLKCIPHER))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_cbc_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_CBC;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	/* Only DMA for ablkcipher, since givcipher not yet supported */
+	if ((cryp_mode == CRYP_MODE_DMA) &&
+			(*flags & CRYPTO_ALG_TYPE_ABLKCIPHER))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_ctr_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_CTR;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	/* Only DMA for ablkcipher, since givcipher not yet supported */
+	if ((cryp_mode == CRYP_MODE_DMA) &&
+			(*flags & CRYPTO_ALG_TYPE_ABLKCIPHER))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int aes_ctr_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 *flags = &cipher->base.crt_flags;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_AES_CTR;
+	ctx->blocksize = AES_BLOCK_SIZE;
+
+	/* Only DMA for ablkcipher, since givcipher not yet supported */
+	if ((cryp_mode == CRYP_MODE_DMA) &&
+			(*flags & CRYPTO_ALG_TYPE_ABLKCIPHER))
+		return ablk_dma_crypt(areq);
+
+	/* For everything except DMA, we run the non DMA version. */
+	return ablk_crypt(areq);
+}
+
+static int des_ecb_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_ECB;
+	ctx->blocksize = DES_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des_ecb_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_ECB;
+	ctx->blocksize = DES_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des_cbc_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_CBC;
+	ctx->blocksize = DES_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des_cbc_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_DES_CBC;
+	ctx->blocksize = DES_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des3_ecb_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_ECB;
+	ctx->blocksize = DES3_EDE_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des3_ecb_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_ECB;
+	ctx->blocksize = DES3_EDE_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des3_cbc_encrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_ENCRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_CBC;
+	ctx->blocksize = DES3_EDE_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+static int des3_cbc_decrypt(struct ablkcipher_request *areq)
+{
+	struct crypto_ablkcipher *cipher = crypto_ablkcipher_reqtfm(areq);
+	struct cryp_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	ctx->config.algodir = CRYP_ALGORITHM_DECRYPT;
+	ctx->config.algomode = CRYP_ALGO_TDES_CBC;
+	ctx->blocksize = DES3_EDE_BLOCK_SIZE;
+
+	/*
+	 * Run the non DMA version also for DMA, since DMA is currently not
+	 * working for DES.
+	 */
+	return ablk_crypt(areq);
+}
+
+/**
+ * struct crypto_alg aes_alg
+ */
+static struct crypto_alg aes_alg = {
+	.cra_name		=	"aes",
+	.cra_driver_name	=	"aes-ux500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_CIPHER,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(aes_alg.cra_list),
+	.cra_u			=	{
+		.cipher = {
+			.cia_min_keysize	=	AES_MIN_KEY_SIZE,
+			.cia_max_keysize	=	AES_MAX_KEY_SIZE,
+			.cia_setkey		=	aes_setkey,
+			.cia_encrypt		=	aes_encrypt,
+			.cia_decrypt		=	aes_decrypt
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des_alg
+ */
+static struct crypto_alg des_alg = {
+	.cra_name		=	"des",
+	.cra_driver_name	=	"des-ux500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_CIPHER,
+	.cra_blocksize		=	DES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des_alg.cra_list),
+	.cra_u			=	{
+		.cipher = {
+			.cia_min_keysize	=	DES_KEY_SIZE,
+			.cia_max_keysize	=	DES_KEY_SIZE,
+			.cia_setkey		=	des_setkey,
+			.cia_encrypt		=	des_encrypt,
+			.cia_decrypt		=	des_decrypt
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des3_alg
+ */
+static struct crypto_alg des3_alg = {
+	.cra_name		=	"des3_ede",
+	.cra_driver_name	=	"des3_ede-ux500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_CIPHER,
+	.cra_blocksize		=	DES3_EDE_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des3_alg.cra_list),
+	.cra_u			=	{
+		.cipher = {
+			.cia_min_keysize	=	DES3_EDE_KEY_SIZE,
+			.cia_max_keysize	=	DES3_EDE_KEY_SIZE,
+			.cia_setkey		=	des3_setkey,
+			.cia_encrypt		=	des3_encrypt,
+			.cia_decrypt		=	des3_decrypt
+		}
+	}
+};
+
+/**
+ * struct crypto_alg aes_ecb_alg
+ */
+static struct crypto_alg aes_ecb_alg = {
+	.cra_name		=	"ecb(aes)",
+	.cra_driver_name	=	"ecb-aes-ux500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(aes_ecb_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	AES_MIN_KEY_SIZE,
+			.max_keysize	=	AES_MAX_KEY_SIZE,
+			.setkey		=	aes_ablkcipher_setkey,
+			.encrypt	=	aes_ecb_encrypt,
+			.decrypt	=	aes_ecb_decrypt,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg aes_cbc_alg
+ */
+static struct crypto_alg aes_cbc_alg = {
+	.cra_name		=	"cbc(aes)",
+	.cra_driver_name	=	"cbc-aes-ux500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(aes_cbc_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	AES_MIN_KEY_SIZE,
+			.max_keysize	=	AES_MAX_KEY_SIZE,
+			.setkey		=	aes_ablkcipher_setkey,
+			.encrypt	=	aes_cbc_encrypt,
+			.decrypt	=	aes_cbc_decrypt,
+			.ivsize		=	AES_BLOCK_SIZE,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg aes_ctr_alg
+ */
+static struct crypto_alg aes_ctr_alg = {
+	.cra_name		=	"ctr(aes)",
+	.cra_driver_name	=	"ctr-aes-ux500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	AES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(aes_ctr_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	AES_MIN_KEY_SIZE,
+			.max_keysize	=	AES_MAX_KEY_SIZE,
+			.setkey		=	aes_ablkcipher_setkey,
+			.encrypt	=	aes_ctr_encrypt,
+			.decrypt	=	aes_ctr_decrypt,
+			.ivsize		=	AES_BLOCK_SIZE,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des_ecb_alg
+ */
+static struct crypto_alg des_ecb_alg = {
+	.cra_name		=	"ecb(des)",
+	.cra_driver_name	=	"ecb-des-ux500",
+	.cra_priority		=	100,
+	.cra_flags              =       CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	DES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des_ecb_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	DES_KEY_SIZE,
+			.max_keysize	=	DES_KEY_SIZE,
+			.setkey		=	des_ablkcipher_setkey,
+			.encrypt	=	des_ecb_encrypt,
+			.decrypt	=	des_ecb_decrypt,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des_cbc_alg
+ */
+static struct crypto_alg des_cbc_alg = {
+	.cra_name		=	"cbc(des)",
+	.cra_driver_name	=	"cbc-des-ux500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	DES_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des_cbc_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	DES_KEY_SIZE,
+			.max_keysize	=	DES_KEY_SIZE,
+			.setkey		=	des_ablkcipher_setkey,
+			.encrypt	=	des_cbc_encrypt,
+			.decrypt	=	des_cbc_decrypt,
+			.ivsize		=	DES_BLOCK_SIZE,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des3_ecb_alg
+ */
+static struct crypto_alg des3_ecb_alg = {
+	.cra_name		=	"ecb(des3_ede)",
+	.cra_driver_name	=	"ecb-des3_ede-ux500",
+	.cra_priority		=	100,
+	.cra_flags              =       CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	DES3_EDE_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des3_ecb_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	DES3_EDE_KEY_SIZE,
+			.max_keysize	=	DES3_EDE_KEY_SIZE,
+			.setkey		=	des3_ablkcipher_setkey,
+			.encrypt	=	des3_ecb_encrypt,
+			.decrypt	=	des3_ecb_decrypt,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg des3_cbc_alg
+ */
+static struct crypto_alg des3_cbc_alg = {
+	.cra_name		=	"cbc(des3_ede)",
+	.cra_driver_name	=	"cbc-des3_ede-ux500",
+	.cra_priority		=	100,
+	.cra_flags		=	CRYPTO_ALG_TYPE_ABLKCIPHER |
+					CRYPTO_ALG_ASYNC,
+	.cra_blocksize		=	DES3_EDE_BLOCK_SIZE,
+	.cra_ctxsize		=	sizeof(struct cryp_ctx),
+	.cra_alignmask		=	3,
+	.cra_type		=	&crypto_ablkcipher_type,
+	.cra_module		=	THIS_MODULE,
+	.cra_list		=	LIST_HEAD_INIT(des3_cbc_alg.cra_list),
+	.cra_u			=	{
+		.ablkcipher	=	{
+			.min_keysize	=	DES3_EDE_KEY_SIZE,
+			.max_keysize	=	DES3_EDE_KEY_SIZE,
+			.setkey		=	des3_ablkcipher_setkey,
+			.encrypt	=	des3_cbc_encrypt,
+			.decrypt	=	des3_cbc_decrypt,
+			.ivsize		=	DES3_EDE_BLOCK_SIZE,
+		}
+	}
+};
+
+/**
+ * struct crypto_alg *ux500_cryp_algs[] -
+ */
+static struct crypto_alg *ux500_cryp_algs[] = {
+	&aes_alg,
+	&des_alg,
+	&des3_alg,
+	&aes_ecb_alg,
+	&aes_cbc_alg,
+	&aes_ctr_alg,
+	&des_ecb_alg,
+	&des_cbc_alg,
+	&des3_ecb_alg,
+	&des3_cbc_alg,
+};
+
+/**
+ * cryp_algs_register_all -
+ */
+static int cryp_algs_register_all(void)
+{
+	int ret;
+	int i;
+	int count;
+
+	pr_debug("[%s]", __func__);
+
+	for (i = 0; i < ARRAY_SIZE(ux500_cryp_algs); i++) {
+		ret = crypto_register_alg(ux500_cryp_algs[i]);
+		if (ret) {
+			count = i;
+			pr_err("[%s] alg registration failed",
+				ux500_cryp_algs[i]->cra_driver_name);
+			goto unreg;
+		}
+	}
+	return 0;
+unreg:
+	for (i = 0; i < count; i++)
+		crypto_unregister_alg(ux500_cryp_algs[i]);
+	return ret;
+}
+
+/**
+ * cryp_algs_unregister_all -
+ */
+static void cryp_algs_unregister_all(void)
+{
+	int i;
+
+	pr_debug(DEV_DBG_NAME " [%s]", __func__);
+
+	for (i = 0; i < ARRAY_SIZE(ux500_cryp_algs); i++)
+		crypto_unregister_alg(ux500_cryp_algs[i]);
+}
+
+static int ux500_cryp_probe(struct platform_device *pdev)
+{
+	int ret;
+	int cryp_error = 0;
+	struct resource *res = NULL;
+	struct resource *res_irq = NULL;
+	struct cryp_device_data *device_data;
+	struct cryp_protection_config prot = {
+		.privilege_access = CRYP_STATE_ENABLE
+	};
+	struct device *dev = &pdev->dev;
+
+	dev_dbg(dev, "[%s]", __func__);
+	device_data = kzalloc(sizeof(struct cryp_device_data), GFP_ATOMIC);
+	if (!device_data) {
+		dev_err(dev, "[%s]: kzalloc() failed!", __func__);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	device_data->dev = dev;
+	device_data->current_ctx = NULL;
+
+	/* Grab the DMA configuration from platform data. */
+	mem_to_engine = &((struct cryp_platform_data *)
+			 dev->platform_data)->mem_to_engine;
+	engine_to_mem = &((struct cryp_platform_data *)
+			 dev->platform_data)->engine_to_mem;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(dev, "[%s]: platform_get_resource() failed",
+				__func__);
+		ret = -ENODEV;
+		goto out_kfree;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (res == NULL) {
+		dev_err(dev, "[%s]: request_mem_region() failed",
+				__func__);
+		ret = -EBUSY;
+		goto out_kfree;
+	}
+
+	device_data->base = ioremap(res->start, resource_size(res));
+	if (!device_data->base) {
+		dev_err(dev, "[%s]: ioremap failed!", __func__);
+		ret = -ENOMEM;
+		goto out_free_mem;
+	}
+
+	spin_lock_init(&device_data->ctx_lock);
+	spin_lock_init(&device_data->power_state_spinlock);
+
+	/* Enable power for CRYP hardware block */
+	device_data->pwr_regulator = ux500_regulator_get(&pdev->dev);
+	if (IS_ERR(device_data->pwr_regulator)) {
+		dev_err(dev, "[%s]: could not get cryp regulator", __func__);
+		ret = PTR_ERR(device_data->pwr_regulator);
+		device_data->pwr_regulator = NULL;
+		goto out_unmap;
+	}
+
+	/* Enable the clk for CRYP hardware block */
+	device_data->clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(device_data->clk)) {
+		dev_err(dev, "[%s]: clk_get() failed!", __func__);
+		ret = PTR_ERR(device_data->clk);
+		goto out_regulator;
+	}
+
+	/* Enable device power (and clock) */
+	ret = cryp_enable_power(device_data->dev, device_data, false);
+	if (ret) {
+		dev_err(dev, "[%s]: cryp_enable_power() failed!", __func__);
+		goto out_clk;
+	}
+
+	cryp_error = cryp_check(device_data);
+	if (cryp_error != 0) {
+		dev_err(dev, "[%s]: cryp_init() failed!", __func__);
+		ret = -EINVAL;
+		goto out_power;
+	}
+
+	cryp_error = cryp_configure_protection(device_data, &prot);
+	if (cryp_error != 0) {
+		dev_err(dev, "[%s]: cryp_configure_protection() failed!",
+			__func__);
+		ret = -EINVAL;
+		goto out_power;
+	}
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq) {
+		dev_err(dev, "[%s]: IORESOURCE_IRQ unavailable",
+			__func__);
+		goto out_power;
+	}
+
+	ret = request_irq(res_irq->start,
+			  cryp_interrupt_handler,
+			  0,
+			  "cryp1",
+			  device_data);
+	if (ret) {
+		dev_err(dev, "[%s]: Unable to request IRQ", __func__);
+		goto out_power;
+	}
+
+	if (cryp_mode == CRYP_MODE_DMA)
+		cryp_dma_setup_channel(device_data, dev);
+
+	platform_set_drvdata(pdev, device_data);
+
+	/* Put the new device into the device list... */
+	klist_add_tail(&device_data->list_node, &driver_data.device_list);
+
+	/* ... and signal that a new device is available. */
+	up(&driver_data.device_allocation);
+
+	atomic_set(&session_id, 1);
+
+	ret = cryp_algs_register_all();
+	if (ret) {
+		dev_err(dev, "[%s]: cryp_algs_register_all() failed!",
+			__func__);
+		goto out_power;
+	}
+
+	if (cryp_disable_power(&pdev->dev, device_data, false))
+		dev_err(dev, "[%s]: cryp_disable_power() failed!", __func__);
+
+	return 0;
+
+out_power:
+	cryp_disable_power(&pdev->dev, device_data, false);
+
+out_clk:
+	clk_put(device_data->clk);
+
+out_regulator:
+	ux500_regulator_put(device_data->pwr_regulator);
+
+out_unmap:
+	iounmap(device_data->base);
+
+out_free_mem:
+	release_mem_region(res->start, resource_size(res));
+
+out_kfree:
+	kfree(device_data);
+out:
+	return ret;
+}
+
+static int ux500_cryp_remove(struct platform_device *pdev)
+{
+	struct resource *res = NULL;
+	struct resource *res_irq = NULL;
+	struct cryp_device_data *device_data;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	/* Try to decrease the number of available devices. */
+	if (down_trylock(&driver_data.device_allocation))
+		return -EBUSY;
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (device_data->current_ctx) {
+		/* The device is busy */
+		spin_unlock(&device_data->ctx_lock);
+		/* Return the device to the pool. */
+		up(&driver_data.device_allocation);
+		return -EBUSY;
+	}
+
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		cryp_algs_unregister_all();
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
+			__func__);
+	else {
+		disable_irq(res_irq->start);
+		free_irq(res_irq->start, device_data);
+	}
+
+	if (cryp_disable_power(&pdev->dev, device_data, false))
+		dev_err(&pdev->dev, "[%s]: cryp_disable_power() failed",
+			__func__);
+
+	clk_put(device_data->clk);
+	ux500_regulator_put(device_data->pwr_regulator);
+
+	iounmap(device_data->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, res->end - res->start + 1);
+
+	kfree(device_data);
+
+	return 0;
+}
+
+static void ux500_cryp_shutdown(struct platform_device *pdev)
+{
+	struct resource *res_irq = NULL;
+	struct cryp_device_data *device_data;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return;
+	}
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (!device_data->current_ctx) {
+		if (down_trylock(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: Cryp still in use!"
+				"Shutting down anyway...", __func__);
+		/**
+		 * (Allocate the device)
+		 * Need to set this to non-null (dummy) value,
+		 * to avoid usage if context switching.
+		 */
+		device_data->current_ctx++;
+	}
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		cryp_algs_unregister_all();
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
+			__func__);
+	else {
+		disable_irq(res_irq->start);
+		free_irq(res_irq->start, device_data);
+	}
+
+	if (cryp_disable_power(&pdev->dev, device_data, false))
+		dev_err(&pdev->dev, "[%s]: cryp_disable_power() failed",
+			__func__);
+
+}
+
+static int ux500_cryp_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int ret;
+	struct cryp_device_data *device_data;
+	struct resource *res_irq;
+	struct cryp_ctx *temp_ctx = NULL;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	/* Handle state? */
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		dev_err(&pdev->dev, "[%s]: IORESOURCE_IRQ, unavailable",
+			__func__);
+	else
+		disable_irq(res_irq->start);
+
+	spin_lock(&device_data->ctx_lock);
+	if (!device_data->current_ctx)
+		device_data->current_ctx++;
+	spin_unlock(&device_data->ctx_lock);
+
+	if (device_data->current_ctx == ++temp_ctx) {
+		if (down_interruptible(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: down_interruptible() "
+					"failed", __func__);
+		ret = cryp_disable_power(&pdev->dev, device_data, false);
+
+	} else
+		ret = cryp_disable_power(&pdev->dev, device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: cryp_disable_power()", __func__);
+
+	return ret;
+}
+
+static int ux500_cryp_resume(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct cryp_device_data *device_data;
+	struct resource *res_irq;
+	struct cryp_ctx *temp_ctx = NULL;
+
+	dev_dbg(&pdev->dev, "[%s]", __func__);
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	spin_lock(&device_data->ctx_lock);
+	if (device_data->current_ctx == ++temp_ctx)
+		device_data->current_ctx = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+
+	if (!device_data->current_ctx)
+		up(&driver_data.device_allocation);
+	else
+		ret = cryp_enable_power(&pdev->dev, device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: cryp_enable_power() failed!",
+			__func__);
+	else {
+		res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+		if (res_irq)
+			enable_irq(res_irq->start);
+	}
+
+	return ret;
+}
+
+static struct platform_driver cryp_driver = {
+	.probe  = ux500_cryp_probe,
+	.remove = ux500_cryp_remove,
+	.shutdown = ux500_cryp_shutdown,
+	.suspend  = ux500_cryp_suspend,
+	.resume   = ux500_cryp_resume,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name  = "cryp1"
+	}
+};
+
+static int __init ux500_cryp_mod_init(void)
+{
+	pr_debug("[%s] is called!", __func__);
+	klist_init(&driver_data.device_list, NULL, NULL);
+	/* Initialize the semaphore to 0 devices (locked state) */
+	sema_init(&driver_data.device_allocation, 0);
+	return platform_driver_register(&cryp_driver);
+}
+
+static void __exit ux500_cryp_mod_fini(void)
+{
+	pr_debug("[%s] is called!", __func__);
+	platform_driver_unregister(&cryp_driver);
+	return;
+}
+
+module_init(ux500_cryp_mod_init);
+module_exit(ux500_cryp_mod_fini);
+
+module_param(cryp_mode, int, 0);
+
+MODULE_DESCRIPTION("Driver for ST-Ericsson UX500 CRYP crypto engine.");
+MODULE_ALIAS("aes-all");
+MODULE_ALIAS("des-all");
+
+MODULE_LICENSE("GPL");
diff --git a/drivers/crypto/ux500/cryp/cryp_irq.c b/drivers/crypto/ux500/cryp/cryp_irq.c
new file mode 100644
index 00000000000..08d291cdbe6
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_irq.c
@@ -0,0 +1,45 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/bitmap.h>
+#include <linux/device.h>
+
+#include "cryp.h"
+#include "cryp_p.h"
+#include "cryp_irq.h"
+#include "cryp_irqp.h"
+
+void cryp_enable_irq_src(struct cryp_device_data *device_data, u32 irq_src)
+{
+	u32 i;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	i = readl_relaxed(&device_data->base->imsc);
+	i = i | irq_src;
+	writel_relaxed(i, &device_data->base->imsc);
+}
+
+void cryp_disable_irq_src(struct cryp_device_data *device_data, u32 irq_src)
+{
+	u32 i;
+
+	dev_dbg(device_data->dev, "[%s]", __func__);
+
+	i = readl_relaxed(&device_data->base->imsc);
+	i = i & ~irq_src;
+	writel_relaxed(i, &device_data->base->imsc);
+}
+
+bool cryp_pending_irq_src(struct cryp_device_data *device_data, u32 irq_src)
+{
+	return (readl_relaxed(&device_data->base->mis) & irq_src) > 0;
+}
diff --git a/drivers/crypto/ux500/cryp/cryp_irq.h b/drivers/crypto/ux500/cryp/cryp_irq.h
new file mode 100644
index 00000000000..5a7837f1b8f
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_irq.h
@@ -0,0 +1,31 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef _CRYP_IRQ_H_
+#define _CRYP_IRQ_H_
+
+#include "cryp.h"
+
+enum cryp_irq_src_id {
+	CRYP_IRQ_SRC_INPUT_FIFO = 0x1,
+	CRYP_IRQ_SRC_OUTPUT_FIFO = 0x2,
+	CRYP_IRQ_SRC_ALL = 0x3
+};
+
+/**
+ * M0 Funtions
+ */
+void cryp_enable_irq_src(struct cryp_device_data *device_data, u32 irq_src);
+
+void cryp_disable_irq_src(struct cryp_device_data *device_data, u32 irq_src);
+
+bool cryp_pending_irq_src(struct cryp_device_data *device_data, u32 irq_src);
+
+#endif				/* _CRYP_IRQ_H_ */
diff --git a/drivers/crypto/ux500/cryp/cryp_irqp.h b/drivers/crypto/ux500/cryp/cryp_irqp.h
new file mode 100644
index 00000000000..8b339cc34bf
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_irqp.h
@@ -0,0 +1,125 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef __CRYP_IRQP_H_
+#define __CRYP_IRQP_H_
+
+#include "cryp_irq.h"
+
+/**
+ *
+ * CRYP Registers - Offset mapping
+ *     +-----------------+
+ * 00h | CRYP_CR         |  Configuration register
+ *     +-----------------+
+ * 04h | CRYP_SR         |  Status register
+ *     +-----------------+
+ * 08h | CRYP_DIN        |  Data In register
+ *     +-----------------+
+ * 0ch | CRYP_DOUT       |  Data out register
+ *     +-----------------+
+ * 10h | CRYP_DMACR      |  DMA control register
+ *     +-----------------+
+ * 14h | CRYP_IMSC       |  IMSC
+ *     +-----------------+
+ * 18h | CRYP_RIS        |  Raw interrupt status
+ *     +-----------------+
+ * 1ch | CRYP_MIS        |  Masked interrupt status.
+ *     +-----------------+
+ *       Key registers
+ *       IVR registers
+ *       Peripheral
+ *       Cell IDs
+ *
+ *       Refer data structure for other register map
+ */
+
+/**
+ * struct cryp_register
+ * @cr			- Configuration register
+ * @status		- Status register
+ * @din			- Data input register
+ * @din_size		- Data input size register
+ * @dout		- Data output register
+ * @dout_size		- Data output size register
+ * @dmacr		- Dma control register
+ * @imsc		- Interrupt mask set/clear register
+ * @ris			- Raw interrupt status
+ * @mis			- Masked interrupt statu register
+ * @key_1_l		- Key register 1 L
+ * @key_1_r		- Key register 1 R
+ * @key_2_l		- Key register 2 L
+ * @key_2_r		- Key register 2 R
+ * @key_3_l		- Key register 3 L
+ * @key_3_r		- Key register 3 R
+ * @key_4_l		- Key register 4 L
+ * @key_4_r		- Key register 4 R
+ * @init_vect_0_l	- init vector 0 L
+ * @init_vect_0_r	- init vector 0 R
+ * @init_vect_1_l	- init vector 1 L
+ * @init_vect_1_r	- init vector 1 R
+ * @cryp_unused1	- unused registers
+ * @itcr		- Integration test control register
+ * @itip		- Integration test input register
+ * @itop		- Integration test output register
+ * @cryp_unused2	- unused registers
+ * @periphId0		- FE0 CRYP Peripheral Identication Register
+ * @periphId1		- FE4
+ * @periphId2		- FE8
+ * @periphId3		- FEC
+ * @pcellId0		- FF0  CRYP PCell Identication Register
+ * @pcellId1		- FF4
+ * @pcellId2		- FF8
+ * @pcellId3		- FFC
+ */
+struct cryp_register {
+	u32 cr;			/* Configuration register   */
+	u32 sr;			/* Status register          */
+	u32 din;		/* Data input register      */
+	u32 din_size;		/* Data input size register */
+	u32 dout;		/* Data output register     */
+	u32 dout_size;		/* Data output size register */
+	u32 dmacr;		/* Dma control register     */
+	u32 imsc;		/* Interrupt mask set/clear register */
+	u32 ris;		/* Raw interrupt status             */
+	u32 mis;		/* Masked interrupt statu register  */
+
+	u32 key_1_l;		/*Key register 1 L */
+	u32 key_1_r;		/*Key register 1 R */
+	u32 key_2_l;		/*Key register 2 L */
+	u32 key_2_r;		/*Key register 2 R */
+	u32 key_3_l;		/*Key register 3 L */
+	u32 key_3_r;		/*Key register 3 R */
+	u32 key_4_l;		/*Key register 4 L */
+	u32 key_4_r;		/*Key register 4 R */
+
+	u32 init_vect_0_l;	/*init vector 0 L */
+	u32 init_vect_0_r;	/*init vector 0 R */
+	u32 init_vect_1_l;	/*init vector 1 L */
+	u32 init_vect_1_r;	/*init vector 1 R */
+
+	u32 cryp_unused1[(0x80 - 0x58) / sizeof(u32)];	/* unused registers */
+	u32 itcr;		/*Integration test control register */
+	u32 itip;		/*Integration test input register */
+	u32 itop;		/*Integration test output register */
+	u32 cryp_unused2[(0xFE0 - 0x8C) / sizeof(u32)];	/* unused registers */
+
+	u32 periphId0;		/* FE0  CRYP Peripheral Identication Register */
+	u32 periphId1;		/* FE4 */
+	u32 periphId2;		/* FE8 */
+	u32 periphId3;		/* FEC */
+
+	u32 pcellId0;		/* FF0  CRYP PCell Identication Register */
+	u32 pcellId1;		/* FF4 */
+	u32 pcellId2;		/* FF8 */
+	u32 pcellId3;		/* FFC */
+};
+
+#endif
diff --git a/drivers/crypto/ux500/cryp/cryp_p.h b/drivers/crypto/ux500/cryp/cryp_p.h
new file mode 100644
index 00000000000..0e070829edc
--- /dev/null
+++ b/drivers/crypto/ux500/cryp/cryp_p.h
@@ -0,0 +1,124 @@
+/**
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson.
+ * Author: Jonas Linde <jonas.linde@stericsson.com> for ST-Ericsson.
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson.
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef _CRYP_P_H_
+#define _CRYP_P_H_
+
+#include <linux/io.h>
+#include <linux/bitops.h>
+
+#include "cryp.h"
+#include "cryp_irqp.h"
+
+/**
+ * Generic Macros
+ */
+#define CRYP_SET_BITS(reg_name, mask) \
+	writel_relaxed((readl_relaxed(reg_name) | mask), reg_name)
+
+#define CRYP_WRITE_BIT(reg_name, val, mask) \
+	writel_relaxed(((readl_relaxed(reg_name) & ~(mask)) |\
+			((val) & (mask))), reg_name)
+
+#define CRYP_TEST_BITS(reg_name, val) \
+	(readl_relaxed(reg_name) & (val))
+
+#define CRYP_PUT_BITS(reg, val, shift, mask) \
+	writel_relaxed(((readl_relaxed(reg) & ~(mask)) | \
+		(((u32)val << shift) & (mask))), reg)
+
+/**
+ * CRYP specific Macros
+ */
+#define CRYP_PERIPHERAL_ID0		0xE3
+#define CRYP_PERIPHERAL_ID1		0x05
+
+#define CRYP_PERIPHERAL_ID2_DB8500	0x28
+#define CRYP_PERIPHERAL_ID2_DB5500	0x29
+#define CRYP_PERIPHERAL_ID3		0x00
+
+#define CRYP_PCELL_ID0			0x0D
+#define CRYP_PCELL_ID1			0xF0
+#define CRYP_PCELL_ID2			0x05
+#define CRYP_PCELL_ID3			0xB1
+
+/**
+ * CRYP register default values
+ */
+#define MAX_DEVICE_SUPPORT		2
+
+/* Priv set, keyrden set and datatype 8bits swapped set as default. */
+#define CRYP_CR_DEFAULT			0x0482
+#define CRYP_DMACR_DEFAULT		0x0
+#define CRYP_IMSC_DEFAULT		0x0
+#define CRYP_DIN_DEFAULT		0x0
+#define CRYP_DOUT_DEFAULT		0x0
+#define CRYP_KEY_DEFAULT		0x0
+#define CRYP_INIT_VECT_DEFAULT		0x0
+
+/**
+ * CRYP Control register specific mask
+ */
+#define CRYP_CR_SECURE_MASK		BIT(0)
+#define CRYP_CR_PRLG_MASK		BIT(1)
+#define CRYP_CR_ALGODIR_MASK		BIT(2)
+#define CRYP_CR_ALGOMODE_MASK		(BIT(5) | BIT(4) | BIT(3))
+#define CRYP_CR_DATATYPE_MASK		(BIT(7) | BIT(6))
+#define CRYP_CR_KEYSIZE_MASK		(BIT(9) | BIT(8))
+#define CRYP_CR_KEYRDEN_MASK		BIT(10)
+#define CRYP_CR_KSE_MASK		BIT(11)
+#define CRYP_CR_START_MASK		BIT(12)
+#define CRYP_CR_INIT_MASK		BIT(13)
+#define CRYP_CR_FFLUSH_MASK		BIT(14)
+#define CRYP_CR_CRYPEN_MASK		BIT(15)
+#define CRYP_CR_CONTEXT_SAVE_MASK	(CRYP_CR_SECURE_MASK |\
+					 CRYP_CR_PRLG_MASK |\
+					 CRYP_CR_ALGODIR_MASK |\
+					 CRYP_CR_ALGOMODE_MASK |\
+					 CRYP_CR_DATATYPE_MASK |\
+					 CRYP_CR_KEYSIZE_MASK |\
+					 CRYP_CR_KEYRDEN_MASK |\
+					 CRYP_CR_DATATYPE_MASK)
+
+
+#define CRYP_SR_INFIFO_READY_MASK	(BIT(0) | BIT(1))
+#define CRYP_SR_IFEM_MASK		BIT(0)
+#define CRYP_SR_BUSY_MASK		BIT(4)
+
+/**
+ * Bit position used while setting bits in register
+ */
+#define CRYP_CR_PRLG_POS		1
+#define CRYP_CR_ALGODIR_POS		2
+#define CRYP_CR_ALGOMODE_POS		3
+#define CRYP_CR_DATATYPE_POS		6
+#define CRYP_CR_KEYSIZE_POS		8
+#define CRYP_CR_KEYRDEN_POS		10
+#define CRYP_CR_KSE_POS			11
+#define CRYP_CR_START_POS		12
+#define CRYP_CR_INIT_POS		13
+#define CRYP_CR_CRYPEN_POS		15
+
+#define CRYP_SR_BUSY_POS		4
+
+/**
+ * CRYP PCRs------PC_NAND control register
+ * BIT_MASK
+ */
+#define CRYP_DMA_REQ_MASK		(BIT(1) | BIT(0))
+#define CRYP_DMA_REQ_MASK_POS		0
+
+
+struct cryp_system_context {
+	/* CRYP Register structure */
+	struct cryp_register *p_cryp_reg[MAX_DEVICE_SUPPORT];
+};
+
+#endif
diff --git a/drivers/crypto/ux500/hash/Makefile b/drivers/crypto/ux500/hash/Makefile
new file mode 100644
index 00000000000..b2f90d9bac7
--- /dev/null
+++ b/drivers/crypto/ux500/hash/Makefile
@@ -0,0 +1,11 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Shujuan Chen (shujuan.chen@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+ifdef CONFIG_CRYPTO_DEV_UX500_DEBUG
+CFLAGS_hash_core.o := -DDEBUG -O0
+endif
+
+obj-$(CONFIG_CRYPTO_DEV_UX500_HASH) += ux500_hash.o
+ux500_hash-objs :=  hash_core.o
diff --git a/drivers/crypto/ux500/hash/hash_alg.h b/drivers/crypto/ux500/hash/hash_alg.h
new file mode 100644
index 00000000000..b8619ea4a27
--- /dev/null
+++ b/drivers/crypto/ux500/hash/hash_alg.h
@@ -0,0 +1,387 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen (shujuan.chen@stericsson.com)
+ * Author: Joakim Bech (joakim.xx.bech@stericsson.com)
+ * Author: Berne Hebark (berne.hebark@stericsson.com))
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#ifndef _HASH_ALG_H
+#define _HASH_ALG_H
+
+#include <linux/bitops.h>
+
+#define HASH_BLOCK_SIZE			64
+#define HASH_DMA_ALIGN_SIZE		4
+#define HASH_DMA_PERFORMANCE_MIN_SIZE	1024
+#define HASH_BYTES_PER_WORD		4
+
+/* Maximum value of the length's high word */
+#define HASH_HIGH_WORD_MAX_VAL		0xFFFFFFFFUL
+
+/* Power on Reset values HASH registers */
+#define HASH_RESET_CR_VALUE		0x0
+#define HASH_RESET_STR_VALUE		0x0
+
+/* Number of context swap registers */
+#define HASH_CSR_COUNT			52
+
+#define HASH_RESET_CSRX_REG_VALUE	0x0
+#define HASH_RESET_CSFULL_REG_VALUE	0x0
+#define HASH_RESET_CSDATAIN_REG_VALUE	0x0
+
+#define HASH_RESET_INDEX_VAL		0x0
+#define HASH_RESET_BIT_INDEX_VAL	0x0
+#define HASH_RESET_BUFFER_VAL		0x0
+#define HASH_RESET_LEN_HIGH_VAL		0x0
+#define HASH_RESET_LEN_LOW_VAL		0x0
+
+/* Control register bitfields */
+#define HASH_CR_RESUME_MASK	0x11FCF
+
+#define HASH_CR_SWITCHON_POS	31
+#define HASH_CR_SWITCHON_MASK	BIT(31)
+
+#define HASH_CR_EMPTYMSG_POS	20
+#define HASH_CR_EMPTYMSG_MASK	BIT(20)
+
+#define HASH_CR_DINF_POS	12
+#define HASH_CR_DINF_MASK	BIT(12)
+
+#define HASH_CR_NBW_POS		8
+#define HASH_CR_NBW_MASK	0x00000F00UL
+
+#define HASH_CR_LKEY_POS	16
+#define HASH_CR_LKEY_MASK	BIT(16)
+
+#define HASH_CR_ALGO_POS	7
+#define HASH_CR_ALGO_MASK	BIT(7)
+
+#define HASH_CR_MODE_POS	6
+#define HASH_CR_MODE_MASK	BIT(6)
+
+#define HASH_CR_DATAFORM_POS	4
+#define HASH_CR_DATAFORM_MASK	(BIT(4) | BIT(5))
+
+#define HASH_CR_DMAE_POS	3
+#define HASH_CR_DMAE_MASK	BIT(3)
+
+#define HASH_CR_INIT_POS	2
+#define HASH_CR_INIT_MASK	BIT(2)
+
+#define HASH_CR_PRIVN_POS	1
+#define HASH_CR_PRIVN_MASK	BIT(1)
+
+#define HASH_CR_SECN_POS	0
+#define HASH_CR_SECN_MASK	BIT(0)
+
+/* Start register bitfields */
+#define HASH_STR_DCAL_POS	8
+#define HASH_STR_DCAL_MASK	BIT(8)
+#define HASH_STR_DEFAULT	0x0
+
+#define HASH_STR_NBLW_POS	0
+#define HASH_STR_NBLW_MASK	0x0000001FUL
+
+#define HASH_NBLW_MAX_VAL	0x1F
+
+/* PrimeCell IDs */
+#define HASH_P_ID0		0xE0
+#define HASH_P_ID1		0x05
+#define HASH_P_ID2		0x38
+#define HASH_P_ID3		0x00
+#define HASH_CELL_ID0		0x0D
+#define HASH_CELL_ID1		0xF0
+#define HASH_CELL_ID2		0x05
+#define HASH_CELL_ID3		0xB1
+
+#define HASH_SET_BITS(reg_name, mask)	\
+	writel_relaxed((readl_relaxed(reg_name) | mask), reg_name)
+
+#define HASH_CLEAR_BITS(reg_name, mask)	\
+	writel_relaxed((readl_relaxed(reg_name) & ~mask), reg_name)
+
+#define HASH_PUT_BITS(reg, val, shift, mask)	\
+	writel_relaxed(((readl(reg) & ~(mask)) |	\
+		(((u32)val << shift) & (mask))), reg)
+
+#define HASH_SET_DIN(val, len)	writesl(&device_data->base->din, (val), (len))
+
+#define HASH_INITIALIZE			\
+	HASH_PUT_BITS(			\
+		&device_data->base->cr,	\
+		0x01, HASH_CR_INIT_POS,	\
+		HASH_CR_INIT_MASK)
+
+#define HASH_SET_DATA_FORMAT(data_format)				\
+		HASH_PUT_BITS(						\
+			&device_data->base->cr,				\
+			(u32) (data_format), HASH_CR_DATAFORM_POS,	\
+			HASH_CR_DATAFORM_MASK)
+#define HASH_SET_NBLW(val)					\
+		HASH_PUT_BITS(					\
+			&device_data->base->str,		\
+			(u32) (val), HASH_STR_NBLW_POS,		\
+			HASH_STR_NBLW_MASK)
+#define HASH_SET_DCAL					\
+		HASH_PUT_BITS(				\
+			&device_data->base->str,	\
+			0x01, HASH_STR_DCAL_POS,	\
+			HASH_STR_DCAL_MASK)
+
+/* Hardware access method */
+enum hash_mode {
+	HASH_MODE_CPU,
+	HASH_MODE_DMA
+};
+
+/**
+ * struct uint64 - Structure to handle 64 bits integers.
+ * @high_word:	Most significant bits.
+ * @low_word:	Least significant bits.
+ *
+ * Used to handle 64 bits integers.
+ */
+struct uint64 {
+	u32 high_word;
+	u32 low_word;
+};
+
+/**
+ * struct hash_register - Contains all registers in u8500 hash hardware.
+ * @cr:		HASH control register (0x000).
+ * @din:	HASH data input register (0x004).
+ * @str:	HASH start register (0x008).
+ * @hx:		HASH digest register 0..7 (0x00c-0x01C).
+ * @padding0:	Reserved (0x02C).
+ * @itcr:	Integration test control register (0x080).
+ * @itip:	Integration test input register (0x084).
+ * @itop:	Integration test output register (0x088).
+ * @padding1:	Reserved (0x08C).
+ * @csfull:	HASH context full register (0x0F8).
+ * @csdatain:	HASH context swap data input register (0x0FC).
+ * @csrx:	HASH context swap register 0..51 (0x100-0x1CC).
+ * @padding2:	Reserved (0x1D0).
+ * @periphid0:	HASH peripheral identification register 0 (0xFE0).
+ * @periphid1:	HASH peripheral identification register 1 (0xFE4).
+ * @periphid2:	HASH peripheral identification register 2 (0xFE8).
+ * @periphid3:	HASH peripheral identification register 3 (0xFEC).
+ * @cellid0:	HASH PCell identification register 0 (0xFF0).
+ * @cellid1:	HASH PCell identification register 1 (0xFF4).
+ * @cellid2:	HASH PCell identification register 2 (0xFF8).
+ * @cellid3:	HASH PCell identification register 3 (0xFFC).
+ *
+ * The device communicates to the HASH via 32-bit-wide control registers
+ * accessible via the 32-bit width AMBA rev. 2.0 AHB Bus. Below is a structure
+ * with the registers used.
+ */
+struct hash_register {
+	u32 cr;
+	u32 din;
+	u32 str;
+	u32 hx[8];
+
+	u32 padding0[(0x080 - 0x02C) / sizeof(u32)];
+
+	u32 itcr;
+	u32 itip;
+	u32 itop;
+
+	u32 padding1[(0x0F8 - 0x08C) / sizeof(u32)];
+
+	u32 csfull;
+	u32 csdatain;
+	u32 csrx[HASH_CSR_COUNT];
+
+	u32 padding2[(0xFE0 - 0x1D0) / sizeof(u32)];
+
+	u32 periphid0;
+	u32 periphid1;
+	u32 periphid2;
+	u32 periphid3;
+
+	u32 cellid0;
+	u32 cellid1;
+	u32 cellid2;
+	u32 cellid3;
+};
+
+/**
+ * struct hash_state - Hash context state.
+ * @temp_cr:	Temporary HASH Control Register.
+ * @str_reg:	HASH Start Register.
+ * @din_reg:	HASH Data Input Register.
+ * @csr[52]:	HASH Context Swap Registers 0-39.
+ * @csfull:	HASH Context Swap Registers 40 ie Status flags.
+ * @csdatain:	HASH Context Swap Registers 41 ie Input data.
+ * @buffer:	Working buffer for messages going to the hardware.
+ * @length:	Length of the part of message hashed so far (floor(N/64) * 64).
+ * @index:	Valid number of bytes in buffer (N % 64).
+ * @bit_index:	Valid number of bits in buffer (N % 8).
+ *
+ * This structure is used between context switches, i.e. when ongoing jobs are
+ * interupted with new jobs. When this happens we need to store intermediate
+ * results in software.
+ *
+ * WARNING: "index" is the  member of the structure, to be sure  that "buffer"
+ * is aligned on a 4-bytes boundary. This is highly implementation dependent
+ * and MUST be checked whenever this code is ported on new platforms.
+ */
+struct hash_state {
+	u32		temp_cr;
+	u32		str_reg;
+	u32		din_reg;
+	u32		csr[52];
+	u32		csfull;
+	u32		csdatain;
+	u32		buffer[HASH_BLOCK_SIZE / sizeof(u32)];
+	struct uint64	length;
+	u8		index;
+	u8		bit_index;
+};
+
+/**
+ * enum hash_device_id - HASH device ID.
+ * @HASH_DEVICE_ID_0: Hash hardware with ID 0
+ * @HASH_DEVICE_ID_1: Hash hardware with ID 1
+ */
+enum hash_device_id {
+	HASH_DEVICE_ID_0 = 0,
+	HASH_DEVICE_ID_1 = 1
+};
+
+/**
+ * enum hash_data_format - HASH data format.
+ * @HASH_DATA_32_BITS:	32 bits data format
+ * @HASH_DATA_16_BITS:	16 bits data format
+ * @HASH_DATA_8_BITS:	8 bits data format.
+ * @HASH_DATA_1_BITS:	1 bit data format.
+ */
+enum hash_data_format {
+	HASH_DATA_32_BITS	= 0x0,
+	HASH_DATA_16_BITS	= 0x1,
+	HASH_DATA_8_BITS	= 0x2,
+	HASH_DATA_1_BIT		= 0x3
+};
+
+/**
+ * enum hash_algo - Enumeration for selecting between SHA1 or SHA2 algorithm.
+ * @HASH_ALGO_SHA1: Indicates that SHA1 is used.
+ * @HASH_ALGO_SHA2: Indicates that SHA2 (SHA256) is used.
+ */
+enum hash_algo {
+	HASH_ALGO_SHA1		= 0x0,
+	HASH_ALGO_SHA256	= 0x1
+};
+
+/**
+ * enum hash_op - Enumeration for selecting between HASH or HMAC mode.
+ * @HASH_OPER_MODE_HASH: Indicates usage of normal HASH mode.
+ * @HASH_OPER_MODE_HMAC: Indicates usage of HMAC.
+ */
+enum hash_op {
+	HASH_OPER_MODE_HASH = 0x0,
+	HASH_OPER_MODE_HMAC = 0x1
+};
+
+/**
+ * struct hash_config - Configuration data for the hardware.
+ * @data_format:	Format of data entered into the hash data in register.
+ * @algorithm:		Algorithm selection bit.
+ * @oper_mode:		Operating mode selection bit.
+ */
+struct hash_config {
+	int data_format;
+	int algorithm;
+	int oper_mode;
+};
+
+/**
+ * struct hash_dma - Structure used for dma.
+ * @mask:		DMA capabilities bitmap mask.
+ * @complete:		Used to maintain state for a "completion".
+ * @chan_mem2hash:	DMA channel.
+ * @cfg_mem2hash:	DMA channel configuration.
+ * @sg_len:		Scatterlist length.
+ * @sg:			Scatterlist.
+ * @nents:		Number of sg entries.
+ */
+struct hash_dma {
+	dma_cap_mask_t		mask;
+	struct completion	complete;
+	struct dma_chan		*chan_mem2hash;
+	void			*cfg_mem2hash;
+	int			sg_len;
+	struct scatterlist	*sg;
+	int			nents;
+};
+
+/**
+ * struct hash_ctx - The context used for hash calculations.
+ * @key:	The key used in the operation.
+ * @keylen:	The length of the key.
+ * @updated:	Indicates if hardware is initialized for new operations.
+ * @state:	The state of the current calculations.
+ * @config:	The current configuration.
+ * @digestsize:	The size of current digest.
+ * @device:	Pointer to the device structure.
+ * @dma_mode:	Used in special cases (workaround), e.g. need to change to
+ *		cpu mode, if not supported/working in dma mode.
+ */
+struct hash_ctx {
+	u8			*key;
+	u32			keylen;
+	u8			updated;
+	struct hash_state	state;
+	struct hash_config	config;
+	int			digestsize;
+	struct hash_device_data	*device;
+	bool			dma_mode;
+};
+
+/**
+ * struct hash_device_data - structure for a hash device.
+ * @base:		Pointer to the hardware base address.
+ * @list_node:		For inclusion in klist.
+ * @dev:		Pointer to the device dev structure.
+ * @ctx_lock:		Spinlock for current_ctx.
+ * @current_ctx:	Pointer to the currently allocated context.
+ * @power_state:	TRUE = power state on, FALSE = power state off.
+ * @power_state_lock:	Spinlock for power_state.
+ * @regulator:		Pointer to the device's power control.
+ * @clk:		Pointer to the device's clock control.
+ * @restore_dev_state:	TRUE = saved state, FALSE = no saved state.
+ * @dma:		Structure used for dma.
+ */
+struct hash_device_data {
+	struct hash_register __iomem	*base;
+	struct klist_node		list_node;
+	struct device			*dev;
+	struct spinlock			ctx_lock;
+	struct hash_ctx			*current_ctx;
+	bool				power_state;
+	struct spinlock			power_state_lock;
+	struct ux500_regulator		*regulator;
+	struct clk			*clk;
+	bool				restore_dev_state;
+	struct hash_dma			dma;
+};
+
+int hash_check_hw(struct hash_device_data *device_data);
+
+int hash_setconfiguration(struct hash_device_data *device_data,
+		struct hash_config *config);
+
+void hash_begin(struct hash_device_data *device_data, struct hash_ctx *ctx);
+
+void hash_get_digest(struct hash_device_data *device_data,
+		u8 *digest, int algorithm);
+
+int hash_hw_update(struct ahash_request *req);
+
+int hash_save_state(struct hash_device_data *device_data,
+		struct hash_state *state);
+
+int hash_resume_state(struct hash_device_data *device_data,
+		const struct hash_state *state);
+
+#endif
diff --git a/drivers/crypto/ux500/hash/hash_alg_p.h b/drivers/crypto/ux500/hash/hash_alg_p.h
new file mode 100755
index 00000000000..c85faaeba6f
--- /dev/null
+++ b/drivers/crypto/ux500/hash/hash_alg_p.h
@@ -0,0 +1,26 @@
+/*****************************************************************************/
+/**
+*  � ST-Ericsson, 2009 - All rights reserved
+*  Reproduction and Communication of this document is strictly prohibited
+*  unless specifically authorized in writing by ST-Ericsson
+*
+*  static Header file of HASH Processor
+*  Specification release related to this implementation: A_V2.2
+*  AUTHOR :  ST-Ericsson
+*/
+/*****************************************************************************/
+
+#ifndef _HASH_P_H_
+#define _HASH_P_H_
+
+/*--------------------------------------------------------------------------*
+ * Includes                                                                 *
+ *--------------------------------------------------------------------------*/
+#include "hash_alg.h"
+
+/*--------------------------------------------------------------------------*
+ * Defines                                                                  *
+ *--------------------------------------------------------------------------*/
+
+#endif /* End _HASH_P_H_ */
+
diff --git a/drivers/crypto/ux500/hash/hash_core.c b/drivers/crypto/ux500/hash/hash_core.c
new file mode 100644
index 00000000000..05a06b00ca0
--- /dev/null
+++ b/drivers/crypto/ux500/hash/hash_core.c
@@ -0,0 +1,2080 @@
+/*
+ * Cryptographic API.
+ * Support for Nomadik hardware crypto engine.
+
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Shujuan Chen <shujuan.chen@stericsson.com> for ST-Ericsson
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com> for ST-Ericsson
+ * Author: Berne Hebark <berne.herbark@stericsson.com> for ST-Ericsson.
+ * Author: Niklas Hernaeus <niklas.hernaeus@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/klist.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/crypto.h>
+
+#include <linux/regulator/dbx500-prcmu.h>
+#include <linux/dmaengine.h>
+#include <linux/bitops.h>
+
+#include <crypto/internal/hash.h>
+#include <crypto/sha.h>
+#include <crypto/scatterwalk.h>
+#include <crypto/algapi.h>
+
+#include <mach/crypto-ux500.h>
+#include <mach/hardware.h>
+
+#include "hash_alg.h"
+
+#define DEV_DBG_NAME "hashX hashX:"
+
+static int hash_mode;
+module_param(hash_mode, int, 0);
+MODULE_PARM_DESC(hash_mode, "CPU or DMA mode. CPU = 0 (default), DMA = 1");
+
+/**
+ * Pre-calculated empty message digests.
+ */
+static u8 zero_message_hash_sha1[SHA1_DIGEST_SIZE] = {
+	0xda, 0x39, 0xa3, 0xee, 0x5e, 0x6b, 0x4b, 0x0d,
+	0x32, 0x55, 0xbf, 0xef, 0x95, 0x60, 0x18, 0x90,
+	0xaf, 0xd8, 0x07, 0x09
+};
+
+static u8 zero_message_hash_sha256[SHA256_DIGEST_SIZE] = {
+	0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c, 0x14,
+	0x9a, 0xfb, 0xf4, 0xc8, 0x99, 0x6f, 0xb9, 0x24,
+	0x27, 0xae, 0x41, 0xe4, 0x64, 0x9b, 0x93, 0x4c,
+	0xa4, 0x95, 0x99, 0x1b, 0x78, 0x52, 0xb8, 0x55
+};
+
+/* HMAC-SHA1, no key */
+static u8 zero_message_hmac_sha1[SHA1_DIGEST_SIZE] = {
+	0xfb, 0xdb, 0x1d, 0x1b, 0x18, 0xaa, 0x6c, 0x08,
+	0x32, 0x4b, 0x7d, 0x64, 0xb7, 0x1f, 0xb7, 0x63,
+	0x70, 0x69, 0x0e, 0x1d
+};
+
+/* HMAC-SHA256, no key */
+static u8 zero_message_hmac_sha256[SHA256_DIGEST_SIZE] = {
+	0xb6, 0x13, 0x67, 0x9a, 0x08, 0x14, 0xd9, 0xec,
+	0x77, 0x2f, 0x95, 0xd7, 0x78, 0xc3, 0x5f, 0xc5,
+	0xff, 0x16, 0x97, 0xc4, 0x93, 0x71, 0x56, 0x53,
+	0xc6, 0xc7, 0x12, 0x14, 0x42, 0x92, 0xc5, 0xad
+};
+
+/**
+ * struct hash_driver_data - data specific to the driver.
+ *
+ * @device_list:	A list of registered devices to choose from.
+ * @device_allocation:	A semaphore initialized with number of devices.
+ */
+struct hash_driver_data {
+	struct klist		device_list;
+	struct semaphore	device_allocation;
+};
+
+static struct hash_driver_data	driver_data;
+
+/* Declaration of functions */
+/**
+ * hash_messagepad - Pads a message and write the nblw bits.
+ * @device_data:	Structure for the hash device.
+ * @message:		Last word of a message
+ * @index_bytes:	The number of bytes in the last message
+ *
+ * This function manages the final part of the digest calculation, when less
+ * than 512 bits (64 bytes) remain in message. This means index_bytes < 64.
+ *
+ * Reentrancy: Non Re-entrant.
+ */
+static void hash_messagepad(struct hash_device_data *device_data,
+		const u32 *message, u8 index_bytes);
+
+/**
+ * release_hash_device - Releases a previously allocated hash device.
+ * @device_data:	Structure for the hash device.
+ *
+ */
+static void release_hash_device(struct hash_device_data *device_data)
+{
+	spin_lock(&device_data->ctx_lock);
+	device_data->current_ctx->device = NULL;
+	device_data->current_ctx = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	/*
+	 * The down_interruptible part for this semaphore is called in
+	 * cryp_get_device_data.
+	 */
+	up(&driver_data.device_allocation);
+}
+
+static void hash_dma_setup_channel(struct hash_device_data *device_data,
+				struct device *dev)
+{
+	struct hash_platform_data *platform_data = dev->platform_data;
+	dma_cap_zero(device_data->dma.mask);
+	dma_cap_set(DMA_SLAVE, device_data->dma.mask);
+
+	device_data->dma.cfg_mem2hash = platform_data->mem_to_engine;
+	device_data->dma.chan_mem2hash =
+		dma_request_channel(device_data->dma.mask,
+				platform_data->dma_filter,
+				device_data->dma.cfg_mem2hash);
+
+	init_completion(&device_data->dma.complete);
+}
+
+static void hash_dma_callback(void *data)
+{
+	struct hash_ctx *ctx = (struct hash_ctx *) data;
+
+	complete(&ctx->device->dma.complete);
+}
+
+static int hash_set_dma_transfer(struct hash_ctx *ctx, struct scatterlist *sg,
+		int len, enum dma_data_direction direction)
+{
+	struct dma_async_tx_descriptor *desc = NULL;
+	struct dma_chan *channel = NULL;
+	dma_cookie_t cookie;
+
+	if (direction != DMA_TO_DEVICE) {
+		dev_err(ctx->device->dev, "[%s] Invalid DMA direction",
+				__func__);
+		return -EFAULT;
+	}
+
+	sg->length = ALIGN(sg->length, HASH_DMA_ALIGN_SIZE);
+
+	channel = ctx->device->dma.chan_mem2hash;
+	ctx->device->dma.sg = sg;
+	ctx->device->dma.sg_len = dma_map_sg(channel->device->dev,
+			ctx->device->dma.sg, ctx->device->dma.nents,
+			direction);
+
+	if (!ctx->device->dma.sg_len) {
+		dev_err(ctx->device->dev,
+				"[%s]: Could not map the sg list (TO_DEVICE)",
+				__func__);
+		return -EFAULT;
+	}
+
+	dev_dbg(ctx->device->dev, "[%s]: Setting up DMA for buffer "
+			"(TO_DEVICE)", __func__);
+	desc = channel->device->device_prep_slave_sg(channel,
+			ctx->device->dma.sg, ctx->device->dma.sg_len,
+			direction, DMA_CTRL_ACK | DMA_PREP_INTERRUPT, ctx);
+	if (!desc) {
+		dev_err(ctx->device->dev,
+			"[%s]: device_prep_slave_sg() failed!", __func__);
+		return -EFAULT;
+	}
+
+	desc->callback = hash_dma_callback;
+	desc->callback_param = ctx;
+
+	cookie = desc->tx_submit(desc);
+	dma_async_issue_pending(channel);
+
+	return 0;
+}
+
+static void hash_dma_done(struct hash_ctx *ctx)
+{
+	struct dma_chan *chan;
+
+	chan = ctx->device->dma.chan_mem2hash;
+	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dma_unmap_sg(chan->device->dev, ctx->device->dma.sg,
+			ctx->device->dma.sg_len, DMA_TO_DEVICE);
+
+}
+
+static int hash_dma_write(struct hash_ctx *ctx,
+		struct scatterlist *sg, int len)
+{
+	int error = hash_set_dma_transfer(ctx, sg, len, DMA_TO_DEVICE);
+	if (error) {
+		dev_dbg(ctx->device->dev, "[%s]: hash_set_dma_transfer() "
+			"failed", __func__);
+		return error;
+	}
+
+	return len;
+}
+
+/**
+ * get_empty_message_digest - Returns a pre-calculated digest for
+ * the empty message.
+ * @device_data:	Structure for the hash device.
+ * @zero_hash:		Buffer to return the empty message digest.
+ * @zero_hash_size:	Hash size of the empty message digest.
+ * @zero_digest:	True if zero_digest returned.
+ */
+static int get_empty_message_digest(
+		struct hash_device_data *device_data,
+		u8 *zero_hash, u32 *zero_hash_size, bool *zero_digest)
+{
+	int ret = 0;
+	struct hash_ctx *ctx = device_data->current_ctx;
+	*zero_digest = false;
+
+	/**
+	 * Caller responsible for ctx != NULL.
+	 */
+
+	if (HASH_OPER_MODE_HASH == ctx->config.oper_mode) {
+		if (HASH_ALGO_SHA1 == ctx->config.algorithm) {
+			memcpy(zero_hash, &zero_message_hash_sha1[0],
+					SHA1_DIGEST_SIZE);
+			*zero_hash_size = SHA1_DIGEST_SIZE;
+			*zero_digest = true;
+		} else if (HASH_ALGO_SHA256 ==
+				ctx->config.algorithm) {
+			memcpy(zero_hash, &zero_message_hash_sha256[0],
+					SHA256_DIGEST_SIZE);
+			*zero_hash_size = SHA256_DIGEST_SIZE;
+			*zero_digest = true;
+		} else {
+			dev_err(device_data->dev, "[%s] "
+					"Incorrect algorithm!"
+					, __func__);
+			ret = -EINVAL;
+			goto out;
+		}
+	} else if (HASH_OPER_MODE_HMAC == ctx->config.oper_mode) {
+		if (!ctx->keylen) {
+			if (HASH_ALGO_SHA1 == ctx->config.algorithm) {
+				memcpy(zero_hash, &zero_message_hmac_sha1[0],
+						SHA1_DIGEST_SIZE);
+				*zero_hash_size = SHA1_DIGEST_SIZE;
+				*zero_digest = true;
+			} else if (HASH_ALGO_SHA256 == ctx->config.algorithm) {
+				memcpy(zero_hash, &zero_message_hmac_sha256[0],
+						SHA256_DIGEST_SIZE);
+				*zero_hash_size = SHA256_DIGEST_SIZE;
+				*zero_digest = true;
+			} else {
+				dev_err(device_data->dev, "[%s] "
+						"Incorrect algorithm!"
+						, __func__);
+				ret = -EINVAL;
+				goto out;
+			}
+		} else {
+			dev_dbg(device_data->dev, "[%s] Continue hash "
+					"calculation, since hmac key avalable",
+					__func__);
+		}
+	}
+out:
+
+	return ret;
+}
+
+/**
+ * hash_disable_power - Request to disable power and clock.
+ * @device_data:	Structure for the hash device.
+ * @save_device_state:	If true, saves the current hw state.
+ *
+ * This function request for disabling power (regulator) and clock,
+ * and could also save current hw state.
+ */
+static int hash_disable_power(
+		struct hash_device_data *device_data,
+		bool			save_device_state)
+{
+	int ret = 0;
+	struct device *dev = device_data->dev;
+
+	spin_lock(&device_data->power_state_lock);
+	if (!device_data->power_state)
+		goto out;
+
+	if (save_device_state && device_data->current_ctx) {
+		hash_save_state(device_data,
+				&device_data->current_ctx->state);
+		device_data->restore_dev_state = true;
+	}
+
+	clk_disable(device_data->clk);
+	ret = ux500_regulator_atomic_disable(device_data->regulator);
+	if (ret)
+		dev_err(dev, "[%s] regulator_disable() failed!", __func__);
+
+	device_data->power_state = false;
+
+out:
+	spin_unlock(&device_data->power_state_lock);
+
+	return ret;
+}
+
+/**
+ * hash_enable_power - Request to enable power and clock.
+ * @device_data:		Structure for the hash device.
+ * @restore_device_state:	If true, restores a previous saved hw state.
+ *
+ * This function request for enabling power (regulator) and clock,
+ * and could also restore a previously saved hw state.
+ */
+static int hash_enable_power(
+		struct hash_device_data *device_data,
+		bool			restore_device_state)
+{
+	int ret = 0;
+	struct device *dev = device_data->dev;
+
+	spin_lock(&device_data->power_state_lock);
+	if (!device_data->power_state) {
+		ret = ux500_regulator_atomic_enable(device_data->regulator);
+		if (ret) {
+			dev_err(dev, "[%s]: regulator_enable() failed!",
+					__func__);
+			goto out;
+		}
+		ret = clk_enable(device_data->clk);
+		if (ret) {
+			dev_err(dev, "[%s]: clk_enable() failed!",
+					__func__);
+			ret = ux500_regulator_atomic_disable(
+					device_data->regulator);
+			goto out;
+		}
+		device_data->power_state = true;
+	}
+
+	if (device_data->restore_dev_state) {
+		if (restore_device_state) {
+			device_data->restore_dev_state = false;
+			hash_resume_state(device_data,
+				&device_data->current_ctx->state);
+		}
+	}
+out:
+	spin_unlock(&device_data->power_state_lock);
+
+	return ret;
+}
+
+/**
+ * hash_get_device_data - Checks for an available hash device and return it.
+ * @hash_ctx:		Structure for the hash context.
+ * @device_data:	Structure for the hash device.
+ *
+ * This function check for an available hash device and return it to
+ * the caller.
+ * Note! Caller need to release the device, calling up().
+ */
+static int hash_get_device_data(struct hash_ctx *ctx,
+				struct hash_device_data **device_data)
+{
+	int			ret;
+	struct klist_iter	device_iterator;
+	struct klist_node	*device_node;
+	struct hash_device_data *local_device_data = NULL;
+
+	/* Wait until a device is available */
+	ret = down_interruptible(&driver_data.device_allocation);
+	if (ret)
+		return ret;  /* Interrupted */
+
+	/* Select a device */
+	klist_iter_init(&driver_data.device_list, &device_iterator);
+	device_node = klist_next(&device_iterator);
+	while (device_node) {
+		local_device_data = container_of(device_node,
+					   struct hash_device_data, list_node);
+		spin_lock(&local_device_data->ctx_lock);
+		/* current_ctx allocates a device, NULL = unallocated */
+		if (local_device_data->current_ctx) {
+			device_node = klist_next(&device_iterator);
+		} else {
+			local_device_data->current_ctx = ctx;
+			ctx->device = local_device_data;
+			spin_unlock(&local_device_data->ctx_lock);
+			break;
+		}
+		spin_unlock(&local_device_data->ctx_lock);
+	}
+	klist_iter_exit(&device_iterator);
+
+	if (!device_node) {
+		/**
+		 * No free device found.
+		 * Since we allocated a device with down_interruptible, this
+		 * should not be able to happen.
+		 * Number of available devices, which are contained in
+		 * device_allocation, is therefore decremented by not doing
+		 * an up(device_allocation).
+		 */
+		return -EBUSY;
+	}
+
+	*device_data = local_device_data;
+
+	return 0;
+}
+
+/**
+ * hash_hw_write_key - Writes the key to the hardware registries.
+ *
+ * @device_data:	Structure for the hash device.
+ * @key:		Key to be written.
+ * @keylen:		The lengt of the key.
+ *
+ * Note! This function DOES NOT write to the NBLW registry, even though
+ * specified in the the hw design spec. Either due to incorrect info in the
+ * spec or due to a bug in the hw.
+ */
+static void hash_hw_write_key(struct hash_device_data *device_data,
+		const u8 *key, unsigned int keylen)
+{
+	u32 word = 0;
+	int nwords = 1;
+
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+
+	while (keylen >= 4) {
+		word = ((u32) (key[3] & 0xff) << 24) |
+			((u32) (key[2] & 0xff) << 16) |
+			((u32) (key[1] & 0xff) << 8) |
+			((u32) (key[0] & 0xff));
+
+		HASH_SET_DIN(&word, nwords);
+		keylen -= 4;
+		key += 4;
+	}
+
+	/* Take care of the remaining bytes in the last word */
+	if (keylen) {
+		word = 0;
+		while (keylen) {
+			word |= (key[keylen - 1] << (8 * (keylen - 1)));
+			keylen--;
+		}
+
+		HASH_SET_DIN(&word, nwords);
+	}
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	HASH_SET_DCAL;
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+}
+
+/**
+ * init_hash_hw - Initialise the hash hardware for a new calculation.
+ * @device_data:	Structure for the hash device.
+ * @ctx:		The hash context.
+ *
+ * This function will enable the bits needed to clear and start a new
+ * calculation.
+ */
+static int init_hash_hw(struct hash_device_data *device_data,
+		struct hash_ctx *ctx)
+{
+	int ret = 0;
+
+	ret = hash_setconfiguration(device_data, &ctx->config);
+	if (ret) {
+		dev_err(device_data->dev, "[%s] hash_setconfiguration() "
+				"failed!", __func__);
+		return ret;
+	}
+
+	hash_begin(device_data, ctx);
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC)
+		hash_hw_write_key(device_data, ctx->key, ctx->keylen);
+
+	return ret;
+}
+
+/**
+ * hash_get_nents - Return number of entries (nents) in scatterlist (sg).
+ *
+ * @sg:		Scatterlist.
+ * @size:	Size in bytes.
+ * @aligned:	True if sg data aligned to work in DMA mode.
+ *
+ * Reentrancy: Non Re-entrant
+ */
+static int hash_get_nents(struct scatterlist *sg, int size, bool *aligned)
+{
+	int nents = 0;
+	bool aligned_data = true;
+
+	while (size > 0 && sg) {
+		nents++;
+		size -= sg->length;
+
+		/* hash_set_dma_transfer will align last nent */
+		if ((aligned && !IS_ALIGNED(sg->offset, HASH_DMA_ALIGN_SIZE))
+			|| (!IS_ALIGNED(sg->length, HASH_DMA_ALIGN_SIZE) &&
+				size > 0))
+			aligned_data = false;
+
+		sg = sg_next(sg);
+	}
+
+	if (aligned)
+		*aligned = aligned_data;
+
+	if (size != 0)
+		return -EFAULT;
+
+	return nents;
+}
+
+/**
+ * hash_dma_valid_data - checks for dma valid sg data.
+ * @sg:		Scatterlist.
+ * @datasize:	Datasize in bytes.
+ *
+ * NOTE! This function checks for dma valid sg data, since dma
+ * only accept datasizes of even wordsize.
+ */
+static bool hash_dma_valid_data(struct scatterlist *sg, int datasize)
+{
+	bool aligned;
+
+	/* Need to include at least one nent, else error */
+	if (hash_get_nents(sg, datasize, &aligned) < 1)
+		return false;
+
+	return aligned;
+}
+
+/**
+ * hash_init - Common hash init function for SHA1/SHA2 (SHA256).
+ * @req: The hash request for the job.
+ *
+ * Initialize structures.
+ */
+static int hash_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	if (!ctx->key)
+		ctx->keylen = 0;
+
+	memset(&ctx->state, 0, sizeof(struct hash_state));
+	ctx->updated = 0;
+	if (hash_mode == HASH_MODE_DMA) {
+		if ((ctx->config.oper_mode == HASH_OPER_MODE_HMAC) &&
+				cpu_is_u5500()) {
+			pr_debug(DEV_DBG_NAME " [%s] HMAC and DMA not working "
+					"on u5500, directing to CPU mode.",
+					__func__);
+			ctx->dma_mode = false; /* Don't use DMA in this case */
+			goto out;
+		}
+
+		if (req->nbytes < HASH_DMA_ALIGN_SIZE) {
+			ctx->dma_mode = false; /* Don't use DMA in this case */
+
+			pr_debug(DEV_DBG_NAME " [%s] DMA mode, but direct "
+					"to CPU mode for data size < %d",
+					__func__, HASH_DMA_ALIGN_SIZE);
+		} else {
+			if (req->nbytes >= HASH_DMA_PERFORMANCE_MIN_SIZE &&
+					hash_dma_valid_data(req->src,
+						req->nbytes)) {
+				ctx->dma_mode = true;
+			} else {
+				ctx->dma_mode = false;
+				pr_debug(DEV_DBG_NAME " [%s] DMA mode, but use"
+						" CPU mode for datalength < %d"
+						" or non-aligned data, except "
+						"in last nent", __func__,
+						HASH_DMA_PERFORMANCE_MIN_SIZE);
+			}
+		}
+	}
+out:
+	return 0;
+}
+
+/**
+ * hash_processblock - This function processes a single block of 512 bits (64
+ *                     bytes), word aligned, starting at message.
+ * @device_data:	Structure for the hash device.
+ * @message:		Block (512 bits) of message to be written to
+ *			the HASH hardware.
+ *
+ * Reentrancy: Non Re-entrant.
+ */
+static void hash_processblock(
+		struct hash_device_data *device_data,
+		const u32 *message, int length)
+{
+	int len = length / HASH_BYTES_PER_WORD;
+	/*
+	 * NBLW bits. Reset the number of bits in last word (NBLW).
+	 */
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+
+	/*
+	 * Write message data to the HASH_DIN register.
+	 */
+	HASH_SET_DIN(message, len);
+}
+
+/**
+ * hash_messagepad - Pads a message and write the nblw bits.
+ * @device_data:	Structure for the hash device.
+ * @message:		Last word of a message.
+ * @index_bytes:	The number of bytes in the last message.
+ *
+ * This function manages the final part of the digest calculation, when less
+ * than 512 bits (64 bytes) remain in message. This means index_bytes < 64.
+ *
+ * Reentrancy: Non Re-entrant.
+ */
+static void hash_messagepad(struct hash_device_data *device_data,
+		const u32 *message, u8 index_bytes)
+{
+	int nwords = 1;
+
+	/*
+	 * Clear hash str register, only clear NBLW
+	 * since DCAL will be reset by hardware.
+	 */
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+
+	/* Main loop */
+	while (index_bytes >= 4) {
+		HASH_SET_DIN(message, nwords);
+		index_bytes -= 4;
+		message++;
+	}
+
+	if (index_bytes)
+		HASH_SET_DIN(message, nwords);
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	/* num_of_bytes == 0 => NBLW <- 0 (32 bits valid in DATAIN) */
+	HASH_SET_NBLW(index_bytes * 8);
+	dev_dbg(device_data->dev, "[%s] DIN=0x%08x NBLW=%d", __func__,
+			readl_relaxed(&device_data->base->din),
+			(int)(readl_relaxed(&device_data->base->str) &
+				HASH_STR_NBLW_MASK));
+	HASH_SET_DCAL;
+	dev_dbg(device_data->dev, "[%s] after dcal -> DIN=0x%08x NBLW=%d",
+			__func__, readl_relaxed(&device_data->base->din),
+			(int)(readl_relaxed(&device_data->base->str) &
+				HASH_STR_NBLW_MASK));
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+}
+
+/**
+ * hash_incrementlength - Increments the length of the current message.
+ * @ctx: Hash context
+ * @incr: Length of message processed already
+ *
+ * Overflow cannot occur, because conditions for overflow are checked in
+ * hash_hw_update.
+ */
+static void hash_incrementlength(struct hash_ctx *ctx, u32 incr)
+{
+	ctx->state.length.low_word += incr;
+
+	/* Check for wrap-around */
+	if (ctx->state.length.low_word < incr)
+		ctx->state.length.high_word++;
+}
+
+/**
+ * hash_setconfiguration - Sets the required configuration for the hash
+ *                         hardware.
+ * @device_data:	Structure for the hash device.
+ * @config:		Pointer to a configuration structure.
+ *
+ * Reentrancy: Non Re-entrant
+ * Reentrancy issues:
+ *              1. Global variable registry(cofiguration register,
+ *                 parameter register, divider register) is being modified
+ *
+ * Comments     1.  : User need to call hash_begin API after calling this
+ *                    API i.e. the current configuration is set only when
+ *                    bit INIT is set and we set INIT bit in hash_begin.
+ *                    Changing the configuration during a computation has
+ *                    no effect so we first set configuration by calling
+ *                    this API and then set the INIT bit for the HASH
+ *                    processor and the curent configuration is taken into
+ *                    account. As reading INIT bit (with correct protection
+ *                    rights) will always return 0b so we can't make a check
+ *                    at software level. So the user has to initialize the
+ *                    device for new configuration to take in to effect.
+ *              2.    The default value of data format is 00b ie the format
+ *                    of data entered in HASH_DIN register is 32-bit data.
+ *                    The data written in HASH_DIN is used directly by the
+ *                    HASH processing, without re ordering.
+ */
+int hash_setconfiguration(struct hash_device_data *device_data,
+		struct hash_config *config)
+{
+	int ret = 0;
+
+	if (config->algorithm != HASH_ALGO_SHA1 &&
+	    config->algorithm != HASH_ALGO_SHA256)
+		return -EPERM;
+
+	/*
+	 * DATAFORM bits. Set the DATAFORM bits to 0b11, which means the data
+	 * to be written to HASH_DIN is considered as 32 bits.
+	 */
+	HASH_SET_DATA_FORMAT(config->data_format);
+
+	/*
+	 * ALGO bit. Set to 0b1 for SHA-1 and 0b0 for SHA-256
+	 */
+	switch (config->algorithm) {
+	case HASH_ALGO_SHA1:
+		HASH_SET_BITS(&device_data->base->cr, HASH_CR_ALGO_MASK);
+		break;
+
+	case HASH_ALGO_SHA256:
+		HASH_CLEAR_BITS(&device_data->base->cr, HASH_CR_ALGO_MASK);
+		break;
+
+	default:
+		dev_err(device_data->dev, "[%s] Incorrect algorithm.",
+				__func__);
+		return -EPERM;
+	}
+
+	/*
+	 * MODE bit. This bit selects between HASH or HMAC mode for the
+	 * selected algorithm. 0b0 = HASH and 0b1 = HMAC.
+	 */
+	if (HASH_OPER_MODE_HASH == config->oper_mode)
+		HASH_CLEAR_BITS(&device_data->base->cr,
+				HASH_CR_MODE_MASK);
+	else if (HASH_OPER_MODE_HMAC == config->oper_mode) {
+		HASH_SET_BITS(&device_data->base->cr,
+				HASH_CR_MODE_MASK);
+		if (device_data->current_ctx->keylen > HASH_BLOCK_SIZE) {
+			/* Truncate key to blocksize */
+			dev_dbg(device_data->dev, "[%s] LKEY set", __func__);
+			HASH_SET_BITS(&device_data->base->cr,
+					HASH_CR_LKEY_MASK);
+		} else {
+			dev_dbg(device_data->dev, "[%s] LKEY cleared",
+					__func__);
+			HASH_CLEAR_BITS(&device_data->base->cr,
+					HASH_CR_LKEY_MASK);
+		}
+	} else {	/* Wrong hash mode */
+		ret = -EPERM;
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+	}
+	return ret;
+}
+
+/**
+ * hash_begin - This routine resets some globals and initializes the hash
+ *              hardware.
+ * @device_data:	Structure for the hash device.
+ * @ctx:		Hash context.
+ *
+ * Reentrancy: Non Re-entrant
+ *
+ * Comments     1.  : User need to call hash_setconfiguration API before
+ *                    calling this API i.e. the current configuration is set
+ *                    only when bit INIT is set and we set INIT bit in
+ *                    hash_begin. Changing the configuration during a
+ *                    computation has no effect so we first set
+ *                    configuration by calling this API and then set the
+ *                    INIT bit for the HASH processor and the current
+ *                    configuration is taken into account. As reading INIT
+ *                    bit (with correct protection rights) will always
+ *                    return 0b so we can't make a check at software level.
+ *                    So the user has to initialize the device for new
+ *                    configuration to take in to effect.
+ */
+void hash_begin(struct hash_device_data *device_data, struct hash_ctx *ctx)
+{
+	/* HW and SW initializations */
+	/* Note: there is no need to initialize buffer and digest members */
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	/*
+	 * INIT bit. Set this bit to 0b1 to reset the HASH processor core and
+	 * prepare the initialize the HASH accelerator to compute the message
+	 * digest of a new message.
+	 */
+	HASH_INITIALIZE;
+
+	/*
+	 * NBLW bits. Reset the number of bits in last word (NBLW).
+	 */
+	HASH_CLEAR_BITS(&device_data->base->str, HASH_STR_NBLW_MASK);
+}
+
+int hash_process_data(
+		struct hash_device_data *device_data,
+		struct hash_ctx *ctx, int msg_length, u8 *data_buffer,
+		u8 *buffer, u8 *index)
+{
+	int ret = 0;
+	u32 count;
+
+	do {
+		if ((*index + msg_length) < HASH_BLOCK_SIZE) {
+			for (count = 0; count < msg_length; count++) {
+				buffer[*index + count] =
+					*(data_buffer + count);
+			}
+			*index += msg_length;
+			msg_length = 0;
+		} else {
+			if (ctx->updated) {
+
+				ret = hash_resume_state(device_data,
+						&ctx->state);
+				if (ret) {
+					dev_err(device_data->dev, "[%s] "
+							"hash_resume_state()"
+							" failed!", __func__);
+					goto out;
+				}
+			} else {
+				ret = init_hash_hw(device_data, ctx);
+				if (ret) {
+					dev_err(device_data->dev, "[%s] "
+							"init_hash_hw()"
+							" failed!", __func__);
+					goto out;
+				}
+				ctx->updated = 1;
+			}
+			/*
+			 * If 'data_buffer' is four byte aligned and
+			 * local buffer does not have any data, we can
+			 * write data directly from 'data_buffer' to
+			 * HW peripheral, otherwise we first copy data
+			 * to a local buffer
+			 */
+			if ((0 == (((u32)data_buffer) % 4))
+					&& (0 == *index))
+				hash_processblock(device_data,
+						(const u32 *)
+						data_buffer, HASH_BLOCK_SIZE);
+			else {
+				for (count = 0; count <
+						(u32)(HASH_BLOCK_SIZE -
+							*index);
+						count++) {
+					buffer[*index + count] =
+						*(data_buffer + count);
+				}
+				hash_processblock(device_data,
+						(const u32 *)buffer,
+						HASH_BLOCK_SIZE);
+			}
+			hash_incrementlength(ctx, HASH_BLOCK_SIZE);
+			data_buffer += (HASH_BLOCK_SIZE - *index);
+
+			msg_length -= (HASH_BLOCK_SIZE - *index);
+			*index = 0;
+
+			ret = hash_save_state(device_data,
+					&ctx->state);
+			if (ret) {
+				dev_err(device_data->dev, "[%s] "
+						"hash_save_state()"
+						" failed!", __func__);
+				goto out;
+			}
+		}
+	} while (msg_length != 0);
+out:
+
+	return ret;
+}
+
+/**
+ * hash_dma_final - The hash dma final function for SHA1/SHA256.
+ * @req:	The hash request for the job.
+ */
+static int hash_dma_final(struct ahash_request *req)
+{
+	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct hash_device_data *device_data;
+	u8 digest[SHA256_DIGEST_SIZE];
+	int bytes_written = 0;
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32) ctx);
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+				"hash_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	if (ctx->updated) {
+		ret = hash_resume_state(device_data, &ctx->state);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_resume_state() "
+					"failed!", __func__);
+			goto out_power;
+		}
+
+	}
+
+	if (!ctx->updated) {
+		ret = hash_setconfiguration(device_data, &ctx->config);
+		if (ret) {
+			dev_err(device_data->dev, "[%s] "
+					"hash_setconfiguration() failed!",
+					__func__);
+			goto out_power;
+		}
+
+		/* Enable DMA input */
+		if (hash_mode != HASH_MODE_DMA || !ctx->dma_mode) {
+			HASH_CLEAR_BITS(&device_data->base->cr,
+					HASH_CR_DMAE_MASK);
+		} else {
+			HASH_SET_BITS(&device_data->base->cr,
+					HASH_CR_DMAE_MASK);
+			HASH_SET_BITS(&device_data->base->cr,
+					HASH_CR_PRIVN_MASK);
+		}
+
+		HASH_INITIALIZE;
+
+		if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC)
+			hash_hw_write_key(device_data, ctx->key, ctx->keylen);
+
+		/* Number of bits in last word = (nbytes * 8) % 32 */
+		HASH_SET_NBLW((req->nbytes * 8) % 32);
+		ctx->updated = 1;
+	}
+
+	/* Store the nents in the dma struct. */
+	ctx->device->dma.nents = hash_get_nents(req->src, req->nbytes, NULL);
+	if (!ctx->device->dma.nents) {
+		dev_err(device_data->dev, "[%s] "
+				"ctx->device->dma.nents = 0", __func__);
+		goto out_power;
+	}
+
+	bytes_written = hash_dma_write(ctx, req->src, req->nbytes);
+	if (bytes_written != req->nbytes) {
+		dev_err(device_data->dev, "[%s] "
+				"hash_dma_write() failed!", __func__);
+		goto out_power;
+	}
+
+	wait_for_completion(&ctx->device->dma.complete);
+	hash_dma_done(ctx);
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC && ctx->key) {
+		unsigned int keylen = ctx->keylen;
+		u8 *key = ctx->key;
+
+		dev_dbg(device_data->dev, "[%s] keylen: %d", __func__,
+				ctx->keylen);
+		hash_hw_write_key(device_data, key, keylen);
+	}
+
+	hash_get_digest(device_data, digest, ctx->config.algorithm);
+	memcpy(req->result, digest, ctx->digestsize);
+
+out_power:
+	/* Disable power (and clock) */
+	if (hash_disable_power(device_data, false))
+		dev_err(device_data->dev, "[%s] hash_disable_power() failed!",
+				__func__);
+
+out:
+	release_hash_device(device_data);
+
+	/**
+	 * Allocated in setkey, and only used in HMAC.
+	 */
+	kfree(ctx->key);
+
+	return ret;
+}
+
+/**
+ * hash_hw_final - The final hash calculation function
+ * @req:	The hash request for the job.
+ */
+int hash_hw_final(struct ahash_request *req)
+{
+	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct hash_device_data *device_data;
+	u8 digest[SHA256_DIGEST_SIZE];
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32) ctx);
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+				"hash_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	if (ctx->updated) {
+		ret = hash_resume_state(device_data, &ctx->state);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_resume_state() "
+					"failed!", __func__);
+			goto out_power;
+		}
+	} else if (req->nbytes == 0 && ctx->keylen == 0) {
+		u8 zero_hash[SHA256_DIGEST_SIZE];
+		u32 zero_hash_size = 0;
+		bool zero_digest = false;
+		/**
+		 * Use a pre-calculated empty message digest
+		 * (workaround since hw return zeroes, hw bug!?)
+		 */
+		ret = get_empty_message_digest(device_data, &zero_hash[0],
+				&zero_hash_size, &zero_digest);
+		if (!ret && likely(zero_hash_size == ctx->digestsize) &&
+				zero_digest) {
+			memcpy(req->result, &zero_hash[0], ctx->digestsize);
+			goto out_power;
+		} else if (!ret && !zero_digest) {
+			dev_dbg(device_data->dev, "[%s] HMAC zero msg with "
+					"key, continue...", __func__);
+		} else {
+			dev_err(device_data->dev, "[%s] ret=%d, or wrong "
+					"digest size? %s", __func__, ret,
+					(zero_hash_size == ctx->digestsize) ?
+					"true" : "false");
+			/* Return error */
+			goto out_power;
+		}
+	} else if (req->nbytes == 0 && ctx->keylen > 0) {
+		dev_err(device_data->dev, "[%s] Empty message with "
+				"keylength > 0, NOT supported.", __func__);
+		goto out_power;
+	}
+
+	if (!ctx->updated) {
+		ret = init_hash_hw(device_data, ctx);
+		if (ret) {
+			dev_err(device_data->dev, "[%s] init_hash_hw() "
+					"failed!", __func__);
+			goto out_power;
+		}
+	}
+
+	if (ctx->state.index) {
+		hash_messagepad(device_data, ctx->state.buffer,
+				ctx->state.index);
+	} else {
+		HASH_SET_DCAL;
+		while (device_data->base->str & HASH_STR_DCAL_MASK)
+			cpu_relax();
+	}
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC && ctx->key) {
+		unsigned int keylen = ctx->keylen;
+		u8 *key = ctx->key;
+
+		dev_dbg(device_data->dev, "[%s] keylen: %d", __func__,
+				ctx->keylen);
+		hash_hw_write_key(device_data, key, keylen);
+	}
+
+	hash_get_digest(device_data, digest, ctx->config.algorithm);
+	memcpy(req->result, digest, ctx->digestsize);
+
+out_power:
+	/* Disable power (and clock) */
+	if (hash_disable_power(device_data, false))
+		dev_err(device_data->dev, "[%s] hash_disable_power() failed!",
+				__func__);
+
+out:
+	release_hash_device(device_data);
+
+	/**
+	 * Allocated in setkey, and only used in HMAC.
+	 */
+	kfree(ctx->key);
+
+	return ret;
+}
+
+/**
+ * hash_hw_update - Updates current HASH computation hashing another part of
+ *                  the message.
+ * @req:	Byte array containing the message to be hashed (caller
+ *		allocated).
+ *
+ * Reentrancy: Non Re-entrant
+ */
+int hash_hw_update(struct ahash_request *req)
+{
+	int ret = 0;
+	u8 index = 0;
+	u8 *buffer;
+	struct hash_device_data *device_data;
+	u8 *data_buffer;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct crypto_hash_walk walk;
+	int msg_length = crypto_hash_walk_first(req, &walk);
+
+	/* Empty message ("") is correct indata */
+	if (msg_length == 0)
+		return ret;
+
+	index = ctx->state.index;
+	buffer = (u8 *)ctx->state.buffer;
+
+	/* Check if ctx->state.length + msg_length
+	   overflows */
+	if (msg_length > (ctx->state.length.low_word + msg_length) &&
+			HASH_HIGH_WORD_MAX_VAL ==
+			ctx->state.length.high_word) {
+		pr_err(DEV_DBG_NAME " [%s] HASH_MSG_LENGTH_OVERFLOW!",
+				__func__);
+		return -EPERM;
+	}
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+				"hash_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	/* Main loop */
+	while (0 != msg_length) {
+		data_buffer = walk.data;
+		ret = hash_process_data(device_data, ctx,
+				msg_length, data_buffer, buffer, &index);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_internal_hw_"
+					"update() failed!", __func__);
+			goto out_power;
+		}
+
+		msg_length = crypto_hash_walk_done(&walk, 0);
+	}
+
+	ctx->state.index = index;
+	dev_dbg(device_data->dev, "[%s] indata length=%d, "
+		"bin=%d))", __func__, ctx->state.index, ctx->state.bit_index);
+
+out_power:
+	/* Disable power (and clock) */
+	if (hash_disable_power(device_data, false))
+		dev_err(device_data->dev, "[%s]: "
+				"hash_disable_power() failed!", __func__);
+out:
+	release_hash_device(device_data);
+
+	return ret;
+}
+
+/**
+ * hash_resume_state - Function that resumes the state of an calculation.
+ * @device_data:	Pointer to the device structure.
+ * @device_state:	The state to be restored in the hash hardware
+ *
+ * Reentrancy: Non Re-entrant
+ */
+int hash_resume_state(struct hash_device_data *device_data,
+		const struct hash_state *device_state)
+{
+	u32 temp_cr;
+	s32 count;
+	int hash_mode = HASH_OPER_MODE_HASH;
+
+	if (NULL == device_state) {
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+		return -EPERM;
+	}
+
+	/* Check correctness of index and length members */
+	if (device_state->index > HASH_BLOCK_SIZE
+	    || (device_state->length.low_word % HASH_BLOCK_SIZE) != 0) {
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+		return -EPERM;
+	}
+
+	/*
+	 * INIT bit. Set this bit to 0b1 to reset the HASH processor core and
+	 * prepare the initialize the HASH accelerator to compute the message
+	 * digest of a new message.
+	 */
+	HASH_INITIALIZE;
+
+	temp_cr = device_state->temp_cr;
+	writel_relaxed(temp_cr & HASH_CR_RESUME_MASK, &device_data->base->cr);
+
+	if (device_data->base->cr & HASH_CR_MODE_MASK)
+		hash_mode = HASH_OPER_MODE_HMAC;
+	else
+		hash_mode = HASH_OPER_MODE_HASH;
+
+	for (count = 0; count < HASH_CSR_COUNT; count++) {
+		if ((count >= 36) && (hash_mode == HASH_OPER_MODE_HASH))
+			break;
+
+		writel_relaxed(device_state->csr[count],
+				&device_data->base->csrx[count]);
+	}
+
+	writel_relaxed(device_state->csfull, &device_data->base->csfull);
+	writel_relaxed(device_state->csdatain, &device_data->base->csdatain);
+
+	writel_relaxed(device_state->str_reg, &device_data->base->str);
+	writel_relaxed(temp_cr, &device_data->base->cr);
+
+	return 0;
+}
+
+/**
+ * hash_save_state - Function that saves the state of hardware.
+ * @device_data:	Pointer to the device structure.
+ * @device_state:	The strucure where the hardware state should be saved.
+ *
+ * Reentrancy: Non Re-entrant
+ */
+int hash_save_state(struct hash_device_data *device_data,
+		struct hash_state *device_state)
+{
+	u32 temp_cr;
+	u32 count;
+	int hash_mode = HASH_OPER_MODE_HASH;
+
+	if (NULL == device_state) {
+		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
+				__func__);
+		return -EPERM;
+	}
+
+	/* Write dummy value to force digest intermediate calculation. This
+	 * actually makes sure that there isn't any ongoing calculation in the
+	 * hardware.
+	 */
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	temp_cr = readl_relaxed(&device_data->base->cr);
+
+	device_state->str_reg = readl_relaxed(&device_data->base->str);
+
+	device_state->din_reg = readl_relaxed(&device_data->base->din);
+
+	if (device_data->base->cr & HASH_CR_MODE_MASK)
+		hash_mode = HASH_OPER_MODE_HMAC;
+	else
+		hash_mode = HASH_OPER_MODE_HASH;
+
+	for (count = 0; count < HASH_CSR_COUNT; count++) {
+		if ((count >= 36) && (hash_mode == HASH_OPER_MODE_HASH))
+			break;
+
+		device_state->csr[count] =
+			readl_relaxed(&device_data->base->csrx[count]);
+	}
+
+	device_state->csfull = readl_relaxed(&device_data->base->csfull);
+	device_state->csdatain = readl_relaxed(&device_data->base->csdatain);
+
+	device_state->temp_cr = temp_cr;
+
+	return 0;
+}
+
+/**
+ * hash_check_hw - This routine checks for peripheral Ids and PCell Ids.
+ * @device_data:
+ *
+ */
+int hash_check_hw(struct hash_device_data *device_data)
+{
+	int ret = 0;
+
+	if (NULL == device_data) {
+		ret = -EPERM;
+		pr_err(DEV_DBG_NAME " [%s] HASH_INVALID_PARAMETER!",
+			__func__);
+		goto out;
+	}
+
+	/* Checking Peripheral Ids  */
+	if ((HASH_P_ID0 == readl_relaxed(&device_data->base->periphid0))
+		&& (HASH_P_ID1 == readl_relaxed(&device_data->base->periphid1))
+		&& (HASH_P_ID2 == readl_relaxed(&device_data->base->periphid2))
+		&& (HASH_P_ID3 == readl_relaxed(&device_data->base->periphid3))
+		&& (HASH_CELL_ID0 == readl_relaxed(&device_data->base->cellid0))
+		&& (HASH_CELL_ID1 == readl_relaxed(&device_data->base->cellid1))
+		&& (HASH_CELL_ID2 == readl_relaxed(&device_data->base->cellid2))
+		&& (HASH_CELL_ID3 == readl_relaxed(&device_data->base->cellid3))
+	   ) {
+		ret = 0;
+		goto out;;
+	} else {
+		ret = -EPERM;
+		dev_err(device_data->dev, "[%s] HASH_UNSUPPORTED_HW!",
+				__func__);
+		goto out;
+	}
+out:
+	return ret;
+}
+
+/**
+ * hash_get_digest - Gets the digest.
+ * @device_data:	Pointer to the device structure.
+ * @digest:		User allocated byte array for the calculated digest.
+ * @algorithm:		The algorithm in use.
+ *
+ * Reentrancy: Non Re-entrant, global variable registry (hash control register)
+ * is being modified.
+ *
+ * Note that, if this is called before the final message has been handle it
+ * will return the intermediate message digest.
+ */
+void hash_get_digest(struct hash_device_data *device_data,
+		u8 *digest, int algorithm)
+{
+	u32 temp_hx_val, count;
+	int loop_ctr;
+
+	if (algorithm != HASH_ALGO_SHA1 && algorithm != HASH_ALGO_SHA256) {
+		dev_err(device_data->dev, "[%s] Incorrect algorithm %d",
+				__func__, algorithm);
+		return;
+	}
+
+	if (algorithm == HASH_ALGO_SHA1)
+		loop_ctr = SHA1_DIGEST_SIZE / sizeof(u32);
+	else
+		loop_ctr = SHA256_DIGEST_SIZE / sizeof(u32);
+
+	dev_dbg(device_data->dev, "[%s] digest array:(0x%x)",
+			__func__, (u32) digest);
+
+	/* Copy result into digest array */
+	for (count = 0; count < loop_ctr; count++) {
+		temp_hx_val = readl_relaxed(&device_data->base->hx[count]);
+		digest[count * 4] = (u8) ((temp_hx_val >> 24) & 0xFF);
+		digest[count * 4 + 1] = (u8) ((temp_hx_val >> 16) & 0xFF);
+		digest[count * 4 + 2] = (u8) ((temp_hx_val >> 8) & 0xFF);
+		digest[count * 4 + 3] = (u8) ((temp_hx_val >> 0) & 0xFF);
+	}
+}
+
+/**
+ * hash_update - The hash update function for SHA1/SHA2 (SHA256).
+ * @req: The hash request for the job.
+ */
+static int ahash_update(struct ahash_request *req)
+{
+	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	if (hash_mode != HASH_MODE_DMA || !ctx->dma_mode)
+		ret = hash_hw_update(req);
+	/* Skip update for DMA, all data will be passed to DMA in final */
+
+	if (ret) {
+		pr_err(DEV_DBG_NAME " [%s] hash_hw_update() failed!",
+				__func__);
+	}
+
+	return ret;
+}
+
+/**
+ * hash_final - The hash final function for SHA1/SHA2 (SHA256).
+ * @req:	The hash request for the job.
+ */
+static int ahash_final(struct ahash_request *req)
+{
+	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	pr_debug(DEV_DBG_NAME " [%s] data size: %d", __func__, req->nbytes);
+
+	if ((hash_mode == HASH_MODE_DMA) && ctx->dma_mode)
+		ret = hash_dma_final(req);
+	else
+		ret = hash_hw_final(req);
+
+	if (ret) {
+		pr_err(DEV_DBG_NAME " [%s] hash_hw/dma_final() failed",
+				__func__);
+	}
+
+	return ret;
+}
+
+static int hash_setkey(struct crypto_ahash *tfm,
+		const u8 *key, unsigned int keylen, int alg)
+{
+	int ret = 0;
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	/**
+	 * Freed in final.
+	 */
+	ctx->key = kmalloc(keylen, GFP_KERNEL);
+	if (!ctx->key) {
+		pr_err(DEV_DBG_NAME " [%s] Failed to allocate ctx->key "
+		       "for %d\n", __func__, alg);
+		return -ENOMEM;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	return ret;
+ }
+
+static int ahash_sha1_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ctx->config.data_format = HASH_DATA_8_BITS;
+	ctx->config.algorithm = HASH_ALGO_SHA1;
+	ctx->config.oper_mode = HASH_OPER_MODE_HASH;
+	ctx->digestsize = SHA1_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int ahash_sha256_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ctx->config.data_format = HASH_DATA_8_BITS;
+	ctx->config.algorithm = HASH_ALGO_SHA256;
+	ctx->config.oper_mode = HASH_OPER_MODE_HASH;
+	ctx->digestsize = SHA256_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int ahash_sha1_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	ret1 = ahash_sha1_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int ahash_sha256_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	ret1 = ahash_sha256_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int hmac_sha1_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ctx->config.data_format	= HASH_DATA_8_BITS;
+	ctx->config.algorithm	= HASH_ALGO_SHA1;
+	ctx->config.oper_mode	= HASH_OPER_MODE_HMAC;
+	ctx->digestsize		= SHA1_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int hmac_sha256_init(struct ahash_request *req)
+{
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+
+	ctx->config.data_format	= HASH_DATA_8_BITS;
+	ctx->config.algorithm	= HASH_ALGO_SHA256;
+	ctx->config.oper_mode	= HASH_OPER_MODE_HMAC;
+	ctx->digestsize		= SHA256_DIGEST_SIZE;
+
+	return hash_init(req);
+}
+
+static int hmac_sha1_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	ret1 = hmac_sha1_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int hmac_sha256_digest(struct ahash_request *req)
+{
+	int ret2, ret1;
+
+	ret1 = hmac_sha256_init(req);
+	if (ret1)
+		goto out;
+
+	ret1 = ahash_update(req);
+	ret2 = ahash_final(req);
+
+out:
+	return ret1 ? ret1 : ret2;
+}
+
+static int hmac_sha1_setkey(struct crypto_ahash *tfm,
+		const u8 *key, unsigned int keylen)
+{
+	return hash_setkey(tfm, key, keylen, HASH_ALGO_SHA1);
+}
+
+static int hmac_sha256_setkey(struct crypto_ahash *tfm,
+		const u8 *key, unsigned int keylen)
+{
+	return hash_setkey(tfm, key, keylen, HASH_ALGO_SHA256);
+}
+
+static struct ahash_alg ahash_sha1_alg = {
+	.init			 = ahash_sha1_init,
+	.update			 = ahash_update,
+	.final			 = ahash_final,
+	.digest			 = ahash_sha1_digest,
+	.halg.digestsize	 = SHA1_DIGEST_SIZE,
+	.halg.statesize		 = sizeof(struct hash_ctx),
+	.halg.base = {
+		.cra_name	 = "sha1",
+		.cra_driver_name = "sha1-ux500",
+		.cra_flags	 = CRYPTO_ALG_TYPE_AHASH | CRYPTO_ALG_ASYNC,
+		.cra_blocksize	 = SHA1_BLOCK_SIZE,
+		.cra_ctxsize	 = sizeof(struct hash_ctx),
+		.cra_module	 = THIS_MODULE,
+	}
+};
+
+static struct ahash_alg ahash_sha256_alg = {
+	.init			 = ahash_sha256_init,
+	.update			 = ahash_update,
+	.final			 = ahash_final,
+	.digest			 = ahash_sha256_digest,
+	.halg.digestsize	 = SHA256_DIGEST_SIZE,
+	.halg.statesize		 = sizeof(struct hash_ctx),
+	.halg.base = {
+		.cra_name        = "sha256",
+		.cra_driver_name = "sha256-ux500",
+		.cra_flags       = CRYPTO_ALG_TYPE_AHASH | CRYPTO_ALG_ASYNC,
+		.cra_blocksize   = SHA256_BLOCK_SIZE,
+		.cra_ctxsize	 = sizeof(struct hash_ctx),
+		.cra_type	 = &crypto_ahash_type,
+		.cra_module      = THIS_MODULE,
+	}
+};
+
+static struct ahash_alg hmac_sha1_alg = {
+	.init			 = hmac_sha1_init,
+	.update			 = ahash_update,
+	.final			 = ahash_final,
+	.digest			 = hmac_sha1_digest,
+	.setkey			 = hmac_sha1_setkey,
+	.halg.digestsize	 = SHA1_DIGEST_SIZE,
+	.halg.statesize		 = sizeof(struct hash_ctx),
+	.halg.base = {
+		.cra_name        = "hmac(sha1)",
+		.cra_driver_name = "hmac-sha1-ux500",
+		.cra_flags       = CRYPTO_ALG_TYPE_AHASH | CRYPTO_ALG_ASYNC,
+		.cra_blocksize   = SHA1_BLOCK_SIZE,
+		.cra_ctxsize	 = sizeof(struct hash_ctx),
+		.cra_type	 = &crypto_ahash_type,
+		.cra_module      = THIS_MODULE,
+	}
+};
+
+static struct ahash_alg hmac_sha256_alg = {
+	.init			 = hmac_sha256_init,
+	.update			 = ahash_update,
+	.final			 = ahash_final,
+	.digest			 = hmac_sha256_digest,
+	.setkey			 = hmac_sha256_setkey,
+	.halg.digestsize	 = SHA256_DIGEST_SIZE,
+	.halg.statesize		 = sizeof(struct hash_ctx),
+	.halg.base = {
+		.cra_name        = "hmac(sha256)",
+		.cra_driver_name = "hmac-sha256-ux500",
+		.cra_flags       = CRYPTO_ALG_TYPE_AHASH | CRYPTO_ALG_ASYNC,
+		.cra_blocksize   = SHA256_BLOCK_SIZE,
+		.cra_ctxsize	 = sizeof(struct hash_ctx),
+		.cra_type	 = &crypto_ahash_type,
+		.cra_module      = THIS_MODULE,
+	}
+};
+
+/**
+ * struct hash_alg *ux500_hash_algs[] -
+ */
+static struct ahash_alg *ux500_ahash_algs[] = {
+	&ahash_sha1_alg,
+	&ahash_sha256_alg,
+	&hmac_sha1_alg,
+	&hmac_sha256_alg
+};
+
+/**
+ * hash_algs_register_all -
+ */
+static int ahash_algs_register_all(struct hash_device_data *device_data)
+{
+	int ret;
+	int i;
+	int count;
+
+	for (i = 0; i < ARRAY_SIZE(ux500_ahash_algs); i++) {
+		ret = crypto_register_ahash(ux500_ahash_algs[i]);
+		if (ret) {
+			count = i;
+			dev_err(device_data->dev, "[%s] alg registration"
+					" failed",
+				ux500_ahash_algs[i]->halg.base.cra_driver_name);
+			goto unreg;
+		}
+	}
+	return 0;
+unreg:
+	for (i = 0; i < count; i++)
+		crypto_unregister_ahash(ux500_ahash_algs[i]);
+	return ret;
+}
+
+/**
+ * hash_algs_unregister_all -
+ */
+static void ahash_algs_unregister_all(struct hash_device_data *device_data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ux500_ahash_algs); i++)
+		crypto_unregister_ahash(ux500_ahash_algs[i]);
+}
+
+/**
+ * ux500_hash_probe - Function that probes the hash hardware.
+ * @pdev: The platform device.
+ */
+static int ux500_hash_probe(struct platform_device *pdev)
+{
+	int			ret = 0;
+	struct resource		*res = NULL;
+	struct hash_device_data *device_data;
+	struct device		*dev = &pdev->dev;
+
+	device_data = kzalloc(sizeof(struct hash_device_data), GFP_ATOMIC);
+	if (!device_data) {
+		dev_dbg(dev, "[%s] kzalloc() failed!", __func__);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	device_data->dev = dev;
+	device_data->current_ctx = NULL;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_dbg(dev, "[%s] platform_get_resource() failed!", __func__);
+		ret = -ENODEV;
+		goto out_kfree;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (res == NULL) {
+		dev_dbg(dev, "[%s] request_mem_region() failed!", __func__);
+		ret = -EBUSY;
+		goto out_kfree;
+	}
+
+	device_data->base = ioremap(res->start, resource_size(res));
+	if (!device_data->base) {
+		dev_err(dev, "[%s] ioremap() failed!",
+				__func__);
+		ret = -ENOMEM;
+		goto out_free_mem;
+	}
+	spin_lock_init(&device_data->ctx_lock);
+	spin_lock_init(&device_data->power_state_lock);
+
+	/* Enable power for HASH1 hardware block */
+	device_data->regulator = ux500_regulator_get(dev);
+	if (IS_ERR(device_data->regulator)) {
+		dev_err(dev, "[%s] regulator_get() failed!", __func__);
+		ret = PTR_ERR(device_data->regulator);
+		device_data->regulator = NULL;
+		goto out_unmap;
+	}
+
+	/* Enable the clock for HASH1 hardware block */
+	device_data->clk = clk_get(dev, NULL);
+	if (IS_ERR(device_data->clk)) {
+		dev_err(dev, "[%s] clk_get() failed!", __func__);
+		ret = PTR_ERR(device_data->clk);
+		goto out_regulator;
+	}
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(dev, "[%s]: hash_enable_power() failed!", __func__);
+		goto out_clk;
+	}
+
+	ret = hash_check_hw(device_data);
+	if (ret) {
+		dev_err(dev, "[%s] hash_check_hw() failed!", __func__);
+		goto out_power;
+	}
+
+	if (hash_mode == HASH_MODE_DMA)
+		hash_dma_setup_channel(device_data, dev);
+
+	platform_set_drvdata(pdev, device_data);
+
+	/* Put the new device into the device list... */
+	klist_add_tail(&device_data->list_node, &driver_data.device_list);
+	/* ... and signal that a new device is available. */
+	up(&driver_data.device_allocation);
+
+	ret = ahash_algs_register_all(device_data);
+	if (ret) {
+		dev_err(dev, "[%s] ahash_algs_register_all() "
+				"failed!", __func__);
+		goto out_power;
+	}
+
+	if (hash_disable_power(device_data, false))
+		dev_err(dev, "[%s]: hash_disable_power() failed!", __func__);
+
+	dev_info(dev, "[%s] successfully probed\n", __func__);
+	return 0;
+
+out_power:
+	hash_disable_power(device_data, false);
+
+out_clk:
+	clk_put(device_data->clk);
+
+out_regulator:
+	ux500_regulator_put(device_data->regulator);
+
+out_unmap:
+	iounmap(device_data->base);
+
+out_free_mem:
+	release_mem_region(res->start, resource_size(res));
+
+out_kfree:
+	kfree(device_data);
+out:
+	return ret;
+}
+
+/**
+ * ux500_hash_remove - Function that removes the hash device from the platform.
+ * @pdev: The platform device.
+ */
+static int ux500_hash_remove(struct platform_device *pdev)
+{
+	struct resource		*res;
+	struct hash_device_data *device_data;
+	struct device		*dev = &pdev->dev;
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(dev, "[%s]: platform_get_drvdata() failed!",
+			__func__);
+		return -ENOMEM;
+	}
+
+	/* Try to decrease the number of available devices. */
+	if (down_trylock(&driver_data.device_allocation))
+		return -EBUSY;
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (device_data->current_ctx) {
+		/* The device is busy */
+		spin_unlock(&device_data->ctx_lock);
+		/* Return the device to the pool. */
+		up(&driver_data.device_allocation);
+		return -EBUSY;
+	}
+
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		ahash_algs_unregister_all(device_data);
+
+	if (hash_disable_power(device_data, false))
+		dev_err(dev, "[%s]: hash_disable_power() failed",
+			__func__);
+
+	clk_put(device_data->clk);
+	ux500_regulator_put(device_data->regulator);
+
+	iounmap(device_data->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+
+	kfree(device_data);
+
+	return 0;
+}
+
+/**
+ * ux500_hash_shutdown - Function that shutdown the hash device.
+ * @pdev: The platform device
+ */
+static void ux500_hash_shutdown(struct platform_device *pdev)
+{
+	struct resource *res = NULL;
+	struct hash_device_data *device_data;
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
+				__func__);
+		return;
+	}
+
+	/* Check that the device is free */
+	spin_lock(&device_data->ctx_lock);
+	/* current_ctx allocates a device, NULL = unallocated */
+	if (!device_data->current_ctx) {
+		if (down_trylock(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: Cryp still in use!"
+				"Shutting down anyway...", __func__);
+		/**
+		 * (Allocate the device)
+		 * Need to set this to non-null (dummy) value,
+		 * to avoid usage if context switching.
+		 */
+		device_data->current_ctx++;
+	}
+	spin_unlock(&device_data->ctx_lock);
+
+	/* Remove the device from the list */
+	if (klist_node_attached(&device_data->list_node))
+		klist_remove(&device_data->list_node);
+
+	/* If this was the last device, remove the services */
+	if (list_empty(&driver_data.device_list.k_list))
+		ahash_algs_unregister_all(device_data);
+
+	iounmap(device_data->base);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+
+	if (hash_disable_power(device_data, false))
+		dev_err(&pdev->dev, "[%s] hash_disable_power() failed",
+				__func__);
+}
+
+/**
+ * ux500_hash_suspend - Function that suspends the hash device.
+ * @pdev:	The platform device.
+ * @state:	-
+ */
+static int ux500_hash_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int ret;
+	struct hash_device_data *device_data;
+	struct hash_ctx *temp_ctx = NULL;
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
+				__func__);
+		return -ENOMEM;
+	}
+
+	spin_lock(&device_data->ctx_lock);
+	if (!device_data->current_ctx)
+		device_data->current_ctx++;
+	spin_unlock(&device_data->ctx_lock);
+
+	if (device_data->current_ctx == ++temp_ctx) {
+		if (down_interruptible(&driver_data.device_allocation))
+			dev_dbg(&pdev->dev, "[%s]: down_interruptible() "
+					"failed", __func__);
+		ret = hash_disable_power(device_data, false);
+
+	} else
+		ret = hash_disable_power(device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: hash_disable_power()", __func__);
+
+	return ret;
+}
+
+/**
+ * ux500_hash_resume - Function that resume the hash device.
+ * @pdev:	The platform device.
+ */
+static int ux500_hash_resume(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct hash_device_data *device_data;
+	struct hash_ctx *temp_ctx = NULL;
+
+	device_data = platform_get_drvdata(pdev);
+	if (!device_data) {
+		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
+				__func__);
+		return -ENOMEM;
+	}
+
+	spin_lock(&device_data->ctx_lock);
+	if (device_data->current_ctx == ++temp_ctx)
+		device_data->current_ctx = NULL;
+	spin_unlock(&device_data->ctx_lock);
+
+	if (!device_data->current_ctx)
+		up(&driver_data.device_allocation);
+	else
+		ret = hash_enable_power(device_data, true);
+
+	if (ret)
+		dev_err(&pdev->dev, "[%s]: hash_enable_power() failed!",
+			__func__);
+
+	return ret;
+}
+
+static struct platform_driver hash_driver = {
+	.probe  = ux500_hash_probe,
+	.remove = ux500_hash_remove,
+	.shutdown = ux500_hash_shutdown,
+	.suspend  = ux500_hash_suspend,
+	.resume   = ux500_hash_resume,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name  = "hash1",
+	}
+};
+
+/**
+ * ux500_hash_mod_init - The kernel module init function.
+ */
+static int __init ux500_hash_mod_init(void)
+{
+	klist_init(&driver_data.device_list, NULL, NULL);
+	/* Initialize the semaphore to 0 devices (locked state) */
+	sema_init(&driver_data.device_allocation, 0);
+
+	return platform_driver_register(&hash_driver);
+}
+
+/**
+ * ux500_hash_mod_fini - The kernel module exit function.
+ */
+static void __exit ux500_hash_mod_fini(void)
+{
+	platform_driver_unregister(&hash_driver);
+	return;
+}
+
+module_init(ux500_hash_mod_init);
+module_exit(ux500_hash_mod_fini);
+
+MODULE_DESCRIPTION("Driver for ST-Ericsson UX500 HASH engine.");
+MODULE_LICENSE("GPL");
+
+MODULE_ALIAS("sha1-all");
+MODULE_ALIAS("sha256-all");
+MODULE_ALIAS("hmac-sha1-all");
+MODULE_ALIAS("hmac-sha256-all");
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index 2ed1ac3513f..0fe43c3f598 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -345,6 +345,7 @@ struct d40_base {
 	int				  irq;
 	int				  num_phy_chans;
 	int				  num_log_chans;
+	struct device_dma_parameters	  dma_parms;
 	struct dma_device		  dma_both;
 	struct dma_device		  dma_slave;
 	struct dma_device		  dma_memcpy;
@@ -2577,6 +2578,14 @@ static int d40_set_runtime_config(struct dma_chan *chan,
 		return -EINVAL;
 	}
 
+	if (src_maxburst > 16) {
+		src_maxburst = 16;
+		dst_maxburst = src_maxburst * src_addr_width / dst_addr_width;
+	} else if (dst_maxburst > 16) {
+		dst_maxburst = 16;
+		src_maxburst = dst_maxburst * dst_addr_width / src_addr_width;
+	}
+
 	ret = dma40_config_to_halfchannel(d40c, &cfg->src_info,
 					  src_addr_width,
 					  src_maxburst);
@@ -2639,6 +2648,56 @@ static int d40_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 	return -ENXIO;
 }
 
+dma_addr_t stedma40_get_src_addr(struct dma_chan *chan)
+{
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	dma_addr_t addr;
+
+	if (chan_is_physical(d40c))
+		addr = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+			     d40c->phy_chan->num * D40_DREG_PCDELTA +
+			     D40_CHAN_REG_SSPTR);
+	else {
+		unsigned long lower;
+		unsigned long upper;
+
+		/*
+		 * There is a potential for overflow between the time the two
+		 * halves of the pointer are read.
+		 */
+		lower = d40c->lcpa->lcsp0 & D40_MEM_LCSP0_SPTR_MASK;
+		upper = d40c->lcpa->lcsp1 & D40_MEM_LCSP1_SPTR_MASK;
+
+		addr = upper | lower;
+	}
+
+	return addr;
+}
+EXPORT_SYMBOL(stedma40_get_src_addr);
+
+dma_addr_t stedma40_get_dst_addr(struct dma_chan *chan)
+{
+	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
+	dma_addr_t addr;
+
+	if (chan_is_physical(d40c))
+		addr = readl(d40c->base->virtbase + D40_DREG_PCBASE +
+			     d40c->phy_chan->num * D40_DREG_PCDELTA +
+			     D40_CHAN_REG_SDPTR);
+	else {
+		unsigned long lower;
+		unsigned long upper;
+
+		lower = d40c->lcpa->lcsp2 & D40_MEM_LCSP2_DPTR_MASK;
+		upper = d40c->lcpa->lcsp3 & D40_MEM_LCSP3_DPTR_MASK;
+
+		addr = upper | lower;
+	}
+
+	return addr;
+}
+EXPORT_SYMBOL(stedma40_get_dst_addr);
+
 /* Initialization functions */
 
 static void __init d40_chan_init(struct d40_base *base, struct dma_device *dma,
@@ -2773,8 +2832,6 @@ static int dma40_pm_suspend(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct d40_base *base = platform_get_drvdata(pdev);
 	int ret = 0;
-	if (!pm_runtime_suspended(dev))
-		return -EBUSY;
 
 	if (base->lcpa_regulator)
 		ret = regulator_disable(base->lcpa_regulator);
@@ -3358,6 +3415,13 @@ static int __init d40_probe(struct platform_device *pdev)
 	if (err)
 		goto failure;
 
+	base->dev->dma_parms = &base->dma_parms;
+	err = dma_set_max_seg_size(base->dev, 0xffff);
+	if (err) {
+		d40_err(&pdev->dev, "Failed to set dma max seg size\n");
+		goto failure;
+	}
+
 	d40_hw_init(base);
 
 	dev_info(base->dev, "initialized\n");
diff --git a/drivers/dma/ste_dma40_ll.c b/drivers/dma/ste_dma40_ll.c
index cad9e1daedf..d47d1fa36b9 100644
--- a/drivers/dma/ste_dma40_ll.c
+++ b/drivers/dma/ste_dma40_ll.c
@@ -102,16 +102,18 @@ void d40_phy_cfg(struct stedma40_chan_cfg *cfg,
 		src |= cfg->src_info.data_width << D40_SREG_CFG_ESIZE_POS;
 		dst |= cfg->dst_info.data_width << D40_SREG_CFG_ESIZE_POS;
 
+		/* Set the priority bit to high for the physical channel */
+		if (cfg->high_priority) {
+			src |= 1 << D40_SREG_CFG_PRI_POS;
+			dst |= 1 << D40_SREG_CFG_PRI_POS;
+		}
+
 	} else {
 		/* Logical channel */
 		dst |= 1 << D40_SREG_CFG_LOG_GIM_POS;
 		src |= 1 << D40_SREG_CFG_LOG_GIM_POS;
 	}
 
-	if (cfg->high_priority) {
-		src |= 1 << D40_SREG_CFG_PRI_POS;
-		dst |= 1 << D40_SREG_CFG_PRI_POS;
-	}
 
 	if (cfg->src_info.big_endian)
 		src |= 1 << D40_SREG_CFG_LBE_POS;
@@ -331,10 +333,10 @@ void d40_log_lli_lcpa_write(struct d40_log_lli_full *lcpa,
 {
 	d40_log_lli_link(lli_dst, lli_src, next, flags);
 
-	writel(lli_src->lcsp02, &lcpa[0].lcsp0);
-	writel(lli_src->lcsp13, &lcpa[0].lcsp1);
-	writel(lli_dst->lcsp02, &lcpa[0].lcsp2);
-	writel(lli_dst->lcsp13, &lcpa[0].lcsp3);
+	writel_relaxed(lli_src->lcsp02, &lcpa[0].lcsp0);
+	writel_relaxed(lli_src->lcsp13, &lcpa[0].lcsp1);
+	writel_relaxed(lli_dst->lcsp02, &lcpa[0].lcsp2);
+	writel_relaxed(lli_dst->lcsp13, &lcpa[0].lcsp3);
 }
 
 void d40_log_lli_lcla_write(struct d40_log_lli *lcla,
@@ -344,10 +346,10 @@ void d40_log_lli_lcla_write(struct d40_log_lli *lcla,
 {
 	d40_log_lli_link(lli_dst, lli_src, next, flags);
 
-	writel(lli_src->lcsp02, &lcla[0].lcsp02);
-	writel(lli_src->lcsp13, &lcla[0].lcsp13);
-	writel(lli_dst->lcsp02, &lcla[1].lcsp02);
-	writel(lli_dst->lcsp13, &lcla[1].lcsp13);
+	writel_relaxed(lli_src->lcsp02, &lcla[0].lcsp02);
+	writel_relaxed(lli_src->lcsp13, &lcla[0].lcsp13);
+	writel_relaxed(lli_dst->lcsp02, &lcla[1].lcsp02);
+	writel_relaxed(lli_dst->lcsp13, &lcla[1].lcsp13);
 }
 
 static void d40_log_fill_lli(struct d40_log_lli *lli,
diff --git a/drivers/dma/ste_dma40_ll.h b/drivers/dma/ste_dma40_ll.h
index 51e8e5396e9..2f3395d6850 100644
--- a/drivers/dma/ste_dma40_ll.h
+++ b/drivers/dma/ste_dma40_ll.h
@@ -94,10 +94,13 @@
 
 /* LCSP2 */
 #define D40_MEM_LCSP2_ECNT_POS		16
+#define D40_MEM_LCSP2_DPTR_POS		 0
 
 #define D40_MEM_LCSP2_ECNT_MASK		(0xFFFF << D40_MEM_LCSP2_ECNT_POS)
+#define D40_MEM_LCSP2_DPTR_MASK		(0xFFFF << D40_MEM_LCSP2_DPTR_POS)
 
 /* LCSP3 */
+#define D40_MEM_LCSP3_DPTR_POS		16
 #define D40_MEM_LCSP3_DCFG_MST_POS	15
 #define D40_MEM_LCSP3_DCFG_TIM_POS	14
 #define D40_MEM_LCSP3_DCFG_EIM_POS	13
@@ -107,6 +110,7 @@
 #define D40_MEM_LCSP3_DLOS_POS		 1
 #define D40_MEM_LCSP3_DTCP_POS		 0
 
+#define D40_MEM_LCSP3_DPTR_MASK		(0xFFFF << D40_MEM_LCSP3_DPTR_POS)
 #define D40_MEM_LCSP3_DLOS_MASK		(0x7F << D40_MEM_LCSP3_DLOS_POS)
 #define D40_MEM_LCSP3_DTCP_MASK		(0x1 << D40_MEM_LCSP3_DTCP_POS)
 
diff --git a/drivers/gator/Kconfig b/drivers/gator/Kconfig
new file mode 100644
index 00000000000..14b3d619d18
--- /dev/null
+++ b/drivers/gator/Kconfig
@@ -0,0 +1,33 @@
+config GATOR
+	tristate "Gator module for ARM's Streamline Performance Analyzer"
+	default m
+	depends on PROFILING
+	depends on HIGH_RES_TIMERS
+	depends on LOCAL_TIMERS || !(ARM && SMP)
+	select TRACING
+
+config GATOR_WITH_MALI_SUPPORT
+	bool
+
+choice
+	prompt "Enable Mali GPU support in Gator"
+	depends on GATOR
+	optional
+
+config GATOR_MALI_400MP
+	bool "Mali-400MP"
+	select GATOR_WITH_MALI_SUPPORT
+
+config GATOR_MALI_T6XX
+	bool "Mali-T604 or Mali-T658"
+	select GATOR_WITH_MALI_SUPPORT
+
+endchoice
+
+config GATOR_MALI_PATH
+	string "Path to Mali driver"
+	depends on GATOR_WITH_MALI_SUPPORT
+	default "drivers/gpu/arm/mali400mp"
+	help
+	  The gator code adds this to its include path so it can get the Mali
+	  trace headers with: #include "linux/mali_linux_trace.h"
diff --git a/drivers/gator/LICENSE b/drivers/gator/LICENSE
new file mode 100644
index 00000000000..d159169d105
--- /dev/null
+++ b/drivers/gator/LICENSE
@@ -0,0 +1,339 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 2, June 1991
+
+ Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+License is intended to guarantee your freedom to share and change free
+software--to make sure the software is free for all its users.  This
+General Public License applies to most of the Free Software
+Foundation's software and to any other program whose authors commit to
+using it.  (Some other Free Software Foundation software is covered by
+the GNU Lesser General Public License instead.)  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+this service if you wish), that you receive source code or can get it
+if you want it, that you can change the software or use pieces of it
+in new free programs; and that you know you can do these things.
+
+  To protect your rights, we need to make restrictions that forbid
+anyone to deny you these rights or to ask you to surrender the rights.
+These restrictions translate to certain responsibilities for you if you
+distribute copies of the software, or if you modify it.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must give the recipients all the rights that
+you have.  You must make sure that they, too, receive or can get the
+source code.  And you must show them these terms so they know their
+rights.
+
+  We protect your rights with two steps: (1) copyright the software, and
+(2) offer you this license which gives you legal permission to copy,
+distribute and/or modify the software.
+
+  Also, for each author's protection and ours, we want to make certain
+that everyone understands that there is no warranty for this free
+software.  If the software is modified by someone else and passed on, we
+want its recipients to know that what they have is not the original, so
+that any problems introduced by others will not reflect on the original
+authors' reputations.
+
+  Finally, any free program is threatened constantly by software
+patents.  We wish to avoid the danger that redistributors of a free
+program will individually obtain patent licenses, in effect making the
+program proprietary.  To prevent this, we have made it clear that any
+patent must be licensed for everyone's free use or not licensed at all.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                    GNU GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License applies to any program or other work which contains
+a notice placed by the copyright holder saying it may be distributed
+under the terms of this General Public License.  The "Program", below,
+refers to any such program or work, and a "work based on the Program"
+means either the Program or any derivative work under copyright law:
+that is to say, a work containing the Program or a portion of it,
+either verbatim or with modifications and/or translated into another
+language.  (Hereinafter, translation is included without limitation in
+the term "modification".)  Each licensee is addressed as "you".
+
+Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running the Program is not restricted, and the output from the Program
+is covered only if its contents constitute a work based on the
+Program (independent of having been made by running the Program).
+Whether that is true depends on what the Program does.
+
+  1. You may copy and distribute verbatim copies of the Program's
+source code as you receive it, in any medium, provided that you
+conspicuously and appropriately publish on each copy an appropriate
+copyright notice and disclaimer of warranty; keep intact all the
+notices that refer to this License and to the absence of any warranty;
+and give any other recipients of the Program a copy of this License
+along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and
+you may at your option offer warranty protection in exchange for a fee.
+
+  2. You may modify your copy or copies of the Program or any portion
+of it, thus forming a work based on the Program, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) You must cause the modified files to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    b) You must cause any work that you distribute or publish, that in
+    whole or in part contains or is derived from the Program or any
+    part thereof, to be licensed as a whole at no charge to all third
+    parties under the terms of this License.
+
+    c) If the modified program normally reads commands interactively
+    when run, you must cause it, when started running for such
+    interactive use in the most ordinary way, to print or display an
+    announcement including an appropriate copyright notice and a
+    notice that there is no warranty (or else, saying that you provide
+    a warranty) and that users may redistribute the program under
+    these conditions, and telling the user how to view a copy of this
+    License.  (Exception: if the Program itself is interactive but
+    does not normally print such an announcement, your work based on
+    the Program is not required to print an announcement.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Program,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Program.
+
+In addition, mere aggregation of another work not based on the Program
+with the Program (or with a work based on the Program) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may copy and distribute the Program (or a work based on it,
+under Section 2) in object code or executable form under the terms of
+Sections 1 and 2 above provided that you also do one of the following:
+
+    a) Accompany it with the complete corresponding machine-readable
+    source code, which must be distributed under the terms of Sections
+    1 and 2 above on a medium customarily used for software interchange; or,
+
+    b) Accompany it with a written offer, valid for at least three
+    years, to give any third party, for a charge no more than your
+    cost of physically performing source distribution, a complete
+    machine-readable copy of the corresponding source code, to be
+    distributed under the terms of Sections 1 and 2 above on a medium
+    customarily used for software interchange; or,
+
+    c) Accompany it with the information you received as to the offer
+    to distribute corresponding source code.  (This alternative is
+    allowed only for noncommercial distribution and only if you
+    received the program in object code or executable form with such
+    an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for
+making modifications to it.  For an executable work, complete source
+code means all the source code for all modules it contains, plus any
+associated interface definition files, plus the scripts used to
+control compilation and installation of the executable.  However, as a
+special exception, the source code distributed need not include
+anything that is normally distributed (in either source or binary
+form) with the major components (compiler, kernel, and so on) of the
+operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering
+access to copy from a designated place, then offering equivalent
+access to copy the source code from the same place counts as
+distribution of the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  4. You may not copy, modify, sublicense, or distribute the Program
+except as expressly provided under this License.  Any attempt
+otherwise to copy, modify, sublicense or distribute the Program is
+void, and will automatically terminate your rights under this License.
+However, parties who have received copies, or rights, from you under
+this License will not have their licenses terminated so long as such
+parties remain in full compliance.
+
+  5. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Program or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Program (or any work based on the
+Program), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+  6. Each time you redistribute the Program (or any work based on the
+Program), the recipient automatically receives a license from the
+original licensor to copy, distribute or modify the Program subject to
+these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties to
+this License.
+
+  7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Program at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Program by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under
+any particular circumstance, the balance of the section is intended to
+apply and the section as a whole is intended to apply in other
+circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system, which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  8. If the distribution and/or use of the Program is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Program under this License
+may add an explicit geographical distribution limitation excluding
+those countries, so that distribution is permitted only in or among
+countries not thus excluded.  In such case, this License incorporates
+the limitation as if written in the body of this License.
+
+  9. The Free Software Foundation may publish revised and/or new versions
+of the General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Program
+specifies a version number of this License which applies to it and "any
+later version", you have the option of following the terms and conditions
+either of that version or of any later version published by the Free
+Software Foundation.  If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software
+Foundation.
+
+  10. If you wish to incorporate parts of the Program into other free
+programs whose distribution conditions are different, write to the author
+to ask for permission.  For software which is copyrighted by the Free
+Software Foundation, write to the Free Software Foundation; we sometimes
+make exceptions for this.  Our decision will be guided by the two goals
+of preserving the free status of all derivatives of our free software and
+of promoting the sharing and reuse of software generally.
+
+                            NO WARRANTY
+
+  11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
+REPAIR OR CORRECTION.
+
+  12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the program's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    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.
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program is interactive, make it output a short notice like this
+when it starts in an interactive mode:
+
+    Gnomovision version 69, Copyright (C) year name of author
+    Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, the commands you use may
+be called something other than `show w' and `show c'; they could even be
+mouse-clicks or menu items--whatever suits your program.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the program, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the program
+  `Gnomovision' (which makes passes at compilers) written by James Hacker.
+
+  <signature of Ty Coon>, 1 April 1989
+  Ty Coon, President of Vice
+
+This General Public License does not permit incorporating your program into
+proprietary programs.  If your program is a subroutine library, you may
+consider it more useful to permit linking proprietary applications with the
+library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.
diff --git a/drivers/gator/Makefile b/drivers/gator/Makefile
new file mode 100644
index 00000000000..60d858741dd
--- /dev/null
+++ b/drivers/gator/Makefile
@@ -0,0 +1,64 @@
+ifneq ($(KERNELRELEASE),)
+
+# Uncomment the following line to enable kernel stack unwinding within gator, or update gator_backtrace.c
+# EXTRA_CFLAGS +=	-DGATOR_KERNEL_STACK_UNWINDING
+
+obj-$(CONFIG_GATOR) := gator.o
+
+gator-y :=	gator_main.o \
+		gator_events_irq.o \
+		gator_events_sched.o \
+		gator_events_net.o \
+		gator_events_block.o \
+		gator_events_meminfo.o \
+		gator_events_perf_pmu.o
+
+gator-y +=	gator_events_mmaped.o
+
+ifeq ($(CONFIG_GATOR_WITH_MALI_SUPPORT),y)
+
+ifeq ($(CONFIG_GATOR_MALI_T6XX),y)
+gator-y +=	gator_events_mali_t6xx.o
+else
+gator-y +=	gator_events_mali.o
+endif
+
+ccflags-y += -I$(CONFIG_GATOR_MALI_PATH)
+ccflags-$(CONFIG_GATOR_MALI_400MP) += -DMALI_SUPPORT=MALI_400
+ccflags-$(CONFIG_GATOR_MALI_T6XX) += -DMALI_SUPPORT=MALI_T6xx
+endif
+
+gator-$(CONFIG_ARM) +=	gator_events_armv6.o \
+			gator_events_armv7.o \
+			gator_events_l2c-310.o \
+			gator_events_scorpion.o
+
+$(obj)/gator_main.o: $(obj)/gator_events.h
+
+clean-files := gator_events.h
+
+# Note, in the recipe below we use "cd $(srctree) && cd $(src)" rather than
+# "cd $(srctree)/$(src)" because under DKMS  $(src) is an absolute path, and we
+# can't just use $(src) because for normal kernel builds this is relative to
+# $(srctree)
+
+       chk_events.h = :
+ quiet_chk_events.h = echo '  CHK     $@'
+silent_chk_events.h = :
+$(obj)/gator_events.h: FORCE
+	@$($(quiet)chk_events.h)
+	$(Q)cd $(srctree) && cd $(src) ; $(CONFIG_SHELL) gator_events.sh $(abspath $@)
+
+else
+
+all:
+	@echo
+	@echo "usage:"
+	@echo "      make -C <kernel_build_dir> M=\`pwd\` ARCH=arm CROSS_COMPILE=<...> modules"
+	@echo
+	$(error)
+
+clean:
+	rm -f *.o .*.cmd gator_events.h modules.order Module.symvers gator.ko gator.mod.c
+
+endif
diff --git a/drivers/gator/gator.h b/drivers/gator/gator.h
new file mode 100644
index 00000000000..6b961099531
--- /dev/null
+++ b/drivers/gator/gator.h
@@ -0,0 +1,100 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 GATOR_H_
+#define GATOR_H_
+
+#include <linux/version.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/list.h>
+
+#define GATOR_PERF_SUPPORT		LINUX_VERSION_CODE >= KERNEL_VERSION(3, 0, 0)
+#define GATOR_PERF_PMU_SUPPORT  GATOR_PERF_SUPPORT && defined(CONFIG_PERF_EVENTS) && defined(CONFIG_HW_PERF_EVENTS)
+#define GATOR_NO_PERF_SUPPORT   (!(GATOR_PERF_SUPPORT))
+#define GATOR_CPU_FREQ_SUPPORT  (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 38)) && defined(CONFIG_CPU_FREQ)
+
+// cpu ids
+#define ARM1136		0xb36
+#define ARM1156		0xb56
+#define ARM1176		0xb76
+#define ARM11MPCORE 0xb02
+#define CORTEX_A5	0xc05
+#define CORTEX_A7	0xc07
+#define CORTEX_A8	0xc08
+#define CORTEX_A9	0xc09
+#define CORTEX_A15	0xc0f
+#define SCORPION	0x00f
+#define SCORPIONMP	0x02d
+#define KRAITSIM	0x049
+#define KRAIT       0x04d
+
+/******************************************************************************
+ * Filesystem
+ ******************************************************************************/
+int gatorfs_create_file_perm(struct super_block *sb, struct dentry *root,
+	char const *name, const struct file_operations *fops, int perm);
+
+struct dentry *gatorfs_mkdir(struct super_block *sb,
+	struct dentry *root, char const *name);
+
+int gatorfs_create_ulong(struct super_block *sb, struct dentry *root,
+	char const *name, unsigned long *val);
+
+int gatorfs_create_ro_ulong(struct super_block *sb, struct dentry *root,
+	char const *name, unsigned long *val);
+
+void gator_op_create_files(struct super_block *sb, struct dentry *root);
+
+/******************************************************************************
+ * Tracepoints
+ ******************************************************************************/
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 32)
+#	error Kernels prior to 2.6.32 not supported
+#elif LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 35)
+#	define GATOR_DEFINE_PROBE(probe_name, proto) \
+		static void probe_##probe_name(PARAMS(proto))
+#	define GATOR_REGISTER_TRACE(probe_name) \
+		register_trace_##probe_name(probe_##probe_name)
+#	define GATOR_UNREGISTER_TRACE(probe_name) \
+		unregister_trace_##probe_name(probe_##probe_name)
+#else
+#	define GATOR_DEFINE_PROBE(probe_name, proto) \
+		static void probe_##probe_name(void *data, PARAMS(proto))
+#	define GATOR_REGISTER_TRACE(probe_name) \
+		register_trace_##probe_name(probe_##probe_name, NULL)
+#	define GATOR_UNREGISTER_TRACE(probe_name) \
+		unregister_trace_##probe_name(probe_##probe_name, NULL)
+#endif
+
+/******************************************************************************
+ * Events
+ ******************************************************************************/
+struct gator_interface {
+	int  (*create_files)(struct super_block *sb, struct dentry *root);
+	int  (*start)(void);
+	void (*stop)(void);
+	int  (*online)(int** buffer);
+	int  (*offline)(int** buffer);
+	void  (*online_dispatch)(int cpu);  // called in process context but may not be running on core 'cpu'
+	void  (*offline_dispatch)(int cpu); // called in process context but may not be running on core 'cpu'
+	int  (*read)(int **buffer);
+	int  (*read64)(long long **buffer);
+	struct list_head list;
+};
+
+// gator_events_init is used as a search term in gator_events.sh
+#define gator_events_init(initfn) \
+	static inline int __gator_events_init_test(void) \
+	{ return initfn(); }
+
+int gator_events_install(struct gator_interface *interface);
+int gator_events_get_key(void);
+extern u32 gator_cpuid(void);
+
+#endif // GATOR_H_
diff --git a/drivers/gator/gator_annotate.c b/drivers/gator/gator_annotate.c
new file mode 100644
index 00000000000..b2288b3ad81
--- /dev/null
+++ b/drivers/gator/gator_annotate.c
@@ -0,0 +1,149 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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/slab.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <asm/uaccess.h>
+#include <asm/current.h>
+#include <linux/spinlock.h>
+
+static DEFINE_SPINLOCK(annotate_lock);
+static bool collect_annotations = false;
+
+static int annotate_copy(struct file *file, char const __user *buf, size_t count)
+{
+	int cpu = 0;
+	int write = per_cpu(gator_buffer_write, cpu)[ANNOTATE_BUF];
+
+	if (file == NULL) {
+		// copy from kernel
+		memcpy(&per_cpu(gator_buffer, cpu)[ANNOTATE_BUF][write], buf, count);
+	} else {
+		// copy from user space
+		if (copy_from_user(&per_cpu(gator_buffer, cpu)[ANNOTATE_BUF][write], buf, count) != 0)
+			return -1;
+	}
+	per_cpu(gator_buffer_write, cpu)[ANNOTATE_BUF] = (write + count) & gator_buffer_mask[ANNOTATE_BUF];
+
+	return 0;
+}
+
+static ssize_t annotate_write(struct file *file, char const __user *buf, size_t count_orig, loff_t *offset)
+{
+	int tid, cpu, header_size, available, contiguous, length1, length2, size, count = count_orig & 0x7fffffff;
+
+	if (*offset)
+		return -EINVAL;
+
+	if (!collect_annotations) {
+		return count_orig;
+	}
+
+	cpu = 0; // Annotation only uses a single per-cpu buffer as the data must be in order to the engine
+
+	if (file == NULL) {
+		tid = -1; // set the thread id to the kernel thread
+	} else {
+		tid = current->pid;
+	}
+
+	// synchronize between cores
+	spin_lock(&annotate_lock);
+
+	// determine total size of the payload
+	header_size = MAXSIZE_PACK32 * 3 + MAXSIZE_PACK64;
+	available = buffer_bytes_available(cpu, ANNOTATE_BUF) - header_size;
+	size = count < available ? count : available;
+
+	if (size <= 0) {
+		size = 0;
+		goto annotate_write_out;
+	}
+
+	// synchronize shared variables annotateBuf and annotatePos
+	if (collect_annotations && per_cpu(gator_buffer, cpu)[ANNOTATE_BUF]) {
+		gator_buffer_write_packed_int(cpu, ANNOTATE_BUF, smp_processor_id());
+		gator_buffer_write_packed_int(cpu, ANNOTATE_BUF, tid);
+		gator_buffer_write_packed_int64(cpu, ANNOTATE_BUF, gator_get_time());
+		gator_buffer_write_packed_int(cpu, ANNOTATE_BUF, size);
+
+		// determine the sizes to capture, length1 + length2 will equal size
+		contiguous = contiguous_space_available(cpu, ANNOTATE_BUF);
+		if (size < contiguous) {
+			length1 = size;
+			length2 = 0;
+		} else {
+			length1 = contiguous;
+			length2 = size - contiguous;
+		}
+
+		if (annotate_copy(file, buf, length1) != 0) {
+			size = -EINVAL;
+			goto annotate_write_out;
+		}
+
+		if (length2 > 0 && annotate_copy(file, &buf[length1], length2) != 0) {
+			size = -EINVAL;
+			goto annotate_write_out;
+		}
+
+		// Check and commit; commit is set to occur once buffer is 3/4 full
+		buffer_check(cpu, ANNOTATE_BUF);
+	}
+
+annotate_write_out:
+	spin_unlock(&annotate_lock);
+
+	// return the number of bytes written
+	return size;
+}
+
+#include "gator_annotate_kernel.c"
+
+static int annotate_release(struct inode *inode, struct file *file)
+{
+	int cpu = 0;
+
+	// synchronize between cores
+	spin_lock(&annotate_lock);
+
+	if (per_cpu(gator_buffer, cpu)[ANNOTATE_BUF] && buffer_check_space(cpu, ANNOTATE_BUF, MAXSIZE_PACK64 + 2 * MAXSIZE_PACK32)) {
+		uint32_t tid = current->pid;
+		gator_buffer_write_packed_int(cpu, ANNOTATE_BUF, tid);
+		gator_buffer_write_packed_int64(cpu, ANNOTATE_BUF, 0); // time
+		gator_buffer_write_packed_int(cpu, ANNOTATE_BUF, 0);   // size
+	}
+
+	spin_unlock(&annotate_lock);
+
+	return 0;
+}
+
+static const struct file_operations annotate_fops = {
+	.write		= annotate_write,
+	.release	= annotate_release
+};
+
+static int gator_annotate_create_files(struct super_block *sb, struct dentry *root)
+{
+	return gatorfs_create_file_perm(sb, root, "annotate", &annotate_fops, 0666);
+}
+
+static int gator_annotate_start(void)
+{
+	collect_annotations = true;
+	return 0;
+}
+
+static void gator_annotate_stop(void)
+{
+	collect_annotations = false;
+}
diff --git a/drivers/gator/gator_annotate_kernel.c b/drivers/gator/gator_annotate_kernel.c
new file mode 100755
index 00000000000..ffab08795b7
--- /dev/null
+++ b/drivers/gator/gator_annotate_kernel.c
@@ -0,0 +1,90 @@
+/**
+ * Copyright (C) ARM Limited 2012. 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.
+ *
+ */
+
+static void kannotate_write(char* ptr, unsigned int size)
+{
+	int retval;
+	int pos = 0;
+	loff_t offset = 0;
+	while (pos < size) {
+		 retval = annotate_write(NULL, &ptr[pos], size - pos, &offset);
+		 if (retval < 0) {
+			 printk(KERN_WARNING "gator: kannotate_write failed with return value %d\n", retval);
+			 return;
+		 }
+		 pos += retval;
+	}
+}
+
+// String annotation
+void gator_annotate(char* string)
+{
+	kannotate_write(string, strlen(string) + 1);
+}
+EXPORT_SYMBOL(gator_annotate);
+
+// String annotation with color
+void gator_annotate_color(int color, char* string)
+{
+	kannotate_write((char*)&color, sizeof(color));
+	kannotate_write(string, strlen(string) + 1);
+}
+EXPORT_SYMBOL(gator_annotate_color);
+
+// Terminate an annotation
+void gator_annotate_end(void)
+{
+	char nul = 0;
+	kannotate_write(&nul, sizeof(nul));
+}
+EXPORT_SYMBOL(gator_annotate_end);
+
+// Image annotation with optional string
+void gator_annotate_visual(char* data, unsigned int length, char* string)
+{
+	long long visual_annotation = 0x011c | (strlen(string) << 16) | ((long long)length << 32);
+	kannotate_write((char*)&visual_annotation, 8);
+	kannotate_write(string, strlen(string));
+	kannotate_write(data, length);
+}
+EXPORT_SYMBOL(gator_annotate_visual);
+
+// Marker annotation
+void gator_annotate_marker(void)
+{
+	int marker_annotation = 0x00021c;
+	kannotate_write((char*)&marker_annotation, 3);
+}
+EXPORT_SYMBOL(gator_annotate_marker);
+
+// Marker annotation with a string
+void gator_annotate_marker_str(char* string)
+{
+	int marker_annotation = 0x021c;
+	kannotate_write((char*)&marker_annotation, 2);
+	kannotate_write(string, strlen(string) + 1);
+}
+EXPORT_SYMBOL(gator_annotate_marker_str);
+
+// Marker annotation with a color
+void gator_annotate_marker_color(int color)
+{
+	long long marker_annotation = (0x021c | ((long long)color << 16)) & 0x0000ffffffffffffLL;
+	kannotate_write((char*)&marker_annotation, 7);
+}
+EXPORT_SYMBOL(gator_annotate_marker_color);
+
+// Marker annotationw ith a string and color
+void gator_annotate_marker_color_str(int color, char* string)
+{
+	long long marker_annotation = 0x021c | ((long long)color << 16);
+	kannotate_write((char*)&marker_annotation, 6);
+	kannotate_write(string, strlen(string) + 1);
+}
+EXPORT_SYMBOL(gator_annotate_marker_color_str);
diff --git a/drivers/gator/gator_backtrace.c b/drivers/gator/gator_backtrace.c
new file mode 100644
index 00000000000..50783d69594
--- /dev/null
+++ b/drivers/gator/gator_backtrace.c
@@ -0,0 +1,114 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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.
+ *
+ */
+
+/*
+ * EABI backtrace stores {fp,lr} on the stack.
+ */
+struct frame_tail_eabi {
+	unsigned long fp; // points to prev_lr
+	unsigned long lr;
+};
+
+static void arm_backtrace_eabi(int cpu, int buftype, struct pt_regs * const regs, unsigned int depth)
+{
+#if defined(__arm__)
+	struct frame_tail_eabi *tail;
+	struct frame_tail_eabi *next;
+	struct frame_tail_eabi *ptrtail;
+	struct frame_tail_eabi buftail;
+	unsigned long fp = regs->ARM_fp;
+	unsigned long sp = regs->ARM_sp;
+	unsigned long lr = regs->ARM_lr;
+	int is_user_mode = user_mode(regs);
+
+	if (!is_user_mode) {
+		return;
+	}
+
+	/* entry preamble may not have executed */
+	gator_add_trace(cpu, buftype, lr);
+
+	/* check tail is valid */
+	if (fp == 0 || fp < sp) {
+		return;
+	}
+
+	tail = (struct frame_tail_eabi *)(fp - 4);
+
+	while (depth-- && tail && !((unsigned long) tail & 3)) {
+		/* Also check accessibility of one struct frame_tail beyond */
+		if (!access_ok(VERIFY_READ, tail, sizeof(struct frame_tail_eabi)))
+			return;
+		if (__copy_from_user_inatomic(&buftail, tail, sizeof(struct frame_tail_eabi)))
+			return;
+		ptrtail = &buftail;
+
+		lr = ptrtail[0].lr;
+		gator_add_trace(cpu, buftype, lr);
+
+		/* frame pointers should progress back up the stack, towards higher addresses */
+		next = (struct frame_tail_eabi *)(lr - 4);
+		if (tail >= next || lr == 0) {
+			fp = ptrtail[0].fp;
+			next = (struct frame_tail_eabi *)(fp - 4);
+			/* check tail is valid */
+			if (tail >= next || fp == 0) {
+				return;
+			}
+		}
+
+		tail = next;
+	}
+#endif
+}
+
+#if defined(__arm__)
+static DEFINE_PER_CPU(int, backtrace_buffer);
+static int report_trace(struct stackframe *frame, void *d)
+{
+	struct module *mod;
+	unsigned int *depth = d, addr = frame->pc, cookie = NO_COOKIE, cpu = smp_processor_id();
+
+	if (*depth) {
+		mod = __module_address(addr);
+		if (mod) {
+			cookie = get_cookie(cpu, per_cpu(backtrace_buffer, cpu), current, NULL, mod, true);
+			addr = addr - (unsigned long)mod->module_core;
+		}
+		marshal_backtrace(addr & ~1, cookie);
+		(*depth)--;
+	}
+
+	return *depth == 0;
+}
+#endif
+
+// Uncomment the following line to enable kernel stack unwinding within gator, note it can also be defined from the Makefile
+// #define GATOR_KERNEL_STACK_UNWINDING
+static void kernel_backtrace(int cpu, int buftype, struct pt_regs * const regs)
+{
+#if defined(__arm__)
+#ifdef GATOR_KERNEL_STACK_UNWINDING
+	int depth = gator_backtrace_depth;
+#else
+	int depth = 1;
+#endif
+	struct stackframe frame;
+	if (depth == 0)
+		depth = 1;
+	frame.fp = regs->ARM_fp;
+	frame.sp = regs->ARM_sp;
+	frame.lr = regs->ARM_lr;
+	frame.pc = regs->ARM_pc;
+	per_cpu(backtrace_buffer, cpu) = buftype;
+	walk_stackframe(&frame, report_trace, &depth);
+#else
+	marshal_backtrace(PC_REG & ~1, NO_COOKIE);
+#endif
+}
diff --git a/drivers/gator/gator_cookies.c b/drivers/gator/gator_cookies.c
new file mode 100644
index 00000000000..7b5091696bf
--- /dev/null
+++ b/drivers/gator/gator_cookies.c
@@ -0,0 +1,401 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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.
+ *
+ */
+
+#define COOKIEMAP_ENTRIES	1024		/* must be power of 2 */
+#define TRANSLATE_SIZE		256
+#define MAX_COLLISIONS		2
+
+static uint32_t *gator_crc32_table;
+static uint32_t translate_buffer_mask;
+
+static DEFINE_PER_CPU(char *, translate_text);
+static DEFINE_PER_CPU(uint32_t, cookie_next_key);
+static DEFINE_PER_CPU(uint64_t *, cookie_keys);
+static DEFINE_PER_CPU(uint32_t *, cookie_values);
+static DEFINE_PER_CPU(int, translate_buffer_read);
+static DEFINE_PER_CPU(int, translate_buffer_write);
+static DEFINE_PER_CPU(unsigned int *, translate_buffer);
+
+static inline uint32_t get_cookie(int cpu, int buftype, struct task_struct *task, struct vm_area_struct *vma, struct module *mod, bool in_interrupt);
+static void wq_cookie_handler(struct work_struct *unused);
+DECLARE_WORK(cookie_work, wq_cookie_handler);
+
+static uint32_t cookiemap_code(uint64_t value64) {
+	uint32_t value = (uint32_t)((value64 >> 32) + value64);
+	uint32_t cookiecode = (value >> 24) & 0xff;
+	cookiecode = cookiecode * 31 + ((value >> 16) & 0xff);
+	cookiecode = cookiecode * 31 + ((value >> 8) & 0xff);
+	cookiecode = cookiecode * 31 + ((value >> 0) & 0xff);
+	cookiecode &= (COOKIEMAP_ENTRIES-1);
+	return cookiecode * MAX_COLLISIONS;
+}
+
+static uint32_t gator_chksum_crc32(char *data)
+{
+   register unsigned long crc;
+   unsigned char *block = data;
+   int i, length = strlen(data);
+
+   crc = 0xFFFFFFFF;
+   for (i = 0; i < length; i++) {
+      crc = ((crc >> 8) & 0x00FFFFFF) ^ gator_crc32_table[(crc ^ *block++) & 0xFF];
+   }
+
+   return (crc ^ 0xFFFFFFFF);
+}
+
+/*
+ * Exists
+ *  Pre:  [0][1][v][3]..[n-1]
+ *  Post: [v][0][1][3]..[n-1]
+ */
+static uint32_t cookiemap_exists(uint64_t key) {
+	unsigned long x, flags, retval = 0;
+	int cpu = smp_processor_id();
+	uint32_t cookiecode = cookiemap_code(key);
+	uint64_t *keys = &(per_cpu(cookie_keys, cpu)[cookiecode]);
+	uint32_t *values = &(per_cpu(cookie_values, cpu)[cookiecode]);
+
+	// Can be called from interrupt handler or from work queue
+	local_irq_save(flags);
+	for (x = 0; x < MAX_COLLISIONS; x++) {
+		if (keys[x] == key) {
+			uint32_t value = values[x];
+			for (; x > 0; x--) {
+				keys[x] = keys[x-1];
+				values[x] = values[x-1];
+			}
+			keys[0] = key;
+			values[0] = value;
+			retval = value;
+			break;
+		}
+	}
+	local_irq_restore(flags);
+
+	return retval;
+}
+
+/*
+ * Add
+ *  Pre:  [0][1][2][3]..[n-1]
+ *  Post: [v][0][1][2]..[n-2]
+ */
+static void cookiemap_add(uint64_t key, uint32_t value) {
+	int cpu = smp_processor_id();
+	int cookiecode = cookiemap_code(key);
+	uint64_t *keys = &(per_cpu(cookie_keys, cpu)[cookiecode]);
+	uint32_t *values = &(per_cpu(cookie_values, cpu)[cookiecode]);
+	int x;
+
+	for (x = MAX_COLLISIONS-1; x > 0; x--) {
+		keys[x] = keys[x-1];
+		values[x] = values[x-1];
+	}
+	keys[0] = key;
+	values[0] = value;
+}
+
+static void translate_buffer_write_int(int cpu, unsigned int x)
+{
+	per_cpu(translate_buffer, cpu)[per_cpu(translate_buffer_write, cpu)++] = x;
+	per_cpu(translate_buffer_write, cpu) &= translate_buffer_mask;
+}
+
+static unsigned int translate_buffer_read_int(int cpu)
+{
+	unsigned int value = per_cpu(translate_buffer, cpu)[per_cpu(translate_buffer_read, cpu)++];
+	per_cpu(translate_buffer_read, cpu) &= translate_buffer_mask;
+	return value;
+}
+
+static void wq_cookie_handler(struct work_struct *unused)
+{
+	struct task_struct *task;
+	struct vm_area_struct *vma;
+	int cpu = smp_processor_id();
+	unsigned int cookie, commit;
+
+	mutex_lock(&start_mutex);
+
+	if (gator_started != 0) {
+		commit = per_cpu(translate_buffer_write, cpu);
+		while (per_cpu(translate_buffer_read, cpu) != commit) {
+			task = (struct task_struct *)translate_buffer_read_int(cpu);
+			vma = (struct vm_area_struct *)translate_buffer_read_int(cpu);
+			cookie = get_cookie(cpu, BACKTRACE_BUF, task, vma, NULL, false);
+		}
+	}
+
+	mutex_unlock(&start_mutex);
+}
+
+// Retrieve full name from proc/pid/cmdline for java processes on Android
+static int translate_app_process(char** text, int cpu, struct task_struct * task, struct vm_area_struct *vma, bool in_interrupt)
+{
+	void *maddr;
+	unsigned int len;
+	unsigned long addr;
+	struct mm_struct *mm;
+	struct page *page = NULL;
+	struct vm_area_struct *page_vma;
+	int bytes, offset, retval = 0, ptr;
+	char * buf = per_cpu(translate_text, cpu);
+
+	// Push work into a work queue if in atomic context as the kernel functions below might sleep
+	// Rely on the in_interrupt variable rather than in_irq() or in_interrupt() kernel functions, as the value of these functions seems
+	//   inconsistent during a context switch between android/linux versions
+	if (in_interrupt) {
+		// Check if already in buffer
+		ptr = per_cpu(translate_buffer_read, cpu);
+		while (ptr != per_cpu(translate_buffer_write, cpu)) {
+			if (per_cpu(translate_buffer, cpu)[ptr] == (int)task)
+				goto out;
+			ptr = (ptr + 2) & translate_buffer_mask;
+		}
+
+		translate_buffer_write_int(cpu, (unsigned int)task);
+		translate_buffer_write_int(cpu, (unsigned int)vma);
+		schedule_work(&cookie_work);
+		goto out;
+	}
+
+	mm = get_task_mm(task);
+	if (!mm)
+		goto out;
+	if (!mm->arg_end)
+		goto outmm;
+	addr = mm->arg_start;
+	len = mm->arg_end - mm->arg_start;
+
+	if (len > TRANSLATE_SIZE)
+		len = TRANSLATE_SIZE;
+
+	down_read(&mm->mmap_sem);
+	while (len) {
+		if (get_user_pages(task, mm, addr, 1, 0, 1, &page, &page_vma) <= 0)
+			goto outsem;
+
+		maddr = kmap(page);
+		offset = addr & (PAGE_SIZE-1);
+		bytes = len;
+		if (bytes > PAGE_SIZE - offset)
+			bytes = PAGE_SIZE - offset;
+
+		copy_from_user_page(page_vma, page, addr, buf, maddr + offset, bytes);
+
+		kunmap(page);	// release page allocated by get_user_pages()
+		page_cache_release(page);
+
+		len -= bytes;
+		buf += bytes;
+		addr += bytes;
+
+		*text = per_cpu(translate_text, cpu);
+		retval = 1;
+	}
+
+	// On app_process startup, /proc/pid/cmdline is initially "zygote" then "<pre-initialized>" but changes after an initial startup period
+	if (strcmp(*text, "zygote") == 0 || strcmp(*text, "<pre-initialized>") == 0)
+		retval = 0;
+
+outsem:
+	up_read(&mm->mmap_sem);
+outmm:
+	mmput(mm);
+out:
+	return retval;
+}
+
+static inline uint32_t get_cookie(int cpu, int buftype, struct task_struct *task, struct vm_area_struct *vma, struct module *mod, bool in_interrupt)
+{
+	unsigned long flags, cookie;
+	struct path *path;
+	uint64_t key;
+	char *text;
+
+	if (mod) {
+		text = mod->name;
+	} else {
+		if (!vma || !vma->vm_file) {
+			return INVALID_COOKIE;
+		}
+		path = &vma->vm_file->f_path;
+		if (!path || !path->dentry) {
+			return INVALID_COOKIE;
+		}
+
+		text = (char*)path->dentry->d_name.name;
+	}
+
+	key = gator_chksum_crc32(text);
+	key = (key << 32) | (uint32_t)task->tgid;
+
+	cookie = cookiemap_exists(key);
+	if (cookie) {
+		return cookie;
+	}
+
+	if (strcmp(text, "app_process") == 0 && !mod) {
+		if (!translate_app_process(&text, cpu, task, vma, in_interrupt))
+			return INVALID_COOKIE;
+	}
+
+	// Can be called from interrupt handler or from work queue or from scheduler trace
+	local_irq_save(flags);
+
+	cookie = INVALID_COOKIE;
+	if (marshal_cookie_header(text)) {
+		cookie = per_cpu(cookie_next_key, cpu) += nr_cpu_ids;
+		cookiemap_add(key, cookie);
+		marshal_cookie(cookie, text);
+	}
+
+	local_irq_restore(flags);
+
+	return cookie;
+}
+
+static int get_exec_cookie(int cpu, int buftype, struct task_struct *task)
+{
+	unsigned long cookie = NO_COOKIE;
+	struct mm_struct *mm = task->mm;
+	struct vm_area_struct *vma;
+
+	// kernel threads have no address space
+	if (!mm)
+		return cookie;
+
+	for (vma = mm->mmap; vma; vma = vma->vm_next) {
+		if (!vma->vm_file)
+			continue;
+		if (!(vma->vm_flags & VM_EXECUTABLE))
+			continue;
+		cookie = get_cookie(cpu, buftype, task, vma, NULL, true);
+		break;
+	}
+
+	return cookie;
+}
+
+static unsigned long get_address_cookie(int cpu, int buftype, struct task_struct *task, unsigned long addr, off_t *offset)
+{
+	unsigned long cookie = NO_COOKIE;
+	struct mm_struct *mm = task->mm;
+	struct vm_area_struct *vma;
+
+	if (!mm)
+		return cookie;
+
+	for (vma = find_vma(mm, addr); vma; vma = vma->vm_next) {
+		if (addr < vma->vm_start || addr >= vma->vm_end)
+			continue;
+
+		if (vma->vm_file) {
+			cookie = get_cookie(cpu, buftype, task, vma, NULL, true);
+			*offset = (vma->vm_pgoff << PAGE_SHIFT) + addr - vma->vm_start;
+		} else {
+			/* must be an anonymous map */
+			*offset = addr;
+		}
+
+		break;
+	}
+
+	if (!vma)
+		cookie = INVALID_COOKIE;
+
+	return cookie;
+}
+
+static int cookies_initialize(void)
+{
+	uint32_t crc, poly;
+	int i, j, cpu, size, err = 0;
+
+	int translate_buffer_size = 512; // must be a power of 2
+	translate_buffer_mask = translate_buffer_size / sizeof(per_cpu(translate_buffer, 0)[0]) - 1;
+
+	for_each_present_cpu(cpu) {
+		per_cpu(cookie_next_key, cpu) = nr_cpu_ids + cpu;
+
+		size = COOKIEMAP_ENTRIES * MAX_COLLISIONS * sizeof(uint64_t);
+		per_cpu(cookie_keys, cpu) = (uint64_t*)kmalloc(size, GFP_KERNEL);
+		if (!per_cpu(cookie_keys, cpu)) {
+			err = -ENOMEM;
+			goto cookie_setup_error;
+		}
+		memset(per_cpu(cookie_keys, cpu), 0, size);
+
+		size = COOKIEMAP_ENTRIES * MAX_COLLISIONS * sizeof(uint32_t);
+		per_cpu(cookie_values, cpu) = (uint32_t*)kmalloc(size, GFP_KERNEL);
+		if (!per_cpu(cookie_values, cpu)) {
+			err = -ENOMEM;
+			goto cookie_setup_error;
+		}
+		memset(per_cpu(cookie_values, cpu), 0, size);
+
+		per_cpu(translate_buffer, cpu) = (unsigned int *)kmalloc(translate_buffer_size, GFP_KERNEL);
+		if (!per_cpu(translate_buffer, cpu)) {
+			err = -ENOMEM;
+			goto cookie_setup_error;
+		}
+
+		per_cpu(translate_buffer_write, cpu) = 0;
+		per_cpu(translate_buffer_read, cpu) = 0;
+
+		per_cpu(translate_text, cpu) = (char *)kmalloc(TRANSLATE_SIZE, GFP_KERNEL);
+		if (!per_cpu(translate_text, cpu)) {
+			err = -ENOMEM;
+			goto cookie_setup_error;
+		}
+	}
+
+	// build CRC32 table
+	poly = 0x04c11db7;
+	gator_crc32_table = (uint32_t*)kmalloc(256 * sizeof(uint32_t), GFP_KERNEL);
+	for (i = 0; i < 256; i++) {
+		crc = i;
+		for (j = 8; j > 0; j--) {
+			if (crc & 1) {
+				crc = (crc >> 1) ^ poly;
+			} else {
+				crc >>= 1;
+			}
+		}
+		gator_crc32_table[i] = crc;
+	}
+
+cookie_setup_error:
+	return err;
+}
+
+static void cookies_release(void)
+{
+	int cpu;
+
+	for_each_present_cpu(cpu) {
+		kfree(per_cpu(cookie_keys, cpu));
+		per_cpu(cookie_keys, cpu) = NULL;
+
+		kfree(per_cpu(cookie_values, cpu));
+		per_cpu(cookie_values, cpu) = NULL;
+
+		kfree(per_cpu(translate_buffer, cpu));
+		per_cpu(translate_buffer, cpu) = NULL;
+		per_cpu(translate_buffer_read, cpu) = 0;
+		per_cpu(translate_buffer_write, cpu) = 0;
+
+		kfree(per_cpu(translate_text, cpu));
+		per_cpu(translate_text, cpu) = NULL;
+	}
+
+	kfree(gator_crc32_table);
+	gator_crc32_table = NULL;
+}
diff --git a/drivers/gator/gator_ebs.c b/drivers/gator/gator_ebs.c
new file mode 100644
index 00000000000..1208d69fbfd
--- /dev/null
+++ b/drivers/gator/gator_ebs.c
@@ -0,0 +1,158 @@
+/**
+ * Copyright (C) ARM Limited 2012. 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(__arm__) && (GATOR_PERF_PMU_SUPPORT)
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+
+#include <asm/pmu.h>
+
+static DEFINE_MUTEX(perf_mutex);
+
+extern int pmnc_counters;
+extern int ccnt;
+extern unsigned long pmnc_enabled[];
+extern unsigned long pmnc_event[];
+extern unsigned long pmnc_count[];
+extern unsigned long pmnc_key[];
+
+static DEFINE_PER_CPU(struct perf_event *, pevent);
+static DEFINE_PER_CPU(struct perf_event_attr *, pevent_attr);
+static DEFINE_PER_CPU(int, key);
+static DEFINE_PER_CPU(unsigned int, prev_value);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 1, 0)
+static void ebs_overflow_handler(struct perf_event *event, int unused, struct perf_sample_data *data, struct pt_regs *regs)
+#else
+static void ebs_overflow_handler(struct perf_event *event, struct perf_sample_data *data, struct pt_regs *regs)
+#endif
+{
+	int cpu = smp_processor_id();
+
+	if (event != per_cpu(pevent, cpu))
+		return;
+
+	// Output backtrace
+	gator_add_sample(cpu, BACKTRACE_BUF, regs);
+
+	// Collect counters
+	collect_counters();
+}
+
+static void gator_event_sampling_online_dispatch(int cpu)
+{
+	struct perf_event * ev;
+
+	if (!event_based_sampling)
+		return;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 1, 0)
+	ev = per_cpu(pevent, cpu) = perf_event_create_kernel_counter(per_cpu(pevent_attr, cpu), cpu, 0, ebs_overflow_handler);
+#else
+	ev = per_cpu(pevent, cpu) = perf_event_create_kernel_counter(per_cpu(pevent_attr, cpu), cpu, 0, ebs_overflow_handler, 0);
+#endif
+
+	if (IS_ERR(ev)) {
+		pr_err("gator: unable to start event-based-sampling");
+		return;
+	}
+
+	if (ev->state != PERF_EVENT_STATE_ACTIVE) {
+		pr_err("gator: unable to start event-based-sampling");
+		perf_event_release_kernel(ev);
+		return;
+	}
+
+	ev->pmu->read(ev);
+	per_cpu(prev_value, cpu) = local64_read(&ev->count);
+}
+
+static void gator_event_sampling_offline_dispatch(int cpu)
+{
+	struct perf_event * pe = NULL;
+
+	mutex_lock(&perf_mutex);
+	if (per_cpu(pevent, cpu)) {
+		pe = per_cpu(pevent, cpu);
+		per_cpu(pevent, cpu) = NULL;
+	}
+	mutex_unlock(&perf_mutex);
+
+	if (pe) {
+		perf_event_release_kernel(pe);
+	}
+}
+
+static int gator_event_sampling_start(void)
+{
+	int cnt, event = 0, count = 0, ebs_key = 0, cpu;
+
+	for_each_present_cpu(cpu) {
+		per_cpu(pevent, cpu) = NULL;
+		per_cpu(pevent_attr, cpu) = NULL;
+	}
+
+	event_based_sampling = false;
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		if (pmnc_count[cnt] > 0) {
+			event_based_sampling = true;
+			event = pmnc_event[cnt];
+			count = pmnc_count[cnt];
+			ebs_key = pmnc_key[cnt];
+			break;
+		}
+	}
+
+	if (!event_based_sampling)
+		return 0;
+
+	for_each_present_cpu(cpu) {
+		u32 size = sizeof(struct perf_event_attr);
+		per_cpu(pevent_attr, cpu) = kmalloc(size, GFP_KERNEL);
+		if (!per_cpu(pevent_attr, cpu))
+			return -1;
+
+		memset(per_cpu(pevent_attr, cpu), 0, size);
+		per_cpu(pevent_attr, cpu)->type = PERF_TYPE_RAW;
+		per_cpu(pevent_attr, cpu)->size = size;
+		per_cpu(pevent_attr, cpu)->config = event;
+		per_cpu(pevent_attr, cpu)->sample_period = count;
+		per_cpu(pevent_attr, cpu)->pinned = 1;
+
+		// handle special case for ccnt
+		if (cnt == ccnt) {
+			per_cpu(pevent_attr, cpu)->type = PERF_TYPE_HARDWARE;
+			per_cpu(pevent_attr, cpu)->config = PERF_COUNT_HW_CPU_CYCLES;
+		}
+
+		per_cpu(key, cpu) = ebs_key;
+	}
+
+	return 0;
+}
+
+static void gator_event_sampling_stop(void)
+{
+	int cpu;
+
+	for_each_present_cpu(cpu) {
+		if (per_cpu(pevent_attr, cpu)) {
+			kfree(per_cpu(pevent_attr, cpu));
+			per_cpu(pevent_attr, cpu) = NULL;
+		}
+	}
+}
+
+#else
+static void gator_event_sampling_online_dispatch(int cpu) {}
+static void gator_event_sampling_offline_dispatch(int cpu) {}
+static int gator_event_sampling_start(void) {return 0;}
+static void gator_event_sampling_stop(void) {}
+#endif
diff --git a/drivers/gator/gator_events.sh b/drivers/gator/gator_events.sh
new file mode 100755
index 00000000000..5467dd6d17d
--- /dev/null
+++ b/drivers/gator/gator_events.sh
@@ -0,0 +1,19 @@
+#!/bin/sh
+
+EVENTS=`grep gator_events_init *.c | sed 's/.\+gator_events_init(\(.\+\)).\+/\1/'`
+
+(
+	echo /\* This file is auto generated \*/
+	echo
+	for EVENT in $EVENTS; do
+		echo __weak int $EVENT\(void\)\;
+	done
+	echo
+	echo static int \(*gator_events_list[]\)\(void\) = {
+	for EVENT in $EVENTS; do
+		echo \	$EVENT,
+	done
+	echo }\;
+) > $1.tmp
+
+cmp -s $1 $1.tmp && rm $1.tmp || mv $1.tmp $1
diff --git a/drivers/gator/gator_events_armv6.c b/drivers/gator/gator_events_armv6.c
new file mode 100644
index 00000000000..5f989ba4e86
--- /dev/null
+++ b/drivers/gator/gator_events_armv6.c
@@ -0,0 +1,244 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 "gator.h"
+
+// gator_events_perf_pmu.c is used if perf is supported
+#if GATOR_NO_PERF_SUPPORT
+
+static const char *pmnc_name;
+
+/*
+ * Per-CPU PMCR
+ */
+#define PMCR_E			(1 << 0)	/* Enable */
+#define PMCR_P			(1 << 1)	/* Count reset */
+#define PMCR_C			(1 << 2)	/* Cycle counter reset */
+#define PMCR_OFL_PMN0	(1 << 8)	/* Count reg 0 overflow */
+#define PMCR_OFL_PMN1	(1 << 9)	/* Count reg 1 overflow */
+#define PMCR_OFL_CCNT	(1 << 10)	/* Cycle counter overflow */
+
+#define PMN0 0
+#define PMN1 1
+#define CCNT 2
+#define CNTMAX	(CCNT+1)
+
+static int pmnc_counters = 0;
+static unsigned long pmnc_enabled[CNTMAX];
+static unsigned long pmnc_event[CNTMAX];
+static unsigned long pmnc_key[CNTMAX];
+
+static DEFINE_PER_CPU(int[CNTMAX * 2], perfCnt);
+
+static inline void armv6_pmnc_write(u32 val)
+{
+	/* upper 4bits and 7, 11 are write-as-0 */
+	val &= 0x0ffff77f;
+	asm volatile("mcr p15, 0, %0, c15, c12, 0" : : "r" (val));
+}
+
+static inline u32 armv6_pmnc_read(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 0, %0, c15, c12, 0" : "=r" (val));
+	return val;
+}
+
+static void armv6_pmnc_reset_counter(unsigned int cnt)
+{
+	u32 val = 0;
+	switch (cnt) {
+	case CCNT:
+		asm volatile("mcr p15, 0, %0, c15, c12, 1" : : "r" (val));
+		break;
+	case PMN0:
+		asm volatile("mcr p15, 0, %0, c15, c12, 2" : : "r" (val));
+		break;
+	case PMN1:
+		asm volatile("mcr p15, 0, %0, c15, c12, 3" : : "r" (val));
+		break;
+	}
+}
+
+int gator_events_armv6_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+	int i;
+
+	pmnc_counters = 3;
+
+	for (i = PMN0; i <= CCNT; i++) {
+		char buf[40];
+		if (i == CCNT) {
+			snprintf(buf, sizeof buf, "ARM_%s_ccnt", pmnc_name);
+		} else {
+			snprintf(buf, sizeof buf, "ARM_%s_cnt%d", pmnc_name, i);
+		}
+		dir = gatorfs_mkdir(sb, root, buf);
+		if (!dir) {
+			return -1;
+		}
+		gatorfs_create_ulong(sb, dir, "enabled", &pmnc_enabled[i]);
+		gatorfs_create_ro_ulong(sb, dir, "key", &pmnc_key[i]);
+		if (i != CCNT) {
+			gatorfs_create_ulong(sb, dir, "event", &pmnc_event[i]);
+		}
+	}
+
+	return 0;
+}
+
+static int gator_events_armv6_online(int** buffer)
+{
+	unsigned int cnt, len = 0, cpu = smp_processor_id();
+	u32 pmnc;
+
+	if (armv6_pmnc_read() & PMCR_E) {
+		armv6_pmnc_write(armv6_pmnc_read() & ~PMCR_E);
+	}
+
+	/* initialize PMNC, reset overflow, D bit, C bit and P bit. */
+	armv6_pmnc_write(PMCR_OFL_PMN0 | PMCR_OFL_PMN1 | PMCR_OFL_CCNT |
+			PMCR_C | PMCR_P);
+
+	/* configure control register */
+	for (pmnc = 0, cnt = PMN0; cnt <= CCNT; cnt++) {
+		unsigned long event;
+
+		if (!pmnc_enabled[cnt])
+			continue;
+
+		event = pmnc_event[cnt] & 255;
+
+		// Set event (if destined for PMNx counters)
+		if (cnt == PMN0) {
+			pmnc |= event << 20;
+		} else if (cnt == PMN1) {
+			pmnc |= event << 12;
+		}
+
+		// Reset counter
+		armv6_pmnc_reset_counter(cnt);
+	}
+	armv6_pmnc_write(pmnc | PMCR_E);
+
+	// return zero values, no need to read as the counters were just reset
+	for (cnt = PMN0; cnt <= CCNT; cnt++) {
+		if (pmnc_enabled[cnt]) {
+			per_cpu(perfCnt, cpu)[len++] = pmnc_key[cnt];
+			per_cpu(perfCnt, cpu)[len++] = 0;
+		}
+	}
+
+	if (buffer)
+		*buffer = per_cpu(perfCnt, cpu);
+
+	return len;
+}
+
+static int gator_events_armv6_offline(int** buffer)
+{
+	unsigned int cnt;
+
+	armv6_pmnc_write(armv6_pmnc_read() & ~PMCR_E);
+	for (cnt = PMN0; cnt <= CCNT; cnt++) {
+		armv6_pmnc_reset_counter(cnt);
+	}
+
+	return 0;
+}
+
+static void gator_events_armv6_stop(void)
+{
+	unsigned int cnt;
+
+	for (cnt = PMN0; cnt <= CCNT; cnt++) {
+		pmnc_enabled[cnt] = 0;
+		pmnc_event[cnt] = 0;
+	}
+}
+
+static int gator_events_armv6_read(int **buffer)
+{
+	int cnt, len = 0;
+	int cpu = smp_processor_id();
+
+	// a context switch may occur before the online hotplug event, thus need to check that the pmu is enabled
+	if (!(armv6_pmnc_read() & PMCR_E)) {
+		return 0;
+	}
+
+	for (cnt = PMN0; cnt <= CCNT; cnt++) {
+		if (pmnc_enabled[cnt]) {
+			u32 value = 0;
+			switch (cnt) {
+			case CCNT:
+				asm volatile("mrc p15, 0, %0, c15, c12, 1" : "=r" (value));
+				break;
+			case PMN0:
+				asm volatile("mrc p15, 0, %0, c15, c12, 2" : "=r" (value));
+				break;
+			case PMN1:
+				asm volatile("mrc p15, 0, %0, c15, c12, 3" : "=r" (value));
+				break;
+			}
+			armv6_pmnc_reset_counter(cnt);
+
+			per_cpu(perfCnt, cpu)[len++] = pmnc_key[cnt];
+			per_cpu(perfCnt, cpu)[len++] = value;
+		}
+	}
+
+	if (buffer)
+		*buffer = per_cpu(perfCnt, cpu);
+
+	return len;
+}
+
+static struct gator_interface gator_events_armv6_interface = {
+	.create_files = gator_events_armv6_create_files,
+	.stop = gator_events_armv6_stop,
+	.online = gator_events_armv6_online,
+	.offline = gator_events_armv6_offline,
+	.read = gator_events_armv6_read,
+};
+
+int gator_events_armv6_init(void)
+{
+	unsigned int cnt;
+
+	switch (gator_cpuid()) {
+	case ARM1136:
+	case ARM1156:
+	case ARM1176:
+		pmnc_name = "ARM11";
+		break;
+	case ARM11MPCORE:
+		pmnc_name = "ARM11MPCore";
+		break;
+	default:
+		return -1;
+	}
+
+	for (cnt = PMN0; cnt <= CCNT; cnt++) {
+		pmnc_enabled[cnt] = 0;
+		pmnc_event[cnt] = 0;
+		pmnc_key[cnt] = gator_events_get_key();
+	}
+
+	return gator_events_install(&gator_events_armv6_interface);
+}
+
+gator_events_init(gator_events_armv6_init);
+
+#else
+int gator_events_armv6_init(void)
+{
+	return -1;
+}
+#endif
diff --git a/drivers/gator/gator_events_armv7.c b/drivers/gator/gator_events_armv7.c
new file mode 100644
index 00000000000..590421d110c
--- /dev/null
+++ b/drivers/gator/gator_events_armv7.c
@@ -0,0 +1,319 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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.
+ */
+
+/*  Disabling interrupts
+ *    Many of the functions below disable interrupts via local_irq_save(). This disabling of interrupts is done to prevent any race conditions
+ *    between multiple entities (e.g. hrtimer interrupts and event based interrupts) calling the same functions. As accessing the pmu involves
+ *    several steps (disable, select, read, enable), these steps must be performed atomically. Normal synchronization routines cannot be used
+ *    as these functions are being called from interrupt context.
+ */
+
+#include "gator.h"
+
+// gator_events_perf_pmu.c is used if perf is supported
+#if GATOR_NO_PERF_SUPPORT
+
+// Per-CPU PMNC: config reg
+#define PMNC_E		(1 << 0)	/* Enable all counters */
+#define PMNC_P		(1 << 1)	/* Reset all counters */
+#define PMNC_C		(1 << 2)	/* Cycle counter reset */
+#define	PMNC_MASK	0x3f		/* Mask for writable bits */
+
+// ccnt reg
+#define CCNT_REG	(1 << 31)
+
+#define CCNT 		0
+#define CNT0		1
+#define CNTMAX 		(6+1)
+
+static const char *pmnc_name;
+static int pmnc_counters;
+
+static unsigned long pmnc_enabled[CNTMAX];
+static unsigned long pmnc_event[CNTMAX];
+static unsigned long pmnc_key[CNTMAX];
+
+static DEFINE_PER_CPU(int[CNTMAX * 2], perfCnt);
+
+inline void armv7_pmnc_write(u32 val)
+{
+	val &= PMNC_MASK;
+	asm volatile("mcr p15, 0, %0, c9, c12, 0" : : "r" (val));
+}
+
+inline u32 armv7_pmnc_read(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 0, %0, c9, c12, 0" : "=r" (val));
+	return val;
+}
+
+inline u32 armv7_ccnt_read(u32 reset_value)
+{
+	unsigned long flags;
+	u32 newval = -reset_value;
+	u32 den = CCNT_REG;
+	u32 val;
+
+	local_irq_save(flags);
+	asm volatile("mcr p15, 0, %0, c9, c12, 2" : : "r" (den));	// disable
+	asm volatile("mrc p15, 0, %0, c9, c13, 0" : "=r" (val));	// read
+	asm volatile("mcr p15, 0, %0, c9, c13, 0" : : "r" (newval));// new value
+	asm volatile("mcr p15, 0, %0, c9, c12, 1" : : "r" (den));	// enable
+	local_irq_restore(flags);
+
+	return val;
+}
+
+inline u32 armv7_cntn_read(unsigned int cnt, u32 reset_value)
+{
+	unsigned long flags;
+	u32 newval = -reset_value;
+	u32 sel = (cnt - CNT0);
+	u32 den = 1 << sel;
+	u32 oldval;
+
+	local_irq_save(flags);
+	asm volatile("mcr p15, 0, %0, c9, c12, 2" : : "r" (den));	// disable
+	asm volatile("mcr p15, 0, %0, c9, c12, 5" : : "r" (sel));	// select
+	asm volatile("mrc p15, 0, %0, c9, c13, 2" : "=r" (oldval));	// read
+    asm volatile("mcr p15, 0, %0, c9, c13, 2" : : "r" (newval));// new value
+	asm volatile("mcr p15, 0, %0, c9, c12, 1" : : "r" (den));	// enable
+	local_irq_restore(flags);
+
+	return oldval;
+}
+
+static inline void armv7_pmnc_disable_interrupt(unsigned int cnt)
+{
+	u32 val = cnt ? (1 << (cnt - CNT0)) : (1 << 31);
+	asm volatile("mcr p15, 0, %0, c9, c14, 2" : : "r" (val));
+}
+
+inline u32 armv7_pmnc_reset_interrupt(void)
+{
+	// Get and reset overflow status flags
+	u32 flags;
+	asm volatile("mrc p15, 0, %0, c9, c12, 3" : "=r" (flags));
+	flags &= 0x8000003f;
+	asm volatile("mcr p15, 0, %0, c9, c12, 3" : : "r" (flags));
+	return flags;
+}
+
+static inline u32 armv7_pmnc_enable_counter(unsigned int cnt)
+{
+	u32 val = cnt ? (1 << (cnt - CNT0)) : CCNT_REG;
+	asm volatile("mcr p15, 0, %0, c9, c12, 1" : : "r" (val));
+	return cnt;
+}
+
+static inline u32 armv7_pmnc_disable_counter(unsigned int cnt)
+{
+	u32 val = cnt ? (1 << (cnt - CNT0)) : CCNT_REG;
+	asm volatile("mcr p15, 0, %0, c9, c12, 2" : : "r" (val));
+	return cnt;
+}
+
+static inline int armv7_pmnc_select_counter(unsigned int cnt)
+{
+	u32 val = (cnt - CNT0);
+	asm volatile("mcr p15, 0, %0, c9, c12, 5" : : "r" (val));
+	return cnt;
+}
+
+static inline void armv7_pmnc_write_evtsel(unsigned int cnt, u32 val)
+{
+	if (armv7_pmnc_select_counter(cnt) == cnt) {
+		asm volatile("mcr p15, 0, %0, c9, c13, 1" : : "r" (val));
+	}
+}
+
+static int gator_events_armv7_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+	int i;
+
+	for (i = 0; i < pmnc_counters; i++) {
+		char buf[40];
+		if (i == 0) {
+			snprintf(buf, sizeof buf, "ARM_%s_ccnt", pmnc_name);
+		} else {
+			snprintf(buf, sizeof buf, "ARM_%s_cnt%d", pmnc_name, i-1);
+		}
+		dir = gatorfs_mkdir(sb, root, buf);
+		if (!dir) {
+			return -1;
+		}
+		gatorfs_create_ulong(sb, dir, "enabled", &pmnc_enabled[i]);
+		gatorfs_create_ro_ulong(sb, dir, "key", &pmnc_key[i]);
+		if (i > 0) {
+			gatorfs_create_ulong(sb, dir, "event", &pmnc_event[i]);
+		}
+	}
+
+	return 0;
+}
+
+static int gator_events_armv7_online(int** buffer)
+{
+	unsigned int cnt, len = 0, cpu = smp_processor_id();
+
+	if (armv7_pmnc_read() & PMNC_E) {
+		armv7_pmnc_write(armv7_pmnc_read() & ~PMNC_E);
+	}
+
+	// Initialize & Reset PMNC: C bit and P bit
+	armv7_pmnc_write(PMNC_P | PMNC_C);
+
+	// Reset overflow flags
+	armv7_pmnc_reset_interrupt();
+
+	for (cnt = CCNT; cnt < CNTMAX; cnt++) {
+		unsigned long event;
+
+		if (!pmnc_enabled[cnt])
+			continue;
+
+		// Disable counter
+		armv7_pmnc_disable_counter(cnt);
+
+		event = pmnc_event[cnt] & 255;
+
+		// Set event (if destined for PMNx counters), we don't need to set the event if it's a cycle count
+		if (cnt != CCNT)
+			armv7_pmnc_write_evtsel(cnt, event);
+
+		armv7_pmnc_disable_interrupt(cnt);
+
+		// Reset counter
+		cnt ? armv7_cntn_read(cnt, 0) : armv7_ccnt_read(0);
+
+		// Enable counter
+		armv7_pmnc_enable_counter(cnt);
+	}
+
+	// enable
+	armv7_pmnc_write(armv7_pmnc_read() | PMNC_E);
+
+	// return zero values, no need to read as the counters were just reset
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		if (pmnc_enabled[cnt]) {
+			per_cpu(perfCnt, cpu)[len++] = pmnc_key[cnt];
+			per_cpu(perfCnt, cpu)[len++] = 0;
+		}
+	}
+
+	if (buffer)
+		*buffer = per_cpu(perfCnt, cpu);
+
+	return len;
+}
+
+static int gator_events_armv7_offline(int** buffer)
+{
+	// disbale all counters, including PMCCNTR; overflow IRQs will not be signaled
+	armv7_pmnc_write(armv7_pmnc_read() & ~PMNC_E);
+	
+	return 0;
+}
+
+static void gator_events_armv7_stop(void)
+{
+	unsigned int cnt;
+
+	for (cnt = CCNT; cnt < CNTMAX; cnt++) {
+		pmnc_enabled[cnt] = 0;
+		pmnc_event[cnt] = 0;
+	}
+}
+
+static int gator_events_armv7_read(int **buffer)
+{
+	int cnt, len = 0;
+	int cpu = smp_processor_id();
+
+	// a context switch may occur before the online hotplug event, thus need to check that the pmu is enabled
+	if (!(armv7_pmnc_read() & PMNC_E)) {
+		return 0;
+	}
+
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		if (pmnc_enabled[cnt]) {
+			int value;
+			if (cnt == CCNT) {
+				value = armv7_ccnt_read(0);
+			} else {
+				value = armv7_cntn_read(cnt, 0);
+			}
+			per_cpu(perfCnt, cpu)[len++] = pmnc_key[cnt];
+			per_cpu(perfCnt, cpu)[len++] = value;
+		}
+	}
+
+	if (buffer)
+		*buffer = per_cpu(perfCnt, cpu);
+
+	return len;
+}
+
+static struct gator_interface gator_events_armv7_interface = {
+	.create_files = gator_events_armv7_create_files,
+	.stop = gator_events_armv7_stop,
+	.online = gator_events_armv7_online,
+	.offline = gator_events_armv7_offline,
+	.read = gator_events_armv7_read,
+};
+
+int gator_events_armv7_init(void)
+{
+	unsigned int cnt;
+
+	switch (gator_cpuid()) {
+	case CORTEX_A5:
+		pmnc_name = "Cortex-A5";
+		pmnc_counters = 2;
+		break;
+	case CORTEX_A7:
+		pmnc_name = "Cortex-A7";
+		pmnc_counters = 4;
+		break;
+	case CORTEX_A8:
+		pmnc_name = "Cortex-A8";
+		pmnc_counters = 4;
+		break;
+	case CORTEX_A9:
+		pmnc_name = "Cortex-A9";
+		pmnc_counters = 6;
+		break;
+	case CORTEX_A15:
+		pmnc_name = "Cortex-A15";
+		pmnc_counters = 6;
+		break;
+	default:
+		return -1;
+	}
+
+	pmnc_counters++; // CNT[n] + CCNT
+
+	for (cnt = CCNT; cnt < CNTMAX; cnt++) {
+		pmnc_enabled[cnt] = 0;
+		pmnc_event[cnt] = 0;
+		pmnc_key[cnt] = gator_events_get_key();
+	}
+
+	return gator_events_install(&gator_events_armv7_interface);
+}
+
+gator_events_init(gator_events_armv7_init);
+
+#else
+int gator_events_armv7_init(void)
+{
+	return -1;
+}
+#endif
diff --git a/drivers/gator/gator_events_block.c b/drivers/gator/gator_events_block.c
new file mode 100644
index 00000000000..a8b811413f3
--- /dev/null
+++ b/drivers/gator/gator_events_block.c
@@ -0,0 +1,175 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 "gator.h"
+#include <trace/events/block.h>
+
+#define BLOCK_RQ_WR		0
+#define BLOCK_RQ_RD		1
+
+#define BLOCK_TOTAL		(BLOCK_RQ_RD+1)
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36)
+#define EVENTWRITE REQ_RW
+#else
+#define EVENTWRITE REQ_WRITE
+#endif
+
+static ulong block_rq_wr_enabled;
+static ulong block_rq_rd_enabled;
+static ulong block_rq_wr_key;
+static ulong block_rq_rd_key;
+static DEFINE_PER_CPU(int[BLOCK_TOTAL], blockCnt);
+static DEFINE_PER_CPU(int[BLOCK_TOTAL * 4], blockGet);
+static DEFINE_PER_CPU(bool, new_data_avail);
+
+GATOR_DEFINE_PROBE(block_rq_complete, TP_PROTO(struct request_queue *q, struct request *rq))
+{
+	unsigned long flags;
+	int write, size;
+	int cpu = smp_processor_id();
+
+	if (!rq)
+		return;
+
+	write = rq->cmd_flags & EVENTWRITE;
+	size = rq->resid_len;
+
+	if (!size)
+		return;
+
+	// disable interrupts to synchronize with gator_events_block_read()
+	// spinlocks not needed since percpu buffers are used
+	local_irq_save(flags);
+	if (write)
+		per_cpu(blockCnt, cpu)[BLOCK_RQ_WR] += size;
+	else
+		per_cpu(blockCnt, cpu)[BLOCK_RQ_RD] += size;
+	local_irq_restore(flags);
+
+	per_cpu(new_data_avail, cpu) = true;
+}
+
+static int gator_events_block_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+
+	/* block_complete_wr */
+	dir = gatorfs_mkdir(sb, root, "Linux_block_rq_wr");
+	if (!dir) {
+		return -1;
+	}
+	gatorfs_create_ulong(sb, dir, "enabled", &block_rq_wr_enabled);
+	gatorfs_create_ro_ulong(sb, dir, "key", &block_rq_wr_key);
+
+	/* block_complete_rd */
+	dir = gatorfs_mkdir(sb, root, "Linux_block_rq_rd");
+	if (!dir) {
+		return -1;
+	}
+	gatorfs_create_ulong(sb, dir, "enabled", &block_rq_rd_enabled);
+	gatorfs_create_ro_ulong(sb, dir, "key", &block_rq_rd_key);
+
+	return 0;
+}
+
+static int gator_events_block_start(void)
+{
+	int cpu;
+
+	for_each_present_cpu(cpu) 
+		per_cpu(new_data_avail, cpu) = true;
+
+	// register tracepoints
+	if (block_rq_wr_enabled || block_rq_rd_enabled)
+		if (GATOR_REGISTER_TRACE(block_rq_complete))
+			goto fail_block_rq_exit;
+	pr_debug("gator: registered block event tracepoints\n");
+
+	return 0;
+
+	// unregister tracepoints on error
+fail_block_rq_exit:
+	pr_err("gator: block event tracepoints failed to activate, please verify that tracepoints are enabled in the linux kernel\n");
+
+	return -1;
+}
+
+static void gator_events_block_stop(void)
+{
+	if (block_rq_wr_enabled || block_rq_rd_enabled)
+		GATOR_UNREGISTER_TRACE(block_rq_complete);
+	pr_debug("gator: unregistered block event tracepoints\n");
+
+	block_rq_wr_enabled = 0;
+	block_rq_rd_enabled = 0;
+}
+
+static int gator_events_block_read(int **buffer)
+{
+	unsigned long flags;
+	int len, value, cpu, data = 0;
+	cpu = smp_processor_id();
+
+	if (per_cpu(new_data_avail, cpu) == false)
+		return 0;
+
+	per_cpu(new_data_avail, cpu) = false;
+
+	len = 0;
+	if (block_rq_wr_enabled) {
+		local_irq_save(flags);
+		value = per_cpu(blockCnt, cpu)[BLOCK_RQ_WR];
+		per_cpu(blockCnt, cpu)[BLOCK_RQ_WR] = 0;
+		local_irq_restore(flags);
+		per_cpu(blockGet, cpu)[len++] = block_rq_wr_key;
+		per_cpu(blockGet, cpu)[len++] = 0; // indicates to Streamline that value bytes were written now, not since the last message
+		per_cpu(blockGet, cpu)[len++] = block_rq_wr_key;
+		per_cpu(blockGet, cpu)[len++] = value;
+		data += value;
+	}
+	if (block_rq_rd_enabled) {
+		local_irq_save(flags);
+		value = per_cpu(blockCnt, cpu)[BLOCK_RQ_RD];
+		per_cpu(blockCnt, cpu)[BLOCK_RQ_RD] = 0;
+		local_irq_restore(flags);
+		per_cpu(blockGet, cpu)[len++] = block_rq_rd_key;
+		per_cpu(blockGet, cpu)[len++] = 0; // indicates to Streamline that value bytes were read now, not since the last message
+		per_cpu(blockGet, cpu)[len++] = block_rq_rd_key;
+		per_cpu(blockGet, cpu)[len++] = value;
+		data += value;
+	}
+
+	if (data != 0)
+		per_cpu(new_data_avail, cpu) = true;
+
+	if (buffer)
+		*buffer = per_cpu(blockGet, cpu);
+
+	return len;
+}
+
+static struct gator_interface gator_events_block_interface = {
+	.create_files = gator_events_block_create_files,
+	.start = gator_events_block_start,
+	.stop = gator_events_block_stop,
+	.read = gator_events_block_read,
+};
+
+int gator_events_block_init(void)
+{
+	block_rq_wr_enabled = 0;
+	block_rq_rd_enabled = 0;
+
+	block_rq_wr_key = gator_events_get_key();
+	block_rq_rd_key = gator_events_get_key();
+
+	return gator_events_install(&gator_events_block_interface);
+}
+gator_events_init(gator_events_block_init);
diff --git a/drivers/gator/gator_events_irq.c b/drivers/gator/gator_events_irq.c
new file mode 100644
index 00000000000..435bc86fb5d
--- /dev/null
+++ b/drivers/gator/gator_events_irq.c
@@ -0,0 +1,188 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 "gator.h"
+#include <trace/events/irq.h>
+
+#define HARDIRQ		0
+#define SOFTIRQ		1
+#define TOTALIRQ	(SOFTIRQ+1)
+
+static ulong hardirq_enabled;
+static ulong softirq_enabled;
+static ulong hardirq_key;
+static ulong softirq_key;
+static DEFINE_PER_CPU(int[TOTALIRQ], irqCnt);
+static DEFINE_PER_CPU(int[TOTALIRQ * 2], irqGet);
+
+GATOR_DEFINE_PROBE(irq_handler_exit, TP_PROTO(int irq,
+		struct irqaction *action, int ret))
+{
+	unsigned long flags;
+
+	// disable interrupts to synchronize with gator_events_irq_read()
+	// spinlocks not needed since percpu buffers are used
+	local_irq_save(flags);
+	per_cpu(irqCnt, smp_processor_id())[HARDIRQ]++;
+	local_irq_restore(flags);
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 37)
+GATOR_DEFINE_PROBE(softirq_exit, TP_PROTO(struct softirq_action *h, struct softirq_action *vec))
+#else
+GATOR_DEFINE_PROBE(softirq_exit, TP_PROTO(unsigned int vec_nr))
+#endif
+{
+	unsigned long flags;
+
+	// disable interrupts to synchronize with gator_events_irq_read()
+	// spinlocks not needed since percpu buffers are used
+	local_irq_save(flags);
+	per_cpu(irqCnt, smp_processor_id())[SOFTIRQ]++;
+	local_irq_restore(flags);
+}
+
+static int gator_events_irq_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+
+	/* irq */
+	dir = gatorfs_mkdir(sb, root, "Linux_irq_irq");
+	if (!dir) {
+		return -1;
+	}
+	gatorfs_create_ulong(sb, dir, "enabled", &hardirq_enabled);
+	gatorfs_create_ro_ulong(sb, dir, "key", &hardirq_key);
+
+	/* soft irq */
+	dir = gatorfs_mkdir(sb, root, "Linux_irq_softirq");
+	if (!dir) {
+		return -1;
+	}
+	gatorfs_create_ulong(sb, dir, "enabled", &softirq_enabled);
+	gatorfs_create_ro_ulong(sb, dir, "key", &softirq_key);
+
+	return 0;
+}
+
+static int gator_events_irq_online(int** buffer)
+{
+	int len = 0, cpu = smp_processor_id();
+	unsigned long flags; // not necessary as we are in interrupt context anyway, but doesn't hurt
+
+	// synchronization with the irq_exit functions is not necessary as the values are being reset
+	if (hardirq_enabled) {
+		local_irq_save(flags);
+		per_cpu(irqCnt, cpu)[HARDIRQ] = 0;
+		local_irq_restore(flags);
+		per_cpu(irqGet, cpu)[len++] = hardirq_key;
+		per_cpu(irqGet, cpu)[len++] = 0;
+	}
+
+	if (softirq_enabled) {
+		local_irq_save(flags);
+		per_cpu(irqCnt, cpu)[SOFTIRQ] = 0;
+		local_irq_restore(flags);
+		per_cpu(irqGet, cpu)[len++] = softirq_key;
+		per_cpu(irqGet, cpu)[len++] = 0;
+	}
+
+	if (buffer)
+		*buffer = per_cpu(irqGet, cpu);
+
+	return len;
+}
+
+static int gator_events_irq_start(void)
+{
+	// register tracepoints
+	if (hardirq_enabled)
+		if (GATOR_REGISTER_TRACE(irq_handler_exit))
+			goto fail_hardirq_exit;
+	if (softirq_enabled)
+		if (GATOR_REGISTER_TRACE(softirq_exit))
+			goto fail_softirq_exit;
+	pr_debug("gator: registered irq tracepoints\n");
+
+	return 0;
+
+	// unregister tracepoints on error
+fail_softirq_exit:
+	if (hardirq_enabled)
+		GATOR_UNREGISTER_TRACE(irq_handler_exit);
+fail_hardirq_exit:
+	pr_err("gator: irq tracepoints failed to activate, please verify that tracepoints are enabled in the linux kernel\n");
+
+	return -1;
+}
+
+static void gator_events_irq_stop(void)
+{
+	if (hardirq_enabled)
+		GATOR_UNREGISTER_TRACE(irq_handler_exit);
+	if (softirq_enabled)
+		GATOR_UNREGISTER_TRACE(softirq_exit);
+	pr_debug("gator: unregistered irq tracepoints\n");
+
+	hardirq_enabled = 0;
+	softirq_enabled = 0;
+}
+
+static int gator_events_irq_read(int **buffer)
+{
+	unsigned long flags; // not necessary as we are in interrupt context anyway, but doesn't hurt
+	int len, value;
+	int cpu = smp_processor_id();
+
+	len = 0;
+	if (hardirq_enabled) {
+		local_irq_save(flags);
+		value = per_cpu(irqCnt, cpu)[HARDIRQ];
+		per_cpu(irqCnt, cpu)[HARDIRQ] = 0;
+		local_irq_restore(flags);
+
+		per_cpu(irqGet, cpu)[len++] = hardirq_key;
+		per_cpu(irqGet, cpu)[len++] = value;
+	}
+
+	if (softirq_enabled) {
+		local_irq_save(flags);
+		value = per_cpu(irqCnt, cpu)[SOFTIRQ];
+		per_cpu(irqCnt, cpu)[SOFTIRQ] = 0;
+		local_irq_restore(flags);
+
+		per_cpu(irqGet, cpu)[len++] = softirq_key;
+		per_cpu(irqGet, cpu)[len++] = value;
+	}
+
+	if (buffer)
+		*buffer = per_cpu(irqGet, cpu);
+
+	return len;
+}
+
+static struct gator_interface gator_events_irq_interface = {
+	.create_files = gator_events_irq_create_files,
+	.online = gator_events_irq_online,
+	.start = gator_events_irq_start,
+	.stop = gator_events_irq_stop,
+	.read = gator_events_irq_read,
+};
+
+int gator_events_irq_init(void)
+{
+	hardirq_key = gator_events_get_key();
+	softirq_key = gator_events_get_key();
+
+	hardirq_enabled = 0;
+	softirq_enabled = 0;
+
+	return gator_events_install(&gator_events_irq_interface);
+}
+gator_events_init(gator_events_irq_init);
diff --git a/drivers/gator/gator_events_l2c-310.c b/drivers/gator/gator_events_l2c-310.c
new file mode 100644
index 00000000000..bd1c48a64e2
--- /dev/null
+++ b/drivers/gator/gator_events_l2c-310.c
@@ -0,0 +1,179 @@
+/**
+ * l2c310 (L2 Cache Controller) event counters for gator
+ *
+ * Copyright (C) ARM Limited 2010-2012. 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/io.h>
+#include <asm/hardware/cache-l2x0.h>
+
+#include "gator.h"
+
+#define L2C310_COUNTERS_NUM 2
+
+static struct {
+	unsigned long enabled;
+	unsigned long event;
+	unsigned long key;
+} l2c310_counters[L2C310_COUNTERS_NUM];
+
+static int l2c310_buffer[L2C310_COUNTERS_NUM * 2];
+
+static void __iomem *l2c310_base;
+
+
+
+static void gator_events_l2c310_reset_counters(void)
+{
+	u32 val = readl(l2c310_base + L2X0_EVENT_CNT_CTRL);
+
+	val |= ((1 << L2C310_COUNTERS_NUM) - 1) << 1;
+
+	writel(val, l2c310_base + L2X0_EVENT_CNT_CTRL);
+}
+
+
+static int gator_events_l2c310_create_files(struct super_block *sb,
+		struct dentry *root)
+{
+	int i;
+
+	for (i = 0; i < L2C310_COUNTERS_NUM; i++) {
+		char buf[16];
+		struct dentry *dir;
+
+		snprintf(buf, sizeof(buf), "L2C-310_cnt%d", i);
+		dir = gatorfs_mkdir(sb, root, buf);
+		if (WARN_ON(!dir))
+			return -1;
+		gatorfs_create_ulong(sb, dir, "enabled",
+				&l2c310_counters[i].enabled);
+		gatorfs_create_ulong(sb, dir, "event",
+				&l2c310_counters[i].event);
+		gatorfs_create_ro_ulong(sb, dir, "key",
+				&l2c310_counters[i].key);
+	}
+
+	return 0;
+}
+
+static int gator_events_l2c310_start(void)
+{
+	static const unsigned long l2x0_event_cntx_cfg[L2C310_COUNTERS_NUM] = {
+		L2X0_EVENT_CNT0_CFG,
+		L2X0_EVENT_CNT1_CFG,
+	};
+	int i;
+
+	/* Counter event sources */
+	for (i = 0; i < L2C310_COUNTERS_NUM; i++)
+		writel((l2c310_counters[i].event & 0xf) << 2,
+				l2c310_base + l2x0_event_cntx_cfg[i]);
+
+	gator_events_l2c310_reset_counters();
+
+	/* Event counter enable */
+	writel(1, l2c310_base + L2X0_EVENT_CNT_CTRL);
+
+	return 0;
+}
+
+static void gator_events_l2c310_stop(void)
+{
+	/* Event counter disable */
+	writel(0, l2c310_base + L2X0_EVENT_CNT_CTRL);
+}
+
+static int gator_events_l2c310_read(int **buffer)
+{
+	static const unsigned long l2x0_event_cntx_val[L2C310_COUNTERS_NUM] = {
+		L2X0_EVENT_CNT0_VAL,
+		L2X0_EVENT_CNT1_VAL,
+	};
+	int i;
+	int len = 0;
+
+	if (smp_processor_id())
+		return 0;
+
+	for (i = 0; i < L2C310_COUNTERS_NUM; i++) {
+		if (l2c310_counters[i].enabled) {
+			l2c310_buffer[len++] = l2c310_counters[i].key;
+			l2c310_buffer[len++] = readl(l2c310_base +
+					l2x0_event_cntx_val[i]);
+		}
+	}
+	
+	/* l2c310 counters are saturating, not wrapping in case of overflow */
+	gator_events_l2c310_reset_counters();
+
+	if (buffer)
+		*buffer = l2c310_buffer;
+
+	return len;
+}
+
+static struct gator_interface gator_events_l2c310_interface = {
+	.create_files = gator_events_l2c310_create_files,
+	.start = gator_events_l2c310_start,
+	.stop = gator_events_l2c310_stop,
+	.read = gator_events_l2c310_read,
+};
+
+static void __maybe_unused gator_events_l2c310_probe(unsigned long phys)
+{
+	if (l2c310_base)
+		return;
+
+	l2c310_base = ioremap(phys, SZ_4K);
+	if (l2c310_base) {
+		u32 cache_id = readl(l2c310_base + L2X0_CACHE_ID);
+
+		if ((cache_id & 0xff0003c0) != 0x410000c0) {
+			iounmap(l2c310_base);
+			l2c310_base = NULL;
+		}
+	}
+}
+
+int gator_events_l2c310_init(void)
+{
+	int i;
+
+	if (gator_cpuid() != CORTEX_A5 && gator_cpuid() != CORTEX_A9)
+		return -1;
+
+#if defined(CONFIG_ARCH_EXYNOS4) || defined(CONFIG_ARCH_S5PV310)
+	gator_events_l2c310_probe(0x10502000);
+#endif
+#if defined(CONFIG_ARCH_OMAP4)
+	gator_events_l2c310_probe(0x48242000);
+#endif
+#if defined(CONFIG_ARCH_TEGRA)
+	gator_events_l2c310_probe(0x50043000);
+#endif
+#if defined(CONFIG_ARCH_U8500)
+	gator_events_l2c310_probe(0xa0412000);
+#endif
+#if defined(CONFIG_ARCH_VEXPRESS)
+	// A9x4 core tile (HBI-0191)
+	gator_events_l2c310_probe(0x1e00a000);
+	// New memory map tiles
+	gator_events_l2c310_probe(0x2c0f0000);
+#endif
+	if (!l2c310_base)
+		return -1;
+
+	for (i = 0; i < L2C310_COUNTERS_NUM; i++) {
+		l2c310_counters[i].enabled = 0;
+		l2c310_counters[i].key = gator_events_get_key();
+	}
+
+	return gator_events_install(&gator_events_l2c310_interface);
+}
+gator_events_init(gator_events_l2c310_init);
diff --git a/drivers/gator/gator_events_mali.c b/drivers/gator/gator_events_mali.c
new file mode 100755
index 00000000000..e3b6e13923a
--- /dev/null
+++ b/drivers/gator/gator_events_mali.c
@@ -0,0 +1,732 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 "gator.h"
+
+#include <linux/module.h>
+#include <linux/time.h>
+#include <linux/math64.h>
+
+#include "linux/mali_linux_trace.h"
+
+#if !defined(GATOR_MALI_INTERFACE_STYLE)
+/*
+ * At the moment, we only have users with the old style interface, so
+ * make our life easier by making it the default...
+ */
+#define GATOR_MALI_INTERFACE_STYLE (2)
+#endif
+
+/*
+ * There are (currently) three different variants of the comms between gator and Mali:
+ * 1 (deprecated): No software counter support
+ * 2 (deprecated): Tracepoint called for each separate s/w counter value as it appears
+ * 3 (default): Single tracepoint for all s/w counters in a bundle.
+ * Interface style 3 is the default if no other is specified.  1 and 2 will be eliminated when
+ * existing Mali DDKs are upgraded.
+ */
+
+#if !defined(GATOR_MALI_INTERFACE_STYLE)
+#define GATOR_MALI_INTERFACE_STYLE (3)
+#endif
+
+#define MALI_200     (0x0a07)
+#define MALI_300     (0x0b06) //This is not actually true; Mali-300 is also 0x0b07
+#define MALI_400     (0x0b07)
+#define MALI_T6xx    (0x0056)
+
+/*
+ * List of possible actions allowing DDK to be controlled by Streamline.
+ * The following numbers are used by DDK to control the frame buffer dumping.
+ */
+#define FBDUMP_CONTROL_ENABLE (1)
+#define FBDUMP_CONTROL_RATE (2)
+#define SW_EVENTS_ENABLE      (3)
+#define FBDUMP_CONTROL_RESIZE_FACTOR (4)
+
+/*
+ * Check that the MALI_SUPPORT define is set to one of the allowable device codes.
+ */
+#if !defined(MALI_SUPPORT)
+#error MALI_SUPPORT not defined!
+#elif (MALI_SUPPORT != MALI_200) && (MALI_SUPPORT != MALI_300) && (MALI_SUPPORT != MALI_400) && (MALI_SUPPORT != MALI_T6xx)
+#error MALI_SUPPORT set to an invalid device code
+#endif
+
+static const char *mali_name;
+
+enum counters {
+    /* Timeline activity */
+    ACTIVITY_VP = 0,
+    ACTIVITY_FP0,
+    ACTIVITY_FP1,
+    ACTIVITY_FP2,
+    ACTIVITY_FP3,
+    
+    /* L2 cache counters */
+    COUNTER_L2_C0,
+    COUNTER_L2_C1,
+
+    /* Vertex processor counters */
+    COUNTER_VP_C0,
+    COUNTER_VP_C1, 
+
+    /* Fragment processor counters */
+    COUNTER_FP0_C0,    
+    COUNTER_FP0_C1,     
+    COUNTER_FP1_C0,
+    COUNTER_FP1_C1,
+    COUNTER_FP2_C0,
+    COUNTER_FP2_C1,
+    COUNTER_FP3_C0,
+    COUNTER_FP3_C1,
+
+    /* EGL Software Counters */
+    COUNTER_EGL_BLIT_TIME,
+
+    /* GLES Software Counters */
+    COUNTER_GLES_DRAW_ELEMENTS_CALLS,
+    COUNTER_GLES_DRAW_ELEMENTS_NUM_INDICES,
+    COUNTER_GLES_DRAW_ELEMENTS_NUM_TRANSFORMED,
+    COUNTER_GLES_DRAW_ARRAYS_CALLS,
+    COUNTER_GLES_DRAW_ARRAYS_NUM_TRANSFORMED,
+    COUNTER_GLES_DRAW_POINTS,
+    COUNTER_GLES_DRAW_LINES,
+    COUNTER_GLES_DRAW_LINE_LOOP,
+    COUNTER_GLES_DRAW_LINE_STRIP,
+    COUNTER_GLES_DRAW_TRIANGLES,
+    COUNTER_GLES_DRAW_TRIANGLE_STRIP,
+    COUNTER_GLES_DRAW_TRIANGLE_FAN,
+    COUNTER_GLES_NON_VBO_DATA_COPY_TIME,
+    COUNTER_GLES_UNIFORM_BYTES_COPIED_TO_MALI,
+    COUNTER_GLES_UPLOAD_TEXTURE_TIME,
+    COUNTER_GLES_UPLOAD_VBO_TIME,
+    COUNTER_GLES_NUM_FLUSHES,
+    COUNTER_GLES_NUM_VSHADERS_GENERATED,
+    COUNTER_GLES_NUM_FSHADERS_GENERATED,
+    COUNTER_GLES_VSHADER_GEN_TIME,
+    COUNTER_GLES_FSHADER_GEN_TIME,
+    COUNTER_GLES_INPUT_TRIANGLES,
+    COUNTER_GLES_VXCACHE_HIT,
+    COUNTER_GLES_VXCACHE_MISS,
+    COUNTER_GLES_VXCACHE_COLLISION,
+    COUNTER_GLES_CULLED_TRIANGLES,
+    COUNTER_GLES_CULLED_LINES,
+    COUNTER_GLES_BACKFACE_TRIANGLES,
+    COUNTER_GLES_GBCLIP_TRIANGLES,
+    COUNTER_GLES_GBCLIP_LINES,
+    COUNTER_GLES_TRIANGLES_DRAWN,
+    COUNTER_GLES_DRAWCALL_TIME,
+    COUNTER_GLES_TRIANGLES_COUNT,
+    COUNTER_GLES_INDEPENDENT_TRIANGLES_COUNT,
+    COUNTER_GLES_STRIP_TRIANGLES_COUNT,
+    COUNTER_GLES_FAN_TRIANGLES_COUNT,
+    COUNTER_GLES_LINES_COUNT,
+    COUNTER_GLES_INDEPENDENT_LINES_COUNT,
+    COUNTER_GLES_STRIP_LINES_COUNT,
+    COUNTER_GLES_LOOP_LINES_COUNT,
+
+	COUNTER_FILMSTRIP,
+
+    NUMBER_OF_EVENTS
+};
+
+#define FIRST_ACTIVITY_EVENT    ACTIVITY_VP
+#define LAST_ACTIVITY_EVENT     ACTIVITY_FP3
+
+#define FIRST_HW_COUNTER        COUNTER_L2_C0
+#define LAST_HW_COUNTER         COUNTER_FP3_C1
+
+#define FIRST_SW_COUNTER        COUNTER_EGL_BLIT_TIME
+#define LAST_SW_COUNTER         COUNTER_GLES_LOOP_LINES_COUNT
+
+#define FIRST_SPECIAL_COUNTER   COUNTER_FILMSTRIP
+#define LAST_SPECIAL_COUNTER    COUNTER_FILMSTRIP
+
+/* gatorfs variables for counter enable state,
+ * the event the counter should count and the
+ * 'key' (a unique id set by gatord and returned
+ * by gator.ko)
+ */
+static unsigned long counter_enabled[NUMBER_OF_EVENTS];
+static unsigned long counter_event[NUMBER_OF_EVENTS];
+static unsigned long counter_key[NUMBER_OF_EVENTS];
+
+/* The data we have recorded */
+static u32 counter_data[NUMBER_OF_EVENTS];
+/* The address to sample (or 0 if samples are sent to us) */
+static u32* counter_address[NUMBER_OF_EVENTS];
+
+/* An array used to return the data we recorded
+ * as key,value pairs hence the *2
+ */
+static unsigned long counter_dump[NUMBER_OF_EVENTS * 2];
+static unsigned long counter_prev[NUMBER_OF_EVENTS];
+
+/* Note whether tracepoints have been registered */
+static int trace_registered;
+
+/**
+ * Calculate the difference and handle the overflow.
+ */
+static u32 get_difference(u32 start, u32 end)
+{
+	if (start - end >= 0)
+	{
+		return start - end;
+	}
+
+	// Mali counters are unsigned 32 bit values that wrap.
+	return (4294967295u - end) + start;
+}
+
+/**
+ * Returns non-zero if the given counter ID is an activity counter.
+ */
+static inline int is_activity_counter(unsigned int event_id)
+{
+    return (event_id >= FIRST_ACTIVITY_EVENT && 
+        event_id <= LAST_ACTIVITY_EVENT);
+}
+
+/**
+ * Returns non-zero if the given counter ID is a hardware counter.
+ */
+static inline int is_hw_counter(unsigned int event_id)
+{
+    return (event_id >= FIRST_HW_COUNTER && event_id <= LAST_HW_COUNTER);
+}
+
+/**
+ * Returns non-zero if the given counter ID is a software counter.
+ */
+static inline int is_sw_counter(unsigned int event_id)
+{
+    return (event_id >= FIRST_SW_COUNTER && event_id <= LAST_SW_COUNTER);
+}
+
+/*
+ * The Mali DDK uses s64 types to contain software counter values, but gator
+ * can only use a maximum of 32 bits. This function scales a software counter
+ * to an appopriate range.
+ */
+static u32 scale_sw_counter_value(unsigned int event_id, signed long long value)
+{
+    u32 scaled_value;
+
+    switch (event_id) {
+        case COUNTER_GLES_UPLOAD_TEXTURE_TIME:
+        case COUNTER_GLES_UPLOAD_VBO_TIME:
+            scaled_value = (u32)div_s64(value, 1000000);
+            break;
+        default:
+            scaled_value = (u32)value;
+            break;
+    }
+
+    return scaled_value;
+}
+
+/* Probe for continuously sampled counter */
+#if 0 //WE_DONT_CURRENTLY_USE_THIS_SO_SUPPRESS_WARNING
+GATOR_DEFINE_PROBE(mali_sample_address, TP_PROTO(unsigned int event_id, u32* addr))
+{
+    /* Turning on too many pr_debug statements in frequently called functions
+     * can cause stability and/or performance problems
+     */
+    //pr_debug("gator: mali_sample_address %d %d\n", event_id, addr);
+    if (event_id >= ACTIVITY_VP && event_id <= COUNTER_FP3_C1) {
+        counter_address[event_id] = addr;
+    }
+}
+#endif
+
+/* Probe for hardware counter events */
+GATOR_DEFINE_PROBE(mali_hw_counter, TP_PROTO(unsigned int event_id, unsigned int value))
+{
+    /* Turning on too many pr_debug statements in frequently called functions
+     * can cause stability and/or performance problems
+     */
+    //pr_debug("gator: mali_hw_counter %d %d\n", event_id, value);
+    if (is_hw_counter(event_id)) {
+        counter_data[event_id] = value;
+    }
+}
+
+#if GATOR_MALI_INTERFACE_STYLE == 2
+GATOR_DEFINE_PROBE(mali_sw_counter, TP_PROTO(unsigned int event_id, signed long long value))
+{
+    if (is_sw_counter(event_id)) {
+        counter_data[event_id] = scale_sw_counter_value(event_id, value);
+    }
+}
+#endif /* GATOR_MALI_INTERFACE_STYLE == 2 */
+
+
+#if GATOR_MALI_INTERFACE_STYLE == 3
+GATOR_DEFINE_PROBE(mali_sw_counters, TP_PROTO(pid_t pid, pid_t tid, void * surface_id, unsigned int * counters))
+{
+	u32 i;
+
+	/* Copy over the values for those counters which are enabled. */
+	for(i=FIRST_SW_COUNTER; i <= LAST_SW_COUNTER; i++)
+	{
+		if(counter_enabled[i])
+		{
+			counter_data[i] = (u32)(counters[i - FIRST_SW_COUNTER]);
+		}
+	}
+}
+#endif /* GATOR_MALI_INTERFACE_STYLE == 3 */
+
+//TODO need to work out how many fp units we have
+u32 gator_mali_get_n_fp(void) {
+    return 4;
+}
+
+//TODO need to work out what kind of Mali we are looking at
+u32 gator_mali_get_id(void) {
+    return MALI_SUPPORT;
+}
+
+int gator_events_mali_create_files(struct super_block *sb, struct dentry *root) {
+    struct dentry *dir;
+    int event;
+    int n_fp = gator_mali_get_n_fp();
+
+    /*
+     * Create the filesystem entries for vertex processor, fragement processor
+     * and L2 cache timeline and hardware counters. Software counters get 
+     * special handling after this block.
+     */
+    for (event = FIRST_ACTIVITY_EVENT; event <= LAST_HW_COUNTER; event++)
+    {
+        char buf[40];
+
+        /* 
+         * We can skip this event if it's for a non-existent fragment
+         * processor.
+         */
+        if (((event - ACTIVITY_FP0 >= n_fp) && (event < COUNTER_L2_C0)) ||
+            (((event - COUNTER_FP0_C0)/2 >= n_fp)))
+        {
+            continue;
+        }
+
+        /* Otherwise, set up the filesystem entry for this event. */
+        switch (event) {
+            case ACTIVITY_VP:
+                snprintf(buf, sizeof buf, "ARM_%s_VP_active", mali_name);
+                break;
+            case ACTIVITY_FP0:
+            case ACTIVITY_FP1:
+            case ACTIVITY_FP2:
+            case ACTIVITY_FP3:
+                snprintf(buf, sizeof buf, "ARM_%s_FP%d_active", 
+                    mali_name, event - ACTIVITY_FP0);
+                break;
+            case COUNTER_L2_C0:
+            case COUNTER_L2_C1:
+                snprintf(buf, sizeof buf, "ARM_%s_L2_cnt%d", 
+                    mali_name, event - COUNTER_L2_C0);
+                break;
+            case COUNTER_VP_C0:
+            case COUNTER_VP_C1:
+                snprintf(buf, sizeof buf, "ARM_%s_VP_cnt%d", 
+                    mali_name, event - COUNTER_VP_C0);
+                break;
+            case COUNTER_FP0_C0:
+            case COUNTER_FP0_C1:
+            case COUNTER_FP1_C0:
+            case COUNTER_FP1_C1:
+            case COUNTER_FP2_C0:
+            case COUNTER_FP2_C1:
+            case COUNTER_FP3_C0:
+            case COUNTER_FP3_C1:
+                snprintf(buf, sizeof buf, "ARM_%s_FP%d_cnt%d", mali_name, 
+                    (event - COUNTER_FP0_C0) / 2, (event - COUNTER_FP0_C0) % 2);
+                break;
+            default:
+                printk("gator: trying to create file for non-existent counter (%d)\n", event);
+                continue;
+        }
+
+        dir = gatorfs_mkdir(sb, root, buf);
+        
+        if (!dir) {
+            return -1;
+        }
+        
+        gatorfs_create_ulong(sb, dir, "enabled", &counter_enabled[event]);
+        
+        /* Only create an event node for counters that can change what they count */
+        if (event >= COUNTER_L2_C0) {
+            gatorfs_create_ulong(sb, dir, "event", &counter_event[event]);
+        }
+        
+        gatorfs_create_ro_ulong(sb, dir, "key", &counter_key[event]);
+    }
+
+    /* Now set up the software counter entries */
+    for (event = FIRST_SW_COUNTER; event <= LAST_SW_COUNTER; event++)
+    {
+        char buf[40];
+
+        snprintf(buf, sizeof(buf), "ARM_%s_SW_%d", mali_name, event);
+
+        dir = gatorfs_mkdir(sb, root, buf);
+
+        if (!dir) {
+            return -1;
+        }
+
+        gatorfs_create_ulong(sb, dir, "enabled", &counter_enabled[event]);
+        gatorfs_create_ro_ulong(sb, dir, "key", &counter_key[event]);
+    }
+
+	/* Now set up the special counter entries */
+    for (event = FIRST_SPECIAL_COUNTER; event <= LAST_SPECIAL_COUNTER; event++)
+    {
+        char buf[40];
+
+        snprintf(buf, sizeof(buf), "ARM_%s_Filmstrip", mali_name);
+
+        dir = gatorfs_mkdir(sb, root, buf);
+
+        if (!dir) {
+            return -1;
+        }
+
+        gatorfs_create_ulong(sb, dir, "event", &counter_event[event]);
+        gatorfs_create_ulong(sb, dir, "enabled", &counter_enabled[event]);
+        gatorfs_create_ro_ulong(sb, dir, "key", &counter_key[event]);
+    }
+
+
+    return 0;
+}
+
+//TODO
+void _mali_profiling_set_event(unsigned int, unsigned int);
+void _mali_osk_fb_control_set(unsigned int, unsigned int);
+void _mali_profiling_control(unsigned int, unsigned int);
+
+void _mali_profiling_get_counters(unsigned int*, unsigned int*, unsigned int*, unsigned int*);
+void (*_mali_profiling_get_counters_function_pointer)(unsigned int*, unsigned int*, unsigned int*, unsigned int*);
+
+/*
+ * Examine list of software counters and determine if any one is enabled.
+ * Returns 1 if any counter is enabled, 0 if none is.
+ */
+static int is_any_sw_counter_enabled(void)
+{
+	unsigned int i;
+
+	for (i = FIRST_SW_COUNTER; i <= LAST_SW_COUNTER; i++)
+	{
+		if (counter_enabled[i])
+		{
+			return 1;	/* At least one counter is enabled */
+		}
+	}
+
+	return 0;	/* No s/w counters enabled */
+}
+
+static void mali_counter_initialize(void) 
+{
+    /* If a Mali driver is present and exporting the appropriate symbol
+     * then we can request the HW counters (of which there are only 2)
+     * be configured to count the desired events
+     */
+    void (*set_hw_event)(unsigned int, unsigned int);
+	void (*set_fb_event)(unsigned int, unsigned int);
+	void (*mali_control)(unsigned int, unsigned int);
+
+    set_hw_event = symbol_get(_mali_profiling_set_event);
+
+    if (set_hw_event) {
+        int i;
+
+        pr_debug("gator: mali online _mali_profiling_set_event symbol @ %p\n",set_hw_event);
+
+        for (i = FIRST_HW_COUNTER; i <= LAST_HW_COUNTER; i++) {
+            if (counter_enabled[i]) {
+                set_hw_event(i, counter_event[i]);
+            } else {
+                set_hw_event(i, 0xFFFFFFFF);
+            }
+        }
+
+        symbol_put(_mali_profiling_set_event);
+    } else {
+        printk("gator: mali online _mali_profiling_set_event symbol not found\n");
+    }
+
+	set_fb_event = symbol_get(_mali_osk_fb_control_set);
+
+	if (set_fb_event) {
+		pr_debug("gator: mali online _mali_osk_fb_control_set symbol @ %p\n", set_fb_event);
+
+        set_fb_event(0,(counter_enabled[COUNTER_FILMSTRIP]?1:0));
+
+		symbol_put(_mali_osk_fb_control_set);
+	} else {
+		printk("gator: mali online _mali_osk_fb_control_set symbol not found\n");
+	}
+
+	/* Generic control interface for Mali DDK. */
+	mali_control = symbol_get(_mali_profiling_control);
+	if (mali_control) {
+		/* The event attribute in the XML file keeps the actual frame rate. */
+		unsigned int rate = counter_event[COUNTER_FILMSTRIP] & 0xff;
+		unsigned int resize_factor = (counter_event[COUNTER_FILMSTRIP] >> 8) & 0xff;
+
+		pr_debug("gator: mali online _mali_profiling_control symbol @ %p\n", mali_control);
+
+		mali_control(SW_EVENTS_ENABLE, (is_any_sw_counter_enabled()?1:0));
+		mali_control(FBDUMP_CONTROL_ENABLE, (counter_enabled[COUNTER_FILMSTRIP]?1:0));
+		mali_control(FBDUMP_CONTROL_RATE, rate);
+		mali_control(FBDUMP_CONTROL_RESIZE_FACTOR, resize_factor);
+
+		pr_debug("gator: sent mali_control enabled=%d, rate=%d\n", (counter_enabled[COUNTER_FILMSTRIP]?1:0), rate);
+
+		symbol_put(_mali_profiling_control);
+	} else {
+		printk("gator: mali online _mali_profiling_control symbol not found\n");
+	}
+
+	_mali_profiling_get_counters_function_pointer =  symbol_get(_mali_profiling_get_counters);
+	if (_mali_profiling_get_counters_function_pointer){
+		pr_debug("gator: mali online _mali_profiling_get_counters symbol @ %p\n", _mali_profiling_get_counters_function_pointer);
+		counter_prev[COUNTER_L2_C0] = 0;
+		counter_prev[COUNTER_L2_C1] = 0;
+	}
+	else{
+		pr_debug("gator WARNING: mali _mali_profiling_get_counters symbol  not defined");
+	}
+}
+
+static void mali_counter_deinitialize(void) 
+{
+    void (*set_hw_event)(unsigned int, unsigned int);
+	void (*set_fb_event)(unsigned int, unsigned int);
+	void (*mali_control)(unsigned int, unsigned int);
+
+    set_hw_event = symbol_get(_mali_profiling_set_event);
+
+    if (set_hw_event) {
+        int i;
+        
+        pr_debug("gator: mali offline _mali_profiling_set_event symbol @ %p\n",set_hw_event);
+        for (i = FIRST_HW_COUNTER; i <= LAST_HW_COUNTER; i++) {
+            set_hw_event(i, 0xFFFFFFFF);
+        }
+        
+        symbol_put(_mali_profiling_set_event);
+    } else {
+        printk("gator: mali offline _mali_profiling_set_event symbol not found\n");
+    }
+
+	set_fb_event = symbol_get(_mali_osk_fb_control_set);
+
+	if (set_fb_event) {
+		pr_debug("gator: mali offline _mali_osk_fb_control_set symbol @ %p\n", set_fb_event);
+
+        set_fb_event(0,0);
+
+		symbol_put(_mali_osk_fb_control_set);
+	} else {
+		printk("gator: mali offline _mali_osk_fb_control_set symbol not found\n");
+	}
+	
+	/* Generic control interface for Mali DDK. */
+	mali_control = symbol_get(_mali_profiling_control);
+
+	if (mali_control) {
+		pr_debug("gator: mali offline _mali_profiling_control symbol @ %p\n", set_fb_event);
+
+		/* Reset the DDK state - disable counter collection */
+		mali_control(SW_EVENTS_ENABLE, 0);
+
+		mali_control(FBDUMP_CONTROL_ENABLE, 0);
+
+		symbol_put(_mali_profiling_control);
+	} else {
+		printk("gator: mali offline _mali_profiling_control symbol not found\n");
+	}
+
+	if (_mali_profiling_get_counters_function_pointer){
+		symbol_put(_mali_profiling_get_counters);
+	}
+
+}
+
+static int gator_events_mali_start(void) {
+    // register tracepoints
+    if (GATOR_REGISTER_TRACE(mali_hw_counter)) {
+        printk("gator: mali_hw_counter tracepoint failed to activate\n");
+        return -1;
+    }
+
+#if GATOR_MALI_INTERFACE_STYLE == 1
+		/* None. */
+#elif GATOR_MALI_INTERFACE_STYLE == 2
+		/* For patched Mali driver. */
+    if (GATOR_REGISTER_TRACE(mali_sw_counter)) {
+        printk("gator: mali_sw_counter tracepoint failed to activate\n");
+        return -1;
+    }
+#elif GATOR_MALI_INTERFACE_STYLE == 3
+/* For Mali drivers with built-in support. */
+    if (GATOR_REGISTER_TRACE(mali_sw_counters)) {
+        printk("gator: mali_sw_counters tracepoint failed to activate\n");
+        return -1;
+    }
+#else
+#error Unknown GATOR_MALI_INTERFACE_STYLE option.
+#endif
+
+    trace_registered = 1;
+
+    mali_counter_initialize();
+    return 0;
+}
+
+static void gator_events_mali_stop(void) {
+    unsigned int cnt;
+
+    pr_debug("gator: mali stop\n");
+
+    if (trace_registered) {
+        GATOR_UNREGISTER_TRACE(mali_hw_counter);
+
+#if GATOR_MALI_INTERFACE_STYLE == 1
+    /* None. */
+#elif GATOR_MALI_INTERFACE_STYLE == 2
+    /* For patched Mali driver. */
+    GATOR_UNREGISTER_TRACE(mali_sw_counter);
+#elif GATOR_MALI_INTERFACE_STYLE == 3
+    /* For Mali drivers with built-in support. */
+    GATOR_UNREGISTER_TRACE(mali_sw_counters);
+#else
+#error Unknown GATOR_MALI_INTERFACE_STYLE option.
+#endif
+
+        pr_debug("gator: mali timeline tracepoint deactivated\n");
+        
+        trace_registered = 0;
+    }
+
+    for (cnt = FIRST_ACTIVITY_EVENT; cnt < NUMBER_OF_EVENTS; cnt++) {
+        counter_enabled[cnt] = 0;
+        counter_event[cnt] = 0;
+        counter_address[cnt] = NULL;
+    }
+
+    mali_counter_deinitialize();
+}
+
+static int gator_events_mali_read(int **buffer) {
+    int cnt, len = 0;
+
+    if (smp_processor_id()) return 0;
+
+    // Read the L2 C0 and C1 here.
+    if (counter_enabled[COUNTER_L2_C0] || counter_enabled[COUNTER_L2_C1] ) {
+        u32 src0 = 0;
+        u32 val0 = 0;
+        u32 src1 = 0;
+        u32 val1 = 0;
+
+        // Poke the driver to get the counter values
+        if (_mali_profiling_get_counters_function_pointer){
+            _mali_profiling_get_counters_function_pointer(&src0, &val0, &src1, &val1);
+        }
+
+        if (counter_enabled[COUNTER_L2_C0])
+        {
+            // Calculate and save src0's counter val0
+            counter_dump[len++] = counter_key[COUNTER_L2_C0];
+            counter_dump[len++] = get_difference(val0, counter_prev[COUNTER_L2_C0]);
+        }
+
+        if (counter_enabled[COUNTER_L2_C1])
+        {
+            // Calculate and save src1's counter val1
+            counter_dump[len++] = counter_key[COUNTER_L2_C1];
+            counter_dump[len++] = get_difference(val1, counter_prev[COUNTER_L2_C1]);
+        }
+
+        // Save the previous values for the counters.
+        counter_prev[COUNTER_L2_C0] = val0;
+        counter_prev[COUNTER_L2_C1] = val1;
+    }
+
+    // Process other (non-timeline) counters.
+    for (cnt = COUNTER_VP_C0; cnt <= LAST_SW_COUNTER; cnt++) {
+        if (counter_enabled[cnt]) {
+            counter_dump[len++] = counter_key[cnt];
+            counter_dump[len++] = counter_data[cnt];
+
+            counter_data[cnt] = 0;
+        }
+    }
+
+    if (buffer) {
+        *buffer = (int*) counter_dump;
+    }
+
+    return len;
+}
+
+static struct gator_interface gator_events_mali_interface = {
+    .create_files = gator_events_mali_create_files,
+    .start = gator_events_mali_start,
+    .stop = gator_events_mali_stop,
+    .read = gator_events_mali_read,
+};
+
+int gator_events_mali_init(void)
+{
+    unsigned int cnt;
+    u32 id = gator_mali_get_id();
+
+    switch (id) {
+    case MALI_T6xx:
+        mali_name = "Mali-T6xx";
+        break;
+    case MALI_400:
+        mali_name = "Mali-400";
+        break;
+    case MALI_300:
+        mali_name = "Mali-300";
+        break;
+    case MALI_200:
+        mali_name = "Mali-200";
+        break;
+    default:
+        printk("Unknown Mali ID (%d)\n", id);
+        return -1;
+    }
+
+    pr_debug("gator: mali init\n");
+
+    for (cnt = FIRST_ACTIVITY_EVENT; cnt < NUMBER_OF_EVENTS; cnt++) {
+        counter_enabled[cnt] = 0;
+        counter_event[cnt] = 0;
+        counter_key[cnt] = gator_events_get_key();
+        counter_address[cnt] = NULL;
+		counter_data[cnt] = 0;
+    }
+
+    trace_registered = 0;
+
+    return gator_events_install(&gator_events_mali_interface);
+}
+gator_events_init(gator_events_mali_init);
diff --git a/drivers/gator/gator_events_meminfo.c b/drivers/gator/gator_events_meminfo.c
new file mode 100644
index 00000000000..ad552ef6c6e
--- /dev/null
+++ b/drivers/gator/gator_events_meminfo.c
@@ -0,0 +1,230 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 "gator.h"
+#include <linux/workqueue.h>
+#include <trace/events/kmem.h>
+#include <linux/hardirq.h>
+
+#define MEMINFO_MEMFREE		0
+#define MEMINFO_MEMUSED		1
+#define MEMINFO_BUFFERRAM	2
+#define MEMINFO_TOTAL		3
+
+static ulong meminfo_global_enabled;
+static ulong meminfo_enabled[MEMINFO_TOTAL];
+static ulong meminfo_key[MEMINFO_TOTAL];
+static unsigned long long meminfo_buffer[MEMINFO_TOTAL * 2];
+static int meminfo_length = 0;
+static unsigned int mem_event = 0;
+static bool new_data_avail;
+
+static void wq_sched_handler(struct work_struct *wsptr);
+
+DECLARE_WORK(work, wq_sched_handler);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 3, 0)
+GATOR_DEFINE_PROBE(mm_page_free_direct, TP_PROTO(struct page *page, unsigned int order)) {
+#else
+GATOR_DEFINE_PROBE(mm_page_free, TP_PROTO(struct page *page, unsigned int order)) {
+#endif
+	mem_event++;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 3, 0)
+GATOR_DEFINE_PROBE(mm_pagevec_free, TP_PROTO(struct page *page, int cold)) {
+#else
+GATOR_DEFINE_PROBE(mm_page_free_batched, TP_PROTO(struct page *page, int cold)) {
+#endif
+	mem_event++;
+}
+
+GATOR_DEFINE_PROBE(mm_page_alloc, TP_PROTO(struct page *page, unsigned int order, gfp_t gfp_flags, int migratetype)) {
+	mem_event++;
+}
+
+static int gator_events_meminfo_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+	int i;
+
+	for (i = 0; i < MEMINFO_TOTAL; i++) {
+		switch (i) {
+		case MEMINFO_MEMFREE:
+			dir = gatorfs_mkdir(sb, root, "Linux_meminfo_memfree");
+			break;
+		case MEMINFO_MEMUSED:
+			dir = gatorfs_mkdir(sb, root, "Linux_meminfo_memused");
+			break;
+		case MEMINFO_BUFFERRAM:
+			dir = gatorfs_mkdir(sb, root, "Linux_meminfo_bufferram");
+			break;
+		default:
+			return -1;
+		}
+		if (!dir) {
+			return -1;
+		}
+		gatorfs_create_ulong(sb, dir, "enabled", &meminfo_enabled[i]);
+		gatorfs_create_ro_ulong(sb, dir, "key", &meminfo_key[i]);
+	}
+
+	return 0;
+}
+
+static int gator_events_meminfo_start(void)
+{
+	int i;
+
+	new_data_avail = true;
+	for (i = 0; i < MEMINFO_TOTAL; i++) {
+		if (meminfo_enabled[i]) {
+			meminfo_global_enabled = 1;
+		}
+	}
+
+	if (meminfo_global_enabled == 0)
+		return 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 3, 0)
+	if (GATOR_REGISTER_TRACE(mm_page_free_direct))
+#else
+	if (GATOR_REGISTER_TRACE(mm_page_free))
+#endif
+		goto mm_page_free_exit;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 3, 0)
+	if (GATOR_REGISTER_TRACE(mm_pagevec_free))
+#else
+	if (GATOR_REGISTER_TRACE(mm_page_free_batched))
+#endif
+		goto mm_page_free_batched_exit;
+	if (GATOR_REGISTER_TRACE(mm_page_alloc))
+		goto mm_page_alloc_exit;
+
+	return 0;
+
+mm_page_alloc_exit:
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 3, 0)
+	GATOR_UNREGISTER_TRACE(mm_pagevec_free);
+#else
+	GATOR_UNREGISTER_TRACE(mm_page_free_batched);
+#endif
+mm_page_free_batched_exit:
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 3, 0)
+	GATOR_UNREGISTER_TRACE(mm_page_free_direct);
+#else
+	GATOR_UNREGISTER_TRACE(mm_page_free);
+#endif
+mm_page_free_exit:
+	return -1;
+}
+
+static void gator_events_meminfo_stop(void)
+{
+	int i;
+
+	if (meminfo_global_enabled) {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 3, 0)
+		GATOR_UNREGISTER_TRACE(mm_page_free_direct);
+		GATOR_UNREGISTER_TRACE(mm_pagevec_free);
+#else
+		GATOR_UNREGISTER_TRACE(mm_page_free);
+		GATOR_UNREGISTER_TRACE(mm_page_free_batched);
+#endif
+		GATOR_UNREGISTER_TRACE(mm_page_alloc);
+	}
+
+	meminfo_global_enabled = 0;
+	for (i = 0; i < MEMINFO_TOTAL; i++) {
+		meminfo_enabled[i] = 0;
+	}
+}
+
+// Must be run in process context (work queue) as the kernel function si_meminfo() can sleep
+static void wq_sched_handler(struct work_struct *wsptr)
+{
+	struct sysinfo info;
+	int i, len;
+	unsigned long long value;
+
+	meminfo_length = len = 0;
+
+	si_meminfo(&info);
+	for (i = 0; i < MEMINFO_TOTAL; i++) {
+		if (meminfo_enabled[i]) {
+			switch (i) {
+			case MEMINFO_MEMFREE:
+				value = info.freeram * PAGE_SIZE;
+				break;
+			case MEMINFO_MEMUSED:
+				value = (info.totalram - info.freeram) * PAGE_SIZE;
+				break;
+			case MEMINFO_BUFFERRAM:
+				value = info.bufferram * PAGE_SIZE;
+				break;
+			default:
+				value = 0;
+				break;
+			}
+			meminfo_buffer[len++] = (unsigned long long)meminfo_key[i];
+			meminfo_buffer[len++] = value;
+		}
+	}
+
+	meminfo_length = len;
+	new_data_avail = true;
+}
+
+static int gator_events_meminfo_read(long long **buffer)
+{
+	static unsigned int last_mem_event = 0;
+
+	if (smp_processor_id() || !meminfo_global_enabled)
+		return 0;
+
+	if (last_mem_event != mem_event) {
+		last_mem_event = mem_event;
+		if (in_interrupt()) {
+			schedule_work(&work);
+		} else {
+			wq_sched_handler(NULL);
+		}
+	}
+
+	if (!new_data_avail)
+		return 0;
+
+	new_data_avail = false;
+
+	if (buffer)
+		*buffer = meminfo_buffer;
+
+	return meminfo_length;
+}
+
+static struct gator_interface gator_events_meminfo_interface = {
+	.create_files = gator_events_meminfo_create_files,
+	.start = gator_events_meminfo_start,
+	.stop = gator_events_meminfo_stop,
+	.read64 = gator_events_meminfo_read,
+};
+
+int gator_events_meminfo_init(void)
+{
+	int i;
+
+	meminfo_global_enabled = 0;
+	for (i = 0; i < MEMINFO_TOTAL; i++) {
+		meminfo_enabled[i] = 0;
+		meminfo_key[i] = gator_events_get_key();
+	}
+
+	return gator_events_install(&gator_events_meminfo_interface);
+}
+gator_events_init(gator_events_meminfo_init);
diff --git a/drivers/gator/gator_events_mmaped.c b/drivers/gator/gator_events_mmaped.c
new file mode 100644
index 00000000000..f81c40240d9
--- /dev/null
+++ b/drivers/gator/gator_events_mmaped.c
@@ -0,0 +1,231 @@
+/*
+ * Example events provider
+ *
+ * Copyright (C) ARM Limited 2010-2012. 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.
+ *
+ * Similar entries to those below must be present in the events.xml file.
+ * To add them to the events.xml, create an events-mmap.xml with the 
+ * following contents and rebuild gatord:
+ *
+ * <counter_set name="mmaped_cntX">
+ *   <counter name="mmaped_cnt0"/>
+ *   <counter name="mmaped_cnt1"/>
+ *   <counter name="mmaped_cnt2"/>
+ * </counter_set>
+ * <category name="mmaped" counter_set="mmaped_cntX" per_cpu="no">
+ *   <event event="0x0" title="Simulated" name="Sine" display="maximum" average_selection="yes" description="Sort-of-sine"/>
+ *   <event event="0x1" title="Simulated" name="Triangle" display="maximum" average_selection="yes" description="Triangular wave"/>
+ *   <event event="0x2" title="Simulated" name="PWM" display="maximum" average_selection="yes" description="PWM Signal"/>
+ * </category>
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/ratelimit.h>
+
+#include "gator.h"
+
+#define MMAPED_COUNTERS_NUM 3
+
+static struct {
+	unsigned long enabled;
+	unsigned long event;
+	unsigned long key;
+} mmaped_counters[MMAPED_COUNTERS_NUM];
+
+static int mmaped_buffer[MMAPED_COUNTERS_NUM * 2];
+
+#ifdef TODO
+static void __iomem *mmaped_base;
+#endif
+
+#ifndef TODO
+static s64 prev_time;
+#endif
+
+/* Adds mmaped_cntX directories and enabled, event, and key files to /dev/gator/events */
+static int gator_events_mmaped_create_files(struct super_block *sb,
+		struct dentry *root)
+{
+	int i;
+
+	for (i = 0; i < MMAPED_COUNTERS_NUM; i++) {
+		char buf[16];
+		struct dentry *dir;
+
+		snprintf(buf, sizeof(buf), "mmaped_cnt%d", i);
+		dir = gatorfs_mkdir(sb, root, buf);
+		if (WARN_ON(!dir))
+			return -1;
+		gatorfs_create_ulong(sb, dir, "enabled",
+				&mmaped_counters[i].enabled);
+		gatorfs_create_ulong(sb, dir, "event",
+				&mmaped_counters[i].event);
+		gatorfs_create_ro_ulong(sb, dir, "key",
+				&mmaped_counters[i].key);
+	}
+
+	return 0;
+}
+
+static int gator_events_mmaped_start(void)
+{
+#ifdef TODO
+	for (i = 0; i < MMAPED_COUNTERS_NUM; i++)
+		writel(mmaped_counters[i].event,
+				mmaped_base + COUNTERS_CONFIG_OFFSET[i]);
+
+	writel(ENABLED, COUNTERS_CONTROL_OFFSET);
+#endif
+
+#ifndef TODO
+	struct timespec ts;
+	getnstimeofday(&ts);
+	prev_time = timespec_to_ns(&ts);
+#endif
+
+	return 0;
+}
+
+static void gator_events_mmaped_stop(void)
+{
+#ifdef TODO
+	writel(DISABLED, COUNTERS_CONTROL_OFFSET);
+#endif
+}
+
+#ifndef TODO
+/* This function "simulates" counters, generating values of fancy
+ * functions like sine or triangle... */
+static int mmaped_simulate(int counter, int delta_in_us)
+{
+	int result = 0;
+
+	switch (counter) {
+	case 0: /* sort-of-sine */
+		{
+			static int t = 0;
+			int x;
+
+			t += delta_in_us;
+			if (t > 2048000)
+				t = 0;
+
+			if (t % 1024000 < 512000)
+				x = 512000 - (t % 512000);
+			else
+				x = t % 512000;
+
+			result = 32 * x / 512000;
+			result = result * result;
+
+			if (t < 1024000)
+				result = 1922 - result;
+		}
+		break;
+	case 1: /* triangle */
+		{
+			static int v, d = 1;
+
+			v = v + d * delta_in_us;
+			if (v < 0) {
+				v = 0;
+				d = 1;
+			} else if (v > 1000000) {
+				v = 1000000;
+				d = -1;
+			}
+
+			result = v;
+		}
+		break;
+	case 2: /* PWM signal */
+		{
+			static int t, dc, x;
+
+			t += delta_in_us;
+			if (t > 1000000)
+				t = 0;
+			if (x / 1000000 != (x + delta_in_us) / 1000000)
+				dc = (dc + 100000) % 1000000;
+			x += delta_in_us;
+			
+			result = t < dc ? 0 : 10;
+		}
+		break;
+	}
+
+	return result;
+}
+#endif
+
+static int gator_events_mmaped_read(int **buffer)
+{
+	int i;
+	int len = 0;
+#ifndef TODO
+	int delta_in_us;
+	struct timespec ts;
+	s64 time;
+#endif
+
+	/* System wide counters - read from one core only */
+	if (smp_processor_id())
+		return 0;
+
+#ifndef TODO	
+	getnstimeofday(&ts);
+	time = timespec_to_ns(&ts);
+	delta_in_us = (int)(time - prev_time) / 1000;
+	prev_time = time;
+#endif
+
+	for (i = 0; i < MMAPED_COUNTERS_NUM; i++) {
+		if (mmaped_counters[i].enabled) {
+			mmaped_buffer[len++] = mmaped_counters[i].key;
+#ifdef TODO
+			mmaped_buffer[len++] = readl(mmaped_base +
+					COUNTERS_VALUE_OFFSET[i]);
+#else
+			mmaped_buffer[len++] = mmaped_simulate(
+					mmaped_counters[i].event, delta_in_us);
+#endif
+		}
+	}
+	
+	if (buffer)
+		*buffer = mmaped_buffer;
+
+	return len;
+}
+
+static struct gator_interface gator_events_mmaped_interface = {
+	.create_files = gator_events_mmaped_create_files,
+	.start = gator_events_mmaped_start,
+	.stop = gator_events_mmaped_stop,
+	.read = gator_events_mmaped_read,
+};
+
+/* Must not be static! */
+int __init gator_events_mmaped_init(void)
+{
+	int i;
+
+#ifdef TODO
+	mmaped_base = ioremap(COUNTERS_PHYS_ADDR, SZ_4K);
+	if (!mmaped_base)
+		return -ENOMEM;	
+#endif
+
+	for (i = 0; i < MMAPED_COUNTERS_NUM; i++) {
+		mmaped_counters[i].enabled = 0;
+		mmaped_counters[i].key = gator_events_get_key();
+	}
+
+	return gator_events_install(&gator_events_mmaped_interface);
+}
+gator_events_init(gator_events_mmaped_init);
diff --git a/drivers/gator/gator_events_net.c b/drivers/gator/gator_events_net.c
new file mode 100644
index 00000000000..9298905e626
--- /dev/null
+++ b/drivers/gator/gator_events_net.c
@@ -0,0 +1,157 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 "gator.h"
+#include <linux/netdevice.h>
+#include <linux/hardirq.h>
+
+#define NETRX		0
+#define NETTX		1
+#define TOTALNET	2
+
+static ulong netrx_enabled;
+static ulong nettx_enabled;
+static ulong netrx_key;
+static ulong nettx_key;
+static int rx_total, tx_total;
+static ulong netPrev[TOTALNET];
+static int netGet[TOTALNET * 4];
+
+static void get_network_stats(struct work_struct *wsptr) {
+	int rx = 0, tx = 0;
+	struct net_device *dev;
+
+	for_each_netdev(&init_net, dev) {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36)
+		const struct net_device_stats *stats = dev_get_stats(dev);
+#else
+		struct rtnl_link_stats64 temp;
+		const struct rtnl_link_stats64 *stats = dev_get_stats(dev, &temp);
+#endif
+		rx += stats->rx_bytes;
+		tx += stats->tx_bytes;
+	}
+	rx_total = rx;
+	tx_total = tx;
+}
+DECLARE_WORK(wq_get_stats, get_network_stats);
+
+static void calculate_delta(int *rx, int *tx)
+{
+	int rx_calc, tx_calc;
+
+	rx_calc = (int)(rx_total - netPrev[NETRX]);
+	if (rx_calc < 0)
+		rx_calc = 0;
+	netPrev[NETRX] += rx_calc;
+
+	tx_calc = (int)(tx_total - netPrev[NETTX]);
+	if (tx_calc < 0)
+		tx_calc = 0;
+	netPrev[NETTX] += tx_calc;
+
+	*rx = rx_calc;
+	*tx = tx_calc;
+}
+
+static int gator_events_net_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+
+	dir = gatorfs_mkdir(sb, root, "Linux_net_rx");
+	if (!dir) {
+		return -1;
+	}
+	gatorfs_create_ulong(sb, dir, "enabled", &netrx_enabled);
+	gatorfs_create_ro_ulong(sb, dir, "key", &netrx_key);
+
+	dir = gatorfs_mkdir(sb, root, "Linux_net_tx");
+	if (!dir) {
+		return -1;
+	}
+	gatorfs_create_ulong(sb, dir, "enabled", &nettx_enabled);
+	gatorfs_create_ro_ulong(sb, dir, "key", &nettx_key);
+
+	return 0;
+}
+
+static int gator_events_net_start(void)
+{
+	get_network_stats(NULL);
+	netPrev[NETRX] = rx_total;
+	netPrev[NETTX] = tx_total;
+	return 0;
+}
+
+static void gator_events_net_stop(void)
+{
+	netrx_enabled = 0;
+	nettx_enabled = 0;
+}
+
+static int gator_events_net_read(int **buffer)
+{
+	int len, rx_delta, tx_delta;
+	static int last_rx_delta = 0, last_tx_delta = 0;
+
+	if (smp_processor_id() != 0)
+		return 0;
+
+	if (!netrx_enabled && !nettx_enabled)
+		return 0;
+
+	if (in_interrupt()){
+		schedule_work(&wq_get_stats);
+	} else {
+		get_network_stats(NULL);
+	}
+
+	calculate_delta(&rx_delta, &tx_delta);
+
+	len = 0;
+	if (netrx_enabled && last_rx_delta != rx_delta) {
+		last_rx_delta = rx_delta;
+		netGet[len++] = netrx_key;
+		netGet[len++] = 0; // indicates to Streamline that rx_delta bytes were transmitted now, not since the last message
+		netGet[len++] = netrx_key;
+		netGet[len++] = rx_delta;
+	}
+
+	if (nettx_enabled && last_tx_delta != tx_delta) {
+		last_tx_delta = tx_delta;
+		netGet[len++] = nettx_key;
+		netGet[len++] = 0; // indicates to Streamline that tx_delta bytes were transmitted now, not since the last message
+		netGet[len++] = nettx_key;
+		netGet[len++] = tx_delta;
+	}
+
+	if (buffer)
+		*buffer = netGet;
+
+	return len;
+}
+
+static struct gator_interface gator_events_net_interface = {
+	.create_files = gator_events_net_create_files,
+	.start = gator_events_net_start,
+	.stop = gator_events_net_stop,
+	.read = gator_events_net_read,
+};
+
+int gator_events_net_init(void)
+{
+	netrx_key = gator_events_get_key();
+	nettx_key = gator_events_get_key();
+
+	netrx_enabled = 0;
+	nettx_enabled = 0;
+
+	return gator_events_install(&gator_events_net_interface);
+}
+gator_events_init(gator_events_net_init);
diff --git a/drivers/gator/gator_events_perf_pmu.c b/drivers/gator/gator_events_perf_pmu.c
new file mode 100755
index 00000000000..da76a9cb409
--- /dev/null
+++ b/drivers/gator/gator_events_perf_pmu.c
@@ -0,0 +1,306 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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/slab.h>
+#include <linux/perf_event.h>
+#include "gator.h"
+
+// gator_events_armvX.c is used for Linux 2.6.x
+#if GATOR_PERF_PMU_SUPPORT
+
+static const char *pmnc_name;
+int pmnc_counters;
+int ccnt = 0;
+
+#define CNTMAX (6+1)
+
+static DEFINE_MUTEX(perf_mutex);
+
+unsigned long pmnc_enabled[CNTMAX];
+unsigned long pmnc_event[CNTMAX];
+unsigned long pmnc_count[CNTMAX];
+unsigned long pmnc_key[CNTMAX];
+
+static DEFINE_PER_CPU(int[CNTMAX], perfCurr);
+static DEFINE_PER_CPU(int[CNTMAX], perfPrev);
+static DEFINE_PER_CPU(int[CNTMAX], perfPrevDelta);
+static DEFINE_PER_CPU(int[CNTMAX * 2], perfCnt);
+static DEFINE_PER_CPU(struct perf_event *[CNTMAX], pevent);
+static DEFINE_PER_CPU(struct perf_event_attr *[CNTMAX], pevent_attr);
+
+static void gator_events_perf_pmu_stop(void);
+
+static int gator_events_perf_pmu_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+	int i;
+
+	for (i = 0; i < pmnc_counters; i++) {
+		char buf[40];
+		if (i == 0) {
+			snprintf(buf, sizeof buf, "%s_ccnt", pmnc_name);
+		} else {
+			snprintf(buf, sizeof buf, "%s_cnt%d", pmnc_name, i-1);
+		}
+		dir = gatorfs_mkdir(sb, root, buf);
+		if (!dir) {
+			return -1;
+		}
+		gatorfs_create_ulong(sb, dir, "enabled", &pmnc_enabled[i]);
+		gatorfs_create_ulong(sb, dir, "count", &pmnc_count[i]);
+		gatorfs_create_ro_ulong(sb, dir, "key", &pmnc_key[i]);
+		if (i > 0) {
+			gatorfs_create_ulong(sb, dir, "event", &pmnc_event[i]);
+		}
+	}
+
+	return 0;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 1, 0)
+static void dummy_handler(struct perf_event *event, int unused, struct perf_sample_data *data, struct pt_regs *regs)
+#else
+static void dummy_handler(struct perf_event *event, struct perf_sample_data *data, struct pt_regs *regs)
+#endif
+{
+// Required as perf_event_create_kernel_counter() requires an overflow handler, even though all we do is poll
+}
+
+static int gator_events_perf_pmu_online(int** buffer)
+{
+	int cnt, len = 0, cpu = smp_processor_id();
+
+	// read the counters and toss the invalid data, return zero instead
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		struct perf_event * ev = per_cpu(pevent, cpu)[cnt];
+		if (ev != NULL && ev->state == PERF_EVENT_STATE_ACTIVE) {
+			ev->pmu->read(ev);
+			per_cpu(perfPrev, cpu)[cnt] = per_cpu(perfCurr, cpu)[cnt] = local64_read(&ev->count);
+			per_cpu(perfPrevDelta, cpu)[cnt] = 0;
+			per_cpu(perfCnt, cpu)[len++] = pmnc_key[cnt];
+			per_cpu(perfCnt, cpu)[len++] = 0;
+		}
+	}
+
+	if (buffer)
+		*buffer = per_cpu(perfCnt, cpu);
+
+	return len;
+}
+
+static void gator_events_perf_pmu_online_dispatch(int cpu)
+{
+	int cnt;
+
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		if (per_cpu(pevent, cpu)[cnt] != NULL || per_cpu(pevent_attr, cpu)[cnt] == 0)
+			continue;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 1, 0)
+		per_cpu(pevent, cpu)[cnt] = perf_event_create_kernel_counter(per_cpu(pevent_attr, cpu)[cnt], cpu, 0, dummy_handler);
+#else
+		per_cpu(pevent, cpu)[cnt] = perf_event_create_kernel_counter(per_cpu(pevent_attr, cpu)[cnt], cpu, 0, dummy_handler, 0);
+#endif
+		if (IS_ERR(per_cpu(pevent, cpu)[cnt])) {
+			pr_debug("gator: unable to online a counter on cpu %d\n", cpu);
+			per_cpu(pevent, cpu)[cnt] = NULL;
+			continue;
+		}
+
+		if (per_cpu(pevent, cpu)[cnt]->state != PERF_EVENT_STATE_ACTIVE) {
+			pr_debug("gator: inactive counter on cpu %d\n", cpu);
+			perf_event_release_kernel(per_cpu(pevent, cpu)[cnt]);
+			per_cpu(pevent, cpu)[cnt] = NULL;
+			continue;
+		}
+	}
+}
+
+static void gator_events_perf_pmu_offline_dispatch(int cpu)
+{
+	int cnt;
+	struct perf_event * pe;
+
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		pe = NULL;
+		mutex_lock(&perf_mutex);
+		if (per_cpu(pevent, cpu)[cnt]) {
+			pe = per_cpu(pevent, cpu)[cnt];
+			per_cpu(pevent, cpu)[cnt] = NULL;
+		}
+		mutex_unlock(&perf_mutex);
+
+		if (pe) {
+			perf_event_release_kernel(pe);
+		}
+	}
+}
+
+static int gator_events_perf_pmu_start(void)
+{
+	int cnt, cpu;
+	u32 size = sizeof(struct perf_event_attr);
+
+	for_each_present_cpu(cpu) {
+		for (cnt = 0; cnt < pmnc_counters; cnt++) {
+			per_cpu(pevent, cpu)[cnt] = NULL;
+			if (!pmnc_enabled[cnt]) // Skip disabled counters
+				continue;
+
+			per_cpu(perfPrev, cpu)[cnt] = 0;
+			per_cpu(perfCurr, cpu)[cnt] = 0;
+			per_cpu(perfPrevDelta, cpu)[cnt] = 0;
+			per_cpu(pevent_attr, cpu)[cnt] = kmalloc(size, GFP_KERNEL);
+			if (!per_cpu(pevent_attr, cpu)[cnt]) {
+				gator_events_perf_pmu_stop();
+				return -1;
+			}
+
+			memset(per_cpu(pevent_attr, cpu)[cnt], 0, size);
+			per_cpu(pevent_attr, cpu)[cnt]->type = PERF_TYPE_RAW;
+			per_cpu(pevent_attr, cpu)[cnt]->size = size;
+			per_cpu(pevent_attr, cpu)[cnt]->config = pmnc_event[cnt];
+			per_cpu(pevent_attr, cpu)[cnt]->sample_period = 0;
+			per_cpu(pevent_attr, cpu)[cnt]->pinned = 1;
+
+			// handle special case for ccnt
+			if (cnt == ccnt) {
+				per_cpu(pevent_attr, cpu)[cnt]->type = PERF_TYPE_HARDWARE;
+				per_cpu(pevent_attr, cpu)[cnt]->config = PERF_COUNT_HW_CPU_CYCLES;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static void gator_events_perf_pmu_stop(void)
+{
+	unsigned int cnt, cpu;
+
+	for_each_present_cpu(cpu) {
+		for (cnt = 0; cnt < pmnc_counters; cnt++) {
+			if (per_cpu(pevent_attr, cpu)[cnt]) {
+				kfree(per_cpu(pevent_attr, cpu)[cnt]);
+				per_cpu(pevent_attr, cpu)[cnt] = NULL;
+			}
+		}
+	}
+
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		pmnc_enabled[cnt] = 0;
+		pmnc_event[cnt] = 0;
+		pmnc_count[cnt] = 0;
+	}
+}
+
+static int gator_events_perf_pmu_read(int **buffer)
+{
+	int cnt, delta, len = 0;
+	int cpu = smp_processor_id();
+
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		struct perf_event * ev = per_cpu(pevent, cpu)[cnt];
+		if (ev != NULL && ev->state == PERF_EVENT_STATE_ACTIVE) {
+			ev->pmu->read(ev);
+			per_cpu(perfCurr, cpu)[cnt] = local64_read(&ev->count);
+			delta = per_cpu(perfCurr, cpu)[cnt] - per_cpu(perfPrev, cpu)[cnt];
+			if (delta != per_cpu(perfPrevDelta, cpu)[cnt]) {
+				per_cpu(perfPrevDelta, cpu)[cnt] = delta;
+				per_cpu(perfPrev, cpu)[cnt] = per_cpu(perfCurr, cpu)[cnt];
+				per_cpu(perfCnt, cpu)[len++] = pmnc_key[cnt];
+				if (delta < 0)
+					delta *= -1;
+				per_cpu(perfCnt, cpu)[len++] = delta;
+			}
+		}
+	}
+
+	if (buffer)
+		*buffer = per_cpu(perfCnt, cpu);
+
+	return len;
+}
+
+static struct gator_interface gator_events_perf_pmu_interface = {
+	.create_files = gator_events_perf_pmu_create_files,
+	.start = gator_events_perf_pmu_start,
+	.stop = gator_events_perf_pmu_stop,
+	.online = gator_events_perf_pmu_online,
+	.online_dispatch = gator_events_perf_pmu_online_dispatch,
+	.offline_dispatch = gator_events_perf_pmu_offline_dispatch,
+	.read = gator_events_perf_pmu_read,
+};
+
+int gator_events_perf_pmu_init(void)
+{
+	unsigned int cnt;
+
+	switch (gator_cpuid()) {
+	case ARM1136:
+	case ARM1156:
+	case ARM1176:
+		pmnc_name = "ARM_ARM11";
+		pmnc_counters = 3;
+		ccnt = 2;
+		break;
+	case ARM11MPCORE:
+		pmnc_name = "ARM_ARM11MPCore";
+		pmnc_counters = 3;
+		break;
+	case CORTEX_A5:
+		pmnc_name = "ARM_Cortex-A5";
+		pmnc_counters = 2;
+		break;
+	case CORTEX_A7:
+		pmnc_name = "ARM_Cortex-A7";
+		pmnc_counters = 4;
+		break;
+	case CORTEX_A8:
+		pmnc_name = "ARM_Cortex-A8";
+		pmnc_counters = 4;
+		break;
+	case CORTEX_A9:
+		pmnc_name = "ARM_Cortex-A9";
+		pmnc_counters = 6;
+		break;
+	case CORTEX_A15:
+		pmnc_name = "ARM_Cortex-A15";
+		pmnc_counters = 6;
+		break;
+	case SCORPION:
+		pmnc_name = "Scorpion";
+		pmnc_counters = 4;
+		break;
+	case SCORPIONMP:
+		pmnc_name = "ScorpionMP";
+		pmnc_counters = 4;
+		break;
+	case KRAITSIM:
+	case KRAIT:
+		pmnc_name = "Krait";
+		pmnc_counters = 4;
+		break;
+	default:
+		return -1;
+	}
+
+	pmnc_counters++; // CNT[n] + CCNT
+
+	for (cnt = 0; cnt < CNTMAX; cnt++) {
+		pmnc_enabled[cnt] = 0;
+		pmnc_event[cnt] = 0;
+		pmnc_count[cnt] = 0;
+		pmnc_key[cnt] = gator_events_get_key();
+	}
+
+	return gator_events_install(&gator_events_perf_pmu_interface);
+}
+
+gator_events_init(gator_events_perf_pmu_init);
+#endif
diff --git a/drivers/gator/gator_events_sched.c b/drivers/gator/gator_events_sched.c
new file mode 100644
index 00000000000..9bed3641dbb
--- /dev/null
+++ b/drivers/gator/gator_events_sched.c
@@ -0,0 +1,114 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 "gator.h"
+#include <trace/events/sched.h>
+
+#define SCHED_SWITCH	0
+#define SCHED_TOTAL		(SCHED_SWITCH+1)
+
+static ulong sched_switch_enabled;
+static ulong sched_switch_key;
+static DEFINE_PER_CPU(int[SCHED_TOTAL], schedCnt);
+static DEFINE_PER_CPU(int[SCHED_TOTAL * 2], schedGet);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 35)
+GATOR_DEFINE_PROBE(sched_switch, TP_PROTO(struct rq *rq, struct task_struct *prev, struct task_struct *next))
+#else
+GATOR_DEFINE_PROBE(sched_switch, TP_PROTO(struct task_struct *prev, struct task_struct *next))
+#endif
+{
+	unsigned long flags;
+
+	// disable interrupts to synchronize with gator_events_sched_read()
+	// spinlocks not needed since percpu buffers are used
+	local_irq_save(flags);
+	per_cpu(schedCnt, smp_processor_id())[SCHED_SWITCH]++;
+	local_irq_restore(flags);
+}
+
+static int gator_events_sched_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+
+	/* switch */
+	dir = gatorfs_mkdir(sb, root, "Linux_sched_switch");
+	if (!dir) {
+		return -1;
+	}
+	gatorfs_create_ulong(sb, dir, "enabled", &sched_switch_enabled);
+	gatorfs_create_ro_ulong(sb, dir, "key", &sched_switch_key);
+
+	return 0;
+}
+
+static int gator_events_sched_start(void)
+{
+	// register tracepoints
+	if (sched_switch_enabled)
+		if (GATOR_REGISTER_TRACE(sched_switch))
+			goto sched_switch_exit;
+	pr_debug("gator: registered scheduler event tracepoints\n");
+
+	return 0;
+
+	// unregister tracepoints on error
+sched_switch_exit:
+	pr_err("gator: scheduler event tracepoints failed to activate, please verify that tracepoints are enabled in the linux kernel\n");
+
+	return -1;
+}
+
+static void gator_events_sched_stop(void)
+{
+	if (sched_switch_enabled)
+		GATOR_UNREGISTER_TRACE(sched_switch);
+	pr_debug("gator: unregistered scheduler event tracepoints\n");
+
+	sched_switch_enabled = 0;
+}
+
+static int gator_events_sched_read(int **buffer)
+{
+	unsigned long flags;
+	int len, value;
+	int cpu = smp_processor_id();
+
+	len = 0;
+	if (sched_switch_enabled) {
+		local_irq_save(flags);
+		value = per_cpu(schedCnt, cpu)[SCHED_SWITCH];
+		per_cpu(schedCnt, cpu)[SCHED_SWITCH] = 0;
+		local_irq_restore(flags);
+		per_cpu(schedGet, cpu)[len++] = sched_switch_key;
+		per_cpu(schedGet, cpu)[len++] = value;
+	}
+
+	if (buffer)
+		*buffer = per_cpu(schedGet, cpu);
+
+	return len;
+}
+
+static struct gator_interface gator_events_sched_interface = {
+	.create_files = gator_events_sched_create_files,
+	.start = gator_events_sched_start,
+	.stop = gator_events_sched_stop,
+	.read = gator_events_sched_read,
+};
+
+int gator_events_sched_init(void)
+{
+	sched_switch_enabled = 0;
+
+	sched_switch_key = gator_events_get_key();
+
+	return gator_events_install(&gator_events_sched_interface);
+}
+gator_events_init(gator_events_sched_init);
diff --git a/drivers/gator/gator_events_scorpion.c b/drivers/gator/gator_events_scorpion.c
new file mode 100644
index 00000000000..ed0d8de6b9f
--- /dev/null
+++ b/drivers/gator/gator_events_scorpion.c
@@ -0,0 +1,678 @@
+/**
+ * Copyright (C) ARM Limited 2011-2012. 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 "gator.h"
+
+// gator_events_perf_pmu.c is used if perf is supported
+#if GATOR_NO_PERF_SUPPORT
+
+static const char *pmnc_name;
+static int pmnc_counters;
+
+// Per-CPU PMNC: config reg
+#define PMNC_E		(1 << 0)	/* Enable all counters */
+#define PMNC_P		(1 << 1)	/* Reset all counters */
+#define PMNC_C		(1 << 2)	/* Cycle counter reset */
+#define PMNC_D		(1 << 3)	/* CCNT counts every 64th cpu cycle */
+#define PMNC_X		(1 << 4)	/* Export to ETM */
+#define PMNC_DP		(1 << 5)	/* Disable CCNT if non-invasive debug*/
+#define	PMNC_MASK	0x3f		/* Mask for writable bits */
+
+// ccnt reg
+#define CCNT_REG	(1 << 31)
+
+#define CCNT 		0
+#define CNT0		1
+#define CNTMAX 		(4+1)
+
+static unsigned long pmnc_enabled[CNTMAX];
+static unsigned long pmnc_event[CNTMAX];
+static unsigned long pmnc_key[CNTMAX];
+
+static DEFINE_PER_CPU(int[CNTMAX * 2], perfCnt);
+
+enum scorpion_perf_types {
+	SCORPION_ICACHE_EXPL_INV                = 0x4c,
+	SCORPION_ICACHE_MISS                    = 0x4d,
+	SCORPION_ICACHE_ACCESS                  = 0x4e,
+	SCORPION_ICACHE_CACHEREQ_L2             = 0x4f,
+	SCORPION_ICACHE_NOCACHE_L2              = 0x50,
+	SCORPION_HIQUP_NOPED                    = 0x51,
+	SCORPION_DATA_ABORT                     = 0x52,
+	SCORPION_IRQ                            = 0x53,
+	SCORPION_FIQ                            = 0x54,
+	SCORPION_ALL_EXCPT                      = 0x55,
+	SCORPION_UNDEF                          = 0x56,
+	SCORPION_SVC                            = 0x57,
+	SCORPION_SMC                            = 0x58,
+	SCORPION_PREFETCH_ABORT                 = 0x59,
+	SCORPION_INDEX_CHECK                    = 0x5a,
+	SCORPION_NULL_CHECK                     = 0x5b,
+	SCORPION_EXPL_ICIALLU                   = 0x5c,
+	SCORPION_IMPL_ICIALLU                   = 0x5d,
+	SCORPION_NONICIALLU_BTAC_INV            = 0x5e,
+	SCORPION_ICIMVAU_IMPL_ICIALLU           = 0x5f,
+	SCORPION_SPIPE_ONLY_CYCLES              = 0x60,
+	SCORPION_XPIPE_ONLY_CYCLES              = 0x61,
+	SCORPION_DUAL_CYCLES                    = 0x62,
+	SCORPION_DISPATCH_ANY_CYCLES            = 0x63,
+	SCORPION_FIFO_FULLBLK_CMT               = 0x64,
+	SCORPION_FAIL_COND_INST                 = 0x65,
+	SCORPION_PASS_COND_INST                 = 0x66,
+	SCORPION_ALLOW_VU_CLK                   = 0x67,
+	SCORPION_VU_IDLE                        = 0x68,
+	SCORPION_ALLOW_L2_CLK                   = 0x69,
+	SCORPION_L2_IDLE                        = 0x6a,
+	SCORPION_DTLB_IMPL_INV_SCTLR_DACR       = 0x6b,
+	SCORPION_DTLB_EXPL_INV                  = 0x6c,
+	SCORPION_DTLB_MISS                      = 0x6d,
+	SCORPION_DTLB_ACCESS                    = 0x6e,
+	SCORPION_ITLB_MISS                      = 0x6f,
+	SCORPION_ITLB_IMPL_INV                  = 0x70,
+	SCORPION_ITLB_EXPL_INV                  = 0x71,
+	SCORPION_UTLB_D_MISS                    = 0x72,
+	SCORPION_UTLB_D_ACCESS                  = 0x73,
+	SCORPION_UTLB_I_MISS                    = 0x74,
+	SCORPION_UTLB_I_ACCESS                  = 0x75,
+	SCORPION_UTLB_INV_ASID                  = 0x76,
+	SCORPION_UTLB_INV_MVA                   = 0x77,
+	SCORPION_UTLB_INV_ALL                   = 0x78,
+	SCORPION_S2_HOLD_RDQ_UNAVAIL            = 0x79,
+	SCORPION_S2_HOLD                        = 0x7a,
+	SCORPION_S2_HOLD_DEV_OP                 = 0x7b,
+	SCORPION_S2_HOLD_ORDER                  = 0x7c,
+	SCORPION_S2_HOLD_BARRIER                = 0x7d,
+	SCORPION_VIU_DUAL_CYCLE                 = 0x7e,
+	SCORPION_VIU_SINGLE_CYCLE               = 0x7f,
+	SCORPION_VX_PIPE_WAR_STALL_CYCLES       = 0x80,
+	SCORPION_VX_PIPE_WAW_STALL_CYCLES       = 0x81,
+	SCORPION_VX_PIPE_RAW_STALL_CYCLES       = 0x82,
+	SCORPION_VX_PIPE_LOAD_USE_STALL         = 0x83,
+	SCORPION_VS_PIPE_WAR_STALL_CYCLES       = 0x84,
+	SCORPION_VS_PIPE_WAW_STALL_CYCLES       = 0x85,
+	SCORPION_VS_PIPE_RAW_STALL_CYCLES       = 0x86,
+	SCORPION_EXCEPTIONS_INV_OPERATION       = 0x87,
+	SCORPION_EXCEPTIONS_DIV_BY_ZERO         = 0x88,
+	SCORPION_COND_INST_FAIL_VX_PIPE         = 0x89,
+	SCORPION_COND_INST_FAIL_VS_PIPE         = 0x8a,
+	SCORPION_EXCEPTIONS_OVERFLOW            = 0x8b,
+	SCORPION_EXCEPTIONS_UNDERFLOW           = 0x8c,
+	SCORPION_EXCEPTIONS_DENORM              = 0x8d,
+#ifdef CONFIG_ARCH_MSM_SCORPIONMP
+	SCORPIONMP_NUM_BARRIERS                 = 0x8e,
+	SCORPIONMP_BARRIER_CYCLES               = 0x8f,
+#else
+	SCORPION_BANK_AB_HIT                    = 0x8e,
+	SCORPION_BANK_AB_ACCESS                 = 0x8f,
+	SCORPION_BANK_CD_HIT                    = 0x90,
+	SCORPION_BANK_CD_ACCESS                 = 0x91,
+	SCORPION_BANK_AB_DSIDE_HIT              = 0x92,
+	SCORPION_BANK_AB_DSIDE_ACCESS           = 0x93,
+	SCORPION_BANK_CD_DSIDE_HIT              = 0x94,
+	SCORPION_BANK_CD_DSIDE_ACCESS           = 0x95,
+	SCORPION_BANK_AB_ISIDE_HIT              = 0x96,
+	SCORPION_BANK_AB_ISIDE_ACCESS           = 0x97,
+	SCORPION_BANK_CD_ISIDE_HIT              = 0x98,
+	SCORPION_BANK_CD_ISIDE_ACCESS           = 0x99,
+	SCORPION_ISIDE_RD_WAIT                  = 0x9a,
+	SCORPION_DSIDE_RD_WAIT                  = 0x9b,
+	SCORPION_BANK_BYPASS_WRITE              = 0x9c,
+	SCORPION_BANK_AB_NON_CASTOUT            = 0x9d,
+	SCORPION_BANK_AB_L2_CASTOUT             = 0x9e,
+	SCORPION_BANK_CD_NON_CASTOUT            = 0x9f,
+	SCORPION_BANK_CD_L2_CASTOUT             = 0xa0,
+#endif
+	MSM_MAX_EVT
+};
+
+struct scorp_evt {
+	u32 evt_type;
+	u32 val;
+	u8 grp;
+	u32 evt_type_act;
+};
+
+static const struct scorp_evt sc_evt[] = {
+	{SCORPION_ICACHE_EXPL_INV, 0x80000500, 0, 0x4d},
+	{SCORPION_ICACHE_MISS, 0x80050000, 0, 0x4e},
+	{SCORPION_ICACHE_ACCESS, 0x85000000, 0, 0x4f},
+	{SCORPION_ICACHE_CACHEREQ_L2, 0x86000000, 0, 0x4f},
+	{SCORPION_ICACHE_NOCACHE_L2, 0x87000000, 0, 0x4f},
+	{SCORPION_HIQUP_NOPED, 0x80080000, 0, 0x4e},
+	{SCORPION_DATA_ABORT, 0x8000000a, 0, 0x4c},
+	{SCORPION_IRQ, 0x80000a00, 0, 0x4d},
+	{SCORPION_FIQ, 0x800a0000, 0, 0x4e},
+	{SCORPION_ALL_EXCPT, 0x8a000000, 0, 0x4f},
+	{SCORPION_UNDEF, 0x8000000b, 0, 0x4c},
+	{SCORPION_SVC, 0x80000b00, 0, 0x4d},
+	{SCORPION_SMC, 0x800b0000, 0, 0x4e},
+	{SCORPION_PREFETCH_ABORT, 0x8b000000, 0, 0x4f},
+	{SCORPION_INDEX_CHECK, 0x8000000c, 0, 0x4c},
+	{SCORPION_NULL_CHECK, 0x80000c00, 0, 0x4d},
+	{SCORPION_EXPL_ICIALLU, 0x8000000d, 0, 0x4c},
+	{SCORPION_IMPL_ICIALLU, 0x80000d00, 0, 0x4d},
+	{SCORPION_NONICIALLU_BTAC_INV, 0x800d0000, 0, 0x4e},
+	{SCORPION_ICIMVAU_IMPL_ICIALLU, 0x8d000000, 0, 0x4f},
+
+	{SCORPION_SPIPE_ONLY_CYCLES, 0x80000600, 1, 0x51},
+	{SCORPION_XPIPE_ONLY_CYCLES, 0x80060000, 1, 0x52},
+	{SCORPION_DUAL_CYCLES, 0x86000000, 1, 0x53},
+	{SCORPION_DISPATCH_ANY_CYCLES, 0x89000000, 1, 0x53},
+	{SCORPION_FIFO_FULLBLK_CMT, 0x8000000d, 1, 0x50},
+	{SCORPION_FAIL_COND_INST, 0x800d0000, 1, 0x52},
+	{SCORPION_PASS_COND_INST, 0x8d000000, 1, 0x53},
+	{SCORPION_ALLOW_VU_CLK, 0x8000000e, 1, 0x50},
+	{SCORPION_VU_IDLE, 0x80000e00, 1, 0x51},
+	{SCORPION_ALLOW_L2_CLK, 0x800e0000, 1, 0x52},
+	{SCORPION_L2_IDLE, 0x8e000000, 1, 0x53},
+
+	{SCORPION_DTLB_IMPL_INV_SCTLR_DACR, 0x80000001, 2, 0x54},
+	{SCORPION_DTLB_EXPL_INV, 0x80000100, 2, 0x55},
+	{SCORPION_DTLB_MISS, 0x80010000, 2, 0x56},
+	{SCORPION_DTLB_ACCESS, 0x81000000, 2, 0x57},
+	{SCORPION_ITLB_MISS, 0x80000200, 2, 0x55},
+	{SCORPION_ITLB_IMPL_INV, 0x80020000, 2, 0x56},
+	{SCORPION_ITLB_EXPL_INV, 0x82000000, 2, 0x57},
+	{SCORPION_UTLB_D_MISS, 0x80000003, 2, 0x54},
+	{SCORPION_UTLB_D_ACCESS, 0x80000300, 2, 0x55},
+	{SCORPION_UTLB_I_MISS, 0x80030000, 2, 0x56},
+	{SCORPION_UTLB_I_ACCESS, 0x83000000, 2, 0x57},
+	{SCORPION_UTLB_INV_ASID, 0x80000400, 2, 0x55},
+	{SCORPION_UTLB_INV_MVA, 0x80040000, 2, 0x56},
+	{SCORPION_UTLB_INV_ALL, 0x84000000, 2, 0x57},
+	{SCORPION_S2_HOLD_RDQ_UNAVAIL, 0x80000800, 2, 0x55},
+	{SCORPION_S2_HOLD, 0x88000000, 2, 0x57},
+	{SCORPION_S2_HOLD_DEV_OP, 0x80000900, 2, 0x55},
+	{SCORPION_S2_HOLD_ORDER, 0x80090000, 2, 0x56},
+	{SCORPION_S2_HOLD_BARRIER, 0x89000000, 2, 0x57},
+
+	{SCORPION_VIU_DUAL_CYCLE, 0x80000001, 4, 0x5c},
+	{SCORPION_VIU_SINGLE_CYCLE, 0x80000100, 4, 0x5d},
+	{SCORPION_VX_PIPE_WAR_STALL_CYCLES, 0x80000005, 4, 0x5c},
+	{SCORPION_VX_PIPE_WAW_STALL_CYCLES, 0x80000500, 4, 0x5d},
+	{SCORPION_VX_PIPE_RAW_STALL_CYCLES, 0x80050000, 4, 0x5e},
+	{SCORPION_VX_PIPE_LOAD_USE_STALL, 0x80000007, 4, 0x5c},
+	{SCORPION_VS_PIPE_WAR_STALL_CYCLES, 0x80000008, 4, 0x5c},
+	{SCORPION_VS_PIPE_WAW_STALL_CYCLES, 0x80000800, 4, 0x5d},
+	{SCORPION_VS_PIPE_RAW_STALL_CYCLES, 0x80080000, 4, 0x5e},
+	{SCORPION_EXCEPTIONS_INV_OPERATION, 0x8000000b, 4, 0x5c},
+	{SCORPION_EXCEPTIONS_DIV_BY_ZERO, 0x80000b00, 4, 0x5d},
+	{SCORPION_COND_INST_FAIL_VX_PIPE, 0x800b0000, 4, 0x5e},
+	{SCORPION_COND_INST_FAIL_VS_PIPE, 0x8b000000, 4, 0x5f},
+	{SCORPION_EXCEPTIONS_OVERFLOW, 0x8000000c, 4, 0x5c},
+	{SCORPION_EXCEPTIONS_UNDERFLOW, 0x80000c00, 4, 0x5d},
+	{SCORPION_EXCEPTIONS_DENORM, 0x8c000000, 4, 0x5f},
+
+#ifdef CONFIG_ARCH_MSM_SCORPIONMP
+    {SCORPIONMP_NUM_BARRIERS, 0x80000e00, 3, 0x59},
+    {SCORPIONMP_BARRIER_CYCLES, 0x800e0000, 3, 0x5a},
+#else
+	{SCORPION_BANK_AB_HIT, 0x80000001, 3, 0x58},
+	{SCORPION_BANK_AB_ACCESS, 0x80000100, 3, 0x59},
+	{SCORPION_BANK_CD_HIT, 0x80010000, 3, 0x5a},
+	{SCORPION_BANK_CD_ACCESS, 0x81000000, 3, 0x5b},
+	{SCORPION_BANK_AB_DSIDE_HIT, 0x80000002, 3, 0x58},
+	{SCORPION_BANK_AB_DSIDE_ACCESS, 0x80000200, 3, 0x59},
+	{SCORPION_BANK_CD_DSIDE_HIT, 0x80020000, 3, 0x5a},
+	{SCORPION_BANK_CD_DSIDE_ACCESS, 0x82000000, 3, 0x5b},
+	{SCORPION_BANK_AB_ISIDE_HIT, 0x80000003, 3, 0x58},
+	{SCORPION_BANK_AB_ISIDE_ACCESS, 0x80000300, 3, 0x59},
+	{SCORPION_BANK_CD_ISIDE_HIT, 0x80030000, 3, 0x5a},
+	{SCORPION_BANK_CD_ISIDE_ACCESS, 0x83000000, 3, 0x5b},
+	{SCORPION_ISIDE_RD_WAIT, 0x80000009, 3, 0x58},
+	{SCORPION_DSIDE_RD_WAIT, 0x80090000, 3, 0x5a},
+	{SCORPION_BANK_BYPASS_WRITE, 0x8000000a, 3, 0x58},
+	{SCORPION_BANK_AB_NON_CASTOUT, 0x8000000c, 3, 0x58},
+	{SCORPION_BANK_AB_L2_CASTOUT, 0x80000c00, 3, 0x59},
+	{SCORPION_BANK_CD_NON_CASTOUT, 0x800c0000, 3, 0x5a},
+	{SCORPION_BANK_CD_L2_CASTOUT, 0x8c000000, 3, 0x5b},
+#endif
+};
+
+static inline void scorpion_pmnc_write(u32 val)
+{
+	val &= PMNC_MASK;
+	asm volatile("mcr p15, 0, %0, c9, c12, 0" : : "r" (val));
+}
+
+static inline u32 scorpion_pmnc_read(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 0, %0, c9, c12, 0" : "=r" (val));
+	return val;
+}
+
+static inline u32 scorpion_ccnt_read(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 0, %0, c9, c13, 0" : "=r" (val));
+	return val;
+}
+
+static inline u32 scorpion_cntn_read(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 0, %0, c9, c13, 2" : "=r" (val));
+	return val;
+}
+
+static inline u32 scorpion_pmnc_enable_counter(unsigned int cnt)
+{
+	u32 val;
+
+	if (cnt >= CNTMAX) {
+		pr_err("gator: CPU%u enabling wrong PMNC counter %d\n", smp_processor_id(), cnt);
+		return -1;
+	}
+
+	if (cnt == CCNT)
+		val = CCNT_REG;
+	else
+		val = (1 << (cnt - CNT0));
+
+	asm volatile("mcr p15, 0, %0, c9, c12, 1" : : "r" (val));
+
+	return cnt;
+}
+
+static inline u32 scorpion_pmnc_disable_counter(unsigned int cnt)
+{
+	u32 val;
+
+	if (cnt >= CNTMAX) {
+		pr_err("gator: CPU%u disabling wrong PMNC counter %d\n", smp_processor_id(), cnt);
+		return -1;
+	}
+
+	if (cnt == CCNT)
+		val = CCNT_REG;
+	else
+		val = (1 << (cnt - CNT0));
+
+	asm volatile("mcr p15, 0, %0, c9, c12, 2" : : "r" (val));
+
+	return cnt;
+}
+
+static inline int scorpion_pmnc_select_counter(unsigned int cnt)
+{
+	u32 val;
+
+	if ((cnt == CCNT) || (cnt >= CNTMAX)) {
+		pr_err("gator: CPU%u selecting wrong PMNC counter %d\n", smp_processor_id(), cnt);
+		return -1;
+	}
+
+	val = (cnt - CNT0);
+	asm volatile("mcr p15, 0, %0, c9, c12, 5" : : "r" (val));
+
+	return cnt;
+}
+
+static u32 scorpion_read_lpm0(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 0, %0, c15, c0, 0" : "=r" (val));
+	return val;
+}
+
+static void scorpion_write_lpm0(u32 val)
+{
+	asm volatile("mcr p15, 0, %0, c15, c0, 0" : : "r" (val));
+}
+
+static u32 scorpion_read_lpm1(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 1, %0, c15, c0, 0" : "=r" (val));
+	return val;
+}
+
+static void scorpion_write_lpm1(u32 val)
+{
+	asm volatile("mcr p15, 1, %0, c15, c0, 0" : : "r" (val));
+}
+
+static u32 scorpion_read_lpm2(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 2, %0, c15, c0, 0" : "=r" (val));
+	return val;
+}
+
+static void scorpion_write_lpm2(u32 val)
+{
+	asm volatile("mcr p15, 2, %0, c15, c0, 0" : : "r" (val));
+}
+
+static u32 scorpion_read_l2lpm(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 3, %0, c15, c2, 0" : "=r" (val));
+	return val;
+}
+
+static void scorpion_write_l2lpm(u32 val)
+{
+	asm volatile("mcr p15, 3, %0, c15, c2, 0" : : "r" (val));
+}
+
+static u32 scorpion_read_vlpm(void)
+{
+	u32 val;
+	asm volatile("mrc p10, 7, %0, c11, c0, 0" : "=r" (val));
+	return val;
+}
+
+static void scorpion_write_vlpm(u32 val)
+{
+	asm volatile("mcr p10, 7, %0, c11, c0, 0" : : "r" (val));
+}
+
+struct scorpion_access_funcs {
+	u32 (*read) (void);
+	void (*write) (u32);
+};
+
+struct scorpion_access_funcs scor_func[] = {
+	{scorpion_read_lpm0, scorpion_write_lpm0},
+	{scorpion_read_lpm1, scorpion_write_lpm1},
+	{scorpion_read_lpm2, scorpion_write_lpm2},
+	{scorpion_read_l2lpm, scorpion_write_l2lpm},
+	{scorpion_read_vlpm, scorpion_write_vlpm},
+};
+
+u32 venum_orig_val;
+u32 fp_orig_val;
+
+static void scorpion_pre_vlpm(void)
+{
+	u32 venum_new_val;
+	u32 fp_new_val;
+
+	/* CPACR Enable CP10 access*/
+	asm volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (venum_orig_val));
+	venum_new_val = venum_orig_val | 0x00300000;
+	asm volatile("mcr p15, 0, %0, c1, c0, 2" : : "r" (venum_new_val));
+	/* Enable FPEXC */
+	asm volatile("mrc p10, 7, %0, c8, c0, 0" : "=r" (fp_orig_val));
+	fp_new_val = fp_orig_val | 0x40000000;
+	asm volatile("mcr p10, 7, %0, c8, c0, 0" : : "r" (fp_new_val));
+}
+
+static void scorpion_post_vlpm(void)
+{
+	/* Restore FPEXC*/
+	asm volatile("mcr p10, 7, %0, c8, c0, 0" : : "r" (fp_orig_val));
+	/* Restore CPACR*/
+	asm volatile("mcr p15, 0, %0, c1, c0, 2" : : "r" (venum_orig_val));
+}
+
+#define COLMN0MASK 0x000000ff
+#define COLMN1MASK 0x0000ff00
+#define COLMN2MASK 0x00ff0000
+static u32 scorpion_get_columnmask(u32 setval)
+{
+	if (setval & COLMN0MASK)
+		return 0xffffff00;
+	else if (setval & COLMN1MASK)
+		return 0xffff00ff;
+	else if (setval & COLMN2MASK)
+		return 0xff00ffff;
+	else
+		return 0x80ffffff;
+}
+
+static void scorpion_evt_setup(u32 gr, u32 setval)
+{
+	u32 val;
+	if (gr == 4)
+		scorpion_pre_vlpm();
+	val = scorpion_get_columnmask(setval) & scor_func[gr].read();
+	val = val | setval;
+	scor_func[gr].write(val);
+	if (gr == 4)
+		scorpion_post_vlpm();
+}
+
+static int get_scorpion_evtinfo(unsigned int evt_type, struct scorp_evt *evtinfo)
+{
+	u32 idx;
+	if ((evt_type < 0x4c) || (evt_type >= MSM_MAX_EVT))
+		return 0;
+	idx = evt_type - 0x4c;
+	if (sc_evt[idx].evt_type == evt_type) {
+		evtinfo->val = sc_evt[idx].val;
+		evtinfo->grp = sc_evt[idx].grp;
+		evtinfo->evt_type_act = sc_evt[idx].evt_type_act;
+		return 1;
+	}
+	return 0;
+}
+
+static inline void scorpion_pmnc_write_evtsel(unsigned int cnt, u32 val)
+{
+	if (scorpion_pmnc_select_counter(cnt) == cnt) {
+		if (val < 0x40) {
+			asm volatile("mcr p15, 0, %0, c9, c13, 1" : : "r" (val));
+		} else {
+			u32 zero = 0;
+			struct scorp_evt evtinfo;
+			// extract evtinfo.grp and evtinfo.tevt_type_act from val
+			if (get_scorpion_evtinfo(val, &evtinfo) == 0) return;
+			asm volatile("mcr p15, 0, %0, c9, c13, 1" : : "r" (evtinfo.evt_type_act));
+			asm volatile("mcr p15, 0, %0, c9, c15, 0" : : "r" (zero));
+			scorpion_evt_setup(evtinfo.grp, val);
+		}
+	}
+}
+
+static void scorpion_pmnc_reset_counter(unsigned int cnt)
+{
+	u32 val = 0;
+
+	if (cnt == CCNT) {
+		scorpion_pmnc_disable_counter(cnt);
+
+		asm volatile("mcr p15, 0, %0, c9, c13, 0" : : "r" (val));
+
+		if (pmnc_enabled[cnt] != 0)
+		    scorpion_pmnc_enable_counter(cnt);
+
+	} else if (cnt >= CNTMAX) {
+		pr_err("gator: CPU%u resetting wrong PMNC counter %d\n", smp_processor_id(), cnt);
+	} else {
+		scorpion_pmnc_disable_counter(cnt);
+
+		if (scorpion_pmnc_select_counter(cnt) == cnt)
+		    asm volatile("mcr p15, 0, %0, c9, c13, 2" : : "r" (val));
+
+		if (pmnc_enabled[cnt] != 0)
+		    scorpion_pmnc_enable_counter(cnt);
+	}
+}
+
+static int gator_events_scorpion_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+	int i;
+
+	for (i = 0; i < pmnc_counters; i++) {
+		char buf[40];
+		if (i == 0) {
+			snprintf(buf, sizeof buf, "%s_ccnt", pmnc_name);
+		} else {
+			snprintf(buf, sizeof buf, "%s_cnt%d", pmnc_name, i-1);
+		}
+		dir = gatorfs_mkdir(sb, root, buf);
+		if (!dir) {
+			return -1;
+		}
+		gatorfs_create_ulong(sb, dir, "enabled", &pmnc_enabled[i]);
+		gatorfs_create_ro_ulong(sb, dir, "key", &pmnc_key[i]);		
+		if (i > 0) {
+			gatorfs_create_ulong(sb, dir, "event", &pmnc_event[i]);
+		}
+	}
+
+	return 0;
+}
+
+static int gator_events_scorpion_online(int** buffer)
+{
+	unsigned int cnt, len = 0, cpu = smp_processor_id();
+
+	if (scorpion_pmnc_read() & PMNC_E) {
+		scorpion_pmnc_write(scorpion_pmnc_read() & ~PMNC_E);
+	}
+
+	/* Initialize & Reset PMNC: C bit and P bit */
+	scorpion_pmnc_write(PMNC_P | PMNC_C);
+
+	for (cnt = CCNT; cnt < CNTMAX; cnt++) {
+		unsigned long event;
+
+		if (!pmnc_enabled[cnt])
+			continue;
+
+		// disable counter
+		scorpion_pmnc_disable_counter(cnt);
+
+		event = pmnc_event[cnt] & 255;
+
+		// Set event (if destined for PMNx counters), We don't need to set the event if it's a cycle count
+		if (cnt != CCNT)
+			scorpion_pmnc_write_evtsel(cnt, event);
+
+		// reset counter
+		scorpion_pmnc_reset_counter(cnt);
+
+		// Enable counter, do not enable interrupt for this counter
+		scorpion_pmnc_enable_counter(cnt);
+	}
+
+	// enable
+	scorpion_pmnc_write(scorpion_pmnc_read() | PMNC_E);
+
+	// read the counters and toss the invalid data, return zero instead
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		if (pmnc_enabled[cnt]) {
+			int value;
+			if (cnt == CCNT) {
+				value = scorpion_ccnt_read();
+			} else if (scorpion_pmnc_select_counter(cnt) == cnt) {
+				value = scorpion_cntn_read();
+			} else {
+				value = 0;
+			}
+			scorpion_pmnc_reset_counter(cnt);
+
+			per_cpu(perfCnt, cpu)[len++] = pmnc_key[cnt];
+			per_cpu(perfCnt, cpu)[len++] = 0;
+		}
+	}
+
+	if (buffer)
+		*buffer = per_cpu(perfCnt, cpu);
+
+	return len;
+}
+
+static int gator_events_scorpion_offline(int** buffer)
+{
+	scorpion_pmnc_write(scorpion_pmnc_read() & ~PMNC_E);
+	return 0;
+}
+
+static void gator_events_scorpion_stop(void)
+{
+	unsigned int cnt;
+
+	for (cnt = CCNT; cnt < CNTMAX; cnt++) {
+		pmnc_enabled[cnt] = 0;
+		pmnc_event[cnt] = 0;
+	}
+}
+
+static int gator_events_scorpion_read(int **buffer)
+{
+	int cnt, len = 0;
+	int cpu = smp_processor_id();
+
+	// a context switch may occur before the online hotplug event, thus need to check that the pmu is enabled
+	if (!(scorpion_pmnc_read() & PMNC_E)) {
+		return 0;
+	}
+
+	for (cnt = 0; cnt < pmnc_counters; cnt++) {
+		if (pmnc_enabled[cnt]) {
+			int value;
+			if (cnt == CCNT) {
+				value = scorpion_ccnt_read();
+			} else if (scorpion_pmnc_select_counter(cnt) == cnt) {
+				value = scorpion_cntn_read();
+			} else {
+				value = 0;
+			}
+			scorpion_pmnc_reset_counter(cnt);
+
+			per_cpu(perfCnt, cpu)[len++] = pmnc_key[cnt];
+			per_cpu(perfCnt, cpu)[len++] = value;
+		}
+	}
+
+	if (buffer)
+		*buffer = per_cpu(perfCnt, cpu);
+
+	return len;
+}
+
+static struct gator_interface gator_events_scorpion_interface = {
+	.create_files = gator_events_scorpion_create_files,
+	.stop = gator_events_scorpion_stop,
+	.online = gator_events_scorpion_online,
+	.offline = gator_events_scorpion_offline,
+	.read = gator_events_scorpion_read,
+};
+
+int gator_events_scorpion_init(void)
+{
+	unsigned int cnt;
+
+	switch (gator_cpuid()) {
+	case SCORPION:
+		pmnc_name = "Scorpion";
+		pmnc_counters = 4;
+		break;
+	case SCORPIONMP:
+		pmnc_name = "ScorpionMP";
+		pmnc_counters = 4;
+		break;
+	default:
+		return -1;
+	}
+
+	pmnc_counters++; // CNT[n] + CCNT
+
+	for (cnt = CCNT; cnt < CNTMAX; cnt++) {
+		pmnc_enabled[cnt] = 0;
+		pmnc_event[cnt] = 0;
+		pmnc_key[cnt] = gator_events_get_key();
+	}
+
+	return gator_events_install(&gator_events_scorpion_interface);
+}
+
+gator_events_init(gator_events_scorpion_init);
+
+#else
+int gator_events_scorpion_init(void)
+{
+	return -1;
+}
+#endif
diff --git a/drivers/gator/gator_fs.c b/drivers/gator/gator_fs.c
new file mode 100644
index 00000000000..39adfbecd06
--- /dev/null
+++ b/drivers/gator/gator_fs.c
@@ -0,0 +1,292 @@
+/**
+ * @file gatorfs.c
+ *
+ * @remark Copyright 2002 OProfile authors
+ * @remark Read the file COPYING
+ *
+ * @author John Levon
+ *
+ * A simple filesystem for configuration and
+ * access of oprofile.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/pagemap.h>
+#include <asm/uaccess.h>
+
+#define gatorfs_MAGIC 0x24051020
+#define TMPBUFSIZE 50
+DEFINE_SPINLOCK(gatorfs_lock);
+
+static struct inode *gatorfs_get_inode(struct super_block *sb, int mode)
+{
+	struct inode *inode = new_inode(sb);
+
+	if (inode) {
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 37)
+		inode->i_ino = get_next_ino();
+#endif
+		inode->i_mode = mode;
+		inode->i_atime = inode->i_mtime = inode->i_ctime = CURRENT_TIME;
+	}
+	return inode;
+}
+
+static const struct super_operations s_ops = {
+	.statfs		= simple_statfs,
+	.drop_inode 	= generic_delete_inode,
+};
+
+ssize_t gatorfs_str_to_user(char const *str, char __user *buf, size_t count, loff_t *offset)
+{
+	return simple_read_from_buffer(buf, count, offset, str, strlen(str));
+}
+
+ssize_t gatorfs_ulong_to_user(unsigned long val, char __user *buf, size_t count, loff_t *offset)
+{
+	char tmpbuf[TMPBUFSIZE];
+	size_t maxlen = snprintf(tmpbuf, TMPBUFSIZE, "%lu\n", val);
+	if (maxlen > TMPBUFSIZE)
+		maxlen = TMPBUFSIZE;
+	return simple_read_from_buffer(buf, count, offset, tmpbuf, maxlen);
+}
+
+int gatorfs_ulong_from_user(unsigned long *val, char const __user *buf, size_t count)
+{
+	char tmpbuf[TMPBUFSIZE];
+	unsigned long flags;
+
+	if (!count)
+		return 0;
+
+	if (count > TMPBUFSIZE - 1)
+		return -EINVAL;
+
+	memset(tmpbuf, 0x0, TMPBUFSIZE);
+
+	if (copy_from_user(tmpbuf, buf, count))
+		return -EFAULT;
+
+	spin_lock_irqsave(&gatorfs_lock, flags);
+	*val = simple_strtoul(tmpbuf, NULL, 0);
+	spin_unlock_irqrestore(&gatorfs_lock, flags);
+	return 0;
+}
+
+static ssize_t ulong_read_file(struct file *file, char __user *buf, size_t count, loff_t *offset)
+{
+	unsigned long *val = file->private_data;
+	return gatorfs_ulong_to_user(*val, buf, count, offset);
+}
+
+static ssize_t ulong_write_file(struct file *file, char const __user *buf, size_t count, loff_t *offset)
+{
+	unsigned long *value = file->private_data;
+	int retval;
+
+	if (*offset)
+		return -EINVAL;
+
+	retval = gatorfs_ulong_from_user(value, buf, count);
+
+	if (retval)
+		return retval;
+	return count;
+}
+
+static int default_open(struct inode *inode, struct file *filp)
+{
+	if (inode->i_private)
+		filp->private_data = inode->i_private;
+	return 0;
+}
+
+static const struct file_operations ulong_fops = {
+	.read		= ulong_read_file,
+	.write		= ulong_write_file,
+	.open		= default_open,
+};
+
+static const struct file_operations ulong_ro_fops = {
+	.read		= ulong_read_file,
+	.open		= default_open,
+};
+
+static struct dentry *__gatorfs_create_file(struct super_block *sb,
+	struct dentry *root, char const *name, const struct file_operations *fops,
+	int perm)
+{
+	struct dentry *dentry;
+	struct inode *inode;
+
+	dentry = d_alloc_name(root, name);
+	if (!dentry)
+		return NULL;
+	inode = gatorfs_get_inode(sb, S_IFREG | perm);
+	if (!inode) {
+		dput(dentry);
+		return NULL;
+	}
+	inode->i_fop = fops;
+	d_add(dentry, inode);
+	return dentry;
+}
+
+int gatorfs_create_ulong(struct super_block *sb, struct dentry *root,
+	char const *name, unsigned long *val)
+{
+	struct dentry *d = __gatorfs_create_file(sb, root, name,
+						     &ulong_fops, 0644);
+	if (!d)
+		return -EFAULT;
+
+	d->d_inode->i_private = val;
+	return 0;
+}
+
+int gatorfs_create_ro_ulong(struct super_block *sb, struct dentry *root,
+	char const *name, unsigned long *val)
+{
+	struct dentry *d = __gatorfs_create_file(sb, root, name,
+						     &ulong_ro_fops, 0444);
+	if (!d)
+		return -EFAULT;
+
+	d->d_inode->i_private = val;
+	return 0;
+}
+
+static ssize_t atomic_read_file(struct file *file, char __user *buf, size_t count, loff_t *offset)
+{
+	atomic_t *val = file->private_data;
+	return gatorfs_ulong_to_user(atomic_read(val), buf, count, offset);
+}
+
+static const struct file_operations atomic_ro_fops = {
+	.read		= atomic_read_file,
+	.open		= default_open,
+};
+
+int gatorfs_create_ro_atomic(struct super_block *sb, struct dentry *root,
+	char const *name, atomic_t *val)
+{
+	struct dentry *d = __gatorfs_create_file(sb, root, name,
+						     &atomic_ro_fops, 0444);
+	if (!d)
+		return -EFAULT;
+
+	d->d_inode->i_private = val;
+	return 0;
+}
+
+int gatorfs_create_file(struct super_block *sb, struct dentry *root,
+	char const *name, const struct file_operations *fops)
+{
+	if (!__gatorfs_create_file(sb, root, name, fops, 0644))
+		return -EFAULT;
+	return 0;
+}
+
+int gatorfs_create_file_perm(struct super_block *sb, struct dentry *root,
+	char const *name, const struct file_operations *fops, int perm)
+{
+	if (!__gatorfs_create_file(sb, root, name, fops, perm))
+		return -EFAULT;
+	return 0;
+}
+
+struct dentry *gatorfs_mkdir(struct super_block *sb,
+	struct dentry *root, char const *name)
+{
+	struct dentry *dentry;
+	struct inode *inode;
+
+	dentry = d_alloc_name(root, name);
+	if (!dentry)
+		return NULL;
+	inode = gatorfs_get_inode(sb, S_IFDIR | 0755);
+	if (!inode) {
+		dput(dentry);
+		return NULL;
+	}
+	inode->i_op = &simple_dir_inode_operations;
+	inode->i_fop = &simple_dir_operations;
+	d_add(dentry, inode);
+	return dentry;
+}
+
+static int gatorfs_fill_super(struct super_block *sb, void *data, int silent)
+{
+	struct inode *root_inode;
+	struct dentry *root_dentry;
+
+	sb->s_blocksize = PAGE_CACHE_SIZE;
+	sb->s_blocksize_bits = PAGE_CACHE_SHIFT;
+	sb->s_magic = gatorfs_MAGIC;
+	sb->s_op = &s_ops;
+	sb->s_time_gran = 1;
+
+	root_inode = gatorfs_get_inode(sb, S_IFDIR | 0755);
+	if (!root_inode)
+		return -ENOMEM;
+	root_inode->i_op = &simple_dir_inode_operations;
+	root_inode->i_fop = &simple_dir_operations;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 4, 0)
+	root_dentry = d_alloc_root(root_inode);
+#else
+	root_dentry = d_make_root(root_inode);
+#endif
+
+	if (!root_dentry) {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 4, 0)
+		iput(root_inode);
+#endif
+		return -ENOMEM;
+	}
+
+	sb->s_root = root_dentry;
+
+	gator_op_create_files(sb, root_dentry);
+
+	// FIXME: verify kill_litter_super removes our dentries
+	return 0;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 39)
+static int gatorfs_get_sb(struct file_system_type *fs_type,
+	int flags, const char *dev_name, void *data, struct vfsmount *mnt)
+{
+	return get_sb_single(fs_type, flags, data, gatorfs_fill_super, mnt);
+}
+#else
+static struct dentry *gatorfs_mount(struct file_system_type *fs_type,
+	int flags, const char *dev_name, void *data)
+{
+	return mount_nodev(fs_type, flags, data, gatorfs_fill_super);
+}
+#endif
+
+static struct file_system_type gatorfs_type = {
+	.owner		= THIS_MODULE,
+	.name		= "gatorfs",
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 39)
+	 	.get_sb		= gatorfs_get_sb,
+#else
+	 	.mount		= gatorfs_mount,
+#endif
+
+	.kill_sb	= kill_litter_super,
+};
+
+int __init gatorfs_register(void)
+{
+	return register_filesystem(&gatorfs_type);
+}
+
+void gatorfs_unregister(void)
+{
+	unregister_filesystem(&gatorfs_type);
+}
diff --git a/drivers/gator/gator_hrtimer_gator.c b/drivers/gator/gator_hrtimer_gator.c
new file mode 100755
index 00000000000..846fba44cff
--- /dev/null
+++ b/drivers/gator/gator_hrtimer_gator.c
@@ -0,0 +1,97 @@
+/**
+ * Copyright (C) ARM Limited 2011-2012. 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.
+ *
+ */
+
+// gator_hrtimer_perf.c is used if perf is supported
+//   update, gator_hrtimer_gator.c always used until issues resolved with perf hrtimers
+#if 1
+
+void (*callback)(void);
+DEFINE_PER_CPU(struct hrtimer, percpu_hrtimer);
+DEFINE_PER_CPU(int, hrtimer_is_active);
+static ktime_t profiling_interval;
+static void gator_hrtimer_online(int cpu);
+static void gator_hrtimer_offline(int cpu);
+
+static enum hrtimer_restart gator_hrtimer_notify(struct hrtimer *hrtimer)
+{
+	hrtimer_forward_now(hrtimer, profiling_interval);
+	(*callback)();
+	return HRTIMER_RESTART;
+}
+
+static void gator_hrtimer_switch_cpus_online(void *unused)
+{
+	gator_hrtimer_online(smp_processor_id());
+}
+
+static void gator_hrtimer_online(int cpu)
+{
+	struct hrtimer *hrtimer = &per_cpu(percpu_hrtimer, cpu);
+
+	if (cpu != smp_processor_id()) {
+		smp_call_function_single(cpu, gator_hrtimer_switch_cpus_online, NULL, 1);
+		return;
+	}
+
+	if (per_cpu(hrtimer_is_active, cpu) || profiling_interval.tv64 == 0)
+		return;
+
+	per_cpu(hrtimer_is_active, cpu) = 1;
+	hrtimer_init(hrtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	hrtimer->function = gator_hrtimer_notify;
+	hrtimer_start(hrtimer, profiling_interval, HRTIMER_MODE_REL_PINNED);
+}
+
+static void gator_hrtimer_switch_cpus_offline(void *unused)
+{
+	gator_hrtimer_offline(smp_processor_id());
+}
+
+static void gator_hrtimer_offline(int cpu)
+{
+	struct hrtimer *hrtimer = &per_cpu(percpu_hrtimer, cpu);
+
+	if (cpu != smp_processor_id()) {
+		smp_call_function_single(cpu, gator_hrtimer_switch_cpus_offline, NULL, 1);
+		return;
+	}
+
+	if (!per_cpu(hrtimer_is_active, cpu))
+		return;
+
+	per_cpu(hrtimer_is_active, cpu) = 0;
+	hrtimer_cancel(hrtimer);
+}
+
+static int gator_hrtimer_init(int interval, void (*func)(void))
+{
+	int cpu;
+
+	(callback) = (func);
+
+	for_each_present_cpu(cpu) {
+		per_cpu(hrtimer_is_active, cpu) = 0;
+	}
+
+	// calculate profiling interval
+	if (interval > 0) {
+		profiling_interval = ns_to_ktime(1000000000UL / interval);
+	} else {
+		profiling_interval.tv64 = 0;
+	}
+
+	return 0;
+}
+
+static void gator_hrtimer_shutdown(void)
+{
+	/* empty */
+}
+
+#endif
diff --git a/drivers/gator/gator_hrtimer_perf.c b/drivers/gator/gator_hrtimer_perf.c
new file mode 100755
index 00000000000..7c0333f6445
--- /dev/null
+++ b/drivers/gator/gator_hrtimer_perf.c
@@ -0,0 +1,113 @@
+/**
+ * Copyright (C) ARM Limited 2011-2012. 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.
+ *
+ */
+
+// gator_hrtimer_gator.c is used if perf is not supported
+//   update, gator_hrtimer_gator.c always used until issues resolved with perf hrtimers
+#if 0
+
+// Note: perf Cortex support added in 2.6.35 and PERF_COUNT_SW_CPU_CLOCK/hrtimer broken on 2.6.35 and 2.6.36
+//       not relevant as this code is not active until 3.0.0, but wanted to document the issue
+
+void (*callback)(void);
+static int profiling_interval;
+static DEFINE_PER_CPU(struct perf_event *, perf_hrtimer);
+static DEFINE_PER_CPU(struct perf_event_attr *, perf_hrtimer_attr);
+
+static void gator_hrtimer_shutdown(void);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 1, 0)
+static void hrtimer_overflow_handler(struct perf_event *event, int unused, struct perf_sample_data *data, struct pt_regs *regs)
+#else
+static void hrtimer_overflow_handler(struct perf_event *event, struct perf_sample_data *data, struct pt_regs *regs)
+#endif
+{
+	(*callback)();
+}
+
+static int gator_online_single_hrtimer(int cpu)
+{
+	if (per_cpu(perf_hrtimer, cpu) != 0 || per_cpu(perf_hrtimer_attr, cpu) == 0)
+		return 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 1, 0)
+	per_cpu(perf_hrtimer, cpu) = perf_event_create_kernel_counter(per_cpu(perf_hrtimer_attr, cpu), cpu, 0, hrtimer_overflow_handler);
+#else
+	per_cpu(perf_hrtimer, cpu) = perf_event_create_kernel_counter(per_cpu(perf_hrtimer_attr, cpu), cpu, 0, hrtimer_overflow_handler, 0);
+#endif
+	if (IS_ERR(per_cpu(perf_hrtimer, cpu))) {
+		per_cpu(perf_hrtimer, cpu) = NULL;
+		return -1;
+	}
+
+	if (per_cpu(perf_hrtimer, cpu)->state != PERF_EVENT_STATE_ACTIVE) {
+		perf_event_release_kernel(per_cpu(perf_hrtimer, cpu));
+		per_cpu(perf_hrtimer, cpu) = NULL;
+		return -1;
+	}
+
+	return 0;
+}
+
+static void gator_hrtimer_online(int cpu)
+{
+	if (gator_online_single_hrtimer(cpu) < 0) {
+		pr_debug("gator: unable to online the hrtimer on cpu%d\n", cpu);
+	}
+}
+
+static void gator_hrtimer_offline(int cpu)
+{
+	if (per_cpu(perf_hrtimer, cpu)) {
+		perf_event_release_kernel(per_cpu(perf_hrtimer, cpu));
+		per_cpu(perf_hrtimer, cpu) = NULL;
+	}
+}
+
+static int gator_hrtimer_init(int interval, void (*func)(void))
+{
+	u32 size = sizeof(struct perf_event_attr);
+	int cpu;
+
+	callback = func;
+
+	// calculate profiling interval
+	profiling_interval = 1000000000 / interval;
+
+	for_each_present_cpu(cpu) {
+		per_cpu(perf_hrtimer, cpu) = 0;
+		per_cpu(perf_hrtimer_attr, cpu) = kmalloc(size, GFP_KERNEL);
+		if (per_cpu(perf_hrtimer_attr, cpu) == 0) {
+			gator_hrtimer_shutdown();
+			return -1;
+		}
+
+		memset(per_cpu(perf_hrtimer_attr, cpu), 0, size);
+		per_cpu(perf_hrtimer_attr, cpu)->type = PERF_TYPE_SOFTWARE;
+		per_cpu(perf_hrtimer_attr, cpu)->size = size;
+		per_cpu(perf_hrtimer_attr, cpu)->config = PERF_COUNT_SW_CPU_CLOCK;
+		per_cpu(perf_hrtimer_attr, cpu)->sample_period = profiling_interval;
+		per_cpu(perf_hrtimer_attr, cpu)->pinned = 1;
+	}
+
+	return 0;
+}
+
+static void gator_hrtimer_shutdown(void)
+{
+	int cpu;
+
+	for_each_present_cpu(cpu) {
+		if (per_cpu(perf_hrtimer_attr, cpu)) {
+			kfree(per_cpu(perf_hrtimer_attr, cpu));
+			per_cpu(perf_hrtimer_attr, cpu) = NULL;
+		}
+	}
+}
+
+#endif
diff --git a/drivers/gator/gator_main.c b/drivers/gator/gator_main.c
new file mode 100644
index 00000000000..988045f187d
--- /dev/null
+++ b/drivers/gator/gator_main.c
@@ -0,0 +1,1086 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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.
+ *
+ */
+
+static unsigned long gator_protocol_version = 9;
+
+#include <linux/slab.h>
+#include <linux/cpu.h>
+#include <linux/sched.h>
+#include <linux/irq.h>
+#include <linux/vmalloc.h>
+#include <linux/hardirq.h>
+#include <linux/highmem.h>
+#include <linux/pagemap.h>
+#include <linux/suspend.h>
+#include <linux/module.h>
+#include <linux/perf_event.h>
+#include <asm/stacktrace.h>
+#include <asm/uaccess.h>
+
+#include "gator.h"
+#include "gator_events.h"
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 32)
+#error kernels prior to 2.6.32 are not supported
+#endif
+
+#if !defined(CONFIG_GENERIC_TRACER) && !defined(CONFIG_TRACING)
+#error gator requires the kernel to have CONFIG_GENERIC_TRACER or CONFIG_TRACING defined
+#endif
+
+#ifndef CONFIG_PROFILING
+#error gator requires the kernel to have CONFIG_PROFILING defined
+#endif
+
+#ifndef CONFIG_HIGH_RES_TIMERS
+#error gator requires the kernel to have CONFIG_HIGH_RES_TIMERS defined to support PC sampling
+#endif
+
+#if defined(__arm__) && defined(CONFIG_SMP) && !defined(CONFIG_LOCAL_TIMERS)
+#error gator requires the kernel to have CONFIG_LOCAL_TIMERS defined on SMP systems
+#endif
+
+#if (GATOR_PERF_SUPPORT) && (!(GATOR_PERF_PMU_SUPPORT))
+#ifndef CONFIG_PERF_EVENTS
+#warning gator requires the kernel to have CONFIG_PERF_EVENTS defined to support pmu hardware counters
+#elif !defined CONFIG_HW_PERF_EVENTS
+#warning gator requires the kernel to have CONFIG_HW_PERF_EVENTS defined to support pmu hardware counters
+#endif
+#endif
+
+/******************************************************************************
+ * DEFINES
+ ******************************************************************************/
+#define BACKTRACE_BUFFER_SIZE    (128*1024)
+#define COUNTER_BUFFER_SIZE      (128*1024)
+#define ANNOTATE_BUFFER_SIZE     (64*1024) // annotate  counters have the core as part of the data and the core value in the frame header may be discarded
+#define SCHED_TRACE_BUFFER_SIZE  (128*1024)
+#define GPU_TRACE_BUFFER_SIZE    (64*1024)
+#define COUNTER2_BUFFER_SIZE     (64*1024) // counters2 counters have the core as part of the data and the core value in the frame header may be discarded
+#define WFI_BUFFER_SIZE          (32*1024) // wfi       counters have the core as part of the data and the core value in the frame header may be discarded
+
+#define NO_COOKIE				0UL
+#define INVALID_COOKIE			~0UL
+
+#define FRAME_BACKTRACE     1
+#define FRAME_COUNTER       2
+#define FRAME_ANNOTATE      3
+#define FRAME_SCHED_TRACE   4
+#define FRAME_GPU_TRACE     5
+#define FRAME_COUNTER2      6
+#define FRAME_WFI           7
+
+#define MESSAGE_COOKIE              1
+#define MESSAGE_START_BACKTRACE     5
+#define MESSAGE_END_BACKTRACE       7
+#define MESSAGE_SUMMARY             9
+#define MESSAGE_PID_NAME            11
+
+#define MAXSIZE_PACK32		5
+#define MAXSIZE_PACK64		9
+
+#if defined(__arm__)
+#define PC_REG regs->ARM_pc
+#else
+#define PC_REG regs->ip
+#endif
+
+enum {BACKTRACE_BUF, COUNTER_BUF, SCHED_TRACE_BUF, GPU_TRACE_BUF, ANNOTATE_BUF, COUNTER2_BUF, WFI_BUF, NUM_GATOR_BUFS};
+
+/******************************************************************************
+ * Globals
+ ******************************************************************************/
+static unsigned long gator_cpu_cores;
+static unsigned long userspace_buffer_size;
+static unsigned long gator_backtrace_depth;
+
+static unsigned long gator_started;
+static unsigned long gator_buffer_opened;
+static unsigned long gator_timer_count;
+static unsigned long gator_response_type;
+static DEFINE_MUTEX(start_mutex);
+static DEFINE_MUTEX(gator_buffer_mutex);
+
+bool event_based_sampling;
+
+static DECLARE_WAIT_QUEUE_HEAD(gator_buffer_wait);
+static LIST_HEAD(gator_events);
+
+/******************************************************************************
+ * Prototypes
+ ******************************************************************************/
+static void buffer_check(int cpu, int buftype);
+static int buffer_bytes_available(int cpu, int buftype);
+static bool buffer_check_space(int cpu, int buftype, int bytes);
+static int contiguous_space_available(int cpu, int bufytpe);
+static void gator_buffer_write_packed_int(int cpu, int buftype, unsigned int x);
+static void gator_buffer_write_packed_int64(int cpu, int buftype, unsigned long long x);
+static void gator_buffer_write_bytes(int cpu, int buftype, char *x, int len);
+static void gator_buffer_write_string(int cpu, int buftype, char *x);
+static void gator_add_trace(int cpu, int buftype, unsigned int address);
+static void gator_add_sample(int cpu, int buftype, struct pt_regs * const regs);
+static uint64_t gator_get_time(void);
+
+static uint32_t gator_buffer_size[NUM_GATOR_BUFS];
+static uint32_t gator_buffer_mask[NUM_GATOR_BUFS];
+static DEFINE_PER_CPU(int[NUM_GATOR_BUFS], gator_buffer_read);
+static DEFINE_PER_CPU(int[NUM_GATOR_BUFS], gator_buffer_write);
+static DEFINE_PER_CPU(int[NUM_GATOR_BUFS], gator_buffer_commit);
+static DEFINE_PER_CPU(int[NUM_GATOR_BUFS], buffer_space_available);
+static DEFINE_PER_CPU(char *[NUM_GATOR_BUFS], gator_buffer);
+
+/******************************************************************************
+ * Application Includes
+ ******************************************************************************/
+#include "gator_marshaling.c"
+#include "gator_hrtimer_perf.c"
+#include "gator_hrtimer_gator.c"
+#include "gator_cookies.c"
+#include "gator_trace_sched.c"
+#include "gator_trace_power.c"
+#include "gator_trace_gpu.c"
+#include "gator_backtrace.c"
+#include "gator_annotate.c"
+#include "gator_fs.c"
+#include "gator_ebs.c"
+#include "gator_pack.c"
+
+/******************************************************************************
+ * Misc
+ ******************************************************************************/
+#if defined(__arm__)
+u32 gator_cpuid(void)
+{
+	u32 val;
+	asm volatile("mrc p15, 0, %0, c0, c0, 0" : "=r" (val));
+	return (val >> 4) & 0xfff;
+}
+#endif
+
+/******************************************************************************
+ * Commit interface
+ ******************************************************************************/
+static bool buffer_commit_ready(int* cpu, int* buftype)
+{
+	int cpu_x, x;
+	for_each_present_cpu(cpu_x) {
+		for (x = 0; x < NUM_GATOR_BUFS; x++)
+			if (per_cpu(gator_buffer_commit, cpu_x)[x] != per_cpu(gator_buffer_read, cpu_x)[x]) {
+				*cpu = cpu_x;
+				*buftype = x;
+				return true;
+			}
+	}
+	return false;
+}
+
+/******************************************************************************
+ * Buffer management
+ ******************************************************************************/
+static int buffer_bytes_available(int cpu, int buftype)
+{
+	int remaining, filled;
+
+	filled = per_cpu(gator_buffer_write, cpu)[buftype] - per_cpu(gator_buffer_read, cpu)[buftype];
+	if (filled < 0) {
+		filled += gator_buffer_size[buftype];
+	}
+
+	remaining = gator_buffer_size[buftype] - filled;
+
+	if (per_cpu(buffer_space_available, cpu)[buftype]) {
+		// Give some extra room; also allows space to insert the overflow error packet
+		remaining -= 200;
+	} else {
+		// Hysteresis, prevents multiple overflow messages
+		remaining -= 2000;
+	}
+
+	return remaining;
+}
+
+static int contiguous_space_available(int cpu, int buftype)
+{
+	int remaining = buffer_bytes_available(cpu, buftype);
+	int contiguous = gator_buffer_size[buftype] - per_cpu(gator_buffer_write, cpu)[buftype];
+	if (remaining < contiguous)
+		return remaining;
+	else
+		return contiguous;
+}
+
+static bool buffer_check_space(int cpu, int buftype, int bytes)
+{
+	int remaining = buffer_bytes_available(cpu, buftype);
+
+	if (remaining < bytes) {
+		per_cpu(buffer_space_available, cpu)[buftype] = false;
+	} else {
+		per_cpu(buffer_space_available, cpu)[buftype] = true;
+	}
+
+	return per_cpu(buffer_space_available, cpu)[buftype];
+}
+
+static void gator_buffer_write_bytes(int cpu, int buftype, char *x, int len)
+{
+	int i;
+	u32 write = per_cpu(gator_buffer_write, cpu)[buftype];
+	u32 mask = gator_buffer_mask[buftype];
+	char* buffer = per_cpu(gator_buffer, cpu)[buftype];
+
+	for (i = 0; i < len; i++) {
+		buffer[write] = x[i];
+		write = (write + 1) & mask;
+	}
+
+	per_cpu(gator_buffer_write, cpu)[buftype] = write;
+}
+
+static void gator_buffer_write_string(int cpu, int buftype, char *x)
+{
+	int len = strlen(x);
+	gator_buffer_write_packed_int(cpu, buftype, len);
+	gator_buffer_write_bytes(cpu, buftype, x, len);
+}
+
+static void gator_buffer_header(int cpu, int buftype)
+{
+	int frame;
+
+	if (buftype == BACKTRACE_BUF)
+		frame = FRAME_BACKTRACE;
+	else if (buftype == COUNTER_BUF)
+		frame = FRAME_COUNTER;
+	else if (buftype == ANNOTATE_BUF)
+		frame = FRAME_ANNOTATE;
+	else if (buftype == SCHED_TRACE_BUF)
+		frame = FRAME_SCHED_TRACE;
+	else if (buftype == GPU_TRACE_BUF)
+		frame = FRAME_GPU_TRACE;
+	else if (buftype == COUNTER2_BUF)
+		frame = FRAME_COUNTER2;
+	else if (buftype == WFI_BUF)
+		frame = FRAME_WFI;
+	else
+		frame = -1;
+
+	if (per_cpu(gator_buffer, cpu)[buftype]) {
+		marshal_frame(cpu, buftype, frame);
+	}
+}
+
+static void gator_commit_buffer(int cpu, int buftype)
+{
+	if (!per_cpu(gator_buffer, cpu)[buftype])
+		return;
+
+	per_cpu(gator_buffer_commit, cpu)[buftype] = per_cpu(gator_buffer_write, cpu)[buftype];
+	gator_buffer_header(cpu, buftype);
+	wake_up(&gator_buffer_wait);
+}
+
+static void buffer_check(int cpu, int buftype)
+{
+	int filled = per_cpu(gator_buffer_write, cpu)[buftype] - per_cpu(gator_buffer_commit, cpu)[buftype];
+	if (filled < 0) {
+		filled += gator_buffer_size[buftype];
+	}
+	if (filled >= ((gator_buffer_size[buftype] * 3) / 4)) {
+		gator_commit_buffer(cpu, buftype);
+	}
+}
+
+static void gator_add_trace(int cpu, int buftype, unsigned int address)
+{
+	off_t offset = 0;
+	unsigned long cookie = get_address_cookie(cpu, buftype, current, address & ~1, &offset);
+
+	if (cookie == NO_COOKIE || cookie == INVALID_COOKIE) {
+		offset = address;
+	}
+
+	marshal_backtrace(offset & ~1, cookie);
+}
+
+static void gator_add_sample(int cpu, int buftype, struct pt_regs * const regs)
+{
+	int inKernel = regs ? !user_mode(regs) : 1;
+	unsigned long exec_cookie = inKernel ? NO_COOKIE : get_exec_cookie(cpu, buftype, current);
+
+	if (!regs)
+		return;
+
+	if (!marshal_backtrace_header(exec_cookie, current->tgid, current->pid, inKernel))
+		return;
+
+	if (inKernel) {
+		kernel_backtrace(cpu, buftype, regs);
+	} else {
+		// Cookie+PC
+		gator_add_trace(cpu, buftype, PC_REG);
+
+		// Backtrace
+		if (gator_backtrace_depth)
+			arm_backtrace_eabi(cpu, buftype, regs, gator_backtrace_depth);
+	}
+
+	marshal_backtrace_footer();
+}
+
+/******************************************************************************
+ * hrtimer interrupt processing
+ ******************************************************************************/
+static void gator_timer_interrupt(void)
+{
+	struct pt_regs * const regs = get_irq_regs();
+	int cpu = smp_processor_id();
+
+	// Output backtrace
+	gator_add_sample(cpu, BACKTRACE_BUF, regs);
+
+	// Collect counters
+	collect_counters();
+}
+
+static int gator_running;
+
+// This function runs in interrupt context and on the appropriate core
+static void gator_timer_offline(void* unused)
+{
+	struct gator_interface *gi;
+	int i, len, cpu = smp_processor_id();
+	int* buffer;
+
+	gator_trace_sched_offline();
+	gator_trace_power_offline();
+
+	gator_hrtimer_offline(cpu);
+
+	// Offline any events and output counters
+	if (marshal_event_header()) {
+		list_for_each_entry(gi, &gator_events, list) {
+			if (gi->offline) {
+				len = gi->offline(&buffer);
+				marshal_event(len, buffer);
+			}
+		}
+	}
+
+	// Flush all buffers on this core
+	for (i = 0; i < NUM_GATOR_BUFS; i++)
+		gator_commit_buffer(cpu, i);
+}
+
+// This function runs in process context and may be running on a core other than core 'cpu'
+static void gator_timer_offline_dispatch(int cpu)
+{
+	struct gator_interface *gi;
+
+	list_for_each_entry(gi, &gator_events, list)
+		if (gi->offline_dispatch)
+			gi->offline_dispatch(cpu);
+
+	gator_event_sampling_offline_dispatch(cpu);
+}
+
+static void gator_timer_stop(void)
+{
+	int cpu;
+
+	if (gator_running) {
+		on_each_cpu(gator_timer_offline, NULL, 1);
+		for_each_online_cpu(cpu) {
+			gator_timer_offline_dispatch(cpu);
+		}
+
+		gator_running = 0;
+		gator_hrtimer_shutdown();
+	}
+}
+
+// This function runs in interrupt context and on the appropriate core
+static void gator_timer_online(void* unused)
+{
+	struct gator_interface *gi;
+	int len, cpu = smp_processor_id();
+	int* buffer;
+
+	gator_trace_power_online();
+
+	// online any events and output counters
+	if (marshal_event_header()) {
+		list_for_each_entry(gi, &gator_events, list) {
+			if (gi->online) {
+				len = gi->online(&buffer);
+				marshal_event(len, buffer);
+			}
+		}
+	}
+
+	gator_hrtimer_online(cpu);
+}
+
+// This function runs in interrupt context and may be running on a core other than core 'cpu'
+static void gator_timer_online_dispatch(int cpu)
+{
+	struct gator_interface *gi;
+
+	list_for_each_entry(gi, &gator_events, list)
+		if (gi->online_dispatch)
+			gi->online_dispatch(cpu);
+
+	gator_event_sampling_online_dispatch(cpu);
+}
+
+int gator_timer_start(unsigned long sample_rate)
+{
+	int cpu;
+
+	if (gator_running) {
+		pr_notice("gator: already running\n");
+		return 0;
+	}
+
+	gator_running = 1;
+
+	// event based sampling trumps hr timer based sampling
+	if (event_based_sampling)
+		sample_rate = 0;
+
+	if (gator_hrtimer_init(sample_rate, gator_timer_interrupt) == -1)
+		return -1;
+
+	for_each_online_cpu(cpu) {
+		gator_timer_online_dispatch(cpu);
+	}
+	on_each_cpu(gator_timer_online, NULL, 1);
+
+	return 0;
+}
+
+static uint64_t gator_get_time(void)
+{
+	struct timespec ts;
+	uint64_t timestamp;
+
+	getnstimeofday(&ts);
+	timestamp = timespec_to_ns(&ts);
+
+	return timestamp;
+}
+
+/******************************************************************************
+ * cpu hotplug and pm notifiers
+ ******************************************************************************/
+static int __cpuinit gator_hotcpu_notify(struct notifier_block *self, unsigned long action, void *hcpu)
+{
+	long cpu = (long)hcpu;
+
+	switch (action) {
+		case CPU_DOWN_PREPARE:
+		case CPU_DOWN_PREPARE_FROZEN:
+			smp_call_function_single(cpu, gator_timer_offline, NULL, 1);
+			gator_timer_offline_dispatch(cpu);
+			break;
+		case CPU_ONLINE:
+		case CPU_ONLINE_FROZEN:
+			gator_timer_online_dispatch(cpu);
+			smp_call_function_single(cpu, gator_timer_online, NULL, 1);
+			break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block __refdata gator_hotcpu_notifier = {
+	.notifier_call = gator_hotcpu_notify,
+};
+
+// n.b. calling "on_each_cpu" only runs on those that are online
+// Registered linux events are not disabled, so their counters will continue to collect
+static int gator_pm_notify(struct notifier_block *nb, unsigned long event, void *dummy)
+{
+	int cpu;
+
+	switch (event) {
+		case PM_HIBERNATION_PREPARE:
+		case PM_SUSPEND_PREPARE:
+			unregister_hotcpu_notifier(&gator_hotcpu_notifier);
+			unregister_scheduler_tracepoints();
+			on_each_cpu(gator_timer_offline, NULL, 1);
+			for_each_online_cpu(cpu) {
+				gator_timer_offline_dispatch(cpu);
+			}
+			break;
+		case PM_POST_HIBERNATION:
+		case PM_POST_SUSPEND:
+			for_each_online_cpu(cpu) {
+				gator_timer_online_dispatch(cpu);
+			}
+			on_each_cpu(gator_timer_online, NULL, 1);
+			register_scheduler_tracepoints();
+			register_hotcpu_notifier(&gator_hotcpu_notifier);
+			break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block gator_pm_notifier = {
+	.notifier_call = gator_pm_notify,
+};
+
+static int gator_notifier_start(void)
+{
+	int retval;
+	retval = register_hotcpu_notifier(&gator_hotcpu_notifier);
+	if (retval == 0)
+		retval = register_pm_notifier(&gator_pm_notifier);
+	return retval;
+}
+
+static void gator_notifier_stop(void)
+{
+	unregister_pm_notifier(&gator_pm_notifier);
+	unregister_hotcpu_notifier(&gator_hotcpu_notifier);
+}
+
+/******************************************************************************
+ * Main
+ ******************************************************************************/
+static void gator_summary(void)
+{
+	uint64_t timestamp, uptime = 0;
+	struct timespec uptime_ts;
+	void (*m2b)(struct timespec *ts);
+
+	timestamp = gator_get_time();
+
+	do_posix_clock_monotonic_gettime(&uptime_ts);
+	m2b = symbol_get(monotonic_to_bootbased);
+	if (m2b) {
+		m2b(&uptime_ts);
+		uptime = (long long)uptime_ts.tv_sec * 1000000000 + uptime_ts.tv_nsec;
+	}
+
+	marshal_summary(timestamp, uptime);
+}
+
+int gator_events_install(struct gator_interface *interface)
+{
+	list_add_tail(&interface->list, &gator_events);
+
+	return 0;
+}
+
+int gator_events_get_key(void)
+{
+	// key of zero is reserved as a timestamp
+	static int key = 1;
+
+	return key++;
+}
+
+static int gator_init(void)
+{
+	int i;
+
+	// events sources (gator_events.h, generated by gator_events.sh)
+	for (i = 0; i < ARRAY_SIZE(gator_events_list); i++)
+		if (gator_events_list[i])
+			gator_events_list[i]();
+
+	gator_trace_power_init();
+
+	return 0;
+}
+
+static int gator_start(void)
+{
+	unsigned long cpu, i;
+	struct gator_interface *gi;
+
+	// Initialize the buffer with the frame type and core
+	for_each_present_cpu(cpu) {
+		for (i = 0; i < NUM_GATOR_BUFS; i++) {
+			gator_buffer_header(cpu, i);
+		}
+	}
+
+	// Capture the start time  
+	gator_summary();
+
+	// start all events
+	list_for_each_entry(gi, &gator_events, list) {
+		if (gi->start && gi->start() != 0) {
+			struct list_head *ptr = gi->list.prev;
+
+			while (ptr != &gator_events) {
+				gi = list_entry(ptr, struct gator_interface, list);
+
+				if (gi->stop)
+					gi->stop();
+
+				ptr = ptr->prev;
+			}
+			goto events_failure;
+		}
+	}
+
+	// cookies shall be initialized before trace_sched_start() and gator_timer_start()
+	if (cookies_initialize())
+		goto cookies_failure;
+	if (gator_annotate_start())
+		goto annotate_failure;
+	if (gator_trace_sched_start())
+		goto sched_failure;
+	if (gator_trace_power_start())
+		goto power_failure;
+	if (gator_trace_gpu_start())
+		goto gpu_failure;
+	if (gator_event_sampling_start())
+		goto event_sampling_failure;
+	if (gator_timer_start(gator_timer_count))
+		goto timer_failure;
+	if (gator_notifier_start())
+		goto notifier_failure;
+
+	return 0;
+
+notifier_failure:
+	gator_timer_stop();
+timer_failure:
+	gator_event_sampling_stop();
+event_sampling_failure:
+	gator_trace_gpu_stop();
+gpu_failure:
+	gator_trace_power_stop();
+power_failure:
+	gator_trace_sched_stop();
+sched_failure:
+	gator_annotate_stop();
+annotate_failure:
+	cookies_release();
+cookies_failure:
+	// stop all events
+	list_for_each_entry(gi, &gator_events, list)
+		if (gi->stop)
+			gi->stop();
+events_failure:
+
+	return -1;
+}
+
+static void gator_stop(void)
+{
+	struct gator_interface *gi;
+
+	// stop all events
+	list_for_each_entry(gi, &gator_events, list)
+		if (gi->stop)
+			gi->stop();
+
+	gator_annotate_stop();
+	gator_trace_sched_stop();
+	gator_trace_power_stop();
+	gator_trace_gpu_stop();
+	gator_event_sampling_stop();
+
+	// stop all interrupt callback reads before tearing down other interfaces
+	gator_notifier_stop(); // should be called before gator_timer_stop to avoid re-enabling the hrtimer after it has been offlined
+	gator_timer_stop();
+}
+
+/******************************************************************************
+ * Filesystem
+ ******************************************************************************/
+/* fopen("buffer") */
+static int gator_op_setup(void)
+{
+	int err = 0;
+	int cpu, i;
+
+	mutex_lock(&start_mutex);
+
+	gator_buffer_size[BACKTRACE_BUF] = BACKTRACE_BUFFER_SIZE;
+	gator_buffer_mask[BACKTRACE_BUF] = BACKTRACE_BUFFER_SIZE - 1;
+
+	gator_buffer_size[COUNTER_BUF] = COUNTER_BUFFER_SIZE;
+	gator_buffer_mask[COUNTER_BUF] = COUNTER_BUFFER_SIZE - 1;
+
+	gator_buffer_size[SCHED_TRACE_BUF] = SCHED_TRACE_BUFFER_SIZE;
+	gator_buffer_mask[SCHED_TRACE_BUF] = SCHED_TRACE_BUFFER_SIZE - 1;
+
+	gator_buffer_size[GPU_TRACE_BUF] = GPU_TRACE_BUFFER_SIZE;
+	gator_buffer_mask[GPU_TRACE_BUF] = GPU_TRACE_BUFFER_SIZE - 1;
+
+	gator_buffer_size[ANNOTATE_BUF] = ANNOTATE_BUFFER_SIZE;
+	gator_buffer_mask[ANNOTATE_BUF] = ANNOTATE_BUFFER_SIZE - 1;
+
+	gator_buffer_size[COUNTER2_BUF] = COUNTER2_BUFFER_SIZE;
+	gator_buffer_mask[COUNTER2_BUF] = COUNTER2_BUFFER_SIZE - 1;
+
+	gator_buffer_size[WFI_BUF] = WFI_BUFFER_SIZE;
+	gator_buffer_mask[WFI_BUF] = WFI_BUFFER_SIZE - 1;
+
+	// Initialize percpu per buffer variables
+	for (i = 0; i < NUM_GATOR_BUFS; i++) {
+		// Verify buffers are a power of 2
+		if (gator_buffer_size[i] & (gator_buffer_size[i] - 1)) {
+			err = -ENOEXEC;
+			goto setup_error;
+		}
+
+		for_each_present_cpu(cpu) {
+			per_cpu(gator_buffer_read, cpu)[i] = 0;
+			per_cpu(gator_buffer_write, cpu)[i] = 0;
+			per_cpu(gator_buffer_commit, cpu)[i] = 0;
+			per_cpu(buffer_space_available, cpu)[i] = true;
+
+			// Annotation is a special case that only uses a single buffer
+			if (cpu > 0 && i == ANNOTATE_BUF) {
+				per_cpu(gator_buffer, cpu)[i] = NULL;
+				continue;
+			}
+
+			per_cpu(gator_buffer, cpu)[i] = vmalloc(gator_buffer_size[i]);
+			if (!per_cpu(gator_buffer, cpu)[i]) {
+				err = -ENOMEM;
+				goto setup_error;
+			}
+		}
+	}
+
+setup_error:
+	mutex_unlock(&start_mutex);
+	return err;
+}
+
+/* Actually start profiling (echo 1>/dev/gator/enable) */
+static int gator_op_start(void)
+{
+	int err = 0;
+
+	mutex_lock(&start_mutex);
+
+	if (gator_started || gator_start())
+		err = -EINVAL;
+	else
+		gator_started = 1;
+
+	mutex_unlock(&start_mutex);
+
+	return err;
+}
+
+/* echo 0>/dev/gator/enable */
+static void gator_op_stop(void)
+{
+	mutex_lock(&start_mutex);
+
+	if (gator_started) {
+		gator_stop();
+
+		mutex_lock(&gator_buffer_mutex);
+
+		gator_started = 0;
+		cookies_release();
+		wake_up(&gator_buffer_wait);
+
+		mutex_unlock(&gator_buffer_mutex);
+	}
+
+	mutex_unlock(&start_mutex);
+}
+
+static void gator_shutdown(void)
+{
+	int cpu, i;
+
+	mutex_lock(&start_mutex);
+
+	for_each_present_cpu(cpu) {
+		mutex_lock(&gator_buffer_mutex);
+		for (i = 0; i < NUM_GATOR_BUFS; i++) {
+			vfree(per_cpu(gator_buffer, cpu)[i]);
+			per_cpu(gator_buffer, cpu)[i] = NULL;
+			per_cpu(gator_buffer_read, cpu)[i] = 0;
+			per_cpu(gator_buffer_write, cpu)[i] = 0;
+			per_cpu(gator_buffer_commit, cpu)[i] = 0;
+			per_cpu(buffer_space_available, cpu)[i] = true;
+		}
+		mutex_unlock(&gator_buffer_mutex);
+	}
+
+	mutex_unlock(&start_mutex);
+}
+
+static int gator_set_backtrace(unsigned long val)
+{
+	int err = 0;
+
+	mutex_lock(&start_mutex);
+
+	if (gator_started)
+		err = -EBUSY;
+	else
+		gator_backtrace_depth = val;
+
+	mutex_unlock(&start_mutex);
+
+	return err;
+}
+
+static ssize_t enable_read(struct file *file, char __user *buf, size_t count, loff_t *offset)
+{
+	return gatorfs_ulong_to_user(gator_started, buf, count, offset);
+}
+
+static ssize_t enable_write(struct file *file, char const __user *buf, size_t count, loff_t *offset)
+{
+	unsigned long val;
+	int retval;
+
+	if (*offset)
+		return -EINVAL;
+
+	retval = gatorfs_ulong_from_user(&val, buf, count);
+	if (retval)
+		return retval;
+
+	if (val)
+		retval = gator_op_start();
+	else
+		gator_op_stop();
+
+	if (retval)
+		return retval;
+	return count;
+}
+
+static const struct file_operations enable_fops = {
+	.read		= enable_read,
+	.write		= enable_write,
+};
+
+static int userspace_buffer_open(struct inode *inode, struct file *file)
+{
+	int err = -EPERM;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	if (test_and_set_bit_lock(0, &gator_buffer_opened))
+		return -EBUSY;
+
+	if ((err = gator_op_setup()))
+		goto fail;
+
+	/* NB: the actual start happens from userspace
+	 * echo 1 >/dev/gator/enable
+	 */
+
+	return 0;
+
+fail:
+	__clear_bit_unlock(0, &gator_buffer_opened);
+	return err;
+}
+
+static int userspace_buffer_release(struct inode *inode, struct file *file)
+{
+	gator_op_stop();
+	gator_shutdown();
+	__clear_bit_unlock(0, &gator_buffer_opened);
+	return 0;
+}
+
+static ssize_t userspace_buffer_read(struct file *file, char __user *buf,
+				 size_t count, loff_t *offset)
+{
+	int retval = -EINVAL;
+	int commit = 0, length, length1, length2, read, byte, type_length;
+	char *buffer1;
+	char *buffer2 = NULL;
+	int cpu, buftype;
+
+	/* do not handle partial reads */
+	if (count != userspace_buffer_size || *offset)
+		return -EINVAL;
+
+	// sleep until the condition is true or a signal is received
+	// the condition is checked each time gator_buffer_wait is woken up
+	buftype = cpu = -1;
+	wait_event_interruptible(gator_buffer_wait, buffer_commit_ready(&cpu, &buftype) || !gator_started);
+
+	if (signal_pending(current))
+		return -EINTR;
+
+	length2 = 0;
+	retval = -EFAULT;
+
+	mutex_lock(&gator_buffer_mutex);
+
+	if (buftype == -1 || cpu == -1) {
+		retval = 0;
+		goto out;
+	}
+
+	read = per_cpu(gator_buffer_read, cpu)[buftype];
+	commit = per_cpu(gator_buffer_commit, cpu)[buftype];
+
+	/* May happen if the buffer is freed during pending reads. */
+	if (!per_cpu(gator_buffer, cpu)[buftype]) {
+		retval = -EFAULT;
+		goto out;
+	}
+
+	/* determine the size of two halves */
+	length1 = commit - read;
+	buffer1 = &(per_cpu(gator_buffer, cpu)[buftype][read]);
+	buffer2 = &(per_cpu(gator_buffer, cpu)[buftype][0]);
+	if (length1 < 0) {
+		length1 = gator_buffer_size[buftype] - read;
+		length2 = commit;
+	}
+
+	// post-populate the length, which does not include the response type length nor the length itself, i.e. only the length of the payload
+	type_length = gator_response_type ? 1 : 0;
+	length = length1 + length2 - type_length - sizeof(int);
+	for (byte = 0; byte < sizeof(int); byte++) {
+		per_cpu(gator_buffer, cpu)[buftype][(read + type_length + byte) & gator_buffer_mask[buftype]] = (length >> byte * 8) & 0xFF;
+	}
+
+	/* start, middle or end */
+	if (length1 > 0) {
+		if (copy_to_user(&buf[0], buffer1, length1)) {
+			goto out;
+		}
+	}
+
+	/* possible wrap around */
+	if (length2 > 0) {
+		if (copy_to_user(&buf[length1], buffer2, length2)) {
+			goto out;
+		}
+	}
+
+	per_cpu(gator_buffer_read, cpu)[buftype] = commit;
+	retval = length1 + length2;
+
+	/* kick just in case we've lost an SMP event */
+	wake_up(&gator_buffer_wait);
+
+out:
+	mutex_unlock(&gator_buffer_mutex);
+	return retval;
+}
+
+const struct file_operations gator_event_buffer_fops = {
+	.open		= userspace_buffer_open,
+	.release	= userspace_buffer_release,
+	.read		= userspace_buffer_read,
+};
+
+static ssize_t depth_read(struct file *file, char __user *buf, size_t count, loff_t *offset)
+{
+	return gatorfs_ulong_to_user(gator_backtrace_depth, buf, count,
+					offset);
+}
+
+static ssize_t depth_write(struct file *file, char const __user *buf, size_t count, loff_t *offset)
+{
+	unsigned long val;
+	int retval;
+
+	if (*offset)
+		return -EINVAL;
+
+	retval = gatorfs_ulong_from_user(&val, buf, count);
+	if (retval)
+		return retval;
+
+	retval = gator_set_backtrace(val);
+
+	if (retval)
+		return retval;
+	return count;
+}
+
+static const struct file_operations depth_fops = {
+	.read		= depth_read,
+	.write		= depth_write
+};
+
+void gator_op_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+	struct gator_interface *gi;
+	int cpu;
+
+	/* reinitialize default values */
+	gator_cpu_cores = 0;
+	for_each_present_cpu(cpu) {
+		gator_cpu_cores++;
+	}
+	userspace_buffer_size =	BACKTRACE_BUFFER_SIZE;
+	gator_response_type = 1;
+
+	gatorfs_create_file(sb, root, "enable", &enable_fops);
+	gatorfs_create_file(sb, root, "buffer", &gator_event_buffer_fops);
+	gatorfs_create_file(sb, root, "backtrace_depth", &depth_fops);
+	gatorfs_create_ulong(sb, root, "cpu_cores", &gator_cpu_cores);
+	gatorfs_create_ulong(sb, root, "buffer_size", &userspace_buffer_size);
+	gatorfs_create_ulong(sb, root, "tick", &gator_timer_count);
+	gatorfs_create_ulong(sb, root, "response_type", &gator_response_type);
+	gatorfs_create_ro_ulong(sb, root, "version", &gator_protocol_version);
+
+	// Annotate interface
+	gator_annotate_create_files(sb, root);
+
+	// Linux Events
+	dir = gatorfs_mkdir(sb, root, "events");
+	list_for_each_entry(gi, &gator_events, list)
+		if (gi->create_files)
+			gi->create_files(sb, dir);
+
+	// Power interface
+	gator_trace_power_create_files(sb, dir);
+}
+
+/******************************************************************************
+ * Module
+ ******************************************************************************/
+static int __init gator_module_init(void)
+{
+	if (gatorfs_register()) {
+		return -1;
+	}
+
+	if (gator_init()) {
+		gatorfs_unregister();
+		return -1;
+	}
+
+	return 0;
+}
+
+static void __exit gator_module_exit(void)
+{
+	tracepoint_synchronize_unregister();
+	gatorfs_unregister();
+}
+
+module_init(gator_module_init);
+module_exit(gator_module_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("ARM Ltd");
+MODULE_DESCRIPTION("Gator system profiler");
diff --git a/drivers/gator/gator_marshaling.c b/drivers/gator/gator_marshaling.c
new file mode 100755
index 00000000000..630d142bf70
--- /dev/null
+++ b/drivers/gator/gator_marshaling.c
@@ -0,0 +1,239 @@
+/**
+ * Copyright (C) ARM Limited 2012. 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.
+ *
+ */
+
+static void marshal_summary(long long timestamp, long long uptime) {
+	int cpu = 0;
+	gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, MESSAGE_SUMMARY);
+	gator_buffer_write_packed_int64(cpu, BACKTRACE_BUF, timestamp);
+	gator_buffer_write_packed_int64(cpu, BACKTRACE_BUF, uptime);
+}
+
+static bool marshal_cookie_header(char* text) {
+	int cpu = smp_processor_id();
+	return buffer_check_space(cpu, BACKTRACE_BUF, strlen(text) + 2 * MAXSIZE_PACK32);
+}
+
+static void marshal_cookie(int cookie, char* text) {
+	int cpu = smp_processor_id();
+	gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, MESSAGE_COOKIE);
+	gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, cookie);
+	gator_buffer_write_string(cpu, BACKTRACE_BUF, text);
+}
+
+static void marshal_pid_name(int pid, char* name) {
+	unsigned long flags, cpu;
+	local_irq_save(flags);
+	cpu = smp_processor_id();
+	if (buffer_check_space(cpu, BACKTRACE_BUF, TASK_COMM_LEN + 2 * MAXSIZE_PACK32 + MAXSIZE_PACK64)) {
+		gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, MESSAGE_PID_NAME);
+		gator_buffer_write_packed_int64(cpu, BACKTRACE_BUF, gator_get_time());
+		gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, pid);
+		gator_buffer_write_string(cpu, BACKTRACE_BUF, name);
+	}
+	local_irq_restore(flags);
+}
+
+static bool marshal_backtrace_header(int exec_cookie, int tgid, int pid, int inKernel) {
+	int cpu = smp_processor_id();
+	if (buffer_check_space(cpu, BACKTRACE_BUF, gator_backtrace_depth * 2 * MAXSIZE_PACK32)) {
+		gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, MESSAGE_START_BACKTRACE);
+		gator_buffer_write_packed_int64(cpu, BACKTRACE_BUF, gator_get_time());
+		gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, exec_cookie);
+		gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, tgid); 
+		gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, pid);
+		gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, inKernel);
+		return true;
+	}
+
+	// Check and commit; commit is set to occur once buffer is 3/4 full
+	buffer_check(cpu, BACKTRACE_BUF);
+
+	return false;
+}
+
+static void marshal_backtrace(int address, int cookie) {
+	int cpu = smp_processor_id();
+	gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, address);
+	gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, cookie);
+}
+
+static void marshal_backtrace_footer(void) {
+	int cpu = smp_processor_id();
+	gator_buffer_write_packed_int(cpu, BACKTRACE_BUF, MESSAGE_END_BACKTRACE);
+
+	// Check and commit; commit is set to occur once buffer is 3/4 full
+	buffer_check(cpu, BACKTRACE_BUF);
+}
+
+static bool marshal_event_header(void) {
+	unsigned long flags, cpu = smp_processor_id();
+	bool retval = false;
+	
+	local_irq_save(flags);
+	if (buffer_check_space(cpu, COUNTER_BUF, MAXSIZE_PACK32 + MAXSIZE_PACK64)) {
+		gator_buffer_write_packed_int(cpu, COUNTER_BUF, 0); // key of zero indicates a timestamp
+		gator_buffer_write_packed_int64(cpu, COUNTER_BUF, gator_get_time());
+		retval = true;
+	}
+	local_irq_restore(flags);
+
+	// Check and commit; commit is set to occur once buffer is 3/4 full
+	buffer_check(cpu, COUNTER_BUF);
+
+	return retval;
+}
+
+static void marshal_event(int len, int* buffer) {
+	unsigned long i, flags, cpu = smp_processor_id();
+
+	if (len <= 0)
+		return;
+	
+	// length must be even since all data is a (key, value) pair
+	if (len & 0x1) {
+		pr_err("gator: invalid counter data detected and discarded");
+		return;
+	}
+
+	// events must be written in key,value pairs
+	for (i = 0; i < len; i += 2) {
+		local_irq_save(flags);
+		if (!buffer_check_space(cpu, COUNTER_BUF, MAXSIZE_PACK32 * 2)) {
+			local_irq_restore(flags);
+			break;
+		}
+		gator_buffer_write_packed_int(cpu, COUNTER_BUF, buffer[i]);
+		gator_buffer_write_packed_int(cpu, COUNTER_BUF, buffer[i + 1]);
+		local_irq_restore(flags);
+	}
+
+	// Check and commit; commit is set to occur once buffer is 3/4 full
+	buffer_check(cpu, COUNTER_BUF);
+}
+
+static void marshal_event64(int len, long long* buffer64) {
+	unsigned long i, flags, cpu = smp_processor_id();
+
+	if (len <= 0)
+		return;
+	
+	// length must be even since all data is a (key, value) pair
+	if (len & 0x1) {
+		pr_err("gator: invalid counter data detected and discarded");
+		return;
+	}
+
+	// events must be written in key,value pairs
+	for (i = 0; i < len; i += 2) {
+		local_irq_save(flags);
+		if (!buffer_check_space(cpu, COUNTER_BUF, MAXSIZE_PACK64 * 2)) {
+			local_irq_restore(flags);
+			break;
+		}
+		gator_buffer_write_packed_int64(cpu, COUNTER_BUF, buffer64[i]);
+		gator_buffer_write_packed_int64(cpu, COUNTER_BUF, buffer64[i + 1]);
+		local_irq_restore(flags);
+	}
+
+	// Check and commit; commit is set to occur once buffer is 3/4 full
+	buffer_check(cpu, COUNTER_BUF);
+}
+
+#if GATOR_CPU_FREQ_SUPPORT
+static void marshal_event_single(int core, int key, int value) {
+	unsigned long flags, cpu;
+	
+	local_irq_save(flags);
+	cpu = smp_processor_id();
+	if (buffer_check_space(cpu, COUNTER2_BUF, MAXSIZE_PACK64 + MAXSIZE_PACK32 * 3)) {
+		gator_buffer_write_packed_int64(cpu, COUNTER2_BUF, gator_get_time());
+		gator_buffer_write_packed_int(cpu, COUNTER2_BUF, core);
+		gator_buffer_write_packed_int(cpu, COUNTER2_BUF, key);
+		gator_buffer_write_packed_int(cpu, COUNTER2_BUF, value);
+	}
+	local_irq_restore(flags);
+
+	// Check and commit; commit is set to occur once buffer is 3/4 full
+	buffer_check(cpu, COUNTER2_BUF);
+}
+#endif
+
+static void marshal_sched_gpu(int type, int unit, int core, int tgid, int pid) {
+	unsigned long cpu = smp_processor_id(), flags;
+
+	if (!per_cpu(gator_buffer, cpu)[GPU_TRACE_BUF])
+		return;
+
+	local_irq_save(flags);
+	if (buffer_check_space(cpu, GPU_TRACE_BUF, MAXSIZE_PACK64 + 5 * MAXSIZE_PACK32)) {
+		gator_buffer_write_packed_int(cpu, GPU_TRACE_BUF, type);
+		gator_buffer_write_packed_int64(cpu, GPU_TRACE_BUF, gator_get_time());
+		gator_buffer_write_packed_int(cpu, GPU_TRACE_BUF, unit);
+		gator_buffer_write_packed_int(cpu, GPU_TRACE_BUF, core);
+		gator_buffer_write_packed_int(cpu, GPU_TRACE_BUF, tgid);
+		gator_buffer_write_packed_int(cpu, GPU_TRACE_BUF, pid);
+	}
+	local_irq_restore(flags);
+
+	// Check and commit; commit is set to occur once buffer is 3/4 full
+	buffer_check(cpu, GPU_TRACE_BUF);
+}
+
+static void marshal_sched_trace(int type, int pid, int tgid, int cookie, int state) {
+	unsigned long cpu = smp_processor_id(), flags;
+
+	if (!per_cpu(gator_buffer, cpu)[SCHED_TRACE_BUF])
+		return;
+
+	local_irq_save(flags);
+	if (buffer_check_space(cpu, SCHED_TRACE_BUF, MAXSIZE_PACK64 + 5 * MAXSIZE_PACK32)) {
+		gator_buffer_write_packed_int(cpu, SCHED_TRACE_BUF, type);
+		gator_buffer_write_packed_int64(cpu, SCHED_TRACE_BUF, gator_get_time());
+		gator_buffer_write_packed_int(cpu, SCHED_TRACE_BUF, pid);
+		gator_buffer_write_packed_int(cpu, SCHED_TRACE_BUF, tgid);
+		gator_buffer_write_packed_int(cpu, SCHED_TRACE_BUF, cookie);
+		gator_buffer_write_packed_int(cpu, SCHED_TRACE_BUF, state);
+	}
+	local_irq_restore(flags);
+
+	// Check and commit; commit is set to occur once buffer is 3/4 full
+	buffer_check(cpu, SCHED_TRACE_BUF);
+}
+
+#if GATOR_CPU_FREQ_SUPPORT
+static void marshal_wfi(int core, int state) {
+	unsigned long flags, cpu;
+	
+	local_irq_save(flags);
+	cpu = smp_processor_id();
+	if (buffer_check_space(cpu, WFI_BUF, MAXSIZE_PACK64 + MAXSIZE_PACK32 * 2)) {
+		gator_buffer_write_packed_int64(cpu, WFI_BUF, gator_get_time());
+		gator_buffer_write_packed_int(cpu, WFI_BUF, core);
+		gator_buffer_write_packed_int(cpu, WFI_BUF, state);
+	}
+	local_irq_restore(flags);
+
+	// Check and commit; commit is set to occur once buffer is 3/4 full
+	buffer_check(cpu, WFI_BUF);
+}
+#endif
+
+static void marshal_frame(int cpu, int buftype, int frame) {
+	// add response type
+	if (gator_response_type > 0) {
+		gator_buffer_write_packed_int(cpu, buftype, gator_response_type);
+	}
+
+	// leave space for 4-byte unpacked length
+	per_cpu(gator_buffer_write, cpu)[buftype] = (per_cpu(gator_buffer_write, cpu)[buftype] + 4) & gator_buffer_mask[buftype];
+
+	// add frame type and core number
+	gator_buffer_write_packed_int(cpu, buftype, frame);
+	gator_buffer_write_packed_int(cpu, buftype, cpu);
+}
diff --git a/drivers/gator/gator_pack.c b/drivers/gator/gator_pack.c
new file mode 100644
index 00000000000..925469a5b89
--- /dev/null
+++ b/drivers/gator/gator_pack.c
@@ -0,0 +1,164 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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.
+ *
+ */
+
+static void gator_buffer_write_packed_int(int cpu, int buftype, unsigned int x)
+{
+	uint32_t write = per_cpu(gator_buffer_write, cpu)[buftype];
+	uint32_t mask = gator_buffer_mask[buftype];
+	char *buffer = per_cpu(gator_buffer, cpu)[buftype];
+	int write0 = (write + 0) & mask;
+	int write1 = (write + 1) & mask;
+
+	if ((x & 0xffffff80) == 0) {
+		buffer[write0] = x & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write1;
+	} else if ((x & 0xffffc000) == 0) {
+		int write2 = (write + 2) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write2;
+	} else if ((x & 0xffe00000) == 0) {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write3;
+	} else if ((x & 0xf0000000) == 0) {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		int write4 = (write + 4) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) | 0x80;
+		buffer[write3] = (x>>21) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write4;
+	} else {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		int write4 = (write + 4) & mask;
+		int write5 = (write + 5) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) | 0x80;
+		buffer[write3] = (x>>21) | 0x80;
+		buffer[write4] = (x>>28) & 0x0f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write5;
+	}
+}
+
+static void gator_buffer_write_packed_int64(int cpu, int buftype, unsigned long long x)
+{
+	uint32_t write = per_cpu(gator_buffer_write, cpu)[buftype];
+	uint32_t mask = gator_buffer_mask[buftype];
+	char *buffer = per_cpu(gator_buffer, cpu)[buftype];
+	int write0 = (write + 0) & mask;
+	int write1 = (write + 1) & mask;
+
+	if ((x & 0xffffffffffffff80LL) == 0) {
+		buffer[write0] = x & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write1;
+	} else if ((x & 0xffffffffffffc000LL) == 0) {
+		int write2 = (write + 2) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write2;
+	} else if ((x & 0xffffffffffe00000LL) == 0) {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write3;
+	} else if ((x & 0xfffffffff0000000LL) == 0) {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		int write4 = (write + 4) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) | 0x80;
+		buffer[write3] = (x>>21) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write4;
+	} else if ((x & 0xfffffff800000000LL) == 0) {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		int write4 = (write + 4) & mask;
+		int write5 = (write + 5) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) | 0x80;
+		buffer[write3] = (x>>21) | 0x80;
+		buffer[write4] = (x>>28) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write5;
+	} else if ((x & 0xfffffc0000000000LL) == 0) {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		int write4 = (write + 4) & mask;
+		int write5 = (write + 5) & mask;
+		int write6 = (write + 6) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) | 0x80;
+		buffer[write3] = (x>>21) | 0x80;
+		buffer[write4] = (x>>28) | 0x80;
+		buffer[write5] = (x>>35) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write6;
+	} else if ((x & 0xfffe000000000000LL) == 0) {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		int write4 = (write + 4) & mask;
+		int write5 = (write + 5) & mask;
+		int write6 = (write + 6) & mask;
+		int write7 = (write + 7) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) | 0x80;
+		buffer[write3] = (x>>21) | 0x80;
+		buffer[write4] = (x>>28) | 0x80;
+		buffer[write5] = (x>>35) | 0x80;
+		buffer[write6] = (x>>42) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write7;
+	} else if ((x & 0xff00000000000000LL) == 0) {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		int write4 = (write + 4) & mask;
+		int write5 = (write + 5) & mask;
+		int write6 = (write + 6) & mask;
+		int write7 = (write + 7) & mask;
+		int write8 = (write + 8) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) | 0x80;
+		buffer[write3] = (x>>21) | 0x80;
+		buffer[write4] = (x>>28) | 0x80;
+		buffer[write5] = (x>>35) | 0x80;
+		buffer[write6] = (x>>42) | 0x80;
+		buffer[write7] = (x>>49) & 0x7f;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write8;
+	} else {
+		int write2 = (write + 2) & mask;
+		int write3 = (write + 3) & mask;
+		int write4 = (write + 4) & mask;
+		int write5 = (write + 5) & mask;
+		int write6 = (write + 6) & mask;
+		int write7 = (write + 7) & mask;
+		int write8 = (write + 8) & mask;
+		int write9 = (write + 9) & mask;
+		buffer[write0] = x | 0x80;
+		buffer[write1] = (x>>7) | 0x80;
+		buffer[write2] = (x>>14) | 0x80;
+		buffer[write3] = (x>>21) | 0x80;
+		buffer[write4] = (x>>28) | 0x80;
+		buffer[write5] = (x>>35) | 0x80;
+		buffer[write6] = (x>>42) | 0x80;
+		buffer[write7] = (x>>49) | 0x80;
+		buffer[write8] = (x>>56) & 0xff;
+		per_cpu(gator_buffer_write, cpu)[buftype] = write9;
+	}
+}
diff --git a/drivers/gator/gator_trace_gpu.c b/drivers/gator/gator_trace_gpu.c
new file mode 100644
index 00000000000..921932c1fb1
--- /dev/null
+++ b/drivers/gator/gator_trace_gpu.c
@@ -0,0 +1,126 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 "gator.h"
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/time.h>
+#include <linux/math64.h>
+
+#ifdef MALI_SUPPORT
+#include "linux/mali_linux_trace.h"
+#endif
+#include "gator_trace_gpu.h"
+
+#define ACTIVITY_START  1
+#define ACTIVITY_STOP   2
+
+/* Note whether tracepoints have been registered */
+static int mali_trace_registered;
+static int gpu_trace_registered;
+
+#define GPU_START			1
+#define GPU_STOP			2
+
+#define GPU_UNIT_VP			1
+#define GPU_UNIT_FP			2
+#define GPU_UNIT_CL			3
+
+#ifdef MALI_SUPPORT
+
+enum components {
+    COMPONENT_VP0 = 1,
+    COMPONENT_FP0 = 5,
+    COMPONENT_FP1,
+    COMPONENT_FP2,
+    COMPONENT_FP3,
+    COMPONENT_FP4,
+    COMPONENT_FP5,
+    COMPONENT_FP6,
+    COMPONENT_FP7,
+};
+
+GATOR_DEFINE_PROBE(mali_timeline_event, TP_PROTO(unsigned int event_id, unsigned int d0, unsigned int d1, unsigned int d2, unsigned int d3, unsigned int d4))
+{
+	unsigned int component, state;
+	int tgid = 0, pid = 0;
+
+	// do as much work as possible before disabling interrupts
+	component = (event_id >> 16) & 0xFF; // component is an 8-bit field
+	state = (event_id >> 24) & 0xF;      // state is a 4-bit field
+
+	if ((component == COMPONENT_VP0) || (component >= COMPONENT_FP0 && component <= COMPONENT_FP7)) {
+		if (state == ACTIVITY_START || state == ACTIVITY_STOP) {
+			unsigned int type = (state == ACTIVITY_START) ? GPU_START : GPU_STOP;
+			unsigned int unit = (component < COMPONENT_FP0) ? GPU_UNIT_VP : GPU_UNIT_FP;
+			unsigned int core = (component < COMPONENT_FP0) ? component - COMPONENT_VP0 : component - COMPONENT_FP0;
+			if (state == ACTIVITY_START) {
+				tgid = d0;
+				pid = d1;
+			}
+
+			marshal_sched_gpu(type, unit, core, tgid, pid);
+    	}
+    }
+}
+#endif
+
+GATOR_DEFINE_PROBE(gpu_activity_start, TP_PROTO(int gpu_unit, int gpu_core, struct task_struct *p))
+{
+	marshal_sched_gpu(GPU_START, gpu_unit, gpu_core, (int)p->tgid, (int)p->pid);
+}
+
+GATOR_DEFINE_PROBE(gpu_activity_stop, TP_PROTO(int gpu_unit, int gpu_core))
+{
+	marshal_sched_gpu(GPU_STOP, gpu_unit, gpu_core, 0, 0);
+}
+
+int gator_trace_gpu_start(void)
+{
+	/*
+	 * Returns nonzero for installation failed
+	 * Absence of gpu trace points is not an error
+	 */
+
+	gpu_trace_registered = mali_trace_registered = 0;
+
+#ifdef MALI_SUPPORT
+    if (!GATOR_REGISTER_TRACE(mali_timeline_event)) {
+    	mali_trace_registered = 1;
+    }
+#endif
+
+    if (!mali_trace_registered) {
+        if (GATOR_REGISTER_TRACE(gpu_activity_start)) {
+        	return 0;
+        }
+        if (GATOR_REGISTER_TRACE(gpu_activity_stop)) {
+        	GATOR_UNREGISTER_TRACE(gpu_activity_start);
+        	return 0;
+        }
+        gpu_trace_registered = 1;
+    }
+
+	return 0;
+}
+
+void gator_trace_gpu_stop(void)
+{
+#ifdef MALI_SUPPORT
+	if (mali_trace_registered) {
+		GATOR_UNREGISTER_TRACE(mali_timeline_event);
+	}
+#endif
+	if (gpu_trace_registered) {
+		GATOR_UNREGISTER_TRACE(gpu_activity_stop);
+		GATOR_UNREGISTER_TRACE(gpu_activity_start);
+	}
+
+	gpu_trace_registered = mali_trace_registered = 0;
+}
diff --git a/drivers/gator/gator_trace_gpu.h b/drivers/gator/gator_trace_gpu.h
new file mode 100644
index 00000000000..894289b4316
--- /dev/null
+++ b/drivers/gator/gator_trace_gpu.h
@@ -0,0 +1,79 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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.
+ */
+
+#undef TRACE_GPU
+#define TRACE_GPU gpu
+
+#if !defined(_TRACE_GPU_H)
+#define _TRACE_GPU_H
+
+#include <linux/tracepoint.h>
+
+/*
+ * UNIT - the GPU processor type
+ *  1 = Vertex Processor
+ *  2 = Fragment Processor
+ *
+ * CORE - the GPU processor core number
+ *  this is not the CPU core number
+ */
+
+/*
+ * Tracepoint for calling GPU unit start activity on core
+ */
+TRACE_EVENT(gpu_activity_start,
+
+	TP_PROTO(int gpu_unit, int gpu_core, struct task_struct *p),
+
+	TP_ARGS(gpu_unit, gpu_core, p),
+
+	TP_STRUCT__entry(
+		__field(        int,    gpu_unit                )
+		__field(        int,    gpu_core                )
+		__array(        char,   comm,   TASK_COMM_LEN   )
+		__field(        pid_t,  pid                     )
+	),
+
+	TP_fast_assign(
+		__entry->gpu_unit       = gpu_unit;
+		__entry->gpu_core       = gpu_core;
+		memcpy(__entry->comm, p->comm, TASK_COMM_LEN);
+		__entry->pid            = p->pid;
+	),
+
+	TP_printk("unit=%d core=%d comm=%s pid=%d",
+		__entry->gpu_unit, __entry->gpu_core, __entry->comm, __entry->pid)
+);
+
+/*
+ * Tracepoint for calling GPU unit stop activity on core
+ */
+TRACE_EVENT(gpu_activity_stop,
+
+	TP_PROTO(int gpu_unit, int gpu_core),
+
+	TP_ARGS(gpu_unit, gpu_core),
+
+	TP_STRUCT__entry(
+		__field(        int,    gpu_unit                )
+		__field(        int,    gpu_core                )
+	),
+
+	TP_fast_assign(
+		__entry->gpu_unit       = gpu_unit;
+		__entry->gpu_core       = gpu_core;
+	),
+
+	TP_printk("unit=%d core=%d",
+		__entry->gpu_unit, __entry->gpu_core)
+);
+
+#endif /* _TRACE_GPU_H */
+
+/* This part must be outside protection */
+#include <trace/define_trace.h>
diff --git a/drivers/gator/gator_trace_power.c b/drivers/gator/gator_trace_power.c
new file mode 100755
index 00000000000..ca89b19908f
--- /dev/null
+++ b/drivers/gator/gator_trace_power.c
@@ -0,0 +1,160 @@
+/**
+ * Copyright (C) ARM Limited 2011-2012. 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/cpufreq.h>
+#include <trace/events/power.h>
+
+// cpu_frequency and cpu_idle trace points were introduced in Linux kernel v2.6.38
+// the now deprecated power_frequency trace point was available prior to 2.6.38, but only for x86
+#if GATOR_CPU_FREQ_SUPPORT
+enum {
+	POWER_CPU_FREQ,
+	POWER_CPU_IDLE,
+	POWER_TOTAL
+};
+
+static DEFINE_PER_CPU(ulong, idle_prev_state);
+static ulong power_cpu_enabled[POWER_TOTAL];
+static ulong power_cpu_key[POWER_TOTAL];
+
+static int gator_trace_power_create_files(struct super_block *sb, struct dentry *root)
+{
+	struct dentry *dir;
+
+	// cpu_frequency
+	dir = gatorfs_mkdir(sb, root, "Linux_power_cpu_freq");
+	if (!dir) {
+		return -1;
+	}
+	gatorfs_create_ulong(sb, dir, "enabled", &power_cpu_enabled[POWER_CPU_FREQ]);
+	gatorfs_create_ro_ulong(sb, dir, "key", &power_cpu_key[POWER_CPU_FREQ]);
+
+	// cpu_idle
+	dir = gatorfs_mkdir(sb, root, "Linux_power_cpu_idle");
+	if (!dir) {
+		return -1;
+	}
+	gatorfs_create_ulong(sb, dir, "enabled", &power_cpu_enabled[POWER_CPU_IDLE]);
+	gatorfs_create_ro_ulong(sb, dir, "key", &power_cpu_key[POWER_CPU_IDLE]);
+
+	return 0;
+}
+
+// 'cpu' may not equal smp_processor_id(), i.e. may not be running on the core that is having the freq/idle state change
+GATOR_DEFINE_PROBE(cpu_frequency, TP_PROTO(unsigned int frequency, unsigned int cpu))
+{
+	marshal_event_single(cpu, power_cpu_key[POWER_CPU_FREQ], frequency * 1000);
+}
+
+#define WFI_ACTIVE_THRESHOLD 2  // may vary on platform/OS
+#define WFI_EXIT 0
+#define WFI_ENTER 1
+GATOR_DEFINE_PROBE(cpu_idle, TP_PROTO(unsigned int state, unsigned int cpu))
+{
+	// the streamline engine treats all counter values as unsigned
+	if (state & 0x80000000) {
+		state = 0;
+	}
+
+	if (state == per_cpu(idle_prev_state, cpu)) {
+		return;
+	}
+
+	if (state < WFI_ACTIVE_THRESHOLD && per_cpu(idle_prev_state, cpu) >= WFI_ACTIVE_THRESHOLD) {
+		// transition from wfi to non-wfi
+		marshal_wfi(cpu, WFI_EXIT);
+	} else if (state >= WFI_ACTIVE_THRESHOLD && per_cpu(idle_prev_state, cpu) < WFI_ACTIVE_THRESHOLD) {
+		// transition from non-wfi to wfi
+		marshal_wfi(cpu, WFI_ENTER);
+	}
+
+	per_cpu(idle_prev_state, cpu) = state;
+
+	if (power_cpu_enabled[POWER_CPU_IDLE]) {
+		marshal_event_single(cpu, power_cpu_key[POWER_CPU_IDLE], state);
+	}
+}
+
+static void gator_trace_power_online(void)
+{
+	int cpu = smp_processor_id();
+	if (power_cpu_enabled[POWER_CPU_FREQ]) {
+		marshal_event_single(cpu, power_cpu_key[POWER_CPU_FREQ], cpufreq_quick_get(cpu) * 1000);
+	}
+}
+
+static void gator_trace_power_offline(void)
+{
+	// Set frequency to zero on an offline
+	int cpu = smp_processor_id();
+	if (power_cpu_enabled[POWER_CPU_FREQ]) {
+		marshal_event_single(cpu, power_cpu_key[POWER_CPU_FREQ], 0);
+	}
+}
+
+static int gator_trace_power_start(void)
+{
+	int cpu;
+
+	// register tracepoints
+	if (power_cpu_enabled[POWER_CPU_FREQ])
+		if (GATOR_REGISTER_TRACE(cpu_frequency))
+			goto fail_cpu_frequency_exit;
+
+	// Always register for cpu:idle for detecting WFI, independent of power_cpu_enabled[POWER_CPU_IDLE]
+	if (GATOR_REGISTER_TRACE(cpu_idle))
+		goto fail_cpu_idle_exit;
+	pr_debug("gator: registered power event tracepoints\n");
+
+	for_each_present_cpu(cpu) {
+		per_cpu(idle_prev_state, cpu) = 0;
+	}
+
+	return 0;
+
+	// unregister tracepoints on error
+fail_cpu_idle_exit:
+	if (power_cpu_enabled[POWER_CPU_FREQ])
+		GATOR_UNREGISTER_TRACE(cpu_frequency);
+fail_cpu_frequency_exit:
+	pr_err("gator: power event tracepoints failed to activate, please verify that tracepoints are enabled in the linux kernel\n");
+
+	return -1;
+}
+
+static void gator_trace_power_stop(void)
+{
+	int i;
+
+	if (power_cpu_enabled[POWER_CPU_FREQ])
+		GATOR_UNREGISTER_TRACE(cpu_frequency);
+	GATOR_UNREGISTER_TRACE(cpu_idle);
+	pr_debug("gator: unregistered power event tracepoints\n");
+
+	for (i = 0; i < POWER_TOTAL; i++) {
+		power_cpu_enabled[i] = 0;
+	}
+}
+
+void gator_trace_power_init(void)
+{
+	int i;
+	for (i = 0; i < POWER_TOTAL; i++) {
+		power_cpu_enabled[i] = 0;
+		power_cpu_key[i] = gator_events_get_key();
+	}
+}
+#else
+static int gator_trace_power_create_files(struct super_block *sb, struct dentry *root) {return 0;}
+static void gator_trace_power_online(void) {}
+static void gator_trace_power_offline(void) {}
+static int gator_trace_power_start(void) {return 0;}
+static void gator_trace_power_stop(void) {}
+void gator_trace_power_init(void) {}
+#endif
diff --git a/drivers/gator/gator_trace_sched.c b/drivers/gator/gator_trace_sched.c
new file mode 100644
index 00000000000..08b02709485
--- /dev/null
+++ b/drivers/gator/gator_trace_sched.c
@@ -0,0 +1,191 @@
+/**
+ * Copyright (C) ARM Limited 2010-2012. 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 <trace/events/sched.h>
+#include "gator.h"
+
+#define SCHED_SWITCH			1
+#define SCHED_PROCESS_EXIT		2
+
+#define TASK_MAP_ENTRIES		1024		/* must be power of 2 */
+#define TASK_MAX_COLLISIONS		2
+
+static DEFINE_PER_CPU(uint64_t *, taskname_keys);
+
+enum {
+	STATE_WAIT_ON_OTHER = 0,
+	STATE_CONTENTION,
+	STATE_WAIT_ON_IO,
+	STATE_WAIT_ON_MUTEX,
+};
+
+void emit_pid_name(struct task_struct* task)
+{
+	bool found = false;
+	char taskcomm[TASK_COMM_LEN + 3];
+	unsigned long x, cpu = smp_processor_id();
+	uint64_t *keys = &(per_cpu(taskname_keys, cpu)[(task->pid & 0xFF) * TASK_MAX_COLLISIONS]);
+	uint64_t value;
+	
+	value = gator_chksum_crc32(task->comm);
+	value = (value << 32) | (uint32_t)task->pid;
+
+	// determine if the thread name was emitted already
+	for (x = 0; x < TASK_MAX_COLLISIONS; x++) {
+		if (keys[x] == value) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		// shift values, new value always in front
+		uint64_t oldv, newv = value;
+		for (x = 0; x < TASK_MAX_COLLISIONS; x++) {
+			oldv = keys[x];
+			keys[x] = newv;
+			newv = oldv;
+		}
+
+		// emit pid names, cannot use get_task_comm, as it's not exported on all kernel versions
+		if (strlcpy(taskcomm, task->comm, TASK_COMM_LEN) == TASK_COMM_LEN - 1) {
+			// append ellipses if task->comm has length of TASK_COMM_LEN - 1
+			strcat(taskcomm, "...");
+		}
+
+		marshal_pid_name(task->pid, taskcomm);
+	}
+}
+
+
+static void collect_counters(void)
+{
+	int *buffer, len;
+	long long *buffer64;
+	struct gator_interface *gi;
+
+	if (marshal_event_header()) {
+		list_for_each_entry(gi, &gator_events, list) {
+			if (gi->read) {
+				len = gi->read(&buffer);
+				marshal_event(len, buffer);
+			} else if (gi->read64) {
+				len = gi->read64(&buffer64);
+				marshal_event64(len, buffer64);
+			}
+		}
+	}
+}
+
+static void probe_sched_write(int type, struct task_struct* task, struct task_struct* old_task)
+{
+	int cookie = 0, state = 0;
+	int cpu = smp_processor_id();
+	int pid = task->pid;
+	int tgid = task->tgid;
+
+	if (type == SCHED_SWITCH) {
+		// do as much work as possible before disabling interrupts
+		cookie = get_exec_cookie(cpu, BACKTRACE_BUF, task);
+		emit_pid_name(task);
+		if (old_task->state == TASK_RUNNING) {
+			state = STATE_CONTENTION;
+		} else if (old_task->in_iowait) {
+			state = STATE_WAIT_ON_IO;
+#ifdef CONFIG_DEBUG_MUTEXES
+		} else if (old_task->blocked_on) {
+			state = STATE_WAIT_ON_MUTEX;
+#endif
+		} else {
+			state = STATE_WAIT_ON_OTHER;
+		}
+
+		collect_counters();
+	}
+
+	// marshal_sched_trace() disables interrupts as the free may trigger while switch is writing to the buffer; disabling preemption is not sufficient
+	// is disable interrupts necessary now that exit is used instead of free?
+	marshal_sched_trace(type, pid, tgid, cookie, state);
+}
+
+// special case used during a suspend of the system
+static void trace_sched_insert_idle(void)
+{
+	marshal_sched_trace(SCHED_SWITCH, 0, 0, 0, 0);
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 35)
+GATOR_DEFINE_PROBE(sched_switch, TP_PROTO(struct rq *rq, struct task_struct *prev, struct task_struct *next))
+#else
+GATOR_DEFINE_PROBE(sched_switch, TP_PROTO(struct task_struct *prev, struct task_struct *next))
+#endif
+{
+	probe_sched_write(SCHED_SWITCH, next, prev);
+}
+
+GATOR_DEFINE_PROBE(sched_process_exit, TP_PROTO(struct task_struct *p))
+{
+	probe_sched_write(SCHED_PROCESS_EXIT, p, 0);
+}
+
+static int register_scheduler_tracepoints(void) {
+	// register tracepoints
+	if (GATOR_REGISTER_TRACE(sched_switch))
+		goto fail_sched_switch;
+	if (GATOR_REGISTER_TRACE(sched_process_exit))
+		goto fail_sched_process_exit;
+	pr_debug("gator: registered tracepoints\n");
+
+	return 0;
+
+	// unregister tracepoints on error
+fail_sched_process_exit:
+	GATOR_UNREGISTER_TRACE(sched_switch);
+fail_sched_switch:
+	pr_err("gator: tracepoints failed to activate, please verify that tracepoints are enabled in the linux kernel\n");
+
+	return -1;
+}
+
+int gator_trace_sched_start(void)
+{
+	int cpu, size;
+
+	for_each_present_cpu(cpu) {
+		size = TASK_MAP_ENTRIES * TASK_MAX_COLLISIONS * sizeof(uint64_t);
+		per_cpu(taskname_keys, cpu) = (uint64_t*)kmalloc(size, GFP_KERNEL);
+		if (!per_cpu(taskname_keys, cpu))
+			return -1;
+		memset(per_cpu(taskname_keys, cpu), 0, size);
+	}
+
+	return register_scheduler_tracepoints();
+}
+
+void gator_trace_sched_offline(void)
+{
+	trace_sched_insert_idle();
+}
+
+static void unregister_scheduler_tracepoints(void)
+{
+	GATOR_UNREGISTER_TRACE(sched_switch);
+	GATOR_UNREGISTER_TRACE(sched_process_exit);
+	pr_debug("gator: unregistered tracepoints\n");
+}
+
+void gator_trace_sched_stop(void)
+{
+	int cpu;
+	unregister_scheduler_tracepoints();
+
+	for_each_present_cpu(cpu) {
+		kfree(per_cpu(taskname_keys, cpu));
+	}
+}
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index e03653d6935..6226d0cd30a 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -504,7 +504,7 @@ config GPIO_JANZ_TTL
 
 config GPIO_AB8500
 	bool "ST-Ericsson AB8500 Mixed Signal Circuit gpio functions"
-	depends on AB8500_CORE && BROKEN
+	depends on AB8500_CORE
 	help
 	  Select this to enable the AB8500 IC GPIO driver
 
diff --git a/drivers/gpio/gpio-ab8500.c b/drivers/gpio/gpio-ab8500.c
index 050c05d9189..ef75984486c 100644
--- a/drivers/gpio/gpio-ab8500.c
+++ b/drivers/gpio/gpio-ab8500.c
@@ -18,9 +18,16 @@
 #include <linux/gpio.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
-#include <linux/mfd/ab8500.h>
 #include <linux/mfd/abx500.h>
-#include <linux/mfd/ab8500/gpio.h>
+#include <linux/mfd/abx500/ab8500-gpio.h>
+
+
+/*
+ * The AB9540 GPIO support is an extended version of the
+ * AB8500 GPIO support. The AB9540 supports an additional
+ * (7th) register so that more GPIO may be configured and
+ * used.
+ */
 
 /*
  * GPIO registers offset
@@ -32,6 +39,7 @@
 #define AB8500_GPIO_SEL4_REG	0x03
 #define AB8500_GPIO_SEL5_REG	0x04
 #define AB8500_GPIO_SEL6_REG	0x05
+#define AB9540_GPIO_SEL7_REG	0x06
 
 #define AB8500_GPIO_DIR1_REG	0x10
 #define AB8500_GPIO_DIR2_REG	0x11
@@ -39,6 +47,7 @@
 #define AB8500_GPIO_DIR4_REG	0x13
 #define AB8500_GPIO_DIR5_REG	0x14
 #define AB8500_GPIO_DIR6_REG	0x15
+#define AB9540_GPIO_DIR7_REG	0x16
 
 #define AB8500_GPIO_OUT1_REG	0x20
 #define AB8500_GPIO_OUT2_REG	0x21
@@ -46,6 +55,7 @@
 #define AB8500_GPIO_OUT4_REG	0x23
 #define AB8500_GPIO_OUT5_REG	0x24
 #define AB8500_GPIO_OUT6_REG	0x25
+#define AB9540_GPIO_OUT7_REG	0x26
 
 #define AB8500_GPIO_PUD1_REG	0x30
 #define AB8500_GPIO_PUD2_REG	0x31
@@ -53,6 +63,7 @@
 #define AB8500_GPIO_PUD4_REG	0x33
 #define AB8500_GPIO_PUD5_REG	0x34
 #define AB8500_GPIO_PUD6_REG	0x35
+#define AB9540_GPIO_PUD7_REG	0x36
 
 #define AB8500_GPIO_IN1_REG	0x40
 #define AB8500_GPIO_IN2_REG	0x41
@@ -60,9 +71,13 @@
 #define AB8500_GPIO_IN4_REG	0x43
 #define AB8500_GPIO_IN5_REG	0x44
 #define AB8500_GPIO_IN6_REG	0x45
-#define AB8500_GPIO_ALTFUN_REG	0x45
-#define ALTFUN_REG_INDEX	6
+#define AB9540_GPIO_IN7_REG	0x46
+#define AB8500_GPIO_ALTFUN_REG	0x50
+#define AB8500_ALTFUN_REG_INDEX	6
+#define AB9540_ALTFUN_REG_INDEX	7
 #define AB8500_NUM_GPIO		42
+#define AB9540_NUM_GPIO		54
+#define AB8505_NUM_GPIO		53
 #define AB8500_NUM_VIR_GPIO_IRQ	16
 
 enum ab8500_gpio_action {
@@ -73,6 +88,11 @@ enum ab8500_gpio_action {
 	UNMASK
 };
 
+struct ab8500_gpio_irq_cluster {
+	int start;
+	int end;
+};
+
 struct ab8500_gpio {
 	struct gpio_chip chip;
 	struct ab8500 *parent;
@@ -82,7 +102,50 @@ struct ab8500_gpio {
 	enum ab8500_gpio_action irq_action;
 	u16 rising;
 	u16 falling;
+	struct ab8500_gpio_irq_cluster *irq_cluster;
+	int irq_cluster_size;
+};
+
+/*
+ * Only some GPIOs are interrupt capable, and they are
+ * organized in discontiguous clusters:
+ *
+ *	GPIO6 to GPIO13
+ *	GPIO24 and GPIO25
+ *	GPIO36 to GPIO41
+ *	GPIO50 to GPIO54 (AB9540 only)
+ */
+static struct ab8500_gpio_irq_cluster ab8500_irq_clusters[] = {
+	{.start = 5,  .end = 12}, /* GPIO numbers start from 1 */
+	{.start = 23, .end = 24},
+	{.start = 35, .end = 40},
+};
+
+static struct ab8500_gpio_irq_cluster ab9540_irq_clusters[] = {
+	{.start = 5,  .end = 12}, /* GPIO numbers start from 1 */
+	{.start = 23, .end = 24},
+	{.start = 35, .end = 40},
+	{.start = 49, .end = 53},
+};
+
+/*
+ * For AB8505 Only some GPIOs are interrupt capable, and they are
+ * organized in discontiguous clusters:
+ *
+ *	GPIO10 to GPIO11
+ *	GPIO13
+ *	GPIO40 and GPIO41
+ *	GPIO50
+ *	GPIO52 to GPIO53
+ */
+static struct ab8500_gpio_irq_cluster ab8505_irq_clusters[] = {
+	{.start = 9,  .end = 10}, /* GPIO numbers start from 1 */
+	{.start = 12, .end = 12},
+	{.start = 39, .end = 40},
+	{.start = 49, .end = 49},
+	{.start = 51, .end = 52},
 };
+
 /**
  * to_ab8500_gpio() - get the pointer to ab8500_gpio
  * @chip:	Member of the structure ab8500_gpio
@@ -115,7 +178,7 @@ static int ab8500_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	struct ab8500_gpio *ab8500_gpio = to_ab8500_gpio(chip);
 	u8 mask = 1 << (offset % 8);
-	u8 reg = AB8500_GPIO_OUT1_REG + (offset / 8);
+	u8 reg = AB8500_GPIO_IN1_REG + (offset / 8);
 	int ret;
 	u8 data;
 	ret = abx500_get_register_interruptible(ab8500_gpio->dev, AB8500_MISC,
@@ -132,7 +195,7 @@ static void ab8500_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 	struct ab8500_gpio *ab8500_gpio = to_ab8500_gpio(chip);
 	int ret;
 	/* Write the data */
-	ret = ab8500_gpio_set_bits(chip, AB8500_GPIO_OUT1_REG, offset, 1);
+	ret = ab8500_gpio_set_bits(chip, AB8500_GPIO_OUT1_REG, offset, val);
 	if (ret < 0)
 		dev_err(ab8500_gpio->dev, "%s write failed\n", __func__);
 }
@@ -162,28 +225,13 @@ static int ab8500_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 
 static int ab8500_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	/*
-	 * Only some GPIOs are interrupt capable, and they are
-	 * organized in discontiguous clusters:
-	 *
-	 *	GPIO6 to GPIO13
-	 *	GPIO24 and GPIO25
-	 *	GPIO36 to GPIO41
-	 */
-	static struct ab8500_gpio_irq_cluster {
-		int start;
-		int end;
-	} clusters[] = {
-		{.start = 6,  .end = 13},
-		{.start = 24, .end = 25},
-		{.start = 36, .end = 41},
-	};
 	struct ab8500_gpio *ab8500_gpio = to_ab8500_gpio(chip);
 	int base = ab8500_gpio->irq_base;
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(clusters); i++) {
-		struct ab8500_gpio_irq_cluster *cluster = &clusters[i];
+	for (i = 0; i < ab8500_gpio->irq_cluster_size; i++) {
+		struct ab8500_gpio_irq_cluster *cluster =
+			&ab8500_gpio->irq_cluster[i];
 
 		if (offset >= cluster->start && offset <= cluster->end)
 			return base + offset - cluster->start;
@@ -207,7 +255,7 @@ static struct gpio_chip ab8500gpio_chip = {
 
 static unsigned int irq_to_rising(unsigned int irq)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_get_chip_data(irq);
 	int offset = irq - ab8500_gpio->irq_base;
 	int new_irq = offset +  AB8500_INT_GPIO6R
 			+ ab8500_gpio->parent->irq_base;
@@ -216,7 +264,7 @@ static unsigned int irq_to_rising(unsigned int irq)
 
 static unsigned int irq_to_falling(unsigned int irq)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_get_chip_data(irq);
 	int offset = irq - ab8500_gpio->irq_base;
 	int new_irq = offset +  AB8500_INT_GPIO6F
 			+  ab8500_gpio->parent->irq_base;
@@ -261,15 +309,16 @@ static irqreturn_t handle_falling(int irq, void *dev)
 	return IRQ_HANDLED;
 }
 
-static void ab8500_gpio_irq_lock(unsigned int irq)
+static void ab8500_gpio_irq_lock(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
 	mutex_lock(&ab8500_gpio->lock);
 }
 
-static void ab8500_gpio_irq_sync_unlock(unsigned int irq)
+static void ab8500_gpio_irq_sync_unlock(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
+	unsigned int irq = data->irq;
 	int offset = irq - ab8500_gpio->irq_base;
 	bool rising = ab8500_gpio->rising & BIT(offset);
 	bool falling = ab8500_gpio->falling & BIT(offset);
@@ -280,12 +329,12 @@ static void ab8500_gpio_irq_sync_unlock(unsigned int irq)
 		if (rising)
 			ret = request_threaded_irq(irq_to_rising(irq),
 					NULL, handle_rising,
-					IRQF_TRIGGER_RISING,
+					IRQF_TRIGGER_RISING | IRQF_NO_SUSPEND,
 					"ab8500-gpio-r", ab8500_gpio);
 		if (falling)
 			ret = request_threaded_irq(irq_to_falling(irq),
 				       NULL, handle_falling,
-				       IRQF_TRIGGER_FALLING,
+				       IRQF_TRIGGER_FALLING | IRQF_NO_SUSPEND,
 				       "ab8500-gpio-f", ab8500_gpio);
 		break;
 	case SHUTDOWN:
@@ -316,21 +365,22 @@ static void ab8500_gpio_irq_sync_unlock(unsigned int irq)
 }
 
 
-static void ab8500_gpio_irq_mask(unsigned int irq)
+static void ab8500_gpio_irq_mask(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
 	ab8500_gpio->irq_action = MASK;
 }
 
-static void ab8500_gpio_irq_unmask(unsigned int irq)
+static void ab8500_gpio_irq_unmask(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio =  irq_data_get_irq_chip_data(data);
 	ab8500_gpio->irq_action = UNMASK;
 }
 
-static int ab8500_gpio_irq_set_type(unsigned int irq, unsigned int type)
+static int ab8500_gpio_irq_set_type(struct irq_data *data, unsigned int type)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
+	unsigned int irq = data->irq;
 	int offset = irq - ab8500_gpio->irq_base;
 
 	if (type == IRQ_TYPE_EDGE_BOTH) {
@@ -344,28 +394,28 @@ static int ab8500_gpio_irq_set_type(unsigned int irq, unsigned int type)
 	return 0;
 }
 
-unsigned int ab8500_gpio_irq_startup(unsigned int irq)
+unsigned int ab8500_gpio_irq_startup(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
 	ab8500_gpio->irq_action = STARTUP;
 	return 0;
 }
 
-void ab8500_gpio_irq_shutdown(unsigned int irq)
+void ab8500_gpio_irq_shutdown(struct irq_data *data)
 {
-	struct ab8500_gpio *ab8500_gpio = get_irq_chip_data(irq);
+	struct ab8500_gpio *ab8500_gpio = irq_data_get_irq_chip_data(data);
 	ab8500_gpio->irq_action = SHUTDOWN;
 }
 
 static struct irq_chip ab8500_gpio_irq_chip = {
 	.name			= "ab8500-gpio",
-	.startup		= ab8500_gpio_irq_startup,
-	.shutdown		= ab8500_gpio_irq_shutdown,
-	.bus_lock		= ab8500_gpio_irq_lock,
-	.bus_sync_unlock	= ab8500_gpio_irq_sync_unlock,
-	.mask			= ab8500_gpio_irq_mask,
-	.unmask			= ab8500_gpio_irq_unmask,
-	.set_type		= ab8500_gpio_irq_set_type,
+	.irq_startup		= ab8500_gpio_irq_startup,
+	.irq_shutdown		= ab8500_gpio_irq_shutdown,
+	.irq_bus_lock		= ab8500_gpio_irq_lock,
+	.irq_bus_sync_unlock	= ab8500_gpio_irq_sync_unlock,
+	.irq_mask		= ab8500_gpio_irq_mask,
+	.irq_unmask		= ab8500_gpio_irq_unmask,
+	.irq_set_type		= ab8500_gpio_irq_set_type,
 };
 
 static int ab8500_gpio_irq_init(struct ab8500_gpio *ab8500_gpio)
@@ -374,14 +424,14 @@ static int ab8500_gpio_irq_init(struct ab8500_gpio *ab8500_gpio)
 	int irq;
 
 	for (irq = base; irq < base + AB8500_NUM_VIR_GPIO_IRQ ; irq++) {
-		set_irq_chip_data(irq, ab8500_gpio);
-		set_irq_chip_and_handler(irq, &ab8500_gpio_irq_chip,
+		irq_set_chip_data(irq, ab8500_gpio);
+		irq_set_chip_and_handler(irq, &ab8500_gpio_irq_chip,
 				handle_simple_irq);
-		set_irq_nested_thread(irq, 1);
+		irq_set_nested_thread(irq, 1);
 #ifdef CONFIG_ARM
 		set_irq_flags(irq, IRQF_VALID);
 #else
-		set_irq_noprobe(irq);
+		irq_set_noprobe(irq);
 #endif
 	}
 
@@ -397,19 +447,22 @@ static void ab8500_gpio_irq_remove(struct ab8500_gpio *ab8500_gpio)
 #ifdef CONFIG_ARM
 		set_irq_flags(irq, 0);
 #endif
-		set_irq_chip_and_handler(irq, NULL, NULL);
-		set_irq_chip_data(irq, NULL);
+		irq_set_chip_and_handler(irq, NULL, NULL);
+		irq_set_chip_data(irq, NULL);
 	}
 }
 
 static int __devinit ab8500_gpio_probe(struct platform_device *pdev)
 {
+	struct ab8500 *parent = dev_get_drvdata(pdev->dev.parent);
 	struct ab8500_platform_data *ab8500_pdata =
 				dev_get_platdata(pdev->dev.parent);
 	struct ab8500_gpio_platform_data *pdata;
 	struct ab8500_gpio *ab8500_gpio;
 	int ret;
 	int i;
+	int last_gpio_sel_reg;
+	int altfun_reg_index;
 
 	pdata = ab8500_pdata->gpio;
 	if (!pdata)	{
@@ -425,10 +478,36 @@ static int __devinit ab8500_gpio_probe(struct platform_device *pdev)
 	ab8500_gpio->dev = &pdev->dev;
 	ab8500_gpio->parent = dev_get_drvdata(pdev->dev.parent);
 	ab8500_gpio->chip = ab8500gpio_chip;
-	ab8500_gpio->chip.ngpio = AB8500_NUM_GPIO;
 	ab8500_gpio->chip.dev = &pdev->dev;
 	ab8500_gpio->chip.base = pdata->gpio_base;
 	ab8500_gpio->irq_base = pdata->irq_base;
+
+	/* Configure GPIO Settings for specific AB devices */
+	if (cpu_is_u9540()) {
+		ab8500_gpio->chip.ngpio = AB9540_NUM_GPIO;
+		ab8500_gpio->irq_cluster = ab9540_irq_clusters;
+		ab8500_gpio->irq_cluster_size =
+			ARRAY_SIZE(ab9540_irq_clusters);
+		last_gpio_sel_reg = AB9540_GPIO_SEL7_REG;
+		altfun_reg_index = AB9540_ALTFUN_REG_INDEX;
+	} else {
+		if (is_ab8505(parent)) {
+			ab8500_gpio->chip.ngpio = AB8505_NUM_GPIO;
+			ab8500_gpio->irq_cluster = ab8505_irq_clusters;
+			ab8500_gpio->irq_cluster_size =
+				ARRAY_SIZE(ab8505_irq_clusters);
+			last_gpio_sel_reg = AB9540_GPIO_SEL7_REG;
+			altfun_reg_index = AB9540_ALTFUN_REG_INDEX;
+		} else {
+			ab8500_gpio->chip.ngpio = AB8500_NUM_GPIO;
+			ab8500_gpio->irq_cluster = ab8500_irq_clusters;
+			ab8500_gpio->irq_cluster_size =
+				ARRAY_SIZE(ab8500_irq_clusters);
+			last_gpio_sel_reg = AB8500_GPIO_SEL6_REG;
+			altfun_reg_index = AB8500_ALTFUN_REG_INDEX;
+		}
+	}
+
 	/* initialize the lock */
 	mutex_init(&ab8500_gpio->lock);
 	/*
@@ -437,16 +516,28 @@ static int __devinit ab8500_gpio_probe(struct platform_device *pdev)
 	 * These values are for selecting the PINs as
 	 * GPIO or alternate function
 	 */
-	for (i = AB8500_GPIO_SEL1_REG; i <= AB8500_GPIO_SEL6_REG; i++)	{
+	for (i = AB8500_GPIO_SEL1_REG; i <= last_gpio_sel_reg; i++) {
 		ret = abx500_set_register_interruptible(ab8500_gpio->dev,
 				AB8500_MISC, i,
 				pdata->config_reg[i]);
 		if (ret < 0)
 			goto out_free;
+
+		ret = abx500_set_register_interruptible(ab8500_gpio->dev,
+				AB8500_MISC, i + AB8500_GPIO_DIR1_REG,
+				pdata->config_direction[i]);
+		if (ret < 0)
+			goto out_free;
+
+		ret = abx500_set_register_interruptible(ab8500_gpio->dev,
+				AB8500_MISC, i + AB8500_GPIO_PUD1_REG,
+				pdata->config_pullups[i]);
+		if (ret < 0)
+			goto out_free;
 	}
 	ret = abx500_set_register_interruptible(ab8500_gpio->dev, AB8500_MISC,
 				AB8500_GPIO_ALTFUN_REG,
-				pdata->config_reg[ALTFUN_REG_INDEX]);
+				pdata->config_reg[altfun_reg_index]);
 	if (ret < 0)
 		goto out_free;
 
@@ -493,6 +584,86 @@ static int __devexit ab8500_gpio_remove(struct platform_device *pdev)
 	return 0;
 }
 
+int ab8500_config_pulldown(struct device *dev,
+				enum ab8500_pin gpio, bool enable)
+{
+	u8 offset =  gpio - AB8500_PIN_GPIO1;
+	u8 pos = offset % 8;
+	u8 val = enable ? 0 : 1;
+	u8 reg = AB8500_GPIO_PUD1_REG + (offset / 8);
+	int ret;
+
+	ret = abx500_mask_and_set_register_interruptible(dev,
+				AB8500_MISC, reg, 1 << pos, val << pos);
+	if (ret < 0)
+		dev_err(dev, "%s write failed\n", __func__);
+	return ret;
+}
+EXPORT_SYMBOL(ab8500_config_pulldown);
+
+/*
+ * ab8500_gpio_config_select()
+ *
+ * Configure functionality of pin, either specific use or GPIO.
+ * @dev: device pointer
+ * @gpio: gpio number
+ * @gpio_select: true if the pin should be used as GPIO
+ */
+int ab8500_gpio_config_select(struct device *dev,
+				enum ab8500_pin gpio, bool gpio_select)
+{
+	u8 offset = gpio - AB8500_PIN_GPIO1;
+	u8 reg = AB8500_GPIO_SEL1_REG + (offset / 8);
+	u8 pos = offset % 8;
+	u8 val = gpio_select ? 1 : 0;
+	int ret;
+
+	ret = abx500_mask_and_set_register_interruptible(dev,
+		AB8500_MISC, reg, 1 << pos, val << pos);
+	if (ret < 0)
+		dev_err(dev, "%s write failed\n", __func__);
+
+	dev_vdbg(dev, "%s (bank, addr, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
+		__func__, AB8500_MISC, reg, 1 << pos, val << pos);
+
+	return ret;
+}
+
+/*
+ * ab8500_gpio_config_get_select()
+ *
+ * Read currently configured functionality, either specific use or GPIO.
+ * @dev: device pointer
+ * @gpio: gpio number
+ * @gpio_select: pointer to pin selection status
+ */
+int ab8500_gpio_config_get_select(struct device *dev,
+					enum ab8500_pin gpio, bool *gpio_select)
+{
+	u8 offset =  gpio - AB8500_PIN_GPIO1;
+	u8 reg = AB8500_GPIO_SEL1_REG + (offset / 8);
+	u8 pos = offset % 8;
+	u8 val;
+	int ret;
+
+	ret = abx500_get_register_interruptible(dev,
+				AB8500_MISC, reg, &val);
+	if (ret < 0) {
+		dev_err(dev, "%s read failed\n", __func__);
+		return ret;
+	}
+
+	if (val & (1 << pos))
+		*gpio_select = true;
+	else
+		*gpio_select = false;
+
+	dev_vdbg(dev, "%s (bank, addr, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
+		__func__, AB8500_MISC, reg, 1 << pos, val);
+
+	return 0;
+}
+
 static struct platform_driver ab8500_gpio_driver = {
 	.driver = {
 		.name = "ab8500-gpio",
diff --git a/drivers/gpio/gpio-nomadik.c b/drivers/gpio/gpio-nomadik.c
index 839624f9fe6..05547ed48ff 100644
--- a/drivers/gpio/gpio-nomadik.c
+++ b/drivers/gpio/gpio-nomadik.c
@@ -23,12 +23,11 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
+#include <linux/gpio/nomadik.h>
 
 #include <asm/mach/irq.h>
 
 #include <plat/pincfg.h>
-#include <plat/gpio-nomadik.h>
-#include <mach/hardware.h>
 #include <asm/gpio.h>
 
 /*
@@ -58,8 +57,11 @@ struct nmk_gpio_chip {
 	u32 real_wake;
 	u32 rwimsc;
 	u32 fwimsc;
+	u32 rimsc;
+	u32 fimsc;
 	u32 slpm;
 	u32 pull_up;
+	u32 lowemi;
 };
 
 static struct nmk_gpio_chip *
@@ -124,6 +126,24 @@ static void __nmk_gpio_set_pull(struct nmk_gpio_chip *nmk_chip,
 	}
 }
 
+static void __nmk_gpio_set_lowemi(struct nmk_gpio_chip *nmk_chip,
+				  unsigned offset, bool lowemi)
+{
+	u32 bit = BIT(offset);
+	bool enabled = nmk_chip->lowemi & bit;
+
+	if (lowemi == enabled)
+		return;
+
+	if (lowemi)
+		nmk_chip->lowemi |= bit;
+	else
+		nmk_chip->lowemi &= ~bit;
+
+	writel_relaxed(nmk_chip->lowemi,
+		       nmk_chip->addr + NMK_GPIO_LOWEMI);
+}
+
 static void __nmk_gpio_make_input(struct nmk_gpio_chip *nmk_chip,
 				  unsigned offset)
 {
@@ -150,8 +170,8 @@ static void __nmk_gpio_set_mode_safe(struct nmk_gpio_chip *nmk_chip,
 				     unsigned offset, int gpio_mode,
 				     bool glitch)
 {
-	u32 rwimsc = readl(nmk_chip->addr + NMK_GPIO_RWIMSC);
-	u32 fwimsc = readl(nmk_chip->addr + NMK_GPIO_FWIMSC);
+	u32 rwimsc = nmk_chip->rwimsc;
+	u32 fwimsc = nmk_chip->fwimsc;
 
 	if (glitch && nmk_chip->set_ioforce) {
 		u32 bit = BIT(offset);
@@ -173,6 +193,36 @@ static void __nmk_gpio_set_mode_safe(struct nmk_gpio_chip *nmk_chip,
 	}
 }
 
+static void
+nmk_gpio_disable_lazy_irq(struct nmk_gpio_chip *nmk_chip, unsigned offset)
+{
+	u32 falling = nmk_chip->fimsc & BIT(offset);
+	u32 rising = nmk_chip->rimsc & BIT(offset);
+	int gpio = nmk_chip->chip.base + offset;
+	int irq = NOMADIK_GPIO_TO_IRQ(gpio);
+	struct irq_data *d = irq_get_irq_data(irq);
+
+	if (!rising && !falling)
+		return;
+
+	if (!d || !irqd_irq_disabled(d))
+		return;
+
+	if (rising) {
+		nmk_chip->rimsc &= ~BIT(offset);
+		writel_relaxed(nmk_chip->rimsc,
+			       nmk_chip->addr + NMK_GPIO_RIMSC);
+	}
+
+	if (falling) {
+		nmk_chip->fimsc &= ~BIT(offset);
+		writel_relaxed(nmk_chip->fimsc,
+			       nmk_chip->addr + NMK_GPIO_FIMSC);
+	}
+
+	dev_dbg(nmk_chip->chip.dev, "%d: clearing interrupt mask\n", gpio);
+}
+
 static void __nmk_config_pin(struct nmk_gpio_chip *nmk_chip, unsigned offset,
 			     pin_cfg_t cfg, bool sleep, unsigned int *slpmregs)
 {
@@ -238,6 +288,17 @@ static void __nmk_config_pin(struct nmk_gpio_chip *nmk_chip, unsigned offset,
 		__nmk_gpio_set_pull(nmk_chip, offset, pull);
 	}
 
+	__nmk_gpio_set_lowemi(nmk_chip, offset, PIN_LOWEMI(cfg));
+
+	/*
+	 * If the pin is switching to altfunc, and there was an interrupt
+	 * installed on it which has been lazy disabled, actually mask the
+	 * interrupt to prevent spurious interrupts that would occur while the
+	 * pin is under control of the peripheral.  Only SKE does this.
+	 */
+	if (af != NMK_GPIO_ALT_GPIO)
+		nmk_gpio_disable_lazy_irq(nmk_chip, offset);
+
 	/*
 	 * If we've backed up the SLPM registers (glitch workaround), modify
 	 * the backups since they will be restored.
@@ -359,7 +420,7 @@ static int __nmk_config_pins(pin_cfg_t *cfgs, int num, bool sleep)
 /**
  * nmk_config_pin - configure a pin's mux attributes
  * @cfg: pin confguration
- *
+ * @sleep: Non-zero to apply the sleep mode configuration
  * Configures a pin's mode (alternate function or GPIO), its pull up status,
  * and its sleep mode based on the specified configuration.  The @cfg is
  * usually one of the SoC specific macros defined in mach/<soc>-pins.h.  These
@@ -556,27 +617,38 @@ static void __nmk_gpio_irq_modify(struct nmk_gpio_chip *nmk_chip,
 				  int gpio, enum nmk_gpio_irq_type which,
 				  bool enable)
 {
-	u32 rimsc = which == WAKE ? NMK_GPIO_RWIMSC : NMK_GPIO_RIMSC;
-	u32 fimsc = which == WAKE ? NMK_GPIO_FWIMSC : NMK_GPIO_FIMSC;
 	u32 bitmask = nmk_gpio_get_bitmask(gpio);
-	u32 reg;
+	u32 *rimscval;
+	u32 *fimscval;
+	u32 rimscreg;
+	u32 fimscreg;
+
+	if (which == NORMAL) {
+		rimscreg = NMK_GPIO_RIMSC;
+		fimscreg = NMK_GPIO_FIMSC;
+		rimscval = &nmk_chip->rimsc;
+		fimscval = &nmk_chip->fimsc;
+	} else  {
+		rimscreg = NMK_GPIO_RWIMSC;
+		fimscreg = NMK_GPIO_FWIMSC;
+		rimscval = &nmk_chip->rwimsc;
+		fimscval = &nmk_chip->fwimsc;
+	}
 
 	/* we must individually set/clear the two edges */
 	if (nmk_chip->edge_rising & bitmask) {
-		reg = readl(nmk_chip->addr + rimsc);
 		if (enable)
-			reg |= bitmask;
+			*rimscval |= bitmask;
 		else
-			reg &= ~bitmask;
-		writel(reg, nmk_chip->addr + rimsc);
+			*rimscval &= ~bitmask;
+		writel(*rimscval, nmk_chip->addr + rimscreg);
 	}
 	if (nmk_chip->edge_falling & bitmask) {
-		reg = readl(nmk_chip->addr + fimsc);
 		if (enable)
-			reg |= bitmask;
+			*fimscval |= bitmask;
 		else
-			reg &= ~bitmask;
-		writel(reg, nmk_chip->addr + fimsc);
+			*fimscval &= ~bitmask;
+		writel(*fimscval, nmk_chip->addr + fimscreg);
 	}
 }
 
@@ -1008,9 +1080,6 @@ void nmk_gpio_wakeups_suspend(void)
 
 		clk_enable(chip->clk);
 
-		chip->rwimsc = readl(chip->addr + NMK_GPIO_RWIMSC);
-		chip->fwimsc = readl(chip->addr + NMK_GPIO_FWIMSC);
-
 		writel(chip->rwimsc & chip->real_wake,
 		       chip->addr + NMK_GPIO_RWIMSC);
 		writel(chip->fwimsc & chip->real_wake,
@@ -1076,6 +1145,7 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 	struct resource *res;
 	struct clk *clk;
 	int secondary_irq;
+	void __iomem *base;
 	int irq;
 	int ret;
 
@@ -1106,10 +1176,16 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 		goto out;
 	}
 
+	base = ioremap(res->start, resource_size(res));
+	if (!base) {
+		ret = -ENOMEM;
+		goto out_release;
+	}
+
 	clk = clk_get(&dev->dev, NULL);
 	if (IS_ERR(clk)) {
 		ret = PTR_ERR(clk);
-		goto out_release;
+		goto out_unmap;
 	}
 
 	nmk_chip = kzalloc(sizeof(*nmk_chip), GFP_KERNEL);
@@ -1123,7 +1199,7 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 	 */
 	nmk_chip->bank = dev->id;
 	nmk_chip->clk = clk;
-	nmk_chip->addr = io_p2v(res->start);
+	nmk_chip->addr = base;
 	nmk_chip->chip = nmk_gpio_template;
 	nmk_chip->parent_irq = irq;
 	nmk_chip->secondary_parent_irq = secondary_irq;
@@ -1139,6 +1215,10 @@ static int __devinit nmk_gpio_probe(struct platform_device *dev)
 	chip->dev = &dev->dev;
 	chip->owner = THIS_MODULE;
 
+	clk_enable(nmk_chip->clk);
+	nmk_chip->lowemi = readl_relaxed(nmk_chip->addr + NMK_GPIO_LOWEMI);
+	clk_disable(nmk_chip->clk);
+
 	ret = gpiochip_add(&nmk_chip->chip);
 	if (ret)
 		goto out_free;
@@ -1159,6 +1239,8 @@ out_free:
 out_clk:
 	clk_disable(clk);
 	clk_put(clk);
+out_unmap:
+	iounmap(base);
 out_release:
 	release_mem_region(res->start, resource_size(res));
 out:
diff --git a/drivers/gpu/Makefile b/drivers/gpu/Makefile
index cc9277885dd..b2ae09b4cc7 100644
--- a/drivers/gpu/Makefile
+++ b/drivers/gpu/Makefile
@@ -1 +1,2 @@
 obj-y			+= drm/ vga/ stub/
+obj-y  			+= mali/
diff --git a/drivers/gpu/mali/Kconfig b/drivers/gpu/mali/Kconfig
new file mode 100644
index 00000000000..0781bcbd854
--- /dev/null
+++ b/drivers/gpu/mali/Kconfig
@@ -0,0 +1,15 @@
+config GPU_MALI
+       tristate "ARM Mali 200/300/400 support"
+       depends on ARM
+       ---help---
+       This enables support for the ARM Mali 200/300/400 family of GPUs.
+       Platform specific configuration is made in configuration files in the
+       drivers/gpu/mali/src folder
+
+config GPU_MALI_DEBUG
+       boolean "Debug mode (required for instrumentation)"
+       default y
+       depends on GPU_MALI
+       ---help---
+       This enables debug prints in the mali device driver. Debug mode must be
+       enabled in order to use the intrumentation feature of the mali libraries.
diff --git a/drivers/gpu/mali/Makefile b/drivers/gpu/mali/Makefile
new file mode 100644
index 00000000000..93c95aca19a
--- /dev/null
+++ b/drivers/gpu/mali/Makefile
@@ -0,0 +1,10 @@
+MALI_SUBFOLDER := mali400ko/driver/src/devicedrv/mali
+MALIDRM_SUBFOLDER := mali400ko/x11/mali_drm/mali
+MALI_FOLDER := $(srctree)/$(src)/$(MALI_SUBFOLDER)
+ifeq ($(shell [ -d $(MALI_FOLDER) ] && echo "OK"), OK)
+obj-$(CONFIG_GPU_MALI)  += $(MALI_SUBFOLDER)/
+obj-y			+= $(MALIDRM_SUBFOLDER)/
+else
+$(warning WARNING: mali: Could not find $(MALI_FOLDER) - mali device driver will not be built)
+obj-n += ./
+endif
diff --git a/drivers/gpu/mali/mali400ko/.gitignore b/drivers/gpu/mali/mali400ko/.gitignore
new file mode 100644
index 00000000000..e2d66d55882
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/.gitignore
@@ -0,0 +1,32 @@
+#symlinks created during build
+driver/src/devicedrv/mali/arch
+driver/bin
+driver/devdrv
+driver/lib
+
+#build output
+driver/build
+.tmp_versions
+*.cmd
+*.o
+*.d
+driver/src/devicedrv/linux/__mali_build_info.*
+driver/src/devicedrv/linux/modules.order
+driver/src/devicedrv/linux/Module.symvers
+driver/src/devicedrv/mali/Module.symvers
+driver/src/devicedrv/mali/__malidrv_build_info.c
+driver/src/devicedrv/mali/mali.ko
+driver/src/devicedrv/mali/mali.mod.c
+driver/src/devicedrv/mali/modules.order
+driver/out*
+driver/src/shared/essl_compiler/src/shadergen_maligp2/shader_pieces.*
+
+out
+kernel
+
+#temp files from editors and Eclipse project files
+*~
+.cproject
+.project
+.settings
+
diff --git a/drivers/gpu/mali/mali400ko/Makefile b/drivers/gpu/mali/mali400ko/Makefile
new file mode 100644
index 00000000000..9176f9f3795
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/Makefile
@@ -0,0 +1,102 @@
+#This Makefile can be called to do an out-of-kernel build of tha mali kernel module.
+#It is not used when mali is built from the kernel build system.
+
+CROSS_COMPILE ?= arm-eabi-
+
+ifeq ($(TARGET_PRODUCT),ste_u5500)
+CONFIG_UX500_SOC_DB5500=y
+export CONFIG_UX500_SOC_DB5500
+endif
+
+PWD:=$(shell pwd)
+
+#if KERNEL_BUILD_DIR parameter is not set, assume there is a symlink to the kernel
+KERNEL_BUILD_DIR?=$(PWD)/kernel
+
+# android
+ifneq (,$(findstring -android,$(PLATFORM)))
+INSTALL_MOD_PATH?=$(ANDROID_OUT_TARGET_PRODUCT_DIRECTORY)/system
+endif
+
+# lbp
+ifneq (,$(findstring -linux,$(PLATFORM)))
+PREFIX?=$(MMROOT)/linux/install/cortexA9-linux-gnu-href_v1
+INSTALL_MOD_PATH?=$(PREFIX)
+endif
+
+#if still no kernel module path is set, install the modules in a local folder
+ifeq (,$(INSTALL_MOD_PATH))
+INSTALL_MOD_PATH?=$(PWD)/out
+endif
+
+KO_FLAGS=KDIR=$(KERNEL_BUILD_DIR) CROSS_COMPILE=$(CROSS_COMPILE)
+
+.PHONY: all entry check
+
+all: mali-devicedrv install-mali
+
+check:
+	@if [ ! -d $(KERNEL_BUILD_DIR) ] ; then echo "Error: provide a path to your linux kernel through KERNEL_BUILD_DIR"; exit 1 ; fi
+
+## Build ###############
+
+mali-devicedrv: check
+	$(MAKE) -C driver/src/devicedrv/mali $(KO_FLAGS) all
+
+ump-devicedrv: check
+	$(MAKE) -C driver/src/devicedrv/ump $(KO_FLAGS) all
+
+mali_drm-devicedrv: check
+	$(MAKE) -C x11/mali_drm/mali $(KO_FLAGS) all
+
+## Clean ###############
+
+clean-mali:
+	$(MAKE) clean -C driver/src/devicedrv/mali $(KO_FLAGS)
+
+clean-ump:
+	$(MAKE) clean -C driver/src/devicedrv/ump $(KO_FLAGS)
+
+clean-mali_drm:
+	$(MAKE) clean -C x11/mali_drm/mali $(KO_FLAGS)
+
+clean: clean-mali
+
+realclean:
+	rm -f driver/src/devicedrv/mali/common/*.o
+	rm -f driver/src/devicedrv/mali/linux/*.o
+	rm -f driver/src/devicedrv/mali/common/.*.cmd
+	rm -f driver/src/devicedrv/mali/linux/.*.cmd
+	rm -f driver/src/devicedrv/mali/.*.cmd
+	rm -f driver/src/devicedrv/mali/*.c
+
+distclean: realclean
+
+## Install #############
+
+install: install-mali
+
+install-mali: mali-devicedrv
+	$(MAKE) -C $(KERNEL_BUILD_DIR) ARCH=arm CROSS_COMPILE=$(CROSS_COMPILE) KERNELDIR=$(KERNEL_BUILD_DIR) INSTALL_MOD_PATH=$(INSTALL_MOD_PATH) M=$(PWD)/driver/src/devicedrv/mali modules_install
+
+install-ump: ump-devicedrv
+	$(MAKE) -C $(KERNEL_BUILD_DIR) ARCH=arm CROSS_COMPILE=$(CROSS_COMPILE) KERNELDIR=$(KERNEL_BUILD_DIR) INSTALL_MOD_PATH=$(INSTALL_MOD_PATH) M=$(PWD)/driver/src/devicedrv/ump modules_install
+
+install-mali_drm: mali_drm-devicedrv
+	$(MAKE) -C $(KERNEL_BUILD_DIR) ARCH=arm CROSS_COMPILE=$(CROSS_COMPILE) KERNELDIR=$(KERNEL_BUILD_DIR) INSTALL_MOD_PATH=$(INSTALL_MOD_PATH) M=$(PWD)/x11/mali_drm/mali modules_install
+
+
+printenv:
+	@echo "\033[0;44m"
+	@echo "ANDROID_OUT_TARGET_PRODUCT_DIRECTORY: " $(ANDROID_OUT_TARGET_PRODUCT_DIRECTORY)
+	@echo "ANDROID_BSP_ROOT: " $(ANDROID_BSP_ROOT)
+	@echo "KERNEL_BUILD_DIR: " $(KERNEL_BUILD_DIR)
+	@echo "MM_INSTALL_DIR: " $(MM_INSTALL_DIR)
+	@echo "PLATFORM: " $(PLATFORM)
+	@echo "PREFIX: " $(PREFIX)
+	@echo "MMROOT: " $(MMROOT)
+	@echo "INSTALL_MOD_PATH: " $(INSTALL_MOD_PATH)
+	@echo "FOO: " $(FOO)
+	@echo "VARIANT: " $(VARIANT)
+	@echo "\033[0m"
+
diff --git a/drivers/gpu/mali/mali400ko/driver/include/ump/ump_kernel_interface.h b/drivers/gpu/mali/mali400ko/driver/include/ump/ump_kernel_interface.h
new file mode 100644
index 00000000000..3bd928aefdb
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/include/ump/ump_kernel_interface.h
@@ -0,0 +1,236 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_kernel_interface.h
+ *
+ * This file contains the kernel space part of the UMP API.
+ */
+
+#ifndef __UMP_KERNEL_INTERFACE_H__
+#define __UMP_KERNEL_INTERFACE_H__
+
+
+/** @defgroup ump_kernel_space_api UMP Kernel Space API
+ * @{ */
+
+
+#include "ump_kernel_platform.h"
+
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+
+/**
+ * External representation of a UMP handle in kernel space.
+ */
+typedef void * ump_dd_handle;
+
+/**
+ * Typedef for a secure ID, a system wide identificator for UMP memory buffers.
+ */
+typedef unsigned int ump_secure_id;
+
+
+/**
+ * Value to indicate an invalid UMP memory handle.
+ */
+#define UMP_DD_HANDLE_INVALID ((ump_dd_handle)0)
+
+
+/**
+ * Value to indicate an invalid secure Id.
+ */
+#define UMP_INVALID_SECURE_ID ((ump_secure_id)-1)
+
+
+/**
+ * UMP error codes for kernel space.
+ */
+typedef enum
+{
+	UMP_DD_SUCCESS, /**< indicates success */
+	UMP_DD_INVALID, /**< indicates failure */
+} ump_dd_status_code;
+
+
+/**
+ * Struct used to describe a physical block used by UMP memory
+ */
+typedef struct ump_dd_physical_block
+{
+	unsigned long addr; /**< The physical address of the block */
+	unsigned long size; /**< The length of the block, typically page aligned */
+} ump_dd_physical_block;
+
+
+/**
+ * Retrieves the secure ID for the specified UMP memory.
+ *
+ * This identificator is unique across the entire system, and uniquely identifies
+ * the specified UMP memory. This identificator can later be used through the
+ * @ref ump_dd_handle_create_from_secure_id "ump_dd_handle_create_from_secure_id" or
+ * @ref ump_handle_create_from_secure_id "ump_handle_create_from_secure_id"
+ * functions in order to access this UMP memory, for instance from another process.
+ *
+ * @note There is a user space equivalent function called @ref ump_secure_id_get "ump_secure_id_get"
+ *
+ * @see ump_dd_handle_create_from_secure_id
+ * @see ump_handle_create_from_secure_id
+ * @see ump_secure_id_get
+ *
+ * @param mem Handle to UMP memory.
+ *
+ * @return Returns the secure ID for the specified UMP memory.
+ */
+UMP_KERNEL_API_EXPORT ump_secure_id ump_dd_secure_id_get(ump_dd_handle mem);
+
+
+/**
+ * Retrieves a handle to allocated UMP memory.
+ *
+ * The usage of UMP memory is reference counted, so this will increment the reference
+ * count by one for the specified UMP memory.
+ * Use @ref ump_dd_reference_release "ump_dd_reference_release" when there is no longer any
+ * use for the retrieved handle.
+ *
+ * @note There is a user space equivalent function called @ref ump_handle_create_from_secure_id "ump_handle_create_from_secure_id"
+ *
+ * @see ump_dd_reference_release
+ * @see ump_handle_create_from_secure_id
+ *
+ * @param secure_id The secure ID of the UMP memory to open, that can be retrieved using the @ref ump_secure_id_get "ump_secure_id_get " function.
+ *
+ * @return UMP_INVALID_MEMORY_HANDLE indicates failure, otherwise a valid handle is returned.
+ */
+UMP_KERNEL_API_EXPORT ump_dd_handle ump_dd_handle_create_from_secure_id(ump_secure_id secure_id);
+
+
+/**
+ * Retrieves the number of physical blocks used by the specified UMP memory.
+ *
+ * This function retrieves the number of @ref ump_dd_physical_block "ump_dd_physical_block" structs needed
+ * to describe the physical memory layout of the given UMP memory. This can later be used when calling
+ * the functions @ref ump_dd_phys_blocks_get "ump_dd_phys_blocks_get" and
+ * @ref ump_dd_phys_block_get "ump_dd_phys_block_get".
+ *
+ * @see ump_dd_phys_blocks_get
+ * @see ump_dd_phys_block_get
+ *
+ * @param mem Handle to UMP memory.
+ *
+ * @return The number of ump_dd_physical_block structs required to describe the physical memory layout of the specified UMP memory.
+ */
+UMP_KERNEL_API_EXPORT unsigned long ump_dd_phys_block_count_get(ump_dd_handle mem);
+
+
+/**
+ * Retrieves all physical memory block information for specified UMP memory.
+ *
+ * This function can be used by other device drivers in order to create MMU tables.
+ *
+ * @note This function will fail if the num_blocks parameter is either to large or to small.
+ *
+ * @see ump_dd_phys_block_get
+ *
+ * @param mem Handle to UMP memory.
+ * @param blocks An array of @ref ump_dd_physical_block "ump_dd_physical_block" structs that will receive the physical description.
+ * @param num_blocks The number of blocks to return in the blocks array. Use the function
+ *                   @ref ump_dd_phys_block_count_get "ump_dd_phys_block_count_get" first to determine the number of blocks required.
+ *
+ * @return UMP_DD_SUCCESS indicates success, UMP_DD_INVALID indicates failure.
+ */
+UMP_KERNEL_API_EXPORT ump_dd_status_code ump_dd_phys_blocks_get(ump_dd_handle mem, ump_dd_physical_block * blocks, unsigned long num_blocks);
+
+
+/**
+ * Retrieves the physical memory block information for specified block for the specified UMP memory.
+ *
+ * This function can be used by other device drivers in order to create MMU tables.
+ *
+ * @note This function will return UMP_DD_INVALID if the specified index is out of range.
+ *
+ * @see ump_dd_phys_blocks_get
+ *
+ * @param mem Handle to UMP memory.
+ * @param index Which physical info block to retrieve.
+ * @param block Pointer to a @ref ump_dd_physical_block "ump_dd_physical_block" struct which will receive the requested information.
+ *
+ * @return UMP_DD_SUCCESS indicates success, UMP_DD_INVALID indicates failure.
+ */
+UMP_KERNEL_API_EXPORT ump_dd_status_code ump_dd_phys_block_get(ump_dd_handle mem, unsigned long index, ump_dd_physical_block * block);
+
+
+/**
+ * Retrieves the actual size of the specified UMP memory.
+ *
+ * The size is reported in bytes, and is typically page aligned.
+ *
+ * @note There is a user space equivalent function called @ref ump_size_get "ump_size_get"
+ *
+ * @see ump_size_get
+ *
+ * @param mem Handle to UMP memory.
+ *
+ * @return Returns the allocated size of the specified UMP memory, in bytes.
+ */
+UMP_KERNEL_API_EXPORT unsigned long ump_dd_size_get(ump_dd_handle mem);
+
+
+/**
+ * Adds an extra reference to the specified UMP memory.
+ *
+ * This function adds an extra reference to the specified UMP memory. This function should
+ * be used every time a UMP memory handle is duplicated, that is, assigned to another ump_dd_handle
+ * variable. The function @ref ump_dd_reference_release "ump_dd_reference_release" must then be used
+ * to release each copy of the UMP memory handle.
+ *
+ * @note You are not required to call @ref ump_dd_reference_add "ump_dd_reference_add"
+ * for UMP handles returned from
+ * @ref ump_dd_handle_create_from_secure_id "ump_dd_handle_create_from_secure_id",
+ * because these handles are already reference counted by this function.
+ *
+ * @note There is a user space equivalent function called @ref ump_reference_add "ump_reference_add"
+ *
+ * @see ump_reference_add
+ *
+ * @param mem Handle to UMP memory.
+ */
+UMP_KERNEL_API_EXPORT void ump_dd_reference_add(ump_dd_handle mem);
+
+
+/**
+ * Releases a reference from the specified UMP memory.
+ *
+ * This function should be called once for every reference to the UMP memory handle.
+ * When the last reference is released, all resources associated with this UMP memory
+ * handle are freed.
+ *
+ * @note There is a user space equivalent function called @ref ump_reference_release "ump_reference_release"
+ *
+ * @see ump_reference_release
+ *
+ * @param mem Handle to UMP memory.
+ */
+UMP_KERNEL_API_EXPORT void ump_dd_reference_release(ump_dd_handle mem);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+/** @} */ /* end group ump_kernel_space_api */
+
+
+#endif  /* __UMP_KERNEL_INTERFACE_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/include/ump/ump_kernel_interface_ref_drv.h b/drivers/gpu/mali/mali400ko/driver/include/ump/ump_kernel_interface_ref_drv.h
new file mode 100644
index 00000000000..70eea22d3c3
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/include/ump/ump_kernel_interface_ref_drv.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_kernel_interface.h
+ */
+
+#ifndef __UMP_KERNEL_INTERFACE_REF_DRV_H__
+#define __UMP_KERNEL_INTERFACE_REF_DRV_H__
+
+#include "ump_kernel_interface.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Turn specified physical memory into UMP memory. */
+UMP_KERNEL_API_EXPORT ump_dd_handle ump_dd_handle_create_from_phys_blocks(ump_dd_physical_block * blocks, unsigned long num_blocks);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* __UMP_KERNEL_INTERFACE_REF_DRV_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/include/ump/ump_kernel_platform.h b/drivers/gpu/mali/mali400ko/driver/include/ump/ump_kernel_platform.h
new file mode 100644
index 00000000000..55c49885f06
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/include/ump/ump_kernel_platform.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_kernel_platform.h
+ *
+ * This file should define UMP_KERNEL_API_EXPORT,
+ * which dictates how the UMP kernel API should be exported/imported.
+ * Modify this file, if needed, to match your platform setup.
+ */
+
+#ifndef __UMP_KERNEL_PLATFORM_H__
+#define __UMP_KERNEL_PLATFORM_H__
+
+/** @addtogroup ump_kernel_space_api
+ * @{ */
+
+/**
+ * A define which controls how UMP kernel space API functions are imported and exported.
+ * This define should be set by the implementor of the UMP API.
+ */
+
+#if defined(_WIN32)
+
+#if defined(UMP_BUILDING_UMP_LIBRARY)
+#define UMP_KERNEL_API_EXPORT __declspec(dllexport)
+#else
+#define UMP_KERNEL_API_EXPORT __declspec(dllimport)
+#endif
+
+#else
+
+#define UMP_KERNEL_API_EXPORT
+
+#endif
+
+
+/** @} */ /* end group ump_kernel_space_api */
+
+
+#endif /* __UMP_KERNEL_PLATFORM_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/Makefile b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/Makefile
new file mode 100644
index 00000000000..20b57359167
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/Makefile
@@ -0,0 +1,346 @@
+#
+# Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+# 
+# This program is free software and is provided to you under the terms of the GNU General Public License version 2
+# as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+# 
+# A copy of the licence is included with the program, and can also be obtained from Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+#
+
+OSKOS=linux
+FILES_PREFIX=
+
+ifneq ($(KBUILD_EXTMOD),)
+DRIVER_DIR=$(KBUILD_EXTMOD)
+else
+src?=.
+srctree?=.
+DRIVER_DIR?=$(srctree)/$(src)
+M?=$(DRIVER_DIR)
+endif
+MALI_RELEASE_NAME=$(shell cat $(DRIVER_DIR)/.version 2> /dev/null)
+include $(DRIVER_DIR)/Makefile.platform
+include $(DRIVER_DIR)/Makefile.common
+
+# set up defaults if not defined by the user
+ARCH ?= arm
+USING_MMU ?= 1
+USING_UMP ?= 0
+USING_HWMEM ?= 0
+USING_OS_MEMORY ?= 0
+USING_PMM ?= 0
+USING_GPU_UTILIZATION ?= 0
+USING_MALI_RUN_TIME_PM ?= 0
+USING_MALI_PMM_TESTSUITE ?= 0
+OS_MEMORY_KERNEL_BUFFER_SIZE_IN_MB ?= 6
+USING_PROFILING ?= 0
+TIMESTAMP ?= default
+BUILD ?= debug
+TARGET_PLATFORM ?= default
+
+ifeq ($(USING_UMP),1)
+ifneq ($(USING_HWMEM),1)
+	UMP_SYMVERS_FILE = ../ump/Module.symvers
+	KBUILD_EXTRA_SYMBOLS = $(KBUILD_EXTMOD)/$(UMP_SYMVERS_FILE)
+endif
+endif
+
+# Check if a Mali Core sub module should be enabled, true or false returned
+submodule_enabled = $(shell gcc $(DEFINES) -E $1/arch/config.h | grep type | grep -c $(2))
+
+# linux build system integration
+ifneq ($(KERNELRELEASE),)
+# Inside the kernel build system
+
+# This conditional makefile exports the global definition ARM_INTERNAL_BUILD. Customer releases will not include arm_internal.mak
+-include ../../../arm_internal.mak
+
+# Set up our defines, which will be passed to gcc
+DEFINES += -DUSING_OS_MEMORY=$(USING_OS_MEMORY)
+DEFINES += -DUSING_MMU=$(USING_MMU)
+DEFINES += -DUSING_UMP=$(USING_UMP)
+DEFINES += -DUSING_HWMEM=$(USING_HWMEM)
+DEFINES += -D_MALI_OSK_SPECIFIC_INDIRECT_MMAP
+DEFINES += -DMALI_TIMELINE_PROFILING_ENABLED=$(USING_PROFILING)
+DEFINES += -DMALI_POWER_MGMT_TEST_SUITE=$(USING_MALI_PMM_TESTSUITE)
+DEFINES += -DMALI_PMM_RUNTIME_JOB_CONTROL_ON=$(USING_MALI_RUN_TIME_PM)
+DEFINES += -DMALI_STATE_TRACKING=1
+DEFINES += -DMALI_OS_MEMORY_KERNEL_BUFFER_SIZE_IN_MB=$(OS_MEMORY_KERNEL_BUFFER_SIZE_IN_MB)
+
+ifneq ($(call submodule_enabled, $M, PMU),0)
+        MALI_PLATFORM_FILE = platform/mali400-pmu/mali_platform.c
+else
+	MALI_PLATFORM_FILE = platform/$(TARGET_PLATFORM)/mali_platform.c
+endif
+
+DEFINES += -DUSING_MALI_PMM=$(USING_PMM)
+DEFINES += -DMALI_GPU_UTILIZATION=$(USING_GPU_UTILIZATION)
+
+ifeq ($(BUILD), debug)
+DEFINES += -DDEBUG
+endif
+DEFINES += -DSVN_REV=$(SVN_REV)
+DEFINES += -DSVN_REV_STRING=\"$(SVN_REV)\"
+
+# Linux has its own mmap cleanup handlers (see mali_kernel_mem_mmu.c)
+DEFINES += -DMALI_UKK_HAS_IMPLICIT_MMAP_CLEANUP
+
+ifeq ($(USING_UMP),1)
+	DEFINES += -DMALI_USE_UNIFIED_MEMORY_PROVIDER=1
+	EXTRA_CFLAGS += -I$(DRIVER_DIR)/../../../include/ump
+else
+	DEFINES += -DMALI_USE_UNIFIED_MEMORY_PROVIDER=0
+endif
+
+# Use our defines when compiling
+EXTRA_CFLAGS += $(DEFINES) -I$(DRIVER_DIR) -I$(DRIVER_DIR)/common -I$(DRIVER_DIR)/linux -I$(DRIVER_DIR)/platform
+
+# For customer releases the Linux Device Drivers will be provided as ARM proprietary and GPL releases:
+# The ARM proprietary product will only include the license/proprietary directory
+# The GPL product will only include the license/gpl directory
+
+ifeq ($(wildcard $(DRIVER_DIR)/linux/license/gpl/*),)
+EXTRA_CFLAGS += -I$(DRIVER_DIR)/linux/license/proprietary
+else
+EXTRA_CFLAGS += -I$(DRIVER_DIR)/linux/license/gpl
+endif
+EXTRA_CFLAGS += -I$(DRIVER_DIR)/common/pmm
+
+# Source files which always are included in a build
+SRC = \
+	common/mali_kernel_core.c \
+	linux/mali_kernel_linux.c \
+	$(OSKOS)/mali_osk_indir_mmap.c \
+	common/mali_kernel_rendercore.c \
+	common/mali_kernel_descriptor_mapping.c \
+	common/mali_kernel_vsync.c \
+	linux/mali_ukk_vsync.c \
+	$(MALI_PLATFORM_FILE) \
+	$(OSKFILES) \
+	$(UKKFILES)
+	#__malidrv_build_info.c
+	
+ifeq ($(USING_PROFILING),1)
+SRC += \
+	common/mali_kernel_profiling.c \
+	timestamp-$(TIMESTAMP)/mali_timestamp.c
+EXTRA_CFLAGS += -I$(DRIVER_DIR)/timestamp-$(TIMESTAMP)
+endif
+
+# Selecting files to compile by parsing the config file
+
+ifeq ($(USING_PMM),1)
+SRC += \
+	common/pmm/mali_pmm.c \
+	common/pmm/mali_pmm_policy.c \
+	common/pmm/mali_pmm_policy_alwayson.c \
+	common/pmm/mali_pmm_policy_jobcontrol.c \
+	common/pmm/mali_pmm_state.c \
+	linux/mali_kernel_pm.c \
+	linux/mali_osk_pm.c \
+	linux/mali_device_pause_resume.c 
+endif
+
+ifeq ($(USING_GPU_UTILIZATION),1)
+SRC += \
+	common/mali_kernel_utilization.c
+endif
+
+ifneq ($(call submodule_enabled, $M, MALI400PP),0)
+	# Mali-400 PP in use
+	EXTRA_CFLAGS += -DUSING_MALI400
+	SRC += common/mali_kernel_MALI200.c
+endif
+
+ifneq ($(call submodule_enabled, $M, MALI400GP),0)
+	# Mali-400 GP in use
+	SRC += common/mali_kernel_GP2.c
+endif
+
+ifneq ($(call submodule_enabled, $M, MALI300PP),0)
+	# Mali-400 PP in use
+	EXTRA_CFLAGS += -DUSING_MALI400
+	SRC += common/mali_kernel_MALI200.c
+endif
+
+ifneq ($(call submodule_enabled, $M, MALI300GP),0)
+	# Mali-400 GP in use
+	SRC += common/mali_kernel_GP2.c
+endif
+
+ifneq ($(call submodule_enabled, $M, MALI200),0)
+	# Mali200 in use
+	EXTRA_CFLAGS += -DUSING_MALI200
+	SRC += common/mali_kernel_MALI200.c
+endif
+
+ifneq ($(call submodule_enabled, $M, MALIGP2),0)
+	# MaliGP2 in use
+	SRC += common/mali_kernel_GP2.c
+endif
+
+ifneq ($(call submodule_enabled, $M, MMU),0)
+	# Mali MMU in use
+	SRC += common/mali_kernel_mem_mmu.c common/mali_kernel_memory_engine.c common/mali_block_allocator.c common/mali_kernel_mem_os.c
+else
+	# No Mali MMU in use
+	SRC += common/mali_kernel_mem_buddy.c
+endif
+
+ifneq ($(call submodule_enabled, $M, MALI400L2)$(),0)
+	# Mali Level2 cache in use
+	EXTRA_CFLAGS += -DUSING_MALI400_L2_CACHE
+	SRC += common/mali_kernel_l2_cache.c
+endif
+
+ifneq ($(call submodule_enabled, $M, MALI300L2)$(),0)
+	# Mali Level2 cache in use
+	EXTRA_CFLAGS += -DUSING_MALI400_L2_CACHE
+	SRC += common/mali_kernel_l2_cache.c
+endif
+
+ifeq ($(USING_HWMEM),1)
+	# HWMEM used as backend for UMP api
+	EXTRA_CFLAGS += -I$(DRIVER_DIR)/../ump/common
+	SRC += platform/ux500/ump_kernel_api_hwmem.c
+endif
+
+# Target build file
+MODULE:=mali.ko
+
+# Tell the Linux build system from which .o file to create the kernel module
+obj-$(CONFIG_GPU_MALI) := $(MODULE:.ko=.o)
+# Tell the Linux build system to enable building of our .c files
+$(MODULE:.ko=-y) := $(SRC:.c=.o)
+
+else
+# Outside the kernel build system
+
+# Get any user defined KDIR-<names> or maybe even a hardcoded KDIR
+-include KDIR_CONFIGURATION
+
+# Define host system directory
+KDIR-$(shell uname -m):=/lib/modules/$(shell uname -r)/build
+
+ifeq ($(ARCH), arm)
+	# when compiling for ARM we're cross compiling
+	export CROSS_COMPILE ?= arm-none-linux-gnueabi-
+	# default to Virtex5
+	CONFIG ?= pb-virtex5-m200
+else
+	# Compiling for the host
+	CONFIG ?= $(shell uname -m)
+endif
+
+# default cpu to select
+CPU ?= pb11mp
+
+# look up KDIR based om CPU selection
+KDIR ?= $(KDIR-$(CPU))
+
+# validate lookup result
+ifeq ($(KDIR),)
+$(error No KDIR found for platform $(CPU))
+endif
+
+# report detected/selected settings
+ifdef ARM_INTERNAL_BUILD
+$(warning Config $(CONFIG))
+$(warning Host CPU $(CPU))
+$(warning MMU $(USING_MMU))
+$(warning OS_MEMORY $(USING_OS_MEMORY))
+endif
+
+# Validate selected config
+ifneq ($(shell [ -d $(DRIVER_DIR)/arch-$(CONFIG) ] && [ -f $(DRIVER_DIR)/arch-$(CONFIG)/config.h ] && echo "OK"), OK)
+$(warning Current directory is $(shell pwd))
+$(error No configuration found for config $(CONFIG). Check that $(DRIVER_DIR)/arch-$(CONFIG)/config.h exists)
+else
+# Link arch to the selected arch-config directory
+$(shell [ -L $(DRIVER_DIR)/arch ] && rm $(DRIVER_DIR)/arch)
+$(shell ln -sf $(DRIVER_DIR)/arch-$(CONFIG) $(DRIVER_DIR)/arch)
+$(shell touch $(DRIVER_DIR)/arch/config.h)
+
+# Register number of Mali400 cores for routing test jobs to the right Mali-400
+ifeq ($CONFIG, pb-virtex5-m400-1)
+VERSION_STRINGS += USING_MALI400_PP_CORES=1
+endif
+ifeq ($CONFIG, pb-virtex5-m400-2)
+VERSION_STRINGS += USING_MALI400_PP_CORES=2
+endif
+ifeq ($CONFIG, pb-virtex5-m400-3)
+VERSION_STRINGS += USING_MALI400_PP_CORES=3
+endif
+ifeq ($CONFIG, pb-virtex5-m400-4)
+VERSION_STRINGS += USING_MALI400_PP_CORES=4
+endif
+
+endif
+
+# Filename to use when patching. Original will be saved as this
+PATCH_BACKUP_FILE:=config_makefile_patch_backup.h
+
+ifdef ARM_INTERNAL_BUILD
+$(warning Looking for previous backup)
+endif
+# Look for previous backup
+shell_output:=$(shell [ -f arch/$(PATCH_BACKUP_FILE) ] && mv -vf arch/$(PATCH_BACKUP_FILE) arch/config.h  && echo "Patch backup restored")
+ifneq ($(shell_output),)
+ifdef ARM_INTERNAL_BUILD
+$(warning $(shell_output))
+endif
+endif
+
+ifdef PATCH
+ifdef ARM_INTERNAL_BUILD
+$(warning Patching using: $(PATCH) )
+endif
+ifneq ($(shell [ -f arch/$(PATCH).diff ] && echo "OK"), OK)
+# The patch file does not exist
+shell_output:=$(shell echo "Possible PATCH arguments are:" ; find arch/ -name "*.diff" |sed -r 's/.*\/(.*)\.diff/\1/')
+ifdef ARM_INTERNAL_BUILD
+$(warning $(shell_output))
+endif
+$(error Could not find file arch-$(CONFIG)/$(PATCH).diff )
+else
+# Patch file found, do patching
+shell_output:=$(shell cp -f arch/config.h arch/$(PATCH_BACKUP_FILE) && patch --no-backup-if-mismatch -st -p0 arch/config.h -i arch/$(PATCH).diff && echo "OK")
+ifneq ($(shell_output), OK)
+$(warning Output from failed patch: $(shell_output))
+$(shell mv -f arch/$(PATCH_BACKUP_FILE) arch/config.h)
+$(error Could not patch file arch-$(CONFIG)/config.h with arch-$(CONFIG)/$(PATCH).diff.)
+endif
+endif
+endif
+
+# Extend common version-string
+VERSION_STRINGS += BUILD=$(shell echo $(BUILD) | tr a-z A-Z)
+VERSION_STRINGS += CPU=$(CPU)
+VERSION_STRINGS += USING_MMU=$(USING_MMU)
+VERSION_STRINGS += USING_UMP=$(USING_UMP)
+VERSION_STRINGS += USING_HWMEM=$(USING_HWMEM)
+VERSION_STRINGS += USING_PMM=$(USING_PMM)
+VERSION_STRINGS += USING_MALI200=$(call submodule_enabled, ., MALI200)
+VERSION_STRINGS += USING_MALI400=$(call submodule_enabled, ., MALI400)
+VERSION_STRINGS += USING_MALI400_L2_CACHE=$(call submodule_enabled, ., MALI400L2)
+VERSION_STRINGS += USING_GP2=$(call submodule_enabled, ., MALIGP2)
+VERSION_STRINGS += KDIR=$(KDIR)
+
+all: make-build-info-file $(UMP_SYMVERS_FILE)
+	$(MAKE) ARCH=$(ARCH) -C $(KDIR) M=$(CURDIR) CONFIG_GPU_MALI=m modules
+	@([ "$(PATCH)" != "" ] && [ -f arch/$(PATCH_BACKUP_FILE) ] && mv -f arch/$(PATCH_BACKUP_FILE) arch/config.h  && echo "Patch backup restored") || echo No Unpatching needed
+	@rm -f $(FILES_PREFIX)__malidrv_build_info.c $(FILES_PREFIX)__malidrv_build_info.o
+
+clean:
+	$(MAKE) ARCH=$(ARCH) -C $(KDIR) M=$(CURDIR) clean
+	@([ "$(PATCH)" != "" ] && [ -f arch/$(PATCH_BACKUP_FILE) ] && mv -f arch/$(PATCH_BACKUP_FILE) arch/config.h  && echo "Patch backup restored") || echo No Unpatching needed
+
+make-build-info-file:
+	@echo 'char *__malidrv_build_info(void) { return "malidrv: $(VERSION_STRINGS)";}' > $(FILES_PREFIX)__malidrv_build_info.c
+
+kernelrelease:
+	$(MAKE) -C $(KDIR) kernelrelease
+
+# end of outside kernel build system block
+endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/Makefile.common b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/Makefile.common
new file mode 100644
index 00000000000..a8f4189d0cf
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/Makefile.common
@@ -0,0 +1,56 @@
+#
+# Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+# 
+# This program is free software and is provided to you under the terms of the GNU General Public License version 2
+# as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+# 
+# A copy of the licence is included with the program, and can also be obtained from Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+#
+
+OSKFILES=\
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_atomics.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_irq.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_locks.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_low_level_mem.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_math.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_memory.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_misc.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_mali.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_notification.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_time.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_osk_timers.c
+
+UKKFILES=\
+	$(FILES_PREFIX)$(OSKOS)/mali_ukk_mem.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_ukk_gp.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_ukk_pp.c \
+	$(FILES_PREFIX)$(OSKOS)/mali_ukk_core.c
+	
+ifeq ($(USING_PROFILING),1)
+UKKFILES+=\
+	$(FILES_PREFIX)$(OSKOS)/mali_ukk_profiling.c
+endif
+
+ifeq ($(MALI_PLATFORM_FILE),)
+MALI_PLATFORM_FILE=platform/default/mali_platform.c
+endif
+
+# Get subversion revision number, fall back to only ${MALI_RELEASE_NAME} if no svn info is available
+SVN_REV := $(shell (cd $(DRIVER_DIR); (svnversion | grep -qvE '(exported|Unversioned)' && svnversion) || git svn info | grep '^Revision: '| sed -e 's/^Revision: //' ) 2>/dev/null )
+ifeq ($(SVN_REV),)
+SVN_REV := $(MALI_RELEASE_NAME)
+else
+SVN_REV := $(MALI_RELEASE_NAME)-r$(SVN_REV)
+endif
+
+# Common version-string, will be extended by OS-specifc sections
+VERSION_STRINGS =
+VERSION_STRINGS += CONFIG=$(CONFIG)
+VERSION_STRINGS += USING_OS_MEMORY=$(USING_OS_MEMORY)
+VERSION_STRINGS += API_VERSION=$(shell cd $(DRIVER_DIR); grep "\#define _MALI_API_VERSION" $(FILES_PREFIX)common\/mali_uk_types.h | cut -d' ' -f 3 )
+VERSION_STRINGS += REPO_URL=$(shell cd $(DRIVER_DIR); (svn info || git svn info || echo 'URL: $(MALI_RELEASE_NAME)') 2>/dev/null | grep '^URL: ' | cut -d: -f2- | cut -b2-)
+VERSION_STRINGS += REVISION=$(SVN_REV)
+VERSION_STRINGS += CHANGED_REVISION=$(shell cd $(DRIVER_DIR); (svn info || git svn info || echo 'Last Changed Rev: $(MALI_RELEASE_NAME)') 2>/dev/null | grep '^Last Changed Rev: ' | cut -d: -f2- | cut -b2-)
+VERSION_STRINGS += CHANGE_DATE=$(shell cd $(DRIVER_DIR); (svn info || git svn info || echo 'Last Changed Date: $(MALI_RELEASE_NAME)') 2>/dev/null | grep '^Last Changed Date: ' | cut -d: -f2- | cut -b2-)
+VERSION_STRINGS += BUILD_DATE=$(shell date)
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/Makefile.platform b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/Makefile.platform
new file mode 100644
index 00000000000..1f8068e25de
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/Makefile.platform
@@ -0,0 +1,45 @@
+#these are the build options for the ST-Ericsson Ux500 platforms.
+
+ifeq ($(CONFIG_UX500_SOC_DB5500),y)
+TARGET_PLATFORM = default
+USING_GPU_UTILIZATION = 0
+DEFINES += -DSOC_DB5500=1
+endif
+
+CONFIG = ux500
+CPU = $(CONFIG)
+TARGET_PLATFORM ?= $(CONFIG)
+ARCH ?= arm
+USING_MMU ?= 1
+USING_PMM ?= 1
+USING_UMP ?= 1
+USING_HWMEM ?= 1
+USING_OS_MEMORY ?= 1
+USING_GPU_UTILIZATION ?= 1
+
+ifeq ($(CONFIG_GPU_MALI_DEBUG),y)
+BUILD ?= debug
+else
+BUILD ?= release
+endif
+
+KDIR-$(CPU)=$(srctree)
+
+#these are paths relative to the mali400ko/driver/src/devicedrv/mali folder
+#not to be confused with the drivers/gpu/mali symlink in the kernel tree
+EXTRA_CFLAGS += -I$(realpath $(DRIVER_DIR)/../../../include/ump)
+EXTRA_CFLAGS += -I$(realpath $(DRIVER_DIR)/../ump/common)
+
+#The following is duplicated from the main Makefile to ensure that the 'arch'
+#link is created even during an in-kernel build.
+
+# Validate selected config
+ifneq ($(shell [ -d $(DRIVER_DIR)/arch-$(CONFIG) ] && [ -f $(DRIVER_DIR)/arch-$(CONFIG)/config.h ] && echo "OK"), OK)
+$(warning Current directory is $(shell pwd))
+$(error No configuration found for config $(CONFIG). Check that $(DRIVER_DIR)/arch-$(CONFIG)/config.h exists)
+else
+# Link arch to the selected arch-config directory
+$(shell [ -L $(DRIVER_DIR)/arch ] && rm $(DRIVER_DIR)/arch)
+$(shell ln -sf $(DRIVER_DIR)/arch-$(CONFIG) $(DRIVER_DIR)/arch)
+$(shell touch $(DRIVER_DIR)/arch/config.h)
+endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m300/config.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m300/config.h
new file mode 100644
index 00000000000..5011c97a28c
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m300/config.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __ARCH_CONFIG_H__
+#define __ARCH_CONFIG_H__
+
+/* Configuration for the PB platform with ZBT memory enabled */
+
+static _mali_osk_resource_t arch_configuration [] =
+{
+	{
+		.type = PMU,
+		.description = "Mali-300 PMU",
+		.base = 0xC0002000,
+		.irq = -1,
+		.mmu_id = 0
+
+	},
+	{
+		.type = MALI300GP,
+		.description = "Mali-300 GP",
+		.base = 0xC0000000,
+		.irq = -1,
+		.mmu_id = 1
+	},
+	{
+		.type = MALI300PP,
+		.base = 0xc0008000,
+		.irq = -1,
+		.description = "Mali-300 PP",
+		.mmu_id = 2
+	},
+#if USING_MMU
+	{
+		.type = MMU,
+		.base = 0xC0003000,
+		.irq = -1,
+		.description = "Mali-300 MMU for GP",
+		.mmu_id = 1
+	},
+	{
+		.type = MMU,
+		.base = 0xC0004000,
+		.irq = -1,
+		.description = "Mali-300 MMU for PP",
+		.mmu_id = 2
+	},
+#endif
+	{
+		.type = MEMORY,
+		.description = "Mali SDRAM remapped to baseboard",
+		.cpu_usage_adjust = -0x50000000,
+		.alloc_order = 0, /* Highest preference for this memory */
+		.base = 0xD0000000,
+		.size = 0x10000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEMORY,
+		.description = "Mali ZBT",
+		.alloc_order = 5, /* Medium preference for this memory */
+		.base = 0xe1000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEM_VALIDATION,
+		.description = "Framebuffer",
+		.base = 0xe0000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_WRITEABLE | _MALI_PP_READABLE
+	},
+	{
+		.type = MALI300L2,
+		.base = 0xC0001000,
+		.description = "Mali-300 L2 cache"
+	},
+};
+
+#endif /* __ARCH_CONFIG_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-1-direct/config.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-1-direct/config.h
new file mode 100644
index 00000000000..ee53f49bfe0
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-1-direct/config.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __ARCH_CONFIG_H__
+#define __ARCH_CONFIG_H__
+
+/* Configuration for the PB platform with ZBT memory enabled */
+
+static _mali_osk_resource_t arch_configuration [] =
+{
+
+    	{
+                .type = PMU,
+                .description = "Mali-400 PMU",
+                .base = 0xC0002000,
+                .irq = -1,
+                .mmu_id = 0
+        },
+	{
+		.type = MALI400GP,
+		.description = "Mali-400 GP",
+		.base = 0xC0000000,
+		.irq = -1,
+		.mmu_id = 1
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc0008000,
+		.irq = -1,
+		.description = "Mali-400 PP",
+		.mmu_id = 2
+	},
+#if USING_MMU
+	{
+		.type = MMU,
+		.base = 0xC0003000,
+		.irq = -1,
+		.description = "Mali-400 MMU for GP",
+		.mmu_id = 1
+	},
+	{
+		.type = MMU,
+		.base = 0xC0004000,
+		.irq = -1,
+		.description = "Mali-400 MMU for PP",
+		.mmu_id = 2
+	},
+#endif
+	{
+		.type = OS_MEMORY,
+		.description = "OS Memory",
+		.alloc_order = 10, /* Lowest preference for this memory */
+		.size = 96 * 1024 * 1024, /* 96 MB */
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEMORY,
+		.description = "Mali SDRAM remapped to baseboard",
+		.cpu_usage_adjust = 0,
+		.alloc_order = 5, /* Medium preference for this memory */
+		.base = 0x80000000,
+		.size = 0x10000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEMORY,
+		.description = "Mali ZBT",
+		.alloc_order = 0, /* Highest preference for this memory */
+		.base = 0xe1000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEM_VALIDATION,
+		.description = "Framebuffer",
+		.base = 0xe0000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_WRITEABLE | _MALI_PP_READABLE
+	},
+	{
+		.type = MALI400L2,
+		.base = 0xC0001000,
+		.description = "Mali-400 L2 cache"
+	},
+};
+
+#endif /* __ARCH_CONFIG_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-1-pmu/config.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-1-pmu/config.h
new file mode 100644
index 00000000000..5c7c7f3ef13
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-1-pmu/config.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __ARCH_CONFIG_H__
+#define __ARCH_CONFIG_H__
+
+/* Configuration for the PB platform with ZBT memory enabled */
+
+static _mali_osk_resource_t arch_configuration [] =
+{
+	{
+		.type = PMU,
+		.description = "Mali-400 PMU",
+		.base = 0xC0002000,
+		.irq = -1,
+		.mmu_id = 0
+
+	},
+	{
+		.type = MALI400GP,
+		.description = "Mali-400 GP",
+		.base = 0xC0000000,
+		.irq = -1,
+		.mmu_id = 1
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc0008000,
+		.irq = -1,
+		.description = "Mali-400 PP",
+		.mmu_id = 2
+	},
+#if USING_MMU
+	{
+		.type = MMU,
+		.base = 0xC0003000,
+		.irq = -1,
+		.description = "Mali-400 MMU for GP",
+		.mmu_id = 1
+	},
+	{
+		.type = MMU,
+		.base = 0xC0004000,
+		.irq = -1,
+		.description = "Mali-400 MMU for PP",
+		.mmu_id = 2
+	},
+#endif
+	{
+		.type = MEMORY,
+		.description = "Mali SDRAM remapped to baseboard",
+		.cpu_usage_adjust = -0x50000000,
+		.alloc_order = 0, /* Highest preference for this memory */
+		.base = 0xD0000000,
+		.size = 0x10000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEMORY,
+		.description = "Mali ZBT",
+		.alloc_order = 5, /* Medium preference for this memory */
+		.base = 0xe1000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEM_VALIDATION,
+		.description = "Framebuffer",
+		.base = 0xe0000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_WRITEABLE | _MALI_PP_READABLE
+	},
+	{
+		.type = MALI400L2,
+		.base = 0xC0001000,
+		.description = "Mali-400 L2 cache"
+	},
+};
+
+#endif /* __ARCH_CONFIG_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-1/config.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-1/config.h
new file mode 100644
index 00000000000..9f675fd1d23
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-1/config.h
@@ -0,0 +1,79 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __ARCH_CONFIG_H__
+#define __ARCH_CONFIG_H__
+
+/* Configuration for the PB platform with ZBT memory enabled */
+
+static _mali_osk_resource_t arch_configuration [] =
+{
+	{
+		.type = MALI400GP,
+		.description = "Mali-400 GP",
+		.base = 0xC0000000,
+		.irq = -1,
+		.mmu_id = 1
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc0008000,
+		.irq = -1,
+		.description = "Mali-400 PP",
+		.mmu_id = 2
+	},
+#if USING_MMU
+	{
+		.type = MMU,
+		.base = 0xC0003000,
+		.irq = -1,
+		.description = "Mali-400 MMU for GP",
+		.mmu_id = 1
+	},
+	{
+		.type = MMU,
+		.base = 0xC0004000,
+		.irq = -1,
+		.description = "Mali-400 MMU for PP",
+		.mmu_id = 2
+	},
+#endif
+	{
+		.type = MEMORY,
+		.description = "Mali SDRAM remapped to baseboard",
+		.cpu_usage_adjust = -0x50000000,
+		.alloc_order = 0, /* Highest preference for this memory */
+		.base = 0xD0000000,
+		.size = 0x10000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEMORY,
+		.description = "Mali ZBT",
+		.alloc_order = 5, /* Medium preference for this memory */
+		.base = 0xe1000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEM_VALIDATION,
+		.description = "Framebuffer",
+		.base = 0xe0000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_WRITEABLE | _MALI_PP_READABLE
+	},
+	{
+		.type = MALI400L2,
+		.base = 0xC0001000,
+		.description = "Mali-400 L2 cache"
+	},
+};
+
+#endif /* __ARCH_CONFIG_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-2/config.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-2/config.h
new file mode 100644
index 00000000000..cc1f05cb5f4
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-2/config.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __ARCH_CONFIG_H__
+#define __ARCH_CONFIG_H__
+
+/* Configuration for the PB platform with ZBT memory enabled */
+
+static _mali_osk_resource_t arch_configuration [] =
+{
+	{
+		.type = MALI400GP,
+		.description = "Mali-400 GP",
+		.base = 0xC0000000,
+		.irq = -1,
+		.mmu_id = 1
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc0008000,
+		.irq = -1,
+		.description = "Mali-400 PP 0",
+		.mmu_id = 2
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc000A000,
+		.irq = -1,
+		.description = "Mali-400 PP 1",
+		.mmu_id = 3
+},
+#if USING_MMU
+	{
+		.type = MMU,
+		.base = 0xC0003000,
+		.irq = -1,
+		.description = "Mali-400 MMU for GP",
+		.mmu_id = 1
+	},
+	{
+		.type = MMU,
+		.base = 0xC0004000,
+		.irq = -1,
+		.description = "Mali-400 MMU for PP 0",
+		.mmu_id = 2
+	},
+	{
+		.type = MMU,
+		.base = 0xC0005000,
+		.irq = -1,
+		.description = "Mali-400 MMU for PP 1",
+		.mmu_id = 3
+	},
+#endif
+	{
+		.type = MEMORY,
+		.description = "Mali SDRAM remapped to baseboard",
+		.cpu_usage_adjust = -0x50000000,
+		.alloc_order = 0, /* Highest preference for this memory */
+		.base = 0xD0000000,
+		.size = 0x10000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEMORY,
+		.description = "Mali ZBT",
+		.alloc_order = 5, /* Medium preference for this memory */
+		.base = 0xe1000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEM_VALIDATION,
+		.description = "Framebuffer",
+		.base = 0xe0000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_WRITEABLE | _MALI_PP_READABLE
+	},
+	{
+		.type = MALI400L2,
+		.base = 0xC0001000,
+		.description = "Mali-400 L2 cache"
+	},
+};
+
+#endif /* __ARCH_CONFIG_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-3/config.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-3/config.h
new file mode 100644
index 00000000000..a672e7d22ef
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-3/config.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __ARCH_CONFIG_H__
+#define __ARCH_CONFIG_H__
+
+/* Configuration for the PB platform with ZBT memory enabled */
+
+static _mali_osk_resource_t arch_configuration [] =
+{
+	{
+		.type = MALI400GP,
+		.description = "Mali-400 GP",
+		.base = 0xC0000000,
+		.irq = -1,
+		.mmu_id = 1
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc0008000,
+		.irq = -1,
+		.description = "Mali-400 PP 0",
+		.mmu_id = 2
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc000A000,
+		.irq = -1,
+		.description = "Mali-400 PP 1",
+		.mmu_id = 3
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc000C000,
+		.irq = -1,
+		.description = "Mali-400 PP 2",
+		.mmu_id = 4
+	},
+#if USING_MMU
+	{
+		.type = MMU,
+		.base = 0xC0003000,
+		.irq = 102,
+		.description = "Mali-400 MMU for GP",
+		.mmu_id = 1
+	},
+	{
+		.type = MMU,
+		.base = 0xC0004000,
+		.irq = 102,
+		.description = "Mali-400 MMU for PP 0",
+		.mmu_id = 2
+	},
+	{
+		.type = MMU,
+		.base = 0xC0005000,
+		.irq = 102,
+		.description = "Mali-400 MMU for PP 1",
+		.mmu_id = 3
+	},
+	{
+		.type = MMU,
+		.base = 0xC0006000,
+		.irq = 102,
+		.description = "Mali-400 MMU for PP 2",
+		.mmu_id = 4
+	},
+#endif
+	{
+		.type = MEMORY,
+		.description = "Mali SDRAM remapped to baseboard",
+		.cpu_usage_adjust = -0x50000000,
+		.alloc_order = 0, /* Highest preference for this memory */
+		.base = 0xD0000000,
+		.size = 0x10000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEMORY,
+		.description = "Mali ZBT",
+		.alloc_order = 5, /* Medium preference for this memory */
+		.base = 0xe1000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEM_VALIDATION,
+		.description = "Framebuffer",
+		.base = 0xe0000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_WRITEABLE | _MALI_PP_READABLE
+	},
+	{
+		.type = MALI400L2,
+		.base = 0xC0001000,
+		.description = "Mali-400 L2 cache"
+	},
+};
+
+#endif /* __ARCH_CONFIG_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-4/config.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-4/config.h
new file mode 100644
index 00000000000..ae9100c1ae4
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-pb-virtex5-m400-4/config.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __ARCH_CONFIG_H__
+#define __ARCH_CONFIG_H__
+
+/* Configuration for the EB platform with ZBT memory enabled */
+
+static _mali_osk_resource_t arch_configuration [] =
+{
+	{
+		.type = MALI400GP,
+		.description = "Mali-400 GP",
+		.base = 0xC0000000,
+		.irq = -1,
+		.mmu_id = 1
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc0008000,
+		.irq = -1,
+		.description = "Mali-400 PP 0",
+		.mmu_id = 2
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc000A000,
+		.irq = -1,
+		.description = "Mali-400 PP 1",
+		.mmu_id = 3
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc000C000,
+		.irq = -1,
+		.description = "Mali-400 PP 2",
+		.mmu_id = 4
+	},
+	{
+		.type = MALI400PP,
+		.base = 0xc000E000,
+		.irq = -1,
+		.description = "Mali-400 PP 3",
+		.mmu_id = 5
+	},
+#if USING_MMU
+	{
+		.type = MMU,
+		.base = 0xC0003000,
+		.irq = 102,
+		.description = "Mali-400 MMU for GP",
+		.mmu_id = 1
+	},
+	{
+		.type = MMU,
+		.base = 0xC0004000,
+		.irq = 102,
+		.description = "Mali-400 MMU for PP 0",
+		.mmu_id = 2
+	},
+	{
+		.type = MMU,
+		.base = 0xC0005000,
+		.irq = 102,
+		.description = "Mali-400 MMU for PP 1",
+		.mmu_id = 3
+	},
+	{
+		.type = MMU,
+		.base = 0xC0006000,
+		.irq = 102,
+		.description = "Mali-400 MMU for PP 2",
+		.mmu_id = 4
+	},
+	{
+		.type = MMU,
+		.base = 0xC0007000,
+		.irq = 102,
+		.description = "Mali-400 MMU for PP 3",
+		.mmu_id = 5
+	},
+#endif
+	{
+		.type = MEMORY,
+		.description = "Mali SDRAM remapped to baseboard",
+		.cpu_usage_adjust = -0x50000000,
+		.alloc_order = 0, /* Highest preference for this memory */
+		.base = 0xD0000000,
+		.size = 0x10000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEMORY,
+		.description = "Mali ZBT",
+		.alloc_order = 5, /* Medium preference for this memory */
+		.base = 0xe1000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+	{
+		.type = MEM_VALIDATION,
+		.description = "Framebuffer",
+		.base = 0xe0000000,
+		.size = 0x01000000,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_WRITEABLE | _MALI_PP_READABLE
+	},
+	{
+		.type = MALI400L2,
+		.base = 0xC0001000,
+		.description = "Mali-400 L2 cache"
+	},
+};
+
+#endif /* __ARCH_CONFIG_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-ux500/config.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-ux500/config.h
new file mode 100644
index 00000000000..dc324cf0d39
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/arch-ux500/config.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Architecture configuration for ST-Ericsson Ux500 platforms
+ *
+ * Author: Magnus Wendt <magnus.wendt@stericsson.com> for
+ * ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#ifndef __ARCH_UX500_CONFIG_H__
+#define __ARCH_UX500_CONFIG_H__
+
+/* #include <mach/hardware.h> */
+
+#define U5500_SGA_BASE 0x801D0000
+#define U8500_SGA_BASE 0xA0300000
+
+#if defined(SOC_DB5500) && (1 == SOC_DB5500)
+#define UX500_SGA_BASE U5500_SGA_BASE
+#else
+#define UX500_SGA_BASE U8500_SGA_BASE
+#endif
+
+#define MEGABYTE (1024*1024)
+#define MALI_MEM_BASE (128 * MEGABYTE)
+#define MALI_MEM_SIZE ( 32 * MEGABYTE)
+#define OS_MEM_SIZE   (128 * MEGABYTE)
+
+/* Hardware revision u8500 v1: GX570-BU-00000-r0p1
+ * Hardware revision u8500 v2: GX570-BU-00000-r1p0
+ * Hardware revision u5500: GX570-BU-00000-r1p0
+ * configuration registers: 0xA0300000-0xA031FFFFF  (stw8500v1_usermanual p269)
+ *
+ * Shared Peripheral Interrupt assignments:     (stw8500v1_usermanual p265-266)
+ * Nb  | Interrupt Source
+ * 116 | Mali400 combined
+ * 115 | Mali400 geometry processor
+ * 114 | Mali400 geometry processor MMU
+ * 113 | Mali400 pixel processor
+ * 112 | Mali400 pixel processor MMU
+ *
+ * irq offset: 32
+ */
+
+static _mali_osk_resource_t arch_configuration [] =
+{
+	{
+		.type = MALI400GP,
+		.description = "Mali-400 GP",
+		.base = UX500_SGA_BASE + 0x0000,
+		.irq = 115+32,
+		.mmu_id = 1
+	},
+	{
+		.type = MALI400PP,
+		.base = UX500_SGA_BASE + 0x8000,
+		.irq = 113+32,
+		.description = "Mali-400 PP",
+		.mmu_id = 2
+	},
+#if USING_MMU
+	{
+		.type = MMU,
+		.base = UX500_SGA_BASE + 0x3000,
+		.irq = 114+32,
+		.description = "Mali-400 MMU for GP",
+		.mmu_id = 1
+	},
+	{
+		.type = MMU,
+		.base = UX500_SGA_BASE + 0x4000,
+		.irq = 112+32,
+		.description = "Mali-400 MMU for PP",
+		.mmu_id = 2
+	},
+#endif
+	{
+		.type = MEMORY,
+		.description = "Mali SDRAM",
+		.alloc_order = 0, /* Highest preference for this memory */
+		.base = MALI_MEM_BASE,
+		.size = MALI_MEM_SIZE,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE
+	},
+#if USING_OS_MEMORY
+	{
+		.type = OS_MEMORY,
+		.description = "Linux kernel memory",
+		.alloc_order = 5, /* Medium preference for this memory */
+		.size = OS_MEM_SIZE,
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_MMU_READABLE | _MALI_MMU_WRITEABLE
+	},
+#endif
+	{
+		.type = MEM_VALIDATION,
+		.description = "Framebuffer",
+		.base = 0x00000000, /* Validate all memory for now */
+		.size = 2047 * MEGABYTE, /* "2GB ought to be enough for anyone" */
+		.flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_WRITEABLE | _MALI_PP_READABLE
+	},
+	{
+		.type = MALI400L2,
+		.base = UX500_SGA_BASE + 0x1000,
+		.description = "Mali-400 L2 cache"
+	},
+};
+
+#endif /* __ARCH_UX500_CONFIG_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_block_allocator.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_block_allocator.c
new file mode 100644
index 00000000000..fc80a20c813
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_block_allocator.c
@@ -0,0 +1,370 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include "mali_kernel_common.h"
+#include "mali_kernel_core.h"
+#include "mali_kernel_memory_engine.h"
+#include "mali_block_allocator.h"
+#include "mali_osk.h"
+
+#define MALI_BLOCK_SIZE (256UL * 1024UL)  /* 256 kB, remember to keep the ()s */
+
+typedef struct block_info
+{
+	struct block_info * next;
+} block_info;
+
+/* The structure used as the handle produced by block_allocator_allocate,
+ * and removed by block_allocator_release */
+typedef struct block_allocator_allocation
+{
+	/* The list will be released in reverse order */
+	block_info *last_allocated;
+	mali_allocation_engine * engine;
+	mali_memory_allocation * descriptor;
+	u32 start_offset;
+	u32 mapping_length;
+} block_allocator_allocation;
+
+
+typedef struct block_allocator
+{
+    _mali_osk_lock_t *mutex;
+	block_info * all_blocks;
+	block_info * first_free;
+	u32 base;
+	u32 cpu_usage_adjust;
+	u32 num_blocks;
+} block_allocator;
+
+MALI_STATIC_INLINE u32 get_phys(block_allocator * info, block_info * block);
+static mali_physical_memory_allocation_result block_allocator_allocate(void* ctx, mali_allocation_engine * engine,  mali_memory_allocation * descriptor, u32* offset, mali_physical_memory_allocation * alloc_info);
+static void block_allocator_release(void * ctx, void * handle);
+static mali_physical_memory_allocation_result block_allocator_allocate_page_table_block(void * ctx, mali_page_table_block * block);
+static void block_allocator_release_page_table_block( mali_page_table_block *page_table_block );
+static void block_allocator_destroy(mali_physical_memory_allocator * allocator);
+
+mali_physical_memory_allocator * mali_block_allocator_create(u32 base_address, u32 cpu_usage_adjust, u32 size, const char *name)
+{
+	mali_physical_memory_allocator * allocator;
+	block_allocator * info;
+	u32 usable_size;
+	u32 num_blocks;
+
+	usable_size = size & ~(MALI_BLOCK_SIZE - 1);
+	MALI_DEBUG_PRINT(3, ("Mali block allocator create for region starting at 0x%08X length 0x%08X\n", base_address, size));
+	MALI_DEBUG_PRINT(4, ("%d usable bytes\n", usable_size));
+	num_blocks = usable_size / MALI_BLOCK_SIZE;
+	MALI_DEBUG_PRINT(4, ("which becomes %d blocks\n", num_blocks));
+
+	if (usable_size == 0)
+	{
+		MALI_DEBUG_PRINT(1, ("Memory block of size %d is unusable\n", size));
+		return NULL;
+	}
+
+	allocator = _mali_osk_malloc(sizeof(mali_physical_memory_allocator));
+	if (NULL != allocator)
+	{
+		info = _mali_osk_malloc(sizeof(block_allocator));
+		if (NULL != info)
+		{
+            info->mutex = _mali_osk_lock_init( _MALI_OSK_LOCKFLAG_ORDERED, 0, 105);
+            if (NULL != info->mutex)
+            {
+        		info->all_blocks = _mali_osk_malloc(sizeof(block_info) * num_blocks);
+			    if (NULL != info->all_blocks)
+			    {
+				    u32 i;
+				    info->first_free = NULL;
+				    info->num_blocks = num_blocks;
+
+				    info->base = base_address;
+				    info->cpu_usage_adjust = cpu_usage_adjust;
+
+				    for ( i = 0; i < num_blocks; i++)
+				    {
+					    info->all_blocks[i].next = info->first_free;
+					    info->first_free = &info->all_blocks[i];
+				    }
+
+				    allocator->allocate = block_allocator_allocate;
+				    allocator->allocate_page_table_block = block_allocator_allocate_page_table_block;
+				    allocator->destroy = block_allocator_destroy;
+				    allocator->ctx = info;
+					allocator->name = name;
+
+				    return allocator;
+			    }
+                _mali_osk_lock_term(info->mutex);
+            }
+			_mali_osk_free(info);
+		}
+		_mali_osk_free(allocator);
+	}
+
+	return NULL;
+}
+
+static void block_allocator_destroy(mali_physical_memory_allocator * allocator)
+{
+	block_allocator * info;
+	MALI_DEBUG_ASSERT_POINTER(allocator);
+	MALI_DEBUG_ASSERT_POINTER(allocator->ctx);
+	info = (block_allocator*)allocator->ctx;
+
+	_mali_osk_free(info->all_blocks);
+    _mali_osk_lock_term(info->mutex);
+	_mali_osk_free(info);
+	_mali_osk_free(allocator);
+}
+
+MALI_STATIC_INLINE u32 get_phys(block_allocator * info, block_info * block)
+{
+	return info->base + ((block - info->all_blocks) * MALI_BLOCK_SIZE);
+}
+
+static mali_physical_memory_allocation_result block_allocator_allocate(void* ctx, mali_allocation_engine * engine, mali_memory_allocation * descriptor, u32* offset, mali_physical_memory_allocation * alloc_info)
+{
+	block_allocator * info;
+	u32 left;
+	block_info * last_allocated = NULL;
+	mali_physical_memory_allocation_result result = MALI_MEM_ALLOC_NONE;
+	block_allocator_allocation *ret_allocation;
+
+	MALI_DEBUG_ASSERT_POINTER(ctx);
+	MALI_DEBUG_ASSERT_POINTER(descriptor);
+	MALI_DEBUG_ASSERT_POINTER(offset);
+	MALI_DEBUG_ASSERT_POINTER(alloc_info);
+
+	info = (block_allocator*)ctx;
+	left = descriptor->size - *offset;
+	MALI_DEBUG_ASSERT(0 != left);
+
+	if (_MALI_OSK_ERR_OK != _mali_osk_lock_wait(info->mutex, _MALI_OSK_LOCKMODE_RW)) return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+
+	ret_allocation = _mali_osk_malloc( sizeof(block_allocator_allocation) );
+
+	if ( NULL == ret_allocation )
+	{
+		/* Failure; try another allocator by returning MALI_MEM_ALLOC_NONE */
+		_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+		return result;
+	}
+
+	ret_allocation->start_offset = *offset;
+	ret_allocation->mapping_length = 0;
+
+	while ((left > 0) && (info->first_free))
+	{
+		block_info * block;
+		u32 phys_addr;
+		u32 padding;
+		u32 current_mapping_size;
+
+		block = info->first_free;
+		info->first_free = info->first_free->next;
+		block->next = last_allocated;
+		last_allocated = block;
+
+		phys_addr = get_phys(info, block);
+
+		padding = *offset & (MALI_BLOCK_SIZE-1);
+
+ 		if (MALI_BLOCK_SIZE - padding < left)
+		{
+			current_mapping_size = MALI_BLOCK_SIZE - padding;
+		}
+		else
+		{
+			current_mapping_size = left;
+		}
+
+		if (_MALI_OSK_ERR_OK != mali_allocation_engine_map_physical(engine, descriptor, *offset, phys_addr + padding, info->cpu_usage_adjust, current_mapping_size))
+		{
+			MALI_DEBUG_PRINT(1, ("Mapping of physical memory  failed\n"));
+			result = MALI_MEM_ALLOC_INTERNAL_FAILURE;
+			mali_allocation_engine_unmap_physical(engine, descriptor, ret_allocation->start_offset, ret_allocation->mapping_length, (_mali_osk_mem_mapregion_flags_t)0);
+
+			/* release all memory back to the pool */
+			while (last_allocated)
+			{
+				/* This relinks every block we've just allocated back into the free-list */
+				block = last_allocated->next;
+				last_allocated->next = info->first_free;
+				info->first_free = last_allocated;
+				last_allocated = block;
+			}
+
+			break;
+		}
+
+		*offset += current_mapping_size;
+		left -= current_mapping_size;
+		ret_allocation->mapping_length += current_mapping_size;
+	}
+
+	_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+
+	if (last_allocated)
+	{
+		if (left) result = MALI_MEM_ALLOC_PARTIAL;
+		else result = MALI_MEM_ALLOC_FINISHED;
+
+		/* Record all the information about this allocation */
+		ret_allocation->last_allocated = last_allocated;
+		ret_allocation->engine = engine;
+		ret_allocation->descriptor = descriptor;
+
+		alloc_info->ctx = info;
+		alloc_info->handle = ret_allocation;
+		alloc_info->release = block_allocator_release;
+	}
+	else
+	{
+		/* Free the allocation information - nothing to be passed back */
+		_mali_osk_free( ret_allocation );
+	}
+
+	return result;
+}
+
+static void block_allocator_release(void * ctx, void * handle)
+{
+	block_allocator * info;
+	block_info * block, * next;
+	block_allocator_allocation *allocation;
+
+	MALI_DEBUG_ASSERT_POINTER(ctx);
+	MALI_DEBUG_ASSERT_POINTER(handle);
+
+	info = (block_allocator*)ctx;
+	allocation = (block_allocator_allocation*)handle;
+	block = allocation->last_allocated;
+
+	MALI_DEBUG_ASSERT_POINTER(block);
+
+	if (_MALI_OSK_ERR_OK != _mali_osk_lock_wait(info->mutex, _MALI_OSK_LOCKMODE_RW))
+	{
+		MALI_DEBUG_PRINT(1, ("allocator release: Failed to get mutex\n"));
+		return;
+	}
+
+	/* unmap */
+	mali_allocation_engine_unmap_physical(allocation->engine, allocation->descriptor, allocation->start_offset, allocation->mapping_length, (_mali_osk_mem_mapregion_flags_t)0);
+
+	while (block)
+	{
+		MALI_DEBUG_ASSERT(!((block < info->all_blocks) || (block > (info->all_blocks + info->num_blocks))));
+
+		next = block->next;
+
+		/* relink into free-list */
+		block->next = info->first_free;
+		info->first_free = block;
+
+		/* advance the loop */
+		block = next;
+	}
+
+	_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+
+	_mali_osk_free( allocation );}
+
+
+static mali_physical_memory_allocation_result block_allocator_allocate_page_table_block(void * ctx, mali_page_table_block * block)
+{
+	block_allocator * info;
+	mali_physical_memory_allocation_result result = MALI_MEM_ALLOC_INTERNAL_FAILURE;
+
+	MALI_DEBUG_ASSERT_POINTER(ctx);
+	MALI_DEBUG_ASSERT_POINTER(block);
+	info = (block_allocator*)ctx;
+
+	if (_MALI_OSK_ERR_OK != _mali_osk_lock_wait(info->mutex, _MALI_OSK_LOCKMODE_RW)) return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+
+	if (NULL != info->first_free)
+	{
+		void * virt;
+		u32 phys;
+		u32 size;
+		block_info * alloc;
+		alloc = info->first_free;
+
+		phys = get_phys(info, alloc); /* Does not modify info or alloc */
+		size = MALI_BLOCK_SIZE; /* Must be multiple of MALI_MMU_PAGE_SIZE */
+		virt = _mali_osk_mem_mapioregion( phys, size, "Mali block allocator page tables" );
+
+		/* Failure of _mali_osk_mem_mapioregion will result in MALI_MEM_ALLOC_INTERNAL_FAILURE,
+		 * because it's unlikely another allocator will be able to map in. */
+
+		if ( NULL != virt )
+		{
+			block->ctx = info; /* same as incoming ctx */
+			block->handle = alloc;
+			block->phys_base = phys;
+			block->size = size;
+			block->release = block_allocator_release_page_table_block;
+			block->mapping = virt;
+
+			info->first_free = alloc->next;
+
+			alloc->next = NULL; /* Could potentially link many blocks together instead */
+
+			result = MALI_MEM_ALLOC_FINISHED;
+		}
+	}
+	else result = MALI_MEM_ALLOC_NONE;
+
+	_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+
+	return result;
+}
+
+
+static void block_allocator_release_page_table_block( mali_page_table_block *page_table_block )
+{
+	block_allocator * info;
+	block_info * block, * next;
+
+	MALI_DEBUG_ASSERT_POINTER( page_table_block );
+
+	info = (block_allocator*)page_table_block->ctx;
+	block = (block_info*)page_table_block->handle;
+
+	MALI_DEBUG_ASSERT_POINTER(info);
+	MALI_DEBUG_ASSERT_POINTER(block);
+
+
+	if (_MALI_OSK_ERR_OK != _mali_osk_lock_wait(info->mutex, _MALI_OSK_LOCKMODE_RW))
+	{
+		MALI_DEBUG_PRINT(1, ("allocator release: Failed to get mutex\n"));
+		return;
+	}
+
+	/* Unmap all the physical memory at once */
+	_mali_osk_mem_unmapioregion( page_table_block->phys_base, page_table_block->size, page_table_block->mapping );
+
+	/** @note This loop handles the case where more than one block_info was linked.
+	 * Probably unnecssary for page table block releasing. */
+	while (block)
+	{
+		next = block->next;
+
+		MALI_DEBUG_ASSERT(!((block < info->all_blocks) || (block > (info->all_blocks + info->num_blocks))));
+
+		block->next = info->first_free;
+		info->first_free = block;
+
+		block = next;
+	}
+
+	_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+}
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_block_allocator.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_block_allocator.h
new file mode 100644
index 00000000000..c3b5761e23a
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_block_allocator.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_BLOCK_ALLOCATOR_H__
+#define __MALI_BLOCK_ALLOCATOR_H__
+
+#include "mali_kernel_memory_engine.h"
+
+mali_physical_memory_allocator * mali_block_allocator_create(u32 base_address, u32 cpu_usage_adjust, u32 size, const char *name);
+
+#endif /* __MALI_BLOCK_ALLOCATOR_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_GP2.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_GP2.c
new file mode 100644
index 00000000000..e91abe120cb
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_GP2.c
@@ -0,0 +1,1418 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_kernel_core.h"
+#include "mali_kernel_subsystem.h"
+#include "regs/mali_gp_regs.h"
+#include "mali_kernel_rendercore.h"
+#include "mali_osk.h"
+#include "mali_osk_list.h"
+#if MALI_TIMELINE_PROFILING_ENABLED
+#include "mali_kernel_profiling.h"
+#endif
+#if defined(USING_MALI400_L2_CACHE)
+#include "mali_kernel_l2_cache.h"
+#endif
+#if USING_MMU
+#include "mali_kernel_mem_mmu.h" /* Needed for mali_kernel_mmu_force_bus_reset() */
+#endif
+
+#if defined(USING_MALI200)
+#define MALI_GP_SUBSYSTEM_NAME "MaliGP2"
+#define MALI_GP_CORE_TYPE      _MALI_GP2
+#elif defined(USING_MALI400)
+#define MALI_GP_SUBSYSTEM_NAME "Mali-400 GP"
+#define MALI_GP_CORE_TYPE      _MALI_400_GP
+#else
+#error "No supported mali core defined"
+#endif
+
+#define GET_JOB_EMBEDDED_PTR(job) (&((job)->embedded_core_job))
+#define GET_JOBGP2_PTR(job_extern) _MALI_OSK_CONTAINER_OF(job_extern, maligp_job, embedded_core_job)
+
+/* Initialized when this subsystem is initialized. This is determined by the
+ * position in subsystems[], and so the value used to initialize this is
+ * determined at compile time */
+static mali_kernel_subsystem_identifier mali_subsystem_gp_id = -1;
+
+static mali_core_renderunit * last_gp_core_cookie = NULL;
+
+/* Describing a maligp job settings */
+typedef struct maligp_job
+{
+	/* The general job struct common for all mali cores */
+	mali_core_job embedded_core_job;
+	_mali_uk_gp_start_job_s user_input;
+
+	u32 irq_status;
+	u32 status_reg_on_stop;
+	u32 perf_counter0;
+	u32 perf_counter1;
+	u32 vscl_stop_addr;
+	u32 plbcl_stop_addr;
+	u32 heap_current_addr;
+
+	/* The data we will return back to the user */
+	_mali_osk_notification_t *notification_obj;
+
+	int is_stalled_waiting_for_more_memory;
+
+	u32 active_mask;
+	/* progress checking */
+	u32 last_vscl;
+	u32 last_plbcl;
+	/* extended progress checking, only enabled when we can use one of the performance counters */
+	u32 have_extended_progress_checking;
+	u32 vertices;
+
+#if defined(USING_MALI400_L2_CACHE)
+	u32 perf_counter_l2_src0;
+	u32 perf_counter_l2_src1;
+	u32 perf_counter_l2_val0;
+	u32 perf_counter_l2_val1;
+#endif
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+	u32 pid;
+	u32 tid;
+#endif
+} maligp_job;
+
+/*Functions Exposed to the General External System through
+  function pointers.*/
+
+static _mali_osk_errcode_t maligp_subsystem_startup(mali_kernel_subsystem_identifier id);
+#if USING_MMU
+static _mali_osk_errcode_t maligp_subsystem_mmu_connect(mali_kernel_subsystem_identifier id);
+#endif
+static void maligp_subsystem_terminate(mali_kernel_subsystem_identifier id);
+static _mali_osk_errcode_t maligp_subsystem_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue);
+static void maligp_subsystem_session_end(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot);
+static _mali_osk_errcode_t maligp_subsystem_core_system_info_fill(_mali_system_info* info);
+static _mali_osk_errcode_t maligp_renderunit_create(_mali_osk_resource_t * resource);
+#if USING_MMU
+static void maligp_subsystem_broadcast_notification(mali_core_notification_message message, u32 data);
+#endif
+#if MALI_STATE_TRACKING
+void maligp_subsystem_dump_state(void);
+#endif
+
+/* Internal support functions  */
+static _mali_osk_errcode_t maligp_core_version_legal( mali_core_renderunit *core );
+static void maligp_raw_reset( mali_core_renderunit *core);
+static void maligp_reset_hard(struct mali_core_renderunit * core);
+static void maligp_reset(mali_core_renderunit *core);
+static void maligp_initialize_registers_mgmt(mali_core_renderunit *core );
+
+#ifdef DEBUG
+static void maligp_print_regs(int debug_level, mali_core_renderunit *core);
+#endif
+
+/* Functions exposed to mali_core system through functionpointers
+   in the subsystem struct. */
+static _mali_osk_errcode_t subsystem_maligp_start_job(mali_core_job * job, mali_core_renderunit * core);
+static u32 subsystem_maligp_irq_handler_upper_half(mali_core_renderunit * core);
+static int subsystem_maligp_irq_handler_bottom_half(mali_core_renderunit* core);
+static _mali_osk_errcode_t subsystem_maligp_get_new_job_from_user(struct mali_core_session * session, void * argument);
+static _mali_osk_errcode_t subsystem_maligp_suspend_response(struct mali_core_session * session, void * argument);
+static void subsystem_maligp_return_job_to_user(mali_core_job * job, mali_subsystem_job_end_code end_status);
+static void subsystem_maligp_renderunit_delete(mali_core_renderunit * core);
+static void subsystem_maligp_renderunit_reset_core(struct mali_core_renderunit * core, mali_core_reset_style style );
+static void subsystem_maligp_renderunit_probe_core_irq_trigger(struct mali_core_renderunit* core);
+static _mali_osk_errcode_t subsystem_maligp_renderunit_probe_core_irq_finished(struct mali_core_renderunit* core);
+static void subsystem_maligp_renderunit_stop_bus(struct mali_core_renderunit* core);
+
+/* Variables */
+static register_address_and_value default_mgmt_regs[] =
+{
+	{ MALIGP2_REG_ADDR_MGMT_INT_MASK, MALIGP2_REG_VAL_IRQ_MASK_USED }
+};
+
+
+/* This will be one of the subsystems in the array of subsystems:
+	static struct mali_kernel_subsystem * subsystems[];
+  found in file: mali_kernel_core.c
+*/
+
+struct mali_kernel_subsystem mali_subsystem_gp2=
+{
+	maligp_subsystem_startup,                   /* startup */
+	maligp_subsystem_terminate,                 /* shutdown */
+#if USING_MMU
+	maligp_subsystem_mmu_connect,               /* load_complete */
+#else
+    NULL,
+#endif
+	maligp_subsystem_core_system_info_fill,     /* system_info_fill */
+	maligp_subsystem_session_begin,             /* session_begin */
+	maligp_subsystem_session_end,               /* session_end */
+#if USING_MMU
+	maligp_subsystem_broadcast_notification,    /* broadcast_notification */
+#else
+    NULL,
+#endif
+#if MALI_STATE_TRACKING
+	maligp_subsystem_dump_state,                /* dump_state */
+#endif
+} ;
+
+static mali_core_subsystem subsystem_maligp ;
+
+static _mali_osk_errcode_t maligp_subsystem_startup(mali_kernel_subsystem_identifier id)
+{
+	mali_core_subsystem * subsystem;
+
+	MALI_DEBUG_PRINT(3, ("Mali GP: maligp_subsystem_startup\n") ) ;
+
+    mali_subsystem_gp_id = id;
+
+    /* All values get 0 as default */
+	_mali_osk_memset(&subsystem_maligp, 0, sizeof(*subsystem));
+
+	subsystem = &subsystem_maligp;
+	subsystem->start_job = &subsystem_maligp_start_job;
+	subsystem->irq_handler_upper_half = &subsystem_maligp_irq_handler_upper_half;
+	subsystem->irq_handler_bottom_half = &subsystem_maligp_irq_handler_bottom_half;
+	subsystem->get_new_job_from_user = &subsystem_maligp_get_new_job_from_user;
+	subsystem->suspend_response = &subsystem_maligp_suspend_response;
+	subsystem->return_job_to_user = &subsystem_maligp_return_job_to_user;
+	subsystem->renderunit_delete = &subsystem_maligp_renderunit_delete;
+	subsystem->reset_core = &subsystem_maligp_renderunit_reset_core;
+	subsystem->stop_bus = &subsystem_maligp_renderunit_stop_bus;
+	subsystem->probe_core_irq_trigger = &subsystem_maligp_renderunit_probe_core_irq_trigger;
+	subsystem->probe_core_irq_acknowledge = &subsystem_maligp_renderunit_probe_core_irq_finished;
+
+	/* Setting variables in the general core part of the subsystem.*/
+	subsystem->name = MALI_GP_SUBSYSTEM_NAME;
+	subsystem->core_type = MALI_GP_CORE_TYPE;
+	subsystem->id = id;
+
+	/* Initiates the rest of the general core part of the subsystem */
+    MALI_CHECK_NO_ERROR(mali_core_subsystem_init( subsystem ));
+
+	/* This will register the function for adding MALIGP2 cores to the subsystem */
+#if defined(USING_MALI200)
+    MALI_CHECK_NO_ERROR(_mali_kernel_core_register_resource_handler(MALIGP2, maligp_renderunit_create));
+#endif
+#if defined(USING_MALI400)
+	MALI_CHECK_NO_ERROR(_mali_kernel_core_register_resource_handler(MALI400GP, maligp_renderunit_create));
+#endif
+
+	MALI_DEBUG_PRINT(6, ("Mali GP: maligp_subsystem_startup\n") ) ;
+
+	MALI_SUCCESS;
+}
+
+#if USING_MMU
+static _mali_osk_errcode_t maligp_subsystem_mmu_connect(mali_kernel_subsystem_identifier id)
+{
+       mali_core_subsystem_attach_mmu(&subsystem_maligp);
+       MALI_SUCCESS; /* OK */
+}
+#endif
+
+static void maligp_subsystem_terminate(mali_kernel_subsystem_identifier id)
+{
+	MALI_DEBUG_PRINT(3, ("Mali GP: maligp_subsystem_terminate\n") ) ;
+	mali_core_subsystem_cleanup(&subsystem_maligp);
+}
+
+static _mali_osk_errcode_t maligp_subsystem_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue)
+{
+	mali_core_session * session;
+
+	MALI_DEBUG_PRINT(3, ("Mali GP: maligp_subsystem_session_begin\n") ) ;
+    MALI_CHECK_NON_NULL(session = _mali_osk_malloc( sizeof(*session) ), _MALI_OSK_ERR_FAULT);
+
+	_mali_osk_memset(session, 0, sizeof(*session) );
+	*slot = (mali_kernel_subsystem_session_slot)session;
+
+	session->subsystem = &subsystem_maligp;
+
+	session->notification_queue = queue;
+
+#if USING_MMU
+	session->mmu_session = mali_session_data;
+#endif
+
+	mali_core_session_begin(session);
+
+	MALI_DEBUG_PRINT(6, ("Mali GP: maligp_subsystem_session_begin\n") ) ;
+
+    MALI_SUCCESS;
+}
+
+static void maligp_subsystem_session_end(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot)
+{
+	mali_core_session * session;
+	/** @note mali_session_data not needed here */
+
+	MALI_DEBUG_PRINT(3, ("Mali GP: maligp_subsystem_session_end\n") ) ;
+	if ( NULL==slot || NULL==*slot)
+	{
+		MALI_PRINT_ERROR(("Input slot==NULL"));
+		return;
+	}
+	session = (mali_core_session *)*slot;
+	mali_core_session_close(session);
+
+	_mali_osk_free(session);
+	*slot = NULL;
+
+	MALI_DEBUG_PRINT(6, ("Mali GP: maligp_subsystem_session_end\n") ) ;
+}
+
+/**
+ * We fill in info about all the cores we have
+ * @param info Pointer to system info struct to update
+ * @return _MALI_OSK_ERR_OK on success, or another _mali_osk_errcode_t for errors.
+ */
+static _mali_osk_errcode_t maligp_subsystem_core_system_info_fill(_mali_system_info* info)
+{
+	return mali_core_subsystem_system_info_fill(&subsystem_maligp, info);
+}
+
+static _mali_osk_errcode_t maligp_renderunit_create(_mali_osk_resource_t * resource)
+{
+	mali_core_renderunit *core;
+	int err;
+
+	MALI_DEBUG_PRINT(3, ("Mali GP: maligp_renderunit_create\n") ) ;
+	/* Checking that the resource settings are correct */
+#if defined(USING_MALI200)
+	if(MALIGP2 != resource->type)
+	{
+		MALI_PRINT_ERROR(("Can not register this resource as a " MALI_GP_SUBSYSTEM_NAME " core."));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+#elif defined(USING_MALI400)
+	if(MALI400GP != resource->type)
+	{
+		MALI_PRINT_ERROR(("Can not register this resource as a " MALI_GP_SUBSYSTEM_NAME " core."));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+#endif
+	if ( 0 != resource->size )
+	{
+		MALI_PRINT_ERROR(("Memory size set to " MALI_GP_SUBSYSTEM_NAME " core should be zero."));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	if ( NULL == resource->description )
+	{
+		MALI_PRINT_ERROR(("A " MALI_GP_SUBSYSTEM_NAME " core needs a unique description field"));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* Create a new core object */
+	core = (mali_core_renderunit*) _mali_osk_malloc(sizeof(*core));
+	if ( NULL == core )
+	{
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* Variables set to be able to open and register the core */
+	core->subsystem             = &subsystem_maligp ;
+	core->registers_base_addr   = resource->base ;
+	core->size                  = MALIGP2_REGISTER_ADDRESS_SPACE_SIZE ;
+	core->description           = resource->description;
+	core->irq_nr                = resource->irq ;
+#if USING_MMU
+	core->mmu_id                = resource->mmu_id;
+	core->mmu                   = NULL;
+#endif
+#if USING_MALI_PMM
+	/* Set up core's PMM id */
+	core->pmm_id = MALI_PMM_CORE_GP;
+#endif
+
+	err = mali_core_renderunit_init( core );
+    if (_MALI_OSK_ERR_OK != err)
+    {
+		MALI_DEBUG_PRINT(1, ("Failed to initialize renderunit\n"));
+		goto exit_on_error0;
+    }
+
+	/* Map the new core object, setting: core->registers_mapped  */
+	err = mali_core_renderunit_map_registers(core);
+	if (_MALI_OSK_ERR_OK != err) goto exit_on_error1;
+
+	/* Check that the register mapping of the core works.
+	Return 0 if maligp core is present and accessible. */
+	if (mali_benchmark) {
+		core->core_version = MALI_GP_PRODUCT_ID << 16;
+	} else {
+		core->core_version = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_VERSION);
+	}
+
+	err = maligp_core_version_legal(core);
+	if (_MALI_OSK_ERR_OK != err) goto exit_on_error2;
+
+	/* Reset the core. Put the core into a state where it can start to render. */
+	maligp_reset(core);
+
+	/* Registering IRQ, init the work_queue_irq_handle */
+	/* Adding this core as an available renderunit in the subsystem. */
+	err = mali_core_subsystem_register_renderunit(&subsystem_maligp, core);
+	if (_MALI_OSK_ERR_OK != err) goto exit_on_error2;
+
+#ifdef DEBUG
+	MALI_DEBUG_PRINT(4, ("Mali GP: Initial Register settings:\n"));
+	maligp_print_regs(4, core);
+#endif
+
+	MALI_DEBUG_PRINT(6, ("Mali GP: maligp_renderunit_create\n") ) ;
+
+	MALI_SUCCESS;
+
+exit_on_error2:
+	mali_core_renderunit_unmap_registers(core);
+exit_on_error1:
+    mali_core_renderunit_term(core);
+exit_on_error0:
+	_mali_osk_free( core ) ;
+	MALI_PRINT_ERROR(("Renderunit NOT created."));
+    MALI_ERROR((_mali_osk_errcode_t)err);
+}
+
+#if USING_MMU
+/* Used currently only for signalling when MMU has a pagefault */
+static void maligp_subsystem_broadcast_notification(mali_core_notification_message message, u32 data)
+{
+	mali_core_subsystem_broadcast_notification(&subsystem_maligp, message, data);
+}
+#endif
+
+#ifdef DEBUG
+static void maligp_print_regs(int debug_level, mali_core_renderunit *core)
+{
+	if (debug_level <= mali_debug_level)
+	{
+		MALI_DEBUG_PRINT(1, ("  VS 0x%08X 0x%08X, PLBU 0x%08X 0x%08X ALLOC 0x%08X 0x%08X\n",
+			mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_VSCL_START_ADDR),
+			mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_VSCL_END_ADDR),
+			mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBUCL_START_ADDR),
+			mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBUCL_END_ADDR),
+			mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_START_ADDR),
+			mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_END_ADDR))
+		);
+		MALI_DEBUG_PRINT(1, ("  IntRaw 0x%08X  IntMask 0x%08X, Status 0x%02X  Ver: 0x%08X \n",
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_INT_RAWSTAT),
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_INT_MASK),
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_STATUS),
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_VERSION)));
+
+		MALI_DEBUG_PRINT(1, ("  PERF_CNT Enbl:%d %d Src: %02d %02d  VAL: 0x%08X 0x%08X\n",
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_ENABLE),
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_ENABLE),
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_SRC),
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_SRC),
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_VALUE),
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_VALUE)));
+
+		MALI_DEBUG_PRINT(1, ("  VS_START 0x%08X PLBU_START 0x%08X  AXI_ERR 0x%08X\n",
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_VSCL_START_ADDR_READ),
+				mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBCL_START_ADDR_READ),
+				mali_core_renderunit_register_read(core, MALIGP2_CONTR_AXI_BUS_ERROR_STAT)));
+	}
+}
+#endif
+
+static _mali_osk_errcode_t maligp_core_version_legal( mali_core_renderunit *core )
+{
+	u32 mali_type;
+
+	mali_type = core->core_version >> 16;
+
+#if defined(USING_MALI400)
+	if (  MALI400_GP_PRODUCT_ID != mali_type && MALI300_GP_PRODUCT_ID != mali_type )
+#else
+	if (  MALI_GP_PRODUCT_ID != mali_type )
+#endif
+	{
+		MALI_PRINT_ERROR(("Error: reading this from maligp version register: 0x%x\n", core->core_version));
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+	MALI_DEBUG_PRINT(3, ("Mali GP: core_version_legal: Reads correct mali version: %d\n", core->core_version )) ;
+    MALI_SUCCESS;
+}
+
+static void subsystem_maligp_renderunit_stop_bus(struct mali_core_renderunit* core)
+{
+	mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_CMD, MALIGP2_REG_VAL_CMD_STOP_BUS);
+}
+
+static void maligp_reset( mali_core_renderunit *core )
+{
+	if (!mali_benchmark) {
+		maligp_raw_reset(core);
+		maligp_initialize_registers_mgmt(core);
+	}
+}
+
+
+static void maligp_reset_hard( mali_core_renderunit *core )
+{
+	const int reset_finished_loop_count = 15;
+	const u32 reset_wait_target_register = MALIGP2_REG_ADDR_MGMT_WRITE_BOUND_LOW;
+	const u32 reset_invalid_value = 0xC0FFE000;
+	const u32 reset_check_value = 0xC01A0000;
+	const u32 reset_default_value = 0;
+	int i;
+
+	mali_core_renderunit_register_write(core, reset_wait_target_register, reset_invalid_value);
+
+	mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_CMD, MALIGP2_REG_VAL_CMD_RESET);
+
+	for (i = 0; i < reset_finished_loop_count; i++)
+	{
+		mali_core_renderunit_register_write(core, reset_wait_target_register, reset_check_value);
+		if (reset_check_value == mali_core_renderunit_register_read(core, reset_wait_target_register))
+		{
+			MALI_DEBUG_PRINT(5, ("Reset loop exiting after %d iterations\n", i));
+			break;
+		}
+	}
+
+	if (i == reset_finished_loop_count)
+	{
+		MALI_DEBUG_PRINT(1, ("The reset loop didn't work\n"));
+	}
+
+	mali_core_renderunit_register_write(core, reset_wait_target_register, reset_default_value); /* set it back to the default */
+	mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_CLEAR, MALIGP2_REG_VAL_IRQ_MASK_ALL);
+	
+
+}
+
+static void maligp_raw_reset( mali_core_renderunit *core )
+{
+	int i;
+	const int request_loop_count = 20;
+
+	MALI_DEBUG_PRINT(4, ("Mali GP: maligp_raw_reset: %s\n", core->description)) ;
+	if (mali_benchmark) return;
+
+	mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_MASK, 0); /* disable the IRQs */
+
+#if defined(USING_MALI200)
+
+	mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_CMD, MALIGP2_REG_VAL_CMD_STOP_BUS);
+
+	for (i = 0; i < request_loop_count; i++)
+	{
+		if (mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_STATUS) & MALIGP2_REG_VAL_STATUS_BUS_STOPPED) break;
+		_mali_osk_time_ubusydelay(10);
+	}
+
+	MALI_DEBUG_PRINT_IF(1, request_loop_count == i, ("Mali GP: Bus was never stopped during core reset\n"));
+
+	if (request_loop_count==i)
+	{
+		/* Could not stop bus connections from core, probably because some of the already pending
+		bus request has had a page fault, and therefore can not complete before the MMU does PageFault
+		handling. This can be treated as a heavier reset function - which unfortunately reset all
+		the cores on this MMU in addition to the MMU itself */
+#if USING_MMU
+		if ((NULL!=core->mmu) && (MALI_FALSE == core->error_recovery))
+		{
+			MALI_DEBUG_PRINT(1, ("Mali GP: Forcing MMU bus reset\n"));
+			mali_kernel_mmu_force_bus_reset(core->mmu);
+			return;
+		}
+#endif
+		MALI_PRINT(("A MMU reset did not allow GP to stop its bus, system failure, unable to recover\n"));
+		return;
+	}
+
+	/* the bus was stopped OK, complete the reset */
+	/* use the hard reset routine to do the actual reset */
+	maligp_reset_hard(core);
+
+#elif defined(USING_MALI400)
+
+	mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_CLEAR, MALI400GP_REG_VAL_IRQ_RESET_COMPLETED);
+	mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_CMD, MALI400GP_REG_VAL_CMD_SOFT_RESET);
+
+	for (i = 0; i < request_loop_count; i++)
+	{
+		if (mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_INT_RAWSTAT) & /*Bitwise OR*/
+				MALI400GP_REG_VAL_IRQ_RESET_COMPLETED) break;
+		_mali_osk_time_ubusydelay(10);
+	}
+
+	if ( request_loop_count==i )
+	{
+#if USING_MMU
+		/* Could not stop bus connections from core, probably because some of the already pending
+		   bus request has had a page fault, and therefore can not complete before the MMU does PageFault
+		   handling. This can be treated as a heavier reset function - which unfortunately reset all
+		   the cores on this MMU in addition to the MMU itself */
+		if ((NULL!=core->mmu) && (MALI_FALSE == core->error_recovery))
+		{
+			MALI_DEBUG_PRINT(1, ("Mali GP: Forcing Bus reset\n"));
+			mali_kernel_mmu_force_bus_reset(core->mmu);
+			return;
+		}
+#endif
+		MALI_PRINT(("A MMU reset did not allow GP to stop its bus, system failure, unable to recover\n"));
+	}
+	else
+	{
+		mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_CLEAR, MALIGP2_REG_VAL_IRQ_MASK_ALL);
+	}
+
+#else
+#error "no supported mali core defined"
+#endif
+}
+
+/* Sets the registers on maligp according to the const default_mgmt_regs array. */
+static void maligp_initialize_registers_mgmt(mali_core_renderunit *core )
+{
+	int i;
+
+	MALI_DEBUG_PRINT(6, ("Mali GP: maligp_initialize_registers_mgmt: %s\n", core->description)) ;
+	for(i=0 ; i< (sizeof(default_mgmt_regs)/sizeof(*default_mgmt_regs)) ; ++i)
+	{
+		mali_core_renderunit_register_write(core, default_mgmt_regs[i].address, default_mgmt_regs[i].value);
+	}
+}
+
+
+/* Start this job on this core. Return MALI_TRUE if the job was started. */
+static _mali_osk_errcode_t subsystem_maligp_start_job(mali_core_job * job, mali_core_renderunit * core)
+{
+	maligp_job 	*jobgp;
+	u32 startcmd;
+	/* The local extended version of the general structs */
+	jobgp  = _MALI_OSK_CONTAINER_OF(job, maligp_job, embedded_core_job);
+
+	startcmd = 0;
+	if ( jobgp->user_input.frame_registers[0] != jobgp->user_input.frame_registers[1] )
+	{
+		startcmd |= (u32) MALIGP2_REG_VAL_CMD_START_VS;
+	}
+
+	if ( jobgp->user_input.frame_registers[2] != jobgp->user_input.frame_registers[3] )
+	{
+		startcmd |= (u32) MALIGP2_REG_VAL_CMD_START_PLBU;
+	}
+
+	if(0 == startcmd)
+	{
+		MALI_DEBUG_PRINT(4, ("Mali GP: Job: 0x%08x  WILL NOT START SINCE JOB HAS ILLEGAL ADDRESSES\n",
+				(u32)jobgp->user_input.user_job_ptr));
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+
+#ifdef DEBUG
+	MALI_DEBUG_PRINT(4, ("Mali GP: Registers Start\n"));
+	maligp_print_regs(4, core);
+#endif
+
+
+	mali_core_renderunit_register_write_array(
+			core,
+			MALIGP2_REG_ADDR_MGMT_VSCL_START_ADDR,
+			&(jobgp->user_input.frame_registers[0]),
+			sizeof(jobgp->user_input.frame_registers)/sizeof(jobgp->user_input.frame_registers[0]));
+
+	/* This selects which performance counters we are reading */
+	if ( 0 != jobgp->user_input.perf_counter_flag )
+	{
+		if ( jobgp->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_SRC0_ENABLE)
+		{
+			mali_core_renderunit_register_write(
+					core,
+					MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_SRC,
+					jobgp->user_input.perf_counter_src0);
+
+			mali_core_renderunit_register_write(
+					core,
+					MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_ENABLE,
+					MALIGP2_REG_VAL_PERF_CNT_ENABLE);
+		}
+
+		if ( jobgp->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_SRC1_ENABLE)
+		{
+			mali_core_renderunit_register_write(
+					core,
+					MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_SRC,
+					jobgp->user_input.perf_counter_src1);
+
+			mali_core_renderunit_register_write(
+					core,
+					MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_ENABLE,
+					MALIGP2_REG_VAL_PERF_CNT_ENABLE);
+		}
+
+#if defined(USING_MALI400_L2_CACHE)
+		if ( jobgp->user_input.perf_counter_flag & (_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC0_ENABLE|_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC1_ENABLE) )
+		{
+			int force_reset = ( jobgp->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_L2_RESET ) ? 1 : 0;
+			u32 src0 = 0;
+			u32 src1 = 0;
+
+			if ( jobgp->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC0_ENABLE )
+			{
+				src0 = jobgp->user_input.perf_counter_l2_src0;
+			}
+			if ( jobgp->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC1_ENABLE )
+			{
+				src1 = jobgp->user_input.perf_counter_l2_src1;
+			}
+
+			mali_kernel_l2_cache_set_perf_counters(src0, src1, force_reset); /* will activate and possibly reset counters */
+
+			/* Now, retrieve the current values, so we can substract them when the job has completed */
+			mali_kernel_l2_cache_get_perf_counters(&jobgp->perf_counter_l2_src0,
+			                                       &jobgp->perf_counter_l2_val0,
+			                                       &jobgp->perf_counter_l2_src1,
+			                                       &jobgp->perf_counter_l2_val1);
+		}
+#endif
+	}
+
+	if ( 0 == (jobgp->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_SRC1_ENABLE))
+	{
+		/* extended progress checking can be enabled */
+
+		jobgp->have_extended_progress_checking = 1;
+
+		mali_core_renderunit_register_write(
+				core,
+		MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_SRC,
+		MALIGP2_REG_VAL_PERF_CNT1_SRC_NUMBER_OF_VERTICES_PROCESSED
+										   );
+
+		mali_core_renderunit_register_write(
+				core,
+		MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_ENABLE,
+		MALIGP2_REG_VAL_PERF_CNT_ENABLE);
+	}
+
+	subsystem_flush_mapped_mem_cache();
+
+	MALI_DEBUG_PRINT(4, ("Mali GP: STARTING GP WITH CMD: 0x%x\n", startcmd));
+
+	/* This is the command that starts the Core */
+	mali_core_renderunit_register_write(core,
+										MALIGP2_REG_ADDR_MGMT_CMD,
+										startcmd);
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+	_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_START|MALI_PROFILING_MAKE_EVENT_CHANNEL_GP(core->core_number), jobgp->pid, jobgp->tid, 0, 0, 0);
+#endif
+
+	MALI_SUCCESS;
+}
+
+/* Check if given core has an interrupt pending. Return MALI_TRUE and set mask to 0 if pending */
+
+static u32 subsystem_maligp_irq_handler_upper_half(mali_core_renderunit * core)
+{
+	u32 irq_readout;
+
+	if (mali_benchmark) {
+		return (core->current_job ? 1 : 0); /* simulate irq is pending when a job is pending */
+	}
+
+	irq_readout = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_INT_STAT);
+
+	MALI_DEBUG_PRINT(5, ("Mali GP: IRQ: %04x\n", irq_readout)) ;
+
+	if ( MALIGP2_REG_VAL_IRQ_MASK_NONE != irq_readout )
+	{
+		/* Mask out all IRQs from this core until IRQ is handled */
+		mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_MASK	, MALIGP2_REG_VAL_IRQ_MASK_NONE);
+		/* We do need to handle this in a bottom half, return 1 */
+		return 1;
+	}
+	return 0;
+}
+
+/* This function should check if the interrupt indicates that job was finished.
+If so it should update the job-struct, reset the core registers, and return MALI_TRUE, .
+If the job is still working after this function it should return MALI_FALSE.
+The function must also enable the bits in the interrupt mask for the core.
+Called by the bottom half interrupt function. */
+static int subsystem_maligp_irq_handler_bottom_half(mali_core_renderunit* core)
+{
+	mali_core_job * job;
+	maligp_job * jobgp;
+	u32 irq_readout;
+	u32 core_status;
+	u32 vscl;
+	u32 plbcl;
+
+	job = core->current_job;
+
+	if (mali_benchmark) {
+		MALI_DEBUG_PRINT(3, ("MaliGP: Job: Benchmark\n") );
+		irq_readout = MALIGP2_REG_VAL_IRQ_VS_END_CMD_LST | MALIGP2_REG_VAL_IRQ_PLBU_END_CMD_LST;
+		core_status = 0;
+	} else {
+		irq_readout = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_INT_RAWSTAT) & MALIGP2_REG_VAL_IRQ_MASK_USED;
+		core_status = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_STATUS);
+	}
+
+	if (NULL == job)
+	{
+		MALI_DEBUG_ASSERT(CORE_IDLE==core->state);
+		if ( 0 != irq_readout )
+		{
+			MALI_PRINT_ERROR(("Interrupt from a core not running a job. IRQ: 0x%04x Status: 0x%04x", irq_readout, core_status));
+		}
+		return JOB_STATUS_END_UNKNOWN_ERR;
+	}
+	MALI_DEBUG_ASSERT(CORE_IDLE!=core->state);
+
+	jobgp = GET_JOBGP2_PTR(job);
+
+	jobgp->heap_current_addr = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_START_ADDR);
+
+	vscl = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_VSCL_START_ADDR);
+	plbcl = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBUCL_START_ADDR);
+
+	MALI_DEBUG_PRINT(3, ("Mali GP: Job: 0x%08x  IRQ RECEIVED  Rawstat: 0x%x Status: 0x%x\n",
+			(u32)jobgp->user_input.user_job_ptr, irq_readout , core_status )) ;
+
+	jobgp->irq_status |= irq_readout;
+	jobgp->status_reg_on_stop = core_status;
+
+	if ( 0 != jobgp->is_stalled_waiting_for_more_memory )
+	{
+#if MALI_TIMELINE_PROFILING_ENABLED
+		_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_STOP|MALI_PROFILING_MAKE_EVENT_CHANNEL_GP(core->core_number), 0, 0, 0, 0, 0); /* add GP and L2 counters and return status? */
+#endif
+
+		/* Readback the performance counters */
+		if (jobgp->user_input.perf_counter_flag & (_MALI_PERFORMANCE_COUNTER_FLAG_SRC0_ENABLE|_MALI_PERFORMANCE_COUNTER_FLAG_SRC1_ENABLE) )
+		{
+			jobgp->perf_counter0 = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_VALUE);
+			jobgp->perf_counter1 = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_VALUE);
+		}
+
+#if defined(USING_MALI400_L2_CACHE)
+		if (jobgp->user_input.perf_counter_flag & (_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC0_ENABLE|_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC1_ENABLE) )
+		{
+			u32 src0;
+			u32 val0;
+			u32 src1;
+			u32 val1;
+			mali_kernel_l2_cache_get_perf_counters(&src0, &val0, &src1, &val1);
+
+			if (jobgp->perf_counter_l2_src0 == src0)
+			{
+				jobgp->perf_counter_l2_val0 = val0 - jobgp->perf_counter_l2_val0;
+			}
+			else
+			{
+				jobgp->perf_counter_l2_val0 = 0;
+			}
+
+			if (jobgp->perf_counter_l2_src1 == src1)
+			{
+				jobgp->perf_counter_l2_val1 = val1 - jobgp->perf_counter_l2_val1;
+			}
+			else
+			{
+				jobgp->perf_counter_l2_val1 = 0;
+			}
+		}
+#endif
+
+		MALI_DEBUG_PRINT(2, ("Mali GP: Job aborted - userspace would not provide more heap memory.\n"));
+		return JOB_STATUS_END_OOM; /* Core is ready for more jobs.*/
+	}
+	/* finished ? */
+	else if (0 == (core_status & MALIGP2_REG_VAL_STATUS_MASK_ACTIVE))
+	{
+#if MALI_TIMELINE_PROFILING_ENABLED
+		_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_STOP|MALI_PROFILING_MAKE_EVENT_CHANNEL_GP(core->core_number), 0, 0, 0, 0, 0); /* add GP and L2 counters and return status? */
+#endif
+
+#ifdef DEBUG
+		MALI_DEBUG_PRINT(4, ("Mali GP: Registers On job end:\n"));
+		maligp_print_regs(4, core);
+#endif
+		MALI_DEBUG_PRINT_IF(5, irq_readout & 0x04, ("OOM when done, ignoring (reg.current = 0x%x, reg.end = 0x%x)\n",
+		           (void*)mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_START_ADDR),
+		           (void*)mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_END_ADDR))
+		          );
+
+
+		if (0 != jobgp->user_input.perf_counter_flag )
+		{
+			/* Readback the performance counters */
+			if (jobgp->user_input.perf_counter_flag & (_MALI_PERFORMANCE_COUNTER_FLAG_SRC0_ENABLE|_MALI_PERFORMANCE_COUNTER_FLAG_SRC1_ENABLE) )
+			{
+				jobgp->perf_counter0 = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_VALUE);
+				jobgp->perf_counter1 = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_VALUE);
+			}
+
+#if defined(USING_MALI400_L2_CACHE)
+			if (jobgp->user_input.perf_counter_flag & (_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC0_ENABLE|_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC1_ENABLE) )
+			{
+				u32 src0;
+				u32 val0;
+				u32 src1;
+				u32 val1;
+				mali_kernel_l2_cache_get_perf_counters(&src0, &val0, &src1, &val1);
+
+				if (jobgp->perf_counter_l2_src0 == src0)
+				{
+					jobgp->perf_counter_l2_val0 = val0 - jobgp->perf_counter_l2_val0;
+				}
+				else
+				{
+					jobgp->perf_counter_l2_val0 = 0;
+				}
+
+				if (jobgp->perf_counter_l2_src1 == src1)
+				{
+					jobgp->perf_counter_l2_val1 = val1 - jobgp->perf_counter_l2_val1;
+				}
+				else
+				{
+					jobgp->perf_counter_l2_val1 = 0;
+				}
+			}
+#endif
+		}
+
+		mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_CLEAR, MALIGP2_REG_VAL_IRQ_MASK_ALL);
+
+		return JOB_STATUS_END_SUCCESS; /* core idle */
+	}
+	/* sw watchdog timeout handling or time to do hang checking ? */
+	else if (
+	         (CORE_WATCHDOG_TIMEOUT == core->state) ||
+	         (
+	          (CORE_HANG_CHECK_TIMEOUT == core->state) &&
+              (
+	           (jobgp->have_extended_progress_checking ? (mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_VALUE) == jobgp->vertices) : 1/*TRUE*/) &&
+	           ((core_status & MALIGP2_REG_VAL_STATUS_VS_ACTIVE) ? (vscl == jobgp->last_vscl) : 1/*TRUE*/) &&
+	           ((core_status & MALIGP2_REG_VAL_STATUS_PLBU_ACTIVE) ? (plbcl == jobgp->last_plbcl) : 1/*TRUE*/)
+	          )
+	         )
+	        )
+	{
+		/* no progress detected, killed by the watchdog */
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+		_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_STOP|MALI_PROFILING_MAKE_EVENT_CHANNEL_GP(core->core_number), 0, 0, 0, 0, 0); /* add GP and L2 counters and return status? */
+#endif
+
+		MALI_DEBUG_PRINT(1, ("Mali GP: SW-Timeout. Regs:\n"));
+		if (core_status & MALIGP2_REG_VAL_STATUS_VS_ACTIVE) MALI_DEBUG_PRINT(1, ("vscl current = 0x%x last = 0x%x\n", (void*)vscl, (void*)jobgp->last_vscl));
+		if (core_status & MALIGP2_REG_VAL_STATUS_PLBU_ACTIVE) MALI_DEBUG_PRINT(1, ("plbcl current = 0x%x last = 0x%x\n", (void*)plbcl, (void*)jobgp->last_plbcl));
+		if (jobgp->have_extended_progress_checking) MALI_DEBUG_PRINT(1, ("vertices processed = %d, last = %d\n", mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_VALUE),
+		jobgp->vertices));
+#ifdef DEBUG
+		maligp_print_regs(2, core);
+#endif
+		return JOB_STATUS_END_HANG;
+	}
+	/* if hang timeout checking was enabled and we detected progress, will be fall down to this check */
+	/* check for PLBU OOM before the hang check to avoid the race condition of the hw wd trigging while waiting for us to handle the OOM interrupt */
+	else if ( 0 != (irq_readout & MALIGP2_REG_VAL_IRQ_PLBU_OUT_OF_MEM))
+	{
+		mali_core_session *session;
+		_mali_osk_notification_t *notific;
+		_mali_uk_gp_job_suspended_s * suspended_job;
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+		_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_SUSPEND|MALI_PROFILING_MAKE_EVENT_CHANNEL_GP(core->core_number), 0, 0, 0, 0, 0); /* add GP and L2 counters and return status? */
+#endif
+
+		session = job->session;
+
+		MALI_DEBUG_PRINT(4, ("OOM, new heap requested by GP\n"));
+		MALI_DEBUG_PRINT(4, ("Status when OOM: current = 0x%x, end = 0x%x\n",
+		         (void*)mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_START_ADDR),
+		         (void*)mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_END_ADDR))
+		       );
+
+		notific = _mali_osk_notification_create(
+
+		                                       _MALI_NOTIFICATION_GP_STALLED,
+		                                       sizeof( _mali_uk_gp_job_suspended_s )
+		                                      );
+        if ( NULL == notific)
+		{
+			MALI_PRINT_ERROR( ("Mali GP: Could not get notification object\n")) ;
+			return JOB_STATUS_END_OOM; /* Core is ready for more jobs.*/
+		}
+
+		core->state = CORE_WORKING;
+		jobgp->is_stalled_waiting_for_more_memory = 1;
+		suspended_job = (_mali_uk_gp_job_suspended_s *)notific->result_buffer; /* this is ok - result_buffer was malloc'd */
+
+		suspended_job->user_job_ptr = jobgp->user_input.user_job_ptr;
+		suspended_job->reason = _MALIGP_JOB_SUSPENDED_OUT_OF_MEMORY ;
+		suspended_job->cookie = (u32) core;
+		last_gp_core_cookie = core;
+
+		_mali_osk_notification_queue_send( session->notification_queue, notific);
+
+#ifdef DEBUG
+		maligp_print_regs(4, core);
+#endif
+
+		/* stop all active timers */
+		_mali_osk_timer_del( core->timer);
+		_mali_osk_timer_del( core->timer_hang_detection);
+		MALI_DEBUG_PRINT(4, ("Mali GP: PLBU heap empty, sending memory request to userspace\n"));
+		/* save to watchdog_jiffies what was remaining WD timeout value when OOM was triggered */
+		job->watchdog_jiffies = (long)job->watchdog_jiffies - (long)_mali_osk_time_tickcount();
+		/* reuse core->timer as the userspace response timeout handler */
+		_mali_osk_timer_add( core->timer, _mali_osk_time_mstoticks(1000) ); /* wait max 1 sec for userspace to respond */
+		return JOB_STATUS_CONTINUE_RUN; /* The core is NOT available for new jobs. */
+	}
+	/* hw watchdog is reporting a new hang or an existing progress-during-hang check passed? */
+	else if	((CORE_HANG_CHECK_TIMEOUT == core->state) || (irq_readout & jobgp->active_mask & MALIGP2_REG_VAL_IRQ_HANG))
+	{
+		/* check interval in ms */
+		u32 timeout = mali_core_hang_check_timeout_get();
+		MALI_DEBUG_PRINT(3, ("Mali GP: HW/SW Watchdog triggered, checking for progress in %d ms\n", timeout));
+		core->state = CORE_WORKING;
+
+		/* save state for the progress checking */
+		jobgp->last_vscl = vscl;
+		jobgp->last_plbcl = plbcl;
+		if (jobgp->have_extended_progress_checking)
+		{
+			jobgp->vertices = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_VALUE);
+		}
+        /* hw watchdog triggered, set up a progress checker every HANGCHECK ms */
+        _mali_osk_timer_add( core->timer_hang_detection, _mali_osk_time_mstoticks(timeout));
+		jobgp->active_mask &= ~MALIGP2_REG_VAL_IRQ_HANG; /* ignore the hw watchdog from now on */
+		mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_CLEAR, irq_readout);
+		mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_MASK, jobgp->active_mask);
+		return JOB_STATUS_CONTINUE_RUN; /* not finihsed */ }
+	/* no errors, but still working */
+	else if ( ( 0 == (core_status & MALIGP2_REG_VAL_STATUS_MASK_ERROR)) &&
+	          ( 0 != (core_status & MALIGP2_REG_VAL_STATUS_MASK_ACTIVE ))
+	        )
+	{
+		mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_CLEAR, irq_readout);
+		mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_MASK, jobgp->active_mask);
+		return JOB_STATUS_CONTINUE_RUN;
+	}
+	/* Else there must be some error */
+	else
+	{
+#if MALI_TIMELINE_PROFILING_ENABLED
+		_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_STOP|MALI_PROFILING_MAKE_EVENT_CHANNEL_GP(core->core_number), 0, 0, 0, 0, 0); /* add GP and L2 counters and return status? */
+#endif
+
+		MALI_DEBUG_PRINT(1, ("Mali GP: Core crashed? *IRQ: 0x%x Status: 0x%x\n", irq_readout, core_status ));
+		#ifdef DEBUG
+		MALI_DEBUG_PRINT(1, ("Mali GP: Registers Before reset:\n"));
+		maligp_print_regs(1, core);
+		#endif
+		return JOB_STATUS_END_UNKNOWN_ERR;
+	}
+}
+
+
+/* This function is called from the ioctl function and should return a mali_core_job pointer
+to a created mali_core_job object with the data given from userspace */
+static _mali_osk_errcode_t subsystem_maligp_get_new_job_from_user(struct mali_core_session * session, void * argument)
+{
+	maligp_job *jobgp;
+	mali_core_job *job;
+	mali_core_job *previous_replaced_job;
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_OK;
+	_mali_uk_gp_start_job_s * user_ptr_job_input;
+
+	user_ptr_job_input = (_mali_uk_gp_start_job_s *)argument;
+
+    MALI_CHECK_NON_NULL(jobgp = (maligp_job *) _mali_osk_calloc(1, sizeof(maligp_job)), _MALI_OSK_ERR_FAULT);
+
+	/* Copy the job data from the U/K interface */
+	if ( NULL == _mali_osk_memcpy(&jobgp->user_input, user_ptr_job_input, sizeof(_mali_uk_gp_start_job_s) ) )
+	{
+		MALI_PRINT_ERROR( ("Mali GP: Could not copy data from U/K interface.\n")) ;
+        err = _MALI_OSK_ERR_FAULT;
+		goto function_exit;
+	}
+
+	MALI_DEBUG_PRINT(3, ("Mali GP: subsystem_maligp_get_new_job_from_user 0x%x\n", (void*)jobgp->user_input.user_job_ptr));
+
+	MALI_DEBUG_PRINT(3, ("Mali GP: Job Regs: 0x%08X 0x%08X, 0x%08X 0x%08X 0x%08X 0x%08X\n",
+			jobgp->user_input.frame_registers[0],
+			jobgp->user_input.frame_registers[1],
+			jobgp->user_input.frame_registers[2],
+			jobgp->user_input.frame_registers[3],
+			jobgp->user_input.frame_registers[4],
+			jobgp->user_input.frame_registers[5])  );
+
+
+	job = GET_JOB_EMBEDDED_PTR(jobgp);
+
+	job->session = session;
+	job_priority_set(job, jobgp->user_input.priority);
+	job_watchdog_set(job, jobgp->user_input.watchdog_msecs );
+	jobgp->heap_current_addr = jobgp->user_input.frame_registers[4];
+
+	job->abort_id = jobgp->user_input.abort_id;
+
+	jobgp->is_stalled_waiting_for_more_memory = 0;
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+	jobgp->pid = _mali_osk_get_pid();
+	jobgp->tid = _mali_osk_get_tid();
+#endif
+
+	if (NULL != session->job_waiting_to_run)
+	{
+		/* IF NOT( newjow HAS HIGHER PRIORITY THAN waitingjob) EXIT_NOT_START new job */
+		if(!job_has_higher_priority(job, session->job_waiting_to_run))
+		{
+			/* The job we try to add does NOT have higher pri than current */
+			/* Cause jobgp to free: */
+			user_ptr_job_input->status = _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE;
+			goto function_exit;
+		}
+	}
+
+	/* We now know that we have a job, and a slot to put it in */
+
+	jobgp->active_mask = MALIGP2_REG_VAL_IRQ_MASK_USED;
+
+	/* Allocating User Return Data */
+	jobgp->notification_obj = _mali_osk_notification_create(
+			_MALI_NOTIFICATION_GP_FINISHED,
+			sizeof(_mali_uk_gp_job_finished_s) );
+
+	if ( NULL == jobgp->notification_obj )
+	{
+		MALI_PRINT_ERROR( ("Mali GP: Could not get notification_obj.\n")) ;
+		err = _MALI_OSK_ERR_NOMEM;
+		goto function_exit;
+	}
+
+	_MALI_OSK_INIT_LIST_HEAD( &(job->list) ) ;
+
+	MALI_DEBUG_PRINT(4, ("Mali GP: Job: 0x%08x INPUT from user.\n", (u32)jobgp->user_input.user_job_ptr)) ;
+
+	/* This should not happen since we have the checking of priority above */
+	err = mali_core_session_add_job(session, job, &previous_replaced_job);
+	if ( _MALI_OSK_ERR_OK != err )
+	{
+		MALI_PRINT_ERROR( ("Mali GP: Internal error\n")) ;
+		/* Cause jobgp to free: */
+		user_ptr_job_input->status = _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE;
+		_mali_osk_notification_delete( jobgp->notification_obj );
+		goto function_exit;
+	}
+
+	/* If MALI_TRUE: This session had a job with lower priority which were removed.
+	This replaced job is given back to userspace. */
+	if ( NULL != previous_replaced_job )
+	{
+		maligp_job *previous_replaced_jobgp;
+
+		previous_replaced_jobgp = GET_JOBGP2_PTR(previous_replaced_job);
+
+		MALI_DEBUG_PRINT(4, ("Mali GP: Replacing job: 0x%08x\n", (u32)previous_replaced_jobgp->user_input.user_job_ptr)) ;
+
+		/* Copy to the input data (which also is output data) the
+		pointer to the job that were replaced, so that the userspace
+		driver can put this job in the front of its job-queue */
+		user_ptr_job_input->returned_user_job_ptr = previous_replaced_jobgp->user_input.user_job_ptr;
+
+		/** @note failure to 'copy to user' at this point must not free jobgp,
+		 * and so no transaction rollback required in the U/K interface */
+
+		/* This does not cause jobgp to free: */
+		user_ptr_job_input->status = _MALI_UK_START_JOB_STARTED_LOW_PRI_JOB_RETURNED;
+		MALI_DEBUG_PRINT(5, ("subsystem_maligp_get_new_job_from_user: Job added, prev returned\n")) ;
+	}
+	else
+	{
+		/* This does not cause jobgp to free: */
+		user_ptr_job_input->status = _MALI_UK_START_JOB_STARTED;
+		MALI_DEBUG_PRINT(5, ("subsystem_maligp_get_new_job_from_user: Job added\n")) ;
+	}
+
+function_exit:
+	if ( _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE == user_ptr_job_input->status
+		|| _MALI_OSK_ERR_OK != err )
+	{
+		_mali_osk_free(jobgp);
+	}
+	MALI_ERROR(err);
+}
+
+
+static _mali_osk_errcode_t subsystem_maligp_suspend_response(struct mali_core_session * session, void * argument)
+{
+	mali_core_renderunit *core;
+	maligp_job *jobgp;
+	mali_core_job *job;
+
+	_mali_uk_gp_suspend_response_s * suspend_response;
+
+	MALI_DEBUG_PRINT(5, ("subsystem_maligp_suspend_response\n"));
+
+	suspend_response = (_mali_uk_gp_suspend_response_s *)argument;
+
+	/* We read job data from User */
+	/* On a single mali_gp system we can only have one Stalled GP,
+	and therefore one stalled request with a cookie. This checks
+	that we get the correct cookie */
+	if ( last_gp_core_cookie != (mali_core_renderunit *)suspend_response->cookie )
+	{
+		MALI_DEBUG_PRINT(2, ("Mali GP: Got an illegal cookie from Userspace.\n")) ;
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+	core = (mali_core_renderunit *)suspend_response->cookie;
+	last_gp_core_cookie = NULL;
+	job = core->current_job;
+	jobgp = GET_JOBGP2_PTR(job);
+
+	switch( suspend_response->code )
+	{
+		case _MALIGP_JOB_RESUME_WITH_NEW_HEAP :
+			MALI_DEBUG_PRINT(5, ("MALIGP_JOB_RESUME_WITH_NEW_HEAP jiffies: %li\n", _mali_osk_time_tickcount()));
+			MALI_DEBUG_PRINT(4, ("New Heap addr 0x%08x - 0x%08x\n", suspend_response->arguments[0], suspend_response->arguments[1]));
+
+			jobgp->is_stalled_waiting_for_more_memory = 0;
+			job->watchdog_jiffies += _mali_osk_time_tickcount(); /* convert to absolute time again */
+			_mali_osk_timer_mod( core->timer, job->watchdog_jiffies); /* update the timer */
+
+
+			mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_CLEAR, (MALIGP2_REG_VAL_IRQ_PLBU_OUT_OF_MEM | MALIGP2_REG_VAL_IRQ_HANG));
+			mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_MASK, jobgp->active_mask);
+			mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_START_ADDR, suspend_response->arguments[0]);
+			mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_END_ADDR, suspend_response->arguments[1]);
+			mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_CMD, MALIGP2_REG_VAL_CMD_UPDATE_PLBU_ALLOC);
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+			_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_RESUME|MALI_PROFILING_MAKE_EVENT_CHANNEL_GP(core->core_number), 0, 0, 0, 0, 0);
+#endif
+
+			MALI_DEBUG_PRINT(4, ("GP resumed with new heap\n"));
+
+			break;
+
+		case _MALIGP_JOB_ABORT:
+			MALI_DEBUG_PRINT(3, ("MALIGP_JOB_ABORT on heap extend request\n"));
+			_mali_osk_irq_schedulework( core->irq );
+			break;
+
+		default:
+			MALI_PRINT_ERROR(("Wrong Suspend response from userspace\n"));
+	}
+    MALI_SUCCESS;
+}
+
+/* This function is called from the ioctl function and should write the necessary data
+to userspace telling which job was finished and the status and debuginfo for this job.
+The function must also free and cleanup the input job object. */
+static void subsystem_maligp_return_job_to_user( mali_core_job * job, mali_subsystem_job_end_code end_status )
+{
+	maligp_job 	*jobgp;
+	_mali_uk_gp_job_finished_s * job_out;
+	_mali_uk_gp_start_job_s* job_input;
+	mali_core_session *session;
+
+
+	jobgp  = _MALI_OSK_CONTAINER_OF(job, maligp_job, embedded_core_job);
+	job_out =  (_mali_uk_gp_job_finished_s *)jobgp->notification_obj->result_buffer; /* OK - this should've been malloc'd */
+	job_input= &(jobgp->user_input);
+	session = job->session;
+
+	MALI_DEBUG_PRINT(5, ("Mali GP: Job: 0x%08x OUTPUT to user. Runtime: %d ms, irq readout %x\n",
+			(u32)jobgp->user_input.user_job_ptr,
+			job->render_time_msecs,
+		   	jobgp->irq_status)) ;
+
+	_mali_osk_memset(job_out, 0 , sizeof(_mali_uk_gp_job_finished_s));
+
+	job_out->user_job_ptr = job_input->user_job_ptr;
+
+	switch( end_status )
+	{
+		case JOB_STATUS_CONTINUE_RUN:
+		case JOB_STATUS_END_SUCCESS:
+		case JOB_STATUS_END_OOM:
+		case JOB_STATUS_END_ABORT:
+		case JOB_STATUS_END_TIMEOUT_SW:
+		case JOB_STATUS_END_HANG:
+		case JOB_STATUS_END_SEG_FAULT:
+		case JOB_STATUS_END_ILLEGAL_JOB:
+		case JOB_STATUS_END_UNKNOWN_ERR:
+		case JOB_STATUS_END_SHUTDOWN:
+		case JOB_STATUS_END_SYSTEM_UNUSABLE:
+			job_out->status = (mali_subsystem_job_end_code) end_status;
+			break;
+		default:
+			job_out->status = JOB_STATUS_END_UNKNOWN_ERR ;
+	}
+
+	job_out->irq_status = jobgp->irq_status;
+	job_out->status_reg_on_stop = jobgp->status_reg_on_stop;
+	job_out->vscl_stop_addr  = 0;
+	job_out->plbcl_stop_addr = 0;
+	job_out->heap_current_addr  = jobgp->heap_current_addr;
+	job_out->perf_counter0 = jobgp->perf_counter0;
+	job_out->perf_counter1 = jobgp->perf_counter1;
+	job_out->perf_counter_src0 = jobgp->user_input.perf_counter_src0 ;
+	job_out->perf_counter_src1 = jobgp->user_input.perf_counter_src1 ;
+	job_out->render_time = job->render_time_msecs;
+#if defined(USING_MALI400_L2_CACHE)
+	job_out->perf_counter_l2_src0 = jobgp->perf_counter_l2_src0;
+	job_out->perf_counter_l2_src1 = jobgp->perf_counter_l2_src1;
+	job_out->perf_counter_l2_val0 = jobgp->perf_counter_l2_val0;
+	job_out->perf_counter_l2_val1 = jobgp->perf_counter_l2_val1;
+#endif
+
+	_mali_osk_notification_queue_send( session->notification_queue, jobgp->notification_obj);
+	jobgp->notification_obj = NULL;
+
+	_mali_osk_free(jobgp);
+
+	last_gp_core_cookie = NULL;
+}
+
+static void subsystem_maligp_renderunit_delete(mali_core_renderunit * core)
+{
+	MALI_DEBUG_PRINT(5, ("Mali GP: maligp_renderunit_delete\n"));
+	_mali_osk_free(core);
+}
+
+static void subsystem_maligp_renderunit_reset_core(struct mali_core_renderunit * core, mali_core_reset_style style)
+{
+	MALI_DEBUG_PRINT(5, ("Mali GP: renderunit_reset_core\n"));
+
+	switch (style)
+	{
+		case MALI_CORE_RESET_STYLE_RUNABLE:
+			maligp_reset(core);
+			break;
+		case MALI_CORE_RESET_STYLE_DISABLE:
+			maligp_raw_reset(core); /* do the raw reset */
+			mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_MASK, 0); /* then disable the IRQs */
+			break;
+		case MALI_CORE_RESET_STYLE_HARD:
+			maligp_reset_hard(core);
+			maligp_initialize_registers_mgmt(core);
+			break;
+		default:
+			MALI_DEBUG_PRINT(1, ("Unknown reset type %d\n", style));
+			break;
+	}
+}
+
+static void subsystem_maligp_renderunit_probe_core_irq_trigger(struct mali_core_renderunit* core)
+{
+    mali_core_renderunit_register_write(core , MALIGP2_REG_ADDR_MGMT_INT_MASK, MALIGP2_REG_VAL_IRQ_MASK_USED);
+	mali_core_renderunit_register_write(core , MALIGP2_REG_ADDR_MGMT_INT_RAWSTAT, MALIGP2_REG_VAL_CMD_FORCE_HANG );
+	_mali_osk_mem_barrier();
+}
+
+static _mali_osk_errcode_t subsystem_maligp_renderunit_probe_core_irq_finished(struct mali_core_renderunit* core)
+{
+	u32 irq_readout;
+
+	irq_readout = mali_core_renderunit_register_read(core, MALIGP2_REG_ADDR_MGMT_INT_STAT);
+
+	if ( MALIGP2_REG_VAL_IRQ_FORCE_HANG & irq_readout )
+	{
+		mali_core_renderunit_register_write(core, MALIGP2_REG_ADDR_MGMT_INT_CLEAR, MALIGP2_REG_VAL_IRQ_FORCE_HANG);
+		_mali_osk_mem_barrier();
+		MALI_SUCCESS;
+	}
+
+    MALI_ERROR(_MALI_OSK_ERR_FAULT);
+}
+
+_mali_osk_errcode_t _mali_ukk_gp_start_job( _mali_uk_gp_start_job_s *args )
+{
+	mali_core_session * session;
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+    session = (mali_core_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_gp_id);
+    MALI_CHECK_NON_NULL(session, _MALI_OSK_ERR_FAULT);
+    return mali_core_subsystem_ioctl_start_job(session, args);
+}
+
+_mali_osk_errcode_t _mali_ukk_get_gp_number_of_cores( _mali_uk_get_gp_number_of_cores_s *args )
+{
+	mali_core_session * session;
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+    session = (mali_core_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_gp_id);
+    MALI_CHECK_NON_NULL(session, _MALI_OSK_ERR_FAULT);
+    return mali_core_subsystem_ioctl_number_of_cores_get(session, &args->number_of_cores);
+}
+
+_mali_osk_errcode_t _mali_ukk_get_gp_core_version( _mali_uk_get_gp_core_version_s *args )
+{
+	mali_core_session * session;
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+    session = (mali_core_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_gp_id);
+    MALI_CHECK_NON_NULL(session, _MALI_OSK_ERR_FAULT);
+    return mali_core_subsystem_ioctl_core_version_get(session, &args->version);
+}
+
+_mali_osk_errcode_t _mali_ukk_gp_suspend_response( _mali_uk_gp_suspend_response_s *args )
+{
+	mali_core_session * session;
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+    session = (mali_core_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_gp_id);
+    MALI_CHECK_NON_NULL(session, _MALI_OSK_ERR_FAULT);
+    return mali_core_subsystem_ioctl_suspend_response(session, args);
+}
+
+void _mali_ukk_gp_abort_job( _mali_uk_gp_abort_job_s * args)
+{
+	mali_core_session * session;
+    MALI_DEBUG_ASSERT_POINTER(args);
+    if (NULL == args->ctx) return;
+    session = (mali_core_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_gp_id);
+    if (NULL == session) return;
+    mali_core_subsystem_ioctl_abort_job(session, args->abort_id);
+
+}
+
+#if USING_MALI_PMM
+
+_mali_osk_errcode_t maligp_signal_power_up( mali_bool queue_only )
+{
+	MALI_DEBUG_PRINT(4, ("Mali GP: signal power up core - queue_only: %d\n", queue_only ));
+
+	return( mali_core_subsystem_signal_power_up( &subsystem_maligp, 0, queue_only ) );
+}
+	
+_mali_osk_errcode_t maligp_signal_power_down( mali_bool immediate_only )
+{
+	MALI_DEBUG_PRINT(4, ("Mali GP: signal power down core - immediate_only: %d\n", immediate_only ));
+
+	return( mali_core_subsystem_signal_power_down( &subsystem_maligp, 0, immediate_only ) );
+}
+
+#endif
+
+#if MALI_STATE_TRACKING
+void maligp_subsystem_dump_state(void)
+{
+	mali_core_renderunit_dump_state(&subsystem_maligp);
+}
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_MALI200.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_MALI200.c
new file mode 100644
index 00000000000..0ac49379bea
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_MALI200.c
@@ -0,0 +1,1187 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_kernel_pp.h"
+#include "mali_kernel_subsystem.h"
+#include "mali_kernel_core.h"
+#include "regs/mali_200_regs.h"
+#include "mali_kernel_rendercore.h"
+#if MALI_TIMELINE_PROFILING_ENABLED
+#include "mali_kernel_profiling.h"
+#endif
+#ifdef USING_MALI400_L2_CACHE
+#include "mali_kernel_l2_cache.h"
+#endif
+#if USING_MMU
+#include "mali_kernel_mem_mmu.h" /* Needed for mali_kernel_mmu_force_bus_reset() */
+#endif
+
+#include "mali_osk_list.h"
+
+#if defined(USING_MALI200)
+#define MALI_PP_SUBSYSTEM_NAME "Mali200"
+#define MALI_PP_CORE_TYPE      _MALI_200
+#elif defined(USING_MALI400)
+#define MALI_PP_SUBSYSTEM_NAME "Mali-400 PP"
+#define MALI_PP_CORE_TYPE      _MALI_400_PP
+#else
+#error "No supported mali core defined"
+#endif
+
+#define GET_JOB_EMBEDDED_PTR(job) (&((job)->embedded_core_job))
+#define GET_JOB200_PTR(job_extern) _MALI_OSK_CONTAINER_OF(job_extern, mali200_job, embedded_core_job)
+
+/* Initialized when this subsystem is initialized. This is determined by the
+ * position in subsystems[], and so the value used to initialize this is
+ * determined at compile time */
+static mali_kernel_subsystem_identifier mali_subsystem_mali200_id = -1;
+
+/* Describing a mali200 job settings */
+typedef struct mali200_job
+{
+	/* The general job struct common for all mali cores */
+	mali_core_job embedded_core_job;
+	_mali_uk_pp_start_job_s user_input;
+
+	u32 irq_status;
+	u32 perf_counter0;
+	u32 perf_counter1;
+	u32 last_tile_list_addr; /* Neccessary to continue a stopped job */
+
+	u32 active_mask;
+
+	/* The data we will return back to the user */
+	_mali_osk_notification_t *notification_obj;
+
+#if defined(USING_MALI400_L2_CACHE)
+	u32 perf_counter_l2_src0;
+	u32 perf_counter_l2_src1;
+	u32 perf_counter_l2_val0;
+	u32 perf_counter_l2_val1;
+	u32 perf_counter_l2_val0_raw;
+	u32 perf_counter_l2_val1_raw;
+#endif
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+	u32 pid;
+	u32 tid;
+#endif
+} mali200_job;
+
+
+/*Functions Exposed to the General External System through
+  funciont pointers.*/
+
+static _mali_osk_errcode_t mali200_subsystem_startup(mali_kernel_subsystem_identifier id);
+#if USING_MMU
+static _mali_osk_errcode_t mali200_subsystem_mmu_connect(mali_kernel_subsystem_identifier id);
+#endif
+static void mali200_subsystem_terminate(mali_kernel_subsystem_identifier id);
+static _mali_osk_errcode_t mali200_subsystem_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue);
+static void mali200_subsystem_session_end(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot);
+static _mali_osk_errcode_t mali200_subsystem_core_system_info_fill(_mali_system_info* info);
+static _mali_osk_errcode_t mali200_renderunit_create(_mali_osk_resource_t * resource);
+#if USING_MMU
+static void mali200_subsystem_broadcast_notification(mali_core_notification_message message, u32 data);
+#endif
+#if MALI_STATE_TRACKING
+void mali200_subsystem_dump_state(void);
+#endif
+
+/* Internal support functions  */
+static _mali_osk_errcode_t mali200_core_version_legal( mali_core_renderunit *core );
+static void mali200_reset(mali_core_renderunit *core);
+static void mali200_reset_hard(struct mali_core_renderunit * core);
+static void mali200_raw_reset(mali_core_renderunit * core);
+static void mali200_initialize_registers_mgmt(mali_core_renderunit *core );
+
+/* Functions exposed to mali_core system through functionpointers
+   in the subsystem struct. */
+static _mali_osk_errcode_t subsystem_mali200_start_job(mali_core_job * job, mali_core_renderunit * core);
+static _mali_osk_errcode_t subsystem_mali200_get_new_job_from_user(struct mali_core_session * session, void * argument);
+static void subsystem_mali200_return_job_to_user( mali_core_job * job, mali_subsystem_job_end_code end_status);
+static void subsystem_mali200_renderunit_delete(mali_core_renderunit * core);
+static void subsystem_mali200_renderunit_reset_core(struct mali_core_renderunit * core, mali_core_reset_style style);
+static void subsystem_mali200_renderunit_probe_core_irq_trigger(struct mali_core_renderunit* core);
+static _mali_osk_errcode_t subsystem_mali200_renderunit_probe_core_irq_finished(struct mali_core_renderunit* core);
+
+static void subsystem_mali200_renderunit_stop_bus(struct mali_core_renderunit* core);
+static u32 subsystem_mali200_irq_handler_upper_half(struct mali_core_renderunit * core);
+static int subsystem_mali200_irq_handler_bottom_half(struct mali_core_renderunit* core);
+
+/* This will be one of the subsystems in the array of subsystems:
+	static struct mali_kernel_subsystem * subsystems[];
+  found in file: mali_kernel_core.c
+*/
+
+struct mali_kernel_subsystem mali_subsystem_mali200=
+{
+	mali200_subsystem_startup,                  /* startup */
+	mali200_subsystem_terminate,                /* shutdown */
+#if USING_MMU
+	mali200_subsystem_mmu_connect,              /* load_complete */
+#else
+    NULL,
+#endif
+	mali200_subsystem_core_system_info_fill,    /* system_info_fill */
+    mali200_subsystem_session_begin,            /* session_begin */
+	mali200_subsystem_session_end,              /* session_end */
+#if USING_MMU
+	mali200_subsystem_broadcast_notification,   /* broadcast_notification */
+#else
+    NULL,
+#endif
+#if MALI_STATE_TRACKING
+	mali200_subsystem_dump_state,               /* dump_state */
+#endif
+} ;
+
+static mali_core_subsystem subsystem_mali200 ;
+
+static _mali_osk_errcode_t mali200_subsystem_startup(mali_kernel_subsystem_identifier id)
+{
+	mali_core_subsystem * subsystem;
+
+	MALI_DEBUG_PRINT(3, ("Mali PP: mali200_subsystem_startup\n") ) ;
+
+    mali_subsystem_mali200_id = id;
+
+	/* All values get 0 as default */
+	_mali_osk_memset(&subsystem_mali200, 0, sizeof(subsystem_mali200));
+
+	subsystem = &subsystem_mali200;
+	subsystem->start_job = &subsystem_mali200_start_job;
+	subsystem->irq_handler_upper_half = &subsystem_mali200_irq_handler_upper_half;
+	subsystem->irq_handler_bottom_half = &subsystem_mali200_irq_handler_bottom_half;
+	subsystem->get_new_job_from_user = &subsystem_mali200_get_new_job_from_user;
+	subsystem->return_job_to_user = &subsystem_mali200_return_job_to_user;
+	subsystem->renderunit_delete = &subsystem_mali200_renderunit_delete;
+	subsystem->reset_core = &subsystem_mali200_renderunit_reset_core;
+	subsystem->stop_bus = &subsystem_mali200_renderunit_stop_bus;
+	subsystem->probe_core_irq_trigger = &subsystem_mali200_renderunit_probe_core_irq_trigger;
+	subsystem->probe_core_irq_acknowledge = &subsystem_mali200_renderunit_probe_core_irq_finished;
+
+	/* Setting variables in the general core part of the subsystem.*/
+	subsystem->name = MALI_PP_SUBSYSTEM_NAME;
+	subsystem->core_type = MALI_PP_CORE_TYPE;
+	subsystem->id = id;
+
+	/* Initiates the rest of the general core part of the subsystem */
+    MALI_CHECK_NO_ERROR(mali_core_subsystem_init( subsystem ));
+
+	/* This will register the function for adding MALI200 cores to the subsystem */
+#if defined(USING_MALI200)
+	MALI_CHECK_NO_ERROR(_mali_kernel_core_register_resource_handler(MALI200, mali200_renderunit_create));
+#endif
+#if defined(USING_MALI400)
+	MALI_CHECK_NO_ERROR(_mali_kernel_core_register_resource_handler(MALI400PP, mali200_renderunit_create));
+#endif
+
+	MALI_DEBUG_PRINT(6, ("Mali PP: mali200_subsystem_startup\n") ) ;
+
+	MALI_SUCCESS;
+}
+
+#if USING_MMU
+static _mali_osk_errcode_t mali200_subsystem_mmu_connect(mali_kernel_subsystem_identifier id)
+{
+	mali_core_subsystem_attach_mmu(&subsystem_mali200);
+    MALI_SUCCESS; /* OK */
+}
+#endif
+
+static void mali200_subsystem_terminate(mali_kernel_subsystem_identifier id)
+{
+	MALI_DEBUG_PRINT(3, ("Mali PP: mali200_subsystem_terminate\n") ) ;
+	mali_core_subsystem_cleanup(&subsystem_mali200);
+}
+
+static _mali_osk_errcode_t mali200_subsystem_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue)
+{
+	mali_core_session * session;
+
+	MALI_DEBUG_PRINT(3, ("Mali PP: mali200_subsystem_session_begin\n") ) ;
+    MALI_CHECK_NON_NULL(session = _mali_osk_malloc( sizeof(mali_core_session) ), _MALI_OSK_ERR_NOMEM);
+
+	_mali_osk_memset(session, 0, sizeof(*session) );
+	*slot = (mali_kernel_subsystem_session_slot)session;
+
+	session->subsystem = &subsystem_mali200;
+
+	session->notification_queue = queue;
+
+#if USING_MMU
+	session->mmu_session = mali_session_data;
+#endif
+
+	mali_core_session_begin(session);
+
+	MALI_DEBUG_PRINT(6, ("Mali PP: mali200_subsystem_session_begin\n") ) ;
+
+	MALI_SUCCESS;
+}
+
+static void mali200_subsystem_session_end(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot)
+{
+	mali_core_session * session;
+
+	MALI_DEBUG_PRINT(3, ("Mali PP: mali200_subsystem_session_end\n") ) ;
+	if ( NULL==slot || NULL==*slot)
+	{
+		MALI_PRINT_ERROR(("Input slot==NULL"));
+		return;
+	}
+	session = (mali_core_session*) *slot;
+	mali_core_session_close(session);
+
+	_mali_osk_free(session);
+	*slot = NULL;
+
+	MALI_DEBUG_PRINT(6, ("Mali PP: mali200_subsystem_session_end\n") ) ;
+}
+
+/**
+ * We fill in info about all the cores we have
+ * @param info Pointer to system info struct to update
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali200_subsystem_core_system_info_fill(_mali_system_info* info)
+{
+	return mali_core_subsystem_system_info_fill(&subsystem_mali200, info);
+}
+
+
+static _mali_osk_errcode_t mali200_renderunit_create(_mali_osk_resource_t * resource)
+{
+	mali_core_renderunit *core;
+	_mali_osk_errcode_t err;
+
+	MALI_DEBUG_PRINT(3, ("Mali PP: mali200_renderunit_create\n") ) ;
+	/* Checking that the resource settings are correct */
+#if defined(USING_MALI200)
+	if(MALI200 != resource->type)
+	{
+		MALI_PRINT_ERROR(("Can not register this resource as a " MALI_PP_SUBSYSTEM_NAME " core."));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+#elif defined(USING_MALI400)
+	if(MALI400PP != resource->type)
+	{
+		MALI_PRINT_ERROR(("Can not register this resource as a " MALI_PP_SUBSYSTEM_NAME " core."));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+#endif
+	if ( 0 != resource->size )
+	{
+		MALI_PRINT_ERROR(("Memory size set to " MALI_PP_SUBSYSTEM_NAME " core should be zero."));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	if ( NULL == resource->description )
+	{
+		MALI_PRINT_ERROR(("A " MALI_PP_SUBSYSTEM_NAME " core needs a unique description field"));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* Create a new core object */
+	core = (mali_core_renderunit*) _mali_osk_malloc(sizeof(*core));
+	if ( NULL == core )
+	{
+		MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	/* Variables set to be able to open and register the core */
+	core->subsystem = &subsystem_mali200 ;
+	core->registers_base_addr 	= resource->base ;
+	core->size 					= MALI200_REG_SIZEOF_REGISTER_BANK ;
+	core->irq_nr 				= resource->irq ;
+	core->description 			= resource->description;
+#if USING_MMU
+	core->mmu_id                = resource->mmu_id;
+	core->mmu                   = NULL;
+#endif
+#if USING_MALI_PMM
+	/* Set up core's PMM id */
+	switch( subsystem_mali200.number_of_cores )
+	{
+	case 0:
+		core->pmm_id = MALI_PMM_CORE_PP0;
+		break;
+	case 1:
+		core->pmm_id = MALI_PMM_CORE_PP1;
+		break;
+	case 2:
+		core->pmm_id = MALI_PMM_CORE_PP2;
+		break;
+	case 3:
+		core->pmm_id = MALI_PMM_CORE_PP3;
+		break;
+	default:
+		MALI_DEBUG_PRINT(1, ("Unknown supported core for PMM\n"));
+		err = _MALI_OSK_ERR_FAULT;
+		goto exit_on_error0;
+	}
+#endif
+
+	err = mali_core_renderunit_init( core );
+    if (_MALI_OSK_ERR_OK != err)
+    {
+		MALI_DEBUG_PRINT(1, ("Failed to initialize renderunit\n"));
+		goto exit_on_error0;
+    }
+
+	/* Map the new core object, setting: core->registers_mapped  */
+	err = mali_core_renderunit_map_registers(core);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		MALI_DEBUG_PRINT(1, ("Failed to map register\n"));
+		goto exit_on_error1;
+	}
+
+	/* Check that the register mapping of the core works.
+	Return 0 if Mali PP core is present and accessible. */
+	if (mali_benchmark) {
+#if defined(USING_MALI200)
+		core->core_version = (((u32)MALI_PP_PRODUCT_ID) << 16) | 5 /* Fake Mali200-r0p5 */;
+#elif defined(USING_MALI400)
+		core->core_version = (((u32)MALI_PP_PRODUCT_ID) << 16) | 0x0101 /* Fake Mali400-r1p1 */;
+#else
+#error "No supported mali core defined"
+#endif
+	} else {
+		core->core_version = mali_core_renderunit_register_read(
+		                     core,
+		                     MALI200_REG_ADDR_MGMT_VERSION);
+	}
+
+	err = mali200_core_version_legal(core);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		MALI_DEBUG_PRINT(1, ("Invalid core\n"));
+		goto exit_on_error2;
+	}
+
+	/* Reset the core. Put the core into a state where it can start to render. */
+	mali200_reset(core);
+
+	/* Registering IRQ, init the work_queue_irq_handle */
+	/* Adding this core as an available renderunit in the subsystem. */
+	err = mali_core_subsystem_register_renderunit(&subsystem_mali200, core);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		MALI_DEBUG_PRINT(1, ("Failed to register with core\n"));
+		goto exit_on_error2;
+	}
+	MALI_DEBUG_PRINT(6, ("Mali PP: mali200_renderunit_create\n") ) ;
+
+	MALI_SUCCESS;
+
+exit_on_error2:
+	mali_core_renderunit_unmap_registers(core);
+exit_on_error1:
+    mali_core_renderunit_term(core);
+exit_on_error0:
+	_mali_osk_free( core ) ;
+	MALI_PRINT_ERROR(("Renderunit NOT created."));
+    MALI_ERROR(err);
+}
+
+#if USING_MMU
+/* Used currently only for signalling when MMU has a pagefault */
+static void mali200_subsystem_broadcast_notification(mali_core_notification_message message, u32 data)
+{
+	mali_core_subsystem_broadcast_notification(&subsystem_mali200, message, data);
+}
+#endif
+
+static _mali_osk_errcode_t mali200_core_version_legal( mali_core_renderunit *core )
+{
+	u32 mali_type;
+
+	mali_type = core->core_version >> 16;
+#if defined(USING_MALI400)
+	/* Mali300 and Mali400 is compatible, accept either core. */
+	if (MALI400_PP_PRODUCT_ID != mali_type && MALI300_PP_PRODUCT_ID != mali_type)
+#else
+	if (MALI_PP_PRODUCT_ID != mali_type)
+#endif
+	{
+		MALI_PRINT_ERROR(("Error: reading this from " MALI_PP_SUBSYSTEM_NAME " version register: 0x%x\n", core->core_version));
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+	MALI_DEBUG_PRINT(3, ("Mali PP: core_version_legal: Reads correct mali version: %d\n", mali_type) ) ;
+	MALI_SUCCESS;
+}
+
+static void subsystem_mali200_renderunit_stop_bus(struct mali_core_renderunit* core)
+{
+	mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_CTRL_MGMT, MALI200_REG_VAL_CTRL_MGMT_STOP_BUS);
+}
+
+static void mali200_raw_reset( mali_core_renderunit *core )
+{
+	int i;
+	const int request_loop_count = 20;
+
+	MALI_DEBUG_PRINT(4, ("Mali PP: mali200_raw_reset: %s\n", core->description));
+	if (mali_benchmark) return;
+
+	mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_MASK, 0); /* disable IRQs */
+
+#if defined(USING_MALI200)
+
+    mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_CTRL_MGMT, MALI200_REG_VAL_CTRL_MGMT_STOP_BUS);
+
+	for (i = 0; i < request_loop_count; i++)
+	{
+		if (mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_STATUS) & MALI200_REG_VAL_STATUS_BUS_STOPPED) break;
+		_mali_osk_time_ubusydelay(10);
+	}
+
+	MALI_DEBUG_PRINT_IF(1, request_loop_count == i, ("Mali PP: Bus was never stopped during core reset\n"));
+
+
+	if (request_loop_count==i)
+	{
+#if USING_MMU
+		if ((NULL!=core->mmu) && (MALI_FALSE == core->error_recovery))
+		{
+			/* Could not stop bus connections from core, probably because some of the already pending
+			   bus request has had a page fault, and therefore can not complete before the MMU does PageFault
+			   handling. This can be treated as a heavier reset function - which unfortunately reset all
+			   the cores on this MMU in addition to the MMU itself */
+			MALI_DEBUG_PRINT(1, ("Mali PP: Forcing Bus reset\n"));
+			mali_kernel_mmu_force_bus_reset(core->mmu);
+			return;
+		}
+#endif
+		MALI_PRINT(("A MMU reset did not allow PP  to stop its bus, system failure, unable to recover\n"));
+		return;
+	}
+
+	/* use the hard reset routine to do the actual reset */
+	mali200_reset_hard(core);
+
+#elif defined(USING_MALI400)
+
+	mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_CLEAR, MALI400PP_REG_VAL_IRQ_RESET_COMPLETED);
+	mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_CTRL_MGMT, MALI400PP_REG_VAL_CTRL_MGMT_SOFT_RESET);
+
+	for (i = 0; i < request_loop_count; i++)
+	{
+		if (mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_INT_RAWSTAT) & MALI400PP_REG_VAL_IRQ_RESET_COMPLETED) break;
+		_mali_osk_time_ubusydelay(10);
+	}
+
+	if (request_loop_count==i)
+	{
+#if USING_MMU
+		if ((NULL!=core->mmu) && (MALI_FALSE == core->error_recovery))
+		{
+			/* Could not stop bus connections from core, probably because some of the already pending
+			   bus request has had a page fault, and therefore can not complete before the MMU does PageFault
+			   handling. This can be treated as a heavier reset function - which unfortunately reset all
+			   the cores on this MMU in addition to the MMU itself */
+			MALI_DEBUG_PRINT(1, ("Mali PP: Forcing Bus reset\n"));
+			mali_kernel_mmu_force_bus_reset(core->mmu);
+			return;
+		}
+#endif
+		MALI_PRINT(("A MMU reset did not allow PP  to stop its bus, system failure, unable to recover\n"));
+		return;
+	}
+	else
+		mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_CLEAR, MALI200_REG_VAL_IRQ_MASK_ALL);
+
+#else
+#error "no supported mali core defined"
+#endif
+}
+
+static void mali200_reset( mali_core_renderunit *core )
+{
+	if (!mali_benchmark) {
+		mali200_raw_reset(core);
+		mali200_initialize_registers_mgmt(core);
+	}
+}
+
+/* Sets the registers on mali200 according to the const default_mgmt_regs array. */
+static void mali200_initialize_registers_mgmt(mali_core_renderunit *core )
+{
+	MALI_DEBUG_PRINT(6, ("Mali PP: mali200_initialize_registers_mgmt: %s\n", core->description)) ;
+	mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_MASK, MALI200_REG_VAL_IRQ_MASK_USED);
+}
+
+/* Start this job on this core. Return MALI_TRUE if the job was started. */
+static _mali_osk_errcode_t subsystem_mali200_start_job(mali_core_job * job, mali_core_renderunit * core)
+{
+	mali200_job 	*job200;
+
+	/* The local extended version of the general structs */
+	job200  = _MALI_OSK_CONTAINER_OF(job, mali200_job, embedded_core_job);
+
+	if ( (0 == job200->user_input.frame_registers[0]) ||
+	     (0 == job200->user_input.frame_registers[1]) )
+	{
+		MALI_DEBUG_PRINT(4, ("Mali PP: Job: 0x%08x  WILL NOT START SINCE JOB HAS ILLEGAL ADDRESSES\n",
+				(u32)job200->user_input.user_job_ptr));
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	MALI_DEBUG_PRINT(4, ("Mali PP: Job: 0x%08x  START_RENDER  Tile_list: 0x%08x\n",
+			(u32)job200->user_input.user_job_ptr,
+			job200->user_input.frame_registers[0]));
+	MALI_DEBUG_PRINT(6, ("Mali PP:      RSW base addr: 0x%08x  Vertex base addr: 0x%08x\n",
+			job200->user_input.frame_registers[1], job200->user_input.frame_registers[2]));
+
+	/* Frame registers. Copy from mem to physical registers */
+	mali_core_renderunit_register_write_array(
+			core,
+			MALI200_REG_ADDR_FRAME,
+			&(job200->user_input.frame_registers[0]),
+			MALI200_NUM_REGS_FRAME);
+
+	/* Write Back unit 0. Copy from mem to physical registers*/
+	mali_core_renderunit_register_write_array(
+				core,
+				MALI200_REG_ADDR_WB0,
+				&(job200->user_input.wb0_registers[0]),
+				MALI200_NUM_REGS_WBx);
+
+	/* Write Back unit 1. Copy from mem to physical registers */
+	mali_core_renderunit_register_write_array(
+				core,
+				MALI200_REG_ADDR_WB1,
+				&(job200->user_input.wb1_registers[0]),
+				MALI200_NUM_REGS_WBx);
+
+	/* Write Back unit 2. Copy from mem to physical registers */
+	mali_core_renderunit_register_write_array(
+			core,
+			MALI200_REG_ADDR_WB2,
+			&(job200->user_input.wb2_registers[0]),
+			MALI200_NUM_REGS_WBx);
+
+
+	/* This selects which performance counters we are reading */
+	if ( 0 != job200->user_input.perf_counter_flag )
+	{
+		if ( job200->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_SRC0_ENABLE)
+		{
+			mali_core_renderunit_register_write(
+					core,
+					MALI200_REG_ADDR_MGMT_PERF_CNT_0_ENABLE,
+					MALI200_REG_VAL_PERF_CNT_ENABLE);
+			mali_core_renderunit_register_write(
+					core,
+					MALI200_REG_ADDR_MGMT_PERF_CNT_0_SRC,
+					job200->user_input.perf_counter_src0);
+
+		}
+
+		if ( job200->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_SRC1_ENABLE)
+		{
+			mali_core_renderunit_register_write(
+					core,
+					MALI200_REG_ADDR_MGMT_PERF_CNT_1_ENABLE,
+					MALI200_REG_VAL_PERF_CNT_ENABLE);
+			mali_core_renderunit_register_write(
+					core,
+					MALI200_REG_ADDR_MGMT_PERF_CNT_1_SRC,
+					job200->user_input.perf_counter_src1);
+
+		}
+
+#if defined(USING_MALI400_L2_CACHE)
+		if ( job200->user_input.perf_counter_flag & (_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC0_ENABLE|_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC1_ENABLE) )
+		{
+			int force_reset = ( job200->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_L2_RESET ) ? 1 : 0;
+			u32 src0 = 0;
+			u32 src1 = 0;
+
+			if ( job200->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC0_ENABLE )
+			{
+				src0 = job200->user_input.perf_counter_l2_src0;
+			}
+			if ( job200->user_input.perf_counter_flag & _MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC1_ENABLE )
+			{
+				src1 = job200->user_input.perf_counter_l2_src1;
+			}
+
+			mali_kernel_l2_cache_set_perf_counters(src0, src1, force_reset); /* will activate and possibly reset counters */
+
+			/* Now, retrieve the current values, so we can substract them when the job has completed */
+			mali_kernel_l2_cache_get_perf_counters(&job200->perf_counter_l2_src0,
+			                                       &job200->perf_counter_l2_val0,
+			                                       &job200->perf_counter_l2_src1,
+			                                       &job200->perf_counter_l2_val1);
+		}
+#endif
+	}
+
+	subsystem_flush_mapped_mem_cache();
+	_mali_osk_mem_barrier();
+
+	/* This is the command that starts the Core */
+	mali_core_renderunit_register_write(
+			core,
+			MALI200_REG_ADDR_MGMT_CTRL_MGMT,
+			MALI200_REG_VAL_CTRL_MGMT_START_RENDERING);
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+	_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_START|MALI_PROFILING_MAKE_EVENT_CHANNEL_PP(core->core_number), job200->pid, job200->tid, 0, 0, 0);
+#endif
+
+    MALI_SUCCESS;
+}
+
+static u32 subsystem_mali200_irq_handler_upper_half(mali_core_renderunit * core)
+{
+	u32 irq_readout;
+
+	if (mali_benchmark) {
+		return (core->current_job ? 1 : 0); /* simulate irq is pending when a job is pending */
+	}
+
+	MALI_DEBUG_PRINT(5, ("Mali PP: subsystem_mali200_irq_handler_upper_half: %s\n", core->description)) ;
+	irq_readout = mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_INT_STATUS);
+
+	if ( MALI200_REG_VAL_IRQ_MASK_NONE != irq_readout )
+	{
+		/* Mask out all IRQs from this core until IRQ is handled */
+		mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_MASK, MALI200_REG_VAL_IRQ_MASK_NONE);
+		return 1;
+	}
+	return 0;
+}
+
+static int subsystem_mali200_irq_handler_bottom_half(struct mali_core_renderunit* core)
+{
+	u32 irq_readout;
+	u32 current_tile_addr;
+	u32 core_status;
+	mali_core_job * job;
+	mali200_job * job200;
+
+	job = core->current_job;
+	job200 = GET_JOB200_PTR(job);
+
+
+	if (mali_benchmark) {
+		irq_readout = MALI200_REG_VAL_IRQ_END_OF_FRAME;
+		current_tile_addr = 0;
+		core_status = 0;
+	} else {
+		irq_readout = mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_INT_RAWSTAT) & MALI200_REG_VAL_IRQ_MASK_USED;
+		current_tile_addr = mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_CURRENT_REND_LIST_ADDR);
+		core_status = mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_STATUS);
+	}
+
+	if (NULL == job)
+	{
+		MALI_DEBUG_ASSERT(CORE_IDLE==core->state);
+		if ( 0 != irq_readout )
+		{
+			MALI_PRINT_ERROR(("Interrupt from a core not running a job. IRQ: 0x%04x Status: 0x%04x", irq_readout, core_status));
+		}
+		return JOB_STATUS_END_UNKNOWN_ERR;
+	}
+	MALI_DEBUG_ASSERT(CORE_IDLE!=core->state);
+
+	job200->irq_status |= irq_readout;
+
+	MALI_DEBUG_PRINT_IF( 3, ( 0 != irq_readout ),
+	            ("Mali PP: Job: 0x%08x  IRQ RECEIVED  Rawstat: 0x%x Tile_addr: 0x%x Status: 0x%x\n",
+	            (u32)job200->user_input.user_job_ptr, irq_readout ,current_tile_addr ,core_status));
+
+	if ( MALI200_REG_VAL_IRQ_END_OF_FRAME & irq_readout)
+	{
+#if defined(USING_MALI200)
+		mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_CTRL_MGMT, MALI200_REG_VAL_CTRL_MGMT_FLUSH_CACHES);
+#endif
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+		_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_STOP|MALI_PROFILING_MAKE_EVENT_CHANNEL_PP(core->core_number), 0, 0, 0, 0, 0); /* add GP and L2 counters and return status */
+#endif
+
+		if (0 != job200->user_input.perf_counter_flag )
+		{
+			if (job200->user_input.perf_counter_flag & (_MALI_PERFORMANCE_COUNTER_FLAG_SRC0_ENABLE|_MALI_PERFORMANCE_COUNTER_FLAG_SRC1_ENABLE) )
+			{
+				job200->perf_counter0 = mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_PERF_CNT_0_VALUE);
+				job200->perf_counter1 = mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_PERF_CNT_1_VALUE);
+			}
+
+#if defined(USING_MALI400_L2_CACHE)
+			if (job200->user_input.perf_counter_flag & (_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC0_ENABLE|_MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC1_ENABLE) )
+			{
+				u32 src0;
+				u32 val0;
+				u32 src1;
+				u32 val1;
+				mali_kernel_l2_cache_get_perf_counters(&src0, &val0, &src1, &val1);
+
+				if (job200->perf_counter_l2_src0 == src0)
+				{
+					job200->perf_counter_l2_val0_raw = val0;
+					job200->perf_counter_l2_val0 = val0 - job200->perf_counter_l2_val0;
+				}
+				else
+				{
+					job200->perf_counter_l2_val0_raw = 0;
+					job200->perf_counter_l2_val0 = 0;
+				}
+
+				if (job200->perf_counter_l2_src1 == src1)
+				{
+					job200->perf_counter_l2_val1_raw = val1;
+					job200->perf_counter_l2_val1 = val1 - job200->perf_counter_l2_val1;
+				}
+				else
+				{
+					job200->perf_counter_l2_val1_raw = 0;
+					job200->perf_counter_l2_val1 = 0;
+				}
+			}
+#endif
+
+		}
+
+		return JOB_STATUS_END_SUCCESS; /* reschedule */
+	}
+	/* Overall SW watchdog timeout or (time to do hang checking and progress detected)? */
+	else if (
+	         (CORE_WATCHDOG_TIMEOUT == core->state) ||
+	         ((CORE_HANG_CHECK_TIMEOUT == core->state) && (current_tile_addr == job200->last_tile_list_addr))
+	        )
+	{
+#if MALI_TIMELINE_PROFILING_ENABLED
+		_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_STOP|MALI_PROFILING_MAKE_EVENT_CHANNEL_PP(core->core_number), 0, 0, 0, 0, 0); /* add GP and L2 counters and return status */
+#endif
+		/* no progress detected, killed by the watchdog */
+		MALI_DEBUG_PRINT(2, ("M200: SW-Timeout Rawstat: 0x%x Tile_addr: 0x%x Status: 0x%x.\n", irq_readout ,current_tile_addr ,core_status) );
+		/* In this case will the system outside cleanup and reset the core */
+		return JOB_STATUS_END_HANG;
+   	}
+	/* HW watchdog triggered or an existing hang check passed? */
+	else if	((CORE_HANG_CHECK_TIMEOUT == core->state) || (irq_readout & job200->active_mask & MALI200_REG_VAL_IRQ_HANG))
+	{
+		/* check interval in ms */
+		u32 timeout = mali_core_hang_check_timeout_get();
+		MALI_DEBUG_PRINT(3, ("M200: HW/SW Watchdog triggered, checking for progress in %d ms\n", timeout));
+		job200->last_tile_list_addr = current_tile_addr;
+		/* hw watchdog triggered, set up a progress checker every HANGCHECK ms */
+		_mali_osk_timer_add(core->timer_hang_detection, _mali_osk_time_mstoticks(timeout));
+		job200->active_mask &= ~MALI200_REG_VAL_IRQ_HANG; /* ignore the hw watchdoig from now on */
+		mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_CLEAR, irq_readout & ~MALI200_REG_VAL_IRQ_HANG);
+		mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_MASK, job200->active_mask);
+		return JOB_STATUS_CONTINUE_RUN; /* not finished */
+	}
+	/* No irq pending, core still busy */
+	else if ((0 == (irq_readout & MALI200_REG_VAL_IRQ_MASK_USED)) && ( 0 != (core_status & MALI200_REG_VAL_STATUS_RENDERING_ACTIVE)))
+	{
+		mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_CLEAR, irq_readout);
+		mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_MASK, job200->active_mask);
+		return JOB_STATUS_CONTINUE_RUN; /* Not finished */
+	}
+	else
+	{
+#if MALI_TIMELINE_PROFILING_ENABLED
+		_mali_profiling_add_event(MALI_PROFILING_EVENT_TYPE_STOP|MALI_PROFILING_MAKE_EVENT_CHANNEL_PP(core->core_number), 0, 0, 0, 0, 0); /* add GP and L2 counters and return status */
+#endif
+
+		MALI_DEBUG_PRINT(1, ("Mali PP: Job: 0x%08x  CRASH?  Rawstat: 0x%x Tile_addr: 0x%x Status: 0x%x\n",
+				(u32)job200->user_input.user_job_ptr, irq_readout ,current_tile_addr ,core_status) ) ;
+
+		if (irq_readout & MALI200_REG_VAL_IRQ_BUS_ERROR)
+		{
+			u32 bus_error = mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_BUS_ERROR_STATUS);
+
+			MALI_DEBUG_PRINT(1, ("Bus error status: 0x%08X\n", bus_error));
+			MALI_DEBUG_PRINT_IF(1, (bus_error & 0x01), ("Bus write error from id 0x%02x\n", (bus_error>>2) & 0x0F));
+			MALI_DEBUG_PRINT_IF(1, (bus_error & 0x02), ("Bus read error from id 0x%02x\n", (bus_error>>6) & 0x0F));
+			MALI_DEBUG_PRINT_IF(1, (0 == (bus_error & 0x03)), ("Bus error but neither read or write was set as the error reason\n"));
+			(void)bus_error;
+		}
+
+		return JOB_STATUS_END_UNKNOWN_ERR; /* reschedule */
+	}
+}
+
+
+/* This function is called from the ioctl function and should return a mali_core_job pointer
+to a created mali_core_job object with the data given from userspace */
+static _mali_osk_errcode_t subsystem_mali200_get_new_job_from_user(struct mali_core_session * session, void * argument)
+{
+	mali200_job *job200;
+	mali_core_job *job;
+	mali_core_job *previous_replaced_job;
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_OK;
+	_mali_uk_pp_start_job_s * user_ptr_job_input;
+
+    user_ptr_job_input = (_mali_uk_pp_start_job_s *)argument;
+
+    MALI_CHECK_NON_NULL(job200 = (mali200_job *) _mali_osk_malloc(sizeof(mali200_job)), _MALI_OSK_ERR_NOMEM);
+	_mali_osk_memset(job200, 0 , sizeof(mali200_job) );
+
+	/* We read job data from Userspace pointer */
+    if ( NULL == _mali_osk_memcpy((void*)&job200->user_input, user_ptr_job_input, sizeof(job200->user_input)) )
+	{
+		MALI_PRINT_ERROR( ("Mali PP: Could not copy data from U/K interface.\n")) ;
+        err = _MALI_OSK_ERR_FAULT;
+		goto function_exit;
+	}
+
+	MALI_DEBUG_PRINT(5, ("Mali PP: subsystem_mali200_get_new_job_from_user 0x%x\n", (void*)job200->user_input.user_job_ptr));
+
+	MALI_DEBUG_PRINT(5, ("Mali PP: Frameregs: 0x%x 0x%x 0x%x Writeback[1] 0x%x, Pri:%d; Watchd:%d\n",
+			job200->user_input.frame_registers[0], job200->user_input.frame_registers[1], job200->user_input.frame_registers[2],
+			job200->user_input.wb0_registers[1], job200->user_input.priority,
+			job200->user_input.watchdog_msecs));
+
+	if ( job200->user_input.perf_counter_flag)
+	{
+#if defined(USING_MALI400_L2_CACHE)
+		MALI_DEBUG_PRINT(5, ("Mali PP: Performance counters: flag:0x%x src0:0x%x src1:0x%x l2_src0:0x%x l2_src1:0x%x\n",
+				job200->user_input.perf_counter_flag,
+				job200->user_input.perf_counter_src0,
+				job200->user_input.perf_counter_src1,
+				job200->user_input.perf_counter_l2_src0,
+				job200->user_input.perf_counter_l2_src1));
+#else
+		MALI_DEBUG_PRINT(5, ("Mali PP: Performance counters: flag:0x%x src0:0x%x src1:0x%x\n",
+				job200->user_input.perf_counter_flag,
+				job200->user_input.perf_counter_src0,
+				job200->user_input.perf_counter_src1));
+#endif
+	}
+
+	job = GET_JOB_EMBEDDED_PTR(job200);
+
+	job->session = session;
+	job_priority_set(job, job200->user_input.priority);
+	job_watchdog_set(job, job200->user_input.watchdog_msecs );
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+	job200->pid = _mali_osk_get_pid();
+	job200->tid = _mali_osk_get_tid();
+#endif
+
+	job->abort_id = job200->user_input.abort_id;
+	if (NULL != session->job_waiting_to_run)
+	{
+		/* IF NOT( newjow HAS HIGHER PRIORITY THAN waitingjob) EXIT_NOT_START newjob */
+		if(!job_has_higher_priority(job, session->job_waiting_to_run))
+		{
+			/* The job we try to add does NOT have higher pri than current */
+			user_ptr_job_input->status = _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE;
+			goto function_exit;
+		}
+	}
+
+	/* We now know that we has a job, and a empty session slot to put it in */
+
+	job200->active_mask = MALI200_REG_VAL_IRQ_MASK_USED;
+
+	/* Allocating User Return Data */
+	job200->notification_obj = _mali_osk_notification_create(
+			_MALI_NOTIFICATION_PP_FINISHED,
+			sizeof(_mali_uk_pp_job_finished_s) );
+
+	if ( NULL == job200->notification_obj )
+	{
+		MALI_PRINT_ERROR( ("Mali PP: Could not get notification_obj.\n")) ;
+		err = _MALI_OSK_ERR_NOMEM;
+		goto function_exit;
+	}
+
+	_MALI_OSK_INIT_LIST_HEAD( &(job->list) ) ;
+
+	MALI_DEBUG_PRINT(4, ("Mali PP: Job: 0x%08x INPUT from user.\n", (u32)job200->user_input.user_job_ptr)) ;
+
+	/* This should not happen since we have the checking of priority above */
+	if ( _MALI_OSK_ERR_OK != mali_core_session_add_job(session, job, &previous_replaced_job))
+	{
+		MALI_PRINT_ERROR( ("Mali PP: Internal error\n")) ;
+		user_ptr_job_input->status = _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE;
+		_mali_osk_notification_delete( job200->notification_obj );
+		goto function_exit;
+	}
+
+	/* If MALI_TRUE: This session had a job with lower priority which were removed.
+	This replaced job is given back to userspace. */
+	if ( NULL != previous_replaced_job )
+	{
+		mali200_job *previous_replaced_job200;
+
+		previous_replaced_job200 = GET_JOB200_PTR(previous_replaced_job);
+
+		MALI_DEBUG_PRINT(4, ("Mali PP: Replacing job: 0x%08x\n", (u32)previous_replaced_job200->user_input.user_job_ptr)) ;
+
+		/* Copy to the input data (which also is output data) the
+		pointer to the job that were replaced, so that the userspace
+		driver can put this job in the front of its job-queue */
+
+		user_ptr_job_input->returned_user_job_ptr = previous_replaced_job200->user_input.user_job_ptr;
+
+		/** @note failure to 'copy to user' at this point must not free job200,
+		 * and so no transaction rollback required in the U/K interface */
+
+		/* This does not cause job200 to free: */
+		user_ptr_job_input->status = _MALI_UK_START_JOB_STARTED_LOW_PRI_JOB_RETURNED;
+		MALI_DEBUG_PRINT(5, ("subsystem_mali200_get_new_job_from_user: Job added, prev returned\n")) ;
+	}
+	else
+	{
+		/* This does not cause job200 to free: */
+		user_ptr_job_input->status = _MALI_UK_START_JOB_STARTED;
+		MALI_DEBUG_PRINT(5, ("subsystem_mali200_get_new_job_from_user: Job added\n")) ;
+	}
+
+function_exit:
+	if (_MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE == user_ptr_job_input->status
+		|| _MALI_OSK_ERR_OK != err )
+	{
+		_mali_osk_free(job200);
+	}
+	MALI_ERROR(err);
+}
+
+/* This function is called from the ioctl function and should write the necessary data
+to userspace telling which job was finished and the status and debuginfo for this job.
+The function must also free and cleanup the input job object. */
+static void subsystem_mali200_return_job_to_user( mali_core_job * job, mali_subsystem_job_end_code end_status)
+{
+	mali200_job 	*job200;
+	_mali_uk_pp_job_finished_s * job_out;
+	_mali_uk_pp_start_job_s * job_input;
+	mali_core_session *session;
+
+	if (NULL == job)
+	{
+		MALI_DEBUG_PRINT(1, ("subsystem_mali200_return_job_to_user received a NULL ptr\n"));
+		return;
+	}
+
+	job200  = _MALI_OSK_CONTAINER_OF(job, mali200_job, embedded_core_job);
+
+	if (NULL == job200->notification_obj)
+	{
+		MALI_DEBUG_PRINT(1, ("Found job200 with NULL notification object, abandoning userspace sending\n"));
+		return;
+	}
+
+	job_out =  job200->notification_obj->result_buffer;
+	job_input= &(job200->user_input);
+	session = job->session;
+
+	MALI_DEBUG_PRINT(4, ("Mali PP: Job: 0x%08x OUTPUT to user. Runtime: %dms\n",
+			(u32)job200->user_input.user_job_ptr,
+			job->render_time_msecs)) ;
+
+	_mali_osk_memset(job_out, 0 , sizeof(_mali_uk_pp_job_finished_s));
+
+	job_out->user_job_ptr = job_input->user_job_ptr;
+
+	switch( end_status )
+	{
+		case JOB_STATUS_CONTINUE_RUN:
+		case JOB_STATUS_END_SUCCESS:
+		case JOB_STATUS_END_OOM:
+		case JOB_STATUS_END_ABORT:
+		case JOB_STATUS_END_TIMEOUT_SW:
+		case JOB_STATUS_END_HANG:
+		case JOB_STATUS_END_SEG_FAULT:
+		case JOB_STATUS_END_ILLEGAL_JOB:
+		case JOB_STATUS_END_UNKNOWN_ERR:
+		case JOB_STATUS_END_SHUTDOWN:
+		case JOB_STATUS_END_SYSTEM_UNUSABLE:
+			job_out->status = (mali_subsystem_job_end_code) end_status;
+			break;
+
+		default:
+			job_out->status = JOB_STATUS_END_UNKNOWN_ERR ;
+	}
+	job_out->irq_status = job200->irq_status;
+	job_out->perf_counter0 = job200->perf_counter0;
+	job_out->perf_counter1 = job200->perf_counter1;
+	job_out->render_time = job->render_time_msecs;
+
+#if defined(USING_MALI400_L2_CACHE)
+	job_out->perf_counter_l2_src0 = job200->perf_counter_l2_src0;
+	job_out->perf_counter_l2_src1 = job200->perf_counter_l2_src1;
+	job_out->perf_counter_l2_val0 = job200->perf_counter_l2_val0;
+	job_out->perf_counter_l2_val1 = job200->perf_counter_l2_val1;
+	job_out->perf_counter_l2_val0_raw = job200->perf_counter_l2_val0_raw;
+	job_out->perf_counter_l2_val1_raw = job200->perf_counter_l2_val1_raw;
+#endif
+
+	_mali_osk_notification_queue_send( session->notification_queue, job200->notification_obj);
+	job200->notification_obj = NULL;
+
+	_mali_osk_free(job200);
+}
+
+static void subsystem_mali200_renderunit_delete(mali_core_renderunit * core)
+{
+	MALI_DEBUG_PRINT(5, ("Mali PP: mali200_renderunit_delete\n"));
+	_mali_osk_free(core);
+}
+
+static void mali200_reset_hard(struct mali_core_renderunit * core)
+{
+	const int reset_finished_loop_count = 15;
+	const u32 reset_wait_target_register = MALI200_REG_ADDR_MGMT_WRITE_BOUNDARY_LOW;
+	const u32 reset_invalid_value = 0xC0FFE000;
+	const u32 reset_check_value = 0xC01A0000;
+	const u32 reset_default_value = 0;
+	int i;
+
+	MALI_DEBUG_PRINT(5, ("subsystem_mali200_renderunit_reset_core_hard called for core %s\n", core->description));
+
+	mali_core_renderunit_register_write(core, reset_wait_target_register, reset_invalid_value);
+
+	mali_core_renderunit_register_write(
+			core,
+			MALI200_REG_ADDR_MGMT_CTRL_MGMT,
+			MALI200_REG_VAL_CTRL_MGMT_FORCE_RESET);
+
+	for (i = 0; i < reset_finished_loop_count; i++)
+	{
+		mali_core_renderunit_register_write(core, reset_wait_target_register, reset_check_value);
+		if (reset_check_value == mali_core_renderunit_register_read(core, reset_wait_target_register))
+		{
+			MALI_DEBUG_PRINT(5, ("Reset loop exiting after %d iterations\n", i));
+			break;
+		}
+		_mali_osk_time_ubusydelay(10);
+	}
+
+	if (i == reset_finished_loop_count)
+	{
+		MALI_DEBUG_PRINT(1, ("The reset loop didn't work\n"));
+	}
+
+	mali_core_renderunit_register_write(core, reset_wait_target_register, reset_default_value); /* set it back to the default */
+	mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_CLEAR, MALI200_REG_VAL_IRQ_MASK_ALL);
+}
+
+static void subsystem_mali200_renderunit_reset_core(struct mali_core_renderunit * core, mali_core_reset_style style)
+{
+	MALI_DEBUG_PRINT(5, ("Mali PP: renderunit_reset_core\n"));
+
+	switch (style)
+	{
+		case MALI_CORE_RESET_STYLE_RUNABLE:
+			mali200_reset(core);
+			break;
+		case MALI_CORE_RESET_STYLE_DISABLE:
+			mali200_raw_reset(core); /* do the raw reset */
+			mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_MASK, 0); /* then disable the IRQs */
+			break;
+		case MALI_CORE_RESET_STYLE_HARD:
+			mali200_reset_hard(core);
+			break;
+		default:
+			MALI_DEBUG_PRINT(1, ("Unknown reset type %d\n", style));
+	}
+}
+
+static void subsystem_mali200_renderunit_probe_core_irq_trigger(struct mali_core_renderunit* core)
+{
+	mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_MASK, MALI200_REG_VAL_IRQ_MASK_USED);
+	mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_RAWSTAT, MALI200_REG_VAL_IRQ_FORCE_HANG);
+	_mali_osk_mem_barrier();
+}
+
+static _mali_osk_errcode_t subsystem_mali200_renderunit_probe_core_irq_finished(struct mali_core_renderunit* core)
+{
+	u32 irq_readout;
+
+	irq_readout = mali_core_renderunit_register_read(core, MALI200_REG_ADDR_MGMT_INT_STATUS);
+
+	if ( MALI200_REG_VAL_IRQ_FORCE_HANG & irq_readout )
+	{
+		mali_core_renderunit_register_write(core, MALI200_REG_ADDR_MGMT_INT_CLEAR, MALI200_REG_VAL_IRQ_FORCE_HANG);
+		_mali_osk_mem_barrier();
+		MALI_SUCCESS;
+	}
+
+    MALI_ERROR(_MALI_OSK_ERR_FAULT);
+}
+
+_mali_osk_errcode_t _mali_ukk_pp_start_job( _mali_uk_pp_start_job_s *args )
+{
+	mali_core_session * session;
+	MALI_DEBUG_ASSERT_POINTER(args);
+	MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+	session = (mali_core_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_mali200_id);
+	MALI_CHECK_NON_NULL(session, _MALI_OSK_ERR_FAULT);
+	return mali_core_subsystem_ioctl_start_job(session, args);
+}
+
+_mali_osk_errcode_t _mali_ukk_get_pp_number_of_cores( _mali_uk_get_pp_number_of_cores_s *args )
+{
+	mali_core_session * session;
+	MALI_DEBUG_ASSERT_POINTER(args);
+	MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+	session = (mali_core_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_mali200_id);
+	MALI_CHECK_NON_NULL(session, _MALI_OSK_ERR_FAULT);
+	return mali_core_subsystem_ioctl_number_of_cores_get(session, &args->number_of_cores);
+}
+
+_mali_osk_errcode_t _mali_ukk_get_pp_core_version( _mali_uk_get_pp_core_version_s *args )
+{
+	mali_core_session * session;
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+    session = (mali_core_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_mali200_id);
+    MALI_CHECK_NON_NULL(session, _MALI_OSK_ERR_FAULT);
+    return mali_core_subsystem_ioctl_core_version_get(session, &args->version);
+}
+
+void _mali_ukk_pp_abort_job( _mali_uk_pp_abort_job_s * args)
+{
+	mali_core_session * session;
+    MALI_DEBUG_ASSERT_POINTER(args);
+    if (NULL == args->ctx) return;
+    session = (mali_core_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_mali200_id);
+    if (NULL == session) return;
+    mali_core_subsystem_ioctl_abort_job(session, args->abort_id);
+
+}
+
+#if USING_MALI_PMM
+
+_mali_osk_errcode_t malipp_signal_power_up( u32 core_num, mali_bool queue_only )
+{
+	MALI_DEBUG_PRINT(4, ("Mali PP: signal power up core: %d - queue_only: %d\n", core_num, queue_only ));
+
+	return( mali_core_subsystem_signal_power_up( &subsystem_mali200, core_num, queue_only ) );
+}
+	
+_mali_osk_errcode_t malipp_signal_power_down( u32 core_num, mali_bool immediate_only )
+{
+	MALI_DEBUG_PRINT(4, ("Mali PP: signal power down core: %d - immediate_only: %d\n", core_num, immediate_only ));
+
+	return( mali_core_subsystem_signal_power_down( &subsystem_mali200, core_num, immediate_only ) );
+}
+
+#endif
+
+#if MALI_STATE_TRACKING
+void mali200_subsystem_dump_state(void)
+{
+	mali_core_renderunit_dump_state(&subsystem_mali200);
+}
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_common.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_common.h
new file mode 100644
index 00000000000..08516c56a9f
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_common.h
@@ -0,0 +1,171 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_COMMON_H__
+#define __MALI_KERNEL_COMMON_H__
+
+/* Make sure debug is defined when it should be */
+#ifndef DEBUG
+	#if defined(_DEBUG)
+		#define DEBUG
+	#endif
+#endif
+
+/* The file include several useful macros for error checking, debugging and printing.
+ * - MALI_PRINTF(...)           Do not use this function: Will be included in Release builds.
+ * - MALI_DEBUG_PRINT(nr, (X) ) Prints the second argument if nr<=MALI_DEBUG_LEVEL.
+ * - MALI_DEBUG_ERROR( (X) )    Prints an errortext, a source trace, and the given error message.
+ * - MALI_DEBUG_ASSERT(exp,(X)) If the asserted expr is false, the program will exit.
+ * - MALI_DEBUG_ASSERT_POINTER(pointer)  Triggers if the pointer is a zero pointer.
+ * - MALI_DEBUG_CODE( X )       The code inside the macro is only compiled in Debug builds.
+ *
+ * The (X) means that you must add an extra parenthesis around the argumentlist.
+ *
+ * The  printf function: MALI_PRINTF(...) is routed to _mali_osk_debugmsg
+ *
+ * Suggested range for the DEBUG-LEVEL is [1:6] where
+ * [1:2] Is messages with highest priority, indicate possible errors.
+ * [3:4] Is messages with medium priority, output important variables.
+ * [5:6] Is messages with low priority, used during extensive debugging.
+ */
+
+ /**
+ *  Fundamental error macro. Reports an error code. This is abstracted to allow us to
+ *  easily switch to a different error reporting method if we want, and also to allow
+ *  us to search for error returns easily.
+ *
+ *  Note no closing semicolon - this is supplied in typical usage:
+ *
+ *  MALI_ERROR(MALI_ERROR_OUT_OF_MEMORY);
+ */
+#define MALI_ERROR(error_code) return (error_code)
+
+/**
+ *  Basic error macro, to indicate success.
+ *  Note no closing semicolon - this is supplied in typical usage:
+ *
+ *  MALI_SUCCESS;
+ */
+#define MALI_SUCCESS MALI_ERROR(_MALI_OSK_ERR_OK)
+
+/**
+ *	Basic error macro. This checks whether the given condition is true, and if not returns
+ *	from this function with the supplied error code. This is a macro so that we can override it
+ *	for stress testing.
+ *
+ *	Note that this uses the do-while-0 wrapping to ensure that we don't get problems with dangling
+ *	else clauses. Note also no closing semicolon - this is supplied in typical usage:
+ *
+ *	MALI_CHECK((p!=NULL), ERROR_NO_OBJECT);
+ */
+#define MALI_CHECK(condition, error_code) do { if(!(condition)) MALI_ERROR(error_code); } while(0)
+
+/**
+ *	Error propagation macro. If the expression given is anything other than _MALI_OSK_NO_ERROR,
+ *	then the value is returned from the enclosing function as an error code. This effectively
+ *	acts as a guard clause, and propagates error values up the call stack. This uses a
+ *	temporary value to ensure that the error expression is not evaluated twice.
+ *  If the counter for forcing a failure has been set using _mali_force_error, this error will be
+ *  returned without evaluating the expression in MALI_CHECK_NO_ERROR
+ */
+#define MALI_CHECK_NO_ERROR(expression) \
+    do { _mali_osk_errcode_t _check_no_error_result=(expression); \
+         if(_check_no_error_result != _MALI_OSK_ERR_OK) \
+         MALI_ERROR(_check_no_error_result); \
+    } while(0)
+
+/**
+ *  Pointer check macro. Checks non-null pointer.
+ */
+#define MALI_CHECK_NON_NULL(pointer, error_code) MALI_CHECK( ((pointer)!=NULL), (error_code) )
+
+/**
+ *	Error macro with goto. This checks whether the given condition is true, and if not jumps
+ *	to the specified label using a goto. The label must therefore be local to the function in
+ *	which this macro appears. This is most usually used to execute some clean-up code before
+ *	exiting with a call to ERROR.
+ *
+ *	Like the other macros, this is a macro to allow us to override the condition if we wish,
+ *	e.g. to force an error during stress testing.
+ */
+#define MALI_CHECK_GOTO(condition, label) do { if(!(condition)) goto label; } while(0)
+
+/**
+ *  Explicitly ignore a parameter passed into a function, to suppress compiler warnings.
+ *  Should only be used with parameter names.
+ */
+#define MALI_IGNORE(x) x=x
+
+#define MALI_PRINTF(args) _mali_osk_dbgmsg args;
+
+#define MALI_PRINT_ERROR(args) do{ \
+	MALI_PRINTF(("Mali: ERR: %s\n" ,__FILE__)); \
+	MALI_PRINTF(("           %s()%4d\n           ", __FUNCTION__, __LINE__)) ; \
+	MALI_PRINTF(args); \
+	MALI_PRINTF(("\n")); \
+	} while(0)
+
+#define MALI_PRINT(args) do{ \
+	MALI_PRINTF(("Mali: ")); \
+	MALI_PRINTF(args); \
+	} while (0)
+
+#ifdef DEBUG
+extern int mali_debug_level;
+
+#define MALI_DEBUG_CODE(code) code
+#define MALI_DEBUG_PRINT(level, args)  do { \
+	if((level) <=  mali_debug_level)\
+        {MALI_PRINTF(("Mali<" #level ">: ")); MALI_PRINTF(args); } \
+	} while (0)
+
+#define MALI_DEBUG_PRINT_ERROR(args) MALI_PRINT_ERROR(args)
+
+#define MALI_DEBUG_PRINT_IF(level,condition,args)  \
+	if((condition)&&((level) <=  mali_debug_level))\
+        {MALI_PRINTF(("Mali<" #level ">: ")); MALI_PRINTF(args); }
+
+#define MALI_DEBUG_PRINT_ELSE(level, args)\
+	else if((level) <=  mali_debug_level)\
+    { MALI_PRINTF(("Mali<" #level ">: ")); MALI_PRINTF(args); }
+
+/**
+ * @note these variants of DEBUG ASSERTS will cause a debugger breakpoint
+ * to be entered (see _mali_osk_break() ). An alternative would be to call
+ * _mali_osk_abort(), on OSs that support it.
+ */
+#define MALI_DEBUG_PRINT_ASSERT(condition, args) do  {if( !(condition)) { MALI_PRINT_ERROR(args); _mali_osk_break(); } } while(0)
+#define MALI_DEBUG_ASSERT_POINTER(pointer) do  {if( (pointer)== NULL) {MALI_PRINT_ERROR(("NULL pointer " #pointer)); _mali_osk_break();} } while(0)
+#define MALI_DEBUG_ASSERT(condition) do  {if( !(condition)) {MALI_PRINT_ERROR(("ASSERT failed: " #condition )); _mali_osk_break();} } while(0)
+
+#else /* DEBUG */
+
+#define MALI_DEBUG_CODE(code)
+#define MALI_DEBUG_PRINT(string,args) do {} while(0)
+#define MALI_DEBUG_PRINT_ERROR(args) do {} while(0)
+#define MALI_DEBUG_PRINT_IF(level,condition,args) do {} while(0)
+#define MALI_DEBUG_PRINT_ELSE(level,condition,args) do {} while(0)
+#define MALI_DEBUG_PRINT_ASSERT(condition,args) do {} while(0)
+#define MALI_DEBUG_ASSERT_POINTER(pointer) do {} while(0)
+#define MALI_DEBUG_ASSERT(condition) do {} while(0)
+
+#endif /* DEBUG */
+
+/**
+ * variables from user space cannot be dereferenced from kernel space; tagging them
+ * with __user allows the GCC compiler to generate a warning. Other compilers may
+ * not support this so we define it here as an empty macro if the compiler doesn't
+ * define it.
+ */
+#ifndef __user
+#define __user
+#endif
+
+#endif /* __MALI_KERNEL_COMMON_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_core.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_core.c
new file mode 100644
index 00000000000..a40808bd2d8
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_core.c
@@ -0,0 +1,892 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_subsystem.h"
+#include "mali_kernel_mem.h"
+#include "mali_kernel_session_manager.h"
+#include "mali_kernel_pp.h"
+#include "mali_kernel_gp.h"
+#include "mali_osk.h"
+#include "mali_osk_mali.h"
+#include "mali_ukk.h"
+#include "mali_kernel_core.h"
+#include "mali_kernel_rendercore.h"
+#if defined USING_MALI400_L2_CACHE
+#include "mali_kernel_l2_cache.h"
+#endif
+#if USING_MALI_PMM
+#include "mali_pmm.h"
+#endif /* USING_MALI_PMM */
+
+/* platform specific set up */
+#include "mali_platform.h"
+
+/* Initialized when this subsystem is initialized. This is determined by the
+ * position in subsystems[], and so the value used to initialize this is
+ * determined at compile time */
+static mali_kernel_subsystem_identifier mali_subsystem_core_id = -1;
+
+/** Pointer to table of resource definitions available to the Mali driver.
+ *  _mali_osk_resources_init() sets up the pointer to this table.
+ */
+static _mali_osk_resource_t *arch_configuration = NULL;
+
+/** Number of resources initialized by _mali_osk_resources_init() */
+static u32 num_resources;
+
+static _mali_osk_errcode_t register_resources( _mali_osk_resource_t **arch_configuration, u32 num_resources );
+
+static _mali_osk_errcode_t initialize_subsystems(void);
+static void terminate_subsystems(void);
+
+static _mali_osk_errcode_t mali_kernel_subsystem_core_setup(mali_kernel_subsystem_identifier id);
+static void mali_kernel_subsystem_core_cleanup(mali_kernel_subsystem_identifier id);
+static _mali_osk_errcode_t mali_kernel_subsystem_core_system_info_fill(_mali_system_info* info);
+static _mali_osk_errcode_t mali_kernel_subsystem_core_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue);
+
+static _mali_osk_errcode_t build_system_info(void);
+
+/**
+ * @brief handler for MEM_VALIDATION resources
+ *
+ * This resource handler is common to all memory systems. It provides a default
+ * means for validating requests to map in external memory via
+ * _mali_ukk_map_external_mem. In addition, if _mali_ukk_va_to_pa is
+ * implemented, then _mali_ukk_va_to_pa can make use of this MEM_VALIDATION
+ * resource.
+ *
+ * MEM_VALIDATION also provide a CPU physical to Mali physical address
+ * translation, for use by _mali_ukk_map_external_mem.
+ *
+ * @note MEM_VALIDATION resources are only to handle simple cases where a
+ * certain physical address range is allowed to be mapped in by any process,
+ * e.g. a framebuffer at a fixed location. If the implementor has more complex
+ * mapping requirements, then they must either:
+ * - implement their own memory validation function
+ * - or, integrate with UMP.
+ *
+ * @param resource The resource to handle (type MEM_VALIDATION)
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+static _mali_osk_errcode_t mali_kernel_core_resource_mem_validation(_mali_osk_resource_t * resource);
+
+/* MEM_VALIDATION handler state */
+typedef struct
+{
+	u32 phys_base;        /**< Mali physical base of the memory, page aligned */
+	u32 size;             /**< size in bytes of the memory, multiple of page size */
+	s32 cpu_usage_adjust; /**< Offset to add to Mali Physical address to obtain CPU physical address */
+} _mali_mem_validation_t;
+
+#define INVALID_MEM 0xffffffff
+
+static _mali_mem_validation_t mem_validator = { INVALID_MEM, INVALID_MEM, -1 };
+
+static struct mali_kernel_subsystem mali_subsystem_core =
+{
+	mali_kernel_subsystem_core_setup,               /* startup */
+    mali_kernel_subsystem_core_cleanup,             /* shutdown */
+    NULL,                                           /* load_complete */
+    mali_kernel_subsystem_core_system_info_fill,    /* system_info_fill */
+	mali_kernel_subsystem_core_session_begin,       /* session_begin */
+    NULL,                                           /* session_end */
+    NULL,                                           /* broadcast_notification */
+#if MALI_STATE_TRACKING
+	NULL,                                           /* dump_state */
+#endif
+};
+
+static struct mali_kernel_subsystem * subsystems[] =
+{
+	/* always initialize the hw subsystems first */
+	/* always included */
+	&mali_subsystem_memory,
+
+#if USING_MALI_PMM
+	/* The PMM must be initialized before any cores - including L2 cache */
+	&mali_subsystem_pmm,
+#endif
+
+	/* The rendercore subsystem must be initialized before any subsystem based on the
+	 * rendercores is started e.g. mali_subsystem_mali200 and mali_subsystem_gp2 */
+	&mali_subsystem_rendercore,
+
+ 	/* add reference to the subsystem */
+	&mali_subsystem_mali200,
+
+ 	/* add reference to the subsystem */
+	&mali_subsystem_gp2,
+
+#if defined USING_MALI400_L2_CACHE
+	&mali_subsystem_l2_cache,
+#endif
+
+	/* always included */
+	/* NOTE Keep the core entry at the tail of the list */
+	&mali_subsystem_core
+};
+
+#define SUBSYSTEMS_COUNT ( sizeof(subsystems) / sizeof(subsystems[0]) )
+
+/* Pointers to this type available as incomplete struct in mali_kernel_session_manager.h */
+struct mali_session_data
+{
+	void * subsystem_data[SUBSYSTEMS_COUNT];
+	_mali_osk_notification_queue_t * ioctl_queue;
+};
+
+static mali_kernel_resource_registrator resource_handler[RESOURCE_TYPE_COUNT] = { NULL, };
+
+/* system info variables */
+static _mali_osk_lock_t *system_info_lock = NULL;
+static _mali_system_info * system_info = NULL;
+static u32 system_info_size = 0;
+
+/* is called from OS specific driver entry point */
+_mali_osk_errcode_t mali_kernel_constructor( void )
+{
+    _mali_osk_errcode_t err;
+
+	err = mali_platform_init(NULL);
+	if (_MALI_OSK_ERR_OK != err) goto error1;
+
+    err = _mali_osk_init();
+    if (_MALI_OSK_ERR_OK != err) goto error2;
+
+	MALI_DEBUG_PRINT(2, ("\n"));
+	MALI_DEBUG_PRINT(2, ("Inserting Mali v%d device driver. \n",_MALI_API_VERSION));
+	MALI_DEBUG_PRINT(2, ("Compiled: %s, time: %s.\n", __DATE__, __TIME__));
+	MALI_DEBUG_PRINT(2, ("Svn revision: %s\n", SVN_REV_STRING));
+
+    err  = initialize_subsystems();
+    if (_MALI_OSK_ERR_OK != err) goto error3;
+
+    MALI_PRINT(("Mali device driver %s loaded\n", SVN_REV_STRING));
+
+	MALI_SUCCESS;
+
+error3:
+	MALI_PRINT(("Mali subsystems failed\n"));
+    _mali_osk_term();
+error2:
+	MALI_PRINT(("Mali device driver init failed\n"));
+	if (_MALI_OSK_ERR_OK != mali_platform_deinit(NULL))
+	{
+		MALI_PRINT(("Failed to deinit platform\n"));
+	}
+error1:
+	MALI_PRINT(("Failed to init platform\n"));
+	MALI_ERROR(err);
+}
+
+/* is called from OS specific driver exit point */
+void mali_kernel_destructor( void )
+{
+	MALI_DEBUG_PRINT(2, ("\n"));
+	MALI_DEBUG_PRINT(2, ("Unloading Mali v%d device driver.\n",_MALI_API_VERSION));
+	terminate_subsystems(); /* subsystems are responsible for their registered resources */
+    _mali_osk_term();
+
+	if (_MALI_OSK_ERR_OK != mali_platform_deinit(NULL))
+	{
+		MALI_PRINT(("Failed to deinit platform\n"));
+	}
+	MALI_DEBUG_PRINT(2, ("Module unloaded.\n"));
+}
+
+_mali_osk_errcode_t register_resources( _mali_osk_resource_t **arch_configuration, u32 num_resources )
+{
+	_mali_osk_resource_t *arch_resource = *arch_configuration;
+	u32 i;
+#if USING_MALI_PMM
+	u32 is_pmu_first_resource = 1;
+#endif /* USING_MALI_PMM */
+
+	/* loop over arch configuration */
+	for (i = 0; i < num_resources; ++i, arch_resource++)
+	{
+		if (  (arch_resource->type >= RESOURCE_TYPE_FIRST) &&
+		      (arch_resource->type < RESOURCE_TYPE_COUNT) &&
+		      (NULL != resource_handler[arch_resource->type])
+		   )
+		{	
+#if USING_MALI_PMM
+			if((arch_resource->type != PMU) && (is_pmu_first_resource == 1))
+			{
+				_mali_osk_resource_t mali_pmu_virtual_resource;
+				mali_pmu_virtual_resource.type = PMU;
+				mali_pmu_virtual_resource.description = "Virtual PMU";
+				mali_pmu_virtual_resource.base = 0x00000000;
+				mali_pmu_virtual_resource.cpu_usage_adjust = 0;
+				mali_pmu_virtual_resource.size = 0;
+				mali_pmu_virtual_resource.irq = 0;
+				mali_pmu_virtual_resource.flags = 0;
+				mali_pmu_virtual_resource.mmu_id = 0;
+				mali_pmu_virtual_resource.alloc_order = 0;
+				MALI_CHECK_NO_ERROR(resource_handler[mali_pmu_virtual_resource.type](&mali_pmu_virtual_resource));
+			}
+			is_pmu_first_resource = 0;
+#endif /* USING_MALI_PMM */
+
+			MALI_CHECK_NO_ERROR(resource_handler[arch_resource->type](arch_resource));
+			/* the subsystem shutdown process will release all the resources already registered */
+		}
+		else
+		{
+			MALI_DEBUG_PRINT(1, ("No handler installed for resource %s, type %d\n", arch_resource->description, arch_resource->type));
+			MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+		}
+	}
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t initialize_subsystems(void)
+{
+	int i, j;
+    _mali_osk_errcode_t err = _MALI_OSK_ERR_FAULT; /* default error code */
+
+    MALI_CHECK_NON_NULL(system_info_lock = _mali_osk_lock_init( (_mali_osk_lock_flags_t)(_MALI_OSK_LOCKFLAG_SPINLOCK | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE), 0, 0 ), _MALI_OSK_ERR_FAULT);
+
+	for (i = 0; i < (int)SUBSYSTEMS_COUNT; ++i)
+	{
+		if (NULL != subsystems[i]->startup)
+		{
+			/* the subsystem has a startup function defined */
+			err = subsystems[i]->startup(i); /* the subsystem identifier is the offset in our subsystems array */
+			if (_MALI_OSK_ERR_OK != err) goto cleanup;
+		}
+	}
+
+	for (j = 0; j < (int)SUBSYSTEMS_COUNT; ++j)
+	{
+		if (NULL != subsystems[j]->load_complete)
+		{
+			/* the subsystem has a load_complete function defined */
+			err = subsystems[j]->load_complete(j);
+			if (_MALI_OSK_ERR_OK != err) goto cleanup;
+		}
+	}
+
+	/* All systems loaded and resources registered */
+	/* Build system info */
+	if (_MALI_OSK_ERR_OK != build_system_info()) goto cleanup;
+
+	MALI_SUCCESS; /* all ok */
+
+cleanup:
+	/* i is index of subsystem which failed to start, all indices before that has to be shut down */
+	for (i = i - 1; i >= 0; --i)
+	{
+		/* the subsystem identifier is the offset in our subsystems array */
+		/* Call possible shutdown notficiation functions */
+		if (NULL != subsystems[i]->shutdown) subsystems[i]->shutdown(i);
+	}
+
+    _mali_osk_lock_term( system_info_lock );
+    MALI_ERROR(err); /* err is what the module which failed its startup returned, or the default */
+}
+
+static void terminate_subsystems(void)
+{
+	int i;
+	/* shut down subsystems in reverse order from startup */
+	for (i = SUBSYSTEMS_COUNT - 1; i >= 0; --i)
+	{
+		/* the subsystem identifier is the offset in our subsystems array */
+		if (NULL != subsystems[i]->shutdown) subsystems[i]->shutdown(i);
+	}
+    if (system_info_lock) _mali_osk_lock_term( system_info_lock );
+}
+
+void _mali_kernel_core_broadcast_subsystem_message(mali_core_notification_message message, u32 data)
+{
+	int i;
+
+	for (i = 0; i < (int)SUBSYSTEMS_COUNT; ++i)
+	{
+		if (NULL != subsystems[i]->broadcast_notification)
+		{
+			subsystems[i]->broadcast_notification(message, data);
+		}
+	}
+}
+
+static _mali_osk_errcode_t mali_kernel_subsystem_core_setup(mali_kernel_subsystem_identifier id)
+{
+    mali_subsystem_core_id = id;
+
+	/* Register our own resources */
+	MALI_CHECK_NO_ERROR(_mali_kernel_core_register_resource_handler(MEM_VALIDATION, mali_kernel_core_resource_mem_validation));
+
+    /* parse the arch resource definition and tell all the subsystems */
+	/* this is why the core subsystem has to be specified last in the subsystem array */
+    MALI_CHECK_NO_ERROR(_mali_osk_resources_init(&arch_configuration, &num_resources));
+
+    MALI_CHECK_NO_ERROR(register_resources(&arch_configuration, num_resources));
+
+    /* resource parsing succeeded and the subsystem have corretly accepted their resources */
+	MALI_SUCCESS;
+}
+
+static void mali_kernel_subsystem_core_cleanup(mali_kernel_subsystem_identifier id)
+{
+    _mali_osk_resources_term(&arch_configuration, num_resources);
+}
+
+
+static _mali_osk_errcode_t build_system_info(void)
+{
+	unsigned int i;
+	int err = _MALI_OSK_ERR_FAULT;
+	_mali_system_info * new_info, * cleanup;
+	_mali_core_info * current_core;
+	_mali_mem_info * current_mem;
+	u32 new_size = 0;
+
+	/* create a new system info struct */
+	MALI_CHECK_NON_NULL(new_info = (_mali_system_info *)_mali_osk_malloc(sizeof(_mali_system_info)), _MALI_OSK_ERR_NOMEM);
+
+	_mali_osk_memset(new_info, 0, sizeof(_mali_system_info));
+
+	/* if an error happens during any of the system_info_fill calls cleanup the new info structs */
+	cleanup = new_info;
+
+	/* ask each subsystems to fill in their info */
+	for (i = 0; i < SUBSYSTEMS_COUNT; ++i)
+	{
+		if (NULL != subsystems[i]->system_info_fill)
+		{
+			err = subsystems[i]->system_info_fill(new_info);
+			if (_MALI_OSK_ERR_OK != err) goto error_exit;
+		}
+	}
+
+	/* building succeeded, calculate the size */
+
+	/* size needed of the system info struct itself */
+	new_size = sizeof(_mali_system_info);
+
+	/* size needed for the cores */
+	for (current_core = new_info->core_info; NULL != current_core; current_core = current_core->next)
+	{
+		new_size += sizeof(_mali_core_info);
+	}
+
+	/* size needed for the memory banks */
+	for (current_mem = new_info->mem_info; NULL != current_mem; current_mem = current_mem->next)
+	{
+		new_size += sizeof(_mali_mem_info);
+	}
+
+	/* lock system info access so a user wont't get a corrupted version */
+	_mali_osk_lock_wait( system_info_lock, _MALI_OSK_LOCKMODE_RW );
+
+	/* cleanup the old one */
+	cleanup = system_info;
+	/* set new info */
+	system_info = new_info;
+	system_info_size = new_size;
+
+	/* we're safe */
+	_mali_osk_lock_signal( system_info_lock, _MALI_OSK_LOCKMODE_RW );
+
+	/* ok result */
+	err = _MALI_OSK_ERR_OK;
+
+	/* we share the cleanup routine with the error case */
+error_exit:
+	if (NULL == cleanup) MALI_ERROR((_mali_osk_errcode_t)err); /* no cleanup needed, return what err contains */
+
+	/* cleanup */
+
+	/* delete all the core info structs */
+	while (NULL != cleanup->core_info)
+	{
+		current_core = cleanup->core_info;
+		cleanup->core_info = cleanup->core_info->next;
+		_mali_osk_free(current_core);
+	}
+
+	/* delete all the mem info struct */
+	while (NULL != cleanup->mem_info)
+	{
+		current_mem = cleanup->mem_info;
+		cleanup->mem_info = cleanup->mem_info->next;
+		_mali_osk_free(current_mem);
+	}
+
+	/* delete the system info struct itself */
+	_mali_osk_free(cleanup);
+
+	/* return whatever err is, we could end up here in both the error and success cases */
+	MALI_ERROR((_mali_osk_errcode_t)err);
+}
+
+_mali_osk_errcode_t _mali_ukk_get_api_version( _mali_uk_get_api_version_s *args )
+{
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+
+    /* check compatability */
+	if ( args->version == _MALI_UK_API_VERSION )
+	{
+		args->compatible = 1;
+	}
+	else
+	{
+		args->compatible = 0;
+	}
+
+	args->version = _MALI_UK_API_VERSION; /* report our version */
+
+	/* success regardless of being compatible or not */
+    MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t _mali_ukk_get_system_info_size(_mali_uk_get_system_info_size_s *args)
+{
+    MALI_DEBUG_ASSERT_POINTER(args);
+    args->size = system_info_size;
+    MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t _mali_ukk_get_system_info( _mali_uk_get_system_info_s *args )
+{
+	_mali_core_info * current_core;
+	_mali_mem_info * current_mem;
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_FAULT;
+	void * current_write_pos, ** current_patch_pos;
+    u32 adjust_ptr_base;
+
+	/* check input */
+	MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+    MALI_CHECK_NON_NULL(args->system_info, _MALI_OSK_ERR_INVALID_ARGS);
+
+	/* lock the system info */
+	_mali_osk_lock_wait( system_info_lock, _MALI_OSK_LOCKMODE_RW );
+
+	/* first check size */
+	if (args->size < system_info_size) goto exit_when_locked;
+
+	/* we build a copy of system_info in the user space buffer specified by the user and
+     * patch up the pointers. The ukk_private members of _mali_uk_get_system_info_s may
+     * indicate a different base address for patching the pointers (normally the
+     * address of the provided system_info buffer would be used). This is helpful when
+     * the system_info buffer needs to get copied to user space and the pointers need
+     * to be in user space.
+     */
+    if (0 == args->ukk_private)
+    {
+        adjust_ptr_base = (u32)args->system_info;
+    }
+    else
+    {
+        adjust_ptr_base = args->ukk_private;
+    }
+
+	/* copy each struct into the buffer, and update its pointers */
+	current_write_pos = (void *)args->system_info;
+
+	/* first, the master struct */
+	_mali_osk_memcpy(current_write_pos, system_info, sizeof(_mali_system_info));
+
+	/* advance write pointer */
+	current_write_pos = (void *)((u32)current_write_pos + sizeof(_mali_system_info));
+
+	/* first we write the core info structs, patch starts at master's core_info pointer */
+	current_patch_pos = (void **)((u32)args->system_info + offsetof(_mali_system_info, core_info));
+
+	for (current_core = system_info->core_info; NULL != current_core; current_core = current_core->next)
+	{
+
+		/* patch the pointer pointing to this core */
+		*current_patch_pos = (void*)(adjust_ptr_base + ((u32)current_write_pos - (u32)args->system_info));
+
+		/* copy the core info */
+		_mali_osk_memcpy(current_write_pos, current_core, sizeof(_mali_core_info));
+
+		/* update patch pos */
+		current_patch_pos = (void **)((u32)current_write_pos + offsetof(_mali_core_info, next));
+
+		/* advance write pos in memory */
+		current_write_pos = (void *)((u32)current_write_pos + sizeof(_mali_core_info));
+	}
+	/* patching of last patch pos is not needed, since we wrote NULL there in the first place */
+
+	/* then we write the mem info structs, patch starts at master's mem_info pointer */
+	current_patch_pos = (void **)((u32)args->system_info + offsetof(_mali_system_info, mem_info));
+
+	for (current_mem = system_info->mem_info; NULL != current_mem; current_mem = current_mem->next)
+	{
+		/* patch the pointer pointing to this core */
+		*current_patch_pos = (void*)(adjust_ptr_base + ((u32)current_write_pos - (u32)args->system_info));
+
+		/* copy the core info */
+		_mali_osk_memcpy(current_write_pos, current_mem, sizeof(_mali_mem_info));
+
+		/* update patch pos */
+		current_patch_pos = (void **)((u32)current_write_pos + offsetof(_mali_mem_info, next));
+
+		/* advance write pos in memory */
+		current_write_pos = (void *)((u32)current_write_pos + sizeof(_mali_mem_info));
+	}
+	/* patching of last patch pos is not needed, since we wrote NULL there in the first place */
+
+	err = _MALI_OSK_ERR_OK;
+exit_when_locked:
+	_mali_osk_lock_signal( system_info_lock, _MALI_OSK_LOCKMODE_RW );
+    MALI_ERROR(err);
+}
+
+_mali_osk_errcode_t _mali_ukk_wait_for_notification( _mali_uk_wait_for_notification_s *args )
+{
+	_mali_osk_errcode_t err;
+	_mali_osk_notification_t * notification;
+    _mali_osk_notification_queue_t *queue;
+
+    /* check input */
+	MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+
+    queue = (_mali_osk_notification_queue_t *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_core_id);
+
+	/* if the queue does not exist we're currently shutting down */
+	if (NULL == queue)
+	{
+		MALI_DEBUG_PRINT(1, ("No notification queue registered with the session. Asking userspace to stop querying\n"));
+        args->type = _MALI_NOTIFICATION_CORE_SHUTDOWN_IN_PROGRESS;
+		MALI_SUCCESS;
+	}
+
+    /* receive a notification, might sleep */
+	err = _mali_osk_notification_queue_receive(queue, &notification);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+        MALI_ERROR(err); /* errcode returned, pass on to caller */
+    }
+
+	/* copy the buffer to the user */
+    args->type = (_mali_uk_notification_type)notification->notification_type;
+    _mali_osk_memcpy(&args->data, notification->result_buffer, notification->result_buffer_size);
+
+	/* finished with the notification */
+	_mali_osk_notification_delete( notification );
+
+    MALI_SUCCESS; /* all ok */
+}
+
+_mali_osk_errcode_t _mali_ukk_post_notification( _mali_uk_post_notification_s *args )
+{
+	_mali_osk_notification_t * notification;
+    _mali_osk_notification_queue_t *queue;
+
+    /* check input */
+	MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+
+    queue = (_mali_osk_notification_queue_t *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_core_id);
+
+	/* if the queue does not exist we're currently shutting down */
+	if (NULL == queue)
+	{
+		MALI_DEBUG_PRINT(1, ("No notification queue registered with the session. Asking userspace to stop querying\n"));
+		MALI_SUCCESS;
+	}
+
+	notification = _mali_osk_notification_create(args->type, 0);
+	if ( NULL == notification)
+	{
+		MALI_PRINT_ERROR( ("Failed to create notification object\n")) ;
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	_mali_osk_notification_queue_send(queue, notification);
+
+    MALI_SUCCESS; /* all ok */
+}
+
+static _mali_osk_errcode_t mali_kernel_subsystem_core_system_info_fill(_mali_system_info* info)
+{
+    MALI_CHECK_NON_NULL(info, _MALI_OSK_ERR_INVALID_ARGS);
+
+	info->drivermode = _MALI_DRIVER_MODE_NORMAL;
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_kernel_subsystem_core_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue)
+{
+    MALI_CHECK_NON_NULL(slot, _MALI_OSK_ERR_INVALID_ARGS);
+	*slot = queue;
+	MALI_SUCCESS;
+}
+
+/* MEM_VALIDATION resource handler */
+static _mali_osk_errcode_t mali_kernel_core_resource_mem_validation(_mali_osk_resource_t * resource)
+{
+	/* Check that no other MEM_VALIDATION resources exist */
+	MALI_CHECK( ((u32)-1) == mem_validator.phys_base, _MALI_OSK_ERR_FAULT );
+
+	/* Check restrictions on page alignment */
+	MALI_CHECK( 0 == (resource->base & (~_MALI_OSK_CPU_PAGE_MASK)), _MALI_OSK_ERR_FAULT );
+	MALI_CHECK( 0 == (resource->size & (~_MALI_OSK_CPU_PAGE_MASK)), _MALI_OSK_ERR_FAULT );
+	MALI_CHECK( 0 == (resource->cpu_usage_adjust & (~_MALI_OSK_CPU_PAGE_MASK)), _MALI_OSK_ERR_FAULT );
+
+	mem_validator.phys_base = resource->base;
+	mem_validator.size = resource->size;
+	mem_validator.cpu_usage_adjust = resource->cpu_usage_adjust;
+	MALI_DEBUG_PRINT( 2, ("Memory Validator '%s' installed for Mali physical address base==0x%08X, size==0x%08X, cpu_adjust==0x%08X\n",
+						  resource->description, mem_validator.phys_base, mem_validator.size, mem_validator.cpu_usage_adjust ));
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_kernel_core_translate_cpu_to_mali_phys_range( u32 *phys_base, u32 size )
+{
+	u32 mali_phys_base;
+
+	mali_phys_base = *phys_base - mem_validator.cpu_usage_adjust;
+
+	MALI_CHECK( 0 == ( mali_phys_base & (~_MALI_OSK_CPU_PAGE_MASK)), _MALI_OSK_ERR_FAULT );
+	MALI_CHECK( 0 == ( size & (~_MALI_OSK_CPU_PAGE_MASK)), _MALI_OSK_ERR_FAULT );
+
+	MALI_CHECK_NO_ERROR( mali_kernel_core_validate_mali_phys_range( mali_phys_base, size ) );
+
+	*phys_base = mali_phys_base;
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_kernel_core_validate_mali_phys_range( u32 phys_base, u32 size )
+{
+	MALI_CHECK_GOTO( 0 == ( phys_base & (~_MALI_OSK_CPU_PAGE_MASK)), failure );
+	MALI_CHECK_GOTO( 0 == ( size & (~_MALI_OSK_CPU_PAGE_MASK)), failure );
+
+	if ( phys_base             >= mem_validator.phys_base
+		 && (phys_base + size) >= mem_validator.phys_base
+		 && phys_base          <= (mem_validator.phys_base + mem_validator.size)
+		 && (phys_base + size) <= (mem_validator.phys_base + mem_validator.size) )
+	{
+		MALI_SUCCESS;
+	}
+
+ failure:
+	MALI_PRINTF( ("*******************************************************************************\n") );
+	MALI_PRINTF( ("MALI PHYSICAL RANGE VALIDATION ERROR!\n") );
+	MALI_PRINTF( ("\n") );
+	MALI_PRINTF( ("We failed to validate a Mali-Physical range that the user-side wished to map in\n") );
+	MALI_PRINTF( ("\n") );
+	MALI_PRINTF( ("It is likely that the user-side wished to do Direct Rendering, but a suitable\n") );
+	MALI_PRINTF( ("address range validation mechanism has not been correctly setup\n") );
+	MALI_PRINTF( ("\n") );
+	MALI_PRINTF( ("The range supplied was: phys_base=0x%08X, size=0x%08X\n", phys_base, size) );
+	MALI_PRINTF( ("\n") );
+	MALI_PRINTF( ("Please refer to the ARM Mali Software Integration Guide for more information.\n") );
+	MALI_PRINTF( ("\n") );
+	MALI_PRINTF( ("*******************************************************************************\n") );
+
+	MALI_ERROR( _MALI_OSK_ERR_FAULT );
+}
+
+
+_mali_osk_errcode_t _mali_kernel_core_register_resource_handler(_mali_osk_resource_type_t type, mali_kernel_resource_registrator handler)
+{
+	MALI_CHECK(type < RESOURCE_TYPE_COUNT, _MALI_OSK_ERR_INVALID_ARGS);
+	MALI_DEBUG_ASSERT(NULL == resource_handler[type]); /* A handler for resource already exists */
+	resource_handler[type] = handler;
+	MALI_SUCCESS;
+}
+
+void * mali_kernel_session_manager_slot_get(struct mali_session_data * session_data, int id)
+{
+	MALI_DEBUG_ASSERT_POINTER(session_data);
+	if(id >= SUBSYSTEMS_COUNT) { MALI_DEBUG_PRINT(3, ("mali_kernel_session_manager_slot_get: id %d out of range\n", id)); return NULL; }
+
+	if (NULL == session_data) { MALI_DEBUG_PRINT(3, ("mali_kernel_session_manager_slot_get: got NULL session data\n")); return NULL; }
+	return session_data->subsystem_data[id];
+}
+
+_mali_osk_errcode_t _mali_ukk_open(void **context)
+{
+	int i;
+    _mali_osk_errcode_t err;
+	struct mali_session_data * session_data;
+
+	/* allocated struct to track this session */
+	session_data = (struct mali_session_data *)_mali_osk_malloc(sizeof(struct mali_session_data));
+    MALI_CHECK_NON_NULL(session_data, _MALI_OSK_ERR_NOMEM);
+
+	_mali_osk_memset(session_data->subsystem_data, 0, sizeof(session_data->subsystem_data));
+
+	/* create a response queue for this session */
+	session_data->ioctl_queue = _mali_osk_notification_queue_init();
+	if (NULL == session_data->ioctl_queue)
+	{
+		_mali_osk_free(session_data);
+        MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	MALI_DEBUG_PRINT(3, ("Session starting\n"));
+
+	/* call session_begin on all subsystems */
+	for (i = 0; i < (int)SUBSYSTEMS_COUNT; ++i)
+	{
+		if (NULL != subsystems[i]->session_begin)
+		{
+			/* subsystem has a session_begin */
+			err = subsystems[i]->session_begin(session_data, &session_data->subsystem_data[i], session_data->ioctl_queue);
+            MALI_CHECK_GOTO(err == _MALI_OSK_ERR_OK, cleanup);
+		}
+	}
+
+    *context = (void*)session_data;
+
+	MALI_DEBUG_PRINT(3, ("Session started\n"));
+	MALI_SUCCESS;
+
+cleanup:
+	MALI_DEBUG_PRINT(2, ("Session startup failed\n"));
+	/* i is index of subsystem which failed session begin, all indices before that has to be ended */
+	/* end subsystem sessions in the reverse order they where started in */
+	for (i = i - 1; i >= 0; --i)
+	{
+		if (NULL != subsystems[i]->session_end) subsystems[i]->session_end(session_data, &session_data->subsystem_data[i]);
+	}
+
+	_mali_osk_notification_queue_term(session_data->ioctl_queue);
+	_mali_osk_free(session_data);
+
+	/* return what the subsystem which failed session start returned */
+    MALI_ERROR(err);
+}
+
+_mali_osk_errcode_t _mali_ukk_close(void **context)
+{
+    int i;
+	struct mali_session_data * session_data;
+
+    MALI_CHECK_NON_NULL(context, _MALI_OSK_ERR_INVALID_ARGS);
+
+	session_data = (struct mali_session_data *)*context;
+
+	MALI_DEBUG_PRINT(2, ("Session ending\n"));
+
+	/* end subsystem sessions in the reverse order they where started in */
+	for (i = SUBSYSTEMS_COUNT - 1; i >= 0; --i)
+    {
+		if (NULL != subsystems[i]->session_end) subsystems[i]->session_end(session_data, &session_data->subsystem_data[i]);
+	}
+
+	_mali_osk_notification_queue_term(session_data->ioctl_queue);
+	_mali_osk_free(session_data);
+
+    *context = NULL;
+
+	MALI_DEBUG_PRINT(2, ("Session has ended\n"));
+
+    MALI_SUCCESS;
+}
+
+#if USING_MALI_PMM
+
+_mali_osk_errcode_t mali_core_signal_power_up( mali_pmm_core_id core, mali_bool queue_only )
+{
+	switch( core )
+	{
+	case MALI_PMM_CORE_GP:
+		MALI_CHECK_NO_ERROR(maligp_signal_power_up(queue_only));
+		break;
+#if defined USING_MALI400_L2_CACHE
+	case MALI_PMM_CORE_L2:
+		if( !queue_only )
+		{
+			/* Enable L2 cache due to power up */			
+			mali_kernel_l2_cache_do_enable();
+
+			/* Invalidate the cache on power up */
+			MALI_DEBUG_PRINT(5, ("L2 Cache: Invalidate all\n"));
+			MALI_CHECK_NO_ERROR(mali_kernel_l2_cache_invalidate_all());
+		}
+		break;
+#endif
+	case MALI_PMM_CORE_PP0:
+		MALI_CHECK_NO_ERROR(malipp_signal_power_up(0, queue_only));
+		break;
+	case MALI_PMM_CORE_PP1:
+		MALI_CHECK_NO_ERROR(malipp_signal_power_up(1, queue_only));
+		break;
+	case MALI_PMM_CORE_PP2:
+		MALI_CHECK_NO_ERROR(malipp_signal_power_up(2, queue_only));
+		break;
+	case MALI_PMM_CORE_PP3:
+		MALI_CHECK_NO_ERROR(malipp_signal_power_up(3, queue_only));
+		break;
+	default:
+		/* Unknown core */
+		MALI_DEBUG_PRINT_ERROR( ("Unknown core signalled with power up: %d\n", core) );
+		MALI_ERROR( _MALI_OSK_ERR_INVALID_ARGS );
+	}
+	
+	MALI_SUCCESS;
+}
+	
+_mali_osk_errcode_t mali_core_signal_power_down( mali_pmm_core_id core, mali_bool immediate_only )
+{
+	switch( core )
+	{
+	case MALI_PMM_CORE_GP:
+		MALI_CHECK_NO_ERROR(maligp_signal_power_down(immediate_only));
+		break;
+#if defined USING_MALI400_L2_CACHE
+	case MALI_PMM_CORE_L2:
+		/* Nothing to do */
+		break;
+#endif
+	case MALI_PMM_CORE_PP0:
+		MALI_CHECK_NO_ERROR(malipp_signal_power_down(0, immediate_only));
+		break;
+	case MALI_PMM_CORE_PP1:
+		MALI_CHECK_NO_ERROR(malipp_signal_power_down(1, immediate_only));
+		break;
+	case MALI_PMM_CORE_PP2:
+		MALI_CHECK_NO_ERROR(malipp_signal_power_down(2, immediate_only));
+		break;
+	case MALI_PMM_CORE_PP3:
+		MALI_CHECK_NO_ERROR(malipp_signal_power_down(3, immediate_only));
+		break;
+	default:
+		/* Unknown core */
+		MALI_DEBUG_PRINT_ERROR( ("Unknown core signalled with power down: %d\n", core) );
+		MALI_ERROR( _MALI_OSK_ERR_INVALID_ARGS );
+	}
+	
+	MALI_SUCCESS;
+}
+
+#endif
+
+
+
+#if MALI_STATE_TRACKING
+void _mali_kernel_core_dump_state(void)
+{
+	int i;
+	for (i = 0; i < SUBSYSTEMS_COUNT; ++i)
+    {
+		if (NULL != subsystems[i]->dump_state)
+		{
+			subsystems[i]->dump_state();
+		}
+    }
+#if USING_MALI_PMM
+	mali_pmm_dump_os_thread_state();	
+#endif
+}
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_core.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_core.h
new file mode 100644
index 00000000000..3819ecc5e1c
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_core.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_CORE_H__
+#define __MALI_KERNEL_CORE_H__
+
+#include "mali_osk.h"
+
+#if USING_MALI_PMM
+#include "mali_ukk.h"
+#include "mali_pmm.h"
+#include "mali_pmm_system.h"
+#endif
+
+_mali_osk_errcode_t mali_kernel_constructor( void );
+void mali_kernel_destructor( void );
+
+/**
+ * @brief Tranlate CPU physical to Mali physical addresses.
+ *
+ * This function is used to convert CPU physical addresses to Mali Physical
+ * addresses, such that _mali_ukk_map_external_mem may be used to map them
+ * into Mali. This will be used by _mali_ukk_va_to_mali_pa.
+ *
+ * This function only supports physically contiguous regions.
+ *
+ * A default implementation is provided, which uses a registered MEM_VALIDATION
+ * resource to do a static translation. Only an address range which will lie
+ * in the range specified by MEM_VALIDATION will be successfully translated.
+ *
+ * If a more complex, or non-static translation is required, then the
+ * implementor has the following options:
+ * - Rewrite this function to provide such a translation
+ * - Integrate the provider of the memory with UMP.
+ *
+ * @param[in,out] phys_base pointer to the page-aligned base address of the
+ * physical range to be translated
+ *
+ * @param[in] size size of the address range to be translated, which must be a
+ * multiple of the physical page size.
+ *
+ * @return on success, _MALI_OSK_ERR_OK and *phys_base is translated. If the
+ * cpu physical address range is not in the valid range, then a suitable
+ * _mali_osk_errcode_t error.
+ *
+ */
+_mali_osk_errcode_t mali_kernel_core_translate_cpu_to_mali_phys_range( u32 *phys_base, u32 size );
+
+
+/**
+ * @brief Validate a Mali physical address range.
+ *
+ * This function is used to ensure that an address range passed to
+ * _mali_ukk_map_external_mem is allowed to be mapped into Mali.
+ *
+ * This function only supports physically contiguous regions.
+ *
+ * A default implementation is provided, which uses a registered MEM_VALIDATION
+ * resource to do a static translation. Only an address range which will lie
+ * in the range specified by MEM_VALIDATION will be successfully validated.
+ *
+ * If a more complex, or non-static validation is required, then the
+ * implementor has the following options:
+ * - Rewrite this function to provide such a validation
+ * - Integrate the provider of the memory with UMP.
+ *
+ * @param phys_base page-aligned base address of the Mali physical range to be
+ * validated.
+ *
+ * @param size size of the address range to be validated, which must be a
+ * multiple of the physical page size.
+ *
+ * @return _MALI_OSK_ERR_OK if the Mali physical range is valid. Otherwise, a
+ * suitable _mali_osk_errcode_t error.
+ *
+ */
+_mali_osk_errcode_t mali_kernel_core_validate_mali_phys_range( u32 phys_base, u32 size );
+
+#if USING_MALI_PMM
+/**
+ * @brief Signal a power up on a Mali core.
+ *
+ * This function flags a core as powered up.
+ * For PP and GP cores it calls functions that move the core from a power off 
+ * queue into the idle queue ready to run jobs. It also tries to schedule any
+ * pending jobs to run on it. 
+ *
+ * This function will fail if the core is not powered off - either running or
+ * already idle.
+ *
+ * @param core The PMM core id to power up.
+ * @param queue_only When MALI_TRUE only re-queue the core - do not reset.
+ *
+ * @return _MALI_OSK_ERR_OK if the core has been powered up. Otherwise a
+ * suitable _mali_osk_errcode_t error.
+ */ 
+_mali_osk_errcode_t mali_core_signal_power_up( mali_pmm_core_id core, mali_bool queue_only );
+
+/**
+ * @brief Signal a power down on a Mali core.
+ *
+ * This function flags a core as powered down.
+ * For PP and GP cores it calls functions that move the core from an idle
+ * queue into the power off queue. 
+ *
+ * This function will fail if the core is not idle - either running or 
+ * already powered down.
+ *
+ * @param core The PMM core id to power up.
+ * @param immediate_only Do not set the core to pending power down if it can't
+ * power down immediately
+ *
+ * @return _MALI_OSK_ERR_OK if the core has been powered up. Otherwise a
+ * suitable _mali_osk_errcode_t error.
+ */ 
+_mali_osk_errcode_t mali_core_signal_power_down( mali_pmm_core_id core, mali_bool immediate_only );
+
+#endif
+
+/**
+ * Flag to indicate whether or not mali_benchmark is turned on.
+ */
+extern int mali_benchmark;
+
+
+#endif /* __MALI_KERNEL_CORE_H__ */
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_descriptor_mapping.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_descriptor_mapping.c
new file mode 100644
index 00000000000..45c280edd6a
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_descriptor_mapping.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_kernel_descriptor_mapping.h"
+#include "mali_osk.h"
+#include "mali_osk_bitops.h"
+
+#define MALI_PAD_INT(x) (((x) + (BITS_PER_LONG - 1)) & ~(BITS_PER_LONG - 1))
+
+/**
+ * Allocate a descriptor table capable of holding 'count' mappings
+ * @param count Number of mappings in the table
+ * @return Pointer to a new table, NULL on error
+ */
+static mali_descriptor_table * descriptor_table_alloc(int count);
+
+/**
+ * Free a descriptor table
+ * @param table The table to free
+ */
+static void descriptor_table_free(mali_descriptor_table * table);
+
+mali_descriptor_mapping * mali_descriptor_mapping_create(int init_entries, int max_entries)
+{
+	mali_descriptor_mapping * map = _mali_osk_calloc(1, sizeof(mali_descriptor_mapping));
+
+	init_entries = MALI_PAD_INT(init_entries);
+	max_entries = MALI_PAD_INT(max_entries);
+
+	if (NULL != map)
+	{
+		map->table = descriptor_table_alloc(init_entries);
+		if (NULL != map->table)
+		{
+#if !USING_MMU
+            map->lock = _mali_osk_lock_init( (_mali_osk_lock_flags_t)(_MALI_OSK_LOCKFLAG_ORDERED | _MALI_OSK_LOCKFLAG_READERWRITER | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE), 0, 20);
+#else
+            map->lock = _mali_osk_lock_init( (_mali_osk_lock_flags_t)(_MALI_OSK_LOCKFLAG_ORDERED | _MALI_OSK_LOCKFLAG_READERWRITER | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE), 0, 116);
+#endif
+            if (NULL != map->lock)
+            {
+			    _mali_osk_set_nonatomic_bit(0, map->table->usage); /* reserve bit 0 to prevent NULL/zero logic to kick in */
+			    map->max_nr_mappings_allowed = max_entries;
+			    map->current_nr_mappings = init_entries;
+			    return map;
+            }
+        	descriptor_table_free(map->table);
+		}
+		_mali_osk_free(map);
+	}
+	return NULL;
+}
+
+void mali_descriptor_mapping_destroy(mali_descriptor_mapping * map)
+{
+	descriptor_table_free(map->table);
+    _mali_osk_lock_term(map->lock);
+	_mali_osk_free(map);
+}
+
+_mali_osk_errcode_t mali_descriptor_mapping_allocate_mapping(mali_descriptor_mapping * map, void * target, int *odescriptor)
+{
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_FAULT;
+	int new_descriptor;
+
+    MALI_DEBUG_ASSERT_POINTER(map);
+    MALI_DEBUG_ASSERT_POINTER(odescriptor);
+
+    _mali_osk_lock_wait(map->lock, _MALI_OSK_LOCKMODE_RW);
+	new_descriptor = _mali_osk_find_first_zero_bit(map->table->usage, map->current_nr_mappings);
+	if (new_descriptor == map->current_nr_mappings)
+	{
+		/* no free descriptor, try to expand the table */
+		mali_descriptor_table * new_table, * old_table;
+		if (map->current_nr_mappings >= map->max_nr_mappings_allowed) goto unlock_and_exit;
+
+        map->current_nr_mappings += BITS_PER_LONG;
+		new_table = descriptor_table_alloc(map->current_nr_mappings);
+		if (NULL == new_table) goto unlock_and_exit;
+
+        old_table = map->table;
+		_mali_osk_memcpy(new_table->usage, old_table->usage, (sizeof(unsigned long)*map->current_nr_mappings) / BITS_PER_LONG);
+		_mali_osk_memcpy(new_table->mappings, old_table->mappings, map->current_nr_mappings * sizeof(void*));
+		map->table = new_table;
+		descriptor_table_free(old_table);
+	}
+
+	/* we have found a valid descriptor, set the value and usage bit */
+	_mali_osk_set_nonatomic_bit(new_descriptor, map->table->usage);
+	map->table->mappings[new_descriptor] = target;
+	*odescriptor = new_descriptor;
+    err = _MALI_OSK_ERR_OK;
+
+unlock_and_exit:
+    _mali_osk_lock_signal(map->lock, _MALI_OSK_LOCKMODE_RW);
+    MALI_ERROR(err);
+}
+
+void mali_descriptor_mapping_call_for_each(mali_descriptor_mapping * map, void (*callback)(int, void*))
+{
+	int i;
+
+	MALI_DEBUG_ASSERT_POINTER(map);
+	MALI_DEBUG_ASSERT_POINTER(callback);
+
+    _mali_osk_lock_wait(map->lock, _MALI_OSK_LOCKMODE_RO);
+	/* id 0 is skipped as it's an reserved ID not mapping to anything */
+	for (i = 1; i < map->current_nr_mappings; ++i)
+	{
+		if (_mali_osk_test_bit(i, map->table->usage))
+		{
+			callback(i, map->table->mappings[i]);
+		}
+	}
+    _mali_osk_lock_signal(map->lock, _MALI_OSK_LOCKMODE_RO);
+}
+
+_mali_osk_errcode_t mali_descriptor_mapping_get(mali_descriptor_mapping * map, int descriptor, void** target)
+{
+	_mali_osk_errcode_t result = _MALI_OSK_ERR_FAULT;
+	MALI_DEBUG_ASSERT_POINTER(map);
+    _mali_osk_lock_wait(map->lock, _MALI_OSK_LOCKMODE_RO);
+	if ( (descriptor >= 0) && (descriptor < map->current_nr_mappings) && _mali_osk_test_bit(descriptor, map->table->usage) )
+	{
+		*target = map->table->mappings[descriptor];
+		result = _MALI_OSK_ERR_OK;
+	}
+	else *target = NULL;
+    _mali_osk_lock_signal(map->lock, _MALI_OSK_LOCKMODE_RO);
+	MALI_ERROR(result);
+}
+
+_mali_osk_errcode_t mali_descriptor_mapping_set(mali_descriptor_mapping * map, int descriptor, void * target)
+{
+	_mali_osk_errcode_t result = _MALI_OSK_ERR_FAULT;
+    _mali_osk_lock_wait(map->lock, _MALI_OSK_LOCKMODE_RO);
+	if ( (descriptor >= 0) && (descriptor < map->current_nr_mappings) && _mali_osk_test_bit(descriptor, map->table->usage) )
+	{
+		map->table->mappings[descriptor] = target;
+		result = _MALI_OSK_ERR_OK;
+	}
+    _mali_osk_lock_signal(map->lock, _MALI_OSK_LOCKMODE_RO);
+	MALI_ERROR(result);
+}
+
+void mali_descriptor_mapping_free(mali_descriptor_mapping * map, int descriptor)
+{
+    _mali_osk_lock_wait(map->lock, _MALI_OSK_LOCKMODE_RW);
+	if ( (descriptor >= 0) && (descriptor < map->current_nr_mappings) && _mali_osk_test_bit(descriptor, map->table->usage) )
+	{
+		map->table->mappings[descriptor] = NULL;
+		_mali_osk_clear_nonatomic_bit(descriptor, map->table->usage);
+	}
+    _mali_osk_lock_signal(map->lock, _MALI_OSK_LOCKMODE_RW);
+}
+
+static mali_descriptor_table * descriptor_table_alloc(int count)
+{
+	mali_descriptor_table * table;
+
+	table = _mali_osk_calloc(1, sizeof(mali_descriptor_table) + ((sizeof(unsigned long) * count)/BITS_PER_LONG) + (sizeof(void*) * count));
+
+	if (NULL != table)
+	{
+		table->usage = (u32*)((u8*)table + sizeof(mali_descriptor_table));
+		table->mappings = (void**)((u8*)table + sizeof(mali_descriptor_table) + ((sizeof(unsigned long) * count)/BITS_PER_LONG));
+	}
+
+	return table;
+}
+
+static void descriptor_table_free(mali_descriptor_table * table)
+{
+	_mali_osk_free(table);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_descriptor_mapping.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_descriptor_mapping.h
new file mode 100644
index 00000000000..f626aa59455
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_descriptor_mapping.h
@@ -0,0 +1,99 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_kernel_descriptor_mapping.h
+ */
+
+#ifndef __MALI_KERNEL_DESCRIPTOR_MAPPING_H__
+#define __MALI_KERNEL_DESCRIPTOR_MAPPING_H__
+
+#include "mali_osk.h"
+
+/**
+ * The actual descriptor mapping table, never directly accessed by clients
+ */
+typedef struct mali_descriptor_table
+{
+	u32 * usage; /**< Pointer to bitpattern indicating if a descriptor is valid/used or not */
+	void** mappings; /**< Array of the pointers the descriptors map to */
+} mali_descriptor_table;
+
+/**
+ * The descriptor mapping object
+ * Provides a separate namespace where we can map an integer to a pointer
+ */
+typedef struct mali_descriptor_mapping
+{
+    _mali_osk_lock_t *lock; /**< Lock protecting access to the mapping object */
+	int max_nr_mappings_allowed; /**< Max number of mappings to support in this namespace */
+	int current_nr_mappings; /**< Current number of possible mappings */
+	mali_descriptor_table * table; /**< Pointer to the current mapping table */
+} mali_descriptor_mapping;
+
+/**
+ * Create a descriptor mapping object
+ * Create a descriptor mapping capable of holding init_entries growable to max_entries
+ * @param init_entries Number of entries to preallocate memory for
+ * @param max_entries Number of entries to max support
+ * @return Pointer to a descriptor mapping object, NULL on failure
+ */
+mali_descriptor_mapping * mali_descriptor_mapping_create(int init_entries, int max_entries);
+
+/**
+ * Destroy a descriptor mapping object
+ * @param map The map to free
+ */
+void mali_descriptor_mapping_destroy(mali_descriptor_mapping * map);
+
+/**
+ * Allocate a new mapping entry (descriptor ID)
+ * Allocates a new entry in the map.
+ * @param map The map to allocate a new entry in
+ * @param target The value to map to
+ * @return The descriptor allocated, a negative value on error
+ */
+_mali_osk_errcode_t mali_descriptor_mapping_allocate_mapping(mali_descriptor_mapping * map, void * target, int *descriptor);
+
+/**
+ * Get the value mapped to by a descriptor ID
+ * @param map The map to lookup the descriptor id in
+ * @param descriptor The descriptor ID to lookup
+ * @param target Pointer to a pointer which will receive the stored value
+ * @return 0 on successful lookup, negative on error
+ */
+_mali_osk_errcode_t mali_descriptor_mapping_get(mali_descriptor_mapping * map, int descriptor, void** target);
+
+/**
+ * Set the value mapped to by a descriptor ID
+ * @param map The map to lookup the descriptor id in
+ * @param descriptor The descriptor ID to lookup
+ * @param target Pointer to replace the current value with
+ * @return 0 on successful lookup, negative on error
+ */
+_mali_osk_errcode_t mali_descriptor_mapping_set(mali_descriptor_mapping * map, int descriptor, void * target);
+
+/**
+ * Call the specified callback function for each descriptor in map.
+ * Entire function is mutex protected.
+ * @param map The map to do callbacks for
+ * @param callback A callback function which will be calle for each entry in map
+ */
+void mali_descriptor_mapping_call_for_each(mali_descriptor_mapping * map, void (*callback)(int, void*));
+
+/**
+ * Free the descriptor ID
+ * For the descriptor to be reused it has to be freed
+ * @param map The map to free the descriptor from
+ * @param descriptor The descriptor ID to free
+ */
+void mali_descriptor_mapping_free(mali_descriptor_mapping * map, int descriptor);
+
+#endif /* __MALI_KERNEL_DESCRIPTOR_MAPPING_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_gp.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_gp.h
new file mode 100644
index 00000000000..c2467edc677
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_gp.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_GP2_H__
+#define __MALI_KERNEL_GP2_H__
+
+extern struct mali_kernel_subsystem mali_subsystem_gp2;
+
+#if USING_MALI_PMM
+_mali_osk_errcode_t maligp_signal_power_up( mali_bool queue_only );
+_mali_osk_errcode_t maligp_signal_power_down( mali_bool immediate_only );
+#endif
+
+#endif /* __MALI_KERNEL_GP2_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_l2_cache.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_l2_cache.c
new file mode 100644
index 00000000000..fded5c93ab9
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_l2_cache.c
@@ -0,0 +1,515 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_osk_list.h"
+
+#include "mali_kernel_core.h"
+#include "mali_kernel_pp.h"
+#include "mali_kernel_subsystem.h"
+#include "regs/mali_200_regs.h"
+#include "mali_kernel_rendercore.h"
+#include "mali_kernel_l2_cache.h"
+
+/**
+ * Size of the Mali L2 cache registers in bytes
+ */
+#define MALI400_L2_CACHE_REGISTERS_SIZE 0x30
+
+/**
+ * Mali L2 cache register numbers
+ * Used in the register read/write routines.
+ * See the hardware documentation for more information about each register
+ */
+typedef enum mali_l2_cache_register {
+	MALI400_L2_CACHE_REGISTER_STATUS       = 0x0002,
+	/*unused                               = 0x0003 */
+	MALI400_L2_CACHE_REGISTER_COMMAND      = 0x0004, /**< Misc cache commands, e.g. clear */
+	MALI400_L2_CACHE_REGISTER_CLEAR_PAGE   = 0x0005,
+	MALI400_L2_CACHE_REGISTER_MAX_READS    = 0x0006, /**< Limit of outstanding read requests */
+	MALI400_L2_CACHE_REGISTER_ENABLE       = 0x0007, /**< Enable misc cache features */
+	MALI400_L2_CACHE_REGISTER_PERFCNT_SRC0 = 0x0008,
+	MALI400_L2_CACHE_REGISTER_PERFCNT_VAL0 = 0x0009,
+	MALI400_L2_CACHE_REGISTER_PERFCNT_SRC1 = 0x000A,
+	MALI400_L2_CACHE_REGISTER_PERFCNT_VAL1 = 0x000B,
+} mali_l2_cache_register;
+
+
+/**
+ * Mali L2 cache commands
+ * These are the commands that can be sent to the Mali L2 cache unit
+ */
+typedef enum mali_l2_cache_command
+{
+	MALI400_L2_CACHE_COMMAND_CLEAR_ALL = 0x01, /**< Clear the entire cache */
+	/* Read HW TRM carefully before adding/using other commands than the clear above */
+} mali_l2_cache_command;
+
+/**
+ * Mali L2 cache commands
+ * These are the commands that can be sent to the Mali L2 cache unit
+ */
+typedef enum mali_l2_cache_enable
+{
+	MALI400_L2_CACHE_ENABLE_DEFAULT = 0x0, /**< Default state of enable register */
+	MALI400_L2_CACHE_ENABLE_ACCESS = 0x01, /**< Permit cacheable accesses */
+	MALI400_L2_CACHE_ENABLE_READ_ALLOCATE = 0x02, /**< Permit cache read allocate */
+} mali_l2_cache_enable;
+
+/**
+ * Mali L2 cache status bits
+ */
+typedef enum mali_l2_cache_status
+{
+	MALI400_L2_CACHE_STATUS_COMMAND_BUSY = 0x01, /**< Command handler of L2 cache is busy */
+	MALI400_L2_CACHE_STATUS_DATA_BUSY    = 0x02, /**< L2 cache is busy handling data requests */
+} mali_l2_cache_status;
+
+
+/**
+ * Definition of the L2 cache core struct
+ * Used to track a L2 cache unit in the system.
+ * Contains information about the mapping of the registers
+ */
+typedef struct mali_kernel_l2_cache_core
+{
+	unsigned long base; /**< Physical address of the registers */
+	mali_io_address mapped_registers; /**< Virtual mapping of the registers */
+	u32 mapping_size; /**< Size of registers in bytes */
+	_mali_osk_list_t list; /**< Used to link multiple cache cores into a list */
+	_mali_osk_lock_t *lock; /**< Serialize all L2 cache commands */
+} mali_kernel_l2_cache_core;
+
+
+#define MALI400_L2_MAX_READS_DEFAULT 0x1C
+
+int mali_l2_max_reads = MALI400_L2_MAX_READS_DEFAULT;
+
+
+/**
+ * Mali L2 cache subsystem startup function
+ * Called by the driver core when the driver is loaded.
+ *
+ * @param id Identifier assigned by the core to the L2 cache subsystem
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_l2_cache_initialize(mali_kernel_subsystem_identifier id);
+
+/**
+ * Mali L2 cache subsystem shutdown function
+ * Called by the driver core when the driver is unloaded.
+ * Cleans up
+ * @param id Identifier assigned by the core to the L2 cache subsystem
+ */
+static void mali_l2_cache_terminate(mali_kernel_subsystem_identifier id);
+
+/**
+ * L2 cache subsystem complete notification function.
+ * Called by the driver core when all drivers have loaded and all resources has been registered
+ * @param id Identifier assigned by the core to the L2 cache subsystem
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_l2_cache_load_complete(mali_kernel_subsystem_identifier id);
+
+/**
+ * Mali L2 cache subsystem's notification handler for a Mali L2 cache resource instances.
+ * Registered with the core during startup.
+ * Called by the core for each Mali L2 cache described in the active architecture's config.h file.
+ * @param resource The resource to handle (type MALI400L2)
+ * @return 0 if the Mali L2 cache was found and initialized, negative on error
+ */
+static _mali_osk_errcode_t mali_l2_cache_core_create(_mali_osk_resource_t * resource);
+
+/**
+ * Write to a L2 cache register
+ * Writes the given value to the specified register
+ * @param unit The L2 cache to write to
+ * @param reg The register to write to
+ * @param val The value to write to the register
+ */
+static void mali_l2_cache_register_write(mali_kernel_l2_cache_core * unit, mali_l2_cache_register reg, u32 val);
+
+
+
+/**
+ * Invalidate specified L2 cache
+ * @param cache The L2 cache to invalidate
+ * @return 0 if Mali L2 cache was successfully invalidated, otherwise error
+ */
+static _mali_osk_errcode_t mali_kernel_l2_cache_invalidate_all_cache(mali_kernel_l2_cache_core *cache);
+
+
+/*
+	The fixed Mali L2 cache system's mali subsystem interface implementation.
+	We currently handle module and session life-time management.
+*/
+struct mali_kernel_subsystem mali_subsystem_l2_cache =
+{
+	mali_l2_cache_initialize,    /**< startup */
+	mali_l2_cache_terminate,     /**< shutdown */
+	mali_l2_cache_load_complete, /**< load_complete */
+	NULL,                        /**< system_info_fill */
+	NULL,                        /**< session_begin */
+	NULL,                        /**< session_end */
+	NULL,                        /**< broadcast_notification */
+#if MALI_STATE_TRACKING
+	NULL,                        /**< dump_state */
+#endif
+};
+
+
+
+static _MALI_OSK_LIST_HEAD(caches_head);
+
+
+
+
+/* called during module init */
+static _mali_osk_errcode_t mali_l2_cache_initialize(mali_kernel_subsystem_identifier id)
+{
+	_mali_osk_errcode_t err;
+
+	MALI_IGNORE( id );
+
+	MALI_DEBUG_PRINT(2, ( "Mali L2 cache system initializing\n"));
+
+	_MALI_OSK_INIT_LIST_HEAD(&caches_head);
+
+	/* This will register the function for adding Mali L2 cache cores to the subsystem */
+	err = _mali_kernel_core_register_resource_handler(MALI400L2, mali_l2_cache_core_create);
+
+	MALI_ERROR(err);
+}
+
+
+
+/* called if/when our module is unloaded */
+static void mali_l2_cache_terminate(mali_kernel_subsystem_identifier id)
+{
+	mali_kernel_l2_cache_core * cache, *temp_cache;
+
+	MALI_DEBUG_PRINT(2, ( "Mali L2 cache system terminating\n"));
+
+	/* loop over all L2 cache units and shut them down */
+	_MALI_OSK_LIST_FOREACHENTRY( cache, temp_cache, &caches_head, mali_kernel_l2_cache_core, list )
+	{
+		/* reset to defaults */
+		mali_l2_cache_register_write(cache, MALI400_L2_CACHE_REGISTER_MAX_READS, (u32)MALI400_L2_MAX_READS_DEFAULT);
+		mali_l2_cache_register_write(cache, MALI400_L2_CACHE_REGISTER_ENABLE, (u32)MALI400_L2_CACHE_ENABLE_DEFAULT);
+
+		/* remove from the list of cacges on the system */
+		_mali_osk_list_del( &cache->list );
+
+		/* release resources */
+		_mali_osk_mem_unmapioregion( cache->base, cache->mapping_size, cache->mapped_registers );
+		_mali_osk_mem_unreqregion( cache->base, cache->mapping_size );
+		_mali_osk_lock_term( cache->lock );
+		_mali_osk_free( cache );
+
+		#if USING_MALI_PMM
+			/* Unregister the L2 cache with the PMM */
+			malipmm_core_unregister( MALI_PMM_CORE_L2 );
+		#endif
+	}
+}
+
+static _mali_osk_errcode_t mali_l2_cache_core_create(_mali_osk_resource_t * resource)
+{
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_FAULT ;
+	mali_kernel_l2_cache_core * cache = NULL;
+
+	MALI_DEBUG_PRINT(2, ( "Creating Mali L2 cache: %s\n", resource->description));
+
+#if USING_MALI_PMM
+	/* Register the L2 cache with the PMM */
+	err = malipmm_core_register( MALI_PMM_CORE_L2 );
+	if( _MALI_OSK_ERR_OK != err )
+	{
+		MALI_DEBUG_PRINT(1, ( "Failed to register L2 cache unit with PMM"));
+		return err;
+	}
+#endif
+
+	err = _mali_osk_mem_reqregion( resource->base, MALI400_L2_CACHE_REGISTERS_SIZE, resource->description);
+
+	MALI_CHECK_GOTO( _MALI_OSK_ERR_OK == err, err_cleanup_requestmem_failed);
+
+	/* Reset error that might be passed out */
+	err = _MALI_OSK_ERR_FAULT;
+
+	cache = _mali_osk_malloc(sizeof(mali_kernel_l2_cache_core));
+
+	MALI_CHECK_GOTO( NULL != cache, err_cleanup);
+
+	cache->lock = _mali_osk_lock_init( _MALI_OSK_LOCKFLAG_ORDERED | _MALI_OSK_LOCKFLAG_SPINLOCK | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE, 0, 104 );
+
+	MALI_CHECK_GOTO( NULL != cache->lock, err_cleanup);
+
+	/* basic setup */
+	_MALI_OSK_INIT_LIST_HEAD(&cache->list);
+
+	cache->base = resource->base;
+	cache->mapping_size = MALI400_L2_CACHE_REGISTERS_SIZE;
+
+	/* map the registers */
+	cache->mapped_registers = _mali_osk_mem_mapioregion( cache->base, cache->mapping_size, resource->description );
+
+	MALI_CHECK_GOTO( NULL != cache->mapped_registers, err_cleanup);
+
+	/* Invalidate cache (just to keep it in a known state at startup) */
+	err = mali_kernel_l2_cache_invalidate_all_cache(cache);
+
+	MALI_CHECK_GOTO( _MALI_OSK_ERR_OK == err, err_cleanup);
+
+	/* add to our list of L2 caches */
+	_mali_osk_list_add( &cache->list, &caches_head );
+
+	MALI_SUCCESS;
+
+err_cleanup:
+	/* This cleanup used when resources have been requested successfully */
+
+	if ( NULL != cache )
+	{
+		if (NULL != cache->mapped_registers)
+		{
+			_mali_osk_mem_unmapioregion( cache->base, cache->mapping_size, cache->mapped_registers);
+		}
+		else
+		{
+			MALI_DEBUG_PRINT(1, ( "Failed to map Mali L2 cache registers at 0x%08lX\n", cache->base));
+		}
+
+		if( NULL != cache->lock )
+		{
+			_mali_osk_lock_term( cache->lock );
+		}
+		else
+		{
+			MALI_DEBUG_PRINT(1, ( "Failed to allocate a lock for handling a L2 cache unit"));
+		}
+
+		_mali_osk_free( cache );
+	}
+	else
+	{
+		MALI_DEBUG_PRINT(1, ( "Failed to allocate memory for handling a L2 cache unit"));
+	}
+
+	/* A call is to request region, so this must always be reversed */
+	_mali_osk_mem_unreqregion( resource->base, MALI400_L2_CACHE_REGISTERS_SIZE);
+#if USING_MALI_PMM
+	malipmm_core_unregister( MALI_PMM_CORE_L2 );
+#endif
+	return err;
+
+err_cleanup_requestmem_failed:
+	MALI_DEBUG_PRINT(1, ("Failed to request Mali L2 cache '%s' register address space at (0x%08X - 0x%08X)\n",
+						 resource->description, resource->base, resource->base + MALI400_L2_CACHE_REGISTERS_SIZE - 1) );
+#if USING_MALI_PMM
+	malipmm_core_unregister( MALI_PMM_CORE_L2 );
+#endif
+	return err;
+
+}
+
+
+static void mali_l2_cache_register_write(mali_kernel_l2_cache_core * unit, mali_l2_cache_register reg, u32 val)
+{
+	_mali_osk_mem_iowrite32(unit->mapped_registers, (u32)reg * sizeof(u32), val);
+}
+
+
+static u32 mali_l2_cache_register_read(mali_kernel_l2_cache_core * unit, mali_l2_cache_register reg)
+{
+	return _mali_osk_mem_ioread32(unit->mapped_registers, (u32)reg * sizeof(u32));
+}
+
+void mali_kernel_l2_cache_do_enable(void)
+{
+	mali_kernel_l2_cache_core * cache, *temp_cache;
+
+	/* loop over all L2 cache units and enable them*/
+	_MALI_OSK_LIST_FOREACHENTRY( cache, temp_cache, &caches_head, mali_kernel_l2_cache_core, list)
+	{
+		mali_l2_cache_register_write(cache, MALI400_L2_CACHE_REGISTER_ENABLE, (u32)MALI400_L2_CACHE_ENABLE_ACCESS | (u32)MALI400_L2_CACHE_ENABLE_READ_ALLOCATE);
+		mali_l2_cache_register_write(cache, MALI400_L2_CACHE_REGISTER_MAX_READS, (u32)mali_l2_max_reads);
+	}
+}
+
+
+static _mali_osk_errcode_t mali_l2_cache_load_complete(mali_kernel_subsystem_identifier id)
+{
+	mali_kernel_l2_cache_do_enable();
+	MALI_DEBUG_PRINT(2, ( "Mali L2 cache system load complete\n"));
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_kernel_l2_cache_send_command(mali_kernel_l2_cache_core *cache, u32 reg, u32 val)
+{
+	int i = 0;
+	const int loop_count = 100000;
+
+	/*
+	 * Grab lock in order to send commands to the L2 cache in a serialized fashion.
+	 * The L2 cache will ignore commands if it is busy.
+	 */
+	_mali_osk_lock_wait(cache->lock, _MALI_OSK_LOCKMODE_RW);
+
+	/* First, wait for L2 cache command handler to go idle */
+
+	for (i = 0; i < loop_count; i++)
+	{
+		if (!(_mali_osk_mem_ioread32(cache->mapped_registers , (u32)MALI400_L2_CACHE_REGISTER_STATUS * sizeof(u32)) & (u32)MALI400_L2_CACHE_STATUS_COMMAND_BUSY))
+		{
+			break;
+		}
+	}
+
+	if (i == loop_count)
+	{
+		_mali_osk_lock_signal(cache->lock, _MALI_OSK_LOCKMODE_RW);
+		MALI_DEBUG_PRINT(1, ( "Mali L2 cache: aborting wait for command interface to go idle\n"));
+		MALI_ERROR( _MALI_OSK_ERR_FAULT );
+	}
+
+	/* then issue the command */
+	mali_l2_cache_register_write(cache, reg, val);
+
+	_mali_osk_lock_signal(cache->lock, _MALI_OSK_LOCKMODE_RW);
+	MALI_SUCCESS;
+}
+
+
+static _mali_osk_errcode_t mali_kernel_l2_cache_invalidate_all_cache(mali_kernel_l2_cache_core *cache)
+{
+	return mali_kernel_l2_cache_send_command(cache, MALI400_L2_CACHE_REGISTER_COMMAND, MALI400_L2_CACHE_COMMAND_CLEAR_ALL);
+}
+
+_mali_osk_errcode_t mali_kernel_l2_cache_invalidate_all(void)
+{
+	mali_kernel_l2_cache_core * cache, *temp_cache;
+
+	/* loop over all L2 cache units and invalidate them */
+
+	_MALI_OSK_LIST_FOREACHENTRY( cache, temp_cache, &caches_head, mali_kernel_l2_cache_core, list)
+	{
+		MALI_CHECK_NO_ERROR( mali_kernel_l2_cache_invalidate_all_cache(cache) );
+	}
+
+	MALI_SUCCESS;
+}
+
+
+static _mali_osk_errcode_t mali_kernel_l2_cache_invalidate_page_cache(mali_kernel_l2_cache_core *cache, u32 page)
+{
+	return mali_kernel_l2_cache_send_command(cache, MALI400_L2_CACHE_REGISTER_CLEAR_PAGE, page);
+}
+
+_mali_osk_errcode_t mali_kernel_l2_cache_invalidate_page(u32 page)
+{
+	mali_kernel_l2_cache_core * cache, *temp_cache;
+
+	/* loop over all L2 cache units and invalidate them */
+
+	_MALI_OSK_LIST_FOREACHENTRY( cache, temp_cache, &caches_head, mali_kernel_l2_cache_core, list)
+	{
+		MALI_CHECK_NO_ERROR( mali_kernel_l2_cache_invalidate_page_cache(cache, page) );
+	}
+
+	MALI_SUCCESS;
+}
+
+
+void mali_kernel_l2_cache_set_perf_counters(u32 src0, u32 src1, int force_reset)
+{
+	mali_kernel_l2_cache_core * cache, *temp_cache;
+	int reset0 = force_reset;
+	int reset1 = force_reset;
+	MALI_DEBUG_CODE(
+		int changed0 = 0;
+		int changed1 = 0;
+	)
+
+	/* loop over all L2 cache units and activate the counters on them */
+	_MALI_OSK_LIST_FOREACHENTRY(cache, temp_cache, &caches_head, mali_kernel_l2_cache_core, list)
+	{
+		u32 cur_src0 = mali_l2_cache_register_read(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_SRC0);
+		u32 cur_src1 = mali_l2_cache_register_read(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_SRC1);
+
+		if (src0 != cur_src0)
+		{
+			mali_l2_cache_register_write(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_SRC0, src0);
+			MALI_DEBUG_CODE(changed0 = 1;)
+			reset0 = 1;
+		}
+
+		if (src1 != cur_src1)
+		{
+			mali_l2_cache_register_write(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_SRC1, src1);
+			MALI_DEBUG_CODE(changed1 = 1;)
+			reset1 = 1;
+		}
+
+		if (reset0)
+		{
+			mali_l2_cache_register_write(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_VAL0, 0);
+		}
+
+		if (reset1)
+		{
+			mali_l2_cache_register_write(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_VAL1, 0);
+		}
+
+		MALI_DEBUG_PRINT(5, ("L2 cache counters set: SRC0=%u, CHANGED0=%d, RESET0=%d, SRC1=%u, CHANGED1=%d, RESET1=%d\n",
+			src0, changed0, reset0,
+			src1, changed1, reset1));
+	}
+}
+
+
+void mali_kernel_l2_cache_get_perf_counters(u32 *src0, u32 *val0, u32 *src1, u32 *val1)
+{
+	mali_kernel_l2_cache_core * cache, *temp_cache;
+	int first_time = 1;
+	*src0 = 0;
+	*src1 = 0;
+	*val0 = 0;
+	*val1 = 0;
+
+	/* loop over all L2 cache units and read the counters */
+	_MALI_OSK_LIST_FOREACHENTRY(cache, temp_cache, &caches_head, mali_kernel_l2_cache_core, list)
+	{
+		u32 cur_src0 = mali_l2_cache_register_read(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_SRC0);
+		u32 cur_src1 = mali_l2_cache_register_read(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_SRC1);
+		u32 cur_val0 = mali_l2_cache_register_read(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_VAL0);
+		u32 cur_val1 = mali_l2_cache_register_read(cache, MALI400_L2_CACHE_REGISTER_PERFCNT_VAL1);
+
+		MALI_DEBUG_PRINT(5, ("L2 cache counters get: SRC0=%u, VAL0=%u, SRC1=%u, VAL1=%u\n", cur_src0, cur_val0, cur_src1, cur_val1));
+
+		if (first_time)
+		{
+			*src0 = cur_src0;
+			*src1 = cur_src1;
+			first_time = 0;
+		}
+
+		if (*src0 == cur_src0 && *src1 == cur_src1)
+		{
+			*val0 += cur_val0;
+			*val1 += cur_val1;
+		}
+		else
+		{
+			MALI_DEBUG_PRINT(1, ("Warning: Mali L2 caches has different performance counters set, not retrieving data\n"));
+		}
+	}
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_l2_cache.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_l2_cache.h
new file mode 100644
index 00000000000..de3229a0688
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_l2_cache.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_L2_CACHE_H__
+#define __MALI_KERNEL_L2_CACHE_H__
+
+#include "mali_osk.h"
+#include "mali_kernel_subsystem.h"
+extern struct mali_kernel_subsystem mali_subsystem_l2_cache;
+
+_mali_osk_errcode_t mali_kernel_l2_cache_invalidate_all(void);
+_mali_osk_errcode_t mali_kernel_l2_cache_invalidate_page(u32 page);
+
+void mali_kernel_l2_cache_do_enable(void);
+void mali_kernel_l2_cache_set_perf_counters(u32 src0, u32 src1, int force_reset);
+void mali_kernel_l2_cache_get_perf_counters(u32 *src0, u32 *val0, u32 *src1, u32 *val1);
+
+#endif /* __MALI_KERNEL_L2_CACHE_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem.h
new file mode 100644
index 00000000000..681b6dd47c9
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem.h
@@ -0,0 +1,17 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_MEM_H__
+#define __MALI_KERNEL_MEM_H__
+
+#include "mali_kernel_subsystem.h"
+extern struct mali_kernel_subsystem mali_subsystem_memory;
+
+#endif /* __MALI_KERNEL_MEM_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_buddy.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_buddy.c
new file mode 100644
index 00000000000..8277fd1e1c6
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_buddy.c
@@ -0,0 +1,1425 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_core.h"
+#include "mali_kernel_subsystem.h"
+#include "mali_kernel_mem.h"
+#include "mali_kernel_descriptor_mapping.h"
+#include "mali_kernel_session_manager.h"
+
+/* kernel side OS functions and user-kernel interface */
+#include "mali_osk.h"
+#include "mali_osk_mali.h"
+#include "mali_osk_list.h"
+#include "mali_ukk.h"
+
+#ifdef _MALI_OSK_SPECIFIC_INDIRECT_MMAP
+#include "mali_osk_indir_mmap.h"
+#endif
+
+/**
+ * Minimum memory allocation size
+ */
+#define MIN_BLOCK_SIZE (1024*1024UL)
+
+/**
+ * Per-session memory descriptor mapping table sizes
+ */
+#define MALI_MEM_DESCRIPTORS_INIT 64
+#define MALI_MEM_DESCRIPTORS_MAX 4096
+
+/**
+ * Enum uses to store multiple fields in one u32 to keep the memory block struct small
+ */
+enum MISC_SHIFT { MISC_SHIFT_FREE = 0, MISC_SHIFT_ORDER = 1, MISC_SHIFT_TOPLEVEL = 6 };
+enum MISC_MASK { MISC_MASK_FREE = 0x01, MISC_MASK_ORDER = 0x1F, MISC_MASK_TOPLEVEL = 0x1F };
+
+/* forward declaration of the block struct */
+struct mali_memory_block;
+
+/**
+ * Definition of memory bank type.
+ * Represents a memory bank (separate address space)
+ * Each bank keeps track of its block usage.
+ * A buddy system used to track the usage
+*/
+typedef struct mali_memory_bank
+{
+	_mali_osk_list_t list; /* links multiple banks together */
+	_mali_osk_lock_t *lock;
+	u32 base_addr; /* Mali seen address of bank */
+	u32 cpu_usage_adjust; /* Adjustmen factor for what the CPU sees */
+	u32 size; /* the effective size */
+	u32 real_size; /* the real size of the bank, as given by to the subsystem */
+	int min_order;
+	int max_order;
+	struct mali_memory_block * blocklist;
+	_mali_osk_list_t *freelist;
+	_mali_osk_atomic_t num_active_allocations;
+	u32 used_for_flags;
+	u32 alloc_order; /**< Order in which the bank will be used for allocations */
+	const char *name; /**< Descriptive name of the bank */
+} mali_memory_bank;
+
+/**
+ * Definition of the memory block type
+ * Represents a memory block, which is the smallest memory unit operated on.
+ * A block keeps info about its mapping, if in use by a user process
+ */
+typedef struct mali_memory_block
+{
+	_mali_osk_list_t link; /* used for freelist and process usage list*/
+	mali_memory_bank * bank; /* the bank it belongs to */
+	void __user * mapping; /* possible user space mapping of this block */
+	u32 misc; /* used while a block is free to track the number blocks it represents */
+	int descriptor;
+	u32 mmap_cookie; /**< necessary for interaction with _mali_ukk_mem_mmap/munmap */
+} mali_memory_block;
+
+/**
+ * Defintion of the type used to represent memory used by a session.
+ * Containts the head of the list of memory currently in use by a session.
+ */
+typedef struct memory_session
+{
+	_mali_osk_lock_t *lock;
+	_mali_osk_list_t memory_head; /* List of the memory blocks used by this session. */
+	mali_descriptor_mapping * descriptor_mapping; /**< Mapping between userspace descriptors and our pointers */
+} memory_session;
+
+/*
+	Subsystem interface implementation
+*/
+/**
+ * Buddy block memory subsystem startup function
+ * Called by the driver core when the driver is loaded.
+ * Registers the memory systems ioctl handler, resource handlers and memory map function with the core.
+ *
+ * @param id Identifier assigned by the core to the memory subsystem
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_initialize(mali_kernel_subsystem_identifier id);
+
+/**
+ * Buddy block memory subsystem shutdown function
+ * Called by the driver core when the driver is unloaded.
+ * Cleans up
+ * @param id Identifier assigned by the core to the memory subsystem
+ */
+static void mali_memory_core_terminate(mali_kernel_subsystem_identifier id);
+
+/**
+ * Buddy block memory load complete notification function.
+ * Called by the driver core when all drivers have loaded and all resources has been registered
+ * Reports on the memory resources registered
+ * @param id Identifier assigned by the core to the memory subsystem
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_load_complete(mali_kernel_subsystem_identifier id);
+
+
+/**
+ * Buddy block memory subsystem session begin notification
+ * Called by the core when a new session to the driver is started.
+ * Creates a memory session object and sets it as the subsystem slot data for this session
+ * @param slot Pointer to the slot to use for storing per-session data
+ * @param queue The user space event sink
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue);
+
+/**
+ * Buddy block memory subsystem session end notification
+ * Called by the core when a session to the driver has ended.
+ * Cleans up per session data, which includes checking and fixing memory leaks
+ *
+ * @param slot Pointer to the slot to use for storing per-session data
+ */
+static void mali_memory_core_session_end(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot);
+
+/**
+ * Buddy block memory subsystem system info filler
+ * Called by the core when a system info update is needed
+ * We fill in info about all the memory types we have
+ * @param info Pointer to system info struct to update
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_system_info_fill(_mali_system_info* info);
+
+/* our registered resource handlers */
+/**
+ * Buddy block memory subsystem's notification handler for MEMORY resource instances.
+ * Registered with the core during startup.
+ * Called by the core for each memory bank described in the active architecture's config.h file.
+ * Requests memory region ownership and calls backend.
+ * @param resource The resource to handle (type MEMORY)
+ * @return 0 if the memory was claimed and accepted, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_resource_memory(_mali_osk_resource_t * resource);
+
+/**
+ * Buddy block memory subsystem's notification handler for MMU resource instances.
+ * Registered with the core during startup.
+ * Called by the core for each mmu described in the active architecture's config.h file.
+ * @param resource The resource to handle (type MMU)
+ * @return 0 if the MMU was found and initialized, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_resource_mmu(_mali_osk_resource_t * resource);
+
+/**
+ * Buddy block memory subsystem's notification handler for FPGA_FRAMEWORK resource instances.
+ * Registered with the core during startup.
+ * Called by the core for each fpga framework described in the active architecture's config.h file.
+ * @param resource The resource to handle (type FPGA_FRAMEWORK)
+ * @return 0 if the FPGA framework was found and initialized, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_resource_fpga(_mali_osk_resource_t * resource);
+
+/* ioctl command implementations */
+/**
+ * Buddy block memory subsystem's handler for MALI_IOC_MEM_GET_BIG_BLOCK ioctl
+ * Called by the generic ioctl handler when the MALI_IOC_MEM_GET_BIG_BLOCK command is received.
+ * Finds an available memory block and maps into the current process' address space.
+ * @param ukk_private private word for use by the User/Kernel interface
+ * @param session_data Pointer to the per-session object which will track the memory usage
+ * @param argument The argument from the user. A pointer to an struct mali_dd_get_big_block in user space
+ * @return Zero if successful, a standard Linux error value value on error (a negative value)
+ */
+_mali_osk_errcode_t _mali_ukk_get_big_block( _mali_uk_get_big_block_s *args );
+
+/**
+ * Buddy block memory subsystem's handler for MALI_IOC_MEM_FREE_BIG_BLOCK ioctl
+ * Called by the generic ioctl handler when the MALI_IOC_MEM_FREE_BIG_BLOCK command is received.
+ * Unmaps the memory from the process' address space and marks the block as free.
+ * @param session_data Pointer to the per-session object which tracks the memory usage
+ * @param argument The argument from the user. A pointer to an struct mali_dd_get_big_block in user space
+ * @return Zero if successful, a standard Linux error value value on error (a negative value)
+ */
+
+/* this static version allows us to make use of it while holding the memory_session lock.
+ * This is required  for the session_end code */
+static _mali_osk_errcode_t _mali_ukk_free_big_block_internal( struct mali_session_data * mali_session_data, memory_session * session_data, _mali_uk_free_big_block_s *args);
+
+_mali_osk_errcode_t _mali_ukk_free_big_block( _mali_uk_free_big_block_s *args );
+
+/**
+ * Buddy block memory subsystem's memory bank registration routine
+ * Called when a MEMORY resource has been found.
+ * The memory region has already been reserved for use by this driver.
+ * Create a bank object to represent this region and initialize its slots.
+ * @note Can only be called in an module atomic scope, i.e. during module init since no locking is performed
+ * @param phys_base Physical base address of this bank
+ * @param cpu_usage_adjust Adjustment factor for CPU seen address
+ * @param size Size of the bank in bytes
+ * @param flags Memory type bits
+ * @param alloc_order Order in which the bank will be used for allocations
+ * @param name descriptive name of the bank
+ * @return Zero on success, negative on error
+ */
+static int mali_memory_bank_register(u32 phys_base, u32 cpu_usage_adjust, u32 size, u32 flags, u32 alloc_order, const char *name);
+
+/**
+ * Get a block of mali memory of at least the given size and of the given type
+ * This is the backend for get_big_block.
+ * @param type_id The type id of memory requested.
+ * @param minimum_size The size requested
+ * @return Pointer to a block on success, NULL on failure
+ */
+static mali_memory_block * mali_memory_block_get(u32 type_id, u32 minimum_size);
+
+/**
+ * Get the mali seen address of the memory described by the block
+ * @param block The memory block to return the address of
+ * @return The mali seen address of the memory block
+ */
+MALI_STATIC_INLINE u32 block_mali_addr_get(mali_memory_block * block);
+
+/**
+ * Get the cpu seen address of the memory described by the block
+ * The cpu_usage_adjust will be used to change the mali seen phys address
+ * @param block The memory block to return the address of
+ * @return The mali seen address of the memory block
+ */
+MALI_STATIC_INLINE u32 block_cpu_addr_get(mali_memory_block * block);
+
+/**
+ * Get the size of the memory described by the given block
+ * @param block The memory block to return the size of
+ * @return The size of the memory block described by the object
+ */
+MALI_STATIC_INLINE u32 block_size_get(mali_memory_block * block);
+
+/**
+ * Get the user space accessible mapping the memory described by the given memory block
+ * Returns a pointer in user space to the memory, if one has been created.
+ * @param block The memory block to return the mapping of
+ * @return User space pointer to cpu accessible memory or NULL if not mapped
+ */
+MALI_STATIC_INLINE void __user * block_mapping_get(mali_memory_block * block);
+
+/**
+ * Set the user space accessible mapping the memory described by the given memory block.
+ * Sets the stored pointer to user space for the memory described by this block.
+ * @param block The memory block to set mapping info for
+ * @param ptr User space pointer to cpu accessible memory or NULL if not mapped
+ */
+MALI_STATIC_INLINE void block_mapping_set(mali_memory_block * block, void __user * ptr);
+
+/**
+ * Get the cookie for use with _mali_ukk_mem_munmap().
+ * @param block The memory block to get the cookie from
+ * @return the cookie. A return of 0 is still a valid cookie.
+ */
+MALI_STATIC_INLINE u32 block_mmap_cookie_get(mali_memory_block * block);
+
+/**
+ * Set the cookie returned via _mali_ukk_mem_mmap().
+ * @param block The memory block to set the cookie for
+ * @param cookie the cookie
+ */
+MALI_STATIC_INLINE void block_mmap_cookie_set(mali_memory_block * block, u32 cookie);
+
+
+/**
+ * Get a memory block's free status
+ * @param block The block to get the state of
+ */
+MALI_STATIC_INLINE u32 get_block_free(mali_memory_block * block);
+
+/**
+ * Set a memory block's free status
+ * @param block The block to set the state for
+ * @param state The state to set
+ */
+MALI_STATIC_INLINE void set_block_free(mali_memory_block * block, int state);
+
+/**
+ * Set a memory block's order
+ * @param block The block to set the order for
+ * @param order The order to set
+ */
+MALI_STATIC_INLINE void set_block_order(mali_memory_block * block, u32 order);
+
+/**
+ * Get a memory block's order
+ * @param block The block to get the order for
+ * @return The order this block exists on
+ */
+MALI_STATIC_INLINE u32 get_block_order(mali_memory_block * block);
+
+/**
+ * Tag a block as being a toplevel block.
+ * A toplevel block has no buddy and no parent
+ * @param block The block to tag as being toplevel
+ */
+MALI_STATIC_INLINE void set_block_toplevel(mali_memory_block * block, u32 level);
+
+/**
+ * Check if a block is a toplevel block
+ * @param block The block to check
+ * @return 1 if toplevel, 0 else
+ */
+MALI_STATIC_INLINE u32 get_block_toplevel(mali_memory_block * block);
+
+/**
+ * Checks if the given block is a buddy at the given order and that it's free
+ * @param block The block to check
+ * @param order The order to check against
+ * @return 0 if not valid, else 1
+ */
+MALI_STATIC_INLINE int block_is_valid_buddy(mali_memory_block * block, int order);
+
+/*
+ The buddy system uses the following rules to quickly find a blocks buddy
+ and parent (block representing this block at a higher order level):
+ - Given a block with index i the blocks buddy is at index i ^ ( 1 << order)
+ - Given a block with index i the blocks parent is at i & ~(1 << order)
+*/
+
+/**
+ * Get a blocks buddy
+ * @param block The block to find the buddy for
+ * @param order The order to operate on
+ * @return Pointer to the buddy block
+ */
+MALI_STATIC_INLINE mali_memory_block * block_get_buddy(mali_memory_block * block, u32 order);
+
+/**
+ * Get a blocks parent
+ * @param block The block to find the parent for
+ * @param order The order to operate on
+ * @return Pointer to the parent block
+ */
+MALI_STATIC_INLINE mali_memory_block * block_get_parent(mali_memory_block * block, u32 order);
+
+/**
+ * Release mali memory
+ * Backend for free_big_block.
+ * Will release the mali memory described by the given block struct.
+ * @param block Memory block to free
+ */
+static void block_release(mali_memory_block * block);
+
+/* end interface implementation */
+
+/**
+ * List of all the memory banks registerd with the subsystem.
+ * Access to this list is NOT synchronized since it's only
+ * written to during module init and termination.
+ */
+static _MALI_OSK_LIST_HEAD(memory_banks_list);
+
+/*
+	The buddy memory system's mali subsystem interface implementation.
+	We currently handle module and session life-time management.
+*/
+struct mali_kernel_subsystem mali_subsystem_memory =
+{
+	mali_memory_core_initialize,                /* startup */
+	mali_memory_core_terminate,                 /* shutdown */
+    mali_memory_core_load_complete,             /* load_complete */
+	mali_memory_core_system_info_fill,          /* system_info_fill */
+    mali_memory_core_session_begin,             /* session_begin */
+	mali_memory_core_session_end,               /* session_end */
+    NULL,                                       /* broadcast_notification */
+#if MALI_STATE_TRACKING
+	NULL,                                       /* dump_state */
+#endif
+};
+
+/* Initialized when this subsystem is initialized. This is determined by the
+ * position in subsystems[], and so the value used to initialize this is
+ * determined at compile time */
+static mali_kernel_subsystem_identifier mali_subsystem_memory_id = -1;
+
+/* called during module init */
+static _mali_osk_errcode_t mali_memory_core_initialize(mali_kernel_subsystem_identifier id)
+{
+    _MALI_OSK_INIT_LIST_HEAD(&memory_banks_list);
+
+	mali_subsystem_memory_id = id;
+
+	/* register our handlers */
+	MALI_CHECK_NO_ERROR(_mali_kernel_core_register_resource_handler(MEMORY, mali_memory_core_resource_memory));
+
+	MALI_CHECK_NO_ERROR(_mali_kernel_core_register_resource_handler(MMU, mali_memory_core_resource_mmu));
+
+	MALI_CHECK_NO_ERROR(_mali_kernel_core_register_resource_handler(FPGA_FRAMEWORK, mali_memory_core_resource_fpga));
+
+	MALI_SUCCESS;
+}
+
+/* called if/when our module is unloaded */
+static void mali_memory_core_terminate(mali_kernel_subsystem_identifier id)
+{
+	mali_memory_bank * bank, *temp;
+
+	/* loop over all memory banks to free them */
+	/* we use the safe version since we delete the current bank in the body */
+	_MALI_OSK_LIST_FOREACHENTRY(bank, temp, &memory_banks_list, mali_memory_bank, list)
+	{
+		MALI_DEBUG_CODE(int usage_count = _mali_osk_atomic_read(&bank->num_active_allocations));
+ 		/*
+			Report leaked memory
+			If this happens we have a bug in our session cleanup code.
+		*/
+		MALI_DEBUG_PRINT_IF(1, 0 != usage_count,  ("%d allocation(s) from memory bank at 0x%X still in use\n", usage_count, bank->base_addr));
+
+        _mali_osk_atomic_term(&bank->num_active_allocations);
+
+	    _mali_osk_lock_term(bank->lock);
+
+        /* unlink from bank list */
+		_mali_osk_list_del(&bank->list);
+
+		/* release kernel resources used by the bank */
+		_mali_osk_mem_unreqregion(bank->base_addr, bank->real_size);
+
+		/* remove all resources used to represent this bank*/
+		_mali_osk_free(bank->freelist);
+		_mali_osk_free(bank->blocklist);
+
+		/* destroy the bank object itself */
+		_mali_osk_free(bank);
+	}
+
+	/* No need to de-initialize mali_subsystem_memory_id - it could only be
+	 * re-initialized to the same value */
+}
+
+/* load_complete handler */
+static _mali_osk_errcode_t mali_memory_core_load_complete(mali_kernel_subsystem_identifier id)
+{
+	mali_memory_bank * bank, *temp;
+
+	MALI_DEBUG_PRINT( 1, ("Mali memory allocators will be used in this order of preference (lowest number first) :\n"));
+
+	_MALI_OSK_LIST_FOREACHENTRY(bank, temp, &memory_banks_list, mali_memory_bank, list)
+	{
+		if ( NULL != bank->name )
+		{
+			MALI_DEBUG_PRINT( 1, ("\t%d: %s\n", bank->alloc_order, bank->name) );
+		}
+		else
+		{
+			MALI_DEBUG_PRINT( 1, ("\t%d: (UNNAMED ALLOCATOR)\n", bank->alloc_order ) );
+		}
+	}
+	MALI_SUCCESS;
+}
+
+MALI_STATIC_INLINE u32 order_needed_for_size(u32 size, struct mali_memory_bank * bank)
+{
+    u32 order = 0;
+
+    if (0 < size)
+    {
+        for ( order = sizeof(u32)*8 - 1; ((1UL<<order) & size) == 0; --order)
+            /* nothing */;
+
+        /* check if size is pow2, if not we need increment order by one */
+        if (0 != (size & ((1UL<<order)-1))) ++order;
+    }
+
+    if ((NULL != bank) && (order < bank->min_order)) order = bank->min_order;
+    /* Not capped to max order, that doesn't make sense */
+
+    return order;
+}
+
+MALI_STATIC_INLINE u32 maximum_order_which_fits(u32 size)
+{
+	u32 order = 0;
+	u32 powsize = 1;
+	while (powsize < size)
+	{
+		powsize <<= 1;
+		if (powsize > size) break;
+		order++;
+	}
+
+	return order;
+}
+
+/* called for new MEMORY resources */
+static _mali_osk_errcode_t mali_memory_bank_register(u32 phys_base, u32 cpu_usage_adjust, u32 size, u32 flags, u32 alloc_order, const char *name)
+{
+	/* no locking performed due to function contract */
+	int i;
+	u32 left, offset;
+	mali_memory_bank * bank;
+	mali_memory_bank * bank_enum, *temp;
+
+    _mali_osk_errcode_t err;
+
+	/* Only a multiple of MIN_BLOCK_SIZE is usable */
+	u32 usable_size = size & ~(MIN_BLOCK_SIZE - 1);
+
+	/* handle zero sized banks and bank smaller than the fixed block size */
+	if (0 == usable_size)
+	{
+		MALI_PRINT(("Usable size == 0\n"));
+        MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}
+
+	/* warn for banks not a muliple of the block size  */
+	MALI_DEBUG_PRINT_IF(1, usable_size != size, ("Memory bank @ 0x%X not a multiple of minimum block size. %d bytes wasted\n", phys_base, size - usable_size));
+
+	/* check against previous registrations */
+	MALI_DEBUG_CODE(
+        {
+		    _MALI_OSK_LIST_FOREACHENTRY(bank, temp, &memory_banks_list, mali_memory_bank, list)
+		    {
+			    /* duplicate ? */
+			    if (bank->base_addr == phys_base)
+			    {
+				    MALI_PRINT(("Duplicate registration of a memory bank at 0x%X detected\n", phys_base));
+				    MALI_ERROR(_MALI_OSK_ERR_FAULT);
+			    }
+			    /* overlapping ? */
+			    else if (
+						    ( (phys_base > bank->base_addr) && (phys_base < (bank->base_addr + bank->real_size)) ) ||
+						    ( (phys_base + size) > bank->base_addr && ((phys_base + size) < (bank->base_addr + bank->real_size)) )
+					    )
+			    {
+				    MALI_PRINT(("Overlapping memory blocks found. Memory at 0x%X overlaps with memory at 0x%X size 0x%X\n", bank->base_addr, phys_base, size));
+				    MALI_ERROR(_MALI_OSK_ERR_FAULT);
+			    }
+		    }
+        }
+	);
+
+	/* create an object to represent this memory bank */
+	MALI_CHECK_NON_NULL(bank = (mali_memory_bank*)_mali_osk_malloc(sizeof(mali_memory_bank)), _MALI_OSK_ERR_NOMEM);
+
+	/* init the fields */
+	_MALI_OSK_INIT_LIST_HEAD(&bank->list);
+	bank->base_addr = phys_base;
+	bank->cpu_usage_adjust = cpu_usage_adjust;
+	bank->size = usable_size;
+	bank->real_size = size;
+	bank->alloc_order = alloc_order;
+	bank->name = name;
+
+    err = _mali_osk_atomic_init(&bank->num_active_allocations, 0);
+    if (err != _MALI_OSK_ERR_OK)
+    {
+		_mali_osk_free(bank);
+        MALI_ERROR(err);
+    }
+
+	bank->used_for_flags = flags;
+	bank->min_order = order_needed_for_size(MIN_BLOCK_SIZE, NULL);
+	bank->max_order = maximum_order_which_fits(usable_size);
+	bank->lock = _mali_osk_lock_init((_mali_osk_lock_flags_t)(_MALI_OSK_LOCKFLAG_SPINLOCK | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE), 0, 0);
+    if (NULL == bank->lock)
+    {
+        _mali_osk_atomic_term(&bank->num_active_allocations);
+		_mali_osk_free(bank);
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+    }
+
+	bank->blocklist = _mali_osk_calloc(1, sizeof(struct mali_memory_block) * (usable_size / MIN_BLOCK_SIZE));
+	if (NULL == bank->blocklist)
+	{
+        _mali_osk_lock_term(bank->lock);
+        _mali_osk_atomic_term(&bank->num_active_allocations);
+		_mali_osk_free(bank);
+        MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	for (i = 0; i < (usable_size / MIN_BLOCK_SIZE); i++)
+	{
+		bank->blocklist[i].bank = bank;
+	}
+
+	bank->freelist = _mali_osk_calloc(1, sizeof(_mali_osk_list_t) * (bank->max_order - bank->min_order + 1));
+	if (NULL == bank->freelist)
+	{
+        _mali_osk_lock_term(bank->lock);
+		_mali_osk_free(bank->blocklist);
+        _mali_osk_atomic_term(&bank->num_active_allocations);
+		_mali_osk_free(bank);
+        MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	for (i = 0; i < (bank->max_order - bank->min_order + 1); i++) _MALI_OSK_INIT_LIST_HEAD(&bank->freelist[i]);
+
+	/* init slot info */
+	for (offset = 0, left = usable_size; offset < (usable_size / MIN_BLOCK_SIZE); /* updated inside the body */)
+	{
+		u32 block_order;
+		mali_memory_block * block;
+
+		/* the maximum order which fits in the remaining area */
+		block_order = maximum_order_which_fits(left);
+
+		/* find the block pointer */
+		block = &bank->blocklist[offset];
+
+		/* tag the block as being toplevel */
+		set_block_toplevel(block, block_order);
+
+		/* tag it as being free */
+		set_block_free(block, 1);
+
+		/* set the order */
+		set_block_order(block, block_order);
+
+		_mali_osk_list_addtail(&block->link, bank->freelist + (block_order - bank->min_order));
+
+		left -= (1 << block_order);
+		offset += ((1 << block_order) / MIN_BLOCK_SIZE);
+	}
+
+	/* add bank to list of banks on the system */
+	_MALI_OSK_LIST_FOREACHENTRY( bank_enum, temp, &memory_banks_list, mali_memory_bank, list )
+	{
+		if ( bank_enum->alloc_order >= alloc_order )
+		{
+			/* Found insertion point - our item must go before this one */
+			break;
+		}
+	}
+    _mali_osk_list_addtail(&bank->list, &bank_enum->list);
+
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_memory_mmu_register(u32 type, u32 phys_base)
+{
+	/* not supported */
+	return _MALI_OSK_ERR_INVALID_FUNC;
+}
+
+void mali_memory_mmu_unregister(u32 phys_base)
+{
+	/* not supported */
+	return;
+}
+
+static mali_memory_block * mali_memory_block_get(u32 type_id, u32 minimum_size)
+{
+	mali_memory_bank * bank;
+	mali_memory_block * block = NULL;
+	u32 requested_order, current_order;
+
+	/* input validation */
+	if (0 == minimum_size)
+	{
+		/* bad size */
+		MALI_DEBUG_PRINT(2, ("Zero size block requested by mali_memory_block_get\n"));
+		return NULL;
+	}
+
+	bank = (mali_memory_bank*)type_id;
+
+	requested_order = order_needed_for_size(minimum_size, bank);
+
+	MALI_DEBUG_PRINT(4, ("For size %d we need order %d (%d)\n", minimum_size, requested_order, 1 << requested_order));
+
+	_mali_osk_lock_wait(bank->lock, _MALI_OSK_LOCKMODE_RW);
+	/* ! critical section begin */
+
+	MALI_DEBUG_PRINT(7, ("Bank 0x%x locked\n", bank));
+
+	for (current_order = requested_order; current_order <= bank->max_order; ++current_order)
+	{
+		_mali_osk_list_t * list = bank->freelist + (current_order - bank->min_order);
+		MALI_DEBUG_PRINT(7, ("Checking freelist 0x%x for order %d\n", list, current_order));
+		if (0 != _mali_osk_list_empty(list)) continue; /* empty list */
+
+		MALI_DEBUG_PRINT(7, ("Found an entry on the freelist for order %d\n", current_order));
+
+
+		block = _MALI_OSK_LIST_ENTRY(list->next, mali_memory_block, link);
+		_mali_osk_list_delinit(&block->link);
+
+		while (current_order > requested_order)
+		{
+			mali_memory_block * buddy_block;
+			MALI_DEBUG_PRINT(7, ("Splitting block 0x%x\n", block));
+			current_order--;
+			list--;
+			buddy_block = block_get_buddy(block, current_order - bank->min_order);
+			set_block_order(buddy_block, current_order);
+			set_block_free(buddy_block, 1);
+			_mali_osk_list_add(&buddy_block->link, list);
+		}
+
+		set_block_order(block, current_order);
+		set_block_free(block, 0);
+
+		/* update usage count */
+		_mali_osk_atomic_inc(&bank->num_active_allocations);
+
+		break;
+	}
+
+	/* ! critical section end */
+	_mali_osk_lock_signal(bank->lock, _MALI_OSK_LOCKMODE_RW);
+
+	MALI_DEBUG_PRINT(7, ("Lock released for bank 0x%x\n", bank));
+
+	MALI_DEBUG_PRINT_IF(7, NULL != block, ("Block 0x%x allocated\n", block));
+
+	return block;
+}
+
+
+static void block_release(mali_memory_block * block)
+{
+	mali_memory_bank * bank;
+	u32 current_order;
+
+	if (NULL == block) return;
+
+	bank = block->bank;
+
+	/* we're manipulating the free list, so we need to lock it */
+	_mali_osk_lock_wait(bank->lock, _MALI_OSK_LOCKMODE_RW);
+	/* ! critical section begin */
+
+	set_block_free(block, 1);
+	current_order = get_block_order(block);
+
+	while (current_order <= bank->max_order)
+	{
+		mali_memory_block * buddy_block;
+		buddy_block = block_get_buddy(block, current_order - bank->min_order);
+		if (!block_is_valid_buddy(buddy_block, current_order)) break;
+		_mali_osk_list_delinit(&buddy_block->link); /* remove from free list */
+		/* clear tracked data in both blocks */
+		set_block_order(block, 0);
+		set_block_free(block, 0);
+		set_block_order(buddy_block, 0);
+		set_block_free(buddy_block, 0);
+		/* make the parent control the new state */
+		block = block_get_parent(block, current_order - bank->min_order);
+		set_block_order(block, current_order + 1); /* merged has a higher order */
+		set_block_free(block, 1); /* mark it as free */
+		current_order++;
+ 		if (get_block_toplevel(block) == current_order) break; /* stop the merge if we've arrived at a toplevel block */
+	}
+
+	_mali_osk_list_add(&block->link, &bank->freelist[current_order - bank->min_order]);
+
+	/* update bank usage statistics */
+	_mali_osk_atomic_dec(&block->bank->num_active_allocations);
+
+	/* !critical section end */
+	_mali_osk_lock_signal(bank->lock, _MALI_OSK_LOCKMODE_RW);
+
+	return;
+}
+
+MALI_STATIC_INLINE u32 block_get_offset(mali_memory_block * block)
+{
+	return block - block->bank->blocklist;
+}
+
+MALI_STATIC_INLINE u32 block_mali_addr_get(mali_memory_block * block)
+{
+	if (NULL != block) return block->bank->base_addr + MIN_BLOCK_SIZE * block_get_offset(block);
+	else return 0;
+}
+
+MALI_STATIC_INLINE u32 block_cpu_addr_get(mali_memory_block * block)
+{
+	if (NULL != block) return (block->bank->base_addr + MIN_BLOCK_SIZE * block_get_offset(block)) + block->bank->cpu_usage_adjust;
+	else return 0;
+}
+
+MALI_STATIC_INLINE u32 block_size_get(mali_memory_block * block)
+{
+	if (NULL != block) return 1 << get_block_order(block);
+	else return 0;
+}
+
+MALI_STATIC_INLINE void __user * block_mapping_get(mali_memory_block * block)
+{
+	if (NULL != block) return block->mapping;
+	else return NULL;
+}
+
+MALI_STATIC_INLINE void block_mapping_set(mali_memory_block * block, void __user * ptr)
+{
+	if (NULL != block) block->mapping = ptr;
+}
+
+MALI_STATIC_INLINE u32 block_mmap_cookie_get(mali_memory_block * block)
+{
+	if (NULL != block) return block->mmap_cookie;
+	else return 0;
+}
+
+/**
+ * Set the cookie returned via _mali_ukk_mem_mmap().
+ * @param block The memory block to set the cookie for
+ * @param cookie the cookie
+ */
+MALI_STATIC_INLINE void block_mmap_cookie_set(mali_memory_block * block, u32 cookie)
+{
+	if (NULL != block) block->mmap_cookie = cookie;
+}
+
+
+static _mali_osk_errcode_t mali_memory_core_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue)
+{
+	memory_session * session_data;
+
+	/* validate input */
+	if (NULL == slot)
+	{
+		MALI_DEBUG_PRINT(1, ("NULL slot given to memory session begin\n"));
+        MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}
+
+	if (NULL != *slot)
+	{
+		MALI_DEBUG_PRINT(1, ("The slot given to memory session begin already contains data"));
+        MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}
+
+	/* create the session data object */
+	MALI_CHECK_NON_NULL(session_data = _mali_osk_malloc(sizeof(memory_session)), _MALI_OSK_ERR_NOMEM);
+
+	/* create descriptor mapping table */
+	session_data->descriptor_mapping = mali_descriptor_mapping_create(MALI_MEM_DESCRIPTORS_INIT, MALI_MEM_DESCRIPTORS_MAX);
+
+    if (NULL == session_data->descriptor_mapping)
+	{
+		_mali_osk_free(session_data);
+		MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	_MALI_OSK_INIT_LIST_HEAD(&session_data->memory_head); /* no memory in use */
+	session_data->lock = _mali_osk_lock_init((_mali_osk_lock_flags_t)(_MALI_OSK_LOCKFLAG_ONELOCK | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE), 0, 0);
+    if (NULL == session_data->lock)
+    {
+		_mali_osk_free(session_data);
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+    }
+
+	*slot = session_data; /* slot will point to our data object */
+
+	MALI_SUCCESS;
+}
+
+static void mali_memory_core_session_end(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot)
+{
+	memory_session * session_data;
+
+	/* validate input */
+	if (NULL == slot)
+	{
+		MALI_DEBUG_PRINT(1, ("NULL slot given to memory session begin\n"));
+		return;
+	}
+
+	if (NULL == *slot)
+	{
+		MALI_DEBUG_PRINT(1, ("NULL memory_session found in current session object"));
+		return;
+	}
+
+	_mali_osk_lock_wait(((memory_session*)*slot)->lock, _MALI_OSK_LOCKMODE_RW);
+	session_data = (memory_session *)*slot;
+	 /* clear our slot */
+        *slot = NULL;
+
+	/*
+		First free all memory still being used.
+		This can happen if the caller has leaked memory or
+		the application has crashed forcing an auto-session end.
+	*/
+	if (0 == _mali_osk_list_empty(&session_data->memory_head))
+	{
+		mali_memory_block * block, * temp;
+		MALI_DEBUG_PRINT(1, ("Memory found on session usage list during session termination\n"));
+
+		/* use the _safe version since fre_big_block removes the active block from the list we're iterating */
+		_MALI_OSK_LIST_FOREACHENTRY(block, temp, &session_data->memory_head, mali_memory_block, link)
+		{
+			_mali_osk_errcode_t err;
+			_mali_uk_free_big_block_s uk_args;
+
+			MALI_DEBUG_PRINT(4, ("Freeing block 0x%x with mali address 0x%x size %d mapped in user space at 0x%x\n",
+						block,
+						(void*)block_mali_addr_get(block),
+						block_size_get(block),
+						block_mapping_get(block))
+				   	);
+
+			/* free the block */
+			/** @note manual type safety check-point */
+    		uk_args.ctx = mali_session_data;
+			uk_args.cookie = (u32)block->descriptor;
+			err = _mali_ukk_free_big_block_internal( mali_session_data, session_data, &uk_args );
+
+			if ( _MALI_OSK_ERR_OK != err )
+			{
+				MALI_DEBUG_PRINT_ERROR(("_mali_ukk_free_big_block_internal() failed during session termination on block with cookie==0x%X\n",
+										uk_args.cookie)
+									   );
+			}
+		}
+	}
+
+	if (NULL != session_data->descriptor_mapping)
+	{
+		mali_descriptor_mapping_destroy(session_data->descriptor_mapping);
+		session_data->descriptor_mapping = NULL;
+	}
+
+	_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+	_mali_osk_lock_term(session_data->lock);
+
+    /* free the session data object */
+	_mali_osk_free(session_data);
+
+	return;
+}
+
+static _mali_osk_errcode_t mali_memory_core_system_info_fill(_mali_system_info* info)
+{
+	mali_memory_bank * bank, *temp;
+	_mali_mem_info **mem_info_tail;
+
+	/* check input */
+    MALI_CHECK_NON_NULL(info, _MALI_OSK_ERR_INVALID_ARGS);
+
+	/* make sure we won't leak any memory. It could also be that it's an uninitialized variable, but that would be a bug in the caller */
+	MALI_DEBUG_ASSERT(NULL == info->mem_info);
+
+	mem_info_tail = &info->mem_info;
+
+	_MALI_OSK_LIST_FOREACHENTRY(bank, temp, &memory_banks_list, mali_memory_bank, list)
+	{
+		_mali_mem_info * mem_info;
+
+        mem_info = (_mali_mem_info *)_mali_osk_calloc(1, sizeof(_mali_mem_info));
+		if (NULL == mem_info) return _MALI_OSK_ERR_NOMEM; /* memory already allocated will be freed by the caller */
+
+		/* set info */
+		mem_info->size = bank->size;
+		mem_info->flags = (_mali_bus_usage)bank->used_for_flags;
+		mem_info->maximum_order_supported = bank->max_order;
+		mem_info->identifier = (u32)bank;
+
+		/* add to system info linked list */
+		(*mem_info_tail) = mem_info;
+		mem_info_tail = &mem_info->next;
+	}
+
+	/* all OK */
+    MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_memory_core_resource_memory(_mali_osk_resource_t * resource)
+{
+	_mali_osk_errcode_t err;
+
+    /* Request ownership of the memory */
+	if (_MALI_OSK_ERR_OK != _mali_osk_mem_reqregion(resource->base, resource->size, resource->description))
+	{
+		MALI_DEBUG_PRINT(1, ("Failed to request memory region %s (0x%08X - 0x%08X)\n", resource->description, resource->base, resource->base + resource->size - 1));
+        MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	/* call backend */
+	err = mali_memory_bank_register(resource->base, resource->cpu_usage_adjust, resource->size, resource->flags, resource->alloc_order, resource->description);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		/* if backend refused the memory we have to release the region again */
+		MALI_DEBUG_PRINT(1, ("Memory bank registration failed\n"));
+		_mali_osk_mem_unreqregion(resource->base, resource->size);
+        MALI_ERROR(err);
+	}
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_memory_core_resource_mmu(_mali_osk_resource_t * resource)
+{
+	/* Not supported by the fixed block memory system */
+	MALI_DEBUG_PRINT(1, ("MMU resource not supported by non-MMU driver!\n"));
+	MALI_ERROR(_MALI_OSK_ERR_INVALID_FUNC);
+}
+
+static _mali_osk_errcode_t mali_memory_core_resource_fpga(_mali_osk_resource_t * resource)
+{
+	mali_io_address mapping;
+
+	MALI_DEBUG_PRINT(5, ("FPGA framework '%s' @ (0x%08X - 0x%08X)\n",
+			resource->description, resource->base, resource->base + sizeof(u32) * 2 - 1
+		   ));
+
+	mapping = _mali_osk_mem_mapioregion(resource->base + 0x1000, sizeof(u32) * 2, "fpga framework");
+	if (mapping)
+	{
+		u32 data;
+		data = _mali_osk_mem_ioread32(mapping, 0);
+		MALI_DEBUG_PRINT(2, ("FPGA framwork '%s' @ 0x%08X:\n", resource->description, resource->base));
+		MALI_DEBUG_PRINT(2, ("\tBitfile date: %d%02d%02d_%02d%02d\n",
+					  (data >> 20),
+					  (data >> 16) & 0xF,
+					  (data >> 11) & 0x1F,
+					  (data >> 6)  & 0x1F,
+					  (data >> 0)  & 0x3F));
+		data = _mali_osk_mem_ioread32(mapping, sizeof(u32));
+		MALI_DEBUG_PRINT(2, ("\tBitfile SCCS rev: %d\n", data));
+
+		_mali_osk_mem_unmapioregion(resource->base + 0x1000, sizeof(u32) *2, mapping);
+	}
+	else MALI_DEBUG_PRINT(1, ("Failed to access FPGA framwork '%s' @ 0x%08X\n", resource->description, resource->base));
+
+	MALI_SUCCESS;
+}
+
+/* static _mali_osk_errcode_t get_big_block(void * ukk_private, struct mali_session_data * mali_session_data, void __user * argument) */
+_mali_osk_errcode_t _mali_ukk_get_big_block( _mali_uk_get_big_block_s *args )
+{
+	_mali_uk_mem_mmap_s args_mmap = {0, };
+	int md;
+	mali_memory_block * block;
+	_mali_osk_errcode_t err;
+	memory_session * session_data;
+
+	MALI_DEBUG_ASSERT_POINTER( args );
+
+	MALI_DEBUG_ASSERT_POINTER( args->ctx );
+
+	/** @note manual type safety check-point */
+    session_data = (memory_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_memory_id);
+
+    MALI_CHECK_NON_NULL(session_data, _MALI_OSK_ERR_INVALID_ARGS);
+
+	_mali_osk_lock_wait(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+
+    if (!args->type_id)
+	{
+		_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* at least min block size */
+	if (MIN_BLOCK_SIZE > args->minimum_size_requested) args->minimum_size_requested = MIN_BLOCK_SIZE;
+
+	/* perform the actual allocation */
+	block = mali_memory_block_get(args->type_id, args->minimum_size_requested);
+	if ( NULL == block )
+	{
+		/* no memory available with requested type_id */
+		_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+		MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+    if (_MALI_OSK_ERR_OK != mali_descriptor_mapping_allocate_mapping(session_data->descriptor_mapping, block, &md))
+    {
+		block_release(block);
+		_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+       MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+	block->descriptor = md;
+
+
+	/* fill in response */
+	args->mali_address = block_mali_addr_get(block);
+	args->block_size = block_size_get(block);
+	args->cookie = (u32)md;
+	args->flags = block->bank->used_for_flags;
+
+	/* map the block into the process' address space */
+
+	/** @note manual type safety check-point */
+	args_mmap.ukk_private = (void *)args->ukk_private;
+	args_mmap.ctx = args->ctx;
+	args_mmap.size = args->block_size;
+	args_mmap.phys_addr = block_cpu_addr_get(block);
+
+#ifndef _MALI_OSK_SPECIFIC_INDIRECT_MMAP
+	err = _mali_ukk_mem_mmap( &args_mmap );
+#else
+	err = _mali_osk_specific_indirect_mmap( &args_mmap );
+#endif
+
+	/* check if the mapping failed */
+	if ( _MALI_OSK_ERR_OK != err )
+	{
+		MALI_DEBUG_PRINT(1, ("Memory mapping failed 0x%x\n", args->cpuptr));
+		/* mapping failed */
+
+		/* remove descriptor entry */
+		mali_descriptor_mapping_free(session_data->descriptor_mapping, md);
+
+		/* free the mali memory */
+		block_release(block);
+
+		_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+        return err;
+	}
+
+	args->cpuptr = args_mmap.mapping;
+	block_mmap_cookie_set(block, args_mmap.cookie);
+	block_mapping_set(block, args->cpuptr);
+
+	MALI_DEBUG_PRINT(2, ("Mali memory 0x%x (size %d) mapped in process memory space at 0x%x\n", (void*)args->mali_address, args->block_size, args->cpuptr));
+
+	/* track memory in use for the session */
+	_mali_osk_list_addtail(&block->link, &session_data->memory_head);
+
+	/* memory assigned to the session, memory mapped into the process' view */
+	_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+
+	MALI_SUCCESS;
+}
+
+/* Internal code that assumes the memory session lock is held */
+static _mali_osk_errcode_t _mali_ukk_free_big_block_internal( struct mali_session_data * mali_session_data, memory_session * session_data, _mali_uk_free_big_block_s *args)
+{
+	mali_memory_block * block = NULL;
+	_mali_osk_errcode_t err;
+	_mali_uk_mem_munmap_s args_munmap = {0,};
+
+	MALI_DEBUG_ASSERT_POINTER( mali_session_data );
+	MALI_DEBUG_ASSERT_POINTER( session_data );
+	MALI_DEBUG_ASSERT_POINTER( args );
+
+	err = mali_descriptor_mapping_get(session_data->descriptor_mapping, (int)args->cookie, (void**)&block);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		MALI_DEBUG_PRINT(1, ("Invalid memory descriptor %d used to release memory pages\n", (int)args->cookie));
+		MALI_ERROR(err);
+	}
+
+    MALI_DEBUG_ASSERT_POINTER(block);
+
+	MALI_DEBUG_PRINT(4, ("Asked to free block 0x%x with mali address 0x%x size %d mapped in user space at 0x%x\n",
+			block,
+			(void*)block_mali_addr_get(block),
+			block_size_get(block),
+			block_mapping_get(block))
+		   );
+
+	/** @note manual type safety check-point */
+	args_munmap.ctx = (void*)mali_session_data;
+	args_munmap.mapping = block_mapping_get( block );
+	args_munmap.size = block_size_get( block );
+	args_munmap.cookie = block_mmap_cookie_get( block );
+
+#ifndef _MALI_OSK_SPECIFIC_INDIRECT_MMAP
+		_mali_ukk_mem_munmap( &args_munmap );
+#else
+		_mali_osk_specific_indirect_munmap( &args_munmap );
+#endif
+
+	MALI_DEBUG_PRINT(6, ("Session data 0x%x, lock 0x%x\n", session_data, &session_data->lock));
+
+	/* unlink from session usage list */
+	MALI_DEBUG_PRINT(5, ("unlink from session usage list\n"));
+	_mali_osk_list_delinit(&block->link);
+
+	/* remove descriptor entry */
+	mali_descriptor_mapping_free(session_data->descriptor_mapping, (int)args->cookie);
+
+	/* free the mali memory */
+	block_release(block);
+	MALI_DEBUG_PRINT(5, ("Block freed\n"));
+
+	MALI_SUCCESS;
+}
+
+/* static _mali_osk_errcode_t free_big_block( struct mali_session_data * mali_session_data, void __user * argument) */
+_mali_osk_errcode_t _mali_ukk_free_big_block( _mali_uk_free_big_block_s *args )
+{
+	_mali_osk_errcode_t err;
+	struct mali_session_data * mali_session_data;
+	memory_session * session_data;
+
+	MALI_DEBUG_ASSERT_POINTER( args );
+
+	MALI_DEBUG_ASSERT_POINTER( args->ctx );
+
+	/** @note manual type safety check-point */
+	mali_session_data = (struct mali_session_data *)args->ctx;
+
+	/* Must always verify this, since these are provided by the user */
+    MALI_CHECK_NON_NULL(mali_session_data, _MALI_OSK_ERR_INVALID_ARGS);
+
+	session_data = mali_kernel_session_manager_slot_get(mali_session_data, mali_subsystem_memory_id);
+
+    MALI_CHECK_NON_NULL(session_data, _MALI_OSK_ERR_INVALID_ARGS);
+
+	_mali_osk_lock_wait(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+
+	/** @note this has been separated out so that the session_end handler can call this while it has the memory_session lock held */
+	err = _mali_ukk_free_big_block_internal( mali_session_data, session_data, args );
+
+	_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+
+    return err;
+}
+
+MALI_STATIC_INLINE u32 get_block_free(mali_memory_block * block)
+{
+	return (block->misc >> MISC_SHIFT_FREE) & MISC_MASK_FREE;
+}
+
+MALI_STATIC_INLINE void set_block_free(mali_memory_block * block, int state)
+{
+	if (state) block->misc |= (MISC_MASK_FREE << MISC_SHIFT_FREE);
+	else block->misc &= ~(MISC_MASK_FREE << MISC_SHIFT_FREE);
+}
+
+MALI_STATIC_INLINE void set_block_order(mali_memory_block * block, u32 order)
+{
+	block->misc &= ~(MISC_MASK_ORDER << MISC_SHIFT_ORDER);
+	block->misc |= ((order & MISC_MASK_ORDER) << MISC_SHIFT_ORDER);
+}
+
+MALI_STATIC_INLINE u32 get_block_order(mali_memory_block * block)
+{
+	return (block->misc >> MISC_SHIFT_ORDER) & MISC_MASK_ORDER;
+}
+
+MALI_STATIC_INLINE void set_block_toplevel(mali_memory_block * block, u32 level)
+{
+	block->misc |= ((level & MISC_MASK_TOPLEVEL) << MISC_SHIFT_TOPLEVEL);
+}
+
+MALI_STATIC_INLINE u32 get_block_toplevel(mali_memory_block * block)
+{
+	return (block->misc >> MISC_SHIFT_TOPLEVEL) & MISC_MASK_TOPLEVEL;
+}
+
+MALI_STATIC_INLINE int block_is_valid_buddy(mali_memory_block * block, int order)
+{
+	if (get_block_free(block) && (get_block_order(block) == order)) return 1;
+	else return 0;
+}
+
+MALI_STATIC_INLINE mali_memory_block * block_get_buddy(mali_memory_block * block, u32 order)
+{
+	return block + ( (block_get_offset(block) ^ (1 << order)) - block_get_offset(block));
+}
+
+MALI_STATIC_INLINE mali_memory_block * block_get_parent(mali_memory_block * block, u32 order)
+{
+	return block + ((block_get_offset(block) & ~(1 << order)) - block_get_offset(block));
+}
+
+/* This handler registered to mali_mmap for non-MMU builds */
+_mali_osk_errcode_t _mali_ukk_mem_mmap( _mali_uk_mem_mmap_s *args )
+{
+	_mali_osk_errcode_t ret;
+	struct mali_session_data * mali_session_data;
+	mali_memory_allocation * descriptor;
+	memory_session * session_data;
+
+	/* validate input */
+	if (NULL == args) { MALI_DEBUG_PRINT(3,("mali_ukk_mem_mmap: args was NULL\n")); MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS); }
+
+	/* Unpack arguments */
+	mali_session_data = (struct mali_session_data *)args->ctx;
+
+	if (NULL == mali_session_data) { MALI_DEBUG_PRINT(3,("mali_ukk_mem_mmap: mali_session data was NULL\n")); MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS); }
+
+	MALI_DEBUG_ASSERT( mali_subsystem_memory_id >= 0 );
+
+	session_data = mali_kernel_session_manager_slot_get(mali_session_data, mali_subsystem_memory_id);
+	/* validate input */
+	if (NULL == session_data) { MALI_DEBUG_PRINT(3,("mali_ukk_mem_mmap: session data was NULL\n")); MALI_ERROR(_MALI_OSK_ERR_FAULT); }
+
+	descriptor = (mali_memory_allocation*) _mali_osk_calloc( 1, sizeof(mali_memory_allocation) );
+	if (NULL == descriptor) { MALI_DEBUG_PRINT(3,("mali_ukk_mem_mmap: descriptor was NULL\n")); MALI_ERROR(_MALI_OSK_ERR_NOMEM); }
+
+	descriptor->size = args->size;
+	descriptor->mali_address = args->phys_addr;
+	descriptor->mali_addr_mapping_info = (void*)session_data;
+	descriptor->process_addr_mapping_info = args->ukk_private; /* save to be used during physical manager callback */
+	descriptor->flags = MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE;
+
+	ret = _mali_osk_mem_mapregion_init( descriptor );
+	if ( _MALI_OSK_ERR_OK != ret )
+	{
+		MALI_DEBUG_PRINT(3, ("_mali_osk_mem_mapregion_init() failed\n"));
+		_mali_osk_free(descriptor);
+        MALI_ERROR(ret);
+	}
+
+	ret = _mali_osk_mem_mapregion_map( descriptor, 0, &descriptor->mali_address, descriptor->size );
+	if ( _MALI_OSK_ERR_OK != ret )
+	{
+		MALI_DEBUG_PRINT(3, ("_mali_osk_mem_mapregion_map() failed\n"));
+		_mali_osk_mem_mapregion_term( descriptor );
+		_mali_osk_free(descriptor);
+        MALI_ERROR(ret);
+	}
+
+	args->mapping = descriptor->mapping;
+
+	/**
+	 * @note we do not require use of mali_descriptor_mapping here:
+	 * the cookie gets stored in the mali_memory_block struct, which itself is
+	 * protected by mali_descriptor_mapping, and so this cookie never leaves
+	 * kernel space (on any OS).
+	 *
+	 * In the MMU case, we must use a mali_descriptor_mapping, since on _some_
+	 * OSs, the cookie leaves kernel space.
+	 */
+	args->cookie = (u32)descriptor;
+    MALI_SUCCESS;
+}
+
+/* This handler registered to mali_munmap for non-MMU builds */
+_mali_osk_errcode_t _mali_ukk_mem_munmap( _mali_uk_mem_munmap_s *args )
+{
+	mali_memory_allocation * descriptor;
+
+	/** see note in _mali_ukk_mem_mmap() - no need to use descriptor mapping */
+	descriptor = (mali_memory_allocation *)args->cookie;
+	MALI_DEBUG_ASSERT_POINTER(descriptor);
+
+	/* args->mapping and args->size are also discarded. They are only necessary for certain do_munmap implementations. However, they could be used to check the descriptor at this point. */
+	_mali_osk_mem_mapregion_unmap( descriptor, 0, descriptor->size, (_mali_osk_mem_mapregion_flags_t)0 );
+
+	_mali_osk_mem_mapregion_term( descriptor );
+
+	_mali_osk_free(descriptor);
+
+	return _MALI_OSK_ERR_OK;
+}
+
+/**
+ * Stub function to satisfy UDD interface exclusion requirement.
+ * This is because the Base code compiles in \b both MMU and non-MMU calls,
+ * so both sets must be declared (but the 'unused' set may be stub)
+ */
+_mali_osk_errcode_t _mali_ukk_init_mem( _mali_uk_init_mem_s *args )
+{
+	MALI_IGNORE( args );
+	return _MALI_OSK_ERR_FAULT;
+}
+
+/**
+ * Stub function to satisfy UDD interface exclusion requirement.
+ * This is because the Base code compiles in \b both MMU and non-MMU calls,
+ * so both sets must be declared (but the 'unused' set may be stub)
+ */
+_mali_osk_errcode_t _mali_ukk_term_mem( _mali_uk_term_mem_s *args )
+{
+	MALI_IGNORE( args );
+	return _MALI_OSK_ERR_FAULT;
+}
+
+/**
+ * Stub function to satisfy UDD interface exclusion requirement.
+ * This is because the Base code compiles in \b both MMU and non-MMU calls,
+ * so both sets must be declared (but the 'unused' set may be stub)
+ */
+_mali_osk_errcode_t _mali_ukk_map_external_mem( _mali_uk_map_external_mem_s *args )
+{
+	MALI_IGNORE( args );
+	return _MALI_OSK_ERR_FAULT;
+}
+
+/**
+ * Stub function to satisfy UDD interface exclusion requirement.
+ * This is because the Base code compiles in \b both MMU and non-MMU calls,
+ * so both sets must be declared (but the 'unused' set may be stub)
+ */
+_mali_osk_errcode_t _mali_ukk_unmap_external_mem( _mali_uk_unmap_external_mem_s *args )
+{
+	MALI_IGNORE( args );
+	return _MALI_OSK_ERR_FAULT;
+}
+
+/**
+ * Stub function to satisfy UDD interface exclusion requirement.
+ * This is because the Base code compiles in \b both MMU and non-MMU calls,
+ * so both sets must be declared (but the 'unused' set may be stub)
+ */
+_mali_osk_errcode_t _mali_ukk_query_mmu_page_table_dump_size( _mali_uk_query_mmu_page_table_dump_size_s *args )
+{
+	MALI_IGNORE( args );
+	return _MALI_OSK_ERR_FAULT;
+}
+
+/**
+ * Stub function to satisfy UDD interface exclusion requirement.
+ * This is because the Base code compiles in \b both MMU and non-MMU calls,
+ * so both sets must be declared (but the 'unused' set may be stub)
+ */
+_mali_osk_errcode_t _mali_ukk_dump_mmu_page_table( _mali_uk_dump_mmu_page_table_s * args )
+{
+	MALI_IGNORE( args );
+	return _MALI_OSK_ERR_FAULT;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_mmu.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_mmu.c
new file mode 100644
index 00000000000..7a48e27ede7
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_mmu.c
@@ -0,0 +1,2924 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_subsystem.h"
+#include "mali_kernel_mem.h"
+#include "mali_kernel_ioctl.h"
+#include "mali_kernel_descriptor_mapping.h"
+#include "mali_kernel_mem_mmu.h"
+#include "mali_kernel_memory_engine.h"
+#include "mali_block_allocator.h"
+#include "mali_kernel_mem_os.h"
+#include "mali_kernel_session_manager.h"
+#include "mali_kernel_core.h"
+
+#if defined USING_MALI400_L2_CACHE
+#include "mali_kernel_l2_cache.h"
+#endif
+
+#if MALI_USE_UNIFIED_MEMORY_PROVIDER != 0
+#include "ump_kernel_interface.h"
+#endif
+
+/* kernel side OS functions and user-kernel interface */
+#include "mali_osk.h"
+#include "mali_osk_mali.h"
+#include "mali_ukk.h"
+#include "mali_osk_bitops.h"
+#include "mali_osk_list.h"
+
+/**
+ * Size of the MMU registers in bytes
+ */
+#define MALI_MMU_REGISTERS_SIZE 0x24
+
+/**
+ * Size of an MMU page in bytes
+ */
+#define MALI_MMU_PAGE_SIZE 0x1000
+
+/**
+ * Page directory index from address
+ * Calculates the page directory index from the given address
+ */
+#define MALI_MMU_PDE_ENTRY(address) (((address)>>22) & 0x03FF)
+
+/**
+ * Page table index from address
+ * Calculates the page table index from the given address
+ */
+#define MALI_MMU_PTE_ENTRY(address) (((address)>>12) & 0x03FF)
+
+/**
+ * Extract the memory address from an PDE/PTE entry
+ */
+#define MALI_MMU_ENTRY_ADDRESS(value) ((value) & 0xFFFFFC00)
+
+/**
+ * Linux kernel version has marked SA_SHIRQ as deprecated, IRQF_SHARED should be used.
+ * This is to handle older kernels which haven't done this swap.
+ */
+#ifndef IRQF_SHARED
+#define IRQF_SHARED SA_SHIRQ
+#endif /* IRQF_SHARED */
+
+/**
+ * Per-session memory descriptor mapping table sizes
+ */
+#define MALI_MEM_DESCRIPTORS_INIT 64
+#define MALI_MEM_DESCRIPTORS_MAX 65536
+
+/**
+ * Used to disallow more than one core to run a MMU at the same time
+ *
+ * @note This value is hardwired into some systems' configuration files,
+ * which \em might not be a header file (e.g. some external data configuration
+ * file). Therefore, if this value is modified, its occurance must be
+ * \b manually checked for in the entire driver source tree.
+ */
+#define MALI_MMU_DISALLOW_PARALLELL_WORK_OF_MALI_CORES 1
+
+#define MALI_INVALID_PAGE ((u32)(~0))
+
+/**
+ *
+ */
+typedef enum mali_mmu_entry_flags
+{
+	MALI_MMU_FLAGS_PRESENT = 0x01,
+	MALI_MMU_FLAGS_READ_PERMISSION = 0x02,
+	MALI_MMU_FLAGS_WRITE_PERMISSION = 0x04,
+	MALI_MMU_FLAGS_MASK = 0x07
+} mali_mmu_entry_flags;
+
+/**
+ * MMU register numbers
+ * Used in the register read/write routines.
+ * See the hardware documentation for more information about each register
+ */
+typedef enum mali_mmu_register {
+	MALI_MMU_REGISTER_DTE_ADDR = 0x0000, /**< Current Page Directory Pointer */
+	MALI_MMU_REGISTER_STATUS = 0x0001, /**< Status of the MMU */
+	MALI_MMU_REGISTER_COMMAND = 0x0002, /**< Command register, used to control the MMU */
+	MALI_MMU_REGISTER_PAGE_FAULT_ADDR = 0x0003, /**< Logical address of the last page fault */
+	MALI_MMU_REGISTER_ZAP_ONE_LINE = 0x004, /**< Used to invalidate the mapping of a single page from the MMU */
+	MALI_MMU_REGISTER_INT_RAWSTAT = 0x0005, /**< Raw interrupt status, all interrupts visible */
+	MALI_MMU_REGISTER_INT_CLEAR = 0x0006, /**< Indicate to the MMU that the interrupt has been received */
+	MALI_MMU_REGISTER_INT_MASK = 0x0007, /**< Enable/disable types of interrupts */
+	MALI_MMU_REGISTER_INT_STATUS = 0x0008 /**< Interrupt status based on the mask */
+} mali_mmu_register;
+
+/**
+ * MMU interrupt register bits
+ * Each cause of the interrupt is reported
+ * through the (raw) interrupt status registers.
+ * Multiple interrupts can be pending, so multiple bits
+ * can be set at once.
+ */
+typedef enum mali_mmu_interrupt
+{
+	MALI_MMU_INTERRUPT_PAGE_FAULT = 0x01, /**< A page fault occured */
+	MALI_MMU_INTERRUPT_READ_BUS_ERROR = 0x02 /**< A bus read error occured */
+} mali_mmu_interrupt;
+
+/**
+ * MMU commands
+ * These are the commands that can be sent
+ * to the MMU unit.
+ */
+typedef enum mali_mmu_command
+{
+	MALI_MMU_COMMAND_ENABLE_PAGING = 0x00, /**< Enable paging (memory translation) */
+	MALI_MMU_COMMAND_DISABLE_PAGING = 0x01, /**< Disable paging (memory translation) */
+	MALI_MMU_COMMAND_ENABLE_STALL = 0x02, /**<  Enable stall on page fault */
+	MALI_MMU_COMMAND_DISABLE_STALL = 0x03, /**< Disable stall on page fault */
+	MALI_MMU_COMMAND_ZAP_CACHE = 0x04, /**< Zap the entire page table cache */
+	MALI_MMU_COMMAND_PAGE_FAULT_DONE = 0x05, /**< Page fault processed */
+	MALI_MMU_COMMAND_SOFT_RESET = 0x06 /**< Reset the MMU back to power-on settings */
+} mali_mmu_command;
+
+typedef enum mali_mmu_status_bits
+{
+	MALI_MMU_STATUS_BIT_PAGING_ENABLED      = 1 << 0,
+	MALI_MMU_STATUS_BIT_PAGE_FAULT_ACTIVE   = 1 << 1,
+	MALI_MMU_STATUS_BIT_STALL_ACTIVE        = 1 << 2,
+	MALI_MMU_STATUS_BIT_IDLE                = 1 << 3,
+	MALI_MMU_STATUS_BIT_REPLAY_BUFFER_EMPTY = 1 << 4,
+	MALI_MMU_STATUS_BIT_PAGE_FAULT_IS_WRITE = 1 << 5,
+} mali_mmu_status_bits;
+
+/**
+ * Defintion of the type used to represent memory used by a session.
+ * Containts the pointer to the huge user space virtual memory area
+ * used to access the Mali memory.
+ */
+typedef struct memory_session
+{
+	_mali_osk_lock_t *lock; /**< Lock protecting the vm manipulation */
+
+	u32 mali_base_address; /**< Mali virtual memory area used by this session */
+	mali_descriptor_mapping * descriptor_mapping; /**< Mapping between userspace descriptors and our pointers */
+
+	u32 page_directory; /**< Physical address of the memory session's page directory */
+
+	mali_io_address page_directory_mapped; /**< Pointer to the mapped version of the page directory into the kernel's address space */
+	mali_io_address page_entries_mapped[1024]; /**< Pointers to the page tables which exists in the page directory mapped into the kernel's address space */
+	u32   page_entries_usage_count[1024]; /**< Tracks usage count of the page table pages, so they can be releases on the last reference */
+
+	_mali_osk_list_t active_mmus; /**< The MMUs in this session, in increasing order of ID (so we can lock them in the correct order when necessary) */
+	_mali_osk_list_t memory_head; /**< Track all the memory allocated in this session, for freeing on abnormal termination */
+} memory_session;
+
+typedef struct mali_kernel_memory_mmu_idle_callback
+{
+	_mali_osk_list_t link;
+	void (*callback)(void*);
+	void * callback_argument;
+} mali_kernel_memory_mmu_idle_callback;
+
+/**
+ * Definition of the MMU struct
+ * Used to track a MMU unit in the system.
+ * Contains information about the mapping of the registers
+ */
+typedef struct mali_kernel_memory_mmu
+{
+	int id; /**< ID of the MMU, no duplicate IDs may exist on the system */
+	const char * description; /**< Description text received from the resource manager to help identify the resource for people */
+	int irq_nr; /**< IRQ number */
+	u32 base; /**< Physical address of the registers */
+	mali_io_address mapped_registers; /**< Virtual mapping of the registers */
+	u32 mapping_size; /**< Size of registers in bytes */
+	_mali_osk_list_t list; /**< Used to link multiple MMU's into a list */
+	_mali_osk_irq_t *irq;
+	u32 flags; /**< Used to store if there is something special with this mmu. */
+
+	_mali_osk_lock_t *lock; /**< Lock protecting access to the usage fields */
+	/* usage fields */
+	memory_session * active_session; /**< Active session, NULL if no session is active */
+	u32 usage_count; /**< Number of nested activations of the active session */
+	_mali_osk_list_t callbacks; /**< Callback registered for MMU idle notification */
+
+	int in_page_fault_handler;
+
+	_mali_osk_list_t session_link;
+} mali_kernel_memory_mmu;
+
+typedef struct dedicated_memory_info
+{
+	u32 base;
+	u32 size;
+	struct dedicated_memory_info * next;
+} dedicated_memory_info;
+
+/* types used for external_memory and ump_memory physical memory allocators, which are using the mali_allocation_engine */
+#if MALI_USE_UNIFIED_MEMORY_PROVIDER != 0
+typedef struct ump_mem_allocation
+{
+	mali_allocation_engine * engine;
+	mali_memory_allocation * descriptor;
+	u32 initial_offset;
+	u32 size_allocated;
+	ump_dd_handle ump_mem;
+} ump_mem_allocation ;
+#endif
+
+typedef struct external_mem_allocation
+{
+	mali_allocation_engine * engine;
+	mali_memory_allocation * descriptor;
+	u32 initial_offset;
+	u32 size;
+} external_mem_allocation;
+
+/*
+   Subsystem interface implementation
+*/
+/**
+ * Fixed block memory subsystem startup function.
+ * Called by the driver core when the driver is loaded.
+ * Registers the memory systems ioctl handler, resource handlers and memory map function with the core.
+ *
+ * @param id Identifier assigned by the core to the memory subsystem
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_initialize(mali_kernel_subsystem_identifier id);
+
+/**
+ * Fixed block memory subsystem shutdown function.
+ * Called by the driver core when the driver is unloaded.
+ * Cleans up
+ * @param id Identifier assigned by the core to the memory subsystem
+ */
+static void mali_memory_core_terminate(mali_kernel_subsystem_identifier id);
+
+/**
+ * MMU Memory load complete notification function.
+ * Called by the driver core when all drivers have loaded and all resources has been registered
+ * Builds the memory overall memory list
+ * @param id Identifier assigned by the core to the memory subsystem
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_load_complete(mali_kernel_subsystem_identifier id);
+
+/**
+ * Fixed block memory subsystem session begin notification
+ * Called by the core when a new session to the driver is started.
+ * Creates a memory session object and sets it as the subsystem slot data for this session
+ * @param slot Pointer to the slot to use for storing per-session data
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue);
+
+/**
+ * Fixed block memory subsystem session end notification
+ * Called by the core when a session to the driver has ended.
+ * Cleans up per session data, which includes checking and fixing memory leaks
+ *
+ * @param slot Pointer to the slot to use for storing per-session data
+ */
+static void mali_memory_core_session_end(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot);
+
+/**
+ * Fixed block memory subsystem system info filler
+ * Called by the core when a system info update is needed
+ * We fill in info about all the memory types we have
+ * @param info Pointer to system info struct to update
+ * @return 0 on success, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_system_info_fill(_mali_system_info* info);
+
+/* our registered resource handlers */
+
+/**
+ * Fixed block memory subsystem's notification handler for MMU resource instances.
+ * Registered with the core during startup.
+ * Called by the core for each mmu described in the active architecture's config.h file.
+ * @param resource The resource to handle (type MMU)
+ * @return 0 if the MMU was found and initialized, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_resource_mmu(_mali_osk_resource_t * resource);
+
+/**
+ * Fixed block memory subsystem's notification handler for FPGA_FRAMEWORK resource instances.
+ * Registered with the core during startup.
+ * Called by the core for each fpga framework described in the active architecture's config.h file.
+ * @param resource The resource to handle (type FPGA_FRAMEWORK)
+ * @return 0 if the FPGA framework was found and initialized, negative on error
+ */
+static _mali_osk_errcode_t mali_memory_core_resource_fpga(_mali_osk_resource_t * resource);
+
+
+static _mali_osk_errcode_t mali_memory_core_resource_dedicated_memory(_mali_osk_resource_t * resource);
+static _mali_osk_errcode_t mali_memory_core_resource_os_memory(_mali_osk_resource_t * resource);
+
+/**
+ * @brief Internal function for unmapping memory
+ *
+ * Worker function for unmapping memory from a user-process. We assume that the
+ * session/descriptor's lock was obtained before entry. For example, the
+ * wrapper _mali_ukk_mem_munmap() will lock the descriptor, then call this
+ * function to do the actual unmapping. mali_memory_core_session_end() could
+ * also call this directly (depending on compilation options), having locked
+ * the descriptor.
+ *
+ * @param args see _mali_uk_mem_munmap_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+static void _mali_ukk_mem_munmap_internal( _mali_uk_mem_munmap_s *args );
+
+/**
+ * The MMU interrupt handler
+ * Upper half of the MMU interrupt processing.
+ * Called by the kernel when the MMU has triggered an interrupt.
+ * The interrupt function supports IRQ sharing. So it'll probe the MMU in question
+ * @param irq The irq number (not used)
+ * @param dev_id Points to the MMU object being handled
+ * @param regs Registers of interrupted process (not used)
+ * @return Standard Linux interrupt result.
+ *         Subset used by the driver is IRQ_HANDLED processed
+ *                                      IRQ_NONE Not processed
+ */
+static _mali_osk_errcode_t mali_kernel_memory_mmu_interrupt_handler_upper_half(void * data);
+
+/**
+ * The MMU reset hander
+ * Bottom half of the MMU interrupt processing for page faults and bus errors
+ * @param work The item to operate on, NULL in our case
+ */
+static void mali_kernel_memory_mmu_interrupt_handler_bottom_half ( void *data );
+
+/**
+ * Read MMU register value
+ * Reads the contents of the specified register.
+ * @param unit The MMU to read from
+ * @param reg The register to read
+ * @return The contents of the register
+ */
+static u32 mali_mmu_register_read(mali_kernel_memory_mmu * unit, mali_mmu_register reg);
+
+/**
+ * Write to a MMU register
+ * Writes the given value to the specified register
+ * @param unit The MMU to write to
+ * @param reg The register to write to
+ * @param val The value to write to the register
+ */
+static void mali_mmu_register_write(mali_kernel_memory_mmu * unit, mali_mmu_register reg, u32 val);
+
+
+#if MALI_USE_UNIFIED_MEMORY_PROVIDER != 0
+static void ump_memory_release(void * ctx, void * handle);
+static mali_physical_memory_allocation_result ump_memory_commit(void* ctx, mali_allocation_engine * engine, mali_memory_allocation * descriptor, u32* offset, mali_physical_memory_allocation * alloc_info);
+#endif /* MALI_USE_UNIFIED_MEMORY_PROVIDER != 0*/
+
+
+static void external_memory_release(void * ctx, void * handle);
+static mali_physical_memory_allocation_result external_memory_commit(void* ctx, mali_allocation_engine * engine, mali_memory_allocation * descriptor, u32* offset, mali_physical_memory_allocation * alloc_info);
+
+
+
+
+/* nop functions */
+
+/* mali address manager needs to allocate page tables on allocate, write to page table(s) on map, write to page table(s) and release page tables on release */
+static _mali_osk_errcode_t  mali_address_manager_allocate(mali_memory_allocation * descriptor); /* validates the range, allocates memory for the page tables if needed */
+static _mali_osk_errcode_t  mali_address_manager_map(mali_memory_allocation * descriptor, u32 offset, u32 *phys_addr, u32 size);
+static void mali_address_manager_release(mali_memory_allocation * descriptor);
+
+static void mali_mmu_activate_address_space(mali_kernel_memory_mmu * mmu, u32 page_directory);
+
+_mali_osk_errcode_t mali_mmu_page_table_cache_create(void);
+void mali_mmu_page_table_cache_destroy(void);
+
+_mali_osk_errcode_t mali_mmu_get_table_page(u32 *table_page, mali_io_address *mapping);
+void mali_mmu_release_table_page(u32 pa);
+
+static _mali_osk_errcode_t mali_allocate_empty_page_directory(void);
+
+static void mali_free_empty_page_directory(void);
+
+static _mali_osk_errcode_t fill_page(mali_io_address mapping, u32 data);
+
+static _mali_osk_errcode_t mali_allocate_fault_flush_pages(void);
+
+static void mali_free_fault_flush_pages(void);
+
+static void mali_mmu_probe_irq_trigger(mali_kernel_memory_mmu * mmu);
+static _mali_osk_errcode_t mali_mmu_probe_irq_acknowledge(mali_kernel_memory_mmu * mmu);
+
+/* MMU variables */
+
+typedef struct mali_mmu_page_table_allocation
+{
+	_mali_osk_list_t list;
+	u32 * usage_map;
+	u32 usage_count;
+	u32 num_pages;
+	mali_page_table_block pages;
+} mali_mmu_page_table_allocation;
+
+typedef struct mali_mmu_page_table_allocations
+{
+	_mali_osk_lock_t *lock;
+	_mali_osk_list_t partial;
+	_mali_osk_list_t full;
+	/* we never hold on to a empty allocation */
+} mali_mmu_page_table_allocations;
+
+/* Head of the list of MMUs */
+static _MALI_OSK_LIST_HEAD(mmu_head);
+
+/* the mmu page table cache */
+static struct mali_mmu_page_table_allocations page_table_cache;
+
+/* page fault queue flush helper pages
+ * note that the mapping pointers are currently unused outside of the initialization functions */
+static u32 mali_page_fault_flush_page_directory = MALI_INVALID_PAGE;
+static mali_io_address mali_page_fault_flush_page_directory_mapping = NULL;
+static u32 mali_page_fault_flush_page_table = MALI_INVALID_PAGE;
+static mali_io_address mali_page_fault_flush_page_table_mapping = NULL;
+static u32 mali_page_fault_flush_data_page = MALI_INVALID_PAGE;
+static mali_io_address mali_page_fault_flush_data_page_mapping = NULL;
+
+/* an empty page directory (no address valid) which is active on any MMU not currently marked as in use */
+static u32 mali_empty_page_directory = MALI_INVALID_PAGE;
+
+/*
+   The fixed memory system's mali subsystem interface implementation.
+   We currently handle module and session life-time management.
+*/
+struct mali_kernel_subsystem mali_subsystem_memory =
+{
+	mali_memory_core_initialize,        /* startup */
+	mali_memory_core_terminate,         /* shutdown */
+	mali_memory_core_load_complete,     /* load_complete */
+	mali_memory_core_system_info_fill,  /* system_info_fill */
+	mali_memory_core_session_begin,     /* session_begin */
+	mali_memory_core_session_end,       /* session_end */
+	NULL,                               /* broadcast_notification */
+#if MALI_STATE_TRACKING
+	NULL,                               /* dump_state */
+#endif
+};
+
+static mali_kernel_mem_address_manager mali_address_manager =
+{
+	mali_address_manager_allocate, /* allocate */
+	mali_address_manager_release,  /* release */
+	mali_address_manager_map,      /* map_physical */
+	NULL                           /* unmap_physical not present*/
+};
+
+static mali_kernel_mem_address_manager process_address_manager =
+{
+	_mali_osk_mem_mapregion_init,  /* allocate */
+	_mali_osk_mem_mapregion_term,  /* release */
+	_mali_osk_mem_mapregion_map,   /* map_physical */
+	_mali_osk_mem_mapregion_unmap  /* unmap_physical */
+};
+
+static mali_allocation_engine memory_engine = NULL;
+static mali_physical_memory_allocator * physical_memory_allocators = NULL;
+
+static dedicated_memory_info * mem_region_registrations = NULL;
+
+/* Initialized when this subsystem is initialized. This is determined by the
+ * position in subsystems[], and so the value used to initialize this is
+ * determined at compile time */
+static mali_kernel_subsystem_identifier mali_subsystem_memory_id = (mali_kernel_subsystem_identifier)-1;
+
+/* called during module init */
+static _mali_osk_errcode_t mali_memory_core_initialize(mali_kernel_subsystem_identifier id)
+{
+	MALI_DEBUG_PRINT(2, ("MMU memory system initializing\n"));
+
+	/* save our subsystem id for later for use in slot lookup during session activation */
+	mali_subsystem_memory_id = id;
+
+	_MALI_OSK_INIT_LIST_HEAD(&mmu_head);
+
+	MALI_CHECK_NO_ERROR( mali_mmu_page_table_cache_create() );
+
+	/* register our handlers */
+	MALI_CHECK_NO_ERROR( _mali_kernel_core_register_resource_handler(MMU, mali_memory_core_resource_mmu) );
+
+	MALI_CHECK_NO_ERROR( _mali_kernel_core_register_resource_handler(FPGA_FRAMEWORK, mali_memory_core_resource_fpga) );
+
+	MALI_CHECK_NO_ERROR( _mali_kernel_core_register_resource_handler(MEMORY, mali_memory_core_resource_dedicated_memory) );
+
+	MALI_CHECK_NO_ERROR( _mali_kernel_core_register_resource_handler(OS_MEMORY, mali_memory_core_resource_os_memory) );
+
+	memory_engine = mali_allocation_engine_create(&mali_address_manager, &process_address_manager);
+	MALI_CHECK_NON_NULL( memory_engine, _MALI_OSK_ERR_FAULT);
+
+	MALI_SUCCESS;
+}
+
+/* called if/when our module is unloaded */
+static void mali_memory_core_terminate(mali_kernel_subsystem_identifier id)
+{
+	mali_kernel_memory_mmu * mmu, *temp_mmu;
+
+	MALI_DEBUG_PRINT(2, ("MMU memory system terminating\n"));
+
+	/* loop over all MMU units and shut them down */
+	_MALI_OSK_LIST_FOREACHENTRY(mmu, temp_mmu, &mmu_head, mali_kernel_memory_mmu, list)
+	{
+		/* reset to defaults */
+		mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_SOFT_RESET);
+
+		/* unregister the irq */
+		_mali_osk_irq_term(mmu->irq);
+
+		/* remove from the list of MMU's on the system */
+		_mali_osk_list_del(&mmu->list);
+
+		/* release resources */
+		_mali_osk_mem_unmapioregion(mmu->base, mmu->mapping_size, mmu->mapped_registers);
+		_mali_osk_mem_unreqregion(mmu->base, mmu->mapping_size);
+		_mali_osk_free(mmu);
+	}
+
+	/* free global helper pages */
+	mali_free_empty_page_directory();
+	mali_free_fault_flush_pages();
+
+	/* destroy the page table cache before shutting down backends in case we have a page table leak to report */
+	mali_mmu_page_table_cache_destroy();
+
+	while ( NULL != mem_region_registrations)
+	{
+		dedicated_memory_info * m;
+		m = mem_region_registrations;
+		mem_region_registrations = m->next;
+		_mali_osk_mem_unreqregion(m->base, m->size);
+		_mali_osk_free(m);
+	}
+
+	while ( NULL != physical_memory_allocators)
+	{
+		mali_physical_memory_allocator * m;
+		m = physical_memory_allocators;
+		physical_memory_allocators = m->next;
+		m->destroy(m);
+	}
+
+	if (NULL != memory_engine)
+	{
+		mali_allocation_engine_destroy(memory_engine);
+		memory_engine = NULL;
+	}
+
+}
+
+static _mali_osk_errcode_t mali_memory_core_session_begin(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue)
+{
+	memory_session * session_data;
+	_mali_osk_errcode_t err;
+	int i;
+	mali_io_address pd_mapped;
+
+	/* validate input */
+	if (NULL == slot)
+	{
+		MALI_DEBUG_PRINT(1, ("NULL slot given to memory session begin\n"));
+		MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}
+
+	if (NULL != *slot)
+	{
+		MALI_DEBUG_PRINT(1, ("The slot given to memory session begin already contains data"));
+		MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}
+
+	MALI_DEBUG_PRINT(2, ("MMU session begin\n"));
+
+	/* create the session data object */
+	session_data = _mali_osk_calloc(1, sizeof(memory_session));
+	MALI_CHECK_NON_NULL( session_data, _MALI_OSK_ERR_NOMEM );
+
+	/* create descriptor mapping table */
+	session_data->descriptor_mapping = mali_descriptor_mapping_create(MALI_MEM_DESCRIPTORS_INIT, MALI_MEM_DESCRIPTORS_MAX);
+
+	if (NULL == session_data->descriptor_mapping)
+	{
+		_mali_osk_free(session_data);
+		MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	err = mali_mmu_get_table_page(&session_data->page_directory, &pd_mapped);
+
+	session_data->page_directory_mapped = pd_mapped;
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		mali_descriptor_mapping_destroy(session_data->descriptor_mapping);
+		_mali_osk_free(session_data);
+		MALI_ERROR(err);
+	}
+	MALI_DEBUG_ASSERT_POINTER( session_data->page_directory_mapped );
+
+	MALI_DEBUG_PRINT(2, ("Page directory for session 0x%x placed at physical address 0x%08X\n", mali_session_data, session_data->page_directory));
+
+	for (i = 0; i < MALI_MMU_PAGE_SIZE/4; i++)
+	{
+		/* mark each page table as not present */
+		_mali_osk_mem_iowrite32(session_data->page_directory_mapped, sizeof(u32) * i, 0);
+	}
+
+	/* page_table_mapped[] is already set to NULL by _mali_osk_calloc call */
+
+	_MALI_OSK_INIT_LIST_HEAD(&session_data->active_mmus);
+	session_data->lock = _mali_osk_lock_init( _MALI_OSK_LOCKFLAG_ORDERED | _MALI_OSK_LOCKFLAG_ONELOCK | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE, 0, 128);
+	if (NULL == session_data->lock)
+	{
+		mali_mmu_release_table_page(session_data->page_directory);
+		mali_descriptor_mapping_destroy(session_data->descriptor_mapping);
+		_mali_osk_free(session_data);
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* Init the session's memory allocation list */
+	_MALI_OSK_INIT_LIST_HEAD( &session_data->memory_head );
+
+	*slot = session_data; /* slot will point to our data object */
+	MALI_DEBUG_PRINT(2, ("MMU session begin: success\n"));
+	MALI_SUCCESS;
+}
+
+static void descriptor_table_cleanup_callback(int descriptor_id, void* map_target)
+{
+	mali_memory_allocation * descriptor;
+
+	descriptor = (mali_memory_allocation*)map_target;
+
+	MALI_DEBUG_PRINT(1, ("Cleanup of descriptor %d mapping to 0x%x in descriptor table\n", descriptor_id, map_target));
+	MALI_DEBUG_ASSERT(descriptor);
+
+	mali_allocation_engine_release_memory(memory_engine, descriptor);
+	_mali_osk_free(descriptor);
+}
+
+static void mali_memory_core_session_end(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot)
+{
+	memory_session * session_data;
+	int i;
+	const int num_page_table_entries = sizeof(session_data->page_entries_mapped) / sizeof(session_data->page_entries_mapped[0]);
+
+	MALI_DEBUG_PRINT(2, ("MMU session end\n"));
+
+	/* validate input */
+	if (NULL == slot)
+	{
+		MALI_DEBUG_PRINT(1, ("NULL slot given to memory session begin\n"));
+		return;
+	}
+
+	session_data = (memory_session *)*slot;
+
+	if (NULL == session_data)
+	{
+		MALI_DEBUG_PRINT(1, ("No session data found during session end\n"));
+		return;
+	}
+	/* Lock the session so we can modify the memory list */
+	_mali_osk_lock_wait( session_data->lock, _MALI_OSK_LOCKMODE_RW );
+	/* Noninterruptable spinlock type, so must always have locked. Checking should've been done in OSK function. */
+
+#ifndef MALI_UKK_HAS_IMPLICIT_MMAP_CLEANUP
+#if _MALI_OSK_SPECIFIC_INDIRECT_MMAP
+#error Indirect MMAP specified, but UKK does not have implicit MMAP cleanup. Current implementation does not handle this.
+#else
+
+	/* Free all memory engine allocations */
+	if (0 == _mali_osk_list_empty(&session_data->memory_head))
+	{
+		mali_memory_allocation *descriptor;
+		mali_memory_allocation *temp;
+		_mali_uk_mem_munmap_s unmap_args;
+
+		MALI_DEBUG_PRINT(1, ("Memory found on session usage list during session termination\n"));
+
+		unmap_args.ctx = mali_session_data;
+
+		/* use the 'safe' list iterator, since freeing removes the active block from the list we're iterating */
+		_MALI_OSK_LIST_FOREACHENTRY(descriptor, temp, &session_data->memory_head, mali_memory_allocation, list)
+		{
+			MALI_DEBUG_PRINT(4, ("Freeing block with mali address 0x%x size %d mapped in user space at 0x%x\n",
+						descriptor->mali_address, descriptor->size, descriptor->size, descriptor->mapping)
+					);
+			/* ASSERT that the descriptor's lock references the correct thing */
+			MALI_DEBUG_ASSERT(  descriptor->lock == session_data->lock );
+			/* Therefore, we have already locked the descriptor */
+
+			unmap_args.size = descriptor->size;
+			unmap_args.mapping = descriptor->mapping;
+			unmap_args.cookie = (u32)descriptor;
+
+			/*
+			 * This removes the descriptor from the list, and frees the descriptor
+			 *
+			 * Does not handle the _MALI_OSK_SPECIFIC_INDIRECT_MMAP case, since
+			 * the only OS we are aware of that requires indirect MMAP also has
+			 * implicit mmap cleanup.
+			 */
+			_mali_ukk_mem_munmap_internal( &unmap_args );
+		}
+	}
+
+	/* Assert that we really did free everything */
+	MALI_DEBUG_ASSERT( _mali_osk_list_empty(&session_data->memory_head) );
+#endif /* _MALI_OSK_SPECIFIC_INDIRECT_MMAP */
+#endif /* MALI_UKK_HAS_IMPLICIT_MMAP_CLEANUP */
+
+	if (NULL != session_data->descriptor_mapping)
+	{
+		mali_descriptor_mapping_call_for_each(session_data->descriptor_mapping, descriptor_table_cleanup_callback);
+		mali_descriptor_mapping_destroy(session_data->descriptor_mapping);
+		session_data->descriptor_mapping = NULL;
+	}
+
+	for (i = 0; i < num_page_table_entries; i++)
+	{
+		/* free PTE memory */
+		if (session_data->page_directory_mapped && (_mali_osk_mem_ioread32(session_data->page_directory_mapped, sizeof(u32)*i) & MALI_MMU_FLAGS_PRESENT))
+		{
+			mali_mmu_release_table_page( _mali_osk_mem_ioread32(session_data->page_directory_mapped, i*sizeof(u32)) & ~MALI_MMU_FLAGS_MASK);
+			_mali_osk_mem_iowrite32(session_data->page_directory_mapped, i * sizeof(u32), 0);
+		}
+	}
+
+	if (MALI_INVALID_PAGE != session_data->page_directory)
+	{
+		mali_mmu_release_table_page(session_data->page_directory);
+		session_data->page_directory = MALI_INVALID_PAGE;
+	}
+
+	_mali_osk_lock_signal( session_data->lock, _MALI_OSK_LOCKMODE_RW );
+
+	/**
+	 * @note Could the VMA close handler mean that we use the session data after it was freed?
+	 * In which case, would need to refcount the session data, and free on VMA close
+	 */
+
+	/* Free the lock */
+	_mali_osk_lock_term( session_data->lock );
+	/* free the session data object */
+	_mali_osk_free(session_data);
+
+	/* clear our slot */
+	*slot = NULL;
+
+	return;
+}
+
+static _mali_osk_errcode_t mali_allocate_empty_page_directory(void)
+{
+	_mali_osk_errcode_t err;
+	mali_io_address mapping;
+
+	MALI_CHECK_NO_ERROR(mali_mmu_get_table_page(&mali_empty_page_directory, &mapping));
+
+	MALI_DEBUG_ASSERT_POINTER( mapping );
+
+	err = fill_page(mapping, 0);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		mali_mmu_release_table_page(mali_empty_page_directory);
+		mali_empty_page_directory = MALI_INVALID_PAGE;
+	}
+	return err;
+}
+
+static void mali_free_empty_page_directory(void)
+{
+	if (MALI_INVALID_PAGE != mali_empty_page_directory)
+	{
+		mali_mmu_release_table_page(mali_empty_page_directory);
+		mali_empty_page_directory = MALI_INVALID_PAGE;
+	}
+}
+
+static _mali_osk_errcode_t fill_page(mali_io_address mapping, u32 data)
+{
+	int i;
+	MALI_DEBUG_ASSERT_POINTER( mapping );
+
+	for(i = 0; i < MALI_MMU_PAGE_SIZE/4; i++)
+	{
+		_mali_osk_mem_iowrite32( mapping, i * sizeof(u32), data);
+	}
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_allocate_fault_flush_pages(void)
+{
+	_mali_osk_errcode_t err;
+
+	err = mali_mmu_get_table_page(&mali_page_fault_flush_data_page, &mali_page_fault_flush_data_page_mapping);
+	if (_MALI_OSK_ERR_OK == err)
+	{
+		err = mali_mmu_get_table_page(&mali_page_fault_flush_page_table, &mali_page_fault_flush_page_table_mapping);
+		if (_MALI_OSK_ERR_OK == err)
+		{
+			err = mali_mmu_get_table_page(&mali_page_fault_flush_page_directory, &mali_page_fault_flush_page_directory_mapping);
+			if (_MALI_OSK_ERR_OK == err)
+			{
+				fill_page(mali_page_fault_flush_data_page_mapping, 0);
+				fill_page(mali_page_fault_flush_page_table_mapping, mali_page_fault_flush_data_page | MALI_MMU_FLAGS_WRITE_PERMISSION | MALI_MMU_FLAGS_READ_PERMISSION | MALI_MMU_FLAGS_PRESENT);
+				fill_page(mali_page_fault_flush_page_directory_mapping, mali_page_fault_flush_page_table | MALI_MMU_FLAGS_PRESENT);
+				MALI_SUCCESS;
+			}
+			mali_mmu_release_table_page(mali_page_fault_flush_page_table);
+			mali_page_fault_flush_page_table = MALI_INVALID_PAGE;
+			mali_page_fault_flush_page_table_mapping = NULL;
+		}
+		mali_mmu_release_table_page(mali_page_fault_flush_data_page);
+		mali_page_fault_flush_data_page = MALI_INVALID_PAGE;
+		mali_page_fault_flush_data_page_mapping = NULL;
+	}
+	MALI_ERROR(err);
+}
+
+static void mali_free_fault_flush_pages(void)
+{
+	if (MALI_INVALID_PAGE != mali_page_fault_flush_page_directory)
+	{
+		mali_mmu_release_table_page(mali_page_fault_flush_page_directory);
+		mali_page_fault_flush_page_directory = MALI_INVALID_PAGE;
+	}
+
+	if (MALI_INVALID_PAGE != mali_page_fault_flush_page_table)
+	{
+		mali_mmu_release_table_page(mali_page_fault_flush_page_table);
+		mali_page_fault_flush_page_table = MALI_INVALID_PAGE;
+	}
+
+	if (MALI_INVALID_PAGE != mali_page_fault_flush_data_page)
+	{
+		mali_mmu_release_table_page(mali_page_fault_flush_data_page);
+		mali_page_fault_flush_data_page = MALI_INVALID_PAGE;
+	}
+}
+
+static _mali_osk_errcode_t mali_memory_core_load_complete(mali_kernel_subsystem_identifier id)
+{
+	mali_kernel_memory_mmu * mmu, * temp_mmu;
+
+	/* Report the allocators */
+	mali_allocation_engine_report_allocators( physical_memory_allocators );
+
+	/* allocate the helper pages */
+	MALI_CHECK_NO_ERROR( mali_allocate_empty_page_directory() );
+	if (_MALI_OSK_ERR_OK != mali_allocate_fault_flush_pages())
+	{
+		mali_free_empty_page_directory();
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* activate the empty page directory on all MMU's */
+	_MALI_OSK_LIST_FOREACHENTRY(mmu, temp_mmu, &mmu_head, mali_kernel_memory_mmu, list)
+	{
+		mali_mmu_register_write(mmu, MALI_MMU_REGISTER_DTE_ADDR, mali_empty_page_directory);
+		mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ENABLE_PAGING);
+	}
+
+	MALI_DEBUG_PRINT(4, ("MMUs activated\n"));
+	/* the MMU system is now active */
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_memory_core_system_info_fill(_mali_system_info* info)
+{
+	_mali_mem_info * mem_info;
+
+	/* make sure we won't leak any memory. It could also be that it's an uninitialized variable, but that would be a bug in the caller */
+	MALI_DEBUG_ASSERT(NULL == info->mem_info);
+
+	info->has_mmu = 1;
+
+	mem_info = _mali_osk_calloc(1,sizeof(_mali_mem_info));
+	MALI_CHECK_NON_NULL( mem_info, _MALI_OSK_ERR_NOMEM );
+
+	mem_info->size = 2048UL * 1024UL * 1024UL;
+	mem_info->maximum_order_supported = 30;
+	mem_info->flags = _MALI_CPU_WRITEABLE | _MALI_CPU_READABLE | _MALI_PP_READABLE | _MALI_PP_WRITEABLE |_MALI_GP_READABLE | _MALI_GP_WRITEABLE;
+	mem_info->identifier = 0;
+
+	info->mem_info = mem_info;
+
+	/* all OK */
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_memory_core_resource_mmu(_mali_osk_resource_t * resource)
+{
+	mali_kernel_memory_mmu * mmu;
+
+	MALI_DEBUG_PRINT(4, ("MMU '%s' @ (0x%08X - 0x%08X)\n",
+				resource->description, resource->base, resource->base + MALI_MMU_REGISTERS_SIZE - 1
+				));
+
+	if (NULL != mali_memory_core_mmu_lookup(resource->mmu_id))
+	{
+		MALI_DEBUG_PRINT(1, ("Duplicate MMU ids found. The id %d is already in use\n", resource->mmu_id));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	if (_MALI_OSK_ERR_OK != _mali_osk_mem_reqregion(resource->base, MALI_MMU_REGISTERS_SIZE, resource->description))
+	{
+		/* specified addresses are already in used by another driver / the kernel */
+		MALI_DEBUG_PRINT(
+				1, ("Failed to request MMU '%s' register address space at (0x%08X - 0x%08X)\n",
+					resource->description, resource->base, resource->base + MALI_MMU_REGISTERS_SIZE - 1
+				   ));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	mmu = _mali_osk_calloc(1, sizeof(mali_kernel_memory_mmu));
+
+	if (NULL == mmu)
+	{
+		MALI_DEBUG_PRINT(1, ("Failed to allocate memory for handling a MMU unit"));
+		_mali_osk_mem_unreqregion(resource->base, MALI_MMU_REGISTERS_SIZE);
+		MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	/* basic setup */
+	_MALI_OSK_INIT_LIST_HEAD(&mmu->list);
+
+	mmu->id = resource->mmu_id;
+	mmu->irq_nr = resource->irq;
+	mmu->flags = resource->flags;
+	mmu->base = resource->base;
+	mmu->mapping_size = MALI_MMU_REGISTERS_SIZE;
+	mmu->description = resource->description; /* no need to copy */
+	_MALI_OSK_INIT_LIST_HEAD(&mmu->callbacks);
+	_MALI_OSK_INIT_LIST_HEAD(&mmu->session_link);
+	mmu->in_page_fault_handler = 0;
+
+	mmu->lock = _mali_osk_lock_init( _MALI_OSK_LOCKFLAG_ORDERED | _MALI_OSK_LOCKFLAG_ONELOCK | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE, 0, 127-mmu->id);
+	if (NULL == mmu->lock)
+	{
+		MALI_DEBUG_PRINT(1, ("Failed to create mmu lock\n"));
+		_mali_osk_mem_unreqregion(mmu->base, mmu->mapping_size);
+		_mali_osk_free(mmu);
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* map the registers */
+	mmu->mapped_registers = _mali_osk_mem_mapioregion( mmu->base, mmu->mapping_size, mmu->description );
+	if (NULL == mmu->mapped_registers)
+	{
+		/* failed to map the registers */
+		MALI_DEBUG_PRINT(1, ("Failed to map MMU registers at 0x%08X\n", mmu->base));
+		_mali_osk_lock_term(mmu->lock);
+		_mali_osk_mem_unreqregion(mmu->base, MALI_MMU_REGISTERS_SIZE);
+		_mali_osk_free(mmu);
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	MALI_DEBUG_PRINT(4, ("MMU '%s' @ (0x%08X - 0x%08X) mapped to 0x%08X\n",
+				resource->description, resource->base, resource->base + MALI_MMU_REGISTERS_SIZE - 1,  mmu->mapped_registers
+				));
+
+	/* setup MMU interrupt mask */
+	/* set all values to known defaults */
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_SOFT_RESET);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_MASK, MALI_MMU_INTERRUPT_PAGE_FAULT | MALI_MMU_INTERRUPT_READ_BUS_ERROR);
+	/* setup MMU page directory pointer */
+	/* The mali_page_directory pointer is guaranteed to be 4kb aligned because we've used get_zeroed_page to accquire it */
+	/* convert the kernel virtual address into a physical address and set */
+
+	/* add to our list of MMU's */
+	_mali_osk_list_addtail(&mmu->list, &mmu_head);
+
+	mmu->irq = _mali_osk_irq_init(
+			mmu->irq_nr,
+			mali_kernel_memory_mmu_interrupt_handler_upper_half,
+			mali_kernel_memory_mmu_interrupt_handler_bottom_half,
+			(_mali_osk_irq_trigger_t)mali_mmu_probe_irq_trigger,
+			(_mali_osk_irq_ack_t)mali_mmu_probe_irq_acknowledge,
+			mmu,
+			"mali_mmu_irq_handlers"
+			);
+	if (NULL == mmu->irq)
+	{
+		_mali_osk_list_del(&mmu->list);
+		_mali_osk_lock_term(mmu->lock);
+		_mali_osk_mem_unmapioregion( mmu->base, mmu->mapping_size, mmu->mapped_registers );
+		_mali_osk_mem_unreqregion(resource->base, MALI_MMU_REGISTERS_SIZE);
+		_mali_osk_free(mmu);
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* set to a known state */
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_SOFT_RESET);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_MASK, MALI_MMU_INTERRUPT_PAGE_FAULT | MALI_MMU_INTERRUPT_READ_BUS_ERROR);
+
+	MALI_DEBUG_PRINT(2, ("MMU registered\n"));
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_memory_core_resource_fpga(_mali_osk_resource_t * resource)
+{
+	mali_io_address mapping;
+
+	MALI_DEBUG_PRINT(5, ("FPGA framework '%s' @ (0x%08X - 0x%08X)\n",
+				resource->description, resource->base, resource->base + sizeof(u32) * 2 - 1
+				));
+
+	mapping = _mali_osk_mem_mapioregion(resource->base + 0x1000, sizeof(u32) * 2, "fpga framework");
+	if (mapping)
+	{
+		MALI_DEBUG_CODE(u32 data = )
+			_mali_osk_mem_ioread32(mapping, 0);
+		MALI_DEBUG_PRINT(2, ("FPGA framwork '%s' @ 0x%08X:\n", resource->description, resource->base));
+		MALI_DEBUG_PRINT(2, ("\tBitfile date: %d%02d%02d_%02d%02d\n",
+					(data >> 20),
+					(data >> 16) & 0xF,
+					(data >> 11) & 0x1F,
+					(data >> 6)  & 0x1F,
+					(data >> 0)  & 0x3F));
+		MALI_DEBUG_CODE(data = )
+			_mali_osk_mem_ioread32(mapping, sizeof(u32));
+		MALI_DEBUG_PRINT(2, ("\tBitfile SCCS rev: %d\n", data));
+
+		_mali_osk_mem_unmapioregion(resource->base + 0x1000, sizeof(u32) *2, mapping);
+	}
+	else MALI_DEBUG_PRINT(1, ("Failed to access FPGA framwork '%s' @ 0x%08X\n", resource->description, resource->base));
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_memory_core_resource_os_memory(_mali_osk_resource_t * resource)
+{
+	mali_physical_memory_allocator * allocator;
+	mali_physical_memory_allocator ** next_allocator_list;
+
+	u32 alloc_order = resource->alloc_order;
+
+	allocator = mali_os_allocator_create(resource->size, resource->cpu_usage_adjust, resource->description);
+	if (NULL == allocator)
+	{
+		MALI_DEBUG_PRINT(1, ("Failed to create OS memory allocator\n"));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	allocator->alloc_order = alloc_order;
+
+	/* link in the allocator: insertion into ordered list
+	 * resources of the same alloc_order will be Last-in-first */
+	next_allocator_list = &physical_memory_allocators;
+
+	while ( NULL != *next_allocator_list &&
+			(*next_allocator_list)->alloc_order < alloc_order )
+	{
+		next_allocator_list = &((*next_allocator_list)->next);
+	}
+
+	allocator->next = (*next_allocator_list);
+	(*next_allocator_list) = allocator;
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_memory_core_resource_dedicated_memory(_mali_osk_resource_t * resource)
+{
+	mali_physical_memory_allocator * allocator;
+	mali_physical_memory_allocator ** next_allocator_list;
+	dedicated_memory_info * cleanup_data;
+
+	u32 alloc_order = resource->alloc_order;
+
+	/* do the lowlevel linux operation first */
+
+	/* Request ownership of the memory */
+	if (_MALI_OSK_ERR_OK != _mali_osk_mem_reqregion(resource->base, resource->size, resource->description))
+	{
+		MALI_DEBUG_PRINT(1, ("Failed to request memory region %s (0x%08X - 0x%08X)\n", resource->description, resource->base, resource->base + resource->size - 1));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* create generic block allocator object to handle it */
+	allocator = mali_block_allocator_create(resource->base, resource->cpu_usage_adjust, resource->size, resource->description );
+
+	if (NULL == allocator)
+	{
+		MALI_DEBUG_PRINT(1, ("Memory bank registration failed\n"));
+		_mali_osk_mem_unreqregion(resource->base, resource->size);
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* save lowlevel cleanup info */
+	allocator->alloc_order = alloc_order;
+
+	cleanup_data = _mali_osk_malloc(sizeof(dedicated_memory_info));
+
+	if (NULL == cleanup_data)
+	{
+		_mali_osk_mem_unreqregion(resource->base, resource->size);
+		allocator->destroy(allocator);
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	cleanup_data->base = resource->base;
+	cleanup_data->size = resource->size;
+
+	cleanup_data->next = mem_region_registrations;
+	mem_region_registrations = cleanup_data;
+
+	/* link in the allocator: insertion into ordered list
+	 * resources of the same alloc_order will be Last-in-first */
+	next_allocator_list = &physical_memory_allocators;
+
+	while ( NULL != *next_allocator_list &&
+			(*next_allocator_list)->alloc_order < alloc_order )
+	{
+		next_allocator_list = &((*next_allocator_list)->next);
+	}
+
+	allocator->next = (*next_allocator_list);
+	(*next_allocator_list) = allocator;
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t mali_kernel_memory_mmu_interrupt_handler_upper_half(void * data)
+{
+	mali_kernel_memory_mmu * mmu;
+	u32 int_stat;
+
+	if (mali_benchmark) MALI_SUCCESS;
+
+	mmu = (mali_kernel_memory_mmu *)data;
+
+	MALI_DEBUG_ASSERT_POINTER(mmu);
+
+	/* check if it was our device which caused the interrupt (we could be sharing the IRQ line) */
+	int_stat = mali_mmu_register_read(mmu, MALI_MMU_REGISTER_INT_STATUS);
+	if (0 == int_stat)
+	{
+		MALI_DEBUG_PRINT(5, ("Ignoring shared interrupt\n"));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT); /* no bits set, we are sharing the IRQ line and someone else caused the interrupt */
+	}
+
+	MALI_DEBUG_PRINT(1, ("mali_kernel_memory_mmu_interrupt_handler_upper_half\n"));
+
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_MASK, 0);
+
+	mali_mmu_register_read(mmu, MALI_MMU_REGISTER_STATUS);
+
+	if (int_stat & MALI_MMU_INTERRUPT_PAGE_FAULT)
+	{
+		MALI_PRINT(("Page fault on %s\n", mmu->description));
+
+		_mali_osk_irq_schedulework(mmu->irq);
+	}
+	if (int_stat & MALI_MMU_INTERRUPT_READ_BUS_ERROR)
+	{
+		MALI_PRINT(("Bus read error on %s\n", mmu->description));
+		/* clear interrupt flag */
+		mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_CLEAR, MALI_MMU_INTERRUPT_READ_BUS_ERROR);
+		/* reenable it */
+		mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_MASK, mali_mmu_register_read(mmu, MALI_MMU_REGISTER_INT_MASK) | MALI_MMU_INTERRUPT_READ_BUS_ERROR);
+	}
+
+	MALI_SUCCESS;
+}
+
+
+static void mali_kernel_mmu_bus_reset(mali_kernel_memory_mmu * mmu)
+{
+
+#if defined(USING_MALI200)
+	int i;
+	const int replay_buffer_check_interval = 10; /* must be below 1000 */
+	const int replay_buffer_max_number_of_checks = 100;
+#endif
+
+	_mali_osk_lock_wait(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+	/* add an extra reference while handling the page fault */
+	mmu->usage_count++;
+	_mali_osk_lock_signal(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+
+	MALI_DEBUG_PRINT(4, ("Sending stop bus request to cores\n"));
+	/* request to stop the bus, but don't wait for it to actually stop */
+	_mali_kernel_core_broadcast_subsystem_message(MMU_KILL_STEP1_STOP_BUS_FOR_ALL_CORES, (u32)mmu);
+
+#if defined(USING_MALI200)
+	/* no new request will come from any of the connected cores from now
+	 * we must now flush the playback buffer for any requests queued already
+	 */
+	MALI_DEBUG_PRINT(4, ("Switching to the special page fault flush page directory\n"));
+	/* don't use the mali_mmu_activate_address_space function here as we can't stall the MMU */
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_DTE_ADDR, mali_page_fault_flush_page_directory);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ZAP_CACHE);
+	/* resume the MMU */
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_CLEAR, MALI_MMU_INTERRUPT_PAGE_FAULT);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_PAGE_FAULT_DONE);
+	/* the MMU will now play back all the requests, all going to our special page fault flush data page */
+
+	/* just to be safe, check that the playback buffer is empty before continuing */
+	if (!mali_benchmark) {
+		for (i = 0; i < replay_buffer_max_number_of_checks; i++)
+		{
+			if (mali_mmu_register_read(mmu, MALI_MMU_REGISTER_STATUS) & MALI_MMU_STATUS_BIT_REPLAY_BUFFER_EMPTY) break;
+			_mali_osk_time_ubusydelay(replay_buffer_check_interval);
+		}
+
+		MALI_DEBUG_PRINT_IF(1, i == replay_buffer_max_number_of_checks, ("MMU: %s: Failed to flush replay buffer on page fault\n", mmu->description));
+		MALI_DEBUG_PRINT(1, ("Replay playback took %ld usec\n", i * replay_buffer_check_interval));
+	}
+#endif
+	/* notify all subsystems that the core should be reset once the bus is actually stopped */
+	MALI_DEBUG_PRINT(4,("Sending job abort command to subsystems\n"));
+	_mali_kernel_core_broadcast_subsystem_message(MMU_KILL_STEP2_RESET_ALL_CORES_AND_ABORT_THEIR_JOBS, (u32)mmu);
+
+	/* reprogram the MMU */
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_SOFT_RESET);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_MASK, MALI_MMU_INTERRUPT_PAGE_FAULT | MALI_MMU_INTERRUPT_READ_BUS_ERROR);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_DTE_ADDR, mali_empty_page_directory); /* no session is active, so just activate the empty page directory */
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ENABLE_PAGING);
+
+	/* release the extra address space reference, will schedule */
+	mali_memory_core_mmu_release_address_space_reference(mmu);
+
+	/* resume normal operation */
+	_mali_kernel_core_broadcast_subsystem_message(MMU_KILL_STEP3_CONTINUE_JOB_HANDLING, (u32)mmu);
+	MALI_DEBUG_PRINT(4, ("Page fault handling complete\n"));
+}
+
+void mali_kernel_mmu_reset(void * input_mmu)
+{
+	mali_kernel_memory_mmu * mmu;
+	MALI_DEBUG_ASSERT_POINTER(input_mmu);
+	mmu = (mali_kernel_memory_mmu *)input_mmu;
+
+	MALI_DEBUG_PRINT(4, ("Mali MMU: mali_kernel_mmu_reset: %s\n", mmu->description));
+
+	if ( 0 != mmu->in_page_fault_handler)
+	{
+		/* This is possible if the bus can never be stopped for some reason */
+		MALI_PRINT_ERROR(("Stopping the Memory bus not possible. Mali reset could not be performed."));
+		return;
+	}
+	_mali_osk_lock_wait(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_SOFT_RESET);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_MASK, MALI_MMU_INTERRUPT_PAGE_FAULT | MALI_MMU_INTERRUPT_READ_BUS_ERROR);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_DTE_ADDR, mali_empty_page_directory); /* no session is active, so just activate the empty page directory */
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ENABLE_PAGING);
+	_mali_osk_lock_signal(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+
+}
+
+void mali_kernel_mmu_force_bus_reset(void * input_mmu)
+{
+	mali_kernel_memory_mmu * mmu;
+	MALI_DEBUG_ASSERT_POINTER(input_mmu);
+	mmu = (mali_kernel_memory_mmu *)input_mmu;
+	if ( 0 != mmu->in_page_fault_handler)
+	{
+		/* This is possible if the bus can never be stopped for some reason */
+		MALI_PRINT_ERROR(("Stopping the Memory bus not possible. Mali reset could not be performed."));
+		return;
+	}
+	MALI_DEBUG_PRINT(1, ("Mali MMU: Force_bus_reset.\n"));
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_MASK, 0);
+	mali_kernel_mmu_bus_reset(mmu);
+}
+
+
+static void mali_kernel_memory_mmu_interrupt_handler_bottom_half(void * data)
+{
+	mali_kernel_memory_mmu * mmu;
+	u32 raw, fault_address, status;
+
+	if (NULL == data)
+	{
+		MALI_PRINT_ERROR(("MMU IRQ work queue: NULL argument"));
+		return; /* Error */
+	}
+	mmu = (mali_kernel_memory_mmu*)data;
+
+
+	MALI_DEBUG_PRINT(4, ("Locking subsystems\n"));
+	/* lock all subsystems */
+	_mali_kernel_core_broadcast_subsystem_message(MMU_KILL_STEP0_LOCK_SUBSYSTEM, (u32)mmu);
+
+	raw = mali_mmu_register_read(mmu, MALI_MMU_REGISTER_INT_RAWSTAT);
+	status = mali_mmu_register_read(mmu, MALI_MMU_REGISTER_STATUS);
+
+	if ( (0==(raw & MALI_MMU_INTERRUPT_PAGE_FAULT)) &&  (0==(status & MALI_MMU_STATUS_BIT_PAGE_FAULT_ACTIVE)) )
+	{
+		MALI_DEBUG_PRINT(1, ("MMU: Page fault bottom half: No Irq found.\n"));
+		MALI_DEBUG_PRINT(4, ("Unlocking subsystems"));
+		_mali_kernel_core_broadcast_subsystem_message(MMU_KILL_STEP4_UNLOCK_SUBSYSTEM, (u32)mmu);
+		return;
+	}
+
+	mmu->in_page_fault_handler = 1;
+
+	fault_address = mali_mmu_register_read(mmu, MALI_MMU_REGISTER_PAGE_FAULT_ADDR);
+	MALI_PRINT(("Page fault detected at 0x%x from bus id %d of type %s on %s\n",
+				(void*)fault_address,
+				(status >> 6) & 0x1F,
+				(status & 32) ? "write" : "read",
+				mmu->description)
+			  );
+
+	if (NULL == mmu->active_session)
+	{
+		MALI_PRINT(("Spurious memory access detected from MMU %s\n", mmu->description));
+	}
+	else
+	{
+		MALI_PRINT(("Active page directory at 0x%08X\n", mmu->active_session->page_directory));
+		MALI_PRINT(("Info from page table for VA 0x%x:\n", (void*)fault_address));
+		MALI_PRINT(("DTE entry: PTE at 0x%x marked as %s\n",
+					(void*)(_mali_osk_mem_ioread32(mmu->active_session->page_directory_mapped,
+					MALI_MMU_PDE_ENTRY(fault_address) * sizeof(u32)) & ~MALI_MMU_FLAGS_MASK),
+					_mali_osk_mem_ioread32(mmu->active_session->page_directory_mapped,
+					MALI_MMU_PDE_ENTRY(fault_address) * sizeof(u32)) & MALI_MMU_FLAGS_PRESENT ? "present" : "not present"
+				   ));
+
+		if (_mali_osk_mem_ioread32(mmu->active_session->page_directory_mapped, MALI_MMU_PDE_ENTRY(fault_address) * sizeof(u32)) & MALI_MMU_FLAGS_PRESENT)
+		{
+			mali_io_address pte;
+			u32 data;
+			pte = mmu->active_session->page_entries_mapped[MALI_MMU_PDE_ENTRY(fault_address)];
+			data = _mali_osk_mem_ioread32(pte, MALI_MMU_PTE_ENTRY(fault_address) * sizeof(u32));
+			MALI_PRINT(("PTE entry: Page at 0x%x, %s %s %s\n",
+						(void*)(data & ~MALI_MMU_FLAGS_MASK),
+						data & MALI_MMU_FLAGS_PRESENT ? "present" : "not present",
+						data & MALI_MMU_FLAGS_READ_PERMISSION ? "readable" : "",
+						data & MALI_MMU_FLAGS_WRITE_PERMISSION ? "writable" : ""
+					   ));
+		}
+		else
+		{
+			MALI_PRINT(("PTE entry: Not present\n"));
+		}
+	}
+
+
+	mali_kernel_mmu_bus_reset(mmu);
+
+	mmu->in_page_fault_handler = 0;
+
+	/* unlock all subsystems */
+	MALI_DEBUG_PRINT(4, ("Unlocking subsystems"));
+	_mali_kernel_core_broadcast_subsystem_message(MMU_KILL_STEP4_UNLOCK_SUBSYSTEM, (u32)mmu);
+
+}
+
+
+static u32 mali_mmu_register_read(mali_kernel_memory_mmu * unit, mali_mmu_register reg)
+{
+	u32 val;
+
+	if (mali_benchmark) return 0;
+
+	val = _mali_osk_mem_ioread32(unit->mapped_registers, (u32)reg * sizeof(u32));
+
+	MALI_DEBUG_PRINT(6, ("mali_mmu_register_read addr:0x%04X val:0x%08x\n", (u32)reg * sizeof(u32),val));
+
+	return val;
+}
+
+static void mali_mmu_register_write(mali_kernel_memory_mmu * unit, mali_mmu_register reg, u32 val)
+{
+	if (mali_benchmark) return;
+
+	MALI_DEBUG_PRINT(6, ("mali_mmu_register_write  addr:0x%04X val:0x%08x\n", (u32)reg * sizeof(u32), val));
+
+	_mali_osk_mem_iowrite32(unit->mapped_registers, (u32)reg * sizeof(u32), val);
+}
+
+
+#if MALI_USE_UNIFIED_MEMORY_PROVIDER != 0
+static mali_physical_memory_allocation_result ump_memory_commit(void* ctx, mali_allocation_engine * engine, mali_memory_allocation * descriptor, u32* offset, mali_physical_memory_allocation * alloc_info)
+{
+	ump_dd_handle ump_mem;
+	u32 nr_blocks;
+	u32 i;
+	ump_dd_physical_block * ump_blocks;
+	ump_mem_allocation *ret_allocation;
+
+    MALI_DEBUG_ASSERT_POINTER(ctx);
+    MALI_DEBUG_ASSERT_POINTER(engine);
+    MALI_DEBUG_ASSERT_POINTER(descriptor);
+    MALI_DEBUG_ASSERT_POINTER(alloc_info);
+
+	ret_allocation = _mali_osk_malloc( sizeof( ump_mem_allocation ) );
+	if ( NULL==ret_allocation ) return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+
+	ump_mem = (ump_dd_handle)ctx;
+
+	MALI_DEBUG_PRINT(4, ("In ump_memory_commit\n"));
+
+	nr_blocks = ump_dd_phys_block_count_get(ump_mem);
+
+	MALI_DEBUG_PRINT(4, ("Have %d blocks\n", nr_blocks));
+
+	if (nr_blocks == 0)
+	{
+		MALI_DEBUG_PRINT(1, ("No block count\n"));
+		_mali_osk_free( ret_allocation );
+		return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+	}
+
+	ump_blocks = _mali_osk_malloc(sizeof(*ump_blocks)*nr_blocks );
+	if ( NULL==ump_blocks )
+	{
+		_mali_osk_free( ret_allocation );
+		return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+	}
+
+	if (UMP_DD_INVALID == ump_dd_phys_blocks_get(ump_mem, ump_blocks, nr_blocks))
+	{
+		_mali_osk_free(ump_blocks);
+		_mali_osk_free( ret_allocation );
+		return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+	}
+
+	/* Store away the initial offset for unmapping purposes */
+	ret_allocation->initial_offset = *offset;
+
+	for(i=0; i<nr_blocks; ++i)
+	{
+		MALI_DEBUG_PRINT(4, ("Mapping in 0x%08x size %d\n", ump_blocks[i].addr , ump_blocks[i].size));
+		if (_MALI_OSK_ERR_OK != mali_allocation_engine_map_physical(engine, descriptor, *offset, ump_blocks[i].addr , 0, ump_blocks[i].size ))
+		{
+			u32 size_allocated = *offset - ret_allocation->initial_offset;
+			MALI_DEBUG_PRINT(1, ("Mapping of external memory failed\n"));
+
+			/* unmap all previous blocks (if any) */
+			mali_allocation_engine_unmap_physical(engine, descriptor, ret_allocation->initial_offset, size_allocated, (_mali_osk_mem_mapregion_flags_t)0 );
+
+			_mali_osk_free(ump_blocks);
+			_mali_osk_free(ret_allocation);
+			return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+		}
+		*offset += ump_blocks[i].size;
+	}
+
+	if (descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_GUARD_PAGE)
+	{
+		/* Map in an extra virtual guard page at the end of the VMA */
+		MALI_DEBUG_PRINT(4, ("Mapping in extra guard page\n"));
+		if (_MALI_OSK_ERR_OK != mali_allocation_engine_map_physical(engine, descriptor, *offset, ump_blocks[0].addr , 0, _MALI_OSK_MALI_PAGE_SIZE ))
+		{
+			u32 size_allocated = *offset - ret_allocation->initial_offset;
+			MALI_DEBUG_PRINT(1, ("Mapping of external memory (guard page) failed\n"));
+
+			/* unmap all previous blocks (if any) */
+			mali_allocation_engine_unmap_physical(engine, descriptor, ret_allocation->initial_offset, size_allocated, (_mali_osk_mem_mapregion_flags_t)0 );
+
+			_mali_osk_free(ump_blocks);
+			_mali_osk_free(ret_allocation);
+			return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+		}
+		*offset += _MALI_OSK_MALI_PAGE_SIZE;
+	}
+
+	_mali_osk_free( ump_blocks );
+
+	ret_allocation->engine = engine;
+	ret_allocation->descriptor = descriptor;
+	ret_allocation->ump_mem = ump_mem;
+	ret_allocation->size_allocated = *offset - ret_allocation->initial_offset;
+
+	alloc_info->ctx = NULL;
+	alloc_info->handle = ret_allocation;
+	alloc_info->next = NULL;
+	alloc_info->release = ump_memory_release;
+
+	return MALI_MEM_ALLOC_FINISHED;
+}
+
+static void ump_memory_release(void * ctx, void * handle)
+{
+	ump_dd_handle ump_mem;
+	ump_mem_allocation *allocation;
+
+	allocation = (ump_mem_allocation *)handle;
+
+	MALI_DEBUG_ASSERT_POINTER( allocation );
+
+	ump_mem = allocation->ump_mem;
+
+	MALI_DEBUG_ASSERT(UMP_DD_HANDLE_INVALID!=ump_mem);
+
+	/* At present, this is a no-op. But, it allows the mali_address_manager to
+	 * do unmapping of a subrange in future. */
+	mali_allocation_engine_unmap_physical( allocation->engine,
+										   allocation->descriptor,
+										   allocation->initial_offset,
+										   allocation->size_allocated,
+										   (_mali_osk_mem_mapregion_flags_t)0
+										   );
+	_mali_osk_free( allocation );
+
+
+	ump_dd_reference_release(ump_mem) ;
+	return;
+}
+
+_mali_osk_errcode_t _mali_ukk_attach_ump_mem( _mali_uk_attach_ump_mem_s *args )
+{
+	ump_dd_handle ump_mem;
+	mali_physical_memory_allocator external_memory_allocator;
+	memory_session * session_data;
+	mali_memory_allocation * descriptor;
+	int md;
+
+  	MALI_DEBUG_ASSERT_POINTER(args);
+  	MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+
+	session_data = (memory_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_memory_id);
+	MALI_CHECK_NON_NULL(session_data, _MALI_OSK_ERR_INVALID_ARGS);
+
+	/* check arguments */
+	/* NULL might be a valid Mali address */
+	if ( ! args->size) MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+
+	/* size must be a multiple of the system page size */
+	if ( args->size % _MALI_OSK_MALI_PAGE_SIZE ) MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+
+	MALI_DEBUG_PRINT(3,
+	                 ("Requested to map ump memory with secure id %d into virtual memory 0x%08X, size 0x%08X\n",
+	                  args->secure_id, args->mali_address, args->size));
+
+	ump_mem = ump_dd_handle_create_from_secure_id( (int)args->secure_id ) ;
+
+	if ( UMP_DD_HANDLE_INVALID==ump_mem ) MALI_ERROR(_MALI_OSK_ERR_FAULT);
+
+	descriptor = _mali_osk_calloc(1, sizeof(mali_memory_allocation));
+	if (NULL == descriptor)
+	{
+		ump_dd_reference_release(ump_mem);
+		MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	descriptor->size = args->size;
+	descriptor->mapping = NULL;
+	descriptor->mali_address = args->mali_address;
+	descriptor->mali_addr_mapping_info = (void*)session_data;
+	descriptor->process_addr_mapping_info = NULL; /* do not map to process address space */
+	if (args->flags & _MALI_MAP_EXTERNAL_MAP_GUARD_PAGE)
+	{
+		descriptor->flags = MALI_MEMORY_ALLOCATION_FLAG_MAP_GUARD_PAGE;
+	}
+	_mali_osk_list_init( &descriptor->list );
+
+	if (_MALI_OSK_ERR_OK != mali_descriptor_mapping_allocate_mapping(session_data->descriptor_mapping, descriptor, &md))
+	{
+		ump_dd_reference_release(ump_mem);
+		_mali_osk_free(descriptor);
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	external_memory_allocator.allocate = ump_memory_commit;
+	external_memory_allocator.allocate_page_table_block = NULL;
+	external_memory_allocator.ctx = ump_mem;
+	external_memory_allocator.name = "UMP Memory";
+	external_memory_allocator.next = NULL;
+
+	if (_MALI_OSK_ERR_OK != mali_allocation_engine_allocate_memory(memory_engine, descriptor, &external_memory_allocator, NULL))
+	{
+		mali_descriptor_mapping_free(session_data->descriptor_mapping, md);
+		ump_dd_reference_release(ump_mem);
+		_mali_osk_free(descriptor);
+		MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	args->cookie = md;
+
+	MALI_DEBUG_PRINT(5,("Returning from UMP attach\n"));
+
+	/* All OK */
+	MALI_SUCCESS;
+}
+
+
+_mali_osk_errcode_t _mali_ukk_release_ump_mem( _mali_uk_release_ump_mem_s *args )
+{
+	mali_memory_allocation * descriptor;
+    memory_session * session_data;
+
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+
+    session_data = (memory_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_memory_id);
+    MALI_CHECK_NON_NULL(session_data, _MALI_OSK_ERR_INVALID_ARGS);
+
+    if (_MALI_OSK_ERR_OK != mali_descriptor_mapping_get(session_data->descriptor_mapping, args->cookie, (void**)&descriptor))
+	{
+		MALI_DEBUG_PRINT(1, ("Invalid memory descriptor %d used to release ump memory\n", args->cookie));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	mali_descriptor_mapping_free(session_data->descriptor_mapping, args->cookie);
+	mali_allocation_engine_release_memory(memory_engine, descriptor);
+	_mali_osk_free(descriptor);
+
+	MALI_SUCCESS;
+
+}
+#endif /* MALI_USE_UNIFIED_MEMORY_PROVIDER != 0 */
+
+
+static mali_physical_memory_allocation_result external_memory_commit(void* ctx, mali_allocation_engine * engine, mali_memory_allocation * descriptor, u32* offset, mali_physical_memory_allocation * alloc_info)
+{
+	u32 * data;
+	external_mem_allocation * ret_allocation;
+
+    MALI_DEBUG_ASSERT_POINTER(ctx);
+    MALI_DEBUG_ASSERT_POINTER(engine);
+    MALI_DEBUG_ASSERT_POINTER(descriptor);
+    MALI_DEBUG_ASSERT_POINTER(alloc_info);
+
+	ret_allocation = _mali_osk_malloc( sizeof(external_mem_allocation) );
+
+	if ( NULL == ret_allocation )
+	{
+		return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+	}
+
+	data = (u32*)ctx;
+
+	ret_allocation->engine = engine;
+	ret_allocation->descriptor = descriptor;
+	ret_allocation->initial_offset = *offset;
+
+	alloc_info->ctx = NULL;
+	alloc_info->handle = ret_allocation;
+	alloc_info->next = NULL;
+	alloc_info->release = external_memory_release;
+
+	MALI_DEBUG_PRINT(3, ("External map: mapping phys 0x%08X at mali virtual address 0x%08X staring at offset 0x%08X length 0x%08X\n", data[0], descriptor->mali_address, *offset, data[1]));
+
+	if (_MALI_OSK_ERR_OK != mali_allocation_engine_map_physical(engine, descriptor, *offset, data[0], 0, data[1]))
+	{
+		MALI_DEBUG_PRINT(1, ("Mapping of external memory failed\n"));
+		_mali_osk_free(ret_allocation);
+		return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+	}
+	*offset += data[1];
+
+	if (descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_GUARD_PAGE)
+	{
+		/* Map in an extra virtual guard page at the end of the VMA */
+		MALI_DEBUG_PRINT(4, ("Mapping in extra guard page\n"));
+		if (_MALI_OSK_ERR_OK != mali_allocation_engine_map_physical(engine, descriptor, *offset, data[0], 0, _MALI_OSK_MALI_PAGE_SIZE))
+		{
+			u32 size_allocated = *offset - ret_allocation->initial_offset;
+			MALI_DEBUG_PRINT(1, ("Mapping of external memory (guard page) failed\n"));
+
+			/* unmap what we previously mapped */
+			mali_allocation_engine_unmap_physical(engine, descriptor, ret_allocation->initial_offset, size_allocated, (_mali_osk_mem_mapregion_flags_t)0 );
+			_mali_osk_free(ret_allocation);
+			return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+		}
+		*offset += _MALI_OSK_MALI_PAGE_SIZE;
+	}
+
+	ret_allocation->size = *offset - ret_allocation->initial_offset;
+
+	return MALI_MEM_ALLOC_FINISHED;
+}
+
+static void external_memory_release(void * ctx, void * handle)
+{
+	external_mem_allocation * allocation;
+
+	allocation = (external_mem_allocation *) handle;
+	MALI_DEBUG_ASSERT_POINTER( allocation );
+
+	/* At present, this is a no-op. But, it allows the mali_address_manager to
+	 * do unmapping of a subrange in future. */
+
+	mali_allocation_engine_unmap_physical( allocation->engine,
+										   allocation->descriptor,
+										   allocation->initial_offset,
+										   allocation->size,
+										   (_mali_osk_mem_mapregion_flags_t)0
+										   );
+
+	_mali_osk_free( allocation );
+
+	return;
+}
+
+_mali_osk_errcode_t _mali_ukk_map_external_mem( _mali_uk_map_external_mem_s *args )
+{
+	mali_physical_memory_allocator external_memory_allocator;
+    memory_session * session_data;
+    u32 info[2];
+	mali_memory_allocation * descriptor;
+	int md;
+
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+
+    session_data = (memory_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_memory_id);
+    MALI_CHECK_NON_NULL(session_data, _MALI_OSK_ERR_INVALID_ARGS);
+
+    external_memory_allocator.allocate = external_memory_commit;
+	external_memory_allocator.allocate_page_table_block = NULL;
+	external_memory_allocator.ctx = &info[0];
+	external_memory_allocator.name = "External Memory";
+	external_memory_allocator.next = NULL;
+
+	/* check arguments */
+	/* NULL might be a valid Mali address */
+	if ( ! args->size) MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+
+	/* size must be a multiple of the system page size */
+	if ( args->size % _MALI_OSK_MALI_PAGE_SIZE ) MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+
+	MALI_DEBUG_PRINT(3,
+	        ("Requested to map physical memory 0x%x-0x%x into virtual memory 0x%x\n",
+	        (void*)args->phys_addr,
+	        (void*)(args->phys_addr + args->size -1),
+	        (void*)args->mali_address)
+	);
+
+	/* Validate the mali physical range */
+	MALI_CHECK_NO_ERROR( mali_kernel_core_validate_mali_phys_range( args->phys_addr, args->size ) );
+
+	info[0] = args->phys_addr;
+	info[1] = args->size;
+
+	descriptor = _mali_osk_calloc(1, sizeof(mali_memory_allocation));
+	if (NULL == descriptor) MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+
+	descriptor->size = args->size;
+	descriptor->mapping = NULL;
+	descriptor->mali_address = args->mali_address;
+	descriptor->mali_addr_mapping_info = (void*)session_data;
+	descriptor->process_addr_mapping_info = NULL; /* do not map to process address space */
+	descriptor->lock = NULL;
+	if (args->flags & _MALI_MAP_EXTERNAL_MAP_GUARD_PAGE)
+	{
+		descriptor->flags = MALI_MEMORY_ALLOCATION_FLAG_MAP_GUARD_PAGE;
+	}
+	_mali_osk_list_init( &descriptor->list );
+
+	if (_MALI_OSK_ERR_OK != mali_descriptor_mapping_allocate_mapping(session_data->descriptor_mapping, descriptor, &md))
+	{
+		_mali_osk_free(descriptor);
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	if (_MALI_OSK_ERR_OK != mali_allocation_engine_allocate_memory(memory_engine, descriptor, &external_memory_allocator, NULL))
+	{
+		mali_descriptor_mapping_free(session_data->descriptor_mapping, md);
+		_mali_osk_free(descriptor);
+		MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+	args->cookie = md;
+
+	MALI_DEBUG_PRINT(5,("Returning from range_map_external_memory\n"));
+
+	/* All OK */
+	MALI_SUCCESS;
+}
+
+
+_mali_osk_errcode_t _mali_ukk_unmap_external_mem( _mali_uk_unmap_external_mem_s *args )
+{
+	mali_memory_allocation * descriptor;
+    memory_session * session_data;
+
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+
+    session_data = (memory_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_memory_id);
+    MALI_CHECK_NON_NULL(session_data, _MALI_OSK_ERR_INVALID_ARGS);
+
+    if (_MALI_OSK_ERR_OK != mali_descriptor_mapping_get(session_data->descriptor_mapping, args->cookie, (void**)&descriptor))
+	{
+		MALI_DEBUG_PRINT(1, ("Invalid memory descriptor %d used to unmap external memory\n", args->cookie));
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	mali_descriptor_mapping_free(session_data->descriptor_mapping, args->cookie);
+	mali_allocation_engine_release_memory(memory_engine, descriptor);
+	_mali_osk_free(descriptor);
+
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t _mali_ukk_init_mem( _mali_uk_init_mem_s *args )
+{
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+
+	args->memory_size = 2 * 1024 * 1024 * 1024UL; /* 2GB address space */
+	args->mali_address_base = 1 * 1024 * 1024 * 1024UL; /* staring at 1GB, causing this layout: (0-1GB unused)(1GB-3G usage by Mali)(3G-4G unused) */
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t _mali_ukk_term_mem( _mali_uk_term_mem_s *args )
+{
+    MALI_DEBUG_ASSERT_POINTER(args);
+    MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+    MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_mmu_page_table_cache_create(void)
+{
+    page_table_cache.lock = _mali_osk_lock_init( _MALI_OSK_LOCKFLAG_ORDERED | _MALI_OSK_LOCKFLAG_ONELOCK | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE, 0, 110);
+    MALI_CHECK_NON_NULL( page_table_cache.lock, _MALI_OSK_ERR_FAULT );
+	_MALI_OSK_INIT_LIST_HEAD(&page_table_cache.partial);
+	_MALI_OSK_INIT_LIST_HEAD(&page_table_cache.full);
+    MALI_SUCCESS;
+}
+
+void mali_mmu_page_table_cache_destroy(void)
+{
+	mali_mmu_page_table_allocation * alloc, *temp;
+
+	_MALI_OSK_LIST_FOREACHENTRY(alloc, temp, &page_table_cache.partial, mali_mmu_page_table_allocation, list)
+	{
+		MALI_DEBUG_PRINT_IF(1, 0 != alloc->usage_count, ("Destroying page table cache while pages are tagged as in use. %d allocations still marked as in use.\n", alloc->usage_count));
+		_mali_osk_list_del(&alloc->list);
+		alloc->pages.release(&alloc->pages);
+		_mali_osk_free(alloc->usage_map);
+		_mali_osk_free(alloc);
+	}
+
+	MALI_DEBUG_PRINT_IF(1, 0 == _mali_osk_list_empty(&page_table_cache.full), ("Page table cache full list contains one or more elements \n"));
+
+	_MALI_OSK_LIST_FOREACHENTRY(alloc, temp, &page_table_cache.full, mali_mmu_page_table_allocation, list)
+	{
+		MALI_DEBUG_PRINT(1, ("Destroy alloc 0x%08X with usage count %d\n", (u32)alloc, alloc->usage_count));
+		_mali_osk_list_del(&alloc->list);
+		alloc->pages.release(&alloc->pages);
+		_mali_osk_free(alloc->usage_map);
+		_mali_osk_free(alloc);
+	}
+}
+
+_mali_osk_errcode_t mali_mmu_get_table_page(u32 *table_page, mali_io_address *mapping)
+{
+	_mali_osk_lock_wait(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+
+	if (0 == _mali_osk_list_empty(&page_table_cache.partial))
+	{
+		mali_mmu_page_table_allocation * alloc = _MALI_OSK_LIST_ENTRY(page_table_cache.partial.next, mali_mmu_page_table_allocation, list);
+		int page_number = _mali_osk_find_first_zero_bit(alloc->usage_map, alloc->num_pages);
+		MALI_DEBUG_PRINT(6, ("Partial page table allocation found, using page offset %d\n", page_number));
+		_mali_osk_set_nonatomic_bit(page_number, alloc->usage_map);
+		alloc->usage_count++;
+		if (alloc->num_pages == alloc->usage_count)
+		{
+			/* full, move alloc to full list*/
+			_mali_osk_list_move(&alloc->list, &page_table_cache.full);
+		}
+    	_mali_osk_lock_signal(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+
+        *table_page = (MALI_MMU_PAGE_SIZE * page_number) + alloc->pages.phys_base;
+		*mapping =  (mali_io_address)((MALI_MMU_PAGE_SIZE * page_number) + (u32)alloc->pages.mapping);
+		MALI_DEBUG_PRINT(4, ("Page table allocated for VA=0x%08X, MaliPA=0x%08X\n", *mapping, *table_page ));
+        MALI_SUCCESS;
+	}
+	else
+	{
+		mali_mmu_page_table_allocation * alloc;
+		/* no free pages, allocate a new one */
+
+		alloc = (mali_mmu_page_table_allocation *)_mali_osk_calloc(1, sizeof(mali_mmu_page_table_allocation));
+		if (NULL == alloc)
+		{
+        	_mali_osk_lock_signal(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+            *table_page = MALI_INVALID_PAGE;
+			MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+		}
+
+		_MALI_OSK_INIT_LIST_HEAD(&alloc->list);
+
+		if (_MALI_OSK_ERR_OK != mali_allocation_engine_allocate_page_tables(memory_engine, &alloc->pages, physical_memory_allocators))
+		{
+			MALI_DEBUG_PRINT(1, ("No more memory for page tables\n"));
+			_mali_osk_free(alloc);
+        	_mali_osk_lock_signal(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+            *table_page = MALI_INVALID_PAGE;
+			*mapping = NULL;
+			MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+		}
+
+		/* create the usage map */
+		alloc->num_pages = alloc->pages.size / MALI_MMU_PAGE_SIZE;
+		alloc->usage_count = 1;
+		MALI_DEBUG_PRINT(3, ("New page table cache expansion, %d pages in new cache allocation\n", alloc->num_pages));
+		alloc->usage_map = _mali_osk_calloc(1, ((alloc->num_pages + BITS_PER_LONG - 1) & ~(BITS_PER_LONG-1) / BITS_PER_LONG) * sizeof(unsigned long));
+		if (NULL == alloc->usage_map)
+		{
+			MALI_DEBUG_PRINT(1, ("Failed to allocate memory to describe MMU page table cache usage\n"));
+			alloc->pages.release(&alloc->pages);
+			_mali_osk_free(alloc);
+        	_mali_osk_lock_signal(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+            *table_page = MALI_INVALID_PAGE;
+			*mapping = NULL;
+            MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+		}
+
+		/* clear memory allocation */
+		fill_page(alloc->pages.mapping, 0);
+
+		_mali_osk_set_nonatomic_bit(0, alloc->usage_map);
+
+		_mali_osk_list_add(&alloc->list, &page_table_cache.partial);
+
+    	_mali_osk_lock_signal(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+		*table_page = alloc->pages.phys_base; /* return the first page */
+		*mapping = alloc->pages.mapping; /* Mapping for first page */
+		MALI_DEBUG_PRINT(4, ("Page table allocated for VA=0x%08X, MaliPA=0x%08X\n", *mapping, *table_page ));
+        MALI_SUCCESS;
+	}
+}
+
+void mali_mmu_release_table_page(u32 pa)
+{
+	mali_mmu_page_table_allocation * alloc, * temp_alloc;
+
+	MALI_DEBUG_PRINT_IF(1, pa & 4095, ("Bad page address 0x%x given to mali_mmu_release_table_page\n", (void*)pa));
+
+	MALI_DEBUG_PRINT(4, ("Releasing table page 0x%08X to the cache\n", pa));
+
+   	_mali_osk_lock_wait(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+
+	/* find the entry this address belongs to */
+	/* first check the partial list */
+	_MALI_OSK_LIST_FOREACHENTRY(alloc, temp_alloc, &page_table_cache.partial, mali_mmu_page_table_allocation, list)
+	{
+		u32 start = alloc->pages.phys_base;
+		u32 last = start + (alloc->num_pages - 1) * MALI_MMU_PAGE_SIZE;
+		if (pa >= start && pa <= last)
+		{
+            MALI_DEBUG_ASSERT(0 != _mali_osk_test_bit((pa - start)/MALI_MMU_PAGE_SIZE, alloc->usage_map));
+			_mali_osk_clear_nonatomic_bit((pa - start)/MALI_MMU_PAGE_SIZE, alloc->usage_map);
+			alloc->usage_count--;
+
+			_mali_osk_memset((void*)( ((u32)alloc->pages.mapping) + (pa - start) ), 0, MALI_MMU_PAGE_SIZE);
+
+			if (0 == alloc->usage_count)
+			{
+				/* empty, release whole page alloc */
+				_mali_osk_list_del(&alloc->list);
+				alloc->pages.release(&alloc->pages);
+				_mali_osk_free(alloc->usage_map);
+				_mali_osk_free(alloc);
+			}
+           	_mali_osk_lock_signal(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+        	MALI_DEBUG_PRINT(4, ("(partial list)Released table page 0x%08X to the cache\n", pa));
+			return;
+		}
+	}
+
+	/* the check the full list */
+	_MALI_OSK_LIST_FOREACHENTRY(alloc, temp_alloc, &page_table_cache.full, mali_mmu_page_table_allocation, list)
+	{
+		u32 start = alloc->pages.phys_base;
+		u32 last = start + (alloc->num_pages - 1) * MALI_MMU_PAGE_SIZE;
+		if (pa >= start && pa <= last)
+		{
+			_mali_osk_clear_nonatomic_bit((pa - start)/MALI_MMU_PAGE_SIZE, alloc->usage_map);
+			alloc->usage_count--;
+
+			_mali_osk_memset((void*)( ((u32)alloc->pages.mapping) + (pa - start) ), 0, MALI_MMU_PAGE_SIZE);
+
+			/* transfer to partial list */
+			_mali_osk_list_move(&alloc->list, &page_table_cache.partial);
+           	_mali_osk_lock_signal(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+        	MALI_DEBUG_PRINT(4, ("(full list)Released table page 0x%08X to the cache\n", pa));
+			return;
+		}
+	}
+
+	MALI_DEBUG_PRINT(1, ("pa 0x%x not found in the page table cache\n", (void*)pa));
+
+   	_mali_osk_lock_signal(page_table_cache.lock, _MALI_OSK_LOCKMODE_RW);
+}
+
+void* mali_memory_core_mmu_lookup(u32 id)
+{
+	mali_kernel_memory_mmu * mmu, * temp_mmu;
+
+	/* find an MMU with a matching id */
+	_MALI_OSK_LIST_FOREACHENTRY(mmu, temp_mmu, &mmu_head, mali_kernel_memory_mmu, list)
+	{
+		if (id == mmu->id) return mmu;
+	}
+
+	/* not found */
+	return NULL;
+}
+
+void mali_mmu_activate_address_space(mali_kernel_memory_mmu * mmu, u32 page_directory)
+{
+	const int delay_in_usecs = 10;
+	const int max_loop_count = 10;
+	int i;
+
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ENABLE_STALL);
+
+	if (!mali_benchmark) {
+		for (i = 0; i < max_loop_count; ++i)
+		{
+			if (mali_mmu_register_read(mmu, MALI_MMU_REGISTER_STATUS) & MALI_MMU_STATUS_BIT_STALL_ACTIVE) break;
+			_mali_osk_time_ubusydelay(delay_in_usecs);
+		}
+		MALI_DEBUG_PRINT_IF(1, (max_loop_count == i), ("Stall request failed, swapping anyway\n"));
+	}
+
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_DTE_ADDR, page_directory);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ZAP_CACHE);
+	mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_DISABLE_STALL);
+}
+
+_mali_osk_errcode_t mali_memory_core_mmu_activate_page_table(void* mmu_ptr, struct mali_session_data * mali_session_data, void(*callback)(void*), void * callback_argument)
+{
+	memory_session * requested_memory_session;
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_FAULT;
+	mali_kernel_memory_mmu * mmu;
+
+	MALI_DEBUG_ASSERT_POINTER(mmu_ptr);
+	MALI_DEBUG_ASSERT_POINTER(mali_session_data);
+
+	mmu = (mali_kernel_memory_mmu *)mmu_ptr;
+
+	MALI_DEBUG_PRINT(4, ("Asked to activate page table for session 0x%x on MMU %s\n", mali_session_data, mmu->description));
+	requested_memory_session = mali_kernel_session_manager_slot_get(mali_session_data, mali_subsystem_memory_id);
+	MALI_DEBUG_PRINT(5, ("Session 0x%x looked up as using memory session 0x%x\n", mali_session_data, requested_memory_session));
+
+	MALI_DEBUG_ASSERT_POINTER(requested_memory_session);
+
+	MALI_DEBUG_PRINT(7, ("Taking locks\n"));
+
+	_mali_osk_lock_wait(requested_memory_session->lock, _MALI_OSK_LOCKMODE_RW);
+	_mali_osk_lock_wait(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+	if (0 == mmu->usage_count)
+	{
+		/* no session currently active, activate the requested session */
+		MALI_DEBUG_ASSERT(NULL == mmu->active_session);
+		mmu->active_session = requested_memory_session;
+		mmu->usage_count = 1;
+		MALI_DEBUG_PRINT(4, ("MMU idle, activating page directory 0x%08X on MMU %s\n", requested_memory_session->page_directory, mmu->description));
+		mali_mmu_activate_address_space(mmu, requested_memory_session->page_directory);
+		{
+			/* Insert mmu into the right place in the active_mmus list so that
+			 * it is still sorted. The list must be sorted by ID so we can get
+			 * the mutexes in the right order in
+			 * _mali_ukk_mem_munmap_internal().
+			 */
+			_mali_osk_list_t *entry;
+			for (entry = requested_memory_session->active_mmus.next;
+				 entry != &requested_memory_session->active_mmus;
+				 entry = entry->next)
+			{
+				mali_kernel_memory_mmu *temp = _MALI_OSK_LIST_ENTRY(entry, mali_kernel_memory_mmu, session_link);
+				if (mmu->id < temp->id)
+					break;
+			}
+			/* If we broke out, then 'entry' points to the list node of the
+			 * first mmu with a greater ID; otherwise, it points to
+			 * active_mmus. We want to add *before* this node.
+			 */
+			_mali_osk_list_addtail(&mmu->session_link, entry);
+		}
+		err = _MALI_OSK_ERR_OK;
+	}
+
+	/* Allow two cores to run in parallel if they come from the same session */
+	else if (
+	           (mmu->in_page_fault_handler == 0) &&
+	           (requested_memory_session == mmu->active_session ) &&
+	           (0==(MALI_MMU_DISALLOW_PARALLELL_WORK_OF_MALI_CORES & mmu->flags))
+	        )
+	{
+		/* nested activation detected, just update the reference count */
+		MALI_DEBUG_PRINT(4, ("Nested activation detected, %d previous activations found\n", mmu->usage_count));
+		mmu->usage_count++;
+		err = _MALI_OSK_ERR_OK;
+	}
+
+	else if (NULL != callback)
+	{
+		/* can't activate right now, notify caller on idle via callback */
+		mali_kernel_memory_mmu_idle_callback * callback_object, * temp_callback_object;
+		int found = 0;
+
+		MALI_DEBUG_PRINT(3, ("The MMU is busy and is using a different address space, callback given\n"));
+		/* check for existing registration */
+		_MALI_OSK_LIST_FOREACHENTRY(callback_object, temp_callback_object, &mmu->callbacks, mali_kernel_memory_mmu_idle_callback, link)
+		{
+			if (callback_object->callback == callback)
+			{
+				found = 1;
+				break;
+			}
+		}
+
+		if (found)
+		{
+			MALI_DEBUG_PRINT(5, ("Duplicate callback registration found, ignoring\n"));
+			/* callback already registered */
+			err = _MALI_OSK_ERR_BUSY;
+		}
+	 	else
+		{
+			MALI_DEBUG_PRINT(5,("New callback, registering\n"));
+			/* register the new callback */
+			callback_object = _mali_osk_malloc(sizeof(mali_kernel_memory_mmu_idle_callback));
+			if (NULL != callback_object)
+			{
+				MALI_DEBUG_PRINT(7,("Callback struct setup\n"));
+				callback_object->callback = callback;
+				callback_object->callback_argument = callback_argument;
+				_mali_osk_list_addtail(&callback_object->link, &mmu->callbacks);
+				err = _MALI_OSK_ERR_BUSY;
+			}
+		}
+	}
+
+	_mali_osk_lock_signal(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+	_mali_osk_lock_signal(requested_memory_session->lock, _MALI_OSK_LOCKMODE_RW);
+
+    MALI_ERROR(err);
+}
+
+void mali_memory_core_mmu_release_address_space_reference(void* mmu_ptr)
+{
+	mali_kernel_memory_mmu_idle_callback * callback_object, * temp;
+	mali_kernel_memory_mmu * mmu;
+	memory_session * session;
+
+	_MALI_OSK_LIST_HEAD(callbacks);
+
+    MALI_DEBUG_ASSERT_POINTER(mmu_ptr);
+	mmu = (mali_kernel_memory_mmu *)mmu_ptr;
+
+	session = mmu->active_session;
+
+	/* support that we handle spurious page faults */
+	if (NULL != session)
+	{
+		_mali_osk_lock_wait(session->lock, _MALI_OSK_LOCKMODE_RW);
+	}
+
+  	_mali_osk_lock_wait(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+	MALI_DEBUG_PRINT(4, ("Deactivation of address space on MMU %s, %d references exists\n", mmu->description, mmu->usage_count));
+    MALI_DEBUG_ASSERT(0 != mmu->usage_count);
+	mmu->usage_count--;
+	if (0 != mmu->usage_count)
+	{
+		MALI_DEBUG_PRINT(4, ("MMU still in use by this address space, %d references still exists\n", mmu->usage_count));
+      	_mali_osk_lock_signal(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+		/* support that we handle spurious page faults */
+		if (NULL != session)
+		{
+			_mali_osk_lock_signal(session->lock, _MALI_OSK_LOCKMODE_RW);
+		}
+		return;
+	}
+
+	MALI_DEBUG_PRINT(4, ("Activating the empty page directory on %s\n", mmu->description));
+
+	/* last reference gone, deactivate current address space */
+	mali_mmu_activate_address_space(mmu, mali_empty_page_directory);
+
+	/* unlink from session */
+	_mali_osk_list_delinit(&mmu->session_link);
+	/* remove the active session pointer */
+	mmu->active_session = NULL;
+
+	/* Notify all registered callbacks.
+	 * We have to be clever here:
+	 * We must call the callbacks with the spinlock unlocked and
+	 * the callback list emptied to allow them to re-register.
+	 * So we make a copy of the list, clears the list and then later call the callbacks on the local copy
+	 */
+	/* copy list */
+	_MALI_OSK_INIT_LIST_HEAD(&callbacks);
+	_mali_osk_list_splice(&mmu->callbacks, &callbacks);
+	/* clear the original, allowing new registrations during the callback */
+	_MALI_OSK_INIT_LIST_HEAD(&mmu->callbacks);
+
+	/* end of mmu manipulation, so safe to unlock */
+   	_mali_osk_lock_signal(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+
+	/* then finally remove the (possible) session lock, supporting that no session was active (spurious page fault handling) */
+	if (NULL != session)
+	{
+		_mali_osk_lock_signal(session->lock, _MALI_OSK_LOCKMODE_RW);
+	}
+
+	_MALI_OSK_LIST_FOREACHENTRY(callback_object, temp, &callbacks, mali_kernel_memory_mmu_idle_callback, link)
+	{
+        MALI_DEBUG_ASSERT_POINTER(callback_object->callback);
+		(callback_object->callback)(callback_object->callback_argument);
+		_mali_osk_list_del(&callback_object->link);
+		_mali_osk_free(callback_object);
+	}
+}
+
+void mali_memory_core_mmu_unregister_callback(void* mmu_ptr, void(*callback)(void*))
+{
+	mali_kernel_memory_mmu_idle_callback * callback_object, * temp_callback_object;
+	mali_kernel_memory_mmu * mmu;
+    MALI_DEBUG_ASSERT_POINTER(mmu_ptr);
+
+    MALI_DEBUG_ASSERT_POINTER(callback);
+    MALI_DEBUG_ASSERT_POINTER(mmu_ptr);
+
+	mmu = (mali_kernel_memory_mmu *)mmu_ptr;
+
+  	_mali_osk_lock_wait(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+	_MALI_OSK_LIST_FOREACHENTRY(callback_object, temp_callback_object, &mmu->callbacks, mali_kernel_memory_mmu_idle_callback, link)
+	{
+        MALI_DEBUG_ASSERT_POINTER(callback_object->callback);
+		if (callback_object->callback == callback)
+		{
+			_mali_osk_list_del(&callback_object->link);
+			_mali_osk_free(callback_object);
+			break;
+		}
+	}
+  	_mali_osk_lock_signal(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+}
+
+static _mali_osk_errcode_t mali_address_manager_allocate(mali_memory_allocation * descriptor)
+{
+	/* allocate page tables, if needed */
+	int i;
+	const int first_pde_idx = MALI_MMU_PDE_ENTRY(descriptor->mali_address);
+	int last_pde_idx;
+	memory_session * session_data;
+#if defined USING_MALI400_L2_CACHE
+	int has_active_mmus = 0;
+	int page_dir_updated = 0;
+#endif
+
+
+	if (descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_GUARD_PAGE)
+	{
+		last_pde_idx = MALI_MMU_PDE_ENTRY(descriptor->mali_address + _MALI_OSK_MALI_PAGE_SIZE + descriptor->size - 1);
+	}
+	else
+	{
+		last_pde_idx = MALI_MMU_PDE_ENTRY(descriptor->mali_address + descriptor->size - 1);
+	}
+
+	session_data = (memory_session*)descriptor->mali_addr_mapping_info;
+    MALI_DEBUG_ASSERT_POINTER(session_data);
+
+	MALI_DEBUG_PRINT(4, ("allocating page tables for Mali virtual address space 0x%08X to 0x%08X\n", descriptor->mali_address, descriptor->mali_address + descriptor->size - 1));
+
+#if defined USING_MALI400_L2_CACHE
+	if (0 == _mali_osk_list_empty(&session_data->active_mmus))
+	{
+		/*
+		 * We have active MMUs, so we are probably in the process of alocating more memory for a suspended GP job (PLBU heap)
+		 * From Mali-400 MP r1p0, MMU page directory/tables are also cached by the Mali L2 cache, thus we need to invalidate the page directory
+		 * from the L2 cache if we add new page directory entries (PDEs) to the page directory.
+		 * We only need to do this when we have an active MMU, because we otherwise invalidate the entire Mali L2 cache before at job start
+		 */
+		has_active_mmus = 1;
+	}
+#endif
+
+	for (i = first_pde_idx; i <= last_pde_idx; i++)
+	{
+		if ( 0 == (_mali_osk_mem_ioread32(session_data->page_directory_mapped, i * sizeof(u32)) & MALI_MMU_FLAGS_PRESENT) )
+		{
+			u32 pte_phys;
+			mali_io_address pte_mapped;
+            _mali_osk_errcode_t err;
+
+			/* allocate a new page table */
+            MALI_DEBUG_ASSERT(0 == session_data->page_entries_usage_count[i]);
+            MALI_DEBUG_ASSERT(NULL == session_data->page_entries_mapped[i]);
+
+			err = mali_mmu_get_table_page(&pte_phys, &pte_mapped);
+            if (_MALI_OSK_ERR_OK == err)
+			{
+				session_data->page_entries_mapped[i] = pte_mapped;
+				MALI_DEBUG_ASSERT_POINTER( session_data->page_entries_mapped[i] );
+
+				_mali_osk_mem_iowrite32(session_data->page_directory_mapped, i * sizeof(u32), pte_phys | MALI_MMU_FLAGS_PRESENT); /* mark page table as present */
+
+				/* update usage count */
+				session_data->page_entries_usage_count[i]++;
+#if defined USING_MALI400_L2_CACHE
+				page_dir_updated = 1;
+#endif
+				continue; /* continue loop */
+			}
+
+			MALI_DEBUG_PRINT(1, ("Page table alloc failed\n"));
+			break; /* abort loop, failed to allocate one or more page tables */
+		}
+		else
+		{
+			session_data->page_entries_usage_count[i]++;
+		}
+	}
+
+	if (i <= last_pde_idx)
+	{
+		/* one or more pages could not be allocated, release reference count for the ones we added one for */
+		/* adjust for the one which caused the for loop to be aborted */
+		i--;
+
+		while (i >= first_pde_idx)
+		{
+            MALI_DEBUG_ASSERT(0 != session_data->page_entries_usage_count[i]);
+			session_data->page_entries_usage_count[i]--;
+			if (0 == session_data->page_entries_usage_count[i])
+			{
+				/* last reference removed */
+				mali_mmu_release_table_page(MALI_MMU_ENTRY_ADDRESS(_mali_osk_mem_ioread32(session_data->page_directory_mapped, i * sizeof(u32))));
+				session_data->page_entries_mapped[i] = NULL;
+				_mali_osk_mem_iowrite32(session_data->page_directory_mapped, i * sizeof(u32), 0); /* mark as not present in the page directory */
+			}
+			i--;
+		}
+
+		MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+
+#if defined USING_MALI400_L2_CACHE
+	if (1 == has_active_mmus && 1 == page_dir_updated)
+	{
+		/*
+		 * We have updated the page directory and have an active MMU using it, so invalidate it in the Mali L2 cache.
+		 */
+		mali_kernel_l2_cache_invalidate_page(session_data->page_directory);
+	}
+#endif
+
+	/* all OK */
+	MALI_SUCCESS;
+}
+
+static void mali_address_manager_release(mali_memory_allocation * descriptor)
+{
+	int first_pde_idx;
+	int last_pde_idx;
+	memory_session * session_data;
+	u32 mali_address;
+	u32 mali_address_end;
+	u32 left;
+	int i;
+#if defined USING_MALI400_L2_CACHE
+	int has_active_mmus = 0;
+	int page_dir_updated = 0;
+#endif
+
+    MALI_DEBUG_ASSERT_POINTER(descriptor);
+	session_data = (memory_session*)descriptor->mali_addr_mapping_info;
+    MALI_DEBUG_ASSERT_POINTER(session_data);
+    MALI_DEBUG_ASSERT_POINTER(session_data->page_directory_mapped);
+
+	mali_address = descriptor->mali_address;
+	mali_address_end = descriptor->mali_address + descriptor->size;
+	left = descriptor->size;
+
+	first_pde_idx = MALI_MMU_PDE_ENTRY(mali_address);
+	last_pde_idx = MALI_MMU_PDE_ENTRY(mali_address_end - 1);
+
+	MALI_DEBUG_PRINT(3, ("Zapping Mali MMU table for address 0x%08X size 0x%08X\n", mali_address, left));
+	MALI_DEBUG_PRINT(4, ("Zapping PDE %d through %d\n", first_pde_idx, last_pde_idx));
+
+#if defined USING_MALI400_L2_CACHE
+	if (0 == _mali_osk_list_empty(&session_data->active_mmus))
+	{
+		/*
+		 * From Mali-400 MP r1p0, MMU page directory/tables are also cached by the Mali L2 cache, thus we need to invalidate the page tables
+		 * from the L2 cache to ensure that the memory is unmapped.
+		 * We only need to do this when we have an active MMU, because we otherwise invalidate the entire Mali L2 cache before at job start
+		 */
+		has_active_mmus = 1;
+	}
+#endif
+
+
+	for (i = first_pde_idx; i <= last_pde_idx; i++)
+	{
+		const int size_inside_pte = left < 0x400000 ? left : 0x400000;
+
+        MALI_DEBUG_ASSERT_POINTER(session_data->page_entries_mapped[i]);
+        MALI_DEBUG_ASSERT(0 != session_data->page_entries_usage_count[i]);
+		MALI_DEBUG_PRINT(4, ("PDE %d\n", i));
+
+		session_data->page_entries_usage_count[i]--;
+
+		if (0 == session_data->page_entries_usage_count[i])
+		{
+			MALI_DEBUG_PRINT(4, ("Releasing page table as this is the last reference\n"));
+			/* last reference removed, no need to zero out each PTE  */
+			mali_mmu_release_table_page(MALI_MMU_ENTRY_ADDRESS(_mali_osk_mem_ioread32(session_data->page_directory_mapped, i * sizeof(u32))));
+			session_data->page_entries_mapped[i] = NULL;
+			_mali_osk_mem_iowrite32(session_data->page_directory_mapped, i * sizeof(u32), 0); /* mark as not present in the page directory */
+#if defined USING_MALI400_L2_CACHE
+			page_dir_updated = 1;
+#endif
+		}
+		else
+		{
+			int j;
+			const int first_pte_idx = MALI_MMU_PTE_ENTRY(mali_address);
+			const int last_pte_idx = MALI_MMU_PTE_ENTRY(mali_address + size_inside_pte - 1);
+
+			MALI_DEBUG_PRINT(4, ("Partial page table fill detected, zapping entries %d through %d (page table at 0x%08X)\n", first_pte_idx, last_pte_idx, MALI_MMU_ENTRY_ADDRESS(_mali_osk_mem_ioread32(session_data->page_directory_mapped, i * sizeof(u32)))));
+
+			for (j = first_pte_idx; j <= last_pte_idx; j++)
+			{
+				_mali_osk_mem_iowrite32(session_data->page_entries_mapped[i], j * sizeof(u32), 0);
+			}
+
+			MALI_DEBUG_PRINT(5, ("zap complete\n"));
+
+			mali_address += size_inside_pte;
+
+#if defined USING_MALI400_L2_CACHE
+			if (1 == has_active_mmus)
+			{
+				/* Invalidate the page we've just modified */
+				mali_kernel_l2_cache_invalidate_page( _mali_osk_mem_ioread32(session_data->page_directory_mapped, i*sizeof(u32)) & ~MALI_MMU_FLAGS_MASK);
+			}
+#endif
+		}
+		left -= size_inside_pte;
+	}
+
+#if defined USING_MALI400_L2_CACHE
+	if ((1 == page_dir_updated) && (1== has_active_mmus))
+	{
+		/* The page directory was also updated */
+		mali_kernel_l2_cache_invalidate_page(session_data->page_directory);
+	}
+#endif
+}
+
+static _mali_osk_errcode_t mali_address_manager_map(mali_memory_allocation * descriptor, u32 offset, u32 *phys_addr, u32 size)
+{
+	memory_session * session_data;
+	u32 mali_address;
+	u32 mali_address_end;
+	u32 current_phys_addr;
+#if defined USING_MALI400_L2_CACHE
+	int has_active_mmus = 0;
+#endif
+
+    MALI_DEBUG_ASSERT_POINTER(descriptor);
+
+	MALI_DEBUG_ASSERT_POINTER( phys_addr );
+
+	current_phys_addr = *phys_addr;
+
+	session_data = (memory_session*)descriptor->mali_addr_mapping_info;
+    MALI_DEBUG_ASSERT_POINTER(session_data);
+
+	mali_address = descriptor->mali_address + offset;
+	mali_address_end = descriptor->mali_address + offset + size;
+
+#if defined USING_MALI400_L2_CACHE
+	if (0 == _mali_osk_list_empty(&session_data->active_mmus))
+	{
+		/*
+		 * We have active MMUs, so we are probably in the process of alocating more memory for a suspended GP job (PLBU heap)
+		 * From Mali-400 MP r1p0, MMU page directory/tables are also cached by the Mali L2 cache, thus we need to invalidate the page tables
+		 * from the L2 cache when we have allocated more heap memory.
+		 * We only need to do this when we have an active MMU, because we otherwise invalidate the entire Mali L2 cache before at job start
+		 */
+		has_active_mmus = 1;
+	}
+#endif
+
+	MALI_DEBUG_PRINT(6, ("Mali map: mapping 0x%08X to Mali address 0x%08X length 0x%08X\n", current_phys_addr, mali_address, size));
+
+    MALI_DEBUG_ASSERT_POINTER(session_data->page_entries_mapped);
+
+	for ( ; mali_address < mali_address_end; mali_address += MALI_MMU_PAGE_SIZE, current_phys_addr += MALI_MMU_PAGE_SIZE)
+	{
+        MALI_DEBUG_ASSERT_POINTER(session_data->page_entries_mapped[MALI_MMU_PDE_ENTRY(mali_address)]);
+		_mali_osk_mem_iowrite32(session_data->page_entries_mapped[MALI_MMU_PDE_ENTRY(mali_address)], MALI_MMU_PTE_ENTRY(mali_address) * sizeof(u32), current_phys_addr | MALI_MMU_FLAGS_WRITE_PERMISSION | MALI_MMU_FLAGS_READ_PERMISSION | MALI_MMU_FLAGS_PRESENT);
+	}
+
+#if defined USING_MALI400_L2_CACHE
+	if (1 == has_active_mmus)
+	{
+		int i;
+		const int first_pde_idx = MALI_MMU_PDE_ENTRY(mali_address);
+		const int last_pde_idx = MALI_MMU_PDE_ENTRY(mali_address_end - 1);
+
+		/*
+		 * Invalidate the updated page table(s), incase they have been used for something
+		 * else since last job start (invalidation of entire Mali L2 cache)
+		 */
+		for (i = first_pde_idx; i <= last_pde_idx; i++)
+		{
+			mali_kernel_l2_cache_invalidate_page( _mali_osk_mem_ioread32(session_data->page_directory_mapped, i*sizeof(u32)) & ~MALI_MMU_FLAGS_MASK);
+		}
+	}
+#endif
+
+	MALI_SUCCESS;
+}
+
+/* This handler registered to mali_mmap for MMU builds */
+_mali_osk_errcode_t _mali_ukk_mem_mmap( _mali_uk_mem_mmap_s *args )
+{
+	struct mali_session_data * mali_session_data;
+	mali_memory_allocation * descriptor;
+	memory_session * session_data;
+
+	/* validate input */
+	if (NULL == args) { MALI_DEBUG_PRINT(3,("mali_ukk_mem_mmap: args was NULL\n")); MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS); }
+
+	/* Unpack arguments */
+	mali_session_data = (struct mali_session_data *)args->ctx;
+
+	if (NULL == mali_session_data) { MALI_DEBUG_PRINT(3,("mali_ukk_mem_mmap: mali_session data was NULL\n")); MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS); }
+
+	MALI_DEBUG_ASSERT( mali_subsystem_memory_id >= 0 );
+
+	session_data = mali_kernel_session_manager_slot_get(mali_session_data, mali_subsystem_memory_id);
+	/* validate input */
+	if (NULL == session_data) { MALI_DEBUG_PRINT(3,("mali_ukk_mem_mmap: session data was NULL\n")); MALI_ERROR(_MALI_OSK_ERR_FAULT); }
+
+	descriptor = (mali_memory_allocation*) _mali_osk_calloc( 1, sizeof(mali_memory_allocation) );
+	if (NULL == descriptor) { MALI_DEBUG_PRINT(3,("mali_ukk_mem_mmap: descriptor was NULL\n")); MALI_ERROR(_MALI_OSK_ERR_NOMEM); }
+
+	descriptor->size = args->size;
+	descriptor->mali_address = args->phys_addr;
+	descriptor->mali_addr_mapping_info = (void*)session_data;
+
+	descriptor->process_addr_mapping_info = args->ukk_private; /* save to be used during physical manager callback */
+	descriptor->flags = MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE;
+    descriptor->lock = session_data->lock;
+	_mali_osk_list_init( &descriptor->list );
+
+   	_mali_osk_lock_wait(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+
+	if (0 == mali_allocation_engine_allocate_memory(memory_engine, descriptor, physical_memory_allocators, &session_data->memory_head))
+	{
+		mali_kernel_memory_mmu * mmu, * temp_mmu;
+
+		_MALI_OSK_LIST_FOREACHENTRY(mmu, temp_mmu, &session_data->active_mmus, mali_kernel_memory_mmu, session_link)
+		{
+			/* no need to lock the MMU as we own it already */
+			MALI_DEBUG_PRINT(5, ("Zapping the cache of mmu %s as it's using the page table we have updated\n", mmu->description));
+
+			_mali_osk_lock_wait(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+
+			mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ENABLE_STALL);
+			if (!mali_benchmark) {
+				while ( (mali_mmu_register_read(mmu, MALI_MMU_REGISTER_STATUS) & MALI_MMU_STATUS_BIT_STALL_ACTIVE) == 0) _mali_osk_time_ubusydelay(1);
+			}
+			mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ZAP_CACHE);
+			mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_DISABLE_STALL);
+
+			_mali_osk_lock_signal(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+		}
+
+       	_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+
+        /* All ok, write out any information generated from this call */
+        args->mapping = descriptor->mapping;
+        args->cookie = (u32)descriptor;
+
+        MALI_DEBUG_PRINT(7, ("MMAP OK\n"));
+		/* All done */
+		MALI_SUCCESS;
+	}
+	else
+	{
+       	_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+		/* OOM, but not a fatal error */
+		MALI_DEBUG_PRINT(4, ("Memory allocation failure, OOM\n"));
+		_mali_osk_free(descriptor);
+		/* Linux will free the CPU address allocation, userspace client the Mali address allocation */
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+}
+
+static void _mali_ukk_mem_munmap_internal( _mali_uk_mem_munmap_s *args )
+{
+	memory_session * session_data;
+	mali_kernel_memory_mmu * mmu, * temp_mmu;
+	mali_memory_allocation * descriptor;
+
+	descriptor = (mali_memory_allocation *)args->cookie;
+    MALI_DEBUG_ASSERT_POINTER(descriptor);
+
+    /** @note args->context unused; we use the memory_session from the cookie */
+    /* args->mapping and args->size are also discarded. They are only necessary
+    for certain do_munmap implementations. However, they could be used to check the
+    descriptor at this point. */
+
+    session_data = (memory_session*)descriptor->mali_addr_mapping_info;
+    MALI_DEBUG_ASSERT_POINTER(session_data);
+
+	/* Stall the MMU(s) which is using the address space we're operating on.
+	 * Note that active_mmus must be sorted in order of ID to avoid a mutex
+	 * ordering violation.
+	 */
+	_MALI_OSK_LIST_FOREACHENTRY(mmu, temp_mmu, &session_data->active_mmus, mali_kernel_memory_mmu, session_link)
+	{
+		const int max_loop_count = 100;
+		const int sleep_duration = 1; /* must be below 1000 */
+		int i;
+
+		_mali_osk_lock_wait(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+
+		mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ENABLE_STALL);
+
+		if (!mali_benchmark)
+		{
+			for ( i = 0; i < max_loop_count; i++)
+			{
+			   if (mali_mmu_register_read(mmu, MALI_MMU_REGISTER_STATUS) & MALI_MMU_STATUS_BIT_STALL_ACTIVE) break;
+			   _mali_osk_time_ubusydelay(sleep_duration);
+			}
+
+			MALI_DEBUG_PRINT_IF(3, max_loop_count == i, ("Stall failed, trying zap anyway\n"));
+		}
+	}
+
+	/* This function also removes the memory from the session's memory list */
+	mali_allocation_engine_release_memory(memory_engine, descriptor);
+	_mali_osk_free(descriptor);
+
+	/* any L2 maintenance was done during mali_allocation_engine_release_memory */
+	/* the session is locked, so the active mmu list should be the same */
+	/* zap the TLB and resume operation */
+	_MALI_OSK_LIST_FOREACHENTRY(mmu, temp_mmu, &session_data->active_mmus, mali_kernel_memory_mmu, session_link)
+	{
+		mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_ZAP_CACHE);
+		mali_mmu_register_write(mmu, MALI_MMU_REGISTER_COMMAND, MALI_MMU_COMMAND_DISABLE_STALL);
+
+		_mali_osk_lock_signal(mmu->lock, _MALI_OSK_LOCKMODE_RW);
+	}
+}
+
+/* Handler for unmapping memory for MMU builds */
+_mali_osk_errcode_t _mali_ukk_mem_munmap( _mali_uk_mem_munmap_s *args )
+{
+	mali_memory_allocation * descriptor;
+	_mali_osk_lock_t *descriptor_lock;
+
+	descriptor = (mali_memory_allocation *)args->cookie;
+    MALI_DEBUG_ASSERT_POINTER(descriptor);
+
+    /** @note args->context unused; we use the memory_session from the cookie */
+    /* args->mapping and args->size are also discarded. They are only necessary
+    for certain do_munmap implementations. However, they could be used to check the
+    descriptor at this point. */
+
+    MALI_DEBUG_ASSERT_POINTER((memory_session*)descriptor->mali_addr_mapping_info);
+    
+	descriptor_lock = descriptor->lock; /* should point to the session data lock... */
+
+	if (descriptor_lock)
+	{
+		_mali_osk_lock_wait( descriptor_lock, _MALI_OSK_LOCKMODE_RW );
+	}
+	/* Noninterruptable spinlock type, so must always have locked. Checking should've been done in OSK function. */
+
+	_mali_ukk_mem_munmap_internal( args );
+	/* descriptor is no longer valid - it may've been freed */
+
+	if (descriptor_lock)
+	{
+		_mali_osk_lock_signal( descriptor_lock, _MALI_OSK_LOCKMODE_RW );
+	}
+	return _MALI_OSK_ERR_OK;
+}
+
+/* Is called when the rendercore wants the mmu to give an interrupt */
+static void mali_mmu_probe_irq_trigger(mali_kernel_memory_mmu * mmu)
+{
+    MALI_DEBUG_PRINT(2, ("mali_mmu_probe_irq_trigger\n"));
+    mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_RAWSTAT, MALI_MMU_INTERRUPT_PAGE_FAULT|MALI_MMU_INTERRUPT_READ_BUS_ERROR);
+}
+
+/* Is called when the irq probe wants the mmu to acknowledge an interrupt from the hw */
+static _mali_osk_errcode_t mali_mmu_probe_irq_acknowledge(mali_kernel_memory_mmu * mmu)
+{
+	u32 int_stat;
+
+    int_stat = mali_mmu_register_read(mmu, MALI_MMU_REGISTER_INT_STATUS);
+
+    MALI_DEBUG_PRINT(2, ("mali_mmu_probe_irq_acknowledge: intstat 0x%x\n", int_stat));
+    if (int_stat & MALI_MMU_INTERRUPT_PAGE_FAULT)
+    {
+	    MALI_DEBUG_PRINT(2, ("Probe: Page fault detect: PASSED\n"));
+	    mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_CLEAR, MALI_MMU_INTERRUPT_PAGE_FAULT);
+    }
+    else MALI_DEBUG_PRINT(1, ("Probe: Page fault detect: FAILED\n"));
+
+    if (int_stat & MALI_MMU_INTERRUPT_READ_BUS_ERROR)
+    {
+	    MALI_DEBUG_PRINT(2, ("Probe: Bus read error detect: PASSED\n"));
+	    mali_mmu_register_write(mmu, MALI_MMU_REGISTER_INT_CLEAR, MALI_MMU_INTERRUPT_READ_BUS_ERROR);
+    }
+    else MALI_DEBUG_PRINT(1, ("Probe: Bus read error detect: FAILED\n"));
+
+    if ( (int_stat & (MALI_MMU_INTERRUPT_PAGE_FAULT|MALI_MMU_INTERRUPT_READ_BUS_ERROR)) ==
+                     (MALI_MMU_INTERRUPT_PAGE_FAULT|MALI_MMU_INTERRUPT_READ_BUS_ERROR))
+    {
+        MALI_SUCCESS;
+    }
+
+    MALI_ERROR(_MALI_OSK_ERR_FAULT);
+}
+
+struct dump_info
+{
+	u32 buffer_left;
+	u32 register_writes_size;
+	u32 page_table_dump_size;
+	u32 *buffer;
+};
+
+static _mali_osk_errcode_t writereg(u32 where, u32 what, const char * comment, struct dump_info * info, int dump_to_serial)
+{
+	if (dump_to_serial) MALI_DEBUG_PRINT(1, ("writereg %08X %08X # %s\n", where, what, comment));
+
+	if (NULL != info)
+	{
+		info->register_writes_size += sizeof(u32)*2; /* two 32-bit words */
+
+		if (NULL != info->buffer)
+		{
+			/* check that we have enough space */
+			if (info->buffer_left < sizeof(u32)*2) MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+
+			*info->buffer = where;
+			info->buffer++;
+
+			*info->buffer = what;
+			info->buffer++;
+
+			info->buffer_left -= sizeof(u32)*2;
+		}
+	}
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t dump_page(mali_io_address page, u32 phys_addr, struct dump_info * info, int dump_to_serial)
+{
+	if (dump_to_serial)
+	{
+		int i;
+		for (i = 0; i < 256; i++)
+		{
+			MALI_DEBUG_PRINT(1, ("%08X: %08X %08X %08X %08X\n", phys_addr + 16*i, _mali_osk_mem_ioread32(page, (i*4 + 0) * sizeof(u32)),
+			                                                                      _mali_osk_mem_ioread32(page, (i*4 + 1) * sizeof(u32)),
+			                                                                      _mali_osk_mem_ioread32(page, (i*4 + 2) * sizeof(u32)),
+			                                                                      _mali_osk_mem_ioread32(page, (i*4 + 3) * sizeof(u32))));
+
+		}
+	}
+
+	if (NULL != info)
+	{
+		/* 4096 for the page and 4 bytes for the address */
+		const u32 page_size_in_elements = MALI_MMU_PAGE_SIZE / 4;
+		const u32 page_size_in_bytes = MALI_MMU_PAGE_SIZE;
+		const u32 dump_size_in_bytes = MALI_MMU_PAGE_SIZE + 4;
+
+		info->page_table_dump_size += dump_size_in_bytes;
+
+		if (NULL != info->buffer)
+		{
+			if (info->buffer_left < dump_size_in_bytes) MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+
+			*info->buffer = phys_addr;
+			info->buffer++;
+
+			_mali_osk_memcpy(info->buffer, page, page_size_in_bytes);
+			info->buffer += page_size_in_elements;
+
+			info->buffer_left -= dump_size_in_bytes;
+		}
+	}
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t dump_mmu_page_table(memory_session * session_data, struct dump_info * info)
+{
+	MALI_DEBUG_ASSERT_POINTER(session_data);
+	MALI_DEBUG_ASSERT_POINTER(info);
+
+	if (NULL != session_data->page_directory_mapped)
+	{
+		int i;
+
+		MALI_CHECK_NO_ERROR(
+            dump_page(session_data->page_directory_mapped, session_data->page_directory, info, 0)
+            );
+
+		for (i = 0; i < 1024; i++)
+		{
+			if (NULL != session_data->page_entries_mapped[i])
+			{
+				MALI_CHECK_NO_ERROR(
+                    dump_page(session_data->page_entries_mapped[i], _mali_osk_mem_ioread32(session_data->page_directory_mapped, i * sizeof(u32)) & ~MALI_MMU_FLAGS_MASK, info, 0)
+                    );
+			}
+		}
+	}
+
+	MALI_SUCCESS;
+}
+
+static _mali_osk_errcode_t dump_mmu_registers(memory_session * session_data, struct dump_info * info)
+{
+	MALI_CHECK_NO_ERROR(writereg(0x00000000, session_data->page_directory, "set the page directory address", info, 0));
+	MALI_CHECK_NO_ERROR(writereg(0x00000008, 4, "zap???", info, 0));
+	MALI_CHECK_NO_ERROR(writereg(0x00000008, 0, "enable paging", info, 0));
+    MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t _mali_ukk_query_mmu_page_table_dump_size( _mali_uk_query_mmu_page_table_dump_size_s *args )
+{
+	struct dump_info info = { 0, 0, 0, NULL };
+	memory_session * session_data;
+
+    MALI_DEBUG_ASSERT_POINTER(args);
+  	MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+
+    session_data = (memory_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_memory_id);
+
+    MALI_CHECK_NO_ERROR(dump_mmu_registers(session_data, &info));
+	MALI_CHECK_NO_ERROR(dump_mmu_page_table(session_data, &info));
+	args->size = info.register_writes_size + info.page_table_dump_size;
+    MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t _mali_ukk_dump_mmu_page_table( _mali_uk_dump_mmu_page_table_s * args )
+{
+	struct dump_info info = { 0, 0, 0, NULL };
+	memory_session * session_data;
+
+  	MALI_DEBUG_ASSERT_POINTER(args);
+  	MALI_CHECK_NON_NULL(args->ctx, _MALI_OSK_ERR_INVALID_ARGS);
+    MALI_CHECK_NON_NULL(args->buffer, _MALI_OSK_ERR_INVALID_ARGS);
+
+    session_data = (memory_session *)mali_kernel_session_manager_slot_get(args->ctx, mali_subsystem_memory_id);
+
+	info.buffer_left = args->size;
+	info.buffer = args->buffer;
+
+	args->register_writes = info.buffer;
+	MALI_CHECK_NO_ERROR(dump_mmu_registers(session_data, &info));
+
+	args->page_table_dump = info.buffer;
+	MALI_CHECK_NO_ERROR(dump_mmu_page_table(session_data, &info));
+
+	args->register_writes_size = info.register_writes_size;
+	args->page_table_dump_size = info.page_table_dump_size;
+
+    MALI_SUCCESS;
+}
+
+/**
+ * Stub function to satisfy UDD interface exclusion requirement.
+ * This is because the Base code compiles in \b both MMU and non-MMU calls,
+ * so both sets must be declared (but the 'unused' set may be stub)
+ */
+_mali_osk_errcode_t _mali_ukk_get_big_block( _mali_uk_get_big_block_s *args )
+{
+	MALI_IGNORE( args );
+	return _MALI_OSK_ERR_FAULT;
+}
+
+/**
+ * Stub function to satisfy UDD interface exclusion requirement.
+ * This is because the Base code compiles in \b both MMU and non-MMU calls,
+ * so both sets must be declared (but the 'unused' set may be stub)
+ */
+_mali_osk_errcode_t _mali_ukk_free_big_block( _mali_uk_free_big_block_s *args )
+{
+	MALI_IGNORE( args );
+	return _MALI_OSK_ERR_FAULT;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_mmu.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_mmu.h
new file mode 100644
index 00000000000..a199fd66b5a
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_mmu.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_MEM_MMU_H__
+#define __MALI_KERNEL_MEM_MMU_H__
+
+#include "mali_kernel_session_manager.h"
+
+/**
+ * Lookup a MMU core by ID.
+ * @param id ID of the MMU to find
+ * @return NULL if ID not found or valid, non-NULL if a core was found.
+ */
+void* mali_memory_core_mmu_lookup(u32 id);
+
+/**
+ * Activate a user session with its address space on the given MMU.
+ * If the session can't be activated due to that the MMU is busy and
+ * a callback pointer is given, the callback will be called once the MMU becomes idle.
+ * If the same callback pointer is registered multiple time it will only be called once.
+ * Nested activations are supported.
+ * Each call must be matched by a call to @see mali_memory_core_mmu_release_address_space_reference
+ *
+ * @param mmu The MMU to activate the address space on
+ * @param mali_session_data The user session object which address space to activate
+ * @param callback Pointer to the function to call when the MMU becomes idle
+ * @param callback_arg Argument given to the callback
+ * @return 0 if the address space was activated, -EBUSY if the MMU was busy, -EFAULT in all other cases.
+ */
+int mali_memory_core_mmu_activate_page_table(void* mmu_ptr, struct mali_session_data * mali_session_data, void(*callback)(void*), void * callback_argument);
+
+/**
+ * Release a reference to the current active address space.
+ * Once the last reference is released any callback(s) registered will be called before the function returns
+ *
+ * @note Caution must be shown calling this function with locks held due to that callback can be called
+ * @param mmu The mmu to release a reference to the active address space of
+ */
+void mali_memory_core_mmu_release_address_space_reference(void* mmu);
+
+/**
+ * Soft reset of MMU - needed after power up
+ *
+ * @param mmu_ptr The MMU pointer registered with the relevant core
+ */
+void mali_kernel_mmu_reset(void * mmu_ptr);
+
+void mali_kernel_mmu_force_bus_reset(void * mmu_ptr);
+
+/**
+ * Unregister a previously registered callback.
+ * @param mmu The MMU to unregister the callback on
+ * @param callback The function to unregister
+ */
+void mali_memory_core_mmu_unregister_callback(void* mmu, void(*callback)(void*));
+
+
+
+#endif /* __MALI_KERNEL_MEM_MMU_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_os.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_os.c
new file mode 100644
index 00000000000..845de935ec9
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_os.c
@@ -0,0 +1,309 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_kernel_core.h"
+#include "mali_kernel_memory_engine.h"
+#include "mali_osk.h"
+
+typedef struct os_allocation
+{
+	u32 num_pages;
+	u32 offset_start;
+	mali_allocation_engine * engine;
+	mali_memory_allocation * descriptor;
+} os_allocation;
+
+typedef struct os_allocator
+{
+	_mali_osk_lock_t *mutex;
+
+	/**
+	 * Maximum number of pages to allocate from the OS
+	 */
+	u32 num_pages_max;
+
+	/**
+	 * Number of pages allocated from the OS
+	 */
+	u32 num_pages_allocated;
+
+	/** CPU Usage adjustment (add to mali physical address to get cpu physical address) */
+	u32 cpu_usage_adjust;
+} os_allocator;
+
+static mali_physical_memory_allocation_result os_allocator_allocate(void* ctx, mali_allocation_engine * engine,  mali_memory_allocation * descriptor, u32* offset, mali_physical_memory_allocation * alloc_info);
+static mali_physical_memory_allocation_result os_allocator_allocate_page_table_block(void * ctx, mali_page_table_block * block);
+static void os_allocator_release(void * ctx, void * handle);
+static void os_allocator_page_table_block_release( mali_page_table_block *page_table_block );
+static void os_allocator_destroy(mali_physical_memory_allocator * allocator);
+
+mali_physical_memory_allocator * mali_os_allocator_create(u32 max_allocation, u32 cpu_usage_adjust, const char *name)
+{
+	mali_physical_memory_allocator * allocator;
+	os_allocator * info;
+
+	max_allocation = (max_allocation + _MALI_OSK_CPU_PAGE_SIZE-1) & ~(_MALI_OSK_CPU_PAGE_SIZE-1);
+
+	MALI_DEBUG_PRINT(2, ("Mali OS memory allocator created with max allocation size of 0x%X bytes, cpu_usage_adjust 0x%08X\n", max_allocation, cpu_usage_adjust));
+
+	allocator = _mali_osk_malloc(sizeof(mali_physical_memory_allocator));
+	if (NULL != allocator)
+	{
+		info = _mali_osk_malloc(sizeof(os_allocator));
+		if (NULL != info)
+		{
+			info->num_pages_max = max_allocation / _MALI_OSK_CPU_PAGE_SIZE;
+			info->num_pages_allocated = 0;
+			info->cpu_usage_adjust = cpu_usage_adjust;
+
+			info->mutex = _mali_osk_lock_init( _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE | _MALI_OSK_LOCKFLAG_ORDERED, 0, 106);
+            if (NULL != info->mutex)
+            {
+			    allocator->allocate = os_allocator_allocate;
+			    allocator->allocate_page_table_block = os_allocator_allocate_page_table_block;
+			    allocator->destroy = os_allocator_destroy;
+			    allocator->ctx = info;
+				allocator->name = name;
+
+			    return allocator;
+            }
+            _mali_osk_free(info);
+		}
+		_mali_osk_free(allocator);
+	}
+
+	return NULL;
+}
+
+static void os_allocator_destroy(mali_physical_memory_allocator * allocator)
+{
+	os_allocator * info;
+	MALI_DEBUG_ASSERT_POINTER(allocator);
+	MALI_DEBUG_ASSERT_POINTER(allocator->ctx);
+	info = (os_allocator*)allocator->ctx;
+	_mali_osk_lock_term(info->mutex);
+	_mali_osk_free(info);
+	_mali_osk_free(allocator);
+}
+
+static mali_physical_memory_allocation_result os_allocator_allocate(void* ctx, mali_allocation_engine * engine,  mali_memory_allocation * descriptor, u32* offset, mali_physical_memory_allocation * alloc_info)
+{
+	mali_physical_memory_allocation_result result = MALI_MEM_ALLOC_NONE;
+	u32 left;
+	os_allocator * info;
+	os_allocation * allocation;
+	int pages_allocated = 0;
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_OK;
+
+	MALI_DEBUG_ASSERT_POINTER(ctx);
+	MALI_DEBUG_ASSERT_POINTER(engine);
+	MALI_DEBUG_ASSERT_POINTER(descriptor);
+	MALI_DEBUG_ASSERT_POINTER(offset);
+	MALI_DEBUG_ASSERT_POINTER(alloc_info);
+
+	info = (os_allocator*)ctx;
+	left = descriptor->size - *offset;
+
+	if (_MALI_OSK_ERR_OK != _mali_osk_lock_wait(info->mutex, _MALI_OSK_LOCKMODE_RW)) return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+
+	/** @note this code may not work on Linux, or may require a more complex Linux implementation */
+	allocation = _mali_osk_malloc(sizeof(os_allocation));
+	if (NULL != allocation)
+	{
+		u32 os_mem_max_usage = info->num_pages_max * _MALI_OSK_CPU_PAGE_SIZE;
+		allocation->offset_start = *offset;
+		allocation->num_pages = ((left + _MALI_OSK_CPU_PAGE_SIZE - 1) & ~(_MALI_OSK_CPU_PAGE_SIZE - 1)) >> _MALI_OSK_CPU_PAGE_ORDER;
+		MALI_DEBUG_PRINT(6, ("Allocating page array of size %d bytes\n", allocation->num_pages * sizeof(struct page*)));
+
+		while (left > 0 && ((info->num_pages_allocated + pages_allocated) < info->num_pages_max) && _mali_osk_mem_check_allocated(os_mem_max_usage))
+		{
+			err = mali_allocation_engine_map_physical(engine, descriptor, *offset, MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC, info->cpu_usage_adjust, _MALI_OSK_CPU_PAGE_SIZE);
+			if ( _MALI_OSK_ERR_OK != err)
+			{
+				if (  _MALI_OSK_ERR_NOMEM == err)
+				{
+					/* 'Partial' allocation (or, out-of-memory on first page) */
+					break;
+				}
+
+				MALI_DEBUG_PRINT(1, ("Mapping of physical memory failed\n"));
+
+				/* Fatal error, cleanup any previous pages allocated. */
+				if ( pages_allocated > 0 )
+				{
+					mali_allocation_engine_unmap_physical( engine, descriptor, allocation->offset_start, _MALI_OSK_CPU_PAGE_SIZE*pages_allocated, _MALI_OSK_MEM_MAPREGION_FLAG_OS_ALLOCATED_PHYSADDR );
+					/* (*offset) doesn't need to be restored; it will not be used by the caller on failure */
+				}
+
+				pages_allocated = 0;
+
+				result = MALI_MEM_ALLOC_INTERNAL_FAILURE;
+				break;
+			}
+
+			/* Loop iteration */
+			if (left < _MALI_OSK_CPU_PAGE_SIZE) left = 0;
+			else left -= _MALI_OSK_CPU_PAGE_SIZE;
+
+			pages_allocated++;
+
+			*offset += _MALI_OSK_CPU_PAGE_SIZE;
+		}
+
+		/* Loop termination; decide on result */
+		if (pages_allocated)
+		{
+			MALI_DEBUG_PRINT(6, ("Allocated %d pages\n", pages_allocated));
+			if (left) result = MALI_MEM_ALLOC_PARTIAL;
+			else result = MALI_MEM_ALLOC_FINISHED;
+
+            /* Some OS do not perform a full cache flush (including all outer caches) for uncached mapped memory.
+             * They zero the memory through a cached mapping, then flush the inner caches but not the outer caches.
+             * This is required for MALI to have the correct view of the memory. 
+             */
+            _mali_osk_cache_ensure_uncached_range_flushed( (void *)descriptor, allocation->offset_start, pages_allocated *_MALI_OSK_CPU_PAGE_SIZE );
+			allocation->num_pages = pages_allocated;
+			allocation->engine = engine;         /* Necessary to make the engine's unmap call */
+			allocation->descriptor = descriptor; /* Necessary to make the engine's unmap call */
+			info->num_pages_allocated += pages_allocated;
+
+			MALI_DEBUG_PRINT(6, ("%d out of %d pages now allocated\n", info->num_pages_allocated, info->num_pages_max));
+
+			alloc_info->ctx = info;
+			alloc_info->handle = allocation;
+			alloc_info->release = os_allocator_release;
+		}
+		else
+		{
+			MALI_DEBUG_PRINT(6, ("Releasing pages array due to no pages allocated\n"));
+			_mali_osk_free( allocation );
+		}
+	}
+
+	_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+
+	return result;
+}
+
+static void os_allocator_release(void * ctx, void * handle)
+{
+	os_allocator * info;
+	os_allocation * allocation;
+	mali_allocation_engine * engine;
+	mali_memory_allocation * descriptor;
+
+	MALI_DEBUG_ASSERT_POINTER(ctx);
+	MALI_DEBUG_ASSERT_POINTER(handle);
+
+	info = (os_allocator*)ctx;
+	allocation = (os_allocation*)handle;
+	engine = allocation->engine;
+	descriptor = allocation->descriptor;
+
+	MALI_DEBUG_ASSERT_POINTER( engine );
+	MALI_DEBUG_ASSERT_POINTER( descriptor );
+
+	if (_MALI_OSK_ERR_OK != _mali_osk_lock_wait(info->mutex, _MALI_OSK_LOCKMODE_RW))
+	{
+		MALI_DEBUG_PRINT(1, ("allocator release: Failed to get mutex\n"));
+		return;
+	}
+
+	MALI_DEBUG_PRINT(6, ("Releasing %d os pages\n", allocation->num_pages));
+
+	MALI_DEBUG_ASSERT( allocation->num_pages <= info->num_pages_allocated);
+	info->num_pages_allocated -= allocation->num_pages;
+
+	mali_allocation_engine_unmap_physical( engine, descriptor, allocation->offset_start, _MALI_OSK_CPU_PAGE_SIZE*allocation->num_pages, _MALI_OSK_MEM_MAPREGION_FLAG_OS_ALLOCATED_PHYSADDR );
+
+	_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+
+	_mali_osk_free(allocation);
+}
+
+static mali_physical_memory_allocation_result os_allocator_allocate_page_table_block(void * ctx, mali_page_table_block * block)
+{
+	const int allocation_order = 6; /* _MALI_OSK_CPU_PAGE_SIZE << 6 */
+	void *virt;
+	const u32 pages_to_allocate = 1 << allocation_order;
+	const u32 size = _MALI_OSK_CPU_PAGE_SIZE << allocation_order;
+	os_allocator * info;
+
+	u32 cpu_phys_base;
+
+	MALI_DEBUG_ASSERT_POINTER(ctx);
+	info = (os_allocator*)ctx;
+
+	/* Ensure we don't allocate more than we're supposed to from the ctx */
+	if (_MALI_OSK_ERR_OK != _mali_osk_lock_wait(info->mutex, _MALI_OSK_LOCKMODE_RW)) return MALI_MEM_ALLOC_INTERNAL_FAILURE;
+
+	if ( (info->num_pages_allocated + pages_to_allocate > info->num_pages_max) && _mali_osk_mem_check_allocated(info->num_pages_max * _MALI_OSK_CPU_PAGE_SIZE) )
+	{
+		/* return OOM */
+		_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+		return MALI_MEM_ALLOC_NONE;
+	}
+
+	virt = _mali_osk_mem_allocioregion( &cpu_phys_base, size );
+
+	if ( NULL == virt )
+	{
+		/* return OOM */
+		_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+		return MALI_MEM_ALLOC_NONE;
+	}
+
+	block->release = os_allocator_page_table_block_release;
+	block->ctx = ctx;
+	block->handle = (void*)allocation_order;
+	block->size = size;
+	block->phys_base = cpu_phys_base - info->cpu_usage_adjust;
+	block->mapping = virt;
+
+	info->num_pages_allocated += pages_to_allocate;
+
+	_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+
+	return MALI_MEM_ALLOC_FINISHED;
+}
+
+static void os_allocator_page_table_block_release( mali_page_table_block *page_table_block )
+{
+	os_allocator * info;
+	u32 allocation_order;
+	u32 pages_allocated;
+
+	MALI_DEBUG_ASSERT_POINTER( page_table_block );
+
+	info = (os_allocator*)page_table_block->ctx;
+
+	MALI_DEBUG_ASSERT_POINTER( info );
+
+	allocation_order = (u32)page_table_block->handle;
+
+	pages_allocated = 1 << allocation_order;
+
+	MALI_DEBUG_ASSERT( pages_allocated * _MALI_OSK_CPU_PAGE_SIZE == page_table_block->size );
+
+	if (_MALI_OSK_ERR_OK != _mali_osk_lock_wait(info->mutex, _MALI_OSK_LOCKMODE_RW))
+	{
+		MALI_DEBUG_PRINT(1, ("allocator release: Failed to get mutex\n"));
+		return;
+	}
+
+	MALI_DEBUG_ASSERT( pages_allocated <= info->num_pages_allocated);
+	info->num_pages_allocated -= pages_allocated;
+
+	/* Adjust phys_base from mali physical address to CPU physical address */
+	_mali_osk_mem_freeioregion( page_table_block->phys_base + info->cpu_usage_adjust, page_table_block->size, page_table_block->mapping );
+
+	_mali_osk_lock_signal(info->mutex, _MALI_OSK_LOCKMODE_RW);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_os.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_os.h
new file mode 100644
index 00000000000..ff49f8a816a
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_mem_os.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_MEM_OS_H__
+#define __MALI_KERNEL_MEM_OS_H__
+
+/**
+ * @brief Creates an object that manages allocating OS memory
+ *
+ * Creates an object that provides an interface to allocate OS memory and
+ * have it mapped into the Mali virtual memory space.
+ *
+ * The object exposes pointers to
+ * - allocate OS memory
+ * - allocate Mali page tables in OS memory
+ * - destroy the object
+ *
+ * Allocations from OS memory are of type mali_physical_memory_allocation
+ * which provides a function to release the allocation.
+ *
+ * @param max_allocation max. number of bytes that can be allocated from OS memory
+ * @param cpu_usage_adjust value to add to mali physical addresses to obtain CPU physical addresses
+ * @param name description of the allocator
+ * @return pointer to mali_physical_memory_allocator object. NULL on failure.
+ **/
+mali_physical_memory_allocator * mali_os_allocator_create(u32 max_allocation, u32 cpu_usage_adjust, const char *name);
+
+#endif /* __MALI_KERNEL_MEM_OS_H__ */
+
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_memory_engine.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_memory_engine.c
new file mode 100644
index 00000000000..3b4ace05f01
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_memory_engine.c
@@ -0,0 +1,348 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_kernel_core.h"
+#include "mali_kernel_memory_engine.h"
+#include "mali_osk.h"
+#include "mali_osk_list.h"
+
+typedef struct memory_engine
+{
+	mali_kernel_mem_address_manager * mali_address;
+	mali_kernel_mem_address_manager * process_address;
+} memory_engine;
+
+mali_allocation_engine mali_allocation_engine_create(mali_kernel_mem_address_manager * mali_address_manager, mali_kernel_mem_address_manager * process_address_manager)
+{
+	memory_engine * engine;
+
+	/* Mali Address Manager need not support unmap_physical */
+	MALI_DEBUG_ASSERT_POINTER(mali_address_manager);
+	MALI_DEBUG_ASSERT_POINTER(mali_address_manager->allocate);
+	MALI_DEBUG_ASSERT_POINTER(mali_address_manager->release);
+	MALI_DEBUG_ASSERT_POINTER(mali_address_manager->map_physical);
+
+	/* Process Address Manager must support unmap_physical for OS allocation
+	 * error path handling */
+	MALI_DEBUG_ASSERT_POINTER(process_address_manager);
+	MALI_DEBUG_ASSERT_POINTER(process_address_manager->allocate);
+	MALI_DEBUG_ASSERT_POINTER(process_address_manager->release);
+	MALI_DEBUG_ASSERT_POINTER(process_address_manager->map_physical);
+	MALI_DEBUG_ASSERT_POINTER(process_address_manager->unmap_physical);
+
+
+	engine = (memory_engine*)_mali_osk_malloc(sizeof(memory_engine));
+	if (NULL == engine) return NULL;
+
+	engine->mali_address = mali_address_manager;
+	engine->process_address = process_address_manager;
+
+	return (mali_allocation_engine)engine;
+}
+
+void mali_allocation_engine_destroy(mali_allocation_engine engine)
+{
+	MALI_DEBUG_ASSERT_POINTER(engine);
+	_mali_osk_free(engine);
+}
+
+_mali_osk_errcode_t mali_allocation_engine_allocate_memory(mali_allocation_engine mem_engine, mali_memory_allocation * descriptor, mali_physical_memory_allocator * physical_allocators, _mali_osk_list_t *tracking_list )
+{
+	memory_engine * engine = (memory_engine*)mem_engine;
+
+	MALI_DEBUG_ASSERT_POINTER(engine);
+	MALI_DEBUG_ASSERT_POINTER(descriptor);
+	MALI_DEBUG_ASSERT_POINTER(physical_allocators);
+	/* ASSERT that the list member has been initialized, even if it won't be
+	 * used for tracking. We need it to be initialized to see if we need to
+	 * delete it from a list in the release function. */
+	MALI_DEBUG_ASSERT( NULL != descriptor->list.next && NULL != descriptor->list.prev );
+
+	if (_MALI_OSK_ERR_OK == engine->mali_address->allocate(descriptor))
+	{
+		_mali_osk_errcode_t res = _MALI_OSK_ERR_OK;
+		if ( descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE )
+		{
+			res = engine->process_address->allocate(descriptor);
+		}
+		if ( _MALI_OSK_ERR_OK == res )
+		{
+			/* address space setup OK, commit physical memory to the allocation */
+			mali_physical_memory_allocator * active_allocator = physical_allocators;
+			struct mali_physical_memory_allocation * active_allocation_tracker = &descriptor->physical_allocation;
+			u32 offset = 0;
+
+			while ( NULL != active_allocator )
+			{
+				switch (active_allocator->allocate(active_allocator->ctx, mem_engine, descriptor, &offset, active_allocation_tracker))
+				{
+					case MALI_MEM_ALLOC_FINISHED:
+						if ( NULL != tracking_list )
+						{
+							/* Insert into the memory session list */
+							/* ASSERT that it is not already part of a list */
+							MALI_DEBUG_ASSERT( _mali_osk_list_empty( &descriptor->list ) );
+							_mali_osk_list_add( &descriptor->list, tracking_list );
+						}
+
+						MALI_SUCCESS; /* all done */
+					case MALI_MEM_ALLOC_NONE:
+						/* reuse current active_allocation_tracker */
+						MALI_DEBUG_PRINT( 4, ("Memory Engine Allocate: No allocation on %s, resorting to %s\n",
+											  ( active_allocator->name ) ? active_allocator->name : "UNNAMED",
+											  ( active_allocator->next ) ? (( active_allocator->next->name )? active_allocator->next->name : "UNNAMED") : "NONE") );
+						active_allocator = active_allocator->next;
+						break;
+					case MALI_MEM_ALLOC_PARTIAL:
+						if (NULL != active_allocator->next)
+						{
+							/* need a new allocation tracker */
+							active_allocation_tracker->next = _mali_osk_calloc(1, sizeof(mali_physical_memory_allocation));
+							if (NULL != active_allocation_tracker->next)
+							{
+								active_allocation_tracker = active_allocation_tracker->next;
+								MALI_DEBUG_PRINT( 2, ("Memory Engine Allocate: Partial allocation on %s, resorting to %s\n",
+													  ( active_allocator->name ) ? active_allocator->name : "UNNAMED",
+													  ( active_allocator->next ) ? (( active_allocator->next->name )? active_allocator->next->name : "UNNAMED") : "NONE") );
+								active_allocator = active_allocator->next;
+								break;
+							}
+						}
+					   /* FALL THROUGH */
+					case MALI_MEM_ALLOC_INTERNAL_FAILURE:
+					   active_allocator = NULL; /* end the while loop */
+					   break;
+				}
+			}
+
+			MALI_DEBUG_PRINT(3, ("Non-fatal OOM, have to cleanup, stopped at offset %d for size %d\n", offset, descriptor->size));
+
+			/* allocation failure, start cleanup */
+			/* loop over any potential partial allocations */
+			active_allocation_tracker = &descriptor->physical_allocation;
+			while (NULL != active_allocation_tracker)
+			{
+				/* handle blank trackers which will show up during failure */
+				if (NULL != active_allocation_tracker->release)
+				{
+					active_allocation_tracker->release(active_allocation_tracker->ctx, active_allocation_tracker->handle);
+				}
+				active_allocation_tracker = active_allocation_tracker->next;
+			}
+
+			/* free the allocation tracker objects themselves, skipping the tracker stored inside the descriptor itself */
+			for ( active_allocation_tracker = descriptor->physical_allocation.next; active_allocation_tracker != NULL; )
+			{
+				void * buf = active_allocation_tracker;
+				active_allocation_tracker = active_allocation_tracker->next;
+				_mali_osk_free(buf);
+			}
+
+			/* release the address spaces */
+
+			if ( descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE )
+			{
+				engine->process_address->release(descriptor);
+			}
+		}
+		engine->mali_address->release(descriptor);
+	}
+
+	MALI_ERROR(_MALI_OSK_ERR_FAULT);
+}
+
+void mali_allocation_engine_release_memory(mali_allocation_engine mem_engine, mali_memory_allocation * descriptor)
+{
+	memory_engine * engine = (memory_engine*)mem_engine;
+	mali_physical_memory_allocation * active_allocation_tracker;
+
+	MALI_DEBUG_ASSERT_POINTER(engine);
+	MALI_DEBUG_ASSERT_POINTER(descriptor);
+
+	/* Determine whether we need to remove this from a tracking list */
+	if ( ! _mali_osk_list_empty( &descriptor->list ) )
+	{
+		_mali_osk_list_del( &descriptor->list );
+		/* Clear the list for debug mode, catch use-after-free */
+		MALI_DEBUG_CODE( descriptor->list.next = descriptor->list.prev = NULL; )
+	}
+
+	engine->mali_address->release(descriptor);
+
+	active_allocation_tracker = &descriptor->physical_allocation;
+	while (NULL != active_allocation_tracker)
+	{
+		MALI_DEBUG_ASSERT_POINTER(active_allocation_tracker->release);
+		active_allocation_tracker->release(active_allocation_tracker->ctx, active_allocation_tracker->handle);
+		active_allocation_tracker = active_allocation_tracker->next;
+	}
+
+	/* free the allocation tracker objects themselves, skipping the tracker stored inside the descriptor itself */
+	for ( active_allocation_tracker = descriptor->physical_allocation.next; active_allocation_tracker != NULL; )
+	{
+		void * buf = active_allocation_tracker;
+		active_allocation_tracker = active_allocation_tracker->next;
+		_mali_osk_free(buf);
+	}
+
+	if ( descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE )
+	{
+		engine->process_address->release(descriptor);
+	}
+}
+
+
+_mali_osk_errcode_t mali_allocation_engine_map_physical(mali_allocation_engine mem_engine, mali_memory_allocation * descriptor, u32 offset, u32 phys, u32 cpu_usage_adjust, u32 size)
+{
+	_mali_osk_errcode_t err;
+	memory_engine * engine = (memory_engine*)mem_engine;
+	_mali_osk_mem_mapregion_flags_t unmap_flags = (_mali_osk_mem_mapregion_flags_t)0;
+
+	MALI_DEBUG_ASSERT_POINTER(engine);
+	MALI_DEBUG_ASSERT_POINTER(descriptor);
+
+	MALI_DEBUG_PRINT(7, ("Mapping phys 0x%08X length 0x%08X at offset 0x%08X\n", phys, size, offset));
+
+	MALI_DEBUG_ASSERT_POINTER(engine->mali_address);
+	MALI_DEBUG_ASSERT_POINTER(engine->mali_address->map_physical);
+
+	/* Handle process address manager first, because we may need them to
+	 * allocate the physical page */
+	if ( descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE )
+	{
+		/* Handle OS-allocated specially, since an adjustment may be required */
+		if ( MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC == phys )
+		{
+			MALI_DEBUG_ASSERT( _MALI_OSK_CPU_PAGE_SIZE == size );
+
+			/* Set flags to use on error path */
+			unmap_flags |= _MALI_OSK_MEM_MAPREGION_FLAG_OS_ALLOCATED_PHYSADDR;
+
+			err = engine->process_address->map_physical(descriptor, offset, &phys, size);
+			/* Adjust for cpu physical address to mali physical address */
+			phys -= cpu_usage_adjust;
+		}
+		else
+		{
+			u32 cpu_phys;
+			/* Adjust mali physical address to cpu physical address */
+			cpu_phys = phys + cpu_usage_adjust;
+			err = engine->process_address->map_physical(descriptor, offset, &cpu_phys, size);
+		}
+
+		if ( _MALI_OSK_ERR_OK != err )
+		{
+			MALI_ERROR( err );
+		}
+	}
+
+	MALI_DEBUG_PRINT(4, ("Mapping phys 0x%08X length 0x%08X at offset 0x%08X to CPUVA 0x%08X\n", phys, size, offset, (u32)(descriptor->mapping) + offset));
+
+	/* Mali address manager must use the physical address - no point in asking
+	 * it to allocate another one for us */
+	MALI_DEBUG_ASSERT( MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC != phys );
+
+	err = engine->mali_address->map_physical(descriptor, offset, &phys, size);
+
+	if ( _MALI_OSK_ERR_OK != err )
+	{
+		if ( descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE )
+		{
+			MALI_DEBUG_PRINT( 2, ("Process address manager succeeded, but Mali Address manager failed for phys=0x%08X size=0x%08X, offset=0x%08X. Will unmap.\n", phys, size, offset));
+			engine->process_address->unmap_physical(descriptor, offset, size, unmap_flags);
+		}
+
+		MALI_ERROR( err );
+	}
+
+	MALI_SUCCESS;
+}
+
+void mali_allocation_engine_unmap_physical(mali_allocation_engine mem_engine, mali_memory_allocation * descriptor, u32 offset, u32 size, _mali_osk_mem_mapregion_flags_t unmap_flags )
+{
+	memory_engine * engine = (memory_engine*)mem_engine;
+
+	MALI_DEBUG_ASSERT_POINTER(engine);
+	MALI_DEBUG_ASSERT_POINTER(descriptor);
+
+	MALI_DEBUG_PRINT(7, ("UnMapping length 0x%08X at offset 0x%08X\n", size, offset));
+
+	MALI_DEBUG_ASSERT_POINTER(engine->mali_address);
+	MALI_DEBUG_ASSERT_POINTER(engine->process_address);
+
+	if ( descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE )
+	{
+		/* Mandetory for process_address manager to have an unmap function*/
+		engine->process_address->unmap_physical( descriptor, offset, size, unmap_flags );
+	}
+
+	/* Optional for mali_address manager to have an unmap function*/
+	if ( NULL != engine->mali_address->unmap_physical )
+	{
+		engine->mali_address->unmap_physical( descriptor, offset, size, unmap_flags );
+	}
+}
+
+
+_mali_osk_errcode_t mali_allocation_engine_allocate_page_tables(mali_allocation_engine engine, mali_page_table_block * descriptor, mali_physical_memory_allocator * physical_provider)
+{
+	mali_physical_memory_allocator * active_allocator = physical_provider;
+
+	MALI_DEBUG_ASSERT_POINTER(descriptor);
+	MALI_DEBUG_ASSERT_POINTER(physical_provider);
+
+	while ( NULL != active_allocator )
+	{
+		switch (active_allocator->allocate_page_table_block(active_allocator->ctx, descriptor))
+		{
+			case MALI_MEM_ALLOC_FINISHED:
+				MALI_SUCCESS; /* all done */
+			case MALI_MEM_ALLOC_NONE:
+				/* try next */
+				MALI_DEBUG_PRINT( 2, ("Memory Engine Allocate PageTables: No allocation on %s, resorting to %s\n",
+									  ( active_allocator->name ) ? active_allocator->name : "UNNAMED",
+									  ( active_allocator->next ) ? (( active_allocator->next->name )? active_allocator->next->name : "UNNAMED") : "NONE") );
+				active_allocator = active_allocator->next;
+				break;
+			case MALI_MEM_ALLOC_PARTIAL:
+				MALI_DEBUG_PRINT(1, ("Invalid return value from allocate_page_table_block call: MALI_MEM_ALLOC_PARTIAL\n"));
+				/* FALL THROUGH */
+			case MALI_MEM_ALLOC_INTERNAL_FAILURE:
+				MALI_DEBUG_PRINT(1, ("Aborting due to allocation failure\n"));
+				active_allocator = NULL; /* end the while loop */
+				break;
+		}
+	}
+
+	MALI_ERROR(_MALI_OSK_ERR_FAULT);
+}
+
+
+void mali_allocation_engine_report_allocators( mali_physical_memory_allocator * physical_provider )
+{
+	mali_physical_memory_allocator * active_allocator = physical_provider;
+	MALI_DEBUG_ASSERT_POINTER(physical_provider);
+
+	MALI_DEBUG_PRINT( 1, ("Mali memory allocators will be used in this order of preference (lowest numbered first) :\n"));
+	while ( NULL != active_allocator )
+	{
+		if ( NULL != active_allocator->name )
+		{
+			MALI_DEBUG_PRINT( 1, ("\t%d: %s\n", active_allocator->alloc_order, active_allocator->name) );
+		}
+		else
+		{
+			MALI_DEBUG_PRINT( 1, ("\t%d: (UNNAMED ALLOCATOR)\n", active_allocator->alloc_order) );
+		}
+		active_allocator = active_allocator->next;
+	}
+
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_memory_engine.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_memory_engine.h
new file mode 100644
index 00000000000..80a2b4bb3a7
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_memory_engine.h
@@ -0,0 +1,145 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_MEMORY_ENGINE_H__
+#define  __MALI_KERNEL_MEMORY_ENGINE_H__
+
+typedef void * mali_allocation_engine;
+
+typedef enum { MALI_MEM_ALLOC_FINISHED, MALI_MEM_ALLOC_PARTIAL, MALI_MEM_ALLOC_NONE, MALI_MEM_ALLOC_INTERNAL_FAILURE } mali_physical_memory_allocation_result;
+
+typedef struct mali_physical_memory_allocation
+{
+	void (*release)(void * ctx, void * handle); /**< Function to call on to release the physical memory */
+	void * ctx;
+	void * handle;
+	struct mali_physical_memory_allocation * next;
+} mali_physical_memory_allocation;
+
+struct mali_page_table_block;
+
+typedef struct mali_page_table_block
+{
+	void (*release)(struct mali_page_table_block *page_table_block);
+	void * ctx;
+	void * handle;
+	u32 size; /**< In bytes, should be a multiple of MALI_MMU_PAGE_SIZE to avoid internal fragementation */
+	u32 phys_base; /**< Mali physical address */
+	mali_io_address mapping;
+} mali_page_table_block;
+
+
+/** @addtogroup _mali_osk_low_level_memory
+ * @{ */
+
+typedef enum
+{
+	MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE = 0x1,
+	MALI_MEMORY_ALLOCATION_FLAG_MAP_GUARD_PAGE     = 0x2,
+} mali_memory_allocation_flag;
+
+/**
+ * Supplying this 'magic' physical address requests that the OS allocate the
+ * physical address at page commit time, rather than commiting a specific page
+ */
+#define MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC ((u32)(-1))
+
+typedef struct mali_memory_allocation
+{
+	/* Information about the allocation */
+	void * mapping; /**< CPU virtual address where the memory is mapped at */
+	u32 mali_address; /**< The Mali seen address of the memory allocation */
+	u32 size; /**< Size of the allocation */
+	u32 permission; /**< Permission settings */
+	mali_memory_allocation_flag flags;
+
+	_mali_osk_lock_t * lock;
+
+	/* Manager specific information pointers */
+	void * mali_addr_mapping_info; /**< Mali address allocation specific info */
+	void * process_addr_mapping_info; /**< Mapping manager specific info */
+
+	mali_physical_memory_allocation physical_allocation;
+
+	_mali_osk_list_t list; /**< List for linking together memory allocations into the session's memory head */
+} mali_memory_allocation;
+/** @} */ /* end group _mali_osk_low_level_memory */
+
+
+typedef struct mali_physical_memory_allocator
+{
+	mali_physical_memory_allocation_result (*allocate)(void* ctx, mali_allocation_engine * engine, mali_memory_allocation * descriptor, u32* offset, mali_physical_memory_allocation * alloc_info);
+	mali_physical_memory_allocation_result (*allocate_page_table_block)(void * ctx, mali_page_table_block * block); /* MALI_MEM_ALLOC_PARTIAL not allowed */
+	void (*destroy)(struct mali_physical_memory_allocator * allocator);
+	void * ctx;
+	const char * name; /**< Descriptive name for use in mali_allocation_engine_report_allocators, or NULL */
+	u32 alloc_order; /**< Order in which the allocations should happen */
+	struct mali_physical_memory_allocator * next;
+} mali_physical_memory_allocator;
+
+typedef struct mali_kernel_mem_address_manager
+{
+	_mali_osk_errcode_t (*allocate)(mali_memory_allocation *); /**< Function to call to reserve an address */
+	void (*release)(mali_memory_allocation *); /**< Function to call to free the address allocated */
+
+	 /**
+	  * Function called for each physical sub allocation.
+	  * Called for each physical block allocated by the physical memory manager.
+	  * @param[in] descriptor The memory descriptor in question
+	  * @param[in] off Offset from the start of range
+	  * @param[in,out] phys_addr A pointer to the physical address of the start of the
+	  * physical block. When *phys_addr == MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC
+	  * is used, this requests the function must allocate the physical page
+	  * itself, and return it through the pointer provided.
+	  * @param[in] size Length in bytes of the physical block
+	  * @return _MALI_OSK_ERR_OK on success.
+	  * A value of type _mali_osk_errcode_t other than _MALI_OSK_ERR_OK indicates failure.
+	  * Specifically, _MALI_OSK_ERR_UNSUPPORTED indicates that the function
+	  * does not support allocating physical pages itself.
+	  */
+	 _mali_osk_errcode_t (*map_physical)(mali_memory_allocation * descriptor, u32 offset, u32 *phys_addr, u32 size);
+
+	 /**
+	  * Function called to remove a physical sub allocation.
+	  * Called on error paths where one of the address managers fails.
+	  *
+	  * @note this is optional. For address managers where this is not
+	  * implemented, the value of this member is NULL. The memory engine
+	  * currently does not require the mali address manager to be able to
+	  * unmap individual pages, but the process address manager must have this
+	  * capability.
+	  *
+	  * @param[in] descriptor The memory descriptor in question
+	  * @param[in] off Offset from the start of range
+	  * @param[in] size Length in bytes of the physical block
+	  * @param[in] flags flags to use on a per-page basis. For OS-allocated
+	  * physical pages, this must include _MALI_OSK_MEM_MAPREGION_FLAG_OS_ALLOCATED_PHYSADDR.
+	  * @return _MALI_OSK_ERR_OK on success.
+	  * A value of type _mali_osk_errcode_t other than _MALI_OSK_ERR_OK indicates failure.
+	  */
+	void (*unmap_physical)(mali_memory_allocation * descriptor, u32 offset, u32 size, _mali_osk_mem_mapregion_flags_t flags);
+
+} mali_kernel_mem_address_manager;
+
+mali_allocation_engine mali_allocation_engine_create(mali_kernel_mem_address_manager * mali_address_manager, mali_kernel_mem_address_manager * process_address_manager);
+
+void mali_allocation_engine_destroy(mali_allocation_engine engine);
+
+int mali_allocation_engine_allocate_memory(mali_allocation_engine engine, mali_memory_allocation * descriptor, mali_physical_memory_allocator * physical_provider, _mali_osk_list_t *tracking_list );
+void mali_allocation_engine_release_memory(mali_allocation_engine engine, mali_memory_allocation * descriptor);
+
+int mali_allocation_engine_map_physical(mali_allocation_engine engine, mali_memory_allocation * descriptor, u32 offset, u32 phys, u32 cpu_usage_adjust, u32 size);
+void mali_allocation_engine_unmap_physical(mali_allocation_engine engine, mali_memory_allocation * descriptor, u32 offset, u32 size, _mali_osk_mem_mapregion_flags_t unmap_flags);
+
+int mali_allocation_engine_allocate_page_tables(mali_allocation_engine, mali_page_table_block * descriptor, mali_physical_memory_allocator * physical_provider);
+
+void mali_allocation_engine_report_allocators(mali_physical_memory_allocator * physical_provider);
+
+#endif /* __MALI_KERNEL_MEMORY_ENGINE_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_pp.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_pp.h
new file mode 100644
index 00000000000..ff1e153678c
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_pp.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_PP_H__
+#define __MALI_KERNEL_PP_H__
+
+extern struct mali_kernel_subsystem mali_subsystem_mali200;
+
+#if USING_MALI_PMM
+_mali_osk_errcode_t malipp_signal_power_up( u32 core_num, mali_bool queue_only );
+_mali_osk_errcode_t malipp_signal_power_down( u32 core_num, mali_bool immediate_only );
+#endif
+
+#endif /* __MALI_KERNEL_PP_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_profiling.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_profiling.c
new file mode 100644
index 00000000000..07bb894b1b4
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_profiling.c
@@ -0,0 +1,240 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_osk_mali.h"
+#include "mali_ukk.h"
+#include "mali_timestamp.h"
+
+#define MALI_PROFILING_MAX_BUFFER_ENTRIES 1048576
+
+typedef struct mali_profiling_entry
+{
+	u64 timestamp;
+	u32 event_id;
+	u32 data[5];
+} mali_profiling_entry;
+
+
+typedef enum mali_profiling_state
+{
+	MALI_PROFILING_STATE_UNINITIALIZED,
+	MALI_PROFILING_STATE_IDLE,
+	MALI_PROFILING_STATE_RUNNING,
+	MALI_PROFILING_STATE_RETURN,
+} mali_profiling_state;
+
+
+static _mali_osk_lock_t *lock = NULL;
+static mali_profiling_state prof_state = MALI_PROFILING_STATE_UNINITIALIZED;
+static mali_profiling_entry* profile_entries = NULL;
+static u32 profile_entry_count = 0;
+static _mali_osk_atomic_t profile_insert_index;
+static _mali_osk_atomic_t profile_entries_written;
+
+
+_mali_osk_errcode_t _mali_profiling_init(void)
+{
+	profile_entries = NULL;
+	profile_entry_count = 0;
+	_mali_osk_atomic_init(&profile_insert_index, 0);
+	_mali_osk_atomic_init(&profile_entries_written, 0);
+
+	lock = _mali_osk_lock_init( _MALI_OSK_LOCKFLAG_SPINLOCK | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE, 0, 0 );
+	if (NULL == lock)
+	{
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	prof_state = MALI_PROFILING_STATE_IDLE;
+
+	return _MALI_OSK_ERR_OK;
+}
+
+void _mali_profiling_term(void)
+{
+	prof_state = MALI_PROFILING_STATE_UNINITIALIZED;
+
+	/* wait for all elements to be completely inserted into array */
+	while (_mali_osk_atomic_read(&profile_insert_index) != _mali_osk_atomic_read(&profile_entries_written))
+	{
+		/* do nothing */;
+	}
+
+	if (NULL != profile_entries)
+	{
+		_mali_osk_free(profile_entries);
+		profile_entries = NULL;
+	}
+
+	if (NULL != lock)
+	{
+		_mali_osk_lock_term(lock);
+		lock = NULL;
+	}
+}
+
+inline _mali_osk_errcode_t _mali_profiling_add_event(u32 event_id, u32 data0, u32 data1, u32 data2, u32 data3, u32 data4)
+{
+	u32 cur_index = _mali_osk_atomic_inc_return(&profile_insert_index) - 1;
+
+	if (prof_state != MALI_PROFILING_STATE_RUNNING || cur_index >= profile_entry_count)
+	{
+		/*
+		 * Not in recording mode, or buffer is full
+		 * Decrement index again, and early out
+		 */
+		_mali_osk_atomic_dec(&profile_insert_index);
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	profile_entries[cur_index].timestamp = _mali_timestamp_get();
+	profile_entries[cur_index].event_id = event_id;
+	profile_entries[cur_index].data[0] = data0;
+	profile_entries[cur_index].data[1] = data1;
+	profile_entries[cur_index].data[2] = data2;
+	profile_entries[cur_index].data[3] = data3;
+	profile_entries[cur_index].data[4] = data4;
+
+	_mali_osk_atomic_inc(&profile_entries_written);
+
+	return _MALI_OSK_ERR_OK;
+}
+
+
+_mali_osk_errcode_t _mali_ukk_profiling_start(_mali_uk_profiling_start_s *args)
+{
+	_mali_osk_errcode_t ret;
+
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+
+	if (prof_state != MALI_PROFILING_STATE_IDLE)
+	{
+		_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		return _MALI_OSK_ERR_INVALID_ARGS; /* invalid to call this function in this state */
+	}
+
+	if (args->limit > MALI_PROFILING_MAX_BUFFER_ENTRIES)
+	{
+		args->limit = MALI_PROFILING_MAX_BUFFER_ENTRIES;
+	}
+
+	profile_entries = _mali_osk_malloc(args->limit * sizeof(mali_profiling_entry));
+	profile_entry_count = args->limit;
+	if (NULL == profile_entries)
+	{
+		_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	ret = _mali_timestamp_reset();
+
+	if (ret == _MALI_OSK_ERR_OK)
+	{
+		prof_state = MALI_PROFILING_STATE_RUNNING;
+	}
+	else
+	{
+		_mali_osk_free(profile_entries);
+		profile_entries = NULL;
+	}
+
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	return ret;
+}
+
+_mali_osk_errcode_t _mali_ukk_profiling_add_event(_mali_uk_profiling_add_event_s *args)
+{
+	/* Always add process and thread identificator in the first two data elements for events from user space */
+	return _mali_profiling_add_event(args->event_id, _mali_osk_get_pid(), _mali_osk_get_tid(), args->data[2], args->data[3], args->data[4]);
+}
+
+_mali_osk_errcode_t _mali_ukk_profiling_stop(_mali_uk_profiling_stop_s *args)
+{
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+
+	if (prof_state != MALI_PROFILING_STATE_RUNNING)
+	{
+		_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		return _MALI_OSK_ERR_INVALID_ARGS; /* invalid to call this function in this state */
+	}
+
+	/* go into return state (user to retreive events), no more events will be added after this */
+	prof_state = MALI_PROFILING_STATE_RETURN;
+
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+
+	/* wait for all elements to be completely inserted into array */
+	while (_mali_osk_atomic_read(&profile_insert_index) != _mali_osk_atomic_read(&profile_entries_written))
+	{
+		/* do nothing */;
+	}
+
+	args->count = _mali_osk_atomic_read(&profile_insert_index);
+
+	return _MALI_OSK_ERR_OK;
+}
+
+
+_mali_osk_errcode_t _mali_ukk_profiling_get_event(_mali_uk_profiling_get_event_s *args)
+{
+	u32 index = args->index;
+
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+
+	if (prof_state != MALI_PROFILING_STATE_RETURN)
+	{
+		_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		return _MALI_OSK_ERR_INVALID_ARGS; /* invalid to call this function in this state */
+	}
+
+	if (index >= _mali_osk_atomic_read(&profile_entries_written))
+	{
+		_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	args->timestamp = profile_entries[index].timestamp;
+	args->event_id = profile_entries[index].event_id;
+	args->data[0] = profile_entries[index].data[0];
+	args->data[1] = profile_entries[index].data[1];
+	args->data[2] = profile_entries[index].data[2];
+	args->data[3] = profile_entries[index].data[3];
+	args->data[4] = profile_entries[index].data[4];
+
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	return _MALI_OSK_ERR_OK;
+}
+
+_mali_osk_errcode_t _mali_ukk_profiling_clear(_mali_uk_profiling_clear_s *args)
+{
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+
+	if (prof_state != MALI_PROFILING_STATE_RETURN)
+	{
+		_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		return _MALI_OSK_ERR_INVALID_ARGS; /* invalid to call this function in this state */
+	}
+
+	prof_state = MALI_PROFILING_STATE_IDLE;
+	profile_entry_count = 0;
+	_mali_osk_atomic_init(&profile_insert_index, 0);
+	_mali_osk_atomic_init(&profile_entries_written, 0);
+	if (NULL != profile_entries)
+	{
+		_mali_osk_free(profile_entries);
+		profile_entries = NULL;
+	}
+
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	return _MALI_OSK_ERR_OK;
+}
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_profiling.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_profiling.h
new file mode 100644
index 00000000000..5d3aa6eb841
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_profiling.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_PROFILING_H__
+#define __MALI_KERNEL_PROFILING_H__
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+
+#include <../../../include/cinstr/mali_cinstr_profiling_events_m200.h>
+
+/**
+ * Initialize the profiling module.
+ * @return _MALI_OSK_ERR_OK on success, otherwise failure.
+ */
+_mali_osk_errcode_t _mali_profiling_init(void);
+
+/*
+ * Terminate the profiling module.
+ */
+void _mali_profiling_term(void);
+
+/**
+ * Add an profiling event
+ *
+ * @param event_id The event identificator.
+ * @param data0 - First data parameter, depending on event_id specified.
+ * @param data1 - Second data parameter, depending on event_id specified.
+ * @param data2 - Third data parameter, depending on event_id specified.
+ * @param data3 - Fourth data parameter, depending on event_id specified.
+ * @param data4 - Fifth data parameter, depending on event_id specified.
+ * @return _MALI_OSK_ERR_OK on success, otherwise failure.
+ */
+_mali_osk_errcode_t _mali_profiling_add_event(u32 event_id, u32 data0, u32 data1, u32 data2, u32 data3, u32 data4);
+
+#endif /* MALI_TIMELINE_PROFILING_ENABLED */
+
+#endif /* __MALI_KERNEL_PROFILING_H__ */
+
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_rendercore.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_rendercore.c
new file mode 100644
index 00000000000..1db8f3ad5ab
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_rendercore.c
@@ -0,0 +1,2032 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_kernel_core.h"
+#include "mali_osk.h"
+#include "mali_kernel_pp.h"
+#include "mali_kernel_subsystem.h"
+#include "mali_kernel_rendercore.h"
+#include "mali_osk_list.h"
+#if MALI_GPU_UTILIZATION
+#include "mali_kernel_utilization.h"
+#endif
+#if MALI_TIMELINE_PROFILING_ENABLED
+#include "mali_kernel_profiling.h"
+#endif
+#if USING_MMU
+#include "mali_kernel_mem_mmu.h"
+#endif /* USING_MMU */
+#if defined USING_MALI400_L2_CACHE
+#include "mali_kernel_l2_cache.h"
+#endif /* USING_MALI400_L2_CACHE */
+
+#define HANG_CHECK_MSECS_MIN 100
+#define HANG_CHECK_MSECS_MAX 2000 /* 2 secs */
+#define HANG_CHECK_MSECS_DEFAULT 500 /* 500 ms */
+
+#define WATCHDOG_MSECS_MIN (2*HANG_CHECK_MSECS_MIN)
+#define WATCHDOG_MSECS_MAX 3600000 /* 1 hour */
+#define WATCHDOG_MSECS_DEFAULT 900000 /* 15 mins */
+
+/* max value that will be converted from jiffies to msecs and written to job->render_time_msecs */
+#define JOB_MAX_JIFFIES 100000
+
+int mali_hang_check_interval = HANG_CHECK_MSECS_DEFAULT;
+int mali_max_job_runtime = WATCHDOG_MSECS_DEFAULT;
+
+/* Subsystem entrypoints: */
+static _mali_osk_errcode_t rendercore_subsystem_startup(mali_kernel_subsystem_identifier id);
+static void rendercore_subsystem_terminate(mali_kernel_subsystem_identifier id);
+#if USING_MMU
+static void rendercore_subsystem_broadcast_notification(mali_core_notification_message message, u32 data);
+#endif
+
+
+static void mali_core_subsystem_cleanup_all_renderunits(struct mali_core_subsystem* subsys);
+static void mali_core_subsystem_move_core_set_idle(struct mali_core_renderunit *core);
+
+static mali_core_session * mali_core_subsystem_get_waiting_session(mali_core_subsystem *subsystem);
+static mali_core_job * mali_core_subsystem_release_session_get_job(mali_core_subsystem *subsystem, mali_core_session * session);
+
+static void find_and_abort(mali_core_session* session, u32 abort_id);
+
+static void mali_core_job_start_on_core(mali_core_job *job, mali_core_renderunit *core);
+#if USING_MMU
+static void mali_core_subsystem_callback_schedule_wrapper(void* sub);
+#endif
+static void mali_core_subsystem_schedule(mali_core_subsystem*subsystem);
+static void mali_core_renderunit_detach_job_from_core(mali_core_renderunit* core, mali_subsystem_reschedule_option reschedule, mali_subsystem_job_end_code end_status);
+
+static void  mali_core_renderunit_irq_handler_remove(struct mali_core_renderunit *core);
+
+static _mali_osk_errcode_t  mali_core_irq_handler_upper_half (void * data);
+static void mali_core_irq_handler_bottom_half ( void *data );
+
+#if USING_MMU
+static void lock_subsystem(struct mali_core_subsystem * subsys);
+static void unlock_subsystem(struct mali_core_subsystem * subsys);
+#endif
+
+
+/**
+ * This will be one of the subsystems in the array of subsystems:
+ * static struct mali_kernel_subsystem * subsystems[];
+ * found in file: mali_kernel_core.c
+ *
+ * This subsystem is necessary for operations common to all rendercore
+ * subsystems. For example, mali_subsystem_mali200 and mali_subsystem_gp2 may
+ * share a mutex when RENDERCORES_USE_GLOBAL_MUTEX is non-zero.
+ */
+struct mali_kernel_subsystem mali_subsystem_rendercore=
+{
+	rendercore_subsystem_startup,                  /* startup */
+	rendercore_subsystem_terminate,                /* shutdown */
+	NULL,                                          /* load_complete */
+	NULL,                                          /* system_info_fill */
+	NULL,                                          /* session_begin */
+	NULL,                                          /* session_end */
+#if USING_MMU
+	rendercore_subsystem_broadcast_notification,  /* broadcast_notification */
+#else
+	NULL,
+#endif
+#if MALI_STATE_TRACKING
+	NULL,                                          /* dump_state */
+#endif
+} ;
+
+static _mali_osk_lock_t *rendercores_global_mutex = NULL;
+static u32 rendercores_global_mutex_is_held = 0;
+static u32 rendercores_global_mutex_owner = 0;
+
+/** The 'dummy' rendercore subsystem to allow global subsystem mutex to be
+ * locked for all subsystems that extend the ''rendercore'' */
+static mali_core_subsystem rendercore_dummy_subsystem = {0,};
+
+/*
+ * Rendercore Subsystem functions.
+ *
+ * These are exposed by mali_subsystem_rendercore
+ */
+
+/**
+ * @brief Initialize the Rendercore subsystem.
+ *
+ * This must be called before any other subsystem that extends the
+ * ''rendercore'' may be initialized. For example, this must be called before
+ * the following functions:
+ * - mali200_subsystem_startup(), from mali_subsystem_mali200
+ * - maligp_subsystem_startup(), from mali_subsystem_gp2
+ *
+ * @note This function is separate from mali_core_subsystem_init(). They
+ * are related, in that mali_core_subsystem_init() may use the structures
+ * initialized by rendercore_subsystem_startup()
+ */
+static _mali_osk_errcode_t rendercore_subsystem_startup(mali_kernel_subsystem_identifier id)
+{
+	rendercores_global_mutex_is_held = 0;
+	rendercores_global_mutex = _mali_osk_lock_init(
+					(_mali_osk_lock_flags_t)(_MALI_OSK_LOCKFLAG_NONINTERRUPTABLE | _MALI_OSK_LOCKFLAG_ORDERED),
+					0, 129);
+
+    if (NULL == rendercores_global_mutex)
+    {
+		MALI_PRINT_ERROR(("Failed: _mali_osk_lock_init\n")) ;
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+    }
+
+	rendercore_dummy_subsystem.name = "Rendercore Global Subsystem"; /* On the constant pool, do not free */
+	rendercore_dummy_subsystem.magic_nr = SUBSYSTEM_MAGIC_NR; /* To please the Subsystem Mutex code */
+
+#if MALI_GPU_UTILIZATION
+	if (mali_utilization_init() != _MALI_OSK_ERR_OK)
+	{
+		_mali_osk_lock_term(rendercores_global_mutex);
+		rendercores_global_mutex = NULL;
+		MALI_PRINT_ERROR(("Failed: mali_utilization_init\n")) ;
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+#endif
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+	if (_mali_profiling_init() != _MALI_OSK_ERR_OK)
+	{
+		/* No biggie if we wheren't able to initialize the profiling */
+		MALI_PRINT_ERROR(("Rendercore: Failed to initialize profiling, feature will be unavailable\n")) ;
+	}
+#endif
+
+	MALI_DEBUG_PRINT(2, ("Rendercore: subsystem global mutex initialized\n")) ;
+	MALI_SUCCESS;
+}
+
+/**
+ * @brief Terminate the Rendercore subsystem.
+ *
+ * This must only be called \b after any other subsystem that extends the
+ * ''rendercore'' has been terminated. For example, this must be called \b after
+ * the following functions:
+ * - mali200_subsystem_terminate(), from mali_subsystem_mali200
+ * - maligp_subsystem_terminate(), from mali_subsystem_gp2
+ *
+ * @note This function is separate from mali_core_subsystem_cleanup(), though,
+ * the subsystems that extend ''rendercore'' must still call
+ * mali_core_subsystem_cleanup() when they terminate.
+ */
+static void rendercore_subsystem_terminate(mali_kernel_subsystem_identifier id)
+{
+	/* Catch double-terminate */
+	MALI_DEBUG_ASSERT_POINTER( rendercores_global_mutex );
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+	_mali_profiling_term();
+#endif
+
+#if MALI_GPU_UTILIZATION
+	mali_utilization_term();
+#endif
+
+	rendercore_dummy_subsystem.name = NULL; /* The original string was on the constant pool, do not free */
+	rendercore_dummy_subsystem.magic_nr = 0;
+
+	/* ASSERT that no-one's holding this */
+	MALI_DEBUG_PRINT_ASSERT( 0 == rendercores_global_mutex_is_held,
+							 ("Rendercores' Global Mutex was held at termination time. Have the subsystems that extend ''rendercore'' been terminated?\n") );
+
+	_mali_osk_lock_term( rendercores_global_mutex );
+	rendercores_global_mutex = NULL;
+
+	MALI_DEBUG_PRINT(2, ("Rendercore: subsystem global mutex terminated\n")) ;
+}
+
+
+#if USING_MMU
+/**
+ * @brief Handle certain Rendercore subsystem broadcast notifications
+ *
+ * When RENDERCORES_USE_GLOBAL_MUTEX is non-zero, this handles the following messages:
+ * - MMU_KILL_STEP0_LOCK_SUBSYSTEM
+ * - MMU_KILL_STEP4_UNLOCK_SUBSYSTEM
+ *
+ * The purpose is to manage the Rendercode Global Mutex, which cannot be
+ * managed by any system that extends the ''rendercore''.
+ *
+ * All other messages must be handled by mali_core_subsystem_broadcast_notification()
+ *
+ *
+ * When RENDERCORES_USE_GLOBAL_MUTEX is 0, this function does nothing.
+ * Instead, the subsystem that extends the ''rendercore' \b must handle its
+ * own mutexes - refer to mali_core_subsystem_broadcast_notification().
+ *
+ * Used currently only for signalling when MMU has a pagefault
+ */
+static void rendercore_subsystem_broadcast_notification(mali_core_notification_message message, u32 data)
+{
+	switch(message)
+	{
+		case MMU_KILL_STEP0_LOCK_SUBSYSTEM:
+			lock_subsystem( &rendercore_dummy_subsystem );
+			break;
+		case MMU_KILL_STEP4_UNLOCK_SUBSYSTEM:
+			unlock_subsystem( &rendercore_dummy_subsystem );
+			break;
+
+		case MMU_KILL_STEP1_STOP_BUS_FOR_ALL_CORES:
+			/** FALLTHROUGH */
+		case MMU_KILL_STEP2_RESET_ALL_CORES_AND_ABORT_THEIR_JOBS:
+			/** FALLTHROUGH */
+		case MMU_KILL_STEP3_CONTINUE_JOB_HANDLING:
+			break;
+
+		default:
+			MALI_PRINT_ERROR(("Illegal message: 0x%x, data: 0x%x\n", (u32)message, data));
+			break;
+	}
+
+}
+#endif
+
+/*
+ * Functions inherited by the subsystems that extend the ''rendercore''.
+ */
+
+u32 mali_core_renderunit_register_read(mali_core_renderunit *core, u32 relative_address)
+{
+	u32 read_val;
+
+	#if USING_MALI_PMM
+		if( core->state == CORE_OFF )
+		{
+			MALI_PRINT_ERROR(("Core is OFF during read: Core:%s Addr:0x%04X\n", 
+				core->description,relative_address));
+			return 0xDEADBEEF;
+		}
+	#endif
+
+	MALI_DEBUG_ASSERT((relative_address & 0x03) == 0);
+
+	if (mali_benchmark) return 0;
+
+	if (relative_address >= core->size)
+	{
+		MALI_PRINT_ERROR(("Trying to read from illegal register: 0x%04x in core: %s\n",
+		        relative_address, core->description));
+		return 0xDEADBEEF;
+	}
+
+	read_val = _mali_osk_mem_ioread32(core->registers_mapped, relative_address);
+
+	MALI_DEBUG_PRINT(6, ("Core: renderunit_register_read: Core:%s Addr:0x%04X Val:0x%08x\n",
+	        core->description,relative_address, read_val));
+
+	return read_val;
+}
+
+void  mali_core_renderunit_register_read_array(mali_core_renderunit *core,
+                                               u32 relative_address,
+                                               u32 * result_array,
+                                               u32 nr_of_regs
+                                              )
+{
+	/* NOTE Do not use burst reads against the registers */
+
+	u32 i;
+
+	for(i=0; i<nr_of_regs; ++i)
+	{
+		result_array[i] = mali_core_renderunit_register_read(core, relative_address + i*4);
+	}
+
+	MALI_DEBUG_PRINT(6, ("Core: renderunit_register_read_array: Core:%s Addr:0x%04X Nr_regs: %u\n",
+	        core->description,relative_address, nr_of_regs));
+}
+
+void mali_core_renderunit_register_write(mali_core_renderunit *core, u32 relative_address, u32 new_val)
+{
+	#if USING_MALI_PMM
+		if( core->state == CORE_OFF )
+		{
+			MALI_PRINT_ERROR(("Core is OFF during write: Core:%s Addr:0x%04X Val:0x%08x\n",
+				core->description,relative_address, new_val));
+			return;
+		}
+	#endif
+			
+	MALI_DEBUG_ASSERT((relative_address & 0x03) == 0);
+
+	if (mali_benchmark) return;
+
+	if (relative_address >= core->size)
+	{
+		MALI_PRINT_ERROR(("Trying to write to illegal register: 0x%04x in core: %s",
+		        relative_address, core->description));
+		return;
+	}
+
+	MALI_DEBUG_PRINT(6, ("mali_core_renderunit_register_write: Core:%s Addr:0x%04X Val:0x%08x\n",
+            core->description,relative_address, new_val));
+
+	_mali_osk_mem_iowrite32(core->registers_mapped, relative_address, new_val);
+}
+
+
+void  mali_core_renderunit_register_write_array(mali_core_renderunit *core,
+                                                u32 relative_address,
+                                                u32 * source_array,
+                                                u32 nr_of_regs)
+{
+
+	u32 i;
+	MALI_DEBUG_PRINT(6, ("Core: renderunit_register_write_array: Core:%s Addr:0x%04X Nr_regs: %u\n",
+	        core->description,relative_address, nr_of_regs));
+
+	/* Do not use burst writes against the registers */
+
+	for( i = 0; i< nr_of_regs; i++)
+	{
+		mali_core_renderunit_register_write(core, relative_address + i*4, source_array[i]);
+	}
+}
+
+void mali_core_renderunit_timeout_function_hang_detection(void *arg)
+{
+	mali_bool action = MALI_FALSE;
+	mali_core_renderunit * core;
+
+	core = (mali_core_renderunit *) arg;
+	if( !core ) return;
+
+	/* if NOT idle OR has TIMED_OUT */
+	if ( !((CORE_WATCHDOG_TIMEOUT == core->state ) || (CORE_IDLE== core->state)) )
+	{
+		core->state = CORE_HANG_CHECK_TIMEOUT;
+		action = MALI_TRUE;
+	}
+
+	if(action) _mali_osk_irq_schedulework(core->irq);
+}
+
+
+void mali_core_renderunit_timeout_function(void *arg)
+{
+	mali_core_renderunit * core;
+	mali_bool is_watchdog;
+
+	core = (mali_core_renderunit *)arg;
+	if( !core ) return;
+
+	is_watchdog = MALI_TRUE;
+	if (mali_benchmark)
+	{
+		/* poll based core */
+		mali_core_job *job;
+		job = core->current_job;
+		if ( (NULL != job) &&
+		     (0 != _mali_osk_time_after(job->watchdog_jiffies,_mali_osk_time_tickcount()))
+		   )
+		{
+			core->state = CORE_POLL;
+			is_watchdog = MALI_FALSE;
+		}
+	}
+
+	if (is_watchdog)
+	{
+		MALI_DEBUG_PRINT(3, ("SW-Watchdog timeout: Core:%s\n", core->description));
+		core->state = CORE_WATCHDOG_TIMEOUT;
+	}
+
+	_mali_osk_irq_schedulework(core->irq);
+}
+
+/* Used by external renderunit_create<> function */
+_mali_osk_errcode_t mali_core_renderunit_init(mali_core_renderunit * core)
+{
+	MALI_DEBUG_PRINT(5, ("Core: renderunit_init: Core:%s\n", core->description));
+
+	_MALI_OSK_INIT_LIST_HEAD(&core->list) ;
+	core->timer = _mali_osk_timer_init();
+    if (NULL == core->timer)
+    {
+	    MALI_PRINT_ERROR(("Core: renderunit_init: Core:%s -- cannot init timer\n", core->description));
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+    }
+
+    _mali_osk_timer_setcallback(core->timer, mali_core_renderunit_timeout_function, (void *)core);
+
+	core->timer_hang_detection = _mali_osk_timer_init();
+    if (NULL == core->timer_hang_detection)
+    {
+        _mali_osk_timer_term(core->timer);
+	    MALI_PRINT_ERROR(("Core: renderunit_init: Core:%s -- cannot init hang detection timer\n", core->description));
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+    }
+
+    _mali_osk_timer_setcallback(core->timer_hang_detection, mali_core_renderunit_timeout_function_hang_detection, (void *)core);
+
+#if USING_MALI_PMM
+	/* Init no pending power downs */
+	core->pend_power_down = MALI_FALSE;
+	
+	/* Register the core with the PMM - which powers it up */
+	if (_MALI_OSK_ERR_OK != malipmm_core_register( core->pmm_id ))
+	{
+		_mali_osk_timer_term(core->timer);
+		_mali_osk_timer_term(core->timer_hang_detection);
+	    MALI_PRINT_ERROR(("Core: renderunit_init: Core:%s -- cannot register with PMM\n", core->description));        
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+#endif /* USING_MALI_PMM */
+
+	core->error_recovery = MALI_FALSE;
+	core->in_detach_function = MALI_FALSE;
+	core->state = CORE_IDLE;
+	core->current_job = NULL;
+	core->magic_nr = CORE_MAGIC_NR;
+#if USING_MMU
+	core->mmu = NULL;
+#endif /* USING_MMU */
+
+    MALI_SUCCESS;
+}
+
+void mali_core_renderunit_term(mali_core_renderunit * core)
+{
+	MALI_DEBUG_PRINT(5, ("Core: renderunit_term: Core:%s\n", core->description));
+
+    if (NULL != core->timer)
+    {
+        _mali_osk_timer_term(core->timer);
+        core->timer = NULL;
+    }
+    if (NULL != core->timer_hang_detection)
+    {
+        _mali_osk_timer_term(core->timer_hang_detection);
+        core->timer_hang_detection = NULL;
+    }
+
+#if USING_MALI_PMM
+	/* Unregister the core with the PMM */
+	malipmm_core_unregister( core->pmm_id );
+#endif
+}
+
+/* Used by external renderunit_create<> function */
+_mali_osk_errcode_t mali_core_renderunit_map_registers(mali_core_renderunit *core)
+{
+	MALI_DEBUG_PRINT(3, ("Core: renderunit_map_registers: Core:%s\n", core->description)) ;
+	if( (0 == core->registers_base_addr) ||
+	    (0 == core->size) ||
+	    (NULL == core->description)
+	  )
+	{
+		MALI_PRINT_ERROR(("Missing fields in the core structure %u %u 0x%x;\n", core->registers_base_addr, core->size, core->description));
+        MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}
+
+	if (_MALI_OSK_ERR_OK != _mali_osk_mem_reqregion(core->registers_base_addr, core->size, core->description))
+	{
+		MALI_PRINT_ERROR(("Could not request register region (0x%08X - 0x%08X) to core: %s\n",
+		         core->registers_base_addr, core->registers_base_addr + core->size - 1, core->description));
+        MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}
+	else
+	{
+		MALI_DEBUG_PRINT(6, ("Success: request_mem_region: (0x%08X - 0x%08X) Core:%s\n",
+		        core->registers_base_addr, core->registers_base_addr + core->size - 1, core->description));
+	}
+
+	core->registers_mapped = _mali_osk_mem_mapioregion( core->registers_base_addr, core->size, core->description );
+
+	if ( 0 == core->registers_mapped )
+	{
+		MALI_PRINT_ERROR(("Could not ioremap registers for %s .\n", core->description));
+		_mali_osk_mem_unreqregion(core->registers_base_addr, core->size);
+        MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+	}
+	else
+	{
+		MALI_DEBUG_PRINT(6, ("Success: ioremap_nocache: Internal ptr: (0x%08X - 0x%08X) Core:%s\n",
+		        (u32) core->registers_mapped,
+		        ((u32)core->registers_mapped)+ core->size - 1,
+		        core->description));
+	}
+
+	MALI_DEBUG_PRINT(4, ("Success: Mapping registers to core: %s\n",core->description));
+
+	MALI_SUCCESS;
+}
+
+/* Used by external renderunit_create<> function + other places */
+void mali_core_renderunit_unmap_registers(mali_core_renderunit *core)
+{
+	MALI_DEBUG_PRINT(3, ("Core: renderunit_unmap_registers: Core:%s\n", core->description));
+	if (0 == core->registers_mapped)
+	{
+		MALI_PRINT_ERROR(("Trying to unmap register-mapping with NULL from core: %s\n", core->description));
+		return;
+	}
+	_mali_osk_mem_unmapioregion(core->registers_base_addr, core->size, core->registers_mapped);
+	core->registers_mapped = 0;
+	_mali_osk_mem_unreqregion(core->registers_base_addr, core->size);
+}
+
+static void mali_core_renderunit_irq_handler_remove(mali_core_renderunit *core)
+{
+	MALI_DEBUG_PRINT(3, ("Core: renderunit_irq_handler_remove: Core:%s\n", core->description));
+    _mali_osk_irq_term(core->irq);
+}
+
+mali_core_renderunit * mali_core_renderunit_get_mali_core_nr(mali_core_subsystem *subsys, u32 mali_core_nr)
+{
+	mali_core_renderunit * core;
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+	if (subsys->number_of_cores <= mali_core_nr)
+	{
+		MALI_PRINT_ERROR(("Trying to get illegal mali_core_nr: 0x%x for %s", mali_core_nr, subsys->name));
+		return NULL;
+	}
+	core = (subsys->mali_core_array)[mali_core_nr];
+	MALI_DEBUG_PRINT(6, ("Core: renderunit_get_mali_core_nr: Core:%s\n", core->description));
+	MALI_CHECK_CORE(core);
+	return core;
+}
+
+/* Is used by external function:
+	subsystem_startup<> */
+_mali_osk_errcode_t mali_core_subsystem_init(mali_core_subsystem* new_subsys)
+{
+	int i;
+
+	/* These function pointers must have been set on before calling this function */
+	if (
+	       ( NULL == new_subsys->name ) ||
+	       ( NULL == new_subsys->start_job ) ||
+	       ( NULL == new_subsys->irq_handler_upper_half ) ||
+	       ( NULL == new_subsys->irq_handler_bottom_half ) ||
+	       ( NULL == new_subsys->get_new_job_from_user ) ||
+	       ( NULL == new_subsys->return_job_to_user )
+	   )
+	{
+		MALI_PRINT_ERROR(("Missing functions in subsystem."));
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	MALI_DEBUG_PRINT(2, ("Core: subsystem_init: %s\n", new_subsys->name)) ;
+
+	/* Catch use-before-initialize/use-after-terminate */
+	MALI_DEBUG_ASSERT_POINTER( rendercores_global_mutex );
+
+	new_subsys->magic_nr = SUBSYSTEM_MAGIC_NR;
+
+	_MALI_OSK_INIT_LIST_HEAD(&new_subsys->renderunit_idle_head); /* Idle cores of this type */
+	_MALI_OSK_INIT_LIST_HEAD(&new_subsys->renderunit_off_head);  /* Powered off cores of this type */
+
+	/* Linked list for each priority of sessions with a job ready for scheduleing */
+	for(i=0; i<PRIORITY_LEVELS; ++i)
+	{
+		_MALI_OSK_INIT_LIST_HEAD(&new_subsys->awaiting_sessions_head[i]);
+	}
+
+	/* Linked list of all sessions connected to this coretype */
+	_MALI_OSK_INIT_LIST_HEAD(&new_subsys->all_sessions_head);
+
+	MALI_SUCCESS;
+}
+
+#if USING_MMU
+void mali_core_subsystem_attach_mmu(mali_core_subsystem* subsys)
+{
+	u32 i;
+	mali_core_renderunit * core;
+
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsys);
+
+	for(i=0 ; i < subsys->number_of_cores ; ++i)
+	{
+		core = mali_core_renderunit_get_mali_core_nr(subsys,i);
+		if ( NULL==core ) break;
+		core->mmu = mali_memory_core_mmu_lookup(core->mmu_id);
+		MALI_DEBUG_PRINT(2, ("Attach mmu: 0x%x to core: %s in subsystem: %s\n", core->mmu, core->description, subsys->name));
+	}
+
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+}
+#endif
+
+/* This will register an IRQ handler, and add the core to the list of available cores for this subsystem. */
+_mali_osk_errcode_t mali_core_subsystem_register_renderunit(mali_core_subsystem* subsys, mali_core_renderunit * core)
+{
+	mali_core_renderunit ** mali_core_array;
+	u32 previous_nr;
+	u32 previous_size;
+	u32 new_nr;
+	u32 new_size;
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_FAULT;
+
+	/* If any of these are 0 there is an error */
+	if(0 == core->subsystem ||
+	   0 == core->registers_base_addr ||
+	   0 == core->size ||
+	   0 == core->description)
+	{
+		MALI_PRINT_ERROR(("Missing fields in the core structure 0x%x 0x%x 0x%x;\n",
+		        core->registers_base_addr, core->size, core->description));
+        MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}
+
+	MALI_DEBUG_PRINT(3, ("Core: subsystem_register_renderunit: %s\n", core->description));
+
+	MALI_CHECK_NON_NULL(
+        core->irq = _mali_osk_irq_init(
+                            core->irq_nr,
+                            mali_core_irq_handler_upper_half,
+                            mali_core_irq_handler_bottom_half,
+                            (_mali_osk_irq_trigger_t)subsys->probe_core_irq_trigger,
+                            (_mali_osk_irq_ack_t)subsys->probe_core_irq_acknowledge,
+                            core,
+                            "mali_core_irq_handlers"
+                            ),
+        _MALI_OSK_ERR_FAULT
+        );
+
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsys);
+
+	/* Update which core number this is */
+	core->core_number = subsys->number_of_cores;
+
+	/* Update the array of cores in the subsystem. */
+	previous_nr   = subsys->number_of_cores;
+	previous_size = sizeof(mali_core_renderunit*)*previous_nr;
+	new_nr        = previous_nr + 1;
+	new_size      = sizeof(mali_core_renderunit*)*new_nr;
+
+	if (0 != previous_nr)
+	{
+		if (NULL == subsys->mali_core_array)
+		{
+			MALI_PRINT_ERROR(("Internal error"));
+			goto exit_function;
+		}
+
+		mali_core_array = (mali_core_renderunit **) _mali_osk_malloc( new_size );
+		if (NULL == mali_core_array )
+		{
+			MALI_PRINT_ERROR(("Out of mem"));
+            err = _MALI_OSK_ERR_NOMEM;
+			goto exit_function;
+		}
+		_mali_osk_memcpy(mali_core_array, subsys->mali_core_array, previous_size);
+		_mali_osk_free( subsys->mali_core_array);
+		MALI_DEBUG_PRINT(5, ("Success: adding a new core to subsystem array %s\n", core->description) ) ;
+	}
+	else
+	{
+		mali_core_array = (mali_core_renderunit **) _mali_osk_malloc( new_size );
+		if (NULL == mali_core_array )
+		{
+			MALI_PRINT_ERROR(("Out of mem"));
+            err = _MALI_OSK_ERR_NOMEM;
+			goto exit_function;
+		}
+		MALI_DEBUG_PRINT(6, ("Success: adding first core to subsystem array %s\n", core->description) ) ;
+	}
+	subsys->mali_core_array = mali_core_array;
+	mali_core_array[previous_nr] = core;
+
+	/* Add the core to the list of available cores on the system */
+	_mali_osk_list_add(&(core->list), &(subsys->renderunit_idle_head));
+
+	/* Update total number of cores */
+	subsys->number_of_cores = new_nr;
+	MALI_DEBUG_PRINT(6, ("Success: mali_core_subsystem_register_renderunit %s\n", core->description));
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+    MALI_SUCCESS;
+
+exit_function:
+	mali_core_renderunit_irq_handler_remove(core);
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+    MALI_ERROR(err);
+}
+
+
+/**
+ * Called by the core when a system info update is needed
+ * We fill in info about all the core types available
+ * @param subsys Pointer to the core's @a mali_core_subsystem data structure
+ * @param info Pointer to system info struct to update
+ * @return _MALI_OSK_ERR_OK on success, or another _mali_osk_errcode_t error code on failure
+ */
+_mali_osk_errcode_t mali_core_subsystem_system_info_fill(mali_core_subsystem* subsys, _mali_system_info* info)
+{
+ 	u32 i;
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_OK;  /* OK if no cores to update info for */
+	mali_core_renderunit * core;
+	_mali_core_info **core_info_nextp;
+	_mali_core_info * cinfo;
+
+	MALI_DEBUG_PRINT(4, ("mali_core_subsystem_system_info_fill: %s\n", subsys->name) ) ;
+
+	/* check input */
+    MALI_CHECK_NON_NULL(info, _MALI_OSK_ERR_INVALID_ARGS);
+
+	core_info_nextp = &(info->core_info);
+	cinfo = info->core_info;
+
+	while(NULL!=cinfo)
+	{
+		core_info_nextp = &(cinfo->next);
+		cinfo = cinfo->next;
+	}
+
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsys);
+	for(i=0 ; i < subsys->number_of_cores ; ++i)
+	{
+		core = mali_core_renderunit_get_mali_core_nr(subsys,i);
+		if ( NULL==core )
+		{
+			err = _MALI_OSK_ERR_FAULT;
+			goto early_exit;
+		}
+		cinfo = (_mali_core_info *)_mali_osk_calloc(1, sizeof(_mali_core_info));
+		if ( NULL==cinfo )
+		{
+			err = _MALI_OSK_ERR_NOMEM;
+			goto early_exit;
+		}
+		cinfo->version = core->core_version;
+		cinfo->type =subsys->core_type;
+		cinfo->reg_address = core->registers_base_addr;
+		cinfo->core_nr     = i;
+		cinfo->next = NULL;
+		/* Writing this address to the previous' *(&next) ptr */
+		*core_info_nextp = cinfo;
+		/* Setting the next_ptr to point to &this->next_ptr */
+		core_info_nextp = &(cinfo->next);
+	}
+early_exit:
+	if ( _MALI_OSK_ERR_OK != err) MALI_PRINT_ERROR(("Error: In mali_core_subsystem_system_info_fill %d\n", err));
+	MALI_DEBUG_CODE(
+		cinfo = info->core_info;
+
+		MALI_DEBUG_PRINT(3, ("Current list of cores\n"));
+		while( NULL != cinfo )
+		{
+			MALI_DEBUG_PRINT(3, ("Type:     0x%x\n", cinfo->type));
+			MALI_DEBUG_PRINT(3, ("Version:  0x%x\n", cinfo->version));
+			MALI_DEBUG_PRINT(3, ("Reg_addr: 0x%x\n", cinfo->reg_address));
+			MALI_DEBUG_PRINT(3, ("Core_nr:  0x%x\n", cinfo->core_nr));
+			MALI_DEBUG_PRINT(3, ("Flags:    0x%x\n", cinfo->flags));
+			MALI_DEBUG_PRINT(3, ("*****\n"));
+			cinfo  = cinfo->next;
+		}
+	);
+
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+    MALI_ERROR(err);
+}
+
+
+/* Is used by external function:
+	subsystem_terminate<> */
+void mali_core_subsystem_cleanup(mali_core_subsystem* subsys)
+{
+	u32 i;
+	mali_core_renderunit * core;
+
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsys);
+	MALI_DEBUG_PRINT(2, ("Core: subsystem_cleanup: %s\n", subsys->name )) ;
+
+	for(i=0 ; i < subsys->number_of_cores ; ++i)
+	{
+		core = mali_core_renderunit_get_mali_core_nr(subsys,i);
+
+#if USING_MMU
+			if (NULL != core->mmu)
+			{
+				/* the MMU is attached in the load_complete callback, which will never be called if the module fails to load, handle that case */
+				mali_memory_core_mmu_unregister_callback(core->mmu, mali_core_subsystem_callback_schedule_wrapper);
+			}
+#endif
+
+		MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+
+		mali_core_renderunit_irq_handler_remove(core);
+
+		/* When a process terminates, all cores running jobs from that process is reset and put to idle.
+			That means that when the module is unloading (this code) we are guaranteed that all cores are idle.
+			However: if something (we can't think of) is really wrong, a core may give an interrupt during this
+			unloading, and we may now in the code have a bottom-half-processing pending from the interrupts
+			we deregistered above. To be sure that the bottom halves do not access the structures after they
+			are deallocated we flush the bottom-halves processing here, before the deallocation. */
+
+		MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsys);
+
+#if USING_MALI_PMM
+			/* Only reset when we are using PMM and the core is not off */
+#if MALI_PMM_NO_PMU
+				/* We need to reset when there is no PMU - but this will
+				 * cause the register read/write functions to report an
+				 * error (hence the if to check for CORE_OFF below) we 
+				 * change state to allow the reset to happen.
+				 */
+				core->state = CORE_IDLE;
+#endif
+			if( core->state != CORE_OFF )
+			{
+				subsys->reset_core( core, MALI_CORE_RESET_STYLE_DISABLE );
+			}
+#else
+			/* Always reset the core */
+			subsys->reset_core( core, MALI_CORE_RESET_STYLE_DISABLE );
+#endif
+
+		mali_core_renderunit_unmap_registers(core);
+
+		_mali_osk_list_delinit(&core->list);
+
+		mali_core_renderunit_term(core);
+
+		subsys->renderunit_delete(core);
+	}
+
+	mali_core_subsystem_cleanup_all_renderunits(subsys);
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+	MALI_DEBUG_PRINT(6, ("SUCCESS: mali_core_subsystem_cleanup: %s\n", subsys->name )) ;
+}
+
+_mali_osk_errcode_t mali_core_subsystem_ioctl_number_of_cores_get(mali_core_session * session, u32 *number_of_cores)
+{
+	mali_core_subsystem * subsystem;
+
+	subsystem = session->subsystem;
+	if ( NULL != number_of_cores )
+	{
+		*number_of_cores = subsystem->number_of_cores;
+	}
+
+	MALI_DEBUG_PRINT(4, ("Core: ioctl_number_of_cores_get: %s: %u\n", subsystem->name, *number_of_cores) ) ;
+
+    MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_core_subsystem_ioctl_start_job(mali_core_session * session, void *job_data)
+{
+    mali_core_subsystem * subsystem;
+    _mali_osk_errcode_t err;
+
+    /* need the subsystem to run callback function */
+    subsystem = session->subsystem;
+    MALI_CHECK_NON_NULL(subsystem, _MALI_OSK_ERR_FAULT);
+
+    MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsystem);
+    err = subsystem->get_new_job_from_user(session, job_data);
+    MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsystem);
+
+    MALI_ERROR(err);
+}
+
+
+/* We return the version number to the first core in this subsystem */
+_mali_osk_errcode_t mali_core_subsystem_ioctl_core_version_get(mali_core_session * session, _mali_core_version *version)
+{
+	mali_core_subsystem * subsystem;
+	mali_core_renderunit * core0;
+	u32 nr_return;
+
+	subsystem = session->subsystem;
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsystem);
+
+	core0 = mali_core_renderunit_get_mali_core_nr(subsystem, 0);
+
+	if( NULL == core0 )
+	{
+		MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsystem);
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	nr_return = core0->core_version;
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsystem);
+
+	MALI_DEBUG_PRINT(4, ("Core: ioctl_core_version_get: %s: %u\n", subsystem->name, nr_return )) ;
+
+	*version = nr_return;
+
+	MALI_SUCCESS;
+}
+
+void mali_core_subsystem_ioctl_abort_job(mali_core_session * session, u32 id)
+{
+	find_and_abort(session, id);
+}
+
+static mali_bool job_should_be_aborted(mali_core_job *job, u32 abort_id)
+{
+	if ( job->abort_id == abort_id ) return MALI_TRUE;
+	else return MALI_FALSE;
+}
+
+static void find_and_abort(mali_core_session* session, u32 abort_id)
+{
+	mali_core_subsystem * subsystem;
+	mali_core_renderunit *core;
+	mali_core_renderunit *tmp;
+	mali_core_job *job;
+
+	subsystem = session->subsystem;
+
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB( subsystem );
+
+	job = session->job_waiting_to_run;
+	if ( (job!=NULL) && job_should_be_aborted (job, abort_id) )
+	{
+		MALI_DEBUG_PRINT(3, ("Core: Aborting %s job, with id nr: %u, from the waiting_to_run slot.\n", subsystem->name, abort_id ));
+		session->job_waiting_to_run = NULL;
+		_mali_osk_list_delinit(&(session->awaiting_sessions_list));
+		subsystem->awaiting_sessions_sum_all_priorities--;
+		subsystem->return_job_to_user( job , JOB_STATUS_END_ABORT);
+	}
+
+	_MALI_OSK_LIST_FOREACHENTRY( core, tmp, &session->renderunits_working_head, mali_core_renderunit, list )
+	{
+		job = core->current_job;
+		if ( (job!=NULL) && (job_should_be_aborted (job, abort_id) ) )
+		{
+			MALI_DEBUG_PRINT(3, ("Core: Aborting %s job, with id nr: %u, which is currently running on mali.\n", subsystem->name, abort_id ));
+			if ( core->state==CORE_IDLE )
+			{
+				MALI_PRINT_ERROR(("Aborting core with running job which is idle. Must be something very wrong."));
+				goto end_bug;
+			}
+			mali_core_renderunit_detach_job_from_core(core, SUBSYSTEM_RESCHEDULE, JOB_STATUS_END_ABORT);
+		}
+	}
+end_bug:
+
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE( subsystem );
+}
+
+
+_mali_osk_errcode_t mali_core_subsystem_ioctl_suspend_response(mali_core_session * session, void *argument)
+{
+    mali_core_subsystem * subsystem;
+    _mali_osk_errcode_t err = _MALI_OSK_ERR_FAULT;
+
+    /* need the subsystem to run callback function */
+    subsystem = session->subsystem;
+    MALI_CHECK_NON_NULL(subsystem, _MALI_OSK_ERR_FAULT);
+
+    MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsystem);
+	if ( NULL != subsystem->suspend_response)
+	{
+		MALI_DEBUG_PRINT(4, ("MALI_IOC_CORE_CMD_SUSPEND_RESPONSE start\n"));
+		err = subsystem->suspend_response(session, argument);
+		MALI_DEBUG_PRINT(4, ("MALI_IOC_CORE_CMD_SUSPEND_RESPONSE end\n"));
+	}
+
+    MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsystem);
+
+    return err;
+}
+
+
+/* Is used by internal function:
+	mali_core_subsystem_cleanup<>s */
+/* All cores should be removed before calling this function
+Must hold subsystem_mutex before entering this function */
+static void mali_core_subsystem_cleanup_all_renderunits(mali_core_subsystem* subsys)
+{
+	int i;
+	_mali_osk_free(subsys->mali_core_array);
+	subsys->number_of_cores = 0;
+
+	MALI_DEBUG_PRINT(5, ("Core: subsystem_cleanup_all_renderunits: %s\n", subsys->name) ) ;
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+
+	if ( ! _mali_osk_list_empty(&(subsys->renderunit_idle_head)))
+	{
+		MALI_PRINT_ERROR(("List renderunit_list_idle should be empty."));
+		_MALI_OSK_INIT_LIST_HEAD(&(subsys->renderunit_idle_head)) ;
+	}
+
+	if ( ! _mali_osk_list_empty(&(subsys->renderunit_off_head)))
+	{
+		MALI_PRINT_ERROR(("List renderunit_list_off should be empty."));
+		_MALI_OSK_INIT_LIST_HEAD(&(subsys->renderunit_off_head)) ;
+	}
+
+	for(i=0; i<PRIORITY_LEVELS; ++i)
+	{
+		if ( ! _mali_osk_list_empty(&(subsys->awaiting_sessions_head[i])))
+		{
+			MALI_PRINT_ERROR(("List awaiting_sessions_linkedlist should be empty."));
+			_MALI_OSK_INIT_LIST_HEAD(&(subsys->awaiting_sessions_head[i])) ;
+			subsys->awaiting_sessions_sum_all_priorities = 0;
+		}
+	}
+
+	if ( ! _mali_osk_list_empty(&(subsys->all_sessions_head)))
+	{
+		MALI_PRINT_ERROR(("List all_sessions_linkedlist should be empty."));
+		_MALI_OSK_INIT_LIST_HEAD(&(subsys->all_sessions_head)) ;
+	}
+}
+
+/* Is used by internal functions:
+	mali_core_irq_handler_bottom_half<>;
+	mali_core_subsystem_schedule<>;	*/
+/* Will release the core.*/
+/* Must hold subsystem_mutex before entering this function */
+static void mali_core_subsystem_move_core_set_idle(mali_core_renderunit *core)
+{
+	mali_core_subsystem *subsystem;
+#if USING_MALI_PMM
+	mali_core_status oldstatus;
+#endif
+	subsystem = core->subsystem;
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsystem);
+	MALI_CHECK_CORE(core);
+	MALI_CHECK_SUBSYSTEM(subsystem);
+
+    _mali_osk_timer_del(core->timer);
+    _mali_osk_timer_del(core->timer_hang_detection);
+
+    MALI_DEBUG_PRINT(5, ("Core: subsystem_move_core_set_idle: %s\n", core->description) ) ;
+
+	core->current_job = NULL ;
+
+#if USING_MALI_PMM
+
+	oldstatus = core->state;
+	
+	if( core->pend_power_down )
+	{
+		core->state = CORE_OFF ;
+		_mali_osk_list_move( &core->list, &subsystem->renderunit_off_head );
+		/* Done the move from the active queues, so the pending power down can be done */
+		core->pend_power_down = MALI_FALSE;
+		malipmm_core_power_down_okay( core->pmm_id );
+	}
+	else
+	{
+		core->state = CORE_IDLE ;
+		_mali_osk_list_move( &core->list, &subsystem->renderunit_idle_head );
+	}
+
+	if( CORE_OFF != oldstatus )
+	{
+		/* Message that this core is now idle or in fact off */
+		_mali_uk_pmm_message_s event = {
+			NULL,
+			MALI_PMM_EVENT_JOB_FINISHED,
+			0 };
+		event.data = core->pmm_id;
+		_mali_ukk_pmm_event_message( &event );
+#if USING_MMU
+		/* Only free the reference when entering idle state from
+		 * anything other than power off
+		 */
+		mali_memory_core_mmu_release_address_space_reference(core->mmu);
+#endif /* USING_MMU */
+	}
+
+
+#else /* !USING_MALI_PMM */
+
+	core->state = CORE_IDLE ;
+	_mali_osk_list_move( &core->list, &subsystem->renderunit_idle_head );
+
+#if USING_MMU
+		mali_memory_core_mmu_release_address_space_reference(core->mmu);
+#endif
+
+#endif /* USING_MALI_PMM */
+}
+
+/* Must hold subsystem_mutex before entering this function */
+static void mali_core_subsystem_move_set_working(mali_core_renderunit *core, mali_core_job *job)
+{
+	mali_core_subsystem *subsystem;
+	mali_core_session *session;
+
+	session   = job->session;
+	subsystem = core->subsystem;
+
+	MALI_CHECK_CORE(core);
+	MALI_CHECK_JOB(job);
+	MALI_CHECK_SUBSYSTEM(subsystem);
+
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsystem);
+
+	MALI_DEBUG_PRINT(5, ("Core: subsystem_move_set_working: %s\n", core->description) ) ;
+
+	core->current_job = job ;
+	core->state = CORE_WORKING ;
+	job->start_time_jiffies = _mali_osk_time_tickcount();
+	_mali_osk_list_move( &core->list, &session->renderunits_working_head );
+
+}
+
+#if USING_MALI_PMM
+
+/* Must hold subsystem_mutex before entering this function */
+static void mali_core_subsystem_move_core_set_off(mali_core_renderunit *core)
+{
+	mali_core_subsystem *subsystem;
+	subsystem = core->subsystem;
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsystem);
+	MALI_CHECK_CORE(core);
+	MALI_CHECK_SUBSYSTEM(subsystem);
+
+	/* Cores must be idle before powering off */
+	MALI_DEBUG_ASSERT(core->state == CORE_IDLE);
+
+    MALI_DEBUG_PRINT(5, ("Core: subsystem_move_core_set_off: %s\n", core->description) ) ;
+
+	core->current_job = NULL ;
+	core->state = CORE_OFF ;
+	_mali_osk_list_move( &core->list, &subsystem->renderunit_off_head );
+}
+
+#endif /* USING_MALI_PMM */
+
+/* Is used by internal function:
+	mali_core_subsystem_schedule<>;	*/
+/* Returns the job with the highest priority for the subsystem. NULL if none*/
+/* Must hold subsystem_mutex before entering this function */
+static mali_core_session * mali_core_subsystem_get_waiting_session(mali_core_subsystem *subsystem)
+{
+	int i;
+
+	MALI_CHECK_SUBSYSTEM(subsystem);
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsystem);
+
+	if ( 0 == subsystem->awaiting_sessions_sum_all_priorities )
+	{
+		MALI_DEBUG_PRINT(5, ("Core: subsystem_get_waiting_job: No awaiting session found\n"));
+		return NULL;
+	}
+
+	for( i=0; i<PRIORITY_LEVELS ; ++i)
+	{
+		if (!_mali_osk_list_empty(&subsystem->awaiting_sessions_head[i]))
+		{
+			return _MALI_OSK_LIST_ENTRY(subsystem->awaiting_sessions_head[i].next, mali_core_session, awaiting_sessions_list);
+		}
+	}
+
+	return NULL;
+}
+
+static mali_core_job * mali_core_subsystem_release_session_get_job(mali_core_subsystem *subsystem, mali_core_session * session)
+{
+	mali_core_job *job;
+	MALI_CHECK_SUBSYSTEM(subsystem);
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsystem);
+
+	_mali_osk_list_delinit(&session->awaiting_sessions_list);
+	subsystem->awaiting_sessions_sum_all_priorities--;
+	job = session->job_waiting_to_run;
+	session->job_waiting_to_run = NULL;
+	MALI_CHECK_JOB(job);
+	return job;
+}
+
+/* Is used by internal functions:
+	mali_core_subsystem_schedule<> */
+/* This will start the job on the core. It will also release the core if it did not start.*/
+/* Must hold subsystem_mutex before entering this function */
+static void mali_core_job_start_on_core(mali_core_job *job, mali_core_renderunit *core)
+{
+	mali_core_session *session;
+	mali_core_subsystem *subsystem;
+	_mali_osk_errcode_t err;
+	session   = job->session;
+	subsystem = core->subsystem;
+
+	MALI_CHECK_CORE(core);
+	MALI_CHECK_JOB(job);
+	MALI_CHECK_SUBSYSTEM(subsystem);
+	MALI_CHECK_SESSION(session);
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsystem);
+
+	MALI_DEBUG_PRINT(4, ("Core: job_start_on_core: job=0x%x, session=0x%x, core=%s\n", job, session, core->description));
+
+	MALI_DEBUG_ASSERT(NULL == core->current_job) ;
+	MALI_DEBUG_ASSERT(CORE_IDLE == core->state );
+
+	mali_core_subsystem_move_set_working(core, job);
+
+#if defined USING_MALI400_L2_CACHE
+	/* Invalidate the L2 cache */
+	if (_MALI_OSK_ERR_OK != mali_kernel_l2_cache_invalidate_all() )
+	{
+		MALI_DEBUG_PRINT(4, ("Core: Clear of L2 failed, return job. System may not be usable for some reason.\n"));
+		mali_core_subsystem_move_core_set_idle(core);
+		subsystem->return_job_to_user(job,JOB_STATUS_END_SYSTEM_UNUSABLE );
+		return;
+	}
+#endif
+
+	/* Tries to start job on the core. Returns MALI_FALSE if the job could not be started */
+	err = subsystem->start_job(job, core);
+
+#if MALI_GPU_UTILIZATION
+	mali_utilization_core_start();
+#endif
+
+	if ( _MALI_OSK_ERR_OK != err )
+	{
+		/* This will happen only if there is something in the job object
+		which make it inpossible to start. Like if it require illegal memory.*/
+		MALI_DEBUG_PRINT(4, ("Core: start_job failed, return job and putting core back into idle list\n"));
+		mali_core_subsystem_move_core_set_idle(core);
+		subsystem->return_job_to_user(job,JOB_STATUS_END_ILLEGAL_JOB );
+	}
+	else
+	{
+		u32 delay = _mali_osk_time_mstoticks(job->watchdog_msecs)+1;
+		job->watchdog_jiffies = _mali_osk_time_tickcount() + delay;
+		if (mali_benchmark)
+		{
+			_mali_osk_timer_add(core->timer, 1);
+		}
+		else
+		{
+			_mali_osk_timer_add(core->timer, delay);
+		}
+	}
+}
+
+#if USING_MMU
+static void mali_core_subsystem_callback_schedule_wrapper(void* sub)
+{
+	mali_core_subsystem * subsystem;
+	subsystem = (mali_core_subsystem *)sub;
+	MALI_DEBUG_PRINT(3, ("MMU: Is schedulling subsystem: %s\n", subsystem->name));
+	mali_core_subsystem_schedule(subsystem);
+}
+#endif
+
+/* Is used by internal function:
+	mali_core_irq_handler_bottom_half
+	mali_core_session_add_job
+*/
+/* Must hold subsystem_mutex before entering this function */
+static void mali_core_subsystem_schedule(mali_core_subsystem * subsystem)
+{
+	mali_core_renderunit *core, *tmp;
+	mali_core_session *session;
+	mali_core_job *job;
+
+	MALI_DEBUG_PRINT(5, ("Core: subsystem_schedule: %s\n", subsystem->name )) ;
+
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsystem);
+
+	/* First check that there are sessions with jobs waiting to run */
+	if ( 0 == subsystem->awaiting_sessions_sum_all_priorities)
+	{
+		MALI_DEBUG_PRINT(6, ("Core: No jobs available for %s\n", subsystem->name) ) ;
+		return;
+	}
+
+	/* Returns the session with the highest priority job for the subsystem. NULL if none*/
+	session = mali_core_subsystem_get_waiting_session(subsystem);
+
+	if (NULL == session)
+	{
+		MALI_DEBUG_PRINT(6, ("Core: Schedule: No runnable job found\n"));
+		return;
+	}
+
+	_MALI_OSK_LIST_FOREACHENTRY(core, tmp, &subsystem->renderunit_idle_head, mali_core_renderunit, list)
+	{
+#if USING_MMU
+		int err = mali_memory_core_mmu_activate_page_table(core->mmu, session->mmu_session, mali_core_subsystem_callback_schedule_wrapper, subsystem);
+		if (0 == err)
+		{
+			/* core points to a core where the MMU page table activation succeeded */
+#endif
+			/* This will remove the job from queue system */
+			job = mali_core_subsystem_release_session_get_job(subsystem, session);
+			MALI_DEBUG_ASSERT_POINTER(job);
+
+			MALI_DEBUG_PRINT(6, ("Core: Schedule: Got a job 0x%x\n", job));
+
+#if USING_MALI_PMM
+			{
+				/* Message that there is a job scheduled to run 
+				 * NOTE: mali_core_job_start_on_core() can fail to start
+				 * the job for several reasons, but it will move the core
+				 * back to idle which will create the FINISHED message
+				 * so we can still say that the job is SCHEDULED
+				 */
+				_mali_uk_pmm_message_s event = {
+					NULL,
+					MALI_PMM_EVENT_JOB_SCHEDULED,
+					0 };
+				event.data = core->pmm_id;
+				_mali_ukk_pmm_event_message( &event );
+			}
+#endif
+			/* This will {remove core from freelist AND start the job on the core}*/
+			mali_core_job_start_on_core(job, core);
+
+			MALI_DEBUG_PRINT(6, ("Core: Schedule: Job started, done\n"));
+			return;
+#if USING_MMU
+		}
+#endif
+	}
+	MALI_DEBUG_PRINT(6, ("Core: Schedule: Could not activate MMU. Scheduelling postponed to MMU, checking next.\n"));
+
+#if USING_MALI_PMM
+	{
+		/* Message that there are jobs to run */
+		_mali_uk_pmm_message_s event = {
+			NULL,
+			MALI_PMM_EVENT_JOB_QUEUED,
+			0 };
+		if( subsystem->core_type == _MALI_GP2 || subsystem->core_type == _MALI_400_GP )
+		{
+			event.data = MALI_PMM_CORE_GP;
+		}
+		else
+		{
+			/* Check the PP is supported by the PMM */
+			MALI_DEBUG_ASSERT( subsystem->core_type == _MALI_200 || subsystem->core_type == _MALI_400_PP );
+			/* We state that all PP cores are scheduled to inform the PMM
+			 * that it may need to power something up!
+			 */
+			event.data = MALI_PMM_CORE_PP_ALL;
+		}
+		_mali_ukk_pmm_event_message( &event );
+	}
+#endif /* USING_MALI_PMM */
+
+}
+
+/* Is used by external function:
+	session_begin<> */
+void mali_core_session_begin(mali_core_session * session)
+{
+	mali_core_subsystem * subsystem;
+
+	subsystem = session->subsystem;
+	if ( NULL == subsystem )
+	{
+		MALI_PRINT_ERROR(("Missing data in struct\n"));
+		return;
+	}
+	MALI_DEBUG_PRINT(2, ("Core: session_begin: for %s\n", session->subsystem->name )) ;
+
+	session->magic_nr = SESSION_MAGIC_NR;
+
+	_MALI_OSK_INIT_LIST_HEAD(&session->renderunits_working_head);
+
+	session->job_waiting_to_run = NULL;
+	_MALI_OSK_INIT_LIST_HEAD(&session->awaiting_sessions_list);
+	_MALI_OSK_INIT_LIST_HEAD(&session->all_sessions_list);
+
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsystem);
+	_mali_osk_list_add(&session->all_sessions_list, &session->subsystem->all_sessions_head);
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsystem);
+
+	MALI_DEBUG_PRINT(5, ("Core: session_begin: for %s DONE\n", session->subsystem->name) ) ;
+}
+
+#if USING_MMU
+static void mali_core_renderunit_stop_bus(mali_core_renderunit* core)
+{
+	core->subsystem->stop_bus(core);
+}
+#endif
+
+void mali_core_session_close(mali_core_session * session)
+{
+	mali_core_subsystem * subsystem;
+	mali_core_renderunit *core;
+
+	subsystem = session->subsystem;
+	MALI_DEBUG_ASSERT_POINTER(subsystem);
+
+	MALI_DEBUG_PRINT(2, ("Core: session_close: for %s\n", session->subsystem->name) ) ;
+
+	/* We must grab subsystem mutex since the list this session belongs to
+	is owned by the subsystem */
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB( subsystem );
+
+	/* Return the potensial waiting job to user */
+	if ( session->job_waiting_to_run )
+	{
+		subsystem->return_job_to_user( session->job_waiting_to_run, JOB_STATUS_END_SHUTDOWN );
+		session->job_waiting_to_run = NULL;
+		_mali_osk_list_delinit(&(session->awaiting_sessions_list));
+		subsystem->awaiting_sessions_sum_all_priorities--;
+	}
+
+	/* Kill active cores working for this session - freeing their jobs
+	   Since the handling of one core also could stop jobs from another core, there is a while loop */
+	while ( ! _mali_osk_list_empty(&session->renderunits_working_head) )
+	{
+		core = _MALI_OSK_LIST_ENTRY(session->renderunits_working_head.next, mali_core_renderunit, list);
+		MALI_DEBUG_PRINT(3, ("Core: session_close: Core was working: %s\n", core->description )) ;
+		mali_core_renderunit_detach_job_from_core(core, SUBSYSTEM_RESCHEDULE, JOB_STATUS_END_SHUTDOWN );
+		break;
+	}
+	_MALI_OSK_INIT_LIST_HEAD(&session->renderunits_working_head); /* Not necessary - we will _mali_osk_free session*/
+
+	/* Remove this session from the global sessionlist */
+	_mali_osk_list_delinit(&session->all_sessions_list);
+
+	MALI_DEBUG_PRINT(5, ("Core: session_close: for %s FINISHED\n", session->subsystem->name )) ;
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE( subsystem );
+}
+
+/* Must hold subsystem_mutex before entering this function */
+_mali_osk_errcode_t mali_core_session_add_job(mali_core_session * session, mali_core_job *job, mali_core_job **job_return)
+{
+	mali_core_subsystem * subsystem;
+
+	job->magic_nr = JOB_MAGIC_NR;
+	MALI_CHECK_SESSION(session);
+
+	subsystem = session->subsystem;
+	MALI_CHECK_SUBSYSTEM(subsystem);
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsystem);
+
+	MALI_DEBUG_PRINT(5, ("Core: session_add_job: for %s\n", subsystem->name )) ;
+
+	/* Setting the default value; No job to return */
+	MALI_DEBUG_ASSERT_POINTER(job_return);
+	*job_return = NULL;
+
+	if ( NULL != session->job_waiting_to_run)
+	{
+		MALI_DEBUG_PRINT(5, ("The session already had a job waiting\n")) ;
+		/* Checing if the new job has a higher priority than the one that was pending.*/
+		if ( job_has_higher_priority(job,session->job_waiting_to_run))
+		{
+			/* Remove this session from current priority */
+			_mali_osk_list_del( &(session->awaiting_sessions_list));
+			subsystem->awaiting_sessions_sum_all_priorities--;
+			/* Returning the previous waiting job through the input double pointer*/
+			*job_return = session->job_waiting_to_run;
+		}
+		else
+		{
+			MALI_PRINT_ERROR(("Illegal internal state."));
+			/* There was a job waiting in this session, and the priority of this job
+			   we try to add was NOT higher. Return -1 indicated new job NOT enqueued.*/
+			/* We check prior to calling this function that we are not in this state.*/
+            MALI_ERROR(_MALI_OSK_ERR_FAULT);
+		}
+	}
+	/* Continue to add the new job as the next job from this session */
+	MALI_DEBUG_PRINT(6, ("Core: session_add_job job=0x%x\n", job));
+
+	/* Adding this session to the subsystem list of sessions with pending job, with priority */
+	session->job_waiting_to_run = job;
+
+	_mali_osk_list_addtail( &(session->awaiting_sessions_list), &(subsystem->awaiting_sessions_head[job->priority]));
+	subsystem->awaiting_sessions_sum_all_priorities++;
+
+	mali_core_subsystem_schedule(subsystem);
+
+	MALI_DEBUG_PRINT(6, ("Core: session_add_job: for %s FINISHED\n", session->subsystem->name )) ;
+
+	MALI_SUCCESS;
+}
+
+static void mali_core_job_set_run_time(mali_core_job * job)
+{
+	u32 jiffies_used;
+	jiffies_used =	_mali_osk_time_tickcount() - job->start_time_jiffies;
+	if ( jiffies_used > JOB_MAX_JIFFIES )
+	{
+		MALI_PRINT_ERROR(("Job used too many jiffies: %d\n", jiffies_used ));
+		jiffies_used = 0;
+	}
+	job->render_time_msecs = _mali_osk_time_tickstoms(jiffies_used);
+}
+
+static void mali_core_renderunit_detach_job_from_core(mali_core_renderunit* core, mali_subsystem_reschedule_option reschedule, mali_subsystem_job_end_code end_status)
+{
+	mali_core_job * job;
+	mali_core_subsystem * subsystem;
+	mali_bool already_in_detach_function;
+
+	job = core->current_job;
+	subsystem = core->subsystem;
+	MALI_DEBUG_ASSERT(CORE_IDLE != core->state);
+
+	/* The reset_core() called some lines below might call this detach
+	 * funtion again. To protect the core object from being modified by 
+	 * recursive calls, the in_detach_function would track if it is an recursive call
+	 */
+	already_in_detach_function = core->in_detach_function;
+	
+	if ( MALI_FALSE == already_in_detach_function )
+	{
+		core->in_detach_function = MALI_TRUE;
+		if ( NULL != job )
+		{
+			mali_core_job_set_run_time(job);
+			core->current_job = NULL;
+		}
+	}
+
+	if (JOB_STATUS_END_SEG_FAULT == end_status)
+	{
+		subsystem->reset_core( core, MALI_CORE_RESET_STYLE_HARD );
+	}
+	else
+	{
+		subsystem->reset_core( core, MALI_CORE_RESET_STYLE_RUNABLE );
+	}
+
+	if ( MALI_FALSE == already_in_detach_function )
+	{
+		if ( CORE_IDLE != core->state )
+		{
+			#if MALI_GPU_UTILIZATION
+			mali_utilization_core_end();
+			#endif
+			mali_core_subsystem_move_core_set_idle(core);
+		}
+
+		core->in_detach_function = MALI_FALSE;
+
+		if ( SUBSYSTEM_RESCHEDULE == reschedule )
+		{
+			mali_core_subsystem_schedule(subsystem);
+		}
+		if ( NULL != job )
+		{
+			core->subsystem->return_job_to_user(job, end_status);
+		}
+	}
+}
+
+#if USING_MMU
+/* This function intentionally does not release the semaphore. You must run
+   stop_bus_for_all_cores(), reset_all_cores_on_mmu() and continue_job_handling()
+   after calling this function, and then call unlock_subsystem() to release the
+   semaphore. */
+
+static void lock_subsystem(struct mali_core_subsystem * subsys)
+{
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsys);
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+}
+
+/* You must run lock_subsystem() before entering this function, to ensure that
+   the subsystem mutex is held.
+   Later, unlock_subsystem() can be called to release the mutex.
+
+   This function only stops cores behind the given MMU, unless "mmu" is NULL, in
+   which case all cores are stopped.
+*/
+static void stop_bus_for_all_cores_on_mmu(struct mali_core_subsystem * subsys, void* mmu)
+{
+	u32 i;
+
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+	MALI_DEBUG_PRINT(2,("Handling: bus stop %s\n", subsys->name ));
+	for(i=0 ; i < subsys->number_of_cores ; ++i)
+	{
+		mali_core_renderunit * core;
+		core = mali_core_renderunit_get_mali_core_nr(subsys,i);
+
+		/* We stop only cores behind the given MMU, unless MMU is NULL */
+		if ( (NULL!=mmu) && (core->mmu != mmu) ) continue;
+
+		if ( CORE_IDLE != core->state )
+		{
+			MALI_DEBUG_PRINT(4, ("Stopping bus on core %s\n", core->description));
+			mali_core_renderunit_stop_bus(core);
+			core->error_recovery = MALI_TRUE;
+		}
+		else
+		{
+			MALI_DEBUG_PRINT(4,("Core: not active %s\n", core->description ));
+		}
+	}
+	/* Mutex is still being held, to prevent things to happen while we do cleanup */
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+}
+
+/* You must run lock_subsystem() before entering this function, to ensure that
+   the subsystem mutex is held.
+   Later, unlock_subsystem() can be called to release the mutex.
+
+   This function only resets cores behind the given MMU, unless "mmu" is NULL, in
+   which case all cores are reset.
+*/
+static void reset_all_cores_on_mmu(struct mali_core_subsystem * subsys, void* mmu)
+{
+	u32 i;
+
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+	MALI_DEBUG_PRINT(3, ("Handling: reset cores from mmu: 0x%x on %s\n", mmu, subsys->name ));
+	for(i=0 ; i < subsys->number_of_cores ; ++i)
+	{
+		mali_core_renderunit * core;
+		core = mali_core_renderunit_get_mali_core_nr(subsys,i);
+
+		/* We reset only cores behind the given MMU, unless MMU is NULL */
+		if ( (NULL!=mmu) && (core->mmu != mmu) ) continue;
+
+		if ( CORE_IDLE != core->state )
+		{
+			MALI_DEBUG_PRINT(4, ("Abort and reset core: %s\n", core->description ));
+			mali_core_renderunit_detach_job_from_core(core, SUBSYSTEM_WAIT, JOB_STATUS_END_SEG_FAULT);
+		}
+		else
+		{
+			MALI_DEBUG_PRINT(4, ("Core: not active %s\n", core->description ));
+		}
+	}
+	MALI_DEBUG_PRINT(4, ("Handling: done %s\n", subsys->name ));
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+}
+
+/* You must run lock_subsystem() before entering this function, to ensure that
+   the subsystem mutex is held.
+   Later, unlock_subsystem() can be called to release the mutex. */
+static void continue_job_handling(struct mali_core_subsystem * subsys)
+{
+	u32 i, j;
+
+	MALI_DEBUG_PRINT(3, ("Handling: Continue: %s\n", subsys->name ));
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+
+
+	for(i=0 ; i < subsys->number_of_cores ; ++i)
+	{
+		mali_core_renderunit * core;
+		core = mali_core_renderunit_get_mali_core_nr(subsys,i);
+		core->error_recovery = MALI_FALSE;
+	}
+
+	i = subsys->number_of_cores;
+	j = subsys->awaiting_sessions_sum_all_priorities;
+	
+	/* Schedule MIN(nr_waiting_jobs , number of cores) times */
+	while( i-- && j--)
+	{
+		mali_core_subsystem_schedule(subsys);
+	}
+ 	MALI_DEBUG_PRINT(4, ("Handling: done %s\n", subsys->name ));
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+}
+
+/* Unlock the subsystem. */
+static void unlock_subsystem(struct mali_core_subsystem * subsys)
+{
+	MALI_ASSERT_MUTEX_IS_GRABBED(subsys);
+ 	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+}
+
+void mali_core_subsystem_broadcast_notification(struct mali_core_subsystem * subsys, mali_core_notification_message message, u32 data)
+{
+	void * mmu;
+	mmu = (void*) data;
+
+	switch(message)
+	{
+		case MMU_KILL_STEP0_LOCK_SUBSYSTEM:
+			break;
+		case MMU_KILL_STEP1_STOP_BUS_FOR_ALL_CORES:
+			stop_bus_for_all_cores_on_mmu(subsys, mmu);
+			break;
+		case MMU_KILL_STEP2_RESET_ALL_CORES_AND_ABORT_THEIR_JOBS:
+			reset_all_cores_on_mmu(subsys, mmu );
+			break;
+		case MMU_KILL_STEP3_CONTINUE_JOB_HANDLING:
+			continue_job_handling(subsys);
+			break;
+		case MMU_KILL_STEP4_UNLOCK_SUBSYSTEM:
+			break;
+
+		default:
+			MALI_PRINT_ERROR(("Illegal message: 0x%x, data: 0x%x\n", (u32)message, data));
+			break;
+	}
+}
+#endif /* USING_MMU */
+
+void job_watchdog_set(mali_core_job * job, u32 watchdog_msecs)
+{
+	if (watchdog_msecs == 0) job->watchdog_msecs = mali_max_job_runtime; /* use the default */
+	else if (watchdog_msecs > WATCHDOG_MSECS_MAX) job->watchdog_msecs = WATCHDOG_MSECS_MAX; /* no larger than max */
+	else if (watchdog_msecs < WATCHDOG_MSECS_MIN) job->watchdog_msecs = WATCHDOG_MSECS_MIN; /* not below min */
+	else job->watchdog_msecs = watchdog_msecs;
+}
+
+u32 mali_core_hang_check_timeout_get(void)
+{
+	/* check the value. The user might have set the value outside the allowed range */
+	if (mali_hang_check_interval > HANG_CHECK_MSECS_MAX) mali_hang_check_interval = HANG_CHECK_MSECS_MAX; /* cap to max */
+	else if (mali_hang_check_interval < HANG_CHECK_MSECS_MIN) mali_hang_check_interval = HANG_CHECK_MSECS_MIN; /* cap to min */
+
+	/* return the active value */
+	return mali_hang_check_interval;
+}
+
+static _mali_osk_errcode_t  mali_core_irq_handler_upper_half (void * data)
+{
+	mali_core_renderunit *core;
+	u32 has_pending_irq;
+
+    core  = (mali_core_renderunit * )data;
+
+	if ( (NULL == core) ||
+		 (NULL == core->subsystem) ||
+		 (NULL == core->subsystem->irq_handler_upper_half) )
+	{
+		MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}
+	MALI_CHECK_CORE(core);
+	MALI_CHECK_SUBSYSTEM(core->subsystem);
+
+	has_pending_irq = core->subsystem->irq_handler_upper_half(core);
+
+	if ( has_pending_irq )
+	{
+		_mali_osk_irq_schedulework( core->irq ) ;
+		MALI_SUCCESS;
+	}
+
+	if (mali_benchmark) MALI_SUCCESS;
+
+	MALI_ERROR(_MALI_OSK_ERR_FAULT);
+}
+
+static void mali_core_irq_handler_bottom_half ( void *data )
+{
+	mali_core_renderunit *core;
+	mali_core_subsystem* subsystem;
+
+	mali_subsystem_job_end_code job_status;
+
+    core  = (mali_core_renderunit * )data;
+
+	MALI_CHECK_CORE(core);
+	subsystem = core->subsystem;
+	MALI_CHECK_SUBSYSTEM(subsystem);
+
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB( subsystem );
+	if ( CORE_IDLE == core->state ) goto end_function;
+
+	MALI_DEBUG_PRINT(5, ("IRQ: handling irq from core %s\n", core->description )) ;
+
+	_mali_osk_cache_flushall();
+
+	/* This function must also update the job status flag */
+	job_status = subsystem->irq_handler_bottom_half( core );
+
+	/* Retval is nonzero if the job is finished. */
+	if ( JOB_STATUS_CONTINUE_RUN != job_status )
+	{
+		mali_core_renderunit_detach_job_from_core(core, SUBSYSTEM_RESCHEDULE, job_status);
+	}
+	else
+	{
+		switch ( core->state )
+		{
+			case CORE_WATCHDOG_TIMEOUT:
+				MALI_DEBUG_PRINT(2, ("Watchdog SW Timeout of job from core: %s\n", core->description ));
+				mali_core_renderunit_detach_job_from_core(core, SUBSYSTEM_RESCHEDULE, JOB_STATUS_END_TIMEOUT_SW );
+				break;
+
+			case CORE_POLL:
+				MALI_DEBUG_PRINT(5, ("Poll core: %s\n", core->description )) ;
+				core->state = CORE_WORKING;
+				_mali_osk_timer_add( core->timer, 1);
+				break;
+
+			default:
+				MALI_DEBUG_PRINT(4, ("IRQ: The job on the core continue to run: %s\n", core->description )) ;
+				break;
+		}
+	}
+end_function:
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsystem);
+}
+
+void subsystem_flush_mapped_mem_cache(void)
+{
+	_mali_osk_cache_flushall();
+	_mali_osk_mem_barrier();
+}
+
+#if USING_MALI_PMM
+
+_mali_osk_errcode_t mali_core_subsystem_signal_power_down(mali_core_subsystem *subsys, u32 mali_core_nr, mali_bool immediate_only)
+{
+	mali_core_renderunit * core = NULL;
+
+	MALI_CHECK_SUBSYSTEM(subsys);
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsys);
+
+	/* It is possible that this signal funciton can be called during a driver exit, 
+	 * and so the requested core may now be destroyed. (This is due to us not having
+	 * the subsys lock before signalling power down).
+	 * mali_core_renderunit_get_mali_core_nr() will report a Mali ERR because
+	 * the core number is out of range (which is a valid error in other cases). 
+	 * So instead we check here (now that we have the subsys lock) and let the 
+	 * caller cope with the core get failure and check that the core has 
+	 * been unregistered in the PMM as part of its destruction.
+	 */
+	if ( subsys->number_of_cores > mali_core_nr )
+	{
+		core = mali_core_renderunit_get_mali_core_nr(subsys, mali_core_nr);
+	}
+
+	if ( NULL == core )
+	{
+		/* Couldn't find the core */
+		MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+		MALI_DEBUG_PRINT( 5, ("Core: Failed to find core to power down\n") );
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+	else if ( core->state != CORE_IDLE )
+	{
+		/* When powering down we either set a pending power down flag here so we
+		 * can power down cleanly after the job completes or we don't set the 
+		 * flag if we have been asked to only do a power down right now
+		 * In either case, return that the core is busy
+		 */
+		if ( !immediate_only ) core->pend_power_down = MALI_TRUE;
+		MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+		MALI_DEBUG_PRINT( 5, ("Core: No idle core to power down\n") );
+        MALI_ERROR(_MALI_OSK_ERR_BUSY);
+	}
+
+	/* Shouldn't have a pending power down flag set */
+	MALI_DEBUG_ASSERT( !core->pend_power_down );
+
+	/* Move core to off queue */
+	mali_core_subsystem_move_core_set_off(core);
+	
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+
+    MALI_SUCCESS;
+}
+	
+_mali_osk_errcode_t mali_core_subsystem_signal_power_up(mali_core_subsystem *subsys, u32 mali_core_nr, mali_bool queue_only)
+{
+	mali_core_renderunit * core;
+
+	MALI_CHECK_SUBSYSTEM(subsys);
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsys);
+
+	core = mali_core_renderunit_get_mali_core_nr(subsys, mali_core_nr);
+
+	if( core == NULL )
+	{
+		/* Couldn't find the core */
+		MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+		MALI_DEBUG_PRINT( 5, ("Core: Failed to find core to power up\n") );
+        MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+	else if( core->state != CORE_OFF )
+	{
+		/* This will usually happen because we are trying to cancel a pending power down */
+		core->pend_power_down = MALI_FALSE;
+		MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+		MALI_DEBUG_PRINT( 5, ("Core: No powered off core to power up (cancelled power down?)\n") );
+        MALI_ERROR(_MALI_OSK_ERR_BUSY);
+	}
+
+	/* Shouldn't have a pending power down set */
+	MALI_DEBUG_ASSERT( !core->pend_power_down );
+
+	/* Move core to idle queue */
+	mali_core_subsystem_move_core_set_idle(core);
+
+	if( !queue_only )
+	{
+		/* Reset MMU & core - core must be idle to allow this */
+#if USING_MMU
+		if ( NULL!=core->mmu )
+		{
+#if defined(USING_MALI200)
+			if (core->pmm_id != MALI_PMM_CORE_PP0)
+			{
+#endif
+				mali_kernel_mmu_reset(core->mmu);
+#if defined(USING_MALI200)
+			}
+#endif
+
+		}
+#endif /* USING_MMU */
+		subsys->reset_core( core, MALI_CORE_RESET_STYLE_RUNABLE );
+	}
+
+	/* Need to schedule work to start on this core */
+	mali_core_subsystem_schedule(subsys);
+	
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys);
+
+    MALI_SUCCESS;
+}
+
+#endif /* USING_MALI_PMM */
+
+#if MALI_STATE_TRACKING
+void mali_core_renderunit_dump_state(mali_core_subsystem* subsystem)
+{
+	u32 i;
+	mali_core_renderunit *core;
+	mali_core_renderunit *tmp_core;
+	
+	mali_core_session* session;
+	mali_core_session* tmp_session;
+
+	MALI_CORE_SUBSYSTEM_MUTEX_GRAB( subsystem );
+
+	MALI_PRINT(("Subsystem;\n"));
+	MALI_PRINT(("  Name: %s\n", subsystem->name));
+
+	for (i = 0; i < subsystem->number_of_cores; i++)
+	{
+		MALI_PRINT(("  Core: #%u\n", subsystem->mali_core_array[i]->core_number));
+		MALI_PRINT(("    Description: %s\n", subsystem->mali_core_array[i]->description));
+		switch(subsystem->mali_core_array[i]->state)
+		{
+			case CORE_IDLE:
+				MALI_PRINT(("    State: CORE_IDLE\n"));
+				break;
+			case CORE_WORKING:
+				MALI_PRINT(("    State: CORE_WORKING\n"));
+					break;
+			case CORE_WATCHDOG_TIMEOUT:
+				MALI_PRINT(("    State: CORE_WATCHDOG_TIMEOUT\n"));
+				break;
+			case CORE_POLL:
+				MALI_PRINT(("    State: CORE_POLL\n"));
+				break;
+			case CORE_HANG_CHECK_TIMEOUT:
+				MALI_PRINT(("    State: CORE_HANG_CHECK_TIMEOUT\n"));
+				break;
+			case CORE_OFF:
+				MALI_PRINT(("    State: CORE_OFF\n"));
+				break;
+			default:
+				MALI_PRINT(("    State: Unknown (0x%X)\n", subsystem->mali_core_array[i]->state));
+				break;
+		}
+		MALI_PRINT(("    Current job: 0x%x\n", (u32)(subsystem->mali_core_array[i]->current_job)));
+		MALI_PRINT(("    Core version: 0x%x\n", subsystem->mali_core_array[i]->core_version));
+#if USING_MALI_PMM
+		MALI_PRINT(("    PMM id: 0x%x\n", subsystem->mali_core_array[i]->pmm_id));
+		MALI_PRINT(("    Power down requested: %s\n", subsystem->mali_core_array[i]->pend_power_down ? "TRUE" : "FALSE"));
+#endif
+	}
+
+	MALI_PRINT(("  Cores on idle list:\n"));
+	_MALI_OSK_LIST_FOREACHENTRY(core, tmp_core, &subsystem->renderunit_idle_head, mali_core_renderunit, list)
+	{
+		MALI_PRINT(("    Core #%u\n", core->core_number));
+	}
+
+	MALI_PRINT(("  Cores on off list:\n"));
+	_MALI_OSK_LIST_FOREACHENTRY(core, tmp_core, &subsystem->renderunit_off_head, mali_core_renderunit, list)
+	{
+		MALI_PRINT(("    Core #%u\n", core->core_number));
+	}
+
+	MALI_PRINT(("  Connected sessions:\n"));
+	_MALI_OSK_LIST_FOREACHENTRY(session, tmp_session, &subsystem->all_sessions_head, mali_core_session, all_sessions_list)
+	{
+		MALI_PRINT(("    Session 0x%X:\n", (u32)session));
+		MALI_PRINT(("      Waiting job: 0x%X\n", (u32)session->job_waiting_to_run));
+		MALI_PRINT(("      Notification queue: %s\n", _mali_osk_notification_queue_is_empty(session->notification_queue) ? "EMPTY" : "NON-EMPTY"));
+	}
+
+	MALI_PRINT(("  Waiting sessions sum all priorities: %u\n", subsystem->awaiting_sessions_sum_all_priorities));
+	for (i = 0; i < PRIORITY_LEVELS; i++)
+	{
+		MALI_PRINT(("    Waiting sessions with priority %u:\n", i));
+		_MALI_OSK_LIST_FOREACHENTRY(session, tmp_session, &subsystem->awaiting_sessions_head[i], mali_core_session, awaiting_sessions_list)
+		{
+			MALI_PRINT(("      Session 0x%X:\n", (u32)session));
+			MALI_PRINT(("        Waiting job: 0x%X\n", (u32)session->job_waiting_to_run));
+			MALI_PRINT(("        Notification queue: %s\n", _mali_osk_notification_queue_is_empty(session->notification_queue) ? "EMPTY" : "NON-EMPTY"));
+		}
+	}
+
+	MALI_CORE_SUBSYSTEM_MUTEX_RELEASE( subsystem );
+}
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_rendercore.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_rendercore.h
new file mode 100644
index 00000000000..94bce94d4e1
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_rendercore.h
@@ -0,0 +1,355 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_RENDERCORE_H__
+#define __MALI_RENDERCORE_H__
+
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_subsystem.h"
+
+#define PRIORITY_LEVELS 3
+#define PRIORITY_MAX 0
+#define PRIORITY_MIN (PRIORITY_MAX+PRIORITY_LEVELS-1)
+
+/* This file contains what we need in kernel for all core types. */
+
+typedef enum
+{
+	CORE_IDLE,              /**< Core is ready for a new job */
+	CORE_WORKING,           /**< Core is working on a job */
+	CORE_WATCHDOG_TIMEOUT,  /**< Core is working but it has timed out */
+	CORE_POLL,              /**< Poll timer triggered, pending handling */
+	CORE_HANG_CHECK_TIMEOUT,/**< Timeout for hang detection */
+	CORE_OFF                /**< Core is powered off */
+} mali_core_status;
+
+typedef enum
+{
+	SUBSYSTEM_RESCHEDULE,
+	SUBSYSTEM_WAIT
+} mali_subsystem_reschedule_option;
+
+typedef enum
+{
+	MALI_CORE_RESET_STYLE_RUNABLE,
+	MALI_CORE_RESET_STYLE_DISABLE,
+	MALI_CORE_RESET_STYLE_HARD
+} mali_core_reset_style;
+
+typedef enum
+{
+	JOB_STATUS_CONTINUE_RUN			= 0x01,
+	JOB_STATUS_END_SUCCESS			= 1<<(16+0),
+	JOB_STATUS_END_OOM				= 1<<(16+1),
+	JOB_STATUS_END_ABORT			= 1<<(16+2),
+	JOB_STATUS_END_TIMEOUT_SW		= 1<<(16+3),
+	JOB_STATUS_END_HANG				= 1<<(16+4),
+	JOB_STATUS_END_SEG_FAULT		= 1<<(16+5),
+	JOB_STATUS_END_ILLEGAL_JOB		= 1<<(16+6),
+	JOB_STATUS_END_UNKNOWN_ERR		= 1<<(16+7),
+	JOB_STATUS_END_SHUTDOWN			= 1<<(16+8),
+	JOB_STATUS_END_SYSTEM_UNUSABLE	= 1<<(16+9)
+} mali_subsystem_job_end_code;
+
+
+struct mali_core_job;
+struct mali_core_subsystem;
+struct mali_core_renderunit;
+struct mali_core_session;
+
+/* We have one of these subsystems for each core type */
+typedef struct mali_core_subsystem
+{
+	struct mali_core_renderunit ** mali_core_array; /* An array of all cores of this type */
+	u32 number_of_cores; /* Number of cores in this list */
+
+	_mali_core_type core_type;
+
+	u32 magic_nr;
+
+	_mali_osk_list_t renderunit_idle_head; /* Idle cores of this type */
+	_mali_osk_list_t renderunit_off_head;  /* Powered off cores of this type */
+
+	/* Linked list for each priority of sessions with a job ready for scheduelling */
+	_mali_osk_list_t awaiting_sessions_head[PRIORITY_LEVELS];
+	u32 awaiting_sessions_sum_all_priorities;
+
+	/* Linked list of all sessions connected to this coretype */
+	_mali_osk_list_t all_sessions_head;
+
+	/* Linked list of all sessions connected to this coretype */
+    struct _mali_osk_notification_queue_t * notification_queue;
+
+	const char * name;
+	mali_kernel_subsystem_identifier id;
+
+	/**** Functions registered for this core type. Set during mali_core_init ******/
+	/* Start this job on this core. Return MALI_TRUE if the job was started. */
+	_mali_osk_errcode_t (*start_job)(struct mali_core_job * job, struct mali_core_renderunit * core);
+
+	/* Check if given core has an interrupt pending. Return MALI_TRUE and set mask to 0 if pending */
+	u32 (*irq_handler_upper_half)(struct mali_core_renderunit * core);
+
+	/* This function should check if the interrupt indicates that job was finished.
+	If so it should update the job-struct, reset the core registers, and return MALI_TRUE, .
+	If the job is still working after this function it should return MALI_FALSE.
+	The function must also enable the bits in the interrupt mask for the core.
+	Called by the bottom half interrupt function.	*/
+	int (*irq_handler_bottom_half)(struct mali_core_renderunit* core);
+
+	/* This function is called from the ioctl function and should return a mali_core_job pointer
+	to a created mali_core_job object with the data given from userspace */
+	_mali_osk_errcode_t (*get_new_job_from_user)(struct mali_core_session * session, void * argument);
+
+	_mali_osk_errcode_t (*suspend_response)(struct mali_core_session * session, void * argument);
+
+	/* This function is called from the ioctl function and should write the necessary data
+	 to userspace telling which job was finished and the status and debuginfo for this job.
+	 The function must also free and cleanup the input job object. */
+	void (*return_job_to_user)(struct mali_core_job * job, mali_subsystem_job_end_code end_status);
+
+	/* Is called when a subsystem shuts down. This function needs to
+	   release internal pointers in the core struct, and free the
+	   core struct before returning.
+	   It is not allowed to write to any registers, since this
+	   unmapping is already done. */
+	void (*renderunit_delete)(struct mali_core_renderunit * core);
+
+	/* Is called when we want to abort a job that is running on the core.
+	   This is done if program exits while core is running */
+	void (*reset_core)(struct mali_core_renderunit * core, mali_core_reset_style style);
+
+	/* Is called when the rendercore wants the core to give an interrupt */
+	void (*probe_core_irq_trigger)(struct mali_core_renderunit* core);
+
+	/* Is called when the irq probe wants the core to acknowledge an interrupt from the hw */
+	_mali_osk_errcode_t (*probe_core_irq_acknowledge)(struct mali_core_renderunit* core);
+
+	/* Called when the rendercore want to issue a bus stop request to a core */
+	void (*stop_bus)(struct mali_core_renderunit* core);
+} mali_core_subsystem;
+
+
+/* Per core data. This must be embedded into each core type internal core info. */
+typedef struct mali_core_renderunit
+{
+	struct mali_core_subsystem * subsystem; /* The core belongs to this subsystem */
+	_mali_osk_list_t list;                  /* Is always in subsystem->idle_list OR session->renderunits_working */
+	mali_core_status  state;
+	mali_bool error_recovery;               /* Indicates if the core is waiting for external help to recover (typically the MMU) */
+	mali_bool in_detach_function;
+	struct mali_core_job * current_job;     /* Current job being processed on this core ||NULL */
+	u32 magic_nr;
+ 	_mali_osk_timer_t * timer;
+	_mali_osk_timer_t * timer_hang_detection;
+
+	mali_io_address registers_mapped;       /* IO-mapped pointer to registers */
+	u32   registers_base_addr;              /* Base addres of the registers */
+	u32 size;                               /* The size of registers_mapped */
+	const char * description;               /* Description of this core. */
+	u32 irq_nr;                             /* The IRQ nr for this core */
+	u32 core_version;
+#if USING_MMU
+	u32 mmu_id;
+	void * mmu;                             /* The MMU this rendercore is behind.*/
+#endif
+#if USING_MALI_PMM
+	mali_pmm_core_id pmm_id;                /* The PMM core id */
+	mali_bool pend_power_down;              /* Power down is requested */
+#endif
+
+	u32 core_number;                        /* 0 for first detected core of this type, 1 for second and so on */
+
+    _mali_osk_irq_t *irq;
+} mali_core_renderunit;
+
+
+/* Per open FILE data. */
+/* You must held subsystem->mutex before any transactions to this datatype. */
+typedef struct mali_core_session
+{
+	struct mali_core_subsystem * subsystem;	   /* The session belongs to this subsystem */
+	_mali_osk_list_t renderunits_working_head; /* List of renderunits working for this session */
+	struct mali_core_job *job_waiting_to_run;  /* The next job from this session to run */
+
+	_mali_osk_list_t awaiting_sessions_list; /* Linked list of sessions with jobs, for each priority */
+	_mali_osk_list_t all_sessions_list;      /* Linked list of all sessions on the system. */
+
+    _mali_osk_notification_queue_t * notification_queue; /* Messages back to Base in userspace*/
+#if USING_MMU
+	struct mali_session_data * mmu_session; /* The session associated with the MMU page tables for this core */
+#endif
+	u32 magic_nr;
+} mali_core_session;
+
+
+/* This must be embedded into a specific mali_core_job struct */
+/* use this macro to get spesific mali_core_job:  container_of(ptr, type, member)*/
+typedef struct mali_core_job
+{
+	_mali_osk_list_t list; /* Linked list of jobs. Used by struct mali_core_session */
+	struct mali_core_session *session;
+	u32 magic_nr;
+	u32 priority;
+	u32 watchdog_msecs;
+	u32 render_time_msecs ;
+	u32 start_time_jiffies;
+	unsigned long watchdog_jiffies;
+	u32 abort_id;
+} mali_core_job;
+
+/*
+ * The rendercode subsystem is included in the subsystems[] array.
+ */
+extern struct mali_kernel_subsystem mali_subsystem_rendercore;
+
+void subsystem_flush_mapped_mem_cache(void);
+
+
+#define SUBSYSTEM_MAGIC_NR 	0xdeadbeef
+#define CORE_MAGIC_NR 		0xcafebabe
+#define SESSION_MAGIC_NR 	0xbabe1234
+#define JOB_MAGIC_NR 		0x0123abcd
+
+
+#define MALI_CHECK_SUBSYSTEM(subsystem)\
+	do { \
+		if ( SUBSYSTEM_MAGIC_NR != subsystem->magic_nr) MALI_PRINT_ERROR(("Wrong magic number"));\
+	} while (0)
+
+#define MALI_CHECK_CORE(CORE)\
+	do { \
+		if ( CORE_MAGIC_NR != CORE->magic_nr) MALI_PRINT_ERROR(("Wrong magic number"));\
+} while (0)
+
+#define MALI_CHECK_SESSION(SESSION)\
+	do { \
+		if ( SESSION_MAGIC_NR != SESSION->magic_nr) MALI_PRINT_ERROR(("Wrong magic number"));\
+} while (0)
+
+#define MALI_CHECK_JOB(JOB)\
+	do { \
+		if ( JOB_MAGIC_NR != JOB->magic_nr) MALI_PRINT_ERROR(("Wrong magic number"));\
+} while (0)
+
+
+/* Check if job_a has higher priority than job_b */
+MALI_STATIC_INLINE int job_has_higher_priority(mali_core_job * job_a, mali_core_job * job_b)
+{
+	/* The lowest number has the highest priority */
+	return (int) (job_a->priority < job_b->priority);
+}
+
+MALI_STATIC_INLINE void job_priority_set(mali_core_job * job, u32 priority)
+{
+	if (priority > PRIORITY_MIN) job->priority = PRIORITY_MIN;
+	else job->priority = priority;
+}
+
+void job_watchdog_set(mali_core_job * job, u32 watchdog_msecs);
+
+/* For use by const default register settings (e.g. set these after reset) */
+typedef struct register_address_and_value
+{
+	u32 address;
+	u32 value;
+} register_address_and_value ;
+
+
+/* For use by dynamic default register settings (e.g. set these after reset) */
+typedef struct register_address_and_value_list
+{
+	_mali_osk_list_t list;
+	register_address_and_value item;
+} register_address_and_value_list ;
+
+/* Used if the user wants to set a continious block of registers */
+typedef struct register_array_user
+{
+	u32 entries_in_array;
+	u32 start_address;
+	void __user * reg_array;
+}register_array_user;
+
+
+#define MALI_CORE_SUBSYSTEM_MUTEX_GRAB(subsys) \
+	do { \
+		MALI_DEBUG_PRINT(5, ("MUTEX: GRAB %s() %d on %s\n",__FUNCTION__, __LINE__, subsys->name)); \
+		_mali_osk_lock_wait( rendercores_global_mutex, _MALI_OSK_LOCKMODE_RW); \
+		MALI_DEBUG_PRINT(5, ("MUTEX: GRABBED %s() %d on %s\n",__FUNCTION__, __LINE__, subsys->name)); \
+		if ( SUBSYSTEM_MAGIC_NR != subsys->magic_nr ) MALI_PRINT_ERROR(("Wrong magic number"));\
+		rendercores_global_mutex_is_held = 1; \
+		rendercores_global_mutex_owner = _mali_osk_get_tid();  \
+	} while (0) ;
+
+#define MALI_CORE_SUBSYSTEM_MUTEX_RELEASE(subsys) \
+	do { \
+		MALI_DEBUG_PRINT(5, ("MUTEX: RELEASE %s() %d on %s\n",__FUNCTION__, __LINE__, subsys->name)); \
+		rendercores_global_mutex_is_held = 0; \
+		 rendercores_global_mutex_owner = 0; \
+		if ( SUBSYSTEM_MAGIC_NR != subsys->magic_nr ) MALI_PRINT_ERROR(("Wrong magic number"));\
+		_mali_osk_lock_signal( rendercores_global_mutex, _MALI_OSK_LOCKMODE_RW); \
+		MALI_DEBUG_PRINT(5, ("MUTEX: RELEASED %s() %d on %s\n",__FUNCTION__, __LINE__, subsys->name)); \
+		if ( SUBSYSTEM_MAGIC_NR != subsys->magic_nr ) MALI_PRINT_ERROR(("Wrong magic number"));\
+	} while (0) ;
+
+
+#define MALI_ASSERT_MUTEX_IS_GRABBED(input_pointer)\
+	do { \
+		if ( 0 == rendercores_global_mutex_is_held ) MALI_PRINT_ERROR(("ASSERT MUTEX SHOULD BE GRABBED"));\
+		if ( SUBSYSTEM_MAGIC_NR != input_pointer->magic_nr ) MALI_PRINT_ERROR(("Wrong magic number"));\
+		if ( rendercores_global_mutex_owner != _mali_osk_get_tid() ) MALI_PRINT_ERROR(("Owner mismatch"));\
+	} while (0)
+
+
+u32   mali_core_renderunit_register_read(struct mali_core_renderunit *core, u32 relative_address);
+void  mali_core_renderunit_register_read_array(struct mali_core_renderunit *core, u32 relative_address,  u32 * result_array, u32 nr_of_regs);
+void  mali_core_renderunit_register_write(struct mali_core_renderunit *core, u32 relative_address, u32 new_val);
+void  mali_core_renderunit_register_write_array(struct mali_core_renderunit *core, u32 relative_address,  u32 * write_array, u32 nr_of_regs);
+
+_mali_osk_errcode_t  mali_core_renderunit_init(struct mali_core_renderunit * core);
+void  mali_core_renderunit_term(struct mali_core_renderunit * core);
+int   mali_core_renderunit_map_registers(struct mali_core_renderunit *core);
+void  mali_core_renderunit_unmap_registers(struct mali_core_renderunit *core);
+int   mali_core_renderunit_irq_handler_add(struct mali_core_renderunit *core);
+mali_core_renderunit * mali_core_renderunit_get_mali_core_nr(mali_core_subsystem *subsys, u32 mali_core_nr);
+
+int mali_core_subsystem_init(struct mali_core_subsystem * new_subsys);
+#if USING_MMU
+void mali_core_subsystem_attach_mmu(mali_core_subsystem* subsys);
+#endif
+int mali_core_subsystem_register_renderunit(struct mali_core_subsystem * subsys, struct mali_core_renderunit * core);
+int mali_core_subsystem_system_info_fill(mali_core_subsystem* subsys, _mali_system_info* info);
+void mali_core_subsystem_cleanup(struct mali_core_subsystem * subsys);
+#if USING_MMU
+void mali_core_subsystem_broadcast_notification(struct mali_core_subsystem * subsys, mali_core_notification_message message, u32 data);
+#endif
+void mali_core_session_begin(mali_core_session *session);
+void mali_core_session_close(mali_core_session * session);
+int mali_core_session_add_job(mali_core_session * session, mali_core_job *job, mali_core_job **job_return);
+u32 mali_core_hang_check_timeout_get(void);
+
+_mali_osk_errcode_t mali_core_subsystem_ioctl_start_job(mali_core_session * session, void *job_data);
+_mali_osk_errcode_t mali_core_subsystem_ioctl_number_of_cores_get(mali_core_session * session, u32 *number_of_cores);
+_mali_osk_errcode_t mali_core_subsystem_ioctl_core_version_get(mali_core_session * session, _mali_core_version *version);
+_mali_osk_errcode_t mali_core_subsystem_ioctl_suspend_response(mali_core_session * session, void* argument);
+void mali_core_subsystem_ioctl_abort_job(mali_core_session * session, u32 id);
+
+#if USING_MALI_PMM
+_mali_osk_errcode_t mali_core_subsystem_signal_power_down(mali_core_subsystem *subsys, u32 mali_core_nr, mali_bool immediate_only);
+_mali_osk_errcode_t mali_core_subsystem_signal_power_up(mali_core_subsystem *subsys, u32 mali_core_nr, mali_bool queue_only);
+#endif
+
+#if MALI_STATE_TRACKING
+void mali_core_renderunit_dump_state(mali_core_subsystem* subsystem);
+#endif
+
+#endif /* __MALI_RENDERCORE_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_session_manager.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_session_manager.h
new file mode 100644
index 00000000000..ce290d0c1c9
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_session_manager.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_SESSION_MANAGER_H__
+#define __MALI_KERNEL_SESSION_MANAGER_H__
+
+/* Incomplete struct to pass around pointers to it */
+struct mali_session_data;
+
+void * mali_kernel_session_manager_slot_get(struct mali_session_data * session, int id);
+
+#endif /* __MALI_KERNEL_SESSION_MANAGER_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_subsystem.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_subsystem.h
new file mode 100644
index 00000000000..e8c6530668c
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_subsystem.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_kernel_subsystem.h
+ */
+
+#ifndef __MALI_KERNEL_SUBSYSTEM_H__
+#define __MALI_KERNEL_SUBSYSTEM_H__
+
+#include "mali_osk.h"
+#include "mali_uk_types.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_session_manager.h"
+
+/* typedefs of the datatypes used in the hook functions */
+typedef void * mali_kernel_subsystem_session_slot;
+typedef int mali_kernel_subsystem_identifier;
+typedef _mali_osk_errcode_t (*mali_kernel_resource_registrator)(_mali_osk_resource_t *);
+
+/**
+ * Broadcast notification messages
+ */
+typedef enum mali_core_notification_message
+{
+	MMU_KILL_STEP0_LOCK_SUBSYSTEM,                        /**< Request to lock subsystem */
+	MMU_KILL_STEP1_STOP_BUS_FOR_ALL_CORES,                /**< Request to stop all buses */
+	MMU_KILL_STEP2_RESET_ALL_CORES_AND_ABORT_THEIR_JOBS,  /**< Request kill all jobs, and not start more jobs */
+	MMU_KILL_STEP3_CONTINUE_JOB_HANDLING,                 /**< Request to continue with new jobs on all cores */
+	MMU_KILL_STEP4_UNLOCK_SUBSYSTEM                       /**< Request to unlock subsystem */
+} mali_core_notification_message;
+
+/**
+ * A function pointer can be NULL if the subsystem isn't interested in the event.
+ */
+typedef struct mali_kernel_subsystem
+{
+	/* subsystem control */
+	_mali_osk_errcode_t (*startup)(mali_kernel_subsystem_identifier id); /**< Called during module load or system startup*/
+	void (*shutdown)(mali_kernel_subsystem_identifier id); /**< Called during module unload or system shutdown */
+
+	/**
+	 * Called during module load or system startup.
+	 * Called when all subsystems have reported startup OK and all resources where successfully initialized
+	*/
+	_mali_osk_errcode_t (*load_complete)(mali_kernel_subsystem_identifier id);
+
+	/* per subsystem handlers */
+	_mali_osk_errcode_t (*system_info_fill)(_mali_system_info* info); /**< Fill info into info struct. MUST allocate memory with kmalloc, since it's kfree'd */
+
+	/* per session handlers */
+	/**
+	 * Informs about a new session.
+	 * slot can be used to track per-session per-subsystem data.
+	 * queue can be used to send events to user space.
+	 * _mali_osk_errcode_t error return value.
+	 */
+	_mali_osk_errcode_t (*session_begin)(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot, _mali_osk_notification_queue_t * queue);
+	/**
+	 * Informs that a session is ending
+	 * slot was the same as given during session_begin
+	 */
+	void (*session_end)(struct mali_session_data * mali_session_data, mali_kernel_subsystem_session_slot * slot);
+
+	/* Used by subsystems to send messages to each other. This is the receiving end */
+	void (*broadcast_notification)(mali_core_notification_message message, u32 data);
+
+#if MALI_STATE_TRACKING
+	/** Dump the current state of the subsystem */
+	void (*dump_state)(void);
+#endif
+} mali_kernel_subsystem;
+
+/* functions used by the subsystems to interact with the core */
+/**
+ * Register a resouce handler
+ * @param type The resoruce type to register a handler for
+ * @param handler Pointer to the function handling this resource
+ * @return _MALI_OSK_ERR_OK on success. Otherwise, a suitable _mali_osk_errcode_t error.
+ */
+_mali_osk_errcode_t _mali_kernel_core_register_resource_handler(_mali_osk_resource_type_t type, mali_kernel_resource_registrator handler);
+
+/* function used to interact with other subsystems */
+/**
+ * Broadcast a message
+ * Sends a message to all subsystems which have registered a broadcast notification handler
+ * @param message The message to send
+ * @param data Message specific extra data
+ */
+void _mali_kernel_core_broadcast_subsystem_message(mali_core_notification_message message, u32 data);
+
+#if MALI_STATE_TRACKING
+/**
+ * Tell all subsystems to dump their current state
+ */
+void _mali_kernel_core_dump_state(void);
+#endif
+
+
+#endif /* __MALI_KERNEL_SUBSYSTEM_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_utilization.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_utilization.c
new file mode 100644
index 00000000000..04653b06ad5
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_utilization.c
@@ -0,0 +1,207 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_utilization.h"
+#include "mali_osk.h"
+#include "mali_platform.h"
+
+/* Define how often to calculate and report GPU utilization, in milliseconds */
+#define MALI_GPU_UTILIZATION_TIMEOUT 500
+
+static _mali_osk_lock_t *time_data_lock;
+
+static _mali_osk_atomic_t num_running_cores;
+
+static u64 period_start_time = 0;
+static u64 work_start_time = 0;
+static u64 accumulated_work_time = 0;
+
+static _mali_osk_timer_t *utilization_timer = NULL;
+static mali_bool timer_running = MALI_FALSE;
+
+
+static void calculate_gpu_utilization(void* arg)
+{
+	u64 time_now;
+	u64 time_period;
+	u32 leading_zeroes;
+	u32 shift_val;
+	u32 work_normalized;
+	u32 period_normalized;
+	u32 utilization;
+
+	_mali_osk_lock_wait(time_data_lock, _MALI_OSK_LOCKMODE_RW);
+
+	if (accumulated_work_time == 0 && work_start_time == 0)
+	{
+		/* Don't reschedule timer, this will be started if new work arrives */
+		timer_running = MALI_FALSE;
+
+		_mali_osk_lock_signal(time_data_lock, _MALI_OSK_LOCKMODE_RW);
+
+		/* No work done for this period, report zero usage */
+		mali_gpu_utilization_handler(0);
+
+		return;
+	}
+
+	time_now = _mali_osk_time_get_ns();
+	time_period = time_now - period_start_time;
+
+	/* If we are currently busy, update working period up to now */
+	if (work_start_time != 0)
+	{
+		accumulated_work_time += (time_now - work_start_time);
+		work_start_time = time_now;
+	}
+
+	/*
+	 * We have two 64-bit values, a dividend and a divisor.
+	 * To avoid dependencies to a 64-bit divider, we shift down the two values
+	 * equally first.
+	 * We shift the dividend up and possibly the divisor down, making the result X in 256.
+	 */
+
+	/* Shift the 64-bit values down so they fit inside a 32-bit integer */
+	leading_zeroes = _mali_osk_clz((u32)(time_period >> 32));
+	shift_val = 32 - leading_zeroes;
+	work_normalized = (u32)(accumulated_work_time >> shift_val);
+	period_normalized = (u32)(time_period >> shift_val);
+
+	/*
+	 * Now, we should report the usage in parts of 256
+	 * this means we must shift up the dividend or down the divisor by 8
+	 * (we could do a combination, but we just use one for simplicity,
+	 * but the end result should be good enough anyway)
+	 */
+	if (period_normalized > 0x00FFFFFF)
+	{
+		/* The divisor is so big that it is safe to shift it down */
+		period_normalized >>= 8;
+	}
+	else
+	{
+		/*
+		 * The divisor is so small that we can shift up the dividend, without loosing any data.
+		 * (dividend is always smaller than the divisor)
+		 */
+		work_normalized <<= 8;
+	}
+
+	utilization = work_normalized / period_normalized;
+
+	accumulated_work_time = 0;
+	period_start_time = time_now; /* starting a new period */
+
+	_mali_osk_lock_signal(time_data_lock, _MALI_OSK_LOCKMODE_RW);
+
+	_mali_osk_timer_add(utilization_timer, _mali_osk_time_mstoticks(MALI_GPU_UTILIZATION_TIMEOUT));
+
+	mali_gpu_utilization_handler(utilization);
+}
+
+
+
+_mali_osk_errcode_t mali_utilization_init(void)
+{
+	time_data_lock = _mali_osk_lock_init( _MALI_OSK_LOCKFLAG_SPINLOCK_IRQ|_MALI_OSK_LOCKFLAG_NONINTERRUPTABLE, 0, 0 );
+	if (NULL == time_data_lock)
+	{
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	_mali_osk_atomic_init(&num_running_cores, 0);
+
+	utilization_timer = _mali_osk_timer_init();
+	if (NULL == utilization_timer)
+	{
+		_mali_osk_lock_term(time_data_lock);
+		return _MALI_OSK_ERR_FAULT;
+	}
+	_mali_osk_timer_setcallback(utilization_timer, calculate_gpu_utilization, NULL);
+
+	return _MALI_OSK_ERR_OK;
+}
+
+void mali_utilization_suspend(void)
+{
+	if (NULL != utilization_timer)
+	{
+		_mali_osk_timer_del(utilization_timer);
+		timer_running = MALI_FALSE;
+	}
+}
+
+void mali_utilization_term(void)
+{
+	if (NULL != utilization_timer)
+	{
+		_mali_osk_timer_del(utilization_timer);
+		timer_running = MALI_FALSE;
+		_mali_osk_timer_term(utilization_timer);
+		utilization_timer = NULL;
+	}
+
+	_mali_osk_atomic_term(&num_running_cores);
+
+	_mali_osk_lock_term(time_data_lock);
+}
+
+
+
+void mali_utilization_core_start(void)
+{
+	if (_mali_osk_atomic_inc_return(&num_running_cores) == 1)
+	{
+		/*
+		 * We went from zero cores working, to one core working,
+		 * we now consider the entire GPU for being busy
+		 */
+
+		_mali_osk_lock_wait(time_data_lock, _MALI_OSK_LOCKMODE_RW);
+
+		work_start_time = _mali_osk_time_get_ns();
+
+		if (timer_running != MALI_TRUE)
+		{
+			timer_running = MALI_TRUE;
+			period_start_time = work_start_time; /* starting a new period */
+
+			_mali_osk_lock_signal(time_data_lock, _MALI_OSK_LOCKMODE_RW);
+
+			_mali_osk_timer_add(utilization_timer, _mali_osk_time_mstoticks(MALI_GPU_UTILIZATION_TIMEOUT));
+		}
+		else
+		{
+			_mali_osk_lock_signal(time_data_lock, _MALI_OSK_LOCKMODE_RW);
+		}
+	}
+}
+
+
+
+void mali_utilization_core_end(void)
+{
+	if (_mali_osk_atomic_dec_return(&num_running_cores) == 0)
+	{
+		/*
+		 * No more cores are working, so accumulate the time we was busy.
+		 */
+		u64 time_now;
+
+		_mali_osk_lock_wait(time_data_lock, _MALI_OSK_LOCKMODE_RW);
+
+		time_now = _mali_osk_time_get_ns();
+		accumulated_work_time += (time_now - work_start_time);
+		work_start_time = 0;
+
+		_mali_osk_lock_signal(time_data_lock, _MALI_OSK_LOCKMODE_RW);
+	}
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_utilization.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_utilization.h
new file mode 100644
index 00000000000..f6485006729
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_utilization.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_UTILIZATION_H__
+#define __MALI_KERNEL_UTILIZATION_H__
+
+#include "mali_osk.h"
+
+/**
+ * Initialize/start the Mali GPU utilization metrics reporting.
+ *
+ * @return _MALI_OSK_ERR_OK on success, otherwise failure.
+ */
+_mali_osk_errcode_t mali_utilization_init(void);
+
+/**
+ * Terminate the Mali GPU utilization metrics reporting
+ */
+void mali_utilization_term(void);
+
+/**
+ * Should be called when a job is about to execute a job
+ */
+void mali_utilization_core_start(void);
+
+/**
+ * Should be called to stop the utilization timer during system suspend
+ */
+void mali_utilization_suspend(void);
+
+/**
+ * Should be called when a job has completed executing a job
+ */
+void mali_utilization_core_end(void);
+
+
+#endif /* __MALI_KERNEL_UTILIZATION_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_vsync.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_vsync.c
new file mode 100644
index 00000000000..8dfa3a393ba
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_kernel_vsync.c
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_osk_mali.h"
+#include "mali_ukk.h"
+/*#include "mali_timestamp.h"
+*/
+
+_mali_osk_errcode_t _mali_ukk_vsync_event_report(_mali_uk_vsync_event_report_s *args)
+{
+	_mali_uk_vsync_event event = (_mali_uk_vsync_event)args->event;
+	MALI_IGNORE(event); /* event is not used for release code, and that is OK */
+/*	u64 ts = _mali_timestamp_get();
+ */
+
+	MALI_DEBUG_PRINT(4, ("Received VSYNC event: %d\n", event));
+
+	MALI_SUCCESS;
+}
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk.h
new file mode 100644
index 00000000000..c35d90577a3
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk.h
@@ -0,0 +1,1620 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk.h
+ * Defines the OS abstraction layer for the kernel device driver (OSK)
+ */
+
+#ifndef __MALI_OSK_H__
+#define __MALI_OSK_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @addtogroup uddapi Unified Device Driver (UDD) APIs
+ *
+ * @{
+ */
+
+/**
+ * @addtogroup oskapi UDD OS Abstraction for Kernel-side (OSK) APIs
+ *
+ * @{
+ */
+
+/** @defgroup _mali_osk_miscellaneous OSK Miscellaneous functions, constants and types
+ * @{ */
+
+/* Define integer types used by OSK. Note: these currently clash with Linux so we only define them if not defined already */
+#ifndef __KERNEL__
+	typedef unsigned char      u8;
+	typedef signed char        s8;
+	typedef unsigned short     u16;
+	typedef signed short       s16;
+	typedef unsigned int       u32;
+	typedef signed int         s32;
+	typedef unsigned long long u64;
+	#define BITS_PER_LONG (sizeof(long)*8)
+#else
+	/* Ensure Linux types u32, etc. are defined */
+	#include <linux/types.h>
+#endif
+
+/** @brief Mali Boolean type which uses MALI_TRUE and MALI_FALSE
+  */
+	typedef unsigned long mali_bool;
+	
+#ifndef MALI_TRUE
+	#define MALI_TRUE ((mali_bool)1)
+#endif
+
+#ifndef MALI_FALSE
+	#define MALI_FALSE ((mali_bool)0)
+#endif
+
+/**
+ * @brief OSK Error codes
+ *
+ * Each OS may use its own set of error codes, and may require that the
+ * User/Kernel interface take certain error code. This means that the common
+ * error codes need to be sufficiently rich to pass the correct error code
+ * thorugh from the OSK to U/K layer, across all OSs.
+ *
+ * The result is that some error codes will appear redundant on some OSs.
+ * Under all OSs, the OSK layer must translate native OS error codes to
+ * _mali_osk_errcode_t codes. Similarly, the U/K layer must translate from
+ * _mali_osk_errcode_t codes to native OS error codes.
+ */
+typedef enum
+{
+    _MALI_OSK_ERR_OK = 0, /**< Success. */
+    _MALI_OSK_ERR_FAULT = -1, /**< General non-success */
+    _MALI_OSK_ERR_INVALID_FUNC = -2, /**< Invalid function requested through User/Kernel interface (e.g. bad IOCTL number) */
+    _MALI_OSK_ERR_INVALID_ARGS = -3, /**< Invalid arguments passed through User/Kernel interface */
+    _MALI_OSK_ERR_NOMEM = -4, /**< Insufficient memory */
+    _MALI_OSK_ERR_TIMEOUT = -5, /**< Timeout occurred */
+    _MALI_OSK_ERR_RESTARTSYSCALL = -6, /**< Special: On certain OSs, must report when an interruptable mutex is interrupted. Ignore otherwise. */
+    _MALI_OSK_ERR_ITEM_NOT_FOUND = -7, /**< Table Lookup failed */
+    _MALI_OSK_ERR_BUSY = -8, /**< Device/operation is busy. Try again later */
+	_MALI_OSK_ERR_UNSUPPORTED = -9, /**< Optional part of the interface used, and is unsupported */
+} _mali_osk_errcode_t;
+
+/** @} */ /* end group _mali_osk_miscellaneous */
+
+
+/** @defgroup _mali_osk_irq OSK IRQ handling
+ * @{ */
+
+/** @brief Private type for IRQ handling objects */
+typedef struct _mali_osk_irq_t_struct _mali_osk_irq_t;
+
+/** @brief Optional function to trigger an irq from a resource
+ *
+ * This function is implemented by the common layer to allow probing of a resource's IRQ.
+ * @param arg resource-specific data */
+typedef void  (*_mali_osk_irq_trigger_t)( void * arg );
+
+/** @brief Optional function to acknowledge an irq from a resource
+ *
+ * This function is implemented by the common layer to allow probing of a resource's IRQ.
+ * @param arg resource-specific data
+ * @return _MALI_OSK_ERR_OK if the IRQ was successful, or a suitable _mali_osk_errcode_t on failure. */
+typedef _mali_osk_errcode_t (*_mali_osk_irq_ack_t)( void * arg );
+
+/** @brief IRQ 'upper-half' handler callback.
+ *
+ * This function is implemented by the common layer to do the initial handling of a
+ * resource's IRQ. This maps on to the concept of an ISR that does the minimum
+ * work necessary before handing off to an IST.
+ *
+ * The communication of the resource-specific data from the ISR to the IST is
+ * handled by the OSK implementation.
+ *
+ * On most systems, the IRQ upper-half handler executes in IRQ context.
+ * Therefore, the system may have restrictions about what can be done in this
+ * context
+ *
+ * If an IRQ upper-half handler requires more work to be done than can be
+ * acheived in an IRQ context, then it may defer the work with
+ * _mali_osk_irq_schedulework(). Refer to \ref _mali_osk_irq_schedulework() for
+ * more information.
+ *
+ * @param arg resource-specific data
+ * @return _MALI_OSK_ERR_OK if the IRQ was correctly handled, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+typedef _mali_osk_errcode_t  (*_mali_osk_irq_uhandler_t)( void * arg );
+
+/** @brief IRQ 'bottom-half' handler callback.
+ *
+ * This function is implemented by the common layer to do the deferred handling
+ * of a resource's IRQ. Usually, this work cannot be carried out in IRQ context
+ * by the IRQ upper-half handler.
+ *
+ * The IRQ bottom-half handler maps on to the concept of an IST that may
+ * execute some time after the actual IRQ has fired.
+ *
+ * All OSK-registered IRQ bottom-half handlers will be serialized, across all
+ * CPU-cores in the system.
+ *
+ * Refer to \ref _mali_osk_irq_schedulework() for more information on the
+ * IRQ work-queue, and the calling of the IRQ bottom-half handler.
+ *
+ * @param arg resource-specific data
+ */
+typedef void (*_mali_osk_irq_bhandler_t)( void * arg );
+/** @} */ /* end group _mali_osk_irq */
+
+
+/** @defgroup _mali_osk_atomic OSK Atomic counters
+ * @{ */
+
+/** @brief Public type of atomic counters
+ *
+ * This is public for allocation on stack. On systems that support it, this is just a single 32-bit value.
+ * On others, it could be encapsulating an object stored elsewhere.
+ *
+ * Even though the structure has space for a u32, the counters will only
+ * represent signed 24-bit integers.
+ *
+ * Regardless of implementation, the \ref _mali_osk_atomic functions \b must be used
+ * for all accesses to the variable's value, even if atomicity is not required.
+ * Do not access u.val or u.obj directly.
+ */
+typedef struct
+{
+    union
+    {
+        u32 val;
+        void *obj;
+    } u;
+} _mali_osk_atomic_t;
+/** @} */ /* end group _mali_osk_atomic */
+
+
+/** @defgroup _mali_osk_lock OSK Mutual Exclusion Locks
+ * @{ */
+
+/** @brief OSK Mutual Exclusion Lock flags type
+ *
+ * Flags are supplied at the point where the Lock is initialized. Each flag can
+ * be combined with others using bitwise OR, '|'.
+ *
+ * The flags must be sufficiently rich to cope with all our OSs. This means
+ * that on some OSs, certain flags can be completely ignored. We define a
+ * number of terms that are significant across all OSs:
+ *
+ * - Sleeping/non-sleeping mutexs. Sleeping mutexs can block on waiting, and so
+ * schedule out the current thread. This is significant on OSs where there are
+ * situations in which the current thread must not be put to sleep. On OSs
+ * without this restriction, sleeping and non-sleeping mutexes can be treated
+ * as the same (if that is required).
+ * - Interruptable/non-interruptable mutexes. For sleeping mutexes, it may be
+ * possible for the sleep to be interrupted for a reason other than the thread
+ * being able to obtain the lock. OSs behaving in this way may provide a
+ * mechanism to control whether sleeping mutexes can be interrupted. On OSs
+ * that do not support the concept of interruption, \b or they do not support
+ * control of mutex interruption, then interruptable mutexes may be treated
+ * as non-interruptable.
+ *
+ * Some constrains apply to the lock type flags:
+ *
+ * - Spinlocks are by nature, non-interruptable. Hence, they must always be
+ * combined with the NONINTERRUPTABLE flag, because it is meaningless to ask
+ * for a spinlock that is interruptable (and this highlights its
+ * non-interruptable-ness). For example, on certain OSs they should be used when
+ * you must not sleep.
+ * - Reader/writer is an optimization hint, and any type of lock can be
+ * reader/writer. Since this is an optimization hint, the implementation need
+ * not respect this for any/all types of lock. For example, on certain OSs,
+ * there's no interruptable reader/writer mutex. If such a thing were requested
+ * on that OS, the fact that interruptable was requested takes priority over the
+ * reader/writer-ness, because reader/writer-ness is not necessary for correct
+ * operation.
+ * - Any lock can use the order parameter.
+ * - A onelock is an optimization hint specific to certain OSs. It can be
+ * specified when it is known that only one lock will be held by the thread,
+ * and so can provide faster mutual exclusion. This can be safely ignored if
+ * such optimization is not required/present.
+ *
+ * The absence of any flags (the value 0) results in a sleeping-mutex, which is interruptable.
+ */
+typedef enum
+{
+	_MALI_OSK_LOCKFLAG_SPINLOCK = 0x1,          /**< Specifically, don't sleep on those architectures that require it */
+	_MALI_OSK_LOCKFLAG_NONINTERRUPTABLE = 0x2,  /**< The mutex cannot be interrupted, e.g. delivery of signals on those architectures where this is required */
+	_MALI_OSK_LOCKFLAG_READERWRITER = 0x4,      /**< Optimise for readers/writers */
+	_MALI_OSK_LOCKFLAG_ORDERED = 0x8,           /**< Use the order parameter; otherwise use automatic ordering */
+	_MALI_OSK_LOCKFLAG_ONELOCK = 0x10,          /**< Each thread can only hold one lock at a time */
+	_MALI_OSK_LOCKFLAG_SPINLOCK_IRQ = 0x20,    /**<  IRQ version of spinlock */
+	/** @enum _mali_osk_lock_flags_t
+	 *
+	 * Flags from 0x10000--0x80000000 are RESERVED for User-mode */
+
+} _mali_osk_lock_flags_t;
+
+/** @brief Mutual Exclusion Lock Mode Optimization hint
+ *
+ * The lock mode is used to implement the read/write locking of locks specified
+ * as _MALI_OSK_LOCKFLAG_READERWRITER. In this case, the RO mode can be used
+ * to allow multiple concurrent readers, but no writers. The RW mode is used for
+ * writers, and so will wait for all readers to release the lock (if any present).
+ * Further readers and writers will wait until the writer releases the lock.
+ *
+ * The mode is purely an optimization hint: for example, it is permissible for
+ * all locks to behave in RW mode, regardless of that supplied.
+ *
+ * It is an error to attempt to use locks in anything other that RW mode when
+ * _MALI_OSK_LOCKFLAG_READERWRITER is not supplied.
+ *
+ */
+typedef enum
+{
+	_MALI_OSK_LOCKMODE_UNDEF = -1,  /**< Undefined lock mode. For internal use only */
+	_MALI_OSK_LOCKMODE_RW    = 0x0, /**< Read-write mode, default. All readers and writers are mutually-exclusive */
+	_MALI_OSK_LOCKMODE_RO,          /**< Read-only mode, to support multiple concurrent readers, but mutual exclusion in the presence of writers. */
+	/** @enum _mali_osk_lock_mode_t
+	 *
+	 * Lock modes 0x40--0x7F are RESERVED for User-mode */
+} _mali_osk_lock_mode_t;
+
+/** @brief Private type for Mutual Exclusion lock objects */
+typedef struct _mali_osk_lock_t_struct _mali_osk_lock_t;
+/** @} */ /* end group _mali_osk_lock */
+
+/** @defgroup _mali_osk_low_level_memory OSK Low-level Memory Operations
+ * @{ */
+
+/**
+ * @brief Private data type for use in IO accesses to/from devices.
+ *
+ * This represents some range that is accessible from the device. Examples
+ * include:
+ * - Device Registers, which could be readable and/or writeable.
+ * - Memory that the device has access to, for storing configuration structures.
+ *
+ * Access to this range must be made through the _mali_osk_mem_ioread32() and
+ * _mali_osk_mem_iowrite32() functions.
+ */
+typedef struct _mali_io_address * mali_io_address;
+
+/** @defgroup _MALI_OSK_CPU_PAGE CPU Physical page size macros.
+ *
+ * The order of the page size is supplied for
+ * ease of use by algorithms that might require it, since it is easier to know
+ * it ahead of time rather than calculating it.
+ *
+ * The Mali Page Mask macro masks off the lower bits of a physical address to
+ * give the start address of the page for that physical address.
+ *
+ * @note The Mali device driver code is designed for systems with 4KB page size.
+ * Changing these macros will not make the entire Mali device driver work with
+ * page sizes other than 4KB.
+ *
+ * @note The CPU Physical Page Size has been assumed to be the same as the Mali
+ * Physical Page Size.
+ *
+ * @{
+ */
+
+/** CPU Page Order, as log to base 2 of the Page size. @see _MALI_OSK_CPU_PAGE_SIZE */
+#define _MALI_OSK_CPU_PAGE_ORDER ((u32)12)
+/** CPU Page Size, in bytes.               */
+#define _MALI_OSK_CPU_PAGE_SIZE (((u32)1) << (_MALI_OSK_CPU_PAGE_ORDER))
+/** CPU Page Mask, which masks off the offset within a page */
+#define _MALI_OSK_CPU_PAGE_MASK (~((((u32)1) << (_MALI_OSK_CPU_PAGE_ORDER)) - ((u32)1)))
+/** @} */ /* end of group _MALI_OSK_CPU_PAGE */
+
+/** @defgroup _MALI_OSK_MALI_PAGE Mali Physical Page size macros
+ *
+ * Mali Physical page size macros. The order of the page size is supplied for
+ * ease of use by algorithms that might require it, since it is easier to know
+ * it ahead of time rather than calculating it.
+ *
+ * The Mali Page Mask macro masks off the lower bits of a physical address to
+ * give the start address of the page for that physical address.
+ *
+ * @note The Mali device driver code is designed for systems with 4KB page size.
+ * Changing these macros will not make the entire Mali device driver work with
+ * page sizes other than 4KB.
+ *
+ * @note The Mali Physical Page Size has been assumed to be the same as the CPU
+ * Physical Page Size.
+ *
+ * @{
+ */
+
+/** Mali Page Order, as log to base 2 of the Page size. @see _MALI_OSK_MALI_PAGE_SIZE */
+#define _MALI_OSK_MALI_PAGE_ORDER ((u32)12)
+/** Mali Page Size, in bytes.               */
+#define _MALI_OSK_MALI_PAGE_SIZE (((u32)1) << (_MALI_OSK_MALI_PAGE_ORDER))
+/** Mali Page Mask, which masks off the offset within a page */
+#define _MALI_OSK_MALI_PAGE_MASK (~((((u32)1) << (_MALI_OSK_MALI_PAGE_ORDER)) - ((u32)1)))
+/** @} */ /* end of group _MALI_OSK_MALI_PAGE*/
+
+/** @brief flags for mapping a user-accessible memory range
+ *
+ * Where a function with prefix '_mali_osk_mem_mapregion' accepts flags as one
+ * of the function parameters, it will use one of these. These allow per-page
+ * control over mappings. Compare with the mali_memory_allocation_flag type,
+ * which acts over an entire range
+ *
+ * These may be OR'd together with bitwise OR (|), but must be cast back into
+ * the type after OR'ing.
+ */
+typedef enum
+{
+	_MALI_OSK_MEM_MAPREGION_FLAG_OS_ALLOCATED_PHYSADDR = 0x1, /**< Physical address is OS Allocated */
+} _mali_osk_mem_mapregion_flags_t;
+/** @} */ /* end group _mali_osk_low_level_memory */
+
+/** @defgroup _mali_osk_notification OSK Notification Queues
+ * @{ */
+
+/** @brief Private type for notification queue objects */
+typedef struct _mali_osk_notification_queue_t_struct _mali_osk_notification_queue_t;
+
+/** @brief Public notification data object type */
+typedef struct _mali_osk_notification_t_struct
+{
+	u32 notification_type;   /**< The notification type */
+	u32 result_buffer_size; /**< Size of the result buffer to copy to user space */
+	void * result_buffer;   /**< Buffer containing any type specific data */
+} _mali_osk_notification_t;
+
+/** @} */ /* end group _mali_osk_notification */
+
+
+/** @defgroup _mali_osk_timer OSK Timer Callbacks
+ * @{ */
+
+/** @brief Function to call when a timer expires
+ *
+ * When a timer expires, this function is called. Note that on many systems,
+ * a timer callback will be executed in IRQ context. Therefore, restrictions
+ * may apply on what can be done inside the timer callback.
+ *
+ * If a timer requires more work to be done than can be acheived in an IRQ
+ * context, then it may defer the work with a work-queue. For example, it may
+ * use \ref _mali_osk_irq_schedulework() to make use of the IRQ bottom-half handler
+ * to carry out the remaining work.
+ *
+ * Stopping the timer with \ref _mali_osk_timer_del() blocks on compeletion of
+ * the callback. Therefore, the callback may not obtain any mutexes also held
+ * by any callers of _mali_osk_timer_del(). Otherwise, a deadlock may occur.
+ *
+ * @param arg Function-specific data */
+typedef void (*_mali_osk_timer_callback_t)(void * arg );
+
+/** @brief Private type for Timer Callback Objects */
+typedef struct _mali_osk_timer_t_struct _mali_osk_timer_t;
+/** @} */ /* end group _mali_osk_timer */
+
+
+/** @addtogroup _mali_osk_list OSK Doubly-Linked Circular Lists
+ * @{ */
+
+/** @brief Public List objects.
+ *
+ * To use, add a _mali_osk_list_t member to the structure that may become part
+ * of a list. When traversing the _mali_osk_list_t objects, use the
+ * _MALI_OSK_CONTAINER_OF() macro to recover the structure from its
+ *_mali_osk_list_t member
+ *
+ * Each structure may have multiple _mali_osk_list_t members, so that the
+ * structure is part of multiple lists. When traversing lists, ensure that the
+ * correct _mali_osk_list_t member is used, because type-checking will be
+ * lost by the compiler.
+ */
+typedef struct _mali_osk_list_s
+{
+	struct _mali_osk_list_s *next;
+	struct _mali_osk_list_s *prev;
+} _mali_osk_list_t;
+
+/** @brief Initialize a list to be a head of an empty list
+ * @param exp the list to initialize. */
+#define _MALI_OSK_INIT_LIST_HEAD(exp) _mali_osk_list_init(exp)
+
+/** @brief Define a list variable, which is uninitialized.
+ * @param exp the name of the variable that the list will be defined as. */
+#define _MALI_OSK_LIST_HEAD(exp)      _mali_osk_list_t exp
+
+/** @brief Find the containing structure of another structure
+ *
+ * This is the reverse of the operation 'offsetof'. This means that the
+ * following condition is satisfied:
+ *
+ *   ptr == _MALI_OSK_CONTAINER_OF( &ptr->member, type, member )
+ *
+ * When ptr is of type 'type'.
+ *
+ * Its purpose it to recover a larger structure that has wrapped a smaller one.
+ *
+ * @note no type or memory checking occurs to ensure that a wrapper structure
+ * does in fact exist, and that it is being recovered with respect to the
+ * correct member.
+ *
+ * @param ptr the pointer to the member that is contained within the larger
+ * structure
+ * @param type the type of the structure that contains the member
+ * @param member the name of the member in the structure that ptr points to.
+ * @return a pointer to a \a type object which contains \a member, as pointed
+ * to by \a ptr.
+ */
+#define _MALI_OSK_CONTAINER_OF(ptr, type, member) \
+             ((type *)( ((char *)ptr) - offsetof(type,member) ))
+
+/** @brief Find the containing structure of a list
+ *
+ * When traversing a list, this is used to recover the containing structure,
+ * given that is contains a _mali_osk_list_t member.
+ *
+ * Each list must be of structures of one type, and must link the same members
+ * together, otherwise it will not be possible to correctly recover the
+ * sturctures that the lists link.
+ *
+ * @note no type or memory checking occurs to ensure that a structure does in
+ * fact exist for the list entry, and that it is being recovered with respect
+ * to the correct list member.
+ *
+ * @param ptr the pointer to the _mali_osk_list_t member in this structure
+ * @param type the type of the structure that contains the member
+ * @param member the member of the structure that ptr points to.
+ * @return a pointer to a \a type object which contains the _mali_osk_list_t
+ * \a member, as pointed to by the _mali_osk_list_t \a *ptr.
+ */
+#define _MALI_OSK_LIST_ENTRY(ptr, type, member) \
+            _MALI_OSK_CONTAINER_OF(ptr, type, member)
+
+/** @brief Enumerate a list safely
+ *
+ * With this macro, lists can be enumerated in a 'safe' manner. That is,
+ * entries can be deleted from the list without causing an error during
+ * enumeration. To achieve this, a 'temporary' pointer is required, which must
+ * be provided to the macro.
+ *
+ * Use it like a 'for()', 'while()' or 'do()' construct, and so it must be
+ * followed by a statement or compound-statement which will be executed for
+ * each list entry.
+ *
+ * Upon loop completion, providing that an early out was not taken in the
+ * loop body, then it is guaranteed that ptr->member == list, even if the loop
+ * body never executed.
+ *
+ * @param ptr a pointer to an object of type 'type', which points to the
+ * structure that contains the currently enumerated list entry.
+ * @param tmp a pointer to an object of type 'type', which must not be used
+ * inside the list-execution statement.
+ * @param list a pointer to a _mali_osk_list_t, from which enumeration will
+ * begin
+ * @param type the type of the structure that contains the _mali_osk_list_t
+ * member that is part of the list to be enumerated.
+ * @param member the _mali_osk_list_t member of the structure that is part of
+ * the list to be enumerated.
+ */
+#define _MALI_OSK_LIST_FOREACHENTRY(ptr, tmp, list, type, member)         \
+        for (ptr = _MALI_OSK_LIST_ENTRY((list)->next, type, member),      \
+             tmp = _MALI_OSK_LIST_ENTRY(ptr->member.next, type, member); \
+             &ptr->member != (list);                                    \
+             ptr = tmp, tmp = _MALI_OSK_LIST_ENTRY(tmp->member.next, type, member))
+/** @} */ /* end group _mali_osk_list */
+
+
+/** @addtogroup _mali_osk_miscellaneous
+ * @{ */
+
+/** @brief The known resource types
+ *
+ * @note \b IMPORTANT: these must remain fixed, and only be extended. This is
+ * because not all systems use a header file for reading in their resources.
+ * The resources may instead come from a data file where these resources are
+ * 'hard-coded' in, because there's no easy way of transferring the enum values
+ * into such data files. E.g. the C-Pre-processor does \em not process enums.
+ */
+typedef enum _mali_osk_resource_type
+{
+	RESOURCE_TYPE_FIRST =0,  /**< Duplicate resource marker for the first resource*/
+	MEMORY              =0,  /**< Physically contiguous memory block, not managed by the OS */
+	OS_MEMORY           =1,  /**< Memory managed by and shared with the OS */
+	MALI200             =3,  /**< Mali200 Programmable Fragment Shader */
+	MALIGP2             =4,  /**< MaliGP2 Programmable Vertex Shader */
+	MMU                 =5,  /**< Mali MMU (Memory Management Unit) */
+	FPGA_FRAMEWORK      =6,  /**< Mali registers specific to FPGA implementations */
+	MALI400L2           =7,  /**< Mali400 L2 Cache */
+	MALI300L2           =7, /**< Mali300 L2 Cache */
+	MALI400GP           =8,  /**< Mali400 Programmable Vertex Shader Core */
+	MALI300GP           =8, /**< Mali300 Programmable Vertex Shader Core */
+	MALI400PP           =9,  /**< Mali400 Programmable Fragment Shader Core */
+	MALI300PP           =9, /**< Mali300 Programmable Fragment Shader Core */
+	MEM_VALIDATION      =10, /**< External Memory Validator */
+	PMU                 =11, /**< Power Manangement Unit */
+	RESOURCE_TYPE_COUNT      /**< The total number of known resources */
+} _mali_osk_resource_type_t;
+
+/** @brief resource description struct
+ *
+ * _mali_osk_resources_init() will enumerate objects of this type. Not all
+ * members have a valid meaning across all types.
+ *
+ * The mmu_id is used to group resources to a certain MMU, since there may be
+ * more than one MMU in the system, and each resource may be using a different
+ * MMU:
+ * - For MMU resources, the setting of mmu_id is a uniquely identifying number.
+ * - For Other resources, the setting of mmu_id determines which MMU the
+ * resource uses.
+ */
+typedef struct _mali_osk_resource
+{
+	_mali_osk_resource_type_t type; /**< type of the resource */
+	const char * description;       /**< short description of the resource */
+	u32 base;                       /**< Physical base address of the resource, as seen by Mali resources. */
+	s32 cpu_usage_adjust;           /**< Offset added to the base address of the resource to arrive at the CPU physical address of the resource (if different from the Mali physical address) */
+	u32 size;                       /**< Size in bytes of the resource - either the size of its register range, or the size of the memory block. */
+	u32 irq;                        /**< IRQ number delivered to the CPU, or -1 to tell the driver to probe for it (if possible) */
+	u32 flags;                      /**< Resources-specific flags. */
+	u32 mmu_id;                     /**< Identifier for Mali MMU resources. */
+	u32 alloc_order;                /**< Order in which MEMORY/OS_MEMORY resources are used */
+} _mali_osk_resource_t;
+/** @} */ /* end group _mali_osk_miscellaneous */
+
+
+#include "mali_kernel_memory_engine.h"   /* include for mali_memory_allocation and mali_physical_memory_allocation type */
+
+/** @addtogroup _mali_osk_irq
+ * @{ */
+
+/** @brief Fake IRQ number for testing purposes
+ */
+#define _MALI_OSK_IRQ_NUMBER_FAKE ((u32)0xFFFFFFF1)
+
+/** @addtogroup _mali_osk_irq
+ * @{ */
+
+/** @brief PMM Virtual IRQ number
+ */
+#define _MALI_OSK_IRQ_NUMBER_PMM ((u32)0xFFFFFFF2)
+
+
+/** @brief Initialize IRQ handling for a resource
+ *
+ * The _mali_osk_irq_t returned must be written into the resource-specific data
+ * pointed to by data. This is so that the upper and lower handlers can call
+ * _mali_osk_irq_schedulework().
+ *
+ * @note The caller must ensure that the resource does not generate an
+ * interrupt after _mali_osk_irq_init() finishes, and before the
+ * _mali_osk_irq_t is written into the resource-specific data. Otherwise,
+ * the upper-half handler will fail to call _mali_osk_irq_schedulework().
+ *
+ * @param irqnum The IRQ number that the resource uses, as seen by the CPU.
+ * The value -1 has a special meaning which indicates the use of probing, and trigger_func and ack_func must be
+ * non-NULL.
+ * @param uhandler The upper-half handler, corresponding to a ISR handler for
+ * the resource
+ * @param bhandler The lower-half handler, corresponding to an IST handler for
+ * the resource
+ * @param trigger_func Optional: a function to trigger the resource's irq, to
+ * probe for the interrupt. Use NULL if irqnum != -1.
+ * @param ack_func Optional: a function to acknowledge the resource's irq, to
+ * probe for the interrupt. Use NULL if irqnum != -1.
+ * @param data resource-specific data, which will be passed to uhandler,
+ * bhandler and (if present) trigger_func and ack_funnc
+ * @param description textual description of the IRQ resource.
+ * @return on success, a pointer to a _mali_osk_irq_t object, which represents
+ * the IRQ handling on this resource. NULL on failure.
+ */
+_mali_osk_irq_t *_mali_osk_irq_init( u32 irqnum, _mali_osk_irq_uhandler_t uhandler,	_mali_osk_irq_bhandler_t bhandler, _mali_osk_irq_trigger_t trigger_func, _mali_osk_irq_ack_t ack_func, void *data, const char *description );
+
+/** @brief Cause a queued, deferred call of the IRQ bottom-half.
+ *
+ * _mali_osk_irq_schedulework provides a mechanism for enqueuing deferred calls
+ * to the IRQ bottom-half handler. The queue is known as the IRQ work-queue.
+ * After calling _mali_osk_irq_schedulework(), the IRQ bottom-half handler will
+ * be scheduled to run at some point in the future.
+ *
+ * This is called by the IRQ upper-half to defer further processing of
+ * IRQ-related work to the IRQ bottom-half handler. This is necessary for work
+ * that cannot be done in an IRQ context by the IRQ upper-half handler. Timer
+ * callbacks also use this mechanism, because they are treated as though they
+ * operate in an IRQ context. Refer to \ref _mali_osk_timer_t for more
+ * information.
+ *
+ * Code that operates in a kernel-process context (with no IRQ context
+ * restrictions) may also enqueue deferred calls to the IRQ bottom-half. The
+ * advantage over direct calling is that deferred calling allows the caller and
+ * IRQ bottom half to hold the same mutex, with a guarantee that they will not
+ * deadlock just by using this mechanism.
+ *
+ * _mali_osk_irq_schedulework() places deferred call requests on a queue, to
+ * allow for more than one thread to make a deferred call. Therfore, if it is
+ * called 'K' times, then the IRQ bottom-half will be scheduled 'K' times too.
+ * 'K' is a number that is implementation-specific.
+ *
+ * _mali_osk_irq_schedulework() is guaranteed to not block on:
+ * - enqueuing a deferred call request.
+ * - the completion of the IRQ bottom-half handler.
+ *
+ * This is to prevent deadlock. For example, if _mali_osk_irq_schedulework()
+ * blocked, then it would cause a deadlock when the following two conditions
+ * hold:
+ * - The IRQ bottom-half callback (of type _mali_osk_irq_bhandler_t) locks
+ * a mutex
+ * - And, at the same time, the caller of _mali_osk_irq_schedulework() also
+ * holds the same mutex
+ *
+ * @note care must be taken to not overflow the queue that
+ * _mali_osk_irq_schedulework() operates on. Code must be structured to
+ * ensure that the number of requests made to the queue is bounded. Otherwise,
+ * IRQs will be lost.
+ *
+ * The queue that _mali_osk_irq_schedulework implements is a FIFO of N-writer,
+ * 1-reader type. The writers are the callers of _mali_osk_irq_schedulework
+ * (all OSK-registered IRQ upper-half handlers in the system, watchdog timers,
+ * callers from a Kernel-process context). The reader is a single thread that
+ * handles all OSK-registered IRQs.
+ *
+ * The consequence of the queue being a 1-reader type is that calling
+ * _mali_osk_irq_schedulework() on different _mali_osk_irq_t objects causes
+ * their IRQ bottom-halves to be serialized, across all CPU-cores in the
+ * system.
+ *
+ * @param irq a pointer to the _mali_osk_irq_t object corresponding to the
+ * resource whose IRQ bottom-half must begin processing.
+ */
+void _mali_osk_irq_schedulework( _mali_osk_irq_t *irq );
+
+/** @brief Terminate IRQ handling on a resource.
+ *
+ * This will disable the interrupt from the device, and then waits for the
+ * IRQ work-queue to finish the work that is currently in the queue. That is,
+ * for every deferred call currently in the IRQ work-queue, it waits for each
+ * of those to be processed by their respective IRQ bottom-half handler.
+ *
+ * This function is used to ensure that the bottom-half handler of the supplied
+ * IRQ object will not be running at the completion of this function call.
+ * However, the caller must ensure that no other sources could call the
+ * _mali_osk_irq_schedulework() on the same IRQ object. For example, the
+ * relevant timers must be stopped.
+ *
+ * @note While this function is being called, other OSK-registered IRQs in the
+ * system may enqueue work for their respective bottom-half handlers. This
+ * function will not wait for those entries in the work-queue to be flushed.
+ *
+ * Since this blocks on the completion of work in the IRQ work-queue, the
+ * caller of this function \b must \b not hold any mutexes that are taken by
+ * any OSK-registered IRQ bottom-half handler. To do so may cause a deadlock.
+ *
+ * @param irq a pointer to the _mali_osk_irq_t object corresponding to the
+ * resource whose IRQ handling is to be terminated.
+ */
+void _mali_osk_irq_term( _mali_osk_irq_t *irq );
+/** @} */ /* end group _mali_osk_irq */
+
+
+/** @addtogroup _mali_osk_atomic
+ * @{ */
+
+/** @brief Decrement an atomic counter
+ *
+ * @note It is an error to decrement the counter beyond -(1<<23)
+ *
+ * @param atom pointer to an atomic counter */
+void _mali_osk_atomic_dec( _mali_osk_atomic_t *atom );
+
+/** @brief Decrement an atomic counter, return new value
+ *
+ * Although the value returned is a u32, only numbers with signed 24-bit
+ * precision (sign extended to u32) are returned.
+ *
+ * @note It is an error to decrement the counter beyond -(1<<23)
+ *
+ * @param atom pointer to an atomic counter
+ * @return The new value, after decrement */
+u32 _mali_osk_atomic_dec_return( _mali_osk_atomic_t *atom );
+
+/** @brief Increment an atomic counter
+ *
+ * @note It is an error to increment the counter beyond (1<<23)-1
+ *
+ * @param atom pointer to an atomic counter */
+void _mali_osk_atomic_inc( _mali_osk_atomic_t *atom );
+
+/** @brief Increment an atomic counter, return new value
+ *
+ * Although the value returned is a u32, only numbers with signed 24-bit
+ * precision (sign extended to u32) are returned.
+ *
+ * @note It is an error to increment the counter beyond (1<<23)-1
+ *
+ * @param atom pointer to an atomic counter */
+u32 _mali_osk_atomic_inc_return( _mali_osk_atomic_t *atom );
+
+/** @brief Initialize an atomic counter
+ *
+ * The counters have storage for signed 24-bit integers. Initializing to signed
+ * values requiring more than 24-bits storage will fail.
+ *
+ * @note the parameter required is a u32, and so signed integers should be
+ * cast to u32.
+ *
+ * @param atom pointer to an atomic counter
+ * @param val the value to initialize the atomic counter.
+ * @return _MALI_OSK_ERR_OK on success, otherwise, a suitable
+ * _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_osk_atomic_init( _mali_osk_atomic_t *atom, u32 val );
+
+/** @brief Read a value from an atomic counter
+ *
+ * Although the value returned is a u32, only numbers with signed 24-bit
+ * precision (sign extended to u32) are returned.
+ *
+ * This can only be safely used to determine the value of the counter when it
+ * is guaranteed that other threads will not be modifying the counter. This
+ * makes its usefulness limited.
+ *
+ * @param atom pointer to an atomic counter
+ */
+u32 _mali_osk_atomic_read( _mali_osk_atomic_t *atom );
+
+/** @brief Terminate an atomic counter
+ *
+ * @param atom pointer to an atomic counter
+ */
+void _mali_osk_atomic_term( _mali_osk_atomic_t *atom );
+/** @} */  /* end group _mali_osk_atomic */
+
+
+/** @defgroup _mali_osk_memory OSK Memory Allocation
+ * @{ */
+
+/** @brief Allocate zero-initialized memory.
+ *
+ * Returns a buffer capable of containing at least \a n elements of \a size
+ * bytes each. The buffer is initialized to zero.
+ *
+ * The buffer is suitably aligned for storage and subsequent access of every
+ * type that the compiler supports. Therefore, the pointer to the start of the
+ * buffer may be cast into any pointer type, and be subsequently accessed from
+ * such a pointer, without loss of information.
+ *
+ * When the buffer is no longer in use, it must be freed with _mali_osk_free().
+ * Failure to do so will cause a memory leak.
+ *
+ * @note Most toolchains supply memory allocation functions that meet the
+ * compiler's alignment requirements.
+ *
+ * @param n Number of elements to allocate
+ * @param size Size of each element
+ * @return On success, the zero-initialized buffer allocated. NULL on failure
+ */
+void *_mali_osk_calloc( u32 n, u32 size );
+
+/** @brief Allocate memory.
+ *
+ * Returns a buffer capable of containing at least \a size bytes. The
+ * contents of the buffer are undefined.
+ *
+ * The buffer is suitably aligned for storage and subsequent access of every
+ * type that the compiler supports. Therefore, the pointer to the start of the
+ * buffer may be cast into any pointer type, and be subsequently accessed from
+ * such a pointer, without loss of information.
+ *
+ * When the buffer is no longer in use, it must be freed with _mali_osk_free().
+ * Failure to do so will cause a memory leak.
+ *
+ * @note Most toolchains supply memory allocation functions that meet the
+ * compiler's alignment requirements.
+ *
+ * Remember to free memory using _mali_osk_free().
+ * @param size Number of bytes to allocate
+ * @return On success, the buffer allocated. NULL on failure.
+ */
+void *_mali_osk_malloc( u32 size );
+
+/** @brief Free memory.
+ *
+ * Reclaims the buffer pointed to by the parameter \a ptr for the system.
+ * All memory returned from _mali_osk_malloc() and _mali_osk_calloc()
+ * must be freed before the application exits. Otherwise,
+ * a memory leak will occur.
+ *
+ * Memory must be freed once. It is an error to free the same non-NULL pointer
+ * more than once.
+ *
+ * It is legal to free the NULL pointer.
+ *
+ * @param ptr Pointer to buffer to free
+ */
+void _mali_osk_free( void *ptr );
+
+/** @brief Copies memory.
+ *
+ * Copies the \a len bytes from the buffer pointed by the parameter \a src
+ * directly to the buffer pointed by \a dst.
+ *
+ * It is an error for \a src to overlap \a dst anywhere in \a len bytes.
+ *
+ * @param dst Pointer to the destination array where the content is to be
+ * copied.
+ * @param src Pointer to the source of data to be copied.
+ * @param len Number of bytes to copy.
+ * @return \a dst is always passed through unmodified.
+ */
+void *_mali_osk_memcpy( void *dst, const void *src, u32 len );
+
+/** @brief Fills memory.
+ *
+ * Sets the first \a n bytes of the block of memory pointed to by \a s to
+ * the specified value
+ * @param s Pointer to the block of memory to fill.
+ * @param c Value to be set, passed as u32. Only the 8 Least Significant Bits (LSB)
+ * are used.
+ * @param n Number of bytes to be set to the value.
+ * @return \a s is always passed through unmodified
+ */
+void *_mali_osk_memset( void *s, u32 c, u32 n );
+/** @} */ /* end group _mali_osk_memory */
+
+
+/** @brief Checks the amount of memory allocated
+ *
+ * Checks that not more than \a max_allocated bytes are allocated. 
+ *
+ * Some OS bring up an interactive out of memory dialogue when the
+ * system runs out of memory. This can stall non-interactive
+ * apps (e.g. automated test runs). This function can be used to
+ * not trigger the OOM dialogue by keeping allocations
+ * within a certain limit.
+ *
+ * @return MALI_TRUE when \a max_allocated bytes are not in use yet. MALI_FALSE 
+ * when at least \a max_allocated bytes are in use.
+ */
+mali_bool _mali_osk_mem_check_allocated( u32 max_allocated );
+
+/** @addtogroup _mali_osk_lock
+ * @{ */
+
+/** @brief Initialize a Mutual Exclusion Lock
+ *
+ * Locks are created in the signalled (unlocked) state.
+ *
+ * initial must be zero, since there is currently no means of expressing
+ * whether a reader/writer lock should be initially locked as a reader or
+ * writer. This would require some encoding to be used.
+ *
+ * 'Automatic' ordering means that locks must be obtained in the order that
+ * they were created. For all locks that can be held at the same time, they must
+ * either all provide the order parameter, or they all must use 'automatic'
+ * ordering - because there is no way of mixing 'automatic' and 'manual'
+ * ordering.
+ *
+ * @param flags flags combined with bitwise OR ('|'), or zero. There are
+ * restrictions on which flags can be combined, @see _mali_osk_lock_flags_t.
+ * @param initial For future expansion into semaphores. SBZ.
+ * @param order The locking order of the mutex. That is, locks obtained by the
+ * same thread must have been created with an increasing order parameter, for
+ * deadlock prevention. Setting to zero causes 'automatic' ordering to be used.
+ * @return On success, a pointer to a _mali_osk_lock_t object. NULL on failure.
+ */
+_mali_osk_lock_t *_mali_osk_lock_init( _mali_osk_lock_flags_t flags, u32 initial, u32 order );
+
+/** @brief Wait for a lock to be signalled (obtained)
+
+ * After a thread has successfully waited on the lock, the lock is obtained by
+ * the thread, and is marked as unsignalled. The thread releases the lock by
+ * signalling it.
+ *
+ * In the case of Reader/Writer locks, multiple readers can obtain a lock in
+ * the absence of writers, which is a performance optimization (providing that
+ * the readers never write to the protected resource).
+ *
+ * To prevent deadlock, locks must always be obtained in the same order.
+ *
+ * For locks marked as _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE, it is a
+ * programming error for the function to exit without obtaining the lock. This
+ * means that the error code must only be checked for interruptible locks.
+ *
+ * @param lock the lock to wait upon (obtain).
+ * @param mode the mode in which the lock should be obtained. Unless the lock
+ * was created with _MALI_OSK_LOCKFLAG_READERWRITER, this must be
+ * _MALI_OSK_LOCKMODE_RW.
+ * @return On success, _MALI_OSK_ERR_OK. For interruptible locks, a suitable
+ * _mali_osk_errcode_t will be returned on failure, and the lock will not be
+ * obtained. In this case, the error code must be propagated up to the U/K
+ * interface.
+ */
+_mali_osk_errcode_t _mali_osk_lock_wait( _mali_osk_lock_t *lock, _mali_osk_lock_mode_t mode);
+
+
+/** @brief Signal (release) a lock
+ *
+ * Locks may only be signalled by the thread that originally waited upon the
+ * lock.
+ *
+ * @note In the OSU, a flag exists to allow any thread to signal a
+ * lock. Such functionality is not present in the OSK.
+ *
+ * @param lock the lock to signal (release).
+ * @param mode the mode in which the lock should be obtained. This must match
+ * the mode in which the lock was waited upon.
+ */
+void _mali_osk_lock_signal( _mali_osk_lock_t *lock, _mali_osk_lock_mode_t mode );
+
+/** @brief Terminate a lock
+ *
+ * This terminates a lock and frees all associated resources.
+ *
+ * It is a programming error to terminate the lock when it is held (unsignalled)
+ * by a thread.
+ *
+ * @param lock the lock to terminate.
+ */
+void _mali_osk_lock_term( _mali_osk_lock_t *lock );
+/** @} */ /* end group _mali_osk_lock */
+
+
+/** @addtogroup _mali_osk_low_level_memory
+ * @{ */
+
+/** @brief Issue a memory barrier
+ *
+ * This defines an arbitrary memory barrier operation, which affects memory
+ * mapped by _mali_osk_mem_mapregion. It will not be needed for memory
+ * mapped through _mali_osk_mem_mapioregion.
+ */
+void _mali_osk_mem_barrier( void );
+
+/** @brief Map a physically contiguous region into kernel space
+ *
+ * This is primarily used for mapping in registers from resources, and Mali-MMU
+ * page tables. The mapping is only visable from kernel-space.
+ *
+ * Access has to go through _mali_osk_mem_ioread32 and _mali_osk_mem_iowrite32
+ *
+ * @param phys CPU-physical base address of the memory to map in. This must
+ * be aligned to the system's page size, which is assumed to be 4K.
+ * @param size the number of bytes of physically contiguous address space to
+ * map in
+ * @param description A textual description of the memory being mapped in.
+ * @return On success, a Mali IO address through which the mapped-in
+ * memory/registers can be accessed. NULL on failure.
+ */
+mali_io_address _mali_osk_mem_mapioregion( u32 phys, u32 size, const char *description );
+
+/** @brief Unmap a physically contiguous address range from kernel space.
+ *
+ * The address range should be one previously mapped in through
+ * _mali_osk_mem_mapioregion.
+ *
+ * It is a programming error to do (but not limited to) the following:
+ * - attempt an unmap twice
+ * - unmap only part of a range obtained through _mali_osk_mem_mapioregion
+ * - unmap more than the range obtained through  _mali_osk_mem_mapioregion
+ * - unmap an address range that was not successfully mapped using
+ * _mali_osk_mem_mapioregion
+ * - provide a mapping that does not map to phys.
+ *
+ * @param phys CPU-physical base address of the memory that was originally
+ * mapped in. This must be aligned to the system's page size, which is assumed
+ * to be 4K
+ * @param size The number of bytes that were originally mapped in.
+ * @param mapping The Mali IO address through which the mapping is
+ * accessed.
+ */
+void _mali_osk_mem_unmapioregion( u32 phys, u32 size, mali_io_address mapping );
+
+/** @brief Allocate and Map a physically contiguous region into kernel space
+ *
+ * This is used for allocating physically contiguous regions (such as Mali-MMU
+ * page tables) and mapping them into kernel space. The mapping is only
+ * visible from kernel-space.
+ *
+ * The alignment of the returned memory is guaranteed to be at least
+ * _MALI_OSK_CPU_PAGE_SIZE.
+ *
+ * Access must go through _mali_osk_mem_ioread32 and _mali_osk_mem_iowrite32
+ *
+ * @note This function is primarily to provide support for OSs that are
+ * incapable of separating the tasks 'allocate physically contiguous memory'
+ * and 'map it into kernel space'
+ *
+ * @param[out] phys CPU-physical base address of memory that was allocated.
+ * (*phys) will be guaranteed to be aligned to at least
+ * _MALI_OSK_CPU_PAGE_SIZE on success.
+ *
+ * @param[in] size the number of bytes of physically contiguous memory to
+ * allocate. This must be a multiple of _MALI_OSK_CPU_PAGE_SIZE.
+ *
+ * @return On success, a Mali IO address through which the mapped-in
+ * memory/registers can be accessed. NULL on failure, and (*phys) is unmodified.
+ */
+mali_io_address _mali_osk_mem_allocioregion( u32 *phys, u32 size );
+
+/** @brief Free a physically contiguous address range from kernel space.
+ *
+ * The address range should be one previously mapped in through
+ * _mali_osk_mem_allocioregion.
+ *
+ * It is a programming error to do (but not limited to) the following:
+ * - attempt a free twice on the same ioregion
+ * - free only part of a range obtained through _mali_osk_mem_allocioregion
+ * - free more than the range obtained through  _mali_osk_mem_allocioregion
+ * - free an address range that was not successfully mapped using
+ * _mali_osk_mem_allocioregion
+ * - provide a mapping that does not map to phys.
+ *
+ * @param phys CPU-physical base address of the memory that was originally
+ * mapped in, which was aligned to _MALI_OSK_CPU_PAGE_SIZE.
+ * @param size The number of bytes that were originally mapped in, which was
+ * a multiple of _MALI_OSK_CPU_PAGE_SIZE.
+ * @param mapping The Mali IO address through which the mapping is
+ * accessed.
+ */
+void _mali_osk_mem_freeioregion( u32 phys, u32 size, mali_io_address mapping );
+
+/** @brief Request a region of physically contiguous memory
+ *
+ * This is used to ensure exclusive access to a region of physically contigous
+ * memory.
+ *
+ * It is acceptable to implement this as a stub. However, it is then the job
+ * of the System Integrator to ensure that no other device driver will be using
+ * the physical address ranges used by Mali, while the Mali device driver is
+ * loaded.
+ *
+ * @param phys CPU-physical base address of the memory to request. This must
+ * be aligned to the system's page size, which is assumed to be 4K.
+ * @param size the number of bytes of physically contiguous address space to
+ * request.
+ * @param description A textual description of the memory being requested.
+ * @return _MALI_OSK_ERR_OK on success. Otherwise, a suitable
+ * _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_osk_mem_reqregion( u32 phys, u32 size, const char *description );
+
+/** @brief Un-request a region of physically contiguous memory
+ *
+ * This is used to release a regious of physically contiguous memory previously
+ * requested through _mali_osk_mem_reqregion, so that other device drivers may
+ * use it. This will be called at time of Mali device driver termination.
+ *
+ * It is a programming error to attempt to:
+ * - unrequest a region twice
+ * - unrequest only part of a range obtained through _mali_osk_mem_reqregion
+ * - unrequest more than the range obtained through  _mali_osk_mem_reqregion
+ * - unrequest an address range that was not successfully requested using
+ * _mali_osk_mem_reqregion
+ *
+ * @param phys CPU-physical base address of the memory to un-request. This must
+ * be aligned to the system's page size, which is assumed to be 4K
+ * @param size the number of bytes of physically contiguous address space to
+ * un-request.
+ */
+void _mali_osk_mem_unreqregion( u32 phys, u32 size );
+
+/** @brief Read from a location currently mapped in through
+ * _mali_osk_mem_mapioregion
+ *
+ * This reads a 32-bit word from a 32-bit aligned location. It is a programming
+ * error to provide unaligned locations, or to read from memory that is not
+ * mapped in, or not mapped through either _mali_osk_mem_mapioregion() or
+ * _mali_osk_mem_allocioregion().
+ *
+ * @param mapping Mali IO address to read from
+ * @param offset Byte offset from the given IO address to operate on, must be a multiple of 4
+ * @return the 32-bit word from the specified location.
+ */
+u32 _mali_osk_mem_ioread32( volatile mali_io_address mapping, u32 offset );
+
+/** @brief Write to a location currently mapped in through
+ * _mali_osk_mem_mapioregion
+ *
+ * This write a 32-bit word to a 32-bit aligned location. It is a programming
+ * error to provide unaligned locations, or to write to memory that is not
+ * mapped in, or not mapped through either _mali_osk_mem_mapioregion() or
+ * _mali_osk_mem_allocioregion().
+ *
+ * @param mapping Mali IO address to write to
+ * @param offset Byte offset from the given IO address to operate on, must be a multiple of 4
+ * @param val the 32-bit word to write.
+ */
+void _mali_osk_mem_iowrite32( volatile mali_io_address mapping, u32 offset, u32 val );
+
+/** @brief Flush all CPU caches
+ *
+ * This should only be implemented if flushing of the cache is required for
+ * memory mapped in through _mali_osk_mem_mapregion.
+ */
+void _mali_osk_cache_flushall( void );
+
+/** @brief Flush any caches necessary for the CPU and MALI to have the same view of a range of uncached mapped memory
+ *
+ * This should only be implemented if your OS doesn't do a full cache flush (inner & outer)
+ * after allocating uncached mapped memory.
+ *
+ * Some OS do not perform a full cache flush (including all outer caches) for uncached mapped memory.
+ * They zero the memory through a cached mapping, then flush the inner caches but not the outer caches.
+ * This is required for MALI to have the correct view of the memory. 
+ */
+void _mali_osk_cache_ensure_uncached_range_flushed( void *uncached_mapping, u32 offset, u32 size );
+
+/** @} */ /* end group _mali_osk_low_level_memory */
+
+
+/** @addtogroup _mali_osk_notification
+ *
+ * User space notification framework
+ *
+ * Communication with user space of asynchronous events is performed through a
+ * synchronous call to the \ref u_k_api.
+ *
+ * Since the events are asynchronous, the events have to be queued until a
+ * synchronous U/K API call can be made by user-space. A U/K API call might also
+ * be received before any event has happened. Therefore the notifications the
+ * different subsystems wants to send to user space has to be queued for later
+ * reception, or a U/K API call has to be blocked until an event has occured.
+ *
+ * Typical uses of notifications are after running of jobs on the hardware or
+ * when changes to the system is detected that needs to be relayed to user
+ * space.
+ *
+ * After an event has occured user space has to be notified using some kind of
+ * message. The notification framework supports sending messages to waiting
+ * threads or queueing of messages until a U/K API call is made.
+ *
+ * The notification queue is a FIFO. There are no restrictions on the numbers
+ * of readers or writers in the queue.
+ *
+ * A message contains what user space needs to identifiy how to handle an
+ * event. This includes a type field and a possible type specific payload.
+ *
+ * A notification to user space is represented by a
+ * \ref _mali_osk_notification_t object. A sender gets hold of such an object
+ * using _mali_osk_notification_create(). The buffer given by the
+ * _mali_osk_notification_t::result_buffer field in the object is used to store
+ * any type specific data. The other fields are internal to the queue system
+ * and should not be touched.
+ *
+ * @{ */
+
+/** @brief Create a notification object
+ *
+ * Returns a notification object which can be added to the queue of
+ * notifications pending for user space transfer.
+ *
+ * The implementation will initialize all members of the
+ * \ref _mali_osk_notification_t object. In particular, the
+ * _mali_osk_notification_t::result_buffer member will be initialized to point
+ * to \a size bytes of storage, and that storage will be suitably aligned for
+ * storage of any structure. That is, the created buffer meets the same
+ * requirements as _mali_osk_malloc().
+ *
+ * The notification object must be deleted when not in use. Use
+ * _mali_osk_notification_delete() for deleting it.
+ *
+ * @note You \b must \b not call _mali_osk_free() on a \ref _mali_osk_notification_t,
+ * object, or on a _mali_osk_notification_t::result_buffer. You must only use
+ * _mali_osk_notification_delete() to free the resources assocaited with a
+ * \ref _mali_osk_notification_t object.
+ *
+ * @param type The notification type
+ * @param size The size of the type specific buffer to send
+ * @return Pointer to a notification object with a suitable buffer, or NULL on error.
+ */
+_mali_osk_notification_t *_mali_osk_notification_create( u32 type, u32 size );
+
+/** @brief Delete a notification object
+ *
+ * This must be called to reclaim the resources of a notification object. This
+ * includes:
+ * - The _mali_osk_notification_t::result_buffer
+ * - The \ref _mali_osk_notification_t itself.
+ *
+ * A notification object \b must \b not be used after it has been deleted by
+ * _mali_osk_notification_delete().
+ *
+ * In addition, the notification object may not be deleted while it is in a
+ * queue. That is, if it has been placed on a queue with
+ * _mali_osk_notification_queue_send(), then it must not be deleted until
+ * it has been received by a call to _mali_osk_notification_queue_receive().
+ * Otherwise, the queue may be corrupted.
+ *
+ * @param object the notification object to delete.
+ */
+void _mali_osk_notification_delete( _mali_osk_notification_t *object );
+
+/** @brief Create a notification queue
+ *
+ * Creates a notification queue which can be used to queue messages for user
+ * delivery and get queued messages from
+ *
+ * The queue is a FIFO, and has no restrictions on the numbers of readers or
+ * writers.
+ *
+ * When the queue is no longer in use, it must be terminated with
+ * \ref _mali_osk_notification_queue_term(). Failure to do so will result in a
+ * memory leak.
+ *
+ * @return Pointer to a new notification queue or NULL on error.
+ */
+_mali_osk_notification_queue_t *_mali_osk_notification_queue_init( void );
+
+/** @brief Destroy a notification queue
+ *
+ * Destroys a notification queue and frees associated resources from the queue.
+ *
+ * A notification queue \b must \b not be destroyed in the following cases:
+ * - while there are \ref _mali_osk_notification_t objects in the queue.
+ * - while there are writers currently acting upon the queue. That is, while
+ * a thread is currently calling \ref _mali_osk_notification_queue_send() on
+ * the queue, or while a thread may call
+ * \ref _mali_osk_notification_queue_send() on the queue in the future.
+ * - while there are readers currently waiting upon the queue. That is, while
+ * a thread is currently calling \ref _mali_osk_notification_queue_receive() on
+ * the queue, or while a thread may call
+ * \ref _mali_osk_notification_queue_receive() on the queue in the future.
+ *
+ * Therefore, all \ref _mali_osk_notification_t objects must be flushed and
+ * deleted by the code that makes use of the notification queues, since only
+ * they know the structure of the _mali_osk_notification_t::result_buffer
+ * (even if it may only be a flat sturcture).
+ *
+ * @note Since the queue is a FIFO, the code using notification queues may
+ * create its own 'flush' type of notification, to assist in flushing the
+ * queue.
+ *
+ * Once the queue has been destroyed, it must not be used again.
+ *
+ * @param queue The queue to destroy
+ */
+void _mali_osk_notification_queue_term( _mali_osk_notification_queue_t *queue );
+
+/** @brief Schedule notification for delivery
+ *
+ * When a \ref _mali_osk_notification_t object has been created successfully
+ * and set up, it may be added to the queue of objects waiting for user space
+ * transfer.
+ *
+ * The sending will not block if the queue is full.
+ *
+ * A \ref _mali_osk_notification_t object \b must \b not be put on two different
+ * queues at the same time, or enqueued twice onto a single queue before
+ * reception. However, it is acceptable for it to be requeued \em after reception
+ * from a call to _mali_osk_notification_queue_receive(), even onto the same queue.
+ *
+ * Again, requeuing must also not enqueue onto two different queues at the same
+ * time, or enqueue onto the same queue twice before reception.
+ *
+ * @param queue The notification queue to add this notification to
+ * @param object The entry to add
+ */
+void _mali_osk_notification_queue_send( _mali_osk_notification_queue_t *queue, _mali_osk_notification_t *object );
+
+#if MALI_STATE_TRACKING
+/** @brief Receive a notification from a queue
+ *
+ * Check if a notification queue is empty.
+ *
+ * @param queue The queue to check.
+ * @return MALI_TRUE if queue is empty, otherwise MALI_FALSE.
+ */
+mali_bool _mali_osk_notification_queue_is_empty( _mali_osk_notification_queue_t *queue );
+#endif
+
+/** @brief Receive a notification from a queue
+ *
+ * Receives a single notification from the given queue.
+ *
+ * If no notifciations are ready the thread will sleep until one becomes ready.
+ * Therefore, notifications may not be received into an
+ * IRQ or 'atomic' context (that is, a context where sleeping is disallowed).
+ *
+ * @param queue The queue to receive from
+ * @param result Pointer to storage of a pointer of type
+ * \ref _mali_osk_notification_t*. \a result will be written to such that the
+ * expression \a (*result) will evaluate to a pointer to a valid
+ * \ref _mali_osk_notification_t object, or NULL if none were received.
+ * @return _MALI_OSK_ERR_OK on success. _MALI_OSK_ERR_RESTARTSYSCALL if the sleep was interrupted.
+ */
+_mali_osk_errcode_t _mali_osk_notification_queue_receive( _mali_osk_notification_queue_t *queue, _mali_osk_notification_t **result );
+
+/** @brief Dequeues a notification from a queue
+ *
+ * Receives a single notification from the given queue.
+ *
+ * If no notifciations are ready the function call will return an error code.
+ *
+ * @param queue The queue to receive from
+ * @param result Pointer to storage of a pointer of type
+ * \ref _mali_osk_notification_t*. \a result will be written to such that the
+ * expression \a (*result) will evaluate to a pointer to a valid
+ * \ref _mali_osk_notification_t object, or NULL if none were received.
+ * @return _MALI_OSK_ERR_OK on success, _MALI_OSK_ERR_ITEM_NOT_FOUND if queue was empty.
+ */
+_mali_osk_errcode_t _mali_osk_notification_queue_dequeue( _mali_osk_notification_queue_t *queue, _mali_osk_notification_t **result );
+
+/** @} */ /* end group _mali_osk_notification */
+
+
+/** @addtogroup _mali_osk_timer
+ *
+ * Timers use the OS's representation of time, which are 'ticks'. This is to
+ * prevent aliasing problems between the internal timer time, and the time
+ * asked for.
+ *
+ * @{ */
+
+/** @brief Initialize a timer
+ *
+ * Allocates resources for a new timer, and initializes them. This does not
+ * start the timer.
+ *
+ * @return a pointer to the allocated timer object, or NULL on failure.
+ */
+_mali_osk_timer_t *_mali_osk_timer_init(void);
+
+/** @brief Start a timer
+ *
+ * It is an error to start a timer without setting the callback via
+ * _mali_osk_timer_setcallback().
+ *
+ * It is an error to use this to start an already started timer.
+ *
+ * The timer will expire in \a ticks_to_expire ticks, at which point, the
+ * callback function will be invoked with the callback-specific data,
+ * as registered by _mali_osk_timer_setcallback().
+ *
+ * @param tim the timer to start
+ * @param ticks_to_expire the amount of time in ticks for the timer to run
+ * before triggering.
+ */
+void _mali_osk_timer_add( _mali_osk_timer_t *tim, u32 ticks_to_expire );
+
+/** @brief Modify a timer
+ *
+ * Set the absolute time at which a timer will expire, and start it if it is
+ * stopped. If \a expiry_tick is in the past (determined by
+ * _mali_osk_time_after() ), the timer fires immediately.
+ *
+ * It is an error to modify a timer without setting the callback via
+ *  _mali_osk_timer_setcallback().
+ *
+ * The timer will expire at absolute time \a expiry_tick, at which point, the
+ * callback function will be invoked with the callback-specific data, as set
+ * by _mali_osk_timer_setcallback().
+ *
+ * @param tim the timer to modify, and start if necessary
+ * @param expiry_tick the \em absolute time in ticks at which this timer should
+ * trigger.
+ *
+ */
+void _mali_osk_timer_mod( _mali_osk_timer_t *tim, u32 expiry_tick);
+
+/** @brief Stop a timer, and block on its completion.
+ *
+ * Stop the timer. When the function returns, it is guaranteed that the timer's
+ * callback will not be running on any CPU core.
+ *
+ * Since stoping the timer blocks on compeletion of the callback, the callback
+ * may not obtain any mutexes that the caller holds. Otherwise, a deadlock will
+ * occur.
+ *
+ * @note While the callback itself is guaranteed to not be running, work
+ * enqueued on the IRQ work-queue by the timer (with
+ * \ref _mali_osk_irq_schedulework()) may still run. The timer callback and IRQ
+ * bottom-half handler must take this into account.
+ *
+ * It is legal to stop an already stopped timer.
+ *
+ * @param tim the timer to stop.
+ *
+ */
+void _mali_osk_timer_del( _mali_osk_timer_t *tim );
+
+/** @brief Set a timer's callback parameters.
+ *
+ * This must be called at least once before a timer is started/modified.
+ *
+ * After a timer has been stopped or expires, the callback remains set. This
+ * means that restarting the timer will call the same function with the same
+ * parameters on expiry.
+ *
+ * @param tim the timer to set callback on.
+ * @param callback Function to call when timer expires
+ * @param data Function-specific data to supply to the function on expiry.
+ */
+void _mali_osk_timer_setcallback( _mali_osk_timer_t *tim, _mali_osk_timer_callback_t callback, void *data );
+
+/** @brief Terminate a timer, and deallocate resources.
+ *
+ * The timer must first be stopped by calling _mali_osk_timer_del().
+ *
+ * It is a programming error for _mali_osk_timer_term() to be called on:
+ * - timer that is currently running
+ * - a timer that is currently executing its callback.
+ *
+ * @param tim the timer to deallocate.
+ */
+void _mali_osk_timer_term( _mali_osk_timer_t *tim );
+/** @} */ /* end group _mali_osk_timer */
+
+
+/** @defgroup _mali_osk_time OSK Time functions
+ *
+ * \ref _mali_osk_time use the OS's representation of time, which are
+ * 'ticks'. This is to prevent aliasing problems between the internal timer
+ * time, and the time asked for.
+ *
+ * OS tick time is measured as a u32. The time stored in a u32 may either be
+ * an absolute time, or a time delta between two events. Whilst it is valid to
+ * use math opeartors to \em change the tick value represented as a u32, it
+ * is often only meaningful to do such operations on time deltas, rather than
+ * on absolute time. However, it is meaningful to add/subtract time deltas to
+ * absolute times.
+ *
+ * Conversion between tick time and milliseconds (ms) may not be loss-less,
+ * and are \em implementation \em depenedant.
+ *
+ * Code use OS time must take this into account, since:
+ * - a small OS time may (or may not) be rounded
+ * - a large time may (or may not) overflow
+ *
+ * @{ */
+
+/** @brief Return whether ticka occurs after tickb
+ *
+ * Some OSs handle tick 'rollover' specially, and so can be more robust against
+ * tick counters rolling-over. This function must therefore be called to
+ * determine if a time (in ticks) really occurs after another time (in ticks).
+ *
+ * @param ticka ticka
+ * @param tickb tickb
+ * @return non-zero if ticka represents a time that occurs after tickb.
+ * Zero otherwise.
+ */
+int	_mali_osk_time_after( u32 ticka, u32 tickb );
+
+/** @brief Convert milliseconds to OS 'ticks'
+ *
+ * @param ms time interval in milliseconds
+ * @return the corresponding time interval in OS ticks.
+ */
+u32	_mali_osk_time_mstoticks( u32 ms );
+
+/** @brief Convert OS 'ticks' to milliseconds
+ *
+ * @param ticks time interval in OS ticks.
+ * @return the corresponding time interval in milliseconds
+ */
+u32	_mali_osk_time_tickstoms( u32 ticks );
+
+
+/** @brief Get the current time in OS 'ticks'.
+ * @return the current time in OS 'ticks'.
+ */
+u32	_mali_osk_time_tickcount( void );
+
+/** @brief Cause a microsecond delay
+ *
+ * The delay will have microsecond resolution, and is necessary for correct
+ * operation of the driver. At worst, the delay will be \b at least \a usecs
+ * microseconds, and so may be (significantly) more.
+ *
+ * This function may be implemented as a busy-wait, which is the most sensible
+ * implementation. On OSs where there are situations in which a thread must not
+ * sleep, this is definitely implemented as a busy-wait.
+ *
+ * @param usecs the number of microseconds to wait for.
+ */
+void _mali_osk_time_ubusydelay( u32 usecs );
+
+/** @brief Return time in nano seconds, since any given reference.
+ * 
+ * @return Time in nano seconds
+ */
+u64 _mali_osk_time_get_ns( void );
+
+
+/** @} */ /* end group _mali_osk_time */
+
+/** @defgroup _mali_osk_math OSK Math
+ * @{ */
+
+/** @brief Count Leading Zeros (Little-endian)
+ *
+ * @note This function must be implemented to support the reference
+ * implementation of _mali_osk_find_first_zero_bit, as defined in
+ * mali_osk_bitops.h.
+ *
+ * @param val 32-bit words to count leading zeros on
+ * @return the number of leading zeros.
+ */
+u32 _mali_osk_clz( u32 val );
+/** @} */ /* end group _mali_osk_math */
+
+
+/** @addtogroup _mali_osk_miscellaneous
+ * @{ */
+
+/** @brief Output a device driver debug message.
+ *
+ * The interpretation of \a fmt is the same as the \c format parameter in
+ * _mali_osu_vsnprintf().
+ *
+ * @param fmt a _mali_osu_vsnprintf() style format string
+ * @param ... a variable-number of parameters suitable for \a fmt
+ */
+void _mali_osk_dbgmsg( const char *fmt, ... );
+
+/** @brief Abnormal process abort.
+ *
+ * Terminates the caller-process if this function is called.
+ *
+ * This function will be called from Debug assert-macros in mali_kernel_common.h.
+ *
+ * This function will never return - because to continue from a Debug assert
+ * could cause even more problems, and hinder debugging of the initial problem.
+ *
+ * This function is only used in Debug builds, and is not used in Release builds.
+ */
+void _mali_osk_abort(void);
+
+/** @brief Sets breakpoint at point where function is called.
+ *
+ * This function will be called from Debug assert-macros in mali_kernel_common.h,
+ * to assist in debugging. If debugging at this level is not required, then this
+ * function may be implemented as a stub.
+ *
+ * This function is only used in Debug builds, and is not used in Release builds.
+ */
+void _mali_osk_break(void);
+
+/** @brief Return an identificator for calling process.
+ *
+ * @return Identificator for calling process.
+ */
+u32 _mali_osk_get_pid(void);
+
+/** @brief Return an identificator for calling thread.
+ *
+ * @return Identificator for calling thread.
+ */
+u32 _mali_osk_get_tid(void);
+
+/** @} */ /* end group  _mali_osk_miscellaneous */
+
+
+/** @} */ /* end group osuapi */
+
+/** @} */ /* end group uddapi */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#include "mali_osk_specific.h"           /* include any per-os specifics */
+
+/* Check standard inlines */
+#ifndef MALI_STATIC_INLINE
+	#error MALI_STATIC_INLINE not defined on your OS
+#endif
+
+#ifndef MALI_NON_STATIC_INLINE
+	#error MALI_NON_STATIC_INLINE not defined on your OS
+#endif
+
+#endif /* __MALI_OSK_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk_bitops.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk_bitops.h
new file mode 100644
index 00000000000..28026ba1e6d
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk_bitops.h
@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_bitops.h
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+#ifndef __MALI_OSK_BITOPS_H__
+#define __MALI_OSK_BITOPS_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+MALI_STATIC_INLINE void _mali_internal_clear_bit( u32 bit, u32 *addr )
+{
+	MALI_DEBUG_ASSERT( bit < 32 );
+	MALI_DEBUG_ASSERT( NULL != addr );
+
+	(*addr) &= ~(1 << bit);
+}
+
+MALI_STATIC_INLINE void _mali_internal_set_bit( u32 bit, u32 *addr )
+{
+	MALI_DEBUG_ASSERT( bit < 32 );
+	MALI_DEBUG_ASSERT( NULL != addr );
+
+	(*addr) |= (1 << bit);
+}
+
+MALI_STATIC_INLINE u32 _mali_internal_test_bit( u32 bit, u32 value )
+{
+	MALI_DEBUG_ASSERT( bit < 32 );
+	return value & (1 << bit);
+}
+
+MALI_STATIC_INLINE int _mali_internal_find_first_zero_bit( u32 value )
+{
+	u32 inverted;
+	u32 negated;
+	u32 isolated;
+	u32 leading_zeros;
+
+	/* Begin with xxx...x0yyy...y, where ys are 1, number of ys is in range  0..31 */
+	inverted = ~value; /* zzz...z1000...0 */
+	/* Using count_trailing_zeros on inverted value -
+	 * See ARM System Developers Guide for details of count_trailing_zeros */
+
+	/* Isolate the zero: it is preceeded by a run of 1s, so add 1 to it */
+	negated = (u32)-inverted ; /* -a == ~a + 1 (mod 2^n) for n-bit numbers */
+    /* negated = xxx...x1000...0 */
+
+	isolated = negated & inverted ; /* xxx...x1000...0 & zzz...z1000...0, zs are ~xs */
+	/* And so the first zero bit is in the same position as the 1 == number of 1s that preceeded it
+	 * Note that the output is zero if value was all 1s */
+
+	leading_zeros = _mali_osk_clz( isolated );
+
+	return 31 - leading_zeros;
+}
+
+
+/** @defgroup _mali_osk_bitops OSK Non-atomic Bit-operations
+ * @{ */
+
+/**
+ * These bit-operations do not work atomically, and so locks must be used if
+ * atomicity is required.
+ *
+ * Reference implementations for Little Endian are provided, and so it should
+ * not normally be necessary to re-implement these. Efficient bit-twiddling
+ * techniques are used where possible, implemented in portable C.
+ *
+ * Note that these reference implementations rely on _mali_osk_clz() being
+ * implemented.
+ */
+
+/** @brief Clear a bit in a sequence of 32-bit words
+ * @param nr bit number to clear, starting from the (Little-endian) least
+ * significant bit
+ * @param addr starting point for counting.
+ */
+MALI_STATIC_INLINE void _mali_osk_clear_nonatomic_bit( u32 nr, u32 *addr )
+{
+	addr += nr >> 5; /* find the correct word */
+	nr = nr & ((1 << 5)-1); /* The bit number within the word */
+
+	_mali_internal_clear_bit( nr, addr );
+}
+
+/** @brief Set a bit in a sequence of 32-bit words
+ * @param nr bit number to set, starting from the (Little-endian) least
+ * significant bit
+ * @param addr starting point for counting.
+ */
+MALI_STATIC_INLINE void _mali_osk_set_nonatomic_bit( u32 nr, u32 *addr )
+{
+	addr += nr >> 5; /* find the correct word */
+	nr = nr & ((1 << 5)-1); /* The bit number within the word */
+
+	_mali_internal_set_bit( nr, addr );
+}
+
+/** @brief Test a bit in a sequence of 32-bit words
+ * @param nr bit number to test, starting from the (Little-endian) least
+ * significant bit
+ * @param addr starting point for counting.
+ * @return zero if bit was clear, non-zero if set. Do not rely on the return
+ * value being related to the actual word under test.
+ */
+MALI_STATIC_INLINE u32 _mali_osk_test_bit( u32 nr, u32 *addr )
+{
+	addr += nr >> 5; /* find the correct word */
+	nr = nr & ((1 << 5)-1); /* The bit number within the word */
+
+	return _mali_internal_test_bit( nr, *addr );
+}
+
+/* Return maxbit if not found */
+/** @brief Find the first zero bit in a sequence of 32-bit words
+ * @param addr starting point for search.
+ * @param maxbit the maximum number of bits to search
+ * @return the number of the first zero bit found, or maxbit if none were found
+ * in the specified range.
+ */
+MALI_STATIC_INLINE u32 _mali_osk_find_first_zero_bit( const u32 *addr, u32 maxbit )
+{
+	u32 total;
+
+	for ( total = 0; total < maxbit; total += 32, ++addr )
+	{
+		int result;
+		result = _mali_internal_find_first_zero_bit( *addr );
+
+		/* non-negative signifies the bit was found */
+		if ( result >= 0 )
+		{
+			total += (u32)result;
+			break;
+		}
+	}
+
+	/* Now check if we reached maxbit or above */
+	if ( total >= maxbit )
+	{
+		total = maxbit;
+	}
+
+	return total; /* either the found bit nr, or maxbit if not found */
+}
+/** @} */ /* end group _mali_osk_bitops */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_OSK_BITOPS_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk_list.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk_list.h
new file mode 100644
index 00000000000..a8d15f2a107
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk_list.h
@@ -0,0 +1,184 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_list.h
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+#ifndef __MALI_OSK_LIST_H__
+#define __MALI_OSK_LIST_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+MALI_STATIC_INLINE void __mali_osk_list_add(_mali_osk_list_t *new_entry, _mali_osk_list_t *prev, _mali_osk_list_t *next)
+{
+    next->prev = new_entry;
+    new_entry->next = next;
+    new_entry->prev = prev;
+    prev->next = new_entry;
+}
+
+MALI_STATIC_INLINE void __mali_osk_list_del(_mali_osk_list_t *prev, _mali_osk_list_t *next)
+{
+    next->prev = prev;
+    prev->next = next;
+}
+
+/** @addtogroup _mali_osk_list
+ * @{ */
+
+/** Reference implementations of Doubly-linked Circular Lists are provided.
+ * There is often no need to re-implement these.
+ *
+ * @note The implementation may differ subtly from any lists the OS provides.
+ * For this reason, these lists should not be mixed with OS-specific lists
+ * inside the OSK/UKK implementation. */
+
+/** @brief Initialize a list element.
+ *
+ * All list elements must be initialized before use.
+ *
+ * Do not use on any list element that is present in a list without using
+ * _mali_osk_list_del first, otherwise this will break the list.
+ *
+ * @param list the list element to initialize
+ */
+MALI_STATIC_INLINE void _mali_osk_list_init( _mali_osk_list_t *list )
+{
+    list->next = list;
+    list->prev = list;
+}
+
+/** @brief Insert a single list element after an entry in a list
+ *
+ * As an example, if this is inserted to the head of a list, then this becomes
+ * the first element of the list.
+ *
+ * Do not use to move list elements from one list to another, as it will break
+ * the originating list.
+ *
+ *
+ * @param newlist the list element to insert
+ * @param list the list in which to insert. The new element will be the next
+ * entry in this list
+ */
+MALI_STATIC_INLINE void _mali_osk_list_add( _mali_osk_list_t *new_entry, _mali_osk_list_t *list )
+{
+    __mali_osk_list_add(new_entry, list, list->next);
+}
+
+/** @brief Insert a single list element before an entry in a list
+ *
+ * As an example, if this is inserted to the head of a list, then this becomes
+ * the last element of the list.
+ *
+ * Do not use to move list elements from one list to another, as it will break
+ * the originating list.
+ *
+ * @param newlist the list element to insert
+ * @param list the list in which to insert. The new element will be the previous
+ * entry in this list
+ */
+MALI_STATIC_INLINE void _mali_osk_list_addtail( _mali_osk_list_t *new_entry, _mali_osk_list_t *list )
+{
+    __mali_osk_list_add(new_entry, list->prev, list);
+}
+
+/** @brief Remove a single element from a list
+ *
+ * The element will no longer be present in the list. The removed list element
+ * will be uninitialized, and so should not be traversed. It must be
+ * initialized before further use.
+ *
+ * @param list the list element to remove.
+ */
+MALI_STATIC_INLINE void _mali_osk_list_del( _mali_osk_list_t *list )
+{
+    __mali_osk_list_del(list->prev, list->next);
+}
+
+/** @brief Remove a single element from a list, and re-initialize it
+ *
+ * The element will no longer be present in the list. The removed list element
+ * will initialized, and so can be used as normal.
+ *
+ * @param list the list element to remove and initialize.
+ */
+MALI_STATIC_INLINE void _mali_osk_list_delinit( _mali_osk_list_t *list )
+{
+    __mali_osk_list_del(list->prev, list->next);
+    _mali_osk_list_init(list);
+}
+
+/** @brief Determine whether a list is empty.
+ *
+ * An empty list is one that contains a single element that points to itself.
+ *
+ * @param list the list to check.
+ * @return non-zero if the list is empty, and zero otherwise.
+ */
+MALI_STATIC_INLINE int _mali_osk_list_empty( _mali_osk_list_t *list )
+{
+    return list->next == list;
+}
+
+/** @brief Move a list element from one list to another.
+ *
+ * The list element must be initialized.
+ *
+ * As an example, moving a list item to the head of a new list causes this item
+ * to be the first element in the new list.
+ *
+ * @param move the list element to move
+ * @param list the new list into which the element will be inserted, as the next
+ * element in the list.
+ */
+MALI_STATIC_INLINE void _mali_osk_list_move( _mali_osk_list_t *move_entry, _mali_osk_list_t *list )
+{
+    __mali_osk_list_del(move_entry->prev, move_entry->next);
+    _mali_osk_list_add(move_entry, list);
+}
+
+/** @brief Join two lists
+ *
+ * The list element must be initialized.
+ *
+ * Allows you to join a list into another list at a specific location
+ *
+ * @param list the new list to add
+ * @param at the location in a list to add the new list into
+ */
+MALI_STATIC_INLINE void _mali_osk_list_splice( _mali_osk_list_t *list, _mali_osk_list_t *at )
+{
+    if (!_mali_osk_list_empty(list))
+    {
+        /* insert all items from 'list' after 'at'  */
+        _mali_osk_list_t *first = list->next;
+        _mali_osk_list_t *last = list->prev;
+        _mali_osk_list_t *split = at->next;
+
+        first->prev = at;
+        at->next = first;
+
+        last->next  = split;
+        split->prev = last;
+    }
+}
+/** @} */ /* end group _mali_osk_list */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_OSK_LIST_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk_mali.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk_mali.h
new file mode 100644
index 00000000000..9a046fb91eb
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_osk_mali.h
@@ -0,0 +1,252 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_mali.h
+ * Defines the OS abstraction layer which is specific for the Mali kernel device driver (OSK)
+ */
+
+#ifndef __MALI_OSK_MALI_H__
+#define __MALI_OSK_MALI_H__
+
+#include <mali_osk.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/** @addtogroup _mali_osk_miscellaneous
+ * @{ */
+
+/** @brief Initialize the OSK layer
+ *
+ * This function is used to setup any initialization of OSK functionality, if
+ * required.
+ *
+ * This must be the first function called from the common code, specifically,
+ * from the common code entry-point, mali_kernel_constructor.
+ *
+ * The OS-integration into the OS's kernel must handle calling of
+ * mali_kernel_constructor when the device driver is loaded.
+ *
+ * @return _MALI_OSK_ERR_OK on success, or a suitable _mali_osk_errcode_t on
+ * failure.
+ */
+_mali_osk_errcode_t _mali_osk_init( void );
+
+/** @brief Terminate the OSK layer
+ *
+ * This function is used to terminate any resources initialized by
+ * _mali_osk_init.
+ *
+ * This must be the last function called from the common code, specifically,
+ * from the common code closedown function, mali_kernel_destructor, and the
+ * error path in mali_kernel_constructor.
+ *
+ * The OS-integration into the OS's kernel must handle calling of
+ * mali_kernel_destructor when the device driver is terminated.
+ */
+void _mali_osk_term( void );
+
+/** @brief Read the Mali Resource configuration
+ *
+ * Populates a _mali_arch_resource_t array from configuration settings, which
+ * are stored in an OS-specific way.
+ *
+ * For example, these may be compiled in to a static structure, or read from
+ * the filesystem at startup.
+ *
+ * On failure, do not call _mali_osk_resources_term.
+ *
+ * @param arch_config a pointer to the store the pointer to the resources
+ * @param num_resources the number of resources read
+ * @return _MALI_OSK_ERR_OK on success. _MALI_OSK_ERR_NOMEM on allocation
+ * error. For other failures, a suitable _mali_osk_errcode_t is returned.
+ */
+_mali_osk_errcode_t _mali_osk_resources_init( _mali_osk_resource_t **arch_config, u32 *num_resources );
+
+/** @brief Free resources allocated by _mali_osk_resources_init.
+ *
+ * Frees the _mali_arch_resource_t array allocated by _mali_osk_resources_init
+ *
+ * @param arch_config a pointer to the stored the pointer to the resources
+ * @param num_resources the number of resources in the array
+ */
+void _mali_osk_resources_term( _mali_osk_resource_t **arch_config, u32 num_resources);
+/** @} */ /* end group _mali_osk_miscellaneous */
+
+/** @addtogroup _mali_osk_low_level_memory
+ * @{ */
+
+/** @brief Initialize a user-space accessible memory range
+ *
+ * This initializes a virtual address range such that it is reserved for the
+ * current process, but does not map any physical pages into this range.
+ *
+ * This function may initialize or adjust any members of the
+ * mali_memory_allocation \a descriptor supplied, before the physical pages are
+ * mapped in with _mali_osk_mem_mapregion_map().
+ *
+ * The function will always be called with MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE
+ * set in \a descriptor->flags. It is an error to call this function without
+ * setting this flag. Otherwise, \a descriptor->flags bits are reserved for
+ * future expansion
+ *
+ * The \a descriptor's process_addr_mapping_info member can be modified to
+ * allocate OS-specific information. Note that on input, this will be a
+ * ukk_private word from the U/K inteface, as inserted by _mali_ukk_mem_mmap().
+ * This is used to pass information from the U/K interface to the OSK interface,
+ * if necessary. The precise usage of the process_addr_mapping_info member
+ * depends on the U/K implementation of _mali_ukk_mem_mmap().
+ *
+ * Therefore, the U/K implementation of _mali_ukk_mem_mmap() and the OSK
+ * implementation of  _mali_osk_mem_mapregion_init() must agree on the meaning and
+ * usage of the ukk_private word and process_addr_mapping_info member.
+ *
+ * Refer to \ref u_k_api for more information on the U/K interface.
+ *
+ * On successful return, \a descriptor's mapping member will be correct for
+ * use with _mali_osk_mem_mapregion_term() and _mali_osk_mem_mapregion_map().
+ *
+ * @param descriptor the mali_memory_allocation to initialize.
+ */
+_mali_osk_errcode_t _mali_osk_mem_mapregion_init( mali_memory_allocation * descriptor );
+
+/** @brief Terminate a user-space accessible memory range
+ *
+ * This terminates a virtual address range reserved in the current user process,
+ * where none, some or all of the virtual address ranges have mappings to
+ * physical pages.
+ *
+ * It will unmap any physical pages that had been mapped into a reserved
+ * virtual address range for the current process, and then releases the virtual
+ * address range. Any extra book-keeping information or resources allocated
+ * during _mali_osk_mem_mapregion_init() will also be released.
+ *
+ * The \a descriptor itself is not freed - this must be handled by the caller of
+ * _mali_osk_mem_mapregion_term().
+ *
+ * The function will always be called with MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE
+ * set in descriptor->flags. It is an error to call this function without
+ * setting this flag. Otherwise, descriptor->flags bits are reserved for
+ * future expansion
+ *
+ * @param descriptor the mali_memory_allocation to terminate.
+ */
+void _mali_osk_mem_mapregion_term( mali_memory_allocation * descriptor );
+
+/** @brief Map physical pages into a user process's virtual address range
+ *
+ * This is used to map a number of physically contigous pages into a
+ * user-process's virtual address range, which was previously reserved by a
+ * call to _mali_osk_mem_mapregion_init().
+ *
+ * This need not provide a mapping for the entire virtual address range
+ * reserved for \a descriptor - it may be used to map single pages per call.
+ *
+ * The function will always be called with MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE
+ * set in \a descriptor->flags. It is an error to call this function without
+ * setting this flag. Otherwise, \a descriptor->flags bits are reserved for
+ * future expansion
+ *
+ * The function may supply \a *phys_addr == \ref MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC.
+ * In this case, \a size must be set to \ref _MALI_OSK_CPU_PAGE_SIZE, and the function
+ * will allocate the physical page itself. The physical address of the
+ * allocated page will be returned through \a phys_addr.
+ *
+ * It is an error to set \a size != \ref _MALI_OSK_CPU_PAGE_SIZE while
+ * \a *phys_addr == \ref MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC,
+ * since it is not always possible for OSs to support such a setting through this
+ * interface.
+ *
+ * @note \b IMPORTANT: This code must validate the input parameters. If the
+ * range defined by \a offset and \a size is outside the range allocated in
+ * \a descriptor, then this function \b MUST not attempt any mapping, and must
+ * instead return a suitable \ref _mali_osk_errcode_t \b failure code.
+ *
+ * @param[in,out] descriptor the mali_memory_allocation representing the
+ * user-process's virtual address range to map into.
+ *
+ * @param[in] offset the offset into the virtual address range. This is only added
+ * to the mapping member of the \a descriptor, and not the \a phys_addr parameter.
+ * It must be a multiple of \ref _MALI_OSK_CPU_PAGE_SIZE.
+ *
+ * @param[in,out] phys_addr a pointer to the physical base address to begin the
+ * mapping from. If \a size == \ref _MALI_OSK_CPU_PAGE_SIZE and
+ * \a *phys_addr == \ref MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC, then this
+ * function will allocate the physical page itself, and return the
+ * physical address of the page through \a phys_addr, which will be aligned to
+ * \ref _MALI_OSK_CPU_PAGE_SIZE. Otherwise, \a *phys_addr must be aligned to
+ * \ref _MALI_OSK_CPU_PAGE_SIZE, and is unmodified after the call.
+ * \a phys_addr is unaffected by the \a offset parameter.
+ *
+ * @param[in] size the number of bytes to map in. This must be a multiple of
+ * \ref _MALI_OSK_CPU_PAGE_SIZE.
+ *
+ * @return _MALI_OSK_ERR_OK on sucess, otherwise a _mali_osk_errcode_t value
+ * on failure
+ *
+ * @note could expand to use _mali_osk_mem_mapregion_flags_t instead of
+ * \ref MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC, but note that we must
+ * also modify the mali process address manager in the mmu/memory engine code.
+ */
+_mali_osk_errcode_t _mali_osk_mem_mapregion_map( mali_memory_allocation * descriptor, u32 offset, u32 *phys_addr, u32 size );
+
+
+/** @brief Unmap physical pages from a user process's virtual address range
+ *
+ * This is used to unmap a number of physically contigous pages from a
+ * user-process's virtual address range, which were previously mapped by a
+ * call to _mali_osk_mem_mapregion_map(). If the range specified was allocated
+ * from OS memory, then that memory will be returned to the OS. Whilst pages
+ * will be mapped out, the Virtual address range remains reserved, and at the
+ * same base address.
+ *
+ * When this function is used to unmap pages from OS memory
+ * (_mali_osk_mem_mapregion_map() was called with *phys_addr ==
+ * \ref MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC), then the \a flags must
+ * include \ref _MALI_OSK_MEM_MAPREGION_FLAG_OS_ALLOCATED_PHYSADDR. This is because
+ * it is not always easy for an OS implementation to discover whether the
+ * memory was OS allocated or not (and so, how it should release the memory).
+ *
+ * For this reason, only a range of pages of the same allocation type (all OS
+ * allocated, or none OS allocacted) may be unmapped in one call. Multiple
+ * calls must be made if allocations of these different types exist across the
+ * entire region described by the \a descriptor.
+ *
+ * The function will always be called with MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE
+ * set in \a descriptor->flags. It is an error to call this function without
+ * setting this flag. Otherwise, \a descriptor->flags bits are reserved for
+ * future expansion
+ *
+ * @param[in,out] descriptor the mali_memory_allocation representing the
+ * user-process's virtual address range to map into.
+ *
+ * @param[in] offset the offset into the virtual address range. This is only added
+ * to the mapping member of the \a descriptor. \a offset must be a multiple of
+ * \ref _MALI_OSK_CPU_PAGE_SIZE.
+ *
+ * @param[in] size the number of bytes to unmap. This must be a multiple of
+ * \ref _MALI_OSK_CPU_PAGE_SIZE.
+ *
+ * @param[in] flags specifies how the memory should be unmapped. For a range
+ * of pages that were originally OS allocated, this must have
+ * \ref _MALI_OSK_MEM_MAPREGION_FLAG_OS_ALLOCATED_PHYSADDR set.
+ */
+void _mali_osk_mem_mapregion_unmap( mali_memory_allocation * descriptor, u32 offset, u32 size, _mali_osk_mem_mapregion_flags_t flags );
+/** @} */ /* end group _mali_osk_low_level_memory */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_OSK_MALI_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_uk_types.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_uk_types.h
new file mode 100644
index 00000000000..9a1583658c9
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_uk_types.h
@@ -0,0 +1,1148 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_uk_types.h
+ * Defines the types and constants used in the user-kernel interface
+ */
+
+#ifndef __MALI_UK_TYPES_H__
+#define __MALI_UK_TYPES_H__
+
+/*
+ * NOTE: Because this file can be included from user-side and kernel-side,
+ * it is up to the includee to ensure certain typedefs (e.g. u32) are already
+ * defined when #including this.
+ */
+#include "regs/mali_200_regs.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @addtogroup uddapi Unified Device Driver (UDD) APIs
+ *
+ * @{
+ */
+
+/**
+ * @addtogroup u_k_api UDD User/Kernel Interface (U/K) APIs
+ *
+ * @{
+ */
+
+/** @defgroup _mali_uk_core U/K Core
+ * @{ */
+
+/** Definition of subsystem numbers, to assist in creating a unique identifier
+ * for each U/K call.
+ *
+ * @see _mali_uk_functions */
+typedef enum
+{
+    _MALI_UK_CORE_SUBSYSTEM,      /**< Core Group of U/K calls */
+    _MALI_UK_MEMORY_SUBSYSTEM,    /**< Memory Group of U/K calls */
+    _MALI_UK_PP_SUBSYSTEM,        /**< Fragment Processor Group of U/K calls */
+    _MALI_UK_GP_SUBSYSTEM,        /**< Vertex Processor Group of U/K calls */
+	_MALI_UK_PROFILING_SUBSYSTEM, /**< Profiling Group of U/K calls */
+    _MALI_UK_PMM_SUBSYSTEM,       /**< Power Management Module Group of U/K calls */
+	_MALI_UK_VSYNC_SUBSYSTEM,     /**< VSYNC Group of U/K calls */
+} _mali_uk_subsystem_t;
+
+/** Within a function group each function has its unique sequence number
+ * to assist in creating a unique identifier for each U/K call.
+ *
+ * An ordered pair of numbers selected from
+ * ( \ref _mali_uk_subsystem_t,\ref  _mali_uk_functions) will uniquely identify the
+ * U/K call across all groups of functions, and all functions. */
+typedef enum
+{
+	/** Core functions */
+
+    _MALI_UK_OPEN                    = 0, /**< _mali_ukk_open() */
+    _MALI_UK_CLOSE,                       /**< _mali_ukk_close() */
+    _MALI_UK_GET_SYSTEM_INFO_SIZE,        /**< _mali_ukk_get_system_info_size() */
+    _MALI_UK_GET_SYSTEM_INFO,             /**< _mali_ukk_get_system_info() */
+    _MALI_UK_WAIT_FOR_NOTIFICATION,       /**< _mali_ukk_wait_for_notification() */
+    _MALI_UK_GET_API_VERSION,             /**< _mali_ukk_get_api_version() */
+    _MALI_UK_POST_NOTIFICATION,           /**< _mali_ukk_post_notification() */
+
+	/** Memory functions */
+
+    _MALI_UK_INIT_MEM                = 0, /**< _mali_ukk_init_mem() */
+    _MALI_UK_TERM_MEM,                    /**< _mali_ukk_term_mem() */
+    _MALI_UK_GET_BIG_BLOCK,               /**< _mali_ukk_get_big_block() */
+    _MALI_UK_FREE_BIG_BLOCK,              /**< _mali_ukk_free_big_block() */
+    _MALI_UK_MAP_MEM,                     /**< _mali_ukk_mem_mmap() */
+    _MALI_UK_UNMAP_MEM,                   /**< _mali_ukk_mem_munmap() */
+    _MALI_UK_QUERY_MMU_PAGE_TABLE_DUMP_SIZE, /**< _mali_ukk_mem_get_mmu_page_table_dump_size() */
+    _MALI_UK_DUMP_MMU_PAGE_TABLE,         /**< _mali_ukk_mem_dump_mmu_page_table() */
+    _MALI_UK_ATTACH_UMP_MEM,             /**< _mali_ukk_attach_ump_mem() */
+    _MALI_UK_RELEASE_UMP_MEM,           /**< _mali_ukk_release_ump_mem() */
+    _MALI_UK_MAP_EXT_MEM,                 /**< _mali_uku_map_external_mem() */
+    _MALI_UK_UNMAP_EXT_MEM,               /**< _mali_uku_unmap_external_mem() */
+    _MALI_UK_VA_TO_MALI_PA,               /**< _mali_uku_va_to_mali_pa() */
+
+    /** Common functions for each core */
+
+    _MALI_UK_START_JOB           = 0,     /**< Start a Fragment/Vertex Processor Job on a core */
+	_MALI_UK_ABORT_JOB,                   /**< Abort a job */
+    _MALI_UK_GET_NUMBER_OF_CORES,         /**< Get the number of Fragment/Vertex Processor cores */
+    _MALI_UK_GET_CORE_VERSION,            /**< Get the Fragment/Vertex Processor version compatible with all cores */
+
+    /** Fragment Processor Functions  */
+
+    _MALI_UK_PP_START_JOB            = _MALI_UK_START_JOB,            /**< _mali_ukk_pp_start_job() */
+    _MALI_UK_PP_ABORT_JOB            = _MALI_UK_ABORT_JOB,            /**< _mali_ukk_pp_abort_job() */
+    _MALI_UK_GET_PP_NUMBER_OF_CORES  = _MALI_UK_GET_NUMBER_OF_CORES,  /**< _mali_ukk_get_pp_number_of_cores() */
+    _MALI_UK_GET_PP_CORE_VERSION     = _MALI_UK_GET_CORE_VERSION,     /**< _mali_ukk_get_pp_core_version() */
+
+    /** Vertex Processor Functions  */
+
+    _MALI_UK_GP_START_JOB            = _MALI_UK_START_JOB,            /**< _mali_ukk_gp_start_job() */
+    _MALI_UK_GP_ABORT_JOB            = _MALI_UK_ABORT_JOB,            /**< _mali_ukk_gp_abort_job() */
+    _MALI_UK_GET_GP_NUMBER_OF_CORES  = _MALI_UK_GET_NUMBER_OF_CORES,  /**< _mali_ukk_get_gp_number_of_cores() */
+    _MALI_UK_GET_GP_CORE_VERSION     = _MALI_UK_GET_CORE_VERSION,     /**< _mali_ukk_get_gp_core_version() */
+    _MALI_UK_GP_SUSPEND_RESPONSE,                                     /**< _mali_ukk_gp_suspend_response() */
+
+	/** Profiling functions */
+
+	_MALI_UK_PROFILING_START         = 0, /**< __mali_uku_profiling_start() */
+	_MALI_UK_PROFILING_ADD_EVENT,         /**< __mali_uku_profiling_add_event() */
+	_MALI_UK_PROFILING_STOP,              /**< __mali_uku_profiling_stop() */
+	_MALI_UK_PROFILING_GET_EVENT,         /**< __mali_uku_profiling_get_event() */
+	_MALI_UK_PROFILING_CLEAR,             /**< __mali_uku_profiling_clear() */
+
+#if USING_MALI_PMM
+    /** Power Management Module Functions */
+    _MALI_UK_PMM_EVENT_MESSAGE = 0,       /**< Raise an event message */
+#endif
+
+	/** VSYNC reporting fuctions */
+	_MALI_UK_VSYNC_EVENT_REPORT      = 0, /**< _mali_ukk_vsync_event_report() */
+
+} _mali_uk_functions;
+
+/** @brief Get the size necessary for system info
+ *
+ * @see _mali_ukk_get_system_info_size()
+ */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 size;                       /**< [out] size of buffer necessary to hold system information data, in bytes */
+} _mali_uk_get_system_info_size_s;
+
+
+/** @defgroup _mali_uk_getsysteminfo U/K Get System Info
+ * @{ */
+
+/**
+ * Type definition for the core version number.
+ * Used when returning the version number read from a core
+ *
+ * Its format is that of the 32-bit Version register for a particular core.
+ * Refer to the "Mali200 and MaliGP2 3D Graphics Processor Technical Reference
+ * Manual", ARM DDI 0415C, for more information.
+ */
+typedef u32 _mali_core_version;
+
+/**
+ * Enum values for the different modes the driver can be put in.
+ * Normal is the default mode. The driver then uses a job queue and takes job objects from the clients.
+ * Job completion is reported using the _mali_ukk_wait_for_notification call.
+ * The driver blocks this io command until a job has completed or failed or a timeout occurs.
+ *
+ * The 'raw' mode is reserved for future expansion.
+ */
+typedef enum _mali_driver_mode
+{
+	_MALI_DRIVER_MODE_RAW = 1,    /**< Reserved for future expansion */
+	_MALI_DRIVER_MODE_NORMAL = 2  /**< Normal mode of operation */
+} _mali_driver_mode;
+
+/** @brief List of possible cores
+ *
+ * add new entries to the end of this enum */
+typedef enum _mali_core_type
+{
+	_MALI_GP2 = 2,                /**< MaliGP2 Programmable Vertex Processor */
+	_MALI_200 = 5,                /**< Mali200 Programmable Fragment Processor */
+	_MALI_400_GP = 6,             /**< Mali400 Programmable Vertex Processor */
+	_MALI_400_PP = 7,             /**< Mali400 Programmable Fragment Processor */
+	/* insert new core here, do NOT alter the existing values */
+} _mali_core_type;
+
+/** @brief Information about each Mali Core
+ *
+ * Information is stored in a linked list, which is stored entirely in the
+ * buffer pointed to by the system_info member of the
+ * _mali_uk_get_system_info_s arguments provided to _mali_ukk_get_system_info()
+ *
+ * Both Fragment Processor (PP) and Vertex Processor (GP) cores are represented
+ * by this struct.
+ *
+ * The type is reported by the type field, _mali_core_info::_mali_core_type.
+ *
+ * Each core is given a unique Sequence number identifying it, the core_nr
+ * member.
+ *
+ * Flags are taken directly from the resource's flags, and are currently unused.
+ *
+ * Multiple mali_core_info structs are linked in a single linked list using the next field
+ */
+typedef struct _mali_core_info
+{
+	_mali_core_type type;            /**< Type of core */
+	_mali_core_version version;      /**< Core Version, as reported by the Core's Version Register */
+	u32 reg_address;                 /**< Address of Registers */
+	u32 core_nr;                     /**< Sequence number */
+	u32 flags;                       /**< Flags. Currently Unused. */
+	struct _mali_core_info * next;   /**< Next core in Linked List */
+} _mali_core_info;
+
+/** @brief Capabilities of Memory Banks
+ *
+ * These may be used to restrict memory banks for certain uses. They may be
+ * used when access is not possible (e.g. Bus does not support access to it)
+ * or when access is possible but not desired (e.g. Access is slow).
+ *
+ * In the case of 'possible but not desired', there is no way of specifying
+ * the flags as an optimization hint, so that the memory could be used as a
+ * last resort.
+ *
+ * @see _mali_mem_info
+ */
+typedef enum _mali_bus_usage
+{
+
+	_MALI_PP_READABLE   = (1<<0),  /** Readable by the Fragment Processor */
+	_MALI_PP_WRITEABLE  = (1<<1),  /** Writeable by the Fragment Processor */
+	_MALI_GP_READABLE   = (1<<2),  /** Readable by the Vertex Processor */
+	_MALI_GP_WRITEABLE  = (1<<3),  /** Writeable by the Vertex Processor */
+	_MALI_CPU_READABLE  = (1<<4),  /** Readable by the CPU */
+	_MALI_CPU_WRITEABLE = (1<<5),  /** Writeable by the CPU */
+	_MALI_MMU_READABLE  = _MALI_PP_READABLE | _MALI_GP_READABLE,   /** Readable by the MMU (including all cores behind it) */
+	_MALI_MMU_WRITEABLE = _MALI_PP_WRITEABLE | _MALI_GP_WRITEABLE, /** Writeable by the MMU (including all cores behind it) */
+} _mali_bus_usage;
+
+/** @brief Information about the Mali Memory system
+ *
+ * Information is stored in a linked list, which is stored entirely in the
+ * buffer pointed to by the system_info member of the
+ * _mali_uk_get_system_info_s arguments provided to _mali_ukk_get_system_info()
+ *
+ * Each element of the linked list describes a single Mali Memory bank.
+ * Each allocation can only come from one bank, and will not cross multiple
+ * banks.
+ *
+ * Each bank is uniquely identified by its identifier member. On Mali-nonMMU
+ * systems, to allocate from this bank, the value of identifier must be passed
+ * as the type_id member of the  _mali_uk_get_big_block_s arguments to
+ * _mali_ukk_get_big_block.
+ *
+ * On Mali-MMU systems, there is only one bank, which describes the maximum
+ * possible address range that could be allocated (which may be much less than
+ * the available physical memory)
+ *
+ * The flags member describes the capabilities of the memory. It is an error
+ * to attempt to build a job for a particular core (PP or GP) when the memory
+ * regions used do not have the capabilities for supporting that core. This
+ * would result in a job abort from the Device Driver.
+ *
+ * For example, it is correct to build a PP job where read-only data structures
+ * are taken from a memory with _MALI_PP_READABLE set and
+ * _MALI_PP_WRITEABLE clear, and a framebuffer with  _MALI_PP_WRITEABLE set and
+ * _MALI_PP_READABLE clear. However, it would be incorrect to use a framebuffer
+ * where _MALI_PP_WRITEABLE is clear.
+ */
+typedef struct _mali_mem_info
+{
+	u32 size;                     /**< Size of the memory bank in bytes */
+	_mali_bus_usage flags;        /**< Capabilitiy flags of the memory */
+	u32 maximum_order_supported;  /**< log2 supported size */
+	u32 identifier;               /**< Unique identifier, to be used in allocate calls */
+	struct _mali_mem_info * next; /**< Next List Link */
+} _mali_mem_info;
+
+/** @brief Info about the whole Mali system.
+ *
+ * This Contains a linked list of the cores and memory banks available. Each
+ * list pointer will remain inside the system_info buffer supplied in the
+ * _mali_uk_get_system_info_s arguments to a _mali_ukk_get_system_info call.
+ *
+ * The has_mmu member must be inspected to ensure the correct group of
+ * Memory function calls is obtained - that is, those for either Mali-MMU
+ * or Mali-nonMMU. @see _mali_uk_memory
+ */
+typedef struct _mali_system_info
+{
+	_mali_core_info * core_info;  /**< List of _mali_core_info structures */
+	_mali_mem_info * mem_info;    /**< List of _mali_mem_info structures */
+	u32 has_mmu;                  /**< Non-zero if Mali-MMU present. Zero otherwise. */
+	_mali_driver_mode drivermode; /**< Reserved. Must always be _MALI_DRIVER_MODE_NORMAL */
+} _mali_system_info;
+
+/** @brief Arguments to _mali_ukk_get_system_info()
+ *
+ * A buffer of the size returned by _mali_ukk_get_system_info_size() must be
+ * allocated, and the pointer to this buffer must be written into the
+ * system_info member. The buffer must be suitably aligned for storage of
+ * the _mali_system_info structure - for example, one returned by
+ * _mali_osk_malloc(), which will be suitably aligned for any structure.
+ *
+ * The ukk_private member must be set to zero by the user-side. Under an OS
+ * implementation, the U/K interface must write in the user-side base address
+ * into the ukk_private member, so that the common code in
+ * _mali_ukk_get_system_info() can determine how to adjust the pointers such
+ * that they are sensible from user space. Leaving ukk_private as NULL implies
+ * that no pointer adjustment is necessary - which will be the case on a
+ * bare-metal/RTOS system.
+ *
+ * @see _mali_system_info
+ */
+typedef struct
+{
+    void *ctx;                              /**< [in,out] user-kernel context (trashed on output) */
+	u32 size;                               /**< [in] size of buffer provided to store system information data */
+	_mali_system_info * system_info;        /**< [in,out] pointer to buffer to store system information data. No initialisation of buffer required on input. */
+	u32 ukk_private;                        /**< [in] Kernel-side private word inserted by certain U/K interface implementations. Caller must set to Zero. */
+} _mali_uk_get_system_info_s;
+/** @} */ /* end group _mali_uk_getsysteminfo */
+
+/** @} */ /* end group _mali_uk_core */
+
+
+/** @defgroup _mali_uk_gp U/K Vertex Processor
+ * @{ */
+
+/** @defgroup _mali_uk_gp_suspend_response_s Vertex Processor Suspend Response
+ * @{ */
+
+/** @brief Arguments for _mali_ukk_gp_suspend_response()
+ *
+ * When _mali_wait_for_notification() receives notification that a
+ * Vertex Processor job was suspended, you need to send a response to indicate
+ * what needs to happen with this job. You can either abort or resume the job.
+ *
+ * - set @c code to indicate response code. This is either @c _MALIGP_JOB_ABORT or
+ * @c _MALIGP_JOB_RESUME_WITH_NEW_HEAP to indicate you will provide a new heap
+ * for the job that will resolve the out of memory condition for the job.
+ * - copy the @c cookie value from the @c _mali_uk_gp_job_suspended_s notification;
+ * this is an identifier for the suspended job
+ * - set @c arguments[0] and @c arguments[1] to zero if you abort the job. If
+ * you resume it, @c argument[0] should specify the Mali start address for the new
+ * heap and @c argument[1] the Mali end address of the heap.
+ * - pass in the user-kernel context @c ctx that was returned from _mali_ukk_open()
+ *
+ */
+typedef enum _maligp_job_suspended_response_code
+{
+	_MALIGP_JOB_ABORT,                  /**< Abort the Vertex Processor job */
+	_MALIGP_JOB_RESUME_WITH_NEW_HEAP    /**< Resume the Vertex Processor job with a new heap */
+} _maligp_job_suspended_response_code;
+
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 cookie;                     /**< [in] cookie from the _mali_uk_gp_job_suspended_s notification */
+	_maligp_job_suspended_response_code code; /**< [in] abort or resume response code, see \ref _maligp_job_suspended_response_code */
+	u32 arguments[2];               /**< [in] 0 when aborting a job. When resuming a job, the Mali start and end address for a new heap to resume the job with */
+} _mali_uk_gp_suspend_response_s;
+
+/** @} */ /* end group _mali_uk_gp_suspend_response_s */
+
+/** @defgroup _mali_uk_gpstartjob_s Vertex Processor Start Job
+ * @{ */
+
+/** @brief Status indicating the result of starting a Vertex or Fragment processor job */
+typedef enum
+{
+    _MALI_UK_START_JOB_STARTED,                         /**< Job started */
+    _MALI_UK_START_JOB_STARTED_LOW_PRI_JOB_RETURNED,    /**< Job started and bumped a lower priority job that was pending execution */
+    _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE           /**< Job could not be started at this time. Try starting the job again */
+} _mali_uk_start_job_status;
+
+/** @brief Status indicating the result of the execution of a Vertex or Fragment processor job  */
+
+typedef enum
+{
+	_MALI_UK_JOB_STATUS_END_SUCCESS         = 1<<(16+0),
+	_MALI_UK_JOB_STATUS_END_OOM             = 1<<(16+1),
+	_MALI_UK_JOB_STATUS_END_ABORT           = 1<<(16+2),
+	_MALI_UK_JOB_STATUS_END_TIMEOUT_SW      = 1<<(16+3),
+	_MALI_UK_JOB_STATUS_END_HANG            = 1<<(16+4),
+	_MALI_UK_JOB_STATUS_END_SEG_FAULT       = 1<<(16+5),
+	_MALI_UK_JOB_STATUS_END_ILLEGAL_JOB     = 1<<(16+6),
+	_MALI_UK_JOB_STATUS_END_UNKNOWN_ERR     = 1<<(16+7),
+	_MALI_UK_JOB_STATUS_END_SHUTDOWN        = 1<<(16+8),
+	_MALI_UK_JOB_STATUS_END_SYSTEM_UNUSABLE = 1<<(16+9)
+} _mali_uk_job_status;
+
+#define MALIGP2_NUM_REGS_FRAME (6)
+
+/** @brief Arguments for _mali_ukk_gp_start_job()
+ *
+ * To start a Vertex Processor job
+ * - associate the request with a reference to a @c mali_gp_job_info by setting
+ * user_job_ptr to the address of the @c mali_gp_job_info of the job.
+ * - set @c priority to the priority of the @c mali_gp_job_info
+ * - specify a timeout for the job by setting @c watchdog_msecs to the number of
+ * milliseconds the job is allowed to run. Specifying a value of 0 selects the
+ * default timeout in use by the device driver.
+ * - copy the frame registers from the @c mali_gp_job_info into @c frame_registers.
+ * - set the @c perf_counter_flag, @c perf_counter_src0 and @c perf_counter_src1 to zero
+ * for a non-instrumented build. For an instrumented build you can use up
+ * to two performance counters. Set the corresponding bit in @c perf_counter_flag
+ * to enable them. @c perf_counter_src0 and @c perf_counter_src1 specify
+ * the source of what needs to get counted (e.g. number of vertex loader
+ * cache hits). For source id values, see ARM DDI0415A, Table 3-60.
+ * - pass in the user-kernel context @c ctx that was returned from _mali_ukk_open()
+ *
+ * When @c _mali_ukk_gp_start_job() returns @c _MALI_OSK_ERR_OK, status contains the
+ * result of the request (see \ref _mali_uk_start_job_status). If the job could
+ * not get started (@c _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE) it should be
+ * tried again. If the job had a higher priority than the one currently pending
+ * execution (@c _MALI_UK_START_JOB_STARTED_LOW_PRI_JOB_RETURNED), it will bump
+ * the lower priority job and returns the address of the @c mali_gp_job_info
+ * for that job in @c returned_user_job_ptr. That job should get requeued.
+ *
+ * After the job has started, @c _mali_wait_for_notification() will be notified
+ * that the job finished or got suspended. It may get suspended due to
+ * resource shortage. If it finished (see _mali_ukk_wait_for_notification())
+ * the notification will contain a @c _mali_uk_gp_job_finished_s result. If
+ * it got suspended the notification will contain a @c _mali_uk_gp_job_suspended_s
+ * result.
+ *
+ * The @c _mali_uk_gp_job_finished_s contains the job status (see \ref _mali_uk_job_status),
+ * the number of milliseconds the job took to render, and values of core registers
+ * when the job finished (irq status, performance counters, renderer list
+ * address). A job has finished succesfully when its status is
+ * @c _MALI_UK_JOB_STATUS_FINISHED. If the hardware detected a timeout while rendering
+ * the job, or software detected the job is taking more than watchdog_msecs to
+ * complete, the status will indicate @c _MALI_UK_JOB_STATUS_HANG.
+ * If the hardware detected a bus error while accessing memory associated with the
+ * job, status will indicate @c _MALI_UK_JOB_STATUS_SEG_FAULT.
+ * status will indicate @c _MALI_UK_JOB_STATUS_NOT_STARTED if the driver had to
+ * stop the job but the job didn't start on the hardware yet, e.g. when the
+ * driver shutdown.
+ *
+ * In case the job got suspended, @c _mali_uk_gp_job_suspended_s contains
+ * the @c user_job_ptr identifier used to start the job with, the @c reason
+ * why the job stalled (see \ref _maligp_job_suspended_reason) and a @c cookie
+ * to identify the core on which the job stalled.  This @c cookie will be needed
+ * when responding to this nofication by means of _mali_ukk_gp_suspend_response().
+ * (see _mali_ukk_gp_suspend_response()). The response is either to abort or
+ * resume the job. If the job got suspended due to an out of memory condition
+ * you may be able to resolve this by providing more memory and resuming the job.
+ *
+ */
+typedef struct
+{
+    void *ctx;                          /**< [in,out] user-kernel context (trashed on output) */
+    u32 user_job_ptr;                   /**< [in] identifier for the job in user space, a @c mali_gp_job_info* */
+    u32 priority;                       /**< [in] job priority. A lower number means higher priority */
+    u32 watchdog_msecs;                 /**< [in] maximum allowed runtime in milliseconds. The job gets killed if it runs longer than this. A value of 0 selects the default used by the device driver. */
+    u32 frame_registers[MALIGP2_NUM_REGS_FRAME]; /**< [in] core specific registers associated with this job */
+    u32 perf_counter_flag;              /**< [in] bitmask indicating which performance counters to enable, see \ref _MALI_PERFORMANCE_COUNTER_FLAG_SRC0_ENABLE and related macro definitions */
+    u32 perf_counter_src0;              /**< [in] source id for performance counter 0 (see ARM DDI0415A, Table 3-60) */
+    u32 perf_counter_src1;              /**< [in] source id for performance counter 1 (see ARM DDI0415A, Table 3-60) */
+    u32 returned_user_job_ptr;          /**< [out] identifier for the returned job in user space, a @c mali_gp_job_info* */
+    _mali_uk_start_job_status status;   /**< [out] indicates job start status (success, previous job returned, requeue) */
+	u32 abort_id;                       /**< [in] abort id of this job, used to identify this job for later abort requests */
+	u32 perf_counter_l2_src0;           /**< [in] soruce id for Mali-400 MP L2 cache performance counter 0 */
+	u32 perf_counter_l2_src1;           /**< [in] source id for Mali-400 MP L2 cache performance counter 1 */
+} _mali_uk_gp_start_job_s;
+
+#define _MALI_PERFORMANCE_COUNTER_FLAG_SRC0_ENABLE (1<<0) /**< Enable performance counter SRC0 for a job */
+#define _MALI_PERFORMANCE_COUNTER_FLAG_SRC1_ENABLE (1<<1) /**< Enable performance counter SRC1 for a job */
+#define _MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC0_ENABLE (1<<2) /**< Enable performance counter L2_SRC0 for a job */
+#define _MALI_PERFORMANCE_COUNTER_FLAG_L2_SRC1_ENABLE (1<<3) /**< Enable performance counter L2_SRC1 for a job */
+#define _MALI_PERFORMANCE_COUNTER_FLAG_L2_RESET       (1<<4) /**< Enable performance counter L2_RESET for a job */
+
+/** @} */ /* end group _mali_uk_gpstartjob_s */
+
+typedef struct
+{
+    u32 user_job_ptr;               /**< [out] identifier for the job in user space */
+    _mali_uk_job_status status;     /**< [out] status of finished job */
+    u32 irq_status;                 /**< [out] value of the GP interrupt rawstat register (see ARM DDI0415A) */
+    u32 status_reg_on_stop;         /**< [out] value of the GP control register */
+    u32 vscl_stop_addr;             /**< [out] value of the GP VLSCL start register */
+    u32 plbcl_stop_addr;            /**< [out] value of the GP PLBCL start register */
+    u32 heap_current_addr;          /**< [out] value of the GP PLB PL heap start address register */
+    u32 perf_counter_src0;          /**< [out] source id for performance counter 0 (see ARM DDI0415A, Table 3-60) */
+    u32 perf_counter_src1;          /**< [out] source id for performance counter 1 (see ARM DDI0415A, Table 3-60) */
+    u32 perf_counter0;              /**< [out] value of perfomance counter 0 (see ARM DDI0415A) */
+    u32 perf_counter1;              /**< [out] value of perfomance counter 1 (see ARM DDI0415A) */
+    u32 render_time;                /**< [out] number of milliseconds it took for the job to render */
+	u32 perf_counter_l2_src0;       /**< [out] soruce id for Mali-400 MP L2 cache performance counter 0 */
+	u32 perf_counter_l2_src1;       /**< [out] soruce id for Mali-400 MP L2 cache performance counter 1 */
+	u32 perf_counter_l2_val0;       /**< [out] Value of the Mali-400 MP L2 cache performance counter 0 */
+	u32 perf_counter_l2_val1;       /**< [out] Value of the Mali-400 MP L2 cache performance counter 1 */
+} _mali_uk_gp_job_finished_s;
+
+typedef enum _maligp_job_suspended_reason
+{
+	_MALIGP_JOB_SUSPENDED_OUT_OF_MEMORY  /**< Polygon list builder unit (PLBU) has run out of memory */
+} _maligp_job_suspended_reason;
+
+typedef struct
+{
+	u32 user_job_ptr;                    /**< [out] identifier for the job in user space */
+	_maligp_job_suspended_reason reason; /**< [out] reason why the job stalled */
+	u32 cookie;                          /**< [out] identifier for the core in kernel space on which the job stalled */
+} _mali_uk_gp_job_suspended_s;
+
+/** @} */ /* end group _mali_uk_gp */
+
+
+/** @defgroup _mali_uk_pp U/K Fragment Processor
+ * @{ */
+
+/** @defgroup _mali_uk_ppstartjob_s Fragment Processor Start Job
+ * @{ */
+
+/** @brief Arguments for _mali_ukk_pp_start_job()
+ *
+ * To start a Fragment Processor job
+ * - associate the request with a reference to a mali_pp_job by setting
+ * @c user_job_ptr to the address of the @c mali_pp_job of the job.
+ * - set @c priority to the priority of the mali_pp_job
+ * - specify a timeout for the job by setting @c watchdog_msecs to the number of
+ * milliseconds the job is allowed to run. Specifying a value of 0 selects the
+ * default timeout in use by the device driver.
+ * - copy the frame registers from the @c mali_pp_job into @c frame_registers.
+ * For MALI200 you also need to copy the write back 0,1 and 2 registers.
+ * - set the @c perf_counter_flag, @c perf_counter_src0 and @c perf_counter_src1 to zero
+ * for a non-instrumented build. For an instrumented build you can use up
+ * to two performance counters. Set the corresponding bit in @c perf_counter_flag
+ * to enable them. @c perf_counter_src0 and @c perf_counter_src1 specify
+ * the source of what needs to get counted (e.g. number of vertex loader
+ * cache hits). For source id values, see ARM DDI0415A, Table 3-60.
+ * - pass in the user-kernel context in @c ctx that was returned from _mali_ukk_open()
+ *
+ * When _mali_ukk_pp_start_job() returns @c _MALI_OSK_ERR_OK, @c status contains the
+ * result of the request (see \ref _mali_uk_start_job_status). If the job could
+ * not get started (@c _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE) it should be
+ * tried again. If the job had a higher priority than the one currently pending
+ * execution (@c _MALI_UK_START_JOB_STARTED_LOW_PRI_JOB_RETURNED), it will bump
+ * the lower priority job and returns the address of the @c mali_pp_job
+ * for that job in @c returned_user_job_ptr. That job should get requeued.
+ *
+ * After the job has started, _mali_wait_for_notification() will be notified
+ * when the job finished. The notification will contain a
+ * @c _mali_uk_pp_job_finished_s result. It contains the @c user_job_ptr
+ * identifier used to start the job with, the job @c status (see \ref _mali_uk_job_status),
+ * the number of milliseconds the job took to render, and values of core registers
+ * when the job finished (irq status, performance counters, renderer list
+ * address). A job has finished succesfully when its status is
+ * @c _MALI_UK_JOB_STATUS_FINISHED. If the hardware detected a timeout while rendering
+ * the job, or software detected the job is taking more than @c watchdog_msecs to
+ * complete, the status will indicate @c _MALI_UK_JOB_STATUS_HANG.
+ * If the hardware detected a bus error while accessing memory associated with the
+ * job, status will indicate @c _MALI_UK_JOB_STATUS_SEG_FAULT.
+ * status will indicate @c _MALI_UK_JOB_STATUS_NOT_STARTED if the driver had to
+ * stop the job but the job didn't start on the hardware yet, e.g. when the
+ * driver shutdown.
+ *
+ */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+    u32 user_job_ptr;               /**< [in] identifier for the job in user space */
+    u32 priority;                   /**< [in] job priority. A lower number means higher priority */
+    u32 watchdog_msecs;             /**< [in] maximum allowed runtime in milliseconds. The job gets killed if it runs longer than this. A value of 0 selects the default used by the device driver. */
+    u32 frame_registers[MALI200_NUM_REGS_FRAME]; /**< [in] core specific registers associated with this job, see ARM DDI0415A */
+    u32 wb0_registers[MALI200_NUM_REGS_WBx];
+    u32 wb1_registers[MALI200_NUM_REGS_WBx];
+    u32 wb2_registers[MALI200_NUM_REGS_WBx];
+    u32 perf_counter_flag;              /**< [in] bitmask indicating which performance counters to enable, see \ref _MALI_PERFORMANCE_COUNTER_FLAG_SRC0_ENABLE and related macro definitions */
+    u32 perf_counter_src0;              /**< [in] source id for performance counter 0 (see ARM DDI0415A, Table 3-60) */
+    u32 perf_counter_src1;              /**< [in] source id for performance counter 1 (see ARM DDI0415A, Table 3-60) */
+    u32 returned_user_job_ptr;          /**< [out] identifier for the returned job in user space */
+    _mali_uk_start_job_status status;   /**< [out] indicates job start status (success, previous job returned, requeue) */
+	u32 abort_id;                       /**< [in] abort id of this job, used to identify this job for later abort requests */
+	u32 perf_counter_l2_src0;           /**< [in] soruce id for Mali-400 MP L2 cache performance counter 0 */
+	u32 perf_counter_l2_src1;           /**< [in] source id for Mali-400 MP L2 cache performance counter 1 */
+} _mali_uk_pp_start_job_s;
+/** @} */ /* end group _mali_uk_ppstartjob_s */
+
+typedef struct
+{
+    u32 user_job_ptr;               /**< [out] identifier for the job in user space */
+    _mali_uk_job_status status;     /**< [out] status of finished job */
+    u32 irq_status;                 /**< [out] value of interrupt rawstat register (see ARM DDI0415A) */
+    u32 last_tile_list_addr;        /**< [out] value of renderer list register (see ARM DDI0415A); necessary to restart a stopped job */
+    u32 perf_counter_src0;          /**< [out] source id for performance counter 0 (see ARM DDI0415A, Table 3-60) */
+    u32 perf_counter_src1;          /**< [out] source id for performance counter 1 (see ARM DDI0415A, Table 3-60) */
+    u32 perf_counter0;              /**< [out] value of perfomance counter 0 (see ARM DDI0415A) */
+    u32 perf_counter1;              /**< [out] value of perfomance counter 1 (see ARM DDI0415A) */
+    u32 render_time;                /**< [out] number of milliseconds it took for the job to render */
+	u32 perf_counter_l2_src0;       /**< [out] soruce id for Mali-400 MP L2 cache performance counter 0 */
+	u32 perf_counter_l2_src1;       /**< [out] soruce id for Mali-400 MP L2 cache performance counter 1 */
+	u32 perf_counter_l2_val0;       /**< [out] Value of the Mali-400 MP L2 cache performance counter 0 */
+	u32 perf_counter_l2_val1;       /**< [out] Value of the Mali-400 MP L2 cache performance counter 1 */
+	u32 perf_counter_l2_val0_raw;   /**< [out] Raw value of the Mali-400 MP L2 cache performance counter 0 */
+	u32 perf_counter_l2_val1_raw;   /**< [out] Raw value of the Mali-400 MP L2 cache performance counter 1 */
+} _mali_uk_pp_job_finished_s;
+/** @} */ /* end group _mali_uk_pp */
+
+
+/** @addtogroup _mali_uk_core U/K Core
+ * @{ */
+
+/** @defgroup _mali_uk_waitfornotification_s Wait For Notification
+ * @{ */
+
+/** @brief Notification type encodings
+ *
+ * Each Notification type is an ordered pair of (subsystem,id), and is unique.
+ *
+ * The encoding of subsystem,id into a 32-bit word is:
+ * encoding = (( subsystem << _MALI_NOTIFICATION_SUBSYSTEM_SHIFT ) & _MALI_NOTIFICATION_SUBSYSTEM_MASK)
+ *            | (( id <<  _MALI_NOTIFICATION_ID_SHIFT ) & _MALI_NOTIFICATION_ID_MASK)
+ *
+ * @see _mali_uk_wait_for_notification_s
+ */
+typedef enum
+{
+	/** core notifications */
+
+	_MALI_NOTIFICATION_CORE_SHUTDOWN_IN_PROGRESS =  (_MALI_UK_CORE_SUBSYSTEM << 16) | 0x20,
+	_MALI_NOTIFICATION_APPLICATION_QUIT =           (_MALI_UK_CORE_SUBSYSTEM << 16) | 0x40,
+
+	/** Fragment Processor notifications */
+
+	_MALI_NOTIFICATION_PP_FINISHED =                (_MALI_UK_PP_SUBSYSTEM << 16) | 0x10,
+
+	/** Vertex Processor notifications */
+
+	_MALI_NOTIFICATION_GP_FINISHED =                (_MALI_UK_GP_SUBSYSTEM << 16) | 0x10,
+	_MALI_NOTIFICATION_GP_STALLED =                 (_MALI_UK_GP_SUBSYSTEM << 16) | 0x20,
+} _mali_uk_notification_type;
+
+/** to assist in splitting up 32-bit notification value in subsystem and id value */
+#define _MALI_NOTIFICATION_SUBSYSTEM_MASK 0xFFFF0000
+#define _MALI_NOTIFICATION_SUBSYSTEM_SHIFT 16
+#define _MALI_NOTIFICATION_ID_MASK 0x0000FFFF
+#define _MALI_NOTIFICATION_ID_SHIFT 0
+
+
+/** @brief Arguments for _mali_ukk_wait_for_notification()
+ *
+ * On successful return from _mali_ukk_wait_for_notification(), the members of
+ * this structure will indicate the reason for notification.
+ *
+ * Specifically, the source of the notification can be identified by the
+ * subsystem and id fields of the mali_uk_notification_type in the code.type
+ * member. The type member is encoded in a way to divide up the types into a
+ * subsystem field, and a per-subsystem ID field. See
+ * _mali_uk_notification_type for more information.
+ *
+ * Interpreting the data union member depends on the notification type:
+ *
+ * - type == _MALI_NOTIFICATION_CORE_SHUTDOWN_IN_PROGRESS
+ *     - The kernel side is shutting down. No further
+ * _mali_uk_wait_for_notification() calls should be made.
+ *     - In this case, the value of the data union member is undefined.
+ *     - This is used to indicate to the user space client that it should close
+ * the connection to the Mali Device Driver.
+ * - type == _MALI_NOTIFICATION_PP_FINISHED
+ *    - The notification data is of type _mali_uk_pp_job_finished_s. It contains the user_job_ptr
+ * identifier used to start the job with, the job status, the number of milliseconds the job took to render,
+ * and values of core registers when the job finished (irq status, performance counters, renderer list
+ * address).
+ *    - A job has finished succesfully when its status member is _MALI_UK_JOB_STATUS_FINISHED.
+ *    - If the hardware detected a timeout while rendering the job, or software detected the job is
+ * taking more than watchdog_msecs (see _mali_ukk_pp_start_job()) to complete, the status member will
+ * indicate _MALI_UK_JOB_STATUS_HANG.
+ *    - If the hardware detected a bus error while accessing memory associated with the job, status will
+ * indicate _MALI_UK_JOB_STATUS_SEG_FAULT.
+ *    - Status will indicate MALI_UK_JOB_STATUS_NOT_STARTED if the driver had to stop the job but the job
+ * didn't start the hardware yet, e.g. when the driver closes.
+ * - type == _MALI_NOTIFICATION_GP_FINISHED
+ *     - The notification data is of type _mali_uk_gp_job_finished_s. The notification is similar to that of
+ * type == _MALI_NOTIFICATION_PP_FINISHED, except that several other GP core register values are returned.
+ * The status values have the same meaning for type == _MALI_NOTIFICATION_PP_FINISHED.
+ * - type == _MALI_NOTIFICATION_GP_STALLED
+ *     - The nofication data is of type _mali_uk_gp_job_suspended_s. It contains the user_job_ptr
+ * identifier used to start the job with, the reason why the job stalled and a cookie to identify the core on
+ * which the job stalled.
+ *     - The reason member of gp_job_suspended is set to _MALIGP_JOB_SUSPENDED_OUT_OF_MEMORY
+ * when the polygon list builder unit has run out of memory.
+ */
+typedef struct
+{
+    void *ctx;                       /**< [in,out] user-kernel context (trashed on output) */
+	_mali_uk_notification_type type; /**< [out] Type of notification available */
+    union
+    {
+        _mali_uk_gp_job_suspended_s gp_job_suspended;/**< [out] Notification data for _MALI_NOTIFICATION_GP_STALLED notification type */
+        _mali_uk_gp_job_finished_s  gp_job_finished; /**< [out] Notification data for _MALI_NOTIFICATION_GP_FINISHED notification type */
+        _mali_uk_pp_job_finished_s  pp_job_finished; /**< [out] Notification data for _MALI_NOTIFICATION_PP_FINISHED notification type */
+    } data;
+} _mali_uk_wait_for_notification_s;
+
+/** @brief Arguments for _mali_ukk_post_notification()
+ *
+ * Posts the specified notification to the notification queue for this application.
+ * This is used to send a quit message to the callback thread.
+ */
+typedef struct
+{
+    void *ctx;                       /**< [in,out] user-kernel context (trashed on output) */
+	_mali_uk_notification_type type; /**< [in] Type of notification to post */
+} _mali_uk_post_notification_s;
+/** @} */ /* end group _mali_uk_waitfornotification_s */
+
+/** @defgroup _mali_uk_getapiversion_s Get API Version
+ * @{ */
+
+/** helpers for Device Driver API version handling */
+
+/** @brief Encode a version ID from a 16-bit input
+ *
+ * @note the input is assumed to be 16 bits. It must not exceed 16 bits. */
+#define _MAKE_VERSION_ID(x) (((x) << 16UL) | (x))
+
+/** @brief Check whether a 32-bit value is likely to be Device Driver API
+ * version ID. */
+#define _IS_VERSION_ID(x) (((x) & 0xFFFF) == (((x) >> 16UL) & 0xFFFF))
+
+/** @brief Decode a 16-bit version number from a 32-bit Device Driver API version
+ * ID */
+#define _GET_VERSION(x) (((x) >> 16UL) & 0xFFFF)
+
+/** @brief Determine whether two 32-bit encoded version IDs match */
+#define _IS_API_MATCH(x, y) (IS_VERSION_ID((x)) && IS_VERSION_ID((y)) && (GET_VERSION((x)) == GET_VERSION((y))))
+
+/**
+ * API version define.
+ * Indicates the version of the kernel API
+ * The version is a 16bit integer incremented on each API change.
+ * The 16bit integer is stored twice in a 32bit integer
+ * For example, for version 1 the value would be 0x00010001
+ */
+#define _MALI_API_VERSION 8
+#define _MALI_UK_API_VERSION _MAKE_VERSION_ID(_MALI_API_VERSION)
+
+/**
+ * The API version is a 16-bit integer stored in both the lower and upper 16-bits
+ * of a 32-bit value. The 16-bit API version value is incremented on each API
+ * change. Version 1 would be 0x00010001. Used in _mali_uk_get_api_version_s.
+ */
+typedef u32 _mali_uk_api_version;
+
+/** @brief Arguments for _mali_uk_get_api_version()
+ *
+ * The user-side interface version must be written into the version member,
+ * encoded using _MAKE_VERSION_ID(). It will be compared to the API version of
+ * the kernel-side interface.
+ *
+ * On successful return, the version member will be the API version of the
+ * kernel-side interface. _MALI_UK_API_VERSION macro defines the current version
+ * of the API.
+ *
+ * The compatible member must be checked to see if the version of the user-side
+ * interface is compatible with the kernel-side interface, since future versions
+ * of the interface may be backwards compatible.
+ */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	_mali_uk_api_version version;   /**< [in,out] API version of user-side interface. */
+	int compatible;                 /**< [out] @c 1 when @version is compatible, @c 0 otherwise */
+} _mali_uk_get_api_version_s;
+/** @} */ /* end group _mali_uk_getapiversion_s */
+
+/** @} */ /* end group _mali_uk_core */
+
+
+/** @defgroup _mali_uk_memory U/K Memory
+ * @{ */
+
+/** @brief Arguments for _mali_ukk_init_mem(). */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 mali_address_base;          /**< [out] start of MALI address space */
+	u32 memory_size;                /**< [out] total MALI address space available */
+} _mali_uk_init_mem_s;
+
+/** @brief Arguments for _mali_ukk_term_mem(). */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+} _mali_uk_term_mem_s;
+
+/** @brief Arguments for _mali_ukk_get_big_block()
+ *
+ * - type_id should be set to the value of the identifier member of one of the
+ * _mali_mem_info structures returned through _mali_ukk_get_system_info()
+ * - ukk_private must be zero when calling from user-side. On Kernel-side, the
+ * OS implementation of the U/K interface can use it to communicate data to the
+ * OS implementation of the OSK layer. Specifically, ukk_private will be placed
+ * into the ukk_private member of the _mali_uk_mem_mmap_s structure. See
+ * _mali_ukk_mem_mmap() for more details.
+ * - minimum_size_requested will be updated if it is too small
+ * - block_size will always be >= minimum_size_requested, because the underlying
+ * allocation mechanism may only be able to divide up memory regions in certain
+ * ways. To avoid wasting memory, block_size should always be taken into account
+ * rather than assuming minimum_size_requested was really allocated.
+ * - to free the memory, the returned cookie member must be stored, and used to
+ * refer to it.
+ */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 type_id;                    /**< [in] the type id of the memory bank to allocate memory from */
+	u32 minimum_size_requested;     /**< [in,out] minimum size of the allocation */
+	u32 ukk_private;                /**< [in] Kernel-side private word inserted by certain U/K interface implementations. Caller must set to Zero. */
+	u32 mali_address;               /**< [out] address of the allocation in mali address space */
+	void *cpuptr;                   /**< [out] address of the allocation in the current process address space */
+	u32 block_size;                 /**< [out] size of the block that got allocated */
+	u32 flags;                      /**< [out] flags associated with the allocated block, of type _mali_bus_usage */
+	u32 cookie;                     /**< [out] identifier for the allocated block in kernel space  */
+} _mali_uk_get_big_block_s;
+
+/** @brief Arguments for _mali_ukk_free_big_block()
+ *
+ * All that is required is that the cookie member must be set to the value of
+ * the cookie member returned through _mali_ukk_get_big_block()
+ */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 cookie;                     /**< [in] identifier for mapped memory object in kernel space  */
+} _mali_uk_free_big_block_s;
+
+/** @note Mali-MMU only */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 phys_addr;                  /**< [in] physical address */
+	u32 size;                       /**< [in] size */
+	u32 mali_address;               /**< [in] mali address to map the physical memory to */
+	u32 rights;                     /**< [in] rights necessary for accessing memory */
+	u32 flags;                      /**< [in] flags, see \ref _MALI_MAP_EXTERNAL_MAP_GUARD_PAGE */
+	u32 cookie;                     /**< [out] identifier for mapped memory object in kernel space  */
+} _mali_uk_map_external_mem_s;
+
+/** Flag for _mali_uk_map_external_mem_s and _mali_uk_attach_ump_mem_s */
+#define _MALI_MAP_EXTERNAL_MAP_GUARD_PAGE (1<<0)
+
+/** @note Mali-MMU only */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 cookie;                     /**< [out] identifier for mapped memory object in kernel space  */
+} _mali_uk_unmap_external_mem_s;
+
+/** @note This is identical to _mali_uk_map_external_mem_s above, however phys_addr is replaced by secure_id */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 secure_id;                  /**< [in] secure id */
+	u32 size;                       /**< [in] size */
+	u32 mali_address;               /**< [in] mali address to map the physical memory to */
+	u32 rights;                     /**< [in] rights necessary for accessing memory */
+	u32 flags;                      /**< [in] flags, see \ref _MALI_MAP_EXTERNAL_MAP_GUARD_PAGE */
+	u32 cookie;                     /**< [out] identifier for mapped memory object in kernel space  */
+} _mali_uk_attach_ump_mem_s;
+
+/** @note Mali-MMU only; will be supported in future version */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 cookie;                     /**< [in] identifier for mapped memory object in kernel space  */
+} _mali_uk_release_ump_mem_s;
+
+/** @brief Arguments for _mali_ukk_va_to_mali_pa()
+ *
+ * if size is zero or not a multiple of the system's page size, it will be
+ * rounded up to the next multiple of the page size. This will occur before
+ * any other use of the size parameter.
+ *
+ * if va is not PAGE_SIZE aligned, it will be rounded down to the next page
+ * boundary.
+ *
+ * The range (va) to ((u32)va)+(size-1) inclusive will be checked for physical
+ * contiguity.
+ *
+ * The implementor will check that the entire physical range is allowed to be mapped
+ * into user-space.
+ *
+ * Failure will occur if either of the above are not satisfied.
+ *
+ * Otherwise, the physical base address of the range is returned through pa,
+ * va is updated to be page aligned, and size is updated to be a non-zero
+ * multiple of the system's pagesize.
+ */
+typedef struct
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	void *va;                       /**< [in,out] Virtual address of the start of the range */
+	u32 pa;                         /**< [out] Physical base address of the range */
+	u32 size;                       /**< [in,out] Size of the range, in bytes. */
+} _mali_uk_va_to_mali_pa_s;
+
+
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 size;                       /**< [out] size of MMU page table information (registers + page tables) */
+} _mali_uk_query_mmu_page_table_dump_size_s;
+
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 size;                       /**< [in] size of buffer to receive mmu page table information */
+    void *buffer;                   /**< [in,out] buffer to receive mmu page table information */
+    u32 register_writes_size;       /**< [out] size of MMU register dump */
+	u32 *register_writes;           /**< [out] pointer within buffer where MMU register dump is stored */
+	u32 page_table_dump_size;       /**< [out] size of MMU page table dump */
+	u32 *page_table_dump;           /**< [out] pointer within buffer where MMU page table dump is stored */
+} _mali_uk_dump_mmu_page_table_s;
+
+/** @} */ /* end group _mali_uk_memory */
+
+
+/** @addtogroup _mali_uk_pp U/K Fragment Processor
+ * @{ */
+
+/** @brief Arguments for _mali_ukk_get_pp_number_of_cores()
+ *
+ * - pass in the user-kernel context @c ctx that was returned from _mali_ukk_open()
+ * - Upon successful return from _mali_ukk_get_pp_number_of_cores(), @c number_of_cores
+ * will contain the number of Fragment Processor cores in the system.
+ */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+    u32 number_of_cores;            /**< [out] number of Fragment Processor cores in the system */
+} _mali_uk_get_pp_number_of_cores_s;
+
+/** @brief Arguments for _mali_ukk_get_pp_core_version()
+ *
+ * - pass in the user-kernel context @c ctx that was returned from _mali_ukk_open()
+ * - Upon successful return from _mali_ukk_get_pp_core_version(), @c version contains
+ * the version that all Fragment Processor cores are compatible with.
+ */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+    _mali_core_version version;     /**< [out] version returned from core, see \ref _mali_core_version  */
+} _mali_uk_get_pp_core_version_s;
+
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+    u32 abort_id;                   /**< [in] ID of job(s) to abort */
+} _mali_uk_pp_abort_job_s;
+
+/** @} */ /* end group _mali_uk_pp */
+
+
+/** @addtogroup _mali_uk_gp U/K Vertex Processor
+ * @{ */
+
+/** @brief Arguments for _mali_ukk_get_gp_number_of_cores()
+ *
+ * - pass in the user-kernel context @c ctx that was returned from _mali_ukk_open()
+ * - Upon successful return from _mali_ukk_get_gp_number_of_cores(), @c number_of_cores
+ * will contain the number of Vertex Processor cores in the system.
+ */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+    u32 number_of_cores;            /**< [out] number of Vertex Processor cores in the system */
+} _mali_uk_get_gp_number_of_cores_s;
+
+/** @brief Arguments for _mali_ukk_get_gp_core_version()
+ *
+ * - pass in the user-kernel context @c ctx that was returned from _mali_ukk_open()
+ * - Upon successful return from _mali_ukk_get_gp_core_version(), @c version contains
+ * the version that all Vertex Processor cores are compatible with.
+ */
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+    _mali_core_version version;     /**< [out] version returned from core, see \ref _mali_core_version */
+} _mali_uk_get_gp_core_version_s;
+
+typedef struct
+{
+    void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+    u32 abort_id;                   /**< [in] ID of job(s) to abort */
+} _mali_uk_gp_abort_job_s;
+
+typedef struct
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 limit;                      /**< [in,out] The desired limit for number of events to record on input, actual limit on output */
+} _mali_uk_profiling_start_s;
+
+typedef struct
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 event_id;                   /**< [in] event id to register (see  enum mali_profiling_events for values) */
+	u32 data[5];                    /**< [in] event specific data */
+} _mali_uk_profiling_add_event_s;
+
+typedef struct
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 count;                      /**< [out] The number of events sampled */
+} _mali_uk_profiling_stop_s;
+
+typedef struct
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	u32 index;                      /**< [in] which index to get (starting at zero) */
+	u64 timestamp;                  /**< [out] timestamp of event */
+	u32 event_id;                   /**< [out] event id of event (see  enum mali_profiling_events for values) */
+	u32 data[5];                    /**< [out] event specific data */
+} _mali_uk_profiling_get_event_s;
+
+typedef struct
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+} _mali_uk_profiling_clear_s;
+
+
+
+/** @} */ /* end group _mali_uk_gp */
+
+
+/** @addtogroup _mali_uk_memory U/K Memory
+ * @{ */
+
+/** @brief Arguments to _mali_ukk_mem_mmap()
+ *
+ * Use of the phys_addr member depends on whether the driver is compiled for
+ * Mali-MMU or nonMMU:
+ * - in the nonMMU case, this is the physical address of the memory as seen by
+ * the CPU (which may be a constant offset from that used by Mali)
+ * - in the MMU case, this is the Mali Virtual base address of the memory to
+ * allocate, and the particular physical pages used to back the memory are
+ * entirely determined by _mali_ukk_mem_mmap(). The details of the physical pages
+ * are not reported to user-space for security reasons.
+ *
+ * The cookie member must be stored for use later when freeing the memory by
+ * calling _mali_ukk_mem_munmap(). In the Mali-MMU case, the cookie is secure.
+ *
+ * The ukk_private word must be set to zero when calling from user-space. On
+ * Kernel-side, the  OS implementation of the U/K interface can use it to
+ * communicate data to the OS implementation of the OSK layer. In particular,
+ * _mali_ukk_get_big_block() directly calls _mali_ukk_mem_mmap directly, and
+ * will communicate its own ukk_private word through the ukk_private member
+ * here. The common code itself will not inspect or modify the ukk_private
+ * word, and so it may be safely used for whatever purposes necessary to
+ * integrate Mali Memory handling into the OS.
+ *
+ * The uku_private member is currently reserved for use by the user-side
+ * implementation of the U/K interface. Its value must be zero.
+ */
+typedef struct
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	void *mapping;                  /**< [out] Returns user-space virtual address for the mapping */
+	u32 size;                       /**< [in] Size of the requested mapping */
+	u32 phys_addr;                  /**< [in] Physical address - could be offset, depending on caller+callee convention */
+	u32 cookie;                     /**< [out] Returns a cookie for use in munmap calls */
+	void *uku_private;              /**< [in] User-side Private word used by U/K interface */
+	void *ukk_private;              /**< [in] Kernel-side Private word used by U/K interface */
+} _mali_uk_mem_mmap_s;
+
+/** @brief Arguments to _mali_ukk_mem_munmap()
+ *
+ * The cookie and mapping members must be that returned from the same previous
+ * call to _mali_ukk_mem_mmap(). The size member must correspond to cookie
+ * and mapping - that is, it must be the value originally supplied to a call to
+ * _mali_ukk_mem_mmap that returned the values of mapping and cookie.
+ *
+ * An error will be returned if an attempt is made to unmap only part of the
+ * originally obtained range, or to unmap more than was originally obtained.
+ */
+typedef struct
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	void *mapping;                  /**< [in] The mapping returned from mmap call */
+	u32 size;                       /**< [in] The size passed to mmap call */
+	u32 cookie;                     /**< [in] Cookie from mmap call */
+} _mali_uk_mem_munmap_s;
+/** @} */ /* end group _mali_uk_memory */
+
+#if USING_MALI_PMM
+
+/** @defgroup _mali_uk_pmm U/K Power Management Module
+ * @{ */
+
+/** @brief Power management event message identifiers.
+ *
+ * U/K events start after id 200, and can range up to 999
+ * Adding new events will require updates to the PMM mali_pmm_event_id type
+ */
+#define _MALI_PMM_EVENT_UK_EXAMPLE 201
+
+/** @brief Generic PMM message data type, that will be dependent on the event msg
+ */
+typedef u32 mali_pmm_message_data;
+
+
+/** @brief Arguments to _mali_ukk_pmm_event_message()
+ */
+typedef struct 
+{
+	void *ctx;                          /**< [in,out] user-kernel context (trashed on output) */
+	u32 id;                             /**< [in] event id */
+	mali_pmm_message_data data;         /**< [in] specific data associated with the event */
+} _mali_uk_pmm_message_s;
+
+/** @} */ /* end group _mali_uk_pmm */
+#endif /* USING_MALI_PMM */
+
+/** @defgroup _mali_uk_vsync U/K VSYNC Wait Reporting Module
+ * @{ */
+
+/** @brief VSYNC events
+ *
+ * These events are reported when DDK starts to wait for vsync and when the
+ * vsync has occured and the DDK can continue on the next frame.
+ */
+typedef enum _mali_uk_vsync_event
+{
+	_MALI_UK_VSYNC_EVENT_BEGIN_WAIT = 0,
+	_MALI_UK_VSYNC_EVENT_END_WAIT
+} _mali_uk_vsync_event;
+
+/** @brief Arguments to _mali_ukk_vsync_event()
+ *
+ */
+typedef struct
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	_mali_uk_vsync_event event;     /**< [in] VSYNCH event type */
+} _mali_uk_vsync_event_report_s;
+
+/** @} */ /* end group _mali_uk_vsync */
+
+/** @} */ /* end group u_k_api */
+
+/** @} */ /* end group uddapi */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_UK_TYPES_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_ukk.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_ukk.h
new file mode 100644
index 00000000000..d066046901d
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/mali_ukk.h
@@ -0,0 +1,710 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_ukk.h
+ * Defines the kernel-side interface of the user-kernel interface
+ */
+
+#ifndef __MALI_UKK_H__
+#define __MALI_UKK_H__
+
+#include "mali_osk.h"
+#include "mali_uk_types.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @addtogroup uddapi Unified Device Driver (UDD) APIs
+ *
+ * @{
+ */
+
+/**
+ * @addtogroup u_k_api UDD User/Kernel Interface (U/K) APIs
+ *
+ * - The _mali_uk functions are an abstraction of the interface to the device
+ * driver. On certain OSs, this would be implemented via the IOCTL interface.
+ * On other OSs, it could be via extension of some Device Driver Class, or
+ * direct function call for Bare metal/RTOSs.
+ * - It is important to note that:
+ *   -  The Device Driver has implemented the _mali_ukk set of functions
+ *   -  The Base Driver calls the corresponding set of _mali_uku functions.
+ * - What requires porting is solely the calling mechanism from User-side to
+ * Kernel-side, and propagating back the results.
+ * - Each U/K function is associated with a (group, number) pair from
+ * \ref _mali_uk_functions to make it possible for a common function in the
+ * Base Driver and Device Driver to route User/Kernel calls from/to the
+ * correct _mali_uk function. For example, in an IOCTL system, the IOCTL number
+ * would be formed based on the group and number assigned to the _mali_uk
+ * function, as listed in \ref _mali_uk_functions. On the user-side, each
+ * _mali_uku function would just make an IOCTL with the IOCTL-code being an
+ * encoded form of the (group, number) pair. On the kernel-side, the Device
+ * Driver's IOCTL handler decodes the IOCTL-code back into a (group, number)
+ * pair, and uses this to determine which corresponding _mali_ukk should be
+ * called.
+ *   - Refer to \ref _mali_uk_functions for more information about this
+ * (group, number) pairing.
+ * - In a system where there is no distinction between user and kernel-side,
+ * the U/K interface may be implemented as:@code
+ * MALI_STATIC_INLINE _mali_osk_errcode_t _mali_uku_examplefunction( _mali_uk_examplefunction_s *args )
+ * {
+ *     return mali_ukk_examplefunction( args );
+ * }
+ * @endcode
+ * - Therefore, all U/K calls behave \em as \em though they were direct
+ * function calls (but the \b implementation \em need \em not be a direct
+ * function calls)
+ *
+ * @note Naming the _mali_uk functions the same on both User and Kernel sides
+ * on non-RTOS systems causes debugging issues when setting breakpoints. In
+ * this case, it is not clear which function the breakpoint is put on.
+ * Therefore the _mali_uk functions in user space are prefixed with \c _mali_uku
+ * and in kernel space with \c _mali_ukk. The naming for the argument
+ * structures is unaffected.
+ *
+ * - The _mali_uk functions are synchronous.
+ * - Arguments to the _mali_uk functions are passed in a structure. The only
+ * parameter passed to the _mali_uk functions is a pointer to this structure.
+ * This first member of this structure, ctx, is a pointer to a context returned
+ * by _mali_uku_open(). For example:@code
+ * typedef struct
+ * {
+ *     void *ctx;
+ *     u32 number_of_cores;
+ * } _mali_uk_get_gp_number_of_cores_s;
+ * @endcode
+ *
+ * - Each _mali_uk function has its own argument structure named after the
+ *  function. The argument is distinguished by the _s suffix.
+ * - The argument types are defined by the base driver and user-kernel
+ *  interface.
+ * - All _mali_uk functions return a standard \ref _mali_osk_errcode_t.
+ * - Only arguments of type input or input/output need be initialized before
+ * calling a _mali_uk function.
+ * - Arguments of type output and input/output are only valid when the
+ * _mali_uk function returns \ref _MALI_OSK_ERR_OK.
+ * - The \c ctx member is always invalid after it has been used by a
+ * _mali_uk function, except for the context management functions
+ *
+ *
+ * \b Interface \b restrictions
+ *
+ * The requirements of the interface mean that an implementation of the
+ * User-kernel interface may do no 'real' work. For example, the following are
+ * illegal in the User-kernel implementation:
+ * - Calling functions necessary for operation on all systems,  which would
+ * not otherwise get called on RTOS systems.
+ *     - For example, a  U/K interface that calls multiple _mali_ukk functions
+ * during one particular U/K call. This could not be achieved by the same code
+ * which uses direct function calls for the U/K interface.
+ * -  Writing in values to the args members, when otherwise these members would
+ * not hold a useful value for a direct function call U/K interface.
+ *     - For example, U/K interface implementation that take NULL members in
+ * their arguments structure from the user side, but those members are
+ * replaced with non-NULL values in the kernel-side of the U/K interface
+ * implementation. A scratch area for writing data is one such example. In this
+ * case, a direct function call U/K interface would segfault, because no code
+ * would be present to replace the NULL pointer with a meaningful pointer.
+ *     - Note that we discourage the case where the U/K implementation changes
+ * a NULL argument member to non-NULL, and then the Device Driver code (outside
+ * of the U/K layer) re-checks this member for NULL, and corrects it when
+ * necessary. Whilst such code works even on direct function call U/K
+ * intefaces, it reduces the testing coverage of the Device Driver code. This
+ * is because we have no way of testing the NULL == value path on an OS
+ * implementation.
+ *
+ * A number of allowable examples exist where U/K interfaces do 'real' work:
+ * - The 'pointer switching' technique for \ref _mali_ukk_get_system_info
+ *     - In this case, without the pointer switching on direct function call
+ * U/K interface, the Device Driver code still sees the same thing: a pointer
+ * to which it can write memory. This is because such a system has no
+ * distinction between a user and kernel pointer.
+ * - Writing an OS-specific value into the ukk_private member for
+ * _mali_ukk_mem_mmap().
+ *     - In this case, this value is passed around by Device Driver code, but
+ * its actual value is never checked. Device Driver code simply passes it from
+ * the U/K layer to the OSK layer, where it can be acted upon. In this case,
+ * \em some OS implementations of the U/K (_mali_ukk_mem_mmap()) and OSK
+ * (_mali_osk_mem_mapregion_init()) functions will collaborate on the
+ *  meaning of ukk_private member. On other OSs, it may be unused by both
+ * U/K and OSK layers
+ * - On OS systems (not including direct function call U/K interface
+ * implementations), _mali_ukk_get_big_block() may succeed, but the subsequent
+ * copying to user space may fail.
+ *     - A problem scenario exists: some memory has been reserved by
+ * _mali_ukk_get_big_block(), but the user-mode will be unaware of it (it will
+ * never receive any information about this memory). In this case, the U/K
+ * implementation must do everything necessary to 'rollback' the \em atomic
+ * _mali_ukk_get_big_block() transaction.
+ *     - Therefore, on error inside the U/K interface implementation itself,
+ * it will be as though the _mali_ukk function itself had failed, and cleaned
+ * up after itself.
+ *     - Compare this to a direct function call U/K implementation, where all
+ * error cleanup is handled by the _mali_ukk function itself. The direct
+ * function call U/K interface implementation is automatically atomic.
+ *
+ * The last example highlights a consequence of all U/K interface
+ * implementations: they must be atomic with respect to the Device Driver code.
+ * And therefore, should Device Driver code succeed but the U/K implementation
+ * fail afterwards (but before return to user-space), then the U/K
+ * implementation must cause appropriate cleanup actions to preserve the
+ * atomicity of the interface.
+ *
+ * @{
+ */
+
+
+/** @defgroup _mali_uk_context U/K Context management
+ *
+ * These functions allow for initialisation of the user-kernel interface once per process.
+ *
+ * Generally the context will store the OS specific object to communicate with the kernel device driver and further
+ * state information required by the specific implementation. The context is shareable among all threads in the caller process.
+ *
+ * On IOCTL systems, this is likely to be a file descriptor as a result of opening the kernel device driver.
+ *
+ * On a bare-metal/RTOS system with no distinction between kernel and
+ * user-space, the U/K interface simply calls the _mali_ukk variant of the
+ * function by direct function call. In this case, the context returned is the
+ * mali_session_data from _mali_ukk_open().
+ *
+ * The kernel side implementations of the U/K interface expect the first member of the argument structure to
+ * be the context created by _mali_uku_open(). On some OS implementations, the meaning of this context
+ * will be different between user-side and kernel-side. In which case, the kernel-side will need to replace this context
+ * with the kernel-side equivalent, because user-side will not have access to kernel-side data. The context parameter
+ * in the argument structure therefore has to be of type input/output.
+ *
+ * It should be noted that the caller cannot reuse the \c ctx member of U/K
+ * argument structure after a U/K call, because it may be overwritten. Instead,
+ * the context handle must always be stored  elsewhere, and copied into
+ * the appropriate U/K argument structure for each user-side call to
+ * the U/K interface. This is not usually a problem, since U/K argument
+ * structures are usually placed on the stack.
+ *
+ * @{ */
+
+/** @brief Begin a new Mali Device Driver session
+ *
+ * This is used to obtain a per-process context handle for all future U/K calls.
+ *
+ * @param context pointer to storage to return a (void*)context handle.
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_open( void **context );
+
+/** @brief End a Mali Device Driver session
+ *
+ * This should be called when the process no longer requires use of the Mali Device Driver.
+ *
+ * The context handle must not be used after it has been closed.
+ *
+ * @param context pointer to a stored (void*)context handle.
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_close( void **context );
+
+/** @} */ /* end group _mali_uk_context */
+
+
+/** @addtogroup _mali_uk_core U/K Core
+ *
+ * The core functions provide the following functionality:
+ * - verify that the user and kernel API are compatible
+ * - retrieve information about the cores and memory banks in the system
+ * - wait for the result of jobs started on a core
+ *
+ * @{ */
+
+/** @brief Returns the size of the buffer needed for a _mali_ukk_get_system_info call
+ *
+ * This function must be called before a call is made to
+ * _mali_ukk_get_system_info, so that memory of the correct size can be
+ * allocated, and a pointer to this memory written into the system_info member
+ * of _mali_uk_get_system_info_s.
+ *
+ * @param args see _mali_uk_get_system_info_size_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_get_system_info_size( _mali_uk_get_system_info_size_s *args );
+
+/** @brief Returns information about the system (cores and memory banks)
+ *
+ * A buffer for this needs to be allocated by the caller. The size of the buffer required is returned by
+ * _mali_ukk_get_system_info_size(). The user is responsible for freeing the buffer.
+ *
+ * The _mali_system_info structure will be written to the start of this buffer,
+ * and the core_info and mem_info lists will be written to locations inside
+ * the buffer, and will be suitably aligned.
+ *
+ * Under OS implementations of the U/K interface we need to pack/unpack
+ * pointers across the user/kernel boundary. This has required that we malloc()
+ * an intermediate buffer inside the kernel-side U/K interface, and free it
+ * before returning to user-side. To avoid modifying common code, we do the
+ * following pseudo-code, which we shall call 'pointer switching':
+ *
+ * @code
+ * {
+ *     Copy_From_User(kargs, args, ... );
+ *     void __user * local_ptr = kargs->system_info;
+ *     kargs->system_info = _mali_osk_malloc( ... );
+ *     _mali_ukk_get_system_info( kargs );
+ *     Copy_To_User( local_ptr, kargs->system_info, ... );
+ *     _mali_osk_free( kargs->system_info );
+ * }
+ * @endcode
+ * @note The user-side's args->system_info members was unmodified here.
+ *
+ * However, the current implementation requires an extra ukk_private word so that the common code can work out
+ * how to patch pointers to user-mode for an OS's U/K implementation, this should be set to the user-space
+ * destination address for pointer-patching to occur. When NULL, it is unused, an no pointer-patching occurs in the
+ * common code.
+ *
+ * @param args see _mali_uk_get_system_info_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_get_system_info( _mali_uk_get_system_info_s *args );
+
+/** @brief Waits for a job notification.
+ *
+ * Sleeps until notified or a timeout occurs. Returns information about the notification.
+ *
+ * @param args see _mali_uk_wait_for_notification_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_wait_for_notification( _mali_uk_wait_for_notification_s *args );
+
+/** @brief Post a notification to the notification queue of this application.
+ *
+ * @param args see _mali_uk_post_notification_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_post_notification( _mali_uk_post_notification_s *args );
+
+/** @brief Verifies if the user and kernel side of this API are compatible.
+ *
+ * @param args see _mali_uk_get_api_version_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_get_api_version( _mali_uk_get_api_version_s *args );
+/** @} */ /* end group _mali_uk_core */
+
+
+/** @addtogroup _mali_uk_memory U/K Memory
+ *
+ * The memory functions provide functionality with and without a Mali-MMU present.
+ *
+ * For Mali-MMU based systems, the following functionality is provided:
+ * - Initialize and terminate MALI virtual address space
+ * - Allocate/deallocate physical memory to a MALI virtual address range and map into/unmap from the
+ * current process address space
+ * - Map/unmap external physical memory into the MALI virtual address range
+ *
+ * For Mali-nonMMU based systems:
+ * - Allocate/deallocate MALI memory
+ *
+ * @{ */
+
+/**
+ * @brief Initialize the Mali-MMU Memory system
+ *
+ * For Mali-MMU builds of the drivers, this function must be called before any
+ * other functions in the \ref _mali_uk_memory group are called.
+ *
+ * @note This function is for Mali-MMU builds \b only. It should not be called
+ * when the drivers are built without Mali-MMU support.
+ *
+ * @param args see \ref _mali_uk_init_mem_s in mali_uk_types.h
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable
+ * _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_init_mem( _mali_uk_init_mem_s *args );
+
+/**
+ * @brief Terminate the MMU Memory system
+ *
+ * For Mali-MMU builds of the drivers, this function must be called when
+ * functions in the \ref _mali_uk_memory group will no longer be called. This
+ * function must be called before the application terminates.
+ *
+ * @note This function is for Mali-MMU builds \b only. It should not be called
+ * when the drivers are built without Mali-MMU support.
+ *
+ * @param args see \ref _mali_uk_term_mem_s in mali_uk_types.h
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable
+ * _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_term_mem( _mali_uk_term_mem_s *args );
+
+/** @brief Map a block of memory into the current user process
+ *
+ * Allocates a minimum of minimum_size_requested bytes of MALI memory and maps it into the current
+ * process space. The number of bytes allocated is returned in args->block_size.
+ *
+ * This is only used for Mali-nonMMU mode.
+ *
+ * @param args see _mali_uk_get_big_block_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_get_big_block( _mali_uk_get_big_block_s *args );
+
+/** @brief Unmap a block of memory from the current user process
+ *
+ * Frees allocated MALI memory and unmaps it from the current process space. The previously allocated memory
+ * is indicated by the cookie as returned by _mali_ukk_get_big_block().
+ *
+ * This is only used for Mali-nonMMU mode.
+ *
+ * @param args see _mali_uk_free_big_block_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_free_big_block( _mali_uk_free_big_block_s *args );
+
+/** @brief Map Mali Memory into the current user process
+ *
+ * Maps Mali memory into the current user process in a generic way.
+ *
+ * This function is to be used for Mali-MMU mode. The function is available in both Mali-MMU and Mali-nonMMU modes,
+ * but should not be called by a user process in Mali-nonMMU mode. In Mali-nonMMU mode, the function is callable
+ * from the kernel side, and is used to implement _mali_ukk_get_big_block() in this case.
+ *
+ * The implementation and operation of _mali_ukk_mem_mmap() is dependant on whether the driver is built for Mali-MMU
+ * or Mali-nonMMU:
+ * - In the nonMMU case, _mali_ukk_mem_mmap() requires a physical address to be specified. For this reason, an OS U/K
+ * implementation should not allow this to be called from user-space. In any case, nonMMU implementations are
+ * inherently insecure, and so the overall impact is minimal. Mali-MMU mode should be used if security is desired.
+ * - In the MMU case, _mali_ukk_mem_mmap() the _mali_uk_mem_mmap_s::phys_addr
+ * member is used for the \em Mali-virtual address desired for the mapping. The
+ * implementation of _mali_ukk_mem_mmap() will allocate both the CPU-virtual
+ * and CPU-physical addresses, and can cope with mapping a contiguous virtual
+ * address range to a sequence of non-contiguous physical pages. In this case,
+ * the CPU-physical addresses are not communicated back to the user-side, as
+ * they are unnecsessary; the \em Mali-virtual address range must be used for
+ * programming Mali structures.
+ *
+ * This means that in the first (nonMMU) case, the caller must manage the physical address allocations. The caller
+ * in this case is _mali_ukk_get_big_block(), which does indeed manage the Mali physical address ranges.
+ *
+ * In the second (MMU) case, _mali_ukk_mem_mmap() handles management of
+ * CPU-virtual and CPU-physical ranges, but the \em caller must manage the
+ * \em Mali-virtual address range from the user-side.
+ *
+ * @note Mali-virtual address ranges are entirely separate between processes.
+ * It is not possible for a process to accidentally corrupt another process'
+ * \em Mali-virtual address space.
+ *
+ * @param args see _mali_uk_mem_mmap_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_mem_mmap( _mali_uk_mem_mmap_s *args );
+
+/** @brief Unmap Mali Memory from the current user process
+ *
+ * Unmaps Mali memory from the current user process in a generic way. This only operates on Mali memory supplied
+ * from _mali_ukk_mem_mmap().
+ *
+ * @param args see _mali_uk_mem_munmap_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_mem_munmap( _mali_uk_mem_munmap_s *args );
+
+/** @brief Determine the buffer size necessary for an MMU page table dump.
+ * @param args see _mali_uk_query_mmu_page_table_dump_size_s in mali_uk_types.h
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_query_mmu_page_table_dump_size( _mali_uk_query_mmu_page_table_dump_size_s *args );
+/** @brief Dump MMU Page tables.
+ * @param args see _mali_uk_dump_mmu_page_table_s in mali_uk_types.h
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_dump_mmu_page_table( _mali_uk_dump_mmu_page_table_s * args );
+
+/** @brief Map a physically contiguous range of memory into Mali
+ * @param args see _mali_uk_map_external_mem_s in mali_uk_types.h
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_map_external_mem( _mali_uk_map_external_mem_s *args );
+
+/** @brief Unmap a physically contiguous range of memory from Mali
+ * @param args see _mali_uk_unmap_external_mem_s in mali_uk_types.h
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_unmap_external_mem( _mali_uk_unmap_external_mem_s *args );
+
+#if MALI_USE_UNIFIED_MEMORY_PROVIDER != 0
+/** @brief Map UMP memory into Mali
+ * @param args see _mali_uk_attach_ump_mem_s in mali_uk_types.h
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_attach_ump_mem( _mali_uk_attach_ump_mem_s *args );
+/** @brief Unmap UMP memory from Mali
+ * @param args see _mali_uk_release_ump_mem_s in mali_uk_types.h
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_release_ump_mem( _mali_uk_release_ump_mem_s *args );
+#endif /* MALI_USE_UNIFIED_MEMORY_PROVIDER */
+
+/** @brief Determine virtual-to-physical mapping of a contiguous memory range
+ * (optional)
+ *
+ * This allows the user-side to do a virtual-to-physical address translation.
+ * In conjunction with _mali_uku_map_external_mem, this can be used to do
+ * direct rendering.
+ *
+ * This function will only succeed on a virtual range that is mapped into the
+ * current process, and that is contigious.
+ *
+ * If va is not page-aligned, then it is rounded down to the next page
+ * boundary. The remainer is added to size, such that ((u32)va)+size before
+ * rounding is equal to ((u32)va)+size after rounding. The rounded modified
+ * va and size will be written out into args on success.
+ *
+ * If the supplied size is zero, or not a multiple of the system's PAGE_SIZE,
+ * then size will be rounded up to the next multiple of PAGE_SIZE before
+ * translation occurs. The rounded up size will be written out into args on
+ * success.
+ *
+ * On most OSs, virtual-to-physical address translation is a priveledged
+ * function. Therefore, the implementer must validate the range supplied, to
+ * ensure they are not providing arbitrary virtual-to-physical address
+ * translations. While it is unlikely such a mechanism could be used to
+ * compromise the security of a system on its own, it is possible it could be
+ * combined with another small security risk to cause a much larger security
+ * risk.
+ *
+ * @note This is an optional part of the interface, and is only used by certain
+ * implementations of libEGL. If the platform layer in your libEGL
+ * implementation does not require Virtual-to-Physical address translation,
+ * then this function need not be implemented. A stub implementation should not
+ * be required either, as it would only be removed by the compiler's dead code
+ * elimination.
+ *
+ * @note if implemented, this function is entirely platform-dependant, and does
+ * not exist in common code.
+ *
+ * @param args see _mali_uk_va_to_mali_pa_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_va_to_mali_pa( _mali_uk_va_to_mali_pa_s * args );
+
+/** @} */ /* end group _mali_uk_memory */
+
+
+/** @addtogroup _mali_uk_pp U/K Fragment Processor
+ *
+ * The Fragment Processor (aka PP (Pixel Processor)) functions provide the following functionality:
+ * - retrieving version of the fragment processors
+ * - determine number of fragment processors
+ * - starting a job on a fragment processor
+ *
+ * @{ */
+
+/** @brief Issue a request to start a new job on a Fragment Processor.
+ *
+ * If the request fails args->status is set to _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE and you can
+ * try to start the job again.
+ *
+ * An existing job could be returned for requeueing if the new job has a higher priority than a previously started job
+ * which the hardware hasn't actually started processing yet. In this case the new job will be started instead and the
+ * existing one returned, otherwise the new job is started and the status field args->status is set to
+ * _MALI_UK_START_JOB_STARTED.
+ *
+ * If an existing lower priority job is returned, args->returned_user_job_ptr contains a
+ * pointer to the returned job and the status field args->status is set to
+ * _MALI_UK_START_JOB_STARTED_LOW_PRI_JOB_RETURNED.
+ *
+ * Job completion can be awaited with _mali_ukk_wait_for_notification().
+ *
+ * @param args see _mali_uk_pp_start_job_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_pp_start_job( _mali_uk_pp_start_job_s *args );
+
+/** @brief Returns the number of Fragment Processors in the system
+ *
+ * @param args see _mali_uk_get_pp_number_of_cores_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_get_pp_number_of_cores( _mali_uk_get_pp_number_of_cores_s *args );
+
+/** @brief Returns the version that all Fragment Processor cores are compatible with.
+ *
+ * This function may only be called when _mali_ukk_get_pp_number_of_cores() indicated at least one Fragment
+ * Processor core is available.
+ *
+ * @param args see _mali_uk_get_pp_core_version_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_get_pp_core_version( _mali_uk_get_pp_core_version_s *args );
+
+/** @brief Abort any PP jobs with the given ID.
+ *
+ * Jobs internally queued or currently running on the hardware is to be stopped/aborted.
+ * Jobs aborted are reported via the normal job completion system.
+ * Any jobs, running or internally queued should be aborted imediately.
+ * Normal notifiction procedures to report on the status of these jobs.
+ *
+ *
+ * @param args see _malu_uk_pp_abort_job_s in "mali_uk_types.h"
+ */
+void _mali_ukk_pp_abort_job( _mali_uk_pp_abort_job_s *args );
+/** @} */ /* end group _mali_uk_pp */
+
+
+/** @addtogroup _mali_uk_gp U/K Vertex Processor
+ *
+ * The Vertex Processor (aka GP (Geometry Processor)) functions provide the following functionality:
+ * - retrieving version of the Vertex Processors
+ * - determine number of Vertex Processors available
+ * - starting a job on a Vertex Processor
+ *
+ * @{ */
+
+/** @brief Issue a request to start a new job on a Vertex Processor.
+ *
+ * If the request fails args->status is set to _MALI_UK_START_JOB_NOT_STARTED_DO_REQUEUE and you can
+ * try to start the job again.
+ *
+ * An existing job could be returned for requeueing if the new job has a higher priority than a previously started job
+ * which the hardware hasn't actually started processing yet. In this case the new job will be started and the
+ * existing one returned, otherwise the new job is started and the status field args->status is set to
+ * _MALI_UK_START_JOB_STARTED.
+ *
+ * If an existing lower priority job is returned, args->returned_user_job_ptr contains a pointer to
+ * the returned job and the status field args->status is set to
+ * _MALI_UK_START_JOB_STARTED_LOW_PRI_JOB_RETURNED.
+ *
+ * Job completion can be awaited with _mali_ukk_wait_for_notification().
+ *
+ * @param args see _mali_uk_gp_start_job_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_gp_start_job( _mali_uk_gp_start_job_s *args );
+
+/** @brief Returns the number of Vertex Processors in the system.
+ *
+ * @param args see _mali_uk_get_gp_number_of_cores_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_get_gp_number_of_cores( _mali_uk_get_gp_number_of_cores_s *args );
+
+/** @brief Returns the version that all Vertex Processor cores are compatible with.
+ *
+ * This function may only be called when _mali_uk_get_gp_number_of_cores() indicated at least one Vertex
+ * Processor core is available.
+ *
+ * @param args see _mali_uk_get_gp_core_version_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_get_gp_core_version( _mali_uk_get_gp_core_version_s *args );
+
+/** @brief Resume or abort suspended Vertex Processor jobs.
+ *
+ * After receiving notification that a Vertex Processor job was suspended from
+ * _mali_ukk_wait_for_notification() you can use this function to resume or abort the job.
+ *
+ * @param args see _mali_uk_gp_suspend_response_s in "mali_uk_types.h"
+ * @return _MALI_OSK_ERR_OK on success, otherwise a suitable _mali_osk_errcode_t on failure.
+ */
+_mali_osk_errcode_t _mali_ukk_gp_suspend_response( _mali_uk_gp_suspend_response_s *args );
+
+/** @brief Abort any GP jobs with the given ID.
+ *
+ * Jobs internally queued or currently running on the hardware is to be stopped/aborted.
+ * Jobs aborted are reported via the normal job completion system.
+ *
+ * Any jobs, running or internally queued should be aborted imediately.
+ * Normal notifiction procedures to report on the status of these jobs.
+ *
+ * @param args see _mali_uk_gp_abort_job_s in "mali_uk_types.h"
+ */
+void _mali_ukk_gp_abort_job( _mali_uk_gp_abort_job_s *args );
+/** @} */ /* end group _mali_uk_gp */
+
+#if USING_MALI_PMM
+/** @addtogroup _mali_uk_pmm U/K Power Management Module
+ * @{ */
+
+/* @brief Power Management Module event message
+ *
+ * @note The event message can fail to be sent due to OOM but this is
+ * stored in the PMM state machine to be handled later
+ *
+ * @param args see _mali_uk_pmm_event_message_s in "mali_uk_types.h"
+ */
+void _mali_ukk_pmm_event_message( _mali_uk_pmm_message_s *args );
+/** @} */ /* end group _mali_uk_pmm */
+#endif /* USING_MALI_PMM */
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+/** @addtogroup _mali_uk_profiling U/K Timeline profiling module
+ * @{ */
+
+/** @brief Start recording profiling events.
+ *
+ * @param args see _mali_uk_profiling_start_s in "mali_uk_types.h"
+ */
+_mali_osk_errcode_t _mali_ukk_profiling_start(_mali_uk_profiling_start_s *args);
+
+/** @brief Add event to profiling buffer.
+ *
+ * @param args see _mali_uk_profiling_add_event_s in "mali_uk_types.h"
+ */
+_mali_osk_errcode_t _mali_ukk_profiling_add_event(_mali_uk_profiling_add_event_s *args);
+
+/** @brief Stop recording profiling events.
+ * 
+ * @param args see _mali_uk_profiling_stop_s in "mali_uk_types.h"
+ */
+_mali_osk_errcode_t _mali_ukk_profiling_stop(_mali_uk_profiling_stop_s *args);
+
+/** @brief Retrieve a recorded profiling event.
+ *
+ * @param args see _mali_uk_profiling_get_event_s in "mali_uk_types.h"
+ */
+_mali_osk_errcode_t _mali_ukk_profiling_get_event(_mali_uk_profiling_get_event_s *args);
+
+/** @brief Clear recorded profiling events.
+ *
+ * @param args see _mali_uk_profiling_clear_s in "mali_uk_types.h"
+ */
+_mali_osk_errcode_t _mali_ukk_profiling_clear(_mali_uk_profiling_clear_s *args);
+
+/** @} */ /* end group _mali_uk_profiling */
+#endif
+
+/** @addtogroup _mali_uk_vsync U/K VSYNC reporting module
+ * @{ */
+
+/** @brief Report events related to vsync.
+ *
+ * @note Events should be reported when starting to wait for vsync and when the
+ * waiting is finished. This information can then be used in kernel space to
+ * complement the GPU utilization metric.
+ *
+ * @param args see _mali_uk_vsync_event_report_s in "mali_uk_types.h"
+ */
+_mali_osk_errcode_t _mali_ukk_vsync_event_report(_mali_uk_vsync_event_report_s *args);
+
+/** @} */ /* end group _mali_uk_vsync */
+
+/** @} */ /* end group u_k_api */
+
+/** @} */ /* end group uddapi */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_UKK_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm.c
new file mode 100644
index 00000000000..eeb29589ddc
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm.c
@@ -0,0 +1,921 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm.c
+ * Implementation of the power management module for the kernel device driver
+ */
+
+#if USING_MALI_PMM
+
+#include "mali_ukk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_subsystem.h"
+
+#include "mali_pmm.h"
+#include "mali_pmm_system.h"
+#include "mali_pmm_state.h"
+#include "mali_pmm_policy.h"
+#include "mali_platform.h"
+
+/* Internal PMM subsystem state */
+static _mali_pmm_internal_state_t *pmm_state = NULL;
+/* Mali kernel subsystem id */
+static mali_kernel_subsystem_identifier mali_subsystem_pmm_id = -1;
+
+#define GET_PMM_STATE_PTR (pmm_state)
+
+/* Internal functions */
+static _mali_osk_errcode_t malipmm_create(_mali_osk_resource_t *resource);
+static void pmm_event_process( void );
+_mali_osk_errcode_t malipmm_irq_uhandler(void *data);
+void malipmm_irq_bhandler(void *data);
+
+/** @brief Start the PMM subsystem
+ *
+ * @param id Subsystem id to uniquely identify this subsystem
+ * @return _MALI_OSK_ERR_OK if the system started successfully, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t malipmm_kernel_subsystem_start( mali_kernel_subsystem_identifier id );
+
+/** @brief Perform post start up of the PMM subsystem
+ *
+ * Post start up includes initializing the current policy, now that the system is
+ * completely started - to stop policies turning off hardware during the start up
+ * 
+ * @param id the unique subsystem id
+ * @return _MALI_OSK_ERR_OK if the post startup was successful, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t malipmm_kernel_load_complete( mali_kernel_subsystem_identifier id );
+
+/** @brief Terminate the PMM subsystem
+ *
+ * @param id the unique subsystem id
+ */
+void malipmm_kernel_subsystem_terminate( mali_kernel_subsystem_identifier id );
+
+#if MALI_STATE_TRACKING
+	void malipmm_subsystem_dump_state( void );
+#endif
+
+
+/* This will be one of the subsystems in the array of subsystems:
+	static struct mali_kernel_subsystem * subsystems[];
+  found in file: mali_kernel_core.c
+*/
+struct mali_kernel_subsystem mali_subsystem_pmm=
+{
+	malipmm_kernel_subsystem_start,                     /* startup */
+	malipmm_kernel_subsystem_terminate,                 /* shutdown */
+	malipmm_kernel_load_complete,                       /* loaded all subsystems */
+	NULL,
+	NULL,
+	NULL,
+	NULL,
+#if MALI_STATE_TRACKING
+	malipmm_subsystem_dump_state,                       /* dump_state */
+#endif
+};
+
+#if PMM_OS_TEST
+
+u32 power_test_event = 0;
+mali_bool power_test_flag = MALI_FALSE;
+_mali_osk_timer_t *power_test_timer = NULL;
+
+void _mali_osk_pmm_power_up_done(mali_pmm_message_data data)
+{
+	MALI_PRINT(("POWER TEST OS UP DONE\n"));
+}
+
+void _mali_osk_pmm_power_down_done(mali_pmm_message_data data)
+{
+	MALI_PRINT(("POWER TEST OS DOWN DONE\n"));
+}
+
+/**
+ * Symbian OS Power Up call to the driver
+ */
+void power_test_callback( void *arg )
+{
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	power_test_flag = MALI_TRUE;
+	_mali_osk_irq_schedulework( pmm->irq );
+}
+
+void power_test_start()
+{
+	power_test_timer = _mali_osk_timer_init();
+	_mali_osk_timer_setcallback( power_test_timer, power_test_callback, NULL );
+	
+	/* First event is power down */
+	power_test_event = MALI_PMM_EVENT_OS_POWER_DOWN;
+	_mali_osk_timer_add( power_test_timer, 10000 );
+}
+
+mali_bool power_test_check()
+{
+	if( power_test_flag )
+	{
+		_mali_uk_pmm_message_s event = {
+					NULL,
+					0,
+					1 };
+		event.id = power_test_event;
+
+		power_test_flag = MALI_FALSE;
+
+		/* Send event */
+		_mali_ukk_pmm_event_message( &event );
+
+		/* Switch to next event to test */
+		if( power_test_event == MALI_PMM_EVENT_OS_POWER_DOWN )
+		{
+			power_test_event = MALI_PMM_EVENT_OS_POWER_UP;
+		}
+		else
+		{
+			power_test_event = MALI_PMM_EVENT_OS_POWER_DOWN;
+		}
+		_mali_osk_timer_add( power_test_timer, 5000 );
+		
+		return MALI_TRUE;
+	}
+	
+	return MALI_FALSE;
+}
+
+void power_test_end()
+{
+	_mali_osk_timer_del( power_test_timer );
+	_mali_osk_timer_term( power_test_timer );
+	power_test_timer = NULL;
+}
+
+#endif
+
+void _mali_ukk_pmm_event_message( _mali_uk_pmm_message_s *args )
+{
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+	_mali_osk_notification_t *msg;
+	mali_pmm_message_t *event;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_DEBUG_ASSERT_POINTER(args);
+
+	MALIPMM_DEBUG_PRINT( ("PMM: sending message\n") );
+
+#if MALI_PMM_TRACE && MALI_PMM_TRACE_SENT_EVENTS
+	_mali_pmm_trace_event_message( args, MALI_FALSE );
+#endif
+
+	msg = _mali_osk_notification_create( MALI_PMM_NOTIFICATION_TYPE, sizeof( mali_pmm_message_t ) );
+
+	if( msg )
+	{
+		event = (mali_pmm_message_t *)msg->result_buffer;
+		event->id = args->id;
+		event->ts = _mali_osk_time_tickcount();
+		event->data = args->data;
+		
+		_mali_osk_atomic_inc( &(pmm->messages_queued) );
+
+		if( args->id > MALI_PMM_EVENT_INTERNALS )
+		{
+			/* Internal PMM message */
+			_mali_osk_notification_queue_send( pmm->iqueue, msg );
+			#if (MALI_PMM_TRACE || MALI_STATE_TRACKING)
+				pmm->imessages_sent++;
+			#endif
+		}
+		else
+		{
+			/* Real event */
+			_mali_osk_notification_queue_send( pmm->queue, msg );
+			#if (MALI_PMM_TRACE || MALI_STATE_TRACKING)
+				pmm->messages_sent++;
+			#endif
+		}
+	}
+	else
+	{
+		MALI_PRINT_ERROR( ("PMM: Could not send message %d", args->id) );
+		/* Make note of this OOM - which has caused a missed event */
+		pmm->missed++;
+	}
+	
+	/* Schedule time to look at the event or the fact we couldn't create an event */
+	_mali_osk_irq_schedulework( pmm->irq );
+}
+
+mali_pmm_state _mali_pmm_state( void )
+{
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	if( pmm && (mali_subsystem_pmm_id != -1) )
+	{
+		return pmm->state;
+	}
+
+	/* No working subsystem yet */
+	return MALI_PMM_STATE_UNAVAILABLE;
+}
+
+
+mali_pmm_core_mask _mali_pmm_cores_list( void )
+{
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	return pmm->cores_registered;
+}
+
+mali_pmm_core_mask _mali_pmm_cores_powered( void )
+{
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	return pmm->cores_powered;
+}
+
+
+_mali_osk_errcode_t _mali_pmm_list_policies(
+		u32 policy_list_size,
+		mali_pmm_policy *policy_list,
+		u32 *policies_available )
+{
+	/* TBD - This is currently a stub function for basic power management */
+
+	MALI_ERROR( _MALI_OSK_ERR_UNSUPPORTED );
+}
+
+_mali_osk_errcode_t _mali_pmm_set_policy( mali_pmm_policy policy )
+{
+	/* TBD - This is currently a stub function for basic power management */
+
+/* TBD - When this is not a stub... include tracing...
+#if MALI_PMM_TRACE
+	_mali_pmm_trace_policy_change( old, newpolicy );
+#endif
+*/
+	MALI_ERROR( _MALI_OSK_ERR_UNSUPPORTED );
+}
+
+_mali_osk_errcode_t _mali_pmm_get_policy( mali_pmm_policy *policy )
+{
+	if( policy )
+	{
+		_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+		MALI_DEBUG_ASSERT_POINTER(pmm);
+
+		if( pmm )
+		{
+			*policy = pmm->policy;
+			MALI_SUCCESS;
+		}
+		else
+		{
+			 *policy = MALI_PMM_POLICY_NONE;
+			 MALI_ERROR( _MALI_OSK_ERR_FAULT );
+		}
+	}
+
+	/* No return argument */
+	MALI_ERROR( _MALI_OSK_ERR_INVALID_ARGS );
+}
+
+#if MALI_PMM_TRACE
+
+/* Event names - order must match mali_pmm_event_id enum */
+static char *pmm_trace_events[] = {
+	"OS_POWER_UP",
+	"OS_POWER_DOWN",
+	"JOB_SCHEDULED",
+	"JOB_QUEUED",
+	"JOB_FINISHED",
+	"TIMEOUT",
+};
+
+/* UK event names - order must match mali_pmm_event_id enum */
+static char *pmm_trace_events_uk[] = {
+	"UKS",
+	"UK_EXAMPLE",
+};
+
+/* Internal event names - order must match mali_pmm_event_id enum */
+static char *pmm_trace_events_internal[] = {
+	"INTERNALS",
+	"INTERNAL_POWER_UP_ACK",
+	"INTERNAL_POWER_DOWN_ACK",
+};
+
+/* State names - order must match mali_pmm_state enum */
+static char *pmm_trace_state[] = {
+	"UNAVAILABLE",
+	"SYSTEM ON",
+	"SYSTEM OFF",
+	"SYSTEM TRANSITION",
+};
+
+/* Policy names - order must match mali_pmm_policy enum */
+static char *pmm_trace_policy[] = {
+	"NONE",
+	"ALWAYS ON",
+	"JOB CONTROL",
+};
+
+void _mali_pmm_trace_hardware_change( mali_pmm_core_mask old, mali_pmm_core_mask newstate )
+{
+	const char *dname;
+	const char *cname;
+	const char *ename;
+
+	if( old != newstate )
+	{
+		if( newstate == 0 )
+		{
+			dname = "NO cores";
+		}
+		else
+		{
+			dname = pmm_trace_get_core_name( newstate );
+		}
+			
+		/* These state checks only work if the assumption that only cores can be
+		 * turned on or turned off in seperate actions is true. If core power states can
+		 * be toggled (some one, some off) at the same time, this check does not work
+		 */
+		if( old > newstate )
+		{
+			/* Cores have turned off */
+			cname = pmm_trace_get_core_name( old - newstate );
+			ename = "OFF";
+		}
+		else
+		{
+			/* Cores have turned on */
+			cname = pmm_trace_get_core_name( newstate - old );
+			ename = "ON";
+		}
+		MALI_PRINT( ("PMM Trace: Hardware %s ON, %s just turned %s. { 0x%08x -> 0x%08x }", dname, cname, ename, old, newstate) );
+	}
+}
+
+void _mali_pmm_trace_state_change( mali_pmm_state old, mali_pmm_state newstate )
+{
+	if( old != newstate )
+	{
+		MALI_PRINT( ("PMM Trace: State changed from %s to %s", pmm_trace_state[old], pmm_trace_state[newstate]) );
+	}
+}
+
+void _mali_pmm_trace_policy_change( mali_pmm_policy old, mali_pmm_policy newpolicy )
+{
+	if( old != newpolicy )
+	{
+		MALI_PRINT( ("PMM Trace: Policy changed from %s to %s", pmm_trace_policy[old], pmm_trace_policy[newpolicy]) );
+	}
+}
+
+void _mali_pmm_trace_event_message( mali_pmm_message_t *event, mali_bool received )
+{
+	const char *ename;
+	const char *dname;
+	const char *tname;
+	const char *format = "PMM Trace: Event %s { (%d) %s, %d ticks, (0x%x) %s }";
+
+	MALI_DEBUG_ASSERT_POINTER(event);
+
+	tname = (received) ? "received" : "sent";
+
+	if( event->id >= MALI_PMM_EVENT_INTERNALS )
+	{
+		ename = pmm_trace_events_internal[((int)event->id) - MALI_PMM_EVENT_INTERNALS];
+	}
+	else if( event->id >= MALI_PMM_EVENT_UKS )
+	{
+		ename = pmm_trace_events_uk[((int)event->id) - MALI_PMM_EVENT_UKS];
+	}
+	else
+	{
+		ename = pmm_trace_events[event->id];
+	}
+
+	switch( event->id )
+	{
+		case MALI_PMM_EVENT_OS_POWER_UP:
+		case MALI_PMM_EVENT_OS_POWER_DOWN:
+			dname = "os event";
+			break;
+
+		case MALI_PMM_EVENT_JOB_SCHEDULED:
+		case MALI_PMM_EVENT_JOB_QUEUED:
+		case MALI_PMM_EVENT_JOB_FINISHED:
+		case MALI_PMM_EVENT_INTERNAL_POWER_UP_ACK:
+		case MALI_PMM_EVENT_INTERNAL_POWER_DOWN_ACK:
+			dname = pmm_trace_get_core_name( (mali_pmm_core_mask)event->data );
+			break;
+			
+		case MALI_PMM_EVENT_TIMEOUT:
+			dname = "timeout start";
+			/* Print data with a different format */
+			format = "PMM Trace: Event %s { (%d) %s, %d ticks, %d ticks %s }";
+			break;
+		default:
+			dname = "unknown data";
+	}
+
+	MALI_PRINT( (format, tname, (u32)event->id, ename, event->ts, (u32)event->data, dname) );
+}
+
+#endif /* MALI_PMM_TRACE */
+
+
+/****************** Mali Kernel API *****************/
+
+_mali_osk_errcode_t malipmm_kernel_subsystem_start( mali_kernel_subsystem_identifier id )
+{
+	mali_subsystem_pmm_id = id;
+	MALI_CHECK_NO_ERROR(_mali_kernel_core_register_resource_handler(PMU, malipmm_create));
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t malipmm_create(_mali_osk_resource_t *resource)
+{
+	/* Create PMM state memory */
+	MALI_DEBUG_ASSERT( pmm_state == NULL );
+	pmm_state = (_mali_pmm_internal_state_t *) _mali_osk_malloc(sizeof(*pmm_state));
+	MALI_CHECK_NON_NULL( pmm_state, _MALI_OSK_ERR_NOMEM );	
+
+	/* All values get 0 as default */
+	_mali_osk_memset(pmm_state, 0, sizeof(*pmm_state));
+
+	/* Set up the initial PMM state */
+	pmm_state->waiting = 0;
+	pmm_state->status = MALI_PMM_STATUS_IDLE;
+	pmm_state->state = MALI_PMM_STATE_UNAVAILABLE; /* Until a core registers */
+
+	/* Set up policy via compile time option for the moment */
+#if MALI_PMM_ALWAYS_ON
+	pmm_state->policy = MALI_PMM_POLICY_ALWAYS_ON;
+#else 
+	pmm_state->policy = MALI_PMM_POLICY_JOB_CONTROL;
+#endif
+	
+#if MALI_PMM_TRACE
+	_mali_pmm_trace_policy_change( MALI_PMM_POLICY_NONE, pmm_state->policy );
+#endif
+
+	/* Set up assumes all values are initialized to NULL or MALI_FALSE, so
+	 * we can exit halfway through set up and perform clean up
+	 */
+#if !MALI_PMM_NO_PMU
+	if( mali_platform_init(resource) != _MALI_OSK_ERR_OK ) goto pmm_fail_cleanup;
+	pmm_state->pmu_initialized = MALI_TRUE;
+#endif
+
+	pmm_state->queue = _mali_osk_notification_queue_init();
+	if( !pmm_state->queue ) goto pmm_fail_cleanup;
+
+	pmm_state->iqueue = _mali_osk_notification_queue_init();
+	if( !pmm_state->iqueue ) goto pmm_fail_cleanup;
+
+	/* We are creating an IRQ handler just for the worker thread it gives us */
+	pmm_state->irq = _mali_osk_irq_init( _MALI_OSK_IRQ_NUMBER_PMM,
+		malipmm_irq_uhandler,
+		malipmm_irq_bhandler,
+		NULL,
+		NULL,
+		(void *)pmm_state,            /* PMM state is passed to IRQ */
+		"PMM handler" );
+
+	if( !pmm_state->irq ) goto pmm_fail_cleanup;
+
+	pmm_state->lock = _mali_osk_lock_init((_mali_osk_lock_flags_t)(_MALI_OSK_LOCKFLAG_READERWRITER | _MALI_OSK_LOCKFLAG_ORDERED), 0, 75);
+	if( !pmm_state->lock ) goto pmm_fail_cleanup;
+
+	if( _mali_osk_atomic_init( &(pmm_state->messages_queued), 0 ) != _MALI_OSK_ERR_OK )
+	{
+		goto pmm_fail_cleanup;
+	}
+
+	MALIPMM_DEBUG_PRINT( ("PMM: subsystem created, policy=%d\n", pmm_state->policy) );
+
+	MALI_SUCCESS;
+
+pmm_fail_cleanup:
+	MALI_PRINT_ERROR( ("PMM: subsystem failed to be created\n") );
+	if( pmm_state )
+	{
+		_mali_osk_resource_type_t t = PMU;
+		if( pmm_state->lock ) _mali_osk_lock_term( pmm_state->lock );
+		if( pmm_state->irq ) _mali_osk_irq_term( pmm_state->irq );
+		if( pmm_state->queue ) _mali_osk_notification_queue_term( pmm_state->queue );
+		if( pmm_state->iqueue ) _mali_osk_notification_queue_term( pmm_state->iqueue );		
+		if( pmm_state->pmu_initialized ) ( mali_platform_deinit(&t) );
+		_mali_osk_free(pmm_state);
+		pmm_state = NULL; 
+	}
+	MALI_ERROR( _MALI_OSK_ERR_FAULT );
+}
+
+_mali_osk_errcode_t malipmm_kernel_load_complete( mali_kernel_subsystem_identifier id )
+{
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	MALIPMM_DEBUG_PRINT( ("PMM: subsystem loaded, policy initializing\n") );
+
+#if PMM_OS_TEST
+	power_test_start();
+#endif
+
+	/* Initialize the profile now the system has loaded - so that cores are 
+	 * not turned off during start up 
+	 */
+	return pmm_policy_init( pmm );
+}
+
+void malipmm_kernel_subsystem_terminate( mali_kernel_subsystem_identifier id )
+{
+	/* Check this is the right system */
+	MALI_DEBUG_ASSERT( id == mali_subsystem_pmm_id );
+	MALI_DEBUG_ASSERT_POINTER(pmm_state);
+
+	if( pmm_state )
+	{
+		_mali_osk_resource_type_t t = PMU;
+#if PMM_OS_TEST
+		power_test_end();
+#endif
+		/* Get the lock so we can shutdown */
+		MALI_PMM_LOCK(pmm_state);
+#if MALI_STATE_TRACKING
+		pmm_state->mali_pmm_lock_acquired = 1;
+#endif /* MALI_STATE_TRACKING */
+		pmm_state->status = MALI_PMM_STATUS_OFF;
+#if MALI_STATE_TRACKING
+		pmm_state->mali_pmm_lock_acquired = 0;
+#endif /* MALI_STATE_TRACKING */
+		MALI_PMM_UNLOCK(pmm_state);
+		pmm_policy_term(pmm_state);
+		_mali_osk_irq_term( pmm_state->irq );
+		_mali_osk_notification_queue_term( pmm_state->queue );
+		_mali_osk_notification_queue_term( pmm_state->iqueue );
+		if( pmm_state->pmu_initialized ) mali_platform_deinit(&t);
+		_mali_osk_atomic_term( &(pmm_state->messages_queued) );
+		MALI_PMM_LOCK_TERM(pmm_state);
+		_mali_osk_free(pmm_state);
+		pmm_state = NULL; 
+	}
+
+	MALIPMM_DEBUG_PRINT( ("PMM: subsystem terminated\n") );
+}
+
+_mali_osk_errcode_t malipmm_core_register( mali_pmm_core_id core )
+{
+	_mali_osk_errcode_t err;
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+
+	if( pmm == NULL )
+	{
+		/* PMM state has not been created, this is because the PMU resource has not been 
+		 * created yet.
+		 * This probably means that the PMU resource has not been specfied as the first 
+		 * resource in the config file
+		 */
+		MALI_PRINT_ERROR( ("PMM: Cannot register core %s because the PMU resource has not been\n initialized. Please make sure the PMU resource is the first resource in the\n resource configuration.\n", 
+							pmm_trace_get_core_name(core)) );
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	MALI_PMM_LOCK(pmm);
+
+#if MALI_STATE_TRACKING
+	pmm->mali_pmm_lock_acquired = 1;
+#endif /* MALI_STATE_TRACKING */
+
+
+	/* Check if the core is registered more than once in PMM */
+	MALI_DEBUG_ASSERT( (pmm->cores_registered & core) == 0 );
+
+	MALIPMM_DEBUG_PRINT( ("PMM: core registered: (0x%x) %s\n", core, pmm_trace_get_core_name(core)) );
+
+#if !MALI_PMM_NO_PMU
+	/* Make sure the core is powered up */
+	err = mali_platform_powerup( core );
+#else
+	err = _MALI_OSK_ERR_OK;
+#endif
+	if( _MALI_OSK_ERR_OK == err )
+	{
+#if MALI_PMM_TRACE
+		mali_pmm_core_mask old_power = pmm->cores_powered;
+#endif
+		/* Assume a registered core is now powered up and idle */
+		pmm->cores_registered |= core;
+		pmm->cores_idle |= core;
+		pmm->cores_powered |= core;
+		pmm_update_system_state( pmm );
+
+#if MALI_PMM_TRACE
+		_mali_pmm_trace_hardware_change( old_power, pmm->cores_powered );
+#endif		
+	}
+	else
+	{
+		MALI_PRINT_ERROR( ("PMM: Error(%d) powering up registered core: (0x%x) %s\n", 
+								err, core, pmm_trace_get_core_name(core)) );
+	}
+
+#if MALI_STATE_TRACKING
+	pmm->mali_pmm_lock_acquired = 0;
+#endif /* MALI_STATE_TRACKING */
+
+	MALI_PMM_UNLOCK(pmm);
+	
+	return err;
+}
+
+void malipmm_core_unregister( mali_pmm_core_id core )
+{
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	MALI_PMM_LOCK(pmm);
+#if MALI_STATE_TRACKING
+	pmm->mali_pmm_lock_acquired = 1;
+#endif /* MALI_STATE_TRACKING */
+
+
+	/* Check if the core is registered in PMM */
+	MALI_PMM_DEBUG_ASSERT_CORES_SUBSET( pmm->cores_registered, core );
+
+	MALIPMM_DEBUG_PRINT( ("PMM: core unregistered: (0x%x) %s\n", core, pmm_trace_get_core_name(core)) );
+
+	{
+#if MALI_PMM_TRACE
+		mali_pmm_core_mask old_power = pmm->cores_powered;
+#endif
+
+#if !MALI_PMM_NO_PMU
+		/* Turn off the core */
+		if( mali_platform_powerdown( core ) != _MALI_OSK_ERR_OK )
+		{
+			MALI_PRINT_ERROR( ("PMM: Error powering down unregistered core: (0x%x) %s\n", 
+									core, pmm_trace_get_core_name(core)) );
+		}
+#endif
+
+		/* Remove the core from the system */
+		pmm->cores_registered &= (~core);
+		pmm->cores_idle &= (~core);
+		pmm->cores_powered &= (~core);
+		pmm->cores_pend_down &= (~core);
+		pmm->cores_pend_up &= (~core);
+		pmm->cores_ack_down &= (~core);
+		pmm->cores_ack_up &= (~core);
+
+		pmm_update_system_state( pmm );
+
+#if MALI_PMM_TRACE
+		_mali_pmm_trace_hardware_change( old_power, pmm->cores_powered );
+#endif		
+	}
+
+#if MALI_STATE_TRACKING
+	pmm->mali_pmm_lock_acquired = 0;
+#endif /* MALI_STATE_TRACKING */
+
+	MALI_PMM_UNLOCK(pmm);
+}
+void malipmm_core_power_down_okay( mali_pmm_core_id core )
+{
+	_mali_uk_pmm_message_s event = {
+		NULL,
+		MALI_PMM_EVENT_INTERNAL_POWER_DOWN_ACK,
+		0 };
+
+	event.data = core;
+
+	_mali_ukk_pmm_event_message( &event );
+}
+
+void malipmm_set_policy_check()
+{
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	pmm->check_policy = MALI_TRUE;
+
+	/* To check the policy we need to schedule some work */
+	_mali_osk_irq_schedulework( pmm->irq );
+}
+
+_mali_osk_errcode_t malipmm_irq_uhandler(void *data)
+{
+	MALIPMM_DEBUG_PRINT( ("PMM: uhandler - not expected to be used\n") );
+
+	MALI_SUCCESS;
+}
+
+void malipmm_irq_bhandler(void *data)
+{
+	_mali_pmm_internal_state_t *pmm;
+	pmm = (_mali_pmm_internal_state_t *)data;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+#if PMM_OS_TEST
+	if( power_test_check() ) return;
+#endif
+
+	MALI_PMM_LOCK(pmm);
+#ifdef MALI_STATE_TRACKING
+	pmm->mali_pmm_lock_acquired = 1;
+#endif /* MALI_STATE_TRACKING */
+
+	/* Quick out when we are shutting down */
+	if( pmm->status == MALI_PMM_STATUS_OFF )
+	{
+
+	#if MALI_STATE_TRACKING
+		pmm->mali_pmm_lock_acquired = 0;
+	#endif /* MALI_STATE_TRACKING */
+
+		MALI_PMM_UNLOCK(pmm);
+		return;
+	}
+
+	MALIPMM_DEBUG_PRINT( ("PMM: bhandler - Processing event\n") );
+
+	if( pmm->missed > 0 )
+	{
+		MALI_PRINT_ERROR( ("PMM: Failed to send %d events", pmm->missed) );
+		pmm_fatal_reset( pmm );
+	}
+	
+	if( pmm->check_policy )
+	{
+		pmm->check_policy = MALI_FALSE;
+		pmm_policy_check_policy(pmm);
+	}
+	else
+	{
+		/* Perform event processing */
+		pmm_event_process();
+		if( pmm->fatal_power_err )
+		{
+			/* Try a reset */
+			pmm_fatal_reset( pmm );
+		}
+	}
+
+#if MALI_STATE_TRACKING
+	pmm->mali_pmm_lock_acquired = 0;
+#endif /* MALI_STATE_TRACKING */
+
+	MALI_PMM_UNLOCK(pmm);
+}
+
+static void pmm_event_process( void )
+{
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_OK;
+	_mali_osk_notification_t *msg = NULL;
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+	mali_pmm_message_t *event;
+	u32 process_messages;
+
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+
+	/* Max number of messages to process before exiting - as we shouldn't stay
+	 * processing the messages for a long time
+	 */
+	process_messages = _mali_osk_atomic_read( &(pmm->messages_queued) );
+
+	while( process_messages > 0 )
+	{
+		/* Check internal message queue first */
+		err = _mali_osk_notification_queue_dequeue( pmm->iqueue, &msg );
+
+		if( err != _MALI_OSK_ERR_OK )
+		{
+			if( pmm->status == MALI_PMM_STATUS_IDLE || pmm->status == MALI_PMM_STATUS_OS_WAITING || pmm->status == MALI_PMM_STATUS_DVFS_PAUSE) 	
+			{
+				if( pmm->waiting > 0 ) pmm->waiting--;
+
+				/* We aren't busy changing state, so look at real events */
+				err = _mali_osk_notification_queue_dequeue( pmm->queue, &msg );
+
+				if( err != _MALI_OSK_ERR_OK )
+				{
+					pmm->no_events++;
+					MALIPMM_DEBUG_PRINT( ("PMM: event_process - No message to process\n") );
+					/* Nothing to do - so return */
+					return;
+				}
+				else
+				{					
+					#if (MALI_PMM_TRACE || MALI_STATE_TRACKING)
+						pmm->messages_received++;
+					#endif		
+				}
+			}
+			else
+			{
+				/* Waiting for an internal message */
+				pmm->waiting++;
+				MALIPMM_DEBUG_PRINT( ("PMM: event_process - Waiting for internal message, messages queued=%d\n", pmm->waiting) );
+				return;
+			}
+		}
+		else
+		{
+			#if (MALI_PMM_TRACE || MALI_STATE_TRACKING)
+				pmm->imessages_received++;
+			#endif		
+		}
+
+		MALI_DEBUG_ASSERT_POINTER( msg );
+		/* Check the message type matches */
+		MALI_DEBUG_ASSERT( msg->notification_type == MALI_PMM_NOTIFICATION_TYPE );
+
+		event = msg->result_buffer;
+
+		_mali_osk_atomic_dec( &(pmm->messages_queued) );
+		process_messages--;
+
+		#if MALI_PMM_TRACE
+			/* Trace before we process the event in case we have an error */
+			_mali_pmm_trace_event_message( event, MALI_TRUE );
+		#endif
+		err = pmm_policy_process( pmm, event );
+
+		
+		if( err != _MALI_OSK_ERR_OK )
+		{
+			MALI_PRINT_ERROR( ("PMM: Error(%d) in policy %d when processing event message with id: %d", 
+					err, pmm->policy, event->id) );
+		}
+		
+		/* Delete notification */
+		_mali_osk_notification_delete ( msg );
+
+		if( pmm->fatal_power_err )
+		{
+			/* Nothing good has happened - exit */
+			return;
+		}
+
+			
+		#if MALI_PMM_TRACE
+			MALI_PRINT( ("PMM Trace: Event processed, msgs (sent/read) = %d/%d, int msgs (sent/read) = %d/%d, no events = %d, waiting = %d\n", 
+					pmm->messages_sent, pmm->messages_received, pmm->imessages_sent, pmm->imessages_received, pmm->no_events, pmm->waiting) );
+		#endif
+	}
+
+	if( pmm->status == MALI_PMM_STATUS_IDLE && pmm->waiting > 0 )
+	{
+		/* For events we ignored whilst we were busy, add a new
+		 * scheduled time to look at them */
+		_mali_osk_irq_schedulework( pmm->irq );
+	}
+}
+
+#if MALI_STATE_TRACKING
+void malipmm_subsystem_dump_state(void)
+{
+	malipmm_state_dump();	
+}
+#endif
+
+#if (defined(DEBUG) || MALI_STATE_TRACKING)
+void malipmm_state_dump()
+{
+	_mali_pmm_internal_state_t *pmm = GET_PMM_STATE_PTR;
+
+	if( !pmm )
+	{
+		MALI_PRINT(("PMM: Null state\n"));
+	}
+	else
+	{
+		MALI_PRINT(("Locks::\nPMM_LOCK_STATUS=%ld",pmm->mali_pmm_lock_acquired));
+		MALI_PRINT(("PMM state:\nPrevious_status=%d\nstatus=%d\nCurrent_event=%d\npolicy=%d\ncheck_policy=%d\nstate=%d\n", pmm->mali_last_pmm_status,pmm->status, pmm->mali_new_event_status, pmm->policy, pmm->check_policy, pmm->state));
+		MALI_PRINT(("PMM cores:\ncores_registered=%d\ncores_powered=%d\ncores_idle=%d\ncores_pend_down=%d\ncores_pend_up=%d\ncores_ack_down=%d\ncores_ack_up=%d\n", pmm->cores_registered, pmm->cores_powered, pmm->cores_idle, pmm->cores_pend_down, pmm->cores_pend_up, pmm->cores_ack_down, pmm->cores_ack_up));	
+		MALI_PRINT(("PMM misc:\npmu_init=%d\nmessages_queued=%d\nwaiting=%d\nno_events=%d\nmissed=%d\nfatal_power_err=%d\n", pmm->pmu_initialized, _mali_osk_atomic_read( &(pmm->messages_queued) ), pmm->waiting, pmm->no_events, pmm->missed, pmm->fatal_power_err));
+	}
+}
+#endif
+
+#endif /* USING_MALI_PMM */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm.h
new file mode 100644
index 00000000000..fe7a046cb47
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm.h
@@ -0,0 +1,323 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm.h
+ * Defines the power management module for the kernel device driver
+ */
+
+#ifndef __MALI_PMM_H__
+#define __MALI_PMM_H__
+
+/* For mali_pmm_message_data and MALI_PMM_EVENT_UK_* defines */
+#include "mali_uk_types.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @defgroup pmmapi Power Management Module APIs
+ *
+ * @{
+ */
+
+/** OS event tester */
+#define PMM_OS_TEST 0
+
+/** @brief Compile option to turn on/off tracing */
+#define MALI_PMM_TRACE 0
+#define MALI_PMM_TRACE_SENT_EVENTS 0
+
+/** @brief Compile option to switch between always on or job control PMM policy */
+#define MALI_PMM_ALWAYS_ON 0
+
+/** @brief Overrides hardware PMU and uses software simulation instead 
+ *  @note This even stops intialization of PMU and cores being powered on at start up
+ */
+#define MALI_PMM_NO_PMU 0
+
+/** @brief PMM debug print to control debug message level */
+#define MALIPMM_DEBUG_PRINT(args) \
+	MALI_DEBUG_PRINT(3, args)
+
+
+/** @brief power management event message identifiers.
+ */
+/* These must match up with the pmm_trace_events & pmm_trace_events_internal 
+ * arrays
+ */
+typedef enum mali_pmm_event_id
+{
+	MALI_PMM_EVENT_OS_POWER_UP             =    0,   /**< OS power up event */
+	MALI_PMM_EVENT_OS_POWER_DOWN           =    1,   /**< OS power down event */
+	MALI_PMM_EVENT_JOB_SCHEDULED           =    2,   /**< Job scheduled to run event */
+	MALI_PMM_EVENT_JOB_QUEUED              =    3,   /**< Job queued (but not run) event */
+	MALI_PMM_EVENT_JOB_FINISHED            =    4,   /**< Job finished event */
+	MALI_PMM_EVENT_TIMEOUT                 =    5,   /**< Time out timer has expired */
+	MALI_PMM_EVENT_DVFS_PAUSE              =    6,   /**< Mali device pause event */
+	MALI_PMM_EVENT_DVFS_RESUME             =    7,   /**< Mali device resume event */
+
+	MALI_PMM_EVENT_UKS                     =  200,   /**< Events from the user-side start here */
+	MALI_PMM_EVENT_UK_EXAMPLE              =  _MALI_PMM_EVENT_UK_EXAMPLE,
+
+	MALI_PMM_EVENT_INTERNALS               = 1000,
+	MALI_PMM_EVENT_INTERNAL_POWER_UP_ACK   = 1001,   /**< Internal power up acknowledgement */
+	MALI_PMM_EVENT_INTERNAL_POWER_DOWN_ACK = 1002,   /**< Internal power down acknowledgment */
+} mali_pmm_event_id;
+
+
+/** @brief Use this when the power up/down callbacks do not need any OS data. */
+#define MALI_PMM_NO_OS_DATA 1
+
+
+/* @brief Geometry and pixel processor identifiers for the PMM
+ *
+ * @note these match the ARM Mali 400 PMU hardware definitions, apart from the "SYSTEM"
+ */
+typedef enum mali_pmm_core_id_tag
+{
+	MALI_PMM_CORE_SYSTEM = 0x00000000,          /**< All of the Mali hardware */
+	MALI_PMM_CORE_GP     = 0x00000001,          /**< Mali GP2 */
+	MALI_PMM_CORE_L2     = 0x00000002,          /**< Level 2 cache */
+	MALI_PMM_CORE_PP0    = 0x00000004,          /**< Mali 200 pixel processor 0 */
+	MALI_PMM_CORE_PP1    = 0x00000008,          /**< Mali 200 pixel processor 1 */
+	MALI_PMM_CORE_PP2    = 0x00000010,          /**< Mali 200 pixel processor 2 */
+	MALI_PMM_CORE_PP3    = 0x00000020,          /**< Mali 200 pixel processor 3 */
+	MALI_PMM_CORE_PP_ALL = 0x0000003C           /**< Mali 200 pixel processors 0-3 */
+} mali_pmm_core_id;
+
+/* @brief PMM bitmask of mali_pmm_core_ids
+ */
+typedef u32 mali_pmm_core_mask;
+
+/* @brief PMM event timestamp type
+ */
+typedef u32 mali_pmm_timestamp;
+
+/** @brief power management event message struct
+ */
+typedef struct _mali_pmm_message
+{
+	mali_pmm_event_id id;               /**< event id */
+	mali_pmm_message_data data;         /**< specific data associated with the event */
+	mali_pmm_timestamp ts;              /**< timestamp the event was placed in the event queue */
+} mali_pmm_message_t;
+
+
+
+/** @brief the state of the power management module.
+ */
+/* These must match up with the pmm_trace_state array */
+typedef enum mali_pmm_state_tag
+{
+	MALI_PMM_STATE_UNAVAILABLE       = 0,       /**< PMM is not available */
+	MALI_PMM_STATE_SYSTEM_ON         = 1,       /**< All of the Mali hardware is on */
+	MALI_PMM_STATE_SYSTEM_OFF        = 2,       /**< All of the Mali hardware is off */
+	MALI_PMM_STATE_SYSTEM_TRANSITION = 3        /**< System is changing state */
+} mali_pmm_state;
+
+
+/** @brief a power management policy.
+ */
+/* These must match up with the pmm_trace_policy array */
+typedef enum mali_pmm_policy_tag
+{
+	MALI_PMM_POLICY_NONE        = 0,            /**< No policy */
+	MALI_PMM_POLICY_ALWAYS_ON   = 1,            /**< Always on policy */
+	MALI_PMM_POLICY_JOB_CONTROL = 2,            /**< Job control policy */
+	MALI_PMM_POLICY_RUNTIME_JOB_CONTROL = 3     /**< Run time power management control policy */
+} mali_pmm_policy;
+
+/** @brief Function to report to the OS when the power down has finished
+ *
+ * @param data The event message data that initiated the power down
+ */
+void _mali_osk_pmm_power_down_done(mali_pmm_message_data data);
+
+/** @brief Function to report to the OS when the power up has finished
+ *
+ * @param data The event message data that initiated the power up
+ */
+void _mali_osk_pmm_power_up_done(mali_pmm_message_data data);
+
+/** @brief Function to report that DVFS operation done 
+ *
+ * @param data The event message data
+ */
+void _mali_osk_pmm_dvfs_operation_done(mali_pmm_message_data data);
+
+#if MALI_POWER_MGMT_TEST_SUITE
+/** @brief Function to notify power management events
+ *
+ * @param data The event message data
+ */
+void _mali_osk_pmm_policy_events_notifications(mali_pmm_event_id event_id);
+
+#endif
+
+/** @brief Function to report the OS that device is idle
+ *
+ *  @note inform the OS that device is idle
+ */
+_mali_osk_errcode_t _mali_osk_pmm_dev_idle( void );
+
+/** @brief Function to report the OS to activate device
+ *
+ * @note inform the os that device needs to be activated
+ */
+void _mali_osk_pmm_dev_activate( void );
+
+/** @brief Queries the current state of the PMM software
+ *
+ * @note the state of the PMM can change after this call has returned
+ *
+ * @return the current PMM state value
+ */
+mali_pmm_state _mali_pmm_state( void );
+
+/** @brief List of cores that are registered with the PMM
+ *
+ * This will return the cores that have been currently registered with the PMM,
+ * which is a bitwise OR of the mali_pmm_core_id_tags. A value of 0x0 means that
+ * there are no cores registered.
+ *
+ * @note the list of cores can change after this call has returned
+ *
+ * @return a bit mask representing all the cores that have been registered with the PMM
+ */
+mali_pmm_core_mask _mali_pmm_cores_list( void );
+
+/** @brief List of cores that are powered up in the PMM
+ *
+ * This will return the subset of the cores that can be listed using mali_pmm_cores_
+ * list, that have power. It is a bitwise OR of the mali_pmm_core_id_tags. A value of
+ * 0x0 means that none of the cores registered are powered.
+ *
+ * @note the list of cores can change after this call has returned
+ *
+ * @return a bit mask representing all the cores that are powered up
+ */
+mali_pmm_core_mask _mali_pmm_cores_powered( void );
+
+
+/** @brief List of power management policies that are supported by the PMM
+ *
+ * Given an empty array of policies - policy_list - which contains the number
+ * of entries as specified by - policy_list_size, this function will populate
+ * the list with the available policies. If the policy_list is too small for
+ * all the policies then only policy_list_size entries will be returned. If the
+ * policy_list is bigger than the number of available policies then, the extra
+ * entries will be set to MALI_PMM_POLICY_NONE.
+ * The function will also update available_policies with the number of policies
+ * that are available, even if it exceeds the policy_list_size.
+ * The function will succeed if all policies could be returned, else it will
+ * fail if none or only a subset of policies could be returned.
+ * The function will also fail if no policy_list is supplied, though
+ * available_policies is optional.
+ *
+ * @note this is a STUB function and is not yet implemented
+ *
+ * @param policy_list_size is the number of policies that can be returned in
+ * the policy_list argument
+ * @param policy_list is an array of policies that should be populated with
+ * the list of policies that are supported by the PMM
+ * @param policies_available optional argument, if non-NULL will be set to the
+ * number of policies available
+ * @return _MALI_OSK_ERR_OK if the policies could be listed, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t _mali_pmm_list_policies(
+		u32 policy_list_size,
+		mali_pmm_policy *policy_list,
+		u32 *policies_available );
+
+/** @brief Set the power management policy in the PMM
+ *
+ * Given a valid supported policy, this function will change the PMM to use
+ * this new policy
+ * The function will fail if the policy given is invalid or unsupported.
+ *
+ * @note this is a STUB function and is not yet implemented
+ *
+ * @param policy the new policy to be set
+ * @return _MALI_OSK_ERR_OK if the policy could be set, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t _mali_pmm_set_policy( mali_pmm_policy policy );
+
+/** @brief Get the current power management policy in the PMM
+ *
+ * Given a pointer to a policy data type, this function will return the current
+ * policy that is in effect for the PMM. This maybe out of date if there is a
+ * pending set policy call that has not been serviced.
+ * The function will fail if the policy given is NULL.
+ *
+ * @note the policy of the PMM can change after this call has returned
+ *
+ * @param policy a pointer to a policy that can be updated to the current
+ * policy
+ * @return _MALI_OSK_ERR_OK if the policy could be returned, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t _mali_pmm_get_policy( mali_pmm_policy *policy );
+
+#if MALI_PMM_TRACE
+
+/** @brief Indicates when a hardware state change occurs in the PMM
+ *
+ * @param old a mask of the cores indicating the previous state of the cores
+ * @param newstate a mask of the cores indicating the new current state of the cores
+ */
+void _mali_pmm_trace_hardware_change( mali_pmm_core_mask old, mali_pmm_core_mask newstate );
+
+/** @brief Indicates when a state change occurs in the PMM
+ *
+ * @param old the previous state for the PMM
+ * @param newstate the new current state of the PMM
+ */
+void _mali_pmm_trace_state_change( mali_pmm_state old, mali_pmm_state newstate );
+
+/** @brief Indicates when a policy change occurs in the PMM
+ *
+ * @param old the previous policy for the PMM
+ * @param newpolicy the new current policy of the PMM
+ */
+void _mali_pmm_trace_policy_change( mali_pmm_policy old, mali_pmm_policy newpolicy );
+
+/** @brief Records when an event message is read by the event system
+ *
+ * @param event the message details
+ * @param received MALI_TRUE when the message is received by the PMM, else it is being sent
+ */
+void _mali_pmm_trace_event_message( mali_pmm_message_t *event, mali_bool received );
+
+#endif /* MALI_PMM_TRACE */
+
+/** @brief Dumps the current state of OS PMM thread
+ */
+#if MALI_STATE_TRACKING
+void mali_pmm_dump_os_thread_state( void );
+#endif /* MALI_STATE_TRACKING */
+
+#if (defined(DEBUG) || MALI_STATE_TRACKING)
+/** @brief Dumps the current state of the PMM
+ */
+void malipmm_state_dump( void );
+#endif
+
+/** @} */ /* end group pmmapi */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_PMM_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy.c
new file mode 100644
index 00000000000..327e8b4c853
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy.c
@@ -0,0 +1,243 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm_policy.c
+ * Implementation of the common routines for power management module 
+ * policies
+ */
+
+#if USING_MALI_PMM
+
+#include "mali_ukk.h"
+#include "mali_kernel_common.h"
+
+#include "mali_pmm.h"
+#include "mali_pmm_system.h"
+#include "mali_pmm_state.h"
+#include "mali_pmm_policy.h"
+
+#include "mali_pmm_policy_alwayson.h"
+#include "mali_pmm_policy_jobcontrol.h"
+
+/* Call back function for timer expiration */
+static void pmm_policy_timer_callback( void *arg );
+
+_mali_osk_errcode_t pmm_policy_timer_init( _pmm_policy_timer_t *pptimer, u32 timeout, mali_pmm_event_id id )
+{
+	MALI_DEBUG_ASSERT_POINTER(pptimer);
+	
+	/* All values get 0 as default */
+	_mali_osk_memset(pptimer, 0, sizeof(*pptimer));
+	
+	pptimer->timer = _mali_osk_timer_init();
+	if( pptimer->timer )
+	{
+		_mali_osk_timer_setcallback( pptimer->timer, pmm_policy_timer_callback, (void *)pptimer );
+		pptimer->timeout = timeout;
+		pptimer->event_id = id;
+		MALI_SUCCESS;
+	}
+	
+	return _MALI_OSK_ERR_FAULT;
+}
+
+static void pmm_policy_timer_callback( void *arg )
+{
+	_pmm_policy_timer_t *pptimer = (_pmm_policy_timer_t *)arg;
+	
+	MALI_DEBUG_ASSERT_POINTER(pptimer);
+	MALI_DEBUG_ASSERT( pptimer->set );
+
+	/* Set timer expired and flag there is a policy to check */
+	pptimer->expired = MALI_TRUE;
+	malipmm_set_policy_check();
+}
+
+
+void pmm_policy_timer_term( _pmm_policy_timer_t *pptimer )
+{
+	MALI_DEBUG_ASSERT_POINTER(pptimer);
+
+	_mali_osk_timer_del( pptimer->timer );
+	_mali_osk_timer_term( pptimer->timer );
+	pptimer->timer = NULL;
+}
+
+mali_bool pmm_policy_timer_start( _pmm_policy_timer_t *pptimer )
+{
+	MALI_DEBUG_ASSERT_POINTER(pptimer);
+	MALI_DEBUG_ASSERT_POINTER(pptimer->timer);
+
+	if( !(pptimer->set) )
+	{
+		pptimer->set = MALI_TRUE;
+		pptimer->expired = MALI_FALSE;
+		pptimer->start = _mali_osk_time_tickcount();
+		_mali_osk_timer_add( pptimer->timer, pptimer->timeout );
+		return MALI_TRUE;
+	}
+	
+	return MALI_FALSE;
+}
+
+mali_bool pmm_policy_timer_stop( _pmm_policy_timer_t *pptimer )
+{
+	MALI_DEBUG_ASSERT_POINTER(pptimer);
+	MALI_DEBUG_ASSERT_POINTER(pptimer->timer);
+
+	if( pptimer->set )
+	{
+		_mali_osk_timer_del( pptimer->timer );
+		pptimer->set = MALI_FALSE;
+		pptimer->expired = MALI_FALSE;
+		return MALI_TRUE;
+	}
+	
+	return MALI_FALSE;
+}
+
+mali_bool pmm_policy_timer_raise_event( _pmm_policy_timer_t *pptimer )
+{
+	MALI_DEBUG_ASSERT_POINTER(pptimer);
+
+	if( pptimer->expired )
+	{
+		_mali_uk_pmm_message_s event = {
+			NULL,
+			MALI_PMM_EVENT_TIMEOUT, /* Assume timeout id, but set it below */
+			0 };
+
+		event.id = pptimer->event_id;
+		event.data = (mali_pmm_message_data)pptimer->start;
+
+		/* Don't need to do any other notification with this timer */
+		pptimer->expired = MALI_FALSE;
+		/* Unset timer so it is free to be set again */
+		pptimer->set = MALI_FALSE;
+		
+		_mali_ukk_pmm_event_message( &event );
+				
+		return MALI_TRUE;
+	}
+	
+	return MALI_FALSE;
+}
+
+mali_bool pmm_policy_timer_valid( u32 timer_start, u32 other_start )
+{
+	return (_mali_osk_time_after( other_start, timer_start ) == 0);
+}
+
+
+_mali_osk_errcode_t pmm_policy_init(_mali_pmm_internal_state_t *pmm)
+{
+	_mali_osk_errcode_t err;
+
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	switch( pmm->policy )
+	{
+		case MALI_PMM_POLICY_ALWAYS_ON:
+		{
+			err = pmm_policy_init_always_on();
+		}
+		break;
+
+		case MALI_PMM_POLICY_JOB_CONTROL:
+		{
+			err = pmm_policy_init_job_control(pmm);
+		}
+		break;
+
+		case MALI_PMM_POLICY_NONE:
+		default:
+			err = _MALI_OSK_ERR_FAULT;
+	}
+	
+	return err;
+}
+
+void pmm_policy_term(_mali_pmm_internal_state_t *pmm)
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	switch( pmm->policy )
+	{
+		case MALI_PMM_POLICY_ALWAYS_ON:
+		{
+			pmm_policy_term_always_on();
+		}
+		break;
+
+		case MALI_PMM_POLICY_JOB_CONTROL:
+		{
+			pmm_policy_term_job_control();
+		}
+		break;
+
+		case MALI_PMM_POLICY_NONE:
+		default:
+			MALI_PRINT_ERROR( ("PMM: Invalid policy terminated %d\n", pmm->policy) );
+	}
+}
+
+
+_mali_osk_errcode_t pmm_policy_process(_mali_pmm_internal_state_t *pmm, mali_pmm_message_t *event)
+{
+	_mali_osk_errcode_t err;
+
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_DEBUG_ASSERT_POINTER(event);
+
+	switch( pmm->policy )
+	{
+		case MALI_PMM_POLICY_ALWAYS_ON:
+		{
+			err = pmm_policy_process_always_on( pmm, event );
+		}
+		break;
+
+		case MALI_PMM_POLICY_JOB_CONTROL:
+		{
+			err = pmm_policy_process_job_control( pmm, event );
+		}
+		break;
+
+		case MALI_PMM_POLICY_NONE:
+		default:
+			err = _MALI_OSK_ERR_FAULT;
+	}
+	
+	return err;
+}
+
+
+void pmm_policy_check_policy( _mali_pmm_internal_state_t *pmm )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	switch( pmm->policy )
+	{
+		case MALI_PMM_POLICY_JOB_CONTROL:
+		{
+			pmm_policy_check_job_control();
+		}
+		break;
+
+		default:
+			/* Nothing needs to be done */
+			break;
+	}
+}
+
+
+#endif /* USING_MALI_PMM */
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy.h
new file mode 100644
index 00000000000..83cb7f29a92
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy.h
@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm_policy.h
+ * Defines the power management module policies
+ */
+
+#ifndef __MALI_PMM_POLICY_H__
+#define __MALI_PMM_POLICY_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @addtogroup pmmapi Power Management Module APIs
+ *
+ * @{
+ *
+ * @defgroup pmmapi_policy Power Management Module Policies
+ *
+ * @{
+ */
+
+/** @brief Generic timer for use with policies
+ */
+typedef struct _pmm_policy_timer
+{
+	u32 timeout;                    /**< Timeout for this timer in ticks */
+	mali_pmm_event_id event_id;     /**< Event id that will be raised when timer expires */
+	_mali_osk_timer_t *timer;       /**< Timer */
+	mali_bool set;                  /**< Timer set */
+	mali_bool expired;              /**< Timer expired - event needs to be raised */
+	u32 start;                      /**< Timer start ticks */
+} _pmm_policy_timer_t;
+
+/** @brief Policy timer initialization
+ *
+ * This will create a timer for use in policies, but won't start it
+ * 
+ * @param pptimer An empty timer structure to be initialized
+ * @param timeout Timeout in ticks for the timer
+ * @param id Event id that will be raised on timeout
+ * @return _MALI_OSK_ERR_OK if the policy could be initialized, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t pmm_policy_timer_init( _pmm_policy_timer_t *pptimer, u32 timeout, mali_pmm_event_id id );
+
+/** @brief Policy timer termination
+ *
+ * This will clean up a timer that was previously used in policies, it
+ * will also stop it if started
+ * 
+ * @param pptimer An initialized timer structure to be terminated
+ */
+void pmm_policy_timer_term( _pmm_policy_timer_t *pptimer );
+
+/** @brief Policy timer start
+ *
+ * This will start a previously created timer for use in policies
+ * When the timer expires after the initialized timeout it will raise 
+ * a PMM event of the event id given on initialization
+ * As data for the event it will pass the start time of the timer
+ * 
+ * @param pptimer A previously initialized policy timer
+ * @return MALI_TRUE if the timer was started, MALI_FALSE if it is already started
+ */
+mali_bool pmm_policy_timer_start( _pmm_policy_timer_t *pptimer );
+
+/** @brief Policy timer stop
+ *
+ * This will stop a previously created timer for use in policies
+ * 
+ * @param pptimer A previously started policy timer
+ * @return MALI_TRUE if the timer was stopped, MALI_FALSE if it is already stopped
+ */
+mali_bool pmm_policy_timer_stop( _pmm_policy_timer_t *pptimer );
+
+/** @brief Policy timer stop
+ *
+ * This raise an event for an expired timer
+ * 
+ * @param pptimer An expired policy timer
+ * @return MALI_TRUE if an event was raised, else MALI_FALSE
+ */
+mali_bool pmm_policy_timer_raise_event( _pmm_policy_timer_t *pptimer );
+
+/** @brief Policy timer valid checker
+ *
+ * This will check that a timer was started after a given time
+ * 
+ * @param timer_start Time the timer was started
+ * @param other_start Time when another event or action occurred
+ * @return MALI_TRUE if the timer was started after the other time, else MALI_FALSE
+ */
+mali_bool pmm_policy_timer_valid( u32 timer_start, u32 other_start );
+
+
+/** @brief Common policy initialization
+ * 
+ * This will initialize the current policy
+ *
+ * @note Any previously initialized policy should be terminated first
+ *
+ * @return _MALI_OSK_ERR_OK if the policy could be initialized, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t pmm_policy_init( _mali_pmm_internal_state_t *pmm );
+
+/** @brief Common policy termination
+ *
+ * This will terminate the current policy.
+ * @note This can be called when a policy has not been initialized
+ */
+void pmm_policy_term( _mali_pmm_internal_state_t *pmm );
+
+/** @brief Common policy state changer
+ *
+ * Given the next available event message, this routine passes it to
+ * the current policy for processing
+ *
+ * @param pmm internal PMM state
+ * @param event PMM event to process
+ * @return _MALI_OSK_ERR_OK if the policy state completed okay, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t pmm_policy_process( _mali_pmm_internal_state_t *pmm, mali_pmm_message_t *event );
+
+
+/** @brief Common policy checker
+ *
+ * If a policy timer fires then this function will be called to
+ * allow the policy to take the correct action
+ *
+ * @param pmm internal PMM state
+ */
+void pmm_policy_check_policy( _mali_pmm_internal_state_t *pmm );
+
+/** @} */ /* End group pmmapi_policy */
+/** @} */ /* End group pmmapi */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_PMM_POLICY_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_alwayson.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_alwayson.c
new file mode 100644
index 00000000000..643bb04553b
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_alwayson.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm_policy_alwayson.c
+ * Implementation of the power management module policy - always on
+ */
+
+#if USING_MALI_PMM
+
+#include "mali_ukk.h"
+#include "mali_kernel_common.h"
+
+#include "mali_pmm.h"
+#include "mali_pmm_system.h"
+#include "mali_pmm_state.h"
+#include "mali_pmm_policy.h"
+#include "mali_pmm_policy_alwayson.h"
+
+_mali_osk_errcode_t pmm_policy_init_always_on(void)
+{
+	/* Nothing to set up */
+	MALI_SUCCESS;
+}
+
+void pmm_policy_term_always_on(void)
+{
+	/* Nothing to tear down */
+}
+
+_mali_osk_errcode_t pmm_policy_process_always_on( _mali_pmm_internal_state_t *pmm, mali_pmm_message_t *event )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_DEBUG_ASSERT_POINTER(event);
+
+	switch( event->id )
+	{
+		case MALI_PMM_EVENT_OS_POWER_DOWN:
+			/* We aren't going to do anything, but signal so we don't block the OS
+			 * NOTE: This may adversely affect any jobs Mali is currently running
+			 */
+			_mali_osk_pmm_power_down_done( event->data );
+		break;
+
+		case MALI_PMM_EVENT_INTERNAL_POWER_UP_ACK:
+		case MALI_PMM_EVENT_INTERNAL_POWER_DOWN_ACK:
+			/* Not expected in this policy */
+			MALI_DEBUG_ASSERT( MALI_FALSE );
+		break;
+
+		case MALI_PMM_EVENT_OS_POWER_UP:
+			/* Nothing to do */
+			_mali_osk_pmm_power_up_done( event->data );
+		break;
+		
+		case MALI_PMM_EVENT_JOB_SCHEDULED:
+		case MALI_PMM_EVENT_JOB_QUEUED:
+		case MALI_PMM_EVENT_JOB_FINISHED:
+			/* Nothing to do - we are always on */
+		break;
+	
+		case MALI_PMM_EVENT_TIMEOUT:
+			/* Not expected in this policy */
+			MALI_DEBUG_ASSERT( MALI_FALSE );
+		break;
+
+		default:
+		MALI_ERROR(_MALI_OSK_ERR_ITEM_NOT_FOUND);
+	}
+
+	MALI_SUCCESS;
+}
+
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_alwayson.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_alwayson.h
new file mode 100644
index 00000000000..a158b09f610
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_alwayson.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm_policy_alwayson.h
+ * Defines the power management module policy for always on
+ */
+
+#ifndef __MALI_PMM_POLICY_ALWAYSON_H__
+#define __MALI_PMM_POLICY_ALWAYSON_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @addtogroup pmmapi_policy Power Management Module Policies
+ *
+ * @{
+ */
+
+/** @brief Always on policy initialization
+ *
+ * @return _MALI_OSK_ERR_OK if the policy could be initialized, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t pmm_policy_init_always_on(void);
+
+/** @brief Always on policy termination
+ */
+void pmm_policy_term_always_on(void);
+
+/** @brief Always on policy state changer
+ *
+ * Given the next available event message, this routine processes it
+ * for the policy and changes state as needed.
+ *
+ * Always on policy will ignore all events and keep the Mali cores on
+ * all the time
+ *
+ * @param pmm internal PMM state
+ * @param event PMM event to process
+ * @return _MALI_OSK_ERR_OK if the policy state completed okay, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t pmm_policy_process_always_on( _mali_pmm_internal_state_t *pmm, mali_pmm_message_t *event );
+
+/** @} */ /* End group pmmapi_policies */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_PMM_POLICY_ALWAYSON_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_jobcontrol.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_jobcontrol.c
new file mode 100644
index 00000000000..8450bd722f4
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_jobcontrol.c
@@ -0,0 +1,461 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm_policy_jobcontrol.c
+ * Implementation of the power management module policy - job control
+ */
+
+#if USING_MALI_PMM
+
+#include "mali_ukk.h"
+#include "mali_kernel_common.h"
+#include "mali_platform.h"
+
+#include "mali_pmm.h"
+#include "mali_pmm_system.h"
+#include "mali_pmm_state.h"
+#include "mali_pmm_policy.h"
+#include "mali_pmm_policy_jobcontrol.h"
+
+typedef struct _pmm_policy_data_job_control
+{
+	_pmm_policy_timer_t latency;  /**< Latency timeout timer for all cores */
+	u32 core_active_start;        /**< Last time a core was set to active */
+	u32 timeout;                  /**< Timeout in ticks for latency timer */
+} _pmm_policy_data_job_control_t;
+
+
+/* @ brief Local data for this policy 
+ */
+static _pmm_policy_data_job_control_t *data_job_control = NULL;
+
+/* @brief Set up the timeout if it hasn't already been set and if there are active cores */
+static void job_control_timeout_setup( _mali_pmm_internal_state_t *pmm, _pmm_policy_timer_t *pptimer )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_DEBUG_ASSERT_POINTER(pptimer);
+
+	/* Do we have an inactivity time out and some powered cores? */
+	if( pptimer->timeout > 0 && pmm->cores_powered != 0 )
+	{
+		/* Is the system idle and all the powered cores are idle? */
+		if( pmm->status == MALI_PMM_STATUS_IDLE && pmm->cores_idle == pmm->cores_powered )
+		{
+			if( pmm_policy_timer_start(pptimer) )
+			{
+				MALIPMM_DEBUG_PRINT( ("PMM policy - Job control: Setting in-activity latency timer\n") );
+			}
+		}
+		else
+		{
+			/* We are not idle so there is no need for an inactivity timer
+			 */
+			if( pmm_policy_timer_stop(pptimer) )
+			{
+				MALIPMM_DEBUG_PRINT( ("PMM policy - Job control: Removing in-activity latency timer\n") );
+			}
+		}
+	}
+}
+
+/* @brief Check the validity of the timeout - and if there is one set */
+static mali_bool job_control_timeout_valid( _mali_pmm_internal_state_t *pmm, _pmm_policy_timer_t *pptimer, u32 timer_start )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_DEBUG_ASSERT_POINTER(pptimer);
+
+	/* Not a valid timer! */
+	if( pptimer->timeout == 0 ) return MALI_FALSE;
+
+	/* Are some cores powered and are they all idle? */
+	if( (pmm->cores_powered != 0) && (pmm->cores_idle == pmm->cores_powered) )
+	{
+		/* Has latency timeout started after the last core was active? */
+		if( pmm_policy_timer_valid( timer_start, data_job_control->core_active_start ) )
+		{
+			 return MALI_TRUE;
+		}
+		else
+		{
+			MALIPMM_DEBUG_PRINT( ("PMM: In-activity latency time out ignored - out of date\n") );
+		}
+	}
+	else
+	{
+		if( pmm->cores_powered == 0 )
+		{
+			MALIPMM_DEBUG_PRINT( ("PMM: In-activity latency time out ignored - cores already off\n") );
+		}
+		else
+		{
+			MALIPMM_DEBUG_PRINT( ("PMM: In-activity latency time out ignored - cores active\n") );
+		}
+	}
+
+	return MALI_FALSE;
+}
+
+_mali_osk_errcode_t pmm_policy_init_job_control( _mali_pmm_internal_state_t *pmm )
+{
+	_mali_osk_errcode_t err;
+	MALI_DEBUG_ASSERT_POINTER( pmm );
+	MALI_DEBUG_ASSERT( data_job_control == NULL );
+	
+	data_job_control = (_pmm_policy_data_job_control_t *) _mali_osk_malloc(sizeof(*data_job_control));
+	MALI_CHECK_NON_NULL( data_job_control, _MALI_OSK_ERR_NOMEM );
+
+	data_job_control->core_active_start = _mali_osk_time_tickcount();
+	data_job_control->timeout = MALI_PMM_POLICY_JOBCONTROL_INACTIVITY_TIMEOUT;
+	
+	err = pmm_policy_timer_init( &data_job_control->latency, data_job_control->timeout, MALI_PMM_EVENT_TIMEOUT );
+	if( err != _MALI_OSK_ERR_OK )
+	{
+		_mali_osk_free( data_job_control );
+		data_job_control = NULL;
+		return err;
+	}
+	
+	/* Start the latency timeout */
+	job_control_timeout_setup( pmm, &data_job_control->latency );
+	
+	MALI_SUCCESS;
+}
+
+void pmm_policy_term_job_control(void)
+{
+	if( data_job_control != NULL )
+	{
+		pmm_policy_timer_term( &data_job_control->latency );
+		_mali_osk_free( data_job_control );
+		data_job_control = NULL;
+	}
+}
+
+static void pmm_policy_job_control_job_queued( _mali_pmm_internal_state_t *pmm )
+{
+	mali_pmm_core_mask cores;
+	mali_pmm_core_mask cores_subset;
+
+	/* Make sure that all cores are powered in this
+	 * simple policy
+	 */
+	cores = pmm->cores_registered;
+	cores_subset = pmm_cores_to_power_up( pmm, cores );
+	if( cores_subset != 0 )
+	{
+		/* There are some cores that need powering up */
+		if( !pmm_invoke_power_up( pmm ) )
+		{
+			/* Need to wait until finished */
+			pmm->status = MALI_PMM_STATUS_POLICY_POWER_UP;
+		}
+	}
+}
+
+_mali_osk_errcode_t pmm_policy_process_job_control( _mali_pmm_internal_state_t *pmm, mali_pmm_message_t *event )
+{
+	mali_pmm_core_mask cores;
+	mali_pmm_core_mask cores_subset;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_DEBUG_ASSERT_POINTER(event);
+	MALI_DEBUG_ASSERT_POINTER(data_job_control);
+
+	MALIPMM_DEBUG_PRINT( ("PMM: Job control policy process start - status=%d\n", pmm->status) );
+
+	/* Mainly the data is the cores */
+	cores = pmm_cores_from_event_data( pmm, event );
+
+#if MALI_STATE_TRACKING
+	pmm->mali_last_pmm_status = pmm->status;
+#endif /* MALI_STATE_TRACKING */
+
+	switch( pmm->status )
+	{
+	/**************** IDLE ****************/
+	case MALI_PMM_STATUS_IDLE:
+		switch( event->id )
+		{
+		case MALI_PMM_EVENT_OS_POWER_UP:
+			/* Not expected in this state */
+			break;
+
+		case MALI_PMM_EVENT_JOB_SCHEDULED:
+
+			/* Update idle cores to indicate active - remove these! */
+			pmm_cores_set_active( pmm, cores );
+			/* Remember when this happened */
+			data_job_control->core_active_start = event->ts;
+#if MALI_POWER_MGMT_TEST_SUITE
+			_mali_osk_pmm_policy_events_notifications(MALI_PMM_EVENT_JOB_SCHEDULED);
+#endif
+
+			/*** FALL THROUGH to QUEUED to check POWER UP ***/
+
+		case MALI_PMM_EVENT_JOB_QUEUED:
+		
+			pmm_policy_job_control_job_queued( pmm );
+#if MALI_POWER_MGMT_TEST_SUITE
+			_mali_osk_pmm_policy_events_notifications(MALI_PMM_EVENT_JOB_QUEUED);
+#endif
+			break;
+		
+		case MALI_PMM_EVENT_DVFS_PAUSE:
+
+			cores_subset = pmm_cores_to_power_down( pmm, cores, MALI_FALSE );
+			if ( cores_subset != 0 )
+			{
+				if ( !pmm_power_down_okay( pmm ) )
+				{
+					pmm->is_dvfs_active = 1;
+					pmm->status = MALI_PMM_STATUS_OS_POWER_DOWN;
+					pmm_save_os_event_data( pmm, event->data );
+					break;
+				}
+			}
+			pmm->status = MALI_PMM_STATUS_DVFS_PAUSE;
+			_mali_osk_pmm_dvfs_operation_done(0);
+			break;
+
+		case MALI_PMM_EVENT_OS_POWER_DOWN:
+
+			/* Need to power down all cores even if we need to wait for them */
+			cores_subset = pmm_cores_to_power_down( pmm, cores, MALI_FALSE );
+			if( cores_subset != 0 )
+			{
+				/* There are some cores that need powering down */
+				if( !pmm_invoke_power_down( pmm ) )
+				{
+					/* We need to wait until they are idle */
+					
+					pmm->status = MALI_PMM_STATUS_OS_POWER_DOWN;
+					/* Save the OS data to respond later */
+					pmm_save_os_event_data( pmm, event->data );
+					/* Exit this case - as we have to wait */
+					break;
+				}
+			}
+			/* Set waiting status */
+			pmm->status = MALI_PMM_STATUS_OS_WAITING;
+			/* All cores now down - respond to OS power event */
+			_mali_osk_pmm_power_down_done( event->data );
+			break;
+
+		case MALI_PMM_EVENT_JOB_FINISHED:
+
+			/* Update idle cores - add these! */
+			pmm_cores_set_idle( pmm, cores );
+#if MALI_POWER_MGMT_TEST_SUITE
+			 _mali_osk_pmm_policy_events_notifications(MALI_PMM_EVENT_JOB_FINISHED);
+#endif
+			if( data_job_control->timeout > 0 )
+			{
+				/* Wait for time out to fire */
+				break;
+			}
+			/* For job control policy - turn off all cores */
+			cores = pmm->cores_powered;
+			
+			/*** FALL THROUGH to TIMEOUT TEST as NO TIMEOUT ***/
+
+		case MALI_PMM_EVENT_TIMEOUT:
+
+			/* Main job control policy - turn off cores after inactivity */
+			if( job_control_timeout_valid( pmm, &data_job_control->latency, (u32)event->data ) )
+ 			{
+				/* Valid timeout of inactivity - so find out if we can power down 
+				 * immedately - if we can't then this means the cores are still in fact
+				 * active 
+				 */
+				cores_subset = pmm_cores_to_power_down( pmm, cores, MALI_TRUE );
+				if( cores_subset != 0 )
+				{
+					/* Check if we can really power down, if not then we are not
+					 * really in-active
+					 */
+					if( !pmm_invoke_power_down( pmm ) )
+					{
+						pmm_power_down_cancel( pmm );
+					}
+				}
+				/* else there are no cores powered up! */
+			}
+#if MALI_POWER_MGMT_TEST_SUITE
+			_mali_osk_pmm_policy_events_notifications(MALI_PMM_EVENT_TIMEOUT);
+#endif
+			break;
+
+		default:
+			/* Unexpected event */
+			MALI_ERROR(_MALI_OSK_ERR_ITEM_NOT_FOUND);
+		}
+		break;
+
+	/******************DVFS PAUSE**************/
+	case MALI_PMM_STATUS_DVFS_PAUSE:
+		switch ( event->id )
+		{
+		case MALI_PMM_EVENT_DVFS_RESUME:
+
+			if ( pmm->cores_powered != 0 )
+			{
+				pmm->cores_ack_down =0;
+				pmm_power_down_cancel( pmm );
+				pmm->status = MALI_PMM_STATUS_IDLE;
+			}
+			else
+			{
+				pmm_policy_job_control_job_queued( pmm );
+			}
+			_mali_osk_pmm_dvfs_operation_done( 0 );
+			break;
+
+		case MALI_PMM_EVENT_OS_POWER_DOWN:
+			/* Set waiting status */
+			pmm->status = MALI_PMM_STATUS_OS_WAITING;
+			if ( pmm->cores_powered != 0 )
+			{
+				if ( pmm_invoke_power_down( pmm ) )
+				{
+					_mali_osk_pmm_power_down_done( 0 );
+					break;
+				}
+			}
+			_mali_osk_pmm_power_down_done( 0 );
+			break;
+		default:
+			break;
+		}
+	break;
+
+	/**************** POWER UP ****************/
+	case MALI_PMM_STATUS_OS_POWER_UP:
+	case MALI_PMM_STATUS_POLICY_POWER_UP:
+		switch( event->id )
+		{
+		case MALI_PMM_EVENT_INTERNAL_POWER_UP_ACK:
+			/* Make sure cores powered off equal what we expect */
+			MALI_DEBUG_ASSERT( cores == pmm->cores_pend_up );
+			pmm_cores_set_up_ack( pmm, cores );
+
+			if( pmm_invoke_power_up( pmm ) )
+			{
+				if( pmm->status == MALI_PMM_STATUS_OS_POWER_UP )
+				{
+					/* Get the OS data and respond to the power up */
+					_mali_osk_pmm_power_up_done( pmm_retrieve_os_event_data( pmm ) );
+				}
+				pmm->status = MALI_PMM_STATUS_IDLE;
+			}
+			break;
+
+		default:
+			/* Unexpected event */
+			MALI_ERROR(_MALI_OSK_ERR_ITEM_NOT_FOUND);
+		}
+		break;
+
+	/**************** POWER DOWN ****************/
+	case MALI_PMM_STATUS_OS_POWER_DOWN:
+	case MALI_PMM_STATUS_POLICY_POWER_DOWN:
+		switch( event->id )
+		{
+
+		case MALI_PMM_EVENT_INTERNAL_POWER_DOWN_ACK:
+			
+			pmm_cores_set_down_ack( pmm, cores );
+			
+			if ( pmm->is_dvfs_active == 1 )
+			{
+				if( pmm_power_down_okay( pmm ) )
+				{
+					pmm->is_dvfs_active = 0;
+					pmm->status = MALI_PMM_STATUS_DVFS_PAUSE;
+					_mali_osk_pmm_dvfs_operation_done( pmm_retrieve_os_event_data( pmm ) );
+				}
+				break;
+			}
+			
+			/* Now check if we can power down */
+			if( pmm_invoke_power_down( pmm ) )
+			{
+				if( pmm->status == MALI_PMM_STATUS_OS_POWER_DOWN )
+				{
+					/* Get the OS data and respond to the power down */
+					_mali_osk_pmm_power_down_done( pmm_retrieve_os_event_data( pmm ) );
+				}
+				pmm->status = MALI_PMM_STATUS_OS_WAITING;
+			}
+			break;
+
+		default:
+			/* Unexpected event */
+			MALI_ERROR(_MALI_OSK_ERR_ITEM_NOT_FOUND);
+		}
+		break;
+		
+	case MALI_PMM_STATUS_OS_WAITING:
+		switch( event->id )
+		{
+		case MALI_PMM_EVENT_OS_POWER_UP:
+			cores_subset = pmm_cores_to_power_up( pmm, cores );
+			if( cores_subset != 0 )
+			{
+				/* There are some cores that need powering up */
+				if( !pmm_invoke_power_up( pmm ) )
+				{
+					/* Need to wait until power up complete */
+					pmm->status = MALI_PMM_STATUS_OS_POWER_UP;
+					/* Save the OS data to respond later */
+					pmm_save_os_event_data( pmm, event->data );
+					/* Exit this case - as we have to wait */
+					break;
+				}
+			}
+			pmm->status = MALI_PMM_STATUS_IDLE;
+			/* All cores now up - respond to OS power up event */
+			_mali_osk_pmm_power_up_done( event->data );
+			break;
+			
+		default:
+			/* All other messages are ignored in this state */
+			break;
+		}
+		break;	
+	
+	default:
+		/* Unexpected state */
+		MALI_ERROR(_MALI_OSK_ERR_FAULT);
+	}
+
+	/* Set in-activity latency timer - if required */
+	job_control_timeout_setup( pmm, &data_job_control->latency );
+
+	/* Update the PMM state */
+	pmm_update_system_state( pmm );
+#if MALI_STATE_TRACKING
+	pmm->mali_new_event_status = event->id;
+#endif /* MALI_STATE_TRACKING */
+
+	MALIPMM_DEBUG_PRINT( ("PMM: Job control policy process end - status=%d and event=%d\n", pmm->status,event->id) );
+
+	MALI_SUCCESS;
+}
+
+void pmm_policy_check_job_control()
+{
+	MALI_DEBUG_ASSERT_POINTER(data_job_control);
+
+	/* Latency timer must have expired raise the event */
+	pmm_policy_timer_raise_event(&data_job_control->latency);
+}
+
+
+#endif /* USING_MALI_PMM */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_jobcontrol.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_jobcontrol.h
new file mode 100644
index 00000000000..455234b80bc
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_policy_jobcontrol.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm_policy.h
+ * Defines the power management module policies
+ */
+
+#ifndef __MALI_PMM_POLICY_JOBCONTROL_H__
+#define __MALI_PMM_POLICY_JOBCONTROL_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @addtogroup pmmapi_policy Power Management Module Policies
+ *
+ * @{
+ */
+
+/** @brief The jobcontrol policy inactivity latency timeout (in ticks) 
+ * before the hardware is switched off
+ *
+ * @note Setting this low whilst tracing or producing debug output can 
+ * cause alot of timeouts to fire which can affect the PMM behaviour
+ */
+#define MALI_PMM_POLICY_JOBCONTROL_INACTIVITY_TIMEOUT 50
+
+/** @brief Job control policy initialization
+ *
+ * @return _MALI_OSK_ERR_OK if the policy could be initialized, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t pmm_policy_init_job_control(_mali_pmm_internal_state_t *pmm);
+
+/** @brief Job control policy termination
+ */
+void pmm_policy_term_job_control(void);
+
+/** @brief Job control policy state changer
+ *
+ * Given the next available event message, this routine processes it
+ * for the policy and changes state as needed.
+ *
+ * Job control policy depends on events from the Mali cores, and will
+ * power down all cores after an inactivity latency timeout. It will
+ * power the cores back on again when a job is scheduled to run.
+ *
+ * @param pmm internal PMM state
+ * @param event PMM event to process
+ * @return _MALI_OSK_ERR_OK if the policy state completed okay, or a suitable
+ * _mali_osk_errcode_t otherwise.
+ */
+_mali_osk_errcode_t pmm_policy_process_job_control( _mali_pmm_internal_state_t *pmm, mali_pmm_message_t *event );
+
+/** @brief Job control policy checker
+ *
+ * The latency timer has fired and we need to raise the correct event to
+ * handle it
+ *
+ * @param pmm internal PMM state
+ */
+void pmm_policy_check_job_control(void);
+
+/** @} */ /* End group pmmapi_policy */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_PMM_POLICY_JOBCONTROL_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_state.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_state.c
new file mode 100644
index 00000000000..a0ac1c7790c
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_state.c
@@ -0,0 +1,718 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm_state.c
+ * Implementation of the power management module internal state
+ */
+
+#if USING_MALI_PMM
+
+#include "mali_ukk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_subsystem.h"
+
+#include "mali_pmm.h"
+#include "mali_pmm_state.h"
+#include "mali_pmm_system.h"
+
+#include "mali_kernel_core.h"
+#include "mali_platform.h"
+
+#define SIZEOF_CORES_LIST 6
+
+/* NOTE: L2 *MUST* be first on the list so that it
+ * is correctly powered on first and powered off last
+ */
+static mali_pmm_core_id cores_list[] = { MALI_PMM_CORE_L2,
+										MALI_PMM_CORE_GP,
+										MALI_PMM_CORE_PP0,
+										MALI_PMM_CORE_PP1,
+										MALI_PMM_CORE_PP2,
+										MALI_PMM_CORE_PP3 };
+
+
+
+void pmm_update_system_state( _mali_pmm_internal_state_t *pmm )
+{
+	mali_pmm_state state;
+
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	if( pmm->cores_registered == 0 )
+	{
+		state = MALI_PMM_STATE_UNAVAILABLE;
+	}
+	else if( pmm->cores_powered == 0 )
+	{
+		state = MALI_PMM_STATE_SYSTEM_OFF;
+	}
+	else if( pmm->cores_powered == pmm->cores_registered )
+	{
+		state = MALI_PMM_STATE_SYSTEM_ON;
+	}
+	else
+	{
+		/* Some other state where not everything is on or off */
+		state = MALI_PMM_STATE_SYSTEM_TRANSITION;
+	}
+
+#if MALI_PMM_TRACE
+	_mali_pmm_trace_state_change( pmm->state, state );
+#endif
+	pmm->state = state;
+}
+
+mali_pmm_core_mask pmm_cores_from_event_data( _mali_pmm_internal_state_t *pmm, mali_pmm_message_t *event )
+{
+	mali_pmm_core_mask cores;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_DEBUG_ASSERT_POINTER(event);
+
+	switch( event->id )
+	{
+	case MALI_PMM_EVENT_OS_POWER_UP:
+	case MALI_PMM_EVENT_OS_POWER_DOWN:
+		/* All cores - the system */
+		cores = pmm->cores_registered;
+		break;
+
+	case MALI_PMM_EVENT_JOB_SCHEDULED:
+	case MALI_PMM_EVENT_JOB_QUEUED:
+	case MALI_PMM_EVENT_JOB_FINISHED:
+	case MALI_PMM_EVENT_INTERNAL_POWER_UP_ACK:
+	case MALI_PMM_EVENT_INTERNAL_POWER_DOWN_ACK:
+		/* Currently the main event data is only the cores
+		 * for these messages
+		 */
+		cores = (mali_pmm_core_mask)event->data;
+		if( cores == MALI_PMM_CORE_SYSTEM )
+		{
+			cores = pmm->cores_registered;
+		}
+		else if( cores == MALI_PMM_CORE_PP_ALL )
+		{
+			/* Get the subset of registered PP cores */
+			cores = (pmm->cores_registered & MALI_PMM_CORE_PP_ALL);
+		}
+		MALI_PMM_DEBUG_ASSERT_CORES_SUBSET( pmm->cores_registered, cores );
+		break;
+
+	default:
+		/* Assume timeout messages - report cores still powered */
+		cores = pmm->cores_powered;
+		break;
+	}
+
+	return cores;
+}
+
+mali_pmm_core_mask pmm_cores_to_power_up( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores )
+{
+	mali_pmm_core_mask cores_subset;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_PMM_DEBUG_ASSERT_CORES_SUBSET( pmm->cores_registered, cores );
+
+	/* Check that cores aren't pending power down when asked for power up */
+	MALI_DEBUG_ASSERT( pmm->cores_pend_down == 0 );
+
+	cores_subset = (~(pmm->cores_powered) & cores);
+	if( cores_subset != 0 )
+	{
+		/* There are some cores that need powering up */
+		pmm->cores_pend_up = cores_subset;
+	}
+
+	return cores_subset;
+}
+
+mali_pmm_core_mask pmm_cores_to_power_down( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores, mali_bool immediate_only )
+{
+	mali_pmm_core_mask cores_subset;
+	_mali_osk_errcode_t err;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_PMM_DEBUG_ASSERT_CORES_SUBSET( pmm->cores_registered, cores );
+
+	/* Check that cores aren't pending power up when asked for power down */
+	MALI_DEBUG_ASSERT( pmm->cores_pend_up == 0 );
+
+	cores_subset = (pmm->cores_powered & cores);
+	if( cores_subset != 0 )
+	{
+		int n;
+		volatile mali_pmm_core_mask *ppowered = &(pmm->cores_powered);
+
+		/* There are some cores that need powering up, but we may
+		 * need to wait until they are idle
+		 */
+		for( n = SIZEOF_CORES_LIST-1; n >= 0; n-- )
+		{
+			if( (cores_list[n] & cores_subset) != 0 )
+			{
+				/* Core is to be powered down */
+				pmm->cores_pend_down |= cores_list[n];
+
+				/* Can't hold the power lock, when acessing subsystem mutex via
+				 * the core power call.
+				 * Due to terminatation of driver requiring a subsystem mutex
+				 * and then power lock held to unregister a core.
+				 * This does mean that the following function could fail
+				 * as the core is unregistered before we tell it to power
+				 * down, but it does not matter as we are terminating
+				 */
+#if MALI_STATE_TRACKING
+                pmm->mali_pmm_lock_acquired = 0;
+#endif /* MALI_STATE_TRACKING */
+
+				MALI_PMM_UNLOCK(pmm);
+				/* Signal the core to power down
+				 * If it is busy (not idle) it will set a pending power down flag 
+				 * (as long as we don't want to only immediately power down). 
+				 * If it isn't busy it will move out of the idle queue right
+				 * away
+				 */
+				err = mali_core_signal_power_down( cores_list[n], immediate_only );
+				MALI_PMM_LOCK(pmm);
+
+#if MALI_STATE_TRACKING
+                pmm->mali_pmm_lock_acquired = 1;
+#endif /* MALI_STATE_TRACKING */
+			
+
+				/* Re-read cores_subset in case it has changed */
+				cores_subset = (*ppowered & cores);
+
+				if( err == _MALI_OSK_ERR_OK )
+				{
+					/* We moved an idle core to the power down queue
+					 * which means it is now acknowledged (if it is still 
+					 * registered)
+					 */
+					pmm->cores_ack_down |= (cores_list[n] & cores_subset);
+				}
+				else
+				{
+					MALI_DEBUG_ASSERT( err == _MALI_OSK_ERR_BUSY ||
+										(err == _MALI_OSK_ERR_FAULT &&
+										(*ppowered & cores_list[n]) == 0) );
+					/* If we didn't move a core - it must be active, so
+					 * leave it pending, so we get an acknowledgement (when
+					 * not in immediate only mode)
+					 * Alternatively we are shutting down and the core has
+					 * been unregistered
+					 */
+				}
+			}
+		}
+	}
+
+	return cores_subset;
+}
+
+void pmm_power_down_cancel( _mali_pmm_internal_state_t *pmm )
+{
+	int n;
+	mali_pmm_core_mask pd, ad;
+	_mali_osk_errcode_t err;
+	volatile mali_pmm_core_mask *pregistered;
+
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	MALIPMM_DEBUG_PRINT( ("PMM: Cancelling power down\n") );
+
+	pd = pmm->cores_pend_down;
+	ad = pmm->cores_ack_down;
+	/* Clear the pending cores so that they don't move to the off
+	 * queue if they haven't already
+	 */
+	pmm->cores_pend_down = 0;
+	pmm->cores_ack_down = 0;
+	pregistered = &(pmm->cores_registered);
+
+	/* Power up all the pending power down cores - just so
+	 * we make sure the system is in a known state, as a
+	 * pending core might have sent an acknowledged message
+	 * which hasn't been read yet.
+	 */
+	for( n = 0; n < SIZEOF_CORES_LIST; n++ )
+	{
+		if( (cores_list[n] & pd) != 0 )
+		{
+			/* Can't hold the power lock, when acessing subsystem mutex via
+			 * the core power call.
+			 * Due to terminatation of driver requiring a subsystem mutex
+			 * and then power lock held to unregister a core.
+			 * This does mean that the following power up function could fail
+			 * as the core is unregistered before we tell it to power
+			 * up, but it does not matter as we are terminating
+			 */
+#if MALI_STATE_TRACKING
+			pmm->mali_pmm_lock_acquired = 0;
+#endif /* MALI_STATE_TRACKING */
+
+			MALI_PMM_UNLOCK(pmm);
+			/* As we are cancelling - only move the cores back to the queue - 
+			 * no reset needed
+			 */
+			err = mali_core_signal_power_up( cores_list[n], MALI_TRUE );
+			MALI_PMM_LOCK(pmm);
+#if MALI_STATE_TRACKING
+			pmm->mali_pmm_lock_acquired = 1;
+#endif /* MALI_STATE_TRACKING */
+
+			/* Update pending list with the current registered cores */
+			pd &= (*pregistered);
+
+			if( err != _MALI_OSK_ERR_OK )
+			{
+				MALI_DEBUG_ASSERT( (err == _MALI_OSK_ERR_BUSY && 
+										((cores_list[n] & ad) == 0))  ||
+										(err == _MALI_OSK_ERR_FAULT &&
+										(*pregistered & cores_list[n]) == 0) );
+				/* If we didn't power up a core - it must be active and 
+				 * hasn't actually tried to power down - this is expected
+				 * for cores that haven't acknowledged
+				 * Alternatively we are shutting down and the core has
+				 * been unregistered
+				 */
+			}
+		}
+	}
+	/* Only used in debug builds */
+	MALI_IGNORE(ad);
+}
+
+
+mali_bool pmm_power_down_okay( _mali_pmm_internal_state_t *pmm )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	return ( pmm->cores_pend_down == pmm->cores_ack_down ? MALI_TRUE : MALI_FALSE );
+}
+
+mali_bool pmm_invoke_power_down( _mali_pmm_internal_state_t *pmm )
+{
+	_mali_osk_errcode_t err;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	/* Check that cores are pending power down during power down invoke */
+	MALI_DEBUG_ASSERT( pmm->cores_pend_down != 0 );
+	/* Check that cores are not pending power up during power down invoke */
+	MALI_DEBUG_ASSERT( pmm->cores_pend_up == 0 );
+
+	if( !pmm_power_down_okay( pmm ) )
+	{
+		MALIPMM_DEBUG_PRINT( ("PMM: Waiting for cores to go idle for power off - 0x%08x / 0x%08x\n", 
+				pmm->cores_pend_down, pmm->cores_ack_down) );
+		return MALI_FALSE;
+	}
+	else
+	{
+#if !MALI_PMM_NO_PMU
+		err = mali_platform_powerdown( pmm->cores_pend_down );
+#else
+		err = _MALI_OSK_ERR_OK;
+#endif
+		
+		if( err == _MALI_OSK_ERR_OK )
+		{
+#if MALI_PMM_TRACE
+			mali_pmm_core_mask old_power = pmm->cores_powered;
+#endif
+			/* Remove powered down cores from idle and powered list */
+			pmm->cores_powered &= ~(pmm->cores_pend_down);
+			pmm->cores_idle &= ~(pmm->cores_pend_down);
+			/* Reset pending/acknowledged status */
+			pmm->cores_pend_down = 0;
+			pmm->cores_ack_down = 0;
+#if MALI_PMM_TRACE
+			_mali_pmm_trace_hardware_change( old_power, pmm->cores_powered );
+#endif
+		}
+		else
+		{
+			MALI_PRINT_ERROR( ("PMM: Failed to get PMU to power down cores - (0x%x) %s", 
+					pmm->cores_pend_down, pmm_trace_get_core_name(pmm->cores_pend_down)) );
+			pmm->fatal_power_err = MALI_TRUE;
+		}
+	}
+
+	return MALI_TRUE;
+}
+
+
+mali_bool pmm_power_up_okay( _mali_pmm_internal_state_t *pmm )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	return ( pmm->cores_pend_up == pmm->cores_ack_up ? MALI_TRUE : MALI_FALSE );
+}
+
+
+mali_bool pmm_invoke_power_up( _mali_pmm_internal_state_t *pmm )
+{
+	_mali_osk_errcode_t err;
+
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+
+	/* Check that cores are pending power up during power up invoke */
+	MALI_DEBUG_ASSERT( pmm->cores_pend_up != 0 );
+	/* Check that cores are not pending power down during power up invoke */
+	MALI_DEBUG_ASSERT( pmm->cores_pend_down == 0 );
+
+	if( pmm_power_up_okay( pmm ) )
+	{
+		/* Power up has completed - sort out subsystem core status */
+		
+		int n;
+		/* Use volatile to access, so that it is updated if any cores are unregistered */
+		volatile mali_pmm_core_mask *ppendup = &(pmm->cores_pend_up);
+#if MALI_PMM_TRACE
+		mali_pmm_core_mask old_power = pmm->cores_powered;
+#endif
+		/* Move cores into idle queues */
+		for( n = 0; n < SIZEOF_CORES_LIST; n++ )
+		{
+			if( (cores_list[n] & (*ppendup)) != 0 )
+			{
+				/* Can't hold the power lock, when acessing subsystem mutex via
+				 * the core power call.
+				 * Due to terminatation of driver requiring a subsystem mutex
+				 * and then power lock held to unregister a core.
+				 * This does mean that the following function could fail
+				 * as the core is unregistered before we tell it to power
+				 * up, but it does not matter as we are terminating
+				 */
+#if MALI_STATE_TRACKING
+				pmm->mali_pmm_lock_acquired = 0;
+#endif /* MALI_STATE_TRACKING */
+
+				MALI_PMM_UNLOCK(pmm);
+				err = mali_core_signal_power_up( cores_list[n], MALI_FALSE );
+				MALI_PMM_LOCK(pmm);
+
+#if MALI_STATE_TRACKING
+				pmm->mali_pmm_lock_acquired = 1;
+#endif /* MALI_STATE_TRACKING */
+
+
+				if( err != _MALI_OSK_ERR_OK )
+				{
+					MALI_DEBUG_ASSERT( (err == _MALI_OSK_ERR_FAULT &&
+										(*ppendup & cores_list[n]) == 0) );
+					/* We only expect this to fail when we are shutting down 
+					 * and the core has been unregistered
+					 */
+				}
+			}
+		}
+		/* Finished power up - add cores to idle and powered list */
+		pmm->cores_powered |= (*ppendup);
+		pmm->cores_idle |= (*ppendup);
+		/* Reset pending/acknowledge status */
+		pmm->cores_pend_up = 0;
+		pmm->cores_ack_up = 0;
+
+#if MALI_PMM_TRACE
+		_mali_pmm_trace_hardware_change( old_power, pmm->cores_powered );
+#endif
+		return MALI_TRUE;
+	}
+	else
+	{
+#if !MALI_PMM_NO_PMU
+		/* Power up must now be done */
+		err = mali_platform_powerup( pmm->cores_pend_up );
+#else
+		err = _MALI_OSK_ERR_OK;
+#endif
+		if( err != _MALI_OSK_ERR_OK )
+		{
+			MALI_PRINT_ERROR( ("PMM: Failed to get PMU to power up cores - (0x%x) %s", 
+					pmm->cores_pend_up, pmm_trace_get_core_name(pmm->cores_pend_up)) );
+			pmm->fatal_power_err = MALI_TRUE;
+		}
+		else
+		{
+			/* TBD - Update core status immediately rather than use event message */
+			_mali_uk_pmm_message_s event = {
+				NULL,
+				MALI_PMM_EVENT_INTERNAL_POWER_UP_ACK,
+				0 };
+			/* All the cores that were pending power up, have now completed power up */
+			event.data = pmm->cores_pend_up;
+			_mali_ukk_pmm_event_message( &event );
+			MALIPMM_DEBUG_PRINT( ("PMM: Sending ACK to power up") );
+		}
+	}
+
+	/* Always return false, as we need an interrupt to acknowledge
+	 * when power up is complete
+	 */
+	return MALI_FALSE;
+}
+
+mali_pmm_core_mask pmm_cores_set_active( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_PMM_DEBUG_ASSERT_CORES_SUBSET( pmm->cores_registered, cores );
+
+	pmm->cores_idle &= (~cores);
+	return pmm->cores_idle;
+}
+
+mali_pmm_core_mask pmm_cores_set_idle( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_PMM_DEBUG_ASSERT_CORES_SUBSET( pmm->cores_registered, cores );
+
+	pmm->cores_idle |= (cores);
+	return pmm->cores_idle;
+}
+
+mali_pmm_core_mask pmm_cores_set_down_ack( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_PMM_DEBUG_ASSERT_CORES_SUBSET( pmm->cores_registered, cores );
+
+	/* Check core is not pending a power down */
+	MALI_DEBUG_ASSERT( (pmm->cores_pend_down & cores) != 0 );
+	/* Check core has not acknowledged power down more than once */
+	MALI_DEBUG_ASSERT( (pmm->cores_ack_down & cores) == 0 );
+
+	pmm->cores_ack_down |= (cores);
+
+	return pmm->cores_ack_down;
+}
+
+void pmm_fatal_reset( _mali_pmm_internal_state_t *pmm )
+{
+	_mali_osk_errcode_t err = _MALI_OSK_ERR_OK;
+	_mali_osk_notification_t *msg = NULL;
+	mali_pmm_status status;
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALIPMM_DEBUG_PRINT( ("PMM: Fatal Reset called") ); 
+
+	MALI_DEBUG_ASSERT( pmm->status != MALI_PMM_STATUS_OFF );
+
+	/* Reset the common status */
+	pmm->waiting = 0;
+	pmm->missed = 0;
+	pmm->fatal_power_err = MALI_FALSE;
+	pmm->no_events = 0;
+	pmm->check_policy = MALI_FALSE;
+	pmm->cores_pend_down = 0;
+	pmm->cores_pend_up = 0;
+	pmm->cores_ack_down = 0;
+	pmm->cores_ack_up = 0;
+	pmm->is_dvfs_active = 0; 
+#if MALI_PMM_TRACE
+	pmm->messages_sent = 0;
+	pmm->messages_received = 0;
+	pmm->imessages_sent = 0;
+	pmm->imessages_received = 0;
+	MALI_PRINT( ("PMM Trace: *** Fatal reset occurred ***") );
+#endif
+
+	/* Set that we are unavailable whilst resetting */
+	pmm->state = MALI_PMM_STATE_UNAVAILABLE;
+	status = pmm->status;
+	pmm->status = MALI_PMM_STATUS_OFF;
+
+	/* We want all cores powered */
+	pmm->cores_powered = pmm->cores_registered;
+	/* The cores may not be idle, but this state will be rectified later */
+	pmm->cores_idle = pmm->cores_registered;
+	
+	/* So power on any cores that are registered */
+	if( pmm->cores_registered != 0 )
+	{
+		int n;
+		volatile mali_pmm_core_mask *pregistered = &(pmm->cores_registered);
+#if !MALI_PMM_NO_PMU
+		err = mali_platform_powerup( pmm->cores_registered );
+#endif
+		if( err != _MALI_OSK_ERR_OK )
+		{
+			/* This is very bad as we can't even be certain the cores are now 
+			 * powered up
+			 */
+			MALI_PRINT_ERROR( ("PMM: Failed to perform PMM reset!\n") );
+			/* TBD driver exit? */
+		}
+
+		for( n = SIZEOF_CORES_LIST-1; n >= 0; n-- )
+		{
+			if( (cores_list[n] & (*pregistered)) != 0 )
+			{
+#if MALI_STATE_TRACKING
+				pmm->mali_pmm_lock_acquired = 0;
+#endif /* MALI_STATE_TRACKING */
+
+				MALI_PMM_UNLOCK(pmm);
+				/* Core is now active - so try putting it in the idle queue */
+				err = mali_core_signal_power_up( cores_list[n], MALI_FALSE );
+				MALI_PMM_LOCK(pmm);
+#if MALI_STATE_TRACKING
+                pmm->mali_pmm_lock_acquired = 1;
+#endif /* MALI_STATE_TRACKING */
+
+				/* We either succeeded, or we were not off anyway, or we have
+				 * just be deregistered 
+				 */
+				MALI_DEBUG_ASSERT( (err == _MALI_OSK_ERR_OK) ||
+									(err == _MALI_OSK_ERR_BUSY) ||
+									(err == _MALI_OSK_ERR_FAULT && 
+									(*pregistered & cores_list[n]) == 0) );
+			}
+		}
+	}
+
+	/* Unblock any pending OS event */
+	if( status == MALI_PMM_STATUS_OS_POWER_UP )
+	{
+		/* Get the OS data and respond to the power up */
+		_mali_osk_pmm_power_up_done( pmm_retrieve_os_event_data( pmm ) );
+	}
+	if( status == MALI_PMM_STATUS_OS_POWER_DOWN )
+	{
+		/* Get the OS data and respond to the power down 
+		 * NOTE: We are not powered down at this point due to power problems,
+		 * so we are lying to the system, but something bad has already 
+		 * happened and we are trying unstick things
+		 * TBD - Add busy loop to power down cores?
+		 */
+		_mali_osk_pmm_power_down_done( pmm_retrieve_os_event_data( pmm ) );
+	}
+		
+	/* Purge the event queues */
+	do
+	{
+		if( _mali_osk_notification_queue_dequeue( pmm->iqueue, &msg ) == _MALI_OSK_ERR_OK )
+		{
+			_mali_osk_notification_delete ( msg );
+			break;
+		}
+	} while (MALI_TRUE);
+
+	do
+	{
+		if( _mali_osk_notification_queue_dequeue( pmm->queue, &msg ) == _MALI_OSK_ERR_OK )
+		{
+			_mali_osk_notification_delete ( msg );
+			break;
+		}
+	} while (MALI_TRUE);
+
+	/* Return status/state to normal */
+	pmm->status = MALI_PMM_STATUS_IDLE;
+	pmm_update_system_state(pmm);
+}
+
+mali_pmm_core_mask pmm_cores_set_up_ack( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores )
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	MALI_PMM_DEBUG_ASSERT_CORES_SUBSET( pmm->cores_registered, cores );
+
+	/* Check core is not pending a power up */
+	MALI_DEBUG_ASSERT( (pmm->cores_pend_up & cores) != 0 );
+	/* Check core has not acknowledged power up more than once */
+	MALI_DEBUG_ASSERT( (pmm->cores_ack_up & cores) == 0 );
+
+	pmm->cores_ack_up |= (cores);
+
+	return pmm->cores_ack_up;
+}
+
+void pmm_save_os_event_data(_mali_pmm_internal_state_t *pmm, mali_pmm_message_data data)
+{
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	/* Check that there is no saved data */
+	MALI_DEBUG_ASSERT( pmm->os_data == 0 );
+	/* Can't store zero data - as retrieve check will fail */
+	MALI_DEBUG_ASSERT( data != 0 );
+
+	pmm->os_data = data;
+}
+
+mali_pmm_message_data pmm_retrieve_os_event_data(_mali_pmm_internal_state_t *pmm)
+{
+	mali_pmm_message_data data;
+
+	MALI_DEBUG_ASSERT_POINTER(pmm);
+	/* Check that there is saved data */
+	MALI_DEBUG_ASSERT( pmm->os_data != 0 );
+
+	/* Get data, and clear the saved version */
+	data = pmm->os_data;
+	pmm->os_data = 0;
+
+	return data;
+}
+
+/* Create list of core names to look up
+ * We are doing it this way to overcome the need for
+ * either string allocation, or stack space, so we
+ * use constant strings instead
+ */
+typedef struct pmm_trace_corelist
+{
+	mali_pmm_core_mask id;
+	const char *name;
+} pmm_trace_corelist_t;
+
+static pmm_trace_corelist_t pmm_trace_cores[] = {
+	{ MALI_PMM_CORE_SYSTEM,  "SYSTEM" },
+	{ MALI_PMM_CORE_GP,      "GP" },
+	{ MALI_PMM_CORE_L2,      "L2" },
+	{ MALI_PMM_CORE_PP0,     "PP0" },
+	{ MALI_PMM_CORE_PP1,     "PP1" },
+	{ MALI_PMM_CORE_PP2,     "PP2" },
+	{ MALI_PMM_CORE_PP3,     "PP3" },
+	{ MALI_PMM_CORE_PP_ALL,  "PP (all)" },
+	{ (MALI_PMM_CORE_GP | MALI_PMM_CORE_L2 | MALI_PMM_CORE_PP0),
+		"GP+L2+PP0" },
+	{ (MALI_PMM_CORE_GP | MALI_PMM_CORE_PP0),
+		"GP+PP0" },	
+	{ (MALI_PMM_CORE_GP | MALI_PMM_CORE_L2 | MALI_PMM_CORE_PP0 | MALI_PMM_CORE_PP1),
+		"GP+L2+PP0+PP1" },
+	{ (MALI_PMM_CORE_GP | MALI_PMM_CORE_PP0 | MALI_PMM_CORE_PP1),
+		"GP+PP0+PP1" },
+	{ 0, NULL } /* Terminator of list */
+};
+
+const char *pmm_trace_get_core_name( mali_pmm_core_mask cores )
+{
+	const char *dname = NULL;
+	int cl;
+	
+	/* Look up name in corelist */
+	cl = 0;
+	while( pmm_trace_cores[cl].name != NULL )
+	{
+		if( pmm_trace_cores[cl].id == cores )
+		{
+			dname = pmm_trace_cores[cl].name;
+			break;
+		}
+		cl++;
+	}
+	
+	if( dname == NULL )
+	{
+		/* We don't know a good short-hand for the configuration */
+		dname = "[multi-core]";
+	}
+
+	return dname;
+}
+
+#endif /* USING_MALI_PMM */
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_state.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_state.h
new file mode 100644
index 00000000000..3c8c31f066d
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_state.h
@@ -0,0 +1,296 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm_state.h
+ * Defines the internal power management module state
+ */
+
+#ifndef __MALI_PMM_STATE_H__
+#define __MALI_PMM_STATE_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @addtogroup pmmapi Power Management Module APIs
+ *
+ * @{
+ *
+ * @defgroup pmmapi_state Power Management Module State
+ *
+ * @{
+ */
+
+/* Check that the subset is really a subset of cores */
+#define MALI_PMM_DEBUG_ASSERT_CORES_SUBSET( cores, subset ) \
+	MALI_DEBUG_ASSERT( ((~(cores)) & (subset)) == 0 )
+
+
+/* Locking macros */
+#define MALI_PMM_LOCK(pmm) \
+	_mali_osk_lock_wait( pmm->lock, _MALI_OSK_LOCKMODE_RW )
+#define MALI_PMM_UNLOCK(pmm) \
+	_mali_osk_lock_signal( pmm->lock, _MALI_OSK_LOCKMODE_RW )
+#define MALI_PMM_LOCK_TERM(pmm) \
+        _mali_osk_lock_term( pmm->lock )
+
+/* Notification type for messages */
+#define MALI_PMM_NOTIFICATION_TYPE 0
+
+/** @brief Status of the PMM state machine
+ */
+typedef enum mali_pmm_status_tag
+{
+	MALI_PMM_STATUS_IDLE,                       /**< PMM is waiting next event */
+	MALI_PMM_STATUS_POLICY_POWER_DOWN,          /**< Policy initiated power down */
+	MALI_PMM_STATUS_POLICY_POWER_UP,            /**< Policy initiated power down */
+        MALI_PMM_STATUS_OS_WAITING,                 /**< PMM is waiting for OS power up */
+	MALI_PMM_STATUS_OS_POWER_DOWN,              /**< OS initiated power down */ 
+	MALI_PMM_STATUS_RUNTIME_IDLE_IN_PROGRESS,
+	MALI_PMM_STATUS_DVFS_PAUSE,                 /**< PMM DVFS Status Pause */
+	MALI_PMM_STATUS_OS_POWER_UP,                /**< OS initiated power up */
+	MALI_PMM_STATUS_OFF,                        /**< PMM is not active */
+} mali_pmm_status;
+
+
+/** @brief Internal state of the PMM
+ */
+typedef struct _mali_pmm_internal_state
+{
+	mali_pmm_status status;                 /**< PMM state machine */
+	mali_pmm_policy policy;                 /**< PMM policy */
+	mali_bool check_policy;                 /**< PMM policy needs checking */
+	mali_pmm_state state;                   /**< PMM state */
+	mali_pmm_core_mask cores_registered;    /**< Bitmask of cores registered */
+	mali_pmm_core_mask cores_powered;       /**< Bitmask of cores powered up */
+	mali_pmm_core_mask cores_idle;          /**< Bitmask of cores idle */
+	mali_pmm_core_mask cores_pend_down;     /**< Bitmask of cores pending power down */
+	mali_pmm_core_mask cores_pend_up;       /**< Bitmask of cores pending power up */
+	mali_pmm_core_mask cores_ack_down;      /**< Bitmask of cores acknowledged power down */
+	mali_pmm_core_mask cores_ack_up;        /**< Bitmask of cores acknowledged power up */
+
+	_mali_osk_notification_queue_t *queue;  /**< PMM event queue */
+	_mali_osk_notification_queue_t *iqueue; /**< PMM internal event queue */
+	_mali_osk_irq_t *irq;                   /**< PMM irq handler */
+	_mali_osk_lock_t *lock;                 /**< PMM lock */
+
+	mali_pmm_message_data os_data;          /**< OS data sent via the OS events */
+
+	mali_bool pmu_initialized;              /**< PMU initialized */
+
+	_mali_osk_atomic_t messages_queued;     /**< PMM event messages queued */
+	u32 waiting;                            /**< PMM waiting events - due to busy */
+	u32 no_events;                          /**< PMM called to process when no events */
+
+	u32 missed;                             /**< PMM missed events due to OOM */
+	mali_bool fatal_power_err;				/**< PMM has had a fatal power error? */
+	u32 is_dvfs_active;			/**< PMM DVFS activity */
+
+#if (defined(DEBUG) || MALI_STATE_TRACKING)
+	u32 mali_last_pmm_status;
+	u32 mali_new_event_status;
+	u32 mali_pmm_lock_acquired;
+#endif
+	
+#if (MALI_PMM_TRACE || MALI_STATE_TRACKING)
+	u32 messages_sent;                      /**< Total event messages sent */
+	u32 messages_received;                  /**< Total event messages received */
+	u32 imessages_sent;                     /**< Total event internal messages sent */
+	u32 imessages_received;                 /**< Total event internal messages received */
+#endif
+} _mali_pmm_internal_state_t;
+
+/** @brief Sets that a policy needs a check before processing events
+ *
+ * A timer or something has expired that needs dealing with
+ */
+void malipmm_set_policy_check(void);
+
+/** @brief Update the PMM externally viewable state depending on the current PMM internal state
+ *
+ * @param pmm internal PMM state
+ * @return MALI_TRUE if the timeout is valid, else MALI_FALSE
+ */
+void pmm_update_system_state( _mali_pmm_internal_state_t *pmm );
+
+/** @brief Returns the core mask from the event data - if applicable
+ *
+ * @param pmm internal PMM state
+ * @param event event message to get the core mask from
+ * @return mask of cores that is relevant to this event message
+ */
+mali_pmm_core_mask pmm_cores_from_event_data( _mali_pmm_internal_state_t *pmm, mali_pmm_message_t *event );
+
+/** @brief Sort out which cores need to be powered up from the given core mask
+ *
+ * All cores that can be powered up will be put into a pending state 
+ *
+ * @param pmm internal PMM state
+ * @param cores mask of cores to check if they need to be powered up
+ * @return mask of cores that need to be powered up, this can be 0 if all cores 
+ * are powered up already
+ */
+mali_pmm_core_mask pmm_cores_to_power_up( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores );
+
+/** @brief Sort out which cores need to be powered down from the given core mask
+ *
+ * All cores that can be powered down will be put into a pending state. If they
+ * can be powered down immediately they will also be acknowledged that they can be
+ * powered down. If the immediate_only flag is set, then only those cores that
+ * can be acknowledged for power down will be put into a pending state.
+ *
+ * @param pmm internal PMM state
+ * @param cores mask of cores to check if they need to be powered down
+ * @param immediate_only MALI_TRUE means that only cores that can power down now will
+ * be put into a pending state
+ * @return mask of cores that need to be powered down, this can be 0 if all cores 
+ * are powered down already
+ */
+mali_pmm_core_mask pmm_cores_to_power_down( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores, mali_bool immediate_only );
+
+/** @brief Cancel an invokation to power down (pmm_invoke_power_down)
+ *
+ * @param pmm internal PMM state
+ */
+void pmm_power_down_cancel( _mali_pmm_internal_state_t *pmm );
+
+/** @brief Check if a call to invoke power down should succeed, or fail
+ *
+ * This will report MALI_FALSE if some of the cores are still active and need
+ * to acknowledge that they are ready to power down
+ *
+ * @param pmm internal PMM state
+ * @return MALI_TRUE if the pending cores to power down have acknowledged they
+ * can power down, else MALI_FALSE
+ */
+mali_bool pmm_power_down_okay( _mali_pmm_internal_state_t *pmm );
+
+/** @brief Try to make all the pending cores power down
+ *
+ * If all the pending cores have acknowledged they can power down, this will call the 
+ * PMU power down function to turn them off
+ *
+ * @param pmm internal PMM state
+ * @return MALI_TRUE if the pending cores have been powered down, else MALI_FALSE
+ */
+mali_bool pmm_invoke_power_down( _mali_pmm_internal_state_t *pmm );
+
+/** @brief Check if all the pending cores to power up have done so
+ *
+ * This will report MALI_FALSE if some of the cores are still powered off
+ * and have not acknowledged that they have powered up
+ *
+ * @param pmm internal PMM state
+ * @return MALI_TRUE if the pending cores to power up have acknowledged they
+ * are now powered up, else MALI_FALSE
+ */
+mali_bool pmm_power_up_okay( _mali_pmm_internal_state_t *pmm );
+
+/** @brief Try to make all the pending cores power up
+ *
+ * If all the pending cores have acknowledged they have powered up, this will
+ * make the cores start processing jobs again, else this will call the PMU
+ * power up function to turn them on, and the PMM is then expected to wait for an
+ * interrupt to acknowledge the power up
+ *
+ * @param pmm internal PMM state
+ * @return MALI_TRUE if the pending cores have been powered up, else MALI_FALSE
+ */
+mali_bool pmm_invoke_power_up( _mali_pmm_internal_state_t *pmm );
+
+/** @brief Set the cores that are now active in the system
+ *
+ * Updates which cores are active and returns which cores are still idle
+ *
+ * @param pmm internal PMM state
+ * @param cores mask of cores to set to active
+ * @return mask of all the cores that are idle
+ */
+mali_pmm_core_mask pmm_cores_set_active( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores );
+
+/** @brief Set the cores that are now idle in the system
+ *
+ * Updates which cores are idle and returns which cores are still idle
+ *
+ * @param pmm internal PMM state
+ * @param cores mask of cores to set to idle
+ * @return mask of all the cores that are idle
+ */
+mali_pmm_core_mask pmm_cores_set_idle( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores );
+
+/** @brief Set the cores that have acknowledged a pending power down
+ *
+ * Updates which cores have acknowledged the pending power down and are now ready 
+ * to be turned off
+ *
+ * @param pmm internal PMM state
+ * @param cores mask of cores that have acknowledged the pending power down
+ * @return mask of all the cores that have acknowledged the power down
+ */
+mali_pmm_core_mask pmm_cores_set_down_ack( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores );
+
+/** @brief Set the cores that have acknowledged a pending power up
+ *
+ * Updates which cores have acknowledged the pending power up and are now
+ * fully powered and ready to run jobs
+ *
+ * @param pmm internal PMM state
+ * @param cores mask of cores that have acknowledged the pending power up
+ * @return mask of all the cores that have acknowledged the power up
+ */
+mali_pmm_core_mask pmm_cores_set_up_ack( _mali_pmm_internal_state_t *pmm, mali_pmm_core_mask cores );
+
+
+/** @brief Tries to reset the PMM and PMU hardware to a known state after any fatal issues
+ *
+ * This will try and make all the cores powered up and reset the PMM state 
+ * to its initial state after core registration - all cores powered but not 
+ * pending or active.
+ * All events in the event queues will be thrown away.
+ *
+ * @note: Any pending power down will be cancelled including the OS calling for power down
+ */
+void pmm_fatal_reset( _mali_pmm_internal_state_t *pmm );
+	
+/** @brief Save the OS specific data for an OS power up/down event
+ *
+ * @param pmm internal PMM state
+ * @param data OS specific event data
+ */
+void pmm_save_os_event_data(_mali_pmm_internal_state_t *pmm, mali_pmm_message_data data);
+
+/** @brief Retrieve the OS specific data for an OS power up/down event
+ *
+ * This will clear the stored OS data, as well as return it.
+ *
+ * @param pmm internal PMM state
+ * @return OS specific event data that was saved previously
+ */
+mali_pmm_message_data pmm_retrieve_os_event_data(_mali_pmm_internal_state_t *pmm);
+
+
+/** @brief Get a human readable name for the cores in a core mask
+ *
+ * @param core the core mask
+ * @return string containing a name relating to the given core mask
+ */
+const char *pmm_trace_get_core_name( mali_pmm_core_mask core );
+
+/** @} */ /* End group pmmapi_state */
+/** @} */ /* End group pmmapi */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_PMM_STATE_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_system.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_system.h
new file mode 100644
index 00000000000..f5106479e33
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/common/pmm/mali_pmm_system.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_pmm_system.h
+ * Defines the power management module system functions
+ */
+
+#ifndef __MALI_PMM_SYSTEM_H__
+#define __MALI_PMM_SYSTEM_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @addtogroup pmmapi Power Management Module APIs
+ *
+ * @{
+ *
+ * @defgroup pmmapi_system Power Management Module System Functions
+ *
+ * @{
+ */
+
+extern struct mali_kernel_subsystem mali_subsystem_pmm;
+
+/** @brief Register a core with the PMM, which will power up
+ * the core
+ *
+ * @param core the core to register with the PMM
+ * @return error if the core cannot be powered up
+ */
+_mali_osk_errcode_t malipmm_core_register( mali_pmm_core_id core );
+
+/** @brief Unregister a core with the PMM
+ *
+ * @param core the core to unregister with the PMM
+ */
+void malipmm_core_unregister( mali_pmm_core_id core );
+
+/** @brief Acknowledge that a power down is okay to happen
+ *
+ * A core should not be running a job, or be in the idle queue when this
+ * is called.
+ *
+ * @param core the core that can now be powered down
+ */
+void malipmm_core_power_down_okay( mali_pmm_core_id core );
+
+/** @} */ /* End group pmmapi_system */
+/** @} */ /* End group pmmapi */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_PMM_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/license/gpl/mali_kernel_license.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/license/gpl/mali_kernel_license.h
new file mode 100644
index 00000000000..e9e5e55a082
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/license/gpl/mali_kernel_license.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2010 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_kernel_license.h
+ * Defines for the macro MODULE_LICENSE.
+ */
+
+#ifndef __MALI_KERNEL_LICENSE_H__
+#define __MALI_KERNEL_LICENSE_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#define MALI_KERNEL_LINUX_LICENSE     "GPL"
+#define MALI_LICENSE_IS_GPL 1
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_KERNEL_LICENSE_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_device_pause_resume.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_device_pause_resume.c
new file mode 100644
index 00000000000..1c1bf306688
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_device_pause_resume.c
@@ -0,0 +1,82 @@
+/**
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_device_pause_resume.c
+ * Implementation of the Mali pause/resume functionality
+ */
+#if USING_MALI_PMM
+#include <linux/version.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+#include "mali_platform.h"
+#include "mali_linux_pm.h"
+#include "mali_device_pause_resume.h"
+#include "mali_pmm.h"
+#include "mali_kernel_license.h"
+#ifdef CONFIG_PM
+#if MALI_LICENSE_IS_GPL
+
+/* Mali Pause Resume APIs */
+int mali_dev_pause()
+{
+	int err = 0;
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+	if ((mali_dvfs_device_state ==  _MALI_DEVICE_SUSPEND) || (mali_device_state == _MALI_DEVICE_SUSPEND_IN_PROGRESS)
+	      || (mali_device_state == _MALI_DEVICE_SUSPEND) 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	      || (mali_device_state == _MALI_DEVICE_EARLYSUSPEND_DISABLE_FB))
+#else
+	)
+#endif
+	{
+		err = -EPERM;
+	}
+	if ((mali_dvfs_device_state ==  _MALI_DEVICE_RESUME) && (!err))
+	{
+		mali_device_suspend(MALI_PMM_EVENT_DVFS_PAUSE, &dvfs_pm_thread);
+		mali_dvfs_device_state = _MALI_DEVICE_SUSPEND;
+	}
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	return err;
+}
+
+EXPORT_SYMBOL(mali_dev_pause);
+
+int mali_dev_resume()
+{
+	int err = 0;
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+	if ((mali_dvfs_device_state == _MALI_DEVICE_RESUME) || (mali_device_state == _MALI_DEVICE_SUSPEND_IN_PROGRESS)
+	     || (mali_device_state == _MALI_DEVICE_SUSPEND) 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	     || (mali_device_state == _MALI_DEVICE_EARLYSUSPEND_DISABLE_FB))
+#else
+	)
+#endif
+	{
+		err = -EPERM;
+	}
+	if (!err)
+	{
+		mali_device_resume(MALI_PMM_EVENT_DVFS_RESUME, &dvfs_pm_thread);
+		mali_dvfs_device_state = _MALI_DEVICE_RESUME;
+	}
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	return err;
+}
+
+EXPORT_SYMBOL(mali_dev_resume);
+
+#endif /* MALI_LICENSE_IS_GPL */
+#endif /* CONFIG_PM */
+#endif /* USING_MALI_PMM */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_device_pause_resume.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_device_pause_resume.h
new file mode 100644
index 00000000000..5362f88cdbd
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_device_pause_resume.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_DEVICE_PAUSE_RESUME_H__
+#define __MALI_DEVICE_PAUSE_RESUME_H__
+
+#if USING_MALI_PMM
+int mali_dev_pause(void);
+int mali_dev_resume(void);
+#endif /* USING_MALI_PMM */
+
+#endif /* __MALI_DEVICE_PAUSE_RESUME_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_ioctl.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_ioctl.h
new file mode 100644
index 00000000000..30a6fa041db
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_ioctl.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_IOCTL_H__
+#define __MALI_KERNEL_IOCTL_H__
+
+#include <linux/types.h>
+#include <linux/ioctl.h>
+#include <linux/fs.h>       /* file system operations */
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * @file mali_kernel_ioctl.h
+ * Interface to the Linux device driver.
+ * This file describes the interface needed to use the Linux device driver.
+ * Its interface is designed to used by the HAL implementation through a thin arch layer.
+ */
+
+/**
+ * ioctl commands
+ */
+
+#define MALI_IOC_BASE           0x82
+#define MALI_IOC_CORE_BASE      (_MALI_UK_CORE_SUBSYSTEM      + MALI_IOC_BASE)
+#define MALI_IOC_MEMORY_BASE    (_MALI_UK_MEMORY_SUBSYSTEM    + MALI_IOC_BASE)
+#define MALI_IOC_PP_BASE        (_MALI_UK_PP_SUBSYSTEM        + MALI_IOC_BASE)
+#define MALI_IOC_GP_BASE        (_MALI_UK_GP_SUBSYSTEM        + MALI_IOC_BASE)
+#define MALI_IOC_PROFILING_BASE (_MALI_UK_PROFILING_SUBSYSTEM + MALI_IOC_BASE)
+#define MALI_IOC_VSYNC_BASE     (_MALI_UK_VSYNC_SUBSYSTEM + MALI_IOC_BASE)
+
+#define MALI_IOC_GET_SYSTEM_INFO_SIZE       _IOR (MALI_IOC_CORE_BASE, _MALI_UK_GET_SYSTEM_INFO_SIZE, _mali_uk_get_system_info_s *)
+#define MALI_IOC_GET_SYSTEM_INFO            _IOR (MALI_IOC_CORE_BASE, _MALI_UK_GET_SYSTEM_INFO, _mali_uk_get_system_info_s *)
+#define MALI_IOC_WAIT_FOR_NOTIFICATION      _IOWR(MALI_IOC_CORE_BASE, _MALI_UK_WAIT_FOR_NOTIFICATION, _mali_uk_wait_for_notification_s *)
+#define MALI_IOC_GET_API_VERSION            _IOWR(MALI_IOC_CORE_BASE, _MALI_UK_GET_API_VERSION, _mali_uk_get_api_version_s *)
+#define MALI_IOC_POST_NOTIFICATION          _IOWR(MALI_IOC_CORE_BASE, _MALI_UK_POST_NOTIFICATION, _mali_uk_post_notification_s *)
+#define MALI_IOC_MEM_GET_BIG_BLOCK          _IOWR(MALI_IOC_MEMORY_BASE, _MALI_UK_GET_BIG_BLOCK, _mali_uk_get_big_block_s *)
+#define MALI_IOC_MEM_FREE_BIG_BLOCK         _IOW (MALI_IOC_MEMORY_BASE, _MALI_UK_FREE_BIG_BLOCK, _mali_uk_free_big_block_s *)
+#define MALI_IOC_MEM_INIT                   _IOR (MALI_IOC_MEMORY_BASE, _MALI_UK_INIT_MEM, _mali_uk_init_mem_s *)
+#define MALI_IOC_MEM_TERM                   _IOW (MALI_IOC_MEMORY_BASE, _MALI_UK_TERM_MEM, _mali_uk_term_mem_s *)
+#define MALI_IOC_MEM_MAP_EXT                _IOWR(MALI_IOC_MEMORY_BASE, _MALI_UK_MAP_EXT_MEM, _mali_uk_map_external_mem_s *)
+#define MALI_IOC_MEM_UNMAP_EXT              _IOW (MALI_IOC_MEMORY_BASE, _MALI_UK_UNMAP_EXT_MEM, _mali_uk_unmap_external_mem_s *)
+#define MALI_IOC_MEM_QUERY_MMU_PAGE_TABLE_DUMP_SIZE _IOR (MALI_IOC_MEMORY_BASE, _MALI_UK_QUERY_MMU_PAGE_TABLE_DUMP_SIZE, _mali_uk_query_mmu_page_table_dump_size_s *)
+#define MALI_IOC_MEM_DUMP_MMU_PAGE_TABLE    _IOWR(MALI_IOC_MEMORY_BASE, _MALI_UK_DUMP_MMU_PAGE_TABLE, _mali_uk_dump_mmu_page_table_s *)
+#define MALI_IOC_MEM_ATTACH_UMP             _IOWR(MALI_IOC_MEMORY_BASE, _MALI_UK_ATTACH_UMP_MEM, _mali_uk_attach_ump_mem_s *)
+#define MALI_IOC_MEM_RELEASE_UMP            _IOW(MALI_IOC_MEMORY_BASE, _MALI_UK_RELEASE_UMP_MEM, _mali_uk_release_ump_mem_s *)
+#define MALI_IOC_PP_START_JOB               _IOWR(MALI_IOC_PP_BASE, _MALI_UK_PP_START_JOB, _mali_uk_pp_start_job_s *)
+#define MALI_IOC_PP_NUMBER_OF_CORES_GET	    _IOR (MALI_IOC_PP_BASE, _MALI_UK_GET_PP_NUMBER_OF_CORES, _mali_uk_get_pp_number_of_cores_s *)
+#define MALI_IOC_PP_CORE_VERSION_GET	    _IOR (MALI_IOC_PP_BASE, _MALI_UK_GET_PP_CORE_VERSION, _mali_uk_get_pp_core_version_s * )
+#define MALI_IOC_PP_ABORT_JOB	            _IOW (MALI_IOC_PP_BASE, _MALI_UK_PP_ABORT_JOB, _mali_uk_pp_abort_job_s * )
+#define MALI_IOC_GP2_START_JOB              _IOWR(MALI_IOC_GP_BASE, _MALI_UK_GP_START_JOB, _mali_uk_gp_start_job_s *)
+#define MALI_IOC_GP2_ABORT_JOB              _IOWR(MALI_IOC_GP_BASE, _MALI_UK_GP_ABORT_JOB, _mali_uk_gp_abort_job_s *)
+#define MALI_IOC_GP2_NUMBER_OF_CORES_GET    _IOR (MALI_IOC_GP_BASE, _MALI_UK_GET_GP_NUMBER_OF_CORES, _mali_uk_get_gp_number_of_cores_s *)
+#define MALI_IOC_GP2_CORE_VERSION_GET	    _IOR (MALI_IOC_GP_BASE, _MALI_UK_GET_GP_CORE_VERSION, _mali_uk_get_gp_core_version_s *)
+#define MALI_IOC_GP2_SUSPEND_RESPONSE	    _IOW (MALI_IOC_GP_BASE, _MALI_UK_GP_SUSPEND_RESPONSE,_mali_uk_gp_suspend_response_s *)
+#define MALI_IOC_PROFILING_START            _IOWR(MALI_IOC_PROFILING_BASE, _MALI_UK_PROFILING_START, _mali_uk_profiling_start_s *)
+#define MALI_IOC_PROFILING_ADD_EVENT        _IOWR(MALI_IOC_PROFILING_BASE, _MALI_UK_PROFILING_ADD_EVENT, _mali_uk_profiling_add_event_s*)
+#define MALI_IOC_PROFILING_STOP             _IOWR(MALI_IOC_PROFILING_BASE, _MALI_UK_PROFILING_STOP, _mali_uk_profiling_stop_s *)
+#define MALI_IOC_PROFILING_GET_EVENT        _IOWR(MALI_IOC_PROFILING_BASE, _MALI_UK_PROFILING_GET_EVENT, _mali_uk_profiling_get_event_s *)
+#define MALI_IOC_PROFILING_CLEAR            _IOWR(MALI_IOC_PROFILING_BASE, _MALI_UK_PROFILING_CLEAR, _mali_uk_profiling_clear_s *)
+#define MALI_IOC_VSYNC_EVENT_REPORT         _IOW (MALI_IOC_VSYNC_BASE, _MALI_UK_VSYNC_EVENT_REPORT, _mali_uk_vsync_event_report_s *)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_KERNEL_IOCTL_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_linux.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_linux.c
new file mode 100644
index 00000000000..a5144faae2b
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_linux.c
@@ -0,0 +1,565 @@
+/**
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_kernel_linux.c
+ * Implementation of the Linux device driver entrypoints
+ */
+#include <linux/module.h>   /* kernel module definitions */
+#include <linux/fs.h>       /* file system operations */
+#include <linux/cdev.h>     /* character device definitions */
+#include <linux/mm.h> /* memory mananger definitions */
+#include <asm/uaccess.h>    /* user space access */
+#include <linux/device.h>
+#include <linux/proc_fs.h>
+
+/* the mali kernel subsystem types */
+#include "mali_kernel_subsystem.h"
+
+/* A memory subsystem always exists, so no need to conditionally include it */
+#include "mali_kernel_common.h"
+#include "mali_kernel_mem.h"
+#include "mali_kernel_session_manager.h"
+#include "mali_kernel_core.h"
+
+#include "mali_osk.h"
+#include "mali_kernel_linux.h"
+#include "mali_ukk.h"
+#include "mali_kernel_ioctl.h"
+#include "mali_ukk_wrappers.h"
+#include "mali_kernel_pm.h"
+
+/* */
+#include "mali_kernel_license.h"
+
+/* Setting this parameter will override memory settings in arch/config.h */
+char *mali_mem = "";
+module_param(mali_mem, charp, S_IRUSR | S_IWUSR | S_IWGRP | S_IROTH); /* rw-r--r-- */
+MODULE_PARM_DESC(mali_mem, "Override mali memory configuration. e.g. 32M@224M or 64M@OS_MEMORY");
+
+/* Module parameter to control log level */
+int mali_debug_level = 2;
+module_param(mali_debug_level, int, S_IRUSR | S_IWUSR | S_IWGRP | S_IRGRP | S_IROTH); /* rw-rw-r-- */
+MODULE_PARM_DESC(mali_debug_level, "Higher number, more dmesg output");
+
+/* By default the module uses any available major, but it's possible to set it at load time to a specific number */
+int mali_major = 0;
+module_param(mali_major, int, S_IRUGO); /* r--r--r-- */
+MODULE_PARM_DESC(mali_major, "Device major number");
+
+int mali_benchmark = 0;
+module_param(mali_benchmark, int, S_IRUSR | S_IWUSR | S_IWGRP | S_IRGRP | S_IROTH); /* rw-rw-r-- */
+MODULE_PARM_DESC(mali_benchmark, "Bypass Mali hardware when non-zero");
+
+extern int mali_hang_check_interval;
+module_param(mali_hang_check_interval, int, S_IRUSR | S_IWUSR | S_IWGRP | S_IRGRP | S_IROTH);
+MODULE_PARM_DESC(mali_hang_check_interval, "Interval at which to check for progress after the hw watchdog has been triggered");
+
+extern int mali_max_job_runtime;
+module_param(mali_max_job_runtime, int, S_IRUSR | S_IWUSR | S_IWGRP | S_IRGRP | S_IROTH);
+MODULE_PARM_DESC(mali_max_job_runtime, "Maximum allowed job runtime in msecs.\nJobs will be killed after this no matter what");
+
+#if defined(USING_MALI400_L2_CACHE)
+extern int mali_l2_max_reads;
+module_param(mali_l2_max_reads, int, S_IRUSR | S_IRGRP | S_IROTH);
+MODULE_PARM_DESC(mali_l2_max_reads, "Maximum reads for Mali L2 cache");
+#endif
+
+struct mali_dev
+{
+	struct cdev cdev;
+#if MALI_LICENSE_IS_GPL
+	struct class *  mali_class;
+#endif
+};
+
+static char mali_dev_name[] = "mali"; /* should be const, but the functions we call requires non-cost */
+
+/* the mali device */
+static struct mali_dev device;
+
+#if MALI_STATE_TRACKING
+static struct proc_dir_entry *proc_entry;
+static int mali_proc_read(char *page, char **start, off_t off, int count, int *eof, void *data);
+#endif
+
+
+static int mali_open(struct inode *inode, struct file *filp);
+static int mali_release(struct inode *inode, struct file *filp);
+#ifdef HAVE_UNLOCKED_IOCTL
+static long mali_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
+#else
+static int mali_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg);
+#endif
+
+static int mali_mmap(struct file * filp, struct vm_area_struct * vma);
+
+/* Linux char file operations provided by the Mali module */
+struct file_operations mali_fops =
+{
+	.owner = THIS_MODULE,
+	.open = mali_open,
+	.release = mali_release,
+#ifdef HAVE_UNLOCKED_IOCTL
+	.unlocked_ioctl = mali_ioctl,
+#else
+	.ioctl = mali_ioctl,
+#endif
+	.mmap = mali_mmap
+};
+
+
+int mali_driver_init(void)
+{
+	int err;
+#if USING_MALI_PMM
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM
+	err = _mali_dev_platform_register();
+	if (err)
+	{
+		return err;
+	}
+#endif
+#endif
+#endif
+	err = mali_kernel_constructor();
+	if (_MALI_OSK_ERR_OK != err)
+	{
+#if USING_MALI_PMM
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM
+		_mali_dev_platform_unregister();
+#endif
+#endif
+#endif
+		MALI_PRINT(("Failed to initialize driver (error %d)\n", err));
+		return -EFAULT;
+	}
+
+    return 0;
+}
+
+void mali_driver_exit(void)
+{
+
+#if USING_MALI_PMM
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+
+	_mali_osk_pmm_dev_activate();
+#endif
+#endif
+#endif
+#endif
+	 mali_kernel_destructor();
+
+#if USING_MALI_PMM
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+	_mali_osk_pmm_dev_idle();
+#endif
+#endif
+#endif
+#endif
+
+#if USING_MALI_PMM
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM
+	_mali_dev_platform_unregister();
+#endif
+#endif
+#endif
+}
+
+/* called from _mali_osk_init */
+int initialize_kernel_device(void)
+{
+	int err;
+	dev_t dev = 0;
+	if (0 == mali_major)
+	{
+		/* auto select a major */
+		err = alloc_chrdev_region(&dev, 0/*first minor*/, 1/*count*/, mali_dev_name);
+		mali_major = MAJOR(dev);
+	}
+	else
+	{
+		/* use load time defined major number */
+		dev = MKDEV(mali_major, 0);
+		err = register_chrdev_region(dev, 1/*count*/, mali_dev_name);
+	}
+
+	if (0 == err)
+	{
+		memset(&device, 0, sizeof(device));
+
+		/* initialize our char dev data */
+		cdev_init(&device.cdev, &mali_fops);
+		device.cdev.owner = THIS_MODULE;
+		device.cdev.ops = &mali_fops;
+
+		/* register char dev with the kernel */
+		err = cdev_add(&device.cdev, dev, 1/*count*/);
+
+		if (0 == err)
+		{
+#if MALI_STATE_TRACKING
+			proc_entry = create_proc_entry(mali_dev_name, 0444, NULL);
+			if (proc_entry != NULL)
+			{
+				proc_entry->read_proc = mali_proc_read;
+#endif
+#if MALI_LICENSE_IS_GPL
+				device.mali_class = class_create(THIS_MODULE, mali_dev_name);
+				if (IS_ERR(device.mali_class))
+				{
+					err = PTR_ERR(device.mali_class);
+				}
+				else
+				{
+					struct device * mdev;
+					mdev = device_create(device.mali_class, NULL, dev, NULL, mali_dev_name);
+					if (!IS_ERR(mdev))
+					{
+						return 0;
+					}
+
+					err = PTR_ERR(mdev);
+				}
+				cdev_del(&device.cdev);
+#else
+				return 0;
+#endif
+#if MALI_STATE_TRACKING
+				remove_proc_entry(mali_dev_name, NULL);
+			}
+			else
+			{
+				err = EFAULT;
+			}
+#endif
+		}
+		unregister_chrdev_region(dev, 1/*count*/);
+	}
+
+
+	return err;
+}
+
+/* called from _mali_osk_term */
+void terminate_kernel_device(void)
+{
+	dev_t dev = MKDEV(mali_major, 0);
+	
+#if MALI_LICENSE_IS_GPL
+	device_destroy(device.mali_class, dev);
+	class_destroy(device.mali_class);
+#endif
+
+#if MALI_STATE_TRACKING
+	remove_proc_entry(mali_dev_name, NULL);
+#endif
+
+	/* unregister char device */
+	cdev_del(&device.cdev);
+	/* free major */
+	unregister_chrdev_region(dev, 1/*count*/);
+	return;
+}
+
+/** @note munmap handler is done by vma close handler */
+static int mali_mmap(struct file * filp, struct vm_area_struct * vma)
+{
+	struct mali_session_data * session_data;
+	_mali_uk_mem_mmap_s args = {0, };
+
+    session_data = (struct mali_session_data *)filp->private_data;
+	if (NULL == session_data)
+	{
+		MALI_PRINT_ERROR(("mmap called without any session data available\n"));
+		return -EFAULT;
+	}
+
+	MALI_DEBUG_PRINT(3, ("MMap() handler: start=0x%08X, phys=0x%08X, size=0x%08X\n", (unsigned int)vma->vm_start, (unsigned int)(vma->vm_pgoff << PAGE_SHIFT), (unsigned int)(vma->vm_end - vma->vm_start)) );
+
+	/* Re-pack the arguments that mmap() packed for us */
+	args.ctx = session_data;
+	args.phys_addr = vma->vm_pgoff << PAGE_SHIFT;
+	args.size = vma->vm_end - vma->vm_start;
+	args.ukk_private = vma;
+
+	/* Call the common mmap handler */
+	MALI_CHECK(_MALI_OSK_ERR_OK ==_mali_ukk_mem_mmap( &args ), -EFAULT);
+
+    return 0;
+}
+
+static int mali_open(struct inode *inode, struct file *filp)
+{
+	struct mali_session_data * session_data;
+    _mali_osk_errcode_t err;
+
+	/* input validation */
+	if (0 != MINOR(inode->i_rdev)) return -ENODEV;
+
+	/* allocated struct to track this session */
+    err = _mali_ukk_open((void **)&session_data);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+	/* initialize file pointer */
+	filp->f_pos = 0;
+
+	/* link in our session data */
+	filp->private_data = (void*)session_data;
+
+	return 0;
+}
+
+static int mali_release(struct inode *inode, struct file *filp)
+{
+    _mali_osk_errcode_t err;
+
+    /* input validation */
+	if (0 != MINOR(inode->i_rdev)) return -ENODEV;
+
+    err = _mali_ukk_close((void **)&filp->private_data);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+	return 0;
+}
+
+int map_errcode( _mali_osk_errcode_t err )
+{
+    switch(err)
+    {
+        case _MALI_OSK_ERR_OK : return 0;
+        case _MALI_OSK_ERR_FAULT: return -EFAULT;
+        case _MALI_OSK_ERR_INVALID_FUNC: return -ENOTTY;
+        case _MALI_OSK_ERR_INVALID_ARGS: return -EINVAL;
+        case _MALI_OSK_ERR_NOMEM: return -ENOMEM;
+        case _MALI_OSK_ERR_TIMEOUT: return -ETIMEDOUT;
+        case _MALI_OSK_ERR_RESTARTSYSCALL: return -ERESTARTSYS;
+        case _MALI_OSK_ERR_ITEM_NOT_FOUND: return -ENOENT;
+        default: return -EFAULT;
+    }
+}
+
+#ifdef HAVE_UNLOCKED_IOCTL
+static long mali_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+#else
+static int mali_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg)
+#endif
+{
+    int err;
+    struct mali_session_data *session_data;
+
+#ifndef HAVE_UNLOCKED_IOCTL
+	/* inode not used */
+	(void)inode;
+#endif
+
+	MALI_DEBUG_PRINT(7, ("Ioctl received 0x%08X 0x%08lX\n", cmd, arg));
+
+    session_data = (struct mali_session_data *)filp->private_data;
+    if (NULL == session_data)
+	{
+		MALI_DEBUG_PRINT(7, ("filp->private_data was NULL\n"));
+		return -ENOTTY;
+	}
+    if (NULL == (void *)arg)
+	{
+		MALI_DEBUG_PRINT(7, ("arg was NULL\n"));
+		return -ENOTTY;
+	}
+
+    switch(cmd)
+    {
+        case MALI_IOC_GET_SYSTEM_INFO_SIZE:
+            err = get_system_info_size_wrapper(session_data, (_mali_uk_get_system_info_size_s __user *)arg);
+            break;
+
+        case MALI_IOC_GET_SYSTEM_INFO:
+            err = get_system_info_wrapper(session_data, (_mali_uk_get_system_info_s __user *)arg);
+            break;
+
+        case MALI_IOC_WAIT_FOR_NOTIFICATION:
+            err = wait_for_notification_wrapper(session_data, (_mali_uk_wait_for_notification_s __user *)arg);
+            break;
+
+        case MALI_IOC_GET_API_VERSION:
+            err = get_api_version_wrapper(session_data, (_mali_uk_get_api_version_s __user *)arg);
+            break;
+
+        case MALI_IOC_POST_NOTIFICATION:
+            err = post_notification_wrapper(session_data, (_mali_uk_post_notification_s __user *)arg);
+            break;
+
+#if MALI_TIMELINE_PROFILING_ENABLED
+        case MALI_IOC_PROFILING_START:
+            err = profiling_start_wrapper(session_data, (_mali_uk_profiling_start_s __user *)arg);
+            break;
+
+		case MALI_IOC_PROFILING_ADD_EVENT:
+            err = profiling_add_event_wrapper(session_data, (_mali_uk_profiling_add_event_s __user *)arg);
+            break;
+
+		case MALI_IOC_PROFILING_STOP:
+            err = profiling_stop_wrapper(session_data, (_mali_uk_profiling_stop_s __user *)arg);
+            break;
+
+		case MALI_IOC_PROFILING_GET_EVENT:
+            err = profiling_get_event_wrapper(session_data, (_mali_uk_profiling_get_event_s __user *)arg);
+            break;
+
+		case MALI_IOC_PROFILING_CLEAR:
+            err = profiling_clear_wrapper(session_data, (_mali_uk_profiling_clear_s __user *)arg);
+            break;
+#endif
+
+        case MALI_IOC_MEM_INIT:
+            err = mem_init_wrapper(session_data, (_mali_uk_init_mem_s __user *)arg);
+            break;
+
+        case MALI_IOC_MEM_TERM:
+            err = mem_term_wrapper(session_data, (_mali_uk_term_mem_s __user *)arg);
+            break;
+
+        case MALI_IOC_MEM_MAP_EXT:
+            err = mem_map_ext_wrapper(session_data, (_mali_uk_map_external_mem_s __user *)arg);
+            break;
+
+        case MALI_IOC_MEM_UNMAP_EXT:
+            err = mem_unmap_ext_wrapper(session_data, (_mali_uk_unmap_external_mem_s __user *)arg);
+            break;
+
+        case MALI_IOC_MEM_QUERY_MMU_PAGE_TABLE_DUMP_SIZE:
+            err = mem_query_mmu_page_table_dump_size_wrapper(session_data, (_mali_uk_query_mmu_page_table_dump_size_s __user *)arg);
+            break;
+
+        case MALI_IOC_MEM_DUMP_MMU_PAGE_TABLE:
+            err = mem_dump_mmu_page_table_wrapper(session_data, (_mali_uk_dump_mmu_page_table_s __user *)arg);
+            break;
+
+        case MALI_IOC_MEM_GET_BIG_BLOCK:
+            err = mem_get_big_block_wrapper(filp, (_mali_uk_get_big_block_s __user *)arg);
+            break;
+
+        case MALI_IOC_MEM_FREE_BIG_BLOCK:
+            err = mem_free_big_block_wrapper(session_data, (_mali_uk_free_big_block_s __user *)arg);
+            break;
+
+#if MALI_USE_UNIFIED_MEMORY_PROVIDER != 0
+
+		case MALI_IOC_MEM_ATTACH_UMP:
+			err = mem_attach_ump_wrapper(session_data, (_mali_uk_attach_ump_mem_s __user *)arg);
+			break;
+
+		case MALI_IOC_MEM_RELEASE_UMP:
+			err = mem_release_ump_wrapper(session_data, (_mali_uk_release_ump_mem_s __user *)arg);
+			break;
+
+#else
+
+		case MALI_IOC_MEM_ATTACH_UMP:
+		case MALI_IOC_MEM_RELEASE_UMP: /* FALL-THROUGH */
+        	MALI_DEBUG_PRINT(2, ("UMP not supported\n"));
+            err = -ENOTTY;
+			break;
+#endif
+
+        case MALI_IOC_PP_START_JOB:
+            err = pp_start_job_wrapper(session_data, (_mali_uk_pp_start_job_s __user *)arg);
+            break;
+
+        case MALI_IOC_PP_ABORT_JOB:
+            err = pp_abort_job_wrapper(session_data, (_mali_uk_pp_abort_job_s __user *)arg);
+            break;
+
+        case MALI_IOC_PP_NUMBER_OF_CORES_GET:
+            err = pp_get_number_of_cores_wrapper(session_data, (_mali_uk_get_pp_number_of_cores_s __user *)arg);
+            break;
+
+        case MALI_IOC_PP_CORE_VERSION_GET:
+            err = pp_get_core_version_wrapper(session_data, (_mali_uk_get_pp_core_version_s __user *)arg);
+            break;
+
+        case MALI_IOC_GP2_START_JOB:
+            err = gp_start_job_wrapper(session_data, (_mali_uk_gp_start_job_s __user *)arg);
+            break;
+
+        case MALI_IOC_GP2_ABORT_JOB:
+            err = gp_abort_job_wrapper(session_data, (_mali_uk_gp_abort_job_s __user *)arg);
+            break;
+
+        case MALI_IOC_GP2_NUMBER_OF_CORES_GET:
+            err = gp_get_number_of_cores_wrapper(session_data, (_mali_uk_get_gp_number_of_cores_s __user *)arg);
+            break;
+
+        case MALI_IOC_GP2_CORE_VERSION_GET:
+            err = gp_get_core_version_wrapper(session_data, (_mali_uk_get_gp_core_version_s __user *)arg);
+            break;
+
+        case MALI_IOC_GP2_SUSPEND_RESPONSE:
+            err = gp_suspend_response_wrapper(session_data, (_mali_uk_gp_suspend_response_s __user *)arg);
+            break;
+
+		case MALI_IOC_VSYNC_EVENT_REPORT:
+		    err = vsync_event_report_wrapper(session_data, (_mali_uk_vsync_event_report_s __user *)arg);
+		    break;
+
+        default:
+        	MALI_DEBUG_PRINT(2, ("No handler for ioctl 0x%08X 0x%08lX\n", cmd, arg));
+            err = -ENOTTY;
+    };
+
+    return err;
+}
+
+#if MALI_STATE_TRACKING
+static int mali_proc_read(char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	int len = 0;
+
+	MALI_DEBUG_PRINT(1, ("mali_proc_read(page=%p, start=%p, off=%u, count=%d, eof=%p, data=%p\n", page, start, off, count, eof, data));
+
+	if (off > 0)
+	{
+		return 0;
+	}
+
+	if (count < 1024)
+	{
+		return 0;
+	}
+
+	len  = sprintf(page + len, "Mali device driver %s\n", SVN_REV_STRING);
+	len += sprintf(page + len, "License: %s\n", MALI_KERNEL_LINUX_LICENSE);
+
+	/*
+	 * A more elegant solution would be to gather information from all subsystems and
+	 * then report it all in the /proc/mali file, but this would require a bit more work.
+	 * Use MALI_PRINT for now so we get the information in the dmesg log at least.
+	 */
+	_mali_kernel_core_dump_state();
+
+	return len;	
+}
+#endif
+
+
+module_init(mali_driver_init);
+module_exit(mali_driver_exit);
+
+MODULE_LICENSE(MALI_KERNEL_LINUX_LICENSE);
+MODULE_AUTHOR("ARM Ltd.");
+MODULE_VERSION(SVN_REV_STRING);
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_linux.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_linux.h
new file mode 100644
index 00000000000..969449d8ef4
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_linux.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_LINUX_H__
+#define __MALI_KERNEL_LINUX_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+_mali_osk_errcode_t initialize_kernel_device(void);
+void terminate_kernel_device(void);
+
+void mali_osk_low_level_mem_init(void);
+void mali_osk_low_level_mem_term(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_KERNEL_LINUX_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_pm.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_pm.c
new file mode 100644
index 00000000000..aaafc151077
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_pm.c
@@ -0,0 +1,786 @@
+/**
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_kernel_pm.c
+ * Implementation of the Linux Power Management for Mali GPU kernel driver
+ */
+#undef CONFIG_HAS_EARLYSUSPEND /*ARM will remove .early_suspend support for r2p2_rel*/
+#if USING_MALI_PMM
+#include <linux/sched.h>
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+
+#ifdef CONFIG_PM_RUNTIME
+#include <linux/pm_runtime.h>
+#endif /* CONFIG_PM_RUNTIME */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/version.h>
+#include <asm/current.h>
+#include <asm/delay.h>
+#include <linux/suspend.h>
+
+#include "mali_platform.h" 
+#include "mali_osk.h"
+#include "mali_uk_types.h"
+#include "mali_pmm.h"
+#include "mali_ukk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_license.h"
+#include "mali_kernel_pm.h"
+#include "mali_device_pause_resume.h"
+#include "mali_linux_pm.h"
+
+#if MALI_GPU_UTILIZATION
+#include "mali_kernel_utilization.h"
+#endif /* MALI_GPU_UTILIZATION */
+
+#if MALI_POWER_MGMT_TEST_SUITE
+#ifdef CONFIG_PM
+#include "mali_linux_pm_testsuite.h"
+unsigned int pwr_mgmt_status_reg = 0;
+#endif /* CONFIG_PM */
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+
+#if MALI_STATE_TRACKING
+	int is_os_pmm_thread_waiting = -1;
+#endif /* MALI_STATE_TRACKING */
+
+/* kernel should be configured with power management support */
+#ifdef CONFIG_PM
+
+/* License should be GPL */
+#if MALI_LICENSE_IS_GPL
+
+/* Linux kernel major version */
+#define LINUX_KERNEL_MAJOR_VERSION 2
+
+/* Linux kernel minor version */
+#define LINUX_KERNEL_MINOR_VERSION 6
+
+/* Linux kernel development version */
+#define LINUX_KERNEL_DEVELOPMENT_VERSION 29
+
+#ifdef CONFIG_PM_DEBUG
+static const char* const mali_states[_MALI_MAX_DEBUG_OPERATIONS] = {
+	[_MALI_DEVICE_SUSPEND] = "suspend",
+	[_MALI_DEVICE_RESUME] = "resume",
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	[_MALI_DEVICE_EARLYSUSPEND_DISABLE_FB] = "early_suspend_level_disable_framebuffer",
+	[_MALI_DEVICE_LATERESUME] = "late_resume",
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+	[_MALI_DVFS_PAUSE_EVENT] = "dvfs_pause",
+	[_MALI_DVFS_RESUME_EVENT] = "dvfs_resume",
+};
+
+#endif /* CONFIG_PM_DEBUG */
+
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+extern void set_mali_parent_power_domain(struct platform_device* dev);
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+#ifndef CONFIG_HAS_EARLYSUSPEND
+static int mali_pwr_suspend_notifier(struct notifier_block *nb,unsigned long event,void* dummy);
+
+static struct notifier_block mali_pwr_notif_block = {
+	.notifier_call = mali_pwr_suspend_notifier
+};
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_PM_RUNTIME */
+
+/* Power management thread pointer */
+struct task_struct *pm_thread;
+
+/* dvfs power management thread */
+struct task_struct *dvfs_pm_thread;
+
+/* is wake up needed */
+short is_wake_up_needed = 0;
+int timeout_fired = 2;
+unsigned int is_mali_pmm_testsuite_enabled = 0;
+
+_mali_device_power_states mali_device_state = _MALI_DEVICE_RESUME;
+_mali_device_power_states mali_dvfs_device_state = _MALI_DEVICE_RESUME;
+_mali_osk_lock_t *lock;
+
+#if MALI_POWER_MGMT_TEST_SUITE
+
+const char* const mali_pmm_recording_events[_MALI_DEVICE_MAX_PMM_EVENTS] = {
+	[_MALI_DEVICE_PMM_TIMEOUT_EVENT] = "timeout",
+	[_MALI_DEVICE_PMM_JOB_SCHEDULING_EVENTS] = "job_scheduling",
+	[_MALI_DEVICE_PMM_REGISTERED_CORES] = "cores",
+
+};
+
+unsigned int mali_timeout_event_recording_on = 0;
+unsigned int mali_job_scheduling_events_recording_on = 0;
+unsigned int is_mali_pmu_present = 0;
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+
+/* Function prototypes */
+static int mali_pm_probe(struct platform_device *pdev);
+static int mali_pm_remove(struct platform_device *pdev);
+
+/* Mali device suspend function */
+static int mali_pm_suspend(struct device *dev);
+
+/* Mali device resume function */
+static int mali_pm_resume(struct device *dev);
+
+/* Run time suspend and resume functions */
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+static int mali_device_runtime_suspend(struct device *dev);
+static int mali_device_runtime_resume(struct device *dev);
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_PM_RUNTIME */
+
+/* Early suspend functions */
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void mali_pm_early_suspend(struct early_suspend *mali_dev);
+static void mali_pm_late_resume(struct early_suspend *mali_dev);
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+
+/* OS suspend and resume callbacks */
+#if !MALI_PMM_RUNTIME_JOB_CONTROL_ON
+#ifndef CONFIG_PM_RUNTIME
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(LINUX_KERNEL_MAJOR_VERSION,LINUX_KERNEL_MINOR_VERSION,LINUX_KERNEL_DEVELOPMENT_VERSION))
+static int mali_pm_os_suspend(struct platform_device *pdev, pm_message_t state);
+#else
+static int mali_pm_os_suspend(struct device *dev);
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(LINUX_KERNEL_MAJOR_VERSION,LINUX_KERNEL_MINOR_VERSION,LINUX_KERNEL_DEVELOPMENT_VERSION))
+static int mali_pm_os_resume(struct platform_device *pdev);
+#else
+static int mali_pm_os_resume(struct device *dev);
+#endif
+#endif /* CONFIG_PM_RUNTIME */
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+
+/* OS Hibernation suspend callback */
+static int mali_pm_os_suspend_on_hibernation(struct device *dev);
+
+/* OS Hibernation resume callback */
+static int mali_pm_os_resume_on_hibernation(struct device *dev);
+
+static void _mali_release_pm(struct device* device);
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(LINUX_KERNEL_MAJOR_VERSION,LINUX_KERNEL_MINOR_VERSION,LINUX_KERNEL_DEVELOPMENT_VERSION))
+static const struct dev_pm_ops mali_dev_pm_ops = {
+
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+	.runtime_suspend = mali_device_runtime_suspend,
+	.runtime_resume = mali_device_runtime_resume,
+#endif  /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif  /* CONFIG_PM_RUNTIME */
+
+#ifndef CONFIG_PM_RUNTIME
+#if !MALI_PMM_RUNTIME_JOB_CONTROL_ON
+	.suspend = mali_pm_os_suspend,
+	.resume = mali_pm_os_resume,
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_PM_RUNTIME */
+	.freeze = mali_pm_os_suspend_on_hibernation,
+	.poweroff = mali_pm_os_suspend_on_hibernation,
+	.thaw = mali_pm_os_resume_on_hibernation,
+	.restore = mali_pm_os_resume_on_hibernation,
+};
+#endif
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(LINUX_KERNEL_MAJOR_VERSION,LINUX_KERNEL_MINOR_VERSION,LINUX_KERNEL_DEVELOPMENT_VERSION))
+struct pm_ext_ops mali_pm_operations = {
+	.base = {
+		.freeze = mali_pm_os_suspend_on_hibernation,
+		.thaw =   mali_pm_os_resume_on_hibernation,
+		.poweroff = mali_pm_os_resume_on_hibernation,
+		.restore = mali_pm_os_resume_on_hibernation,
+	},
+};
+#endif
+
+static struct platform_driver mali_plat_driver = {
+	.probe  = mali_pm_probe,
+	.remove = mali_pm_remove,
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(LINUX_KERNEL_MAJOR_VERSION,LINUX_KERNEL_MINOR_VERSION,LINUX_KERNEL_DEVELOPMENT_VERSION))
+#ifndef CONFIG_PM_RUNTIME
+#if !MALI_PMM_RUNTIME_JOB_CONTROL_ON
+	.suspend = mali_pm_os_suspend,
+	.resume  = mali_pm_os_resume,
+#endif /* CONFIG_PM_RUNTIME */
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+	.pm = &mali_pm_operations,
+#endif
+
+	.driver         = {
+		.name   = "mali_dev",
+		.owner  = THIS_MODULE,
+		.bus = &platform_bus_type,
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(LINUX_KERNEL_MAJOR_VERSION,LINUX_KERNEL_MINOR_VERSION,LINUX_KERNEL_DEVELOPMENT_VERSION))
+		.pm = &mali_dev_pm_ops,
+#endif
+		},
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+/* Early suspend hooks */
+static struct early_suspend mali_dev_early_suspend = {
+	.suspend = mali_pm_early_suspend,
+	.resume = mali_pm_late_resume,
+	.level = EARLY_SUSPEND_LEVEL_DISABLE_FB,
+};
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+
+/* Mali GPU platform device */
+struct platform_device mali_gpu_device = {
+	.name = "mali_dev",
+	.id = 0,
+	.dev.release = _mali_release_pm
+};
+
+/** This function is called when platform device is unregistered. This function
+ * is necessary when the platform device is unregistered.
+ */
+static void _mali_release_pm(struct device *device)
+{
+	MALI_DEBUG_PRINT(4, ("OSPMM: MALI Platform device removed\n" ));
+}
+
+#if MALI_POWER_MGMT_TEST_SUITE
+void mali_is_pmu_present(void)
+{
+	int temp = 0;
+	temp = pmu_get_power_up_down_info();
+	if (4095 == temp)
+	{
+		is_mali_pmu_present = 0;
+	}
+	else
+	{
+		is_mali_pmu_present = 1;
+	}
+}
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+#endif /* MALI_LICENSE_IS_GPL */
+
+#if MALI_LICENSE_IS_GPL
+
+static int mali_wait_for_power_management_policy_event(void)
+{
+	int err = 0;
+	for (; ;)
+	{
+		set_current_state(TASK_INTERRUPTIBLE);
+		if (signal_pending(current))
+		{
+			err = -EINTR;
+			break;
+		}
+		if (is_wake_up_needed == 1)
+		{
+			break;
+		}
+		schedule();
+	}
+	__set_current_state(TASK_RUNNING);
+	is_wake_up_needed =0;
+	return err;
+}
+
+/** This function is invoked when mali device is suspended
+ */
+int mali_device_suspend(unsigned int event_id, struct task_struct **pwr_mgmt_thread)
+{
+	int err = 0;	
+	_mali_uk_pmm_message_s event = {
+	                               NULL,
+	                               event_id,
+	                               timeout_fired};
+	*pwr_mgmt_thread = current;
+	MALI_DEBUG_PRINT(4, ("OSPMM: MALI device is being suspended\n" ));
+	_mali_ukk_pmm_event_message(&event);
+#if MALI_STATE_TRACKING
+	is_os_pmm_thread_waiting = 1;
+#endif /* MALI_STATE_TRACKING */
+	err = mali_wait_for_power_management_policy_event();
+#if MALI_STATE_TRACKING
+	is_os_pmm_thread_waiting = 0;
+#endif /* MALI_STATE_TRACKING */
+	return err;
+}
+
+/** This function is called when Operating system wants to power down
+ * the mali GPU device.
+ */
+static int mali_pm_suspend(struct device *dev)
+{
+	int err = 0;
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+#if MALI_GPU_UTILIZATION
+	mali_utilization_suspend();
+#endif /* MALI_GPU_UTILIZATION */
+	if ((mali_device_state == _MALI_DEVICE_SUSPEND) 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	    || mali_device_state ==  (_MALI_DEVICE_EARLYSUSPEND_DISABLE_FB))
+#else
+	)
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+	{
+		_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		return err;
+	}
+	mali_device_state = _MALI_DEVICE_SUSPEND_IN_PROGRESS;
+	err = mali_device_suspend(MALI_PMM_EVENT_OS_POWER_DOWN, &pm_thread);
+	mali_device_state = _MALI_DEVICE_SUSPEND;
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	return err;
+}
+
+#ifndef CONFIG_PM_RUNTIME
+#if !MALI_PMM_RUNTIME_JOB_CONTROL_ON
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(LINUX_KERNEL_MAJOR_VERSION,LINUX_KERNEL_MINOR_VERSION,LINUX_KERNEL_DEVELOPMENT_VERSION))
+static int mali_pm_os_suspend(struct platform_device *pdev, pm_message_t state)
+#else
+static int mali_pm_os_suspend(struct device *dev)
+#endif
+{
+	int err = 0;
+	err = mali_pm_suspend(NULL);
+	return err;
+}
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_PM_RUNTIME */
+
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+#ifndef CONFIG_HAS_EARLYSUSPEND
+static int mali_pwr_suspend_notifier(struct notifier_block *nb,unsigned long event,void* dummy)
+{
+	int err = 0;
+	switch (event)
+	{
+		case PM_SUSPEND_PREPARE:
+			err = mali_pm_suspend(NULL);
+		break;
+
+		case PM_POST_SUSPEND:
+			err = mali_pm_resume(NULL);
+		break;
+		default:
+		break;
+	}
+	return 0;
+}
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_PM_RUNTIME */
+
+/** This function is called when mali GPU device is to be resumed.
+ */
+int mali_device_resume(unsigned int event_id, struct task_struct **pwr_mgmt_thread)
+{
+	int err = 0;
+	_mali_uk_pmm_message_s event = {
+	                               NULL,
+	                               event_id,
+	                               timeout_fired};
+	*pwr_mgmt_thread = current;
+	MALI_DEBUG_PRINT(4, ("OSPMM: MALI device is being resumed\n" ));
+	_mali_ukk_pmm_event_message(&event);
+	MALI_DEBUG_PRINT(4, ("OSPMM: MALI Power up  event is scheduled\n" ));
+
+#if MALI_STATE_TRACKING
+	is_os_pmm_thread_waiting = 1;
+#endif /* MALI_STATE_TRACKING */
+
+	err = mali_wait_for_power_management_policy_event();
+
+#if MALI_STATE_TRACKING
+	is_os_pmm_thread_waiting = 0;
+#endif /* MALI_STATE_TRACKING */
+
+	return err;
+}
+
+/** This function is called when mali GPU device is to be resumed
+ */
+
+static int mali_pm_resume(struct device *dev)
+{
+	int err = 0;
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+	if (mali_device_state == _MALI_DEVICE_RESUME)
+	{
+		_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		return err;
+	}
+	err = mali_device_resume(MALI_PMM_EVENT_OS_POWER_UP, &pm_thread);
+	mali_device_state = _MALI_DEVICE_RESUME;
+	mali_dvfs_device_state = _MALI_DEVICE_RESUME;
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	return err;
+}
+
+#ifndef CONFIG_PM_RUNTIME
+#if !MALI_PMM_RUNTIME_JOB_CONTROL_ON
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(LINUX_KERNEL_MAJOR_VERSION,LINUX_KERNEL_MINOR_VERSION,LINUX_KERNEL_DEVELOPMENT_VERSION))
+static int mali_pm_os_resume(struct platform_device *pdev)
+#else
+static int mali_pm_os_resume(struct device *dev)
+#endif
+{
+	int err = 0;
+	err = mali_pm_resume(NULL);
+	return err;
+}
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_PM_RUNTIME */
+
+static int mali_pm_os_suspend_on_hibernation(struct device *dev)
+{
+	int err = 0;
+	err = mali_pm_suspend(NULL);
+	return err;
+}
+
+static int mali_pm_os_resume_on_hibernation(struct device *dev)
+{
+	int err = 0;
+	err = mali_pm_resume(NULL);
+	return err;
+}
+
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+/** This function is called when runtime suspend of mali device is required.
+ */
+static int mali_device_runtime_suspend(struct device *dev)
+{
+	MALI_DEBUG_PRINT(4, ("PMMDEBUG: Mali device Run time suspended \n" ));
+	return 0;
+}
+
+/** This function is called when runtime resume of mali device is required.
+ */
+static int mali_device_runtime_resume(struct device *dev)
+{
+	MALI_DEBUG_PRINT(4, ("PMMDEBUG: Mali device Run time Resumed \n" ));
+	return 0;
+}
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /*  CONFIG_PM_RUNTIME */
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+
+/* This function is called from android framework.
+ */
+static void mali_pm_early_suspend(struct early_suspend *mali_dev)
+{
+	switch(mali_dev->level)
+	{
+		/* Screen should be turned off but framebuffer will be accessible */
+		case EARLY_SUSPEND_LEVEL_BLANK_SCREEN:
+			MALI_DEBUG_PRINT(4, ("PMMDEBUG: Screen is off\n" ));
+		break;
+
+		case EARLY_SUSPEND_LEVEL_STOP_DRAWING:
+			MALI_DEBUG_PRINT(4, ("PMMDEBUG: Suspend level stop drawing\n" ));
+		break;
+
+		/* Turn off the framebuffer. In our case No Mali GPU operation */
+		case EARLY_SUSPEND_LEVEL_DISABLE_FB:
+			MALI_DEBUG_PRINT(4, ("PMMDEBUG: Suspend level Disable framebuffer\n" ));
+			_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+#if MALI_GPU_UTILIZATION
+			mali_utilization_suspend();
+#endif /* MALI_GPU_UTILIZATION */
+			if ((mali_device_state == _MALI_DEVICE_SUSPEND) || (mali_device_state == _MALI_DEVICE_EARLYSUSPEND_DISABLE_FB))
+			{
+				_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+				return;
+			}
+			mali_device_suspend(MALI_PMM_EVENT_OS_POWER_DOWN, &pm_thread);
+			mali_device_state =  _MALI_DEVICE_EARLYSUSPEND_DISABLE_FB;
+			_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		break;
+
+		default:
+			MALI_DEBUG_PRINT(4, ("PMMDEBUG: Invalid Suspend Mode\n" ));
+		break;
+	}
+}
+
+/* This function is invoked from android framework when mali device needs to be
+ * resumed.
+ */
+static void mali_pm_late_resume(struct early_suspend *mali_dev)
+{
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+	if (mali_device_state == _MALI_DEVICE_RESUME)
+	{
+		_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+		return;
+	}
+	if (mali_device_state ==  _MALI_DEVICE_EARLYSUSPEND_DISABLE_FB)
+	{
+		mali_device_resume(MALI_PMM_EVENT_OS_POWER_UP, &pm_thread);
+		mali_dvfs_device_state = _MALI_DEVICE_RESUME;
+		mali_device_state =  _MALI_DEVICE_RESUME;
+	}
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	
+}
+
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+
+#ifdef CONFIG_PM_DEBUG
+
+/** This function is used for debugging purposes when the user want to see
+ * which power management operations are supported for
+ * mali device.
+ */
+static ssize_t show_file(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	char *str = buf;
+#if !MALI_POWER_MGMT_TEST_SUITE
+	int pm_counter = 0;
+	for (pm_counter = 0; pm_counter<_MALI_MAX_DEBUG_OPERATIONS; pm_counter++) 
+	{
+		str += sprintf(str, "%s  ", mali_states[pm_counter]);
+	}
+#else
+	str += sprintf(str, "%d  ",pwr_mgmt_status_reg);
+#endif
+	if (str != buf)
+	{
+		*(str-1) = '\n';
+	}
+	return (str-buf);
+}
+
+/** This function is called when user wants to suspend the mali GPU device in order
+ * to simulate the power up and power down events.
+ */
+static ssize_t store_file(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	int err = 0;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend mali_dev;
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+
+#if MALI_POWER_MGMT_TEST_SUITE
+	int test_flag_dvfs = 0;
+        pwr_mgmt_status_reg = 0;
+        mali_is_pmu_present();
+
+#endif
+	if (!strncmp(buf,mali_states[_MALI_DEVICE_SUSPEND],strlen(mali_states[_MALI_DEVICE_SUSPEND])))
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: MALI suspend Power operation is scheduled\n" ));
+		err = mali_pm_suspend(NULL);
+	}
+
+#if MALI_POWER_MGMT_TEST_SUITE
+	else if (!strncmp(buf,mali_pmm_recording_events[_MALI_DEVICE_PMM_REGISTERED_CORES],strlen(mali_pmm_recording_events[_MALI_DEVICE_PMM_REGISTERED_CORES])))
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: MALI Device get number of registerd cores\n" ));
+		pwr_mgmt_status_reg = _mali_pmm_cores_list();
+		return count;
+	}
+	else if (!strncmp(buf,mali_pmm_recording_events[_MALI_DEVICE_PMM_TIMEOUT_EVENT],strlen(mali_pmm_recording_events[_MALI_DEVICE_PMM_TIMEOUT_EVENT])))
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: MALI timeout event recording is enabled\n" ));
+		mali_timeout_event_recording_on = 1;
+	}
+	else if (!strncmp(buf,mali_pmm_recording_events[_MALI_DEVICE_PMM_JOB_SCHEDULING_EVENTS],strlen(mali_pmm_recording_events[_MALI_DEVICE_PMM_JOB_SCHEDULING_EVENTS])))
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: MALI Job scheduling  events recording is enabled\n" ));
+		mali_job_scheduling_events_recording_on = 1;
+	}
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+
+	else if (!strncmp(buf,mali_states[_MALI_DEVICE_RESUME],strlen(mali_states[_MALI_DEVICE_RESUME]))) 
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: MALI Resume Power operation is scheduled\n" ));
+		err = mali_pm_resume(NULL);
+	}
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	else if (!strncmp(buf,mali_states[_MALI_DEVICE_EARLYSUSPEND_DISABLE_FB],strlen(mali_states[_MALI_DEVICE_EARLYSUSPEND_DISABLE_FB]))) 
+	{
+		mali_dev.level = EARLY_SUSPEND_LEVEL_DISABLE_FB;
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: Android early suspend operation is scheduled\n" ));
+		mali_pm_early_suspend(&mali_dev);
+	}
+	else if (!strncmp(buf,mali_states[_MALI_DEVICE_LATERESUME],strlen(mali_states[_MALI_DEVICE_LATERESUME]))) 
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: MALI Resume Power operation is scheduled\n" ));
+		mali_pm_late_resume(NULL);
+	}
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+	else if (!strncmp(buf,mali_states[_MALI_DVFS_PAUSE_EVENT],strlen(mali_states[_MALI_DVFS_PAUSE_EVENT])))
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: MALI DVFS Pause Power operation is scheduled\n" ));
+		err = mali_dev_pause();
+#if MALI_POWER_MGMT_TEST_SUITE
+		test_flag_dvfs = 1;
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+	}
+	else if (!strncmp(buf,mali_states[_MALI_DVFS_RESUME_EVENT],strlen(mali_states[_MALI_DVFS_RESUME_EVENT])))
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: MALI DVFS Resume Power operation is scheduled\n" ));
+		err = mali_dev_resume();
+#if MALI_POWER_MGMT_TEST_SUITE
+		test_flag_dvfs = 1;
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+	}
+	else 
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: Invalid Power Mode Operation selected\n" ));
+	}
+#if MALI_POWER_MGMT_TEST_SUITE
+	if (test_flag_dvfs == 1)
+	{
+		if (err)
+		{
+			pwr_mgmt_status_reg = 2;
+		}
+		else
+		{
+			pwr_mgmt_status_reg = 1;
+		}
+	}
+	else
+	{
+		if (1 == is_mali_pmu_present)
+		{
+			pwr_mgmt_status_reg = pmu_get_power_up_down_info();
+		}
+	}
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+	return count;
+}
+
+/* Device attribute file */
+static DEVICE_ATTR(file, 0644, show_file, store_file);
+#endif /* CONFIG_PM_DEBUG */
+
+static int mali_pm_remove(struct platform_device *pdev)
+{
+#ifdef CONFIG_PM_DEBUG
+	device_remove_file(&mali_gpu_device.dev, &dev_attr_file);
+#endif /* CONFIG_PM_DEBUG */
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+	pm_runtime_disable(&pdev->dev);
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_PM_RUNTIME */
+	return 0;
+}
+
+/** This function is called when the device is probed */
+static int mali_pm_probe(struct platform_device *pdev)
+{
+#ifdef CONFIG_PM_DEBUG
+	int err;
+	err = device_create_file(&mali_gpu_device.dev, &dev_attr_file);
+	if (err)
+	{
+		MALI_DEBUG_PRINT(4, ("PMMDEBUG: Error in creating device file\n" ));
+	}
+#endif /* CONFIG_PM_DEBUG */
+	return 0;
+}
+
+/** This function is called when Mali GPU device is initialized
+ */
+int _mali_dev_platform_register(void)
+{
+	int err;
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON	
+	set_mali_parent_power_domain(&mali_gpu_device);
+#endif
+
+#ifdef CONFIG_PM_RUNTIME
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+	err = register_pm_notifier(&mali_pwr_notif_block);
+	if (err)
+	{
+		return err;
+	}
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+#endif /* CONFIG_PM_RUNTIME */
+	err = platform_device_register(&mali_gpu_device);
+	lock = _mali_osk_lock_init((_mali_osk_lock_flags_t)( _MALI_OSK_LOCKFLAG_READERWRITER | _MALI_OSK_LOCKFLAG_ORDERED), 0, 0);
+	if (!err) 
+	{
+		err = platform_driver_register(&mali_plat_driver);
+		if (!err)
+		{
+#ifdef CONFIG_HAS_EARLYSUSPEND
+			register_early_suspend(&mali_dev_early_suspend);
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+		}
+		else
+		{
+			_mali_osk_lock_term(lock);
+#ifdef CONFIG_PM_RUNTIME
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+			unregister_pm_notifier(&mali_pwr_notif_block);
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+#endif /* CONFIG_PM_RUNTIME */
+			platform_device_unregister(&mali_gpu_device);
+		}
+	}
+	return err;
+}
+
+/** This function is called when Mali GPU device is unloaded
+ */
+void _mali_dev_platform_unregister(void)
+{
+	_mali_osk_lock_term(lock);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	unregister_early_suspend(&mali_dev_early_suspend);
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+
+#ifdef CONFIG_PM_RUNTIME
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+	unregister_pm_notifier(&mali_pwr_notif_block);
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+#endif /* CONFIG_PM_RUNTIME */
+
+	platform_driver_unregister(&mali_plat_driver);
+	platform_device_unregister(&mali_gpu_device);
+}
+
+#endif /* MALI_LICENSE_IS_GPL */
+#endif /* CONFIG_PM */
+
+#if MALI_STATE_TRACKING
+void mali_pmm_dump_os_thread_state( void )
+{
+	MALI_PRINTF(("\nOSPMM::The OS PMM thread state is...%d",is_os_pmm_thread_waiting));
+}
+#endif /* MALI_STATE_TRACKING */
+#endif /* USING_MALI_PMM */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_pm.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_pm.h
new file mode 100644
index 00000000000..6e879fea447
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_kernel_pm.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_KERNEL_PM_H__
+#define __MALI_KERNEL_PM_H__
+
+#ifdef USING_MALI_PMM
+int _mali_dev_platform_register(void);
+void _mali_dev_platform_unregister(void);
+#endif /* USING_MALI_PMM */
+
+#endif /* __MALI_KERNEL_PM_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_linux_dvfs_pause_resume.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_linux_dvfs_pause_resume.c
new file mode 100644
index 00000000000..3d60d18e005
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_linux_dvfs_pause_resume.c
@@ -0,0 +1,72 @@
+/**
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_linux_dvfs_pause_resume.c
+ * Implementation of the Mali pause/resume functionality
+ */
+#if USING_MALI_PMM
+#include <linux/version.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+#include "mali_platform.h"
+#include "mali_linux_pm.h"
+#include "mali_linux_dvfs_pause_resume.h"
+#include "mali_pmm.h"
+#include "mali_kernel_license.h"
+#ifdef CONFIG_PM
+#if MALI_LICENSE_IS_GPL
+
+/* Mali Pause Resume APIs */
+int mali_dev_dvfs_pause()
+{
+	int err = 0;
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+	if ((mali_dvfs_device_state ==  _MALI_DEVICE_SUSPEND) || (mali_device_state == _MALI_DEVICE_SUSPEND_IN_PROGRESS)
+	      || (mali_device_state == _MALI_DEVICE_SUSPEND))
+	{
+		err = -EPERM;
+	}
+	if ((mali_dvfs_device_state ==  _MALI_DEVICE_RESUME) && (!err))
+	{
+		mali_device_suspend(MALI_PMM_EVENT_DVFS_PAUSE, &dvfs_pm_thread);
+		mali_dvfs_device_state = _MALI_DEVICE_SUSPEND;
+	}
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	return err;
+}
+
+EXPORT_SYMBOL(mali_dev_dvfs_pause);
+
+int mali_dev_dvfs_resume()
+{
+	int err = 0;
+	_mali_osk_lock_wait(lock, _MALI_OSK_LOCKMODE_RW);
+	if ((mali_dvfs_device_state == _MALI_DEVICE_RESUME) || (mali_device_state == _MALI_DEVICE_SUSPEND_IN_PROGRESS)
+	     || (mali_device_state == _MALI_DEVICE_SUSPEND))
+	{
+		err = -EPERM;
+	}
+	if (!err)
+	{
+		mali_device_resume(MALI_PMM_EVENT_DVFS_RESUME, &dvfs_pm_thread);
+		mali_dvfs_device_state = _MALI_DEVICE_RESUME;
+	}
+	_mali_osk_lock_signal(lock, _MALI_OSK_LOCKMODE_RW);
+	return err;
+}
+
+EXPORT_SYMBOL(mali_dev_dvfs_resume);
+
+#endif /* MALI_LICENSE_IS_GPL */
+#endif /* CONFIG_PM */
+#endif /* USING_MALI_PMM */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_linux_pm.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_linux_pm.h
new file mode 100644
index 00000000000..614d5e1deee
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_linux_pm.h
@@ -0,0 +1,57 @@
+
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_LINUX_PM_H__
+#define __MALI_LINUX_PM_H__
+
+#if USING_MALI_PMM
+
+#ifdef CONFIG_PM
+/* Number of power states supported for making power up and down */
+typedef enum
+{
+	_MALI_DEVICE_SUSPEND,                         /* Suspend */
+	_MALI_DEVICE_RESUME,                          /* Resume */
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	_MALI_DEVICE_EARLYSUSPEND_DISABLE_FB,         /* Early suspend */
+	_MALI_DEVICE_LATERESUME,                      /* Late resume */
+#endif /* CONFIG_HAS_EARLYSUSPEND */
+	_MALI_DEVICE_SUSPEND_IN_PROGRESS,             /* Suspend in progress */
+	_MALI_DEVICE_MAX_POWER_STATES,                /* Maximum power states */
+} _mali_device_power_states;
+
+/* Number of DVFS events */
+typedef enum
+{
+	_MALI_DVFS_PAUSE_EVENT = _MALI_DEVICE_MAX_POWER_STATES-1,     /* DVFS Pause event */
+	_MALI_DVFS_RESUME_EVENT,                                        /* DVFS Resume event */
+	_MALI_MAX_DEBUG_OPERATIONS,
+} _mali_device_dvfs_events;
+
+extern _mali_device_power_states mali_device_state;
+extern _mali_device_power_states mali_dvfs_device_state;
+extern  _mali_osk_lock_t *lock;
+extern short is_wake_up_needed;
+extern int timeout_fired;
+extern struct platform_device mali_gpu_device;
+
+/* dvfs pm thread */
+extern struct task_struct *dvfs_pm_thread;
+
+/* Power management thread */
+extern struct task_struct *pm_thread;
+
+int mali_device_suspend(u32 event_id, struct task_struct **pwr_mgmt_thread);
+int mali_device_resume(u32 event_id, struct task_struct **pwr_mgmt_thread);
+
+#endif /* CONFIG_PM */
+#endif /* USING_MALI_PMM */
+#endif /* __MALI_LINUX_PM_H___ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_linux_pm_testsuite.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_linux_pm_testsuite.h
new file mode 100644
index 00000000000..c80b0b0a5e3
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_linux_pm_testsuite.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#ifndef __MALI_LINUX_PM_TESTSUITE_H__
+#define __MALI_LINUX_PM_TESTSUITE_H__
+
+#if USING_MALI_PMM
+#if MALI_POWER_MGMT_TEST_SUITE
+#ifdef CONFIG_PM
+
+typedef enum
+{
+        _MALI_DEVICE_PMM_TIMEOUT_EVENT,
+        _MALI_DEVICE_PMM_JOB_SCHEDULING_EVENTS,
+	_MALI_DEVICE_PMM_REGISTERED_CORES,
+        _MALI_DEVICE_MAX_PMM_EVENTS
+
+} _mali_device_pmm_recording_events;
+
+extern unsigned int mali_timeout_event_recording_on;
+extern unsigned int mali_job_scheduling_events_recording_on;
+extern unsigned int pwr_mgmt_status_reg;
+extern unsigned int is_mali_pmm_testsuite_enabled;
+extern unsigned int is_mali_pmu_present;
+
+#endif /* CONFIG_PM */
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+#endif /* USING_MALI_PMM */
+#endif /* __MALI_LINUX_PM_TESTSUITE_H__ */
+
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_atomics.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_atomics.c
new file mode 100644
index 00000000000..cc029c4a8f8
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_atomics.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_atomics.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+#include "mali_osk.h"
+#include <asm/atomic.h>
+#include "mali_kernel_common.h"
+
+void _mali_osk_atomic_dec( _mali_osk_atomic_t *atom )
+{
+    atomic_dec((atomic_t *)&atom->u.val);
+}
+
+u32 _mali_osk_atomic_dec_return( _mali_osk_atomic_t *atom )
+{
+    return atomic_dec_return((atomic_t *)&atom->u.val);
+}
+
+void _mali_osk_atomic_inc( _mali_osk_atomic_t *atom )
+{
+    atomic_inc((atomic_t *)&atom->u.val);
+}
+
+u32 _mali_osk_atomic_inc_return( _mali_osk_atomic_t *atom )
+{
+    return atomic_inc_return((atomic_t *)&atom->u.val);
+}
+
+_mali_osk_errcode_t _mali_osk_atomic_init( _mali_osk_atomic_t *atom, u32 val )
+{
+    MALI_CHECK_NON_NULL(atom, _MALI_OSK_ERR_INVALID_ARGS);
+    atomic_set((atomic_t *)&atom->u.val, val);
+    return _MALI_OSK_ERR_OK;
+}
+
+u32 _mali_osk_atomic_read( _mali_osk_atomic_t *atom )
+{
+    return atomic_read((atomic_t *)&atom->u.val);
+}
+
+void _mali_osk_atomic_term( _mali_osk_atomic_t *atom )
+{
+    MALI_IGNORE(atom);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_indir_mmap.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_indir_mmap.c
new file mode 100644
index 00000000000..b60bf825897
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_indir_mmap.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include <linux/slab.h>
+#include <linux/pagemap.h>
+#include <linux/mm.h>
+#include <linux/mman.h>
+#include <linux/sched.h>
+#include <asm/page.h>
+#include <asm/pgtable.h>
+#include <asm/atomic.h>
+
+#include "mali_osk.h"
+#include "mali_ukk.h"
+#include "mali_kernel_common.h"
+
+/**
+ * @file mali_osk_specific.c
+ * Implementation of per-OS Kernel level specifics
+ */
+
+_mali_osk_errcode_t _mali_osk_specific_indirect_mmap( _mali_uk_mem_mmap_s *args )
+{
+	/* args->ctx ignored here; args->ukk_private required instead */
+	/* we need to lock the mmap semaphore before calling the do_mmap function */
+    down_write(&current->mm->mmap_sem);
+
+    args->mapping = (void __user *)do_mmap(
+											(struct file *)args->ukk_private,
+											0, /* start mapping from any address after NULL */
+											args->size,
+											PROT_READ | PROT_WRITE,
+											MAP_SHARED,
+											args->phys_addr
+										   );
+
+	/* and unlock it after the call */
+	up_write(&current->mm->mmap_sem);
+
+	/* No cookie required here */
+	args->cookie = 0;
+	/* uku_private meaningless, so zero */
+	args->uku_private = NULL;
+
+	if ( (NULL == args->mapping) || IS_ERR((void *)args->mapping) )
+	{
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	/* Success */
+	return _MALI_OSK_ERR_OK;
+}
+
+
+_mali_osk_errcode_t _mali_osk_specific_indirect_munmap( _mali_uk_mem_munmap_s *args )
+{
+	/* args->ctx and args->cookie ignored here */
+
+	if ((NULL != current) && (NULL != current->mm))
+	{
+		/* remove mapping of mali memory from the process' view */
+		/* lock mmap semaphore before call */
+		/* lock mmap_sem before calling do_munmap */
+		down_write(&current->mm->mmap_sem);
+		do_munmap(
+					current->mm,
+					(unsigned long)args->mapping,
+					args->size
+				);
+		/* and unlock after call */
+		up_write(&current->mm->mmap_sem);
+		MALI_DEBUG_PRINT(5, ("unmapped\n"));
+	}
+	else
+	{
+		MALI_DEBUG_PRINT(2, ("Freeing of a big block while no user process attached, assuming crash cleanup in progress\n"));
+	}
+
+	return _MALI_OSK_ERR_OK; /* always succeeds */
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_indir_mmap.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_indir_mmap.h
new file mode 100644
index 00000000000..f87739bb8f9
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_indir_mmap.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_specific.h
+ * Defines per-OS Kernel level specifics, such as unusual workarounds for
+ * certain OSs.
+ */
+
+#ifndef __MALI_OSK_INDIR_MMAP_H__
+#define __MALI_OSK_INDIR_MMAP_H__
+
+#include "mali_uk_types.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * Linux specific means for calling _mali_ukk_mem_mmap/munmap
+ *
+ * The presence of _MALI_OSK_SPECIFIC_INDIRECT_MMAP indicates that
+ * _mali_osk_specific_indirect_mmap and _mali_osk_specific_indirect_munmap
+ * should be used instead of _mali_ukk_mem_mmap/_mali_ukk_mem_munmap.
+ *
+ * The arguments are the same as _mali_ukk_mem_mmap/_mali_ukk_mem_munmap.
+ *
+ * In ALL operating system other than Linux, it is expected that common code
+ * should be able to call _mali_ukk_mem_mmap/_mali_ukk_mem_munmap directly.
+ * Such systems should NOT define _MALI_OSK_SPECIFIC_INDIRECT_MMAP.
+ */
+_mali_osk_errcode_t _mali_osk_specific_indirect_mmap( _mali_uk_mem_mmap_s *args );
+_mali_osk_errcode_t _mali_osk_specific_indirect_munmap( _mali_uk_mem_munmap_s *args );
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_OSK_INDIR_MMAP_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_irq.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_irq.c
new file mode 100644
index 00000000000..1a4bbcecdce
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_irq.c
@@ -0,0 +1,228 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_irq.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+#include <linux/slab.h>	/* For memory allocation */
+#include <linux/workqueue.h>
+
+#include "mali_osk.h"
+#include "mali_kernel_core.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_license.h"
+#include "linux/interrupt.h"
+
+typedef struct _mali_osk_irq_t_struct
+{
+	u32 irqnum;
+	void *data;
+	_mali_osk_irq_uhandler_t uhandler;
+	_mali_osk_irq_bhandler_t bhandler;
+	struct work_struct work_queue_irq_handle; /* Workqueue for the bottom half of the IRQ-handling. This job is activated when this core gets an IRQ.*/
+} mali_osk_irq_object_t;
+
+#if MALI_LICENSE_IS_GPL
+static struct workqueue_struct *pmm_wq=NULL;
+#endif
+
+typedef void (*workqueue_func_t)(void *);
+typedef irqreturn_t (*irq_handler_func_t)(int, void *, struct pt_regs *);
+static irqreturn_t irq_handler_upper_half (int port_name, void* dev_id ); /* , struct pt_regs *regs*/
+
+#if defined(INIT_DELAYED_WORK)
+static void irq_handler_bottom_half ( struct work_struct *work );
+#else
+static void irq_handler_bottom_half ( void *  input );
+#endif
+
+/**
+ * Linux kernel version has marked SA_SHIRQ as deprecated, IRQF_SHARED should be used.
+ * This is to handle older kernels which haven't done this swap.
+ */
+#ifndef IRQF_SHARED
+#define IRQF_SHARED SA_SHIRQ
+#endif /* IRQF_SHARED */
+
+_mali_osk_irq_t *_mali_osk_irq_init( u32 irqnum, _mali_osk_irq_uhandler_t uhandler,	_mali_osk_irq_bhandler_t bhandler, _mali_osk_irq_trigger_t trigger_func, _mali_osk_irq_ack_t ack_func, void *data, const char *description )
+{
+	mali_osk_irq_object_t *irq_object;
+
+	irq_object = kmalloc(sizeof(mali_osk_irq_object_t), GFP_KERNEL);
+	if (NULL == irq_object) return NULL;
+
+	/* workqueue API changed in 2.6.20, support both versions: */
+#if defined(INIT_DELAYED_WORK)
+	/* New syntax: INIT_WORK( struct work_struct *work, void (*function)(struct work_struct *)) */
+	INIT_WORK( &irq_object->work_queue_irq_handle, irq_handler_bottom_half);
+#else
+	/* Old syntax: INIT_WORK( struct work_struct *work, void (*function)(void *), void *data) */
+	INIT_WORK( &irq_object->work_queue_irq_handle, irq_handler_bottom_half, irq_object);
+#endif /* defined(INIT_DELAYED_WORK) */
+
+	if (-1 == irqnum)
+	{
+		/* Probe for IRQ */
+		if ( (NULL != trigger_func) && (NULL != ack_func) )
+		{
+			unsigned long probe_count = 3;
+			_mali_osk_errcode_t err;
+			int irq;
+
+			MALI_DEBUG_PRINT(2, ("Probing for irq\n"));
+
+			do
+			{
+				unsigned long mask;
+
+				mask = probe_irq_on();
+				trigger_func(data);
+
+				_mali_osk_time_ubusydelay(5);
+
+				irq = probe_irq_off(mask);
+				err = ack_func(data);
+			}
+			while (irq < 0 && (err == _MALI_OSK_ERR_OK) && probe_count--);
+
+			if (irq < 0 || (_MALI_OSK_ERR_OK != err)) irqnum = -1;
+			else irqnum = irq;
+		}
+		else irqnum = -1; /* no probe functions, fault */
+
+		if (-1 != irqnum)
+		{
+			/* found an irq */
+			MALI_DEBUG_PRINT(2, ("Found irq %d\n", irqnum));
+		}
+		else
+		{
+			MALI_DEBUG_PRINT(2, ("Probe for irq failed\n"));
+		}
+	}
+	
+	irq_object->irqnum = irqnum;
+	irq_object->uhandler = uhandler;
+	irq_object->bhandler = bhandler;
+	irq_object->data = data;
+
+	/* Is this a real IRQ handler we need? */
+	if (!mali_benchmark && irqnum != _MALI_OSK_IRQ_NUMBER_FAKE && irqnum != _MALI_OSK_IRQ_NUMBER_PMM) 
+	{
+		if (-1 == irqnum)
+		{
+			MALI_DEBUG_PRINT(2, ("No IRQ for core '%s' found during probe\n", description));
+			kfree(irq_object);
+			return NULL;
+		}
+
+		if (0 != request_irq(irqnum, irq_handler_upper_half, IRQF_SHARED, description, irq_object))
+		{
+			MALI_DEBUG_PRINT(2, ("Unable to install IRQ handler for core '%s'\n", description));
+			kfree(irq_object);
+			return NULL;
+		}
+	}
+
+#if MALI_LICENSE_IS_GPL
+	if ( _MALI_OSK_IRQ_NUMBER_PMM == irqnum )
+	{
+		pmm_wq = create_singlethread_workqueue("mali-pmm-wq");
+	}
+#endif
+
+	return irq_object;
+}
+
+void _mali_osk_irq_schedulework( _mali_osk_irq_t *irq )
+{
+	mali_osk_irq_object_t *irq_object = (mali_osk_irq_object_t *)irq;
+#if MALI_LICENSE_IS_GPL
+	if ( irq_object->irqnum == _MALI_OSK_IRQ_NUMBER_PMM )
+	{
+		queue_work(pmm_wq,&irq_object->work_queue_irq_handle);
+	}
+	else
+	{
+#endif
+		schedule_work(&irq_object->work_queue_irq_handle);
+#if MALI_LICENSE_IS_GPL
+	}
+#endif
+}
+
+void _mali_osk_irq_term( _mali_osk_irq_t *irq )
+{
+	mali_osk_irq_object_t *irq_object = (mali_osk_irq_object_t *)irq;
+
+#if MALI_LICENSE_IS_GPL
+	if(irq_object->irqnum == _MALI_OSK_IRQ_NUMBER_PMM )
+	{
+		flush_workqueue(pmm_wq);
+		destroy_workqueue(pmm_wq);
+	}
+#endif
+	if (!mali_benchmark)
+	{
+		free_irq(irq_object->irqnum, irq_object);
+	}
+	kfree(irq_object);
+	flush_scheduled_work();
+}
+
+
+/** This function is called directly in interrupt context from the OS just after
+ * the CPU get the hw-irq from mali, or other devices on the same IRQ-channel.
+ * It is registered one of these function for each mali core. When an interrupt
+ * arrives this function will be called equal times as registered mali cores.
+ * That means that we only check one mali core in one function call, and the
+ * core we check for each turn is given by the \a dev_id variable.
+ * If we detect an pending interrupt on the given core, we mask the interrupt
+ * out by settging the core's IRQ_MASK register to zero.
+ * Then we schedule the mali_core_irq_handler_bottom_half to run as high priority
+ * work queue job.
+ */
+static irqreturn_t irq_handler_upper_half (int port_name, void* dev_id ) /* , struct pt_regs *regs*/
+{
+	mali_osk_irq_object_t *irq_object = (mali_osk_irq_object_t *)dev_id;
+
+	if (irq_object->uhandler(irq_object->data) == _MALI_OSK_ERR_OK)
+	{
+		return IRQ_HANDLED;
+	}
+	return IRQ_NONE;
+}
+
+/* Is executed when an interrupt occur on one core */
+/* workqueue API changed in 2.6.20, support both versions: */
+#if defined(INIT_DELAYED_WORK)
+static void irq_handler_bottom_half ( struct work_struct *work )
+#else
+static void irq_handler_bottom_half ( void *  input )
+#endif
+{
+	mali_osk_irq_object_t *irq_object;
+
+#if defined(INIT_DELAYED_WORK)
+	irq_object = _MALI_OSK_CONTAINER_OF(work, mali_osk_irq_object_t, work_queue_irq_handle);
+#else
+	if ( NULL == input )
+	{
+		MALI_PRINT_ERROR(("IRQ: Null pointer! Illegal!"));
+		return; /* Error */
+	}
+	irq_object = (mali_osk_irq_object_t *) input;
+#endif
+
+	irq_object->bhandler(irq_object->data);
+}
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_locks.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_locks.c
new file mode 100644
index 00000000000..11285efc682
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_locks.c
@@ -0,0 +1,271 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_locks.c
+ * Implemenation of the OS abstraction layer for the kernel device driver
+ */
+
+/* needed to detect kernel version specific code */
+#include <linux/version.h>
+
+#include <linux/spinlock.h>
+#include <linux/rwsem.h>
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+#include <linux/semaphore.h>
+#else /* pre 2.6.26 the file was in the arch specific location */
+#include <asm/semaphore.h>
+#endif
+
+#include <linux/slab.h>
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+
+/* These are all the locks we implement: */
+typedef enum
+{
+	_MALI_OSK_INTERNAL_LOCKTYPE_SPIN,            /* Mutex, implicitly non-interruptable, use spin_lock/spin_unlock */
+	_MALI_OSK_INTERNAL_LOCKTYPE_SPIN_IRQ,        /* Mutex, IRQ version of spinlock, use spin_lock_irqsave/spin_unlock_irqrestore */
+	_MALI_OSK_INTERNAL_LOCKTYPE_MUTEX,           /* Interruptable, use up()/down_interruptable() */
+	_MALI_OSK_INTERNAL_LOCKTYPE_MUTEX_NONINT,    /* Non-Interruptable, use up()/down() */
+	_MALI_OSK_INTERNAL_LOCKTYPE_MUTEX_NONINT_RW, /* Non-interruptable, Reader/Writer, use {up,down}{read,write}() */
+
+	/* Linux supports, but we do not support:
+	 * Non-Interruptable Reader/Writer spinlock mutexes - RW optimization will be switched off
+	 */
+
+	/* Linux does not support:
+	 * One-locks, of any sort - no optimization for this fact will be made.
+	 */
+
+} _mali_osk_internal_locktype;
+
+struct _mali_osk_lock_t_struct
+{
+    _mali_osk_internal_locktype type;
+	unsigned long flags; 
+    union
+    {
+        spinlock_t spinlock;
+        struct semaphore sema;
+        struct rw_semaphore rw_sema;
+    } obj;
+	MALI_DEBUG_CODE(
+				  /** original flags for debug checking */
+				  _mali_osk_lock_flags_t orig_flags;
+				  _mali_osk_lock_mode_t locked_as;
+	); /* MALI_DEBUG_CODE */
+};
+
+_mali_osk_lock_t *_mali_osk_lock_init( _mali_osk_lock_flags_t flags, u32 initial, u32 order )
+{
+    _mali_osk_lock_t *lock = NULL;
+
+	/* Validate parameters: */
+	/* Flags acceptable */
+	MALI_DEBUG_ASSERT( 0 == ( flags & ~(_MALI_OSK_LOCKFLAG_SPINLOCK
+									  | _MALI_OSK_LOCKFLAG_SPINLOCK_IRQ
+									  | _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE
+									  | _MALI_OSK_LOCKFLAG_READERWRITER
+									  | _MALI_OSK_LOCKFLAG_ORDERED
+									  | _MALI_OSK_LOCKFLAG_ONELOCK )) );
+	/* Spinlocks are always non-interruptable */
+	MALI_DEBUG_ASSERT( (((flags & _MALI_OSK_LOCKFLAG_SPINLOCK) || (flags & _MALI_OSK_LOCKFLAG_SPINLOCK_IRQ)) && (flags & _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE))
+					 || !(flags & _MALI_OSK_LOCKFLAG_SPINLOCK));
+	/* Parameter initial SBZ - for future expansion */
+	MALI_DEBUG_ASSERT( 0 == initial );
+
+	lock = kmalloc(sizeof(_mali_osk_lock_t), GFP_KERNEL);
+
+	if ( NULL == lock )
+	{
+		return lock;
+	}
+
+	/* Determine type of mutex: */
+    /* defaults to interruptable mutex if no flags are specified */
+
+	if ( (flags & _MALI_OSK_LOCKFLAG_SPINLOCK) )
+	{
+		/* Non-interruptable Spinlocks override all others */
+		lock->type = _MALI_OSK_INTERNAL_LOCKTYPE_SPIN;
+		spin_lock_init( &lock->obj.spinlock );
+	}
+	else if ( (flags & _MALI_OSK_LOCKFLAG_SPINLOCK_IRQ ) )
+	{
+		lock->type = _MALI_OSK_INTERNAL_LOCKTYPE_SPIN_IRQ;
+		lock->flags = 0;
+		spin_lock_init( &lock->obj.spinlock );
+	}
+	else if ( (flags & _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE)
+			  && (flags & _MALI_OSK_LOCKFLAG_READERWRITER) )
+	{
+		lock->type = _MALI_OSK_INTERNAL_LOCKTYPE_MUTEX_NONINT_RW;
+		init_rwsem( &lock->obj.rw_sema );
+	}
+	else
+	{
+		/* Usual mutex types */
+		if ( (flags & _MALI_OSK_LOCKFLAG_NONINTERRUPTABLE) )
+		{
+			lock->type = _MALI_OSK_INTERNAL_LOCKTYPE_MUTEX_NONINT;
+		}
+		else
+		{
+			lock->type = _MALI_OSK_INTERNAL_LOCKTYPE_MUTEX;
+		}
+
+		/* Initially unlocked */
+		sema_init( &lock->obj.sema, 1 );
+	}
+
+	MALI_DEBUG_CODE(
+				  /* Debug tracking of flags */
+				  lock->orig_flags = flags;
+				  lock->locked_as = _MALI_OSK_LOCKMODE_UNDEF;
+				  ); /* MALI_DEBUG_CODE */
+
+    return lock;
+}
+
+_mali_osk_errcode_t _mali_osk_lock_wait( _mali_osk_lock_t *lock, _mali_osk_lock_mode_t mode)
+{
+    _mali_osk_errcode_t err = _MALI_OSK_ERR_OK;
+
+	/* Parameter validation */
+	MALI_DEBUG_ASSERT_POINTER( lock );
+
+	MALI_DEBUG_ASSERT( _MALI_OSK_LOCKMODE_RW == mode
+					 || _MALI_OSK_LOCKMODE_RO == mode );
+
+	/* Only allow RO locks when the initial object was a Reader/Writer lock
+	 * Since information is lost on the internal locktype, we use the original
+	 * information, which is only stored when built for DEBUG */
+	MALI_DEBUG_ASSERT( _MALI_OSK_LOCKMODE_RW == mode
+					 || (_MALI_OSK_LOCKMODE_RO == mode && (_MALI_OSK_LOCKFLAG_READERWRITER & lock->orig_flags)) );
+
+	switch ( lock->type )
+	{
+	case _MALI_OSK_INTERNAL_LOCKTYPE_SPIN:
+		spin_lock(&lock->obj.spinlock);
+		break;
+	case _MALI_OSK_INTERNAL_LOCKTYPE_SPIN_IRQ:
+		spin_lock_irqsave(&lock->obj.spinlock, lock->flags);
+		break;
+
+	case _MALI_OSK_INTERNAL_LOCKTYPE_MUTEX:
+		if ( down_interruptible(&lock->obj.sema) )
+		{
+			err = _MALI_OSK_ERR_RESTARTSYSCALL;
+		}
+		break;
+
+	case _MALI_OSK_INTERNAL_LOCKTYPE_MUTEX_NONINT:
+		down(&lock->obj.sema);
+		break;
+
+	case _MALI_OSK_INTERNAL_LOCKTYPE_MUTEX_NONINT_RW:
+		if (mode == _MALI_OSK_LOCKMODE_RO)
+        {
+            down_read(&lock->obj.rw_sema);
+        }
+        else
+        {
+            down_write(&lock->obj.rw_sema);
+        }
+		break;
+
+	default:
+		/* Reaching here indicates a programming error, so you will not get here
+		 * on non-DEBUG builds */
+		MALI_DEBUG_PRINT_ERROR( ("Invalid internal lock type: %.8X", lock->type ) );
+		break;
+	}
+
+	/* DEBUG tracking of previously locked state - occurs after lock obtained */
+	MALI_DEBUG_CODE(
+				  if ( _MALI_OSK_ERR_OK == err )
+				  {
+					  /* Assert that this is not currently locked */
+					  MALI_DEBUG_ASSERT( _MALI_OSK_LOCKMODE_UNDEF == lock->locked_as );
+
+					  lock->locked_as = mode;
+				  }
+				  ); /* MALI_DEBUG_CODE */
+
+    return err;
+}
+
+void _mali_osk_lock_signal( _mali_osk_lock_t *lock, _mali_osk_lock_mode_t mode )
+{
+	/* Parameter validation */
+	MALI_DEBUG_ASSERT_POINTER( lock );
+
+	MALI_DEBUG_ASSERT( _MALI_OSK_LOCKMODE_RW == mode
+					 || _MALI_OSK_LOCKMODE_RO == mode );
+
+	/* Only allow RO locks when the initial object was a Reader/Writer lock
+	 * Since information is lost on the internal locktype, we use the original
+	 * information, which is only stored when built for DEBUG */
+	MALI_DEBUG_ASSERT( _MALI_OSK_LOCKMODE_RW == mode
+					 || (_MALI_OSK_LOCKMODE_RO == mode && (_MALI_OSK_LOCKFLAG_READERWRITER & lock->orig_flags)) );
+
+	/* For DEBUG only, assert that we previously locked this, and in the same way (RW/RO) */
+	MALI_DEBUG_ASSERT( mode == lock->locked_as );
+
+	/* DEBUG tracking of previously locked state - occurs before lock released */
+	MALI_DEBUG_CODE( lock->locked_as = _MALI_OSK_LOCKMODE_UNDEF );
+
+	switch ( lock->type )
+	{
+	case _MALI_OSK_INTERNAL_LOCKTYPE_SPIN:
+		spin_unlock(&lock->obj.spinlock);
+		break;
+	case _MALI_OSK_INTERNAL_LOCKTYPE_SPIN_IRQ:
+		spin_unlock_irqrestore(&lock->obj.spinlock, lock->flags);
+		break;
+
+	case _MALI_OSK_INTERNAL_LOCKTYPE_MUTEX:
+		/* FALLTHROUGH */
+	case _MALI_OSK_INTERNAL_LOCKTYPE_MUTEX_NONINT:
+		up(&lock->obj.sema);
+		break;
+
+	case _MALI_OSK_INTERNAL_LOCKTYPE_MUTEX_NONINT_RW:
+		if (mode == _MALI_OSK_LOCKMODE_RO)
+        {
+            up_read(&lock->obj.rw_sema);
+        }
+        else
+        {
+            up_write(&lock->obj.rw_sema);
+        }
+		break;
+
+	default:
+		/* Reaching here indicates a programming error, so you will not get here
+		 * on non-DEBUG builds */
+		MALI_DEBUG_PRINT_ERROR( ("Invalid internal lock type: %.8X", lock->type ) );
+		break;
+	}
+}
+
+void _mali_osk_lock_term( _mali_osk_lock_t *lock )
+{
+	/* Parameter validation  */
+	MALI_DEBUG_ASSERT_POINTER( lock );
+
+	/* For DEBUG only, assert that this is not currently locked */
+	MALI_DEBUG_ASSERT( _MALI_OSK_LOCKMODE_UNDEF == lock->locked_as );
+
+	/* Linux requires no explicit termination of spinlocks, semaphores, or rw_semaphores */
+    kfree(lock);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_low_level_mem.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_low_level_mem.c
new file mode 100644
index 00000000000..6454709caaf
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_low_level_mem.c
@@ -0,0 +1,578 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_low_level_mem.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+/* needed to detect kernel version specific code */
+#include <linux/version.h>
+
+#include <asm/io.h>
+#include <linux/ioport.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/dma-mapping.h>
+#include <asm/cacheflush.h>
+
+#include "mali_osk.h"
+#include "mali_ukk.h" /* required to hook in _mali_ukk_mem_mmap handling */
+#include "mali_kernel_common.h"
+#include "mali_kernel_linux.h"
+
+static void mali_kernel_memory_vma_open(struct vm_area_struct * vma);
+static void mali_kernel_memory_vma_close(struct vm_area_struct * vma);
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+static int mali_kernel_memory_cpu_page_fault_handler(struct vm_area_struct *vma, struct vm_fault *vmf);
+#else
+static unsigned long mali_kernel_memory_cpu_page_fault_handler(struct vm_area_struct * vma, unsigned long address);
+#endif
+
+
+typedef struct mali_vma_usage_tracker
+{
+	int references;
+	u32 cookie;
+} mali_vma_usage_tracker;
+
+
+/* Linked list structure to hold details of all OS allocations in a particular
+ * mapping
+ */
+struct AllocationList
+{
+	struct AllocationList *next;
+	u32 offset;
+	u32 physaddr;
+};
+
+typedef struct AllocationList AllocationList;
+
+/* Private structure to store details of a mapping region returned
+ * from _mali_osk_mem_mapregion_init
+ */
+struct MappingInfo
+{
+	struct vm_area_struct *vma;
+	struct AllocationList *list;
+};
+
+typedef struct MappingInfo MappingInfo;
+
+
+static u32 _kernel_page_allocate(void);
+static void _kernel_page_release(u32 physical_address);
+static AllocationList * _allocation_list_item_get(void);
+static void _allocation_list_item_release(AllocationList * item);
+
+
+/* Variable declarations */
+spinlock_t allocation_list_spinlock; 
+static AllocationList * pre_allocated_memory = (AllocationList*) NULL ;
+static int pre_allocated_memory_size_current  = 0;
+#ifdef MALI_OS_MEMORY_KERNEL_BUFFER_SIZE_IN_MB
+	static int pre_allocated_memory_size_max      = MALI_OS_MEMORY_KERNEL_BUFFER_SIZE_IN_MB * 1024 * 1024;
+#else
+	static int pre_allocated_memory_size_max      = 6 * 1024 * 1024; /* 6 MiB */
+#endif
+
+static struct vm_operations_struct mali_kernel_vm_ops =
+{
+	.open = mali_kernel_memory_vma_open,
+	.close = mali_kernel_memory_vma_close,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+	.fault = mali_kernel_memory_cpu_page_fault_handler
+#else
+	.nopfn = mali_kernel_memory_cpu_page_fault_handler
+#endif
+};
+
+
+void mali_osk_low_level_mem_init(void)
+{
+	spin_lock_init( &allocation_list_spinlock );
+	pre_allocated_memory = (AllocationList*) NULL ;
+}
+
+void mali_osk_low_level_mem_term(void)
+{
+	while ( NULL != pre_allocated_memory )
+	{
+		AllocationList *item;
+		item = pre_allocated_memory;
+		pre_allocated_memory = item->next;
+		_kernel_page_release(item->physaddr);
+		_mali_osk_free( item );
+	}
+	pre_allocated_memory_size_current  = 0;
+}
+
+static u32 _kernel_page_allocate(void)
+{
+	struct page *new_page;
+	u32 linux_phys_addr;
+	
+	new_page = alloc_page(GFP_HIGHUSER | __GFP_ZERO | __GFP_REPEAT | __GFP_NOWARN | __GFP_COLD);
+	
+	if ( NULL == new_page )
+	{
+		return 0;
+	}
+
+	/* Ensure page is flushed from CPU caches. */
+	linux_phys_addr = dma_map_page(NULL, new_page, 0, PAGE_SIZE, DMA_BIDIRECTIONAL);
+
+	return linux_phys_addr;
+}
+
+static void _kernel_page_release(u32 physical_address)
+{
+	struct page *unmap_page;
+
+	#if 1
+	dma_unmap_page(NULL, physical_address, PAGE_SIZE, DMA_BIDIRECTIONAL);
+	#endif
+	
+	unmap_page = pfn_to_page( physical_address >> PAGE_SHIFT );
+	MALI_DEBUG_ASSERT_POINTER( unmap_page );
+	__free_page( unmap_page );
+}
+
+static AllocationList * _allocation_list_item_get(void)
+{
+	AllocationList *item = NULL;
+	unsigned long flags;
+	
+	spin_lock_irqsave(&allocation_list_spinlock,flags);
+	if ( pre_allocated_memory )
+	{
+		item = pre_allocated_memory;
+		pre_allocated_memory = pre_allocated_memory->next;
+		pre_allocated_memory_size_current -= PAGE_SIZE;
+		
+		spin_unlock_irqrestore(&allocation_list_spinlock,flags);
+		return item;
+	}
+	spin_unlock_irqrestore(&allocation_list_spinlock,flags);
+	
+	item = _mali_osk_malloc( sizeof(AllocationList) );
+	if ( NULL == item)
+	{
+		return NULL;
+	}
+
+	item->physaddr = _kernel_page_allocate();
+	if ( 0 == item->physaddr )
+	{
+		/* Non-fatal error condition, out of memory. Upper levels will handle this. */
+		_mali_osk_free( item );
+		return NULL;
+	}
+	return item;
+}
+
+static void _allocation_list_item_release(AllocationList * item)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&allocation_list_spinlock,flags);
+	if ( pre_allocated_memory_size_current < pre_allocated_memory_size_max)
+	{
+		item->next = pre_allocated_memory;
+		pre_allocated_memory = item;
+		pre_allocated_memory_size_current += PAGE_SIZE;
+		spin_unlock_irqrestore(&allocation_list_spinlock,flags);
+		return;
+	}
+	spin_unlock_irqrestore(&allocation_list_spinlock,flags);
+	
+	_kernel_page_release(item->physaddr);
+	_mali_osk_free( item );
+}
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+static int mali_kernel_memory_cpu_page_fault_handler(struct vm_area_struct *vma, struct vm_fault *vmf)
+#else
+static unsigned long mali_kernel_memory_cpu_page_fault_handler(struct vm_area_struct * vma, unsigned long address)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+	void __user * address;
+	address = vmf->virtual_address;
+#endif
+	/*
+	 * We always fail the call since all memory is pre-faulted when assigned to the process.
+	 * Only the Mali cores can use page faults to extend buffers.
+	*/
+
+	MALI_DEBUG_PRINT(1, ("Page-fault in Mali memory region caused by the CPU.\n"));
+	MALI_DEBUG_PRINT(1, ("Tried to access %p (process local virtual address) which is not currently mapped to any Mali memory.\n", (void*)address));
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+	return VM_FAULT_SIGBUS;
+#else
+	return NOPFN_SIGBUS;
+#endif
+}
+
+static void mali_kernel_memory_vma_open(struct vm_area_struct * vma)
+{
+	mali_vma_usage_tracker * vma_usage_tracker;
+	MALI_DEBUG_PRINT(4, ("Open called on vma %p\n", vma));
+
+	vma_usage_tracker = (mali_vma_usage_tracker*)vma->vm_private_data;
+	vma_usage_tracker->references++;
+
+	return;
+}
+
+static void mali_kernel_memory_vma_close(struct vm_area_struct * vma)
+{
+	_mali_uk_mem_munmap_s args = {0, };
+	mali_memory_allocation * descriptor;
+	mali_vma_usage_tracker * vma_usage_tracker;
+	MALI_DEBUG_PRINT(3, ("Close called on vma %p\n", vma));
+
+	vma_usage_tracker = (mali_vma_usage_tracker*)vma->vm_private_data;
+
+	BUG_ON(!vma_usage_tracker);
+	BUG_ON(0 == vma_usage_tracker->references);
+
+	vma_usage_tracker->references--;
+
+	if (0 != vma_usage_tracker->references)
+	{
+		MALI_DEBUG_PRINT(3, ("Ignoring this close, %d references still exists\n", vma_usage_tracker->references));
+		return;
+	}
+
+	/** @note args->context unused, initialized to 0.
+	 * Instead, we use the memory_session from the cookie */
+
+	descriptor = (mali_memory_allocation *)vma_usage_tracker->cookie;
+
+	args.cookie = (u32)descriptor;
+	args.mapping = descriptor->mapping;
+	args.size = descriptor->size;
+
+	_mali_ukk_mem_munmap( &args );
+
+	/* vma_usage_tracker is free()d by _mali_osk_mem_mapregion_term().
+	 * In the case of the memory engine, it is called as the release function that has been registered with the engine*/
+}
+
+
+void _mali_osk_mem_barrier( void )
+{
+	mb();
+}
+
+mali_io_address _mali_osk_mem_mapioregion( u32 phys, u32 size, const char *description )
+{
+	return (mali_io_address)ioremap_nocache(phys, size);
+}
+
+void _mali_osk_mem_unmapioregion( u32 phys, u32 size, mali_io_address virt )
+{
+	iounmap((void*)virt);
+}
+
+mali_io_address _mali_osk_mem_allocioregion( u32 *phys, u32 size )
+{
+	void * virt;
+ 	MALI_DEBUG_ASSERT_POINTER( phys );
+ 	MALI_DEBUG_ASSERT( 0 == (size & ~_MALI_OSK_CPU_PAGE_MASK) );
+ 	MALI_DEBUG_ASSERT( 0 != size );
+
+	/* dma_alloc_* uses a limited region of address space. On most arch/marchs
+	 * 2 to 14 MiB is available. This should be enough for the page tables, which
+	 * currently is the only user of this function. */
+	virt = dma_alloc_coherent(NULL, size, phys, GFP_KERNEL | GFP_DMA );
+
+	MALI_DEBUG_PRINT(3, ("Page table virt: 0x%x = dma_alloc_coherent(size:%d, phys:0x%x, )\n", virt, size, phys));
+
+ 	if ( NULL == virt )
+ 	{
+		MALI_DEBUG_PRINT(1, ("allocioregion: Failed to allocate Pagetable memory, size=0x%.8X\n", size ));
+		MALI_DEBUG_PRINT(1, ("Solution: When configuring and building linux kernel, set CONSISTENT_DMA_SIZE to be 14 MB.\n"));
+ 		return 0;
+ 	}
+
+	MALI_DEBUG_ASSERT( 0 == (*phys & ~_MALI_OSK_CPU_PAGE_MASK) );
+
+ 	return (mali_io_address)virt;
+}
+
+void _mali_osk_mem_freeioregion( u32 phys, u32 size, mali_io_address virt )
+{
+ 	MALI_DEBUG_ASSERT_POINTER( (void*)virt );
+ 	MALI_DEBUG_ASSERT( 0 != size );
+ 	MALI_DEBUG_ASSERT( 0 == (phys & ( (1 << PAGE_SHIFT) - 1 )) );
+
+	dma_free_coherent(NULL, size, virt, phys);
+}
+
+_mali_osk_errcode_t inline _mali_osk_mem_reqregion( u32 phys, u32 size, const char *description )
+{
+	return ((NULL == request_mem_region(phys, size, description)) ? _MALI_OSK_ERR_NOMEM : _MALI_OSK_ERR_OK);
+}
+
+void inline _mali_osk_mem_unreqregion( u32 phys, u32 size )
+{
+	release_mem_region(phys, size);
+}
+
+u32 inline _mali_osk_mem_ioread32( volatile mali_io_address addr, u32 offset )
+{
+	return ioread32(((u8*)addr) + offset);
+}
+
+void inline _mali_osk_mem_iowrite32( volatile mali_io_address addr, u32 offset, u32 val )
+{
+	iowrite32(val, ((u8*)addr) + offset);
+}
+
+void _mali_osk_cache_flushall( void )
+{
+	/** @note Cached memory is not currently supported in this implementation */
+}
+
+void _mali_osk_cache_ensure_uncached_range_flushed( void *uncached_mapping, u32 offset, u32 size )
+{
+	wmb();
+}
+
+_mali_osk_errcode_t _mali_osk_mem_mapregion_init( mali_memory_allocation * descriptor )
+{
+	struct vm_area_struct *vma;
+	mali_vma_usage_tracker * vma_usage_tracker;
+	MappingInfo *mappingInfo;
+
+	if (NULL == descriptor) return _MALI_OSK_ERR_FAULT;
+
+	MALI_DEBUG_ASSERT( 0 != (descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE) );
+
+	vma = (struct vm_area_struct*)descriptor->process_addr_mapping_info;
+
+	if (NULL == vma ) return _MALI_OSK_ERR_FAULT;
+
+	/* Re-write the process_addr_mapping_info */
+	mappingInfo = _mali_osk_calloc( 1, sizeof(MappingInfo) );
+
+	if ( NULL == mappingInfo ) return _MALI_OSK_ERR_FAULT;
+
+	vma_usage_tracker = _mali_osk_calloc( 1, sizeof(mali_vma_usage_tracker) );
+
+	if (NULL == vma_usage_tracker)
+	{
+		MALI_DEBUG_PRINT(2, ("Failed to allocate memory to track memory usage\n"));
+		_mali_osk_free( mappingInfo );
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	mappingInfo->vma = vma;
+	descriptor->process_addr_mapping_info = mappingInfo;
+
+	/* Do the va range allocation - in this case, it was done earlier, so we copy in that information */
+	descriptor->mapping = (void __user*)vma->vm_start;
+	/* list member is already NULL */
+
+	/*
+	  set some bits which indicate that:
+	  The memory is IO memory, meaning that no paging is to be performed and the memory should not be included in crash dumps
+	  The memory is reserved, meaning that it's present and can never be paged out (see also previous entry)
+	*/
+	vma->vm_flags |= VM_IO;
+	vma->vm_flags |= VM_RESERVED;
+	vma->vm_flags |= VM_DONTCOPY;
+
+	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+	vma->vm_ops = &mali_kernel_vm_ops; /* Operations used on any memory system */
+
+	vma_usage_tracker->references = 1; /* set initial reference count to be 1 as vma_open won't be called for the first mmap call */
+	vma_usage_tracker->cookie = (u32)descriptor; /* cookie for munmap */
+
+	vma->vm_private_data = vma_usage_tracker;
+
+	return _MALI_OSK_ERR_OK;
+}
+
+void _mali_osk_mem_mapregion_term( mali_memory_allocation * descriptor )
+{
+	struct vm_area_struct* vma;
+	mali_vma_usage_tracker * vma_usage_tracker;
+	MappingInfo *mappingInfo;
+
+	if (NULL == descriptor) return;
+
+	MALI_DEBUG_ASSERT( 0 != (descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE) );
+
+	mappingInfo = (MappingInfo *)descriptor->process_addr_mapping_info;
+
+	MALI_DEBUG_ASSERT_POINTER( mappingInfo );
+
+	/* Linux does the right thing as part of munmap to remove the mapping
+	 * All that remains is that we remove the vma_usage_tracker setup in init() */
+	vma = mappingInfo->vma;
+
+	MALI_DEBUG_ASSERT_POINTER( vma );
+
+	/* ASSERT that there are no allocations on the list. Unmap should've been
+	 * called on all OS allocations. */
+	MALI_DEBUG_ASSERT( NULL == mappingInfo->list );
+
+	vma_usage_tracker = vma->vm_private_data;
+
+	/* We only get called if mem_mapregion_init succeeded */
+	_mali_osk_free(vma_usage_tracker);
+
+	_mali_osk_free( mappingInfo );
+	return;
+}
+
+_mali_osk_errcode_t _mali_osk_mem_mapregion_map( mali_memory_allocation * descriptor, u32 offset, u32 *phys_addr, u32 size )
+{
+	struct vm_area_struct *vma;
+	MappingInfo *mappingInfo;
+
+	if (NULL == descriptor) return _MALI_OSK_ERR_FAULT;
+
+	MALI_DEBUG_ASSERT_POINTER( phys_addr );
+
+	MALI_DEBUG_ASSERT( 0 != (descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE) );
+
+	MALI_DEBUG_ASSERT( 0 == (size & ~_MALI_OSK_CPU_PAGE_MASK) );
+
+	MALI_DEBUG_ASSERT( 0 == (offset & ~_MALI_OSK_CPU_PAGE_MASK));
+
+	if (NULL == descriptor->mapping) return _MALI_OSK_ERR_INVALID_ARGS;
+
+	if (size > (descriptor->size - offset))
+	{
+		MALI_DEBUG_PRINT(1,("_mali_osk_mem_mapregion_map: virtual memory area not large enough to map physical 0x%x size %x into area 0x%x at offset 0x%xr\n",
+		                    *phys_addr, size, descriptor->mapping, offset));
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	mappingInfo = (MappingInfo *)descriptor->process_addr_mapping_info;
+
+	MALI_DEBUG_ASSERT_POINTER( mappingInfo );
+
+	vma = mappingInfo->vma;
+
+	if (NULL == vma ) return _MALI_OSK_ERR_FAULT;
+
+	MALI_DEBUG_PRINT(7, ("Process map: mapping 0x%08X to process address 0x%08lX length 0x%08X\n", *phys_addr, (long unsigned int)(descriptor->mapping + offset), size));
+
+	if ( MALI_MEMORY_ALLOCATION_OS_ALLOCATED_PHYSADDR_MAGIC == *phys_addr )
+	{
+		_mali_osk_errcode_t ret;
+		AllocationList *alloc_item;
+		u32 linux_phys_frame_num;
+
+		alloc_item = _allocation_list_item_get();
+
+		linux_phys_frame_num = alloc_item->physaddr >> PAGE_SHIFT;
+
+		ret = ( remap_pfn_range( vma, ((u32)descriptor->mapping) + offset, linux_phys_frame_num, size, vma->vm_page_prot) ) ? _MALI_OSK_ERR_FAULT : _MALI_OSK_ERR_OK;
+
+		if ( ret != _MALI_OSK_ERR_OK)
+		{
+			_allocation_list_item_release(alloc_item);
+			return ret;
+		}
+
+		/* Put our alloc_item into the list of allocations on success */
+		alloc_item->next = mappingInfo->list;
+		alloc_item->offset = offset;
+
+		/*alloc_item->physaddr = linux_phys_addr;*/
+		mappingInfo->list = alloc_item;
+
+		/* Write out new physical address on success */
+		*phys_addr = alloc_item->physaddr;
+
+		return ret;
+	}
+
+	/* Otherwise, Use the supplied physical address */
+
+	/* ASSERT that supplied phys_addr is page aligned */
+	MALI_DEBUG_ASSERT( 0 == ((*phys_addr) & ~_MALI_OSK_CPU_PAGE_MASK) );
+
+	return ( remap_pfn_range( vma, ((u32)descriptor->mapping) + offset, *phys_addr >> PAGE_SHIFT, size, vma->vm_page_prot) ) ? _MALI_OSK_ERR_FAULT : _MALI_OSK_ERR_OK;
+
+}
+
+void _mali_osk_mem_mapregion_unmap( mali_memory_allocation * descriptor, u32 offset, u32 size, _mali_osk_mem_mapregion_flags_t flags )
+{
+	MappingInfo *mappingInfo;
+
+   if (NULL == descriptor) return;
+
+	MALI_DEBUG_ASSERT( 0 != (descriptor->flags & MALI_MEMORY_ALLOCATION_FLAG_MAP_INTO_USERSPACE) );
+
+	MALI_DEBUG_ASSERT( 0 == (size & ~_MALI_OSK_CPU_PAGE_MASK) );
+
+	MALI_DEBUG_ASSERT( 0 == (offset & ~_MALI_OSK_CPU_PAGE_MASK) );
+
+	if (NULL == descriptor->mapping) return;
+
+	if (size > (descriptor->size - offset))
+	{
+		MALI_DEBUG_PRINT(1,("_mali_osk_mem_mapregion_unmap: virtual memory area not large enough to unmap size %x from area 0x%x at offset 0x%x\n",
+							size, descriptor->mapping, offset));
+		return;
+	}
+	mappingInfo = (MappingInfo *)descriptor->process_addr_mapping_info;
+
+	MALI_DEBUG_ASSERT_POINTER( mappingInfo );
+
+	if ( 0 != (flags & _MALI_OSK_MEM_MAPREGION_FLAG_OS_ALLOCATED_PHYSADDR) )
+	{
+		/* This physical RAM was allocated in _mali_osk_mem_mapregion_map and
+		 * so needs to be unmapped
+		 */
+		while (size)
+		{
+			/* First find the allocation in the list of allocations */
+			AllocationList *alloc = mappingInfo->list;
+			AllocationList **prev = &(mappingInfo->list);
+			while (NULL != alloc && alloc->offset != offset)
+			{
+				prev = &(alloc->next);
+				alloc = alloc->next;
+			}
+			if (alloc == NULL) {
+				MALI_DEBUG_PRINT(1, ("Unmapping memory that isn't mapped\n"));
+				size -= _MALI_OSK_CPU_PAGE_SIZE;
+				offset += _MALI_OSK_CPU_PAGE_SIZE;
+				continue;
+			}
+
+			_kernel_page_release(alloc->physaddr);
+
+			/* Remove the allocation from the list */
+			*prev = alloc->next;
+			_mali_osk_free( alloc );
+
+			/* Move onto the next allocation */
+			size -= _MALI_OSK_CPU_PAGE_SIZE;
+			offset += _MALI_OSK_CPU_PAGE_SIZE;
+		}
+	}
+
+	/* Linux does the right thing as part of munmap to remove the mapping */
+
+	return;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_mali.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_mali.c
new file mode 100644
index 00000000000..694d8eeaabc
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_mali.c
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Added support for overriding memory settings in arch_configuration using
+ * mali_mem module parameter.
+ *
+ * Author: Magnus Wendt <magnus.wendt@stericsson.com> for
+ * ST-Ericsson.
+ */
+
+/**
+ * @file mali_osk_mali.c
+ * Implementation of the OS abstraction layer which is specific for the Mali kernel device driver
+ */
+#include <linux/kernel.h>
+#include <asm/uaccess.h>
+
+#include "mali_kernel_common.h" /* MALI_xxx macros */
+#include "mali_osk.h"           /* kernel side OS functions */
+#include "mali_uk_types.h"
+#include "mali_kernel_linux.h"  /* exports initialize/terminate_kernel_device() definition of mali_osk_low_level_mem_init() and term */
+#include "arch/config.h"        /* contains the configuration of the arch we are compiling for */
+
+extern char* mali_mem;
+
+/* is called from mali_kernel_constructor in common code */
+_mali_osk_errcode_t _mali_osk_init( void )
+{
+	if (0 != initialize_kernel_device()) MALI_ERROR(_MALI_OSK_ERR_FAULT);
+
+	mali_osk_low_level_mem_init();
+	
+	MALI_SUCCESS;
+}
+
+/* is called from mali_kernel_deconstructor in common code */
+void _mali_osk_term( void )
+{
+	mali_osk_low_level_mem_term();
+	terminate_kernel_device();
+}
+
+_mali_osk_errcode_t _mali_osk_resources_init( _mali_osk_resource_t **arch_config, u32 *num_resources )
+{
+    *num_resources = sizeof(arch_configuration) / sizeof(arch_configuration[0]);
+    *arch_config = arch_configuration;
+
+	/* override the MEMORY resource if a value has been supplied from the command line */
+	if ('\0' != mali_mem[0]) {
+		char *p = mali_mem;
+		_mali_osk_resource_type_t mem_type = MEMORY;
+		unsigned long mem_base = 0;
+		unsigned long mem_size = memparse(p, &p);
+		if (*p == '@') {
+			if ((*(p + 1) == 'O') || (*(p + 1) == 'o')) { /* as in OS. e.g. mali_mem=64M@OS_MEMORY */
+				mem_type = OS_MEMORY;
+				mem_base = 0;
+			} else { /* parse the base address */
+				mem_type = MEMORY;
+				mem_base = memparse(p + 1, &p);
+			}
+		}
+
+		/* change the first memory entry in the architecture config. */
+		if (0 < mem_size) {
+			int i;
+			for (i = 0; i < *num_resources; ++i) {
+				if ((MEMORY == arch_configuration[i].type) ||
+			            (OS_MEMORY == arch_configuration[i].type)) {
+					MALI_DEBUG_PRINT( 1, ("Overriding arch resource[%d] :\n",i));
+					MALI_DEBUG_PRINT( 1, ("Type: %s, base: %x, size %x\n",
+						(OS_MEMORY==mem_type?"OS_MEMORY":"MEMORY"),mem_base,mem_size));
+					arch_configuration[i].type = mem_type;
+					arch_configuration[i].base = mem_base;
+					arch_configuration[i].size = mem_size;
+					break;
+				}
+			}
+		}
+	}
+
+    return _MALI_OSK_ERR_OK;
+}
+
+void _mali_osk_resources_term( _mali_osk_resource_t **arch_config, u32 num_resources )
+{
+    /* Nothing to do */
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_math.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_math.c
new file mode 100644
index 00000000000..2f7fc15847b
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_math.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_math.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+#include "mali_osk.h"
+#include <linux/bitops.h>
+
+u32 inline _mali_osk_clz( u32 input )
+{
+	return 32-fls(input);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_memory.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_memory.c
new file mode 100644
index 00000000000..bf6679f7bac
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_memory.c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_memory.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+#include "mali_osk.h"
+#include <linux/slab.h>
+
+void inline *_mali_osk_calloc( u32 n, u32 size )
+{
+    return kcalloc(n, size, GFP_KERNEL);
+}
+
+void inline *_mali_osk_malloc( u32 size )
+{
+    return kmalloc(size, GFP_KERNEL);
+}
+
+void inline _mali_osk_free( void *ptr )
+{
+    kfree(ptr);
+}
+
+void inline *_mali_osk_memcpy( void *dst, const void *src, u32	len )
+{
+    return memcpy(dst, src, len);
+}
+
+void inline *_mali_osk_memset( void *s, u32 c, u32 n )
+{
+    return memset(s, c, n);
+}
+
+mali_bool _mali_osk_mem_check_allocated( u32 max_allocated )
+{
+	/* No need to prevent an out-of-memory dialogue appearing on Linux,
+	 * so we always return MALI_TRUE.
+	 */
+	return MALI_TRUE;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_misc.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_misc.c
new file mode 100644
index 00000000000..3afd2da736c
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_misc.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_misc.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+#include <linux/kernel.h>
+#include <asm/uaccess.h>
+#include <asm/cacheflush.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+#include "mali_osk.h"
+
+void _mali_osk_dbgmsg( const char *fmt, ... )
+{
+    va_list args;
+    va_start(args, fmt);
+    vprintk(fmt, args);
+	va_end(args);
+}
+
+void _mali_osk_abort(void)
+{
+	/* make a simple fault by dereferencing a NULL pointer */
+	*(int *)0 = 0;
+}
+
+void _mali_osk_break(void)
+{
+	_mali_osk_abort();
+}
+
+u32 _mali_osk_get_pid(void)
+{
+	/* Thread group ID is the process ID on Linux */
+	return (u32)current->tgid;
+}
+
+u32 _mali_osk_get_tid(void)
+{
+	/* pid is actually identifying the thread on Linux */
+	return (u32)current->pid;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_notification.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_notification.c
new file mode 100644
index 00000000000..2efc140f60a
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_notification.c
@@ -0,0 +1,191 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_notification.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+
+/* needed to detect kernel version specific code */
+#include <linux/version.h>
+
+#include <linux/sched.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+#include <linux/semaphore.h>
+#else /* pre 2.6.26 the file was in the arch specific location */
+#include <asm/semaphore.h>
+#endif
+
+/**
+ * Declaration of the notification queue object type
+ * Contains a linked list of notification pending delivery to user space.
+ * It also contains a wait queue of exclusive waiters blocked in the ioctl
+ * When a new notification is posted a single thread is resumed.
+ */
+struct _mali_osk_notification_queue_t_struct
+{
+	struct semaphore mutex; /**< Mutex protecting the list */
+	wait_queue_head_t receive_queue; /**< Threads waiting for new entries to the queue */
+	struct list_head head; /**< List of notifications waiting to be picked up */
+};
+
+typedef struct _mali_osk_notification_wrapper_t_struct
+{
+    struct list_head list;           /**< Internal linked list variable */
+    _mali_osk_notification_t data;   /**< Notification data */
+} _mali_osk_notification_wrapper_t;
+
+_mali_osk_notification_queue_t *_mali_osk_notification_queue_init( void )
+{
+	_mali_osk_notification_queue_t *	result;
+
+	result = (_mali_osk_notification_queue_t *)kmalloc(sizeof(_mali_osk_notification_queue_t), GFP_KERNEL);
+	if (NULL == result) return NULL;
+
+	sema_init(&result->mutex, 1);
+	init_waitqueue_head(&result->receive_queue);
+	INIT_LIST_HEAD(&result->head);
+
+	return result;
+}
+
+_mali_osk_notification_t *_mali_osk_notification_create( u32 type, u32 size )
+{
+	/* OPT Recycling of notification objects */
+    _mali_osk_notification_wrapper_t *notification;
+
+	notification = (_mali_osk_notification_wrapper_t *)kmalloc( sizeof(_mali_osk_notification_wrapper_t) + size, GFP_KERNEL );
+    if (NULL == notification)
+    {
+		MALI_DEBUG_PRINT(1, ("Failed to create a notification object\n"));
+		return NULL;
+    }
+
+	/* Init the list */
+	INIT_LIST_HEAD(&notification->list);
+
+	if (0 != size)
+	{
+		notification->data.result_buffer = ((u8*)notification) + sizeof(_mali_osk_notification_wrapper_t);
+	}
+	else
+	{
+		notification->data.result_buffer = NULL;
+	}
+
+	/* set up the non-allocating fields */
+	notification->data.notification_type = type;
+	notification->data.result_buffer_size = size;
+
+	/* all ok */
+    return &(notification->data);
+}
+
+void _mali_osk_notification_delete( _mali_osk_notification_t *object )
+{
+	_mali_osk_notification_wrapper_t *notification;
+	MALI_DEBUG_ASSERT_POINTER( object );
+
+    notification = container_of( object, _mali_osk_notification_wrapper_t, data );
+
+	/* Remove from the list */
+	list_del(&notification->list);
+	/* Free the container */
+	kfree(notification);
+}
+
+void _mali_osk_notification_queue_term( _mali_osk_notification_queue_t *queue )
+{
+	MALI_DEBUG_ASSERT_POINTER( queue );
+
+	/* not much to do, just free the memory */
+	kfree(queue);
+}
+
+void _mali_osk_notification_queue_send( _mali_osk_notification_queue_t *queue, _mali_osk_notification_t *object )
+{
+	_mali_osk_notification_wrapper_t *notification;
+	MALI_DEBUG_ASSERT_POINTER( queue );
+	MALI_DEBUG_ASSERT_POINTER( object );
+
+    notification = container_of( object, _mali_osk_notification_wrapper_t, data );
+
+	/* lock queue access */
+	down(&queue->mutex);
+	/* add to list */
+	list_add_tail(&notification->list, &queue->head);
+	/* unlock the queue */
+	up(&queue->mutex);
+
+	/* and wake up one possible exclusive waiter */
+	wake_up(&queue->receive_queue);
+}
+
+static int _mali_notification_queue_is_empty( _mali_osk_notification_queue_t *queue )
+{
+	int ret;
+
+	down(&queue->mutex);
+	ret = list_empty(&queue->head);
+	up(&queue->mutex);
+	return ret;
+}
+
+#if MALI_STATE_TRACKING
+mali_bool _mali_osk_notification_queue_is_empty( _mali_osk_notification_queue_t *queue )
+{
+	return _mali_notification_queue_is_empty(queue) ? MALI_TRUE : MALI_FALSE;
+}
+#endif
+
+_mali_osk_errcode_t _mali_osk_notification_queue_dequeue( _mali_osk_notification_queue_t *queue, _mali_osk_notification_t **result )
+{
+	_mali_osk_errcode_t ret = _MALI_OSK_ERR_ITEM_NOT_FOUND;
+	_mali_osk_notification_wrapper_t *wrapper_object;
+
+	down(&queue->mutex);
+
+	if (!list_empty(&queue->head))
+	{
+		wrapper_object = list_entry(queue->head.next, _mali_osk_notification_wrapper_t, list);
+		*result = &(wrapper_object->data);
+		list_del_init(&wrapper_object->list);
+		ret = _MALI_OSK_ERR_OK;
+	}
+
+	up(&queue->mutex);
+
+	return ret;
+}
+
+_mali_osk_errcode_t _mali_osk_notification_queue_receive( _mali_osk_notification_queue_t *queue, _mali_osk_notification_t **result )
+{
+    /* check input */
+	MALI_DEBUG_ASSERT_POINTER( queue );
+	MALI_DEBUG_ASSERT_POINTER( result );
+
+    /* default result */
+	*result = NULL;
+
+	while (_MALI_OSK_ERR_OK != _mali_osk_notification_queue_dequeue(queue, result))
+	{
+		if (wait_event_interruptible(queue->receive_queue, !_mali_notification_queue_is_empty(queue)))
+		{
+			return _MALI_OSK_ERR_RESTARTSYSCALL;
+		}
+	}
+
+	return _MALI_OSK_ERR_OK; /* all ok */
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_pm.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_pm.c
new file mode 100644
index 00000000000..04fcd4572df
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_pm.c
@@ -0,0 +1,195 @@
+/**
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_pm.c
+ * Implementation of the callback functions from common power management
+ */
+
+#include <linux/sched.h>
+
+#ifdef CONFIG_PM_RUNTIME
+#include <linux/pm_runtime.h>
+#endif /* CONFIG_PM_RUNTIME */
+
+#include <linux/platform_device.h>
+
+#include "mali_platform.h"
+#include "mali_osk.h"
+#include "mali_uk_types.h"
+#include "mali_pmm.h"
+#include "mali_ukk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_license.h"
+#include "mali_kernel_pm.h"
+#include "mali_device_pause_resume.h"
+#include "mali_linux_pm.h"
+#include "mali_linux_pm_testsuite.h"
+
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+#ifdef CONFIG_PM_RUNTIME
+static int is_runtime =0;
+#endif /* CONFIG_PM_RUNTIME */
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+
+#if MALI_POWER_MGMT_TEST_SUITE
+
+#ifdef CONFIG_PM
+unsigned int mali_pmm_events_triggered_mask = 0;
+#endif /* CONFIG_PM */
+
+void _mali_osk_pmm_policy_events_notifications(mali_pmm_event_id mali_pmm_event)
+{
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM
+
+	switch (mali_pmm_event)
+	{
+		case MALI_PMM_EVENT_JOB_QUEUED:
+			if (mali_job_scheduling_events_recording_on == 1)
+			{
+				mali_pmm_events_triggered_mask |= (1<<0);
+			}
+		break;
+
+		case MALI_PMM_EVENT_JOB_SCHEDULED:
+			if (mali_job_scheduling_events_recording_on == 1)
+			{
+				mali_pmm_events_triggered_mask |= (1<<1);
+			}
+		break;
+
+		case MALI_PMM_EVENT_JOB_FINISHED:
+			if (mali_job_scheduling_events_recording_on == 1)
+			{
+				mali_pmm_events_triggered_mask |= (1<<2);
+				mali_job_scheduling_events_recording_on = 0;
+				pwr_mgmt_status_reg = mali_pmm_events_triggered_mask;
+			}
+		break;
+
+		case MALI_PMM_EVENT_TIMEOUT:
+			if (mali_timeout_event_recording_on == 1)
+			{
+				pwr_mgmt_status_reg = (1<<3);
+				mali_timeout_event_recording_on = 0;
+			}
+		break;
+
+		default:
+
+		break;
+
+	}
+#endif /* CONFIG_PM */
+
+#endif /* MALI_LICENSE_IS_GPL */
+}
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+
+/** This function is called when the Mali device has completed power up
+ * operation.
+ */
+void _mali_osk_pmm_power_up_done(mali_pmm_message_data data)
+{
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM
+	is_wake_up_needed = 1;
+	wake_up_process(pm_thread);
+	MALI_DEBUG_PRINT(4, ("OSPMM: MALI OSK Power up Done\n" ));
+	return;
+#endif /* CONFIG_PM */
+#endif /* MALI_LICENSE_IS_GPL */
+}
+
+/** This function is called when the Mali device has completed power down
+ * operation.
+ */
+void _mali_osk_pmm_power_down_done(mali_pmm_message_data data)
+{
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM
+	is_wake_up_needed = 1;
+#if MALI_POWER_MGMT_TEST_SUITE
+	if (is_mali_pmu_present == 0)
+	{
+		pwr_mgmt_status_reg = _mali_pmm_cores_list();
+	}
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+	wake_up_process(pm_thread);
+	MALI_DEBUG_PRINT(4, ("OSPMM: MALI Power down Done\n" ));
+	return;
+
+#endif /* CONFIG_PM */
+#endif /* MALI_LICENSE_IS_GPL */
+}
+
+/** This function is invoked when mali device is idle.
+*/
+_mali_osk_errcode_t _mali_osk_pmm_dev_idle(void)
+{
+	_mali_osk_errcode_t err = 0;
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+
+	err = pm_runtime_put_sync(&(mali_gpu_device.dev));	
+	if(err)
+	{
+		MALI_DEBUG_PRINT(4, ("OSPMM: Error in _mali_osk_pmm_dev_idle\n" ));	
+	}
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_PM_RUNTIME */
+#endif /* MALI_LICENSE_IS_GPL */
+	return err;
+}
+
+/** This funtion is invoked when mali device needs to be activated.
+*/
+void _mali_osk_pmm_dev_activate(void)
+{
+	
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM_RUNTIME
+#if MALI_PMM_RUNTIME_JOB_CONTROL_ON
+	int err = 0;
+	if(is_runtime == 0)
+	{
+		pm_suspend_ignore_children(&(mali_gpu_device.dev), true);
+		pm_runtime_enable(&(mali_gpu_device.dev));
+ 		pm_runtime_get_sync(&(mali_gpu_device.dev));
+		is_runtime = 1;
+	}
+	else
+	{
+		err = pm_runtime_get_sync(&(mali_gpu_device.dev));
+	}
+	if(err)
+        {
+		MALI_DEBUG_PRINT(4, ("OSPMM: Error in _mali_osk_pmm_dev_activate\n" ));
+        }
+#endif /* MALI_PMM_RUNTIME_JOB_CONTROL_ON */
+#endif /* CONFIG_PM_RUNTIME */
+#endif /* MALI_LICENSE_IS_GPL */
+}
+
+void _mali_osk_pmm_dvfs_operation_done(mali_pmm_message_data data)
+{
+#if MALI_LICENSE_IS_GPL
+#ifdef CONFIG_PM
+	is_wake_up_needed = 1;
+	wake_up_process(dvfs_pm_thread);
+	MALI_DEBUG_PRINT(4, ("OSPMM: MALI OSK DVFS Operation done\n" ));
+	return;
+#endif /* CONFIG_PM */
+#endif /* MALI_LICENSE_IS_GPL */
+}
+
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_specific.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_specific.h
new file mode 100644
index 00000000000..54acfdd138c
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_specific.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_specific.h
+ * Defines per-OS Kernel level specifics, such as unusual workarounds for
+ * certain OSs.
+ */
+
+#ifndef __MALI_OSK_SPECIFIC_H__
+#define __MALI_OSK_SPECIFIC_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#define MALI_STATIC_INLINE     static inline
+#define MALI_NON_STATIC_INLINE inline
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_OSK_SPECIFIC_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_time.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_time.c
new file mode 100644
index 00000000000..aa7962380e6
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_time.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_time.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+#include "mali_osk.h"
+#include <linux/jiffies.h>
+#include <linux/time.h>
+#include <asm/delay.h>
+
+int	_mali_osk_time_after( u32 ticka, u32 tickb )
+{
+    return time_after((unsigned long)ticka, (unsigned long)tickb);
+}
+
+u32	_mali_osk_time_mstoticks( u32 ms )
+{
+    return msecs_to_jiffies(ms);
+}
+
+u32	_mali_osk_time_tickstoms( u32 ticks )
+{
+    return jiffies_to_msecs(ticks);
+}
+
+u32	_mali_osk_time_tickcount( void )
+{
+    return jiffies;
+}
+
+void _mali_osk_time_ubusydelay( u32 usecs )
+{
+    udelay(usecs);
+}
+
+u64 _mali_osk_time_get_ns( void )
+{
+	struct timespec tsval;
+	getnstimeofday(&tsval);
+	return (u64)timespec_to_ns(&tsval);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_timers.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_timers.c
new file mode 100644
index 00000000000..3dfba76bcb3
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_osk_timers.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_osk_timers.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+#include <linux/timer.h>
+#include <linux/slab.h>
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+
+struct _mali_osk_timer_t_struct
+{
+    struct timer_list timer;
+};
+
+typedef void (*timer_timeout_function_t)(unsigned long);
+
+_mali_osk_timer_t *_mali_osk_timer_init(void)
+{
+    _mali_osk_timer_t *t = (_mali_osk_timer_t*)kmalloc(sizeof(_mali_osk_timer_t), GFP_KERNEL);
+    if (NULL != t) init_timer(&t->timer);
+    return t;
+}
+
+void _mali_osk_timer_add( _mali_osk_timer_t *tim, u32 ticks_to_expire )
+{
+    MALI_DEBUG_ASSERT_POINTER(tim);
+    tim->timer.expires = _mali_osk_time_tickcount() + ticks_to_expire;
+    add_timer(&(tim->timer));
+}
+
+void _mali_osk_timer_mod( _mali_osk_timer_t *tim, u32 expiry_tick)
+{
+    MALI_DEBUG_ASSERT_POINTER(tim);
+    mod_timer(&(tim->timer), expiry_tick);
+}
+
+void _mali_osk_timer_del( _mali_osk_timer_t *tim )
+{
+    MALI_DEBUG_ASSERT_POINTER(tim);
+    del_timer_sync(&(tim->timer));
+}
+
+void _mali_osk_timer_setcallback( _mali_osk_timer_t *tim, _mali_osk_timer_callback_t callback, void *data )
+{
+    MALI_DEBUG_ASSERT_POINTER(tim);
+    tim->timer.data = (unsigned long)data;
+    tim->timer.function = (timer_timeout_function_t)callback;
+}
+
+void _mali_osk_timer_term( _mali_osk_timer_t *tim )
+{
+    MALI_DEBUG_ASSERT_POINTER(tim);
+    kfree(tim);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_core.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_core.c
new file mode 100644
index 00000000000..0f745492613
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_core.c
@@ -0,0 +1,142 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include <linux/fs.h>       /* file system operations */
+#include <linux/slab.h>     /* memort allocation functions */
+#include <asm/uaccess.h>    /* user space access */
+
+#include "mali_ukk.h"
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_session_manager.h"
+#include "mali_ukk_wrappers.h"
+
+int get_api_version_wrapper(struct mali_session_data *session_data, _mali_uk_get_api_version_s __user *uargs)
+{
+	_mali_uk_get_api_version_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+    if (0 != get_user(kargs.version, &uargs->version)) return -EFAULT;
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_get_api_version(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+    if (0 != put_user(kargs.version, &uargs->version)) return -EFAULT;
+    if (0 != put_user(kargs.compatible, &uargs->compatible)) return -EFAULT;
+
+    return 0;
+}
+
+int get_system_info_size_wrapper(struct mali_session_data *session_data, _mali_uk_get_system_info_size_s __user *uargs)
+{
+	_mali_uk_get_system_info_size_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_get_system_info_size(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+    if (0 != put_user(kargs.size, &uargs->size)) return -EFAULT;
+
+    return 0;
+}
+
+int get_system_info_wrapper(struct mali_session_data *session_data, _mali_uk_get_system_info_s __user *uargs)
+{
+	_mali_uk_get_system_info_s kargs;
+    _mali_osk_errcode_t err;
+    _mali_system_info *system_info_user;
+    _mali_system_info *system_info_kernel;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+    if (0 != get_user(kargs.system_info, &uargs->system_info)) return -EFAULT;
+    if (0 != get_user(kargs.size, &uargs->size)) return -EFAULT;
+
+    /* A temporary kernel buffer for the system_info datastructure is passed through the system_info
+     * member. The ukk_private member will point to the user space destination of this buffer so
+     * that _mali_ukk_get_system_info() can correct the pointers in the system_info correctly
+     * for user space.
+     */
+    system_info_kernel = kmalloc(kargs.size, GFP_KERNEL);
+    if (NULL == system_info_kernel) return -EFAULT;
+
+    system_info_user = kargs.system_info;
+    kargs.system_info = system_info_kernel;
+    kargs.ukk_private = (u32)system_info_user;
+    kargs.ctx = session_data;
+
+    err = _mali_ukk_get_system_info(&kargs);
+    if (_MALI_OSK_ERR_OK != err)
+    {
+        kfree(system_info_kernel);
+        return map_errcode(err);
+    }
+
+    if (0 != copy_to_user(system_info_user, system_info_kernel, kargs.size))
+    {
+        kfree(system_info_kernel);
+        return -EFAULT;
+    }
+
+    kfree(system_info_kernel);
+    return 0;
+}
+
+int wait_for_notification_wrapper(struct mali_session_data *session_data, _mali_uk_wait_for_notification_s __user *uargs)
+{
+    _mali_uk_wait_for_notification_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_wait_for_notification(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+	if(_MALI_NOTIFICATION_CORE_SHUTDOWN_IN_PROGRESS != kargs.type)
+	{
+		kargs.ctx = NULL; /* prevent kernel address to be returned to user space */
+		if (0 != copy_to_user(uargs, &kargs, sizeof(_mali_uk_wait_for_notification_s))) return -EFAULT;
+	}
+	else
+	{
+		if (0 != put_user(kargs.type, &uargs->type)) return -EFAULT;
+	}
+
+    return 0;
+}
+
+int post_notification_wrapper(struct mali_session_data *session_data, _mali_uk_post_notification_s __user *uargs)
+{
+	_mali_uk_post_notification_s kargs;
+	_mali_osk_errcode_t err;
+
+	MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+	kargs.ctx = session_data;
+
+	if (0 != get_user(kargs.type, &uargs->type))
+	{
+		return -EFAULT;
+	}
+
+	err = _mali_ukk_post_notification(&kargs);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		return map_errcode(err);
+	}
+
+	return 0;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_gp.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_gp.c
new file mode 100644
index 00000000000..a6f355fd459
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_gp.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include <linux/fs.h>       /* file system operations */
+#include <asm/uaccess.h>    /* user space access */
+
+#include "mali_ukk.h"
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_session_manager.h"
+#include "mali_ukk_wrappers.h"
+
+int gp_start_job_wrapper(struct mali_session_data *session_data, _mali_uk_gp_start_job_s __user *uargs)
+{
+    _mali_uk_gp_start_job_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+    MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+	if (!access_ok(VERIFY_WRITE, uargs, sizeof(_mali_uk_gp_start_job_s)))
+	{
+		return -EFAULT;
+	}
+
+    if (0 != copy_from_user(&kargs, uargs, sizeof(_mali_uk_gp_start_job_s))) return -EFAULT;
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_gp_start_job(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+    kargs.ctx = NULL; /* prevent kernel address to be returned to user space */
+    if (0 != copy_to_user(uargs, &kargs, sizeof(_mali_uk_gp_start_job_s)))
+	{
+		/*
+		 * If this happens, then user space will not know that the job was actually started,
+		 * and if we return a queued job, then user space will still think that one is still queued.
+		 * This will typically lead to a deadlock in user space.
+		 * This could however only happen if user space deliberately passes a user buffer which
+		 * passes the access_ok(VERIFY_WRITE) check, but isn't fully writable at the time of copy_to_user().
+		 * The official Mali driver will never attempt to do that, and kernel space should not be affected.
+		 * That is why we do not bother to do a complex rollback in this very very very rare case.
+		 */
+		return -EFAULT;
+	}
+
+    return 0;
+}
+
+int gp_abort_job_wrapper(struct mali_session_data *session_data, _mali_uk_gp_abort_job_s __user *uargs)
+{
+    _mali_uk_gp_abort_job_s kargs;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+    MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+    if (0 != copy_from_user(&kargs, uargs, sizeof(_mali_uk_gp_abort_job_s))) return -EFAULT;
+
+    kargs.ctx = session_data;
+    _mali_ukk_gp_abort_job(&kargs);
+
+    return 0;
+}
+
+
+int gp_get_core_version_wrapper(struct mali_session_data *session_data, _mali_uk_get_gp_core_version_s __user *uargs)
+{
+    _mali_uk_get_gp_core_version_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+    MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+    kargs.ctx = session_data;
+    err =  _mali_ukk_get_gp_core_version(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+	/* no known transactions to roll-back */
+
+    if (0 != put_user(kargs.version, &uargs->version)) return -EFAULT;
+
+    return 0;
+}
+
+int gp_suspend_response_wrapper(struct mali_session_data *session_data, _mali_uk_gp_suspend_response_s __user *uargs)
+{
+    _mali_uk_gp_suspend_response_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+    MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+    if (0 != copy_from_user(&kargs, uargs, sizeof(_mali_uk_gp_suspend_response_s))) return -EFAULT;
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_gp_suspend_response(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+    if (0 != put_user(kargs.cookie, &uargs->cookie)) return -EFAULT;
+
+    /* no known transactions to roll-back */
+    return 0;
+}
+
+int gp_get_number_of_cores_wrapper(struct mali_session_data *session_data, _mali_uk_get_gp_number_of_cores_s __user *uargs)
+{
+    _mali_uk_get_gp_number_of_cores_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+    MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_get_gp_number_of_cores(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+	/* no known transactions to roll-back */
+
+    if (0 != put_user(kargs.number_of_cores, &uargs->number_of_cores)) return -EFAULT;
+
+    return 0;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_mem.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_mem.c
new file mode 100644
index 00000000000..3f67a1e3328
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_mem.c
@@ -0,0 +1,336 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include <linux/fs.h>       /* file system operations */
+#include <asm/uaccess.h>    /* user space access */
+
+#include "mali_ukk.h"
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_session_manager.h"
+#include "mali_ukk_wrappers.h"
+
+int mem_init_wrapper(struct mali_session_data *session_data, _mali_uk_init_mem_s __user *uargs)
+{
+    _mali_uk_init_mem_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_init_mem(&kargs);
+    if (_MALI_OSK_ERR_OK != err)
+    {
+        return map_errcode(err);
+    }
+
+    if (0 != put_user(kargs.mali_address_base, &uargs->mali_address_base)) goto mem_init_rollback;
+    if (0 != put_user(kargs.memory_size, &uargs->memory_size)) goto mem_init_rollback;
+
+    return 0;
+
+mem_init_rollback:
+	{
+		_mali_uk_term_mem_s kargs;
+		kargs.ctx = session_data;
+		err = _mali_ukk_term_mem(&kargs);
+		if (_MALI_OSK_ERR_OK != err)
+		{
+			MALI_DEBUG_PRINT(4, ("reverting _mali_ukk_init_mem, as a result of failing put_user(), failed\n"));
+		}
+	}
+    return -EFAULT;
+}
+
+int mem_term_wrapper(struct mali_session_data *session_data, _mali_uk_term_mem_s __user *uargs)
+{
+    _mali_uk_term_mem_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_term_mem(&kargs);
+    if (_MALI_OSK_ERR_OK != err)
+    {
+        return map_errcode(err);
+    }
+
+    return 0;
+}
+
+int mem_map_ext_wrapper(struct mali_session_data *session_data, _mali_uk_map_external_mem_s __user * argument)
+{
+	_mali_uk_map_external_mem_s uk_args;
+	_mali_osk_errcode_t err_code;
+
+	/* validate input */
+	/* the session_data pointer was validated by caller */
+    MALI_CHECK_NON_NULL( argument, -EINVAL);
+
+	/* get call arguments from user space. copy_from_user returns how many bytes which where NOT copied */
+	if ( 0 != copy_from_user(&uk_args, (void __user *)argument, sizeof(_mali_uk_map_external_mem_s)) )
+	{
+		return -EFAULT;
+	}
+
+    uk_args.ctx = session_data;
+	err_code = _mali_ukk_map_external_mem( &uk_args );
+
+    if (0 != put_user(uk_args.cookie, &argument->cookie))
+    {
+        if (_MALI_OSK_ERR_OK == err_code)
+        {
+            /* Rollback */
+           	_mali_uk_unmap_external_mem_s uk_args_unmap;
+
+            uk_args_unmap.ctx = session_data;
+            uk_args_unmap.cookie = uk_args.cookie;
+            err_code = _mali_ukk_unmap_external_mem( &uk_args_unmap );
+            if (_MALI_OSK_ERR_OK != err_code)
+            {
+                MALI_DEBUG_PRINT(4, ("reverting _mali_ukk_unmap_external_mem, as a result of failing put_user(), failed\n"));
+            }
+        }
+        return -EFAULT;
+    }
+
+    /* Return the error that _mali_ukk_free_big_block produced */
+	return map_errcode(err_code);
+}
+
+int mem_unmap_ext_wrapper(struct mali_session_data *session_data, _mali_uk_unmap_external_mem_s __user * argument)
+{
+	_mali_uk_unmap_external_mem_s uk_args;
+	_mali_osk_errcode_t err_code;
+
+	/* validate input */
+	/* the session_data pointer was validated by caller */
+    MALI_CHECK_NON_NULL( argument, -EINVAL);
+
+	/* get call arguments from user space. copy_from_user returns how many bytes which where NOT copied */
+	if ( 0 != copy_from_user(&uk_args, (void __user *)argument, sizeof(_mali_uk_unmap_external_mem_s)) )
+	{
+		return -EFAULT;
+	}
+
+    uk_args.ctx = session_data;
+	err_code = _mali_ukk_unmap_external_mem( &uk_args );
+
+	/* Return the error that _mali_ukk_free_big_block produced */
+	return map_errcode(err_code);
+}
+
+#if MALI_USE_UNIFIED_MEMORY_PROVIDER != 0
+int mem_release_ump_wrapper(struct mali_session_data *session_data, _mali_uk_release_ump_mem_s __user * argument)
+{
+	_mali_uk_release_ump_mem_s uk_args;
+	_mali_osk_errcode_t err_code;
+
+	/* validate input */
+	/* the session_data pointer was validated by caller */
+    MALI_CHECK_NON_NULL( argument, -EINVAL);
+
+	/* get call arguments from user space. copy_from_user returns how many bytes which where NOT copied */
+	if ( 0 != copy_from_user(&uk_args, (void __user *)argument, sizeof(_mali_uk_release_ump_mem_s)) )
+	{
+		return -EFAULT;
+	}
+
+    uk_args.ctx = session_data;
+	err_code = _mali_ukk_release_ump_mem( &uk_args );
+
+	/* Return the error that _mali_ukk_free_big_block produced */
+	return map_errcode(err_code);
+}
+
+int mem_attach_ump_wrapper(struct mali_session_data *session_data, _mali_uk_attach_ump_mem_s __user * argument)
+{
+	_mali_uk_attach_ump_mem_s uk_args;
+	_mali_osk_errcode_t err_code;
+
+	/* validate input */
+	/* the session_data pointer was validated by caller */
+    MALI_CHECK_NON_NULL( argument, -EINVAL);
+
+	/* get call arguments from user space. copy_from_user returns how many bytes which where NOT copied */
+	if ( 0 != copy_from_user(&uk_args, (void __user *)argument, sizeof(_mali_uk_attach_ump_mem_s)) )
+	{
+		return -EFAULT;
+	}
+
+    uk_args.ctx = session_data;
+	err_code = _mali_ukk_attach_ump_mem( &uk_args );
+
+    if (0 != put_user(uk_args.cookie, &argument->cookie))
+    {
+        if (_MALI_OSK_ERR_OK == err_code)
+        {
+            /* Rollback */
+           	_mali_uk_release_ump_mem_s uk_args_unmap;
+
+            uk_args_unmap.ctx = session_data;
+            uk_args_unmap.cookie = uk_args.cookie;
+            err_code = _mali_ukk_release_ump_mem( &uk_args_unmap );
+            if (_MALI_OSK_ERR_OK != err_code)
+            {
+                MALI_DEBUG_PRINT(4, ("reverting _mali_ukk_attach_mem, as a result of failing put_user(), failed\n"));
+            }
+        }
+        return -EFAULT;
+    }
+
+    /* Return the error that _mali_ukk_map_external_ump_mem produced */
+	return map_errcode(err_code);
+}
+#endif /* MALI_USE_UNIFIED_MEMORY_PROVIDER */
+
+int mem_query_mmu_page_table_dump_size_wrapper(struct mali_session_data *session_data, _mali_uk_query_mmu_page_table_dump_size_s __user * uargs)
+{
+    _mali_uk_query_mmu_page_table_dump_size_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+    MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+    kargs.ctx = session_data;
+
+    err = _mali_ukk_query_mmu_page_table_dump_size(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+    if (0 != put_user(kargs.size, &uargs->size)) return -EFAULT;
+
+    return 0;
+}
+
+int mem_dump_mmu_page_table_wrapper(struct mali_session_data *session_data, _mali_uk_dump_mmu_page_table_s __user * uargs)
+{
+    _mali_uk_dump_mmu_page_table_s kargs;
+    _mali_osk_errcode_t err;
+    void *buffer;
+    int rc = -EFAULT;
+
+	/* validate input */
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+	/* the session_data pointer was validated by caller */
+
+    kargs.buffer = NULL;
+
+    /* get location of user buffer */
+	if (0 != get_user(buffer, &uargs->buffer)) goto err_exit;
+	/* get size of mmu page table info buffer from user space */
+	if ( 0 != get_user(kargs.size, &uargs->size) ) goto err_exit;
+    /* verify we can access the whole of the user buffer */
+    if (!access_ok(VERIFY_WRITE, buffer, kargs.size)) goto err_exit;
+
+    /* allocate temporary buffer (kernel side) to store mmu page table info */
+    kargs.buffer = _mali_osk_malloc(kargs.size);
+    if (NULL == kargs.buffer)
+    {
+        rc = -ENOMEM;
+        goto err_exit;
+    }
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_dump_mmu_page_table(&kargs);
+    if (_MALI_OSK_ERR_OK != err)
+    {
+        rc = map_errcode(err);
+        goto err_exit;
+    }
+
+    /* copy mmu page table info back to user space and update pointers */
+	if (0 != copy_to_user(uargs->buffer, kargs.buffer, kargs.size) ) goto err_exit;
+    if (0 != put_user((kargs.register_writes - (u32 *)kargs.buffer) + (u32 *)uargs->buffer, &uargs->register_writes)) goto err_exit;
+    if (0 != put_user((kargs.page_table_dump - (u32 *)kargs.buffer) + (u32 *)uargs->buffer, &uargs->page_table_dump)) goto err_exit;
+    if (0 != put_user(kargs.register_writes_size, &uargs->register_writes_size)) goto err_exit;
+    if (0 != put_user(kargs.page_table_dump_size, &uargs->page_table_dump_size)) goto err_exit;
+    rc = 0;
+
+err_exit:
+    if (kargs.buffer) _mali_osk_free(kargs.buffer);
+    return rc;
+}
+
+
+
+int mem_get_big_block_wrapper( struct file * filp, _mali_uk_get_big_block_s __user * argument )
+{
+	_mali_uk_get_big_block_s uk_args;
+	_mali_osk_errcode_t err_code;
+
+	/* validate input */
+	/* the session_data pointer was validated by caller */
+    MALI_CHECK_NON_NULL( argument, -EINVAL);
+
+	/* get call arguments from user space. copy_from_user returns how many bytes which where NOT copied */
+	if ( 0 != copy_from_user(&uk_args, (void __user *)argument, sizeof(_mali_uk_get_big_block_s)) )
+	{
+		return -EFAULT;
+	}
+
+	/* This interface inserts something into the ukk_private word */
+    uk_args.ukk_private = (u32)filp;
+    uk_args.ctx = filp->private_data;
+	err_code = _mali_ukk_get_big_block( &uk_args );
+
+	/* Do not leak the private word back into user space */
+	uk_args.ukk_private = 0;
+
+	if ( _MALI_OSK_ERR_OK != err_code )
+	{
+		return map_errcode(err_code);
+	}
+
+	/* From this point on, we must roll-back any failing action to preserve the
+	 * meaning of the U/K interface (e.g. when excluded) */
+
+	/* transfer response back to user space */
+	if ( 0 != copy_to_user(argument, &uk_args, sizeof(_mali_uk_get_big_block_s)) )
+	{
+		/* Roll-back - the _mali_uk_get_big_block call succeeded, so all
+		 * values in uk_args will be correct */
+		_mali_uk_free_big_block_s uk_args_rollback = {0, };
+
+		uk_args_rollback.ctx = uk_args.ctx;
+		uk_args_rollback.cookie = uk_args.cookie;
+		err_code = _mali_ukk_free_big_block( &uk_args_rollback );
+
+		if ( _MALI_OSK_ERR_OK != err_code )
+		{
+			/* error in DEBUG and RELEASE */
+			MALI_PRINT_ERROR( ("Failed to rollback get_big_block: %.8X\n", (u32)err_code) );
+		}
+        return -EFAULT;
+	}
+
+    return 0;
+}
+
+int mem_free_big_block_wrapper(struct mali_session_data *session_data, _mali_uk_free_big_block_s __user * argument)
+{
+	_mali_uk_free_big_block_s uk_args;
+	_mali_osk_errcode_t err_code;
+
+	/* validate input */
+	/* the session_data pointer was validated by caller */
+    MALI_CHECK_NON_NULL( argument, -EINVAL );
+
+	/* get call arguments from user space. get_user returns 0 on success */
+	if ( 0 != get_user(uk_args.cookie, &argument->cookie) )
+	{
+		return -EFAULT;
+	}
+
+    uk_args.ctx = session_data;
+	err_code = _mali_ukk_free_big_block( &uk_args );
+
+	/* Return the error that _mali_ukk_free_big_block produced */
+	return map_errcode(err_code);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_pp.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_pp.c
new file mode 100644
index 00000000000..f77a177d865
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_pp.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include <linux/fs.h>       /* file system operations */
+#include <asm/uaccess.h>    /* user space access */
+
+#include "mali_ukk.h"
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_session_manager.h"
+#include "mali_ukk_wrappers.h"
+
+int pp_start_job_wrapper(struct mali_session_data *session_data, _mali_uk_pp_start_job_s __user *uargs)
+{
+	_mali_uk_pp_start_job_s kargs;
+	_mali_osk_errcode_t err;
+
+	MALI_CHECK_NON_NULL(uargs, -EINVAL);
+	MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+	if (!access_ok(VERIFY_WRITE, uargs, sizeof(_mali_uk_pp_start_job_s)))
+	{
+		return -EFAULT;
+	}
+
+	if (0 != copy_from_user(&kargs, uargs, sizeof(_mali_uk_pp_start_job_s))) return -EFAULT;
+
+	kargs.ctx = session_data;
+	err = _mali_ukk_pp_start_job(&kargs);
+	if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+	if (0 != put_user(kargs.returned_user_job_ptr, &uargs->returned_user_job_ptr) ||
+	    0 != put_user(kargs.status, &uargs->status))
+	{
+		/*
+		 * If this happens, then user space will not know that the job was actually started,
+		 * and if we return a queued job, then user space will still think that one is still queued.
+		 * This will typically lead to a deadlock in user space.
+		 * This could however only happen if user space deliberately passes a user buffer which
+		 * passes the access_ok(VERIFY_WRITE) check, but isn't fully writable at the time of copy_to_user().
+		 * The official Mali driver will never attempt to do that, and kernel space should not be affected.
+		 * That is why we do not bother to do a complex rollback in this very very very rare case.
+		 */
+		return -EFAULT;
+	}
+
+    return 0;
+}
+
+int pp_abort_job_wrapper(struct mali_session_data *session_data, _mali_uk_pp_abort_job_s __user *uargs)
+{
+    _mali_uk_pp_abort_job_s kargs;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+    MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+    if (0 != copy_from_user(&kargs, uargs, sizeof(_mali_uk_pp_abort_job_s))) return -EFAULT;
+
+    kargs.ctx = session_data;
+    _mali_ukk_pp_abort_job(&kargs);
+
+    return 0;
+}
+
+int pp_get_number_of_cores_wrapper(struct mali_session_data *session_data, _mali_uk_get_pp_number_of_cores_s __user *uargs)
+{
+    _mali_uk_get_pp_number_of_cores_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+    MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_get_pp_number_of_cores(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+    if (0 != put_user(kargs.number_of_cores, &uargs->number_of_cores)) return -EFAULT;
+
+    return 0;
+}
+
+int pp_get_core_version_wrapper(struct mali_session_data *session_data, _mali_uk_get_pp_core_version_s __user *uargs)
+{
+    _mali_uk_get_pp_core_version_s kargs;
+    _mali_osk_errcode_t err;
+
+    MALI_CHECK_NON_NULL(uargs, -EINVAL);
+    MALI_CHECK_NON_NULL(session_data, -EINVAL);
+
+    kargs.ctx = session_data;
+    err = _mali_ukk_get_pp_core_version(&kargs);
+    if (_MALI_OSK_ERR_OK != err) return map_errcode(err);
+
+    if (0 != put_user(kargs.version, &uargs->version)) return -EFAULT;
+
+    return 0;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_profiling.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_profiling.c
new file mode 100644
index 00000000000..636bd03580e
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_profiling.c
@@ -0,0 +1,135 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include <linux/fs.h>       /* file system operations */
+#include <asm/uaccess.h>    /* user space access */
+
+#include "mali_ukk.h"
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_session_manager.h"
+#include "mali_ukk_wrappers.h"
+
+int profiling_start_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_start_s __user *uargs)
+{
+	_mali_uk_profiling_start_s kargs;
+	_mali_osk_errcode_t err;
+
+	MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+	if (0 != copy_from_user(&kargs, uargs, sizeof(_mali_uk_profiling_start_s)))
+	{
+		return -EFAULT;
+	}
+
+	kargs.ctx = session_data;
+	err = _mali_ukk_profiling_start(&kargs);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		return map_errcode(err);
+	}
+
+	if (0 != put_user(kargs.limit, &uargs->limit))
+	{
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+int profiling_add_event_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_add_event_s __user *uargs)
+{
+	_mali_uk_profiling_add_event_s kargs;
+	_mali_osk_errcode_t err;
+
+	MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+	if (0 != copy_from_user(&kargs, uargs, sizeof(_mali_uk_profiling_add_event_s)))
+	{
+		return -EFAULT;
+	}
+
+	kargs.ctx = session_data;
+	err = _mali_ukk_profiling_add_event(&kargs);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		return map_errcode(err);
+	}
+
+	return 0;
+}
+
+int profiling_stop_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_stop_s __user *uargs)
+{
+	_mali_uk_profiling_stop_s kargs;
+	_mali_osk_errcode_t err;
+
+	MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+	kargs.ctx = session_data;
+	err = _mali_ukk_profiling_stop(&kargs);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		return map_errcode(err);
+	}
+
+	if (0 != put_user(kargs.count, &uargs->count))
+	{
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+int profiling_get_event_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_get_event_s __user *uargs)
+{
+	_mali_uk_profiling_get_event_s kargs;
+	_mali_osk_errcode_t err;
+
+	MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+	if (0 != get_user(kargs.index, &uargs->index))
+	{
+		return -EFAULT;
+	}
+
+	kargs.ctx = session_data;
+
+	err = _mali_ukk_profiling_get_event(&kargs);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		return map_errcode(err);
+	}
+
+	kargs.ctx = NULL; /* prevent kernel address to be returned to user space */
+	if (0 != copy_to_user(uargs, &kargs, sizeof(_mali_uk_profiling_get_event_s)))
+	{
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+int profiling_clear_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_clear_s __user *uargs)
+{
+	_mali_uk_profiling_clear_s kargs;
+	_mali_osk_errcode_t err;
+
+	MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+	kargs.ctx = session_data;
+	err = _mali_ukk_profiling_clear(&kargs);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		return map_errcode(err);
+	}
+
+	return 0;
+}
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_vsync.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_vsync.c
new file mode 100644
index 00000000000..965ee411535
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_vsync.c
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+#include <linux/fs.h>       /* file system operations */
+#include <asm/uaccess.h>    /* user space access */
+
+#include "mali_ukk.h"
+#include "mali_osk.h"
+#include "mali_kernel_common.h"
+#include "mali_kernel_session_manager.h"
+#include "mali_ukk_wrappers.h"
+
+
+int vsync_event_report_wrapper(struct mali_session_data *session_data, _mali_uk_vsync_event_report_s __user *uargs)
+{
+	_mali_uk_vsync_event_report_s kargs;
+	_mali_osk_errcode_t err;
+
+	MALI_CHECK_NON_NULL(uargs, -EINVAL);
+
+	if (0 != copy_from_user(&kargs, uargs, sizeof(_mali_uk_vsync_event_report_s)))
+	{
+		return -EFAULT;
+	}
+
+	kargs.ctx = session_data;
+	err = _mali_ukk_vsync_event_report(&kargs);
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		return map_errcode(err);
+	}
+
+	return 0;
+}
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_wrappers.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_wrappers.h
new file mode 100644
index 00000000000..54e3f656b37
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/linux/mali_ukk_wrappers.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_ukk_wrappers.h
+ * Defines the wrapper functions for each user-kernel function
+ */
+
+#ifndef __MALI_UKK_WRAPPERS_H__
+#define __MALI_UKK_WRAPPERS_H__
+
+#include "mali_uk_types.h"
+#include "mali_osk.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+int get_system_info_size_wrapper(struct mali_session_data *session_data, _mali_uk_get_system_info_size_s __user *uargs);
+int get_system_info_wrapper(struct mali_session_data *session_data, _mali_uk_get_system_info_s __user *uargs);
+int wait_for_notification_wrapper(struct mali_session_data *session_data, _mali_uk_wait_for_notification_s __user *uargs);
+int get_api_version_wrapper(struct mali_session_data *session_data, _mali_uk_get_api_version_s __user *uargs);
+int post_notification_wrapper(struct mali_session_data *session_data, _mali_uk_post_notification_s __user *uargs);
+int mem_init_wrapper(struct mali_session_data *session_data, _mali_uk_init_mem_s __user *uargs);
+int mem_term_wrapper(struct mali_session_data *session_data, _mali_uk_term_mem_s __user *uargs);
+int mem_map_ext_wrapper(struct mali_session_data *session_data, _mali_uk_map_external_mem_s __user * argument);
+int mem_unmap_ext_wrapper(struct mali_session_data *session_data, _mali_uk_unmap_external_mem_s __user * argument);
+int mem_query_mmu_page_table_dump_size_wrapper(struct mali_session_data *session_data, _mali_uk_query_mmu_page_table_dump_size_s __user * uargs);
+int mem_dump_mmu_page_table_wrapper(struct mali_session_data *session_data, _mali_uk_dump_mmu_page_table_s __user * uargs);
+
+#if MALI_USE_UNIFIED_MEMORY_PROVIDER != 0
+int mem_attach_ump_wrapper(struct mali_session_data *session_data, _mali_uk_attach_ump_mem_s __user * argument);
+int mem_release_ump_wrapper(struct mali_session_data *session_data, _mali_uk_release_ump_mem_s __user * argument);
+#endif /* MALI_USE_UNIFIED_MEMORY_PROVIDER */
+
+int mem_get_big_block_wrapper( struct file * filp, _mali_uk_get_big_block_s __user * argument );
+int mem_free_big_block_wrapper( struct mali_session_data *session_data, _mali_uk_free_big_block_s __user * argument);
+int pp_start_job_wrapper(struct mali_session_data *session_data, _mali_uk_pp_start_job_s __user *uargs);
+int pp_abort_job_wrapper(struct mali_session_data *session_data, _mali_uk_pp_abort_job_s __user *uargs);
+int pp_get_number_of_cores_wrapper(struct mali_session_data *session_data, _mali_uk_get_pp_number_of_cores_s __user *uargs);
+int pp_get_core_version_wrapper(struct mali_session_data *session_data, _mali_uk_get_pp_core_version_s __user *uargs);
+int gp_start_job_wrapper(struct mali_session_data *session_data, _mali_uk_gp_start_job_s __user *uargs);
+int gp_abort_job_wrapper(struct mali_session_data *session_data, _mali_uk_gp_abort_job_s __user *uargs);
+int gp_get_number_of_cores_wrapper(struct mali_session_data *session_data, _mali_uk_get_gp_number_of_cores_s __user *uargs);
+int gp_get_core_version_wrapper(struct mali_session_data *session_data, _mali_uk_get_gp_core_version_s __user *uargs);
+int gp_suspend_response_wrapper(struct mali_session_data *session_data, _mali_uk_gp_suspend_response_s __user *uargs);
+
+int profiling_start_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_start_s __user *uargs);
+int profiling_add_event_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_add_event_s __user *uargs);
+int profiling_stop_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_stop_s __user *uargs);
+int profiling_get_event_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_get_event_s __user *uargs);
+int profiling_clear_wrapper(struct mali_session_data *session_data, _mali_uk_profiling_clear_s __user *uargs);
+
+int vsync_event_report_wrapper(struct mali_session_data *session_data, _mali_uk_vsync_event_report_s __user *uargs);
+
+int map_errcode( _mali_osk_errcode_t err );
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MALI_UKK_WRAPPERS_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/default/mali_platform.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/default/mali_platform.c
new file mode 100644
index 00000000000..36301a93a2c
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/default/mali_platform.c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_platform.c
+ * Platform specific Mali driver functions for a default platform
+ */
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_platform.h"
+
+
+_mali_osk_errcode_t mali_platform_init(_mali_osk_resource_t *resource)
+{
+    MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_platform_deinit(_mali_osk_resource_type_t *type)
+{
+    MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_platform_powerdown(u32 cores)
+{
+    MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_platform_powerup(u32 cores)
+{
+    MALI_SUCCESS;
+}
+
+void mali_gpu_utilization_handler(u32 utilization)
+{
+}
+
+#if MALI_POWER_MGMT_TEST_SUITE
+u32 pmu_get_power_up_down_info(void)
+{
+	return 4095;
+
+}
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/mali-runtimepm/mali_platform.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/mali-runtimepm/mali_platform.c
new file mode 100644
index 00000000000..b8c47094177
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/mali-runtimepm/mali_platform.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_platform.c
+ * Platform specific Mali driver functions for a default platform
+ */
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_platform.h"
+#include "mali_pmm.h"
+
+static int is_run_time = 0;
+_mali_osk_errcode_t mali_platform_init(_mali_osk_resource_t *resource)
+{
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_platform_deinit(_mali_osk_resource_type_t *type)
+{
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_platform_powerdown(u32 cores)
+{
+	if(is_run_time == 1)
+	{
+		_mali_osk_pmm_dev_idle();
+		is_run_time =0;
+	}
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_platform_powerup(u32 cores)
+{
+	if(is_run_time == 0)
+	{
+		_mali_osk_pmm_dev_activate();
+		is_run_time = 1;
+	}
+	MALI_SUCCESS;
+}
+
+void mali_gpu_utilization_handler(u32 utilization)
+{
+}
+
+#if MALI_POWER_MGMT_TEST_SUITE
+u32 pmu_get_power_up_down_info(void)
+{
+	return 4095;
+
+}
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/mali400-pmu/mali_platform.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/mali400-pmu/mali_platform.c
new file mode 100644
index 00000000000..cf95b8ae09e
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/mali400-pmu/mali_platform.c
@@ -0,0 +1,388 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_platform.c
+ * Platform specific Mali driver functions for Mali 400 PMU hardware
+ */
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_platform.h"
+
+#if USING_MALI_PMM
+
+#include "mali_pmm.h"
+
+/* Internal test on/off */
+#define PMU_TEST 0
+
+/** @brief PMU hardware info
+ */
+typedef struct platform_pmu
+{
+	u32 reg_base_addr;              /**< PMU registers base address */
+	u32 reg_size;                   /**< PMU registers size */
+	const char *name;               /**< PMU name */
+	u32 irq_num;                    /**< PMU irq number */
+
+	mali_io_address reg_mapped;     /**< IO-mapped pointer to registers */
+} platform_pmu_t;
+
+static platform_pmu_t *pmu_info = NULL;
+
+/** @brief Register layout for hardware PMU
+ */
+typedef enum {
+	PMU_REG_ADDR_MGMT_POWER_UP                  = 0x00,     /*< Power up register */
+	PMU_REG_ADDR_MGMT_POWER_DOWN                = 0x04,     /*< Power down register */
+	PMU_REG_ADDR_MGMT_STATUS                    = 0x08,     /*< Core sleep status register */
+	PMU_REG_ADDR_MGMT_INT_MASK                  = 0x0C,     /*< Interrupt mask register */
+	PMU_REG_ADDR_MGMT_INT_RAWSTAT               = 0x10,     /*< Interrupt raw status register */
+	PMU_REG_ADDR_MGMT_INT_STAT                  = 0x14,     /*< Interrupt status register */
+	PMU_REG_ADDR_MGMT_INT_CLEAR                 = 0x18,     /*< Interrupt clear register */
+	PMU_REG_ADDR_MGMT_SW_DELAY                  = 0x1C,     /*< Software delay register */
+	PMU_REG_ADDR_MGMT_MASTER_PWR_UP             = 0x24,     /*< Master power up register */
+	PMU_REGISTER_ADDRESS_SPACE_SIZE             = 0x28,     /*< Size of register space */
+} pmu_reg_addr_mgmt_addr;
+
+/* Internal functions */
+u32 pmu_reg_read(platform_pmu_t *pmu, u32 relative_address);
+void pmu_reg_write(platform_pmu_t *pmu, u32 relative_address, u32 new_val);
+mali_pmm_core_mask pmu_translate_cores_to_pmu(mali_pmm_core_mask cores);
+#if PMU_TEST
+void pmm_pmu_dump_regs( platform_pmu_t *pmu );
+void pmm_pmu_test( platform_pmu_t *pmu, u32 cores );
+#endif
+
+#endif /* USING_MALI_PMM */
+
+
+_mali_osk_errcode_t mali_platform_init(_mali_osk_resource_t *resource)
+{
+#if USING_MALI_PMM
+	if( resource == NULL )
+	{
+		/* Nothing to set up for the system */	
+	}
+	else if( resource->type == PMU )
+	{
+		if( (resource->base == 0) ||
+			(resource->description == NULL) )
+		{
+			/* NOTE: We currently don't care about any other resource settings */
+			MALI_PRINT_ERROR(("PLATFORM mali400-pmu: Missing PMU set up information\n"));
+			MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+		}
+
+		MALI_DEBUG_ASSERT( pmu_info == NULL );
+		pmu_info = (platform_pmu_t *)_mali_osk_malloc(sizeof(*pmu_info));
+		MALI_CHECK_NON_NULL( pmu_info, _MALI_OSK_ERR_NOMEM );	
+
+		/* All values get 0 as default */
+		_mali_osk_memset(pmu_info, 0, sizeof(*pmu_info));
+		
+		pmu_info->reg_base_addr = resource->base;
+		pmu_info->reg_size = (u32)PMU_REGISTER_ADDRESS_SPACE_SIZE;
+		pmu_info->name = resource->description; 
+		pmu_info->irq_num = resource->irq;
+
+		if( _MALI_OSK_ERR_OK != _mali_osk_mem_reqregion(pmu_info->reg_base_addr, pmu_info->reg_size, pmu_info->name) )
+		{
+			MALI_PRINT_ERROR(("PLATFORM mali400-pmu: Could not request register region (0x%08X - 0x%08X) for %s\n",
+					 pmu_info->reg_base_addr, pmu_info->reg_base_addr + pmu_info->reg_size - 1, pmu_info->name));
+			goto cleanup;
+		}
+		else
+		{
+			MALI_DEBUG_PRINT( 4, ("PLATFORM mali400-pmu: Success: request_mem_region: (0x%08X - 0x%08X) for %s\n",
+					 pmu_info->reg_base_addr, pmu_info->reg_base_addr + pmu_info->reg_size - 1, pmu_info->name));
+		}
+
+		pmu_info->reg_mapped = _mali_osk_mem_mapioregion( pmu_info->reg_base_addr, pmu_info->reg_size, pmu_info->name );
+
+		if( 0 == pmu_info->reg_mapped )
+		{
+			MALI_PRINT_ERROR(("PLATFORM mali400-pmu: Could not ioremap registers for %s .\n", pmu_info->name));
+			_mali_osk_mem_unreqregion( pmu_info->reg_base_addr, pmu_info->reg_size );
+			goto cleanup;
+		}
+		else
+		{
+			MALI_DEBUG_PRINT( 4, ("PLATFORM mali400-pmu: Success: ioremap_nocache: Internal ptr: (0x%08X - 0x%08X) for %s\n",
+					(u32) pmu_info->reg_mapped,
+					((u32)pmu_info->reg_mapped)+ pmu_info->reg_size - 1,
+					pmu_info->name));
+		}
+
+		MALI_DEBUG_PRINT( 4, ("PLATFORM mali400-pmu: Success: Mapping registers to %s\n", pmu_info->name));
+
+#if PMU_TEST
+		pmu_test(pmu_info, (MALI_PMM_CORE_GP));
+		pmu_test(pmu_info, (MALI_PMM_CORE_GP|MALI_PMM_CORE_L2|MALI_PMM_CORE_PP0));
+#endif
+
+		MALI_DEBUG_PRINT( 4, ("PLATFORM mali400-pmu: Initialized - %s\n", pmu_info->name) );		
+	}
+	else
+	{
+		/* Didn't expect a different resource */
+		MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}	
+
+	MALI_SUCCESS;
+	
+cleanup:
+	_mali_osk_free(pmu_info);
+	pmu_info = NULL;
+	MALI_ERROR(_MALI_OSK_ERR_NOMEM);
+
+#else
+	/* Nothing to do when not using PMM - as mali already on */
+	MALI_SUCCESS;
+#endif
+
+}
+
+_mali_osk_errcode_t mali_platform_deinit(_mali_osk_resource_type_t *type)
+{
+#if USING_MALI_PMM
+	if( type == NULL )
+	{
+		/* Nothing to tear down for the system */
+	}	
+	else if (*type == PMU)
+	{
+		if( pmu_info )
+		{
+			_mali_osk_mem_unmapioregion(pmu_info->reg_base_addr, pmu_info->reg_size, pmu_info->reg_mapped);
+			_mali_osk_mem_unreqregion(pmu_info->reg_base_addr, pmu_info->reg_size);
+			_mali_osk_free(pmu_info);
+			pmu_info = NULL;
+
+			MALI_DEBUG_PRINT( 4, ("PLATFORM mali400-pmu: Terminated PMU\n") );
+		}
+	}
+	else
+	{
+		/* Didn't expect a different resource */
+		MALI_ERROR(_MALI_OSK_ERR_INVALID_ARGS);
+	}	
+		
+	MALI_SUCCESS;
+
+#else
+	/* Nothing to do when not using PMM */
+	MALI_SUCCESS;
+#endif
+}
+
+_mali_osk_errcode_t mali_platform_powerdown(u32 cores)
+{
+#if USING_MALI_PMM
+	u32 stat;
+	u32 timeout;
+	u32 cores_pmu;
+
+	MALI_DEBUG_ASSERT_POINTER(pmu_info);
+	MALI_DEBUG_ASSERT( cores != 0 ); /* Shouldn't receive zero from PMM */
+	MALI_DEBUG_PRINT( 4, ("PLATFORM mali400-pmu: power down (0x%x)\n", cores) );
+
+	cores_pmu = pmu_translate_cores_to_pmu(cores);
+	pmu_reg_write( pmu_info, (u32)PMU_REG_ADDR_MGMT_POWER_DOWN, cores_pmu );
+
+	/* Wait for cores to be powered down */
+	timeout = 10; /* 10ms */ 
+	do
+	{
+		/* Get status of sleeping cores */
+		stat = pmu_reg_read( pmu_info, (u32)PMU_REG_ADDR_MGMT_STATUS );
+		stat &= cores_pmu;
+		if( stat == cores_pmu ) break; /* All cores we wanted are now asleep */
+		_mali_osk_time_ubusydelay(1000); /* 1ms */
+		timeout--;
+	} while( timeout > 0 );
+
+	if( timeout == 0 ) MALI_ERROR(_MALI_OSK_ERR_TIMEOUT);
+
+	MALI_SUCCESS;
+
+#else
+	/* Nothing to do when not using PMM */
+	MALI_SUCCESS;
+#endif
+}
+
+_mali_osk_errcode_t mali_platform_powerup(u32 cores)
+{
+#if USING_MALI_PMM
+	u32 cores_pmu;
+	u32 stat;
+	u32 timeout;
+	
+	MALI_DEBUG_ASSERT_POINTER(pmu_info);
+	MALI_DEBUG_ASSERT( cores != 0 ); /* Shouldn't receive zero from PMM */
+	MALI_DEBUG_PRINT( 4, ("PLATFORM mali400-pmu: power up (0x%x)\n", cores) );
+
+	/* Don't use interrupts - just poll status */
+	pmu_reg_write( pmu_info, (u32)PMU_REG_ADDR_MGMT_INT_MASK, 0 );
+	cores_pmu = pmu_translate_cores_to_pmu(cores);
+	pmu_reg_write( pmu_info, (u32)PMU_REG_ADDR_MGMT_POWER_UP, cores_pmu );
+
+	timeout = 10; /* 10ms */ 
+	do
+	{
+		/* Get status of sleeping cores */
+		stat = pmu_reg_read( pmu_info, (u32)PMU_REG_ADDR_MGMT_STATUS );
+		stat &= cores_pmu;
+		if( stat == 0 ) break; /* All cores we wanted are now awake */
+		_mali_osk_time_ubusydelay(1000); /* 1ms */
+		timeout--;
+	} while( timeout > 0 );
+
+	if( timeout == 0 ) MALI_ERROR(_MALI_OSK_ERR_TIMEOUT);
+
+	MALI_SUCCESS;
+
+#else
+	/* Nothing to do when not using PMM */
+	MALI_SUCCESS;
+#endif
+}
+
+void mali_gpu_utilization_handler(u32 utilization)
+{
+}
+
+#if USING_MALI_PMM
+
+/***** INTERNAL *****/
+
+/** @brief Internal PMU function to translate the cores bit mask
+ *         into something the hardware PMU understands
+ *
+ * @param cores PMM cores bitmask
+ * @return PMU hardware cores bitmask
+ */
+u32 pmu_translate_cores_to_pmu(mali_pmm_core_mask cores)
+{
+	/* For Mali 400 PMU the cores mask is already the same as what
+	 * the hardware PMU expects.
+	 * For other hardware, some translation can be done here, by
+	 * translating the MALI_PMM_CORE_* bits into specific hardware
+	 * bits
+	 */
+	 return cores;
+}
+
+/** @brief Internal PMU function to read a PMU register
+ *
+ * @param pmu handle that identifies the PMU hardware
+ * @param relative_address relative PMU hardware address to read from
+ * @return 32-bit value that was read from the address
+ */
+u32 pmu_reg_read(platform_pmu_t *pmu, u32 relative_address)
+{
+	u32 read_val;
+
+	MALI_DEBUG_ASSERT_POINTER(pmu);
+	MALI_DEBUG_ASSERT((relative_address & 0x03) == 0);
+	MALI_DEBUG_ASSERT(relative_address < pmu->reg_size);
+
+	read_val = _mali_osk_mem_ioread32(pmu->reg_mapped, relative_address);
+
+	MALI_DEBUG_PRINT( 5, ("PMU: reg_read: %s Addr:0x%04X Val:0x%08x\n",
+			pmu->name, relative_address, read_val));
+
+	return read_val;
+}
+
+/** @brief Internal PMU function to write to a PMU register
+ *
+ * @param pmu handle that identifies the PMU hardware
+ * @param relative_address relative PMU hardware address to write to
+ * @param new_val new 32-bit value to write into the address
+ */
+void pmu_reg_write(platform_pmu_t *pmu, u32 relative_address, u32 new_val)
+{
+	MALI_DEBUG_ASSERT_POINTER(pmu);
+	MALI_DEBUG_ASSERT((relative_address & 0x03) == 0);
+	MALI_DEBUG_ASSERT(relative_address < pmu->reg_size);
+
+	MALI_DEBUG_PRINT( 5, ("PMU: reg_write: %s Addr:0x%04X Val:0x%08x\n",
+			pmu->name, relative_address, new_val));
+
+	_mali_osk_mem_iowrite32(pmu->reg_mapped, relative_address, new_val);
+}
+
+#if MALI_POWER_MGMT_TEST_SUITE
+
+u32 pmu_get_power_up_down_info(void)
+{
+	return pmu_reg_read(pmu_info, (u32)PMU_REG_ADDR_MGMT_STATUS);
+}
+
+#endif /* MALI_POWER_MGMT_TEST_SUITE */
+
+#endif /* USING_MALI_PMM */
+
+
+#if USING_MALI_PMM && PMU_TEST
+
+/***** TEST *****/
+
+void pmu_dump_regs( platform_pmu_t *pmu )
+{
+	u32 addr;
+	for( addr = 0x0; addr < PMU_REGISTER_ADDRESS_SPACE_SIZE; addr += 0x4 )
+	{
+		MALI_PRINT( ("PMU_REG: 0x%08x: 0x%04x\n", (addr + pmu->reg_base_addr), pmu_reg_read( pmu, addr ) ) );
+	}
+}
+
+/* This function is an internal test for the PMU without any Mali h/w interaction */
+void pmu_test( platform_pmu_t *pmu, u32 cores )
+{
+	u32 stat;
+	u32 timeout;
+	
+	MALI_PRINT( ("PMU_TEST: Start\n") );
+	
+	pmu_dump_regs( pmu );
+	
+	MALI_PRINT( ("PMU_TEST: Power down cores: 0x%x\n", cores) );
+	_mali_pmm_pmu_power_down( pmu, cores, MALI_TRUE );
+	
+	stat = pmu_reg_read( pmu, (u32)PMU_REG_ADDR_MGMT_STATUS );
+	MALI_PRINT( ("PMU_TEST: %s\n", (stat & cores) == cores ? "SUCCESS" : "FAIL" ) );
+	
+	pmu_dump_regs( pmu );
+	
+	MALI_PRINT( ("PMU_TEST: Power up cores: 0x%x\n", cores) );
+	_mali_pmm_pmu_power_up( pmu, cores, MALI_FALSE );
+	
+	MALI_PRINT( ("PMU_TEST: Waiting for power up...\n") );
+	timeout = 1000; /* 1 sec */
+	while( !_mali_pmm_pmu_irq_power_up(pmu) && timeout > 0 )
+	{
+		_mali_osk_time_ubusydelay(1000); /* 1ms */
+		timeout--;		
+	} 
+
+	MALI_PRINT( ("PMU_TEST: Waited %dms for interrupt\n", (1000-timeout)) );
+	stat = pmu_reg_read( pmu, (u32)PMU_REG_ADDR_MGMT_STATUS );
+	MALI_PRINT( ("PMU_TEST: %s\n", (stat & cores) == 0 ? "SUCCESS" : "FAIL" ) );
+
+	_mali_pmm_pmu_irq_power_up_clear(pmu);
+
+	pmu_dump_regs( pmu );
+
+	MALI_PRINT( ("PMU_TEST: Finish\n") );
+}
+#endif /* USING_MALI_PMM && PMU_TEST */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/mali_platform.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/mali_platform.h
new file mode 100644
index 00000000000..575c1fb6113
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/mali_platform.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_platform.h
+ * Platform specific Mali driver functions
+ */
+
+#include "mali_osk.h"
+
+#if USING_MALI_PMM
+#include "mali_pmm.h"
+#endif
+
+#if !USING_MALI_PMM
+/* @brief System power up/down cores that can be passed into mali_platform_powerdown/up() */
+#define MALI_PLATFORM_SYSTEM  0
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** @brief Platform specific setup and initialisation of MALI
+ * 
+ * This is called from the entrypoint of the driver to initialize the platform
+ * When using PMM, it is also called from the PMM start up to initialise the 
+ * system PMU
+ *
+ * @param resource This is NULL when called on first driver start up, else it will
+ * be a pointer to a PMU resource
+ * @return _MALI_OSK_ERR_OK on success otherwise, a suitable _mali_osk_errcode_t error.
+ */
+_mali_osk_errcode_t mali_platform_init(_mali_osk_resource_t *resource);
+
+/** @brief Platform specific deinitialisation of MALI
+ * 
+ * This is called on the exit of the driver to terminate the platform
+ * When using PMM, it is also called from the PMM termination code to clean up the
+ * system PMU
+ *
+ * @param type This is NULL when called on driver exit, else it will
+ * be a pointer to a PMU resource type (not the full resource)
+ * @return _MALI_OSK_ERR_OK on success otherwise, a suitable _mali_osk_errcode_t error.
+ */
+_mali_osk_errcode_t mali_platform_deinit(_mali_osk_resource_type_t *type);
+
+/** @brief Platform specific powerdown sequence of MALI
+ * 
+ * Called as part of platform init if there is no PMM support, else the
+ * PMM will call it.
+ *
+ * @param cores This is MALI_PLATFORM_SYSTEM when called without PMM, else it will
+ * be a mask of cores to power down based on the mali_pmm_core_id enum
+ * @return _MALI_OSK_ERR_OK on success otherwise, a suitable _mali_osk_errcode_t error.
+ */
+_mali_osk_errcode_t mali_platform_powerdown(u32 cores);
+
+/** @brief Platform specific powerup sequence of MALI
+ * 
+ * Called as part of platform deinit if there is no PMM support, else the
+ * PMM will call it.
+ *
+ * @param cores This is MALI_PLATFORM_SYSTEM when called without PMM, else it will
+ * be a mask of cores to power down based on the mali_pmm_core_id enum
+ * @return _MALI_OSK_ERR_OK on success otherwise, a suitable _mali_osk_errcode_t error.
+ */
+_mali_osk_errcode_t mali_platform_powerup(u32 cores);
+
+/** @brief Platform specific handling of GPU utilization data
+ *
+ * When GPU utilization data is enabled, this function will be
+ * periodically called.
+ *
+ * @param utilization The workload utilization of the Mali GPU. 0 = no utilization, 256 = full utilization.
+ */
+void mali_gpu_utilization_handler(u32 utilization);
+
+#if USING_MALI_PMM
+#if MALI_POWER_MGMT_TEST_SUITE
+/** @brief function to get status of individual cores
+ *
+ * This function is used by power management test suite to get the status of powered up/down the number
+ * of cores
+ * @param utilization The workload utilization of the Mali GPU. 0 = no utilization, 256 = full utilization.
+ */
+u32 pmu_get_power_up_down_info(void);
+#endif
+#endif
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/ux500/mali_platform.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/ux500/mali_platform.c
new file mode 100644
index 00000000000..481e6e76c54
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/ux500/mali_platform.c
@@ -0,0 +1,190 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Mali related Ux500 platform initialization
+ *
+ * Author: Marta Lofstedt <marta.lofstedt@stericsson.com> for
+ * ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+/**
+ * @file mali_platform.c
+ * Platform specific Mali driver functions for ST-Ericsson's Ux500 platforms
+ */
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_platform.h"
+
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/workqueue.h>
+
+#include <linux/mfd/dbx500-prcmu.h>
+
+#define MALI_HIGH_TO_LOW_LEVEL_UTILIZATION_LIMIT 64
+#define MALI_LOW_TO_HIGH_LEVEL_UTILIZATION_LIMIT 192
+
+static int is_running;
+static struct regulator *regulator;
+static struct clk *clk_sga;
+static u32 last_utilization;
+static struct work_struct mali_utilization_work;
+static struct workqueue_struct *mali_utilization_workqueue;
+
+/* Rationale behind the values for:
+* MALI_HIGH_LEVEL_UTILIZATION_LIMIT and MALI_LOW_LEVEL_UTILIZATION_LIMIT
+* When operating at half clock frequency a faster clock is requested when
+* reaching 75% utilization. When operating at full clock frequency a slower
+* clock is requested when reaching 25% utilization. There is a margin of 25%
+* at the high range of the slow clock to avoid complete saturation of the
+* hardware and there is some overlap to avoid an oscillating situation where
+* the clock goes back and forth from high to low.
+*
+* Utilization on full speed clock
+* 0               64             128             192              255
+* |---------------|---------------|---------------|---------------|
+*                 XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
+*                 |       ^
+*                 V       |
+* XXXXXXXXXXXXXXXXXXXXXXXXX
+* 0       64     128     192      255
+* |-------|-------|-------|-------|
+* Utilization on half speed clock
+*/
+void mali_utilization_function(struct work_struct *ptr)
+{
+	/*By default, platform start with 50% APE OPP and 25% DDR OPP*/
+	static u32 has_requested_low = 1;
+
+	if (last_utilization > MALI_LOW_TO_HIGH_LEVEL_UTILIZATION_LIMIT) {
+		if (has_requested_low) {
+			MALI_DEBUG_PRINT(5, ("MALI GPU utilization: %u SIGNAL_HIGH\n", last_utilization));
+			/*Request 100% APE_OPP.*/
+			if (prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP, "mali", 100)) {
+				MALI_DEBUG_PRINT(2, ("MALI 100% APE_OPP failed\n"));
+				return;
+			}
+			/*
+			* Since the utilization values will be reported higher
+			* if DDR_OPP is lowered, we also request 100% DDR_OPP.
+			*/
+			if (prcmu_qos_add_requirement(PRCMU_QOS_DDR_OPP, "mali", 100)) {
+				MALI_DEBUG_PRINT(2, ("MALI 100% DDR_OPP failed\n"));
+				return;
+			}
+			has_requested_low = 0;
+		}
+	} else {
+		if (last_utilization < MALI_HIGH_TO_LOW_LEVEL_UTILIZATION_LIMIT) {
+			if (!has_requested_low) {
+				/*Remove APE_OPP and DDR_OPP requests*/
+				prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP, "mali");
+				prcmu_qos_remove_requirement(PRCMU_QOS_DDR_OPP, "mali");
+				MALI_DEBUG_PRINT(5, ("MALI GPU utilization: %u SIGNAL_LOW\n", last_utilization));
+				has_requested_low = 1;
+			}
+		}
+	}
+	MALI_DEBUG_PRINT(5, ("MALI GPU utilization: %u\n", last_utilization));
+}
+
+_mali_osk_errcode_t mali_platform_init(_mali_osk_resource_t *resource)
+{
+	is_running = 0;
+	last_utilization = 0;
+
+	mali_utilization_workqueue = create_singlethread_workqueue("mali_utilization_workqueue");
+	if (NULL == mali_utilization_workqueue) {
+		MALI_DEBUG_PRINT(2, ("%s: Failed to setup workqueue %s\n", __func__, "v-mali"));
+		goto error;
+	}
+	INIT_WORK(&mali_utilization_work, mali_utilization_function);
+
+	regulator = regulator_get(NULL, "v-mali");
+	if (IS_ERR(regulator)) {
+		MALI_DEBUG_PRINT(2, ("%s: Failed to get regulator %s\n", __func__, "v-mali"));
+		goto error;
+	}
+
+	clk_sga = clk_get_sys("mali", NULL);
+	if (IS_ERR(clk_sga)) {
+		regulator_put(regulator);
+		MALI_DEBUG_PRINT(2, ("%s: Failed to get clock %s\n", __func__, "mali"));
+		goto error;
+	}
+
+	MALI_SUCCESS;
+error:
+	MALI_DEBUG_PRINT(1, ("SGA initialization failed.\n"));
+	MALI_ERROR(_MALI_OSK_ERR_FAULT);
+}
+
+_mali_osk_errcode_t mali_platform_deinit(_mali_osk_resource_type_t *type)
+{
+	destroy_workqueue(mali_utilization_workqueue);
+	regulator_put(regulator);
+	clk_put(clk_sga);
+	MALI_DEBUG_PRINT(2, ("SGA terminated.\n"));
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_platform_powerdown(u32 cores)
+{
+	if (is_running) {
+		clk_disable(clk_sga);
+		if (regulator) {
+			int ret = regulator_disable(regulator);
+			if (ret < 0) {
+				MALI_DEBUG_PRINT(2, ("%s: Failed to disable regulator %s\n", __func__, "v-mali"));
+				is_running = 0;
+				MALI_ERROR(_MALI_OSK_ERR_FAULT);
+			}
+		}
+		is_running = 0;
+	}
+	MALI_DEBUG_PRINT(4, ("mali_platform_powerdown is_running: %u cores: %u\n", is_running, cores));
+	MALI_SUCCESS;
+}
+
+_mali_osk_errcode_t mali_platform_powerup(u32 cores)
+{
+	if (!is_running) {
+		int ret = regulator_enable(regulator);
+		if (ret < 0) {
+			MALI_DEBUG_PRINT(2, ("%s: Failed to enable regulator %s\n", __func__, "v-mali"));
+			goto error;
+		}
+
+		ret = clk_enable(clk_sga);
+		if (ret < 0) {
+			regulator_disable(regulator);
+			MALI_DEBUG_PRINT(2, ("%s: Failed to enable clock %s\n", __func__, "mali"));
+			goto error;
+		}
+		is_running = 1;
+	}
+	MALI_DEBUG_PRINT(4, ("mali_platform_powerup is_running:%u cores: %u\n", is_running, cores));
+	MALI_SUCCESS;
+error:
+	MALI_DEBUG_PRINT(1, ("Failed to power up.\n"));
+	MALI_ERROR(_MALI_OSK_ERR_FAULT);
+}
+
+void mali_gpu_utilization_handler(u32 utilization)
+{
+	last_utilization = utilization;
+	/*
+	* We should not cancel the potentially not yet run old work
+	* in favor of a new work.
+	* Since the utilization value will change,
+	* the mali_utilization_function will evaluate based on
+	* what is the utilization now and not on what it was
+	* when it was scheduled.
+	*/
+	queue_work(mali_utilization_workqueue, &mali_utilization_work);
+}
+
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/ux500/ump_kernel_api_hwmem.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/ux500/ump_kernel_api_hwmem.c
new file mode 100644
index 00000000000..b321b504839
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/platform/ux500/ump_kernel_api_hwmem.c
@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Magnus Wendt <magnus.wendt@stericsson.com> for
+ * ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include "ump_kernel_types.h"
+#include "mali_kernel_common.h"
+
+#include <linux/hwmem.h>
+#include <linux/err.h>
+
+
+/* The UMP kernel API for hwmem has been mapped so that
+ * ump_dd_handle == hwmem_alloc
+ * ump_secure_id == hwmem global name
+ *
+ * The current implementation is limited to contiguous memory
+ */
+
+ump_secure_id ump_dd_secure_id_get(ump_dd_handle memh)
+{
+	int hwmem_name = hwmem_get_name((struct hwmem_alloc *) memh);
+
+	if (unlikely(hwmem_name < 0)) {
+		MALI_DEBUG_PRINT(1, ("%s: Invalid Alloc 0x%x\n",__func__, memh));
+		return UMP_INVALID_SECURE_ID;
+	}
+
+	return (ump_secure_id)hwmem_name;
+}
+
+
+
+ump_dd_handle ump_dd_handle_create_from_secure_id(ump_secure_id secure_id)
+{
+	struct hwmem_alloc *alloc;
+	enum hwmem_mem_type mem_type;
+	enum hwmem_access access;
+
+	alloc = hwmem_resolve_by_name((int) secure_id);
+
+	if (IS_ERR(alloc)) {
+		MALI_DEBUG_PRINT(1, ("%s: Invalid UMP id %d\n",__func__, secure_id));
+		return UMP_DD_HANDLE_INVALID;
+	}
+
+	hwmem_get_info(alloc, NULL, &mem_type, &access);
+
+	if (unlikely((access & (HWMEM_ACCESS_READ | HWMEM_ACCESS_WRITE | HWMEM_ACCESS_IMPORT)) !=
+	                       (HWMEM_ACCESS_READ | HWMEM_ACCESS_WRITE | HWMEM_ACCESS_IMPORT))) {
+		MALI_DEBUG_PRINT(1, ("%s: Access denied on UMP id %d, (access==%d)\n",
+			__func__, secure_id, access));
+		hwmem_release(alloc);
+		return UMP_DD_HANDLE_INVALID;
+	}
+
+	if (unlikely(HWMEM_MEM_CONTIGUOUS_SYS != mem_type)) {
+		MALI_DEBUG_PRINT(1, ("%s: UMP id %d is non-contiguous! (not supported)\n",
+			__func__, secure_id));
+		hwmem_release(alloc);
+		return UMP_DD_HANDLE_INVALID;
+	}
+
+	return (ump_dd_handle)alloc;
+}
+
+
+
+unsigned long ump_dd_phys_block_count_get(ump_dd_handle memh)
+{
+	return 1;
+}
+
+
+
+ump_dd_status_code ump_dd_phys_blocks_get(ump_dd_handle memh,
+                                          ump_dd_physical_block * blocks,
+                                          unsigned long num_blocks)
+{
+	struct hwmem_mem_chunk hwmem_mem_chunk;
+	size_t hwmem_mem_chunk_length = 1;
+
+	int hwmem_result;
+	struct hwmem_alloc *alloc = (struct hwmem_alloc *)memh;
+
+	if (unlikely(blocks == NULL)) {
+		MALI_DEBUG_PRINT(1, ("%s: blocks == NULL\n",__func__));
+		return UMP_DD_INVALID;
+	}
+
+	if (unlikely(1 != num_blocks)) {
+		MALI_DEBUG_PRINT(1, ("%s: num_blocks == %d (!= 1)\n",__func__, num_blocks));
+		return UMP_DD_INVALID;
+	}
+
+	MALI_DEBUG_PRINT(5, ("Returning physical block information. Alloc: 0x%x\n", memh));
+
+	/* It might not look natural to pin here, but it matches the usage by the mali kernel module */
+	hwmem_result = hwmem_pin(alloc, &hwmem_mem_chunk, &hwmem_mem_chunk_length);
+
+	if (unlikely(hwmem_result < 0)) {
+		MALI_DEBUG_PRINT(1, ("%s: Pin failed. Alloc: 0x%x\n",__func__, memh));
+		return UMP_DD_INVALID;
+	}
+
+	blocks[0].addr = hwmem_mem_chunk.paddr;
+	blocks[0].size = hwmem_mem_chunk.size;
+
+	hwmem_set_domain(alloc, HWMEM_ACCESS_READ | HWMEM_ACCESS_WRITE,
+		HWMEM_DOMAIN_SYNC, NULL);
+
+	return UMP_DD_SUCCESS;
+}
+
+
+
+ump_dd_status_code ump_dd_phys_block_get(ump_dd_handle memh,
+                                         unsigned long index,
+                                         ump_dd_physical_block * block)
+{
+	if (unlikely(0 != index))	{
+		MALI_DEBUG_PRINT(1, ("%s: index == %d (!= 0)\n",__func__, index));
+		return UMP_DD_INVALID;
+	}
+	return ump_dd_phys_blocks_get(memh, block, 1);
+}
+
+
+
+unsigned long ump_dd_size_get(ump_dd_handle memh)
+{
+	struct hwmem_alloc *alloc = (struct hwmem_alloc *)memh;
+	int size;
+
+	hwmem_get_info(alloc, &size, NULL, NULL);
+
+	return size;
+}
+
+
+
+void ump_dd_reference_add(ump_dd_handle memh)
+{
+	/* This is never called from tha mali kernel driver */
+}
+
+
+
+void ump_dd_reference_release(ump_dd_handle memh)
+{
+	struct hwmem_alloc *alloc = (struct hwmem_alloc *)memh;
+
+	hwmem_unpin(alloc);
+	hwmem_release(alloc);
+
+	return;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/readme.txt b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/readme.txt
new file mode 100644
index 00000000000..af40fddd20f
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/readme.txt
@@ -0,0 +1,28 @@
+Building the Mali Device Driver for Linux
+-----------------------------------------
+
+Build the Mali Device Driver for Linux by running the following make command:
+
+KDIR=<kdir_path> USING_MMU=<mmu_option> USING_UMP=<ump_option> \
+BUILD=<build_option> CONFIG=<your_config> make
+
+where
+    kdir_path: Path to your Linux Kernel directory
+    mmu_option: 1 = Mali MMU(s) on
+                0 = Mali MMU(s) off
+    ump_option: 1 = Enable UMP support(*)
+                0 = disable UMP support
+    build_option: debug = debug build of driver
+                  release = release build of driver
+    your_config: Name of the sub-folder to find the required config.h(**) file
+                 ("arch-" will be prepended)
+
+(*)  For newer Linux Kernels, the Module.symvers file for the UMP device driver
+     must be available. The UMP_SYMVERS_FILE variable in the Makefile should
+     point to this file. This file is generated when the UMP driver is built.
+
+(**) The config.h file contains the configuration parameters needed, like where the
+     Mali GPU is located, interrupts it uses, memory and so on.
+
+The result will be a mali.ko file, which can be loaded into the Linux kernel
+by using the insmod command.
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/regs/mali_200_regs.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/regs/mali_200_regs.h
new file mode 100644
index 00000000000..6b30802bb2d
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/regs/mali_200_regs.h
@@ -0,0 +1,170 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef _MALI200_REGS_H_
+#define _MALI200_REGS_H_
+
+/**
+ *  Enum for management register addresses.
+ */
+enum mali200_mgmt_reg
+{
+	MALI200_REG_ADDR_MGMT_VERSION                              = 0x1000,
+	MALI200_REG_ADDR_MGMT_CURRENT_REND_LIST_ADDR               = 0x1004,
+	MALI200_REG_ADDR_MGMT_STATUS                               = 0x1008,
+	MALI200_REG_ADDR_MGMT_CTRL_MGMT                            = 0x100c,
+
+	MALI200_REG_ADDR_MGMT_INT_RAWSTAT                          = 0x1020,
+	MALI200_REG_ADDR_MGMT_INT_CLEAR                            = 0x1024,
+	MALI200_REG_ADDR_MGMT_INT_MASK                             = 0x1028,
+	MALI200_REG_ADDR_MGMT_INT_STATUS                           = 0x102c,
+
+	MALI200_REG_ADDR_MGMT_WRITE_BOUNDARY_LOW                   = 0x1044,
+
+	MALI200_REG_ADDR_MGMT_BUS_ERROR_STATUS                     = 0x1050,
+
+	MALI200_REG_ADDR_MGMT_PERF_CNT_0_ENABLE                    = 0x1080,
+	MALI200_REG_ADDR_MGMT_PERF_CNT_0_SRC                       = 0x1084,
+	MALI200_REG_ADDR_MGMT_PERF_CNT_0_VALUE                     = 0x108c,
+
+	MALI200_REG_ADDR_MGMT_PERF_CNT_1_ENABLE                    = 0x10a0,
+	MALI200_REG_ADDR_MGMT_PERF_CNT_1_SRC                       = 0x10a4,
+	MALI200_REG_ADDR_MGMT_PERF_CNT_1_VALUE                     = 0x10ac,
+
+	MALI200_REG_SIZEOF_REGISTER_BANK                           = 0x10f0
+
+};
+
+#define MALI200_REG_VAL_PERF_CNT_ENABLE 1
+
+enum mali200_mgmt_ctrl_mgmt {
+	MALI200_REG_VAL_CTRL_MGMT_STOP_BUS         = (1<<0),
+#if defined(USING_MALI200)
+	MALI200_REG_VAL_CTRL_MGMT_FLUSH_CACHES     = (1<<3),
+#endif
+	MALI200_REG_VAL_CTRL_MGMT_FORCE_RESET      = (1<<5),
+	MALI200_REG_VAL_CTRL_MGMT_START_RENDERING  = (1<<6),
+#if defined(USING_MALI400)
+	MALI400PP_REG_VAL_CTRL_MGMT_SOFT_RESET     = (1<<7),
+#endif
+};
+
+enum mali200_mgmt_irq {
+	MALI200_REG_VAL_IRQ_END_OF_FRAME          = (1<<0),
+	MALI200_REG_VAL_IRQ_END_OF_TILE           = (1<<1),
+	MALI200_REG_VAL_IRQ_HANG                  = (1<<2),
+	MALI200_REG_VAL_IRQ_FORCE_HANG            = (1<<3),
+	MALI200_REG_VAL_IRQ_BUS_ERROR             = (1<<4),
+	MALI200_REG_VAL_IRQ_BUS_STOP              = (1<<5),
+	MALI200_REG_VAL_IRQ_CNT_0_LIMIT           = (1<<6),
+	MALI200_REG_VAL_IRQ_CNT_1_LIMIT           = (1<<7),
+	MALI200_REG_VAL_IRQ_WRITE_BOUNDARY_ERROR  = (1<<8),
+	MALI400PP_REG_VAL_IRQ_INVALID_PLIST_COMMAND = (1<<9),
+	MALI400PP_REG_VAL_IRQ_CALL_STACK_UNDERFLOW  = (1<<10),
+	MALI400PP_REG_VAL_IRQ_CALL_STACK_OVERFLOW   = (1<<11),
+	MALI400PP_REG_VAL_IRQ_RESET_COMPLETED       = (1<<12),
+};
+
+#if defined USING_MALI200
+#define MALI200_REG_VAL_IRQ_MASK_ALL  ((enum mali200_mgmt_irq) (\
+    MALI200_REG_VAL_IRQ_END_OF_FRAME                           |\
+    MALI200_REG_VAL_IRQ_END_OF_TILE                            |\
+    MALI200_REG_VAL_IRQ_HANG                                   |\
+    MALI200_REG_VAL_IRQ_FORCE_HANG                             |\
+    MALI200_REG_VAL_IRQ_BUS_ERROR                              |\
+    MALI200_REG_VAL_IRQ_BUS_STOP                               |\
+    MALI200_REG_VAL_IRQ_CNT_0_LIMIT                            |\
+    MALI200_REG_VAL_IRQ_CNT_1_LIMIT                            |\
+    MALI200_REG_VAL_IRQ_WRITE_BOUNDARY_ERROR))
+#elif defined USING_MALI400
+#define MALI200_REG_VAL_IRQ_MASK_ALL  ((enum mali200_mgmt_irq) (\
+    MALI200_REG_VAL_IRQ_END_OF_FRAME                           |\
+    MALI200_REG_VAL_IRQ_END_OF_TILE                            |\
+    MALI200_REG_VAL_IRQ_HANG                                   |\
+    MALI200_REG_VAL_IRQ_FORCE_HANG                             |\
+    MALI200_REG_VAL_IRQ_BUS_ERROR                              |\
+    MALI200_REG_VAL_IRQ_BUS_STOP                               |\
+    MALI200_REG_VAL_IRQ_CNT_0_LIMIT                            |\
+    MALI200_REG_VAL_IRQ_CNT_1_LIMIT                            |\
+    MALI200_REG_VAL_IRQ_WRITE_BOUNDARY_ERROR                   |\
+    MALI400PP_REG_VAL_IRQ_INVALID_PLIST_COMMAND                  |\
+    MALI400PP_REG_VAL_IRQ_CALL_STACK_UNDERFLOW                   |\
+    MALI400PP_REG_VAL_IRQ_CALL_STACK_OVERFLOW                    |\
+    MALI400PP_REG_VAL_IRQ_RESET_COMPLETED))
+#else
+#error "No supported mali core defined"
+#endif
+
+#if defined USING_MALI200
+#define MALI200_REG_VAL_IRQ_MASK_USED ((enum mali200_mgmt_irq) (\
+    MALI200_REG_VAL_IRQ_END_OF_FRAME                           |\
+    MALI200_REG_VAL_IRQ_HANG                                   |\
+    MALI200_REG_VAL_IRQ_FORCE_HANG                             |\
+    MALI200_REG_VAL_IRQ_BUS_ERROR                              |\
+    MALI200_REG_VAL_IRQ_WRITE_BOUNDARY_ERROR))
+#elif defined USING_MALI400
+#define MALI200_REG_VAL_IRQ_MASK_USED ((enum mali200_mgmt_irq) (\
+    MALI200_REG_VAL_IRQ_END_OF_FRAME                           |\
+    MALI200_REG_VAL_IRQ_HANG                                   |\
+    MALI200_REG_VAL_IRQ_FORCE_HANG                             |\
+    MALI200_REG_VAL_IRQ_BUS_ERROR                              |\
+    MALI200_REG_VAL_IRQ_BUS_STOP                               |\
+    MALI200_REG_VAL_IRQ_WRITE_BOUNDARY_ERROR                   |\
+    MALI400PP_REG_VAL_IRQ_INVALID_PLIST_COMMAND                  |\
+    MALI400PP_REG_VAL_IRQ_CALL_STACK_UNDERFLOW                   |\
+    MALI400PP_REG_VAL_IRQ_CALL_STACK_OVERFLOW))
+#else
+#error "No supported mali core defined"
+#endif
+
+#define MALI200_REG_VAL_IRQ_MASK_NONE ((enum mali200_mgmt_irq)(0))
+
+enum mali200_mgmt_status {
+	MALI200_REG_VAL_STATUS_RENDERING_ACTIVE     = (1<<0),
+	MALI200_REG_VAL_STATUS_BUS_STOPPED          = (1<<4),
+};
+
+enum mali200_render_unit
+{
+	MALI200_REG_ADDR_FRAME = 0x0000,
+};
+
+#if defined USING_MALI200
+#define MALI200_NUM_REGS_FRAME ((0x04C/4)+1)
+#elif defined USING_MALI400
+#define MALI200_NUM_REGS_FRAME ((0x058/4)+1)
+#else
+#error "No supported mali core defined"
+#endif
+
+enum mali200_wb_unit {
+    MALI200_REG_ADDR_WB0 = 0x0100,
+    MALI200_REG_ADDR_WB1 = 0x0200,
+    MALI200_REG_ADDR_WB2 = 0x0300
+};
+
+/** The number of registers in one single writeback unit */
+#ifndef MALI200_NUM_REGS_WBx
+#define MALI200_NUM_REGS_WBx ((0x02C/4)+1)
+#endif
+
+/* This should be in the top 16 bit of the version register of Mali PP */
+#if defined USING_MALI200
+#define MALI_PP_PRODUCT_ID 0xC807
+#elif defined USING_MALI400
+#define MALI300_PP_PRODUCT_ID 0xCE07
+#define MALI400_PP_PRODUCT_ID 0xCD07
+#define MALI_PP_PRODUCT_ID MALI400_PP_PRODUCT_ID
+#else
+#error "No supported mali core defined"
+#endif
+
+
+#endif /* _MALI200_REGS_H_ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/regs/mali_gp_regs.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/regs/mali_gp_regs.h
new file mode 100644
index 00000000000..11eb55c424e
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/regs/mali_gp_regs.h
@@ -0,0 +1,219 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef _MALIGP2_CONROL_REGS_H_
+#define _MALIGP2_CONROL_REGS_H_
+
+/**
+ * These are the different geometry processor controll registers.
+ * Their usage is to control and monitor the operation of the
+ * Vertex Shader and the Polygon List Builer in the geometry processor.
+ * Addresses are in 32-bit word relative sizes.
+ * @see [P0081] "Geometry Processor Data Structures" for details
+ */
+
+typedef enum {
+	MALIGP2_REG_ADDR_MGMT_VSCL_START_ADDR           = 0x00,
+	MALIGP2_REG_ADDR_MGMT_VSCL_END_ADDR             = 0x04,
+	MALIGP2_REG_ADDR_MGMT_PLBUCL_START_ADDR         = 0x08,
+	MALIGP2_REG_ADDR_MGMT_PLBUCL_END_ADDR           = 0x0c,
+	MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_START_ADDR     = 0x10,
+	MALIGP2_REG_ADDR_MGMT_PLBU_ALLOC_END_ADDR       = 0x14,
+	MALIGP2_REG_ADDR_MGMT_CMD                       = 0x20,
+	MALIGP2_REG_ADDR_MGMT_INT_RAWSTAT               = 0x24,
+	MALIGP2_REG_ADDR_MGMT_INT_CLEAR                 = 0x28,
+	MALIGP2_REG_ADDR_MGMT_INT_MASK                  = 0x2C,
+	MALIGP2_REG_ADDR_MGMT_INT_STAT                  = 0x30,
+	MALIGP2_REG_ADDR_MGMT_WRITE_BOUND_LOW           = 0x34,
+	MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_ENABLE         = 0x3C,
+	MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_ENABLE         = 0x40,
+	MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_SRC            = 0x44,
+	MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_SRC            = 0x48,
+	MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_VALUE          = 0x4C,
+	MALIGP2_REG_ADDR_MGMT_PERF_CNT_1_VALUE          = 0x50,
+	MALIGP2_REG_ADDR_MGMT_STATUS                    = 0x68,
+	MALIGP2_REG_ADDR_MGMT_VERSION                   = 0x6C,
+	MALIGP2_REG_ADDR_MGMT_VSCL_START_ADDR_READ      = 0x80,
+	MALIGP2_REG_ADDR_MGMT_PLBCL_START_ADDR_READ     = 0x84,
+	MALIGP2_CONTR_AXI_BUS_ERROR_STAT                = 0x94,
+	MALIGP2_REGISTER_ADDRESS_SPACE_SIZE             = 0x98,
+} maligp_reg_addr_mgmt_addr;
+
+#define MALIGP2_REG_VAL_PERF_CNT_ENABLE 1
+
+/**
+ * Commands to geometry processor.
+ *  @see MALIGP2_CTRL_REG_CMD
+ */
+typedef enum
+{
+	MALIGP2_REG_VAL_CMD_START_VS			= (1<< 0),
+	MALIGP2_REG_VAL_CMD_START_PLBU			= (1<< 1),
+	MALIGP2_REG_VAL_CMD_UPDATE_PLBU_ALLOC	= (1<< 4),
+	MALIGP2_REG_VAL_CMD_RESET				= (1<< 5),
+	MALIGP2_REG_VAL_CMD_FORCE_HANG			= (1<< 6),
+	MALIGP2_REG_VAL_CMD_STOP_BUS 			= (1<< 9),
+#if defined(USING_MALI400)
+	MALI400GP_REG_VAL_CMD_SOFT_RESET		= (1<<10),
+#endif
+} mgp_contr_reg_val_cmd;
+
+
+/**  @defgroup MALIGP2_IRQ
+ * Interrupt status of geometry processor.
+ *  @see MALIGP2_CTRL_REG_INT_RAWSTAT, MALIGP2_REG_ADDR_MGMT_INT_CLEAR,
+ *       MALIGP2_REG_ADDR_MGMT_INT_MASK, MALIGP2_REG_ADDR_MGMT_INT_STAT
+ * @{
+ */
+#define MALIGP2_REG_VAL_IRQ_VS_END_CMD_LST      (1 << 0)
+#define MALIGP2_REG_VAL_IRQ_PLBU_END_CMD_LST    (1 << 1)
+#define MALIGP2_REG_VAL_IRQ_PLBU_OUT_OF_MEM     (1 << 2)
+#define MALIGP2_REG_VAL_IRQ_VS_SEM_IRQ          (1 << 3)
+#define MALIGP2_REG_VAL_IRQ_PLBU_SEM_IRQ        (1 << 4)
+#define MALIGP2_REG_VAL_IRQ_HANG                (1 << 5)
+#define MALIGP2_REG_VAL_IRQ_FORCE_HANG          (1 << 6)
+#define MALIGP2_REG_VAL_IRQ_PERF_CNT_0_LIMIT    (1 << 7)
+#define MALIGP2_REG_VAL_IRQ_PERF_CNT_1_LIMIT    (1 << 8)
+#define MALIGP2_REG_VAL_IRQ_WRITE_BOUND_ERR     (1 << 9)
+#define MALIGP2_REG_VAL_IRQ_SYNC_ERROR          (1 << 10)
+#define MALIGP2_REG_VAL_IRQ_AXI_BUS_ERROR       (1 << 11)
+#if defined USING_MALI400
+#define MALI400GP_REG_VAL_IRQ_AXI_BUS_STOPPED     (1 << 12)
+#define MALI400GP_REG_VAL_IRQ_VS_INVALID_CMD      (1 << 13)
+#define MALI400GP_REG_VAL_IRQ_PLB_INVALID_CMD     (1 << 14)
+#define MALI400GP_REG_VAL_IRQ_RESET_COMPLETED     (1 << 19)
+#define MALI400GP_REG_VAL_IRQ_SEMAPHORE_UNDERFLOW (1 << 20)
+#define MALI400GP_REG_VAL_IRQ_SEMAPHORE_OVERFLOW  (1 << 21)
+#define MALI400GP_REG_VAL_IRQ_PTR_ARRAY_OUT_OF_BOUNDS  (1 << 22)
+#elif !defined USING_MALI200
+#error "No supported mali core defined"
+#endif
+
+/* Mask defining all IRQs in MaliGP2 */
+#if defined USING_MALI200
+#define MALIGP2_REG_VAL_IRQ_MASK_ALL \
+	(\
+		MALIGP2_REG_VAL_IRQ_VS_END_CMD_LST      | \
+		MALIGP2_REG_VAL_IRQ_PLBU_END_CMD_LST    | \
+		MALIGP2_REG_VAL_IRQ_PLBU_OUT_OF_MEM     | \
+		MALIGP2_REG_VAL_IRQ_VS_SEM_IRQ          | \
+		MALIGP2_REG_VAL_IRQ_PLBU_SEM_IRQ        | \
+		MALIGP2_REG_VAL_IRQ_HANG                | \
+		MALIGP2_REG_VAL_IRQ_FORCE_HANG          | \
+		MALIGP2_REG_VAL_IRQ_PERF_CNT_0_LIMIT    | \
+		MALIGP2_REG_VAL_IRQ_PERF_CNT_1_LIMIT    | \
+		MALIGP2_REG_VAL_IRQ_WRITE_BOUND_ERR     | \
+		MALIGP2_REG_VAL_IRQ_SYNC_ERROR          | \
+		MALIGP2_REG_VAL_IRQ_AXI_BUS_ERROR)
+#elif defined USING_MALI400
+#define MALIGP2_REG_VAL_IRQ_MASK_ALL \
+	(\
+		MALIGP2_REG_VAL_IRQ_VS_END_CMD_LST      | \
+		MALIGP2_REG_VAL_IRQ_PLBU_END_CMD_LST    | \
+		MALIGP2_REG_VAL_IRQ_PLBU_OUT_OF_MEM     | \
+		MALIGP2_REG_VAL_IRQ_VS_SEM_IRQ          | \
+		MALIGP2_REG_VAL_IRQ_PLBU_SEM_IRQ        | \
+		MALIGP2_REG_VAL_IRQ_HANG                | \
+		MALIGP2_REG_VAL_IRQ_FORCE_HANG          | \
+		MALIGP2_REG_VAL_IRQ_PERF_CNT_0_LIMIT    | \
+		MALIGP2_REG_VAL_IRQ_PERF_CNT_1_LIMIT    | \
+		MALIGP2_REG_VAL_IRQ_WRITE_BOUND_ERR     | \
+		MALIGP2_REG_VAL_IRQ_SYNC_ERROR          | \
+		MALIGP2_REG_VAL_IRQ_AXI_BUS_ERROR       | \
+		MALI400GP_REG_VAL_IRQ_AXI_BUS_STOPPED     | \
+		MALI400GP_REG_VAL_IRQ_VS_INVALID_CMD      | \
+		MALI400GP_REG_VAL_IRQ_PLB_INVALID_CMD     | \
+		MALI400GP_REG_VAL_IRQ_RESET_COMPLETED     | \
+		MALI400GP_REG_VAL_IRQ_SEMAPHORE_UNDERFLOW | \
+		MALI400GP_REG_VAL_IRQ_SEMAPHORE_OVERFLOW  | \
+		MALI400GP_REG_VAL_IRQ_PTR_ARRAY_OUT_OF_BOUNDS)
+#else
+#error "No supported mali core defined"
+#endif
+
+/* Mask defining the IRQs in MaliGP2 which we use*/
+#if defined USING_MALI200
+#define MALIGP2_REG_VAL_IRQ_MASK_USED \
+	(\
+		MALIGP2_REG_VAL_IRQ_VS_END_CMD_LST      | \
+		MALIGP2_REG_VAL_IRQ_PLBU_END_CMD_LST    | \
+		MALIGP2_REG_VAL_IRQ_PLBU_OUT_OF_MEM     | \
+		MALIGP2_REG_VAL_IRQ_HANG                | \
+		MALIGP2_REG_VAL_IRQ_FORCE_HANG          | \
+		MALIGP2_REG_VAL_IRQ_WRITE_BOUND_ERR     | \
+		MALIGP2_REG_VAL_IRQ_SYNC_ERROR 			| \
+		MALIGP2_REG_VAL_IRQ_AXI_BUS_ERROR)
+#elif defined USING_MALI400
+#define MALIGP2_REG_VAL_IRQ_MASK_USED \
+	(\
+		MALIGP2_REG_VAL_IRQ_VS_END_CMD_LST      | \
+		MALIGP2_REG_VAL_IRQ_PLBU_END_CMD_LST    | \
+		MALIGP2_REG_VAL_IRQ_PLBU_OUT_OF_MEM     | \
+		MALIGP2_REG_VAL_IRQ_HANG                | \
+		MALIGP2_REG_VAL_IRQ_FORCE_HANG          | \
+		MALIGP2_REG_VAL_IRQ_WRITE_BOUND_ERR     | \
+		MALIGP2_REG_VAL_IRQ_SYNC_ERROR 			| \
+		MALIGP2_REG_VAL_IRQ_AXI_BUS_ERROR       | \
+		MALI400GP_REG_VAL_IRQ_VS_INVALID_CMD      | \
+		MALI400GP_REG_VAL_IRQ_PLB_INVALID_CMD     | \
+		MALI400GP_REG_VAL_IRQ_SEMAPHORE_UNDERFLOW | \
+		MALI400GP_REG_VAL_IRQ_SEMAPHORE_OVERFLOW  | \
+		MALI400GP_REG_VAL_IRQ_PTR_ARRAY_OUT_OF_BOUNDS)
+#else
+#error "No supported mali core defined"
+#endif
+
+/* Mask defining non IRQs on MaliGP2*/
+#define MALIGP2_REG_VAL_IRQ_MASK_NONE 0
+
+/** }@ defgroup MALIGP2_IRQ*/
+
+/** @defgroup MALIGP2_STATUS
+ * The different Status values to the geometry processor.
+ *  @see MALIGP2_CTRL_REG_STATUS
+ * @{
+ */
+#define MALIGP2_REG_VAL_STATUS_VS_ACTIVE         0x0002
+#define MALIGP2_REG_VAL_STATUS_BUS_STOPPED       0x0004
+#define MALIGP2_REG_VAL_STATUS_PLBU_ACTIVE       0x0008
+#define MALIGP2_REG_VAL_STATUS_BUS_ERROR         0x0040
+#define MALIGP2_REG_VAL_STATUS_WRITE_BOUND_ERR   0x0100
+/** }@ defgroup MALIGP2_STATUS*/
+
+#define MALIGP2_REG_VAL_STATUS_MASK_ACTIVE (\
+	MALIGP2_REG_VAL_STATUS_VS_ACTIVE|\
+	MALIGP2_REG_VAL_STATUS_PLBU_ACTIVE)
+
+
+#define MALIGP2_REG_VAL_STATUS_MASK_ERROR (\
+	MALIGP2_REG_VAL_STATUS_BUS_ERROR |\
+	MALIGP2_REG_VAL_STATUS_WRITE_BOUND_ERR )
+
+/* This should be in the top 16 bit of the version register of gp.*/
+#if defined(USING_MALI200)
+#define MALI_GP_PRODUCT_ID 0xA07
+#elif defined(USING_MALI400)
+#define MALI300_GP_PRODUCT_ID 0xC07
+#define MALI400_GP_PRODUCT_ID 0xB07
+#define MALI_GP_PRODUCT_ID MALI400_GP_PRODUCT_ID
+#else
+#error "No supported mali core defined"
+#endif
+
+/**
+ * The different sources for instrumented on the geometry processor.
+ *  @see MALIGP2_REG_ADDR_MGMT_PERF_CNT_0_SRC
+ */
+
+enum MALIGP2_cont_reg_perf_cnt_src {
+	MALIGP2_REG_VAL_PERF_CNT1_SRC_NUMBER_OF_VERTICES_PROCESSED = 0x0a,
+};
+
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-arm11-cc/mali_timestamp.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-arm11-cc/mali_timestamp.c
new file mode 100644
index 00000000000..a6b1d76d567
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-arm11-cc/mali_timestamp.c
@@ -0,0 +1,13 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_timestamp.h"
+
+/* This file is intentionally left empty, as all functions are inlined in mali_profiling_sampler.h */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-arm11-cc/mali_timestamp.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-arm11-cc/mali_timestamp.h
new file mode 100644
index 00000000000..96b709bc166
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-arm11-cc/mali_timestamp.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_TIMESTAMP_H__
+#define __MALI_TIMESTAMP_H__
+
+#include "mali_osk.h" 
+
+MALI_STATIC_INLINE _mali_osk_errcode_t _mali_timestamp_reset(void)
+{
+	/*
+	 * reset counters and overflow flags
+	 */
+
+    u32 mask = (1 << 0) | /* enable all three counters */
+	           (0 << 1) | /* reset both Count Registers to 0x0 */
+	           (1 << 2) | /* reset the Cycle Counter Register to 0x0 */
+	           (0 << 3) | /* 1 = Cycle Counter Register counts every 64th processor clock cycle */
+	           (0 << 4) | /* Count Register 0 interrupt enable */
+	           (0 << 5) | /* Count Register 1 interrupt enable */
+	           (0 << 6) | /* Cycle Counter interrupt enable */
+	           (0 << 8) | /* Count Register 0 overflow flag (clear or write, flag on read) */
+	           (0 << 9) | /* Count Register 1 overflow flag (clear or write, flag on read) */
+	           (1 << 10); /* Cycle Counter Register overflow flag (clear or write, flag on read) */
+
+	__asm__ __volatile__ ("MCR    p15, 0, %0, c15, c12, 0" : : "r" (mask) );
+
+	return _MALI_OSK_ERR_OK;
+}
+
+MALI_STATIC_INLINE u64 _mali_timestamp_get(void)
+{
+	u32 result;
+
+	/* this is for the clock cycles */
+	__asm__ __volatile__ ("MRC    p15, 0, %0, c15, c12, 1" : "=r" (result));
+
+	return (u64)result;
+}
+
+#endif /* __MALI_TIMESTAMP_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-default/mali_timestamp.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-default/mali_timestamp.c
new file mode 100644
index 00000000000..a6b1d76d567
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-default/mali_timestamp.c
@@ -0,0 +1,13 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_timestamp.h"
+
+/* This file is intentionally left empty, as all functions are inlined in mali_profiling_sampler.h */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-default/mali_timestamp.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-default/mali_timestamp.h
new file mode 100644
index 00000000000..11031d675ca
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/mali/timestamp-default/mali_timestamp.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __MALI_TIMESTAMP_H__
+#define __MALI_TIMESTAMP_H__
+
+#include "mali_osk.h" 
+
+MALI_STATIC_INLINE _mali_osk_errcode_t _mali_timestamp_reset(void)
+{
+	return _MALI_OSK_ERR_OK;
+}
+
+MALI_STATIC_INLINE u64 _mali_timestamp_get(void)
+{
+	return _mali_osk_time_get_ns();
+}
+
+#endif /* __MALI_TIMESTAMP_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/Makefile b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/Makefile
new file mode 100644
index 00000000000..97225bbbf0b
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/Makefile
@@ -0,0 +1,115 @@
+#
+# Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+# 
+# This program is free software and is provided to you under the terms of the GNU General Public License version 2
+# as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+# 
+# A copy of the licence is included with the program, and can also be obtained from Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+#
+
+UMP_FILE_PREFIX =
+UDD_FILE_PREFIX = ../mali/
+
+ifneq ($(KBUILD_EXTMOD),)
+include $(KBUILD_EXTMOD)/Makefile.common
+else
+include ./Makefile.common
+endif
+
+# For each arch check: CROSS_COMPILE , KDIR , CFLAGS += -DARCH
+
+ARCH ?= arm
+## @note Should allow overriding of building UMP for non-debug:
+EXTRA_CFLAGS += -DDEBUG -DMALI_STATE_TRACKING=0
+
+# linux build system integration
+
+ifneq ($(KERNELRELEASE),)
+# Inside the kernel build system
+
+EXTRA_CFLAGS += -I$(KBUILD_EXTMOD) -I$(KBUILD_EXTMOD)/common -I$(KBUILD_EXTMOD)/linux -I$(KBUILD_EXTMOD)/../mali/common -I$(KBUILD_EXTMOD)/../mali/linux -I$(KBUILD_EXTMOD)/../../ump/include/ump
+
+# For customer releases the Linux Device Drivers will be provided as ARM proprietary and GPL releases:
+# The ARM proprietary product will only include the license/proprietary directory
+# The GPL product will only include the license/gpl directory
+
+ifeq ($(wildcard $(KBUILD_EXTMOD)/linux/license/gpl/*),)
+EXTRA_CFLAGS += -I$(KBUILD_EXTMOD)/linux/license/proprietary
+else
+EXTRA_CFLAGS += -I$(KBUILD_EXTMOD)/linux/license/gpl
+endif
+
+SRC +=	$(UMP_FILE_PREFIX)linux/ump_kernel_linux.c \
+	$(UMP_FILE_PREFIX)linux/ump_kernel_memory_backend_os.c \
+	$(UMP_FILE_PREFIX)linux/ump_kernel_memory_backend_dedicated.c \
+	$(UMP_FILE_PREFIX)linux/ump_memory_backend.c \
+	$(UMP_FILE_PREFIX)linux/ump_ukk_wrappers.c \
+	$(UMP_FILE_PREFIX)linux/ump_ukk_ref_wrappers.c \
+	$(UMP_FILE_PREFIX)linux/ump_osk_atomics.c \
+	$(UMP_FILE_PREFIX)linux/ump_osk_low_level_mem.c \
+	$(UMP_FILE_PREFIX)linux/ump_osk_misc.c \
+	$(UDD_FILE_PREFIX)linux/mali_osk_atomics.c \
+	$(UDD_FILE_PREFIX)linux/mali_osk_locks.c \
+	$(UDD_FILE_PREFIX)linux/mali_osk_memory.c \
+	$(UDD_FILE_PREFIX)linux/mali_osk_math.c \
+	$(UDD_FILE_PREFIX)linux/mali_osk_misc.c
+
+# Selecting files to compile by parsing the config file
+
+MODULE:=ump.ko
+
+obj-m := $(MODULE:.ko=.o)
+$(MODULE:.ko=-y) := $(SRC:.c=.o)
+
+else
+# Outside the kernel build system
+
+# Get any user defined KDIR-<names> or maybe even a hardcoded KDIR
+-include KDIR_CONFIGURATION
+
+# Define host system directory
+KDIR-$(shell uname -m):=/lib/modules/$(shell uname -r)/build
+
+ifeq ($(ARCH), arm)
+	# when compiling for ARM we're cross compiling
+	export CROSS_COMPILE ?= arm-none-linux-gnueabi-
+	# default to Virtex5
+	CONFIG ?= pb-virtex5
+else
+	# Compiling for the host
+	CONFIG ?= $(shell uname -m)
+endif
+
+# default cpu to select
+CPU ?= ct11mp
+
+# look up KDIR based om CPU selection
+KDIR ?= $(KDIR-$(CPU))
+
+ifeq ($(KDIR),)
+$(error No KDIR found for platform $(CPU))
+endif
+
+# Validate selected config
+ifneq ($(shell [ -d arch-$(CONFIG) ] && [ -f arch-$(CONFIG)/config.h ] && echo "OK"), OK)
+$(warning Current directory is $(shell pwd))
+$(error No configuration found for config $(CONFIG). Check that arch-$(CONFIG)/config.h exists)
+else
+# Link arch to the selected arch-config directory
+$(shell [ -L arch ] && rm arch)
+$(shell ln -sf arch-$(CONFIG) arch)
+$(shell touch arch/config.h)
+endif
+
+all:
+	$(MAKE) ARCH=$(ARCH) -C $(KDIR) M=$(CURDIR) modules
+
+kernelrelease:
+	$(MAKE) -C $(KDIR) kernelrelease
+
+clean:
+	$(MAKE) ARCH=$(ARCH) -C $(KDIR) M=$(CURDIR) clean
+	$(MAKE) ARCH=$(ARCH) -C $(KDIR) M=$(CURDIR)/../mali clean
+
+endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/Makefile.common b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/Makefile.common
new file mode 100644
index 00000000000..f797c85a6f2
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/Makefile.common
@@ -0,0 +1,19 @@
+#
+# Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+# 
+# This program is free software and is provided to you under the terms of the GNU General Public License version 2
+# as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+# 
+# A copy of the licence is included with the program, and can also be obtained from Free Software
+# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+#
+
+SRC = $(UMP_FILE_PREFIX)common/ump_kernel_common.c \
+	$(UMP_FILE_PREFIX)common/ump_kernel_descriptor_mapping.c \
+	$(UMP_FILE_PREFIX)common/ump_kernel_api.c \
+	$(UMP_FILE_PREFIX)common/ump_kernel_ref_drv.c
+
+# Get subversion revision number, fall back to 0000 if no svn info is available
+SVN_REV:=$(shell ((svnversion | grep -qvE '(exported|Unversioned)' && echo -n 'Revision: ' && svnversion) || git svn info | sed -e 's/$$$$/M/' | grep '^Revision: ' || echo ${MALI_RELEASE_NAME}) 2>/dev/null | sed -e 's/^Revision: //')
+EXTRA_CFLAGS += -DSVN_REV=$(SVN_REV)
+EXTRA_CFLAGS += -DSVN_REV_STRING=\"$(SVN_REV)\"
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/arch-pb-virtex5/config.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/arch-pb-virtex5/config.h
new file mode 100644
index 00000000000..38ae1eeb3c2
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/arch-pb-virtex5/config.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __ARCH_CONFIG_H__
+#define __ARCH_CONFIG_H__
+
+#define ARCH_UMP_BACKEND_DEFAULT          0
+#define ARCH_UMP_MEMORY_ADDRESS_DEFAULT   0xCE000000
+#define ARCH_UMP_MEMORY_SIZE_DEFAULT 32UL * 1024UL * 1024UL
+
+#endif /* __ARCH_CONFIG_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_api.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_api.c
new file mode 100644
index 00000000000..719370c3de7
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_api.c
@@ -0,0 +1,329 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_osk.h"
+#include "mali_osk_list.h"
+#include "ump_osk.h"
+#include "ump_uk_types.h"
+#include "ump_kernel_interface.h"
+#include "ump_kernel_common.h"
+
+
+
+/* ---------------- UMP kernel space API functions follows ---------------- */
+
+
+
+UMP_KERNEL_API_EXPORT ump_secure_id ump_dd_secure_id_get(ump_dd_handle memh)
+{
+	ump_dd_mem * mem = (ump_dd_mem *)memh;
+
+	DEBUG_ASSERT_POINTER(mem);
+
+	DBG_MSG(5, ("Returning secure ID. ID: %u\n", mem->secure_id));
+
+	return mem->secure_id;
+}
+
+
+
+UMP_KERNEL_API_EXPORT ump_dd_handle ump_dd_handle_create_from_secure_id(ump_secure_id secure_id)
+{
+	ump_dd_mem * mem;
+
+	_mali_osk_lock_wait(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+
+	DBG_MSG(5, ("Getting handle from secure ID. ID: %u\n", secure_id));
+	if (0 != ump_descriptor_mapping_get(device.secure_id_map, (int)secure_id, (void**)&mem))
+	{
+		_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+		DBG_MSG(1, ("Secure ID not found. ID: %u\n", secure_id));
+		return UMP_DD_HANDLE_INVALID;
+	}
+
+	ump_dd_reference_add(mem);
+
+	_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+
+	return (ump_dd_handle)mem;
+}
+
+
+
+UMP_KERNEL_API_EXPORT unsigned long ump_dd_phys_block_count_get(ump_dd_handle memh)
+{
+	ump_dd_mem * mem = (ump_dd_mem*) memh;
+
+	DEBUG_ASSERT_POINTER(mem);
+
+	return mem->nr_blocks;
+}
+
+
+
+UMP_KERNEL_API_EXPORT ump_dd_status_code ump_dd_phys_blocks_get(ump_dd_handle memh, ump_dd_physical_block * blocks, unsigned long num_blocks)
+{
+	ump_dd_mem * mem = (ump_dd_mem *)memh;
+
+	DEBUG_ASSERT_POINTER(mem);
+
+	if (blocks == NULL)
+	{
+		DBG_MSG(1, ("NULL parameter in ump_dd_phys_blocks_get()\n"));
+		return UMP_DD_INVALID;
+	}
+
+	if (mem->nr_blocks != num_blocks)
+	{
+		DBG_MSG(1, ("Specified number of blocks do not match actual number of blocks\n"));
+		return UMP_DD_INVALID;
+	}
+
+	DBG_MSG(5, ("Returning physical block information. ID: %u\n", mem->secure_id));
+
+	_mali_osk_memcpy(blocks, mem->block_array, sizeof(ump_dd_physical_block) * mem->nr_blocks);
+
+	return UMP_DD_SUCCESS;
+}
+
+
+
+UMP_KERNEL_API_EXPORT ump_dd_status_code ump_dd_phys_block_get(ump_dd_handle memh, unsigned long index, ump_dd_physical_block * block)
+{
+	ump_dd_mem * mem = (ump_dd_mem *)memh;
+
+	DEBUG_ASSERT_POINTER(mem);
+
+	if (block == NULL)
+	{
+		DBG_MSG(1, ("NULL parameter in ump_dd_phys_block_get()\n"));
+		return UMP_DD_INVALID;
+	}
+
+	if (index >= mem->nr_blocks)
+	{
+		DBG_MSG(5, ("Invalid index specified in ump_dd_phys_block_get()\n"));
+		return UMP_DD_INVALID;
+	}
+
+	DBG_MSG(5, ("Returning physical block information. ID: %u, index: %lu\n", mem->secure_id, index));
+
+	*block = mem->block_array[index];
+
+	return UMP_DD_SUCCESS;
+}
+
+
+
+UMP_KERNEL_API_EXPORT unsigned long ump_dd_size_get(ump_dd_handle memh)
+{
+	ump_dd_mem * mem = (ump_dd_mem*)memh;
+
+	DEBUG_ASSERT_POINTER(mem);
+
+	DBG_MSG(5, ("Returning size. ID: %u, size: %lu\n", mem->secure_id, mem->size_bytes));
+
+	return mem->size_bytes;
+}
+
+
+
+UMP_KERNEL_API_EXPORT void ump_dd_reference_add(ump_dd_handle memh)
+{
+	ump_dd_mem * mem = (ump_dd_mem*)memh;
+	int new_ref;
+
+	DEBUG_ASSERT_POINTER(mem);
+
+	new_ref = _ump_osk_atomic_inc_and_read(&mem->ref_count);
+
+	DBG_MSG(4, ("Memory reference incremented. ID: %u, new value: %d\n", mem->secure_id, new_ref));
+}
+
+
+
+UMP_KERNEL_API_EXPORT void ump_dd_reference_release(ump_dd_handle memh)
+{
+	int new_ref;
+	ump_dd_mem * mem = (ump_dd_mem*)memh;
+
+	DEBUG_ASSERT_POINTER(mem);
+
+	/* We must hold this mutex while doing the atomic_dec_and_read, to protect
+	that elements in the ump_descriptor_mapping table is always valid.  If they
+	are not, userspace may accidently map in this secure_ids right before its freed
+	giving a mapped backdoor into unallocated memory.*/
+	_mali_osk_lock_wait(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+
+	new_ref = _ump_osk_atomic_dec_and_read(&mem->ref_count);
+
+	DBG_MSG(4, ("Memory reference decremented. ID: %u, new value: %d\n", mem->secure_id, new_ref));
+
+	if (0 == new_ref)
+	{
+		DBG_MSG(3, ("Final release of memory. ID: %u\n", mem->secure_id));
+
+		ump_descriptor_mapping_free(device.secure_id_map, (int)mem->secure_id);
+
+		_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+		mem->release_func(mem->ctx, mem);
+		_mali_osk_free(mem);
+	}
+	else
+	{
+		_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+	}
+}
+
+
+
+/* --------------- Handling of user space requests follows --------------- */
+
+
+_mali_osk_errcode_t _ump_uku_get_api_version( _ump_uk_api_version_s *args )
+{
+	ump_session_data * session_data;
+
+	DEBUG_ASSERT_POINTER( args );
+	DEBUG_ASSERT_POINTER( args->ctx );
+
+	session_data = (ump_session_data *)args->ctx;
+
+	/* check compatability */
+	if (args->version == UMP_IOCTL_API_VERSION)
+	{
+		DBG_MSG(3, ("API version set to newest %d (compatible)\n", GET_VERSION(args->version)));
+		args->compatible = 1;
+		session_data->api_version = args->version;
+	}
+	else if (args->version == MAKE_VERSION_ID(1))
+	{
+		DBG_MSG(2, ("API version set to depricated: %d (compatible)\n", GET_VERSION(args->version)));
+		args->compatible = 1;
+		session_data->api_version = args->version;
+	}
+	else
+	{
+		DBG_MSG(2, ("API version set to %d (incompatible with client version %d)\n", GET_VERSION(UMP_IOCTL_API_VERSION), GET_VERSION(args->version)));
+		args->compatible = 0;
+		args->version = UMP_IOCTL_API_VERSION; /* report our version */
+	}
+
+	return _MALI_OSK_ERR_OK;
+}
+
+
+_mali_osk_errcode_t _ump_ukk_release( _ump_uk_release_s *release_info )
+{
+	ump_session_memory_list_element * session_memory_element;
+	ump_session_memory_list_element * tmp;
+	ump_session_data * session_data;
+	_mali_osk_errcode_t ret = _MALI_OSK_ERR_INVALID_FUNC;
+	int secure_id;
+
+	DEBUG_ASSERT_POINTER( release_info );
+	DEBUG_ASSERT_POINTER( release_info->ctx );
+
+	/* Retreive the session data */
+	session_data = (ump_session_data*)release_info->ctx;
+
+	/* If there are many items in the memory session list we
+	 * could be de-referencing this pointer a lot so keep a local copy
+	 */
+	secure_id = release_info->secure_id;
+
+	DBG_MSG(4, ("Releasing memory with IOCTL, ID: %u\n", secure_id));
+
+	/* Iterate through the memory list looking for the requested secure ID */
+	_mali_osk_lock_wait(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+	_MALI_OSK_LIST_FOREACHENTRY(session_memory_element, tmp, &session_data->list_head_session_memory_list, ump_session_memory_list_element, list)
+	{
+		if ( session_memory_element->mem->secure_id == secure_id)
+		{
+			ump_dd_mem *release_mem;
+
+			release_mem = session_memory_element->mem;
+			_mali_osk_list_del(&session_memory_element->list);
+			ump_dd_reference_release(release_mem);
+			_mali_osk_free(session_memory_element);
+
+			ret = _MALI_OSK_ERR_OK;
+			break;
+		}
+	}
+
+	_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+ 	DBG_MSG_IF(1, _MALI_OSK_ERR_OK != ret, ("UMP memory with ID %u does not belong to this session.\n", secure_id));
+
+	DBG_MSG(4, ("_ump_ukk_release() returning 0x%x\n", ret));
+	return ret;
+}
+
+_mali_osk_errcode_t _ump_ukk_size_get( _ump_uk_size_get_s *user_interaction )
+{
+	ump_dd_mem * mem;
+	_mali_osk_errcode_t ret = _MALI_OSK_ERR_FAULT;
+
+	DEBUG_ASSERT_POINTER( user_interaction );
+
+	/* We lock the mappings so things don't get removed while we are looking for the memory */
+	_mali_osk_lock_wait(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+	if (0 == ump_descriptor_mapping_get(device.secure_id_map, (int)user_interaction->secure_id, (void**)&mem))
+	{
+		user_interaction->size = mem->size_bytes;
+		DBG_MSG(4, ("Returning size. ID: %u, size: %lu ", (ump_secure_id)user_interaction->secure_id, (unsigned long)user_interaction->size));
+		ret = _MALI_OSK_ERR_OK;
+	}
+	else
+	{
+		 user_interaction->size = 0;
+		DBG_MSG(1, ("Failed to look up mapping in ump_ioctl_size_get(). ID: %u\n", (ump_secure_id)user_interaction->secure_id));
+	}
+
+	_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+	return ret;
+}
+
+
+
+void _ump_ukk_msync( _ump_uk_msync_s *args )
+{
+	ump_dd_mem * mem = NULL;
+	_mali_osk_lock_wait(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+	ump_descriptor_mapping_get(device.secure_id_map, (int)args->secure_id, (void**)&mem);
+	_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+
+	if (NULL==mem)
+	{
+		DBG_MSG(1, ("Failed to look up mapping in _ump_ukk_msync(). ID: %u\n", (ump_secure_id)args->secure_id));
+		return;
+	}
+
+	/* Returns the cache settings back to Userspace */
+	args->is_cached=mem->is_cached;
+
+	/* If this flag is the only one set, we should not do the actual flush, only the readout */
+	if ( _UMP_UK_MSYNC_READOUT_CACHE_ENABLED==args->op )
+	{
+		DBG_MSG(3, ("_ump_ukk_msync READOUT  ID: %u Enabled: %d\n", (ump_secure_id)args->secure_id, mem->is_cached));
+		return;
+	}
+
+	/* Nothing to do if the memory is not caches */
+	if ( 0==mem->is_cached )
+	{
+		DBG_MSG(3, ("_ump_ukk_msync IGNORING ID: %u Enabled: %d  OP: %d\n", (ump_secure_id)args->secure_id, mem->is_cached, args->op));
+		return ;
+	}
+	DBG_MSG(3, ("_ump_ukk_msync FLUSHING ID: %u Enabled: %d  OP: %d\n", (ump_secure_id)args->secure_id, mem->is_cached, args->op));
+
+	/* The actual cache flush - Implemented for each OS*/
+	_ump_osk_msync( mem , args->op);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_common.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_common.c
new file mode 100644
index 00000000000..b99c3e7c7d2
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_common.c
@@ -0,0 +1,387 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_osk_bitops.h"
+#include "mali_osk_list.h"
+#include "ump_osk.h"
+#include "ump_uk_types.h"
+#include "ump_ukk.h"
+#include "ump_kernel_common.h"
+#include "ump_kernel_descriptor_mapping.h"
+#include "ump_kernel_memory_backend.h"
+
+
+
+/**
+ * Define the initial and maximum size of number of secure_ids on the system
+ */
+#define UMP_SECURE_ID_TABLE_ENTRIES_INITIAL (128  )
+#define UMP_SECURE_ID_TABLE_ENTRIES_MAXIMUM (4096 )
+
+
+/**
+ * Define the initial and maximum size of the ump_session_data::cookies_map,
+ * which is a \ref ump_descriptor_mapping. This limits how many secure_ids
+ * may be mapped into a particular process using _ump_ukk_map_mem().
+ */
+
+#define UMP_COOKIES_PER_SESSION_INITIAL (UMP_SECURE_ID_TABLE_ENTRIES_INITIAL )
+#define UMP_COOKIES_PER_SESSION_MAXIMUM (UMP_SECURE_ID_TABLE_ENTRIES_MAXIMUM)
+
+struct ump_dev device;
+
+_mali_osk_errcode_t ump_kernel_constructor(void)
+{
+	_mali_osk_errcode_t err;
+
+	/* Perform OS Specific initialization */
+	err = _ump_osk_init();
+	if( _MALI_OSK_ERR_OK != err )
+	{
+		MSG_ERR(("Failed to initiaze the UMP Device Driver"));
+		return err;
+	}
+
+	/* Init the global device */
+	_mali_osk_memset(&device, 0, sizeof(device) );
+
+	/* Create the descriptor map, which will be used for mapping secure ID to ump_dd_mem structs */
+	device.secure_id_map_lock = _mali_osk_lock_init(_MALI_OSK_LOCKFLAG_NONINTERRUPTABLE, 0 , 0);
+	if (NULL == device.secure_id_map_lock)
+	{
+		MSG_ERR(("Failed to create OSK lock for secure id lookup table\n"));
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	device.secure_id_map = ump_descriptor_mapping_create(UMP_SECURE_ID_TABLE_ENTRIES_INITIAL, UMP_SECURE_ID_TABLE_ENTRIES_MAXIMUM);
+	if (NULL == device.secure_id_map)
+	{
+		_mali_osk_lock_term(device.secure_id_map_lock);
+		MSG_ERR(("Failed to create secure id lookup table\n"));
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	/* Init memory backend */
+	device.backend = ump_memory_backend_create();
+	if (NULL == device.backend)
+	{
+		MSG_ERR(("Failed to create memory backend\n"));
+		_mali_osk_lock_term(device.secure_id_map_lock);
+		ump_descriptor_mapping_destroy(device.secure_id_map);
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	return _MALI_OSK_ERR_OK;
+}
+
+void ump_kernel_destructor(void)
+{
+	DEBUG_ASSERT_POINTER(device.secure_id_map);
+	DEBUG_ASSERT_POINTER(device.secure_id_map_lock);
+
+	_mali_osk_lock_term(device.secure_id_map_lock);
+	device.secure_id_map_lock = NULL;
+
+	ump_descriptor_mapping_destroy(device.secure_id_map);
+	device.secure_id_map = NULL;
+
+	device.backend->shutdown(device.backend);
+	device.backend = NULL;
+
+	ump_memory_backend_destroy();
+
+	_ump_osk_term();
+}
+
+/** Creates a new UMP session
+ */
+_mali_osk_errcode_t _ump_ukk_open( void** context )
+{
+	struct ump_session_data * session_data;
+
+	/* allocated struct to track this session */
+	session_data = (struct ump_session_data *)_mali_osk_malloc(sizeof(struct ump_session_data));
+	if (NULL == session_data)
+	{
+		MSG_ERR(("Failed to allocate ump_session_data in ump_file_open()\n"));
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	session_data->lock = _mali_osk_lock_init(_MALI_OSK_LOCKFLAG_NONINTERRUPTABLE, 0, 0);
+	if( NULL == session_data->lock )
+	{
+		MSG_ERR(("Failed to initialize lock for ump_session_data in ump_file_open()\n"));
+		_mali_osk_free(session_data);
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	session_data->cookies_map = ump_descriptor_mapping_create( UMP_COOKIES_PER_SESSION_INITIAL, UMP_COOKIES_PER_SESSION_MAXIMUM );
+
+	if ( NULL == session_data->cookies_map )
+	{
+		MSG_ERR(("Failed to create descriptor mapping for _ump_ukk_map_mem cookies\n"));
+
+		_mali_osk_lock_term( session_data->lock );
+		_mali_osk_free( session_data );
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	_MALI_OSK_INIT_LIST_HEAD(&session_data->list_head_session_memory_list);
+
+	_MALI_OSK_INIT_LIST_HEAD(&session_data->list_head_session_memory_mappings_list);
+
+	/* Since initial version of the UMP interface did not use the API_VERSION ioctl we have to assume
+	   that it is this version, and not the "latest" one: UMP_IOCTL_API_VERSION
+	   Current and later API versions would do an additional call to this IOCTL and update this variable
+	   to the correct one.*/
+	session_data->api_version = MAKE_VERSION_ID(1);
+
+	*context = (void*)session_data;
+
+	DBG_MSG(2, ("New session opened\n"));
+
+	return _MALI_OSK_ERR_OK;
+}
+
+_mali_osk_errcode_t _ump_ukk_close( void** context )
+{
+	struct ump_session_data * session_data;
+	ump_session_memory_list_element * item;
+	ump_session_memory_list_element * tmp;
+
+	session_data = (struct ump_session_data *)*context;
+	if (NULL == session_data)
+	{
+		MSG_ERR(("Session data is NULL in _ump_ukk_close()\n"));
+		return _MALI_OSK_ERR_INVALID_ARGS;
+	}
+
+	/* Unmap any descriptors mapped in. */
+	if (0 == _mali_osk_list_empty(&session_data->list_head_session_memory_mappings_list))
+	{
+		ump_memory_allocation *descriptor;
+		ump_memory_allocation *temp;
+
+		DBG_MSG(1, ("Memory mappings found on session usage list during session termination\n"));
+
+		/* use the 'safe' list iterator, since freeing removes the active block from the list we're iterating */
+		_MALI_OSK_LIST_FOREACHENTRY(descriptor, temp, &session_data->list_head_session_memory_mappings_list, ump_memory_allocation, list)
+		{
+			_ump_uk_unmap_mem_s unmap_args;
+			DBG_MSG(4, ("Freeing block with phys address 0x%x size 0x%x mapped in user space at 0x%x\n",
+			            descriptor->phys_addr, descriptor->size, descriptor->mapping));
+			unmap_args.ctx = (void*)session_data;
+			unmap_args.mapping = descriptor->mapping;
+			unmap_args.size = descriptor->size;
+			unmap_args._ukk_private = NULL; /* NOTE: unused */
+			unmap_args.cookie = descriptor->cookie;
+
+			/* NOTE: This modifies the list_head_session_memory_mappings_list */
+			_ump_ukk_unmap_mem( &unmap_args );
+		}
+	}
+
+	/* ASSERT that we really did free everything, because _ump_ukk_unmap_mem()
+	 * can fail silently. */
+	DEBUG_ASSERT( _mali_osk_list_empty(&session_data->list_head_session_memory_mappings_list) );
+
+	_MALI_OSK_LIST_FOREACHENTRY(item, tmp, &session_data->list_head_session_memory_list, ump_session_memory_list_element, list)
+	{
+		_mali_osk_list_del(&item->list);
+		DBG_MSG(2, ("Releasing UMP memory %u as part of file close\n", item->mem->secure_id));
+		ump_dd_reference_release(item->mem);
+		_mali_osk_free(item);
+	}
+
+	ump_descriptor_mapping_destroy( session_data->cookies_map );
+
+	_mali_osk_lock_term(session_data->lock);
+	_mali_osk_free(session_data);
+
+	DBG_MSG(2, ("Session closed\n"));
+
+	return _MALI_OSK_ERR_OK;
+}
+
+_mali_osk_errcode_t _ump_ukk_map_mem( _ump_uk_map_mem_s *args )
+{
+	struct ump_session_data * session_data;
+	ump_memory_allocation * descriptor;  /* Describes current mapping of memory */
+	_mali_osk_errcode_t err;
+	unsigned long offset = 0;
+	unsigned long left;
+	ump_dd_handle handle;  /* The real UMP handle for this memory. Its real datatype is ump_dd_mem*  */
+	ump_dd_mem * mem;      /* The real UMP memory. It is equal to the handle, but with exposed struct */
+	u32 block;
+	int map_id;
+
+	session_data = (ump_session_data *)args->ctx;
+	if( NULL == session_data )
+	{
+		MSG_ERR(("Session data is NULL in _ump_ukk_map_mem()\n"));
+		return _MALI_OSK_ERR_INVALID_ARGS;
+	}
+
+	descriptor = (ump_memory_allocation*) _mali_osk_calloc( 1, sizeof(ump_memory_allocation));
+	if (NULL == descriptor)
+	{
+		MSG_ERR(("ump_ukk_map_mem: descriptor allocation failed\n"));
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	handle = ump_dd_handle_create_from_secure_id(args->secure_id);
+	if ( UMP_DD_HANDLE_INVALID == handle)
+	{
+		_mali_osk_free(descriptor);
+		DBG_MSG(1, ("Trying to map unknown secure ID %u\n", args->secure_id));
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	mem = (ump_dd_mem*)handle;
+	DEBUG_ASSERT(mem);
+	if (mem->size_bytes != args->size)
+	{
+		_mali_osk_free(descriptor);
+		ump_dd_reference_release(handle);
+		DBG_MSG(1, ("Trying to map too much or little. ID: %u, virtual size=%lu, UMP size: %lu\n", args->secure_id, args->size, mem->size_bytes));
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	map_id = ump_descriptor_mapping_allocate_mapping( session_data->cookies_map, (void*) descriptor );
+
+	if (map_id < 0)
+	{
+		_mali_osk_free(descriptor);
+		ump_dd_reference_release(handle);
+		DBG_MSG(1, ("ump_ukk_map_mem: unable to allocate a descriptor_mapping for return cookie\n"));
+
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	descriptor->size = args->size;
+	descriptor->handle = handle;
+	descriptor->phys_addr = args->phys_addr;
+	descriptor->process_mapping_info = args->_ukk_private;
+	descriptor->ump_session = session_data;
+	descriptor->cookie = (u32)map_id;
+
+	if ( mem->is_cached )
+	{
+		descriptor->is_cached = 1;
+		args->is_cached       = 1;
+		DBG_MSG(3, ("Mapping UMP secure_id: %d as cached.\n", args->secure_id));
+	}
+	else
+	{
+		descriptor->is_cached = 0;
+		args->is_cached       = 0;
+		DBG_MSG(3, ("Mapping UMP secure_id: %d  as Uncached.\n", args->secure_id));
+	}
+
+	_mali_osk_list_init( &descriptor->list );
+
+	err = _ump_osk_mem_mapregion_init( descriptor );
+	if( _MALI_OSK_ERR_OK != err )
+	{
+		DBG_MSG(1, ("Failed to initialize memory mapping in _ump_ukk_map_mem(). ID: %u\n", args->secure_id));
+		ump_descriptor_mapping_free( session_data->cookies_map, map_id );
+		_mali_osk_free(descriptor);
+		ump_dd_reference_release(mem);
+		return err;
+	}
+
+	DBG_MSG(4, ("Mapping virtual to physical memory: ID: %u, size:%lu, first physical addr: 0x%08lx, number of regions: %lu\n",
+	        mem->secure_id,
+	        mem->size_bytes,
+	        ((NULL != mem->block_array) ? mem->block_array->addr : 0),
+	        mem->nr_blocks));
+
+	left = descriptor->size;
+	/* loop over all blocks and map them in */
+	for (block = 0; block < mem->nr_blocks; block++)
+	{
+		unsigned long size_to_map;
+
+		if (left >  mem->block_array[block].size)
+		{
+			size_to_map = mem->block_array[block].size;
+		}
+		else
+		{
+			size_to_map = left;
+		}
+
+		if (_MALI_OSK_ERR_OK != _ump_osk_mem_mapregion_map(descriptor, offset, (u32 *)&(mem->block_array[block].addr), size_to_map ) )
+		{
+			DBG_MSG(1, ("WARNING: _ump_ukk_map_mem failed to map memory into userspace\n"));
+			ump_descriptor_mapping_free( session_data->cookies_map, map_id );
+			ump_dd_reference_release(mem);
+			_ump_osk_mem_mapregion_term( descriptor );
+			_mali_osk_free(descriptor);
+			return _MALI_OSK_ERR_FAULT;
+		}
+		left -= size_to_map;
+		offset += size_to_map;
+	}
+
+	/* Add to the ump_memory_allocation tracking list */
+	_mali_osk_lock_wait(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+	_mali_osk_list_add( &descriptor->list, &session_data->list_head_session_memory_mappings_list );
+	_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+
+	args->mapping = descriptor->mapping;
+	args->cookie = descriptor->cookie;
+
+	return _MALI_OSK_ERR_OK;
+}
+
+void _ump_ukk_unmap_mem( _ump_uk_unmap_mem_s *args )
+{
+	struct ump_session_data * session_data;
+	ump_memory_allocation * descriptor;
+	ump_dd_handle handle;
+
+	session_data = (ump_session_data *)args->ctx;
+
+	if( NULL == session_data )
+	{
+		MSG_ERR(("Session data is NULL in _ump_ukk_map_mem()\n"));
+		return;
+	}
+
+	if (0 != ump_descriptor_mapping_get( session_data->cookies_map, (int)args->cookie, (void**)&descriptor) )
+	{
+		MSG_ERR(("_ump_ukk_map_mem: cookie 0x%X not found for this session\n", args->cookie ));
+		return;
+	}
+
+	DEBUG_ASSERT_POINTER(descriptor);
+
+	handle = descriptor->handle;
+	if ( UMP_DD_HANDLE_INVALID == handle)
+	{
+		DBG_MSG(1, ("WARNING: Trying to unmap unknown handle: UNKNOWN\n"));
+		return;
+	}
+
+	/* Remove the ump_memory_allocation from the list of tracked mappings */
+	_mali_osk_lock_wait(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+	_mali_osk_list_del( &descriptor->list );
+	_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+
+	ump_descriptor_mapping_free( session_data->cookies_map, (int)args->cookie );
+
+	ump_dd_reference_release(handle);
+
+	_ump_osk_mem_mapregion_term( descriptor );
+	_mali_osk_free(descriptor);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_common.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_common.h
new file mode 100644
index 00000000000..0c55b14bc94
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_common.h
@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __UMP_KERNEL_H__
+#define __UMP_KERNEL_H__
+
+#include "ump_kernel_types.h"
+#include "ump_kernel_interface.h"
+#include "ump_kernel_descriptor_mapping.h"
+#include "ump_kernel_memory_backend.h"
+
+
+#ifdef DEBUG
+	extern int ump_debug_level;
+	#define UMP_DEBUG_PRINT(args) _mali_osk_dbgmsg args
+	#define UMP_DEBUG_CODE(args) args
+	#define DBG_MSG(level,args)  do { /* args should be in brackets */ \
+		((level) <=  ump_debug_level)?\
+		UMP_DEBUG_PRINT(("UMP<" #level ">: ")), \
+		UMP_DEBUG_PRINT(args):0; \
+		} while (0)
+
+	#define DBG_MSG_IF(level,condition,args) /* args should be in brackets */ \
+		if((condition)&&((level) <=  ump_debug_level)) {\
+		UMP_DEBUG_PRINT(("UMP<" #level ">: ")); \
+		UMP_DEBUG_PRINT(args); \
+		}
+
+	#define DBG_MSG_ELSE(level,args) /* args should be in brackets */ \
+		else if((level) <=  ump_debug_level) { \
+		UMP_DEBUG_PRINT(("UMP<" #level ">: ")); \
+		UMP_DEBUG_PRINT(args); \
+		}
+
+	#define DEBUG_ASSERT_POINTER(pointer) do  {if( (pointer)== NULL) MSG_ERR(("NULL pointer " #pointer)); } while(0)
+	#define DEBUG_ASSERT(condition) do  {if(!(condition)) MSG_ERR(("ASSERT failed: " #condition)); } while(0)
+#else /* DEBUG */
+	#define UMP_DEBUG_PRINT(args) do {} while(0)
+	#define UMP_DEBUG_CODE(args)
+	#define DBG_MSG(level,args) do {} while(0)
+	#define DBG_MSG_IF(level,condition,args) do {} while(0)
+	#define DBG_MSG_ELSE(level,args) do {} while(0)
+	#define DEBUG_ASSERT(condition) do {} while(0)
+	#define DEBUG_ASSERT_POINTER(pointer) do  {} while(0)
+#endif /* DEBUG */
+
+#define MSG_ERR(args) do{ /* args should be in brackets */ \
+	 _mali_osk_dbgmsg("UMP: ERR: %s\n" ,__FILE__); \
+	 _mali_osk_dbgmsg( "           %s()%4d\n", __FUNCTION__, __LINE__) ; \
+	 _mali_osk_dbgmsg args ; \
+	 _mali_osk_dbgmsg("\n"); \
+	} while(0)
+
+#define MSG(args) do{ /* args should be in brackets */ \
+		 _mali_osk_dbgmsg("UMP: "); \
+		 _mali_osk_dbgmsg args; \
+		} while (0)
+
+
+
+/*
+ * This struct is used to store per session data.
+ * A session is created when someone open() the device, and
+ * closed when someone close() it or the user space application terminates.
+ */
+typedef struct ump_session_data
+{
+	_mali_osk_list_t list_head_session_memory_list;  /**< List of ump allocations made by the process (elements are ump_session_memory_list_element) */
+	_mali_osk_list_t list_head_session_memory_mappings_list; /**< List of ump_memory_allocations mapped in */
+	int api_version;
+	_mali_osk_lock_t * lock;
+	ump_descriptor_mapping * cookies_map; /**< Secure mapping of cookies from _ump_ukk_map_mem() */
+} ump_session_data;
+
+
+
+/*
+ * This struct is used to track the UMP memory references a session has.
+ * We need to track this in order to be able to clean up after user space processes
+ * which don't do it themself (e.g. due to a crash or premature termination).
+ */
+typedef struct ump_session_memory_list_element
+{
+	struct ump_dd_mem * mem;
+	_mali_osk_list_t list;
+} ump_session_memory_list_element;
+
+
+
+/*
+ * Device specific data, created when device driver is loaded, and then kept as the global variable device.
+ */
+typedef struct ump_dev
+{
+	_mali_osk_lock_t * secure_id_map_lock;
+	ump_descriptor_mapping * secure_id_map;
+	ump_memory_backend * backend;
+} ump_dev;
+
+
+
+extern int ump_debug_level;
+extern struct ump_dev device;
+
+_mali_osk_errcode_t ump_kernel_constructor(void);
+void ump_kernel_destructor(void);
+int map_errcode( _mali_osk_errcode_t err );
+
+/**
+ * variables from user space cannot be dereferenced from kernel space; tagging them
+ * with __user allows the GCC compiler to generate a warning. Other compilers may
+ * not support this so we define it here as an empty macro if the compiler doesn't
+ * define it.
+ */
+#ifndef __user
+#define __user
+#endif
+
+#endif /* __UMP_KERNEL_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_descriptor_mapping.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_descriptor_mapping.c
new file mode 100644
index 00000000000..2531f802127
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_descriptor_mapping.c
@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_kernel_common.h"
+#include "mali_osk.h"
+#include "mali_osk_bitops.h"
+#include "ump_kernel_common.h"
+#include "ump_kernel_descriptor_mapping.h"
+
+#define MALI_PAD_INT(x) (((x) + (BITS_PER_LONG - 1)) & ~(BITS_PER_LONG - 1))
+
+/**
+ * Allocate a descriptor table capable of holding 'count' mappings
+ * @param count Number of mappings in the table
+ * @return Pointer to a new table, NULL on error
+ */
+static ump_descriptor_table * descriptor_table_alloc(int count);
+
+/**
+ * Free a descriptor table
+ * @param table The table to free
+ */
+static void descriptor_table_free(ump_descriptor_table * table);
+
+ump_descriptor_mapping * ump_descriptor_mapping_create(int init_entries, int max_entries)
+{
+	ump_descriptor_mapping * map = _mali_osk_calloc(1, sizeof(ump_descriptor_mapping) );
+
+	init_entries = MALI_PAD_INT(init_entries);
+	max_entries = MALI_PAD_INT(max_entries);
+
+	if (NULL != map)
+	{
+		map->table = descriptor_table_alloc(init_entries);
+		if (NULL != map->table)
+		{
+			map->lock = _mali_osk_lock_init(_MALI_OSK_LOCKFLAG_NONINTERRUPTABLE | _MALI_OSK_LOCKFLAG_READERWRITER, 0 , 0);
+			if ( NULL != map->lock )
+			{
+				_mali_osk_set_nonatomic_bit(0, map->table->usage); /* reserve bit 0 to prevent NULL/zero logic to kick in */
+				map->max_nr_mappings_allowed = max_entries;
+				map->current_nr_mappings = init_entries;
+				return map;
+			}
+			descriptor_table_free(map->table);
+		}
+		_mali_osk_free(map);
+	}
+	return NULL;
+}
+
+void ump_descriptor_mapping_destroy(ump_descriptor_mapping * map)
+{
+	descriptor_table_free(map->table);
+	_mali_osk_lock_term( map->lock );
+	_mali_osk_free(map);
+}
+
+int ump_descriptor_mapping_allocate_mapping(ump_descriptor_mapping * map, void * target)
+{
+ 	int descriptor = -1;/*-EFAULT;*/
+ 	_mali_osk_lock_wait(map->lock, _MALI_OSK_LOCKMODE_RW);
+ 	descriptor = _mali_osk_find_first_zero_bit(map->table->usage, map->current_nr_mappings);
+	if (descriptor == map->current_nr_mappings)
+	{
+		int nr_mappings_new;
+		/* no free descriptor, try to expand the table */
+		ump_descriptor_table * new_table;
+		ump_descriptor_table * old_table = map->table;
+		nr_mappings_new= map->current_nr_mappings *2;
+
+		if (map->current_nr_mappings >= map->max_nr_mappings_allowed)
+		{
+			descriptor = -1;
+			goto unlock_and_exit;
+		}
+
+		new_table = descriptor_table_alloc(nr_mappings_new);
+		if (NULL == new_table)
+		{
+			descriptor = -1;
+			goto unlock_and_exit;
+		}
+
+ 		_mali_osk_memcpy(new_table->usage, old_table->usage, (sizeof(unsigned long)*map->current_nr_mappings) / BITS_PER_LONG);
+ 		_mali_osk_memcpy(new_table->mappings, old_table->mappings, map->current_nr_mappings * sizeof(void*));
+		map->table = new_table;
+		map->current_nr_mappings = nr_mappings_new;
+		descriptor_table_free(old_table);
+	}
+
+	/* we have found a valid descriptor, set the value and usage bit */
+	_mali_osk_set_nonatomic_bit(descriptor, map->table->usage);
+	map->table->mappings[descriptor] = target;
+
+unlock_and_exit:
+	_mali_osk_lock_signal(map->lock, _MALI_OSK_LOCKMODE_RW);
+	return descriptor;
+}
+
+int ump_descriptor_mapping_get(ump_descriptor_mapping * map, int descriptor, void** target)
+{
+ 	int result = -1;/*-EFAULT;*/
+ 	DEBUG_ASSERT(map);
+ 	_mali_osk_lock_wait(map->lock, _MALI_OSK_LOCKMODE_RO);
+ 	if ( (descriptor >= 0) && (descriptor < map->current_nr_mappings) && _mali_osk_test_bit(descriptor, map->table->usage) )
+	{
+		*target = map->table->mappings[descriptor];
+		result = 0;
+	}
+	else *target = NULL;
+	_mali_osk_lock_signal(map->lock, _MALI_OSK_LOCKMODE_RO);
+	return result;
+}
+
+int ump_descriptor_mapping_set(ump_descriptor_mapping * map, int descriptor, void * target)
+{
+ 	int result = -1;/*-EFAULT;*/
+ 	_mali_osk_lock_wait(map->lock, _MALI_OSK_LOCKMODE_RO);
+ 	if ( (descriptor >= 0) && (descriptor < map->current_nr_mappings) && _mali_osk_test_bit(descriptor, map->table->usage) )
+	{
+		map->table->mappings[descriptor] = target;
+		result = 0;
+	}
+	_mali_osk_lock_signal(map->lock, _MALI_OSK_LOCKMODE_RO);
+	return result;
+}
+
+void ump_descriptor_mapping_free(ump_descriptor_mapping * map, int descriptor)
+{
+ 	_mali_osk_lock_wait(map->lock, _MALI_OSK_LOCKMODE_RW);
+ 	if ( (descriptor >= 0) && (descriptor < map->current_nr_mappings) && _mali_osk_test_bit(descriptor, map->table->usage) )
+	{
+		map->table->mappings[descriptor] = NULL;
+		_mali_osk_clear_nonatomic_bit(descriptor, map->table->usage);
+	}
+	_mali_osk_lock_signal(map->lock, _MALI_OSK_LOCKMODE_RW);
+}
+
+static ump_descriptor_table * descriptor_table_alloc(int count)
+{
+	ump_descriptor_table * table;
+
+	table = _mali_osk_calloc(1, sizeof(ump_descriptor_table) + ((sizeof(unsigned long) * count)/BITS_PER_LONG) + (sizeof(void*) * count) );
+
+	if (NULL != table)
+	{
+		table->usage = (u32*)((u8*)table + sizeof(ump_descriptor_table));
+		table->mappings = (void**)((u8*)table + sizeof(ump_descriptor_table) + ((sizeof(unsigned long) * count)/BITS_PER_LONG));
+	}
+
+	return table;
+}
+
+static void descriptor_table_free(ump_descriptor_table * table)
+{
+	_mali_osk_free(table);
+}
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_descriptor_mapping.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_descriptor_mapping.h
new file mode 100644
index 00000000000..92bbe54bd35
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_descriptor_mapping.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_kernel_descriptor_mapping.h
+ */
+
+#ifndef __UMP_KERNEL_DESCRIPTOR_MAPPING_H__
+#define __UMP_KERNEL_DESCRIPTOR_MAPPING_H__
+
+#include "mali_osk.h"
+
+/**
+ * The actual descriptor mapping table, never directly accessed by clients
+ */
+typedef struct ump_descriptor_table
+{
+	u32 * usage; /**< Pointer to bitpattern indicating if a descriptor is valid/used or not */
+	void** mappings; /**< Array of the pointers the descriptors map to */
+} ump_descriptor_table;
+
+/**
+ * The descriptor mapping object
+ * Provides a separate namespace where we can map an integer to a pointer
+ */
+typedef struct ump_descriptor_mapping
+{
+	_mali_osk_lock_t *lock; /**< Lock protecting access to the mapping object */
+	int max_nr_mappings_allowed; /**< Max number of mappings to support in this namespace */
+	int current_nr_mappings; /**< Current number of possible mappings */
+	ump_descriptor_table * table; /**< Pointer to the current mapping table */
+} ump_descriptor_mapping;
+
+/**
+ * Create a descriptor mapping object
+ * Create a descriptor mapping capable of holding init_entries growable to max_entries
+ * @param init_entries Number of entries to preallocate memory for
+ * @param max_entries Number of entries to max support
+ * @return Pointer to a descriptor mapping object, NULL on failure
+ */
+ump_descriptor_mapping * ump_descriptor_mapping_create(int init_entries, int max_entries);
+
+/**
+ * Destroy a descriptor mapping object
+ * @param map The map to free
+ */
+void ump_descriptor_mapping_destroy(ump_descriptor_mapping * map);
+
+/**
+ * Allocate a new mapping entry (descriptor ID)
+ * Allocates a new entry in the map.
+ * @param map The map to allocate a new entry in
+ * @param target The value to map to
+ * @return The descriptor allocated, a negative value on error
+ */
+int ump_descriptor_mapping_allocate_mapping(ump_descriptor_mapping * map, void * target);
+
+/**
+ * Get the value mapped to by a descriptor ID
+ * @param map The map to lookup the descriptor id in
+ * @param descriptor The descriptor ID to lookup
+ * @param target Pointer to a pointer which will receive the stored value
+ * @return 0 on successful lookup, negative on error
+ */
+int ump_descriptor_mapping_get(ump_descriptor_mapping * map, int descriptor, void** target);
+
+/**
+ * Set the value mapped to by a descriptor ID
+ * @param map The map to lookup the descriptor id in
+ * @param descriptor The descriptor ID to lookup
+ * @param target Pointer to replace the current value with
+ * @return 0 on successful lookup, negative on error
+ */
+int ump_descriptor_mapping_set(ump_descriptor_mapping * map, int descriptor, void * target);
+
+/**
+ * Free the descriptor ID
+ * For the descriptor to be reused it has to be freed
+ * @param map The map to free the descriptor from
+ * @param descriptor The descriptor ID to free
+ */
+void ump_descriptor_mapping_free(ump_descriptor_mapping * map, int descriptor);
+
+#endif /* __UMP_KERNEL_DESCRIPTOR_MAPPING_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_memory_backend.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_memory_backend.h
new file mode 100644
index 00000000000..02a64707036
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_memory_backend.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_kernel_memory_mapping.h
+ */
+
+#ifndef __UMP_KERNEL_MEMORY_BACKEND_H__
+#define __UMP_KERNEL_MEMORY_BACKEND_H__
+
+#include "ump_kernel_interface.h"
+#include "ump_kernel_types.h"
+
+
+typedef struct ump_memory_allocation
+{
+	void                    * phys_addr;
+	void                    * mapping;
+	unsigned long             size;
+	ump_dd_handle             handle;
+	void                    * process_mapping_info;
+	u32                       cookie;               /**< necessary on some U/K interface implementations */
+	struct ump_session_data * ump_session;          /**< Session that this allocation belongs to */
+	_mali_osk_list_t          list;                 /**< List for linking together memory allocations into the session's memory head */
+	u32 is_cached;
+} ump_memory_allocation;
+
+typedef struct ump_memory_backend
+{
+	int  (*allocate)(void* ctx, ump_dd_mem * descriptor);
+	void (*release)(void* ctx, ump_dd_mem * descriptor);
+	void (*shutdown)(struct ump_memory_backend * backend);
+	int  (*pre_allocate_physical_check)(void *ctx, u32 size);
+	u32  (*adjust_to_mali_phys)(void *ctx, u32 cpu_phys);
+	void * ctx;
+} ump_memory_backend;
+
+ump_memory_backend * ump_memory_backend_create ( void );
+void ump_memory_backend_destroy( void );
+
+#endif /*__UMP_KERNEL_MEMORY_BACKEND_H__ */
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_ref_drv.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_ref_drv.c
new file mode 100644
index 00000000000..5a997e222a7
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_ref_drv.c
@@ -0,0 +1,194 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "mali_osk.h"
+#include "mali_osk_list.h"
+#include "ump_osk.h"
+#include "ump_uk_types.h"
+
+#include "ump_kernel_interface_ref_drv.h"
+#include "ump_kernel_common.h"
+#include "ump_kernel_descriptor_mapping.h"
+
+#define UMP_MINIMUM_SIZE         4096
+#define UMP_MINIMUM_SIZE_MASK    (~(UMP_MINIMUM_SIZE-1))
+#define UMP_SIZE_ALIGN(x)        (((x)+UMP_MINIMUM_SIZE-1)&UMP_MINIMUM_SIZE_MASK)
+#define UMP_ADDR_ALIGN_OFFSET(x) ((x)&(UMP_MINIMUM_SIZE-1))
+static void phys_blocks_release(void * ctx, struct ump_dd_mem * descriptor);
+
+UMP_KERNEL_API_EXPORT ump_dd_handle ump_dd_handle_create_from_phys_blocks(ump_dd_physical_block * blocks, unsigned long num_blocks)
+{
+	ump_dd_mem * mem;
+	unsigned long size_total = 0;
+	int map_id;
+	u32 i;
+
+	/* Go through the input blocks and verify that they are sane */
+	for (i=0; i < num_blocks; i++)
+	{
+		unsigned long addr = blocks[i].addr;
+		unsigned long size = blocks[i].size;
+
+		DBG_MSG(5, ("Adding physical memory to new handle. Address: 0x%08lx, size: %lu\n", addr, size));
+		size_total += blocks[i].size;
+
+		if (0 != UMP_ADDR_ALIGN_OFFSET(addr))
+		{
+			MSG_ERR(("Trying to create UMP memory from unaligned physical address. Address: 0x%08lx\n", addr));
+			return UMP_DD_HANDLE_INVALID;
+		}
+
+		if (0 != UMP_ADDR_ALIGN_OFFSET(size))
+		{
+			MSG_ERR(("Trying to create UMP memory with unaligned size. Size: %lu\n", size));
+			return UMP_DD_HANDLE_INVALID;
+		}
+	}
+
+	/* Allocate the ump_dd_mem struct for this allocation */
+	mem = _mali_osk_malloc(sizeof(*mem));
+	if (NULL == mem)
+	{
+		DBG_MSG(1, ("Could not allocate ump_dd_mem in ump_dd_handle_create_from_phys_blocks()\n"));
+		return UMP_DD_HANDLE_INVALID;
+	}
+
+	/* Find a secure ID for this allocation */
+	_mali_osk_lock_wait(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+	map_id = ump_descriptor_mapping_allocate_mapping(device.secure_id_map, (void*) mem);
+
+	if (map_id < 0)
+	{
+		_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+		_mali_osk_free(mem);
+		DBG_MSG(1, ("Failed to allocate secure ID in ump_dd_handle_create_from_phys_blocks()\n"));
+		return UMP_DD_HANDLE_INVALID;
+	}
+
+	/* Now, make a copy of the block information supplied by the user */
+	mem->block_array = _mali_osk_malloc(sizeof(ump_dd_physical_block)* num_blocks);
+	if (NULL == mem->block_array)
+	{
+		ump_descriptor_mapping_free(device.secure_id_map, map_id);
+		_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+		_mali_osk_free(mem);
+		DBG_MSG(1, ("Could not allocate a mem handle for function ump_dd_handle_create_from_phys_blocks().\n"));
+		return UMP_DD_HANDLE_INVALID;
+	}
+
+	_mali_osk_memcpy(mem->block_array, blocks, sizeof(ump_dd_physical_block) * num_blocks);
+
+	/* And setup the rest of the ump_dd_mem struct */
+	_mali_osk_atomic_init(&mem->ref_count, 1);
+	mem->secure_id = (ump_secure_id)map_id;
+	mem->size_bytes = size_total;
+	mem->nr_blocks = num_blocks;
+	mem->backend_info = NULL;
+	mem->ctx = NULL;
+	mem->release_func = phys_blocks_release;
+	/* For now UMP handles created by ump_dd_handle_create_from_phys_blocks() is forced to be Uncached */
+	mem->is_cached = 0;
+
+	_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+	DBG_MSG(3, ("UMP memory created. ID: %u, size: %lu\n", mem->secure_id, mem->size_bytes));
+
+	return (ump_dd_handle)mem;
+}
+
+static void phys_blocks_release(void * ctx, struct ump_dd_mem * descriptor)
+{
+	_mali_osk_free(descriptor->block_array);
+	descriptor->block_array = NULL;
+}
+
+_mali_osk_errcode_t _ump_ukk_allocate( _ump_uk_allocate_s *user_interaction )
+{
+	ump_session_data * session_data = NULL;
+	ump_dd_mem *new_allocation = NULL;
+	ump_session_memory_list_element * session_memory_element = NULL;
+	int map_id;
+
+	DEBUG_ASSERT_POINTER( user_interaction );
+	DEBUG_ASSERT_POINTER( user_interaction->ctx );
+
+	session_data = (ump_session_data *) user_interaction->ctx;
+
+	session_memory_element = _mali_osk_calloc( 1, sizeof(ump_session_memory_list_element));
+	if (NULL == session_memory_element)
+	{
+		DBG_MSG(1, ("Failed to allocate ump_session_memory_list_element in ump_ioctl_allocate()\n"));
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+
+	new_allocation = _mali_osk_calloc( 1, sizeof(ump_dd_mem));
+	if (NULL==new_allocation)
+	{
+		_mali_osk_free(session_memory_element);
+		DBG_MSG(1, ("Failed to allocate ump_dd_mem in _ump_ukk_allocate()\n"));
+		return _MALI_OSK_ERR_NOMEM;
+	}
+
+	/* Create a secure ID for this allocation */
+	_mali_osk_lock_wait(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+	map_id = ump_descriptor_mapping_allocate_mapping(device.secure_id_map, (void*)new_allocation);
+
+	if (map_id < 0)
+	{
+		_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+		_mali_osk_free(session_memory_element);
+		_mali_osk_free(new_allocation);
+		DBG_MSG(1, ("Failed to allocate secure ID in ump_ioctl_allocate()\n"));
+		return - _MALI_OSK_ERR_INVALID_FUNC;
+	}
+
+	/* Initialize the part of the new_allocation that we know so for */
+	new_allocation->secure_id = (ump_secure_id)map_id;
+	_mali_osk_atomic_init(&new_allocation->ref_count,1);
+	if ( 0==(UMP_REF_DRV_UK_CONSTRAINT_USE_CACHE & user_interaction->constraints) )
+		 new_allocation->is_cached = 0;
+	else new_allocation->is_cached = 1;
+
+	/* special case a size of 0, we should try to emulate what malloc does in this case, which is to return a valid pointer that must be freed, but can't be dereferences */
+	if (0 == user_interaction->size)
+	{
+		user_interaction->size = 1; /* emulate by actually allocating the minimum block size */
+	}
+
+	new_allocation->size_bytes = UMP_SIZE_ALIGN(user_interaction->size); /* Page align the size */
+
+	/* Now, ask the active memory backend to do the actual memory allocation */
+	if (!device.backend->allocate( device.backend->ctx, new_allocation ) )
+	{
+		DBG_MSG(3, ("OOM: No more UMP memory left. Failed to allocate memory in ump_ioctl_allocate(). Size: %lu, requested size: %lu\n", new_allocation->size_bytes, (unsigned long)user_interaction->size));
+		ump_descriptor_mapping_free(device.secure_id_map, map_id);
+		_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+		_mali_osk_free(new_allocation);
+		_mali_osk_free(session_memory_element);
+		return _MALI_OSK_ERR_INVALID_FUNC;
+	}
+
+	new_allocation->ctx = device.backend->ctx;
+	new_allocation->release_func = device.backend->release;
+
+	_mali_osk_lock_signal(device.secure_id_map_lock, _MALI_OSK_LOCKMODE_RW);
+
+	/* Initialize the session_memory_element, and add it to the session object */
+	session_memory_element->mem = new_allocation;
+	_mali_osk_lock_wait(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+	_mali_osk_list_add(&(session_memory_element->list), &(session_data->list_head_session_memory_list));
+	_mali_osk_lock_signal(session_data->lock, _MALI_OSK_LOCKMODE_RW);
+
+	user_interaction->secure_id = new_allocation->secure_id;
+	user_interaction->size = new_allocation->size_bytes;
+	DBG_MSG(3, ("UMP memory allocated. ID: %u, size: %lu\n", new_allocation->secure_id, new_allocation->size_bytes));
+
+	return _MALI_OSK_ERR_OK;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_types.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_types.h
new file mode 100644
index 00000000000..dc79b6f289b
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_kernel_types.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __UMP_KERNEL_TYPES_H__
+#define __UMP_KERNEL_TYPES_H__
+
+#include "ump_kernel_interface.h"
+#include "mali_osk.h"
+
+/*
+ * This struct is what is "behind" a ump_dd_handle
+ */
+typedef struct ump_dd_mem
+{
+	ump_secure_id secure_id;
+	_mali_osk_atomic_t ref_count;
+	unsigned long size_bytes;
+	unsigned long nr_blocks;
+	ump_dd_physical_block * block_array;
+	void (*release_func)(void * ctx, struct ump_dd_mem * descriptor);
+	void * ctx;
+	void * backend_info;
+	int is_cached;
+} ump_dd_mem;
+
+
+
+#endif /* __UMP_KERNEL_TYPES_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_osk.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_osk.h
new file mode 100644
index 00000000000..73284f02b47
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_osk.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_osk.h
+ * Defines the OS abstraction layer for the UMP kernel device driver (OSK)
+ */
+
+#ifndef __UMP_OSK_H__
+#define __UMP_OSK_H__
+
+#include <mali_osk.h>
+#include <ump_kernel_memory_backend.h>
+#include <ump_uk_types.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+_mali_osk_errcode_t _ump_osk_init( void );
+
+_mali_osk_errcode_t _ump_osk_term( void );
+
+int _ump_osk_atomic_inc_and_read( _mali_osk_atomic_t *atom );
+
+int _ump_osk_atomic_dec_and_read( _mali_osk_atomic_t *atom );
+
+_mali_osk_errcode_t _ump_osk_mem_mapregion_init( ump_memory_allocation *descriptor );
+
+_mali_osk_errcode_t _ump_osk_mem_mapregion_map( ump_memory_allocation * descriptor, u32 offset, u32 * phys_addr, unsigned long size );
+
+void _ump_osk_mem_mapregion_term( ump_memory_allocation * descriptor );
+
+void _ump_osk_msync( ump_dd_mem * mem, ump_uk_msync_op op );
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_uk_types.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_uk_types.h
new file mode 100644
index 00000000000..b08335f61f1
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_uk_types.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_uk_types.h
+ * Defines the types and constants used in the user-kernel interface
+ */
+
+#ifndef __UMP_UK_TYPES_H__
+#define __UMP_UK_TYPES_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/* Helpers for API version handling */
+#define MAKE_VERSION_ID(x) (((x) << 16UL) | (x))
+#define IS_VERSION_ID(x) (((x) & 0xFFFF) == (((x) >> 16UL) & 0xFFFF))
+#define GET_VERSION(x) (((x) >> 16UL) & 0xFFFF)
+#define IS_API_MATCH(x, y) (IS_VERSION_ID((x)) && IS_VERSION_ID((y)) && (GET_VERSION((x)) == GET_VERSION((y))))
+
+/**
+ * API version define.
+ * Indicates the version of the kernel API
+ * The version is a 16bit integer incremented on each API change.
+ * The 16bit integer is stored twice in a 32bit integer
+ * So for version 1 the value would be 0x00010001
+ */
+#define UMP_IOCTL_API_VERSION MAKE_VERSION_ID(2)
+
+typedef enum
+{
+	_UMP_IOC_QUERY_API_VERSION = 1,
+	_UMP_IOC_ALLOCATE,
+	_UMP_IOC_RELEASE,
+	_UMP_IOC_SIZE_GET,
+	_UMP_IOC_MAP_MEM,    /* not used in Linux */
+	_UMP_IOC_UNMAP_MEM,  /* not used in Linux */
+	_UMP_IOC_MSYNC,
+}_ump_uk_functions;
+
+typedef enum
+{
+	UMP_REF_DRV_UK_CONSTRAINT_NONE = 0,
+	UMP_REF_DRV_UK_CONSTRAINT_PHYSICALLY_LINEAR = 1,
+	UMP_REF_DRV_UK_CONSTRAINT_USE_CACHE = 4,
+} ump_uk_alloc_constraints;
+
+typedef enum
+{
+	_UMP_UK_MSYNC_CLEAN = 0,
+	_UMP_UK_MSYNC_CLEAN_AND_INVALIDATE = 1,
+	_UMP_UK_MSYNC_READOUT_CACHE_ENABLED = 128,
+} ump_uk_msync_op;
+
+/**
+ * Get API version ([in,out] u32 api_version, [out] u32 compatible)
+ */
+typedef struct _ump_uk_api_version_s
+{
+	void *ctx;      /**< [in,out] user-kernel context (trashed on output) */
+	u32 version;    /**< Set to the user space version on entry, stores the device driver version on exit */
+	u32 compatible; /**< Non-null if the device is compatible with the client */
+} _ump_uk_api_version_s;
+
+/**
+ * ALLOCATE ([out] u32 secure_id, [in,out] u32 size,  [in] contraints)
+ */
+typedef struct _ump_uk_allocate_s
+{
+	void *ctx;                              /**< [in,out] user-kernel context (trashed on output) */
+	u32 secure_id;                          /**< Return value from DD to Userdriver */
+	u32 size;                               /**< Input and output. Requested size; input. Returned size; output */
+	ump_uk_alloc_constraints constraints;   /**< Only input to Devicedriver */
+} _ump_uk_allocate_s;
+
+/**
+ * SIZE_GET ([in] u32 secure_id, [out]size )
+ */
+typedef struct _ump_uk_size_get_s
+{
+	void *ctx;                              /**< [in,out] user-kernel context (trashed on output) */
+	u32 secure_id;                          /**< Input to DD */
+	u32 size;                               /**< Returned size; output */
+} _ump_uk_size_get_s;
+
+/**
+ * Release ([in] u32 secure_id)
+ */
+typedef struct _ump_uk_release_s
+{
+	void *ctx;                              /**< [in,out] user-kernel context (trashed on output) */
+	u32 secure_id;                          /**< Input to DD */
+} _ump_uk_release_s;
+
+typedef struct _ump_uk_map_mem_s
+{
+	void *ctx;                      /**< [in,out] user-kernel context (trashed on output) */
+	void *mapping;                  /**< [out] Returns user-space virtual address for the mapping */
+	void *phys_addr;                /**< [in] physical address */
+	unsigned long size;             /**< [in] size */
+	u32 secure_id;                  /**< [in] secure_id to assign to mapping */
+	void * _ukk_private;            /**< Only used inside linux port between kernel frontend and common part to store vma */
+	u32 cookie;
+	u32 is_cached;            /**< [in,out] caching of CPU mappings */
+} _ump_uk_map_mem_s;
+
+typedef struct _ump_uk_unmap_mem_s
+{
+	void *ctx;            /**< [in,out] user-kernel context (trashed on output) */
+	void *mapping;
+	u32 size;
+	void * _ukk_private;
+	u32 cookie;
+} _ump_uk_unmap_mem_s;
+
+typedef struct _ump_uk_msync_s
+{
+	void *ctx;            /**< [in,out] user-kernel context (trashed on output) */
+	void *mapping;        /**< [in] mapping addr */
+	void *address;        /**< [in] flush start addr */
+	u32 size;             /**< [in] size to flush */
+	ump_uk_msync_op op;   /**< [in] flush operation */
+	u32 cookie;           /**< [in] cookie stored with reference to the kernel mapping internals */
+	u32 secure_id;        /**< [in] cookie stored with reference to the kernel mapping internals */
+	u32 is_cached;        /**< [out] caching of CPU mappings */
+} _ump_uk_msync_s;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UMP_UK_TYPES_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_ukk.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_ukk.h
new file mode 100644
index 00000000000..a3317fc7b21
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/common/ump_ukk.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_ukk.h
+ * Defines the kernel-side interface of the user-kernel interface
+ */
+
+#ifndef __UMP_UKK_H__
+#define __UMP_UKK_H__
+
+#include "mali_osk.h"
+#include "ump_uk_types.h"
+
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+
+_mali_osk_errcode_t _ump_ukk_open( void** context );
+
+_mali_osk_errcode_t _ump_ukk_close( void** context );
+
+_mali_osk_errcode_t _ump_ukk_allocate( _ump_uk_allocate_s *user_interaction );
+
+_mali_osk_errcode_t _ump_ukk_release( _ump_uk_release_s *release_info );
+
+_mali_osk_errcode_t _ump_ukk_size_get( _ump_uk_size_get_s *user_interaction );
+
+_mali_osk_errcode_t _ump_ukk_map_mem( _ump_uk_map_mem_s *args );
+
+_mali_osk_errcode_t _ump_uku_get_api_version( _ump_uk_api_version_s *args );
+
+void _ump_ukk_unmap_mem( _ump_uk_unmap_mem_s *args );
+
+void _ump_ukk_msync( _ump_uk_msync_s *args );
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UMP_UKK_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/license/gpl/ump_kernel_license.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/license/gpl/ump_kernel_license.h
new file mode 100644
index 00000000000..17b930d2c57
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/license/gpl/ump_kernel_license.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2010 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_kernel_license.h
+ * Defines for the macro MODULE_LICENSE.
+ */
+
+#ifndef __UMP_KERNEL_LICENSE_H__
+#define __UMP_KERNEL_LICENSE_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#define UMP_KERNEL_LINUX_LICENSE     "GPL"
+#define UMP_LICENSE_IS_GPL 1
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UMP_KERNEL_LICENSE_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ioctl.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ioctl.h
new file mode 100644
index 00000000000..b11429816ab
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ioctl.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __UMP_IOCTL_H__
+#define __UMP_IOCTL_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include <linux/types.h>
+#include <linux/ioctl.h>
+
+#include "../common/ump_uk_types.h"
+
+#ifndef __user
+#define __user
+#endif
+
+
+/**
+ * @file UMP_ioctl.h
+ * This file describes the interface needed to use the Linux device driver.
+ * The interface is used by the userpace UMP driver.
+ */
+
+#define UMP_IOCTL_NR 0x90
+
+
+#define UMP_IOC_QUERY_API_VERSION _IOR(UMP_IOCTL_NR, _UMP_IOC_QUERY_API_VERSION, _ump_uk_api_version_s)
+#define UMP_IOC_ALLOCATE  _IOWR(UMP_IOCTL_NR,  _UMP_IOC_ALLOCATE,  _ump_uk_allocate_s)
+#define UMP_IOC_RELEASE  _IOR(UMP_IOCTL_NR,  _UMP_IOC_RELEASE,  _ump_uk_release_s)
+#define UMP_IOC_SIZE_GET  _IOWR(UMP_IOCTL_NR,  _UMP_IOC_SIZE_GET, _ump_uk_size_get_s)
+#define UMP_IOC_MSYNC     _IOW(UMP_IOCTL_NR,  _UMP_IOC_MSYNC, _ump_uk_size_get_s)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UMP_IOCTL_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_linux.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_linux.c
new file mode 100644
index 00000000000..76d6b2f913a
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_linux.c
@@ -0,0 +1,409 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>            /* kernel module definitions */
+#include <linux/fs.h>                /* file system operations */
+#include <linux/cdev.h>              /* character device definitions */
+#include <linux/ioport.h>            /* request_mem_region */
+#include <linux/mm.h>                /* memory management functions and types */
+#include <asm/uaccess.h>             /* user space access */
+#include <asm/atomic.h>
+#include <linux/device.h>
+#include "arch/config.h"             /* Configuration for current platform. The symlinc for arch is set by Makefile */
+#include "ump_ioctl.h"
+#include "ump_kernel_common.h"
+#include "ump_kernel_interface.h"
+#include "ump_kernel_interface_ref_drv.h"
+#include "ump_kernel_descriptor_mapping.h"
+#include "ump_kernel_memory_backend.h"
+#include "ump_kernel_memory_backend_os.h"
+#include "ump_kernel_memory_backend_dedicated.h"
+#include "ump_kernel_license.h"
+
+#include "ump_osk.h"
+#include "ump_ukk.h"
+#include "ump_uk_types.h"
+#include "ump_ukk_wrappers.h"
+#include "ump_ukk_ref_wrappers.h"
+
+
+/* Module parameter to control log level */
+int ump_debug_level = 2;
+module_param(ump_debug_level, int, S_IRUSR | S_IWUSR | S_IWGRP | S_IRGRP | S_IROTH); /* rw-rw-r-- */
+MODULE_PARM_DESC(ump_debug_level, "Higher number, more dmesg output");
+
+/* By default the module uses any available major, but it's possible to set it at load time to a specific number */
+int ump_major = 0;
+module_param(ump_major, int, S_IRUGO); /* r--r--r-- */
+MODULE_PARM_DESC(ump_major, "Device major number");
+
+/* Name of the UMP device driver */
+static char ump_dev_name[] = "ump"; /* should be const, but the functions we call requires non-cost */
+
+
+
+/*
+ * The data which we attached to each virtual memory mapping request we get.
+ * Each memory mapping has a reference to the UMP memory it maps.
+ * We release this reference when the last memory mapping is unmapped.
+ */
+typedef struct ump_vma_usage_tracker
+{
+	int references;
+	ump_dd_handle handle;
+} ump_vma_usage_tracker;
+
+struct ump_device
+{
+	struct cdev cdev;
+#if UMP_LICENSE_IS_GPL
+	struct class * ump_class;
+#endif
+};
+
+/* The global variable containing the global device data */
+static struct ump_device ump_device;
+
+
+/* Forward declare static functions */
+static int ump_file_open(struct inode *inode, struct file *filp);
+static int ump_file_release(struct inode *inode, struct file *filp);
+#ifdef HAVE_UNLOCKED_IOCTL
+static long ump_file_ioctl(struct file *filp, unsigned int cmd, unsigned long arg);
+#else
+static int ump_file_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg);
+#endif
+static int ump_file_mmap(struct file * filp, struct vm_area_struct * vma);
+
+
+/* This variable defines the file operations this UMP device driver offer */
+static struct file_operations ump_fops =
+{
+	.owner   = THIS_MODULE,
+	.open    = ump_file_open,
+	.release = ump_file_release,
+#ifdef HAVE_UNLOCKED_IOCTL
+	.unlocked_ioctl   = ump_file_ioctl,
+#else
+	.ioctl   = ump_file_ioctl,
+#endif
+	.mmap    = ump_file_mmap
+};
+
+
+/* This function is called by Linux to initialize this module.
+ * All we do is initialize the UMP device driver.
+ */
+static int ump_initialize_module(void)
+{
+	_mali_osk_errcode_t err;
+
+	DBG_MSG(2, ("Inserting UMP device driver. Compiled: %s, time: %s\n", __DATE__, __TIME__));
+
+	err = ump_kernel_constructor();
+	if (_MALI_OSK_ERR_OK != err)
+	{
+		MSG_ERR(("UMP device driver init failed\n"));
+		return map_errcode(err);
+	}
+
+	MSG(("UMP device driver %s loaded\n", SVN_REV_STRING));
+	return 0;
+}
+
+
+
+/*
+ * This function is called by Linux to unload/terminate/exit/cleanup this module.
+ * All we do is terminate the UMP device driver.
+ */
+static void ump_cleanup_module(void)
+{
+	DBG_MSG(2, ("Unloading UMP device driver\n"));
+	ump_kernel_destructor();
+	DBG_MSG(2, ("Module unloaded\n"));
+}
+
+
+
+/*
+ * Initialize the UMP device driver.
+ */
+int ump_kernel_device_initialize(void)
+{
+	int err;
+	dev_t dev = 0;
+
+	if (0 == ump_major)
+	{
+		/* auto select a major */
+		err = alloc_chrdev_region(&dev, 0, 1, ump_dev_name);
+		ump_major = MAJOR(dev);
+	}
+	else
+	{
+		/* use load time defined major number */
+		dev = MKDEV(ump_major, 0);
+		err = register_chrdev_region(dev, 1, ump_dev_name);
+	}
+
+	if (0 == err)
+	{
+		memset(&ump_device, 0, sizeof(ump_device));
+
+		/* initialize our char dev data */
+		cdev_init(&ump_device.cdev, &ump_fops);
+		ump_device.cdev.owner = THIS_MODULE;
+		ump_device.cdev.ops = &ump_fops;
+
+		/* register char dev with the kernel */
+		err = cdev_add(&ump_device.cdev, dev, 1/*count*/);
+		if (0 == err)
+		{
+
+#if UMP_LICENSE_IS_GPL
+			ump_device.ump_class = class_create(THIS_MODULE, ump_dev_name);
+			if (IS_ERR(ump_device.ump_class))
+			{
+				err = PTR_ERR(ump_device.ump_class);
+			}
+			else
+			{
+				struct device * mdev;
+				mdev = device_create(ump_device.ump_class, NULL, dev, NULL, ump_dev_name);
+				if (!IS_ERR(mdev))
+				{
+					return 0;
+				}
+
+				err = PTR_ERR(mdev);
+			}
+			cdev_del(&ump_device.cdev);
+#else
+			return 0;
+#endif
+		}
+
+		unregister_chrdev_region(dev, 1);
+	}
+
+	return err;
+}
+
+
+
+/*
+ * Terminate the UMP device driver
+ */
+void ump_kernel_device_terminate(void)
+{
+	dev_t dev = MKDEV(ump_major, 0);
+
+#if UMP_LICENSE_IS_GPL
+	device_destroy(ump_device.ump_class, dev);
+	class_destroy(ump_device.ump_class);
+#endif
+
+	/* unregister char device */
+	cdev_del(&ump_device.cdev);
+
+	/* free major */
+	unregister_chrdev_region(dev, 1);
+}
+
+/*
+ * Open a new session. User space has called open() on us.
+ */
+static int ump_file_open(struct inode *inode, struct file *filp)
+{
+	struct ump_session_data * session_data;
+	_mali_osk_errcode_t err;
+
+	/* input validation */
+	if (0 != MINOR(inode->i_rdev))
+	{
+		MSG_ERR(("Minor not zero in ump_file_open()\n"));
+		return -ENODEV;
+	}
+
+	/* Call the OS-Independent UMP Open function */
+	err = _ump_ukk_open((void**) &session_data );
+	if( _MALI_OSK_ERR_OK != err )
+	{
+		MSG_ERR(("Ump failed to open a new session\n"));
+		return map_errcode( err );
+	}
+
+	filp->private_data = (void*)session_data;
+	filp->f_pos = 0;
+
+	return 0; /* success */
+}
+
+
+
+/*
+ * Close a session. User space has called close() or crashed/terminated.
+ */
+static int ump_file_release(struct inode *inode, struct file *filp)
+{
+	_mali_osk_errcode_t err;
+
+	err = _ump_ukk_close((void**) &filp->private_data );
+	if( _MALI_OSK_ERR_OK != err )
+	{
+		return map_errcode( err );
+	}
+
+	return 0;  /* success */
+}
+
+
+
+/*
+ * Handle IOCTL requests.
+ */
+#ifdef HAVE_UNLOCKED_IOCTL
+static long ump_file_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+#else
+static int ump_file_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg)
+#endif
+{
+	int err = -ENOTTY;
+	void __user * argument;
+	struct ump_session_data * session_data;
+
+#ifndef HAVE_UNLOCKED_IOCTL
+	(void)inode; /* inode not used */
+#endif
+
+	session_data = (struct ump_session_data *)filp->private_data;
+	if (NULL == session_data)
+	{
+		MSG_ERR(("No session data attached to file object\n"));
+		return -ENOTTY;
+	}
+
+	/* interpret the argument as a user pointer to something */
+	argument = (void __user *)arg;
+
+	switch (cmd)
+	{
+		case UMP_IOC_QUERY_API_VERSION:
+			err = ump_get_api_version_wrapper((u32 __user *)argument, session_data);
+			break;
+
+		case UMP_IOC_ALLOCATE :
+			err = ump_allocate_wrapper((u32 __user *)argument, session_data);
+			break;
+
+		case UMP_IOC_RELEASE:
+			err = ump_release_wrapper((u32 __user *)argument, session_data);
+			break;
+
+		case UMP_IOC_SIZE_GET:
+			err = ump_size_get_wrapper((u32 __user *)argument, session_data);
+			break;
+
+		case UMP_IOC_MSYNC:
+			err = ump_msync_wrapper((u32 __user *)argument, session_data);
+			break;
+
+		default:
+			DBG_MSG(1, ("No handler for IOCTL. cmd: 0x%08x, arg: 0x%08lx\n", cmd, arg));
+			err = -EFAULT;
+			break;
+	}
+
+	return err;
+}
+
+int map_errcode( _mali_osk_errcode_t err )
+{
+    switch(err)
+    {
+        case _MALI_OSK_ERR_OK : return 0;
+        case _MALI_OSK_ERR_FAULT: return -EFAULT;
+        case _MALI_OSK_ERR_INVALID_FUNC: return -ENOTTY;
+        case _MALI_OSK_ERR_INVALID_ARGS: return -EINVAL;
+        case _MALI_OSK_ERR_NOMEM: return -ENOMEM;
+        case _MALI_OSK_ERR_TIMEOUT: return -ETIMEDOUT;
+        case _MALI_OSK_ERR_RESTARTSYSCALL: return -ERESTARTSYS;
+        case _MALI_OSK_ERR_ITEM_NOT_FOUND: return -ENOENT;
+        default: return -EFAULT;
+    }
+}
+
+/*
+ * Handle from OS to map specified virtual memory to specified UMP memory.
+ */
+static int ump_file_mmap(struct file * filp, struct vm_area_struct * vma)
+{
+	_ump_uk_map_mem_s args;
+	_mali_osk_errcode_t err;
+	struct ump_session_data * session_data;
+
+	/* Validate the session data */
+	session_data = (struct ump_session_data *)filp->private_data;
+	if (NULL == session_data)
+	{
+		MSG_ERR(("mmap() called without any session data available\n"));
+		return -EFAULT;
+	}
+
+	/* Re-pack the arguments that mmap() packed for us */
+	args.ctx = session_data;
+	args.phys_addr = 0;
+	args.size = vma->vm_end - vma->vm_start;
+	args._ukk_private = vma;
+	args.secure_id = vma->vm_pgoff;
+	args.is_cached = 0;
+
+	if (!(vma->vm_flags & VM_SHARED))
+	{
+		args.is_cached = 1;
+		vma->vm_flags = vma->vm_flags | VM_SHARED | VM_MAYSHARE  ;
+		DBG_MSG(3, ("UMP Map function: Forcing the CPU to use cache\n"));
+	}
+
+	DBG_MSG(4, ("UMP vma->flags: %x\n", vma->vm_flags ));
+
+	/* Call the common mmap handler */
+	err = _ump_ukk_map_mem( &args );
+	if ( _MALI_OSK_ERR_OK != err)
+	{
+		MSG_ERR(("_ump_ukk_map_mem() failed in function ump_file_mmap()"));
+		return map_errcode( err );
+	}
+
+	return 0; /* success */
+}
+
+/* Export UMP kernel space API functions */
+EXPORT_SYMBOL(ump_dd_secure_id_get);
+EXPORT_SYMBOL(ump_dd_handle_create_from_secure_id);
+EXPORT_SYMBOL(ump_dd_phys_block_count_get);
+EXPORT_SYMBOL(ump_dd_phys_block_get);
+EXPORT_SYMBOL(ump_dd_phys_blocks_get);
+EXPORT_SYMBOL(ump_dd_size_get);
+EXPORT_SYMBOL(ump_dd_reference_add);
+EXPORT_SYMBOL(ump_dd_reference_release);
+
+/* Export our own extended kernel space allocator */
+EXPORT_SYMBOL(ump_dd_handle_create_from_phys_blocks);
+
+/* Setup init and exit functions for this module */
+module_init(ump_initialize_module);
+module_exit(ump_cleanup_module);
+
+/* And some module informatio */
+MODULE_LICENSE(UMP_KERNEL_LINUX_LICENSE);
+MODULE_AUTHOR("ARM Ltd.");
+MODULE_VERSION(SVN_REV_STRING);
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_linux.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_linux.h
new file mode 100644
index 00000000000..464c035974b
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_linux.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef __UMP_KERNEL_H__
+#define __UMP_KERNEL_H__
+
+int ump_kernel_device_initialize(void);
+void ump_kernel_device_terminate(void);
+
+
+#endif /* __UMP_KERNEL_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_dedicated.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_dedicated.c
new file mode 100644
index 00000000000..5c8a7f3783f
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_dedicated.c
@@ -0,0 +1,273 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/* needed to detect kernel version specific code */
+#include <linux/version.h>
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+#include <linux/semaphore.h>
+#else /* pre 2.6.26 the file was in the arch specific location */
+#include <asm/semaphore.h>
+#endif
+
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <asm/atomic.h>
+#include <linux/vmalloc.h>
+#include "ump_kernel_common.h"
+#include "ump_kernel_memory_backend.h"
+
+
+
+#define UMP_BLOCK_SIZE (256UL * 1024UL)  /* 256kB, remember to keep the ()s */
+
+
+
+typedef struct block_info
+{
+	struct block_info * next;
+} block_info;
+
+
+
+typedef struct block_allocator
+{
+	struct semaphore mutex;
+	block_info * all_blocks;
+	block_info * first_free;
+	u32 base;
+	u32 num_blocks;
+	u32 num_free;
+} block_allocator;
+
+
+static void block_allocator_shutdown(ump_memory_backend * backend);
+static int block_allocator_allocate(void* ctx, ump_dd_mem * mem);
+static void block_allocator_release(void * ctx, ump_dd_mem * handle);
+static inline u32 get_phys(block_allocator * allocator, block_info * block);
+
+
+
+/*
+ * Create dedicated memory backend
+ */
+ump_memory_backend * ump_block_allocator_create(u32 base_address, u32 size)
+{
+	ump_memory_backend * backend;
+	block_allocator * allocator;
+	u32 usable_size;
+	u32 num_blocks;
+
+	usable_size = (size + UMP_BLOCK_SIZE - 1) & ~(UMP_BLOCK_SIZE - 1);
+	num_blocks = usable_size / UMP_BLOCK_SIZE;
+
+	if (0 == usable_size)
+	{
+		DBG_MSG(1, ("Memory block of size %u is unusable\n", size));
+		return NULL;
+	}
+
+	DBG_MSG(5, ("Creating dedicated UMP memory backend. Base address: 0x%08x, size: 0x%08x\n", base_address, size));
+	DBG_MSG(6, ("%u usable bytes which becomes %u blocks\n", usable_size, num_blocks));
+
+	backend = kzalloc(sizeof(ump_memory_backend), GFP_KERNEL);
+	if (NULL != backend)
+	{
+		allocator = kmalloc(sizeof(block_allocator), GFP_KERNEL);
+		if (NULL != allocator)
+		{
+			allocator->all_blocks = kmalloc(sizeof(block_allocator) * num_blocks, GFP_KERNEL);
+			if (NULL != allocator->all_blocks)
+			{
+				int i;
+
+				allocator->first_free = NULL;
+				allocator->num_blocks = num_blocks;
+				allocator->num_free = num_blocks;
+				allocator->base = base_address;
+				sema_init(&allocator->mutex, 1);
+
+				for (i = 0; i < num_blocks; i++)
+				{
+					allocator->all_blocks[i].next = allocator->first_free;
+					allocator->first_free = &allocator->all_blocks[i];
+				}
+
+				backend->ctx = allocator;
+				backend->allocate = block_allocator_allocate;
+				backend->release = block_allocator_release;
+				backend->shutdown = block_allocator_shutdown;
+				backend->pre_allocate_physical_check = NULL;
+				backend->adjust_to_mali_phys = NULL;
+
+				return backend;
+			}
+			kfree(allocator);
+		}
+		kfree(backend);
+	}
+
+	return NULL;
+}
+
+
+
+/*
+ * Destroy specified dedicated memory backend
+ */
+static void block_allocator_shutdown(ump_memory_backend * backend)
+{
+	block_allocator * allocator;
+
+	BUG_ON(!backend);
+	BUG_ON(!backend->ctx);
+
+	allocator = (block_allocator*)backend->ctx;
+
+	DBG_MSG_IF(1, allocator->num_free != allocator->num_blocks, ("%u blocks still in use during shutdown\n", allocator->num_blocks - allocator->num_free));
+
+	kfree(allocator->all_blocks);
+	kfree(allocator);
+	kfree(backend);
+}
+
+
+
+static int block_allocator_allocate(void* ctx, ump_dd_mem * mem)
+{
+	block_allocator * allocator;
+	u32 left;
+	block_info * last_allocated = NULL;
+	int i = 0;
+
+	BUG_ON(!ctx);
+	BUG_ON(!mem);
+
+	allocator = (block_allocator*)ctx;
+	left = mem->size_bytes;
+
+	BUG_ON(!left);
+	BUG_ON(!&allocator->mutex);
+
+	mem->nr_blocks = ((left + UMP_BLOCK_SIZE - 1) & ~(UMP_BLOCK_SIZE - 1)) / UMP_BLOCK_SIZE;
+	mem->block_array = (ump_dd_physical_block*)vmalloc(sizeof(ump_dd_physical_block) * mem->nr_blocks);
+	if (NULL == mem->block_array)
+	{
+		MSG_ERR(("Failed to allocate block array\n"));
+		return 0;
+	}
+
+	if (down_interruptible(&allocator->mutex))
+	{
+		MSG_ERR(("Could not get mutex to do block_allocate\n"));
+		return 0;
+	}
+
+	mem->size_bytes = 0;
+
+	while ((left > 0) && (allocator->first_free))
+	{
+		block_info * block;
+
+		block = allocator->first_free;
+		allocator->first_free = allocator->first_free->next;
+		block->next = last_allocated;
+		last_allocated = block;
+		allocator->num_free--;
+
+		mem->block_array[i].addr = get_phys(allocator, block);
+		mem->block_array[i].size = UMP_BLOCK_SIZE;
+		mem->size_bytes += UMP_BLOCK_SIZE;
+
+		i++;
+
+		if (left < UMP_BLOCK_SIZE) left = 0;
+		else left -= UMP_BLOCK_SIZE;
+	}
+
+	if (left)
+	{
+		block_info * block;
+		/* release all memory back to the pool */
+		while (last_allocated)
+		{
+			block = last_allocated->next;
+			last_allocated->next = allocator->first_free;
+			allocator->first_free = last_allocated;
+			last_allocated = block;
+			allocator->num_free++;
+		}
+
+		vfree(mem->block_array);
+		mem->backend_info = NULL;
+		mem->block_array = NULL;
+
+		DBG_MSG(4, ("Could not find a mem-block for the allocation.\n"));
+		up(&allocator->mutex);
+
+		return 0;
+	}
+
+	mem->backend_info = last_allocated;
+
+	up(&allocator->mutex);
+	mem->is_cached=0;
+
+	return 1;
+}
+
+
+
+static void block_allocator_release(void * ctx, ump_dd_mem * handle)
+{
+	block_allocator * allocator;
+	block_info * block, * next;
+
+	BUG_ON(!ctx);
+	BUG_ON(!handle);
+
+	allocator = (block_allocator*)ctx;
+	block = (block_info*)handle->backend_info;
+	BUG_ON(!block);
+
+	if (down_interruptible(&allocator->mutex))
+	{
+		MSG_ERR(("Allocator release: Failed to get mutex - memory leak\n"));
+		return;
+	}
+
+	while (block)
+	{
+		next = block->next;
+
+		BUG_ON( (block < allocator->all_blocks) || (block > (allocator->all_blocks + allocator->num_blocks)));
+
+		block->next = allocator->first_free;
+		allocator->first_free = block;
+		allocator->num_free++;
+
+		block = next;
+	}
+	DBG_MSG(3, ("%d blocks free after release call\n", allocator->num_free));
+	up(&allocator->mutex);
+
+	vfree(handle->block_array);
+	handle->block_array = NULL;
+}
+
+
+
+/*
+ * Helper function for calculating the physical base adderss of a memory block
+ */
+static inline u32 get_phys(block_allocator * allocator, block_info * block)
+{
+	return allocator->base + ((block - allocator->all_blocks) * UMP_BLOCK_SIZE);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_dedicated.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_dedicated.h
new file mode 100644
index 00000000000..58ebe15e5a2
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_dedicated.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_kernel_memory_backend_dedicated.h
+ */
+
+#ifndef __UMP_KERNEL_MEMORY_BACKEND_DEDICATED_H__
+#define __UMP_KERNEL_MEMORY_BACKEND_DEDICATED_H__
+
+#include "ump_kernel_memory_backend.h"
+
+ump_memory_backend * ump_block_allocator_create(u32 base_address, u32 size);
+
+#endif /* __UMP_KERNEL_MEMORY_BACKEND_DEDICATED_H__ */
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_os.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_os.c
new file mode 100644
index 00000000000..99c9c223036
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_os.c
@@ -0,0 +1,245 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/* needed to detect kernel version specific code */
+#include <linux/version.h>
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+#include <linux/semaphore.h>
+#else /* pre 2.6.26 the file was in the arch specific location */
+#include <asm/semaphore.h>
+#endif
+
+#include <linux/dma-mapping.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <asm/atomic.h>
+#include <linux/vmalloc.h>
+#include <asm/cacheflush.h>
+#include "ump_kernel_common.h"
+#include "ump_kernel_memory_backend.h"
+
+
+
+typedef struct os_allocator
+{
+	struct semaphore mutex;
+	u32 num_pages_max;       /**< Maximum number of pages to allocate from the OS */
+	u32 num_pages_allocated; /**< Number of pages allocated from the OS */
+} os_allocator;
+
+
+
+static void os_free(void* ctx, ump_dd_mem * descriptor);
+static int os_allocate(void* ctx, ump_dd_mem * descriptor);
+static void os_memory_backend_destroy(ump_memory_backend * backend);
+
+
+
+/*
+ * Create OS memory backend
+ */
+ump_memory_backend * ump_os_memory_backend_create(const int max_allocation)
+{
+	ump_memory_backend * backend;
+	os_allocator * info;
+
+	info = kmalloc(sizeof(os_allocator), GFP_KERNEL);
+	if (NULL == info)
+	{
+		return NULL;
+	}
+
+	info->num_pages_max = max_allocation >> PAGE_SHIFT;
+	info->num_pages_allocated = 0;
+
+	sema_init(&info->mutex, 1);
+
+	backend = kmalloc(sizeof(ump_memory_backend), GFP_KERNEL);
+	if (NULL == backend)
+	{
+		kfree(info);
+		return NULL;
+	}
+
+	backend->ctx = info;
+	backend->allocate = os_allocate;
+	backend->release = os_free;
+	backend->shutdown = os_memory_backend_destroy;
+	backend->pre_allocate_physical_check = NULL;
+	backend->adjust_to_mali_phys = NULL;
+
+	return backend;
+}
+
+
+
+/*
+ * Destroy specified OS memory backend
+ */
+static void os_memory_backend_destroy(ump_memory_backend * backend)
+{
+	os_allocator * info = (os_allocator*)backend->ctx;
+
+	DBG_MSG_IF(1, 0 != info->num_pages_allocated, ("%d pages still in use during shutdown\n", info->num_pages_allocated));
+
+	kfree(info);
+	kfree(backend);
+}
+
+
+
+/*
+ * Allocate UMP memory
+ */
+static int os_allocate(void* ctx, ump_dd_mem * descriptor)
+{
+	u32 left;
+	os_allocator * info;
+	int pages_allocated = 0;
+	int is_cached;
+
+	BUG_ON(!descriptor);
+	BUG_ON(!ctx);
+
+	info = (os_allocator*)ctx;
+	left = descriptor->size_bytes;
+	is_cached = descriptor->is_cached;
+
+	if (down_interruptible(&info->mutex))
+	{
+		DBG_MSG(1, ("Failed to get mutex in os_free\n"));
+		return 0; /* failure */
+	}
+
+	descriptor->backend_info = NULL;
+	descriptor->nr_blocks = ((left + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1)) >> PAGE_SHIFT;
+
+	DBG_MSG(5, ("Allocating page array. Size: %lu\n", descriptor->nr_blocks * sizeof(ump_dd_physical_block)));
+
+	descriptor->block_array = (ump_dd_physical_block *)vmalloc(sizeof(ump_dd_physical_block) * descriptor->nr_blocks);
+	if (NULL == descriptor->block_array)
+	{
+		up(&info->mutex);
+		DBG_MSG(1, ("Block array could not be allocated\n"));
+		return 0; /* failure */
+	}
+
+	while (left > 0 && ((info->num_pages_allocated + pages_allocated) < info->num_pages_max))
+	{
+		struct page * new_page;
+
+		if (is_cached)
+		{
+			new_page = alloc_page(GFP_HIGHUSER | __GFP_ZERO | __GFP_REPEAT | __GFP_NOWARN);
+		} else
+		{
+			new_page = alloc_page(GFP_HIGHUSER | __GFP_ZERO | __GFP_REPEAT | __GFP_NOWARN | __GFP_COLD);
+		}
+		if (NULL == new_page)
+		{
+			break;
+		}
+
+		/* Ensure page caches are flushed. */
+		if ( is_cached )
+		{
+			descriptor->block_array[pages_allocated].addr = page_to_phys(new_page);
+			descriptor->block_array[pages_allocated].size = PAGE_SIZE;
+		} else
+		{
+			descriptor->block_array[pages_allocated].addr = dma_map_page(NULL, new_page, 0, PAGE_SIZE, DMA_BIDIRECTIONAL );
+			descriptor->block_array[pages_allocated].size = PAGE_SIZE;
+		}
+
+		DBG_MSG(5, ("Allocated page 0x%08lx cached: %d\n", descriptor->block_array[pages_allocated].addr, is_cached));
+
+		if (left < PAGE_SIZE)
+		{
+			left = 0;
+		}
+		else
+		{
+			left -= PAGE_SIZE;
+		}
+
+		pages_allocated++;
+	}
+
+	DBG_MSG(5, ("Alloce for ID:%2d got %d pages, cached: %d\n", descriptor->secure_id,  pages_allocated));
+
+	if (left)
+	{
+		DBG_MSG(1, ("Failed to allocate needed pages\n"));
+
+		while(pages_allocated)
+		{
+			pages_allocated--;
+			if ( !is_cached )
+			{
+				dma_unmap_page(NULL, descriptor->block_array[pages_allocated].addr, PAGE_SIZE, DMA_BIDIRECTIONAL);
+			}
+			__free_page(pfn_to_page(descriptor->block_array[pages_allocated].addr >> PAGE_SHIFT) );
+		}
+
+		up(&info->mutex);
+
+		return 0; /* failure */
+	}
+
+	info->num_pages_allocated += pages_allocated;
+
+	DBG_MSG(6, ("%d out of %d pages now allocated\n", info->num_pages_allocated, info->num_pages_max));
+
+	up(&info->mutex);
+
+	return 1; /* success*/
+}
+
+
+/*
+ * Free specified UMP memory
+ */
+static void os_free(void* ctx, ump_dd_mem * descriptor)
+{
+	os_allocator * info;
+	int i;
+
+	BUG_ON(!ctx);
+	BUG_ON(!descriptor);
+
+	info = (os_allocator*)ctx;
+
+	BUG_ON(descriptor->nr_blocks > info->num_pages_allocated);
+
+	if (down_interruptible(&info->mutex))
+	{
+		DBG_MSG(1, ("Failed to get mutex in os_free\n"));
+		return;
+	}
+
+	DBG_MSG(5, ("Releasing %lu OS pages\n", descriptor->nr_blocks));
+
+	info->num_pages_allocated -= descriptor->nr_blocks;
+
+	up(&info->mutex);
+
+	for ( i = 0; i < descriptor->nr_blocks; i++)
+	{
+		DBG_MSG(6, ("Freeing physical page. Address: 0x%08lx\n", descriptor->block_array[i].addr));
+		if ( ! descriptor->is_cached)
+		{
+			dma_unmap_page(NULL, descriptor->block_array[i].addr, PAGE_SIZE, DMA_BIDIRECTIONAL);
+		}
+		__free_page(pfn_to_page(descriptor->block_array[i].addr>>PAGE_SHIFT) );
+	}
+
+	vfree(descriptor->block_array);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_os.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_os.h
new file mode 100644
index 00000000000..d6083477d72
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_kernel_memory_backend_os.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_kernel_memory_backend_os.h
+ */
+
+#ifndef __UMP_KERNEL_MEMORY_BACKEND_OS_H__
+#define __UMP_KERNEL_MEMORY_BACKEND_OS_H__
+
+#include "ump_kernel_memory_backend.h"
+
+ump_memory_backend * ump_os_memory_backend_create(const int max_allocation);
+
+#endif /* __UMP_KERNEL_MEMORY_BACKEND_OS_H__ */
+
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_memory_backend.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_memory_backend.c
new file mode 100644
index 00000000000..1c1190a537f
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_memory_backend.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include <linux/module.h>            /* kernel module definitions */
+#include <linux/ioport.h>            /* request_mem_region */
+
+#include "arch/config.h"             /* Configuration for current platform. The symlink for arch is set by Makefile */
+
+#include "ump_osk.h"
+#include "ump_kernel_common.h"
+#include "ump_kernel_memory_backend_os.h"
+#include "ump_kernel_memory_backend_dedicated.h"
+
+/* Configure which dynamic memory allocator to use */
+int ump_backend = ARCH_UMP_BACKEND_DEFAULT;
+module_param(ump_backend, int, S_IRUGO); /* r--r--r-- */
+MODULE_PARM_DESC(ump_backend, "0 = dedicated memory backend (default), 1 = OS memory backend");
+
+/* The base address of the memory block for the dedicated memory backend */
+unsigned int ump_memory_address = ARCH_UMP_MEMORY_ADDRESS_DEFAULT;
+module_param(ump_memory_address, uint, S_IRUGO); /* r--r--r-- */
+MODULE_PARM_DESC(ump_memory_address, "The physical address to map for the dedicated memory backend");
+
+/* The size of the memory block for the dedicated memory backend */
+unsigned int ump_memory_size = ARCH_UMP_MEMORY_SIZE_DEFAULT;
+module_param(ump_memory_size, uint, S_IRUGO); /* r--r--r-- */
+MODULE_PARM_DESC(ump_memory_size, "The size of fixed memory to map in the dedicated memory backend");
+
+ump_memory_backend* ump_memory_backend_create ( void )
+{
+	ump_memory_backend * backend = NULL;
+
+	/* Create the dynamic memory allocator backend */
+	if (0 == ump_backend)
+	{
+		DBG_MSG(2, ("Using dedicated memory backend\n"));
+
+		DBG_MSG(2, ("Requesting dedicated memory: 0x%08x, size: %u\n", ump_memory_address, ump_memory_size));
+		/* Ask the OS if we can use the specified physical memory */
+		if (NULL == request_mem_region(ump_memory_address, ump_memory_size, "UMP Memory"))
+		{
+			MSG_ERR(("Failed to request memory region (0x%08X - 0x%08X). Is Mali DD already loaded?\n", ump_memory_address, ump_memory_address + ump_memory_size - 1));
+			return NULL;
+		}
+		backend = ump_block_allocator_create(ump_memory_address, ump_memory_size);
+	}
+	else if (1 == ump_backend)
+	{
+		DBG_MSG(2, ("Using OS memory backend, allocation limit: %d\n", ump_memory_size));
+		backend = ump_os_memory_backend_create(ump_memory_size);
+	}
+
+	return backend;
+}
+
+void ump_memory_backend_destroy( void )
+{
+	if (0 == ump_backend)
+	{
+		DBG_MSG(2, ("Releasing dedicated memory: 0x%08x\n", ump_memory_address));
+		release_mem_region(ump_memory_address, ump_memory_size);
+	}
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_osk_atomics.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_osk_atomics.c
new file mode 100644
index 00000000000..b3300ab691a
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_osk_atomics.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_osk_atomics.c
+ * Implementation of the OS abstraction layer for the UMP kernel device driver
+ */
+
+#include "ump_osk.h"
+#include <asm/atomic.h>
+
+int _ump_osk_atomic_dec_and_read( _mali_osk_atomic_t *atom )
+{
+	return atomic_dec_return((atomic_t *)&atom->u.val);
+}
+
+int _ump_osk_atomic_inc_and_read( _mali_osk_atomic_t *atom )
+{
+	return atomic_inc_return((atomic_t *)&atom->u.val);
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_osk_low_level_mem.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_osk_low_level_mem.c
new file mode 100644
index 00000000000..7a1c5e97886
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_osk_low_level_mem.c
@@ -0,0 +1,243 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_osk_memory.c
+ * Implementation of the OS abstraction layer for the kernel device driver
+ */
+
+/* needed to detect kernel version specific code */
+#include <linux/version.h>
+
+#include "ump_osk.h"
+#include "ump_uk_types.h"
+#include "ump_ukk.h"
+#include "ump_kernel_common.h"
+#include <linux/module.h>            /* kernel module definitions */
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+
+#include <asm/memory.h>
+#include <asm/cacheflush.h>
+#include <linux/dma-mapping.h>
+
+typedef struct ump_vma_usage_tracker
+{
+	atomic_t references;
+	ump_memory_allocation *descriptor;
+} ump_vma_usage_tracker;
+
+static void ump_vma_open(struct vm_area_struct * vma);
+static void ump_vma_close(struct vm_area_struct * vma);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+static int ump_cpu_page_fault_handler(struct vm_area_struct *vma, struct vm_fault *vmf);
+#else
+static unsigned long ump_cpu_page_fault_handler(struct vm_area_struct * vma, unsigned long address);
+#endif
+
+static struct vm_operations_struct ump_vm_ops =
+{
+	.open = ump_vma_open,
+	.close = ump_vma_close,
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+	.fault = ump_cpu_page_fault_handler
+#else
+	.nopfn = ump_cpu_page_fault_handler
+#endif
+};
+
+/*
+ * Page fault for VMA region
+ * This should never happen since we always map in the entire virtual memory range.
+ */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+static int ump_cpu_page_fault_handler(struct vm_area_struct *vma, struct vm_fault *vmf)
+#else
+static unsigned long ump_cpu_page_fault_handler(struct vm_area_struct * vma, unsigned long address)
+#endif
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+	void __user * address;
+	address = vmf->virtual_address;
+#endif
+	MSG_ERR(("Page-fault in UMP memory region caused by the CPU\n"));
+	MSG_ERR(("VMA: 0x%08lx, virtual address: 0x%08lx\n", (unsigned long)vma, address));
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+	return VM_FAULT_SIGBUS;
+#else
+	return NOPFN_SIGBUS;
+#endif
+}
+
+static void ump_vma_open(struct vm_area_struct * vma)
+{
+	ump_vma_usage_tracker * vma_usage_tracker;
+	int new_val;
+
+	vma_usage_tracker = (ump_vma_usage_tracker*)vma->vm_private_data;
+	BUG_ON(NULL == vma_usage_tracker);
+
+	new_val = atomic_inc_return(&vma_usage_tracker->references);
+
+	DBG_MSG(4, ("VMA open, VMA reference count incremented. VMA: 0x%08lx, reference count: %d\n", (unsigned long)vma, new_val));
+}
+
+static void ump_vma_close(struct vm_area_struct * vma)
+{
+	ump_vma_usage_tracker * vma_usage_tracker;
+	_ump_uk_unmap_mem_s args;
+	int new_val;
+
+	vma_usage_tracker = (ump_vma_usage_tracker*)vma->vm_private_data;
+	BUG_ON(NULL == vma_usage_tracker);
+
+	new_val = atomic_dec_return(&vma_usage_tracker->references);
+
+	DBG_MSG(4, ("VMA close, VMA reference count decremented. VMA: 0x%08lx, reference count: %d\n", (unsigned long)vma, new_val));
+
+	if (0 == new_val)
+	{
+		ump_memory_allocation * descriptor;
+
+		descriptor = vma_usage_tracker->descriptor;
+
+		args.ctx = descriptor->ump_session;
+		args.cookie = descriptor->cookie;
+		args.mapping = descriptor->mapping;
+		args.size = descriptor->size;
+
+		args._ukk_private = NULL; /** @note unused */
+
+		DBG_MSG(4, ("No more VMA references left, releasing UMP memory\n"));
+		_ump_ukk_unmap_mem( & args );
+
+		/* vma_usage_tracker is free()d by _ump_osk_mem_mapregion_term() */
+	}
+}
+
+_mali_osk_errcode_t _ump_osk_mem_mapregion_init( ump_memory_allocation * descriptor )
+{
+	ump_vma_usage_tracker * vma_usage_tracker;
+	struct vm_area_struct *vma;
+
+	if (NULL == descriptor) return _MALI_OSK_ERR_FAULT;
+
+	vma_usage_tracker = kmalloc(sizeof(ump_vma_usage_tracker), GFP_KERNEL);
+	if (NULL == vma_usage_tracker)
+	{
+		DBG_MSG(1, ("Failed to allocate memory for ump_vma_usage_tracker in _mali_osk_mem_mapregion_init\n"));
+		return -_MALI_OSK_ERR_FAULT;
+	}
+
+	vma = (struct vm_area_struct*)descriptor->process_mapping_info;
+	if (NULL == vma ) return _MALI_OSK_ERR_FAULT;
+
+	vma->vm_private_data = vma_usage_tracker;
+	vma->vm_flags |= VM_IO;
+	vma->vm_flags |= VM_RESERVED;
+
+	if (0==descriptor->is_cached)
+	{
+		vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+	}
+	DBG_MSG(3, ("Mapping with page_prot: 0x%x\n", vma->vm_page_prot ));
+
+	/* Setup the functions which handle further VMA handling */
+	vma->vm_ops = &ump_vm_ops;
+
+	/* Do the va range allocation - in this case, it was done earlier, so we copy in that information */
+	descriptor->mapping = (void __user*)vma->vm_start;
+
+	atomic_set(&vma_usage_tracker->references, 1); /*this can later be increased if process is forked, see ump_vma_open() */
+	vma_usage_tracker->descriptor = descriptor;
+
+	return _MALI_OSK_ERR_OK;
+}
+
+void _ump_osk_mem_mapregion_term( ump_memory_allocation * descriptor )
+{
+	struct vm_area_struct* vma;
+	ump_vma_usage_tracker * vma_usage_tracker;
+
+	if (NULL == descriptor) return;
+
+	/* Linux does the right thing as part of munmap to remove the mapping
+	 * All that remains is that we remove the vma_usage_tracker setup in init() */
+	vma = (struct vm_area_struct*)descriptor->process_mapping_info;
+
+	vma_usage_tracker = vma->vm_private_data;
+
+	/* We only get called if mem_mapregion_init succeeded */
+	kfree(vma_usage_tracker);
+	return;
+}
+
+_mali_osk_errcode_t _ump_osk_mem_mapregion_map( ump_memory_allocation * descriptor, u32 offset, u32 * phys_addr, unsigned long size )
+{
+	struct vm_area_struct *vma;
+	_mali_osk_errcode_t retval;
+
+	if (NULL == descriptor) return _MALI_OSK_ERR_FAULT;
+
+	vma = (struct vm_area_struct*)descriptor->process_mapping_info;
+
+	if (NULL == vma ) return _MALI_OSK_ERR_FAULT;
+
+	retval = remap_pfn_range( vma, ((u32)descriptor->mapping) + offset, (*phys_addr) >> PAGE_SHIFT, size, vma->vm_page_prot) ? _MALI_OSK_ERR_FAULT : _MALI_OSK_ERR_OK;;
+
+		DBG_MSG(4, ("Mapping virtual to physical memory. ID: %u, vma: 0x%08lx, virtual addr:0x%08lx, physical addr: 0x%08lx, size:%lu, prot:0x%x, vm_flags:0x%x RETVAL: 0x%x\n",
+		        ump_dd_secure_id_get(descriptor->handle),
+		        (unsigned long)vma,
+		        (unsigned long)(vma->vm_start + offset),
+		        (unsigned long)*phys_addr,
+		        size,
+		        (unsigned int)vma->vm_page_prot, vma->vm_flags, retval));
+
+	return retval;
+}
+
+
+void _ump_osk_msync( ump_dd_mem * mem, ump_uk_msync_op op )
+{
+	int i;
+	DBG_MSG(3, ("Flushing nr of blocks: %u. First: paddr: 0x%08x vaddr: 0x%08x size:%dB\n", mem->nr_blocks, mem->block_array[0].addr, phys_to_virt(mem->block_array[0].addr), mem->block_array[0].size));
+
+	/* TODO: Use args->size and args->address to select a subrange of this allocation to flush */
+	for (i=0 ; i<mem->nr_blocks; i++)
+	{
+		/* TODO: Find out which flush method is best of 1)Dma OR  2)Normal flush functions */
+		/* TODO: Use args->op to select the flushing method: CLEAN_AND_INVALIDATE or CLEAN */
+		/*#define USING_DMA_FLUSH*/
+		#ifdef  USING_DMA_FLUSH
+			DEBUG_ASSERT( (PAGE_SIZE==mem->block_array[i].size));
+			dma_map_page(NULL, pfn_to_page(mem->block_array[i].addr >> PAGE_SHIFT), 0, PAGE_SIZE, DMA_BIDIRECTIONAL );
+			/*dma_unmap_page(NULL, mem->block_array[i].addr, PAGE_SIZE, DMA_BIDIRECTIONAL);*/
+		#else
+			/* Normal style flush */
+			ump_dd_physical_block *block;
+			u32 start_p, end_p;
+			const void *start_v, *end_v;
+			block = &mem->block_array[i];
+
+			start_p   = (u32)block->addr;
+			start_v = phys_to_virt( start_p ) ;
+
+			end_p   = start_p + block->size-1;
+			end_v   = phys_to_virt( end_p ) ;
+
+			dmac_flush_range(start_v, end_v);
+			outer_flush_range(start_p, end_p);
+		#endif
+	}
+
+	return ;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_osk_misc.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_osk_misc.c
new file mode 100644
index 00000000000..78569c4457b
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_osk_misc.c
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_osk_misc.c
+ * Implementation of the OS abstraction layer for the UMP kernel device driver
+ */
+
+
+#include "ump_osk.h"
+
+#include <linux/kernel.h>
+#include "ump_kernel_linux.h"
+
+/* is called from ump_kernel_constructor in common code */
+_mali_osk_errcode_t _ump_osk_init( void )
+{
+	if (0 != ump_kernel_device_initialize())
+	{
+		return _MALI_OSK_ERR_FAULT;
+	}
+
+	return _MALI_OSK_ERR_OK;
+}
+
+_mali_osk_errcode_t _ump_osk_term( void )
+{
+	ump_kernel_device_terminate();
+	return _MALI_OSK_ERR_OK;
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_ref_wrappers.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_ref_wrappers.c
new file mode 100644
index 00000000000..155635026aa
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_ref_wrappers.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_ukk_wrappers.c
+ * Defines the wrapper functions which turn Linux IOCTL calls into _ukk_ calls for the reference implementation
+ */
+
+
+#include <asm/uaccess.h>             /* user space access */
+
+#include "ump_osk.h"
+#include "ump_uk_types.h"
+#include "ump_ukk.h"
+#include "ump_kernel_common.h"
+
+/*
+ * IOCTL operation; Allocate UMP memory
+ */
+int ump_allocate_wrapper(u32 __user * argument, struct ump_session_data  * session_data)
+{
+	_ump_uk_allocate_s user_interaction;
+	_mali_osk_errcode_t err;
+
+	/* Sanity check input parameters */
+	if (NULL == argument || NULL == session_data)
+	{
+		MSG_ERR(("NULL parameter in ump_ioctl_allocate()\n"));
+		return -ENOTTY;
+	}
+
+	/* Copy the user space memory to kernel space (so we safely can read it) */
+	if (0 != copy_from_user(&user_interaction, argument, sizeof(user_interaction)))
+	{
+		MSG_ERR(("copy_from_user() in ump_ioctl_allocate()\n"));
+		return -EFAULT;
+	}
+
+	user_interaction.ctx = (void *) session_data;
+
+	err = _ump_ukk_allocate( &user_interaction );
+	if( _MALI_OSK_ERR_OK != err )
+	{
+		DBG_MSG(1, ("_ump_ukk_allocate() failed in ump_ioctl_allocate()\n"));
+		return map_errcode(err);
+	}
+	user_interaction.ctx = NULL;
+
+	if (0 != copy_to_user(argument, &user_interaction, sizeof(user_interaction)))
+	{
+		/* If the copy fails then we should release the memory. We can use the IOCTL release to accomplish this */
+		_ump_uk_release_s release_args;
+
+		MSG_ERR(("copy_to_user() failed in ump_ioctl_allocate()\n"));
+
+		release_args.ctx = (void *) session_data;
+		release_args.secure_id = user_interaction.secure_id;
+
+		err = _ump_ukk_release( &release_args );
+		if(_MALI_OSK_ERR_OK != err)
+		{
+			MSG_ERR(("_ump_ukk_release() also failed when trying to release newly allocated memory in ump_ioctl_allocate()\n"));
+		}
+
+		return -EFAULT;
+	}
+
+	return 0; /* success */
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_ref_wrappers.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_ref_wrappers.h
new file mode 100644
index 00000000000..b2bef1152c3
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_ref_wrappers.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_ukk_wrappers.h
+ * Defines the wrapper functions which turn Linux IOCTL calls into _ukk_ calls for the reference implementation
+ */
+
+#ifndef __UMP_UKK_REF_WRAPPERS_H__
+#define __UMP_UKK_REF_WRAPPERS_H__
+
+#include <linux/kernel.h>
+#include "ump_kernel_common.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+
+int ump_allocate_wrapper(u32 __user * argument, struct ump_session_data  * session_data);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __UMP_UKK_REF_WRAPPERS_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_wrappers.c b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_wrappers.c
new file mode 100644
index 00000000000..d14b631246c
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_wrappers.c
@@ -0,0 +1,173 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_ukk_wrappers.c
+ * Defines the wrapper functions which turn Linux IOCTL calls into _ukk_ calls
+ */
+
+#include <asm/uaccess.h>             /* user space access */
+
+#include "ump_osk.h"
+#include "ump_uk_types.h"
+#include "ump_ukk.h"
+#include "ump_kernel_common.h"
+
+/*
+ * IOCTL operation; Negotiate version of IOCTL API
+ */
+int ump_get_api_version_wrapper(u32 __user * argument, struct ump_session_data * session_data)
+{
+	_ump_uk_api_version_s version_info;
+	_mali_osk_errcode_t err;
+
+	/* Sanity check input parameters */
+	if (NULL == argument || NULL == session_data)
+	{
+		MSG_ERR(("NULL parameter in ump_ioctl_get_api_version()\n"));
+		return -ENOTTY;
+	}
+
+	/* Copy the user space memory to kernel space (so we safely can read it) */
+	if (0 != copy_from_user(&version_info, argument, sizeof(version_info)))
+	{
+		MSG_ERR(("copy_from_user() in ump_ioctl_get_api_version()\n"));
+		return -EFAULT;
+	}
+
+	version_info.ctx = (void*) session_data;
+	err = _ump_uku_get_api_version( &version_info );
+	if( _MALI_OSK_ERR_OK != err )
+	{
+		MSG_ERR(("_ump_uku_get_api_version() failed in ump_ioctl_get_api_version()\n"));
+		return map_errcode(err);
+	}
+
+	version_info.ctx = NULL;
+
+	/* Copy ouput data back to user space */
+	if (0 != copy_to_user(argument, &version_info, sizeof(version_info)))
+	{
+		MSG_ERR(("copy_to_user() failed in ump_ioctl_get_api_version()\n"));
+		return -EFAULT;
+	}
+
+	return 0; /* success */
+}
+
+
+/*
+ * IOCTL operation; Release reference to specified UMP memory.
+ */
+int ump_release_wrapper(u32 __user * argument, struct ump_session_data  * session_data)
+{
+	_ump_uk_release_s release_args;
+	_mali_osk_errcode_t err;
+
+	/* Sanity check input parameters */
+	if (NULL == session_data)
+	{
+		MSG_ERR(("NULL parameter in ump_ioctl_release()\n"));
+		return -ENOTTY;
+	}
+
+	/* Copy the user space memory to kernel space (so we safely can read it) */
+	if (0 != copy_from_user(&release_args, argument, sizeof(release_args)))
+	{
+		MSG_ERR(("copy_from_user() in ump_ioctl_get_api_version()\n"));
+		return -EFAULT;
+	}
+
+	release_args.ctx = (void*) session_data;
+	err = _ump_ukk_release( &release_args );
+	if( _MALI_OSK_ERR_OK != err )
+	{
+		MSG_ERR(("_ump_ukk_release() failed in ump_ioctl_release()\n"));
+		return map_errcode(err);
+	}
+
+
+	return 0; /* success */
+}
+
+/*
+ * IOCTL operation; Return size for specified UMP memory.
+ */
+int ump_size_get_wrapper(u32 __user * argument, struct ump_session_data  * session_data)
+{
+	_ump_uk_size_get_s user_interaction;
+	_mali_osk_errcode_t err;
+
+	/* Sanity check input parameters */
+	if (NULL == argument || NULL == session_data)
+	{
+		MSG_ERR(("NULL parameter in ump_ioctl_size_get()\n"));
+		return -ENOTTY;
+	}
+
+	if (0 != copy_from_user(&user_interaction, argument, sizeof(user_interaction)))
+	{
+		MSG_ERR(("copy_from_user() in ump_ioctl_size_get()\n"));
+		return -EFAULT;
+	}
+
+	user_interaction.ctx = (void *) session_data;
+	err = _ump_ukk_size_get( &user_interaction );
+	if( _MALI_OSK_ERR_OK != err )
+	{
+		MSG_ERR(("_ump_ukk_size_get() failed in ump_ioctl_size_get()\n"));
+		return map_errcode(err);
+	}
+
+	user_interaction.ctx = NULL;
+
+	if (0 != copy_to_user(argument, &user_interaction, sizeof(user_interaction)))
+	{
+		MSG_ERR(("copy_to_user() failed in ump_ioctl_size_get()\n"));
+		return -EFAULT;
+	}
+
+	return 0; /* success */
+}
+
+/*
+ * IOCTL operation; Return size for specified UMP memory.
+ */
+ int ump_msync_wrapper(u32 __user * argument, struct ump_session_data  * session_data)
+{
+	_ump_uk_msync_s user_interaction;
+
+	/* Sanity check input parameters */
+	if (NULL == argument || NULL == session_data)
+	{
+		MSG_ERR(("NULL parameter in ump_ioctl_size_get()\n"));
+		return -ENOTTY;
+	}
+
+	if (0 != copy_from_user(&user_interaction, argument, sizeof(user_interaction)))
+	{
+		MSG_ERR(("copy_from_user() in ump_ioctl_msync()\n"));
+		return -EFAULT;
+	}
+
+	user_interaction.ctx = (void *) session_data;
+
+	_ump_ukk_msync( &user_interaction );
+
+	user_interaction.ctx = NULL;
+
+	if (0 != copy_to_user(argument, &user_interaction, sizeof(user_interaction)))
+	{
+		MSG_ERR(("copy_to_user() failed in ump_ioctl_msync()\n"));
+		return -EFAULT;
+	}
+
+	return 0; /* success */
+}
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_wrappers.h b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_wrappers.h
new file mode 100644
index 00000000000..31afe2dca56
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/linux/ump_ukk_wrappers.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2010-2011 ARM Limited. All rights reserved.
+ * 
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ * 
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file ump_ukk_wrappers.h
+ * Defines the wrapper functions which turn Linux IOCTL calls into _ukk_ calls
+ */
+
+#ifndef __UMP_UKK_WRAPPERS_H__
+#define __UMP_UKK_WRAPPERS_H__
+
+#include <linux/kernel.h>
+#include "ump_kernel_common.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+
+
+int ump_get_api_version_wrapper(u32 __user * argument, struct ump_session_data * session_data);
+int ump_release_wrapper(u32 __user * argument, struct ump_session_data  * session_data);
+int ump_size_get_wrapper(u32 __user * argument, struct ump_session_data  * session_data);
+int ump_msync_wrapper(u32 __user * argument, struct ump_session_data  * session_data);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+
+#endif /* __UMP_UKK_WRAPPERS_H__ */
diff --git a/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/readme.txt b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/readme.txt
new file mode 100644
index 00000000000..bf1bf61d60e
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/driver/src/devicedrv/ump/readme.txt
@@ -0,0 +1,17 @@
+Building the UMP Device Driver for Linux
+----------------------------------------
+
+Build the UMP Device Driver for Linux by running the following make command:
+
+KDIR=<kdir_path> CONFIG=<your_config> make
+
+where
+    kdir_path: Path to your Linux Kernel directory
+    your_config: Name of the sub-folder to find the required config.h file
+                 ("arch-" will be prepended)
+
+The config.h file contains the configuration parameters needed, like the
+memory backend to use, and the amount of memory.
+
+The result will be a ump.ko file, which can be loaded into the Linux kernel
+by using the insmod command.
diff --git a/drivers/gpu/mali/mali400ko/mali.spec b/drivers/gpu/mali/mali400ko/mali.spec
new file mode 100644
index 00000000000..62e8fa0e1b4
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/mali.spec
@@ -0,0 +1,57 @@
+%define kernel_target           u8500
+%define kernel_version  	%(find /lib/modules -name "*%{kernel_target}" | cut -c 14-)
+
+Name: mali400ko
+License: GPL
+Summary: Mali400 kernel module
+Version: 1.0
+Release: 1
+URL: http://stericsson.com
+BuildRoot: %{_tmppath}/%{name}
+Requires: kernel
+BuildRequires: kernel-u8500-devel
+Requires(post): ldconfig
+Requires(postun): ldconfig
+
+Source0: mali400ko.tar.gz
+
+%description
+Mali400 kernel module.
+
+%files
+%defattr(-,root,root)
+/lib/modules/%{kernel_version}/extra/mali.ko
+/lib/modules/%{kernel_version}/extra/mali_drm.ko
+
+%prep
+%setup -q
+
+%build
+export CROSS_COMPILE=""
+export KERNELDIR=/usr/src/kernels/%{kernel_version}
+export KERNEL_BUILD_DIR=/usr/src/kernels/%{kernel_version}
+export USING_UMP=1
+export USING_HWMEM=1
+#make V=0 mali-devicedrv
+
+%install
+export CROSS_COMPILE=""
+export KERNELDIR=/usr/src/kernels/%{kernel_version}
+export KERNEL_BUILD_DIR=/usr/src/kernels/%{kernel_version}
+export USING_UMP=1
+export USING_HWMEM=1
+export INSTALL_MOD_DIR=extra
+export INSTALL_MOD_PATH=$RPM_BUILD_ROOT
+make V=0 install-mali install-mali_drm
+
+# Remove kernel modules.* files
+rm -f $RPM_BUILD_ROOT/lib/modules/%{kernel_version}/modules.*
+
+%clean
+rm -rf $RPM_BUILD_ROOT
+
+%post
+
+%postun
+
+
diff --git a/drivers/gpu/mali/mali400ko/x11/mali_drm/README.txt b/drivers/gpu/mali/mali400ko/x11/mali_drm/README.txt
new file mode 100644
index 00000000000..47009fe7686
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/x11/mali_drm/README.txt
@@ -0,0 +1,24 @@
+Notes on integrating the Mali DRM module:
+
+The Mali DRM is a platform device, meaning that you have to add an entry for it in your kernel architecture specification.
+
+Example: (arch/arm/mach-<platform>/mach-<platform>.c)
+
+#ifdef CONFIG_DRM_MALI
+static struct platform_device <platform>_device_mali_drm = {
+	.name = "mali_drm",
+	.id   = -1,
+};
+#endif
+
+static struct platform_device *<platform>_devices[] __initdata = {
+...
+#ifdef CONFIG_DRM_MALI
+	&<platform>_device_mali_drm,
+#endif
+...
+};
+
+Where <platform> is substituted with the selected platform.
+
+The "mali" folder should be placed under drivers/gpu/drm/
diff --git a/drivers/gpu/mali/mali400ko/x11/mali_drm/mali/Makefile b/drivers/gpu/mali/mali400ko/x11/mali_drm/mali/Makefile
new file mode 100644
index 00000000000..0f3ace966df
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/x11/mali_drm/mali/Makefile
@@ -0,0 +1,17 @@
+#
+# * Copyright (C) 2010 ARM Limited. All rights reserved.
+# *
+# * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+# * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+# *
+# * A copy of the licence is included with the program, and can also be obtained from Free Software
+# * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+#
+
+#
+# Makefile for the Mali drm device driver.  This driver provides support for the
+# Direct Rendering Infrastructure (DRI) in XFree86 4.1.0 and higher.
+
+obj-y += mali_drm.o
+mali_drm-objs := mali_drv.o
+EXTRA_CFLAGS += -I$(KBUILD_EXTMOD) -I$(KBUILD_EXTMOD)/include -I$(KBUILD_EXTMOD)/../drm/include/
diff --git a/drivers/gpu/mali/mali400ko/x11/mali_drm/mali/mali_drv.c b/drivers/gpu/mali/mali400ko/x11/mali_drm/mali/mali_drv.c
new file mode 100644
index 00000000000..571ab04e1e2
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/x11/mali_drm/mali/mali_drv.c
@@ -0,0 +1,158 @@
+/**
+ * Copyright (C) 2010 ARM Limited. All rights reserved.
+ *
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ *
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+/**
+ * @file mali_drv.c
+ * Implementation of the Linux device driver entrypoints for Mali DRM
+ */
+#include <linux/module.h>
+#include <linux/vermagic.h>
+#include <drm/drmP.h>
+#include "mali_drv.h"
+
+static struct platform_device *dev0;
+
+void mali_drm_preclose(struct drm_device *dev, struct drm_file *file_priv)
+{
+}
+
+void mali_drm_lastclose(struct drm_device *dev)
+{
+}
+
+static int mali_drm_suspend(struct drm_device *dev, pm_message_t state)
+{
+	return 0;
+}
+
+static int mali_drm_resume(struct drm_device *dev)
+{
+	return 0;
+}
+
+static int mali_drm_load(struct drm_device *dev, unsigned long chipset)
+{
+	return 0;
+}
+
+static int mali_drm_unload(struct drm_device *dev)
+{
+	return 0;
+}
+
+static const struct file_operations mali_driver_fops =
+{
+	.owner = THIS_MODULE,
+	.open = drm_open,
+	.release = drm_release,
+	.unlocked_ioctl = drm_ioctl,
+	.mmap = drm_mmap,
+	.poll = drm_poll,
+	.fasync = drm_fasync,
+};
+
+static struct drm_driver driver =
+{
+	.load = mali_drm_load,
+	.unload = mali_drm_unload,
+	.context_dtor = NULL,
+	.reclaim_buffers = NULL,
+	.reclaim_buffers_idlelocked = NULL,
+	.preclose = mali_drm_preclose,
+	.lastclose = mali_drm_lastclose,
+	.suspend = mali_drm_suspend,
+	.resume = mali_drm_resume,
+	.ioctls = NULL,
+	.fops = &mali_driver_fops,
+	.name = DRIVER_NAME,
+	.desc = DRIVER_DESC,
+	.date = DRIVER_DATE,
+	.major = DRIVER_MAJOR,
+	.minor = DRIVER_MINOR,
+	.patchlevel = DRIVER_PATCHLEVEL,
+};
+
+static struct platform_driver platform_drm_driver;
+
+int mali_drm_init(struct platform_device *dev)
+{
+	printk(KERN_INFO "Mali DRM initialize, driver name: %s, version %d.%d\n", DRIVER_NAME, DRIVER_MAJOR, DRIVER_MINOR);
+	if (dev == dev0) {
+		driver.num_ioctls = 0;
+		return drm_platform_init(&driver, dev0);
+	}
+	return 0;
+
+}
+
+void mali_drm_exit(struct platform_device *dev)
+{
+	if (dev0 == dev)
+		drm_platform_exit(&driver, dev);
+}
+
+static int __devinit mali_platform_drm_probe(struct platform_device *dev)
+{
+	return mali_drm_init(dev);
+}
+
+static int mali_platform_drm_remove(struct platform_device *dev)
+{
+	mali_drm_exit(dev);
+
+	return 0;
+}
+
+static int mali_platform_drm_suspend(struct platform_device *dev, pm_message_t state)
+{
+	return 0;
+}
+
+static int mali_platform_drm_resume(struct platform_device *dev)
+{
+	return 0;
+}
+
+static struct platform_driver platform_drm_driver = {
+	.probe = mali_platform_drm_probe,
+	.remove = __devexit_p(mali_platform_drm_remove),
+	.suspend = mali_platform_drm_suspend,
+	.resume = mali_platform_drm_resume,
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = DRIVER_NAME,
+	},
+};
+
+static int __init mali_platform_drm_init(void)
+{
+	dev0 = platform_device_register_simple("mali_drm", 0, NULL, 0);
+	return platform_driver_register(&platform_drm_driver);
+}
+
+static void __exit mali_platform_drm_exit(void)
+{
+	platform_driver_unregister(&platform_drm_driver);
+	platform_device_unregister(dev0);
+}
+
+#ifdef MODULE
+module_init(mali_platform_drm_init);
+#else
+late_initcall(mali_platform_drm_init);
+#endif
+module_exit(mali_platform_drm_exit);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_LICENSE(DRIVER_LICENSE);
+MODULE_ALIAS(DRIVER_ALIAS);
+MODULE_INFO(vermagic, VERMAGIC_STRING);
diff --git a/drivers/gpu/mali/mali400ko/x11/mali_drm/mali/mali_drv.h b/drivers/gpu/mali/mali400ko/x11/mali_drm/mali/mali_drv.h
new file mode 100644
index 00000000000..188f427ee6d
--- /dev/null
+++ b/drivers/gpu/mali/mali400ko/x11/mali_drm/mali/mali_drv.h
@@ -0,0 +1,25 @@
+/**
+ * Copyright (C) 2010 ARM Limited. All rights reserved.
+ *
+ * This program is free software and is provided to you under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation, and any use by you of this program is subject to the terms of such GNU licence.
+ *
+ * A copy of the licence is included with the program, and can also be obtained from Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef _MALI_DRV_H_
+#define _MALI_DRV_H_
+
+#define DRIVER_AUTHOR	"ARM Ltd."
+#define DRIVER_NAME		"mali_drm"
+#define DRIVER_DESC		"DRM module for Mali-200, Mali-400"
+#define DRIVER_LICENSE  "GPL v2"
+#define DRIVER_ALIAS    "platform:mali_drm"
+#define DRIVER_DATE		"20101111"
+#define DRIVER_VERSION  "0.2"
+#define DRIVER_MAJOR 2
+#define DRIVER_MINOR 1
+#define DRIVER_PATCHLEVEL 1
+
+#endif /* _MALI_DRV_H_ */
diff --git a/drivers/hsi/Kconfig b/drivers/hsi/Kconfig
index d94e38dd80c..f053858683c 100644
--- a/drivers/hsi/Kconfig
+++ b/drivers/hsi/Kconfig
@@ -15,5 +15,6 @@ config HSI_BOARDINFO
 	default y
 
 source "drivers/hsi/clients/Kconfig"
+source "drivers/hsi/controllers/Kconfig"
 
 endif # HSI
diff --git a/drivers/hsi/Makefile b/drivers/hsi/Makefile
index 9d5d33f90de..586e5e0729e 100644
--- a/drivers/hsi/Makefile
+++ b/drivers/hsi/Makefile
@@ -4,3 +4,4 @@
 obj-$(CONFIG_HSI_BOARDINFO)	+= hsi_boardinfo.o
 obj-$(CONFIG_HSI)		+= hsi.o
 obj-y				+= clients/
+obj-y				+= controllers/
diff --git a/drivers/hsi/clients/Kconfig b/drivers/hsi/clients/Kconfig
index 3bacd275f47..46eef1f77fd 100644
--- a/drivers/hsi/clients/Kconfig
+++ b/drivers/hsi/clients/Kconfig
@@ -11,3 +11,9 @@ config HSI_CHAR
 	  If you say Y here, you will enable the HSI/SSI character driver.
 	  This driver provides a simple character device interface for
 	  serial communication with the cellular modem over HSI/SSI bus.
+config HSI_CAIF
+	tristate "CAIF HSI driver"
+	depends on HSI
+	default n
+	---help---
+	  Provides HSI-CAIF glue layer
diff --git a/drivers/hsi/clients/Makefile b/drivers/hsi/clients/Makefile
index 327c0e27c8b..dfe33584975 100644
--- a/drivers/hsi/clients/Makefile
+++ b/drivers/hsi/clients/Makefile
@@ -3,3 +3,4 @@
 #
 
 obj-$(CONFIG_HSI_CHAR)	+= hsi_char.o
+obj-$(CONFIG_HSI_CAIF)	+= cfhsi.o
diff --git a/drivers/hsi/clients/cfhsi.c b/drivers/hsi/clients/cfhsi.c
new file mode 100644
index 00000000000..cf7ce0cb1cb
--- /dev/null
+++ b/drivers/hsi/clients/cfhsi.c
@@ -0,0 +1,318 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author:	Daniel Martensson <Daniel.Martensson@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/scatterlist.h>
+
+#include <net/caif/caif_hsi.h>
+
+#include <linux/hsi/hsi.h>
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Daniel Martensson<daniel.martensson@stericsson.com>");
+MODULE_DESCRIPTION("CAIF HSI V3 glue");
+
+#define NR_OF_CAIF_HSI_CHANNELS	2
+
+struct cfhsi_v3 {
+	struct list_head list;
+	struct cfhsi_dev dev;
+	struct platform_device pdev;
+	struct hsi_msg *tx_msg;
+	struct hsi_msg *rx_msg;
+};
+
+/* TODO: Lists are not protected with regards to device removal. */
+static LIST_HEAD(cfhsi_dev_list);
+
+static struct hsi_client *cfhsi_client;
+
+static int cfhsi_tx(u8 *ptr, int len, struct cfhsi_dev *dev)
+{
+	int res;
+	struct cfhsi_v3 *cfhsi = NULL;
+
+	/* Check length and alignment. */
+	BUG_ON(((int)ptr)%4);
+	BUG_ON(len%4);
+
+	cfhsi = container_of(dev, struct cfhsi_v3, dev);
+
+	sg_init_one(cfhsi->tx_msg->sgt.sgl, (const void *)ptr,
+		    (unsigned int)len);
+
+	/* Write on HSI device. */
+	res = hsi_async_write(cfhsi_client, cfhsi->tx_msg);
+
+	return res;
+}
+
+static int cfhsi_rx(u8 *ptr, int len, struct cfhsi_dev *dev)
+{
+	int res;
+	struct cfhsi_v3 *cfhsi = NULL;
+
+	/* Check length and alignment. */
+	BUG_ON(((int)ptr)%4);
+	BUG_ON(len%4);
+
+	cfhsi = container_of(dev, struct cfhsi_v3, dev);
+
+	sg_init_one(cfhsi->rx_msg->sgt.sgl, (const void *)ptr,
+		    (unsigned int)len);
+
+	/* Read from HSI device. */
+	res = hsi_async_read(cfhsi_client, cfhsi->rx_msg);
+
+	return res;
+}
+
+void cfhsi_v3_release(struct device *dev)
+{
+	pr_warning("%s:%d cfhsi_v3_release called\n", __FILE__, __LINE__);
+}
+
+static inline void cfhsi_v3_destructor(struct hsi_msg *msg)
+{
+	pr_warning("%s:%d cfhsi_v3_destructor called\n", __FILE__, __LINE__);
+}
+
+static inline void cfhsi_v3_read_cb(struct hsi_msg *msg)
+{
+	struct cfhsi_v3 *cfhsi = (struct cfhsi_v3 *)msg->context;
+
+	/* TODO: Error checking. */
+	BUG_ON(!cfhsi->dev.drv);
+	BUG_ON(!cfhsi->dev.drv->rx_done_cb);
+
+	cfhsi->dev.drv->rx_done_cb(cfhsi->dev.drv);
+}
+
+static inline void cfhsi_v3_write_cb(struct hsi_msg *msg)
+{
+	struct cfhsi_v3 *cfhsi = (struct cfhsi_v3 *)msg->context;
+
+	/* TODO: Error checking. */
+	BUG_ON(!cfhsi->dev.drv);
+	BUG_ON(!cfhsi->dev.drv->tx_done_cb);
+
+	cfhsi->dev.drv->tx_done_cb(cfhsi->dev.drv);
+}
+
+static int hsi_proto_probe(struct device *dev)
+{
+	int res;
+	int i;
+	struct cfhsi_v3 *cfhsi = NULL;
+
+	if (cfhsi_client)
+		return -ENODEV; /* TODO: Not correct return. */
+
+	cfhsi_client = to_hsi_client(dev);
+
+	res = hsi_claim_port(cfhsi_client, 0);
+	if (res) {
+		pr_warning("hsi_proto_probe: hsi_claim_port:%d.\n", res);
+		goto err_hsi_claim;
+	}
+
+	/* Right now we don't care about AC_WAKE (No power management). */
+	cfhsi_client->hsi_start_rx = NULL;
+	cfhsi_client->hsi_stop_rx = NULL;
+
+	/* CAIF HSI TX configuration. */
+	cfhsi_client->tx_cfg.mode =  HSI_MODE_STREAM;
+	cfhsi_client->tx_cfg.flow = HSI_FLOW_SYNC;
+	cfhsi_client->tx_cfg.channels = NR_OF_CAIF_HSI_CHANNELS;
+	cfhsi_client->tx_cfg.speed = 100000; /* TODO: What speed should be used. */
+	cfhsi_client->tx_cfg.arb_mode = HSI_ARB_RR;
+
+	/* CAIF HSI RX configuration. */
+	cfhsi_client->rx_cfg.mode =  HSI_MODE_STREAM;
+	cfhsi_client->rx_cfg.flow = HSI_FLOW_SYNC;
+	cfhsi_client->rx_cfg.channels = NR_OF_CAIF_HSI_CHANNELS;
+	cfhsi_client->rx_cfg.speed = 200000; /* TODO: What speed should be used. */
+	cfhsi_client->rx_cfg.arb_mode = HSI_ARB_RR;
+
+	res = hsi_setup(cfhsi_client);
+	if (res) {
+		pr_warning("hsi_proto_probe: hsi_setup:%d.\n", res);
+		goto err_hsi_setup;
+	}
+
+	/* Make sure that AC_WAKE is high (No power management). */
+	res = hsi_start_tx(cfhsi_client);
+	if (res) {
+		pr_warning("hsi_proto_probe: hsi_start_tx:%d.\n", res);
+		goto err_hsi_start_tx;
+	}
+
+	/* Connect channels to CAIF HSI devices. */
+	for (i = 0; i < NR_OF_CAIF_HSI_CHANNELS; i++) {
+		cfhsi = kzalloc(sizeof(struct cfhsi_v3), GFP_KERNEL);
+		if (!cfhsi) {
+			res = -ENOMEM;
+			/* TODO: Error handling. */
+		}
+
+		/* Assign HSI client to this CAIF HSI device. */
+		cfhsi->dev.cfhsi_tx = cfhsi_tx;
+		cfhsi->dev.cfhsi_rx = cfhsi_rx;
+
+		/* Allocate HSI messages. */
+		cfhsi->tx_msg = hsi_alloc_msg(1, GFP_KERNEL);
+		cfhsi->rx_msg = hsi_alloc_msg(1, GFP_KERNEL);
+		if (!cfhsi->tx_msg || !cfhsi->rx_msg) {
+			res = -ENOMEM;
+			/* TODO: Error handling. */
+		}
+
+		/* Set up TX message. */
+		cfhsi->tx_msg->cl = cfhsi_client;
+		cfhsi->tx_msg->context = (void *)cfhsi;
+		cfhsi->tx_msg->complete = cfhsi_v3_write_cb;
+		cfhsi->tx_msg->destructor = cfhsi_v3_destructor;
+		cfhsi->tx_msg->channel = i;
+		cfhsi->tx_msg->ttype = HSI_MSG_WRITE;
+		cfhsi->tx_msg->break_frame = 0; /* No break frame. */
+
+		/* Set up RX message. */
+		cfhsi->rx_msg->cl = cfhsi_client;
+		cfhsi->rx_msg->context = (void *)cfhsi;
+		cfhsi->rx_msg->complete = cfhsi_v3_read_cb;
+		cfhsi->rx_msg->destructor = cfhsi_v3_destructor;
+		cfhsi->rx_msg->channel = i;
+		cfhsi->rx_msg->ttype = HSI_MSG_READ;
+		cfhsi->rx_msg->break_frame = 0; /* No break frame. */
+
+		/* Initialize CAIF HSI platform device. */
+		cfhsi->pdev.name = "cfhsi";
+		cfhsi->pdev.dev.platform_data = &cfhsi->dev;
+		cfhsi->pdev.dev.release = cfhsi_v3_release;
+		/* Use channel number as id. */
+		cfhsi->pdev.id = i;
+		/* Register platform device. */
+		res = platform_device_register(&cfhsi->pdev);
+		if (res) {
+			pr_warning("hsi_proto_probe: plat_dev_reg:%d.\n", res);
+			res = -ENODEV;
+			/* TODO: Error handling. */
+		}
+
+		/* Add HSI device to device list. */
+		list_add_tail(&cfhsi->list, &cfhsi_dev_list);
+	}
+
+	return res;
+
+ err_hsi_start_tx:
+ err_hsi_setup:
+	hsi_release_port(cfhsi_client);
+ err_hsi_claim:
+	cfhsi_client = NULL;
+
+	return res;
+}
+
+static int hsi_proto_remove(struct device *dev)
+{
+	struct cfhsi_v3 *cfhsi = NULL;
+	struct list_head *list_node;
+	struct list_head *n;
+
+	if (!cfhsi_client)
+		return -ENODEV;
+
+	list_for_each_safe(list_node, n, &cfhsi_dev_list) {
+		cfhsi = list_entry(list_node, struct cfhsi_v3, list);
+		/* Remove from list. */
+		list_del(list_node);
+		/* Our HSI device is gone, unregister CAIF HSI device. */
+		platform_device_del(&cfhsi->pdev);
+		hsi_free_msg(cfhsi->tx_msg);
+		hsi_free_msg(cfhsi->rx_msg);
+		/* Free memory. */
+		kfree(cfhsi);
+	}
+
+	hsi_stop_tx(cfhsi_client);
+	hsi_release_port(cfhsi_client);
+
+	cfhsi_client = NULL;
+
+	return 0;
+}
+
+static int hsi_proto_suspend(struct device *dev, pm_message_t mesg)
+{
+	/* Not handled. */
+	pr_info("hsi_proto_suspend.\n");
+
+	return 0;
+}
+
+static int hsi_proto_resume(struct device *dev)
+{
+	/* Not handled. */
+	pr_info("hsi_proto_resume.\n");
+
+	return 0;
+}
+
+static struct hsi_client_driver cfhsi_v3_driver = {
+	.driver = {
+		.name = "cfhsi_v3_driver",
+		.owner	= THIS_MODULE,
+		.probe = hsi_proto_probe,
+		.remove = __devexit_p(hsi_proto_remove),
+		.suspend = hsi_proto_suspend,
+		.resume = hsi_proto_resume,
+	},
+};
+
+static int __init cfhsi_v3_init(void)
+{
+	int res;
+
+	/* Register protocol driver for HSI interface. */
+	res =  hsi_register_client_driver(&cfhsi_v3_driver);
+	if (res)
+		pr_warning("Failed to register CAIF HSI V3 driver.\n");
+
+	return res;
+}
+
+static void __exit cfhsi_v3_exit(void)
+{
+	struct cfhsi_v3 *cfhsi = NULL;
+	struct list_head *list_node;
+	struct list_head *n;
+
+	/* Unregister driver. */
+	hsi_unregister_client_driver(&cfhsi_v3_driver);
+
+	if (!cfhsi_client)
+		return;
+
+	list_for_each_safe(list_node, n, &cfhsi_dev_list) {
+		cfhsi = list_entry(list_node, struct cfhsi_v3, list);
+		platform_device_del(&cfhsi->pdev);
+		hsi_free_msg(cfhsi->tx_msg);
+		hsi_free_msg(cfhsi->rx_msg);
+		kfree(cfhsi);
+	}
+
+	hsi_stop_tx(cfhsi_client);
+	hsi_release_port(cfhsi_client);
+
+	cfhsi_client = NULL;
+}
+
+module_init(cfhsi_v3_init);
+module_exit(cfhsi_v3_exit);
diff --git a/drivers/hsi/controllers/Kconfig b/drivers/hsi/controllers/Kconfig
new file mode 100644
index 00000000000..76d339eaf32
--- /dev/null
+++ b/drivers/hsi/controllers/Kconfig
@@ -0,0 +1,33 @@
+#
+# HSI controllers configuration
+#
+comment "HSI controllers"
+
+config STE_HSI
+       tristate "STE HSI controller driver"
+       depends on (ARCH_U8500 || ARCH_NOMADIK) && HSI
+       default n
+       help
+         ST-Ericsson HSI controller.
+         If you say Y here, you will enable the U8500 HSI hardware driver.
+
+         If unsure, say N.
+
+config OMAP_SSI
+	tristate "OMAP SSI hardware driver"
+	depends on ARCH_OMAP && HSI
+	default n
+	---help---
+	  SSI is a legacy version of HSI. It is usually used to connect
+	  an application engine with a cellular modem.
+	  If you say Y here, you will enable the OMAP SSI hardware driver.
+
+	  If unsure, say N.
+
+if OMAP_SSI
+
+config OMAP_SSI_CONFIG
+	boolean
+	default y
+
+endif # OMAP_SSI
diff --git a/drivers/hsi/controllers/Makefile b/drivers/hsi/controllers/Makefile
new file mode 100644
index 00000000000..475637a0f23
--- /dev/null
+++ b/drivers/hsi/controllers/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for HSI controllers drivers
+#
+
+obj-$(CONFIG_STE_HSI)		+= ste_hsi.o
+obj-$(CONFIG_OMAP_SSI)		+= omap_ssi.o
diff --git a/drivers/hsi/controllers/omap_ssi.c b/drivers/hsi/controllers/omap_ssi.c
new file mode 100644
index 00000000000..a82ea0e13cc
--- /dev/null
+++ b/drivers/hsi/controllers/omap_ssi.c
@@ -0,0 +1,1853 @@
+/*
+ * omap_ssi.c
+ *
+ * Implements the OMAP SSI driver.
+ *
+ * Copyright (C) 2010 Nokia Corporation. All rights reserved.
+ *
+ * Contact: Carlos Chinea <carlos.chinea@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ */
+#include <linux/compiler.h>
+#include <linux/err.h>
+#include <linux/ioport.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/seq_file.h>
+#include <linux/scatterlist.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/hsi/hsi.h>
+#include <linux/debugfs.h>
+#include <plat/omap-pm.h>
+#include <plat/clock.h>
+#include <plat/ssi.h>
+
+#define SSI_MAX_CHANNELS	8
+#define SSI_MAX_GDD_LCH		8
+#define SSI_BYTES_TO_FRAMES(x) ((((x) - 1) >> 2) + 1)
+
+/**
+ * struct ssi_clk_res - Device resource data for the SSI clocks
+ * @clk: Pointer to the clock
+ * @nb: Pointer to the clock notifier for clk, if any
+ */
+struct ssi_clk_res {
+	struct clk *clk;
+	struct notifier_block *nb;
+};
+
+/**
+ * struct gdd_trn - GDD transaction data
+ * @msg: Pointer to the HSI message being served
+ * @sg: Pointer to the current sg entry being served
+ */
+struct gdd_trn {
+	struct hsi_msg		*msg;
+	struct scatterlist	*sg;
+};
+
+/**
+ * struct omap_ssm_ctx - OMAP synchronous serial module (TX/RX) context
+ * @mode: Bit transmission mode
+ * @channels: Number of channels
+ * @framesize: Frame size in bits
+ * @timeout: RX frame timeout
+ * @divisor: TX divider
+ * @arb_mode: Arbitration mode for TX frame (Round robin, priority)
+ */
+struct omap_ssm_ctx {
+	u32	mode;
+	u32	channels;
+	u32	frame_size;
+	union	{
+			u32	timeout; /* Rx Only */
+			struct	{
+					u32	arb_mode;
+					u32	divisor;
+			}; /* Tx only */
+	};
+};
+
+/**
+ * struct omap_ssi_port - OMAP SSI port data
+ * @dev: device associated to the port (HSI port)
+ * @sst_dma: SSI transmitter physical base address
+ * @ssr_dma: SSI receiver physical base address
+ * @sst_base: SSI transmitter base address
+ * @ssr_base: SSI receiver base address
+ * @wk_lock: spin lock to serialize access to the wake lines
+ * @lock: Spin lock to serialize access to the SSI port
+ * @channels: Current number of channels configured (1,2,4 or 8)
+ * @txqueue: TX message queues
+ * @rxqueue: RX message queues
+ * @brkqueue: Queue of incoming HWBREAK requests (FRAME mode)
+ * @irq: IRQ number
+ * @wake_irq: IRQ number for incoming wake line (-1 if none)
+ * @pio_tasklet: Bottom half for PIO transfers and events
+ * @wake_tasklet: Bottom half for incoming wake events
+ * @wkin_cken: Keep track of clock references due to the incoming wake line
+ * @wk_refcount: Reference count for output wake line
+ * @sys_mpu_enable: Context for the interrupt enable register for irq 0
+ * @sst: Context for the synchronous serial transmitter
+ * @ssr: Context for the synchronous serial receiver
+ */
+struct omap_ssi_port {
+	struct device		*dev;
+	dma_addr_t		sst_dma;
+	dma_addr_t		ssr_dma;
+	void __iomem		*sst_base;
+	void __iomem		*ssr_base;
+	spinlock_t		wk_lock;
+	spinlock_t		lock;
+	unsigned int		channels;
+	struct list_head	txqueue[SSI_MAX_CHANNELS];
+	struct list_head	rxqueue[SSI_MAX_CHANNELS];
+	struct list_head	brkqueue;
+	unsigned int		irq;
+	int			wake_irq;
+	struct tasklet_struct	pio_tasklet;
+	struct tasklet_struct	wake_tasklet;
+	unsigned int		wkin_cken:1; /* Workaround */
+	int			wk_refcount;
+	/* OMAP SSI port context */
+	u32			sys_mpu_enable; /* We use only one irq */
+	struct omap_ssm_ctx	sst;
+	struct omap_ssm_ctx	ssr;
+};
+
+/**
+ * struct omap_ssi_controller - OMAP SSI controller data
+ * @dev: device associated to the controller (HSI controller)
+ * @sys: SSI I/O base address
+ * @gdd: GDD I/O base address
+ * @ick: SSI interconnect clock
+ * @fck: SSI functional clock
+ * @ck_refcount: References count for clocks
+ * @gdd_irq: IRQ line for GDD
+ * @gdd_tasklet: bottom half for DMA transfers
+ * @gdd_trn: Array of GDD transaction data for ongoing GDD transfers
+ * @lock: lock to serialize access to GDD
+ * @ck_lock: lock to serialize access to the clocks
+ * @loss_count: To follow if we need to restore context or not
+ * @max_speed: Maximum TX speed (Kb/s) set by the clients.
+ * @sysconfig: SSI controller saved context
+ * @gdd_gcr: SSI GDD saved context
+ * @get_loss: Pointer to omap_pm_get_dev_context_loss_count, if any
+ * @port: Array of pointers of the ports of the controller
+ * @dir: Debugfs SSI root directory
+ */
+struct omap_ssi_controller {
+	struct device		*dev;
+	void __iomem		*sys;
+	void __iomem		*gdd;
+	struct clk		*ick;
+	struct clk		*fck;
+	int			ck_refcount;
+	unsigned int		gdd_irq;
+	struct tasklet_struct	gdd_tasklet;
+	struct gdd_trn		gdd_trn[SSI_MAX_GDD_LCH];
+	spinlock_t		lock;
+	spinlock_t		ck_lock;
+	unsigned long		fck_rate;
+	int			loss_count;
+	u32			max_speed;
+	/* OMAP SSI Controller context */
+	u32			sysconfig;
+	u32			gdd_gcr;
+	int			(*get_loss)(struct device *dev);
+	struct omap_ssi_port	**port;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dir;
+#endif
+};
+
+static inline unsigned int ssi_wakein(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	return gpio_get_value(irq_to_gpio(omap_port->wake_irq));
+}
+
+static int ssi_for_each_port(struct hsi_controller *ssi, void *data,
+			int (*fn)(struct omap_ssi_port *p, void *data))
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	unsigned int i = 0;
+	int err = 0;
+
+	for (i = 0; ((i < ssi->num_ports) && !err); i++)
+		err = (*fn)(omap_ssi->port[i], data);
+
+	return err;
+}
+
+static int ssi_set_port_mode(struct omap_ssi_port *omap_port, void *data)
+{
+	u32 *mode = data;
+
+	__raw_writel(*mode, omap_port->sst_base + SSI_SST_MODE_REG);
+	__raw_writel(*mode, omap_port->ssr_base + SSI_SSR_MODE_REG);
+	/* OCP barrier */
+	*mode = __raw_readl(omap_port->ssr_base + SSI_SSR_MODE_REG);
+
+	return 0;
+}
+
+static inline void ssi_set_mode(struct hsi_controller *ssi, u32 mode)
+{
+	ssi_for_each_port(ssi, &mode, ssi_set_port_mode);
+}
+
+static int ssi_restore_port_mode(struct omap_ssi_port *omap_port,
+						void *data __maybe_unused)
+{
+	u32 mode;
+
+	__raw_writel(omap_port->sst.mode,
+				omap_port->sst_base + SSI_SST_MODE_REG);
+	__raw_writel(omap_port->ssr.mode,
+				omap_port->ssr_base + SSI_SSR_MODE_REG);
+	/* OCP barrier */
+	mode =  __raw_readl(omap_port->ssr_base + SSI_SSR_MODE_REG);
+
+	return 0;
+}
+
+static int ssi_restore_divisor(struct omap_ssi_port *omap_port,
+						void *data __maybe_unused)
+{
+	__raw_writel(omap_port->sst.divisor,
+				omap_port->sst_base + SSI_SST_DIVISOR_REG);
+
+	return 0;
+}
+
+static int ssi_restore_port_ctx(struct omap_ssi_port *omap_port,
+						void *data __maybe_unused)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*base = omap_port->sst_base;
+
+	__raw_writel(omap_port->sys_mpu_enable,
+			omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	/* SST context */
+	__raw_writel(omap_port->sst.frame_size, base + SSI_SST_FRAMESIZE_REG);
+	__raw_writel(omap_port->sst.channels, base + SSI_SST_CHANNELS_REG);
+	__raw_writel(omap_port->sst.arb_mode, base + SSI_SST_ARBMODE_REG);
+	/* SSR context */
+	base = omap_port->ssr_base;
+	__raw_writel(omap_port->ssr.frame_size, base + SSI_SSR_FRAMESIZE_REG);
+	__raw_writel(omap_port->ssr.channels, base + SSI_SSR_CHANNELS_REG);
+	__raw_writel(omap_port->ssr.timeout, base + SSI_SSR_TIMEOUT_REG);
+
+	return 0;
+}
+
+static int ssi_save_port_ctx(struct omap_ssi_port *omap_port,
+						void *data __maybe_unused)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	omap_port->sys_mpu_enable = __raw_readl(omap_ssi->sys +
+					SSI_MPU_ENABLE_REG(port->num, 0));
+
+	return 0;
+}
+
+static int ssi_clk_enable(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int err = 0;
+
+	spin_lock_bh(&omap_ssi->ck_lock);
+	if (omap_ssi->ck_refcount++)
+		goto out;
+	err = clk_enable(omap_ssi->fck);
+	if (unlikely(err < 0))
+		goto out;
+	err = clk_enable(omap_ssi->ick);
+	if (unlikely(err < 0)) {
+		clk_disable(omap_ssi->fck);
+		goto out;
+	}
+	if ((omap_ssi->get_loss) && (omap_ssi->loss_count ==
+				(*omap_ssi->get_loss)(ssi->device.parent)))
+		goto mode; /* We always need to restore the mode & TX divisor */
+
+	__raw_writel(omap_ssi->sysconfig, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	__raw_writel(omap_ssi->gdd_gcr, omap_ssi->gdd + SSI_GDD_GCR_REG);
+
+	ssi_for_each_port(ssi, NULL, ssi_restore_port_ctx);
+mode:
+	ssi_for_each_port(ssi, NULL, ssi_restore_divisor);
+	ssi_for_each_port(ssi, NULL, ssi_restore_port_mode);
+out:
+	spin_unlock_bh(&omap_ssi->ck_lock);
+
+	return err;
+}
+
+static void ssi_clk_disable(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	spin_lock_bh(&omap_ssi->ck_lock);
+	WARN_ON(omap_ssi->ck_refcount <= 0);
+	if (--omap_ssi->ck_refcount)
+		goto out;
+
+	ssi_set_mode(ssi, SSI_MODE_SLEEP);
+
+	if (omap_ssi->get_loss)
+		omap_ssi->loss_count =
+				(*omap_ssi->get_loss)(ssi->device.parent);
+
+	ssi_for_each_port(ssi, NULL, ssi_save_port_ctx);
+	clk_disable(omap_ssi->ick);
+	clk_disable(omap_ssi->fck);
+out:
+	spin_unlock_bh(&omap_ssi->ck_lock);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int ssi_debug_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_controller *ssi = m->private;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*sys = omap_ssi->sys;
+
+	ssi_clk_enable(ssi);
+	seq_printf(m, "REVISION\t: 0x%08x\n",
+					__raw_readl(sys + SSI_REVISION_REG));
+	seq_printf(m, "SYSCONFIG\t: 0x%08x\n",
+					__raw_readl(sys + SSI_SYSCONFIG_REG));
+	seq_printf(m, "SYSSTATUS\t: 0x%08x\n",
+					__raw_readl(sys + SSI_SYSSTATUS_REG));
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_debug_port_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_port *port = m->private;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*base = omap_ssi->sys;
+	unsigned int ch;
+
+	ssi_clk_enable(ssi);
+	if (omap_port->wake_irq > 0)
+		seq_printf(m, "CAWAKE\t\t: %d\n", ssi_wakein(port));
+	seq_printf(m, "WAKE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_WAKE_REG(port->num)));
+	seq_printf(m, "MPU_ENABLE_IRQ%d\t: 0x%08x\n", 0,
+			__raw_readl(base + SSI_MPU_ENABLE_REG(port->num, 0)));
+	seq_printf(m, "MPU_STATUS_IRQ%d\t: 0x%08x\n", 0,
+			__raw_readl(base + SSI_MPU_STATUS_REG(port->num, 0)));
+	/* SST */
+	base = omap_port->sst_base;
+	seq_printf(m, "\nSST\n===\n");
+	seq_printf(m, "ID SST\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_ID_REG));
+	seq_printf(m, "MODE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_MODE_REG));
+	seq_printf(m, "FRAMESIZE\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_FRAMESIZE_REG));
+	seq_printf(m, "DIVISOR\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_DIVISOR_REG));
+	seq_printf(m, "CHANNELS\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_CHANNELS_REG));
+	seq_printf(m, "ARBMODE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_ARBMODE_REG));
+	seq_printf(m, "TXSTATE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_TXSTATE_REG));
+	seq_printf(m, "BUFSTATE\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_BUFSTATE_REG));
+	seq_printf(m, "BREAK\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SST_BREAK_REG));
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		seq_printf(m, "BUFFER_CH%d\t: 0x%08x\n", ch,
+				__raw_readl(base + SSI_SST_BUFFER_CH_REG(ch)));
+	}
+	/* SSR */
+	base = omap_port->ssr_base;
+	seq_printf(m, "\nSSR\n===\n");
+	seq_printf(m, "ID SSR\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_ID_REG));
+	seq_printf(m, "MODE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_MODE_REG));
+	seq_printf(m, "FRAMESIZE\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_FRAMESIZE_REG));
+	seq_printf(m, "CHANNELS\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_CHANNELS_REG));
+	seq_printf(m, "TIMEOUT\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_TIMEOUT_REG));
+	seq_printf(m, "RXSTATE\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_RXSTATE_REG));
+	seq_printf(m, "BUFSTATE\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_BUFSTATE_REG));
+	seq_printf(m, "BREAK\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_BREAK_REG));
+	seq_printf(m, "ERROR\t\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_ERROR_REG));
+	seq_printf(m, "ERRORACK\t: 0x%08x\n",
+				__raw_readl(base + SSI_SSR_ERRORACK_REG));
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		seq_printf(m, "BUFFER_CH%d\t: 0x%08x\n", ch,
+				__raw_readl(base + SSI_SSR_BUFFER_CH_REG(ch)));
+	}
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_debug_gdd_show(struct seq_file *m, void *p __maybe_unused)
+{
+	struct hsi_controller *ssi = m->private;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem	*gdd = omap_ssi->gdd;
+	int lch;
+
+	ssi_clk_enable(ssi);
+	seq_printf(m, "GDD_MPU_STATUS\t: 0x%08x\n",
+		__raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG));
+	seq_printf(m, "GDD_MPU_ENABLE\t: 0x%08x\n\n",
+		__raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG));
+	seq_printf(m, "HW_ID\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_HW_ID_REG));
+	seq_printf(m, "PPORT_ID\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_PPORT_ID_REG));
+	seq_printf(m, "MPORT_ID\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_MPORT_ID_REG));
+	seq_printf(m, "TEST\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_TEST_REG));
+	seq_printf(m, "GCR\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_GCR_REG));
+
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++) {
+		seq_printf(m, "\nGDD LCH %d\n=========\n", lch);
+		seq_printf(m, "CSDP\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CSDP_REG(lch)));
+		seq_printf(m, "CCR\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CCR_REG(lch)));
+		seq_printf(m, "CICR\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CICR_REG(lch)));
+		seq_printf(m, "CSR\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CSR_REG(lch)));
+		seq_printf(m, "CSSA\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_CSSA_REG(lch)));
+		seq_printf(m, "CDSA\t\t: 0x%08x\n",
+				__raw_readl(gdd + SSI_GDD_CDSA_REG(lch)));
+		seq_printf(m, "CEN\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CEN_REG(lch)));
+		seq_printf(m, "CSAC\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CSAC_REG(lch)));
+		seq_printf(m, "CDAC\t\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CDAC_REG(lch)));
+		seq_printf(m, "CLNK_CTRL\t: 0x%04x\n",
+				__raw_readw(gdd + SSI_GDD_CLNK_CTRL_REG(lch)));
+	}
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_div_get(void *data, u64 *val)
+{
+	struct hsi_port *port = data;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+
+	ssi_clk_enable(ssi);
+	*val = __raw_readl(omap_port->sst_base + SSI_SST_DIVISOR_REG);
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_div_set(void *data, u64 val)
+{
+	struct hsi_port *port = data;
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+
+	if (val > 127)
+		return -EINVAL;
+
+	ssi_clk_enable(ssi);
+	__raw_writel(val, omap_port->sst_base + SSI_SST_DIVISOR_REG);
+	omap_port->sst.divisor = val;
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_show, inode->i_private);
+}
+
+static int ssi_port_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_port_show, inode->i_private);
+}
+
+static int ssi_gdd_regs_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ssi_debug_gdd_show, inode->i_private);
+}
+
+static const struct file_operations ssi_regs_fops = {
+	.open		= ssi_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static const struct file_operations ssi_port_regs_fops = {
+	.open		= ssi_port_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static const struct file_operations ssi_gdd_regs_fops = {
+	.open		= ssi_gdd_regs_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+DEFINE_SIMPLE_ATTRIBUTE(ssi_sst_div_fops, ssi_div_get, ssi_div_set, "%llu\n");
+
+static int __init ssi_debug_add_port(struct omap_ssi_port *omap_port,
+								void *data)
+{
+	struct hsi_port *port = to_hsi_port(omap_port->dev);
+	struct dentry *dir = data;
+
+	dir = debugfs_create_dir(dev_name(omap_port->dev), dir);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	debugfs_create_file("regs", S_IRUGO, dir, port, &ssi_port_regs_fops);
+	dir = debugfs_create_dir("sst", dir);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	debugfs_create_file("divisor", S_IRUGO | S_IWUSR, dir, port,
+							&ssi_sst_div_fops);
+
+	return 0;
+}
+
+static int __init ssi_debug_add_ctrl(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct dentry *dir;
+	int err;
+
+	/* SSI controller */
+	omap_ssi->dir = debugfs_create_dir(dev_name(&ssi->device), NULL);
+	if (IS_ERR(omap_ssi->dir))
+		return PTR_ERR(omap_ssi->dir);
+
+	debugfs_create_file("regs", S_IRUGO, omap_ssi->dir, ssi,
+								&ssi_regs_fops);
+	/* SSI GDD (DMA) */
+	dir = debugfs_create_dir("gdd", omap_ssi->dir);
+	if (IS_ERR(dir))
+		goto rback;
+	debugfs_create_file("regs", S_IRUGO, dir, ssi, &ssi_gdd_regs_fops);
+	/* SSI ports */
+	err = ssi_for_each_port(ssi, omap_ssi->dir, ssi_debug_add_port);
+	if (err < 0)
+		goto rback;
+
+	return 0;
+rback:
+	debugfs_remove_recursive(omap_ssi->dir);
+
+	return PTR_ERR(dir);
+}
+
+static void ssi_debug_remove_ctrl(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	debugfs_remove_recursive(omap_ssi->dir);
+}
+#endif /* CONFIG_DEBUG_FS */
+
+static int ssi_claim_lch(struct hsi_msg *msg)
+{
+
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int lch;
+
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++)
+		if (!omap_ssi->gdd_trn[lch].msg) {
+			omap_ssi->gdd_trn[lch].msg = msg;
+			omap_ssi->gdd_trn[lch].sg = msg->sgt.sgl;
+			return lch;
+		}
+
+	return -EBUSY;
+}
+
+static int ssi_start_pio(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 val;
+
+	ssi_clk_enable(ssi);
+	if (msg->ttype == HSI_MSG_WRITE) {
+		val = SSI_DATAACCEPT(msg->channel);
+		ssi_clk_enable(ssi); /* Hold clocks for pio writes */
+	} else {
+		val = SSI_DATAAVAILABLE(msg->channel) | SSI_ERROROCCURED;
+	}
+	dev_dbg(&port->device, "Single %s transfer\n",
+						msg->ttype ? "write" : "read");
+	val |= __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	ssi_clk_disable(ssi);
+	msg->actual_len = 0;
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	return 0;
+}
+
+static int ssi_start_dma(struct hsi_msg *msg, int lch)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *gdd = omap_ssi->gdd;
+	int err;
+	u16 csdp;
+	u16 ccr;
+	u32 s_addr;
+	u32 d_addr;
+	u32 tmp;
+
+	if (msg->ttype == HSI_MSG_READ) {
+		err = dma_map_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents,
+							DMA_FROM_DEVICE);
+		if (err < 0) {
+			dev_dbg(&ssi->device, "DMA map SG failed !\n");
+			return err;
+		}
+		csdp = SSI_DST_BURST_4x32_BIT | SSI_DST_MEMORY_PORT |
+			SSI_SRC_SINGLE_ACCESS0 | SSI_SRC_PERIPHERAL_PORT |
+			SSI_DATA_TYPE_S32;
+		ccr = msg->channel + 0x10 + (port->num * 8); /* Sync */
+		ccr |= SSI_DST_AMODE_POSTINC | SSI_SRC_AMODE_CONST |
+			SSI_CCR_ENABLE;
+		s_addr = omap_port->ssr_dma +
+					SSI_SSR_BUFFER_CH_REG(msg->channel);
+		d_addr = sg_dma_address(msg->sgt.sgl);
+	} else {
+		err = dma_map_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents,
+							DMA_TO_DEVICE);
+		if (err < 0) {
+			dev_dbg(&ssi->device, "DMA map SG failed !\n");
+			return err;
+		}
+		csdp = SSI_SRC_BURST_4x32_BIT | SSI_SRC_MEMORY_PORT |
+			SSI_DST_SINGLE_ACCESS0 | SSI_DST_PERIPHERAL_PORT |
+			SSI_DATA_TYPE_S32;
+		ccr = (msg->channel + 1 + (port->num * 8)) & 0xf; /* Sync */
+		ccr |= SSI_SRC_AMODE_POSTINC | SSI_DST_AMODE_CONST |
+			SSI_CCR_ENABLE;
+		s_addr = sg_dma_address(msg->sgt.sgl);
+		d_addr = omap_port->sst_dma +
+					SSI_SST_BUFFER_CH_REG(msg->channel);
+	}
+	dev_dbg(&ssi->device, "lch %d cdsp %08x ccr %04x s_addr %08x"
+			" d_addr %08x\n", lch, csdp, ccr, s_addr, d_addr);
+	ssi_clk_enable(ssi); /* Hold clocks during the transfer */
+	__raw_writew(csdp, gdd + SSI_GDD_CSDP_REG(lch));
+	__raw_writew(SSI_BLOCK_IE | SSI_TOUT_IE, gdd + SSI_GDD_CICR_REG(lch));
+	__raw_writel(d_addr, gdd + SSI_GDD_CDSA_REG(lch));
+	__raw_writel(s_addr, gdd + SSI_GDD_CSSA_REG(lch));
+	__raw_writew(SSI_BYTES_TO_FRAMES(msg->sgt.sgl->length),
+						gdd + SSI_GDD_CEN_REG(lch));
+
+	spin_lock_bh(&omap_ssi->lock);
+	tmp = __raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp |= SSI_GDD_LCH(lch);
+	__raw_writel(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	spin_unlock_bh(&omap_ssi->lock);
+	__raw_writew(ccr, gdd + SSI_GDD_CCR_REG(lch));
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	return 0;
+}
+
+static int ssi_start_transfer(struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int lch = -1;
+
+	if (list_empty(queue))
+		return 0;
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if (msg->status != HSI_STATUS_QUEUED)
+		return 0;
+	if ((msg->sgt.nents) && (msg->sgt.sgl->length > sizeof(u32)))
+		lch = ssi_claim_lch(msg);
+	if (lch >= 0)
+		return ssi_start_dma(msg, lch);
+	else
+		return ssi_start_pio(msg);
+}
+
+static void ssi_transfer(struct omap_ssi_port *omap_port,
+							struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int err = -1;
+
+	spin_lock_bh(&omap_port->lock);
+	while (err < 0) {
+		err = ssi_start_transfer(queue);
+		if (err < 0) {
+			msg = list_first_entry(queue, struct hsi_msg, link);
+			msg->status = HSI_STATUS_ERROR;
+			msg->actual_len = 0;
+			list_del(&msg->link);
+			spin_unlock_bh(&omap_port->lock);
+			msg->complete(msg);
+			spin_lock_bh(&omap_port->lock);
+		}
+	}
+	spin_unlock_bh(&omap_port->lock);
+}
+
+static u32 ssi_calculate_div(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	u32 tx_fckrate = (u32) omap_ssi->fck_rate;
+
+	/* / 2 : SSI TX clock is always half of the SSI functional clock */
+	tx_fckrate >>= 1;
+	/* Round down when tx_fckrate % omap_ssi->max_speed == 0 */
+	tx_fckrate--;
+	dev_dbg(&ssi->device, "TX div %d for fck_rate %lu Khz speed %d Kb/s\n",
+			tx_fckrate / omap_ssi->max_speed, omap_ssi->fck_rate,
+							omap_ssi->max_speed);
+
+	return tx_fckrate / omap_ssi->max_speed;
+}
+
+static void ssi_error(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 err;
+	u32 val;
+	u32 tmp;
+
+	/* ACK error */
+	err = __raw_readl(omap_port->ssr_base + SSI_SSR_ERROR_REG);
+	dev_err(&port->device, "SSI error: 0x%02x\n", err);
+	if (!err) {
+		dev_dbg(&port->device, "spurious SSI error ignored!\n");
+		return;
+	}
+	spin_lock(&omap_ssi->lock);
+	/* Cancel all GDD read transfers */
+	for (i = 0, val = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if ((msg) && (msg->ttype == HSI_MSG_READ)) {
+			__raw_writew(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+			val |= (1 << i);
+			omap_ssi->gdd_trn[i].msg = NULL;
+		}
+	}
+	tmp = __raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp &= ~val;
+	__raw_writel(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	spin_unlock(&omap_ssi->lock);
+	/* Cancel all PIO read transfers */
+	spin_lock(&omap_port->lock);
+	tmp = __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	tmp &= 0xfeff00ff; /* Disable error & all dataavailable interrupts */
+	__raw_writel(tmp, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	/* ACK error */
+	__raw_writel(err, omap_port->ssr_base + SSI_SSR_ERRORACK_REG);
+	__raw_writel(SSI_ERROROCCURED,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	/* Signal the error all current pending read requests */
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->rxqueue[i], struct hsi_msg,
+									link);
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+		/* Now restart queued reads if any */
+		ssi_transfer(omap_port, &omap_port->rxqueue[i]);
+		spin_lock(&omap_port->lock);
+	}
+	spin_unlock(&omap_port->lock);
+}
+
+static void ssi_break_complete(struct hsi_port *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	struct hsi_msg *tmp;
+	u32 val;
+
+	dev_dbg(&port->device, "HWBREAK received\n");
+
+	spin_lock(&omap_port->lock);
+	val = __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	val &= ~SSI_BREAKDETECTED;
+	__raw_writel(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(0, omap_port->ssr_base + SSI_SSR_BREAK_REG);
+	__raw_writel(SSI_BREAKDETECTED,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	spin_unlock(&omap_port->lock);
+
+	list_for_each_entry_safe(msg, tmp, &omap_port->brkqueue, link) {
+		msg->status = HSI_STATUS_COMPLETED;
+		spin_lock(&omap_port->lock);
+		list_del(&msg->link);
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+	}
+
+}
+
+static int ssi_async_break(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	int err = 0;
+	u32 tmp;
+
+	ssi_clk_enable(ssi);
+	if (msg->ttype == HSI_MSG_WRITE) {
+		if (omap_port->sst.mode != SSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		__raw_writel(1, omap_port->sst_base + SSI_SST_BREAK_REG);
+		msg->status = HSI_STATUS_COMPLETED;
+		msg->complete(msg);
+	} else {
+		if (omap_port->ssr.mode != SSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		spin_lock_bh(&omap_port->lock);
+		tmp = __raw_readl(omap_ssi->sys +
+					SSI_MPU_ENABLE_REG(port->num, 0));
+		__raw_writel(tmp | SSI_BREAKDETECTED,
+			omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+		msg->status = HSI_STATUS_PROCEEDING;
+		list_add_tail(&msg->link, &omap_port->brkqueue);
+		spin_unlock_bh(&omap_port->lock);
+	}
+out:
+	ssi_clk_disable(ssi);
+
+	return err;
+}
+
+static int ssi_async(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct list_head *queue;
+	int err = 0;
+
+	BUG_ON(!msg);
+
+	if (msg->sgt.nents > 1)
+		return -ENOSYS; /* TODO: Add sg support */
+
+	if (msg->break_frame)
+		return ssi_async_break(msg);
+
+	if (msg->ttype) {
+		BUG_ON(msg->channel >= omap_port->sst.channels);
+		queue = &omap_port->txqueue[msg->channel];
+	} else {
+		BUG_ON(msg->channel >= omap_port->ssr.channels);
+		queue = &omap_port->rxqueue[msg->channel];
+	}
+	msg->status = HSI_STATUS_QUEUED;
+	spin_lock_bh(&omap_port->lock);
+	list_add_tail(&msg->link, queue);
+	err = ssi_start_transfer(queue);
+	if (err < 0) {
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+	}
+	spin_unlock_bh(&omap_port->lock);
+	dev_dbg(&port->device, "msg status %d ttype %d ch %d\n",
+				msg->status, msg->ttype, msg->channel);
+
+	return err;
+}
+
+static void ssi_flush_queue(struct list_head *queue, struct hsi_client *cl)
+{
+	struct list_head *node, *tmp;
+	struct hsi_msg *msg;
+
+	list_for_each_safe(node, tmp, queue) {
+		msg = list_entry(node, struct hsi_msg, link);
+		if ((cl) && (cl != msg->cl))
+			continue;
+		list_del(node);
+		pr_debug("flush queue: ch %d, msg %p len %d type %d ctxt %p\n",
+			msg->channel, msg, msg->sgt.sgl->length,
+					msg->ttype, msg->context);
+		if (msg->destructor)
+			msg->destructor(msg);
+		else
+			hsi_free_msg(msg);
+	}
+}
+
+static int ssi_setup(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sst = omap_port->sst_base;
+	void __iomem *ssr = omap_port->ssr_base;
+	u32 div;
+	u32 val;
+	int err = 0;
+
+	ssi_clk_enable(ssi);
+	spin_lock_bh(&omap_port->lock);
+	if (cl->tx_cfg.speed)
+		omap_ssi->max_speed = cl->tx_cfg.speed;
+	div = ssi_calculate_div(ssi);
+	if (div > SSI_MAX_DIVISOR) {
+		dev_err(&cl->device, "Invalid TX speed %d Mb/s (div %d)\n",
+						cl->tx_cfg.speed, div);
+		err = -EINVAL;
+		goto out;
+	}
+	/* Set TX/RX module to sleep to stop TX/RX during cfg update */
+	__raw_writel(SSI_MODE_SLEEP, sst + SSI_SST_MODE_REG);
+	__raw_writel(SSI_MODE_SLEEP, ssr + SSI_SSR_MODE_REG);
+	/* Flush posted write */
+	val = __raw_readl(ssr + SSI_SSR_MODE_REG);
+	/* TX */
+	__raw_writel(31, sst + SSI_SST_FRAMESIZE_REG);
+	__raw_writel(div, sst + SSI_SST_DIVISOR_REG);
+	__raw_writel(cl->tx_cfg.channels, sst + SSI_SST_CHANNELS_REG);
+	__raw_writel(cl->tx_cfg.arb_mode, sst + SSI_SST_ARBMODE_REG);
+	__raw_writel(cl->tx_cfg.mode, sst + SSI_SST_MODE_REG);
+	/* RX */
+	__raw_writel(31, ssr + SSI_SSR_FRAMESIZE_REG);
+	__raw_writel(cl->rx_cfg.channels, ssr + SSI_SSR_CHANNELS_REG);
+	__raw_writel(0, ssr + SSI_SSR_TIMEOUT_REG);
+	/* Cleanup the break queue if we leave FRAME mode */
+	if ((omap_port->ssr.mode == SSI_MODE_FRAME) &&
+		(cl->rx_cfg.mode != SSI_MODE_FRAME))
+		ssi_flush_queue(&omap_port->brkqueue, cl);
+	__raw_writel(cl->rx_cfg.mode, ssr + SSI_SSR_MODE_REG);
+	omap_port->channels = max(cl->rx_cfg.channels, cl->tx_cfg.channels);
+	/* Shadow registering for OFF mode */
+	/* SST */
+	omap_port->sst.divisor = div;
+	omap_port->sst.frame_size = 31;
+	omap_port->sst.channels = cl->tx_cfg.channels;
+	omap_port->sst.arb_mode = cl->tx_cfg.arb_mode;
+	omap_port->sst.mode = cl->tx_cfg.mode;
+	/* SSR */
+	omap_port->ssr.frame_size = 31;
+	omap_port->ssr.timeout = 0;
+	omap_port->ssr.channels = cl->rx_cfg.channels;
+	omap_port->ssr.mode = cl->rx_cfg.mode;
+out:
+	spin_unlock_bh(&omap_port->lock);
+	ssi_clk_disable(ssi);
+
+	return err;
+}
+
+static void ssi_cleanup_queues(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 rxbufstate = 0;
+	u32 txbufstate = 0;
+	u32 status = SSI_ERROROCCURED;
+	u32 tmp;
+
+	ssi_flush_queue(&omap_port->brkqueue, cl);
+	if (list_empty(&omap_port->brkqueue))
+		status |= SSI_BREAKDETECTED;
+
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->txqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->txqueue[i], struct hsi_msg,
+									link);
+		if ((msg->cl == cl) && (msg->status == HSI_STATUS_PROCEEDING)) {
+			txbufstate |= (1 << i);
+			status |= SSI_DATAACCEPT(i);
+			/* Release the clocks writes, also GDD ones */
+			ssi_clk_disable(ssi);
+		}
+		ssi_flush_queue(&omap_port->txqueue[i], cl);
+	}
+	for (i = 0; i < omap_port->channels; i++) {
+		if (list_empty(&omap_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&omap_port->rxqueue[i], struct hsi_msg,
+									link);
+		if ((msg->cl == cl) && (msg->status == HSI_STATUS_PROCEEDING)) {
+			rxbufstate |= (1 << i);
+			status |= SSI_DATAAVAILABLE(i);
+		}
+		ssi_flush_queue(&omap_port->rxqueue[i], cl);
+		/* Check if we keep the error detection interrupt armed */
+		if (!list_empty(&omap_port->rxqueue[i]))
+			status &= ~SSI_ERROROCCURED;
+	}
+	/* Cleanup write buffers */
+	tmp = __raw_readl(omap_port->sst_base + SSI_SST_BUFSTATE_REG);
+	tmp &= ~txbufstate;
+	__raw_writel(tmp, omap_port->sst_base + SSI_SST_BUFSTATE_REG);
+	/* Cleanup read buffers */
+	tmp = __raw_readl(omap_port->ssr_base + SSI_SSR_BUFSTATE_REG);
+	tmp &= ~rxbufstate;
+	__raw_writel(tmp, omap_port->ssr_base + SSI_SSR_BUFSTATE_REG);
+	/* Disarm and ack pending interrupts */
+	tmp = __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	tmp &= ~status;
+	__raw_writel(tmp, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(status, omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+}
+
+static void ssi_cleanup_gdd(struct hsi_controller *ssi, struct hsi_client *cl)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	unsigned int i;
+	u32 val = 0;
+	u32 tmp;
+
+	for (i = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if ((!msg) || (msg->cl != cl))
+			continue;
+		__raw_writew(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+		val |= (1 << i);
+		/*
+		 * Clock references for write will be handled in
+		 * ssi_cleanup_queues
+		 */
+		if (msg->ttype == HSI_MSG_READ)
+			ssi_clk_disable(ssi);
+		omap_ssi->gdd_trn[i].msg = NULL;
+	}
+	tmp = __raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	tmp &= ~val;
+	__raw_writel(tmp, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	__raw_writel(val, omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+}
+
+static int ssi_release(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	spin_lock_bh(&omap_port->lock);
+	ssi_clk_enable(ssi);
+	/* Stop all the pending DMA requests for that client */
+	ssi_cleanup_gdd(ssi, cl);
+	/* Now cleanup all the queues */
+	ssi_cleanup_queues(cl);
+	ssi_clk_disable(ssi);
+	/* If it is the last client of the port, do extra checks and cleanup */
+	if (port->claimed <= 1) {
+		/*
+		 * Drop the clock reference for the incoming wake line
+		 * if it is still kept high by the other side.
+		 */
+		if (omap_port->wkin_cken) {
+			ssi_clk_disable(ssi);
+			omap_port->wkin_cken = 0;
+		}
+		ssi_clk_enable(ssi);
+		/* Stop any SSI TX/RX without a client */
+		ssi_set_mode(ssi, SSI_MODE_SLEEP);
+		omap_port->sst.mode = SSI_MODE_SLEEP;
+		omap_port->ssr.mode = SSI_MODE_SLEEP;
+		ssi_clk_disable(ssi);
+		WARN_ON(omap_port->wk_refcount != 0);
+		WARN_ON(omap_ssi->ck_refcount != 0);
+	}
+	spin_unlock_bh(&omap_port->lock);
+
+	return 0;
+}
+
+static int ssi_flush(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg;
+	void __iomem *sst = omap_port->sst_base;
+	void __iomem *ssr = omap_port->ssr_base;
+	unsigned int i;
+	u32 err;
+
+	ssi_clk_enable(ssi);
+	spin_lock_bh(&omap_port->lock);
+	/* Stop all DMA transfers */
+	for (i = 0; i < SSI_MAX_GDD_LCH; i++) {
+		msg = omap_ssi->gdd_trn[i].msg;
+		if (!msg || (port != hsi_get_port(msg->cl)))
+			continue;
+		__raw_writew(0, omap_ssi->gdd + SSI_GDD_CCR_REG(i));
+		if (msg->ttype == HSI_MSG_READ)
+			ssi_clk_disable(ssi);
+		omap_ssi->gdd_trn[i].msg = NULL;
+	}
+	/* Flush all SST buffers */
+	__raw_writel(0, sst + SSI_SST_BUFSTATE_REG);
+	__raw_writel(0, sst + SSI_SST_TXSTATE_REG);
+	/* Flush all SSR buffers */
+	__raw_writel(0, ssr + SSI_SSR_RXSTATE_REG);
+	__raw_writel(0, ssr + SSI_SSR_BUFSTATE_REG);
+	/* Flush all errors */
+	err = __raw_readl(ssr + SSI_SSR_ERROR_REG);
+	__raw_writel(err, ssr + SSI_SSR_ERRORACK_REG);
+	/* Flush break */
+	__raw_writel(0, ssr + SSI_SSR_BREAK_REG);
+	/* Clear interrupts */
+	__raw_writel(0, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(0xffffff00,
+			omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	__raw_writel(0, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	__raw_writel(0xff, omap_ssi->sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	/* Dequeue all pending requests */
+	for (i = 0; i < omap_port->channels; i++) {
+		/* Release write clocks */
+		if (!list_empty(&omap_port->txqueue[i]))
+			ssi_clk_disable(ssi);
+		ssi_flush_queue(&omap_port->txqueue[i], NULL);
+		ssi_flush_queue(&omap_port->rxqueue[i], NULL);
+	}
+	ssi_flush_queue(&omap_port->brkqueue, NULL);
+	spin_unlock_bh(&omap_port->lock);
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static int ssi_start_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(&port->device, "Wake out high %d\n", omap_port->wk_refcount);
+
+	spin_lock_bh(&omap_port->wk_lock);
+	if (omap_port->wk_refcount++) {
+		spin_unlock_bh(&omap_port->wk_lock);
+		return 0;
+	}
+	ssi_clk_enable(ssi); /* Grab clocks */
+	__raw_writel(SSI_WAKE(0), omap_ssi->sys + SSI_SET_WAKE_REG(port->num));
+	spin_unlock_bh(&omap_port->wk_lock);
+
+	return 0;
+}
+
+static int ssi_stop_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = hsi_get_port(cl);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	dev_dbg(&port->device, "Wake out low %d\n", omap_port->wk_refcount);
+
+	spin_lock_bh(&omap_port->wk_lock);
+	BUG_ON(!omap_port->wk_refcount);
+	if (--omap_port->wk_refcount) {
+		spin_unlock_bh(&omap_port->wk_lock);
+		return 0;
+	}
+	__raw_writel(SSI_WAKE(0),
+				omap_ssi->sys + SSI_CLEAR_WAKE_REG(port->num));
+	ssi_clk_disable(ssi); /* Release clocks */
+	spin_unlock_bh(&omap_port->wk_lock);
+
+	return 0;
+}
+
+static void ssi_pio_complete(struct hsi_port *port, struct list_head *queue)
+{
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg;
+	u32 *buf;
+	u32 reg;
+	u32 val;
+
+	spin_lock(&omap_port->lock);
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if ((!msg->sgt.nents) || (!msg->sgt.sgl->length)) {
+		msg->actual_len = 0;
+		msg->status = HSI_STATUS_PENDING;
+	}
+	if (msg->ttype == HSI_MSG_WRITE)
+		val = SSI_DATAACCEPT(msg->channel);
+	else
+		val = SSI_DATAAVAILABLE(msg->channel);
+	if (msg->status == HSI_STATUS_PROCEEDING) {
+		buf = sg_virt(msg->sgt.sgl) + msg->actual_len;
+		if (msg->ttype == HSI_MSG_WRITE)
+			__raw_writel(*buf, omap_port->sst_base +
+					SSI_SST_BUFFER_CH_REG(msg->channel));
+		 else
+			*buf = __raw_readl(omap_port->ssr_base +
+					SSI_SSR_BUFFER_CH_REG(msg->channel));
+		dev_dbg(&port->device, "ch %d ttype %d 0x%08x\n", msg->channel,
+							msg->ttype, *buf);
+		msg->actual_len += sizeof(*buf);
+		if (msg->actual_len >= msg->sgt.sgl->length)
+			msg->status = HSI_STATUS_COMPLETED;
+		/*
+		 * Wait for the last written frame to be really sent before
+		 * we call the complete callback
+		 */
+		if ((msg->status == HSI_STATUS_PROCEEDING) ||
+				((msg->status == HSI_STATUS_COMPLETED) &&
+					(msg->ttype == HSI_MSG_WRITE))) {
+			__raw_writel(val, omap_ssi->sys +
+					SSI_MPU_STATUS_REG(port->num, 0));
+			spin_unlock(&omap_port->lock);
+
+			return;
+		}
+
+	}
+	/* Transfer completed at this point */
+	reg = __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	if (msg->ttype == HSI_MSG_WRITE)
+		ssi_clk_disable(ssi); /* Release clocks for write transfer */
+	reg &= ~val;
+	__raw_writel(reg, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(val, omap_ssi->sys + SSI_MPU_STATUS_REG(port->num, 0));
+	list_del(&msg->link);
+	spin_unlock(&omap_port->lock);
+	msg->complete(msg);
+	ssi_transfer(omap_port, queue);
+}
+
+static void ssi_gdd_complete(struct hsi_controller *ssi, unsigned int lch)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct hsi_msg *msg = omap_ssi->gdd_trn[lch].msg;
+	struct hsi_port *port = to_hsi_port(msg->cl->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	unsigned int dir;
+	u32 csr;
+	u32 val;
+
+	spin_lock(&omap_ssi->lock);
+
+	val = __raw_readl(omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+	val &= ~SSI_GDD_LCH(lch);
+	__raw_writel(val, omap_ssi->sys + SSI_GDD_MPU_IRQ_ENABLE_REG);
+
+	if (msg->ttype == HSI_MSG_READ) {
+		dir = DMA_FROM_DEVICE;
+		val = SSI_DATAAVAILABLE(msg->channel);
+		ssi_clk_disable(ssi);
+	} else {
+		dir = DMA_TO_DEVICE;
+		val = SSI_DATAACCEPT(msg->channel);
+		/* Keep clocks reference for write pio event */
+	}
+	dma_unmap_sg(&ssi->device, msg->sgt.sgl, msg->sgt.nents, dir);
+	csr = __raw_readw(omap_ssi->gdd + SSI_GDD_CSR_REG(lch));
+	omap_ssi->gdd_trn[lch].msg = NULL; /* release GDD lch */
+	dev_dbg(&port->device, "DMA completed ch %d ttype %d\n",
+				msg->channel, msg->ttype);
+	spin_unlock(&omap_ssi->lock);
+	if (csr & SSI_CSR_TOUR) { /* Timeout error */
+		msg->status = HSI_STATUS_ERROR;
+		msg->actual_len = 0;
+		spin_lock(&omap_port->lock);
+		list_del(&msg->link); /* Dequeue msg */
+		spin_unlock(&omap_port->lock);
+		msg->complete(msg);
+		return;
+	}
+	spin_lock(&omap_port->lock);
+	val |= __raw_readl(omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	__raw_writel(val, omap_ssi->sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	spin_unlock(&omap_port->lock);
+
+	msg->status = HSI_STATUS_COMPLETED;
+	msg->actual_len = sg_dma_len(msg->sgt.sgl);
+}
+
+static void ssi_gdd_tasklet(unsigned long dev)
+{
+	struct hsi_controller *ssi = (struct hsi_controller *)dev;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+	unsigned int lch;
+	u32 status_reg;
+
+	ssi_clk_enable(ssi);
+
+	status_reg = __raw_readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	for (lch = 0; lch < SSI_MAX_GDD_LCH; lch++) {
+		if (status_reg & SSI_GDD_LCH(lch))
+			ssi_gdd_complete(ssi, lch);
+	}
+	__raw_writel(status_reg, sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	status_reg = __raw_readl(sys + SSI_GDD_MPU_IRQ_STATUS_REG);
+	ssi_clk_disable(ssi);
+	if (status_reg)
+		tasklet_hi_schedule(&omap_ssi->gdd_tasklet);
+	else
+		enable_irq(omap_ssi->gdd_irq);
+
+}
+
+static irqreturn_t ssi_gdd_isr(int irq, void *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	tasklet_hi_schedule(&omap_ssi->gdd_tasklet);
+	disable_irq_nosync(irq);
+
+	return IRQ_HANDLED;
+}
+
+static void ssi_pio_tasklet(unsigned long ssi_port)
+{
+	struct hsi_port *port = (struct hsi_port *)ssi_port;
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	void __iomem *sys = omap_ssi->sys;
+	unsigned int ch;
+	u32 status_reg;
+
+	ssi_clk_enable(ssi);
+	status_reg = __raw_readl(sys + SSI_MPU_STATUS_REG(port->num, 0));
+	status_reg &= __raw_readl(sys + SSI_MPU_ENABLE_REG(port->num, 0));
+
+	for (ch = 0; ch < omap_port->channels; ch++) {
+		if (status_reg & SSI_DATAACCEPT(ch))
+			ssi_pio_complete(port, &omap_port->txqueue[ch]);
+		if (status_reg & SSI_DATAAVAILABLE(ch))
+			ssi_pio_complete(port, &omap_port->rxqueue[ch]);
+	}
+	if (status_reg & SSI_BREAKDETECTED)
+		ssi_break_complete(port);
+	if (status_reg & SSI_ERROROCCURED)
+		ssi_error(port);
+
+	status_reg = __raw_readl(sys + SSI_MPU_STATUS_REG(port->num, 0));
+	status_reg &= __raw_readl(sys + SSI_MPU_ENABLE_REG(port->num, 0));
+	ssi_clk_disable(ssi);
+
+	if (status_reg)
+		tasklet_hi_schedule(&omap_port->pio_tasklet);
+	else
+		enable_irq(omap_port->irq);
+}
+
+static irqreturn_t ssi_pio_isr(int irq, void *port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	tasklet_hi_schedule(&omap_port->pio_tasklet);
+	disable_irq_nosync(irq);
+
+	return IRQ_HANDLED;
+}
+
+static void ssi_wake_tasklet(unsigned long ssi_port)
+{
+	struct hsi_port *port = (struct hsi_port *)ssi_port;
+	struct hsi_controller *ssi = to_hsi_controller(port->device.parent);
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+
+	if (ssi_wakein(port)) {
+		/**
+		 * We can have a quick High-Low-High transition in the line.
+		 * In such a case if we have long interrupt latencies,
+		 * we can miss the low event or get twice a high event.
+		 * This workaround will avoid breaking the clock reference
+		 * count when such a situation ocurrs.
+		 */
+		spin_lock(&omap_port->lock);
+		if (!omap_port->wkin_cken) {
+			omap_port->wkin_cken = 1;
+			ssi_clk_enable(ssi);
+		}
+		spin_unlock(&omap_port->lock);
+		dev_dbg(&ssi->device, "Wake in high\n");
+		hsi_event(port, HSI_EVENT_START_RX);
+	} else {
+		dev_dbg(&ssi->device, "Wake in low\n");
+		hsi_event(port, HSI_EVENT_STOP_RX);
+		spin_lock(&omap_port->lock);
+		if (omap_port->wkin_cken) {
+			ssi_clk_disable(ssi);
+			omap_port->wkin_cken = 0;
+		}
+		spin_unlock(&omap_port->lock);
+	}
+}
+
+static irqreturn_t ssi_wake_isr(int irq __maybe_unused, void *ssi_port)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(ssi_port);
+
+	tasklet_hi_schedule(&omap_port->wake_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static int __init ssi_port_irq(struct hsi_port *port,
+						struct platform_device *pd)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct resource *irq;
+	int err;
+
+	irq = platform_get_resource(pd, IORESOURCE_IRQ, (port->num * 3) + 1);
+	if (!irq) {
+		dev_err(&port->device, "Port IRQ resource missing\n");
+		return -ENXIO;
+	}
+	omap_port->irq = irq->start;
+	tasklet_init(&omap_port->pio_tasklet, ssi_pio_tasklet,
+							(unsigned long)port);
+	err = devm_request_irq(&pd->dev, omap_port->irq, ssi_pio_isr,
+						IRQF_DISABLED, irq->name, port);
+	if (err < 0)
+		dev_err(&port->device, "Request IRQ %d failed (%d)\n",
+							omap_port->irq, err);
+	return err;
+}
+
+static int __init ssi_wake_irq(struct hsi_port *port,
+						struct platform_device *pd)
+{
+	struct omap_ssi_port *omap_port = hsi_port_drvdata(port);
+	struct resource *irq;
+	int err;
+
+	irq = platform_get_resource(pd, IORESOURCE_IRQ, (port->num * 3) + 3);
+	if (!irq) {
+		dev_err(&port->device, "Wake in IRQ resource missing");
+		return -ENXIO;
+	}
+	if (irq->flags & IORESOURCE_UNSET) {
+		dev_info(&port->device, "No Wake in support\n");
+		omap_port->wake_irq = -1;
+		return 0;
+	}
+	omap_port->wake_irq = irq->start;
+	tasklet_init(&omap_port->wake_tasklet, ssi_wake_tasklet,
+							(unsigned long)port);
+	err = devm_request_irq(&pd->dev, omap_port->wake_irq, ssi_wake_isr,
+		IRQF_DISABLED | IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+							irq->name, port);
+	if (err < 0)
+		dev_err(&port->device, "Request Wake in IRQ %d failed %d\n",
+						omap_port->wake_irq, err);
+	err = enable_irq_wake(omap_port->wake_irq);
+	if (err < 0)
+		dev_err(&port->device, "Enable wake on the wakeline in irq %d"
+				" failed %d\n", omap_port->wake_irq, err);
+
+	return err;
+}
+
+static void __init ssi_queues_init(struct omap_ssi_port *omap_port)
+{
+	unsigned int ch;
+
+	for (ch = 0; ch < SSI_MAX_CHANNELS; ch++) {
+		INIT_LIST_HEAD(&omap_port->txqueue[ch]);
+		INIT_LIST_HEAD(&omap_port->rxqueue[ch]);
+	}
+	INIT_LIST_HEAD(&omap_port->brkqueue);
+}
+
+static int __init ssi_get_iomem(struct platform_device *pd,
+		unsigned int num, void __iomem **pbase, dma_addr_t *phy)
+{
+	struct resource *mem;
+	struct resource *ioarea;
+	void __iomem *base;
+
+	mem = platform_get_resource(pd, IORESOURCE_MEM, num);
+	if (!mem) {
+		dev_err(&pd->dev, "IO memory region missing (%d)\n", num);
+		return -ENXIO;
+	}
+	ioarea = devm_request_mem_region(&pd->dev, mem->start,
+					resource_size(mem), dev_name(&pd->dev));
+	if (!ioarea) {
+		dev_err(&pd->dev, "%s IO memory region request failed\n",
+								mem->name);
+		return -ENXIO;
+	}
+	base = devm_ioremap(&pd->dev, mem->start, resource_size(mem));
+	if (!base) {
+		dev_err(&pd->dev, "%s IO remap failed\n", mem->name);
+		return -ENXIO;
+	}
+	*pbase = base;
+
+	if (phy)
+		*phy = mem->start;
+
+	return 0;
+}
+
+static int __init ssi_ports_init(struct hsi_controller *ssi,
+					struct platform_device *pd)
+{
+	struct hsi_port *port;
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	struct omap_ssi_port *omap_port;
+	unsigned int i;
+	int err;
+
+	omap_ssi->port = devm_kzalloc(&pd->dev,
+				sizeof(omap_port) * ssi->num_ports, GFP_KERNEL);
+	if (!omap_ssi->port)
+		return -ENOMEM;
+
+	for (i = 0; i < ssi->num_ports; i++) {
+		port = &ssi->port[i];
+		omap_port = devm_kzalloc(&pd->dev, sizeof(*omap_port),
+								GFP_KERNEL);
+		if (!omap_port)
+			return -ENOMEM;
+		port->async = ssi_async;
+		port->setup = ssi_setup;
+		port->flush = ssi_flush;
+		port->start_tx = ssi_start_tx;
+		port->stop_tx = ssi_stop_tx;
+		port->release = ssi_release;
+		hsi_port_set_drvdata(port, omap_port);
+		/* Get SST base addresses*/
+		err = ssi_get_iomem(pd, ((i * 2) + 2), &omap_port->sst_base,
+							&omap_port->sst_dma);
+		if (err < 0)
+			return err;
+		/* Get SSR base addresses */
+		err = ssi_get_iomem(pd, ((i * 2) + 3), &omap_port->ssr_base,
+							&omap_port->ssr_dma);
+		if (err < 0)
+			return err;
+		err = ssi_port_irq(port, pd);
+		if (err < 0)
+			return err;
+		err = ssi_wake_irq(port, pd);
+		if (err < 0)
+			return err;
+		ssi_queues_init(omap_port);
+		spin_lock_init(&omap_port->lock);
+		spin_lock_init(&omap_port->wk_lock);
+		omap_port->dev = &port->device;
+		omap_ssi->port[i] = omap_port;
+	}
+
+	return 0;
+}
+
+static void ssi_ports_exit(struct hsi_controller *ssi)
+{
+	struct omap_ssi_port *omap_port;
+	unsigned int i;
+
+	for (i = 0; i < ssi->num_ports; i++) {
+		omap_port = hsi_port_drvdata(&ssi->port[i]);
+		tasklet_kill(&omap_port->wake_tasklet);
+		tasklet_kill(&omap_port->pio_tasklet);
+	}
+}
+
+static void ssi_clk_release(struct device *dev __maybe_unused, void *res)
+{
+	struct ssi_clk_res *r = res;
+
+	clk_put(r->clk);
+}
+
+static struct clk *__init ssi_devm_clk_get(struct device *dev, const char *id)
+{
+	struct ssi_clk_res *pclk;
+	struct clk *clk;
+
+	pclk = devres_alloc(ssi_clk_release, sizeof(*pclk), GFP_KERNEL);
+	if (!pclk) {
+		dev_err(dev, "Could not allocate the device resource entry\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	clk = clk_get(dev, id);
+	if (IS_ERR(clk)) {
+		dev_err(dev, "clock get %s failed %li\n", id, PTR_ERR(clk));
+		devres_free(pclk);
+	} else {
+		pclk->clk = clk;
+		devres_add(dev, pclk);
+	}
+
+	return clk;
+}
+
+static int __init ssi_add_controller(struct hsi_controller *ssi,
+						struct platform_device *pd)
+{
+	struct omap_ssi_platform_data *omap_ssi_pdata = pd->dev.platform_data;
+	struct omap_ssi_controller *omap_ssi;
+	struct resource *irq;
+	int err;
+
+	omap_ssi = devm_kzalloc(&pd->dev, sizeof(*omap_ssi), GFP_KERNEL);
+	if (!omap_ssi) {
+		dev_err(&pd->dev, "not enough memory for omap ssi\n");
+		return -ENOMEM;
+	}
+	ssi->id = pd->id;
+	ssi->owner = THIS_MODULE;
+	ssi->device.parent = &pd->dev;
+	dev_set_name(&ssi->device, "ssi%d", ssi->id);
+	hsi_controller_set_drvdata(ssi, omap_ssi);
+	omap_ssi->dev = &ssi->device;
+	err = ssi_get_iomem(pd, 0, &omap_ssi->sys, NULL);
+	if (err < 0)
+		return err;
+	err = ssi_get_iomem(pd, 1, &omap_ssi->gdd, NULL);
+	if (err < 0)
+		return err;
+	irq = platform_get_resource(pd, IORESOURCE_IRQ, 0);
+	if (!irq) {
+		dev_err(&pd->dev, "GDD IRQ resource missing\n");
+		return -ENXIO;
+	}
+	omap_ssi->gdd_irq = irq->start;
+	tasklet_init(&omap_ssi->gdd_tasklet, ssi_gdd_tasklet,
+							(unsigned long)ssi);
+	err = devm_request_irq(&pd->dev, omap_ssi->gdd_irq, ssi_gdd_isr,
+						IRQF_DISABLED, irq->name, ssi);
+	if (err < 0) {
+		dev_err(&ssi->device, "Request GDD IRQ %d failed (%d)",
+							omap_ssi->gdd_irq, err);
+		return err;
+	}
+	err = ssi_ports_init(ssi, pd);
+	if (err < 0)
+		return err;
+	omap_ssi->get_loss = omap_ssi_pdata->get_dev_context_loss_count;
+	omap_ssi->max_speed = UINT_MAX;
+	spin_lock_init(&omap_ssi->lock);
+	spin_lock_init(&omap_ssi->ck_lock);
+	omap_ssi->ick = ssi_devm_clk_get(&pd->dev, "ssi_ick");
+	if (IS_ERR(omap_ssi->ick))
+		return PTR_ERR(omap_ssi->ick);
+	omap_ssi->fck = ssi_devm_clk_get(&pd->dev, "ssi_ssr_fck");
+	if (IS_ERR(omap_ssi->fck))
+		return PTR_ERR(omap_ssi->fck);
+	err = hsi_register_controller(ssi);
+
+	return err;
+}
+
+static int __init ssi_hw_init(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+	unsigned int i;
+	u32 val;
+	int err;
+
+	err = ssi_clk_enable(ssi);
+	if (err < 0) {
+		dev_err(&ssi->device, "Failed to enable the clocks %d\n", err);
+		return err;
+	}
+	/* Reseting SSI controller */
+	__raw_writel(SSI_SOFTRESET, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	val = __raw_readl(omap_ssi->sys + SSI_SYSSTATUS_REG);
+	for (i = 0; ((i < 20) && !(val & SSI_RESETDONE)); i++) {
+		msleep(20);
+		val = __raw_readl(omap_ssi->sys + SSI_SYSSTATUS_REG);
+	}
+	if (!(val & SSI_RESETDONE)) {
+		dev_err(&ssi->device, "SSI HW reset failed\n");
+		ssi_clk_disable(ssi);
+		return -EIO;
+	}
+	/* Reseting GDD */
+	__raw_writel(SSI_SWRESET, omap_ssi->gdd + SSI_GDD_GRST_REG);
+	/* Get FCK rate */
+	omap_ssi->fck_rate = clk_get_rate(omap_ssi->fck) / 1000; /* KHz */
+	dev_dbg(&ssi->device, "SSI fck rate %lu KHz\n", omap_ssi->fck_rate);
+	/* Set default PM settings */
+	val = SSI_AUTOIDLE | SSI_SIDLEMODE_SMART | SSI_MIDLEMODE_SMART;
+	__raw_writel(val, omap_ssi->sys + SSI_SYSCONFIG_REG);
+	omap_ssi->sysconfig = val;
+	__raw_writel(SSI_CLK_AUTOGATING_ON, omap_ssi->sys + SSI_GDD_GCR_REG);
+	omap_ssi->gdd_gcr = SSI_CLK_AUTOGATING_ON;
+	ssi_clk_disable(ssi);
+
+	return 0;
+}
+
+static void ssi_remove_controller(struct hsi_controller *ssi)
+{
+	struct omap_ssi_controller *omap_ssi = hsi_controller_drvdata(ssi);
+
+	ssi_ports_exit(ssi);
+	tasklet_kill(&omap_ssi->gdd_tasklet);
+	hsi_unregister_controller(ssi);
+}
+
+static int __init ssi_probe(struct platform_device *pd)
+{
+	struct omap_ssi_platform_data *omap_ssi_pdata = pd->dev.platform_data;
+	struct hsi_controller *ssi;
+	int err;
+
+	if (!omap_ssi_pdata) {
+		dev_err(&pd->dev, "No OMAP SSI platform data\n");
+		return -EINVAL;
+	}
+	ssi = hsi_alloc_controller(omap_ssi_pdata->num_ports, GFP_KERNEL);
+	if (!ssi) {
+		dev_err(&pd->dev, "No memory for controller\n");
+		return -ENOMEM;
+	}
+	platform_set_drvdata(pd, ssi);
+	err = ssi_add_controller(ssi, pd);
+	if (err < 0)
+		goto out1;
+	err = ssi_hw_init(ssi);
+	if (err < 0)
+		goto out2;
+#ifdef CONFIG_DEBUG_FS
+	err = ssi_debug_add_ctrl(ssi);
+	if (err < 0)
+		goto out2;
+#endif
+	return err;
+out2:
+	ssi_remove_controller(ssi);
+out1:
+	platform_set_drvdata(pd, NULL);
+	hsi_free_controller(ssi);
+
+	return err;
+}
+
+static int __exit ssi_remove(struct platform_device *pd)
+{
+	struct hsi_controller *ssi = platform_get_drvdata(pd);
+
+#ifdef CONFIG_DEBUG_FS
+	ssi_debug_remove_ctrl(ssi);
+#endif
+	ssi_remove_controller(ssi);
+	platform_set_drvdata(pd, NULL);
+	hsi_free_controller(ssi);
+
+	return 0;
+}
+
+static struct platform_driver ssi_pdriver = {
+	.remove	= __exit_p(ssi_remove),
+	.driver	= {
+			.name	= "omap_ssi",
+			.owner	= THIS_MODULE,
+	},
+};
+
+static int __init omap_ssi_init(void)
+{
+	pr_info("OMAP SSI hw driver loaded\n");
+	return platform_driver_probe(&ssi_pdriver, ssi_probe);
+}
+module_init(omap_ssi_init);
+
+static void __exit omap_ssi_exit(void)
+{
+	platform_driver_unregister(&ssi_pdriver);
+	pr_info("OMAP SSI driver removed\n");
+}
+module_exit(omap_ssi_exit);
+
+MODULE_ALIAS("platform:omap_ssi");
+MODULE_AUTHOR("Carlos Chinea <carlos.chinea@nokia.com>");
+MODULE_DESCRIPTION("Synchronous Serial Interface Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hsi/controllers/ste_hsi.c b/drivers/hsi/controllers/ste_hsi.c
new file mode 100644
index 00000000000..34e249f0a91
--- /dev/null
+++ b/drivers/hsi/controllers/ste_hsi.c
@@ -0,0 +1,1833 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com> for ST-Ericsson
+ * Author: Lukasz Baj <lukasz.baj@tieto.com> for ST-Ericsson
+ */
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/hsi/hsi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/gpio.h>
+#include <linux/mfd/dbx500-prcmu.h>
+
+#ifdef CONFIG_STE_DMA40
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#endif
+
+#include <mach/hsi.h>
+
+/*
+ * Copy of HSIR/HSIT context for restoring after HW reset (Vape power off).
+ */
+struct ste_hsi_hw_context {
+	unsigned int tx_mode;
+	unsigned int tx_divisor;
+	unsigned int tx_channels;
+	unsigned int rx_mode;
+	unsigned int rx_channels;
+};
+
+/**
+ * struct ste_hsi_controller - STE HSI controller data
+ * @dev: device associated to STE HSI controller
+ * @tx_dma_base: HSI TX peripheral physical address
+ * @rx_dma_base: HSI RX peripheral physical address
+ * @rx_base: HSI RX peripheral virtual address
+ * @tx_base: HSI TX peripheral virtual address
+ * @regulator: STE HSI Vape consumer regulator
+ * @context: copy of client-configured HSI TX / HSI RX registers
+ * @tx_clk: HSI TX core clock (HSITXCLK)
+ * @rx_clk: HSI RX core clock (HSIRXCLK)
+ * @ssitx_clk: HSI TX host clock (HCLK)
+ * @ssirx_clk: HSI RX host clock (HCLK)
+ * @clk_work: structure for delayed HSI clock disabling
+ * @overrun_irq: HSI channels overrun IRQ table
+ * @ck_refcount: reference count for clock enable operation
+ * @ck_lock: locking primitive for HSI clocks
+ * @lock: locking primitive for HSI controller
+ * @use_dma: flag for DMA enabled
+ * @ck_on: flag for HSI clocks enabled
+ */
+struct ste_hsi_controller {
+	struct device *dev;
+	dma_addr_t tx_dma_base;
+	dma_addr_t rx_dma_base;
+	unsigned char __iomem *rx_base;
+	unsigned char __iomem *tx_base;
+	struct regulator *regulator;
+	struct ste_hsi_hw_context *context;
+	struct clk *tx_clk;
+	struct clk *rx_clk;
+	struct clk *ssitx_clk;
+	struct clk *ssirx_clk;
+	struct delayed_work clk_work;
+	int overrun_irq[STE_HSI_MAX_CHANNELS];
+	int ck_refcount;
+	spinlock_t ck_lock;
+	spinlock_t lock;
+	unsigned int use_dma:1;
+	unsigned int ck_on:1;
+};
+
+#ifdef CONFIG_STE_DMA40
+struct ste_hsi_channel_dma {
+	struct dma_chan *dma_chan;
+	struct dma_async_tx_descriptor *desc;
+	dma_cookie_t cookie;
+};
+#endif
+
+struct ste_hsi_port {
+	struct device *dev;
+	struct list_head txqueue[STE_HSI_MAX_CHANNELS];
+	struct list_head rxqueue[STE_HSI_MAX_CHANNELS];
+	struct list_head brkqueue;
+	int cawake_irq;
+	int acwake_gpio;
+	int tx_irq;
+	int rx_irq;
+	int excep_irq;
+	struct tasklet_struct cawake_tasklet;
+	struct tasklet_struct rx_tasklet;
+	struct tasklet_struct tx_tasklet;
+	struct tasklet_struct exception_tasklet;
+	struct tasklet_struct overrun_tasklet;
+	unsigned char channels;
+#ifdef CONFIG_STE_DMA40
+	struct ste_hsi_channel_dma tx_dma[STE_HSI_MAX_CHANNELS];
+	struct ste_hsi_channel_dma rx_dma[STE_HSI_MAX_CHANNELS];
+#endif
+};
+
+#define hsi_to_ste_port(port) (hsi_port_drvdata(port))
+#define hsi_to_ste_controller(con) (hsi_controller_drvdata(con))
+#define client_to_ste_port(cl) (hsi_port_drvdata(hsi_get_port(cl)))
+#define client_to_hsi(cl) \
+	(to_hsi_controller(hsi_get_port(cl)->device.parent))
+#define client_to_ste_controller(cl)  \
+	(hsi_controller_drvdata(client_to_hsi(cl)))
+#define ste_port_to_ste_controller(port) \
+	((struct ste_hsi_controller *)hsi_controller_drvdata(	\
+		to_hsi_controller(port->dev->parent)))
+
+static u32 ste_hsir_periphid[8] = { 0x2C, 0, 0x8, 0x18, 0xD, 0xF0, 0x5, 0xB1 };
+static u32 ste_hsit_periphid[8] = { 0x2B, 0, 0x8, 0x18, 0xD, 0xF0, 0x5, 0xB1 };
+
+/*
+ * linux/amba/bus.h macros can not be used, because 8 bytes are validated:
+ * PERIPHID0..3 and PCELLID0..3 for HSIR and HSIT.
+ */
+static inline int compare_periphid(u32 *id1, u32 *id2, int count)
+{
+	while (count && *id1++ == *id2++)
+		count--;
+
+	return count;
+}
+
+static void ste_hsi_clk_free(struct clk **pclk)
+{
+	if (IS_ERR(*pclk) && *pclk != NULL)
+		clk_put(*pclk);
+	*pclk = NULL;
+}
+
+static void ste_hsi_init_registers(struct ste_hsi_controller *ste_hsi)
+{
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_BUFSTATE);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_FLUSHBITS);
+	/* TO DO: TX channel priorities will be implemented later */
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_PRIORITY);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_DATASWAP);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_DMAEN);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKID);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIC);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+
+	/* 0x23 is reset value per DB8500 Design Spec */
+	writel(0x23, ste_hsi->rx_base + STE_HSI_RX_THRESHOLD);
+
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_BUFSTATE);
+
+	/* HSIR clock recovery mode */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_DETECTOR);
+
+	/* Bits 0,1,2 set to 1 to clear exception flags */
+	writel(0x07, ste_hsi->rx_base + STE_HSI_RX_ACK);
+
+	/* Bits 0..7 set to 1 to clear OVERRUN IRQ  */
+	writel(0xFF, ste_hsi->rx_base + STE_HSI_RX_OVERRUNACK);
+
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_DMAEN);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIC);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+
+	/* Flush all errors */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_EXCEP);
+
+	/* 2 is Flush state, no RX exception generated afterwards */
+	writel(2, ste_hsi->rx_base + STE_HSI_RX_STATE);
+
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+}
+
+static void ste_hsi_setup_registers(struct ste_hsi_controller *ste_hsi)
+{
+	unsigned int buffers, i;
+	struct ste_hsi_hw_context *pcontext = ste_hsi->context;
+
+	/*
+	 * Configure TX
+	 */
+	writel(pcontext->tx_mode, ste_hsi->tx_base + STE_HSI_TX_MODE);
+	writel(pcontext->tx_divisor, ste_hsi->tx_base + STE_HSI_TX_DIVISOR);
+	writel(pcontext->tx_channels, ste_hsi->tx_base + STE_HSI_TX_CHANNELS);
+	/* Calculate buffers number per channel */
+	buffers = STE_HSI_MAX_BUFFERS / pcontext->tx_channels;
+	for (i = 0; i < pcontext->tx_channels; i++) {
+		/* Set 32 bit long frames */
+		writel(31, ste_hsi->tx_base + STE_HSI_TX_FRAMELENX + 4 * i);
+		writel(buffers * i,
+		       ste_hsi->tx_base + STE_HSI_TX_BASEX + 4 * i);
+		writel(buffers - 1,
+		       ste_hsi->tx_base + STE_HSI_TX_SPANX + 4 * i);
+
+	/*
+	 * The DMA burst request and the buffer occupation interrupt are
+	 * asserted when the free space in the corresponding channel buffer
+	 * is greater than the value programmed in TX_WATERMARKX field.
+	 * The field value must be less than the corresponding SPAN value.
+	 */
+#ifdef CONFIG_STE_DMA40
+		writel(STE_HSI_DMA_MAX_BURST-1,
+			ste_hsi->tx_base + STE_HSI_TX_WATERMARKX + 4 * i);
+#else /* IRQ mode */
+		writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKX + 4 * i);
+#endif
+	}
+
+	/*
+	 * The value read from this register gives the synchronized status
+	 * of the transmitter state and this synchronization takes 2 HSITCLK
+	 * cycles plus 3 HCLK cycles.
+	 */
+	while (STE_HSI_STATE_IDLE != readl(ste_hsi->tx_base + STE_HSI_TX_STATE))
+		cpu_relax();
+
+	/*
+	 * Configure RX
+	 */
+	writel(pcontext->rx_mode, ste_hsi->rx_base + STE_HSI_RX_MODE);
+
+	if (STE_HSI_MODE_PIPELINED == pcontext->rx_mode)
+		/*
+		 * 0xFF: The READY line is negated after the start of the
+		 * 256th frame reception in PIPELINED mode.
+		 */
+		writel(0xFF, ste_hsi->rx_base + STE_HSI_RX_FRAMEBURSTCNT);
+	else
+		writel(0, ste_hsi->rx_base + STE_HSI_RX_FRAMEBURSTCNT);
+
+	writel(pcontext->rx_channels, ste_hsi->rx_base + STE_HSI_RX_CHANNELS);
+	/* Calculate buffers number per channel */
+	buffers = STE_HSI_MAX_BUFFERS / pcontext->rx_channels;
+	for (i = 0; i < pcontext->rx_channels; i++) {
+		/* Set 32 bit long frames */
+		writel(31, ste_hsi->rx_base + STE_HSI_RX_FRAMELENX + 4 * i);
+		writel(buffers * i,
+		       ste_hsi->rx_base + STE_HSI_RX_BASEX + 4 * i);
+		writel(buffers - 1,
+		       ste_hsi->rx_base + STE_HSI_RX_SPANX + 4 * i);
+
+	/*
+	 * The DMA burst request and the buffer occupation interrupt are
+	 * asserted when the busy space in the corresponding channel buffer
+	 * is greater than the value programmed in RX_WATERMARKX field.
+	 * The field value must be less than the corresponding SPAN value.
+	 */
+#ifdef CONFIG_STE_DMA40
+		writel(STE_HSI_DMA_MAX_BURST-1,
+			ste_hsi->rx_base + STE_HSI_RX_WATERMARKX + 4 * i);
+#else /* IRQ mode */
+		writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKX + 4 * i);
+#endif
+	}
+
+	/*
+	 * The value read from this register gives the synchronized status
+	 * of the receiver state and this synchronization takes 2 HSIRCLK
+	 * cycles plus 3 HCLK cycles.
+	 */
+	while (STE_HSI_STATE_IDLE != readl(ste_hsi->tx_base + STE_HSI_RX_STATE))
+		cpu_relax();
+}
+
+/*
+ * When cpuidle framework is setting the sleep or deep sleep state then
+ * the Vape is OFF. This results in re-setting the HSIT/HSIR registers
+ * to default (idle) values.
+ * Function ste_hsi_context() is checking and restoring the HSI registers
+ * to these set by the HSI client by ste_hsi_setup().
+ */
+static void ste_hsi_context(struct ste_hsi_controller *ste_hsi)
+{
+	unsigned int tx_channels;
+	unsigned int rx_channels;
+
+
+	tx_channels = readl(ste_hsi->tx_base + STE_HSI_TX_CHANNELS);
+	rx_channels = readl(ste_hsi->rx_base + STE_HSI_RX_CHANNELS);
+
+	/*
+	 * Checking if the context was lost.
+	 * The target config is at least 2 channels for both TX and RX.
+	 * TX and RX channels are set to 1 after HW reset.
+	 */
+	if ((ste_hsi->context->tx_channels != tx_channels) ||
+		(ste_hsi->context->rx_channels != rx_channels)) {
+		/*
+		 * TO DO: remove "dev_info" after thorough testing.
+		 * Debug left for getting the statistics how frequently the context
+		 * is lost during regular HSI operation.
+		 */
+		dev_info(ste_hsi->dev, "context\n");
+
+		ste_hsi_init_registers(ste_hsi);
+		ste_hsi_setup_registers(ste_hsi);
+	}
+}
+
+static void ste_hsi_clks_free(struct ste_hsi_controller *ste_hsi)
+{
+	ste_hsi_clk_free(&ste_hsi->rx_clk);
+	ste_hsi_clk_free(&ste_hsi->tx_clk);
+	ste_hsi_clk_free(&ste_hsi->ssirx_clk);
+	ste_hsi_clk_free(&ste_hsi->ssitx_clk);
+}
+
+static int ste_hsi_clock_enable(struct hsi_controller *hsi)
+{
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int err = 0;
+
+	spin_lock_bh(&ste_hsi->ck_lock);
+	if (ste_hsi->ck_refcount++ || ste_hsi->ck_on)
+		goto out;
+
+	err = clk_enable(ste_hsi->ssirx_clk);
+	if (unlikely(err))
+		goto out;
+
+	err = clk_enable(ste_hsi->ssitx_clk);
+	if (unlikely(err)) {
+		clk_disable(ste_hsi->ssirx_clk);
+		goto out;
+	}
+
+	err = clk_enable(ste_hsi->rx_clk);
+	if (unlikely(err)) {
+		clk_disable(ste_hsi->ssitx_clk);
+		clk_disable(ste_hsi->ssirx_clk);
+		goto out;
+	}
+
+	err = clk_enable(ste_hsi->tx_clk);
+	if (unlikely(err)) {
+		clk_disable(ste_hsi->rx_clk);
+		clk_disable(ste_hsi->ssitx_clk);
+		clk_disable(ste_hsi->ssirx_clk);
+		goto out;
+	}
+
+	ste_hsi->ck_on = 1;
+out:
+	if (err)
+		ste_hsi->ck_refcount--;
+
+	spin_unlock_bh(&ste_hsi->ck_lock);
+
+	return err;
+}
+
+static void ste_hsi_delayed_disable_clock(struct work_struct *work)
+{
+	struct ste_hsi_controller *ste_hsi;
+	ste_hsi = container_of(work, struct ste_hsi_controller, clk_work.work);
+
+	spin_lock_bh(&ste_hsi->ck_lock);
+
+	/*
+	 * If clock should not be off (enable clock called in meantime)
+	 * or clock is already off nothing to do
+	 */
+	if (ste_hsi->ck_refcount || !ste_hsi->ck_on)
+		goto out;
+
+	if (readl(ste_hsi->tx_base + STE_HSI_TX_STATE) != STE_HSI_STATE_IDLE ||
+	    readl(ste_hsi->rx_base + STE_HSI_RX_STATE)
+	    != STE_HSI_STATE_IDLE ||
+	    readl(ste_hsi->rx_base + STE_HSI_RX_BUFSTATE) != 0) {
+		/* Try again later */
+		int err = schedule_delayed_work(&ste_hsi->clk_work, HZ);
+		if (err < 0)
+			dev_err(ste_hsi->dev, "Error scheduling work\n");
+		goto out;
+	}
+
+	/* Actual clocks disable */
+	clk_disable(ste_hsi->tx_clk);
+	clk_disable(ste_hsi->rx_clk);
+	clk_disable(ste_hsi->ssitx_clk);
+	clk_disable(ste_hsi->ssirx_clk);
+	ste_hsi->ck_on = 0;
+
+out:
+	spin_unlock_bh(&ste_hsi->ck_lock);
+}
+
+static void ste_hsi_clock_disable(struct hsi_controller *hsi)
+{
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+
+	spin_lock_bh(&ste_hsi->ck_lock);
+
+	/* Sanity check */
+	if (ste_hsi->ck_refcount <= 0)
+		WARN_ON(ste_hsi->ck_refcount <= 0);
+
+	/* Need clock to be disable now? */
+	if (--ste_hsi->ck_refcount)
+		goto out;
+
+	/*
+	 * If receiver or transmitter is in the middle something delay clock off
+	 */
+	if (readl(ste_hsi->tx_base + STE_HSI_TX_STATE) != STE_HSI_STATE_IDLE ||
+	    readl(ste_hsi->rx_base + STE_HSI_RX_STATE)
+	    != STE_HSI_STATE_IDLE ||
+	    readl(ste_hsi->rx_base + STE_HSI_RX_BUFSTATE) != 0) {
+		int err = schedule_delayed_work(&ste_hsi->clk_work, HZ);
+		if (err < 0)
+			dev_err(&hsi->device, "Error scheduling work\n");
+
+		goto out;
+	}
+
+	/* Actual clocks disabled */
+	clk_disable(ste_hsi->tx_clk);
+	clk_disable(ste_hsi->rx_clk);
+	clk_disable(ste_hsi->ssitx_clk);
+	clk_disable(ste_hsi->ssirx_clk);
+	ste_hsi->ck_on = 0;
+
+out:
+	spin_unlock_bh(&ste_hsi->ck_lock);
+}
+
+static int ste_hsi_start_irq(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	u32 val;
+	int err;
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	ste_hsi_context(ste_hsi);
+
+	msg->actual_len = 0;
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		val = readl(ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM) |
+		    (1 << msg->channel);
+		writel(val, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+	} else {
+		val = readl(ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM) |
+		    (1 << msg->channel);
+		writel(val, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+
+		val = readl(ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM) |
+		    (1 << msg->channel);
+		writel(val, ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+	}
+
+	return 0;
+}
+
+static int ste_hsi_start_transfer(struct ste_hsi_port *ste_port,
+				  struct list_head *queue);
+#ifdef CONFIG_STE_DMA40
+static void ste_hsi_dma_callback(void *dma_async_param)
+{
+	struct hsi_msg *msg = dma_async_param;
+	struct hsi_controller *hsi = client_to_hsi(msg->cl);
+	struct ste_hsi_port *ste_port = client_to_ste_port(msg->cl);
+	struct ste_hsi_controller *ste_hsi = client_to_ste_controller(msg->cl);
+	struct list_head *queue;
+	struct dma_chan *chan;
+	struct ste_hsi_channel_dma *hsi_dma_chan;
+	char *dma_enable_address;
+	enum dma_data_direction direction;
+	u32 dma_mask;
+
+	/* Message finished, remove from list and notify client */
+	spin_lock_bh(&ste_hsi->lock);
+	list_del(&msg->link);
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		queue = &ste_port->txqueue[msg->channel];
+		direction = DMA_TO_DEVICE;
+		dma_enable_address = ste_hsi->tx_base + STE_HSI_TX_DMAEN;
+		hsi_dma_chan = &ste_port->tx_dma[msg->channel];
+	} else {
+		queue = &ste_port->rxqueue[msg->channel];
+		direction = DMA_FROM_DEVICE;
+		dma_enable_address = ste_hsi->rx_base + STE_HSI_RX_DMAEN;
+		hsi_dma_chan = &ste_port->rx_dma[msg->channel];
+	}
+
+	dma_sync_sg_for_cpu(&hsi->device, msg->sgt.sgl,
+			    msg->sgt.nents, direction);
+	chan = hsi_dma_chan->dma_chan;
+
+	/* disable DMA channel on HSI controller */
+	dma_mask = readl(dma_enable_address);
+	writel(dma_mask & ~(1 << msg->channel), dma_enable_address);
+
+	hsi_dma_chan->desc = NULL;
+
+	dma_unmap_sg(&hsi->device, msg->sgt.sgl, msg->sgt.nents, direction);
+
+	msg->status = HSI_STATUS_COMPLETED;
+	msg->actual_len = sg_dma_len(msg->sgt.sgl);
+
+	spin_unlock_bh(&ste_hsi->lock);
+
+	msg->complete(msg);
+
+	ste_hsi_clock_disable(hsi);
+
+	spin_lock_bh(&ste_hsi->lock);
+	ste_hsi_start_transfer(ste_port, queue);
+	spin_unlock_bh(&ste_hsi->lock);
+}
+
+static void dma_device_control(struct ste_hsi_channel_dma *chan,
+			       enum dma_ctrl_cmd cmd, unsigned long arg)
+{
+	chan->dma_chan->device->device_control(chan->dma_chan, cmd, arg);
+}
+
+static void ste_hsi_terminate_dma_chan(struct ste_hsi_channel_dma *chan)
+{
+	if (chan->desc) {
+		dma_device_control(chan, DMA_TERMINATE_ALL, 0);
+		chan->desc = NULL;
+	}
+	chan->cookie = 0;
+}
+
+static void ste_hsi_terminate_dma(struct ste_hsi_port *ste_port)
+{
+	int i;
+
+	for (i = 0; i < ste_port->channels; ++i) {
+		ste_hsi_terminate_dma_chan(&ste_port->tx_dma[i]);
+		ste_hsi_terminate_dma_chan(&ste_port->rx_dma[i]);
+	}
+}
+
+static int ste_hsi_start_dma(struct hsi_msg *msg)
+{
+	struct hsi_controller *hsi = client_to_hsi(msg->cl);
+	struct ste_hsi_port *ste_port = client_to_ste_port(msg->cl);
+	struct ste_hsi_controller *ste_hsi = client_to_ste_controller(msg->cl);
+	struct dma_async_tx_descriptor *desc;
+	struct dma_chan *chan;
+	struct ste_hsi_channel_dma *hsi_dma_chan;
+	char *dma_enable_address;
+	enum dma_data_direction direction;
+	u32 dma_mask;
+	int err;
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	ste_hsi_context(ste_hsi);
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		direction = DMA_TO_DEVICE;
+		dma_enable_address = ste_hsi->tx_base + STE_HSI_TX_DMAEN;
+		hsi_dma_chan = &ste_port->tx_dma[msg->channel];
+	} else {
+		u32 val;
+		direction = DMA_FROM_DEVICE;
+		dma_enable_address = ste_hsi->rx_base + STE_HSI_RX_DMAEN;
+		hsi_dma_chan = &ste_port->rx_dma[msg->channel];
+
+		/* enable overrun for this channel */
+		val = readl(ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM) |
+		    (1 << msg->channel);
+		writel(val, ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+	}
+
+	chan = hsi_dma_chan->dma_chan;
+
+	if (0 == dma_map_sg(&hsi->device, msg->sgt.sgl, msg->sgt.nents,
+			    direction)) {
+		dev_dbg(&hsi->device, "DMA map SG failed !\n");
+		err = -ENOMEM;
+		goto out;
+	}
+	/* Prepare the scatterlist */
+	desc = chan->device->device_prep_slave_sg(chan,
+						  msg->sgt.sgl,
+						  msg->sgt.nents,
+						  direction,
+						  DMA_PREP_INTERRUPT |
+						  DMA_CTRL_ACK);
+
+	if (!desc) {
+		dma_unmap_sg(&hsi->device, msg->sgt.sgl, msg->sgt.nents,
+			     direction);
+		/* "Complete" DMA (errorpath) */
+		ste_hsi_terminate_dma_chan(hsi_dma_chan);
+		err = -EBUSY;
+		goto out;
+	}
+	desc->callback = ste_hsi_dma_callback;
+	desc->callback_param = msg;
+	hsi_dma_chan->cookie = desc->tx_submit(desc);
+	hsi_dma_chan->desc = desc;
+
+	/* Fire the DMA transaction */
+	chan->device->device_issue_pending(chan);
+
+	/* Enable DMA channel on HSI controller */
+	dma_mask = readl(dma_enable_address);
+	writel(dma_mask | 1 << msg->channel, dma_enable_address);
+
+out:
+	if (unlikely(err))
+		ste_hsi_clock_disable(hsi);
+
+	return err;
+}
+
+static void __init ste_hsi_init_dma(struct ste_hsi_platform_data *data,
+				    struct hsi_controller *hsi)
+{
+	struct hsi_port *port;
+	struct ste_hsi_port *ste_port;
+	struct ste_hsi_controller *ste_hsi = hsi_to_ste_controller(hsi);
+	dma_cap_mask_t mask;
+	int i, ch;
+
+	ste_hsi->use_dma = 1;
+	/* Try to acquire a generic DMA engine slave channel */
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	for (i = 0; i < hsi->num_ports; ++i) {
+		port = &hsi->port[i];
+		ste_port = hsi_port_drvdata(port);
+
+		for (ch = 0; ch < STE_HSI_MAX_CHANNELS; ++ch) {
+			ste_port->tx_dma[ch].dma_chan =
+			    dma_request_channel(mask,
+						data->port_cfg[i].dma_filter,
+						&data->port_cfg[i].
+						dma_tx_cfg[ch]);
+
+			ste_port->rx_dma[ch].dma_chan =
+			    dma_request_channel(mask,
+						data->port_cfg[i].dma_filter,
+						&data->port_cfg[i].
+						dma_rx_cfg[ch]);
+		}
+	}
+}
+
+static int ste_hsi_setup_dma(struct hsi_client *cl)
+{
+	int i;
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct dma_slave_config rx_conf = {
+		.src_addr = 0,	/* dynamic data */
+		.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+		.direction = DMA_FROM_DEVICE,
+		.src_maxburst = STE_HSI_DMA_MAX_BURST,
+	};
+	struct dma_slave_config tx_conf = {
+		.dst_addr = 0,	/* dynamic data */
+		.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+		.direction = DMA_TO_DEVICE,
+		.dst_maxburst = STE_HSI_DMA_MAX_BURST,
+	};
+
+	if (!ste_hsi->use_dma)
+		return 0;
+
+	for (i = 0; i < ste_port->channels; ++i) {
+		struct dma_chan *chan;
+
+		chan = ste_port->tx_dma[i].dma_chan;
+		tx_conf.dst_addr = (dma_addr_t) ste_hsi->tx_dma_base +
+		    STE_HSI_TX_BUFFERX + 4 * i;
+		chan->device->device_control(chan,
+					     DMA_SLAVE_CONFIG,
+					     (unsigned long)&tx_conf);
+
+		chan = ste_port->rx_dma[i].dma_chan;
+		rx_conf.src_addr = (dma_addr_t) ste_hsi->rx_dma_base +
+		    STE_HSI_RX_BUFFERX + 4 * i;
+		chan->device->device_control(chan,
+					     DMA_SLAVE_CONFIG,
+					     (unsigned long)&rx_conf);
+	}
+
+	return 0;
+}
+
+#else
+#define ste_hsi_init_dma(data, hsi) do { } while (0)
+#define ste_hsi_start_dma ste_hsi_start_irq
+#define ste_hsi_terminate_dma(ste_port) do { } while (0)
+#define ste_hsi_setup_dma(cl) do { } while (0)
+#endif
+
+static int ste_hsi_start_transfer(struct ste_hsi_port *ste_port,
+				  struct list_head *queue)
+{
+	struct hsi_msg *msg;
+	int err;
+
+	if (list_empty(queue))
+		return 0;
+
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if (msg->status != HSI_STATUS_QUEUED)
+		return 0;
+
+	msg->actual_len = 0;
+	msg->status = HSI_STATUS_PROCEEDING;
+
+	if (ste_port_to_ste_controller(ste_port)->use_dma)
+		err = ste_hsi_start_dma(msg);
+	else
+		err = ste_hsi_start_irq(msg);
+
+	return err;
+}
+
+static void ste_hsi_receive_data(struct hsi_port *port, unsigned int channel)
+{
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct list_head *queue = &ste_port->rxqueue[channel];
+	struct hsi_msg *msg;
+	char *bufferx;
+	u8 *buf;
+	int span;
+
+	spin_lock_bh(&ste_hsi->lock);
+
+	if (list_empty(queue))
+		goto out;
+
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if ((!msg->sgt.nents) || (!msg->sgt.sgl->length)) {
+		msg->actual_len = 0;
+		msg->status = HSI_STATUS_PENDING;
+	}
+
+	if (msg->status == HSI_STATUS_PROCEEDING && msg->ttype == HSI_MSG_READ) {
+		unsigned char len;
+		bufferx = ste_hsi->rx_base + STE_HSI_RX_BUFFERX + 4 * channel;
+
+		len = readl(ste_hsi->rx_base + STE_HSI_RX_GAUGEX + 4 * channel);
+		buf = sg_virt(msg->sgt.sgl);
+		buf += msg->actual_len;
+		while (len--) {
+			*(u32 *) buf = readl(bufferx);
+			buf += 4;
+			msg->actual_len += 4;
+			if (msg->actual_len >= msg->sgt.sgl->length) {
+				msg->status = HSI_STATUS_COMPLETED;
+				break;
+			}
+		}
+	}
+
+	/* re-enable interrupt by watermark manipulation */
+	span = readl(ste_hsi->rx_base + STE_HSI_RX_SPANX + 4 * channel);
+	writel(span, ste_hsi->rx_base + STE_HSI_RX_WATERMARKX + 4 * channel);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKX + 4 * channel);
+
+	/*
+	 * If message was not transmitted completely enable interrupt for
+	 * further work
+	 */
+	if (msg->status == HSI_STATUS_PROCEEDING) {
+		u32 val;
+		val = readl(ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM) |
+		    (1 << channel);
+		writel(val, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+		goto out;
+	}
+
+	/* Message finished, remove from list and notify client */
+	list_del(&msg->link);
+	spin_unlock_bh(&ste_hsi->lock);
+	msg->complete(msg);
+
+	ste_hsi_clock_disable(hsi);
+
+	spin_lock_bh(&ste_hsi->lock);
+
+	ste_hsi_start_transfer(ste_port, queue);
+out:
+	spin_unlock_bh(&ste_hsi->lock);
+}
+
+static void ste_hsi_transmit_data(struct hsi_port *port, unsigned int channel)
+{
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct list_head *queue = &ste_port->txqueue[channel];
+	struct hsi_msg *msg;
+	u8 *buf;
+	int span;
+
+	if (list_empty(queue))
+		return;
+
+	spin_lock_bh(&ste_hsi->lock);
+	msg = list_first_entry(queue, struct hsi_msg, link);
+	if ((!msg->sgt.nents) || (!msg->sgt.sgl->length)) {
+		msg->actual_len = 0;
+		msg->status = HSI_STATUS_PENDING;
+	}
+
+	if (msg->status == HSI_STATUS_PROCEEDING &&
+		msg->ttype == HSI_MSG_WRITE) {
+		unsigned char free_space;
+
+		free_space = readl(ste_hsi->tx_base +
+				   STE_HSI_TX_GAUGEX + 4 * channel);
+		buf = sg_virt(msg->sgt.sgl);
+		buf += msg->actual_len;
+		while (free_space--) {
+			writel(*(u32 *) buf, ste_hsi->tx_base +
+			       STE_HSI_TX_BUFFERX + 4 * channel);
+			buf += 4;
+			msg->actual_len += 4;
+			if (msg->actual_len >= msg->sgt.sgl->length) {
+				msg->status = HSI_STATUS_COMPLETED;
+				break;
+			}
+		}
+	}
+
+	span = readl(ste_hsi->tx_base + STE_HSI_TX_SPANX + 4 * channel);
+	writel(span, ste_hsi->tx_base + STE_HSI_TX_WATERMARKX + 4 * channel);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKX + 4 * channel);
+
+	if (msg->status == HSI_STATUS_PROCEEDING) {
+		u32 val;
+		val = readl(ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM) |
+		    (1 << channel);
+		writel(val, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+		goto out;
+	}
+
+	list_del(&msg->link);
+	spin_unlock_bh(&ste_hsi->lock);
+	msg->complete(msg);
+
+	ste_hsi_clock_disable(hsi);
+
+	spin_lock_bh(&ste_hsi->lock);
+	ste_hsi_start_transfer(ste_port, queue);
+out:
+	spin_unlock_bh(&ste_hsi->lock);
+}
+
+static void ste_hsi_cawake_tasklet(unsigned long data)
+{
+	struct hsi_port *port = (struct hsi_port *)data;
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	u32 prcm_line_value;
+	int level;
+
+	prcm_line_value = prcmu_read(DB8500_PRCM_LINE_VALUE);
+	level = (prcm_line_value & DB8500_PRCM_LINE_VALUE_HSI_CAWAKE0) ? 1 : 0;
+
+	dev_info(ste_hsi->dev, "cawake %s\n", level ? "HIGH" : "LOW");
+	hsi_event(hsi->port, level ? HSI_EVENT_START_RX : HSI_EVENT_STOP_RX);
+	enable_irq(ste_port->cawake_irq);
+}
+
+static irqreturn_t ste_hsi_cawake_isr(int irq, void *data)
+{
+	struct hsi_port *port = data;
+
+	/* IRQ processed only if device initialized */
+	if ((port->device.parent) && (data)) {
+		struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+		disable_irq_nosync(irq);
+		tasklet_hi_schedule(&ste_port->cawake_tasklet);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void ste_hsi_rx_tasklet(unsigned long data)
+{
+	struct hsi_port *port = (struct hsi_port *)data;
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	u32 irq_status, irq_mask;
+	unsigned int i;
+
+	irq_status = readl(ste_hsi->rx_base + STE_HSI_RX_WATERMARKMIS);
+	if (!irq_status)
+		goto out;
+
+	irq_mask = readl(ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+	writel(irq_mask & ~irq_status,
+	       ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+	writel(irq_mask, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIC);
+
+	for (i = 0; i < STE_HSI_MAX_CHANNELS; ++i) {
+		if (irq_status & (1 << i))
+			ste_hsi_receive_data(port, i);
+	}
+out:
+	enable_irq(ste_port->rx_irq);
+}
+
+static irqreturn_t ste_hsi_rx_isr(int irq, void *data)
+{
+	struct hsi_port *port = data;
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+	disable_irq_nosync(irq);
+	tasklet_hi_schedule(&ste_port->rx_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t ste_hsi_tx_isr(int irq, void *data)
+{
+	struct hsi_port *port = data;
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+	disable_irq_nosync(irq);
+	tasklet_hi_schedule(&ste_port->tx_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static void ste_hsi_tx_tasklet(unsigned long data)
+{
+	struct hsi_port *port = (struct hsi_port *)data;
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	u32 irq_status, irq_mask;
+	unsigned int i;
+
+	irq_status = readl(ste_hsi->tx_base + STE_HSI_TX_WATERMARKMIS);
+	if (!irq_status)
+		goto out;
+
+	irq_mask = readl(ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+	writel(irq_mask & ~irq_status,
+	       ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+	writel(irq_mask, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIC);
+
+	for (i = 0; i < STE_HSI_MAX_CHANNELS; ++i) {
+		if (irq_status & (1 << i))
+			ste_hsi_transmit_data(port, i);
+	}
+out:
+	enable_irq(ste_port->tx_irq);
+}
+
+static void ste_hsi_break_complete(struct hsi_port *port,
+				   struct ste_hsi_controller *ste_hsi)
+{
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg, *tmp;
+	u32 mask;
+
+	dev_dbg(port->device.parent, "HWBREAK received\n");
+
+	spin_lock_bh(&ste_hsi->lock);
+
+	mask = readl(ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+	writel(mask & ~STE_HSI_EXCEP_BREAK,
+	       ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+
+	spin_unlock_bh(&ste_hsi->lock);
+
+	list_for_each_entry_safe(msg, tmp, &ste_port->brkqueue, link) {
+		msg->status = HSI_STATUS_COMPLETED;
+		list_del(&msg->link);
+		msg->complete(msg);
+	}
+}
+
+static void ste_hsi_error(struct hsi_port *port)
+{
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_msg *msg;
+	unsigned int i;
+
+	for (i = 0; i < ste_port->channels; i++) {
+		if (list_empty(&ste_port->rxqueue[i]))
+			continue;
+		msg = list_first_entry(&ste_port->rxqueue[i], struct hsi_msg,
+				       link);
+		list_del(&msg->link);
+		msg->status = HSI_STATUS_ERROR;
+		msg->complete(msg);
+		/* Now restart queued reads if any */
+		ste_hsi_start_transfer(ste_port, &ste_port->rxqueue[i]);
+	}
+}
+
+static void ste_hsi_exception_tasklet(unsigned long data)
+{
+	struct hsi_port *port = (struct hsi_port *)data;
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	u32 error_status;
+	u32 error_interrupts;
+
+	error_status = readl(ste_hsi->rx_base + STE_HSI_RX_EXCEP);
+	/*
+	 * sometimes interrupt that cause running this tasklet is already
+	 * inactive so base handling of exception on masked interrupt status
+	 * not on exception state register.
+	 */
+	error_interrupts = readl(ste_hsi->rx_base + STE_HSI_RX_EXCEPMIS);
+
+	if (error_interrupts & STE_HSI_EXCEP_BREAK)
+		ste_hsi_break_complete(port, ste_hsi);
+
+	if (error_interrupts & STE_HSI_EXCEP_TIMEOUT)
+		dev_err(&hsi->device, "timeout exception occurred\n");
+	if (error_interrupts & STE_HSI_EXCEP_OVERRUN)
+		dev_err(&hsi->device, "overrun exception occurred\n");
+	if (error_interrupts & STE_HSI_EXCEP_PARITY)
+		dev_err(&hsi->device, "parity exception occurred\n");
+
+	if (error_interrupts & ~STE_HSI_EXCEP_BREAK)
+		ste_hsi_error(port);
+
+	/* Acknowledge exception interrupts */
+	writel(error_status, ste_hsi->rx_base + STE_HSI_RX_ACK);
+
+	enable_irq(ste_port->excep_irq);
+}
+
+static irqreturn_t ste_hsi_exception_isr(int irq, void *data)
+{
+	struct hsi_port *port = data;
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+	disable_irq_nosync(irq);
+	tasklet_hi_schedule(&ste_port->exception_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static void ste_hsi_overrun_tasklet(unsigned long data)
+{
+	struct hsi_controller *hsi = (struct hsi_controller *)data;
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct hsi_port *hsi_port = &hsi->port[0];
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(hsi_port);
+	struct hsi_msg *msg;
+
+	unsigned int channel;
+	u8 rised_overrun;
+	u8 mask;
+	u8 blocked = 0;
+
+	rised_overrun = (u8) readl(ste_hsi->rx_base + STE_HSI_RX_OVERRUNMIS);
+	mask = rised_overrun;
+	for (channel = 0; mask; ++channel, mask >>= 1) {
+		if (!(mask & 1))
+			continue;
+
+		do {
+			/*
+			 * No more messages, block interrupt
+			 */
+			if (list_empty(&ste_port->rxqueue[channel])) {
+				blocked |= 1 << channel;
+				break;
+			}
+			/*
+			 * Complete with error
+			 */
+			msg = list_first_entry(&ste_port->rxqueue[channel],
+					       struct hsi_msg, link);
+			list_del(&msg->link);
+			msg->status = HSI_STATUS_ERROR;
+			msg->complete(msg);
+
+			/*
+			 * Now restart queued reads if any
+			 * If start_transfer fails, try with next message
+			 */
+			if (ste_hsi_start_transfer(ste_port,
+						   &ste_port->rxqueue[channel]))
+				continue;
+		} while (0);
+	}
+
+	/* Overrun acknowledge */
+	writel(rised_overrun, ste_hsi->rx_base + STE_HSI_RX_OVERRUNACK);
+	writel(~blocked & readl(ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM),
+	       ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+
+	/*
+	 * Enable all that should not be blocked
+	 */
+	mask = rised_overrun & ~blocked;
+	for (channel = 0; mask; ++channel, mask >>= 1)
+		enable_irq(ste_hsi->overrun_irq[channel]);
+}
+
+static irqreturn_t ste_hsi_overrun_isr(int irq, void *data)
+{
+	struct hsi_port *port = data;
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+	disable_irq_nosync(irq);
+	tasklet_hi_schedule(&ste_port->overrun_tasklet);
+
+	return IRQ_HANDLED;
+}
+
+static void __init ste_hsi_queues_init(struct ste_hsi_port *ste_port)
+{
+	unsigned int ch;
+
+	for (ch = 0; ch < STE_HSI_MAX_CHANNELS; ch++) {
+		INIT_LIST_HEAD(&ste_port->txqueue[ch]);
+		INIT_LIST_HEAD(&ste_port->rxqueue[ch]);
+	}
+	INIT_LIST_HEAD(&ste_port->brkqueue);
+}
+
+static int __init ste_hsi_get_iomem(struct platform_device *pdev,
+				    const char *res_name,
+				    unsigned char __iomem **base,
+				    dma_addr_t *phy)
+{
+	struct resource *mem;
+	struct resource *ioarea;
+
+	mem = platform_get_resource_byname(pdev, IORESOURCE_MEM, res_name);
+	if (!mem) {
+		dev_err(&pdev->dev, "IO memory region missing!\n");
+		return -ENXIO;
+	}
+
+	ioarea = devm_request_mem_region(&pdev->dev, mem->start,
+					 resource_size(mem),
+					 dev_name(&pdev->dev));
+	if (!ioarea) {
+		dev_err(&pdev->dev, "Can't request IO memory region!\n");
+		return -ENXIO;
+	}
+
+	*base = devm_ioremap(&pdev->dev, mem->start, resource_size(mem));
+	if (!base) {
+		dev_err(&pdev->dev, "%s IO remap failed!\n", mem->name);
+		return -ENXIO;
+	}
+	if (phy)
+		*phy = (dma_addr_t) mem->start;
+
+	return 0;
+}
+
+static int __init ste_hsi_acwake_gpio_init(struct platform_device *pdev,
+					   int *gpio)
+{
+	int err = 0;
+	const char *gpio_name = "hsi0_acwake";
+	struct resource *resource;
+
+	resource = platform_get_resource_byname(pdev, IORESOURCE_IO, gpio_name);
+	if (unlikely(!resource)) {
+		dev_err(&pdev->dev, "hsi0_acwake does not exist\n");
+		return -EINVAL;
+	}
+
+	*gpio = resource->start;
+	err = gpio_request(*gpio, gpio_name);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Can't request GPIO %d\n", *gpio);
+		return err;
+	}
+
+	/* Initial level set to 0 (LOW) */
+	err = gpio_direction_output(*gpio, 0);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Can't init GPIO %d\n", *gpio);
+		gpio_free(*gpio);
+	}
+
+	return err;
+}
+
+static int __init ste_hsi_get_irq(struct platform_device *pdev,
+				  const char *res_name,
+				  irqreturn_t(*isr) (int, void *), void *data,
+				  int *irq_number)
+{
+	struct resource *irq;
+	int err;
+
+	irq = platform_get_resource_byname(pdev, IORESOURCE_IRQ, res_name);
+	if (!irq) {
+		dev_err(&pdev->dev, "IO memory region missing!\n");
+		return -ENXIO;
+	}
+
+	err = devm_request_irq(&pdev->dev, irq->start, isr,
+			       IRQF_DISABLED, irq->name, data);
+	if (err)
+		dev_err(&pdev->dev, "%s IRQ request failed!\n", irq->name);
+
+	if (irq_number)
+		*irq_number = irq->start;
+
+	return err;
+}
+
+static void ste_hsi_flush_queue(struct list_head *queue, struct hsi_client *cl)
+{
+	struct list_head *node, *tmp;
+	struct hsi_msg *msg;
+
+	list_for_each_safe(node, tmp, queue) {
+		msg = list_entry(node, struct hsi_msg, link);
+		if ((cl) && (cl != msg->cl))
+			continue;
+		list_del(node);
+
+		if (msg->destructor)
+			msg->destructor(msg);
+		else
+			hsi_free_msg(msg);
+	}
+}
+
+static int ste_hsi_async_break(struct hsi_msg *msg)
+{
+	struct hsi_port *port = hsi_get_port(msg->cl);
+	struct ste_hsi_port *ste_port = hsi_to_ste_port(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int err;
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		if (port->tx_cfg.mode != HSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		writel(1, ste_hsi->tx_base + STE_HSI_TX_BREAK);
+		msg->status = HSI_STATUS_COMPLETED;
+		msg->complete(msg);
+	} else {
+		u32 mask;
+		if (port->rx_cfg.mode != HSI_MODE_FRAME) {
+			err = -EINVAL;
+			goto out;
+		}
+		spin_lock_bh(&ste_hsi->lock);
+		msg->status = HSI_STATUS_PROCEEDING;
+		mask = readl(ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+		/* Enable break exception on controller */
+		if (!(mask & STE_HSI_EXCEP_BREAK))
+			writel(mask | STE_HSI_EXCEP_BREAK,
+			       ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+
+		list_add_tail(&msg->link, &ste_port->brkqueue);
+		spin_unlock_bh(&ste_hsi->lock);
+	}
+
+out:
+	ste_hsi_clock_disable(hsi);
+	return err;
+}
+
+static int ste_hsi_async(struct hsi_msg *msg)
+{
+	struct ste_hsi_controller *ste_hsi;
+	struct ste_hsi_port *ste_port;
+	struct list_head *queue;
+	int err = 0;
+
+	if (unlikely(!msg))
+		return -ENOSYS;
+
+	if (msg->sgt.nents > 1)
+		return -ENOSYS;
+
+	if (unlikely(msg->break_frame))
+		return ste_hsi_async_break(msg);
+
+	ste_port = client_to_ste_port(msg->cl);
+	ste_hsi = client_to_ste_controller(msg->cl);
+
+	if (msg->ttype == HSI_MSG_WRITE) {
+		/* TX transfer */
+		BUG_ON(msg->channel >= ste_port->channels);
+		queue = &ste_port->txqueue[msg->channel];
+	} else {
+		/* RX transfer */
+		queue = &ste_port->rxqueue[msg->channel];
+	}
+
+	spin_lock_bh(&ste_hsi->lock);
+	list_add_tail(&msg->link, queue);
+	msg->status = HSI_STATUS_QUEUED;
+
+	err = ste_hsi_start_transfer(ste_port, queue);
+	if (err)
+		list_del(&msg->link);
+
+	spin_unlock_bh(&ste_hsi->lock);
+
+	return err;
+}
+
+static int ste_hsi_setup(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int err;
+	u32 div = 0;
+
+	if (ste_hsi->regulator)
+		regulator_enable(ste_hsi->regulator);
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	if (cl->tx_cfg.speed) {
+		div = clk_get_rate(ste_hsi->tx_clk) / 1000 / cl->tx_cfg.speed;
+		if (div)
+			--div;
+	}
+
+	if (!ste_hsi->context)
+		ste_hsi->context = kzalloc(sizeof(struct ste_hsi_hw_context), GFP_KERNEL);
+
+	if (!ste_hsi->context) {
+		dev_err(ste_hsi->dev, "Not enough memory for context!\n");
+		return -ENOMEM;
+	} else {
+		/* Save HSI context */
+		ste_hsi->context->tx_mode = cl->tx_cfg.mode;
+		ste_hsi->context->tx_divisor = div;
+		ste_hsi->context->tx_channels = cl->tx_cfg.channels;
+
+		if ((HSI_FLOW_PIPE == cl->rx_cfg.flow) &&
+			(HSI_MODE_FRAME == cl->rx_cfg.mode))
+			ste_hsi->context->rx_mode = STE_HSI_MODE_PIPELINED;
+		else
+			ste_hsi->context->rx_mode = cl->rx_cfg.mode;
+
+		ste_hsi->context->rx_channels = cl->rx_cfg.channels;
+	}
+
+	port->tx_cfg = cl->tx_cfg;
+	port->rx_cfg = cl->rx_cfg;
+
+	ste_hsi_setup_registers(ste_hsi);
+
+	ste_port->channels = max(cl->tx_cfg.channels, cl->rx_cfg.channels);
+
+	ste_hsi_setup_dma(cl);
+
+	ste_hsi_clock_disable(hsi);
+
+	if (ste_hsi->regulator)
+		regulator_disable(ste_hsi->regulator);
+
+	return err;
+}
+
+static int ste_hsi_flush(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int i;
+
+	ste_hsi_clock_enable(hsi);
+
+	/* Enter sleep mode */
+	writel(STE_HSI_MODE_SLEEP, ste_hsi->rx_base + STE_HSI_RX_MODE);
+
+	/* Disable DMA, and terminate all outstanding jobs */
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_DMAEN);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_DMAEN);
+	ste_hsi_terminate_dma(ste_port);
+
+	/* Flush HSIT buffers */
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_STATE);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_BUFSTATE);
+
+	/* Flush HSIR pipeline and channel buffers */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_STATE);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_PIPEGAUGE);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_BUFSTATE);
+
+	/* Flush all errors */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_EXCEP);
+
+	/* Clear interrupts */
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIM);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_WATERMARKIC);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIM);
+	writel(0, ste_hsi->tx_base + STE_HSI_TX_WATERMARKIC);
+	writel(0xFF, ste_hsi->rx_base + STE_HSI_RX_OVERRUNACK);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_OVERRUNIM);
+	writel(0, ste_hsi->rx_base + STE_HSI_RX_EXCEPIM);
+	writel(0x0F, ste_hsi->rx_base + STE_HSI_RX_ACK);
+
+	/* Dequeue all pending requests */
+	for (i = 0; i < ste_port->channels; i++) {
+		/* Release write clocks */
+		if (!list_empty(&ste_port->txqueue[i]))
+			ste_hsi_clock_disable(hsi);
+		if (!list_empty(&ste_port->rxqueue[i]))
+			ste_hsi_clock_disable(hsi);
+		ste_hsi_flush_queue(&ste_port->txqueue[i], NULL);
+		ste_hsi_flush_queue(&ste_port->rxqueue[i], NULL);
+	}
+	ste_hsi_flush_queue(&ste_port->brkqueue, NULL);
+
+	ste_hsi_clock_disable(hsi);
+
+	return 0;
+}
+
+static int ste_hsi_start_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+
+	if (ste_hsi->regulator)
+		regulator_enable(ste_hsi->regulator);
+
+	gpio_set_value(ste_port->acwake_gpio, 1); /* HIGH */
+
+	return 0;
+}
+
+static int ste_hsi_stop_tx(struct hsi_client *cl)
+{
+	struct hsi_port *port = to_hsi_port(cl->device.parent);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+	struct hsi_controller *hsi = to_hsi_controller(port->device.parent);
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+
+	gpio_set_value(ste_port->acwake_gpio, 0); /* LOW */
+
+	if (ste_hsi->regulator)
+		regulator_disable(ste_hsi->regulator);
+
+	return 0;
+}
+
+static int ste_hsi_release(struct hsi_client *cl)
+{
+	int err;
+	struct ste_hsi_controller *ste_hsi = client_to_ste_controller(cl);
+
+	err = ste_hsi_flush(cl);
+	cancel_delayed_work(&ste_hsi->clk_work);
+
+	return 0;
+}
+
+static int ste_hsi_ports_init(struct hsi_controller *hsi,
+			      struct platform_device *pdev)
+{
+	struct hsi_port *port;
+	struct ste_hsi_port *ste_port;
+	unsigned int i;
+	char irq_name[20];
+	int err;
+
+	for (i = 0; i < hsi->num_ports; i++) {
+		ste_port = devm_kzalloc(&pdev->dev, sizeof *ste_port,
+					GFP_KERNEL);
+		if (!ste_port)
+			return -ENOMEM;
+
+		port = &hsi->port[i];
+		port->async = ste_hsi_async;
+		port->setup = ste_hsi_setup;
+		port->flush = ste_hsi_flush;
+		port->start_tx = ste_hsi_start_tx;
+		port->stop_tx = ste_hsi_stop_tx;
+		port->release = ste_hsi_release;
+		hsi_port_set_drvdata(port, ste_port);
+		ste_port->dev = &port->device;
+
+		err = ste_hsi_acwake_gpio_init(pdev, &ste_port->acwake_gpio);
+		if (err)
+			return err;
+
+		sprintf(irq_name, "hsi0_cawake");
+		err = ste_hsi_get_irq(pdev, irq_name, ste_hsi_cawake_isr, port,
+				      &ste_port->cawake_irq);
+		if (err)
+			return err;
+
+		sprintf(irq_name, "hsi_rx_irq%d", i);
+		err = ste_hsi_get_irq(pdev, irq_name, ste_hsi_rx_isr, port,
+				      &ste_port->rx_irq);
+		if (err)
+			return err;
+
+		sprintf(irq_name, "hsi_tx_irq%d", i);
+		err = ste_hsi_get_irq(pdev, irq_name, ste_hsi_tx_isr, port,
+				      &ste_port->tx_irq);
+		if (err)
+			return err;
+
+		tasklet_init(&ste_port->cawake_tasklet, ste_hsi_cawake_tasklet,
+			     (unsigned long)port);
+
+		tasklet_init(&ste_port->rx_tasklet, ste_hsi_rx_tasklet,
+			     (unsigned long)port);
+
+		tasklet_init(&ste_port->tx_tasklet, ste_hsi_tx_tasklet,
+			     (unsigned long)port);
+
+		tasklet_init(&ste_port->exception_tasklet,
+			     ste_hsi_exception_tasklet, (unsigned long)port);
+
+		tasklet_init(&ste_port->overrun_tasklet,
+			     ste_hsi_overrun_tasklet, (unsigned long)port);
+
+		sprintf(irq_name, "hsi_rx_excep%d", i);
+		err = ste_hsi_get_irq(pdev, irq_name, ste_hsi_exception_isr,
+				      port, &ste_port->excep_irq);
+		if (err)
+			return err;
+
+		ste_hsi_queues_init(ste_port);
+	}
+	return 0;
+}
+
+static int __init ste_hsi_hw_init(struct hsi_controller *hsi)
+{
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	int err;
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		return err;
+
+	ste_hsi_init_registers(ste_hsi);
+
+	ste_hsi_clock_disable(hsi);
+
+	return err;
+}
+
+static int __init ste_hsi_add_controller(struct hsi_controller *hsi,
+					 struct platform_device *pdev)
+{
+	struct ste_hsi_controller *ste_hsi;
+	char overrun_name[] = "hsi_rx_overrun_chxxx";
+	unsigned char i;
+	int err;
+	unsigned long rate;
+
+	ste_hsi = kzalloc(sizeof(struct ste_hsi_controller), GFP_KERNEL);
+	if (!ste_hsi) {
+		dev_err(&pdev->dev, "Not enough memory for ste_hsi!\n");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&ste_hsi->lock);
+	spin_lock_init(&ste_hsi->ck_lock);
+	INIT_DELAYED_WORK(&ste_hsi->clk_work, ste_hsi_delayed_disable_clock);
+
+	hsi->id = pdev->id;
+	hsi->device.parent = &pdev->dev;
+	dev_set_name(&hsi->device, "ste-hsi.%d", hsi->id);
+	ste_hsi->dev = &hsi->device;
+	hsi_controller_set_drvdata(hsi, ste_hsi);
+
+	/* Get and enable regulator */
+	ste_hsi->regulator = regulator_get(&pdev->dev, "v-hsi");
+	if (IS_ERR(ste_hsi->regulator)) {
+		dev_err(&pdev->dev, "could not get v-hsi regulator\n");
+		ste_hsi->regulator = NULL;
+	} else {
+		regulator_enable(ste_hsi->regulator);
+	}
+
+	/* Get and reserve resources for receiver */
+	err = ste_hsi_get_iomem(pdev, "hsi_rx_base", &ste_hsi->rx_base,
+				&ste_hsi->rx_dma_base);
+	if (err)
+		goto err_free_mem;
+	dev_info(&pdev->dev, "hsi_rx_base = %p\n", ste_hsi->rx_base);
+
+	/* Get and reserve resources for transmitter */
+	err = ste_hsi_get_iomem(pdev, "hsi_tx_base", &ste_hsi->tx_base,
+				&ste_hsi->tx_dma_base);
+	if (err)
+		goto err_free_mem;
+	dev_info(&pdev->dev, "hsi_tx_base = %p\n", ste_hsi->tx_base);
+
+	/* Get HSIT HSITXCLK clock */
+	ste_hsi->tx_clk = clk_get(&pdev->dev, "hsit_hsitxclk");
+	if (IS_ERR(ste_hsi->tx_clk)) {
+		dev_err(&hsi->device, "Couldn't get HSIT HSITXCLK clock\n");
+		err = PTR_ERR(ste_hsi->tx_clk);
+		goto err_free_mem;
+	}
+
+	/* Get HSIR HSIRXCLK clock */
+	ste_hsi->rx_clk = clk_get(&pdev->dev, "hsir_hsirxclk");
+	if (IS_ERR(ste_hsi->rx_clk)) {
+		dev_err(&hsi->device, "Couldn't get HSIR HSIRXCLK clock\n");
+		err = PTR_ERR(ste_hsi->rx_clk);
+		goto err_clk_free;
+	}
+
+	/* Get HSIT HCLK clock */
+	ste_hsi->ssitx_clk = clk_get(&pdev->dev, "hsit_hclk");
+	if (IS_ERR(ste_hsi->ssitx_clk)) {
+		dev_err(&hsi->device, "Couldn't get HSIT HCLK clock\n");
+		err = PTR_ERR(ste_hsi->ssitx_clk);
+		goto err_clk_free;
+	}
+
+	/* Get HSIR HCLK clock */
+	ste_hsi->ssirx_clk = clk_get(&pdev->dev, "hsir_hclk");
+	if (IS_ERR(ste_hsi->ssirx_clk)) {
+		dev_err(&hsi->device, "Couldn't get HSIR HCLK clock\n");
+		err = PTR_ERR(ste_hsi->ssirx_clk);
+		goto err_clk_free;
+	}
+
+	/* Set HSITXCLK rate to 100 MHz */
+	rate = clk_round_rate(ste_hsi->tx_clk, 100000000);
+	err = clk_set_rate(ste_hsi->tx_clk, rate);
+	if (unlikely(err)) {
+		dev_err(&hsi->device, "Couldn't set HSIT clock rate\n");
+		goto err_clk_free;
+	}
+
+	/* Set HSIRXCLK rate to 200 MHz */
+	rate = clk_round_rate(ste_hsi->rx_clk, 200000000);
+	err = clk_set_rate(ste_hsi->rx_clk, rate);
+	if (unlikely(err)) {
+		dev_err(&hsi->device, "Couldn't set HSIR clock rate\n");
+		goto err_clk_free;
+	}
+
+	err = ste_hsi_clock_enable(hsi);
+	if (unlikely(err))
+		goto err_clk_free;
+
+	/* Check if controller is at specified address */
+	if (compare_periphid(ste_hsir_periphid,
+			     (u32 *) (ste_hsi->rx_base + 0xFE0), 8)) {
+		dev_err(&pdev->dev, "No hsir controller at = %p\n",
+			ste_hsi->rx_base);
+		err = -ENXIO;
+		goto err_clk_free;
+	}
+
+	/* Check if controller is at specified address */
+	if (compare_periphid(ste_hsit_periphid,
+			     (u32 *) (ste_hsi->tx_base + 0xFE0), 8)) {
+		dev_err(&pdev->dev, "No hsit controller at = %p\n",
+			ste_hsi->tx_base);
+		err = -ENXIO;
+		goto err_clk_free;
+	}
+	ste_hsi_clock_disable(hsi);
+
+	err = ste_hsi_hw_init(hsi);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to init HSI controller!\n");
+		goto err_clk_free;
+	}
+
+	for (i = 0; i < STE_HSI_MAX_CHANNELS; i++) {
+		sprintf(overrun_name, "hsi_rx_overrun_ch%d", i);
+		err = ste_hsi_get_irq(pdev, overrun_name, ste_hsi_overrun_isr,
+				      hsi, &ste_hsi->overrun_irq[i]);
+		if (err)
+			goto err_clk_free;
+	}
+
+	err = ste_hsi_ports_init(hsi, pdev);
+	if (err)
+		goto err_clk_free;
+
+	err = hsi_register_controller(hsi);
+
+	if (ste_hsi->regulator)
+		regulator_disable(ste_hsi->regulator);
+
+	if (err)
+		goto err_clk_free;
+
+	return 0;
+
+err_clk_free:
+	ste_hsi_clks_free(ste_hsi);
+err_free_mem:
+	kfree(ste_hsi);
+	return err;
+}
+
+static int ste_hsi_remove_controller(struct hsi_controller *hsi,
+				     struct platform_device *pdev)
+{
+	struct ste_hsi_controller *ste_hsi = hsi_controller_drvdata(hsi);
+	struct hsi_port *port = to_hsi_port(&pdev->dev);
+	struct ste_hsi_port *ste_port = hsi_port_drvdata(port);
+
+	if (ste_hsi->regulator)
+		regulator_put(ste_hsi->regulator);
+
+	gpio_free(ste_port->acwake_gpio);
+
+	ste_hsi_clks_free(ste_hsi);
+	hsi_unregister_controller(hsi);
+
+	kfree(ste_hsi->context);
+	kfree(ste_hsi);
+
+	return 0;
+}
+
+static int __init ste_hsi_probe(struct platform_device *pdev)
+{
+	struct hsi_controller *hsi;
+	struct ste_hsi_platform_data *pdata = pdev->dev.platform_data;
+	int err;
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "No HSI platform data!\n");
+		return -EINVAL;
+	}
+
+	hsi = hsi_alloc_controller(pdata->num_ports, GFP_KERNEL);
+	if (!hsi) {
+		dev_err(&pdev->dev, "No memory to allocate HSI controller!\n");
+		return -ENOMEM;
+	}
+	platform_set_drvdata(pdev, hsi);
+
+	err = ste_hsi_add_controller(hsi, pdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Can't add HSI controller!\n");
+		goto err_free_controller;
+	}
+
+	if (pdata->use_dma)
+		ste_hsi_init_dma(pdata, hsi);
+
+	return 0;
+
+err_free_controller:
+	platform_set_drvdata(pdev, NULL);
+	hsi_free_controller(hsi);
+
+	return err;
+}
+
+static int __exit ste_hsi_remove(struct platform_device *pdev)
+{
+	struct hsi_controller *hsi = platform_get_drvdata(pdev);
+
+	ste_hsi_remove_controller(hsi, pdev);
+	hsi_free_controller(hsi);
+
+	return 0;
+}
+
+static struct platform_driver ste_hsi_driver __refdata = {
+	.driver = {
+		   .name = "ste_hsi",
+		   .owner = THIS_MODULE,
+		   },
+	.remove = __exit_p(ste_hsi_remove),
+};
+
+static int __init ste_hsi_init(void)
+{
+	return platform_driver_probe(&ste_hsi_driver, ste_hsi_probe);
+}
+module_init(ste_hsi_init)
+
+static void __exit ste_hsi_exit(void)
+{
+	platform_driver_unregister(&ste_hsi_driver);
+}
+module_exit(ste_hsi_exit)
+
+MODULE_AUTHOR("Lukasz Baj <lukasz.baj@tieto.com");
+MODULE_DESCRIPTION("STE HSI driver.");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 8deedc1b984..0c71c0721c8 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -39,6 +39,44 @@ config HWMON_DEBUG_CHIP
 
 comment "Native drivers"
 
+config SENSORS_AB8500
+	tristate "AB8500 thermal monitoring"
+	depends on AB8500_GPADC
+	default n
+	help
+	  If you say yes here you get support for the thermal sensor part
+	  of the AB8500 chip. The driver includes thermal management for
+	  AB8500 die and two GPADC channels. The GPADC channel are preferably
+	  used to access sensors outside the AB8500 chip.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called abx500-temp.
+
+config SENSORS_AB5500
+	tristate "AB5500 thermal monitoring"
+	depends on AB5500_GPADC
+	default n
+	help
+	  If you say yes here you get support for the thermal sensor part
+	  of the AB5500 chip. The driver includes thermal management for
+	  AB5500 die, pcb and RF XTAL temperature.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called abx500-temp.
+
+config SENSORS_DBX500
+	tristate "DBX500 thermal monitoring"
+	depends on MFD_DB8500_PRCMU || MFD_DB5500_PRCMU
+	default n
+	help
+	  If you say yes here you get support for the thermal sensor part
+	  of the DBX500 chip. The driver includes thermal management for
+	  DBX500 die.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called dbx500_temp.
+
+
 config SENSORS_ABITUGURU
 	tristate "Abit uGuru (rev 1 & 2)"
 	depends on X86 && DMI && EXPERIMENTAL
@@ -687,6 +725,54 @@ config SENSORS_LTC4151
 	  This driver can also be built as a module. If so, the module will
 	  be called ltc4151.
 
+config SENSORS_LSM303DLH
+	tristate "ST LSM303DLH 3-axis accelerometer and 3-axis magnetometer"
+	depends on I2C
+	default n
+	help
+	  This driver provides support for the LSM303DLH chip which includes a
+	  3-axis accelerometer and a 3-axis magnetometer.
+
+	  This driver can also be built as modules.  If so, the module for
+	  accelerometer will be called lsm303dlh_a and for magnetometer it will
+	  be called lsm303dlh_m.
+
+	  Say Y here if you have a device containing lsm303dlh chip.
+
+config SENSORS_LSM303DLH_INPUT_DEVICE
+	bool "ST LSM303DLH INPUT DEVICE"
+	depends on SENSORS_LSM303DLH
+	default n
+	help
+	  This driver allows device to be used as an input device with
+	  interrupts, need to be enabled only when input device support
+	  is required.
+
+config SENSORS_LSM303DLHC
+	tristate "ST LSM303DLHC 3-axis accelerometer and 3-axis magnetometer"
+	depends on I2C
+	default n
+	help
+	  This driver provides support for the LSM303DLHC chip which includes a
+	  3-axis accelerometer and a 3-axis magnetometer.
+
+	  This driver can also be built as modules.  If so, the module for
+	  accelerometer will be called lsm303dlhc_a and for magnetometer it will
+	  be called lsm303dlh_m.
+
+	  Say Y here if you have a device containing lsm303dlhc chip.
+
+config SENSORS_L3G4200D
+	tristate "ST L3G4200D 3-axis gyroscope"
+	depends on I2C
+	default n
+	help
+	  If you say yes here you get support for 3-axis gyroscope device
+	  L3g4200D.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called l3g4200d.
+
 config SENSORS_LTC4215
 	tristate "Linear Technology LTC4215"
 	depends on I2C && EXPERIMENTAL
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 6d3f11f7181..06eb80a52b7 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -19,6 +19,9 @@ obj-$(CONFIG_SENSORS_W83795)	+= w83795.o
 obj-$(CONFIG_SENSORS_W83781D)	+= w83781d.o
 obj-$(CONFIG_SENSORS_W83791D)	+= w83791d.o
 
+obj-$(CONFIG_SENSORS_AB8500)	+= abx500.o ab8500.o
+obj-$(CONFIG_SENSORS_AB5500)	+= abx500.o ab5500.o
+obj-$(CONFIG_SENSORS_DBX500)	+= dbx500.o
 obj-$(CONFIG_SENSORS_ABITUGURU)	+= abituguru.o
 obj-$(CONFIG_SENSORS_ABITUGURU3)+= abituguru3.o
 obj-$(CONFIG_SENSORS_AD7314)	+= ad7314.o
@@ -84,6 +87,9 @@ obj-$(CONFIG_SENSORS_LM93)	+= lm93.o
 obj-$(CONFIG_SENSORS_LM95241)	+= lm95241.o
 obj-$(CONFIG_SENSORS_LM95245)	+= lm95245.o
 obj-$(CONFIG_SENSORS_LTC4151)	+= ltc4151.o
+obj-$(CONFIG_SENSORS_LSM303DLH) += lsm303dlh_a.o lsm303dlh_m.o
+obj-$(CONFIG_SENSORS_LSM303DLHC)+= lsm303dlhc_a.o
+obj-$(CONFIG_SENSORS_L3G4200D) += l3g4200d.o
 obj-$(CONFIG_SENSORS_LTC4215)	+= ltc4215.o
 obj-$(CONFIG_SENSORS_LTC4245)	+= ltc4245.o
 obj-$(CONFIG_SENSORS_LTC4261)	+= ltc4261.o
diff --git a/drivers/hwmon/ab5500.c b/drivers/hwmon/ab5500.c
new file mode 100644
index 00000000000..cafadeba51c
--- /dev/null
+++ b/drivers/hwmon/ab5500.c
@@ -0,0 +1,212 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Martin Persson <martin.persson@stericsson.com> for
+ * ST-Ericsson.
+ * License terms: GNU Gereral Public License (GPL) version 2
+ *
+ * Note:
+ *
+ * If/when the AB5500 thermal warning temperature is reached (threshold
+ * 125C cannot be changed by SW), an interrupt is set and the driver
+ * notifies user space via a sysfs event. If a shut down is not
+ * triggered by user space and temperature reaches beyond critical
+ * limit(130C) pm_power off is called.
+ *
+ * If/when AB5500 thermal shutdown temperature is reached a hardware
+ * shutdown of the AB5500 will occur.
+ */
+
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/hwmon.h>
+#include <linux/sysfs.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+#include <linux/mfd/abx500/ab5500-bm.h>
+#include "abx500.h"
+#include <asm/mach-types.h>
+
+/* AB5500 driver monitors GPADC - XTAL_TEMP, PCB_TEMP,
+ * BTEMP_BALL, BAT_CTRL and DIE_TEMP
+ */
+#define NUM_MONITORED_SENSORS    5
+
+#define SHUTDOWN_AUTO_MIN_LIMIT                 -25
+#define SHUTDOWN_AUTO_MAX_LIMIT                 130
+
+static int ab5500_output_convert(int val, u8 sensor)
+{
+	int res = val;
+	/* GPADC returns die temperature in Celsius
+	 * convert it to millidegree celsius
+	 */
+	if (sensor == DIE_TEMP)
+		res = val * 1000;
+
+	return res;
+}
+
+static int ab5500_read_sensor(struct abx500_temp *data, u8 sensor)
+{
+	int val;
+	/*
+	 * Special treatment for BAT_CTRL node, since this
+	 * temperature measurement is more complex than just
+	 * an ADC readout
+	 */
+	if (sensor == BAT_CTRL)
+		val = ab5500_btemp_get_batctrl_temp(data->ab5500_btemp);
+	else
+		val = ab5500_gpadc_convert(data->ab5500_gpadc, sensor);
+
+	if (val < 0)
+		return val;
+	else
+		return ab5500_output_convert(val, sensor);
+}
+
+static ssize_t ab5500_show_name(struct device *dev,
+				struct device_attribute *devattr,
+				char *buf)
+{
+	return sprintf(buf, "ab5500\n");
+}
+
+static ssize_t ab5500_show_label(struct device *dev,
+				struct device_attribute *devattr,
+				char *buf)
+{
+	char *name;
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int index = attr->index;
+
+	/*
+	 * Make sure these labels correspond to the attribute indexes
+	 * used when calling SENSOR_DEVICE_ATRR.
+	 * Temperature sensors outside ab8500 (read via GPADC) are marked
+	 * with prefix ext_
+	 */
+	switch (index) {
+	case 1:
+		name = "xtal_temp";
+		break;
+	case 2:
+		name = "pcb_temp";
+		break;
+	case 3:
+		name = "bat_temp";
+		break;
+	case 4:
+		name = "bat_ctrl";
+		break;
+	case 5:
+		name = "ab5500";
+		break;
+	default:
+		return -EINVAL;
+	}
+	return sprintf(buf, "%s\n", name);
+}
+
+static int temp_shutdown_trig(int mux)
+{
+	pm_power_off();
+	return 0;
+}
+
+static int ab5500_temp_shutdown_auto(struct abx500_temp *data)
+{
+	int ret;
+	struct adc_auto_input *auto_ip;
+
+	auto_ip = kzalloc(sizeof(struct adc_auto_input), GFP_KERNEL);
+	if (!auto_ip) {
+		dev_err(&data->pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	auto_ip->mux = DIE_TEMP;
+	auto_ip->freq = MS500;
+	/*
+	 * As per product specification, voltage decreases as
+	 * temperature increases. Hence the min and max values
+	 * should be passed in reverse order.
+	 */
+	auto_ip->min = SHUTDOWN_AUTO_MAX_LIMIT;
+	auto_ip->max = SHUTDOWN_AUTO_MIN_LIMIT;
+	auto_ip->auto_adc_callback = temp_shutdown_trig;
+	data->gpadc_auto = auto_ip;
+	ret = ab5500_gpadc_convert_auto(data->ab5500_gpadc,
+					data->gpadc_auto);
+	if (ret < 0)
+		kfree(auto_ip);
+
+	return ret;
+}
+
+static int ab5500_is_visible(struct attribute *attr, int n)
+{
+	return attr->mode;
+}
+
+static int ab5500_temp_irq_handler(int irq, struct abx500_temp *data)
+{
+	/*
+	 * Make sure the magic numbers below corresponds to the node
+	 * used for AB5500 thermal warning from HW.
+	 */
+	mutex_lock(&data->lock);
+	data->crit_alarm[4] = 1;
+	mutex_unlock(&data->lock);
+	sysfs_notify(&data->pdev->dev.kobj, NULL, "temp5_crit_alarm");
+	dev_info(&data->pdev->dev, "ABX500 thermal warning,"
+				" power off system now!\n");
+	return 0;
+}
+
+int __init ab5500_hwmon_init(struct abx500_temp *data)
+{
+	int err;
+
+	data->ab5500_gpadc = ab5500_gpadc_get("ab5500-adc.0");
+	if (IS_ERR(data->ab5500_gpadc))
+		return PTR_ERR(data->ab5500_gpadc);
+
+	data->ab5500_btemp = ab5500_btemp_get();
+	if (IS_ERR(data->ab5500_btemp))
+		return PTR_ERR(data->ab5500_btemp);
+
+	err = ab5500_temp_shutdown_auto(data);
+	if (err < 0) {
+		dev_err(&data->pdev->dev, "Failed to register"
+				" auto trigger(%d)\n", err);
+		return err;
+	}
+
+	/*
+	 * Setup HW defined data.
+	 *
+	 * Reference hardware (HREF):
+	 *
+	 * XTAL_TEMP, PCB_TEMP, BTEMP_BALL refer to millivolts and
+	 * BAT_CTRL and DIE_TEMP refer to millidegrees
+	 *
+	 * Make sure indexes correspond to the attribute indexes
+	 * used when calling SENSOR_DEVICE_ATRR
+	 */
+	data->gpadc_addr[0] = XTAL_TEMP;
+	data->gpadc_addr[1] = PCB_TEMP;
+	data->gpadc_addr[2] = BTEMP_BALL;
+	data->gpadc_addr[3] = BAT_CTRL;
+	data->gpadc_addr[4] = DIE_TEMP;
+	data->monitored_sensors = NUM_MONITORED_SENSORS;
+
+	data->ops.read_sensor = ab5500_read_sensor;
+	data->ops.irq_handler = ab5500_temp_irq_handler;
+	data->ops.show_name  = ab5500_show_name;
+	data->ops.show_label = ab5500_show_label;
+	data->ops.is_visible = ab5500_is_visible;
+
+	return 0;
+}
diff --git a/drivers/hwmon/ab8500.c b/drivers/hwmon/ab8500.c
new file mode 100644
index 00000000000..65a0f381d01
--- /dev/null
+++ b/drivers/hwmon/ab8500.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Martin Persson <martin.persson@stericsson.com> for
+ * ST-Ericsson.
+ * License terms: GNU Gereral Public License (GPL) version 2
+ *
+ * Note:
+ *
+ * If/when the AB8500 thermal warning temperature is reached (threshold
+ * cannot be changed by SW), an interrupt is set and the driver
+ * notifies user space via a sysfs event. If a shut down is not
+ * triggered by user space within a certain time frame,
+ * pm_power off is called.
+ *
+ * If/when AB8500 thermal shutdown temperature is reached a hardware
+ * shutdown of the AB8500 will occur.
+ */
+
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/hwmon.h>
+#include <linux/sysfs.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/abx500/ab8500-gpadc.h>
+#include <linux/mfd/abx500/ab8500-bm.h>
+#include "abx500.h"
+#include <asm/mach-types.h>
+
+#define DEFAULT_POWER_OFF_DELAY 10000
+
+/*
+ * The driver monitors GPADC - ADC_AUX1, ADC_AUX2, BTEMP_BALL
+ * and BAT_CTRL.
+ */
+#define NUM_MONITORED_SENSORS 4
+
+static int ab8500_read_sensor(struct abx500_temp *data, u8 sensor)
+{
+	int val;
+	/*
+	 * Special treatment for the BAT_CTRL node, since this
+	 * temperature measurement is more complex than just
+	 * an ADC readout
+	 */
+	if (sensor == BAT_CTRL)
+		val = ab8500_btemp_get_batctrl_temp(data->ab8500_btemp);
+	else
+		val = ab8500_gpadc_convert(data->ab8500_gpadc, sensor);
+
+	return val;
+}
+
+static void ab8500_thermal_power_off(struct work_struct *work)
+{
+	struct abx500_temp *data = container_of(work, struct abx500_temp,
+						power_off_work.work);
+
+	dev_warn(&data->pdev->dev, "Power off due to AB8500 thermal warning\n");
+	pm_power_off();
+}
+
+static ssize_t ab8500_show_name(struct device *dev,
+				struct device_attribute *devattr,
+				char *buf)
+{
+	return sprintf(buf, "ab8500\n");
+}
+
+static ssize_t ab8500_show_label(struct device *dev,
+				struct device_attribute *devattr,
+				char *buf)
+{
+	char *name;
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int index = attr->index;
+
+	/*
+	 * Make sure these labels correspond to the attribute indexes
+	 * used when calling SENSOR_DEVICE_ATRR.
+	 * Temperature sensors outside ab8500 (read via GPADC) are marked
+	 * with prefix ext_
+	 */
+	switch (index) {
+	case 1:
+		name = "ext_rtc_xtal";
+		break;
+	case 2:
+		name = "ext_db8500";
+		break;
+	case 3:
+		name = "bat_temp";
+		break;
+	case 4:
+		name = "bat_ctrl";
+		break;
+	case 5:
+		name = "ab8500";
+		break;
+	default:
+		return -EINVAL;
+	}
+	return sprintf(buf, "%s\n", name);
+}
+
+static int ab8500_is_visible(struct attribute *attr, int n)
+{
+	if (!strcmp(attr->name, "temp5_input") ||
+	    !strcmp(attr->name, "temp5_min") ||
+	    !strcmp(attr->name, "temp5_max") ||
+	    !strcmp(attr->name, "temp5_max_hyst") ||
+	    !strcmp(attr->name, "temp5_min_alarm") ||
+	    !strcmp(attr->name, "temp5_max_alarm") ||
+	    !strcmp(attr->name, "temp5_max_hyst_alarm"))
+		return 0;
+
+	return attr->mode;
+}
+
+static int ab8500_temp_irq_handler(int irq, struct abx500_temp *data)
+{
+	unsigned long delay_in_jiffies;
+	/*
+	 * Make sure the magic numbers below corresponds to the node
+	 * used for AB8500 thermal warning from HW.
+	 */
+	mutex_lock(&data->lock);
+	data->crit_alarm[4] = 1;
+	mutex_unlock(&data->lock);
+
+	hwmon_notify(data->crit_alarm[4], NULL);
+	sysfs_notify(&data->pdev->dev.kobj, NULL, "temp5_crit_alarm");
+	dev_info(&data->pdev->dev, "AB8500 thermal warning,"
+		" power off in %lu s\n", data->power_off_delay);
+	delay_in_jiffies = msecs_to_jiffies(data->power_off_delay);
+	schedule_delayed_work(&data->power_off_work, delay_in_jiffies);
+	return 0;
+}
+
+int __init ab8500_hwmon_init(struct abx500_temp *data)
+{
+	data->ab8500_gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	if (IS_ERR(data->ab8500_gpadc))
+		return PTR_ERR(data->ab8500_gpadc);
+
+	data->ab8500_btemp = ab8500_btemp_get();
+	if (IS_ERR(data->ab8500_btemp))
+		return PTR_ERR(data->ab8500_btemp);
+
+	INIT_DELAYED_WORK(&data->power_off_work, ab8500_thermal_power_off);
+
+	/*
+	 * Setup HW defined data.
+	 *
+	 * Reference hardware (HREF):
+	 *
+	 * GPADC - ADC_AUX1, connected to NTC R2148 next to RTC_XTAL on HREF
+	 * GPADC - ADC_AUX2, connected to NTC R2150 near DB8500 on HREF
+	 * Hence, temp#_min/max/max_hyst refer to millivolts and not
+	 * millidegrees
+	 * This is not the case for BAT_CTRL where millidegrees is used
+	 *
+	 * HREF HW does not support reading AB8500 temperature. BUT an
+	 * AB8500 IRQ will be launched if die crit temp limit is reached.
+	 *
+	 * Make sure indexes correspond to the attribute indexes
+	 * used when calling SENSOR_DEVICE_ATRR
+	 */
+	data->gpadc_addr[0] = ADC_AUX1;
+	data->gpadc_addr[1] = ADC_AUX2;
+	data->gpadc_addr[2] = BTEMP_BALL;
+	data->gpadc_addr[3] = BAT_CTRL;
+	data->gpadc_addr[4] = DIE_TEMP;
+	data->power_off_delay = DEFAULT_POWER_OFF_DELAY;
+	data->monitored_sensors = NUM_MONITORED_SENSORS;
+
+	data->ops.read_sensor = ab8500_read_sensor;
+	data->ops.irq_handler = ab8500_temp_irq_handler;
+	data->ops.show_name  = ab8500_show_name;
+	data->ops.show_label = ab8500_show_label;
+	data->ops.is_visible = ab8500_is_visible;
+
+	return 0;
+}
diff --git a/drivers/hwmon/abx500.c b/drivers/hwmon/abx500.c
new file mode 100644
index 00000000000..7aa9994c54a
--- /dev/null
+++ b/drivers/hwmon/abx500.c
@@ -0,0 +1,698 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Martin Persson <martin.persson@stericsson.com> for
+ * ST-Ericsson.
+ * License terms: GNU Gereral Public License (GPL) version 2
+ *
+ * Note:
+ *
+ * ABX500 does not provide auto ADC, so to monitor the required
+ * temperatures, a periodic work is used. It is more important
+ * to not wake up the CPU than to perform this job, hence the use
+ * of a deferred delay.
+ *
+ * A deferred delay for thermal monitor is considered safe because:
+ * If the chip gets too hot during a sleep state it's most likely
+ * due to external factors, such as the surrounding temperature.
+ * I.e. no SW decisions will make any difference.
+ *
+ * If/when the ABX500 thermal warning temperature is reached (threshold
+ * cannot be changed by SW), an interrupt is set and the driver
+ * notifies user space via a sysfs event.
+ *
+ * If/when ABX500 thermal shutdown temperature is reached a hardware
+ * shutdown of the ABX500 will occur.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/hwmon.h>
+#include <linux/sysfs.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/jiffies.h>
+#include <linux/mutex.h>
+#include <linux/pm.h>
+#include <asm/mach-types.h>
+
+#include "abx500.h"
+
+#define DEFAULT_MONITOR_DELAY 1000
+
+/*
+ * Thresholds are considered inactive if set to 0.
+ * To avoid confusion for user space applications,
+ * the temp monitor delay is set to 0 if all thresholds
+ * are 0.
+ */
+static bool find_active_thresholds(struct abx500_temp *data)
+{
+	int i;
+	for (i = 0; i < data->monitored_sensors; i++)
+		if (data->max[i] != 0 || data->max_hyst[i] != 0
+		    || data->min[i] != 0)
+			return true;
+
+	dev_dbg(&data->pdev->dev, "No active thresholds,"
+		"cancel deferred job (if it exists)"
+		"and reset temp monitor delay\n");
+	cancel_delayed_work_sync(&data->work);
+	return false;
+}
+
+static inline void schedule_monitor(struct abx500_temp *data)
+{
+	unsigned long delay_in_jiffies;
+	delay_in_jiffies = msecs_to_jiffies(data->gpadc_monitor_delay);
+	schedule_delayed_work(&data->work, delay_in_jiffies);
+}
+
+static inline void gpadc_monitor_exit(struct abx500_temp *data)
+{
+	cancel_delayed_work_sync(&data->work);
+}
+
+static void gpadc_monitor(struct work_struct *work)
+{
+	unsigned long delay_in_jiffies;
+	int val, i, ret;
+	/* Container for alarm node name */
+	char alarm_node[30];
+
+	bool updated_min_alarm = false;
+	bool updated_max_alarm = false;
+	bool updated_max_hyst_alarm = false;
+	struct abx500_temp *data = container_of(work, struct abx500_temp,
+						work.work);
+
+	for (i = 0; i < data->monitored_sensors; i++) {
+		/* Thresholds are considered inactive if set to 0 */
+		if (data->max[i] == 0 && data->max_hyst[i] == 0
+		    && data->min[i] == 0)
+			continue;
+
+		val = data->ops.read_sensor(data, data->gpadc_addr[i]);
+		if (val < 0) {
+			dev_err(&data->pdev->dev, "GPADC read failed\n");
+				continue;
+		}
+
+		mutex_lock(&data->lock);
+		if (data->min[i] != 0) {
+			if (val < data->min[i]) {
+				if (data->min_alarm[i] == 0) {
+					data->min_alarm[i] = 1;
+					updated_min_alarm = true;
+				}
+			} else {
+				if (data->min_alarm[i] == 1) {
+					data->min_alarm[i] = 0;
+					updated_min_alarm = true;
+				}
+			}
+
+		}
+		if (data->max[i] != 0) {
+			if (val > data->max[i]) {
+				if (data->max_alarm[i] == 0) {
+					data->max_alarm[i] = 1;
+					updated_max_alarm = true;
+				}
+			} else {
+				if (data->max_alarm[i] == 1) {
+					data->max_alarm[i] = 0;
+					updated_max_alarm = true;
+				}
+			}
+
+		}
+		if (data->max_hyst[i] != 0) {
+			if (val > data->max_hyst[i]) {
+				if (data->max_hyst_alarm[i] == 0) {
+					data->max_hyst_alarm[i] = 1;
+					updated_max_hyst_alarm = true;
+				}
+			} else {
+				if (data->max_hyst_alarm[i] == 1) {
+					data->max_hyst_alarm[i] = 0;
+					updated_max_hyst_alarm = true;
+				}
+			}
+		}
+		mutex_unlock(&data->lock);
+
+		/* hwmon attr index starts at 1, thus "i+1" below */
+		if (updated_min_alarm) {
+			ret = snprintf(alarm_node, 16, "temp%d_min_alarm",
+				       (i + 1));
+			if (ret < 0) {
+				dev_err(&data->pdev->dev,
+					"Unable to update alarm node (%d)",
+					ret);
+				break;
+			}
+			sysfs_notify(&data->pdev->dev.kobj, NULL, alarm_node);
+		}
+		if (updated_max_alarm) {
+			ret = snprintf(alarm_node, 16, "temp%d_max_alarm",
+				       (i + 1));
+			if (ret < 0) {
+				dev_err(&data->pdev->dev,
+					"Unable to update alarm node (%d)",
+					ret);
+				break;
+			}
+			hwmon_notify(data->max_alarm[i], NULL);
+			sysfs_notify(&data->pdev->dev.kobj, NULL, alarm_node);
+		}
+		if (updated_max_hyst_alarm) {
+			ret = snprintf(alarm_node, 21, "temp%d_max_hyst_alarm",
+				       (i + 1));
+			if (ret < 0) {
+				dev_err(&data->pdev->dev,
+					"Unable to update alarm node (%d)",
+					ret);
+				break;
+			}
+			sysfs_notify(&data->pdev->dev.kobj, NULL, alarm_node);
+		}
+	}
+	delay_in_jiffies = msecs_to_jiffies(data->gpadc_monitor_delay);
+	schedule_delayed_work(&data->work, delay_in_jiffies);
+}
+
+static ssize_t set_temp_monitor_delay(struct device *dev,
+				      struct device_attribute *devattr,
+				      const char *buf, size_t count)
+{
+	int res;
+	unsigned long delay_in_s;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+
+	res = strict_strtoul(buf, 10, &delay_in_s);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	data->gpadc_monitor_delay = delay_in_s * 1000;
+
+	if (find_active_thresholds(data))
+		schedule_monitor(data);
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_temp_power_off_delay(struct device *dev,
+					struct device_attribute *devattr,
+					const char *buf, size_t count)
+{
+	int res;
+	unsigned long delay_in_s;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+
+	res = strict_strtoul(buf, 10, &delay_in_s);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	data->power_off_delay = delay_in_s * 1000;
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t show_temp_monitor_delay(struct device *dev,
+				       struct device_attribute *devattr,
+				       char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	/* return time in s, not ms */
+	return sprintf(buf, "%lu\n", (data->gpadc_monitor_delay) / 1000);
+}
+
+static ssize_t show_temp_power_off_delay(struct device *dev,
+					 struct device_attribute *devattr,
+					 char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	/* return time in s, not ms */
+	return sprintf(buf, "%lu\n", (data->power_off_delay) / 1000);
+}
+
+/* HWMON sysfs interface */
+static ssize_t show_name(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	/*
+	 * To avoid confusion between sensor label and chip name, the function
+	 * "show_label" is not used to return the chip name.
+	 */
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	return data->ops.show_name(dev, devattr, buf);
+}
+
+static ssize_t show_label(struct device *dev,
+			  struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	return data->ops.show_label(dev, devattr, buf);
+}
+
+static ssize_t show_input(struct device *dev,
+			  struct device_attribute *devattr, char *buf)
+{
+	int val;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	u8 gpadc_addr = data->gpadc_addr[attr->index - 1];
+
+	val = data->ops.read_sensor(data, gpadc_addr);
+	if (val < 0)
+		dev_err(&data->pdev->dev, "GPADC read failed\n");
+
+	return sprintf(buf, "%d\n", val);
+}
+
+/* set functions (RW nodes) */
+static ssize_t set_min(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	/*
+	 * Threshold is considered inactive if set to 0
+	 * hwmon attr index starts at 1, thus "attr->index-1" below
+	 */
+	if (val == 0)
+		data->min_alarm[attr->index - 1] = 0;
+
+	data->min[attr->index - 1] = val;
+
+	if (val == 0)
+		(void) find_active_thresholds(data);
+	else
+		schedule_monitor(data);
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_max(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	/*
+	 * Threshold is considered inactive if set to 0
+	 * hwmon attr index starts at 1, thus "attr->index-1" below
+	 */
+	if (val == 0)
+		data->max_alarm[attr->index - 1] = 0;
+
+	data->max[attr->index - 1] = val;
+
+	if (val == 0)
+		(void) find_active_thresholds(data);
+	else
+		schedule_monitor(data);
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_max_hyst(struct device *dev,
+			    struct device_attribute *devattr,
+			    const char *buf, size_t count)
+{
+	unsigned long val;
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	/*
+	 * Threshold is considered inactive if set to 0
+	 * hwmon attr index starts at 1, thus "attr->index-1" below
+	 */
+	if (val == 0)
+		data->max_hyst_alarm[attr->index - 1] = 0;
+
+	data->max_hyst[attr->index - 1] = val;
+
+	if (val == 0)
+		(void) find_active_thresholds(data);
+	else
+		schedule_monitor(data);
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+/*
+ * show functions (RO nodes)
+ */
+static ssize_t show_min(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->min[attr->index - 1]);
+}
+
+static ssize_t show_max(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->max[attr->index - 1]);
+}
+
+static ssize_t show_max_hyst(struct device *dev,
+			     struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->max_hyst[attr->index - 1]);
+}
+
+/* Alarms */
+static ssize_t show_min_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->min_alarm[attr->index - 1]);
+}
+
+static ssize_t show_max_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->max_alarm[attr->index - 1]);
+}
+
+static ssize_t show_max_hyst_alarm(struct device *dev,
+				   struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->max_hyst_alarm[attr->index - 1]);
+}
+
+static ssize_t show_crit_alarm(struct device *dev,
+			       struct device_attribute *devattr, char *buf)
+{
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%ld\n", data->crit_alarm[attr->index - 1]);
+}
+
+static mode_t abx500_attrs_visible(struct kobject *kobj,
+				   struct attribute *a, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct abx500_temp *data = dev_get_drvdata(dev);
+	return data->ops.is_visible(a, n);
+}
+
+static SENSOR_DEVICE_ATTR(temp_monitor_delay, S_IRUGO | S_IWUSR,
+			  show_temp_monitor_delay, set_temp_monitor_delay, 0);
+static SENSOR_DEVICE_ATTR(temp_power_off_delay, S_IRUGO | S_IWUSR,
+			  show_temp_power_off_delay,
+			  set_temp_power_off_delay, 0);
+
+/* Chip name, required by hwmon*/
+static SENSOR_DEVICE_ATTR(name, S_IRUGO, show_name, NULL, 0);
+
+/* GPADC - SENSOR1 */
+static SENSOR_DEVICE_ATTR(temp1_label, S_IRUGO, show_label, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_input, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_min, set_min, 1);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_max, set_max, 1);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 1);
+static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_min_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_max_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_max_hyst_alarm, S_IRUGO,
+			  show_max_hyst_alarm, NULL, 1);
+
+/* GPADC - SENSOR2 */
+static SENSOR_DEVICE_ATTR(temp2_label, S_IRUGO, show_label, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_input, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_min, set_min, 2);
+static SENSOR_DEVICE_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_max, set_max, 2);
+static SENSOR_DEVICE_ATTR(temp2_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 2);
+static SENSOR_DEVICE_ATTR(temp2_min_alarm, S_IRUGO, show_min_alarm, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_max_alarm, S_IRUGO, show_max_alarm, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp2_max_hyst_alarm, S_IRUGO,
+			  show_max_hyst_alarm, NULL, 2);
+
+/* GPADC - SENSOR3 */
+static SENSOR_DEVICE_ATTR(temp3_label, S_IRUGO, show_label, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_input, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp3_min, S_IWUSR | S_IRUGO, show_min, set_min, 3);
+static SENSOR_DEVICE_ATTR(temp3_max, S_IWUSR | S_IRUGO, show_max, set_max, 3);
+static SENSOR_DEVICE_ATTR(temp3_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 3);
+static SENSOR_DEVICE_ATTR(temp3_min_alarm, S_IRUGO, show_min_alarm, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp3_max_alarm, S_IRUGO, show_max_alarm, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp3_max_hyst_alarm, S_IRUGO,
+			  show_max_hyst_alarm, NULL, 3);
+
+/* GPADC - SENSOR4 */
+static SENSOR_DEVICE_ATTR(temp4_label, S_IRUGO, show_label, NULL, 4);
+static SENSOR_DEVICE_ATTR(temp4_input, S_IRUGO, show_input, NULL, 4);
+static SENSOR_DEVICE_ATTR(temp4_min, S_IWUSR | S_IRUGO, show_min, set_min, 4);
+static SENSOR_DEVICE_ATTR(temp4_max, S_IWUSR | S_IRUGO, show_max, set_max, 4);
+static SENSOR_DEVICE_ATTR(temp4_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 4);
+static SENSOR_DEVICE_ATTR(temp4_min_alarm, S_IRUGO, show_min_alarm, NULL, 4);
+static SENSOR_DEVICE_ATTR(temp4_max_alarm, S_IRUGO, show_max_alarm, NULL, 4);
+static SENSOR_DEVICE_ATTR(temp4_max_hyst_alarm, S_IRUGO,
+			  show_max_hyst_alarm, NULL, 4);
+
+/* GPADC - SENSOR5 */
+static SENSOR_DEVICE_ATTR(temp5_label, S_IRUGO, show_label, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp5_input, S_IRUGO, show_input, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp5_min, S_IWUSR | S_IRUGO, show_min, set_min, 5);
+static SENSOR_DEVICE_ATTR(temp5_max, S_IWUSR | S_IRUGO, show_max, set_max, 5);
+static SENSOR_DEVICE_ATTR(temp5_max_hyst, S_IWUSR | S_IRUGO,
+			  show_max_hyst, set_max_hyst, 5);
+static SENSOR_DEVICE_ATTR(temp5_min_alarm, S_IRUGO, show_min_alarm, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp5_max_alarm, S_IRUGO, show_max_alarm, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp5_max_hyst_alarm, S_IRUGO,
+			  show_max_hyst_alarm, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp5_crit_alarm, S_IRUGO,
+			  show_crit_alarm, NULL, 5);
+
+struct attribute *abx500_temp_attributes[] = {
+	&sensor_dev_attr_name.dev_attr.attr,
+	&sensor_dev_attr_temp_monitor_delay.dev_attr.attr,
+	&sensor_dev_attr_temp_power_off_delay.dev_attr.attr,
+	/* GPADC SENSOR1 */
+	&sensor_dev_attr_temp1_label.dev_attr.attr,
+	&sensor_dev_attr_temp1_input.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_hyst_alarm.dev_attr.attr,
+	/* GPADC SENSOR2 */
+	&sensor_dev_attr_temp2_label.dev_attr.attr,
+	&sensor_dev_attr_temp2_input.dev_attr.attr,
+	&sensor_dev_attr_temp2_min.dev_attr.attr,
+	&sensor_dev_attr_temp2_max.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp2_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp2_max_hyst_alarm.dev_attr.attr,
+	/* GPADC SENSOR3 */
+	&sensor_dev_attr_temp3_label.dev_attr.attr,
+	&sensor_dev_attr_temp3_input.dev_attr.attr,
+	&sensor_dev_attr_temp3_min.dev_attr.attr,
+	&sensor_dev_attr_temp3_max.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp3_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp3_max_hyst_alarm.dev_attr.attr,
+	/* GPADC SENSOR4 */
+	&sensor_dev_attr_temp4_label.dev_attr.attr,
+	&sensor_dev_attr_temp4_input.dev_attr.attr,
+	&sensor_dev_attr_temp4_min.dev_attr.attr,
+	&sensor_dev_attr_temp4_max.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp4_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp4_max_hyst_alarm.dev_attr.attr,
+	/* GPADC SENSOR5*/
+	&sensor_dev_attr_temp5_label.dev_attr.attr,
+	&sensor_dev_attr_temp5_input.dev_attr.attr,
+	&sensor_dev_attr_temp5_min.dev_attr.attr,
+	&sensor_dev_attr_temp5_max.dev_attr.attr,
+	&sensor_dev_attr_temp5_max_hyst.dev_attr.attr,
+	&sensor_dev_attr_temp5_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp5_max_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp5_max_hyst_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp5_crit_alarm.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group abx500_temp_group = {
+	.attrs = abx500_temp_attributes,
+	.is_visible = abx500_attrs_visible,
+};
+
+static irqreturn_t abx500_temp_irq_handler(int irq, void *irq_data)
+{
+	struct platform_device *pdev = irq_data;
+	struct abx500_temp *data = platform_get_drvdata(pdev);
+	data->ops.irq_handler(irq, data);
+	return IRQ_HANDLED;
+}
+
+static int setup_irqs(struct platform_device *pdev)
+{
+	int ret;
+	int irq = platform_get_irq_byname(pdev, "ABX500_TEMP_WARM");
+
+	if (irq < 0)
+		dev_err(&pdev->dev, "Get irq by name failed\n");
+
+	ret = request_threaded_irq(irq, NULL, abx500_temp_irq_handler,
+				   IRQF_NO_SUSPEND, "abx500-temp", pdev);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Request threaded irq failed (%d)\n", ret);
+
+	return ret;
+}
+
+static int __devinit abx500_temp_probe(struct platform_device *pdev)
+{
+	struct abx500_temp *data;
+	int err;
+
+	data = kzalloc(sizeof(struct abx500_temp), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->pdev = pdev;
+	mutex_init(&data->lock);
+
+	/* Chip specific initialization */
+	if (!machine_is_u5500())
+		err = ab8500_hwmon_init(data);
+	else
+		err = ab5500_hwmon_init(data);
+	if (err	< 0) {
+		dev_err(&pdev->dev, "abx500 init failed");
+		goto exit;
+	}
+
+	data->hwmon_dev = hwmon_device_register(&pdev->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		dev_err(&pdev->dev, "Class registration failed (%d)\n", err);
+		goto exit;
+	}
+
+	INIT_DELAYED_WORK_DEFERRABLE(&data->work, gpadc_monitor);
+	data->gpadc_monitor_delay =  DEFAULT_MONITOR_DELAY;
+
+	platform_set_drvdata(pdev, data);
+
+	err = sysfs_create_group(&pdev->dev.kobj, &abx500_temp_group);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Create sysfs group failed (%d)\n", err);
+		goto exit_platform_data;
+	}
+
+	err = setup_irqs(pdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "irq setup failed (%d)\n", err);
+		goto exit_sysfs_group;
+	}
+	return 0;
+
+exit_sysfs_group:
+	sysfs_remove_group(&pdev->dev.kobj, &abx500_temp_group);
+exit_platform_data:
+	hwmon_device_unregister(data->hwmon_dev);
+	platform_set_drvdata(pdev, NULL);
+exit:
+	kfree(data->gpadc_auto);
+	kfree(data);
+	return err;
+}
+
+static int __devexit abx500_temp_remove(struct platform_device *pdev)
+{
+	struct abx500_temp *data = platform_get_drvdata(pdev);
+
+	gpadc_monitor_exit(data);
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&pdev->dev.kobj, &abx500_temp_group);
+	platform_set_drvdata(pdev, NULL);
+	kfree(data->gpadc_auto);
+	kfree(data);
+	return 0;
+}
+
+/* No action required in suspend/resume, thus the lack of functions */
+static struct platform_driver abx500_temp_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "abx500-temp",
+	},
+	.probe = abx500_temp_probe,
+	.remove = __devexit_p(abx500_temp_remove),
+};
+
+static int __init abx500_temp_init(void)
+{
+	return platform_driver_register(&abx500_temp_driver);
+}
+
+static void __exit abx500_temp_exit(void)
+{
+	platform_driver_unregister(&abx500_temp_driver);
+}
+
+MODULE_AUTHOR("Martin Persson <martin.persson@stericsson.com>");
+MODULE_DESCRIPTION("ABX500 temperature driver");
+MODULE_LICENSE("GPL");
+
+module_init(abx500_temp_init)
+module_exit(abx500_temp_exit)
diff --git a/drivers/hwmon/abx500.h b/drivers/hwmon/abx500.h
new file mode 100644
index 00000000000..9fe28dac28f
--- /dev/null
+++ b/drivers/hwmon/abx500.h
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * License terms: GNU General Public License v2
+ * Author: Martin Persson <martin.persson@stericsson.com>
+ */
+
+#ifndef _ABX500_H
+#define _ABX500_H
+
+#define NUM_SENSORS 5
+
+struct ab8500_gpadc;
+struct ab5500_gpadc;
+struct ab8500_btemp;
+struct ab5500_btemp;
+struct adc_auto_input;
+struct abx500_temp;
+
+/**
+ * struct abx500_temp_ops - abx500 chip specific ops
+ * @read_sensor: reads gpadc output
+ * @irq_handler: irq handler
+ * @show_name: hwmon device name
+ * @show_label: hwmon attribute label
+ * @is_visible: is attribute visible
+ */
+struct abx500_temp_ops {
+	int (*read_sensor)(struct abx500_temp *, u8);
+	int (*irq_handler)(int, struct abx500_temp *);
+	ssize_t (*show_name)(struct device *,
+			struct device_attribute *, char *);
+	ssize_t (*show_label) (struct device *,
+			struct device_attribute *, char *);
+	int (*is_visible)(struct attribute *, int);
+};
+
+/**
+ * struct abx500_temp - representation of temp mon device
+ * @pdev: platform device
+ * @hwmon_dev: hwmon device
+ * @ab8500_gpadc: gpadc interface for ab8500
+ * @ab5500_gpadc: gpadc interface for ab5500
+ * @btemp: battery temperature interface for ab8500
+ * @adc_auto_input: gpadc auto trigger
+ * @gpadc_addr: gpadc channel address
+ * @temp: sensor temperature input value
+ * @min: sensor temperature min value
+ * @max: sensor temperature max value
+ * @max_hyst: sensor temperature hysteresis value for max limit
+ * @crit: sensor temperature critical value
+ * @min_alarm: sensor temperature min alarm
+ * @max_alarm: sensor temperature max alarm
+ * @max_hyst_alarm: sensor temperature hysteresis alarm
+ * @crit_alarm: sensor temperature critical value alarm
+ * @work: delayed work scheduled to monitor temperature periodically
+ * @power_off_work: delayed work scheduled to power off the system
+		when critical temperature is reached
+ * @lock: mutex
+ * @gpadc_monitor_delay: delay between temperature readings in ms
+ * @power_off_delay: delay before power off in ms
+ * @monitored_sensors: number of monitored sensors
+ */
+struct abx500_temp {
+	struct platform_device *pdev;
+	struct device *hwmon_dev;
+	struct ab8500_gpadc *ab8500_gpadc;
+	struct ab5500_gpadc *ab5500_gpadc;
+	struct ab8500_btemp *ab8500_btemp;
+	struct ab5500_btemp *ab5500_btemp;	
+	struct adc_auto_input *gpadc_auto;
+	struct abx500_temp_ops ops;
+	u8 gpadc_addr[NUM_SENSORS];
+	unsigned long temp[NUM_SENSORS];
+	unsigned long min[NUM_SENSORS];
+	unsigned long max[NUM_SENSORS];
+	unsigned long max_hyst[NUM_SENSORS];
+	unsigned long crit[NUM_SENSORS];
+	unsigned long min_alarm[NUM_SENSORS];
+	unsigned long max_alarm[NUM_SENSORS];
+	unsigned long max_hyst_alarm[NUM_SENSORS];
+	unsigned long crit_alarm[NUM_SENSORS];
+	struct delayed_work work;
+	struct delayed_work power_off_work;
+	struct mutex lock;
+	/* Delay (ms) between temperature readings */
+	unsigned long gpadc_monitor_delay;
+	/* Delay (ms) before power off */
+	unsigned long power_off_delay;
+	int monitored_sensors;
+};
+
+int ab8500_hwmon_init(struct abx500_temp *data) __init;
+int ab5500_hwmon_init(struct abx500_temp *data) __init;
+
+#endif /* _ABX500_H */
diff --git a/drivers/hwmon/dbx500.c b/drivers/hwmon/dbx500.c
new file mode 100644
index 00000000000..c034b48f8dd
--- /dev/null
+++ b/drivers/hwmon/dbx500.c
@@ -0,0 +1,402 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ *
+ * Author: WenHai Fang <wenhai.h.fang@stericsson.com> for
+ * ST-Ericsson.
+ * License terms: GNU Gereral Public License (GPL) version 2
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/hwmon.h>
+#include <linux/sysfs.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/jiffies.h>
+#include <linux/mutex.h>
+#include <linux/pm.h>
+#include <linux/io.h>
+#include <mach/hardware.h>
+
+/*
+ * Default measure period to 0xFF x cycle32k
+ */
+#define DEFAULT_MEASURE_TIME 0xFF
+
+/*
+ * Default critical sensor temperature
+ */
+#define DEFAULT_CRITICAL_TEMP 85
+
+/* This driver monitors DB thermal*/
+#define NUM_SENSORS 1
+
+struct dbx500_temp {
+	struct platform_device *pdev;
+	struct device *hwmon_dev;
+	unsigned char min[NUM_SENSORS];
+	unsigned char max[NUM_SENSORS];
+	unsigned char crit[NUM_SENSORS];
+	unsigned char min_alarm[NUM_SENSORS];
+	unsigned char max_alarm[NUM_SENSORS];
+	unsigned short measure_time;
+	bool monitoring_active;
+	struct mutex lock;
+};
+
+static inline void start_temp_monitoring(struct dbx500_temp *data,
+					 const int index)
+{
+	unsigned int i;
+
+	/* determine if there are any sensors worth monitoring */
+	for (i = 0; i < NUM_SENSORS; i++)
+		if (data->min[i] || data->max[i])
+			goto start_monitoring;
+
+	return;
+
+start_monitoring:
+	/* kick off the monitor job */
+	data->min_alarm[index] = 0;
+	data->max_alarm[index] = 0;
+
+	(void) prcmu_start_temp_sense(data->measure_time);
+	data->monitoring_active = true;
+}
+
+static inline void stop_temp_monitoring(struct dbx500_temp *data)
+{
+	if (data->monitoring_active) {
+		(void) prcmu_stop_temp_sense();
+		data->monitoring_active = false;
+	}
+}
+
+/* HWMON sysfs interface */
+static ssize_t show_name(struct device *dev, struct device_attribute *devattr,
+			 char *buf)
+{
+	return sprintf(buf, "dbx500\n");
+}
+
+static ssize_t show_label(struct device *dev, struct device_attribute *devattr,
+			char *buf)
+{
+	return show_name(dev, devattr, buf);
+}
+
+/* set functions (RW nodes) */
+static ssize_t set_min(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct dbx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	val &= 0xFF;
+	if (val > data->max[attr->index - 1])
+		val = data->max[attr->index - 1];
+
+	data->min[attr->index - 1] = val;
+
+	stop_temp_monitoring(data);
+
+	(void) prcmu_config_hotmon(data->min[attr->index - 1],
+			data->max[attr->index - 1]);
+
+	start_temp_monitoring(data, (attr->index - 1));
+
+	mutex_unlock(&data->lock);
+	return count;
+}
+
+static ssize_t set_max(struct device *dev, struct device_attribute *devattr,
+		       const char *buf, size_t count)
+{
+	unsigned long val;
+	struct dbx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	val &= 0xFF;
+	if (val < data->min[attr->index - 1])
+		val = data->min[attr->index - 1];
+
+	data->max[attr->index - 1] = val;
+
+	stop_temp_monitoring(data);
+
+	(void) prcmu_config_hotmon(data->min[attr->index - 1],
+		data->max[attr->index - 1]);
+
+	start_temp_monitoring(data, (attr->index - 1));
+
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t set_crit(struct device *dev,
+			    struct device_attribute *devattr,
+			    const char *buf, size_t count)
+{
+	unsigned long val;
+	struct dbx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	int res = strict_strtoul(buf, 10, &val);
+	if (res < 0)
+		return res;
+
+	mutex_lock(&data->lock);
+	val &= 0xFF;
+	data->crit[attr->index - 1] = val;
+	(void) prcmu_config_hotdog(data->crit[attr->index - 1]);
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+/*
+ * show functions (RO nodes)
+ * Notice that min/max/crit refer to degrees
+ */
+static ssize_t show_min(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct dbx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->min[attr->index - 1]);
+}
+
+static ssize_t show_max(struct device *dev,
+			struct device_attribute *devattr, char *buf)
+{
+	struct dbx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->max[attr->index - 1]);
+}
+
+static ssize_t show_crit(struct device *dev,
+			     struct device_attribute *devattr, char *buf)
+{
+	struct dbx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->crit[attr->index - 1]);
+}
+
+/* Alarms */
+static ssize_t show_min_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct dbx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->min_alarm[attr->index - 1]);
+}
+
+static ssize_t show_max_alarm(struct device *dev,
+			      struct device_attribute *devattr, char *buf)
+{
+	struct dbx500_temp *data = dev_get_drvdata(dev);
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	/* hwmon attr index starts at 1, thus "attr->index-1" below */
+	return sprintf(buf, "%d\n", data->max_alarm[attr->index - 1]);
+}
+
+/* Chip name, required by hwmon*/
+static SENSOR_DEVICE_ATTR(name, S_IRUGO, show_name, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_min, set_min, 1);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_max, set_max, 1);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IWUSR | S_IRUGO,
+			show_crit, set_crit, 1);
+static SENSOR_DEVICE_ATTR(temp1_label, S_IRUGO, show_label, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_min_alarm, S_IRUGO, show_min_alarm, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp1_max_alarm, S_IRUGO, show_max_alarm, NULL, 1);
+
+static struct attribute *dbx500_temp_attributes[] = {
+	&sensor_dev_attr_name.dev_attr.attr,
+	&sensor_dev_attr_temp1_min.dev_attr.attr,
+	&sensor_dev_attr_temp1_max.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_label.dev_attr.attr,
+	&sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+	&sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group dbx500_temp_group = {
+	.attrs = dbx500_temp_attributes,
+};
+
+static irqreturn_t prcmu_hotmon_low_irq_handler(int irq, void *irq_data)
+{
+	struct platform_device *pdev = irq_data;
+	struct dbx500_temp *data = platform_get_drvdata(pdev);
+
+	mutex_lock(&data->lock);
+	data->min_alarm[0] = 1;
+	mutex_unlock(&data->lock);
+
+	sysfs_notify(&pdev->dev.kobj, NULL, "temp1_min_alarm");
+	dev_dbg(&pdev->dev, "DBX500 thermal low warning\n");
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t prcmu_hotmon_high_irq_handler(int irq, void *irq_data)
+{
+	struct platform_device *pdev = irq_data;
+	struct dbx500_temp *data = platform_get_drvdata(pdev);
+
+	mutex_lock(&data->lock);
+	data->max_alarm[0] = 1;
+	mutex_unlock(&data->lock);
+
+	hwmon_notify(data->max_alarm[0], NULL);
+	sysfs_notify(&pdev->dev.kobj, NULL, "temp1_max_alarm");
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit dbx500_temp_probe(struct platform_device *pdev)
+{
+	struct dbx500_temp *data;
+	int err = 0, i;
+	int irq;
+
+	dev_dbg(&pdev->dev, "dbx500_temp: Function dbx500_temp_probe.\n");
+
+	data = kzalloc(sizeof(struct dbx500_temp), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	irq = platform_get_irq_byname(pdev, "IRQ_HOTMON_LOW");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Get IRQ_HOTMON_LOW failed\n");
+		goto exit;
+	}
+
+	err = request_threaded_irq(irq, NULL,
+				prcmu_hotmon_low_irq_handler,
+				IRQF_NO_SUSPEND,
+				"dbx500_temp_low", pdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "dbx500: Failed allocate HOTMON_LOW.\n");
+		goto exit;
+	} else {
+		dev_dbg(&pdev->dev, "dbx500: Succeed allocate HOTMON_LOW.\n");
+	}
+
+	irq = platform_get_irq_byname(pdev, "IRQ_HOTMON_HIGH");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Get IRQ_HOTMON_HIGH failed\n");
+		goto exit;
+	}
+
+	err = request_threaded_irq(irq, NULL,
+				prcmu_hotmon_high_irq_handler,
+				IRQF_NO_SUSPEND,
+				 "dbx500_temp_high", pdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "dbx500: Failed allocate HOTMON_HIGH.\n");
+		goto exit;
+	} else {
+		dev_dbg(&pdev->dev, "dbx500: Succeed allocate HOTMON_HIGH.\n");
+	}
+
+	data->hwmon_dev = hwmon_device_register(&pdev->dev);
+	if (IS_ERR(data->hwmon_dev)) {
+		err = PTR_ERR(data->hwmon_dev);
+		dev_err(&pdev->dev, "Class registration failed (%d)\n", err);
+		goto exit;
+	}
+
+	for (i = 0; i < NUM_SENSORS; i++) {
+		data->min[i] = 0;
+		data->max[i] = 0;
+		data->crit[i] = DEFAULT_CRITICAL_TEMP;
+		data->min_alarm[i] = 0;
+		data->max_alarm[i] = 0;
+	}
+
+	mutex_init(&data->lock);
+
+	data->pdev = pdev;
+	data->measure_time = DEFAULT_MEASURE_TIME;
+	data->monitoring_active = false;
+
+	/* set PRCMU to disable platform when we get to the critical temp */
+	(void) prcmu_config_hotdog(DEFAULT_CRITICAL_TEMP);
+
+	platform_set_drvdata(pdev, data);
+
+	err = sysfs_create_group(&pdev->dev.kobj, &dbx500_temp_group);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Create sysfs group failed (%d)\n", err);
+		goto exit_platform_data;
+	}
+
+	return 0;
+
+exit_platform_data:
+	platform_set_drvdata(pdev, NULL);
+exit:
+	kfree(data);
+	return err;
+}
+
+static int __devexit dbx500_temp_remove(struct platform_device *pdev)
+{
+	struct dbx500_temp *data = platform_get_drvdata(pdev);
+
+	hwmon_device_unregister(data->hwmon_dev);
+	sysfs_remove_group(&pdev->dev.kobj, &dbx500_temp_group);
+	platform_set_drvdata(pdev, NULL);
+	kfree(data);
+	return 0;
+}
+
+/* No action required in suspend/resume, thus the lack of functions */
+static struct platform_driver dbx500_temp_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "dbx500_temp",
+	},
+	.probe = dbx500_temp_probe,
+	.remove = __devexit_p(dbx500_temp_remove),
+};
+
+static int __init dbx500_temp_init(void)
+{
+	return platform_driver_register(&dbx500_temp_driver);
+}
+
+static void __exit dbx500_temp_exit(void)
+{
+	platform_driver_unregister(&dbx500_temp_driver);
+}
+
+MODULE_AUTHOR("WenHai Fang <wenhai.h.fang@stericsson.com>");
+MODULE_DESCRIPTION("DBX500 temperature driver");
+MODULE_LICENSE("GPL");
+
+module_init(dbx500_temp_init)
+module_exit(dbx500_temp_exit)
diff --git a/drivers/hwmon/hwmon.c b/drivers/hwmon/hwmon.c
index c3c471ca202..8957bbac7a7 100644
--- a/drivers/hwmon/hwmon.c
+++ b/drivers/hwmon/hwmon.c
@@ -21,6 +21,7 @@
 #include <linux/gfp.h>
 #include <linux/spinlock.h>
 #include <linux/pci.h>
+#include <linux/notifier.h>
 
 #define HWMON_ID_PREFIX "hwmon"
 #define HWMON_ID_FORMAT HWMON_ID_PREFIX "%d"
@@ -29,6 +30,8 @@ static struct class *hwmon_class;
 
 static DEFINE_IDA(hwmon_ida);
 
+static BLOCKING_NOTIFIER_HEAD(hwmon_notifier_list);
+
 /**
  * hwmon_device_register - register w/ hwmon
  * @dev: the device to register
@@ -75,6 +78,24 @@ void hwmon_device_unregister(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(hwmon_device_unregister);
 
+int hwmon_notifier_register(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_register(&hwmon_notifier_list, nb);
+}
+EXPORT_SYMBOL(hwmon_notifier_register);
+
+int hwmon_notifier_unregister(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_unregister(&hwmon_notifier_list, nb);
+}
+EXPORT_SYMBOL(hwmon_notifier_unregister);
+
+void hwmon_notify(unsigned long val, void *v)
+{
+	blocking_notifier_call_chain(&hwmon_notifier_list, val, v);
+}
+EXPORT_SYMBOL(hwmon_notify);
+
 static void __init hwmon_pci_quirks(void)
 {
 #if defined CONFIG_X86 && defined CONFIG_PCI
diff --git a/drivers/hwmon/l3g4200d.c b/drivers/hwmon/l3g4200d.c
new file mode 100644
index 00000000000..d3e0b46b169
--- /dev/null
+++ b/drivers/hwmon/l3g4200d.c
@@ -0,0 +1,719 @@
+/*
+ * ST L3G4200D 3-Axis Gyroscope Driver
+ *
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Chethan Krishna N <chethan.krishna@stericsson.com> for ST-Ericsson
+ * Licence terms: GNU General Public Licence (GPL) version 2
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+
+#include <linux/l3g4200d.h>
+#include <linux/regulator/consumer.h>
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+/* l3g4200d gyroscope registers */
+
+#define WHO_AM_I				0x0F
+
+#define CTRL_REG1				0x20    /* CTRL REG1 */
+#define CTRL_REG2				0x21    /* CTRL REG2 */
+#define CTRL_REG3				0x22    /* CTRL_REG3 */
+#define CTRL_REG4				0x23    /* CTRL_REG4 */
+#define CTRL_REG5				0x24    /* CTRL_REG5 */
+#define OUT_TEMP				0x26    /* OUT_TEMP */
+
+#define AXISDATA_REG			0x28
+
+/** Registers Contents */
+
+#define WHOAMI_L3G4200D			0x00D3	/* Expected content for WAI register*/
+
+/* CTRL_REG1 */
+#define PM_OFF					0x00
+#define PM_ON					0x01
+#define ENABLE_ALL_AXES			0x07
+#define BW00					0x00
+#define BW01					0x10
+#define BW10					0x20
+#define BW11					0x30
+#define ODR00					0x00  /* ODR = 100Hz */
+#define ODR01					0x40  /* ODR = 200Hz */
+#define ODR10					0x80  /* ODR = 400Hz */
+#define ODR11					0xC0  /* ODR = 800Hz */
+#define L3G4200D_PM_BIT			3
+#define L3G4200D_PM_MASK		(0x01 << L3G4200D_PM_BIT)
+#define L3G4200D_ODR_BIT		4
+#define L3G4200D_ODR_MASK		(0x0F << L3G4200D_ODR_BIT)
+#define L3G4200D_ODR_MIN_VAL	0x00
+#define L3G4200D_ODR_MAX_VAL	0x0F
+
+/* CTRL_REG4 */
+#define FS250					0x00
+#define FS500					0x01
+#define FS2000					0x03
+#define BDU_ENABLE				0x80
+#define L3G4200D_FS_BIT			4
+#define L3G4200D_FS_MASK		(0x3 << L3G4200D_FS_BIT)
+
+/* multiple byte transfer enable */
+#define MULTIPLE_I2C_TR			0x80
+
+/* device status defines */
+#define DEVICE_OFF 0
+#define DEVICE_ON 1
+#define DEVICE_SUSPENDED 2
+
+/*
+ * L3G4200D gyroscope data
+ * brief structure containing gyroscope values for yaw, pitch and roll in
+ * signed short
+ */
+
+struct l3g4200d_gyro_values {
+	short x;	/* x-axis angular rate data. */
+	short y;	/* y-axis angluar rate data. */
+	short z;	/* z-axis angular rate data. */
+};
+
+struct l3g4200d_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	struct l3g4200d_gyro_values data;
+	struct l3g4200d_gyr_platform_data pdata;
+	struct regulator *regulator;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+	unsigned char powermode;
+	unsigned char odr;
+	unsigned char range;
+	int device_status;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void l3g4200d_early_suspend(struct early_suspend *ddata);
+static void l3g4200d_late_resume(struct early_suspend *ddata);
+#endif
+
+static int l3g4200d_write(struct l3g4200d_data *ddata, u8 reg,
+				u8 val, char *msg)
+{
+	int ret = i2c_smbus_write_byte_data(ddata->client, reg, val);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"i2c_smbus_write_byte_data failed error %d\
+				Register (%s)\n", ret, msg);
+	return ret;
+}
+
+static int l3g4200d_read(struct l3g4200d_data *ddata, u8 reg, char *msg)
+{
+	int ret = i2c_smbus_read_byte_data(ddata->client, reg);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"i2c_smbus_read_byte_data failed error %d\
+				Register (%s)\n", ret, msg);
+	return ret;
+}
+
+static int l3g4200d_readdata(struct l3g4200d_data *ddata)
+{
+	unsigned char gyro_data[6];
+	short data[3];
+	int ret;
+
+	ret = i2c_smbus_read_i2c_block_data(ddata->client,
+			AXISDATA_REG | MULTIPLE_I2C_TR, 6, gyro_data);
+	if (ret < 0) {
+		dev_err(&ddata->client->dev,
+				"i2c_smbus_read_byte_data failed error %d\
+				Register AXISDATA_REG\n", ret);
+		return ret;
+	}
+
+	data[0] = (short) (((gyro_data[1]) << 8) | gyro_data[0]);
+	data[1] = (short) (((gyro_data[3]) << 8) | gyro_data[2]);
+	data[2] = (short) (((gyro_data[5]) << 8) | gyro_data[4]);
+
+	data[ddata->pdata.axis_map_x] = ddata->pdata.negative_x ?
+		-data[ddata->pdata.axis_map_x] : data[ddata->pdata.axis_map_x];
+	data[ddata->pdata.axis_map_y] = ddata->pdata.negative_y ?
+		-data[ddata->pdata.axis_map_y] : data[ddata->pdata.axis_map_y];
+	data[ddata->pdata.axis_map_z] = ddata->pdata.negative_z ?
+		-data[ddata->pdata.axis_map_z] : data[ddata->pdata.axis_map_z];
+
+	ddata->data.x = data[ddata->pdata.axis_map_x];
+	ddata->data.y = data[ddata->pdata.axis_map_y];
+	ddata->data.z = data[ddata->pdata.axis_map_z];
+
+	return ret;
+}
+
+static ssize_t l3g4200d_show_gyrodata(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->powermode == PM_OFF ||
+			ddata->device_status == DEVICE_SUSPENDED) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	ret = l3g4200d_readdata(ddata);
+
+	if (ret < 0) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return sprintf(buf, "%8x:%8x:%8x\n", ddata->data.x, ddata->data.y,
+			ddata->data.z);
+}
+
+static ssize_t l3g4200d_show_range(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->range);
+}
+
+static ssize_t l3g4200d_store_range(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+	long received_value;
+	unsigned char value;
+	int error;
+
+	error = strict_strtol(buf, 0, &received_value);
+	if (error)
+		return error;
+
+	/* check if the received range is in valid range */
+	if (received_value < FS250 || received_value > FS2000)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->powermode == PM_OFF) {
+		dev_info(&ddata->client->dev,
+				"The device is switched off, turn it ON using powermode\n");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* enable the BDU bit */
+	value = BDU_ENABLE;
+	value |= ((received_value << L3G4200D_FS_BIT) & L3G4200D_FS_MASK);
+
+	ddata->range = received_value;
+
+	error = l3g4200d_write(ddata, CTRL_REG4, value, "CTRL_REG4");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+	return count;
+}
+
+static ssize_t l3g4200d_show_datarate(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->odr >> L3G4200D_ODR_BIT);
+}
+
+static ssize_t l3g4200d_store_datarate(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+	long received_value;
+	unsigned char value;
+	int error;
+
+	error = strict_strtol(buf, 0, &received_value);
+	if (error)
+		return error;
+
+	/* check if the received output datarate value is in valid range */
+	if (received_value < L3G4200D_ODR_MIN_VAL ||
+			received_value > L3G4200D_ODR_MAX_VAL)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->powermode == PM_OFF) {
+		dev_info(&ddata->client->dev,
+				"The device is switched off, turn it ON using powermode\n");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/*
+	 * read the current contents of CTRL_REG1
+	 * retain any bits set other than the odr bits
+	 */
+	error = l3g4200d_read(ddata, CTRL_REG1, "CTRL_REG1");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	} else
+		value = error;
+
+	value &= ~L3G4200D_ODR_MASK;
+	value |= ((received_value << L3G4200D_ODR_BIT) & L3G4200D_ODR_MASK);
+
+	ddata->odr = received_value << L3G4200D_ODR_BIT;
+
+	error = l3g4200d_write(ddata, CTRL_REG1, value, "CTRL_REG1");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+	return count;
+}
+
+static ssize_t l3g4200d_show_powermode(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->powermode);
+}
+
+static ssize_t l3g4200d_store_powermode(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+	long received_value;
+	unsigned char value;
+	int error;
+
+	error = strict_strtol(buf, 0, &received_value);
+	if (error)
+		return error;
+
+	/* check if the received power mode is either 0 or 1 */
+	if (received_value < PM_OFF || received_value > PM_ON)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_SUSPENDED &&
+			received_value == PM_OFF) {
+		ddata->powermode = received_value;
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* if sent value is same as current value do nothing */
+	if (ddata->powermode == received_value) {
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* turn on the power suppliy if it was turned off previously */
+	if (ddata->regulator && ddata->powermode == PM_OFF
+			&& (ddata->device_status == DEVICE_OFF
+				|| ddata->device_status == DEVICE_SUSPENDED)) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+	}
+
+	/*
+	 * read the current contents of CTRL_REG1
+	 * retain any bits set other than the power bit
+	 */
+	error = l3g4200d_read(ddata, CTRL_REG1, "CTRL_REG1");
+
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
+		return error;
+	} else
+		value = error;
+
+	value &= ~L3G4200D_PM_MASK;
+	value |= ((received_value << L3G4200D_PM_BIT) & L3G4200D_PM_MASK);
+
+	ddata->powermode = received_value;
+
+	error = l3g4200d_write(ddata, CTRL_REG1, value, "CTRL_REG1");
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	if (received_value == PM_OFF) {
+		/* set the other configuration values to defaults */
+		ddata->odr = ODR00 | BW00;
+		ddata->range = FS250;
+
+		/* turn off the power supply */
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+	mutex_unlock(&ddata->lock);
+	return count;
+}
+
+static ssize_t l3g4200d_show_gyrotemp(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct l3g4200d_data *ddata = platform_get_drvdata(pdev);
+	int ret;
+
+	if (ddata->powermode == PM_OFF ||
+			ddata->device_status == DEVICE_SUSPENDED)
+		return -EINVAL;
+
+	ret = l3g4200d_read(ddata, OUT_TEMP, "OUT_TEMP");
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%d\n", ret);
+}
+
+static DEVICE_ATTR(gyrodata, S_IRUGO, l3g4200d_show_gyrodata, NULL);
+
+static DEVICE_ATTR(range, S_IRUGO | S_IWUSR,
+		l3g4200d_show_range, l3g4200d_store_range);
+
+static DEVICE_ATTR(datarate, S_IRUGO | S_IWUSR,
+		l3g4200d_show_datarate, l3g4200d_store_datarate);
+
+static DEVICE_ATTR(powermode, S_IRUGO | S_IWUSR,
+		l3g4200d_show_powermode, l3g4200d_store_powermode);
+
+static DEVICE_ATTR(gyrotemp, S_IRUGO, l3g4200d_show_gyrotemp, NULL);
+
+static struct attribute *l3g4200d_attributes[] = {
+	&dev_attr_gyrodata.attr,
+	&dev_attr_range.attr,
+	&dev_attr_datarate.attr,
+	&dev_attr_powermode.attr,
+	&dev_attr_gyrotemp.attr,
+	NULL
+};
+
+static const struct attribute_group l3g4200d_attr_group = {
+	.attrs = l3g4200d_attributes,
+};
+
+static int __devinit l3g4200d_probe(struct i2c_client *client,
+		const struct i2c_device_id *devid)
+{
+	int ret = -1;
+	struct l3g4200d_data *ddata = NULL;
+
+	if (!i2c_check_functionality(client->adapter,
+				I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_READ_I2C_BLOCK))
+		goto exit;
+
+	ddata = kzalloc(sizeof(struct l3g4200d_data), GFP_KERNEL);
+	if (ddata == NULL) {
+		dev_err(&client->dev, "memory alocation failed\n");
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	ddata->client = client;
+	i2c_set_clientdata(client, ddata);
+
+	memcpy(&ddata->pdata, client->dev.platform_data, sizeof(ddata->pdata));
+	/* store default values in the data structure */
+	ddata->odr = ODR00 | BW00;
+	ddata->range = FS250;
+	ddata->powermode = PM_OFF;
+	ddata->device_status = DEVICE_OFF;
+
+	dev_set_name(&client->dev, ddata->pdata.name_gyr);
+
+	ddata->regulator = regulator_get(&client->dev, "vdd");
+	if (IS_ERR(ddata->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		ret = PTR_ERR(ddata->regulator);
+		ddata->regulator = NULL;
+		goto error_op_failed;
+	}
+
+	if (ddata->regulator) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+	}
+
+	ret = l3g4200d_read(ddata, WHO_AM_I, "WHO_AM_I");
+	if (ret < 0)
+		goto exit_free_regulator;
+
+	if (ret == WHOAMI_L3G4200D)
+		dev_info(&client->dev, "3-Axis Gyroscope device identification: %d\n", ret);
+	else
+		dev_info(&client->dev, "Gyroscope identification did not match\n");
+
+	mutex_init(&ddata->lock);
+
+	ret = sysfs_create_group(&client->dev.kobj, &l3g4200d_attr_group);
+	if (ret)
+		goto exit_free_regulator;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ddata->early_suspend.level =
+		EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ddata->early_suspend.suspend = l3g4200d_early_suspend;
+	ddata->early_suspend.resume = l3g4200d_late_resume;
+	register_early_suspend(&ddata->early_suspend);
+#endif
+
+	/*
+	 * turn off the supplies until somebody turns on the device
+	 * using l3g4200d_store_powermode
+	 */
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+
+	return ret;
+
+exit_free_regulator:
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		regulator_put(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+error_op_failed:
+	kfree(ddata);
+exit:
+	dev_err(&client->dev, "probe function failed %x\n", ret);
+	return ret;
+}
+
+static int __devexit l3g4200d_remove(struct i2c_client *client)
+{
+	struct l3g4200d_data *ddata;
+	ddata = i2c_get_clientdata(client);
+	sysfs_remove_group(&client->dev.kobj, &l3g4200d_attr_group);
+
+	/* safer to turn off the device */
+	if (ddata->powermode != PM_OFF) {
+		l3g4200d_write(ddata, CTRL_REG1, PM_OFF, "CONTROL");
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			regulator_put(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+
+	i2c_set_clientdata(client, NULL);
+	kfree(ddata);
+
+	return 0;
+}
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_PM)
+
+static int l3g4200d_do_suspend(struct l3g4200d_data *ddata)
+{
+	int ret;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->powermode == PM_OFF) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+	ret = l3g4200d_write(ddata, CTRL_REG1, PM_OFF, "CONTROL");
+
+	/* turn off the power when suspending the device */
+	if (ddata->regulator)
+		regulator_disable(ddata->regulator);
+
+	ddata->device_status = DEVICE_SUSPENDED;
+
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+
+static int l3g4200d_do_resume(struct l3g4200d_data *ddata)
+{
+	unsigned char range_value;
+	unsigned char shifted_powermode = (ddata->powermode << L3G4200D_PM_BIT);
+	unsigned char shifted_odr = (ddata->odr << L3G4200D_ODR_BIT);
+	unsigned context = ((shifted_powermode | shifted_odr) | ENABLE_ALL_AXES);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_ON)
+		goto fail;
+
+	/* in correct mode, no need to change it */
+	if (ddata->powermode == PM_OFF) {
+		ddata->device_status = DEVICE_OFF;
+		goto fail;
+	} else {
+		 ddata->device_status = DEVICE_ON;
+	}
+
+	/* turn on the power when resuming the device */
+	if (ddata->regulator)
+		regulator_enable(ddata->regulator);
+
+	ret = l3g4200d_write(ddata, CTRL_REG1, context, "CONTROL");
+	if (ret < 0)
+		goto fail;
+
+	range_value = ddata->range;
+	range_value <<= L3G4200D_FS_BIT;
+	range_value |= BDU_ENABLE;
+
+	ret = l3g4200d_write(ddata, CTRL_REG4, range_value, "RANGE");
+
+fail:
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+#endif
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#ifdef CONFIG_PM
+static int l3g4200d_suspend(struct device *dev)
+{
+	struct l3g4200d_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = l3g4200d_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device\n");
+
+	return ret;
+}
+
+static int l3g4200d_resume(struct device *dev)
+{
+	struct l3g4200d_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = l3g4200d_do_resume(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device\n");
+
+	return ret;
+}
+
+static const struct dev_pm_ops l3g4200d_dev_pm_ops = {
+	.suspend = l3g4200d_suspend,
+	.resume = l3g4200d_resume,
+};
+#endif
+#else
+static void l3g4200d_early_suspend(struct early_suspend *data)
+{
+	struct l3g4200d_data *ddata =
+		container_of(data, struct l3g4200d_data, early_suspend);
+	int ret;
+
+	ret = l3g4200d_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device\n");
+}
+
+static void l3g4200d_late_resume(struct early_suspend *data)
+{
+	struct l3g4200d_data *ddata =
+		container_of(data, struct l3g4200d_data, early_suspend);
+	int ret;
+
+	ret = l3g4200d_do_resume(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device\n");
+}
+#endif
+
+static const struct i2c_device_id  l3g4200d_id[] = {
+	{"l3g4200d", 0 },
+	{ },
+};
+
+static struct i2c_driver l3g4200d_driver = {
+	.driver = {
+		.name = "l3g4200d",
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+		.pm = &l3g4200d_dev_pm_ops,
+#endif
+	},
+	.probe = l3g4200d_probe,
+	.remove = l3g4200d_remove,
+	.id_table = l3g4200d_id,
+};
+
+static int __init l3g4200d_init(void)
+{
+	return i2c_add_driver(&l3g4200d_driver);
+}
+
+static void __exit l3g4200d_exit(void)
+{
+	i2c_del_driver(&l3g4200d_driver);
+}
+
+module_init(l3g4200d_init);
+module_exit(l3g4200d_exit);
+
+MODULE_DESCRIPTION("l3g4200d digital gyroscope driver");
+MODULE_AUTHOR("Chethan Krishna N");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/lsm303dlh_a.c b/drivers/hwmon/lsm303dlh_a.c
new file mode 100644
index 00000000000..22600c550f9
--- /dev/null
+++ b/drivers/hwmon/lsm303dlh_a.c
@@ -0,0 +1,1375 @@
+/*
+ * lsm303dlh_a.c
+ * ST 3-Axis Accelerometer Driver
+ *
+ * Copyright (C) 2010 STMicroelectronics
+ * Author: Carmine Iascone (carmine.iascone@st.com)
+ * Author: Matteo Dameno (matteo.dameno@st.com)
+ *
+ * Copyright (C) 2010 STEricsson
+ * Author: Mian Yousaf Kaukab <mian.yousaf.kaukab@stericsson.com>
+ * Updated:Preetham Rao Kaskurthi <preetham.rao@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <mach/gpio.h>
+#endif
+
+#include <linux/lsm303dlh.h>
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+#include <linux/regulator/consumer.h>
+
+ /* lsm303dlh accelerometer registers */
+ #define WHO_AM_I    0x0F
+
+ /* ctrl 1: pm2 pm1 pm0 dr1 dr0 zenable yenable zenable */
+ #define CTRL_REG1       0x20    /* power control reg */
+ #define CTRL_REG2       0x21    /* power control reg */
+ #define CTRL_REG3       0x22    /* power control reg */
+ #define CTRL_REG4       0x23    /* interrupt control reg */
+ #define CTRL_REG5       0x24    /* interrupt control reg */
+
+ #define STATUS_REG      0x27    /* status register */
+
+ #define AXISDATA_REG    0x28    /* axis data */
+
+ #define INT1_CFG  0x30    /* interrupt 1 configuration */
+ #define INT1_SRC  0x31    /* interrupt 1 source reg    */
+ #define INT1_THS  0x32    /* interrupt 1 threshold */
+ #define INT1_DURATION  0x33    /* interrupt 1 threshold */
+
+ #define INT2_CFG  0x34    /* interrupt 2 configuration */
+ #define INT2_SRC  0x35    /* interrupt 2 source reg    */
+ #define INT2_THS  0x36    /* interrupt 2 threshold */
+ #define INT2_DURATION  0x37    /* interrupt 2 threshold */
+
+ /* Sensitivity adjustment */
+ #define SHIFT_ADJ_2G 4 /*    1/16*/
+ #define SHIFT_ADJ_4G 3 /*    2/16*/
+ #define SHIFT_ADJ_8G 2 /* ~3.9/16*/
+
+ /* Control register 1 */
+ #define LSM303DLH_A_CR1_PM_BIT 5
+ #define LSM303DLH_A_CR1_PM_MASK (0x7 << LSM303DLH_A_CR1_PM_BIT)
+ #define LSM303DLH_A_CR1_DR_BIT 3
+ #define LSM303DLH_A_CR1_DR_MASK (0x3 << LSM303DLH_A_CR1_DR_BIT)
+ #define LSM303DLH_A_CR1_EN_BIT 0
+ #define LSM303DLH_A_CR1_EN_MASK (0x7 << LSM303DLH_A_CR1_EN_BIT)
+ #define LSM303DLH_A_CR1_AXIS_ENABLE 7
+
+ /* Control register 2 */
+ #define LSM303DLH_A_CR4_ST_BIT 1
+ #define LSM303DLH_A_CR4_ST_MASK (0x1 << LSM303DLH_A_CR4_ST_BIT)
+ #define LSM303DLH_A_CR4_STS_BIT 3
+ #define LSM303DLH_A_CR4_STS_MASK (0x1 << LSM303DLH_A_CR4_STS_BIT)
+ #define LSM303DLH_A_CR4_FS_BIT 4
+ #define LSM303DLH_A_CR4_FS_MASK (0x3 << LSM303DLH_A_CR4_FS_BIT)
+ #define LSM303DLH_A_CR4_BLE_BIT 6
+ #define LSM303DLH_A_CR4_BLE_MASK (0x3 << LSM303DLH_A_CR4_BLE_BIT)
+ #define LSM303DLH_A_CR4_BDU_BIT 7
+ #define LSM303DLH_A_CR4_BDU_MASK (0x1 << LSM303DLH_A_CR4_BDU_BIT)
+
+ /* Control register 3 */
+ #define LSM303DLH_A_CR3_I1_BIT 0
+ #define LSM303DLH_A_CR3_I1_MASK (0x3 << LSM303DLH_A_CR3_I1_BIT)
+ #define LSM303DLH_A_CR3_LIR1_BIT 2
+ #define LSM303DLH_A_CR3_LIR1_MASK (0x1 << LSM303DLH_A_CR3_LIR1_BIT)
+ #define LSM303DLH_A_CR3_I2_BIT 3
+ #define LSM303DLH_A_CR3_I2_MASK (0x3 << LSM303DLH_A_CR3_I2_BIT)
+ #define LSM303DLH_A_CR3_LIR2_BIT 5
+ #define LSM303DLH_A_CR3_LIR2_MASK (0x1 << LSM303DLH_A_CR3_LIR2_BIT)
+ #define LSM303DLH_A_CR3_PPOD_BIT 6
+ #define LSM303DLH_A_CR3_PPOD_MASK (0x1 << LSM303DLH_A_CR3_PPOD_BIT)
+ #define LSM303DLH_A_CR3_IHL_BIT 7
+ #define LSM303DLH_A_CR3_IHL_MASK (0x1 << LSM303DLH_A_CR3_IHL_BIT)
+
+ #define LSM303DLH_A_CR3_I_SELF 0x0
+ #define LSM303DLH_A_CR3_I_OR   0x1
+ #define LSM303DLH_A_CR3_I_DATA 0x2
+ #define LSM303DLH_A_CR3_I_BOOT 0x3
+
+ #define LSM303DLH_A_CR3_LIR_LATCH 0x1
+
+ /* Range */
+ #define LSM303DLH_A_RANGE_2G 0x00
+ #define LSM303DLH_A_RANGE_4G 0x01
+ #define LSM303DLH_A_RANGE_8G 0x03
+
+ /* Mode */
+ #define LSM303DLH_A_MODE_OFF 0x00
+ #define LSM303DLH_A_MODE_NORMAL 0x01
+ #define LSM303DLH_A_MODE_LP_HALF 0x02
+ #define LSM303DLH_A_MODE_LP_1 0x03
+ #define LSM303DLH_A_MODE_LP_2 0x02
+ #define LSM303DLH_A_MODE_LP_5 0x05
+ #define LSM303DLH_A_MODE_LP_10 0x06
+
+ /* Rate */
+ #define LSM303DLH_A_RATE_50 0x00
+ #define LSM303DLH_A_RATE_100 0x01
+ #define LSM303DLH_A_RATE_400 0x02
+ #define LSM303DLH_A_RATE_1000 0x03
+
+ /* Sleep & Wake */
+ #define LSM303DLH_A_SLEEPWAKE_DISABLE 0x00
+ #define LSM303DLH_A_SLEEPWAKE_ENABLE 0x3
+
+/* Multiple byte transfer enable */
+#define MULTIPLE_I2C_TR 0x80
+
+/* device status defines */
+#define DEVICE_OFF 0
+#define DEVICE_ON 1
+#define DEVICE_SUSPENDED 2
+
+/* Range -2048 to 2047 */
+struct lsm303dlh_a_t {
+	short	x;
+	short	y;
+	short	z;
+};
+
+/**
+ * struct lsm303dlh_a_data - data structure used by lsm303dlh_a driver
+ * @client: i2c client
+ * @lock: mutex lock for sysfs operations
+ * @data: lsm303dlh_a_t struct containing x, y and z values
+ * @input_dev: input device
+ * @input_dev2: input device
+ * @pdata: lsm303dlh platform data
+ * @regulator: regulator
+ * @range: current range value of accelerometer
+ * @mode: current mode of operation
+ * @rate: current sampling rate
+ * @sleep_wake: sleep wake setting
+ * @shift_adjust: current shift adjust value set according to range
+ * @interrupt_control: interrupt control settings
+ * @interrupt_channel: interrupt channel 0 or 1
+ * @interrupt_configure: interrupt configurations for two channels
+ * @interrupt_duration: interrupt duration for two channels
+ * @interrupt_threshold: interrupt threshold for two channels
+ * @early_suspend: early suspend structure
+ * @device_status: device is ON, OFF or SUSPENDED
+ * @id: accelerometer device id
+ */
+struct lsm303dlh_a_data {
+	struct i2c_client *client;
+	/* lock for sysfs operations */
+	struct mutex lock;
+	struct lsm303dlh_a_t data;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	struct input_dev *input_dev;
+	struct input_dev *input_dev2;
+#endif
+
+	struct lsm303dlh_platform_data pdata;
+	struct regulator *regulator;
+
+	unsigned char range;
+	unsigned char mode;
+	unsigned char rate;
+	unsigned char sleep_wake;
+	int shift_adjust;
+
+	unsigned char interrupt_control;
+	unsigned int  interrupt_channel;
+
+	unsigned char interrupt_configure[2];
+	unsigned char interrupt_duration[2];
+	unsigned char interrupt_threshold[2];
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+	int device_status;
+	int id;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void lsm303dlh_a_early_suspend(struct early_suspend *data);
+static void lsm303dlh_a_late_resume(struct early_suspend *data);
+#endif
+
+static int lsm303dlh_a_write(struct lsm303dlh_a_data *ddata, u8 reg,
+		u8 val, char *msg)
+{
+	int ret = i2c_smbus_write_byte_data(ddata->client, reg, val);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_write_byte_data failed error %d\
+			Register (%s)\n", ret, msg);
+	return ret;
+}
+
+static int lsm303dlh_a_read(struct lsm303dlh_a_data *ddata, u8 reg, char *msg)
+{
+	int ret = i2c_smbus_read_byte_data(ddata->client, reg);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_read_byte_data failed error %d\
+			 Register (%s)\n", ret, msg);
+	return ret;
+}
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_PM)
+static int lsm303dlh_a_do_suspend(struct lsm303dlh_a_data *ddata)
+{
+	int ret;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+
+	ret = lsm303dlh_a_write(ddata, CTRL_REG1,
+			LSM303DLH_A_MODE_OFF, "CONTROL");
+
+	if (ddata->regulator)
+		regulator_disable(ddata->regulator);
+
+	ddata->device_status = DEVICE_SUSPENDED;
+
+	mutex_unlock(&ddata->lock);
+
+	return ret;
+}
+
+static int lsm303dlh_a_restore(struct lsm303dlh_a_data *ddata)
+{
+	unsigned char reg;
+	unsigned char shifted_mode = (ddata->mode << LSM303DLH_A_CR1_PM_BIT);
+	unsigned char shifted_rate = (ddata->rate << LSM303DLH_A_CR1_DR_BIT);
+	unsigned char context = (shifted_mode | shifted_rate);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_ON) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+	/* in correct mode, no need to change it */
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		ddata->device_status = DEVICE_OFF;
+		mutex_unlock(&ddata->lock);
+		return 0;
+	} else
+		ddata->device_status = DEVICE_ON;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	enable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+	enable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+
+	if (ddata->regulator)
+		regulator_enable(ddata->regulator);
+
+	/* BDU should be enabled by default/recommened */
+	reg = ddata->range;
+	reg |= LSM303DLH_A_CR4_BDU_MASK;
+	context |= LSM303DLH_A_CR1_AXIS_ENABLE;
+
+	ret = lsm303dlh_a_write(ddata, CTRL_REG1, context,
+			"CTRL_REG1");
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, CTRL_REG4, reg, "CTRL_REG4");
+
+	if (ret < 0)
+		goto fail;
+
+	/* write to the boot bit to reboot memory content */
+	ret = lsm303dlh_a_write(ddata, CTRL_REG2, 0x80, "CTRL_REG2");
+
+	if (ret < 0)
+		goto fail;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	ret = lsm303dlh_a_write(ddata, CTRL_REG3, ddata->interrupt_control,
+			"CTRL_REG3");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT1_CFG, ddata->interrupt_configure[0],
+			"INT1_CFG");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT2_CFG, ddata->interrupt_configure[1],
+			"INT2_CFG");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT1_THS, ddata->interrupt_threshold[0],
+			"INT1_THS");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT2_THS, ddata->interrupt_threshold[1],
+			"INT2_THS");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT1_DURATION,
+			ddata->interrupt_duration[0], "INT1_DURATION");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_a_write(ddata, INT1_DURATION,
+			ddata->interrupt_duration[1], "INT1_DURATION");
+
+	if (ret < 0)
+		goto fail;
+#endif
+
+fail:
+	if (ret < 0)
+		dev_err(&ddata->client->dev, "could not restore the device %d\n", ret);
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+#endif
+
+static int lsm303dlh_a_readdata(struct lsm303dlh_a_data *ddata)
+{
+	unsigned char acc_data[6];
+	short data[3];
+
+	int ret = i2c_smbus_read_i2c_block_data(ddata->client,
+			AXISDATA_REG | MULTIPLE_I2C_TR, 6, acc_data);
+	if (ret < 0) {
+		dev_err(&ddata->client->dev,
+				"i2c_smbus_read_byte_data failed error %d\
+				Register AXISDATA_REG \n", ret);
+		return ret;
+	}
+
+	data[0] = (short) (((acc_data[1]) << 8) | acc_data[0]);
+	data[1] = (short) (((acc_data[3]) << 8) | acc_data[2]);
+	data[2] = (short) (((acc_data[5]) << 8) | acc_data[4]);
+
+	data[0] >>= ddata->shift_adjust;
+	data[1] >>= ddata->shift_adjust;
+	data[2] >>= ddata->shift_adjust;
+
+	/* taking position and orientation of x,y,z axis into account*/
+
+	data[ddata->pdata.axis_map_x] = ddata->pdata.negative_x ?
+		-data[ddata->pdata.axis_map_x] : data[ddata->pdata.axis_map_x];
+	data[ddata->pdata.axis_map_y] = ddata->pdata.negative_y ?
+		-data[ddata->pdata.axis_map_y] : data[ddata->pdata.axis_map_y];
+	data[ddata->pdata.axis_map_z] = ddata->pdata.negative_z ?
+		-data[ddata->pdata.axis_map_z] : data[ddata->pdata.axis_map_z];
+
+	ddata->data.x = data[ddata->pdata.axis_map_x];
+	ddata->data.y = data[ddata->pdata.axis_map_y];
+	ddata->data.z = data[ddata->pdata.axis_map_z];
+
+	return ret;
+}
+
+static ssize_t lsm303dlh_a_show_data(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF ||
+			ddata->device_status == DEVICE_SUSPENDED) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	ret = lsm303dlh_a_readdata(ddata);
+
+	if (ret < 0) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return sprintf(buf, "%8x:%8x:%8x\n", ddata->data.x, ddata->data.y,
+			ddata->data.z);
+}
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+static irqreturn_t lsm303dlh_a_gpio_irq(int irq, void *device_data)
+{
+
+	struct lsm303dlh_a_data *ddata = device_data;
+	int ret;
+	unsigned char reg;
+	struct input_dev *input;
+
+	/* know your interrupt source */
+	if (irq == gpio_to_irq(ddata->pdata.irq_a1)) {
+		reg = INT1_SRC;
+		input = ddata->input_dev;
+	} else if (irq == gpio_to_irq(ddata->pdata.irq_a2)) {
+		reg = INT2_SRC;
+		input = ddata->input_dev2;
+	} else {
+		dev_err(&ddata->client->dev, "spurious interrupt");
+		return IRQ_HANDLED;
+	}
+
+	/* read the axis */
+	ret = lsm303dlh_a_readdata(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"reading data of xyz failed error %d\n", ret);
+
+	input_report_abs(input, ABS_X, ddata->data.x);
+	input_report_abs(input, ABS_Y, ddata->data.y);
+	input_report_abs(input, ABS_Z, ddata->data.z);
+	input_sync(input);
+
+	/* clear the value by reading it */
+	ret = lsm303dlh_a_read(ddata, reg, "INTTERUPT SOURCE");
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"clearing interrupt source failed error %d\n", ret);
+
+	return IRQ_HANDLED;
+
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_control(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->interrupt_control);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_control(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->interrupt_control = val;
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG3, val, "CTRL_REG3");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_channel(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->interrupt_channel);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_channel(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	ddata->interrupt_channel = val;
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_configure(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n",
+			ddata->interrupt_configure[ddata->interrupt_channel]);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_configure(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+	    mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->interrupt_configure[ddata->interrupt_channel] = val;
+
+	if (ddata->interrupt_channel == 0x0)
+		error = lsm303dlh_a_write(ddata, INT1_CFG, val, "INT1_CFG");
+	else
+		error = lsm303dlh_a_write(ddata, INT2_CFG, val, "INT2_CFG");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_duration(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n",
+			ddata->interrupt_duration[ddata->interrupt_channel]);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_duration(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->interrupt_duration[ddata->interrupt_channel] = val;
+
+	if (ddata->interrupt_channel == 0x0)
+		error = lsm303dlh_a_write(ddata, INT1_DURATION, val,
+				"INT1_DURATION");
+	else
+		error = lsm303dlh_a_write(ddata, INT2_DURATION, val,
+				"INT2_DURATION");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_interrupt_threshold(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n",
+			ddata->interrupt_threshold[ddata->interrupt_channel]);
+}
+
+static ssize_t lsm303dlh_a_store_interrupt_threshold(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->interrupt_threshold[ddata->interrupt_channel] = val;
+
+	if (ddata->interrupt_channel == 0x0)
+		error = lsm303dlh_a_write(ddata, INT1_THS, val, "INT1_THS");
+	else
+		error = lsm303dlh_a_write(ddata, INT2_THS, val, "INT2_THS");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+#endif
+
+static ssize_t lsm303dlh_a_show_range(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->range >> LSM303DLH_A_CR4_FS_BIT);
+}
+
+static ssize_t lsm303dlh_a_store_range(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	unsigned long bdu_enabled_val;
+	int error;
+
+
+	error = strict_strtol(buf, 0, &val);
+	if (error)
+		return error;
+
+	if (val < LSM303DLH_A_RANGE_2G || val > LSM303DLH_A_RANGE_8G)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->range = val;
+	ddata->range <<= LSM303DLH_A_CR4_FS_BIT;
+
+	/*
+	 * Block mode update is recommended for not
+	 * ending up reading different values
+	 */
+	bdu_enabled_val = ddata->range;
+	bdu_enabled_val |= LSM303DLH_A_CR4_BDU_MASK;
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG4, bdu_enabled_val,
+	"CTRL_REG4");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	switch (val) {
+	case LSM303DLH_A_RANGE_2G:
+		ddata->shift_adjust = SHIFT_ADJ_2G;
+		break;
+	case LSM303DLH_A_RANGE_4G:
+		ddata->shift_adjust = SHIFT_ADJ_4G;
+		break;
+	case LSM303DLH_A_RANGE_8G:
+		ddata->shift_adjust = SHIFT_ADJ_8G;
+		break;
+	default:
+		mutex_unlock(&ddata->lock);
+		return -EINVAL;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_mode(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->mode);
+}
+
+static ssize_t lsm303dlh_a_store_mode(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	unsigned char data;
+	int error;
+	bool set_boot_bit = false;
+
+	error = strict_strtol(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	/* not in correct range */
+
+	if (val < LSM303DLH_A_MODE_OFF || val > LSM303DLH_A_MODE_LP_10) {
+		mutex_unlock(&ddata->lock);
+		return -EINVAL;
+	}
+
+	if (ddata->device_status == DEVICE_SUSPENDED) {
+		if (val == LSM303DLH_A_MODE_OFF) {
+			ddata->mode = val;
+			mutex_unlock(&ddata->lock);
+			return count;
+		} else {
+			/* device is turning on after suspend, reset memory */
+			set_boot_bit = true;
+		}
+	}
+
+	/*  if same mode as existing, return */
+	if (ddata->mode == val) {
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* turn on the supplies if already off */
+	if (ddata->regulator && ddata->mode == LSM303DLH_A_MODE_OFF
+			&& (ddata->device_status == DEVICE_OFF
+				|| ddata->device_status == DEVICE_SUSPENDED)) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+		enable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+		enable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+	}
+
+	data = lsm303dlh_a_read(ddata, CTRL_REG1, "CTRL_REG1");
+
+	/*
+	 * If chip doesn't get reset during suspend/resume,
+	 * x,y and z axis bits are getting cleared,so set
+	 * these bits to get x,y,z axis data.
+	 */
+	data |= LSM303DLH_A_CR1_AXIS_ENABLE;
+	data &= ~LSM303DLH_A_CR1_PM_MASK;
+
+	ddata->mode = val;
+	data |= ((val << LSM303DLH_A_CR1_PM_BIT) & LSM303DLH_A_CR1_PM_MASK);
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG1, data, "CTRL_REG1");
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	/*
+	 * Power on request when device is in suspended state
+	 * write to the boot bit in CTRL_REG2 to reboot memory content
+	 * and ensure correct device behavior after it resumes
+	 */
+	if (set_boot_bit) {
+		error = lsm303dlh_a_write(ddata, CTRL_REG2, 0x80, "CTRL_REG2");
+		if (error < 0) {
+			if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+				regulator_disable(ddata->regulator);
+				ddata->device_status = DEVICE_OFF;
+			}
+			mutex_unlock(&ddata->lock);
+			return error;
+		}
+	}
+
+	if (val == LSM303DLH_A_MODE_OFF) {
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+		disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+		disable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+#endif
+		/*
+		 * No need to store context here
+		 * it is not like suspend/resume
+		 * but fall back to default values
+		 */
+		ddata->rate = LSM303DLH_A_RATE_50;
+		ddata->range = LSM303DLH_A_RANGE_2G;
+		ddata->shift_adjust = SHIFT_ADJ_2G;
+
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_rate(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->rate);
+}
+
+static ssize_t lsm303dlh_a_store_rate(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	unsigned char data;
+	int error;
+
+	error = strict_strtol(buf, 0, &val);
+	if (error)
+		return error;
+
+	if (val < LSM303DLH_A_RATE_50 || val > LSM303DLH_A_RATE_1000)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	data = lsm303dlh_a_read(ddata, CTRL_REG1, "CTRL_REG1");
+
+	data &= ~LSM303DLH_A_CR1_DR_MASK;
+
+	ddata->rate = val;
+
+	data |= ((val << LSM303DLH_A_CR1_DR_BIT) & LSM303DLH_A_CR1_DR_MASK);
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG1, data, "CTRL_REG1");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_sleepwake(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->sleep_wake);
+}
+
+static ssize_t lsm303dlh_a_store_sleepwake(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	int error;
+
+	if (ddata->mode == LSM303DLH_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		return count;
+	}
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	ddata->sleep_wake = val;
+
+	error = lsm303dlh_a_write(ddata, CTRL_REG5, ddata->sleep_wake,
+				"CTRL_REG5");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlh_a_show_id(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->id);
+}
+
+static DEVICE_ATTR(id, S_IRUGO, lsm303dlh_a_show_id, NULL);
+
+static DEVICE_ATTR(data, S_IRUGO, lsm303dlh_a_show_data, NULL);
+
+static DEVICE_ATTR(range, S_IWUSR | S_IRUGO,
+		lsm303dlh_a_show_range, lsm303dlh_a_store_range);
+
+static DEVICE_ATTR(mode, S_IWUSR | S_IRUGO,
+		lsm303dlh_a_show_mode, lsm303dlh_a_store_mode);
+
+static DEVICE_ATTR(rate, S_IWUSR | S_IRUGO,
+		lsm303dlh_a_show_rate, lsm303dlh_a_store_rate);
+
+static DEVICE_ATTR(sleep_wake, S_IWUSR | S_IRUGO,
+		lsm303dlh_a_show_sleepwake, lsm303dlh_a_store_sleepwake);
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+static DEVICE_ATTR(interrupt_control, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_control,
+		lsm303dlh_a_store_interrupt_control);
+
+static DEVICE_ATTR(interrupt_channel, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_channel,
+		lsm303dlh_a_store_interrupt_channel);
+
+static DEVICE_ATTR(interrupt_configure, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_configure,
+		lsm303dlh_a_store_interrupt_configure);
+
+static DEVICE_ATTR(interrupt_duration, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_duration,
+		lsm303dlh_a_store_interrupt_duration);
+
+static DEVICE_ATTR(interrupt_threshold, S_IWUGO | S_IRUGO,
+		lsm303dlh_a_show_interrupt_threshold,
+		lsm303dlh_a_store_interrupt_threshold);
+#endif
+
+static struct attribute *lsm303dlh_a_attributes[] = {
+	&dev_attr_id.attr,
+	&dev_attr_data.attr,
+	&dev_attr_range.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_rate.attr,
+	&dev_attr_sleep_wake.attr,
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	&dev_attr_interrupt_control.attr,
+	&dev_attr_interrupt_channel.attr,
+	&dev_attr_interrupt_configure.attr,
+	&dev_attr_interrupt_duration.attr,
+	&dev_attr_interrupt_threshold.attr,
+#endif
+	NULL
+};
+
+static const struct attribute_group lsm303dlh_a_attr_group = {
+	.attrs = lsm303dlh_a_attributes,
+};
+
+static int __devinit lsm303dlh_a_probe(struct i2c_client *client,
+						const struct i2c_device_id *id)
+{
+	int ret;
+	struct lsm303dlh_a_data *ddata = NULL;
+
+	ddata = kzalloc(sizeof(struct lsm303dlh_a_data), GFP_KERNEL);
+	if (ddata == NULL) {
+		dev_err(&client->dev, "memory alocation failed\n");
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	ddata->client = client;
+	i2c_set_clientdata(client, ddata);
+
+	/* copy platform specific data */
+	memcpy(&ddata->pdata, client->dev.platform_data, sizeof(ddata->pdata));
+	ddata->mode = LSM303DLH_A_MODE_OFF;
+	ddata->rate = LSM303DLH_A_RATE_50;
+	ddata->range = LSM303DLH_A_RANGE_2G;
+	ddata->sleep_wake = LSM303DLH_A_SLEEPWAKE_DISABLE;
+	ddata->shift_adjust = SHIFT_ADJ_2G;
+	ddata->device_status = DEVICE_OFF;
+	dev_set_name(&client->dev, ddata->pdata.name_a);
+
+	ddata->regulator = regulator_get(&client->dev, "vdd");
+	if (IS_ERR(ddata->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		ret = PTR_ERR(ddata->regulator);
+		ddata->regulator = NULL;
+		goto err_op_failed;
+	}
+
+	if (ddata->regulator) {
+		/*
+		 * 0.83 milliamps typical with magnetic sensor setting ODR =
+		 * 7.5 Hz, Accelerometer sensor ODR = 50 Hz.  Double for
+		 * safety.
+		 */
+		regulator_set_optimum_mode(ddata->regulator, 830 * 2);
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+	}
+
+	ret = lsm303dlh_a_read(ddata, WHO_AM_I, "WHO_AM_I");
+	if (ret < 0)
+		goto exit_free_regulator;
+
+	dev_info(&client->dev, "3-Axis Accelerometer, ID : %d\n",
+			 ret);
+	ddata->id = ret;
+
+	mutex_init(&ddata->lock);
+
+	ret = sysfs_create_group(&client->dev.kobj, &lsm303dlh_a_attr_group);
+	if (ret)
+		goto exit_free_regulator;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+
+    /* accelerometer has two interrupts channels
+       (thresholds,durations and sources)
+       and can support two input devices */
+
+	ddata->input_dev = input_allocate_device();
+	if (!ddata->input_dev) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		goto exit_free_regulator;
+	}
+
+	ddata->input_dev2 = input_allocate_device();
+	if (!ddata->input_dev2) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		goto err_input_alloc_failed;
+	}
+
+	set_bit(EV_ABS, ddata->input_dev->evbit);
+	set_bit(EV_ABS, ddata->input_dev2->evbit);
+
+	/* x-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_X, -32768, 32767, 0, 0);
+	input_set_abs_params(ddata->input_dev2, ABS_X, -32768, 32767, 0, 0);
+	/* y-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_Y, -32768, 32767, 0, 0);
+	input_set_abs_params(ddata->input_dev2, ABS_Y, -32768, 32767, 0, 0);
+	/* z-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_Z, -32768, 32767, 0, 0);
+	input_set_abs_params(ddata->input_dev2, ABS_Z, -32768, 32767, 0, 0);
+
+	ddata->input_dev->name = "accelerometer";
+	ddata->input_dev2->name = "motion";
+
+	ret = input_register_device(ddata->input_dev);
+	if (ret) {
+		dev_err(&client->dev, "Unable to register input device: %s\n",
+			ddata->input_dev->name);
+		goto err_input_register_failed;
+	}
+
+	ret = input_register_device(ddata->input_dev2);
+	if (ret) {
+		dev_err(&client->dev, "Unable to register input device: %s\n",
+			ddata->input_dev->name);
+		goto err_input_register_failed2;
+	}
+
+	/* Register interrupt */
+	ret = request_threaded_irq(gpio_to_irq(ddata->pdata.irq_a1), NULL,
+			lsm303dlh_a_gpio_irq,
+			IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			"lsm303dlh_a", ddata);
+	if (ret) {
+		dev_err(&client->dev, "request irq1 failed\n");
+		goto err_input_failed;
+	}
+
+	ret = request_threaded_irq(gpio_to_irq(ddata->pdata.irq_a2), NULL,
+			lsm303dlh_a_gpio_irq,
+			IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
+			"lsm303dlh_a", ddata);
+	if (ret) {
+		dev_err(&client->dev, "request irq2 failed\n");
+		goto err_input_failed;
+	}
+
+	/* only mode can enable it */
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a1));
+	disable_irq(gpio_to_irq(ddata->pdata.irq_a2));
+
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ddata->early_suspend.level =
+			EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ddata->early_suspend.suspend = lsm303dlh_a_early_suspend;
+	ddata->early_suspend.resume = lsm303dlh_a_late_resume;
+	register_early_suspend(&ddata->early_suspend);
+#endif
+
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+	return ret;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+err_input_failed:
+	input_unregister_device(ddata->input_dev2);
+err_input_register_failed2:
+	input_unregister_device(ddata->input_dev);
+err_input_register_failed:
+	input_free_device(ddata->input_dev2);
+err_input_alloc_failed:
+	input_free_device(ddata->input_dev);
+#endif
+exit_free_regulator:
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		regulator_put(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+
+err_op_failed:
+	kfree(ddata);
+err_alloc:
+	dev_err(&client->dev, "probe function fails %x", ret);
+	return ret;
+}
+
+static int __devexit lsm303dlh_a_remove(struct i2c_client *client)
+{
+	int ret;
+	struct lsm303dlh_a_data *ddata;
+
+	ddata = i2c_get_clientdata(client);
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	input_unregister_device(ddata->input_dev);
+	input_unregister_device(ddata->input_dev2);
+	input_free_device(ddata->input_dev);
+	input_free_device(ddata->input_dev2);
+#endif
+	sysfs_remove_group(&client->dev.kobj, &lsm303dlh_a_attr_group);
+
+	/* safer to make device off */
+	if (ddata->mode != LSM303DLH_A_MODE_OFF) {
+		ret = lsm303dlh_a_write(ddata, CTRL_REG1, 0, "CONTROL");
+
+		if (ret < 0) {
+			dev_err(&client->dev, "could not turn off the device %d", ret);
+			return ret;
+		}
+
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			regulator_put(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+
+	i2c_set_clientdata(client, NULL);
+	kfree(ddata);
+
+	return 0;
+}
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#ifdef CONFIG_PM
+static int lsm303dlh_a_suspend(struct device *dev)
+{
+	struct lsm303dlh_a_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = lsm303dlh_a_do_suspend(ddata);
+
+	return ret;
+}
+
+static int lsm303dlh_a_resume(struct device *dev)
+{
+	struct lsm303dlh_a_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret	= lsm303dlh_a_restore(ddata);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device");
+
+	return ret;
+}
+static const struct dev_pm_ops lsm303dlh_a_dev_pm_ops = {
+	.suspend = lsm303dlh_a_suspend,
+	.resume  = lsm303dlh_a_resume,
+};
+#endif
+#else
+static void lsm303dlh_a_early_suspend(struct early_suspend *data)
+{
+	struct lsm303dlh_a_data *ddata =
+		container_of(data, struct lsm303dlh_a_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_a_do_suspend(ddata);
+}
+
+static void lsm303dlh_a_late_resume(struct early_suspend *data)
+{
+	struct lsm303dlh_a_data *ddata =
+		container_of(data, struct lsm303dlh_a_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_a_restore(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"lsm303dlh_a late resume failed\n");
+}
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id lsm303dlh_a_id[] = {
+	{ "lsm303dlh_a", 0 },
+	{ },
+};
+
+static struct i2c_driver lsm303dlh_a_driver = {
+	.probe		= lsm303dlh_a_probe,
+	.remove		= lsm303dlh_a_remove,
+	.id_table	= lsm303dlh_a_id,
+	.driver = {
+		.name = "lsm303dlh_a",
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+		.pm = &lsm303dlh_a_dev_pm_ops,
+#endif
+	},
+};
+
+static int __init lsm303dlh_a_init(void)
+{
+	return i2c_add_driver(&lsm303dlh_a_driver);
+}
+
+static void __exit lsm303dlh_a_exit(void)
+{
+	i2c_del_driver(&lsm303dlh_a_driver);
+}
+
+module_init(lsm303dlh_a_init)
+module_exit(lsm303dlh_a_exit)
+
+MODULE_DESCRIPTION("lSM303DLH 3-Axis Accelerometer Driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("STMicroelectronics");
diff --git a/drivers/hwmon/lsm303dlh_m.c b/drivers/hwmon/lsm303dlh_m.c
new file mode 100644
index 00000000000..270e532f78b
--- /dev/null
+++ b/drivers/hwmon/lsm303dlh_m.c
@@ -0,0 +1,928 @@
+/*
+ * lsm303dlh_m.c
+ * ST 3-Axis Magnetometer Driver
+ *
+ * Copyright (C) 2010 STMicroelectronics
+ * Author: Carmine Iascone (carmine.iascone@st.com)
+ * Author: Matteo Dameno (matteo.dameno@st.com)
+ *
+ * Copyright (C) 2010 STEricsson
+ * Author: Mian Yousaf Kaukab <mian.yousaf.kaukab@stericsson.com>
+ * Updated:Preetham Rao Kaskurthi <preetham.rao@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <mach/gpio.h>
+#endif
+
+#include <linux/lsm303dlh.h>
+#include <linux/regulator/consumer.h>
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+#include <linux/kernel.h>
+
+/* lsm303dlh magnetometer registers */
+#define IRA_REG_M	0x0A
+
+/* Magnetometer registers */
+#define CRA_REG_M	0x00  /* Configuration register A */
+#define CRB_REG_M	0x01  /* Configuration register B */
+#define MR_REG_M	0x02  /* Mode register */
+#define SR_REG_M    0x09  /* Status register */
+
+/* Output register start address*/
+#define OUT_X_M		0x03
+#define OUT_Y_M		0x05
+#define OUT_Z_M		0x07
+
+/* Magnetometer X-Y gain  */
+#define XY_GAIN_1_3	1055 /* XY gain at 1.3G */
+#define XY_GAIN_1_9	 795 /* XY gain at 1.9G */
+#define XY_GAIN_2_5	 635 /* XY gain at 2.5G */
+#define XY_GAIN_4_0	 430 /* XY gain at 4.0G */
+#define XY_GAIN_4_7	 375 /* XY gain at 4.7G */
+#define XY_GAIN_5_6	 320 /* XY gain at 5.6G */
+#define XY_GAIN_8_1	 230 /* XY gain at 8.1G */
+
+/* Magnetometer Z gain  */
+#define Z_GAIN_1_3	950 /* Z gain at 1.3G */
+#define Z_GAIN_1_9	710 /* Z gain at 1.9G */
+#define Z_GAIN_2_5	570 /* Z gain at 2.5G */
+#define Z_GAIN_4_0	385 /* Z gain at 4.0G */
+#define Z_GAIN_4_7	335 /* Z gain at 4.7G */
+#define Z_GAIN_5_6	285 /* Z gain at 5.6G */
+#define Z_GAIN_8_1	205 /* Z gain at 8.1G */
+
+/* Control A regsiter. */
+#define LSM303DLH_M_CRA_DO_BIT 2
+#define LSM303DLH_M_CRA_DO_MASK (0x7 <<  LSM303DLH_M_CRA_DO_BIT)
+#define LSM303DLH_M_CRA_MS_BIT 0
+#define LSM303DLH_M_CRA_MS_MASK (0x3 <<  LSM303DLH_M_CRA_MS_BIT)
+
+/* Control B regsiter. */
+#define LSM303DLH_M_CRB_GN_BIT 5
+#define LSM303DLH_M_CRB_GN_MASK (0x7 <<  LSM303DLH_M_CRB_GN_BIT)
+
+/* Control Mode regsiter. */
+#define LSM303DLH_M_MR_MD_BIT 0
+#define LSM303DLH_M_MR_MD_MASK (0x3 <<  LSM303DLH_M_MR_MD_BIT)
+
+/* Control Status regsiter. */
+#define LSM303DLH_M_SR_RDY_BIT 0
+#define LSM303DLH_M_SR_RDY_MASK (0x1 <<  LSM303DLH_M_SR_RDY_BIT)
+#define LSM303DLH_M_SR_LOC_BIT 1
+#define LSM303DLH_M_SR_LCO_MASK (0x1 <<  LSM303DLH_M_SR_LOC_BIT)
+#define LSM303DLH_M_SR_REN_BIT 2
+#define LSM303DLH_M_SR_REN_MASK (0x1 <<  LSM303DLH_M_SR_REN_BIT)
+
+/* Magnetometer gain setting */
+#define LSM303DLH_M_RANGE_1_3G  0x01
+#define LSM303DLH_M_RANGE_1_9G  0x02
+#define LSM303DLH_M_RANGE_2_5G  0x03
+#define LSM303DLH_M_RANGE_4_0G  0x04
+#define LSM303DLH_M_RANGE_4_7G  0x05
+#define LSM303DLH_M_RANGE_5_6G  0x06
+#define LSM303DLH_M_RANGE_8_1G  0x07
+
+/* Magnetometer capturing mode  */
+#define LSM303DLH_M_MODE_CONTINUOUS 0
+#define LSM303DLH_M_MODE_SINGLE 1
+#define LSM303DLH_M_MODE_SLEEP 3
+
+/* Magnetometer output data rate */
+#define LSM303DLH_M_RATE_00_75 0x00
+#define LSM303DLH_M_RATE_01_50 0x01
+#define LSM303DLH_M_RATE_03_00 0x02
+#define LSM303DLH_M_RATE_07_50 0x03
+#define LSM303DLH_M_RATE_15_00 0x04
+#define LSM303DLH_M_RATE_30_00 0x05
+#define LSM303DLH_M_RATE_75_00 0x06
+
+#ifdef CONFIG_SENSORS_LSM303DLHC
+#define LSM303DLH_M_RATE_220_00 0x07
+#endif
+
+/* Multiple byte transfer enable */
+#define MULTIPLE_I2C_TR 0x80
+
+/* device status defines */
+#define DEVICE_OFF 0
+#define DEVICE_ON 1
+#define DEVICE_SUSPENDED 2
+
+/* device CHIP ID defines */
+#define LSM303DLHC_CHIP_ID 51
+
+/**
+ * struct lsm303dlh_m_data - data structure used by lsm303dlh_m driver
+ * @client: i2c client
+ * @lock: mutex lock for sysfs operations
+ * @input_dev: input device
+ * @regulator: regulator
+ * @pdata: lsm303dlh platform data
+ * @gain: x, y and z axes gain
+ * @data: Magnetic field values of x, y and z axes
+ * @mode: current mode of operation
+ * @rate: current sampling rate
+ * @range: current range value of magnetometer
+ * @early_suspend: early suspend structure
+ * @device_status: device is ON, OFF or SUSPENDED
+ */
+struct lsm303dlh_m_data {
+	struct i2c_client *client;
+	/* lock for sysfs operations */
+	struct mutex lock;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	struct input_dev *input_dev;
+#endif
+	struct regulator *regulator;
+	struct lsm303dlh_platform_data pdata;
+
+	short gain[3];
+	short data[3];
+	unsigned char mode;
+	unsigned char rate;
+	unsigned char range;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+	int device_status;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void lsm303dlh_m_early_suspend(struct early_suspend *data);
+static void lsm303dlh_m_late_resume(struct early_suspend *data);
+#endif
+
+static int lsm303dlh_m_set_mode(struct lsm303dlh_m_data *ddata,
+		unsigned char mode);
+static int lsm303dlh_m_write(struct lsm303dlh_m_data *ddata,
+		u8 reg, u8 val, char *msg)
+{
+	int ret = i2c_smbus_write_byte_data(ddata->client, reg, val);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_write_byte_data failed error %d\
+			Register (%s)\n", ret, msg);
+	return ret;
+}
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_PM)
+static int lsm303dlh_m_do_suspend(struct lsm303dlh_m_data *ddata)
+{
+	int ret;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	disable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+	ret = lsm303dlh_m_set_mode(ddata, LSM303DLH_M_MODE_SLEEP);
+
+	if (ddata->regulator)
+		regulator_disable(ddata->regulator);
+
+	ddata->device_status = DEVICE_SUSPENDED;
+
+	mutex_unlock(&ddata->lock);
+
+	return ret;
+}
+
+static int lsm303dlh_m_restore(struct lsm303dlh_m_data *ddata)
+{
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_ON) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+	/* in correct mode, no need to change it */
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		ddata->device_status = DEVICE_OFF;
+		mutex_unlock(&ddata->lock);
+		return 0;
+	} else
+		 ddata->device_status = DEVICE_ON;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	enable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+	if (ddata->regulator)
+		regulator_enable(ddata->regulator);
+
+	ret = lsm303dlh_m_write(ddata, CRB_REG_M, ddata->range, "SET RANGE");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_m_write(ddata, CRA_REG_M, ddata->rate, "SET RATE");
+
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlh_m_set_mode(ddata, ddata->mode);
+
+	if (ret < 0)
+		goto fail;
+
+fail:
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+#endif
+
+static int lsm303dlh_m_read_multi(struct lsm303dlh_m_data *ddata, u8 reg,
+		u8 count, u8 *val, char *msg)
+{
+	int ret = i2c_smbus_read_i2c_block_data(ddata->client,
+		   reg | MULTIPLE_I2C_TR, count, val);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_read_i2c_block_data failed error %d\
+			 Register (%s)\n", ret, msg);
+	return ret;
+}
+
+static ssize_t lsm303dlh_m_show_rate(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->rate >> LSM303DLH_M_CRA_DO_BIT);
+}
+
+/* set lsm303dlh magnetometer bandwidth */
+static ssize_t lsm303dlh_m_store_rate(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+	unsigned long val;
+	unsigned char data;
+	int error;
+
+	error = strict_strtoul(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	data = ((val << LSM303DLH_M_CRA_DO_BIT) & LSM303DLH_M_CRA_DO_MASK);
+	ddata->rate = data;
+
+	error = lsm303dlh_m_write(ddata, CRA_REG_M, data, "SET RATE");
+
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static int lsm303dlh_m_xyz_read(struct lsm303dlh_m_data *ddata)
+{
+	unsigned char xyz_data[6];
+	short temp;
+	int ret = lsm303dlh_m_read_multi(ddata, OUT_X_M,
+			6, xyz_data, "OUT_X_M");
+	if (ret < 0)
+		return -EINVAL;
+
+	/* MSB is at lower address */
+	ddata->data[0] = (short)
+		(((xyz_data[0]) << 8) | xyz_data[1]);
+	ddata->data[1] = (short)
+		(((xyz_data[2]) << 8) | xyz_data[3]);
+	ddata->data[2] = (short)
+		(((xyz_data[4]) << 8) | xyz_data[5]);
+
+	/* check if chip is DHLC */
+	if (ddata->pdata.chip_id == LSM303DLHC_CHIP_ID) {
+		/*
+		 * the out registers are in x, z and y order
+		 * so swap y and z values
+		 */
+		temp = ddata->data[1];
+		ddata->data[1] = ddata->data[2];
+		ddata->data[2] = temp;
+	}
+	/* taking orientation of x,y,z axis into account*/
+
+	ddata->data[ddata->pdata.axis_map_x] = ddata->pdata.negative_x ?
+		-ddata->data[ddata->pdata.axis_map_x] :
+		ddata->data[ddata->pdata.axis_map_x];
+	ddata->data[ddata->pdata.axis_map_y] = ddata->pdata.negative_y ?
+		-ddata->data[ddata->pdata.axis_map_y] :
+		ddata->data[ddata->pdata.axis_map_y];
+	ddata->data[ddata->pdata.axis_map_z] = ddata->pdata.negative_z ?
+		-ddata->data[ddata->pdata.axis_map_z] :
+		ddata->data[ddata->pdata.axis_map_z];
+
+	return ret;
+}
+
+static ssize_t lsm303dlh_m_gain(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%8x:%8x:%8x\n",
+			ddata->gain[ddata->pdata.axis_map_x],
+			ddata->gain[ddata->pdata.axis_map_y],
+			ddata->gain[ddata->pdata.axis_map_z]);
+}
+
+static ssize_t lsm303dlh_m_values(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP ||
+			ddata->device_status == DEVICE_SUSPENDED) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	ret = lsm303dlh_m_xyz_read(ddata);
+
+	if (ret < 0) {
+		mutex_unlock(&ddata->lock);
+		return -EINVAL;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	/* taking orientation of x,y,z axis into account*/
+
+	return sprintf(buf, "%8x:%8x:%8x\n",
+			ddata->data[ddata->pdata.axis_map_x],
+			ddata->data[ddata->pdata.axis_map_y],
+			ddata->data[ddata->pdata.axis_map_z]);
+}
+
+static int lsm303dlh_m_set_mode(struct lsm303dlh_m_data *ddata,
+		unsigned char mode)
+{
+	int ret;
+
+	mode = (mode << LSM303DLH_M_MR_MD_BIT);
+
+	ret = i2c_smbus_write_byte_data(ddata->client, MR_REG_M, mode);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_write_byte_data failed error %d\
+			Register (%s)\n", ret, "MODE CONTROL");
+
+	return ret;
+}
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+
+static irqreturn_t lsm303dlh_m_gpio_irq(int irq, void *device_data)
+{
+	struct lsm303dlh_m_data *ddata = device_data;
+	int ret;
+
+	ret = lsm303dlh_m_xyz_read(ddata);
+
+	if (ret < 0) {
+		dev_err(&ddata->client->dev,
+				"reading data of xyz failed error %d\n", ret);
+		return IRQ_NONE;
+	}
+
+	/* taking orientation of x,y,z axis into account*/
+
+	input_report_abs(ddata->input_dev, ABS_X,
+			ddata->data[ddata->pdata.axis_map_x]);
+	input_report_abs(ddata->input_dev, ABS_Y,
+		   ddata->data[ddata->pdata.axis_map_y]);
+	input_report_abs(ddata->input_dev, ABS_Z,
+		   ddata->data[ddata->pdata.axis_map_z]);
+	input_sync(ddata->input_dev);
+
+	return IRQ_HANDLED;
+
+}
+#endif
+
+static ssize_t lsm303dlh_m_show_range(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->range >> LSM303DLH_M_CRB_GN_BIT);
+}
+
+static ssize_t lsm303dlh_m_store_range(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+	short xy_gain;
+	short z_gain;
+	unsigned long range;
+	int error;
+
+	error = strict_strtoul(buf, 0, &range);
+
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	switch (range) {
+	case LSM303DLH_M_RANGE_1_3G:
+		xy_gain = XY_GAIN_1_3;
+		z_gain = Z_GAIN_1_3;
+		break;
+	case LSM303DLH_M_RANGE_1_9G:
+		xy_gain = XY_GAIN_1_9;
+		z_gain = Z_GAIN_1_9;
+		break;
+	case LSM303DLH_M_RANGE_2_5G:
+		xy_gain = XY_GAIN_2_5;
+		z_gain = Z_GAIN_2_5;
+		break;
+	case LSM303DLH_M_RANGE_4_0G:
+		xy_gain = XY_GAIN_4_0;
+		z_gain = Z_GAIN_4_0;
+		break;
+	case LSM303DLH_M_RANGE_4_7G:
+		xy_gain = XY_GAIN_4_7;
+		z_gain = Z_GAIN_4_7;
+		break;
+	case LSM303DLH_M_RANGE_5_6G:
+		xy_gain = XY_GAIN_5_6;
+		z_gain = Z_GAIN_5_6;
+		break;
+	case LSM303DLH_M_RANGE_8_1G:
+		xy_gain = XY_GAIN_8_1;
+		z_gain = Z_GAIN_8_1;
+		break;
+	default:
+		mutex_unlock(&ddata->lock);
+		return -EINVAL;
+	}
+
+	ddata->gain[ddata->pdata.axis_map_x] = xy_gain;
+	ddata->gain[ddata->pdata.axis_map_y] = xy_gain;
+	ddata->gain[ddata->pdata.axis_map_z] = z_gain;
+
+	range <<= LSM303DLH_M_CRB_GN_BIT;
+	range &= LSM303DLH_M_CRB_GN_MASK;
+
+	ddata->range = range;
+
+	error = lsm303dlh_m_write(ddata, CRB_REG_M, range, "SET RANGE");
+	mutex_unlock(&ddata->lock);
+
+	if (error < 0)
+		return error;
+
+	return count;
+}
+
+static ssize_t lsm303dlh_m_show_mode(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->mode);
+}
+
+static ssize_t lsm303dlh_m_store_mode(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlh_m_data *ddata = platform_get_drvdata(pdev);
+	unsigned long mode;
+	int error;
+
+	error = strict_strtoul(buf, 0, &mode);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_SUSPENDED &&
+			mode == LSM303DLH_M_MODE_SLEEP) {
+		ddata->mode = (mode >> LSM303DLH_M_MR_MD_BIT);
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/*  if same mode as existing, return */
+	if (ddata->mode == mode) {
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* turn on the supplies if already off */
+	if (ddata->mode == LSM303DLH_M_MODE_SLEEP && ddata->regulator
+			&& (ddata->device_status == DEVICE_OFF
+				|| ddata->device_status == DEVICE_SUSPENDED)) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+		enable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+	}
+
+	error = lsm303dlh_m_set_mode(ddata, mode);
+
+	ddata->mode = (mode >> LSM303DLH_M_MR_MD_BIT);
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	if (mode == LSM303DLH_M_MODE_SLEEP) {
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+		disable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+		/*
+		 * No need to store context here, it is not like
+		 * suspend/resume but fall back to default values
+		 */
+		ddata->rate = LSM303DLH_M_RATE_00_75;
+		ddata->range = LSM303DLH_M_RANGE_1_3G;
+		ddata->range <<= LSM303DLH_M_CRB_GN_BIT;
+		ddata->range &= LSM303DLH_M_CRB_GN_MASK;
+		ddata->gain[ddata->pdata.axis_map_x] = XY_GAIN_1_3;
+		ddata->gain[ddata->pdata.axis_map_y] = XY_GAIN_1_3;
+		ddata->gain[ddata->pdata.axis_map_z] = Z_GAIN_1_3;
+
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static DEVICE_ATTR(gain, S_IRUGO, lsm303dlh_m_gain, NULL);
+
+static DEVICE_ATTR(data, S_IRUGO, lsm303dlh_m_values, NULL);
+
+static DEVICE_ATTR(mode, S_IWUSR | S_IRUGO,
+		lsm303dlh_m_show_mode, lsm303dlh_m_store_mode);
+
+static DEVICE_ATTR(range, S_IWUSR | S_IRUGO,
+		lsm303dlh_m_show_range, lsm303dlh_m_store_range);
+
+static DEVICE_ATTR(rate, S_IWUSR | S_IRUGO,
+		lsm303dlh_m_show_rate, lsm303dlh_m_store_rate);
+
+static struct attribute *lsm303dlh_m_attributes[] = {
+	&dev_attr_gain.attr,
+	&dev_attr_data.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_range.attr,
+	&dev_attr_rate.attr,
+	NULL
+};
+
+static const struct attribute_group lsm303dlh_m_attr_group = {
+	.attrs = lsm303dlh_m_attributes,
+};
+
+static int __devinit lsm303dlh_m_probe(struct i2c_client *client,
+						const struct i2c_device_id *id)
+{
+	int ret;
+	struct lsm303dlh_m_data *ddata = NULL;
+	unsigned char version[3];
+
+	ddata = kzalloc(sizeof(struct lsm303dlh_m_data), GFP_KERNEL);
+	if (ddata == NULL) {
+		dev_err(&client->dev, "memory alocation failed\n");
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	ddata->client = client;
+	i2c_set_clientdata(client, ddata);
+
+	/* copy platform specific data */
+	memcpy(&ddata->pdata, client->dev.platform_data, sizeof(ddata->pdata));
+
+	ddata->mode = LSM303DLH_M_MODE_SLEEP;
+	ddata->rate = LSM303DLH_M_RATE_00_75;
+	ddata->range = LSM303DLH_M_RANGE_1_3G;
+	ddata->range <<= LSM303DLH_M_CRB_GN_BIT;
+	ddata->range &= LSM303DLH_M_CRB_GN_MASK;
+	ddata->gain[ddata->pdata.axis_map_x] = XY_GAIN_1_3;
+	ddata->gain[ddata->pdata.axis_map_y] = XY_GAIN_1_3;
+	ddata->gain[ddata->pdata.axis_map_z] = Z_GAIN_1_3;
+	ddata->device_status = DEVICE_OFF;
+	dev_set_name(&client->dev, ddata->pdata.name_m);
+
+	ddata->regulator = regulator_get(&client->dev, "vdd");
+	if (IS_ERR(ddata->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		ret = PTR_ERR(ddata->regulator);
+		ddata->regulator = NULL;
+		goto err_op_failed;
+	}
+
+	if (ddata->regulator) {
+		/*
+		 * 0.83 milliamps typical with magnetic sensor setting ODR =
+		 * 7.5 Hz, Accelerometer sensor ODR = 50 Hz.  Double for
+		 * safety.
+		 */
+		regulator_set_optimum_mode(ddata->regulator, 830 * 2);
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+	}
+
+	ret = lsm303dlh_m_read_multi(ddata, IRA_REG_M, 3, version, "IRA_REG_M");
+	if (ret < 0)
+		goto exit_free_regulator;
+
+	dev_info(&client->dev, "Magnetometer, ID : %x:%x:%x",
+			version[0], version[1], version[2]);
+
+	mutex_init(&ddata->lock);
+
+	ret = sysfs_create_group(&client->dev.kobj, &lsm303dlh_m_attr_group);
+	if (ret)
+		goto exit_free_regulator;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+
+	ddata->input_dev = input_allocate_device();
+	if (!ddata->input_dev) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		goto exit_free_regulator;
+	}
+
+	set_bit(EV_ABS, ddata->input_dev->evbit);
+
+	/* x-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_X, -32768, 32767, 0, 0);
+	/* y-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_Y, -32768, 32767, 0, 0);
+	/* z-axis acceleration */
+	input_set_abs_params(ddata->input_dev, ABS_Z, -32768, 32767, 0, 0);
+
+	ddata->input_dev->name = "magnetometer";
+
+	ret = input_register_device(ddata->input_dev);
+	if (ret) {
+		dev_err(&client->dev, "Unable to register input device: %s\n",
+				ddata->input_dev->name);
+		goto err_input_register_failed;
+	}
+
+	/* register interrupt */
+	ret = request_threaded_irq(gpio_to_irq(ddata->pdata.irq_m), NULL,
+		   lsm303dlh_m_gpio_irq,
+		   IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING, "lsm303dlh_m",
+		   ddata);
+	if (ret) {
+		dev_err(&client->dev, "request irq EGPIO_PIN_1 failed\n");
+		goto err_input_failed;
+	}
+
+	disable_irq(gpio_to_irq(ddata->pdata.irq_m));
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ddata->early_suspend.level =
+				EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ddata->early_suspend.suspend = lsm303dlh_m_early_suspend;
+	ddata->early_suspend.resume = lsm303dlh_m_late_resume;
+	register_early_suspend(&ddata->early_suspend);
+#endif
+
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+
+	return ret;
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+err_input_failed:
+	input_unregister_device(ddata->input_dev);
+err_input_register_failed:
+	input_free_device(ddata->input_dev);
+#endif
+exit_free_regulator:
+	if (ddata->device_status == DEVICE_ON && ddata->regulator) {
+		regulator_disable(ddata->regulator);
+		regulator_put(ddata->regulator);
+		ddata->device_status = DEVICE_OFF;
+	}
+
+err_op_failed:
+	kfree(ddata);
+err_alloc:
+	dev_err(&client->dev, "lsm303dlh_m_probe failed %x", ret);
+	return ret;
+}
+
+static int __devexit lsm303dlh_m_remove(struct i2c_client *client)
+{
+	struct lsm303dlh_m_data *ddata;
+
+	ddata = i2c_get_clientdata(client);
+
+#ifdef CONFIG_SENSORS_LSM303DLH_INPUT_DEVICE
+	input_unregister_device(ddata->input_dev);
+	input_free_device(ddata->input_dev);
+#endif
+
+	sysfs_remove_group(&client->dev.kobj, &lsm303dlh_m_attr_group);
+
+	/* safer to make device off */
+	if (ddata->mode != LSM303DLH_M_MODE_SLEEP) {
+		lsm303dlh_m_set_mode(ddata, LSM303DLH_M_MODE_SLEEP);
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			regulator_put(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+
+	i2c_set_clientdata(client, NULL);
+	kfree(ddata);
+
+	return 0;
+}
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#ifdef CONFIG_PM
+static int lsm303dlh_m_suspend(struct device *dev)
+{
+	struct lsm303dlh_m_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = lsm303dlh_m_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device");
+
+	return ret;
+}
+
+static int lsm303dlh_m_resume(struct device *dev)
+{
+	struct lsm303dlh_m_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = lsm303dlh_m_restore(ddata);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device");
+
+	return ret;
+}
+static const struct dev_pm_ops lsm303dlh_m_dev_pm_ops = {
+	.suspend = lsm303dlh_m_suspend,
+	.resume  = lsm303dlh_m_resume,
+};
+#endif
+#else
+static void lsm303dlh_m_early_suspend(struct early_suspend *data)
+{
+	struct lsm303dlh_m_data *ddata =
+		container_of(data, struct lsm303dlh_m_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_m_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device");
+}
+
+static void lsm303dlh_m_late_resume(struct early_suspend *data)
+{
+	struct lsm303dlh_m_data *ddata =
+		container_of(data, struct lsm303dlh_m_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlh_m_restore(ddata);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"lsm303dlh_m late resume failed\n");
+}
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id lsm303dlh_m_id[] = {
+	{ "lsm303dlh_m", 0 },
+	{ },
+};
+
+static struct i2c_driver lsm303dlh_m_driver = {
+	.probe		= lsm303dlh_m_probe,
+	.remove		= lsm303dlh_m_remove,
+	.id_table	= lsm303dlh_m_id,
+	.driver = {
+		.name = "lsm303dlh_m",
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+		.pm = &lsm303dlh_m_dev_pm_ops,
+#endif
+	},
+};
+
+static int __init lsm303dlh_m_init(void)
+{
+	return i2c_add_driver(&lsm303dlh_m_driver);
+}
+
+static void __exit lsm303dlh_m_exit(void)
+{
+	i2c_del_driver(&lsm303dlh_m_driver);
+}
+
+module_init(lsm303dlh_m_init);
+module_exit(lsm303dlh_m_exit);
+
+MODULE_DESCRIPTION("lSM303DLH 3-Axis Magnetometer Driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("STMicroelectronics");
diff --git a/drivers/hwmon/lsm303dlhc_a.c b/drivers/hwmon/lsm303dlhc_a.c
new file mode 100644
index 00000000000..cf00ce9d50b
--- /dev/null
+++ b/drivers/hwmon/lsm303dlhc_a.c
@@ -0,0 +1,708 @@
+/*
+ * ST LSM303DLHC 3-Axis Accelerometer Driver
+ *
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Chethan Krishna N <chethan.krishna@stericsson.com> for ST-Ericsson
+ * Licence terms: GNU General Public Licence (GPL) version 2
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+
+#include <linux/lsm303dlh.h>
+#include <linux/regulator/consumer.h>
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+#define WHO_AM_I    0x0F
+
+/* lsm303dlhc accelerometer registers */
+#define CTRL_REG1				0x20
+#define CTRL_REG2				0x21
+#define CTRL_REG3				0x22
+#define CTRL_REG4				0x23
+#define CTRL_REG5				0x24
+#define CTRL_REG6				0x25
+
+/* lsm303dlhc accelerometer defines */
+#define LSM303DLHC_A_MODE_OFF		0x00
+#define LSM303DLHC_A_MODE_ON		0x04
+#define LSM303DLHC_A_MODE_MAX		0x09
+#define LSM303DLHC_A_CR1_MODE_BIT	4
+#define LSM303DLHC_A_CR1_MODE_MASK	(0xF << LSM303DLHC_A_CR1_MODE_BIT)
+ #define LSM303DLHC_A_CR1_AXIS_ENABLE 7
+
+/* Range */
+#define LSM303DLHC_A_RANGE_2G 0x00
+#define LSM303DLHC_A_RANGE_4G 0x01
+#define LSM303DLHC_A_RANGE_8G 0x02
+#define LSM303DLHC_A_RANGE_16G 0x03
+#define LSM303DLHC_A_CR4_FS_BIT 4
+
+/* Sensitivity adjustment */
+#define SHIFT_ADJ_2G 4 /*    1/16*/
+#define SHIFT_ADJ_4G 3 /*    2/16*/
+#define SHIFT_ADJ_8G 2 /* ~3.9/16*/
+#define SHIFT_ADJ_16G 1 /* ~3.9/16*/
+
+#define AXISDATA_REG    0x28    /* axis data */
+
+/* lsm303dlh magnetometer registers */
+#define IRA_REG_M	0x0A
+
+/* multiple byte transfer enable */
+#define MULTIPLE_I2C_TR			0x80
+
+/* device status defines */
+#define DEVICE_OFF 0
+#define DEVICE_ON 1
+#define DEVICE_SUSPENDED 2
+
+struct lsm303dlhc_a_t {
+	short	x;
+	short	y;
+	short	z;
+};
+
+/**
+ * struct lsm303dlhc_a_data - data structure used by lsm303dlhc_a driver
+ * @client: i2c client
+ * @lock: mutex lock for sysfs operations
+ * @data: lsm303dlhc_a_t struct containing x, y and z values
+ * @pdata: lsm303dlh platform data
+ * @regulator: regulator
+ * @range: current range value of accelerometer
+ * @mode: current mode of operation
+ * @rate: current sampling rate
+ * @shift_adjust: current shift adjust value set according to range
+ * @early_suspend: early suspend structure
+ * @device_status: device is ON, OFF or SUSPENDED
+ * @id: accelerometer device id
+ */
+struct lsm303dlhc_a_data {
+	struct i2c_client *client;
+	/* lock for sysfs operations */
+	struct mutex lock;
+	struct lsm303dlhc_a_t data;
+	struct lsm303dlh_platform_data pdata;
+	struct regulator *regulator;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+	unsigned char range;
+	unsigned char mode;
+	unsigned char rate;
+	int shift_adjust;
+	int device_status;
+	int id;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void lsm303dlhc_a_early_suspend(struct early_suspend *data);
+static void lsm303dlhc_a_late_resume(struct early_suspend *data);
+#endif
+
+static int lsm303dlhc_a_write(struct lsm303dlhc_a_data *ddata, u8 reg,
+		u8 val, char *msg)
+{
+	int ret = i2c_smbus_write_byte_data(ddata->client, reg, val);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_write_byte_data failed error %d\
+			Register (%s)\n", ret, msg);
+	return ret;
+}
+
+static int lsm303dlhc_a_read(struct lsm303dlhc_a_data *ddata, u8 reg, char *msg)
+{
+	int ret = i2c_smbus_read_byte_data(ddata->client, reg);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+			"i2c_smbus_read_byte_data failed error %d\
+			 Register (%s)\n", ret, msg);
+	return ret;
+}
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_PM)
+static int lsm303dlhc_a_do_suspend(struct lsm303dlhc_a_data *ddata)
+{
+	int ret;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLHC_A_MODE_OFF) {
+		ret = 0;
+		goto exit;
+	}
+
+	ret = lsm303dlhc_a_write(ddata, CTRL_REG1,
+			LSM303DLHC_A_MODE_OFF, "CONTROL");
+
+	if (ddata->regulator)
+		regulator_disable(ddata->regulator);
+
+	ddata->device_status = DEVICE_SUSPENDED;
+
+exit:
+	mutex_unlock(&ddata->lock);
+
+	return ret;
+}
+
+static int lsm303dlhc_a_restore(struct lsm303dlhc_a_data *ddata)
+{
+	unsigned char reg;
+	unsigned char shifted_mode = (ddata->mode << LSM303DLHC_A_CR1_MODE_BIT);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->device_status == DEVICE_ON) {
+		mutex_unlock(&ddata->lock);
+		return 0;
+	}
+
+	/* in correct mode, no need to change it */
+	if (ddata->mode == LSM303DLHC_A_MODE_OFF) {
+		ddata->device_status = DEVICE_OFF;
+		goto fail;
+	} else
+		ddata->device_status = DEVICE_ON;
+
+	if (ddata->regulator)
+		regulator_enable(ddata->regulator);
+
+	/* BDU should be enabled by default/recommened */
+	reg = ddata->range;
+	shifted_mode |= LSM303DLHC_A_CR1_AXIS_ENABLE;
+
+	ret = lsm303dlhc_a_write(ddata, CTRL_REG1, shifted_mode,
+			"CTRL_REG1");
+	if (ret < 0)
+		goto fail;
+
+	ret = lsm303dlhc_a_write(ddata, CTRL_REG4, reg, "CTRL_REG4");
+
+	if (ret < 0)
+		goto fail;
+
+	/* write to the boot bit to reboot memory content */
+	ret = lsm303dlhc_a_write(ddata, CTRL_REG5, 0x80, "CTRL_REG5");
+
+	if (ret < 0)
+		goto fail;
+
+fail:
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"could not restore the device %d\n", ret);
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+#endif
+
+static int lsm303dlhc_a_readdata(struct lsm303dlhc_a_data *ddata)
+{
+	unsigned char acc_data[6];
+	short data[3];
+
+	int ret = i2c_smbus_read_i2c_block_data(ddata->client,
+			AXISDATA_REG | MULTIPLE_I2C_TR, 6, acc_data);
+	if (ret < 0) {
+		dev_err(&ddata->client->dev,
+				"i2c_smbus_read_byte_data failed error %d\
+				Register AXISDATA_REG \n", ret);
+		return ret;
+	}
+
+	data[0] = (short) (((acc_data[1]) << 8) | acc_data[0]);
+	data[1] = (short) (((acc_data[3]) << 8) | acc_data[2]);
+	data[2] = (short) (((acc_data[5]) << 8) | acc_data[4]);
+
+	data[0] >>= ddata->shift_adjust;
+	data[1] >>= ddata->shift_adjust;
+	data[2] >>= ddata->shift_adjust;
+
+	/* taking position and orientation of x,y,z axis into account*/
+
+	data[ddata->pdata.axis_map_x] = ddata->pdata.negative_x ?
+		-data[ddata->pdata.axis_map_x] : data[ddata->pdata.axis_map_x];
+	data[ddata->pdata.axis_map_y] = ddata->pdata.negative_y ?
+		-data[ddata->pdata.axis_map_y] : data[ddata->pdata.axis_map_y];
+	data[ddata->pdata.axis_map_z] = ddata->pdata.negative_z ?
+		-data[ddata->pdata.axis_map_z] : data[ddata->pdata.axis_map_z];
+
+	ddata->data.x = data[ddata->pdata.axis_map_x];
+	ddata->data.y = data[ddata->pdata.axis_map_y];
+	ddata->data.z = data[ddata->pdata.axis_map_z];
+
+	return ret;
+}
+
+static ssize_t lsm303dlhc_a_show_data(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlhc_a_data *ddata = platform_get_drvdata(pdev);
+	int ret = 0;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLHC_A_MODE_OFF ||
+			ddata->device_status == DEVICE_SUSPENDED) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	ret = lsm303dlhc_a_readdata(ddata);
+
+	if (ret < 0) {
+		mutex_unlock(&ddata->lock);
+		return ret;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return sprintf(buf, "%8x:%8x:%8x\n", ddata->data.x, ddata->data.y,
+			ddata->data.z);
+}
+
+static ssize_t lsm303dlhc_a_show_range(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlhc_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->range >> LSM303DLHC_A_CR4_FS_BIT);
+}
+
+static ssize_t lsm303dlhc_a_store_range(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlhc_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	int error;
+
+	error = strict_strtol(buf, 0, &val);
+	if (error)
+		return error;
+
+	if (val < LSM303DLHC_A_RANGE_2G || val > LSM303DLHC_A_RANGE_16G)
+		return -EINVAL;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->mode == LSM303DLHC_A_MODE_OFF) {
+		dev_info(&ddata->client->dev,
+				"device is switched off,make it ON using MODE");
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	ddata->range = val;
+	ddata->range <<= LSM303DLHC_A_CR4_FS_BIT;
+
+	error = lsm303dlhc_a_write(ddata, CTRL_REG4, ddata->range,
+	"CTRL_REG4");
+	if (error < 0) {
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	switch (val) {
+	case LSM303DLHC_A_RANGE_2G:
+		ddata->shift_adjust = SHIFT_ADJ_2G;
+		break;
+	case LSM303DLHC_A_RANGE_4G:
+		ddata->shift_adjust = SHIFT_ADJ_4G;
+		break;
+	case LSM303DLHC_A_RANGE_8G:
+		ddata->shift_adjust = SHIFT_ADJ_8G;
+		break;
+	case LSM303DLHC_A_RANGE_16G:
+		ddata->shift_adjust = SHIFT_ADJ_16G;
+		break;
+	default:
+		mutex_unlock(&ddata->lock);
+		return -EINVAL;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlhc_a_show_mode(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlhc_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->mode);
+}
+
+static ssize_t lsm303dlhc_a_store_mode(struct device *dev,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlhc_a_data *ddata = platform_get_drvdata(pdev);
+	long val;
+	unsigned char data;
+	int error;
+	bool set_boot_bit = false;
+
+	error = strict_strtol(buf, 0, &val);
+	if (error)
+		return error;
+
+	mutex_lock(&ddata->lock);
+
+	/* not in correct range */
+
+	if (val < LSM303DLHC_A_MODE_OFF || val > LSM303DLHC_A_MODE_MAX) {
+		mutex_unlock(&ddata->lock);
+		return -EINVAL;
+	}
+
+	if (ddata->device_status == DEVICE_SUSPENDED) {
+		if (val == LSM303DLHC_A_MODE_OFF) {
+			ddata->mode = val;
+			mutex_unlock(&ddata->lock);
+			return count;
+		} else {
+			/* device is turning on after suspend, reset memory */
+			set_boot_bit = true;
+		}
+	}
+
+	/*  if same mode as existing, return */
+	if (ddata->mode == val) {
+		mutex_unlock(&ddata->lock);
+		return count;
+	}
+
+	/* turn on the supplies if already off */
+	if (ddata->regulator && ddata->mode == LSM303DLHC_A_MODE_OFF
+			&& (ddata->device_status == DEVICE_OFF
+				|| ddata->device_status == DEVICE_SUSPENDED)) {
+		regulator_enable(ddata->regulator);
+		ddata->device_status = DEVICE_ON;
+	}
+
+	data = lsm303dlhc_a_read(ddata, CTRL_REG1, "CTRL_REG1");
+
+	/*
+	 * If chip doesn't get reset during suspend/resume,
+	 * x,y and z axis bits are getting cleared,so set
+	 * these bits to get x,y,z data.
+	 */
+	data |= LSM303DLHC_A_CR1_AXIS_ENABLE;
+
+	data &= ~LSM303DLHC_A_CR1_MODE_MASK;
+
+	ddata->mode = val;
+
+	data |= ((val << LSM303DLHC_A_CR1_MODE_BIT)
+			& LSM303DLHC_A_CR1_MODE_MASK);
+
+	error = lsm303dlhc_a_write(ddata, CTRL_REG1, data, "CTRL_REG1");
+	if (error < 0) {
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+		mutex_unlock(&ddata->lock);
+		return error;
+	}
+
+	/*
+	 * Power on request when device is in suspended state
+	 * write to the boot bit in CTRL_REG2 to reboot memory content
+	 * and ensure correct device behavior after it resumes
+	 */
+	if (set_boot_bit) {
+		error = lsm303dlhc_a_write(ddata, CTRL_REG5, 0x80, "CTRL_REG5");
+		if (error < 0) {
+			if (ddata->regulator &&
+					ddata->device_status == DEVICE_ON) {
+				regulator_disable(ddata->regulator);
+				ddata->device_status = DEVICE_OFF;
+			}
+			mutex_unlock(&ddata->lock);
+			return error;
+		}
+	}
+
+	if (val == LSM303DLHC_A_MODE_OFF) {
+
+		/*
+		 * No need to store context here
+		 * it is not like suspend/resume
+		 * but fall back to default values
+		 */
+		ddata->range = LSM303DLHC_A_RANGE_2G;
+		ddata->shift_adjust = SHIFT_ADJ_2G;
+
+		if (ddata->regulator && ddata->device_status == DEVICE_ON) {
+			regulator_disable(ddata->regulator);
+			ddata->device_status = DEVICE_OFF;
+		}
+	}
+	mutex_unlock(&ddata->lock);
+
+	return count;
+}
+
+static ssize_t lsm303dlhc_a_show_id(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lsm303dlhc_a_data *ddata = platform_get_drvdata(pdev);
+
+	return sprintf(buf, "%d\n", ddata->id);
+}
+
+static DEVICE_ATTR(id, S_IRUGO, lsm303dlhc_a_show_id, NULL);
+
+static DEVICE_ATTR(data, S_IRUGO, lsm303dlhc_a_show_data, NULL);
+
+static DEVICE_ATTR(range, S_IWUSR | S_IRUGO,
+		lsm303dlhc_a_show_range, lsm303dlhc_a_store_range);
+
+static DEVICE_ATTR(mode, S_IWUSR | S_IRUGO,
+		lsm303dlhc_a_show_mode, lsm303dlhc_a_store_mode);
+
+static struct attribute *lsm303dlhc_a_attributes[] = {
+	&dev_attr_data.attr,
+	&dev_attr_range.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_id.attr,
+	NULL
+};
+
+static const struct attribute_group lsm303dlhc_a_attr_group = {
+	.attrs = lsm303dlhc_a_attributes,
+};
+
+static int __devinit lsm303dlhc_a_probe(struct i2c_client *client,
+						const struct i2c_device_id *id)
+{
+	int ret;
+	struct lsm303dlhc_a_data *adata = NULL;
+
+	adata = kzalloc(sizeof(struct lsm303dlhc_a_data), GFP_KERNEL);
+	if (adata == NULL) {
+		dev_err(&client->dev, "memory alocation failed\n");
+		ret = -ENOMEM;
+		goto err_alloc;
+	}
+
+	adata->client = client;
+	i2c_set_clientdata(client, adata);
+
+	/* copy platform specific data */
+	memcpy(&adata->pdata, client->dev.platform_data, sizeof(adata->pdata));
+	adata->mode = LSM303DLHC_A_MODE_OFF;
+	adata->range = LSM303DLHC_A_RANGE_2G;
+	adata->shift_adjust = SHIFT_ADJ_2G;
+	adata->device_status = DEVICE_OFF;
+	dev_set_name(&client->dev, adata->pdata.name_a);
+
+	adata->regulator = regulator_get(&client->dev, "vdd");
+	if (IS_ERR(adata->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		ret = PTR_ERR(adata->regulator);
+		adata->regulator = NULL;
+		goto err_op_failed;
+	}
+
+	if (adata->regulator) {
+		/*
+		 * 130 microamps typical with magnetic sensor setting ODR = 7.5
+		 * Hz, Accelerometer sensor ODR = 50 Hz.  Double for safety.
+		 */
+		regulator_set_optimum_mode(adata->regulator, 130 * 2);
+		regulator_enable(adata->regulator);
+		adata->device_status = DEVICE_ON;
+	}
+
+	ret = lsm303dlhc_a_read(adata, WHO_AM_I, "WHO_AM_I");
+	if (ret < 0)
+		goto exit_free_regulator;
+
+	dev_info(&client->dev, "3-Axis Accelerometer, ID : %d\n",
+			 ret);
+	adata->id = ret;
+
+	mutex_init(&adata->lock);
+
+	ret = sysfs_create_group(&client->dev.kobj, &lsm303dlhc_a_attr_group);
+	if (ret)
+		goto exit_free_regulator;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	adata->early_suspend.level =
+			EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	adata->early_suspend.suspend = lsm303dlhc_a_early_suspend;
+	adata->early_suspend.resume = lsm303dlhc_a_late_resume;
+	register_early_suspend(&adata->early_suspend);
+#endif
+
+	if (adata->device_status == DEVICE_ON && adata->regulator) {
+		regulator_disable(adata->regulator);
+		adata->device_status = DEVICE_OFF;
+	}
+
+	return ret;
+
+exit_free_regulator:
+	if (adata->device_status == DEVICE_ON && adata->regulator) {
+		regulator_disable(adata->regulator);
+		regulator_put(adata->regulator);
+		adata->device_status = DEVICE_OFF;
+	}
+
+err_op_failed:
+	kfree(adata);
+err_alloc:
+	dev_err(&client->dev, "probe function fails %x", ret);
+	return ret;
+}
+
+static int __devexit lsm303dlhc_a_remove(struct i2c_client *client)
+{
+	int ret;
+	struct lsm303dlhc_a_data *adata;
+
+	adata = i2c_get_clientdata(client);
+	sysfs_remove_group(&client->dev.kobj, &lsm303dlhc_a_attr_group);
+
+	/* safer to make device off */
+	if (adata->mode != LSM303DLHC_A_MODE_OFF) {
+		ret = lsm303dlhc_a_write(adata, CTRL_REG1, 0, "CONTROL");
+
+		if (ret < 0) {
+			dev_err(&client->dev,
+					"could not turn off the device %d",
+					ret);
+			return ret;
+		}
+
+		if (adata->regulator && adata->device_status == DEVICE_ON) {
+			regulator_disable(adata->regulator);
+			regulator_put(adata->regulator);
+			adata->device_status = DEVICE_OFF;
+		}
+	}
+
+	i2c_set_clientdata(client, NULL);
+	kfree(adata);
+
+	return 0;
+}
+
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+static int lsm303dlhc_a_suspend(struct device *dev)
+{
+	struct lsm303dlhc_a_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret = lsm303dlhc_a_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device");
+
+	return ret;
+}
+
+static int lsm303dlhc_a_resume(struct device *dev)
+{
+	struct lsm303dlhc_a_data *ddata;
+	int ret;
+
+	ddata = dev_get_drvdata(dev);
+
+	ret	= lsm303dlhc_a_restore(ddata);
+
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device");
+
+	return ret;
+}
+static const struct dev_pm_ops lsm303dlhc_a_dev_pm_ops = {
+	.suspend = lsm303dlhc_a_suspend,
+	.resume  = lsm303dlhc_a_resume,
+};
+#else
+static void lsm303dlhc_a_early_suspend(struct early_suspend *data)
+{
+	struct lsm303dlhc_a_data *ddata =
+		container_of(data, struct lsm303dlhc_a_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlhc_a_do_suspend(ddata);
+}
+
+static void lsm303dlhc_a_late_resume(struct early_suspend *data)
+{
+	struct lsm303dlhc_a_data *ddata =
+		container_of(data, struct lsm303dlhc_a_data, early_suspend);
+	int ret;
+
+	ret = lsm303dlhc_a_restore(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"lsm303dlhc_a late resume failed\n");
+}
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id lsm303dlhc_a_id[] = {
+	{ "lsm303dlhc_a", 0 },
+	{ },
+};
+
+static struct i2c_driver lsm303dlhc_a_driver = {
+	.probe		= lsm303dlhc_a_probe,
+	.remove		= lsm303dlhc_a_remove,
+	.id_table	= lsm303dlhc_a_id,
+	.driver = {
+		.name = "lsm303dlhc_a",
+	#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+		.pm	=	&lsm303dlhc_a_dev_pm_ops,
+	#endif
+	},
+};
+
+static int __init lsm303dlhc_a_init(void)
+{
+	return i2c_add_driver(&lsm303dlhc_a_driver);
+}
+
+static void __exit lsm303dlhc_a_exit(void)
+{
+	i2c_del_driver(&lsm303dlhc_a_driver);
+}
+
+module_init(lsm303dlhc_a_init)
+module_exit(lsm303dlhc_a_exit)
+
+MODULE_DESCRIPTION("lSM303DLH 3-Axis Accelerometer Driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("STMicroelectronics");
diff --git a/drivers/i2c/busses/i2c-nomadik.c b/drivers/i2c/busses/i2c-nomadik.c
index 5267ab93d55..9ddf2c97d26 100644
--- a/drivers/i2c/busses/i2c-nomadik.c
+++ b/drivers/i2c/busses/i2c-nomadik.c
@@ -431,7 +431,7 @@ static int read_i2c(struct nmk_i2c_dev *dev)
 
 	if (timeout == 0) {
 		/* Controller timed out */
-		dev_err(&dev->pdev->dev, "read from slave 0x%x timed out\n",
+		dev_err(&dev->pdev->dev, "Read from Slave 0x%x timed out\n",
 				dev->cli.slave_adr);
 		status = -ETIMEDOUT;
 	}
@@ -518,7 +518,7 @@ static int write_i2c(struct nmk_i2c_dev *dev)
 
 	if (timeout == 0) {
 		/* Controller timed out */
-		dev_err(&dev->pdev->dev, "write to slave 0x%x timed out\n",
+		dev_err(&dev->pdev->dev, "Write to slave 0x%x timed out\n",
 				dev->cli.slave_adr);
 		status = -ETIMEDOUT;
 	}
@@ -628,12 +628,8 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
 
 	dev->busy = true;
 
-	if (dev->regulator)
-		regulator_enable(dev->regulator);
 	pm_runtime_get_sync(&dev->pdev->dev);
 
-	clk_enable(dev->clk);
-
 	status = init_hw(dev);
 	if (status)
 		goto out;
@@ -666,10 +662,8 @@ static int nmk_i2c_xfer(struct i2c_adapter *i2c_adap,
 	}
 
 out:
-	clk_disable(dev->clk);
-	pm_runtime_put_sync(&dev->pdev->dev);
-	if (dev->regulator)
-		regulator_disable(dev->regulator);
+
+	pm_runtime_put(&dev->pdev->dev);
 
 	dev->busy = false;
 
@@ -859,9 +853,9 @@ static irqreturn_t i2c_irq_handler(int irq, void *arg)
 
 
 #ifdef CONFIG_PM
-static int nmk_i2c_suspend(struct device *dev)
+
+static int nmk_i2c_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	struct platform_device *pdev = to_platform_device(dev);
 	struct nmk_i2c_dev *nmk_i2c = platform_get_drvdata(pdev);
 
 	if (nmk_i2c->busy)
@@ -870,23 +864,53 @@ static int nmk_i2c_suspend(struct device *dev)
 	return 0;
 }
 
-static int nmk_i2c_resume(struct device *dev)
+static int nmk_i2c_suspend_noirq(struct device *dev)
 {
+	struct nmk_i2c_dev *nmk_i2c =
+		platform_get_drvdata(to_platform_device(dev));
+
+	if (nmk_i2c->busy)
+		return -EBUSY;
+
 	return 0;
 }
+
 #else
 #define nmk_i2c_suspend	NULL
-#define nmk_i2c_resume	NULL
+#define nmk_i2c_suspend_noirq NULL
 #endif
 
+static int nmk_i2c_runtime_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct nmk_i2c_dev *nmk_i2c = platform_get_drvdata(pdev);
+
+	clk_disable(nmk_i2c->clk);
+	if (nmk_i2c->regulator)
+		regulator_disable(nmk_i2c->regulator);
+	return 0;
+}
+
+static int nmk_i2c_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct nmk_i2c_dev *nmk_i2c = platform_get_drvdata(pdev);
+
+	if (nmk_i2c->regulator)
+		regulator_enable(nmk_i2c->regulator);
+	clk_enable(nmk_i2c->clk);
+	return 0;
+}
+
 /*
  * We use noirq so that we suspend late and resume before the wakeup interrupt
  * to ensure that we do the !pm_runtime_suspended() check in resume before
  * there has been a regular pm runtime resume (via pm_runtime_get_sync()).
  */
 static const struct dev_pm_ops nmk_i2c_pm = {
-	.suspend_noirq	= nmk_i2c_suspend,
-	.resume_noirq	= nmk_i2c_resume,
+	SET_RUNTIME_PM_OPS(nmk_i2c_runtime_suspend, nmk_i2c_runtime_resume,
+			   NULL)
+	.suspend_noirq	= nmk_i2c_suspend_noirq,
 };
 
 static unsigned int nmk_i2c_functionality(struct i2c_adapter *adap)
@@ -1047,6 +1071,7 @@ static struct platform_driver nmk_i2c_driver = {
 	},
 	.probe = nmk_i2c_probe,
 	.remove = __devexit_p(nmk_i2c_remove),
+	.suspend = nmk_i2c_suspend,
 };
 
 static int __init nmk_i2c_init(void)
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index f354813a13e..8aba6cc2bac 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -151,6 +151,16 @@ config KEYBOARD_BFIN
 	  To compile this driver as a module, choose M here: the
 	  module will be called bf54x-keys.
 
+config KEYBOARD_DB5500
+	tristate "DB5500 keyboard"
+	depends on UX500_SOC_DB5500
+	help
+	  Say Y here to enable the on-chip keypad controller on the
+	  ST-Ericsson U5500 platform.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called db5500_keypad.
+
 config KEYBOARD_LKKBD
 	tristate "DECstation/VAXstation LK201/LK401 keyboard"
 	select SERIO
@@ -381,7 +391,7 @@ config KEYBOARD_NEWTON
 	  To compile this driver as a module, choose M here: the
 	  module will be called newtonkbd.
 
-config KEYBOARD_NOMADIK
+config KEYBOARD_NOMADIK_SKE
 	tristate "ST-Ericsson Nomadik SKE keyboard"
 	depends on PLAT_NOMADIK
 	help
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index df7061f1291..90a01405e51 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -12,6 +12,7 @@ obj-$(CONFIG_KEYBOARD_ATARI)		+= atakbd.o
 obj-$(CONFIG_KEYBOARD_ATKBD)		+= atkbd.o
 obj-$(CONFIG_KEYBOARD_BFIN)		+= bf54x-keys.o
 obj-$(CONFIG_KEYBOARD_DAVINCI)		+= davinci_keyscan.o
+obj-$(CONFIG_KEYBOARD_DB5500)		+= db5500_keypad.o
 obj-$(CONFIG_KEYBOARD_EP93XX)		+= ep93xx_keypad.o
 obj-$(CONFIG_KEYBOARD_GPIO)		+= gpio_keys.o
 obj-$(CONFIG_KEYBOARD_GPIO_POLLED)	+= gpio_keys_polled.o
@@ -31,7 +32,7 @@ obj-$(CONFIG_KEYBOARD_MAX7359)		+= max7359_keypad.o
 obj-$(CONFIG_KEYBOARD_MCS)		+= mcs_touchkey.o
 obj-$(CONFIG_KEYBOARD_MPR121)		+= mpr121_touchkey.o
 obj-$(CONFIG_KEYBOARD_NEWTON)		+= newtonkbd.o
-obj-$(CONFIG_KEYBOARD_NOMADIK)		+= nomadik-ske-keypad.o
+obj-$(CONFIG_KEYBOARD_NOMADIK_SKE)	+= nomadik-ske-keypad.o
 obj-$(CONFIG_KEYBOARD_OMAP)		+= omap-keypad.o
 obj-$(CONFIG_KEYBOARD_OMAP4)		+= omap4-keypad.o
 obj-$(CONFIG_KEYBOARD_OPENCORES)	+= opencores-kbd.o
diff --git a/drivers/input/keyboard/db5500_keypad.c b/drivers/input/keyboard/db5500_keypad.c
new file mode 100644
index 00000000000..729775d99e8
--- /dev/null
+++ b/drivers/input/keyboard/db5500_keypad.c
@@ -0,0 +1,799 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License terms: GNU General Public License, version 2
+ * Author: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
+ */
+
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+#include <linux/input.h>
+#include <linux/input/matrix_keypad.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <mach/db5500-keypad.h>
+#include <linux/regulator/consumer.h>
+
+#define KEYPAD_CTR		0x0
+#define KEYPAD_IRQ_CLEAR	0x4
+#define KEYPAD_INT_ENABLE	0x8
+#define KEYPAD_INT_STATUS	0xC
+#define KEYPAD_ARRAY_01		0x18
+
+#define KEYPAD_NUM_ARRAY_REGS	5
+
+#define KEYPAD_CTR_WRITE_IRQ_ENABLE	(1 << 10)
+#define KEYPAD_CTR_WRITE_CONTROL	(1 << 8)
+#define KEYPAD_CTR_SCAN_ENABLE		(1 << 7)
+
+#define KEYPAD_ARRAY_CHANGEBIT		(1 << 15)
+
+#define KEYPAD_DEBOUNCE_PERIOD_MIN	5	/* ms */
+#define KEYPAD_DEBOUNCE_PERIOD_MAX	80	/* ms */
+
+#define KEYPAD_GND_ROW		8
+
+#define KEYPAD_ROW_SHIFT	3
+#define KEYPAD_KEYMAP_SIZE	\
+	(KEYPAD_MAX_ROWS * KEYPAD_MAX_COLS)
+
+#define	KEY_PRESSED_DELAY	10
+/**
+ * struct db5500_keypad  - data structure used by keypad driver
+ * @irq:	irq number
+ * @base:	keypad registers base address
+ * @input:	pointer to input device object
+ * @board:	keypad platform data
+ * @keymap:	matrix scan code table for keycodes
+ * @clk:	clock structure pointer
+ * @regulator : regulator used by keypad
+ * @switch_work : delayed work variable for switching to gpio
+ * @gpio_work : delayed work variable for reporting key event in gpio mode
+ * @previous_set: previous set of registers
+ * @enable : flag to enable the driver event
+ * @enable_on_resume:   set if keypad should be enabled on resume
+ * @valid_key : hold the state of valid key press
+ * @db5500_rows : rows gpio array for db5500 keypad
+ * @db5500_cols : cols gpio array for db5500 keypad
+ * @gpio_input_irq : array for gpio irqs
+ * @gpio_row : gpio row
+ * @gpio_col : gpio_col
+ */
+struct db5500_keypad {
+	int irq;
+	void __iomem *base;
+	struct input_dev *input;
+	const struct db5500_keypad_platform_data *board;
+	unsigned short keymap[KEYPAD_KEYMAP_SIZE];
+	struct clk *clk;
+	struct regulator *regulator;
+	struct delayed_work switch_work;
+	struct delayed_work gpio_work;
+	u8 previous_set[KEYPAD_MAX_ROWS];
+	bool enable;
+	bool enable_on_resume;
+	bool valid_key;
+	int db5500_rows[KEYPAD_MAX_ROWS];
+	int db5500_cols[KEYPAD_MAX_COLS];
+	int gpio_input_irq[KEYPAD_MAX_ROWS];
+	int gpio_row;
+	int gpio_col;
+};
+
+/**
+ * db5500_keypad_report() - reports the keypad event
+ * @keypad: pointer to device structure
+ * @row: row value of keypad
+ * @curr: current event
+ * @previous: previous event
+ *
+ * This function uses to reports the event of the keypad
+ * and returns NONE.
+ *
+ * By default all column reads are 1111 1111b.  Any press will pull the column
+ * down, leading to a 0 in any of these locations.  We invert these values so
+ * that a 1 means means "column pressed". *
+ * If curr changes from the previous from 0 to 1, we report it as a key press.
+ * If curr changes from the previous from 1 to 0, we report it as a key
+ * release.
+ */
+static void db5500_keypad_report(struct db5500_keypad *keypad, int row,
+				 u8 curr, u8 previous)
+{
+	struct input_dev *input = keypad->input;
+	u8 changed  = curr ^ previous;
+
+	while (changed) {
+		int col = __ffs(changed);
+		bool press = curr & BIT(col);
+		int code = MATRIX_SCAN_CODE(row, col, KEYPAD_ROW_SHIFT);
+
+		input_event(input, EV_MSC, MSC_SCAN, code);
+		input_report_key(input, keypad->keymap[code], press);
+		input_sync(input);
+
+		changed &= ~BIT(col);
+	}
+}
+
+static void db5500_keypad_scan(struct db5500_keypad *keypad)
+{
+	u8 current_set[ARRAY_SIZE(keypad->previous_set)];
+	int tries = 100;
+	bool changebit;
+	u32 data_reg;
+	u8 allrows;
+	u8 common;
+	int i;
+
+	writel(0x1, keypad->base + KEYPAD_IRQ_CLEAR);
+
+again:
+	if (!tries--) {
+		dev_warn(&keypad->input->dev, "values failed to stabilize\n");
+		return;
+	}
+
+	changebit = readl(keypad->base + KEYPAD_ARRAY_01)
+		    & KEYPAD_ARRAY_CHANGEBIT;
+
+	for (i = 0; i < KEYPAD_NUM_ARRAY_REGS; i++) {
+		data_reg = readl(keypad->base + KEYPAD_ARRAY_01 + 4 * i);
+
+		/* If the change bit changed, we need to reread the data */
+		if (changebit != !!(data_reg & KEYPAD_ARRAY_CHANGEBIT))
+			goto again;
+
+		current_set[2 * i] = ~(data_reg & 0xff);
+
+		/* Last array reg has only one valid set of columns */
+		if (i != KEYPAD_NUM_ARRAY_REGS - 1)
+			current_set[2 * i + 1] = ~((data_reg & 0xff0000) >> 16);
+	}
+
+	allrows = current_set[KEYPAD_GND_ROW];
+
+	/*
+	 * Sometimes during a GND row release, an incorrect report is received
+	 * where the ARRAY8 all rows setting does not match the other ARRAY*
+	 * rows.  Ignore this report; the correct one has been observed to
+	 * follow it.
+	 */
+	common = 0xff;
+	for (i = 0; i < KEYPAD_GND_ROW; i++)
+		common &= current_set[i];
+
+	if ((allrows & common) != common)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(current_set); i++) {
+		/*
+		 * If there is an allrows press (GND row), we need to ignore
+		 * the allrows values from the reset of the ARRAYs.
+		 */
+		if (i < KEYPAD_GND_ROW && allrows)
+			current_set[i] &= ~allrows;
+
+		if (keypad->previous_set[i] == current_set[i])
+			continue;
+
+		db5500_keypad_report(keypad, i, current_set[i],
+				     keypad->previous_set[i]);
+	}
+
+	/* update the reference set of array registers */
+	memcpy(keypad->previous_set, current_set, sizeof(keypad->previous_set));
+
+	return;
+}
+
+/**
+ * db5500_keypad_writel() - write into keypad registers
+ * @keypad: pointer to device structure
+ * @val: value to write into register
+ * @reg: register offset
+ *
+ * This function uses to write into the keypad registers
+ * and returns NONE.
+ */
+static void db5500_keypad_writel(struct db5500_keypad *keypad, u32 val, u32 reg)
+{
+	int timeout = 4;
+	int allowedbit;
+
+	switch (reg) {
+	case KEYPAD_CTR:
+		allowedbit = KEYPAD_CTR_WRITE_CONTROL;
+		break;
+	case KEYPAD_INT_ENABLE:
+		allowedbit = KEYPAD_CTR_WRITE_IRQ_ENABLE;
+		break;
+	default:
+		BUG();
+	}
+
+	do {
+		u32 ctr = readl(keypad->base + KEYPAD_CTR);
+
+		if (ctr & allowedbit)
+			break;
+
+		udelay(50);
+	} while (--timeout);
+
+	/* Five 32k clk cycles (~150us) required, we waited 200us */
+	WARN_ON(!timeout);
+
+	writel(val, keypad->base + reg);
+}
+
+/**
+ * db5500_keypad_chip_init() - initialize the keypad chip
+ * @keypad: pointer to device structure
+ *
+ * This function uses to initializes the keypad controller
+ * and returns integer.
+ */
+static int db5500_keypad_chip_init(struct db5500_keypad *keypad)
+{
+	int debounce = keypad->board->debounce_ms;
+	int debounce_hits = 0;
+
+	if (debounce < KEYPAD_DEBOUNCE_PERIOD_MIN)
+		debounce = KEYPAD_DEBOUNCE_PERIOD_MIN;
+
+	if (debounce > KEYPAD_DEBOUNCE_PERIOD_MAX) {
+		debounce_hits = DIV_ROUND_UP(debounce,
+					     KEYPAD_DEBOUNCE_PERIOD_MAX) - 1;
+		debounce = KEYPAD_DEBOUNCE_PERIOD_MAX;
+	}
+
+	/* Convert the milliseconds to the bit mask */
+	debounce = DIV_ROUND_UP(debounce, KEYPAD_DEBOUNCE_PERIOD_MIN) - 1;
+
+	clk_enable(keypad->clk);
+
+	db5500_keypad_writel(keypad,
+			     KEYPAD_CTR_SCAN_ENABLE
+			     | ((debounce_hits & 0x7) << 4)
+			     | debounce,
+			     KEYPAD_CTR);
+
+	db5500_keypad_writel(keypad, 0x1, KEYPAD_INT_ENABLE);
+
+	return 0;
+}
+
+static void db5500_mode_enable(struct db5500_keypad *keypad, bool enable)
+{
+	int i;
+
+	if (!enable) {
+		db5500_keypad_writel(keypad, 0, KEYPAD_CTR);
+		db5500_keypad_writel(keypad, 0, KEYPAD_INT_ENABLE);
+		if (keypad->board->exit)
+			keypad->board->exit();
+		for (i = 0; i < keypad->board->krow; i++) {
+			enable_irq(keypad->gpio_input_irq[i]);
+			enable_irq_wake(keypad->gpio_input_irq[i]);
+		}
+		clk_disable(keypad->clk);
+		regulator_disable(keypad->regulator);
+	} else {
+		regulator_enable(keypad->regulator);
+		clk_enable(keypad->clk);
+		for (i = 0; i < keypad->board->krow; i++) {
+			disable_irq_nosync(keypad->gpio_input_irq[i]);
+			disable_irq_wake(keypad->gpio_input_irq[i]);
+		}
+		if (keypad->board->init)
+			keypad->board->init();
+		db5500_keypad_chip_init(keypad);
+	}
+}
+
+static void db5500_gpio_switch_work(struct work_struct *work)
+{
+	struct db5500_keypad *keypad = container_of(work,
+					struct db5500_keypad, switch_work.work);
+
+	db5500_mode_enable(keypad, false);
+	keypad->enable = false;
+}
+
+static void db5500_gpio_release_work(struct work_struct *work)
+{
+	int code;
+	struct db5500_keypad *keypad = container_of(work,
+					struct db5500_keypad, gpio_work.work);
+	struct input_dev *input = keypad->input;
+
+	code = MATRIX_SCAN_CODE(keypad->gpio_col, keypad->gpio_row,
+						KEYPAD_ROW_SHIFT);
+	input_event(input, EV_MSC, MSC_SCAN, code);
+	input_report_key(input, keypad->keymap[code], 1);
+	input_sync(input);
+	input_report_key(input, keypad->keymap[code], 0);
+	input_sync(input);
+}
+
+static int db5500_read_get_gpio_row(struct db5500_keypad *keypad)
+{
+	int row;
+	int value = 0;
+	int ret;
+
+	/* read all rows GPIO data register values */
+	for (row = 0; row < keypad->board->krow; row++) {
+		ret  = gpio_get_value(keypad->db5500_rows[row]);
+		value += (1 << row) *  ret;
+	}
+
+	/* get the exact row */
+	for (row = 0; row < keypad->board->krow; row++) {
+		if (((1 << row) & value) == 0)
+			return row;
+	}
+
+	return -1;
+}
+
+static void db5500_set_cols(struct db5500_keypad *keypad, int col)
+{
+	int i, ret;
+	int value;
+
+	/*
+	 * Set all columns except the requested column
+	 * output pin as high
+	 */
+	for (i = 0; i < keypad->board->kcol; i++) {
+		if (i == col)
+			value = 0;
+		else
+			value = 1;
+		ret = gpio_request(keypad->db5500_cols[i], "db5500-kpd");
+
+		if (ret < 0) {
+			pr_err("db5500_set_cols: gpio request failed\n");
+			continue;
+		}
+
+		gpio_direction_output(keypad->db5500_cols[i], value);
+		gpio_free(keypad->db5500_cols[i]);
+	}
+}
+
+static void db5500_free_cols(struct db5500_keypad *keypad)
+{
+	int i, ret;
+
+	for (i = 0; i < keypad->board->kcol; i++) {
+		ret = gpio_request(keypad->db5500_cols[i], "db5500-kpd");
+
+		if (ret < 0) {
+			pr_err("db5500_free_cols: gpio request failed\n");
+			continue;
+		}
+
+		gpio_direction_output(keypad->db5500_cols[i], 0);
+		gpio_free(keypad->db5500_cols[i]);
+	}
+}
+
+static void db5500_manual_scan(struct db5500_keypad *keypad)
+{
+	int row;
+	int col;
+
+	keypad->valid_key = false;
+
+	for (col = 0; col < keypad->board->kcol; col++) {
+		db5500_set_cols(keypad, col);
+		row = db5500_read_get_gpio_row(keypad);
+		if (row >= 0) {
+			keypad->valid_key = true;
+			keypad->gpio_row = row;
+			keypad->gpio_col = col;
+			break;
+		}
+	}
+	db5500_free_cols(keypad);
+}
+
+static irqreturn_t db5500_keypad_gpio_irq(int irq, void *dev_id)
+{
+	struct db5500_keypad *keypad = dev_id;
+
+	if (!gpio_get_value(IRQ_TO_GPIO(irq))) {
+		db5500_manual_scan(keypad);
+		if (!keypad->enable) {
+			keypad->enable = true;
+			db5500_mode_enable(keypad, true);
+		}
+
+		/*
+		 * Schedule the work queue to change it to
+		 * report the key pressed, if it is not detected in keypad mode.
+		 */
+		if (keypad->valid_key) {
+			schedule_delayed_work(&keypad->gpio_work,
+						KEY_PRESSED_DELAY);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t db5500_keypad_irq(int irq, void *dev_id)
+{
+	struct db5500_keypad *keypad = dev_id;
+
+	cancel_delayed_work_sync(&keypad->gpio_work);
+	cancel_delayed_work_sync(&keypad->switch_work);
+	db5500_keypad_scan(keypad);
+
+	/*
+	 * Schedule the work queue to change it to
+	 * GPIO mode, if there is no activity in keypad mode
+	 */
+	if (keypad->enable)
+		schedule_delayed_work(&keypad->switch_work,
+				keypad->board->switch_delay);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * db5500_keypad_probe() - Initialze the the keypad driver
+ * @pdev: pointer to platform device structure
+ *
+ * This function will allocate and initialize the instance
+ * data and request the irq and register to input subsystem driver.
+ */
+static int __devinit db5500_keypad_probe(struct platform_device *pdev)
+{
+	struct db5500_keypad_platform_data *plat;
+	struct db5500_keypad *keypad;
+	struct resource *res;
+	struct input_dev *input;
+	void __iomem *base;
+	struct clk *clk;
+	int ret;
+	int irq;
+	int i;
+
+	plat = pdev->dev.platform_data;
+	if (!plat) {
+		dev_err(&pdev->dev, "invalid keypad platform data\n");
+		ret = -EINVAL;
+		goto out_ret;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get keypad irq\n");
+		ret = -EINVAL;
+		goto out_ret;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "missing platform resources\n");
+		ret = -EINVAL;
+		goto out_ret;
+	}
+
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to request I/O memory\n");
+		ret = -EBUSY;
+		goto out_ret;
+	}
+
+	base = ioremap(res->start, resource_size(res));
+	if (!base) {
+		dev_err(&pdev->dev, "failed to remap I/O memory\n");
+		ret = -ENXIO;
+		goto out_freerequest_memregions;
+	}
+
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "failed to clk_get\n");
+		ret = PTR_ERR(clk);
+		goto out_iounmap;
+	}
+
+	keypad = kzalloc(sizeof(struct db5500_keypad), GFP_KERNEL);
+	if (!keypad) {
+		dev_err(&pdev->dev, "failed to allocate keypad memory\n");
+		ret = -ENOMEM;
+		goto out_freeclk;
+	}
+
+	input = input_allocate_device();
+	if (!input) {
+		dev_err(&pdev->dev, "failed to input_allocate_device\n");
+		ret = -ENOMEM;
+		goto out_freekeypad;
+	}
+
+	keypad->regulator = regulator_get(&pdev->dev, "v-ape");
+	if (IS_ERR(keypad->regulator)) {
+		dev_err(&pdev->dev, "regulator_get failed\n");
+		keypad->regulator = NULL;
+		ret = -EINVAL;
+		goto out_regulator_get;
+	} else {
+		ret = regulator_enable(keypad->regulator);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "regulator_enable failed\n");
+			goto out_regulator_enable;
+		}
+	}
+
+	input->id.bustype = BUS_HOST;
+	input->name = "db5500-keypad";
+	input->dev.parent = &pdev->dev;
+
+	input->keycode = keypad->keymap;
+	input->keycodesize = sizeof(keypad->keymap[0]);
+	input->keycodemax = ARRAY_SIZE(keypad->keymap);
+
+	input_set_capability(input, EV_MSC, MSC_SCAN);
+
+	__set_bit(EV_KEY, input->evbit);
+	if (!plat->no_autorepeat)
+		__set_bit(EV_REP, input->evbit);
+
+	matrix_keypad_build_keymap(plat->keymap_data, KEYPAD_ROW_SHIFT,
+				   input->keycode, input->keybit);
+
+	ret = input_register_device(input);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"unable to register input device: %d\n", ret);
+		goto out_freeinput;
+	}
+
+	keypad->irq	= irq;
+	keypad->board	= plat;
+	keypad->input	= input;
+	keypad->base	= base;
+	keypad->clk	= clk;
+
+	INIT_DELAYED_WORK(&keypad->switch_work, db5500_gpio_switch_work);
+	INIT_DELAYED_WORK(&keypad->gpio_work, db5500_gpio_release_work);
+
+	clk_enable(keypad->clk);
+if (!keypad->board->init) {
+		dev_err(&pdev->dev, "init funtion not defined\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
+
+	if (keypad->board->init() < 0) {
+		dev_err(&pdev->dev, "keyboard init config failed\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
+
+	if (!keypad->board->exit) {
+		dev_err(&pdev->dev, "exit funtion not defined\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
+
+	if (keypad->board->exit() < 0) {
+		dev_err(&pdev->dev,  "keyboard exit config failed\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
+
+	for (i = 0; i < keypad->board->krow; i++) {
+		keypad->db5500_rows[i] = *plat->gpio_input_pins;
+		keypad->gpio_input_irq[i] =
+				GPIO_TO_IRQ(keypad->db5500_rows[i]);
+		plat->gpio_input_pins++;
+	}
+
+	for (i = 0; i < keypad->board->kcol; i++) {
+		keypad->db5500_cols[i] = *plat->gpio_output_pins;
+		plat->gpio_output_pins++;
+	}
+
+	for (i = 0; i < keypad->board->krow; i++) {
+		ret =  request_threaded_irq(keypad->gpio_input_irq[i],
+				NULL, db5500_keypad_gpio_irq,
+				IRQF_TRIGGER_FALLING | IRQF_NO_SUSPEND,
+				"db5500-keypad-gpio", keypad);
+		if (ret) {
+			dev_err(&pdev->dev, "allocate gpio irq %d failed\n",
+						keypad->gpio_input_irq[i]);
+			goto out_unregisterinput;
+		}
+		enable_irq_wake(keypad->gpio_input_irq[i]);
+	}
+
+	ret = request_threaded_irq(keypad->irq, NULL, db5500_keypad_irq,
+				   IRQF_ONESHOT, "db5500-keypad", keypad);
+	if (ret) {
+		dev_err(&pdev->dev, "allocate irq %d failed\n", keypad->irq);
+		goto out_unregisterinput;
+	}
+
+	platform_set_drvdata(pdev, keypad);
+
+	clk_disable(keypad->clk);
+	regulator_disable(keypad->regulator);
+	return 0;
+
+out_unregisterinput:
+	input_unregister_device(input);
+	input = NULL;
+	clk_disable(keypad->clk);
+out_freeinput:
+	input_free_device(input);
+out_regulator_enable:
+	regulator_put(keypad->regulator);
+out_regulator_get:
+	input_free_device(input);
+out_freekeypad:
+	kfree(keypad);
+out_freeclk:
+	clk_put(clk);
+out_iounmap:
+	iounmap(base);
+out_freerequest_memregions:
+	release_mem_region(res->start, resource_size(res));
+out_ret:
+	return ret;
+}
+
+/**
+ * db5500_keypad_remove() - Removes the keypad driver
+ * @pdev: pointer to platform device structure
+ *
+ * This function uses to remove the keypad
+ * driver and returns integer.
+ */
+static int __devexit db5500_keypad_remove(struct platform_device *pdev)
+{
+	struct db5500_keypad *keypad = platform_get_drvdata(pdev);
+	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	cancel_delayed_work_sync(&keypad->gpio_work);
+	cancel_delayed_work_sync(&keypad->switch_work);
+	free_irq(keypad->irq, keypad);
+	input_unregister_device(keypad->input);
+
+	clk_disable(keypad->clk);
+	clk_put(keypad->clk);
+
+	if (keypad->board->exit)
+		keypad->board->exit();
+
+	regulator_put(keypad->regulator);
+
+	iounmap(keypad->base);
+
+	if (res)
+		release_mem_region(res->start, resource_size(res));
+
+	kfree(keypad);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/**
+ * db5500_keypad_suspend() - suspend the keypad controller
+ * @dev: pointer to device structure
+ *
+ * This function is used to suspend the
+ * keypad controller and returns integer
+ */
+static int db5500_keypad_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct db5500_keypad *keypad = platform_get_drvdata(pdev);
+	int irq = platform_get_irq(pdev, 0);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(irq);
+	else {
+		cancel_delayed_work_sync(&keypad->gpio_work);
+		cancel_delayed_work_sync(&keypad->switch_work);
+		disable_irq(irq);
+		keypad->enable_on_resume = keypad->enable;
+		if (keypad->enable) {
+			db5500_mode_enable(keypad, false);
+			keypad->enable = false;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * db5500_keypad_resume() - resume the keypad controller
+ * @dev: pointer to device structure
+ *
+ * This function is used to resume the keypad
+ * controller and returns integer.
+ */
+static int db5500_keypad_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct db5500_keypad *keypad = platform_get_drvdata(pdev);
+	int irq = platform_get_irq(pdev, 0);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(irq);
+	else {
+		if (keypad->enable_on_resume && !keypad->enable) {
+			keypad->enable = true;
+			db5500_mode_enable(keypad, true);
+			/*
+			 * Schedule the work queue to change it to GPIO mode
+			 * if there is no activity keypad mode
+			 */
+			 schedule_delayed_work(&keypad->switch_work,
+				 keypad->board->switch_delay);
+		}
+		enable_irq(irq);
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops db5500_keypad_dev_pm_ops = {
+	.suspend = db5500_keypad_suspend,
+	.resume = db5500_keypad_resume,
+};
+#endif
+
+static struct platform_driver db5500_keypad_driver = {
+	.driver = {
+		.name	= "db5500-keypad",
+		.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm	= &db5500_keypad_dev_pm_ops,
+#endif
+	},
+	.probe	= db5500_keypad_probe,
+	.remove	= __devexit_p(db5500_keypad_remove),
+};
+
+/**
+ * db5500_keypad_init() - Initialize the keypad driver
+ *
+ * This function uses to initializes the db5500
+ * keypad driver and returns integer.
+ */
+static int __init db5500_keypad_init(void)
+{
+	return platform_driver_register(&db5500_keypad_driver);
+}
+module_init(db5500_keypad_init);
+
+/**
+ * db5500_keypad_exit() - De-initialize the keypad driver
+ *
+ * This function uses to de-initialize the db5500
+ * keypad driver and returns none.
+ */
+static void __exit db5500_keypad_exit(void)
+{
+	platform_driver_unregister(&db5500_keypad_driver);
+}
+module_exit(db5500_keypad_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Sundar Iyer <sundar.iyer@stericsson.com>");
+MODULE_DESCRIPTION("DB5500 Keypad Driver");
+MODULE_ALIAS("platform:db5500-keypad");
diff --git a/drivers/input/keyboard/gpio_keys.c b/drivers/input/keyboard/gpio_keys.c
index 62bfce468f9..1fdf54bb4f6 100644
--- a/drivers/input/keyboard/gpio_keys.c
+++ b/drivers/input/keyboard/gpio_keys.c
@@ -29,6 +29,7 @@
 #include <linux/of_platform.h>
 #include <linux/of_gpio.h>
 #include <linux/spinlock.h>
+#include <linux/pm_runtime.h>
 
 struct gpio_button_data {
 	const struct gpio_keys_button *button;
@@ -46,6 +47,8 @@ struct gpio_keys_drvdata {
 	struct input_dev *input;
 	struct mutex disable_lock;
 	unsigned int n_buttons;
+	bool enabled;
+	bool enable_after_suspend;
 	int (*enable)(struct device *dev);
 	void (*disable)(struct device *dev);
 	struct gpio_button_data data[0];
@@ -524,6 +527,8 @@ static int gpio_keys_open(struct input_dev *input)
 {
 	struct gpio_keys_drvdata *ddata = input_get_drvdata(input);
 
+	pm_runtime_get_sync(input->dev.parent);
+	ddata->enabled = true;
 	return ddata->enable ? ddata->enable(input->dev.parent) : 0;
 }
 
@@ -533,6 +538,8 @@ static void gpio_keys_close(struct input_dev *input)
 
 	if (ddata->disable)
 		ddata->disable(input->dev.parent);
+	ddata->enabled = false;
+	pm_runtime_put(input->dev.parent);
 }
 
 /*
@@ -675,6 +682,7 @@ static int __devinit gpio_keys_probe(struct platform_device *pdev)
 	ddata->n_buttons = pdata->nbuttons;
 	ddata->enable = pdata->enable;
 	ddata->disable = pdata->disable;
+	ddata->enabled = false;
 	mutex_init(&ddata->disable_lock);
 
 	platform_set_drvdata(pdev, ddata);
@@ -691,6 +699,8 @@ static int __devinit gpio_keys_probe(struct platform_device *pdev)
 	input->id.product = 0x0001;
 	input->id.version = 0x0100;
 
+	pm_runtime_enable(&pdev->dev);
+
 	/* Enable auto repeat feature of Linux input subsystem */
 	if (pdata->rep)
 		__set_bit(EV_REP, input->evbit);
@@ -756,6 +766,8 @@ static int __devexit gpio_keys_remove(struct platform_device *pdev)
 	struct input_dev *input = ddata->input;
 	int i;
 
+	pm_runtime_disable(&pdev->dev);
+
 	sysfs_remove_group(&pdev->dev.kobj, &gpio_keys_attr_group);
 
 	device_init_wakeup(&pdev->dev, 0);
@@ -790,6 +802,10 @@ static int gpio_keys_suspend(struct device *dev)
 			if (bdata->button->wakeup)
 				enable_irq_wake(bdata->irq);
 		}
+	} else {
+		ddata->enable_after_suspend = ddata->enabled;
+		if (ddata->enabled && ddata->disable)
+			ddata->disable(dev);
 	}
 
 	return 0;
@@ -808,6 +824,11 @@ static int gpio_keys_resume(struct device *dev)
 		if (gpio_is_valid(bdata->button->gpio))
 			gpio_keys_gpio_report_event(bdata);
 	}
+
+	if (!device_may_wakeup(dev) && ddata->enable_after_suspend
+	    && ddata->enable)
+		ddata->enable(dev);
+
 	input_sync(ddata->input);
 
 	return 0;
diff --git a/drivers/input/keyboard/nomadik-ske-keypad.c b/drivers/input/keyboard/nomadik-ske-keypad.c
index 101e245944e..6a0707f7f76 100644
--- a/drivers/input/keyboard/nomadik-ske-keypad.c
+++ b/drivers/input/keyboard/nomadik-ske-keypad.c
@@ -2,7 +2,7 @@
  * Copyright (C) ST-Ericsson SA 2010
  *
  * Author: Naveen Kumar G <naveen.gaddipati@stericsson.com> for ST-Ericsson
- * Author: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
+ * co-Author: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
  *
  * License terms:GNU General Public License (GPL) version 2
  *
@@ -12,6 +12,7 @@
 
 #include <linux/platform_device.h>
 #include <linux/interrupt.h>
+#include <linux/workqueue.h>
 #include <linux/spinlock.h>
 #include <linux/io.h>
 #include <linux/delay.h>
@@ -19,8 +20,10 @@
 #include <linux/slab.h>
 #include <linux/clk.h>
 #include <linux/module.h>
+#include <linux/regulator/consumer.h>
 
 #include <plat/ske.h>
+#include <linux/gpio/nomadik.h>
 
 /* SKE_CR bits */
 #define SKE_KPMLT	(0x1 << 6)
@@ -48,17 +51,38 @@
 #define SKE_ASR3	0x2C
 
 #define SKE_NUM_ASRX_REGISTERS	(4)
+#define	KEY_PRESSED_DELAY	10
+
+
+#define KEY_REPORTED	1
+#define KEY_PRESSED	2
 
 /**
  * struct ske_keypad  - data structure used by keypad driver
- * @irq:	irq no
- * @reg_base:	ske regsiters base address
- * @input:	pointer to input device object
- * @board:	keypad platform device
- * @keymap:	matrix scan code table for keycodes
- * @clk:	clock structure pointer
+ * @dev:		Pointer to the structure device
+ * @irq:		irq no
+ * @reg_base:		ske regsiters base address
+ * @input:		pointer to input device object
+ * @board:		keypad platform device
+ * @keymap:		matrix scan code table for keycodes
+ * @clk:		clock structure pointer
+ * @ske_keypad_lock:    lock used while writting into registers
+ * @enable:		flag to enable the driver event
+ * @enable_on_resume:   set if keypad should be enabled on resume
+ * @regulator:		pointer to the regulator used for ske kyepad
+ * @gpio_input_irq:	array for gpio irqs
+ * @key_pressed:	hold the key state
+ * @work:		delayed work variable for gpio switch
+ * @ske_rows:		rows gpio array for ske
+ * @ske_cols:		columns gpio array for ske
+ * @gpio_row:		gpio row
+ * @gpio_col:		gpio column
+ * @gpio_work:		delayed work variable for release gpio key
+ * @keys:		matrix holding key status
+ * @scan_work:		delayed work for scaning new key actions
  */
 struct ske_keypad {
+	struct device *dev;
 	int irq;
 	void __iomem *reg_base;
 	struct input_dev *input;
@@ -66,6 +90,19 @@ struct ske_keypad {
 	unsigned short keymap[SKE_KPD_KEYMAP_SIZE];
 	struct clk *clk;
 	spinlock_t ske_keypad_lock;
+	bool enable;
+	bool enable_on_resume;
+	struct regulator *regulator;
+	int *gpio_input_irq;
+	int key_pressed;
+	struct delayed_work work;
+	int *ske_rows;
+	int *ske_cols;
+	int gpio_row;
+	int gpio_col;
+	struct delayed_work gpio_work;
+	u8 **keys;
+	struct delayed_work scan_work;
 };
 
 static void ske_keypad_set_bits(struct ske_keypad *keypad, u16 addr,
@@ -83,15 +120,15 @@ static void ske_keypad_set_bits(struct ske_keypad *keypad, u16 addr,
 	spin_unlock(&keypad->ske_keypad_lock);
 }
 
-/*
+/**
  * ske_keypad_chip_init: init keypad controller configuration
- *
+ * @keypad: pointer to device structure
  * Enable Multi key press detection, auto scan mode
  */
 static int __init ske_keypad_chip_init(struct ske_keypad *keypad)
 {
 	u32 value;
-	int timeout = 50;
+	int timeout = keypad->board->debounce_ms;
 
 	/* check SKE_RIS to be 0 */
 	while ((readl(keypad->reg_base + SKE_RIS) != 0x00000000) && timeout--)
@@ -100,7 +137,7 @@ static int __init ske_keypad_chip_init(struct ske_keypad *keypad)
 	if (!timeout)
 		return -EINVAL;
 
-	/*
+	/**
 	 * set debounce value
 	 * keypad dbounce is configured in DBCR[15:8]
 	 * dbounce value in steps of 32/32.768 ms
@@ -115,7 +152,7 @@ static int __init ske_keypad_chip_init(struct ske_keypad *keypad)
 	/* enable multi key detection */
 	ske_keypad_set_bits(keypad, SKE_CR, 0x0, SKE_KPMLT);
 
-	/*
+	/**
 	 * set up the number of columns
 	 * KPCN[5:3] defines no. of keypad columns to be auto scanned
 	 */
@@ -134,14 +171,125 @@ static int __init ske_keypad_chip_init(struct ske_keypad *keypad)
 	return 0;
 }
 
+static void ske_mode_enable(struct ske_keypad *keypad, bool enable)
+{
+	int i;
+
+	if (!enable) {
+		dev_dbg(keypad->dev, "%s disable keypad\n", __func__);
+		writel(0, keypad->reg_base + SKE_CR);
+		if (keypad->board->exit)
+			keypad->board->exit();
+		for (i = 0; i < keypad->board->kconnected_rows; i++) {
+			enable_irq(keypad->gpio_input_irq[i]);
+			enable_irq_wake(keypad->gpio_input_irq[i]);
+		}
+		clk_disable(keypad->clk);
+		regulator_disable(keypad->regulator);
+	} else {
+		dev_dbg(keypad->dev, "%s enable keypad\n", __func__);
+		regulator_enable(keypad->regulator);
+		clk_enable(keypad->clk);
+		for (i = 0; i < keypad->board->kconnected_rows; i++) {
+			disable_irq_nosync(keypad->gpio_input_irq[i]);
+			disable_irq_wake(keypad->gpio_input_irq[i]);
+		}
+		if (keypad->board->init)
+			keypad->board->init();
+		ske_keypad_chip_init(keypad);
+	}
+}
+static void ske_enable(struct ske_keypad *keypad, bool enable)
+{
+	keypad->enable = enable;
+	if (keypad->enable) {
+		enable_irq(keypad->irq);
+		ske_mode_enable(keypad, true);
+	} else {
+		ske_mode_enable(keypad, false);
+		disable_irq(keypad->irq);
+	}
+}
+
+static ssize_t ske_show_attr_enable(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct ske_keypad *keypad = platform_get_drvdata(pdev);
+	return sprintf(buf, "%d\n", keypad->enable);
+}
+
+static ssize_t ske_store_attr_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct ske_keypad *keypad = platform_get_drvdata(pdev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if ((val != 0) && (val != 1))
+		return -EINVAL;
+
+	if (keypad->enable != val) {
+		keypad->enable = val ? true : false;
+		ske_enable(keypad, keypad->enable);
+	}
+	return count;
+}
+
+static DEVICE_ATTR(enable, S_IWUSR | S_IRUGO,
+	ske_show_attr_enable, ske_store_attr_enable);
+
+static struct attribute *ske_keypad_attrs[] = {
+	&dev_attr_enable.attr,
+	NULL,
+};
+
+static struct attribute_group ske_attr_group = {
+	.attrs = ske_keypad_attrs,
+};
+
+static void ske_keypad_report(struct ske_keypad *keypad, u8 status, int col)
+{
+	int row = 0, code, pos;
+	u32 ske_ris;
+	int num_of_rows;
+
+	/* find out the row */
+	num_of_rows = hweight8(status);
+	do {
+		pos = __ffs(status);
+		row = pos;
+		status &= ~(1 << pos);
+
+		if (row >= keypad->board->krow)
+			/* no more rows supported by this keypad */
+			break;
+
+		code = MATRIX_SCAN_CODE(row, col, SKE_KEYPAD_ROW_SHIFT);
+		ske_ris = readl(keypad->reg_base + SKE_RIS);
+		keypad->key_pressed = ske_ris & SKE_KPRISA;
+
+		dev_dbg(keypad->dev,
+			"%s key_pressed:%d code:%d row:%d col:%d\n",
+			__func__, keypad->key_pressed, code, row, col);
+
+		if (keypad->key_pressed)
+			keypad->keys[row][col] |= KEY_PRESSED;
+
+		num_of_rows--;
+	} while (num_of_rows);
+}
+
 static void ske_keypad_read_data(struct ske_keypad *keypad)
 {
-	struct input_dev *input = keypad->input;
-	u16 status;
-	int col = 0, row = 0, code;
-	int ske_asr, ske_ris, key_pressed, i;
+	u8 status;
+	int col = 0;
+	int ske_asr, i;
 
-	/*
+	/**
 	 * Read the auto scan registers
 	 *
 	 * Each SKE_ASRx (x=0 to x=3) contains two row values.
@@ -153,59 +301,274 @@ static void ske_keypad_read_data(struct ske_keypad *keypad)
 		if (!ske_asr)
 			continue;
 
-		/* now that ASRx is zero, find out the column x and row y*/
-		if (ske_asr & 0xff) {
+		/* now that ASRx is zero, find out the coloumn x and row y */
+		status = ske_asr & 0xff;
+		if (status) {
 			col = i * 2;
-			status = ske_asr & 0xff;
-		} else {
+			if (col >= keypad->board->kcol)
+				/* no more columns supported by this keypad */
+				break;
+			ske_keypad_report(keypad, status, col);
+		}
+		status = (ske_asr & 0xff00) >> 8;
+		if (status) {
 			col = (i * 2) + 1;
-			status = (ske_asr & 0xff00) >> 8;
+			if (col >= keypad->board->kcol)
+				/* no more columns supported by this keypad */
+				break;
+			ske_keypad_report(keypad, status, col);
 		}
+	}
+}
 
-		/* find out the row */
-		row = __ffs(status);
+static void ske_keypad_scan_work(struct work_struct *work)
+{
+	int timeout = 10;
+	int i, j, code;
+	struct ske_keypad *keypad = container_of(work,
+					struct ske_keypad, scan_work.work);
+	struct input_dev *input = keypad->input;
 
-		code = MATRIX_SCAN_CODE(row, col, SKE_KEYPAD_ROW_SHIFT);
-		ske_ris = readl(keypad->reg_base + SKE_RIS);
-		key_pressed = ske_ris & SKE_KPRISA;
+	/* Wait for autoscan to complete */
+	while (readl(keypad->reg_base + SKE_CR) & SKE_KPASON)
+		cpu_relax();
 
-		input_event(input, EV_MSC, MSC_SCAN, code);
-		input_report_key(input, keypad->keymap[code], key_pressed);
-		input_sync(input);
+	/* SKEx registers are stable and can be read */
+	ske_keypad_read_data(keypad);
+
+	/* Check for key actions */
+	for (i = 0; i < keypad->board->krow; i++) {
+		for (j = 0; j < keypad->board->kcol; j++) {
+			switch (keypad->keys[i][j]) {
+			case KEY_REPORTED:
+				/**
+				 * Key was reported but is no longer pressed,
+				 * report it as released.
+				 */
+				code = MATRIX_SCAN_CODE(i, j,
+							SKE_KEYPAD_ROW_SHIFT);
+				input_event(input, EV_MSC, MSC_SCAN, code);
+				input_report_key(input, keypad->keymap[code],
+						 0);
+				input_sync(input);
+				keypad->keys[i][j] = 0;
+				dev_dbg(keypad->dev,
+					"%s Key release reported, code:%d "
+					"(key %d)\n",
+					__func__, code, keypad->keymap[code]);
+				break;
+			case KEY_PRESSED:
+				/* Key pressed but not yet reported, report */
+				code = MATRIX_SCAN_CODE(i, j,
+							SKE_KEYPAD_ROW_SHIFT);
+				input_event(input, EV_MSC, MSC_SCAN, code);
+				input_report_key(input, keypad->keymap[code],
+						 1);
+				input_sync(input);
+				dev_dbg(keypad->dev,
+					"%s Key press reported, code:%d "
+					"(key %d)\n",
+					__func__, code, keypad->keymap[code]);
+				/* Intentional fall though */
+			case (KEY_REPORTED | KEY_PRESSED):
+				/**
+				 * Key pressed and reported, just reset
+				 * KEY_PRESSED for next scan
+				 */
+				keypad->keys[i][j] = KEY_REPORTED;
+				break;
+			}
+		}
+	}
+
+	if (keypad->key_pressed) {
+		/*
+		 * Key still pressed, schedule work to poll changes in 100 ms
+		 * After increasing the delay from 50 to 100 it is taking
+		 * 2% to 3% load on average.
+		 */
+		schedule_delayed_work(&keypad->scan_work,
+				      msecs_to_jiffies(100));
+	} else {
+		/* For safty measure, clear interrupt once more */
+		ske_keypad_set_bits(keypad, SKE_ICR, 0x0, SKE_KPICA);
+
+		/* Wait for raw interrupt to clear */
+		while ((readl(keypad->reg_base + SKE_RIS) & SKE_KPRISA) &&
+		       --timeout) {
+			udelay(10);
+		}
+
+		if (!timeout)
+			dev_err(keypad->dev,
+				"%s Timeed out waiting on irq to clear\n",
+				__func__);
+
+		/* enable auto scan interrupts */
+		ske_keypad_set_bits(keypad, SKE_IMSC, 0x0, SKE_KPIMA);
+
+		/**
+		 * Schedule the work queue to change it to GPIO mode
+		 * if there is no activity in SKE mode
+		 */
+		if (!keypad->key_pressed && keypad->enable)
+			schedule_delayed_work(&keypad->work,
+					      keypad->board->switch_delay);
 	}
 }
 
+static void ske_gpio_switch_work(struct work_struct *work)
+{
+	struct ske_keypad *keypad = container_of(work,
+					struct ske_keypad, work.work);
+
+	ske_mode_enable(keypad, false);
+	keypad->enable = false;
+}
+
+static void ske_gpio_release_work(struct work_struct *work)
+{
+	int code;
+	struct ske_keypad *keypad = container_of(work,
+					struct ske_keypad, gpio_work.work);
+	struct input_dev *input = keypad->input;
+
+	code = MATRIX_SCAN_CODE(keypad->gpio_row, keypad->gpio_col,
+						SKE_KEYPAD_ROW_SHIFT);
+
+	dev_dbg(keypad->dev, "%s Key press reported, code:%d (key %d)\n",
+		__func__, code, keypad->keymap[code]);
+
+	input_event(input, EV_MSC, MSC_SCAN, code);
+	input_report_key(input, keypad->keymap[code], 1);
+	input_sync(input);
+	input_report_key(input, keypad->keymap[code], 0);
+	input_sync(input);
+}
+
+static int ske_read_get_gpio_row(struct ske_keypad *keypad)
+{
+	int row;
+	int value = 0;
+	int ret;
+
+	/* read all rows GPIO data register values */
+	for (row = 0; row < keypad->board->kconnected_rows ; row++) {
+		ret  = gpio_get_value(keypad->ske_rows[row]);
+		value += (1 << row) *  ret;
+	}
+
+	/* get the exact row */
+	for (row = 0; row < keypad->board->kconnected_rows; row++) {
+		if (((1 << row) & value) == 0)
+			return row;
+	}
+
+	return -1;
+}
+
+static void ske_set_cols(struct ske_keypad *keypad, int col)
+{
+	int i ;
+	int value;
+
+	/**
+	 * Set all columns except the requested column
+	 * output pin as high
+	 */
+	for (i = 0; i < keypad->board->kconnected_cols; i++) {
+		if (i == col)
+			value = 0;
+		else
+			value = 1;
+		gpio_request(keypad->ske_cols[i], "ske-kp");
+		gpio_direction_output(keypad->ske_cols[i], value);
+		gpio_free(keypad->ske_cols[i]);
+	}
+}
+
+static void ske_free_cols(struct ske_keypad *keypad)
+{
+	int i ;
+
+	for (i = 0; i < keypad->board->kconnected_cols; i++) {
+		gpio_request(keypad->ske_cols[i], "ske-kp");
+		gpio_direction_output(keypad->ske_cols[i], 0);
+		gpio_free(keypad->ske_cols[i]);
+	}
+}
+
+static void ske_manual_scan(struct ske_keypad *keypad)
+{
+	int row;
+	int col;
+
+	for (col = 0; col < keypad->board->kconnected_cols; col++) {
+		ske_set_cols(keypad, col);
+		row = ske_read_get_gpio_row(keypad);
+		if (row >= 0) {
+			keypad->key_pressed = 1;
+			keypad->gpio_row = row;
+			keypad->gpio_col = col;
+			break;
+		}
+	}
+	ske_free_cols(keypad);
+}
+
+static irqreturn_t ske_keypad_gpio_irq(int irq, void *dev_id)
+{
+	struct ske_keypad *keypad = dev_id;
+
+	if (!gpio_get_value(NOMADIK_IRQ_TO_GPIO(irq))) {
+		ske_manual_scan(keypad);
+		if (!keypad->enable) {
+			keypad->enable = true;
+			ske_mode_enable(keypad, true);
+			/**
+			 * Schedule the work queue to change it back to GPIO
+			 * mode if there is no activity in SKE mode
+			 */
+			schedule_delayed_work(&keypad->work,
+					      keypad->board->switch_delay);
+		}
+		/**
+		 * Schedule delayed work to report key press if it is not
+		 * detected in SKE mode.
+		 */
+		if (keypad->key_pressed)
+			schedule_delayed_work(&keypad->gpio_work,
+						KEY_PRESSED_DELAY);
+	}
+
+	return IRQ_HANDLED;
+}
 static irqreturn_t ske_keypad_irq(int irq, void *dev_id)
 {
 	struct ske_keypad *keypad = dev_id;
-	int retries = 20;
+	cancel_delayed_work_sync(&keypad->gpio_work);
+	cancel_delayed_work_sync(&keypad->work);
 
 	/* disable auto scan interrupt; mask the interrupt generated */
-	ske_keypad_set_bits(keypad, SKE_IMSC, ~SKE_KPIMA, 0x0);
+	ske_keypad_set_bits(keypad, SKE_IMSC, SKE_KPIMA, 0x0);
 	ske_keypad_set_bits(keypad, SKE_ICR, 0x0, SKE_KPICA);
 
-	while ((readl(keypad->reg_base + SKE_CR) & SKE_KPASON) && --retries)
-		msleep(5);
-
-	if (retries) {
-		/* SKEx registers are stable and can be read */
-		ske_keypad_read_data(keypad);
-	}
-
-	/* enable auto scan interrupts */
-	ske_keypad_set_bits(keypad, SKE_IMSC, 0x0, SKE_KPIMA);
+	schedule_delayed_work(&keypad->scan_work, 0);
 
 	return IRQ_HANDLED;
 }
 
 static int __init ske_keypad_probe(struct platform_device *pdev)
 {
-	const struct ske_keypad_platform_data *plat = pdev->dev.platform_data;
 	struct ske_keypad *keypad;
+	struct resource *res = NULL;
 	struct input_dev *input;
-	struct resource *res;
+	struct clk *clk;
+	void __iomem *reg_base;
+	int ret = 0;
 	int irq;
-	int error;
+	int i;
+	struct ske_keypad_platform_data *plat = pdev->dev.platform_data;
 
 	if (!plat) {
 		dev_err(&pdev->dev, "invalid keypad platform data\n");
@@ -219,42 +582,56 @@ static int __init ske_keypad_probe(struct platform_device *pdev)
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
+	if (res == NULL) {
 		dev_err(&pdev->dev, "missing platform resources\n");
-		return -EINVAL;
+		return -ENXIO;
 	}
 
-	keypad = kzalloc(sizeof(struct ske_keypad), GFP_KERNEL);
-	input = input_allocate_device();
-	if (!keypad || !input) {
-		dev_err(&pdev->dev, "failed to allocate keypad memory\n");
-		error = -ENOMEM;
-		goto err_free_mem;
+	res = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to request I/O memory\n");
+		return -EBUSY;
 	}
 
-	keypad->irq = irq;
-	keypad->board = plat;
-	keypad->input = input;
-	spin_lock_init(&keypad->ske_keypad_lock);
+	reg_base = ioremap(res->start, resource_size(res));
+	if (!reg_base) {
+		dev_err(&pdev->dev, "failed to remap I/O memory\n");
+		ret = -ENXIO;
+		goto out_freerequest_memregions;
+	}
 
-	if (!request_mem_region(res->start, resource_size(res), pdev->name)) {
-		dev_err(&pdev->dev, "failed to request I/O memory\n");
-		error = -EBUSY;
-		goto err_free_mem;
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "failed to clk_get\n");
+		ret = PTR_ERR(clk);
+		goto out_freeioremap;
 	}
 
-	keypad->reg_base = ioremap(res->start, resource_size(res));
-	if (!keypad->reg_base) {
-		dev_err(&pdev->dev, "failed to remap I/O memory\n");
-		error = -ENXIO;
-		goto err_free_mem_region;
+	/* resources are sane; we begin allocation */
+	keypad = kzalloc(sizeof(struct ske_keypad), GFP_KERNEL);
+	if (!keypad) {
+		dev_err(&pdev->dev, "failed to allocate keypad memory\n");
+		goto out_freeclk;
 	}
+	keypad->dev = &pdev->dev;
 
-	keypad->clk = clk_get(&pdev->dev, NULL);
-	if (IS_ERR(keypad->clk)) {
-		dev_err(&pdev->dev, "failed to get clk\n");
-		error = PTR_ERR(keypad->clk);
-		goto err_iounmap;
+	input = input_allocate_device();
+	if (!input) {
+		dev_err(&pdev->dev, "failed to input_allocate_device\n");
+		ret = -ENOMEM;
+		goto out_freekeypad;
+	}
+	keypad->regulator = regulator_get(&pdev->dev, "v-ape");
+	if (IS_ERR(keypad->regulator)) {
+		dev_err(&pdev->dev, "regulator_get failed\n");
+		keypad->regulator = NULL;
+		goto out_regulator_get;
+	} else {
+		ret = regulator_enable(keypad->regulator);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "regulator_enable failed\n");
+			goto out_regulator_enable;
+		}
 	}
 
 	input->id.bustype = BUS_HOST;
@@ -266,38 +643,153 @@ static int __init ske_keypad_probe(struct platform_device *pdev)
 	input->keycodemax = ARRAY_SIZE(keypad->keymap);
 
 	input_set_capability(input, EV_MSC, MSC_SCAN);
+	input_set_drvdata(input, keypad);
 
 	__set_bit(EV_KEY, input->evbit);
 	if (!plat->no_autorepeat)
 		__set_bit(EV_REP, input->evbit);
 
 	matrix_keypad_build_keymap(plat->keymap_data, SKE_KEYPAD_ROW_SHIFT,
-			input->keycode, input->keybit);
+					input->keycode, input->keybit);
+
+	ret = input_register_device(input);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"unable to register input device: %d\n", ret);
+		goto out_freeinput;
+	}
+
+	keypad->irq	= irq;
+	keypad->board	= plat;
+	keypad->input	= input;
+	keypad->reg_base = reg_base;
+	keypad->clk	= clk;
+	INIT_DELAYED_WORK(&keypad->work, ske_gpio_switch_work);
+	INIT_DELAYED_WORK(&keypad->gpio_work, ske_gpio_release_work);
+	INIT_DELAYED_WORK(&keypad->scan_work, ske_keypad_scan_work);
 
+	/* allocations are sane, we begin HW initialization */
 	clk_enable(keypad->clk);
 
-	/* go through board initialization helpers */
-	if (keypad->board->init)
-		keypad->board->init();
+	if (!keypad->board->init) {
+		dev_err(&pdev->dev, "init funtion not defined\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
 
-	error = ske_keypad_chip_init(keypad);
-	if (error) {
-		dev_err(&pdev->dev, "unable to init keypad hardware\n");
-		goto err_clk_disable;
+	if (keypad->board->init() < 0) {
+		dev_err(&pdev->dev, "keyboard init config failed\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
 	}
 
-	error = request_threaded_irq(keypad->irq, NULL, ske_keypad_irq,
-				     IRQF_ONESHOT, "ske-keypad", keypad);
-	if (error) {
-		dev_err(&pdev->dev, "allocate irq %d failed\n", keypad->irq);
-		goto err_clk_disable;
+	if (!keypad->board->exit) {
+		dev_err(&pdev->dev, "exit funtion not defined\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
+
+	if (keypad->board->exit() < 0) {
+		dev_err(&pdev->dev,  "keyboard exit config failed\n");
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
+
+	if (plat->kconnected_rows == 0) {
+		/*
+		 * Board config data does not specify the number of connected
+		 * rows and columns; assume that it matches the specified max
+		 * values.
+		 */
+		plat->kconnected_rows = plat->krow;
+		plat->kconnected_cols = plat->kcol;
+	}
+
+	/* this code doesn't currently support non-square keypad */
+	if (plat->kconnected_rows != plat->kconnected_cols) {
+		dev_err(&pdev->dev,
+			"invalid keypad configuration (not square)\n"),
+		ret = -EINVAL;
+		goto out_unregisterinput;
 	}
 
-	error = input_register_device(input);
-	if (error) {
+	if (plat->kconnected_rows > SKE_KPD_MAX_ROWS ||
+		plat->kconnected_cols > SKE_KPD_MAX_COLS) {
 		dev_err(&pdev->dev,
-				"unable to register input device: %d\n", error);
-		goto err_free_irq;
+			"invalid keypad configuration (too many rows/cols)\n"),
+		ret = -EINVAL;
+		goto out_unregisterinput;
+	}
+
+	keypad->gpio_input_irq = kmalloc(sizeof(*keypad->gpio_input_irq) *
+						plat->kconnected_rows,
+						GFP_KERNEL);
+	if (!keypad->gpio_input_irq) {
+		dev_err(&pdev->dev, "failed to allocate input_irq memory\n");
+		goto out_unregisterinput;
+	}
+
+	keypad->ske_rows = kmalloc(sizeof(*keypad->ske_rows) *
+					plat->kconnected_rows, GFP_KERNEL);
+	if (!keypad->ske_rows) {
+		dev_err(&pdev->dev, "failed to allocate ske_rows memory\n");
+		goto out_freemem_input_irq;
+	}
+
+	keypad->ske_cols = kmalloc(sizeof(*keypad->ske_cols) *
+					plat->kconnected_cols, GFP_KERNEL);
+	if (!keypad->ske_cols) {
+		dev_err(&pdev->dev, "failed to allocate ske_cols memory\n");
+		goto out_freemem_rows;
+	}
+
+	keypad->keys = kzalloc(sizeof(*keypad->keys) * plat->krow, GFP_KERNEL);
+	if (!keypad->keys) {
+		dev_err(&pdev->dev, "failed to allocate keys:rows memory\n");
+		goto out_freemem_cols;
+	}
+	for (i = 0; i < plat->krow; i++) {
+		keypad->keys[i] = kzalloc(sizeof(*keypad->keys[i]) *
+						plat->kcol, GFP_KERNEL);
+		if (!keypad->keys[i]) {
+			dev_err(&pdev->dev,
+				"failed to allocate keys:cols memory\n");
+			goto out_freemem_keys;
+		}
+	}
+
+	for (i = 0; i < plat->kconnected_rows; i++) {
+		keypad->ske_rows[i] = *plat->gpio_input_pins;
+		keypad->ske_cols[i] = *plat->gpio_output_pins;
+		keypad->gpio_input_irq[i] =
+				NOMADIK_GPIO_TO_IRQ(keypad->ske_rows[i]);
+	}
+
+	for (i = 0; i < keypad->board->kconnected_rows; i++) {
+		ret =  request_threaded_irq(keypad->gpio_input_irq[i],
+				NULL, ske_keypad_gpio_irq,
+				IRQF_TRIGGER_FALLING | IRQF_NO_SUSPEND,
+				"ske-keypad-gpio", keypad);
+		if (ret) {
+			dev_err(&pdev->dev, "allocate gpio irq %d failed\n",
+						keypad->gpio_input_irq[i]);
+			goto out_freemem_keys;
+		}
+		enable_irq_wake(keypad->gpio_input_irq[i]);
+	}
+
+	ret =  request_threaded_irq(keypad->irq, NULL, ske_keypad_irq,
+				IRQF_ONESHOT, "ske-keypad", keypad);
+	if (ret) {
+		dev_err(&pdev->dev, "allocate irq %d failed\n", keypad->irq);
+		goto out_freemem_keys;
+	}
+
+	/* sysfs implementation for dynamic enable/disable the input event */
+	ret = sysfs_create_group(&pdev->dev.kobj, &ske_attr_group);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to create sysfs entries\n");
+		goto out_free_irq;
 	}
 
 	if (plat->wakeup_enable)
@@ -305,37 +797,80 @@ static int __init ske_keypad_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, keypad);
 
+	clk_disable(keypad->clk);
+	regulator_disable(keypad->regulator);
+
 	return 0;
 
-err_free_irq:
+out_free_irq:
 	free_irq(keypad->irq, keypad);
-err_clk_disable:
+out_freemem_keys:
+	for (i = 0; i < plat->krow; i++)
+		kfree(keypad->keys[i]);
+	kfree(keypad->keys);
+out_freemem_cols:
+	kfree(keypad->ske_cols);
+out_freemem_rows:
+	kfree(keypad->ske_rows);
+out_freemem_input_irq:
+	kfree(keypad->gpio_input_irq);
+out_unregisterinput:
+	input_unregister_device(input);
+	input = NULL;
 	clk_disable(keypad->clk);
-	clk_put(keypad->clk);
-err_iounmap:
-	iounmap(keypad->reg_base);
-err_free_mem_region:
-	release_mem_region(res->start, resource_size(res));
-err_free_mem:
+out_freeinput:
+	regulator_disable(keypad->regulator);
+out_regulator_enable:
+	regulator_put(keypad->regulator);
+out_regulator_get:
 	input_free_device(input);
+out_freekeypad:
 	kfree(keypad);
-	return error;
+out_freeclk:
+	clk_put(clk);
+out_freeioremap:
+	iounmap(reg_base);
+out_freerequest_memregions:
+	release_mem_region(res->start, resource_size(res));
+	return ret;
 }
 
 static int __devexit ske_keypad_remove(struct platform_device *pdev)
 {
 	struct ske_keypad *keypad = platform_get_drvdata(pdev);
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	int i;
 
-	free_irq(keypad->irq, keypad);
+	cancel_delayed_work_sync(&keypad->gpio_work);
+	cancel_delayed_work_sync(&keypad->work);
+	cancel_delayed_work_sync(&keypad->scan_work);
 
-	input_unregister_device(keypad->input);
+	for (i = 0; i < keypad->board->krow; i++)
+		kfree(keypad->keys[i]);
+	kfree(keypad->keys);
+	kfree(keypad->ske_cols);
+	kfree(keypad->ske_rows);
 
-	clk_disable(keypad->clk);
+	input_unregister_device(keypad->input);
+	sysfs_remove_group(&pdev->dev.kobj, &ske_attr_group);
+	if (keypad->enable)
+		clk_disable(keypad->clk);
 	clk_put(keypad->clk);
 
-	if (keypad->board->exit)
+	if (keypad->enable && keypad->board->exit)
 		keypad->board->exit();
+	else {
+		for (i = 0; i < keypad->board->krow; i++) {
+			disable_irq_nosync(keypad->gpio_input_irq[i]);
+			disable_irq_wake(keypad->gpio_input_irq[i]);
+		}
+	}
+	for (i = 0; i < keypad->board->krow; i++)
+		free_irq(keypad->gpio_input_irq[i], keypad);
+
+	kfree(keypad->gpio_input_irq);
+	free_irq(keypad->irq, keypad);
+	regulator_put(keypad->regulator);
 
 	iounmap(keypad->reg_base);
 	release_mem_region(res->start, resource_size(res));
@@ -353,8 +888,19 @@ static int ske_keypad_suspend(struct device *dev)
 
 	if (device_may_wakeup(dev))
 		enable_irq_wake(irq);
-	else
-		ske_keypad_set_bits(keypad, SKE_IMSC, ~SKE_KPIMA, 0x0);
+	else {
+		cancel_delayed_work_sync(&keypad->gpio_work);
+		cancel_delayed_work_sync(&keypad->work);
+		cancel_delayed_work_sync(&keypad->scan_work);
+		disable_irq(irq);
+
+		keypad->enable_on_resume = keypad->enable;
+
+		if (keypad->enable) {
+			ske_mode_enable(keypad, false);
+			keypad->enable = false;
+		}
+	}
 
 	return 0;
 }
@@ -367,8 +913,20 @@ static int ske_keypad_resume(struct device *dev)
 
 	if (device_may_wakeup(dev))
 		disable_irq_wake(irq);
-	else
-		ske_keypad_set_bits(keypad, SKE_IMSC, 0x0, SKE_KPIMA);
+	else {
+		if (keypad->enable_on_resume && !keypad->enable) {
+			keypad->enable = true;
+			ske_mode_enable(keypad, true);
+			/*
+			 * Schedule the work queue to change it to GPIO mode
+			 * if there is no activity in SKE mode
+			 */
+			if (!keypad->key_pressed)
+				schedule_delayed_work(&keypad->work,
+						keypad->board->switch_delay);
+		}
+		enable_irq(irq);
+	}
 
 	return 0;
 }
@@ -399,6 +957,6 @@ static void __exit ske_keypad_exit(void)
 module_exit(ske_keypad_exit);
 
 MODULE_LICENSE("GPL v2");
-MODULE_AUTHOR("Naveen Kumar <naveen.gaddipati@stericsson.com> / Sundar Iyer <sundar.iyer@stericsson.com>");
+MODULE_AUTHOR("Naveen Kumar <naveen.gaddipati@stericsson.com>");
 MODULE_DESCRIPTION("Nomadik Scroll-Key-Encoder Keypad Driver");
 MODULE_ALIAS("platform:nomadik-ske-keypad");
diff --git a/drivers/input/keyboard/stmpe-keypad.c b/drivers/input/keyboard/stmpe-keypad.c
index 9397cf9c625..892335275dd 100644
--- a/drivers/input/keyboard/stmpe-keypad.c
+++ b/drivers/input/keyboard/stmpe-keypad.c
@@ -108,10 +108,52 @@ struct stmpe_keypad {
 
 	unsigned int rows;
 	unsigned int cols;
+	bool enable;
 
 	unsigned short keymap[STMPE_KEYPAD_KEYMAP_SIZE];
 };
 
+static ssize_t stmpe_show_attr_enable(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct stmpe_keypad *keypad = platform_get_drvdata(pdev);
+	return sprintf(buf, "%d\n", keypad->enable);
+}
+
+static ssize_t stmpe_store_attr_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct stmpe_keypad *keypad = platform_get_drvdata(pdev);
+	struct stmpe *stmpe = keypad->stmpe;
+	unsigned long val;
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if (keypad->enable != val) {
+		keypad->enable = val;
+		if (!val)
+			stmpe_disable(stmpe, STMPE_BLOCK_KEYPAD);
+		else
+			stmpe_enable(stmpe, STMPE_BLOCK_KEYPAD);
+	}
+	return count;
+}
+
+static DEVICE_ATTR(enable, S_IWUSR | S_IRUGO,
+	stmpe_show_attr_enable, stmpe_store_attr_enable);
+
+static struct attribute *stmpe_keypad_attrs[] = {
+	&dev_attr_enable.attr,
+	NULL,
+};
+
+static struct attribute_group stmpe_attr_group = {
+	.attrs = stmpe_keypad_attrs,
+};
+
 static int stmpe_keypad_read_data(struct stmpe_keypad *keypad, u8 *data)
 {
 	const struct stmpe_keypad_variant *variant = keypad->variant;
@@ -285,7 +327,7 @@ static int __devinit stmpe_keypad_probe(struct platform_device *pdev)
 		goto out_freekeypad;
 	}
 
-	input->name = "STMPE keypad";
+	input->name = "STMPE-keypad";
 	input->id.bustype = BUS_I2C;
 	input->dev.parent = &pdev->dev;
 
@@ -332,10 +374,20 @@ static int __devinit stmpe_keypad_probe(struct platform_device *pdev)
 		goto out_unregisterinput;
 	}
 
+	/* sysfs implementation for dynamic enable/disable the input event */
+	ret = sysfs_create_group(&pdev->dev.kobj, &stmpe_attr_group);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to create sysfs entries\n");
+		goto out_free_irq;
+	}
+
+	keypad->enable = true;
 	platform_set_drvdata(pdev, keypad);
 
 	return 0;
 
+out_free_irq:
+	free_irq(irq, keypad);
 out_unregisterinput:
 	input_unregister_device(input);
 	input = NULL;
@@ -354,6 +406,7 @@ static int __devexit stmpe_keypad_remove(struct platform_device *pdev)
 
 	stmpe_disable(stmpe, STMPE_BLOCK_KEYPAD);
 
+	sysfs_remove_group(&pdev->dev.kobj, &stmpe_attr_group);
 	free_irq(irq, keypad);
 	input_unregister_device(keypad->input);
 	platform_set_drvdata(pdev, NULL);
@@ -362,9 +415,43 @@ static int __devexit stmpe_keypad_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int stmpe_keypad_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct stmpe_keypad *keypad = platform_get_drvdata(pdev);
+	struct stmpe *stmpe = keypad->stmpe;
+
+	if (!device_may_wakeup(stmpe->dev))
+		stmpe_disable(stmpe, STMPE_BLOCK_KEYPAD);
+
+	return 0;
+}
+
+static int stmpe_keypad_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct stmpe_keypad *keypad = platform_get_drvdata(pdev);
+	struct stmpe *stmpe = keypad->stmpe;
+
+	if (!device_may_wakeup(stmpe->dev))
+		stmpe_enable(stmpe, STMPE_BLOCK_KEYPAD);
+
+	return 0;
+}
+
+static const struct dev_pm_ops stmpe_keypad_dev_pm_ops = {
+	.suspend = stmpe_keypad_suspend,
+	.resume = stmpe_keypad_resume,
+};
+#endif
+
 static struct platform_driver stmpe_keypad_driver = {
 	.driver.name	= "stmpe-keypad",
 	.driver.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+	.driver.pm	= &stmpe_keypad_dev_pm_ops,
+#endif
 	.probe		= stmpe_keypad_probe,
 	.remove		= __devexit_p(stmpe_keypad_remove),
 };
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 7faf4a7fcaa..1597e9f51cb 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -22,12 +22,26 @@ config INPUT_88PM860X_ONKEY
 	  To compile this driver as a module, choose M here: the module
 	  will be called 88pm860x_onkey.
 
+config INPUT_AB8500_ACCDET
+	bool "AB8500 AV Accessory detection"
+	depends on AB8500_CORE && AB8500_GPADC && GPIO_AB8500
+	help
+	  Say Y here to enable AV accessory detection features for ST-Ericsson's
+	  AB8500 Mix-Sig PMIC.
+
+config INPUT_AB5500_ACCDET
+	bool "AB5500 AV Accessory detection"
+	depends on AB5500_CORE && AB5500_GPADC
+	help
+	  Say Y here to enable AV accessory detection features for ST-Ericsson's
+	  AB5500 Mix-Sig PMIC.
+
 config INPUT_AB8500_PONKEY
-	tristate "AB8500 Pon (PowerOn) Key"
-	depends on AB8500_CORE
+	tristate "AB5500/AB8500 Pon (PowerOn) Key"
+	depends on AB5500_CORE || AB8500_CORE
 	help
-	  Say Y here to use the PowerOn Key for ST-Ericsson's AB8500
-	  Mix-Sig PMIC.
+	  Say Y here to use the PowerOn Key for ST-Ericsson's AB5500/AB8500
+	  Mix-Sig PMICs.
 
 	  To compile this driver as a module, choose M here: the module
 	  will be called ab8500-ponkey.
@@ -312,6 +326,22 @@ config INPUT_KXTJ9_POLLED_MODE
 	help
 	  Say Y here if you need accelerometer to work in polling mode.
 
+config INPUT_LPS001WP
+	tristate "LPS0001WP pressure sensor from ST Micro"
+	default y if MACH_U8500
+	help
+	  This is a pressure sensor connected to I2C, mounted on the
+	  snowball and other ST-E boards
+
+config LPS001WP_INPUT_DEVICE
+         bool "ST LPS001WP INPUT DEVICE"
+         depends on INPUT_LPS001WP
+         default n
+         help
+	   This driver allows device to be used as an input device
+	   need to be enabled only when input device support
+	   is required.
+
 config INPUT_POWERMATE
 	tristate "Griffin PowerMate and Contour Jog support"
 	depends on USB_ARCH_HAS_HCD
@@ -590,4 +620,14 @@ config INPUT_XEN_KBDDEV_FRONTEND
 	  To compile this driver as a module, choose M here: the
 	  module will be called xen-kbdfront.
 
+config INPUT_STE_FF_VIBRA
+	tristate "ST-Ericsson Force Feedback Vibrator"
+	depends on STE_AUDIO_IO_DEV
+	select INPUT_FF_MEMLESS
+	help
+	  This option enables support for ST-Ericsson's Vibrator which
+	  registers as an input force feedback driver.
+
+	  To compile this driver as a module, choose M here. The module will
+	  be called ste_ff_vibra.
 endif
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index f55cdf4916f..4c96bc978d9 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -5,6 +5,8 @@
 # Each configuration option enables a list of files.
 
 obj-$(CONFIG_INPUT_88PM860X_ONKEY)	+= 88pm860x_onkey.o
+obj-$(CONFIG_INPUT_AB8500_ACCDET)       += abx500-accdet.o ab8500-accdet.o
+obj-$(CONFIG_INPUT_AB5500_ACCDET)	+= abx500-accdet.o ab5500-accdet.o
 obj-$(CONFIG_INPUT_AB8500_PONKEY)	+= ab8500-ponkey.o
 obj-$(CONFIG_INPUT_AD714X)		+= ad714x.o
 obj-$(CONFIG_INPUT_AD714X_I2C)		+= ad714x-i2c.o
@@ -29,6 +31,7 @@ obj-$(CONFIG_HP_SDC_RTC)		+= hp_sdc_rtc.o
 obj-$(CONFIG_INPUT_IXP4XX_BEEPER)	+= ixp4xx-beeper.o
 obj-$(CONFIG_INPUT_KEYSPAN_REMOTE)	+= keyspan_remote.o
 obj-$(CONFIG_INPUT_KXTJ9)		+= kxtj9.o
+obj-$(CONFIG_INPUT_LPS001WP)		+= lps001wp_prs.o
 obj-$(CONFIG_INPUT_M68K_BEEP)		+= m68kspkr.o
 obj-$(CONFIG_INPUT_MAX8925_ONKEY)	+= max8925_onkey.o
 obj-$(CONFIG_INPUT_MAX8997_HAPTIC)	+= max8997_haptic.o
@@ -55,3 +58,4 @@ obj-$(CONFIG_INPUT_WISTRON_BTNS)	+= wistron_btns.o
 obj-$(CONFIG_INPUT_WM831X_ON)		+= wm831x-on.o
 obj-$(CONFIG_INPUT_XEN_KBDDEV_FRONTEND)	+= xen-kbdfront.o
 obj-$(CONFIG_INPUT_YEALINK)		+= yealink.o
+obj-$(CONFIG_INPUT_STE_FF_VIBRA)	+= ste_ff_vibra.o
diff --git a/drivers/input/misc/ab5500-accdet.c b/drivers/input/misc/ab5500-accdet.c
new file mode 100644
index 00000000000..fa8e2523126
--- /dev/null
+++ b/drivers/input/misc/ab5500-accdet.c
@@ -0,0 +1,284 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Jarmo K. Kuronen <jarmo.kuronen@symbio.com>
+ *         for ST-Ericsson.
+ *
+ * License terms: GPL V2
+ *
+ * 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 <linux/mfd/abx500/ab5500.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+#include <linux/mfd/abx500.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/input/abx500-accdet.h>
+
+/*
+ * Register definition for accessory detection.
+ */
+#define AB5500_REGU_CTRL1_SPARE_REG	0x84
+#define AB5500_ACC_DET_DB1_REG		0x20
+#define AB5500_ACC_DET_DB2_REG		0x21
+#define AB5500_ACC_DET_CTRL_REG		0x23
+#define AB5500_VDENC_CTRL0		0x80
+
+/* REGISTER: AB8500_ACC_DET_CTRL_REG */
+#define BITS_ACCDETCTRL2_ENA		(0x20 | 0x10 | 0x08)
+#define BITS_ACCDETCTRL1_ENA		(0x02 | 0x01)
+
+/* REGISTER: AB8500_REGU_CTRL1_SPARE_REG */
+#define BIT_REGUCTRL1SPARE_VAMIC1_GROUND	0x01
+
+/* REGISTER: AB8500_IT_SOURCE5_REG */
+#define BIT_ITSOURCE5_ACCDET1		0x02
+
+static struct accessory_irq_descriptor ab5500_irq_desc[] = {
+	{
+		.irq = PLUG_IRQ,
+		.name = "acc_detedt1db_falling",
+		.isr = plug_irq_handler,
+	},
+	{
+		.irq = UNPLUG_IRQ,
+		.name = "acc_detedt1db_rising",
+		.isr = unplug_irq_handler,
+	},
+	{
+		.irq = BUTTON_PRESS_IRQ,
+		.name = "acc_detedt21db_falling",
+		.isr = button_press_irq_handler,
+	},
+	{
+		.irq = BUTTON_RELEASE_IRQ,
+		.name = "acc_detedt21db_rising",
+		.isr = button_release_irq_handler,
+	},
+};
+
+static struct accessory_regu_descriptor ab5500_regu_desc[] = {
+	{
+		.id = REGULATOR_VAMIC1,
+		.name = "v-amic",
+	},
+};
+
+
+/*
+ * configures accdet2 input on/off
+ */
+static void ab5500_config_accdetect2_hw(struct abx500_ad *dd, int enable)
+{
+	int ret = 0;
+
+	if (!dd->accdet2_th_set) {
+		/* Configure accdetect21+22 thresholds */
+		ret = abx500_set_register_interruptible(&dd->pdev->dev,
+				AB5500_BANK_FG_BATTCOM_ACC,
+				AB5500_ACC_DET_DB2_REG,
+				dd->pdata->accdet2122_th);
+		if (ret < 0) {
+			dev_err(&dd->pdev->dev,
+				"%s: Failed to write reg (%d).\n", __func__,
+				ret);
+			goto out;
+		} else {
+			dd->accdet2_th_set = 1;
+		}
+	}
+
+	/* Enable/Disable accdetect21 comparators + pullup */
+	ret = abx500_mask_and_set_register_interruptible(
+			&dd->pdev->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_ACC_DET_CTRL_REG,
+			BITS_ACCDETCTRL2_ENA,
+			enable ? BITS_ACCDETCTRL2_ENA : 0);
+
+	if (ret < 0)
+		dev_err(&dd->pdev->dev, "%s: Failed to update reg (%d).\n",
+				__func__, ret);
+out:
+	return;
+}
+
+/*
+ * configures accdet1 input on/off
+ */
+static void ab5500_config_accdetect1_hw(struct abx500_ad *dd, int enable)
+{
+	int ret;
+
+	if (!dd->accdet1_th_set) {
+		ret = abx500_set_register_interruptible(&dd->pdev->dev,
+				AB5500_BANK_FG_BATTCOM_ACC,
+				AB5500_ACC_DET_DB1_REG,
+				dd->pdata->accdet1_dbth);
+		if (ret < 0)
+			dev_err(&dd->pdev->dev,
+				"%s: Failed to write reg (%d).\n", __func__,
+				ret);
+		else
+			dd->accdet1_th_set = 1;
+	}
+
+	/* enable accdetect1 comparator */
+	ret = abx500_mask_and_set_register_interruptible(
+				&dd->pdev->dev,
+				AB5500_BANK_FG_BATTCOM_ACC,
+				AB5500_ACC_DET_CTRL_REG,
+				BITS_ACCDETCTRL1_ENA,
+				enable ? BITS_ACCDETCTRL1_ENA : 0);
+
+	if (ret < 0)
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to update reg (%d).\n", __func__, ret);
+}
+
+/*
+ * returns the high level status whether some accessory is connected (1|0).
+ */
+static int ab5500_detect_plugged_in(struct abx500_ad *dd)
+{
+	u8 value = 0;
+
+	int status = abx500_get_register_interruptible(
+				&dd->pdev->dev,
+				AB5500_BANK_IT,
+				AB5500_IT_SOURCE3_REG,
+				&value);
+	if (status < 0) {
+		dev_err(&dd->pdev->dev, "%s: reg read failed (%d).\n",
+			__func__, status);
+		return 0;
+	}
+
+	if (dd->pdata->is_detection_inverted)
+		return value & BIT_ITSOURCE5_ACCDET1 ? 1 : 0;
+	else
+		return value & BIT_ITSOURCE5_ACCDET1 ? 0 : 1;
+}
+
+/*
+ * mic_line_voltage_stable - measures a relative stable voltage from spec. input
+ */
+static int ab5500_meas_voltage_stable(struct abx500_ad *dd)
+{
+	int iterations = 2;
+	int v1, v2, dv;
+
+	v1 = ab5500_gpadc_convert((struct ab5500_gpadc *)dd->gpadc,
+			ACC_DETECT2);
+	do {
+		msleep(1);
+		--iterations;
+		v2 = ab5500_gpadc_convert((struct ab5500_gpadc *)dd->gpadc,
+				ACC_DETECT2);
+		dv = abs(v2 - v1);
+		v1 = v2;
+	} while (iterations > 0 && dv > MAX_VOLT_DIFF);
+
+	return v1;
+}
+
+/*
+ * not implemented
+ */
+static int ab5500_meas_alt_voltage_stable(struct abx500_ad *dd)
+{
+	return -1;
+}
+
+/*
+ * configures HW so that it is possible to make decision whether
+ * accessory is connected or not.
+ */
+static void ab5500_config_hw_test_plug_connected(struct abx500_ad *dd,
+		int enable)
+{
+	dev_dbg(&dd->pdev->dev, "%s:%d\n", __func__, enable);
+
+	/* enable mic BIAS2 */
+	if (enable)
+		accessory_regulator_enable(dd, REGULATOR_VAMIC1);
+}
+
+/*
+ * configures HW so that carkit/headset detection can be accomplished.
+ */
+static void ab5500_config_hw_test_basic_carkit(struct abx500_ad *dd, int enable)
+{
+	/* enable mic BIAS2 */
+	if (enable)
+		accessory_regulator_disable(dd, REGULATOR_VAMIC1);
+}
+
+static u8 acc_det_ctrl_suspend_val;
+
+static void ab5500_turn_off_accdet_comparator(struct platform_device *pdev)
+{
+	struct abx500_ad *dd = platform_get_drvdata(pdev);
+
+	/* Turn off AccDetect comparators and pull-up */
+	(void) abx500_get_register_interruptible(
+			&dd->pdev->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_ACC_DET_CTRL_REG,
+			&acc_det_ctrl_suspend_val);
+	(void) abx500_set_register_interruptible(
+			&dd->pdev->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_ACC_DET_CTRL_REG,
+			0);
+}
+
+static void ab5500_turn_on_accdet_comparator(struct platform_device *pdev)
+{
+	struct abx500_ad *dd = platform_get_drvdata(pdev);
+
+	/* Turn on AccDetect comparators and pull-up */
+	(void) abx500_set_register_interruptible(
+			&dd->pdev->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_ACC_DET_CTRL_REG,
+			acc_det_ctrl_suspend_val);
+}
+
+static void *ab5500_accdet_abx500_gpadc_get(void)
+{
+	return ab5500_gpadc_get("ab5500-adc.0");
+}
+
+struct abx500_accdet_platform_data *
+	ab5500_get_platform_data(struct platform_device *pdev)
+{
+	return pdev->dev.platform_data;
+}
+
+struct abx500_ad ab5500_accessory_det_callbacks = {
+	.irq_desc_norm			= ab5500_irq_desc,
+	.irq_desc_inverted		= NULL,
+	.no_irqs			= ARRAY_SIZE(ab5500_irq_desc),
+	.regu_desc			= ab5500_regu_desc,
+	.no_of_regu_desc		= ARRAY_SIZE(ab5500_regu_desc),
+	.config_accdetect2_hw		= ab5500_config_accdetect2_hw,
+	.config_accdetect1_hw		= ab5500_config_accdetect1_hw,
+	.detect_plugged_in		= ab5500_detect_plugged_in,
+	.meas_voltage_stable		= ab5500_meas_voltage_stable,
+	.meas_alt_voltage_stable	= ab5500_meas_alt_voltage_stable,
+	.config_hw_test_basic_carkit	= ab5500_config_hw_test_basic_carkit,
+	.turn_off_accdet_comparator	= ab5500_turn_off_accdet_comparator,
+	.turn_on_accdet_comparator	= ab5500_turn_on_accdet_comparator,
+	.accdet_abx500_gpadc_get	= ab5500_accdet_abx500_gpadc_get,
+	.config_hw_test_plug_connected	= ab5500_config_hw_test_plug_connected,
+	.set_av_switch			= NULL,
+	.get_platform_data		= ab5500_get_platform_data,
+};
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/misc/ab8500-accdet.c b/drivers/input/misc/ab8500-accdet.c
new file mode 100644
index 00000000000..1b96b6b3fef
--- /dev/null
+++ b/drivers/input/misc/ab8500-accdet.c
@@ -0,0 +1,464 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Jarmo K. Kuronen <jarmo.kuronen@symbio.com>
+ *         for ST-Ericsson.
+ *
+ * License terms: GPL V2
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/abx500.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/abx500/ab8500-gpadc.h>
+#include <linux/mfd/abx500/ab8500-gpio.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+#include <linux/input/abx500-accdet.h>
+#ifdef CONFIG_SND_SOC_UX500_AB8500
+#include <sound/ux500_ab8500_ext.h>
+#endif
+
+#define MAX_DET_COUNT			10
+#define MAX_VOLT_DIFF			30
+#define MIN_MIC_POWER			-100
+
+/* Unique value used to identify Headset button input device */
+#define BTN_INPUT_UNIQUE_VALUE	"AB8500HsBtn"
+#define BTN_INPUT_DEV_NAME	"AB8500 Hs Button"
+
+#define DEBOUNCE_PLUG_EVENT_MS		100
+#define DEBOUNCE_PLUG_RETEST_MS		25
+#define DEBOUNCE_UNPLUG_EVENT_MS	0
+
+/*
+ * Register definition for accessory detection.
+ */
+#define AB8500_REGU_CTRL1_SPARE_REG	0x84
+#define AB8500_ACC_DET_DB1_REG		0x80
+#define AB8500_ACC_DET_DB2_REG		0x81
+#define AB8500_ACC_DET_CTRL_REG		0x82
+#define AB8500_IT_SOURCE5_REG		0x04
+
+/* REGISTER: AB8500_ACC_DET_CTRL_REG */
+#define BITS_ACCDETCTRL2_ENA		(0x20 | 0x10 | 0x08)
+#define BITS_ACCDETCTRL1_ENA		(0x02 | 0x01)
+
+/* REGISTER: AB8500_REGU_CTRL1_SPARE_REG */
+#define BIT_REGUCTRL1SPARE_VAMIC1_GROUND	0x01
+
+/* REGISTER: AB8500_IT_SOURCE5_REG */
+#define BIT_ITSOURCE5_ACCDET1		0x04
+
+/* After being loaded, how fast the first check is to be made */
+#define INIT_DELAY_MS			3000
+
+/* Voltage limits (mV) for various types of AV Accessories */
+#define ACCESSORY_DET_VOL_DONTCARE	-1
+#define ACCESSORY_HEADPHONE_DET_VOL_MIN	0
+#define ACCESSORY_HEADPHONE_DET_VOL_MAX	40
+#define ACCESSORY_CARKIT_DET_VOL_MIN	1100
+#define ACCESSORY_CARKIT_DET_VOL_MAX	1300
+#define ACCESSORY_HEADSET_DET_VOL_MIN	0
+#define ACCESSORY_HEADSET_DET_VOL_MAX	200
+#define ACCESSORY_OPENCABLE_DET_VOL_MIN	1730
+#define ACCESSORY_OPENCABLE_DET_VOL_MAX	2150
+
+/* Static data initialization */
+
+static struct accessory_regu_descriptor ab8500_regu_desc[3] = {
+	{
+		.id = REGULATOR_VAUDIO,
+		.name = "v-audio",
+	},
+	{
+		.id = REGULATOR_VAMIC1,
+		.name = "v-amic1",
+	},
+	{
+		.id = REGULATOR_AVSWITCH,
+		.name = "vcc-N2158",
+	},
+};
+
+static struct accessory_irq_descriptor ab8500_irq_desc_norm[] = {
+	{
+		.irq = PLUG_IRQ,
+		.name = "ACC_DETECT_1DB_F",
+		.isr = plug_irq_handler,
+	},
+	{
+		.irq = UNPLUG_IRQ,
+		.name = "ACC_DETECT_1DB_R",
+		.isr = unplug_irq_handler,
+	},
+	{
+		.irq = BUTTON_PRESS_IRQ,
+		.name = "ACC_DETECT_22DB_F",
+		.isr = button_press_irq_handler,
+	},
+	{
+		.irq = BUTTON_RELEASE_IRQ,
+		.name = "ACC_DETECT_22DB_R",
+		.isr = button_release_irq_handler,
+	},
+};
+
+static struct accessory_irq_descriptor ab8500_irq_desc_inverted[] = {
+	{
+		.irq = PLUG_IRQ,
+		.name = "ACC_DETECT_1DB_R",
+		.isr = plug_irq_handler,
+	},
+	{
+		.irq = UNPLUG_IRQ,
+		.name = "ACC_DETECT_1DB_F",
+		.isr = unplug_irq_handler,
+	},
+	{
+		.irq = BUTTON_PRESS_IRQ,
+		.name = "ACC_DETECT_22DB_R",
+		.isr = button_press_irq_handler,
+	},
+	{
+		.irq = BUTTON_RELEASE_IRQ,
+		.name = "ACC_DETECT_22DB_F",
+		.isr = button_release_irq_handler,
+	},
+};
+
+/*
+ * configures accdet2 input on/off
+ */
+static void ab8500_config_accdetect2_hw(struct abx500_ad *dd, int enable)
+{
+	int ret = 0;
+
+	if (!dd->accdet2_th_set) {
+		/* Configure accdetect21+22 thresholds */
+		ret = abx500_set_register_interruptible(&dd->pdev->dev,
+				AB8500_ECI_AV_ACC,
+				AB8500_ACC_DET_DB2_REG,
+				dd->pdata->accdet2122_th);
+		if (ret < 0) {
+			dev_err(&dd->pdev->dev,
+				"%s: Failed to write reg (%d).\n", __func__,
+					ret);
+			goto out;
+		} else {
+			dd->accdet2_th_set = 1;
+		}
+	}
+
+	/* Enable/Disable accdetect21 comparators + pullup */
+	ret = abx500_mask_and_set_register_interruptible(
+			&dd->pdev->dev,
+			AB8500_ECI_AV_ACC,
+			AB8500_ACC_DET_CTRL_REG,
+			BITS_ACCDETCTRL2_ENA,
+			enable ? BITS_ACCDETCTRL2_ENA : 0);
+
+	if (ret < 0)
+		dev_err(&dd->pdev->dev, "%s: Failed to update reg (%d).\n",
+				__func__, ret);
+
+out:
+	return;
+}
+
+/*
+ * configures accdet1 input on/off
+ */
+static void ab8500_config_accdetect1_hw(struct abx500_ad *dd, int enable)
+{
+	int ret;
+
+	if (!dd->accdet1_th_set) {
+		ret = abx500_set_register_interruptible(&dd->pdev->dev,
+				AB8500_ECI_AV_ACC,
+				AB8500_ACC_DET_DB1_REG,
+				dd->pdata->accdet1_dbth);
+		if (ret < 0)
+			dev_err(&dd->pdev->dev,
+				"%s: Failed to write reg (%d).\n", __func__,
+				ret);
+		else
+			dd->accdet1_th_set = 1;
+	}
+
+	/* enable accdetect1 comparator */
+	ret = abx500_mask_and_set_register_interruptible(
+				&dd->pdev->dev,
+				AB8500_ECI_AV_ACC,
+				AB8500_ACC_DET_CTRL_REG,
+				BITS_ACCDETCTRL1_ENA,
+				enable ? BITS_ACCDETCTRL1_ENA : 0);
+
+	if (ret < 0)
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to update reg (%d).\n", __func__, ret);
+}
+
+/*
+ * returns the high level status whether some accessory is connected (1|0).
+ */
+static int ab8500_detect_plugged_in(struct abx500_ad *dd)
+{
+	u8 value = 0;
+
+	int status = abx500_get_register_interruptible(
+				&dd->pdev->dev,
+				AB8500_INTERRUPT,
+				AB8500_IT_SOURCE5_REG,
+				&value);
+	if (status < 0) {
+		dev_err(&dd->pdev->dev, "%s: reg read failed (%d).\n",
+			__func__, status);
+		return 0;
+	}
+
+	if (dd->pdata->is_detection_inverted)
+		return value & BIT_ITSOURCE5_ACCDET1 ? 1 : 0;
+	else
+		return value & BIT_ITSOURCE5_ACCDET1 ? 0 : 1;
+}
+
+#ifdef CONFIG_SND_SOC_UX500_AB8500
+
+/*
+ * meas_voltage_stable - measures relative stable voltage from spec. input
+ */
+static int ab8500_meas_voltage_stable(struct abx500_ad *dd)
+{
+	int ret, mv;
+
+	ret = ux500_ab8500_audio_gpadc_measure((struct ab8500_gpadc *)dd->gpadc,
+			ACC_DETECT2, false, &mv);
+
+	return (ret < 0) ? ret : mv;
+}
+
+/*
+ * meas_alt_voltage_stable - measures relative stable voltage from spec. input
+ */
+static int ab8500_meas_alt_voltage_stable(struct abx500_ad *dd)
+{
+	int ret, mv;
+
+	ret = ux500_ab8500_audio_gpadc_measure((struct ab8500_gpadc *)dd->gpadc,
+			ACC_DETECT2, true, &mv);
+
+	return (ret < 0) ? ret : mv;
+}
+
+#else
+
+/*
+ * meas_voltage_stable - measures relative stable voltage from spec. input
+ */
+static int ab8500_meas_voltage_stable(struct abx500_ad *dd)
+{
+	int iterations = 2;
+	int v1, v2, dv;
+
+	v1 = ab8500_gpadc_convert((struct ab8500_gpadc *)dd->gpadc,
+			ACC_DETECT2);
+	do {
+		msleep(1);
+		--iterations;
+		v2 = ab8500_gpadc_convert((struct ab8500_gpadc *)dd->gpadc,
+				ACC_DETECT2);
+		dv = abs(v2 - v1);
+		v1 = v2;
+	} while (iterations > 0 && dv > MAX_VOLT_DIFF);
+
+	return v1;
+}
+
+/*
+ * not implemented for non soc setups
+ */
+static int ab8500_meas_alt_voltage_stable(struct abx500_ad *dd)
+{
+	return -1;
+}
+
+#endif
+
+/*
+ * configures HW so that it is possible to make decision whether
+ * accessory is connected or not.
+ */
+static void ab8500_config_hw_test_plug_connected(struct abx500_ad *dd,
+		int enable)
+{
+	int ret;
+
+	dev_dbg(&dd->pdev->dev, "%s:%d\n", __func__, enable);
+
+	ret = ab8500_config_pulldown(&dd->pdev->dev,
+					dd->pdata->video_ctrl_gpio, !enable);
+	if (ret < 0) {
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to update reg (%d).\n", __func__, ret);
+		return;
+	}
+
+	if (enable)
+		accessory_regulator_enable(dd, REGULATOR_VAMIC1);
+}
+
+/*
+ * configures HW so that carkit/headset detection can be accomplished.
+ */
+static void ab8500_config_hw_test_basic_carkit(struct abx500_ad *dd, int enable)
+{
+	int ret;
+
+	dev_dbg(&dd->pdev->dev, "%s:%d\n", __func__, enable);
+
+	if (enable)
+		accessory_regulator_disable(dd, REGULATOR_VAMIC1);
+
+	/* Un-Ground the VAMic1 output when enabled */
+	ret = abx500_mask_and_set_register_interruptible(
+				&dd->pdev->dev,
+				AB8500_REGU_CTRL1,
+				AB8500_REGU_CTRL1_SPARE_REG,
+				BIT_REGUCTRL1SPARE_VAMIC1_GROUND,
+				enable ? BIT_REGUCTRL1SPARE_VAMIC1_GROUND : 0);
+	if (ret < 0)
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to update reg (%d).\n", __func__, ret);
+}
+
+/*
+ * sets the av switch direction - audio-in vs video-out
+ */
+static void ab8500_set_av_switch(struct abx500_ad *dd,
+		enum accessory_avcontrol_dir dir)
+{
+	int ret;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (%d)\n", __func__, dir);
+	if (dir == NOT_SET) {
+		ret = gpio_direction_input(dd->pdata->video_ctrl_gpio);
+		dd->gpio35_dir_set = 0;
+		ret = gpio_direction_output(dd->pdata->video_ctrl_gpio, 0);
+		if (dd->pdata->mic_ctrl)
+			 gpio_direction_output(dd->pdata->mic_ctrl, 0);
+	} else if (!dd->gpio35_dir_set) {
+		ret = gpio_direction_output(dd->pdata->video_ctrl_gpio,
+						dir == AUDIO_IN ? 1 : 0);
+		if (ret < 0) {
+			dev_err(&dd->pdev->dev,
+				"%s: video_ctrl pin output config failed (%d).\n",
+								__func__, ret);
+			return;
+		}
+
+		if (dd->pdata->mic_ctrl) {
+			ret = gpio_direction_output(dd->pdata->mic_ctrl,
+					dir == AUDIO_IN ? 1 : 0);
+			if (ret < 0) {
+				dev_err(&dd->pdev->dev,
+						"%s: mic_ctrl pin output"
+						"config failed (%d).\n",
+						__func__, ret);
+				return;
+			}
+		}
+
+		dd->gpio35_dir_set = 1;
+		dev_dbg(&dd->pdev->dev, "AV-SWITCH: %s\n",
+			dir == AUDIO_IN ? "AUDIO_IN" : "VIDEO_OUT");
+	} else {
+		gpio_set_value(dd->pdata->video_ctrl_gpio,
+						dir == AUDIO_IN ? 1 : 0);
+	}
+}
+
+static u8 acc_det_ctrl_suspend_val;
+
+static void ab8500_turn_off_accdet_comparator(struct platform_device *pdev)
+{
+	struct abx500_ad *dd = platform_get_drvdata(pdev);
+
+	/* Turn off AccDetect comparators and pull-up */
+	(void) abx500_get_register_interruptible(
+			&dd->pdev->dev,
+			AB8500_ECI_AV_ACC,
+			AB8500_ACC_DET_CTRL_REG,
+			&acc_det_ctrl_suspend_val);
+	(void) abx500_set_register_interruptible(
+			&dd->pdev->dev,
+			AB8500_ECI_AV_ACC,
+			AB8500_ACC_DET_CTRL_REG,
+			0);
+
+}
+
+static void ab8500_turn_on_accdet_comparator(struct platform_device *pdev)
+{
+	struct abx500_ad *dd = platform_get_drvdata(pdev);
+
+	/* Turn on AccDetect comparators and pull-up */
+	(void) abx500_set_register_interruptible(
+			&dd->pdev->dev,
+			AB8500_ECI_AV_ACC,
+			AB8500_ACC_DET_CTRL_REG,
+			acc_det_ctrl_suspend_val);
+
+}
+
+static void *ab8500_accdet_abx500_gpadc_get(void)
+{
+	return ab8500_gpadc_get("ab8500-gpadc.0");
+}
+
+struct abx500_accdet_platform_data *
+	ab8500_get_platform_data(struct platform_device *pdev)
+{
+	struct ab8500_platform_data *plat;
+
+	plat = dev_get_platdata(pdev->dev.parent);
+
+	if (!plat || !plat->accdet) {
+		dev_err(&pdev->dev, "%s: Failed to get accdet plat data.\n",
+			__func__);
+		return ERR_PTR(-ENODEV);
+	}
+
+	return plat->accdet;
+}
+
+struct abx500_ad ab8500_accessory_det_callbacks = {
+	.irq_desc_norm			= ab8500_irq_desc_norm,
+	.irq_desc_inverted		= ab8500_irq_desc_inverted,
+	.no_irqs			= ARRAY_SIZE(ab8500_irq_desc_norm),
+	.regu_desc			= ab8500_regu_desc,
+	.no_of_regu_desc		= ARRAY_SIZE(ab8500_regu_desc),
+	.config_accdetect2_hw		= ab8500_config_accdetect2_hw,
+	.config_accdetect1_hw		= ab8500_config_accdetect1_hw,
+	.detect_plugged_in		= ab8500_detect_plugged_in,
+	.meas_voltage_stable		= ab8500_meas_voltage_stable,
+	.meas_alt_voltage_stable	= ab8500_meas_alt_voltage_stable,
+	.config_hw_test_basic_carkit	= ab8500_config_hw_test_basic_carkit,
+	.turn_off_accdet_comparator	= ab8500_turn_off_accdet_comparator,
+	.turn_on_accdet_comparator	= ab8500_turn_on_accdet_comparator,
+	.accdet_abx500_gpadc_get	= ab8500_accdet_abx500_gpadc_get,
+	.config_hw_test_plug_connected	= ab8500_config_hw_test_plug_connected,
+	.set_av_switch			= ab8500_set_av_switch,
+	.get_platform_data		= ab8500_get_platform_data,
+};
+
+MODULE_DESCRIPTION("AB8500 AV Accessory detection driver");
+MODULE_ALIAS("platform:ab8500-acc-det");
+MODULE_AUTHOR("ST-Ericsson");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/misc/ab8500-ponkey.c b/drivers/input/misc/ab8500-ponkey.c
index 350fd0c385d..c3c3c51d302 100644
--- a/drivers/input/misc/ab8500-ponkey.c
+++ b/drivers/input/misc/ab8500-ponkey.c
@@ -6,7 +6,6 @@
  *
  * AB8500 Power-On Key handler
  */
-
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
@@ -14,128 +13,208 @@
 #include <linux/interrupt.h>
 #include <linux/mfd/abx500/ab8500.h>
 #include <linux/slab.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+
+/* Ponkey time control bits */
+#define AB5500_MCB		0x2F
+#define AB5500_PONKEY_10SEC	0x0
+#define AB5500_PONKEY_5SEC	0x1
+#define AB5500_PONKEY_DISABLE	0x2
+#define AB5500_PONKEY_TMR_MASK	0x1
+#define AB5500_PONKEY_TR_MASK	0x2
+
+static int ab5500_ponkey_hw_init(struct platform_device *);
+
+struct ab8500_ponkey_variant {
+	const char *irq_falling;
+	const char *irq_rising;
+	int (*hw_init)(struct platform_device *);
+};
+
+static const struct ab8500_ponkey_variant ab5500_onswa = {
+	.irq_falling	= "ONSWAn_falling",
+	.irq_rising	= "ONSWAn_rising",
+	.hw_init	= ab5500_ponkey_hw_init,
+};
+
+static const struct ab8500_ponkey_variant ab8500_ponkey = {
+	.irq_falling	= "ONKEY_DBF",
+	.irq_rising	= "ONKEY_DBR",
+};
 
 /**
- * struct ab8500_ponkey - ab8500 ponkey information
+ * struct ab8500_ponkey_info - ab8500 ponkey information
  * @input_dev: pointer to input device
- * @ab8500: ab8500 parent
  * @irq_dbf: irq number for falling transition
  * @irq_dbr: irq number for rising transition
  */
-struct ab8500_ponkey {
+struct ab8500_ponkey_info {
 	struct input_dev	*idev;
-	struct ab8500		*ab8500;
 	int			irq_dbf;
 	int			irq_dbr;
 };
 
+static int ab5500_ponkey_hw_init(struct platform_device *pdev)
+{
+	u8 val;
+	struct ab5500_ponkey_platform_data *pdata;
+
+	pdata = pdev->dev.platform_data;
+	if (pdata) {
+		switch (pdata->shutdown_secs) {
+		case 0:
+			val = AB5500_PONKEY_DISABLE;
+			break;
+		case 5:
+			val = AB5500_PONKEY_5SEC;
+			break;
+		case 10:
+			val = AB5500_PONKEY_10SEC;
+			break;
+		default:
+			val = AB5500_PONKEY_10SEC;
+		}
+	} else {
+		val = AB5500_PONKEY_10SEC;
+	}
+	return abx500_mask_and_set(
+		&pdev->dev,
+		AB5500_BANK_STARTUP,
+		AB5500_MCB,
+		AB5500_PONKEY_TMR_MASK | AB5500_PONKEY_TR_MASK,
+		val);
+}
+
 /* AB8500 gives us an interrupt when ONKEY is held */
 static irqreturn_t ab8500_ponkey_handler(int irq, void *data)
 {
-	struct ab8500_ponkey *ponkey = data;
+	struct ab8500_ponkey_info *info = data;
 
-	if (irq == ponkey->irq_dbf)
-		input_report_key(ponkey->idev, KEY_POWER, true);
-	else if (irq == ponkey->irq_dbr)
-		input_report_key(ponkey->idev, KEY_POWER, false);
+	if (irq == info->irq_dbf)
+		input_report_key(info->idev, KEY_POWER, true);
+	else if (irq == info->irq_dbr)
+		input_report_key(info->idev, KEY_POWER, false);
 
-	input_sync(ponkey->idev);
+	input_sync(info->idev);
 
 	return IRQ_HANDLED;
 }
 
 static int __devinit ab8500_ponkey_probe(struct platform_device *pdev)
 {
-	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
-	struct ab8500_ponkey *ponkey;
-	struct input_dev *input;
-	int irq_dbf, irq_dbr;
-	int error;
+	const struct ab8500_ponkey_variant *variant;
+	struct ab8500_ponkey_info *info;
+	int irq_dbf, irq_dbr, ret;
+
+	variant = (const struct ab8500_ponkey_variant *)
+		  pdev->id_entry->driver_data;
+
+	if (variant->hw_init) {
+		ret = variant->hw_init(pdev);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "Failed to init hw");
+			return ret;
+		}
+	}
 
-	irq_dbf = platform_get_irq_byname(pdev, "ONKEY_DBF");
+	irq_dbf = platform_get_irq_byname(pdev, variant->irq_falling);
 	if (irq_dbf < 0) {
-		dev_err(&pdev->dev, "No IRQ for ONKEY_DBF, error=%d\n", irq_dbf);
+		dev_err(&pdev->dev, "No IRQ for %s: %d\n",
+			variant->irq_falling, irq_dbf);
 		return irq_dbf;
 	}
 
-	irq_dbr = platform_get_irq_byname(pdev, "ONKEY_DBR");
+	irq_dbr = platform_get_irq_byname(pdev, variant->irq_rising);
 	if (irq_dbr < 0) {
-		dev_err(&pdev->dev, "No IRQ for ONKEY_DBR, error=%d\n", irq_dbr);
+		dev_err(&pdev->dev, "No IRQ for %s: %d\n",
+			variant->irq_rising, irq_dbr);
 		return irq_dbr;
 	}
 
-	ponkey = kzalloc(sizeof(struct ab8500_ponkey), GFP_KERNEL);
-	input = input_allocate_device();
-	if (!ponkey || !input) {
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
+	info = kzalloc(sizeof(struct ab8500_ponkey_info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
 
-	ponkey->idev = input;
-	ponkey->ab8500 = ab8500;
-	ponkey->irq_dbf = irq_dbf;
-	ponkey->irq_dbr = irq_dbr;
+	info->irq_dbf = irq_dbf;
+	info->irq_dbr = irq_dbr;
 
-	input->name = "AB8500 POn(PowerOn) Key";
-	input->dev.parent = &pdev->dev;
+	info->idev = input_allocate_device();
+	if (!info->idev) {
+		dev_err(&pdev->dev, "Failed to allocate input dev\n");
+		ret = -ENOMEM;
+		goto out;
+	}
 
-	input_set_capability(input, EV_KEY, KEY_POWER);
+	info->idev->name = "AB8500 POn(PowerOn) Key";
+	info->idev->dev.parent = &pdev->dev;
+	info->idev->evbit[0] = BIT_MASK(EV_KEY);
+	info->idev->keybit[BIT_WORD(KEY_POWER)] = BIT_MASK(KEY_POWER);
 
-	error = request_any_context_irq(ponkey->irq_dbf, ab8500_ponkey_handler,
-					0, "ab8500-ponkey-dbf", ponkey);
-	if (error < 0) {
-		dev_err(ab8500->dev, "Failed to request dbf IRQ#%d: %d\n",
-			ponkey->irq_dbf, error);
-		goto err_free_mem;
+	ret = input_register_device(info->idev);
+	if (ret) {
+		dev_err(&pdev->dev, "Can't register input device: %d\n", ret);
+		goto out_unfreedevice;
 	}
 
-	error = request_any_context_irq(ponkey->irq_dbr, ab8500_ponkey_handler,
-					0, "ab8500-ponkey-dbr", ponkey);
-	if (error < 0) {
-		dev_err(ab8500->dev, "Failed to request dbr IRQ#%d: %d\n",
-			ponkey->irq_dbr, error);
-		goto err_free_dbf_irq;
+	ret = request_threaded_irq(info->irq_dbf, NULL, ab8500_ponkey_handler,
+					IRQF_NO_SUSPEND, "ab8500-ponkey-dbf",
+					info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to request dbf IRQ#%d: %d\n",
+				info->irq_dbf, ret);
+		goto out_unregisterdevice;
 	}
 
-	error = input_register_device(ponkey->idev);
-	if (error) {
-		dev_err(ab8500->dev, "Can't register input device: %d\n", error);
-		goto err_free_dbr_irq;
+	ret = request_threaded_irq(info->irq_dbr, NULL, ab8500_ponkey_handler,
+					IRQF_NO_SUSPEND, "ab8500-ponkey-dbr",
+					info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to request dbr IRQ#%d: %d\n",
+				info->irq_dbr, ret);
+		goto out_irq_dbf;
 	}
 
-	platform_set_drvdata(pdev, ponkey);
-	return 0;
+	platform_set_drvdata(pdev, info);
 
-err_free_dbr_irq:
-	free_irq(ponkey->irq_dbr, ponkey);
-err_free_dbf_irq:
-	free_irq(ponkey->irq_dbf, ponkey);
-err_free_mem:
-	input_free_device(input);
-	kfree(ponkey);
+	return 0;
 
-	return error;
+out_irq_dbf:
+	free_irq(info->irq_dbf, info);
+out_unregisterdevice:
+	input_unregister_device(info->idev);
+	info->idev = NULL;
+out_unfreedevice:
+	input_free_device(info->idev);
+out:
+	kfree(info);
+	return ret;
 }
 
 static int __devexit ab8500_ponkey_remove(struct platform_device *pdev)
 {
-	struct ab8500_ponkey *ponkey = platform_get_drvdata(pdev);
-
-	free_irq(ponkey->irq_dbf, ponkey);
-	free_irq(ponkey->irq_dbr, ponkey);
-	input_unregister_device(ponkey->idev);
-	kfree(ponkey);
-
-	platform_set_drvdata(pdev, NULL);
+	struct ab8500_ponkey_info *info = platform_get_drvdata(pdev);
 
+	free_irq(info->irq_dbf, info);
+	free_irq(info->irq_dbr, info);
+	input_unregister_device(info->idev);
+	kfree(info);
 	return 0;
 }
 
+static struct platform_device_id ab8500_ponkey_id_table[] = {
+	{ "ab5500-onswa", (kernel_ulong_t)&ab5500_onswa, },
+	{ "ab8500-poweron-key", (kernel_ulong_t)&ab8500_ponkey, },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, ab8500_ponkey_id_table);
+
 static struct platform_driver ab8500_ponkey_driver = {
 	.driver		= {
 		.name	= "ab8500-poweron-key",
 		.owner	= THIS_MODULE,
 	},
+	.id_table	= ab8500_ponkey_id_table,
 	.probe		= ab8500_ponkey_probe,
 	.remove		= __devexit_p(ab8500_ponkey_remove),
 };
diff --git a/drivers/input/misc/abx500-accdet.c b/drivers/input/misc/abx500-accdet.c
new file mode 100644
index 00000000000..4b5017a6e60
--- /dev/null
+++ b/drivers/input/misc/abx500-accdet.c
@@ -0,0 +1,1019 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Jarmo K. Kuronen <jarmo.kuronen@symbio.com>
+ *         for ST-Ericsson.
+ *
+ * License terms: GPL V2
+ *
+ * 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/workqueue.h>
+#include <linux/irq.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/input/abx500-accdet.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/gpio.h>
+#include <linux/mfd/abx500.h>
+
+#include <sound/jack.h>
+#include <sound/soc.h>
+#include <sound/jack.h>
+
+#ifdef CONFIG_SND_SOC_UX500_AB8500
+#include <sound/ux500_ab8500.h>
+#else
+#define ux500_ab8500_jack_report(i)
+#endif
+
+/* Unique value used to identify Headset button input device */
+#define BTN_INPUT_UNIQUE_VALUE	"AB8500HsBtn"
+#define BTN_INPUT_DEV_NAME	"AB8500 Hs Button"
+
+#define DEBOUNCE_PLUG_EVENT_MS		100
+#define DEBOUNCE_PLUG_RETEST_MS		25
+#define DEBOUNCE_UNPLUG_EVENT_MS	0
+
+/* After being loaded, how fast the first check is to be made */
+#define INIT_DELAY_MS			3000
+
+/* Voltage limits (mV) for various types of AV Accessories */
+#define ACCESSORY_DET_VOL_DONTCARE	-1
+#define ACCESSORY_HEADPHONE_DET_VOL_MIN	0
+#define ACCESSORY_HEADPHONE_DET_VOL_MAX	40
+#define ACCESSORY_U_HEADSET_DET_VOL_MIN	47
+#define ACCESSORY_U_HEADSET_DET_VOL_MAX	732
+#define ACCESSORY_U_HEADSET_ALT_DET_VOL_MIN	25
+#define ACCESSORY_U_HEADSET_ALT_DET_VOL_MAX	50
+#define ACCESSORY_CARKIT_DET_VOL_MIN	1100
+#define ACCESSORY_CARKIT_DET_VOL_MAX	1300
+#define ACCESSORY_HEADSET_DET_VOL_MIN	1301
+#define ACCESSORY_HEADSET_DET_VOL_MAX	2000
+#define ACCESSORY_OPENCABLE_DET_VOL_MIN	2001
+#define ACCESSORY_OPENCABLE_DET_VOL_MAX	2150
+
+
+/* Macros */
+
+/*
+ * Conviniency macros to check jack characteristics.
+ */
+#define jack_supports_mic(type) \
+	(type == JACK_TYPE_HEADSET || type == JACK_TYPE_CARKIT)
+#define jack_supports_spkr(type) \
+	((type != JACK_TYPE_DISCONNECTED) && (type != JACK_TYPE_CONNECTED))
+#define jack_supports_buttons(type) \
+	((type == JACK_TYPE_HEADSET) ||\
+	(type == JACK_TYPE_CARKIT) ||\
+	(type == JACK_TYPE_OPENCABLE) ||\
+	(type == JACK_TYPE_CONNECTED))
+
+
+/* Forward declarations */
+static void config_accdetect(struct abx500_ad *dd);
+static enum accessory_jack_type detect(struct abx500_ad *dd, int *required_det);
+
+/* Static data initialization */
+static struct accessory_detect_task detect_ops[] = {
+	{
+		.type = JACK_TYPE_DISCONNECTED,
+		.typename = "DISCONNECTED",
+		.meas_mv = 1,
+		.req_det_count = 1,
+		.minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.maxvol = ACCESSORY_DET_VOL_DONTCARE,
+		.alt_minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.alt_maxvol = ACCESSORY_DET_VOL_DONTCARE
+	},
+	{
+		.type = JACK_TYPE_HEADPHONE,
+		.typename = "HEADPHONE",
+		.meas_mv = 1,
+		.req_det_count = 1,
+		.minvol = ACCESSORY_HEADPHONE_DET_VOL_MIN,
+		.maxvol = ACCESSORY_HEADPHONE_DET_VOL_MAX,
+		.alt_minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.alt_maxvol = ACCESSORY_DET_VOL_DONTCARE
+	},
+	{
+		.type = JACK_TYPE_UNSUPPORTED_HEADSET,
+		.typename = "UNSUPPORTED HEADSET",
+		.meas_mv = 1,
+		.req_det_count = 2,
+		.minvol = ACCESSORY_U_HEADSET_DET_VOL_MIN,
+		.maxvol = ACCESSORY_U_HEADSET_DET_VOL_MAX,
+		.alt_minvol = ACCESSORY_U_HEADSET_ALT_DET_VOL_MIN,
+		.alt_maxvol = ACCESSORY_U_HEADSET_ALT_DET_VOL_MAX
+	},
+	{
+		.type = JACK_TYPE_OPENCABLE,
+		.typename = "OPENCABLE",
+		.meas_mv = 0,
+		.req_det_count = 4,
+		.minvol = ACCESSORY_OPENCABLE_DET_VOL_MIN,
+		.maxvol = ACCESSORY_OPENCABLE_DET_VOL_MAX,
+		.alt_minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.alt_maxvol = ACCESSORY_DET_VOL_DONTCARE
+	},
+	{
+		.type = JACK_TYPE_CARKIT,
+		.typename = "CARKIT",
+		.meas_mv = 1,
+		.req_det_count = 1,
+		.minvol = ACCESSORY_CARKIT_DET_VOL_MIN,
+		.maxvol = ACCESSORY_CARKIT_DET_VOL_MAX,
+		.alt_minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.alt_maxvol = ACCESSORY_DET_VOL_DONTCARE
+	},
+	{
+		.type = JACK_TYPE_HEADSET,
+		.typename = "HEADSET",
+		.meas_mv = 0,
+		.req_det_count = 2,
+		.minvol = ACCESSORY_HEADSET_DET_VOL_MIN,
+		.maxvol = ACCESSORY_HEADSET_DET_VOL_MAX,
+		.alt_minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.alt_maxvol = ACCESSORY_DET_VOL_DONTCARE
+	},
+	{
+		.type = JACK_TYPE_CONNECTED,
+		.typename = "CONNECTED",
+		.meas_mv = 0,
+		.req_det_count = 4,
+		.minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.maxvol = ACCESSORY_DET_VOL_DONTCARE,
+		.alt_minvol = ACCESSORY_DET_VOL_DONTCARE,
+		.alt_maxvol = ACCESSORY_DET_VOL_DONTCARE
+	}
+};
+
+static struct accessory_irq_descriptor *abx500_accdet_irq_desc;
+
+/*
+ * textual represenation of the accessory type
+ */
+static const char *accessory_str(enum accessory_jack_type type)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(detect_ops); i++)
+		if (type == detect_ops[i].type)
+			return detect_ops[i].typename;
+
+	return "UNKNOWN?";
+}
+
+/*
+ * enables regulator but only if it has not been enabled earlier.
+ */
+void accessory_regulator_enable(struct abx500_ad *dd,
+		enum accessory_regulator reg)
+{
+	int i;
+
+	for (i = 0; i < dd->no_of_regu_desc; i++) {
+		if (reg & dd->regu_desc[i].id) {
+			if (!dd->regu_desc[i].enabled) {
+				if (!regulator_enable(dd->regu_desc[i].handle))
+					dd->regu_desc[i].enabled = 1;
+			}
+		}
+	}
+}
+
+/*
+ * disables regulator but only if it has been previously enabled.
+ */
+void accessory_regulator_disable(struct abx500_ad *dd,
+		enum accessory_regulator reg)
+{
+	int i;
+
+	for (i = 0; i < dd->no_of_regu_desc; i++) {
+		if (reg & dd->regu_desc[i].id) {
+			if (dd->regu_desc[i].enabled) {
+				if (!regulator_disable(dd->regu_desc[i].handle))
+					dd->regu_desc[i].enabled = 0;
+			}
+		}
+	}
+}
+
+/*
+ * frees previously retrieved regulators.
+ */
+static void free_regulators(struct abx500_ad *dd)
+{
+	int i;
+
+	for (i = 0; i < dd->no_of_regu_desc; i++) {
+		if (dd->regu_desc[i].handle) {
+			regulator_put(dd->regu_desc[i].handle);
+			dd->regu_desc[i].handle = NULL;
+		}
+	}
+}
+
+/*
+ * gets required regulators.
+ */
+static int create_regulators(struct abx500_ad *dd)
+{
+	int i;
+	int status = 0;
+
+	for (i = 0; i < dd->no_of_regu_desc; i++) {
+		struct regulator *regu =
+			regulator_get(&dd->pdev->dev, dd->regu_desc[i].name);
+		if (IS_ERR(regu)) {
+			status = PTR_ERR(regu);
+			dev_err(&dd->pdev->dev,
+				"%s: Failed to get supply '%s' (%d).\n",
+				__func__, dd->regu_desc[i].name, status);
+			free_regulators(dd);
+			goto out;
+		} else {
+			dd->regu_desc[i].handle = regu;
+		}
+	}
+
+out:
+	return status;
+}
+
+/*
+ * create input device for button press reporting
+ */
+static int create_btn_input_dev(struct abx500_ad *dd)
+{
+	int err;
+
+	dd->btn_input_dev = input_allocate_device();
+	if (!dd->btn_input_dev) {
+		dev_err(&dd->pdev->dev, "%s: Failed to allocate input dev.\n",
+			__func__);
+		err = -ENOMEM;
+		goto out;
+	}
+
+	input_set_capability(dd->btn_input_dev,
+				EV_KEY,
+				dd->pdata->btn_keycode);
+
+	dd->btn_input_dev->name = BTN_INPUT_DEV_NAME;
+	dd->btn_input_dev->uniq = BTN_INPUT_UNIQUE_VALUE;
+	dd->btn_input_dev->dev.parent = &dd->pdev->dev;
+
+	err = input_register_device(dd->btn_input_dev);
+	if (err) {
+		dev_err(&dd->pdev->dev,
+			"%s: register_input_device failed (%d).\n", __func__,
+			err);
+		input_free_device(dd->btn_input_dev);
+		dd->btn_input_dev = NULL;
+		goto out;
+	}
+out:
+	return err;
+}
+
+/*
+ * reports jack status
+ */
+void report_jack_status(struct abx500_ad *dd)
+{
+	int value = 0;
+
+	/* Never report possible open cable */
+	if (dd->jack_type == JACK_TYPE_OPENCABLE)
+		goto out;
+
+	/* Never report same state twice in a row */
+	if (dd->jack_type == dd->reported_jack_type)
+		goto out;
+	dd->reported_jack_type = dd->jack_type;
+
+	dev_dbg(&dd->pdev->dev, "Accessory: %s\n",
+		accessory_str(dd->jack_type));
+
+	/* Never report unsupported headset */
+	if (dd->jack_type == JACK_TYPE_UNSUPPORTED_HEADSET)
+		goto out;
+
+	if (dd->jack_type != JACK_TYPE_DISCONNECTED &&
+		dd->jack_type != JACK_TYPE_UNSPECIFIED)
+		value |= SND_JACK_MECHANICAL;
+	if (jack_supports_mic(dd->jack_type))
+		value |= SND_JACK_MICROPHONE;
+	if (jack_supports_spkr(dd->jack_type))
+		value |= (SND_JACK_HEADPHONE | SND_JACK_LINEOUT);
+	ux500_ab8500_jack_report(value);
+
+out: return;
+}
+
+/*
+ * worker routine to handle accessory unplug case
+ */
+void unplug_irq_handler_work(struct work_struct *work)
+{
+	struct abx500_ad *dd = container_of(work,
+		struct abx500_ad, unplug_irq_work.work);
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	dd->jack_type = dd->jack_type_temp = JACK_TYPE_DISCONNECTED;
+	dd->jack_det_count = dd->total_jack_det_count = 0;
+	dd->btn_state = BUTTON_UNK;
+	config_accdetect(dd);
+
+	accessory_regulator_disable(dd, REGULATOR_ALL);
+
+	report_jack_status(dd);
+}
+
+/*
+ * interrupt service routine for accessory unplug.
+ */
+irqreturn_t unplug_irq_handler(int irq, void *_userdata)
+{
+	struct abx500_ad *dd = _userdata;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (irq=%d)\n", __func__, irq);
+
+	queue_delayed_work(dd->irq_work_queue, &dd->unplug_irq_work,
+			msecs_to_jiffies(DEBOUNCE_UNPLUG_EVENT_MS));
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * interrupt service routine for accessory plug.
+ */
+irqreturn_t plug_irq_handler(int irq, void *_userdata)
+{
+	struct abx500_ad *dd = _userdata;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (irq=%d)\n",
+		__func__, irq);
+
+	switch (dd->jack_type) {
+	case JACK_TYPE_DISCONNECTED:
+	case JACK_TYPE_UNSPECIFIED:
+		queue_delayed_work(dd->irq_work_queue, &dd->detect_work,
+			msecs_to_jiffies(DEBOUNCE_PLUG_EVENT_MS));
+		break;
+
+	default:
+		dev_err(&dd->pdev->dev, "%s: Unexpected plug IRQ\n", __func__);
+		break;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * worker routine to perform detection.
+ */
+static void detect_work(struct work_struct *work)
+{
+	int req_det_count = 1;
+	enum accessory_jack_type new_type;
+	struct abx500_ad *dd = container_of(work,
+		struct abx500_ad, detect_work.work);
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	if (dd->set_av_switch)
+		dd->set_av_switch(dd, AUDIO_IN);
+
+	new_type = detect(dd, &req_det_count);
+
+	dd->total_jack_det_count++;
+	if (dd->jack_type_temp == new_type) {
+		dd->jack_det_count++;
+	} else {
+		dd->jack_det_count = 1;
+		dd->jack_type_temp = new_type;
+	}
+
+	if (dd->total_jack_det_count >= MAX_DET_COUNT) {
+		dev_err(&dd->pdev->dev,
+			"%s: MAX_DET_COUNT(=%d) reached. Bailing out.\n",
+					__func__, MAX_DET_COUNT);
+		queue_delayed_work(dd->irq_work_queue, &dd->unplug_irq_work,
+				msecs_to_jiffies(DEBOUNCE_UNPLUG_EVENT_MS));
+	} else if (dd->jack_det_count >= req_det_count) {
+		dd->total_jack_det_count = dd->jack_det_count = 0;
+		dd->jack_type = new_type;
+		dd->detect_jiffies = jiffies;
+		report_jack_status(dd);
+		config_accdetect(dd);
+	} else {
+		queue_delayed_work(dd->irq_work_queue,
+				&dd->detect_work,
+				msecs_to_jiffies(DEBOUNCE_PLUG_RETEST_MS));
+	}
+}
+
+/*
+ * reports a button event (pressed, released).
+ */
+static void report_btn_event(struct abx500_ad *dd, int down)
+{
+	input_report_key(dd->btn_input_dev, dd->pdata->btn_keycode, down);
+	input_sync(dd->btn_input_dev);
+
+	dev_dbg(&dd->pdev->dev, "HS-BTN: %s\n", down ? "PRESSED" : "RELEASED");
+}
+
+/*
+ * interrupt service routine invoked when hs button is pressed down.
+ */
+irqreturn_t button_press_irq_handler(int irq, void *_userdata)
+{
+	struct abx500_ad *dd = _userdata;
+
+	unsigned long accept_jiffies = dd->detect_jiffies +
+			msecs_to_jiffies(1000);
+	if (time_before(jiffies, accept_jiffies)) {
+		dev_dbg(&dd->pdev->dev, "%s: Skipped spurious btn press.\n",
+			__func__);
+		return IRQ_HANDLED;
+	}
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (irq=%d)\n", __func__, irq);
+
+	if (dd->jack_type == JACK_TYPE_OPENCABLE) {
+		/* Someting got connected to open cable -> detect.. */
+		dd->config_accdetect2_hw(dd, 0);
+		queue_delayed_work(dd->irq_work_queue, &dd->detect_work,
+			msecs_to_jiffies(DEBOUNCE_PLUG_EVENT_MS));
+		return IRQ_HANDLED;
+	}
+
+	if (dd->btn_state == BUTTON_PRESSED)
+		return IRQ_HANDLED;
+
+	if (jack_supports_buttons(dd->jack_type)) {
+		dd->btn_state = BUTTON_PRESSED;
+		report_btn_event(dd, 1);
+	} else {
+		dd->btn_state = BUTTON_UNK;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * interrupts service routine invoked when hs button is released.
+ */
+irqreturn_t button_release_irq_handler(int irq, void *_userdata)
+{
+	struct abx500_ad *dd = _userdata;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter (irq=%d)\n", __func__, irq);
+
+	if (dd->jack_type == JACK_TYPE_OPENCABLE)
+		return IRQ_HANDLED;
+
+	if (dd->btn_state != BUTTON_PRESSED)
+		return IRQ_HANDLED;
+
+	if (jack_supports_buttons(dd->jack_type)) {
+		report_btn_event(dd, 0);
+		dd->btn_state = BUTTON_RELEASED;
+	} else {
+		dd->btn_state = BUTTON_UNK;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * checks whether measured voltage is in given range. depending on arguments,
+ * voltage might be re-measured or previously measured voltage is reused.
+ */
+static int mic_vol_in_range(struct abx500_ad *dd,
+			int lo, int hi, int alt_lo, int alt_hi, int force_read)
+{
+	static int mv = MIN_MIC_POWER;
+	static int alt_mv = MIN_MIC_POWER;
+
+	if (mv == MIN_MIC_POWER || force_read)
+		mv = dd->meas_voltage_stable(dd);
+
+	if (mv < lo || mv > hi)
+		return 0;
+
+	if (ACCESSORY_DET_VOL_DONTCARE == alt_lo &&
+		ACCESSORY_DET_VOL_DONTCARE == alt_hi)
+		return 1;
+
+	if (alt_mv == MIN_MIC_POWER || force_read)
+		alt_mv = dd->meas_alt_voltage_stable(dd);
+
+	if (alt_mv < alt_lo || alt_mv > alt_hi)
+		return 0;
+
+	return 1;
+}
+
+/*
+ * checks whether the currently connected HW is of given type.
+ */
+static int detect_hw(struct abx500_ad *dd,
+			struct accessory_detect_task *task)
+{
+	int status;
+
+	switch (task->type) {
+	case JACK_TYPE_DISCONNECTED:
+		dd->config_hw_test_plug_connected(dd, 1);
+		status = !dd->detect_plugged_in(dd);
+		break;
+	case JACK_TYPE_CONNECTED:
+		dd->config_hw_test_plug_connected(dd, 1);
+		status = dd->detect_plugged_in(dd);
+		break;
+	case JACK_TYPE_CARKIT:
+	case JACK_TYPE_HEADPHONE:
+	case JACK_TYPE_HEADSET:
+	case JACK_TYPE_UNSUPPORTED_HEADSET:
+	case JACK_TYPE_OPENCABLE:
+		status = mic_vol_in_range(dd,
+					task->minvol,
+					task->maxvol,
+					task->alt_minvol,
+					task->alt_maxvol,
+					task->meas_mv);
+		break;
+	default:
+		status = 0;
+	}
+
+	return status;
+}
+
+/*
+ * Tries to detect the currently attached accessory
+ */
+static enum accessory_jack_type detect(struct abx500_ad *dd,
+			int *req_det_count)
+{
+	enum accessory_jack_type type = JACK_TYPE_DISCONNECTED;
+	int i;
+
+	accessory_regulator_enable(dd, REGULATOR_VAUDIO | REGULATOR_AVSWITCH);
+	/* enable the VAMIC1 regulator */
+	dd->config_hw_test_basic_carkit(dd, 0);
+
+	for (i = 0; i < ARRAY_SIZE(detect_ops); ++i) {
+		if (detect_hw(dd, &detect_ops[i])) {
+			type = detect_ops[i].type;
+			*req_det_count = detect_ops[i].req_det_count;
+			break;
+		}
+	}
+
+	dd->config_hw_test_plug_connected(dd, 0);
+
+	if (jack_supports_buttons(type))
+		accessory_regulator_enable(dd, REGULATOR_VAMIC1);
+	else
+		accessory_regulator_disable(dd, REGULATOR_VAMIC1 |
+						REGULATOR_AVSWITCH);
+
+	accessory_regulator_disable(dd, REGULATOR_VAUDIO);
+
+	return type;
+}
+
+/*
+ * registers to specific interrupt
+ */
+static void claim_irq(struct abx500_ad *dd, enum accessory_irq irq_id)
+{
+	int ret;
+	int irq;
+
+	if (dd->pdata->is_detection_inverted)
+		abx500_accdet_irq_desc = dd->irq_desc_inverted;
+	else
+		abx500_accdet_irq_desc = dd->irq_desc_norm;
+
+	if (abx500_accdet_irq_desc[irq_id].registered)
+		return;
+
+	irq = platform_get_irq_byname(
+			dd->pdev,
+			abx500_accdet_irq_desc[irq_id].name);
+	if (irq < 0) {
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to get irq %s\n", __func__,
+			abx500_accdet_irq_desc[irq_id].name);
+		return;
+	}
+
+	ret = request_threaded_irq(irq,
+				NULL,
+				abx500_accdet_irq_desc[irq_id].isr,
+				IRQF_NO_SUSPEND | IRQF_SHARED,
+				abx500_accdet_irq_desc[irq_id].name,
+				dd);
+	if (ret != 0) {
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to claim irq %s (%d)\n",
+			__func__,
+			abx500_accdet_irq_desc[irq_id].name,
+			ret);
+	} else {
+		abx500_accdet_irq_desc[irq_id].registered = 1;
+		dev_dbg(&dd->pdev->dev, "%s: %s\n",
+			__func__, abx500_accdet_irq_desc[irq_id].name);
+	}
+}
+
+/*
+ * releases specific interrupt
+ */
+static void release_irq(struct abx500_ad *dd, enum accessory_irq irq_id)
+{
+	int irq;
+
+	if (dd->pdata->is_detection_inverted)
+		abx500_accdet_irq_desc = dd->irq_desc_inverted;
+	else
+		abx500_accdet_irq_desc = dd->irq_desc_norm;
+
+	if (!abx500_accdet_irq_desc[irq_id].registered)
+		return;
+
+	irq = platform_get_irq_byname(
+			dd->pdev,
+			abx500_accdet_irq_desc[irq_id].name);
+	if (irq < 0) {
+		dev_err(&dd->pdev->dev,
+			"%s: Failed to get irq %s (%d)\n",
+			__func__,
+			abx500_accdet_irq_desc[irq_id].name, irq);
+	} else {
+		free_irq(irq, dd);
+		abx500_accdet_irq_desc[irq_id].registered = 0;
+		dev_dbg(&dd->pdev->dev, "%s: %s\n",
+			__func__, abx500_accdet_irq_desc[irq_id].name);
+	}
+}
+
+/*
+ * configures interrupts + detection hardware to meet the requirements
+ * set by currently attached accessory type.
+ */
+static void config_accdetect(struct abx500_ad *dd)
+{
+	switch (dd->jack_type) {
+	case JACK_TYPE_UNSPECIFIED:
+		dd->config_accdetect1_hw(dd, 1);
+		dd->config_accdetect2_hw(dd, 0);
+
+		release_irq(dd, PLUG_IRQ);
+		release_irq(dd, UNPLUG_IRQ);
+		release_irq(dd, BUTTON_PRESS_IRQ);
+		release_irq(dd, BUTTON_RELEASE_IRQ);
+		if (dd->set_av_switch)
+			dd->set_av_switch(dd, NOT_SET);
+		break;
+
+	case JACK_TYPE_DISCONNECTED:
+	if (dd->set_av_switch)
+		dd->set_av_switch(dd, NOT_SET);
+	case JACK_TYPE_HEADPHONE:
+		dd->config_accdetect1_hw(dd, 1);
+		dd->config_accdetect2_hw(dd, 0);
+
+		claim_irq(dd, PLUG_IRQ);
+		claim_irq(dd, UNPLUG_IRQ);
+		release_irq(dd, BUTTON_PRESS_IRQ);
+		release_irq(dd, BUTTON_RELEASE_IRQ);
+		break;
+
+	case JACK_TYPE_UNSUPPORTED_HEADSET:
+		dd->config_accdetect1_hw(dd, 1);
+		dd->config_accdetect2_hw(dd, 1);
+
+		release_irq(dd, PLUG_IRQ);
+		claim_irq(dd, UNPLUG_IRQ);
+		release_irq(dd, BUTTON_PRESS_IRQ);
+		release_irq(dd, BUTTON_RELEASE_IRQ);
+		if (dd->set_av_switch)
+			dd->set_av_switch(dd, NOT_SET);
+		break;
+
+	case JACK_TYPE_CONNECTED:
+	case JACK_TYPE_HEADSET:
+	case JACK_TYPE_CARKIT:
+	case JACK_TYPE_OPENCABLE:
+		dd->config_accdetect1_hw(dd, 1);
+		dd->config_accdetect2_hw(dd, 1);
+
+		release_irq(dd, PLUG_IRQ);
+		claim_irq(dd, UNPLUG_IRQ);
+		claim_irq(dd, BUTTON_PRESS_IRQ);
+		claim_irq(dd, BUTTON_RELEASE_IRQ);
+		break;
+
+	default:
+		dev_err(&dd->pdev->dev, "%s: Unknown type: %d\n",
+			__func__, dd->jack_type);
+	}
+}
+
+/*
+ * Deferred initialization of the work.
+ */
+static void init_work(struct work_struct *work)
+{
+	struct abx500_ad *dd = container_of(work,
+		struct abx500_ad, init_work.work);
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	dd->jack_type = dd->reported_jack_type = JACK_TYPE_UNSPECIFIED;
+	config_accdetect(dd);
+	queue_delayed_work(dd->irq_work_queue,
+				&dd->detect_work,
+				msecs_to_jiffies(0));
+}
+
+/*
+ * performs platform device initialization
+ */
+static int abx500_accessory_init(struct platform_device *pdev)
+{
+	int ret;
+	struct abx500_ad *dd = (struct abx500_ad *)pdev->id_entry->driver_data;
+
+	dev_dbg(&pdev->dev, "Enter: %s\n", __func__);
+
+	dd->pdev = pdev;
+	dd->pdata = dd->get_platform_data(pdev);
+	if (IS_ERR(dd->pdata))
+		return PTR_ERR(dd->pdata);
+
+	if (dd->pdata->video_ctrl_gpio) {
+		ret = gpio_is_valid(dd->pdata->video_ctrl_gpio);
+		if (!ret) {
+			dev_err(&pdev->dev,
+				"%s: Video ctrl GPIO invalid (%d).\n", __func__,
+						dd->pdata->video_ctrl_gpio);
+
+			return ret;
+		}
+		ret = gpio_request(dd->pdata->video_ctrl_gpio,
+				"Video Control");
+	       if (ret)	{
+			dev_err(&pdev->dev, "%s: Get video ctrl GPIO"
+					"failed.\n", __func__);
+			return ret;
+		}
+	}
+
+	if (dd->pdata->mic_ctrl) {
+		ret = gpio_is_valid(dd->pdata->mic_ctrl);
+		if (!ret) {
+			dev_err(&pdev->dev,
+				"%s: Mic ctrl GPIO invalid (%d).\n", __func__,
+						dd->pdata->mic_ctrl);
+
+			goto mic_ctrl_fail;
+		}
+		ret = gpio_request(dd->pdata->mic_ctrl,
+				"Mic Control");
+	       if (ret)	{
+			dev_err(&pdev->dev, "%s: Get mic ctrl GPIO"
+					"failed.\n", __func__);
+			goto mic_ctrl_fail;
+		}
+	}
+
+	ret = create_btn_input_dev(dd);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "%s: create_button_input_dev failed.\n",
+			__func__);
+		goto fail_no_btn_input_dev;
+	}
+
+	ret = create_regulators(dd);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "%s: failed to create regulators\n",
+			__func__);
+		goto fail_no_regulators;
+	}
+	dd->btn_state = BUTTON_UNK;
+
+	dd->irq_work_queue = create_singlethread_workqueue("abx500_accdet_wq");
+	if (!dd->irq_work_queue) {
+		dev_err(&pdev->dev, "%s: Failed to create wq\n", __func__);
+		ret = -ENOMEM;
+		goto fail_no_mem_for_wq;
+	}
+
+	dd->gpadc = dd->accdet_abx500_gpadc_get();
+
+	INIT_DELAYED_WORK(&dd->detect_work, detect_work);
+	INIT_DELAYED_WORK(&dd->unplug_irq_work, unplug_irq_handler_work);
+	INIT_DELAYED_WORK(&dd->init_work, init_work);
+
+	/* Deferred init/detect since no use for the info early in boot */
+	queue_delayed_work(dd->irq_work_queue,
+				&dd->init_work,
+				msecs_to_jiffies(INIT_DELAY_MS));
+
+	platform_set_drvdata(pdev, dd);
+
+	return 0;
+fail_no_mem_for_wq:
+	free_regulators(dd);
+fail_no_regulators:
+	input_unregister_device(dd->btn_input_dev);
+fail_no_btn_input_dev:
+	if (dd->pdata->mic_ctrl)
+		gpio_free(dd->pdata->mic_ctrl);
+mic_ctrl_fail:
+	if (dd->pdata->video_ctrl_gpio)
+		gpio_free(dd->pdata->video_ctrl_gpio);
+	return ret;
+}
+
+/*
+ * Performs platform device cleanup
+ */
+static void abx500_accessory_cleanup(struct abx500_ad *dd)
+{
+	dev_dbg(&dd->pdev->dev, "Enter: %s\n", __func__);
+
+	dd->jack_type = JACK_TYPE_UNSPECIFIED;
+	config_accdetect(dd);
+
+	if (dd->pdata->mic_ctrl)
+		gpio_free(dd->pdata->mic_ctrl);
+
+	if (dd->pdata->video_ctrl_gpio)
+		gpio_free(dd->pdata->video_ctrl_gpio);
+
+	input_unregister_device(dd->btn_input_dev);
+	free_regulators(dd);
+
+	cancel_delayed_work(&dd->detect_work);
+	cancel_delayed_work(&dd->unplug_irq_work);
+	cancel_delayed_work(&dd->init_work);
+	flush_workqueue(dd->irq_work_queue);
+	destroy_workqueue(dd->irq_work_queue);
+
+}
+
+static int __devinit abx500_acc_detect_probe(struct platform_device *pdev)
+{
+
+	return abx500_accessory_init(pdev);
+}
+
+static int __devexit abx500_acc_detect_remove(struct platform_device *pdev)
+{
+	abx500_accessory_cleanup(platform_get_drvdata(pdev));
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+#if defined(CONFIG_PM)
+static int abx500_acc_detect_suspend(struct device *dev)
+{
+	struct platform_device *pdev = container_of(dev,
+		struct platform_device, dev);
+	struct abx500_ad *dd = platform_get_drvdata(pdev);
+	int irq_id, irq;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	cancel_delayed_work_sync(&dd->unplug_irq_work);
+	cancel_delayed_work_sync(&dd->detect_work);
+	cancel_delayed_work_sync(&dd->init_work);
+
+	if (dd->pdata->is_detection_inverted)
+		abx500_accdet_irq_desc = dd->irq_desc_inverted;
+	else
+		abx500_accdet_irq_desc = dd->irq_desc_norm;
+
+	for (irq_id = 0; irq_id < dd->no_irqs; irq_id++) {
+		if (abx500_accdet_irq_desc[irq_id].registered == 1) {
+			irq = platform_get_irq_byname(
+					dd->pdev,
+					abx500_accdet_irq_desc[irq_id].name);
+
+			disable_irq(irq);
+		}
+	}
+
+	dd->turn_off_accdet_comparator(pdev);
+
+	if (dd->jack_type == JACK_TYPE_HEADSET)
+		accessory_regulator_disable(dd, REGULATOR_VAMIC1);
+
+	return 0;
+}
+
+static int abx500_acc_detect_resume(struct device *dev)
+{
+	struct platform_device *pdev = container_of(dev,
+		struct platform_device, dev);
+	struct abx500_ad *dd = platform_get_drvdata(pdev);
+	int irq_id, irq;
+
+	dev_dbg(&dd->pdev->dev, "%s: Enter\n", __func__);
+
+	if (dd->jack_type == JACK_TYPE_HEADSET)
+		accessory_regulator_enable(dd, REGULATOR_VAMIC1);
+
+	dd->turn_on_accdet_comparator(pdev);
+
+	if (dd->pdata->is_detection_inverted)
+		abx500_accdet_irq_desc = dd->irq_desc_inverted;
+	else
+		abx500_accdet_irq_desc = dd->irq_desc_norm;
+
+	for (irq_id = 0; irq_id < dd->no_irqs; irq_id++) {
+		if (abx500_accdet_irq_desc[irq_id].registered == 1) {
+			irq = platform_get_irq_byname(
+					dd->pdev,
+					abx500_accdet_irq_desc[irq_id].name);
+
+			enable_irq(irq);
+
+		}
+	}
+
+	/* After resume, reinitialize */
+	dd->gpio35_dir_set = dd->accdet1_th_set = dd->accdet2_th_set = 0;
+	queue_delayed_work(dd->irq_work_queue, &dd->init_work, 0);
+
+	return 0;
+}
+#else
+#define abx500_acc_detect_suspend	NULL
+#define abx500_acc_detect_resume	NULL
+#endif
+
+static struct platform_device_id abx500_accdet_ids[] = {
+#ifdef CONFIG_INPUT_AB5500_ACCDET
+	{ "ab5500-acc-det", (kernel_ulong_t)&ab5500_accessory_det_callbacks, },
+#endif
+#ifdef CONFIG_INPUT_AB8500_ACCDET
+	{ "ab8500-acc-det", (kernel_ulong_t)&ab8500_accessory_det_callbacks, },
+#endif
+	{ },
+};
+
+static const struct dev_pm_ops abx_ops = {
+	.suspend = abx500_acc_detect_suspend,
+	.resume = abx500_acc_detect_resume,
+};
+
+static struct platform_driver abx500_acc_detect_platform_driver = {
+	.driver = {
+		.name = "abx500-acc-det",
+		.owner = THIS_MODULE,
+		.pm = &abx_ops,
+	},
+	.probe = abx500_acc_detect_probe,
+	.id_table = abx500_accdet_ids,
+	.remove	= __devexit_p(abx500_acc_detect_remove),
+};
+
+static int __init abx500_acc_detect_init(void)
+{
+	return platform_driver_register(&abx500_acc_detect_platform_driver);
+}
+
+static void __exit abx500_acc_detect_exit(void)
+{
+	platform_driver_unregister(&abx500_acc_detect_platform_driver);
+}
+
+module_init(abx500_acc_detect_init);
+module_exit(abx500_acc_detect_exit);
+
+MODULE_DESCRIPTION("ABx500 AV Accessory detection driver");
+MODULE_ALIAS("platform:abx500-acc-det");
+MODULE_AUTHOR("ST-Ericsson");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/misc/lps001wp_prs.c b/drivers/input/misc/lps001wp_prs.c
new file mode 100644
index 00000000000..45a322729ff
--- /dev/null
+++ b/drivers/input/misc/lps001wp_prs.c
@@ -0,0 +1,1453 @@
+
+/******************** (C) COPYRIGHT 2010 STMicroelectronics ********************
+*
+* File Name	: lps001wp_prs.c
+* Authors	: MSH - Motion Mems BU - Application Team
+*		: Matteo Dameno (matteo.dameno@st.com)
+*		: Carmine Iascone (carmine.iascone@st.com)
+*		: Both authors are willing to be considered the contact
+*		: and update points for the driver.
+* Version	: V 1.1.1
+* Date		: 2010/11/22
+* Description	: LPS001WP pressure temperature sensor driver
+*
+********************************************************************************
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* THE PRESENT SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES
+* OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, FOR THE SOLE
+* PURPOSE TO SUPPORT YOUR APPLICATION DEVELOPMENT.
+* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+*
+******************************************************************************
+
+ Revision 0.9.0 01/10/2010:
+	first beta release
+ Revision 1.1.0 05/11/2010:
+	add sysfs management
+ Revision 1.1.1 22/11/2010:
+	moved to input/misc
+******************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/device.h>
+#include <linux/regulator/consumer.h>
+
+#include <linux/input/lps001wp.h>
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+#define	DEBUG	1
+
+#define	PR_ABS_MAX	 0xffff
+#define	PR_ABS_MIN	 0x0000
+#define	PR_DLT_MAX	 0x7ffff
+#define	PR_DLT_MIN	-0x80000 /* 16-bit signed value */
+#define	TEMP_MAX	 0x7fff
+#define TEMP_MIN	-0x80000 /* 16-bit signed value */
+
+
+#define	SENSITIVITY_T_SHIFT	6	/** =	64 LSB/degrC	*/
+#define	SENSITIVITY_P_SHIFT	4	/** =	16 LSB/mbar	*/
+
+
+#define	OUTDATA_REG	0x28
+#define	REF_PRESS_REG	0X30
+
+#define	WHOAMI_LPS001WP_PRS	0xBA	/*	Expctd content for WAI	*/
+
+/*	CONTROL REGISTERS	*/
+#define	WHO_AM_I	0x0F		/*	WhoAmI register		*/
+#define	CTRL_REG1	0x20		/*	power / ODR control reg	*/
+#define	CTRL_REG2	0x21		/*	boot reg		*/
+#define	CTRL_REG3	0x22		/*	interrupt control reg	*/
+
+#define	STATUS_REG	0X27		/*	status reg		*/
+
+#define PRESS_OUT_L	OUTDATA_REG
+
+
+#define	REF_P_L		REF_PRESS_REG	/*	pressure reference	*/
+#define	REF_P_H		0x31		/*	pressure reference	*/
+#define	THS_P_L		0x32		/*	pressure threshold	*/
+#define	THS_P_H		0x33		/*	pressure threshold	*/
+
+#define	INT_CFG		0x34		/*	interrupt config	*/
+#define	INT_SRC		0x35		/*	interrupt source	*/
+#define	INT_ACK		0x36		/*	interrupt acknoledge	*/
+/*	end CONTROL REGISTRES	*/
+
+
+/* Barometer and Termometer output data rate ODR */
+#define	LPS001WP_PRS_ODR_MASK	0x30	/* Mask to access odr bits only	*/
+#define	LPS001WP_PRS_ODR_7_1	0x00	/* 7Hz baro and 1Hz term ODR	*/
+#define	LPS001WP_PRS_ODR_7_7	0x10	/* 7Hz baro and 7Hz term ODR	*/
+#define	LPS001WP_PRS_ODR_12_12	0x30	/* 12.5Hz baro and 12.5Hz term ODR */
+
+#define	LPS001WP_PRS_ENABLE_MASK	0x40	/*  */
+#define	LPS001WP_PRS_DIFF_MASK		0x08
+#define LPS001WP_PRS_LPOW_MASK		0x80
+
+#define	LPS001WP_PRS_DIFF_ON		0x08
+#define	LPS001WP_PRS_DIFF_OFF		0x00
+
+#define	LPS001WP_PRS_LPOW_ON		0x80
+#define	LPS001WP_PRS_LPOW_OFF		0x00
+
+#define	FUZZ			0
+#define	FLAT			0
+#define	I2C_RETRY_DELAY		5
+#define	I2C_RETRIES		5
+#define	I2C_AUTO_INCREMENT	0x80
+
+/* RESUME STATE INDICES */
+#define	RES_CTRL_REG1		0
+#define	RES_CTRL_REG2		1
+#define	RES_CTRL_REG3		2
+#define	RES_REF_P_L		3
+#define	RES_REF_P_H		4
+#define	RES_THS_P_L		5
+#define	RES_THS_P_H		6
+#define	RES_INT_CFG		7
+
+#define	RESUME_ENTRIES		8
+/* end RESUME STATE INDICES */
+
+/* Pressure Sensor Operating Mode */
+#define	LPS001WP_PRS_DIFF_ENABLE	1
+#define LPS001WP_PRS_DIFF_DISABLE	0
+#define	LPS001WP_PRS_LPOWER_EN		1
+#define	LPS001WP_PRS_LPOWER_DIS		0
+
+static const struct {
+	unsigned int cutoff_ms;
+	unsigned int mask;
+} lps001wp_prs_odr_table[] = {
+		{80,	LPS001WP_PRS_ODR_12_12 },
+		{143,	LPS001WP_PRS_ODR_7_7 },
+		{1000,	LPS001WP_PRS_ODR_7_1 },
+};
+
+/**
+ * struct lps001wp_prs_data - data structure used by lps001wp_prs driver
+ * @client: i2c client
+ * @pdata: lsm303dlh platform data
+ * @lock: mutex lock for sysfs operations
+ * @input_work: work queue to read sensor data
+ * @input_dev: input device
+ * @regulator: regulator
+ * @early_suspend: early suspend structure
+ * @hw_initialized: saves hw initialisation status
+ * @hw_working: saves hw status
+ * @diff_enabled: store value of diff enable
+ * @lpowmode_enabled: flag to set lowpower mode
+ * @enabled: to store mode of device
+ * @on_before_suspend: to store status of device during suspend
+ * @resume_state:store regester values
+ * @reg_addr: stores reg address to debug
+ */
+struct lps001wp_prs_data {
+	struct i2c_client *client;
+	struct lps001wp_prs_platform_data *pdata;
+
+	struct mutex lock;
+#ifdef CONFIG_LPS001WP_INPUT_DEVICE
+	struct delayed_work input_work;
+	struct input_dev *input_dev;
+#endif
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
+
+	int hw_initialized;
+	/* hw_working=-1 means not tested yet */
+	int hw_working;
+	u8 diff_enabled;
+	u8 lpowmode_enabled ;
+
+	atomic_t enabled;
+	int on_before_suspend;
+
+	u8 resume_state[RESUME_ENTRIES];
+
+	struct regulator *regulator;
+
+#ifdef DEBUG
+	u8 reg_addr;
+#endif
+};
+
+struct outputdata {
+	u16 abspress;
+	s16 temperature;
+	s16 deltapress;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void lps001wp_prs_early_suspend(struct early_suspend *data);
+static void lps001wp_prs_late_resume(struct early_suspend *data);
+#endif
+
+
+static int lps001wp_prs_i2c_read(struct lps001wp_prs_data *prs,
+				  u8 *buf, int len)
+{
+	int err;
+	int tries = 0;
+
+	struct i2c_msg msgs[] = {
+		{
+		 .addr = prs->client->addr,
+		 .flags = prs->client->flags & I2C_M_TEN,
+		 .len = 1,
+		 .buf = buf,
+		 },
+		{
+		 .addr = prs->client->addr,
+		 .flags = (prs->client->flags & I2C_M_TEN) | I2C_M_RD,
+		 .len = len,
+		 .buf = buf,
+		 },
+	};
+
+	do {
+		err = i2c_transfer(prs->client->adapter, msgs, 2);
+		if (err != 2)
+			msleep_interruptible(I2C_RETRY_DELAY);
+	} while ((err != 2) && (++tries < I2C_RETRIES));
+
+	if (err != 2) {
+		dev_err(&prs->client->dev, "read transfer error\n");
+		err = -EIO;
+	} else {
+		err = 0;
+	}
+
+	return err;
+}
+
+static int lps001wp_prs_i2c_write(struct lps001wp_prs_data *prs,
+				   u8 *buf, int len)
+{
+	int err;
+	int tries = 0;
+	struct i2c_msg msgs[] = {
+		{
+		 .addr = prs->client->addr,
+		 .flags = prs->client->flags & I2C_M_TEN,
+		 .len = len + 1,
+		 .buf = buf,
+		 },
+	};
+
+	do {
+		err = i2c_transfer(prs->client->adapter, msgs, 1);
+		if (err != 1)
+			msleep_interruptible(I2C_RETRY_DELAY);
+	} while ((err != 1) && (++tries < I2C_RETRIES));
+
+	if (err != 1) {
+		dev_err(&prs->client->dev, "write transfer error\n");
+		err = -EIO;
+	} else {
+		err = 0;
+	}
+
+	return err;
+}
+
+static int lps001wp_prs_register_write(struct lps001wp_prs_data *prs, u8 *buf,
+		u8 reg_address, u8 new_value)
+{
+	int err = -EINVAL;
+
+	/* Sets configuration register at reg_address
+	 *  NOTE: this is a straight overwrite  */
+	buf[0] = reg_address;
+	buf[1] = new_value;
+	err = lps001wp_prs_i2c_write(prs, buf, 1);
+	if (err < 0)
+		return err;
+	return err;
+}
+
+static int lps001wp_prs_register_read(struct lps001wp_prs_data *prs, u8 *buf,
+		u8 reg_address)
+{
+
+	int err = -EINVAL;
+	buf[0] = (reg_address);
+	err = lps001wp_prs_i2c_read(prs, buf, 1);
+
+	return err;
+}
+
+static int lps001wp_prs_register_update(struct lps001wp_prs_data *prs, u8 *buf,
+		u8 reg_address, u8 mask, u8 new_bit_values)
+{
+	int err = -EINVAL;
+	u8 init_val;
+	u8 updated_val;
+	err = lps001wp_prs_register_read(prs, buf, reg_address);
+	if (!(err < 0)) {
+		init_val = buf[0];
+		updated_val = ((mask & new_bit_values) | ((~mask) & init_val));
+		err = lps001wp_prs_register_write(prs, buf, reg_address,
+				updated_val);
+	}
+	return err;
+}
+
+/* */
+
+
+static int lps001wp_prs_hw_init(struct lps001wp_prs_data *prs)
+{
+	int err = -EINVAL;
+	u8 buf[6];
+
+	dev_dbg(&prs->client->dev, "%s: hw init start\n",
+					LPS001WP_PRS_DEV_NAME);
+
+	buf[0] = WHO_AM_I;
+	err = lps001wp_prs_i2c_read(prs, buf, 1);
+	if (err < 0)
+		goto error_firstread;
+	else
+		prs->hw_working = 1;
+	if (buf[0] != WHOAMI_LPS001WP_PRS) {
+		err = -EINVAL; /* TODO:choose the right coded error */
+		goto error_unknown_device;
+	}
+
+
+	buf[0] = (I2C_AUTO_INCREMENT | REF_PRESS_REG);
+	buf[1] = prs->resume_state[RES_REF_P_L];
+	buf[2] = prs->resume_state[RES_REF_P_H];
+	buf[3] = prs->resume_state[RES_THS_P_L];
+	buf[4] = prs->resume_state[RES_THS_P_H];
+	err = lps001wp_prs_i2c_write(prs, buf, 4);
+	if (err < 0)
+		goto error1;
+
+	buf[0] = (I2C_AUTO_INCREMENT | CTRL_REG1);
+	buf[1] = prs->resume_state[RES_CTRL_REG1];
+	buf[2] = prs->resume_state[RES_CTRL_REG2];
+	buf[3] = prs->resume_state[RES_CTRL_REG3];
+	err = lps001wp_prs_i2c_write(prs, buf, 3);
+	if (err < 0)
+		goto error1;
+
+	buf[0] = INT_CFG;
+	buf[1] = prs->resume_state[RES_INT_CFG];
+	err = lps001wp_prs_i2c_write(prs, buf, 1);
+	if (err < 0)
+		goto error1;
+
+
+	prs->hw_initialized = 1;
+	dev_dbg(&prs->client->dev, "%s: hw init done\n", LPS001WP_PRS_DEV_NAME);
+	return 0;
+
+error_firstread:
+	prs->hw_working = 0;
+	dev_warn(&prs->client->dev, "Error reading WHO_AM_I: is device "
+		"available/working?\n");
+	goto error1;
+error_unknown_device:
+	dev_err(&prs->client->dev,
+		"device unknown. Expected: 0x%x,"
+		" Replies: 0x%x\n", WHOAMI_LPS001WP_PRS, buf[0]);
+error1:
+	prs->hw_initialized = 0;
+	dev_err(&prs->client->dev, "hw init error 0x%x,0x%x: %d\n", buf[0],
+			buf[1], err);
+	return err;
+}
+
+static void lps001wp_prs_device_power_off(struct lps001wp_prs_data *prs)
+{
+	int err;
+	u8 buf[2] = { CTRL_REG1, LPS001WP_PRS_PM_OFF };
+
+	err = lps001wp_prs_i2c_write(prs, buf, 1);
+	if (err < 0)
+		dev_err(&prs->client->dev, "soft power off failed: %d\n", err);
+
+	/* disable regulator */
+	if (prs->regulator) {
+		err = regulator_disable(prs->regulator);
+		if (err < 0)
+			dev_err(&prs->client->dev, "failed to disable regulator\n");
+	}
+
+	if (prs->hw_initialized) {
+		prs->hw_initialized = 0;
+	}
+
+}
+
+static int lps001wp_prs_device_power_on(struct lps001wp_prs_data *prs)
+{
+	int err = -EINVAL;
+
+	/* get the regulator the first time */
+	if (!prs->regulator) {
+		prs->regulator = regulator_get(&prs->client->dev, "vdd");
+		if (IS_ERR(prs->regulator)) {
+			dev_err(&prs->client->dev, "failed to get regulator\n");
+			prs->regulator = NULL;
+			return PTR_ERR(prs->regulator);
+		}
+	}
+
+	/* enable it also */
+	err = regulator_enable(prs->regulator);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "failed to enable regulator\n");
+		return err;
+	}
+
+	if (!prs->hw_initialized) {
+		err = lps001wp_prs_hw_init(prs);
+		if (prs->hw_working == 1 && err < 0) {
+			lps001wp_prs_device_power_off(prs);
+			return err;
+		}
+	}
+
+	return 0;
+}
+
+
+
+int lps001wp_prs_update_odr(struct lps001wp_prs_data *prs, int poll_interval_ms)
+{
+	int err = -EINVAL;
+	int i;
+
+	u8 buf[2];
+	u8 updated_val;
+	u8 init_val;
+	u8 new_val;
+	u8 mask = LPS001WP_PRS_ODR_MASK;
+
+	/* Following, looks for the longest possible odr interval scrolling the
+	 * odr_table vector from the end (shortest interval) backward (longest
+	 * interval), to support the poll_interval requested by the system.
+	 * It must be the longest interval lower then the poll interval.*/
+	for (i = ARRAY_SIZE(lps001wp_prs_odr_table) - 1; i >= 0; i--) {
+		if (lps001wp_prs_odr_table[i].cutoff_ms <= poll_interval_ms)
+			break;
+	}
+
+	new_val = lps001wp_prs_odr_table[i].mask;
+
+	/* If device is currently enabled, we need to write new
+	 *  configuration out to it */
+	if (atomic_read(&prs->enabled)) {
+		buf[0] = CTRL_REG1;
+		err = lps001wp_prs_i2c_read(prs, buf, 1);
+		if (err < 0)
+			goto error;
+		init_val = buf[0];
+		prs->resume_state[RES_CTRL_REG1] = init_val;
+
+		buf[0] = CTRL_REG1;
+		updated_val = ((mask & new_val) | ((~mask) & init_val));
+		buf[1] = updated_val;
+		buf[0] = CTRL_REG1;
+		err = lps001wp_prs_i2c_write(prs, buf, 1);
+		if (err < 0)
+			goto error;
+		prs->resume_state[RES_CTRL_REG1] = updated_val;
+	}
+	return err;
+
+error:
+	dev_err(&prs->client->dev, "update odr failed 0x%x,0x%x: %d\n",
+			buf[0], buf[1], err);
+
+	return err;
+}
+
+static int lps001wp_prs_set_press_reference(struct lps001wp_prs_data *prs,
+				u16 new_reference)
+{
+	int err = -EINVAL;
+	u8 const reg_addressL = REF_P_L;
+	u8 const reg_addressH = REF_P_H;
+	u8 bit_valuesL, bit_valuesH;
+	u8 buf[2];
+	/*
+	 * We need to set new configurations, only if device
+	 * is currently enabled
+	 */
+	if (!atomic_read(&prs->enabled))
+		return err;
+	bit_valuesL = (u8) (new_reference & 0x00FF);
+	bit_valuesH = (u8)((new_reference & 0xFF00) >> 8);
+
+	err = lps001wp_prs_register_write(prs, buf, reg_addressL,
+			bit_valuesL);
+	if (err < 0)
+		return err;
+	err = lps001wp_prs_register_write(prs, buf, reg_addressH,
+			bit_valuesH);
+	if (err < 0) {
+		lps001wp_prs_register_write(prs, buf, reg_addressL,
+				prs->resume_state[RES_REF_P_L]);
+		return err;
+	}
+	prs->resume_state[RES_REF_P_L] = bit_valuesL;
+	prs->resume_state[RES_REF_P_H] = bit_valuesH;
+	return err;
+}
+
+static int lps001wp_prs_get_press_reference(struct lps001wp_prs_data *prs,
+		u16 *buf16)
+{
+	int err = -EINVAL;
+
+	u8 bit_valuesL, bit_valuesH;
+	u8 buf[2] = {0};
+	u16 temp = 0;
+	/*
+	 * We need to read configurations, only if device
+	 * is currently enabled
+	 */
+	if (!atomic_read(&prs->enabled))
+		return err;
+	err = lps001wp_prs_register_read(prs, buf, REF_P_L);
+	if (err < 0)
+		return err;
+	bit_valuesL = buf[0];
+	err = lps001wp_prs_register_read(prs, buf, REF_P_H);
+	if (err < 0)
+		return err;
+	bit_valuesH = buf[0];
+
+	temp = (((u16) bit_valuesH) << 8);
+	*buf16 = (temp | ((u16) bit_valuesL));
+
+	return err;
+}
+
+static int lps001wp_prs_lpow_manage(struct lps001wp_prs_data *prs, u8 control)
+{
+	int err = -EINVAL;
+	u8 buf[2] = {0x00, 0x00};
+	u8 const mask = LPS001WP_PRS_LPOW_MASK;
+	u8 bit_values = LPS001WP_PRS_LPOW_OFF;
+
+	/*
+	 * We need to set new configurations, only if device
+	 * is currently enabled
+	 */
+	if (!atomic_read(&prs->enabled))
+		return err;
+	if (control >= LPS001WP_PRS_LPOWER_EN) {
+		bit_values = LPS001WP_PRS_LPOW_ON;
+	}
+
+	err = lps001wp_prs_register_update(prs, buf, CTRL_REG1,
+			mask, bit_values);
+
+	if (err < 0)
+		return err;
+	prs->resume_state[RES_CTRL_REG1] = ((mask & bit_values) |
+			(~mask & prs->resume_state[RES_CTRL_REG1]));
+	if (bit_values == LPS001WP_PRS_LPOW_ON)
+		prs->lpowmode_enabled = 1;
+	else
+		prs->lpowmode_enabled = 0;
+	return err;
+}
+
+static int lps001wp_prs_diffen_manage(struct lps001wp_prs_data *prs, u8 control)
+{
+	int err = -EINVAL;
+	u8 buf[2] = {0x00, 0x00};
+	u8 const mask = LPS001WP_PRS_DIFF_MASK;
+	u8 bit_values = LPS001WP_PRS_DIFF_OFF;
+
+	/*
+	 * We need to set new configurations, only if device
+	 * is currently enabled
+	 */
+	if (!atomic_read(&prs->enabled))
+		return err;
+	if (control >= LPS001WP_PRS_DIFF_ENABLE) {
+		bit_values = LPS001WP_PRS_DIFF_ON;
+	}
+
+	err = lps001wp_prs_register_update(prs, buf, CTRL_REG1,
+			mask, bit_values);
+
+	if (err < 0)
+		return err;
+	prs->resume_state[RES_CTRL_REG1] = ((mask & bit_values) |
+			(~mask & prs->resume_state[RES_CTRL_REG1]));
+	if (bit_values == LPS001WP_PRS_DIFF_ON)
+		prs->diff_enabled = 1;
+	else
+		prs->diff_enabled = 0;
+	return err;
+}
+
+
+static int lps001wp_prs_get_presstemp_data(struct lps001wp_prs_data *prs,
+						struct outputdata *out)
+{
+	int err = -EINVAL;
+	/* Data bytes from hardware	PRESS_OUT_L, PRESS_OUT_H,
+	 *				TEMP_OUT_L, TEMP_OUT_H,
+	 *				DELTA_L, DELTA_H */
+	u8 prs_data[6];
+
+	u16 abspr;
+	s16 temperature, deltapr;
+	int regToRead = 4;
+	prs_data[4] = 0;
+	prs_data[5] = 0;
+
+	if (prs->diff_enabled)
+		regToRead = 6;
+
+	prs_data[0] = (I2C_AUTO_INCREMENT | OUTDATA_REG);
+	err = lps001wp_prs_i2c_read(prs, prs_data, regToRead);
+	if (err < 0)
+		return err;
+
+	abspr = ((((u16) prs_data[1] << 8) | ((u16) prs_data[0])));
+	temperature = ((s16) (((u16) prs_data[3] << 8) | ((u16)prs_data[2])));
+
+	out->abspress = (abspr >> SENSITIVITY_P_SHIFT);
+	out->temperature = (temperature >> SENSITIVITY_T_SHIFT);
+
+	deltapr = ((s16) (((u16) prs_data[5] << 8) | ((u16)prs_data[4])));
+	out->deltapress = deltapr;
+
+	return err;
+}
+
+#ifdef  CONFIG_LPS001WP_INPUT_DEVICE
+static void lps001wp_prs_report_values(struct lps001wp_prs_data *prs,
+					struct outputdata *out)
+{
+	input_report_abs(prs->input_dev, ABS_PR, out->abspress);
+	input_report_abs(prs->input_dev, ABS_TEMP, out->temperature);
+	input_report_abs(prs->input_dev, ABS_DLTPR, out->deltapress);
+	input_sync(prs->input_dev);
+}
+#endif
+
+static int lps001wp_prs_enable(struct lps001wp_prs_data *prs)
+{
+	int err;
+
+	if (!atomic_cmpxchg(&prs->enabled, 0, 1)) {
+		if (prs->regulator)
+			regulator_enable(prs->regulator);
+		err = lps001wp_prs_device_power_on(prs);
+		if (err < 0) {
+			atomic_set(&prs->enabled, 0);
+			return err;
+		}
+#ifdef CONFIG_LPS001WP_INPUT_DEVICE
+		schedule_delayed_work(&prs->input_work,
+			msecs_to_jiffies(prs->pdata->poll_interval));
+#endif
+	}
+
+	return 0;
+}
+
+static int lps001wp_prs_disable(struct lps001wp_prs_data *prs)
+{
+	if (atomic_cmpxchg(&prs->enabled, 1, 0)) {
+#ifdef CONFIG_LPS001WP_INPUT_DEVICE
+		cancel_delayed_work_sync(&prs->input_work);
+#endif
+		lps001wp_prs_device_power_off(prs);
+		if (prs->regulator)
+			regulator_disable(prs->regulator);
+	}
+
+	return 0;
+}
+
+static ssize_t attr_get_polling_rate(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	int val;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	mutex_lock(&prs->lock);
+	val = prs->pdata->poll_interval;
+	mutex_unlock(&prs->lock);
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_polling_rate(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t size)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long interval_ms;
+	int err = -EINVAL;
+
+	if (strict_strtoul(buf, 10, &interval_ms))
+		return -EINVAL;
+	if (!interval_ms)
+		return -EINVAL;
+	mutex_lock(&prs->lock);
+	prs->pdata->poll_interval = interval_ms;
+	err = lps001wp_prs_update_odr(prs, interval_ms);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "failed to update odr %ld\n",
+								interval_ms);
+		size = err;
+	}
+	mutex_unlock(&prs->lock);
+	return size;
+}
+
+static ssize_t attr_get_diff_enable(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	u8 val;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	mutex_lock(&prs->lock);
+	val = prs->diff_enabled;
+	mutex_unlock(&prs->lock);
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_diff_enable(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t size)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val;
+	int err = -EINVAL;
+
+	if (strict_strtoul(buf, 10, &val))
+		return -EINVAL;
+
+	mutex_lock(&prs->lock);
+	err = lps001wp_prs_diffen_manage(prs, (u8) val);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "failed to diff enable %ld\n", val);
+		mutex_unlock(&prs->lock);
+		return err;
+	}
+	mutex_unlock(&prs->lock);
+	return size;
+}
+
+static ssize_t attr_get_enable(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	int val = atomic_read(&prs->enabled);
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_enable(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 10, &val))
+		return -EINVAL;
+
+	if (val)
+		lps001wp_prs_enable(prs);
+	else
+		lps001wp_prs_disable(prs);
+
+	return size;
+}
+
+static ssize_t attr_get_press_ref(struct device *dev,
+				  struct device_attribute *attr, char *buf)
+{
+	int err = -EINVAL;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	u16 val = 0;
+
+	mutex_lock(&prs->lock);
+	err = lps001wp_prs_get_press_reference(prs, &val);
+	mutex_unlock(&prs->lock);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "failed to get ref press\n");
+		return err;
+	}
+
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_press_ref(struct device *dev,
+				  struct device_attribute *attr,
+				  const char *buf, size_t size)
+{
+	int err = -EINVAL;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val = 0;
+
+	if (strict_strtoul(buf, 10, &val))
+		return -EINVAL;
+
+	if (val < PR_ABS_MIN || val > PR_ABS_MAX)
+		return -EINVAL;
+
+	mutex_lock(&prs->lock);
+	err = lps001wp_prs_set_press_reference(prs, val);
+	mutex_unlock(&prs->lock);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "failed to set ref press %ld\n",
+								val);
+		return err;
+	}
+	return size;
+}
+
+
+static ssize_t attr_get_lowpowmode(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	u8 val;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	mutex_lock(&prs->lock);
+	val = prs->lpowmode_enabled;
+	mutex_unlock(&prs->lock);
+	return sprintf(buf, "%d\n", val);
+}
+
+static ssize_t attr_set_lowpowmode(struct device *dev,
+				   struct device_attribute *attr,
+				   const char *buf, size_t size)
+{
+	int err = -EINVAL;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 10, &val))
+		return -EINVAL;
+
+	mutex_lock(&prs->lock);
+	err = lps001wp_prs_lpow_manage(prs, (u8) val);
+	mutex_unlock(&prs->lock);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "failed to set low powermode\n");
+		return err;
+	}
+	return size;
+}
+static ssize_t lps001wp_prs_get_press_data(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	struct outputdata out;
+	int err = -EINVAL;
+	mutex_lock(&prs->lock);
+	/*
+	 * If device is currently enabled, we need to read
+	 *  data from it.
+	 */
+	if (!atomic_read(&prs->enabled))
+		goto out;
+	err = lps001wp_prs_get_presstemp_data(prs, &out);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "get_pressure_data failed\n");
+		goto out;
+	}
+	mutex_unlock(&prs->lock);
+	return sprintf(buf, "%d", out.abspress);
+out:
+	mutex_unlock(&prs->lock);
+	return err;
+}
+
+static ssize_t lps001wp_prs_get_deltapr_data(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	struct outputdata out;
+	int err = -EINVAL;
+	mutex_lock(&prs->lock);
+	/*
+	 * If device is currently enabled, we need to read
+	 *  data from it.
+	 */
+	if (!atomic_read(&prs->enabled)) {
+		mutex_unlock(&prs->lock);
+		return err;
+	}
+	err = lps001wp_prs_get_presstemp_data(prs, &out);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "get_deltapress_data failed\n");
+		mutex_unlock(&prs->lock);
+		return err;
+	}
+	mutex_unlock(&prs->lock);
+	return sprintf(buf, "%d", out.deltapress);
+}
+
+static ssize_t lps001wp_prs_get_temp_data(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	struct outputdata out;
+	int err = -EINVAL;
+	mutex_lock(&prs->lock);
+	/*
+	 * If device is currently enabled, we need to read
+	 *  data from it.
+	 */
+	if (!atomic_read(&prs->enabled)) {
+		mutex_unlock(&prs->lock);
+		return err;
+	}
+	err = lps001wp_prs_get_presstemp_data(prs, &out);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "get_temperature_data failed\n");
+		mutex_unlock(&prs->lock);
+		return err;
+	}
+	mutex_unlock(&prs->lock);
+	return sprintf(buf, "%d", out.temperature);
+}
+#ifdef DEBUG
+static ssize_t attr_reg_set(struct device *dev, struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	int rc;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	u8 x[2];
+	unsigned long val;
+
+	if (strict_strtoul(buf, 16, &val))
+		return -EINVAL;
+	mutex_lock(&prs->lock);
+	x[0] = prs->reg_addr;
+	mutex_unlock(&prs->lock);
+	x[1] = val;
+	rc = lps001wp_prs_i2c_write(prs, x, 1);
+	/*TODO: error need to be managed */
+	return size;
+}
+
+static ssize_t attr_reg_get(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	ssize_t ret;
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	int rc;
+	u8 data;
+
+	mutex_lock(&prs->lock);
+	data = prs->reg_addr;
+	mutex_unlock(&prs->lock);
+	rc = lps001wp_prs_i2c_read(prs, &data, 1);
+	/*TODO: error need to be managed */
+	ret = sprintf(buf, "0x%02x\n", data);
+	return ret;
+}
+
+static ssize_t attr_addr_set(struct device *dev, struct device_attribute *attr,
+				const char *buf, size_t size)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	unsigned long val;
+	if (strict_strtoul(buf, 16, &val))
+		return -EINVAL;
+	mutex_lock(&prs->lock);
+	prs->reg_addr = val;
+	mutex_unlock(&prs->lock);
+	return size;
+}
+#endif
+
+
+
+static struct device_attribute attributes[] = {
+	__ATTR(pollrate_ms, S_IWUSR | S_IRUGO, attr_get_polling_rate,
+		attr_set_polling_rate),
+	__ATTR(enable, S_IWUGO | S_IRUGO, attr_get_enable, attr_set_enable),
+	__ATTR(diff_enable, S_IWUSR | S_IRUGO, attr_get_diff_enable,
+		attr_set_diff_enable),
+	__ATTR(press_reference, S_IWUSR | S_IRUGO, attr_get_press_ref,
+		attr_set_press_ref),
+	__ATTR(lowpow_enable, S_IWUSR | S_IRUGO, attr_get_lowpowmode,
+		attr_set_lowpowmode),
+	__ATTR(press_data, S_IRUGO, lps001wp_prs_get_press_data, NULL),
+	__ATTR(temp_data, S_IRUGO, lps001wp_prs_get_temp_data, NULL),
+	__ATTR(deltapr_data, S_IRUGO, lps001wp_prs_get_deltapr_data, NULL),
+#ifdef DEBUG
+	__ATTR(reg_value, S_IWUSR | S_IRUGO, attr_reg_get, attr_reg_set),
+	__ATTR(reg_addr, S_IWUSR, NULL, attr_addr_set),
+#endif
+};
+
+static int create_sysfs_interfaces(struct device *dev)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(attributes); i++)
+		if (device_create_file(dev, attributes + i))
+			goto error;
+	return 0;
+
+error:
+	for ( ; i >= 0; i--)
+		device_remove_file(dev, attributes + i);
+	dev_err(dev, "%s:Unable to create interface\n", __func__);
+	return -1;
+}
+
+static int remove_sysfs_interfaces(struct device *dev)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(attributes); i++)
+		device_remove_file(dev, attributes + i);
+	return 0;
+}
+
+#ifdef CONFIG_LPS001WP_INPUT_DEVICE
+static void lps001wp_prs_input_work_func(struct work_struct *work)
+{
+	struct lps001wp_prs_data *prs;
+
+	struct outputdata output;
+	struct outputdata *out = &output;
+	int err;
+
+	prs = container_of((struct delayed_work *)work,
+				struct lps001wp_prs_data, input_work);
+
+	mutex_lock(&prs->lock);
+	err = lps001wp_prs_get_presstemp_data(prs, out);
+	if (err < 0)
+		dev_err(&prs->client->dev, "get_pressure_data failed\n");
+	else
+		lps001wp_prs_report_values(prs, out);
+
+	schedule_delayed_work(&prs->input_work,
+				msecs_to_jiffies(prs->pdata->poll_interval));
+	mutex_unlock(&prs->lock);
+}
+
+int lps001wp_prs_input_open(struct input_dev *input)
+{
+	struct lps001wp_prs_data *prs = input_get_drvdata(input);
+
+	return lps001wp_prs_enable(prs);
+}
+
+void lps001wp_prs_input_close(struct input_dev *dev)
+{
+	struct lps001wp_prs_data *prs = input_get_drvdata(dev);
+
+	lps001wp_prs_disable(prs);
+}
+#endif
+
+static int lps001wp_prs_validate_pdata(struct lps001wp_prs_data *prs)
+{
+	prs->pdata->poll_interval = max(prs->pdata->poll_interval,
+					prs->pdata->min_interval);
+
+	/* Enforce minimum polling interval */
+	if (prs->pdata->poll_interval < prs->pdata->min_interval) {
+		dev_err(&prs->client->dev, "minimum poll interval violated\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+#ifdef CONFIG_LPS001WP_INPUT_DEVICE
+static int lps001wp_prs_input_init(struct lps001wp_prs_data *prs)
+{
+	int err;
+	INIT_DELAYED_WORK(&prs->input_work, lps001wp_prs_input_work_func);
+	prs->input_dev = input_allocate_device();
+	if (!prs->input_dev) {
+		err = -ENOMEM;
+		dev_err(&prs->client->dev, "input device allocate failed\n");
+		goto err0;
+	}
+
+	prs->input_dev->open = lps001wp_prs_input_open;
+	prs->input_dev->close = lps001wp_prs_input_close;
+	prs->input_dev->name = LPS001WP_PRS_DEV_NAME;
+	prs->input_dev->id.bustype = BUS_I2C;
+	prs->input_dev->dev.parent = &prs->client->dev;
+
+	input_set_drvdata(prs->input_dev, prs);
+
+	set_bit(EV_ABS, prs->input_dev->evbit);
+
+	input_set_abs_params(prs->input_dev, ABS_PR,
+			PR_ABS_MIN, PR_ABS_MAX, FUZZ, FLAT);
+	input_set_abs_params(prs->input_dev, ABS_TEMP,
+			PR_DLT_MIN, PR_DLT_MAX, FUZZ, FLAT);
+	input_set_abs_params(prs->input_dev, ABS_DLTPR,
+			TEMP_MIN, TEMP_MAX, FUZZ, FLAT);
+
+
+	prs->input_dev->name = "LPS001WP barometer";
+
+	err = input_register_device(prs->input_dev);
+	if (err) {
+		dev_err(&prs->client->dev,
+			"unable to register input polled device %s\n",
+			prs->input_dev->name);
+		goto err1;
+	}
+
+	return 0;
+
+err1:
+	input_free_device(prs->input_dev);
+err0:
+	return err;
+}
+
+static void lps001wp_prs_input_cleanup(struct lps001wp_prs_data *prs)
+{
+	input_unregister_device(prs->input_dev);
+	input_free_device(prs->input_dev);
+}
+#endif
+static int lps001wp_prs_probe(struct i2c_client *client,
+					const struct i2c_device_id *id)
+{
+	struct lps001wp_prs_data *prs;
+	int err = -EINVAL;
+	int tempvalue;
+
+	pr_info("%s: probe start.\n", LPS001WP_PRS_DEV_NAME);
+
+	if (client->dev.platform_data == NULL) {
+		dev_err(&client->dev, "platform data is NULL. exiting.\n");
+		err = -ENODEV;
+		goto exit_check_functionality_failed;
+	}
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "client not i2c capable\n");
+		err = -ENODEV;
+		goto exit_check_functionality_failed;
+	}
+
+	if (!i2c_check_functionality(client->adapter,
+					I2C_FUNC_SMBUS_BYTE |
+					I2C_FUNC_SMBUS_BYTE_DATA |
+					I2C_FUNC_SMBUS_WORD_DATA)) {
+		dev_err(&client->dev, "client not smb-i2c capable:2\n");
+		err = -EIO;
+		goto exit_check_functionality_failed;
+	}
+
+
+	if (!i2c_check_functionality(client->adapter,
+						I2C_FUNC_SMBUS_I2C_BLOCK)){
+		dev_err(&client->dev, "client not smb-i2c capable:3\n");
+		err = -EIO;
+		goto exit_check_functionality_failed;
+	}
+
+
+	prs = kzalloc(sizeof(struct lps001wp_prs_data), GFP_KERNEL);
+	if (prs == NULL) {
+		err = -ENOMEM;
+		dev_err(&client->dev,
+				"failed to allocate memory for module data: "
+					"%d\n", err);
+		goto exit_alloc_data_failed;
+	}
+
+	mutex_init(&prs->lock);
+	mutex_lock(&prs->lock);
+
+	prs->client = client;
+	i2c_set_clientdata(client, prs);
+
+	prs->regulator = regulator_get(&client->dev, "vdd");
+	if (IS_ERR(prs->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		err = PTR_ERR(prs->regulator);
+		prs->regulator = NULL;
+	}
+	if (prs->regulator)
+		regulator_enable(prs->regulator);
+
+	if (i2c_smbus_read_byte(client) < 0) {
+		dev_err(&client->dev, "i2c_smbus_read_byte error!!\n");
+		goto err_mutexunlockfreedata;
+	} else {
+		dev_dbg(&client->dev, "%s Device detected!\n",
+							LPS001WP_PRS_DEV_NAME);
+	}
+
+	/* read chip id */
+	tempvalue = i2c_smbus_read_word_data(client, WHO_AM_I);
+	if ((tempvalue & 0x00FF) == WHOAMI_LPS001WP_PRS) {
+		dev_dbg(&client->dev, "%s I2C driver registered!\n",
+							LPS001WP_PRS_DEV_NAME);
+	} else {
+		prs->client = NULL;
+		dev_dbg(&client->dev, "I2C driver not registered!"
+				" Device unknown\n");
+		goto err_mutexunlockfreedata;
+	}
+
+	prs->pdata = kmalloc(sizeof(*prs->pdata), GFP_KERNEL);
+	if (prs->pdata == NULL) {
+		err = -ENOMEM;
+		dev_err(&client->dev,
+				"failed to allocate memory for pdata: %d\n",
+				err);
+		goto err_mutexunlockfreedata;
+	}
+
+	memcpy(prs->pdata, client->dev.platform_data, sizeof(*prs->pdata));
+
+	err = lps001wp_prs_validate_pdata(prs);
+	if (err < 0) {
+		dev_err(&client->dev, "failed to validate platform data\n");
+		goto exit_kfree_pdata;
+	}
+
+	i2c_set_clientdata(client, prs);
+
+
+	if (prs->pdata->init) {
+		err = prs->pdata->init();
+		if (err < 0) {
+			dev_err(&client->dev, "init failed: %d\n", err);
+			goto err2;
+		}
+	}
+
+	memset(prs->resume_state, 0, ARRAY_SIZE(prs->resume_state));
+
+	prs->resume_state[RES_CTRL_REG1] = LPS001WP_PRS_PM_NORMAL;
+	prs->resume_state[RES_CTRL_REG2] = 0x00;
+	prs->resume_state[RES_CTRL_REG3] = 0x00;
+	prs->resume_state[RES_REF_P_L] = 0x00;
+	prs->resume_state[RES_REF_P_H] = 0x00;
+	prs->resume_state[RES_THS_P_L] = 0x00;
+	prs->resume_state[RES_THS_P_H] = 0x00;
+	prs->resume_state[RES_INT_CFG] = 0x00;
+
+	err = lps001wp_prs_device_power_on(prs);
+	if (err < 0) {
+		dev_err(&client->dev, "power on failed: %d\n", err);
+		goto err2;
+	}
+
+	prs->diff_enabled = 0;
+	prs->lpowmode_enabled = 0;
+	atomic_set(&prs->enabled, 1);
+
+	err = lps001wp_prs_update_odr(prs, prs->pdata->poll_interval);
+	if (err < 0) {
+		dev_err(&client->dev, "update_odr failed\n");
+		goto err_power_off;
+	}
+#ifdef CONFIG_LPS001WP_INPUT_DEVICE
+	err = lps001wp_prs_input_init(prs);
+	if (err < 0) {
+		dev_err(&client->dev, "input init failed\n");
+		goto err_power_off;
+	}
+#endif
+
+	err = create_sysfs_interfaces(&client->dev);
+	if (err < 0) {
+		dev_err(&client->dev,
+			"device LPS001WP_PRS_DEV_NAME sysfs register failed\n");
+		goto err_input_cleanup;
+	}
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	prs->early_suspend.level =
+			EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	prs->early_suspend.suspend = lps001wp_prs_early_suspend;
+	prs->early_suspend.resume = lps001wp_prs_late_resume;
+	register_early_suspend(&prs->early_suspend);
+#endif
+
+
+	lps001wp_prs_device_power_off(prs);
+
+	if (prs->regulator)
+		regulator_disable(prs->regulator);
+
+	/* As default, do not report information */
+	atomic_set(&prs->enabled, 0);
+
+
+	mutex_unlock(&prs->lock);
+
+	dev_info(&client->dev, "%s: probed\n", LPS001WP_PRS_DEV_NAME);
+
+	return 0;
+
+/*
+remove_sysfs_int:
+	remove_sysfs_interfaces(&client->dev);
+*/
+err_input_cleanup:
+#ifdef CONFIG_LPS001WP_INPUT_DEVICE
+	lps001wp_prs_input_cleanup(prs);
+#endif
+err_power_off:
+	lps001wp_prs_device_power_off(prs);
+err2:
+	if (prs->pdata->exit)
+		prs->pdata->exit();
+exit_kfree_pdata:
+	kfree(prs->pdata);
+
+err_mutexunlockfreedata:
+	mutex_unlock(&prs->lock);
+	if (prs->regulator) {
+		regulator_disable(prs->regulator);
+		regulator_put(prs->regulator);
+	}
+	kfree(prs);
+exit_alloc_data_failed:
+exit_check_functionality_failed:
+	dev_err(&client->dev, "%s: Driver Init failed\n",
+					LPS001WP_PRS_DEV_NAME);
+	return err;
+}
+
+static int __devexit lps001wp_prs_remove(struct i2c_client *client)
+{
+	struct lps001wp_prs_data *prs = i2c_get_clientdata(client);
+
+#ifdef CONFIG_LPS001WP_INPUT_DEVICE
+	lps001wp_prs_input_cleanup(prs);
+#endif
+	lps001wp_prs_device_power_off(prs);
+	remove_sysfs_interfaces(&client->dev);
+	if (prs->regulator) {
+		/* Disable the regulator if device is enabled. */
+		if (atomic_read(&prs->enabled))
+			regulator_disable(prs->regulator);
+		regulator_put(prs->regulator);
+	}
+
+	if (prs->pdata->exit)
+		prs->pdata->exit();
+
+	if (prs->regulator)
+		regulator_put(prs->regulator);
+
+	kfree(prs->pdata);
+	kfree(prs);
+
+	return 0;
+}
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+static int lps001wp_prs_resume(struct device *dev)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+
+	if (prs->on_before_suspend)
+		return lps001wp_prs_enable(prs);
+	return 0;
+}
+
+static int lps001wp_prs_suspend(struct device *dev)
+{
+	struct lps001wp_prs_data *prs = dev_get_drvdata(dev);
+	prs->on_before_suspend = atomic_read(&prs->enabled);
+	return lps001wp_prs_disable(prs);
+}
+
+static const struct dev_pm_ops lps001wp_prs_dev_pm_ops = {
+	.suspend = lps001wp_prs_suspend,
+	.resume  = lps001wp_prs_resume,
+};
+#else
+static void lps001wp_prs_early_suspend(struct early_suspend *data)
+{
+	struct lps001wp_prs_data *prs =
+		container_of(data, struct lps001wp_prs_data, early_suspend);
+	prs->on_before_suspend = atomic_read(&prs->enabled);
+	lps001wp_prs_disable(prs);
+}
+
+static void lps001wp_prs_late_resume(struct early_suspend *data)
+{
+	struct lps001wp_prs_data *prs =
+		container_of(data, struct lps001wp_prs_data, early_suspend);
+	if (prs->on_before_suspend)
+		lps001wp_prs_enable(prs);
+}
+#endif
+static const struct i2c_device_id lps001wp_prs_id[]
+		= { { LPS001WP_PRS_DEV_NAME, 0}, { },};
+
+MODULE_DEVICE_TABLE(i2c, lps001wp_prs_id);
+
+static struct i2c_driver lps001wp_prs_driver = {
+	.driver = {
+			.name = LPS001WP_PRS_DEV_NAME,
+			.owner = THIS_MODULE,
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
+			.pm = &lps001wp_prs_dev_pm_ops,
+#endif
+	},
+	.probe = lps001wp_prs_probe,
+	.remove = __devexit_p(lps001wp_prs_remove),
+	.id_table = lps001wp_prs_id,
+};
+
+static int __init lps001wp_prs_init(void)
+{
+	printk(KERN_DEBUG "%s barometer driver: init\n",
+						LPS001WP_PRS_DEV_NAME);
+	return i2c_add_driver(&lps001wp_prs_driver);
+}
+
+static void __exit lps001wp_prs_exit(void)
+{
+	#if DEBUG
+	printk(KERN_DEBUG "%s barometer driver exit\n",
+						LPS001WP_PRS_DEV_NAME);
+	#endif
+	i2c_del_driver(&lps001wp_prs_driver);
+	return;
+}
+
+module_init(lps001wp_prs_init);
+module_exit(lps001wp_prs_exit);
+
+MODULE_DESCRIPTION("STMicrolelectronics lps001wp pressure sensor sysfs driver");
+MODULE_AUTHOR("Matteo Dameno, Carmine Iascone, STMicroelectronics");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/input/misc/ste_ff_vibra.c b/drivers/input/misc/ste_ff_vibra.c
new file mode 100644
index 00000000000..9038e6be046
--- /dev/null
+++ b/drivers/input/misc/ste_ff_vibra.c
@@ -0,0 +1,234 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com>
+ *	   for ST-Ericsson
+ * License Terms: GNU General Public License v2
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <mach/ste_audio_io_vibrator.h>
+
+#define FF_VIBRA_DOWN		0x0000 /* 0 degrees */
+#define FF_VIBRA_LEFT		0x4000 /* 90 degrees */
+#define FF_VIBRA_UP		0x8000 /* 180 degrees */
+#define FF_VIBRA_RIGHT		0xC000 /* 270 degrees */
+
+/**
+ * struct vibra_info - Vibrator information structure
+ * @idev:		Pointer to input device structure
+ * @vibra_workqueue:	Pointer to vibrator workqueue structure
+ * @vibra_work:		Vibrator work
+ * @direction:		Vibration direction
+ * @speed:		Vibration speed
+ *
+ * Structure vibra_info holds vibrator informations
+ **/
+struct vibra_info {
+	struct input_dev	*idev;
+	struct workqueue_struct *vibra_workqueue;
+	struct work_struct	vibra_work;
+	int			direction;
+	unsigned char		speed;
+};
+
+/**
+ * vibra_play_work() - Vibrator work, sets speed and direction
+ * @work:	Pointer to work structure
+ *
+ * This function is called from workqueue, turns on/off vibrator
+ **/
+static void vibra_play_work(struct work_struct *work)
+{
+	struct vibra_info *vinfo = container_of(work,
+				struct vibra_info, vibra_work);
+	struct ste_vibra_speed left_speed = {
+		.positive = 0,
+		.negative = 0,
+	};
+	struct ste_vibra_speed right_speed = {
+		.positive = 0,
+		.negative = 0,
+	};
+
+	/* Divide by 2 because supported range by PWM is 0-100 */
+	vinfo->speed /= 2;
+
+	if ((vinfo->direction > FF_VIBRA_DOWN) &&
+				(vinfo->direction < FF_VIBRA_UP)) {
+		/* 1 - 179 degrees, turn on left vibrator */
+		left_speed.positive = vinfo->speed;
+	} else if (vinfo->direction > FF_VIBRA_UP) {
+		/* more than 180 degrees, turn on right vibrator */
+		right_speed.positive = vinfo->speed;
+	} else {
+		/* 0 (down) or 180 (up) degrees, turn on 2 vibrators */
+		left_speed.positive = vinfo->speed;
+		right_speed.positive = vinfo->speed;
+	}
+
+	ste_audioio_vibrator_pwm_control(STE_AUDIOIO_CLIENT_FF_VIBRA,
+					 left_speed, right_speed);
+}
+
+/**
+ * vibra_play() - Memless device control function
+ * @idev:	Pointer to input device structure
+ * @data:	Pointer to private data (not used)
+ * @effect:	Pointer to force feedback effect structure
+ *
+ * This function controls memless device
+ *
+ * Returns:
+ *	0 - success
+ **/
+static int vibra_play(struct input_dev *idev, void *data,
+			struct ff_effect *effect)
+{
+	struct vibra_info *vinfo = input_get_drvdata(idev);
+
+	vinfo->direction = effect->direction;
+	vinfo->speed = effect->u.rumble.strong_magnitude >> 8;
+	if (!vinfo->speed)
+		/* Shift weak magnitude to make it feelable on vibrator */
+		vinfo->speed = effect->u.rumble.weak_magnitude >> 9;
+
+	queue_work(vinfo->vibra_workqueue, &vinfo->vibra_work);
+
+	return 0;
+}
+
+/**
+ * ste_ff_vibra_open() - Input device open function
+ * @idev:      Pointer to input device structure
+ *
+ * This function is called on opening input device
+ *
+ * Returns:
+ *	-ENOMEM - no memory left
+ *	0	- success
+ **/
+static int ste_ff_vibra_open(struct input_dev *idev)
+{
+	struct vibra_info *vinfo = input_get_drvdata(idev);
+
+	vinfo->vibra_workqueue =
+			create_singlethread_workqueue("ste_ff-ff-vibra");
+	if (!vinfo->vibra_workqueue) {
+		dev_err(&idev->dev, "couldn't create vibra workqueue\n");
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+/**
+ * ste_ff_vibra_close() - Input device close function
+ * @idev:      Pointer to input device structure
+ *
+ * This function is called on closing input device
+ **/
+static void ste_ff_vibra_close(struct input_dev *idev)
+{
+	struct vibra_info *vinfo = input_get_drvdata(idev);
+
+	cancel_work_sync(&vinfo->vibra_work);
+	INIT_WORK(&vinfo->vibra_work, vibra_play_work);
+	destroy_workqueue(vinfo->vibra_workqueue);
+	vinfo->vibra_workqueue = NULL;
+}
+
+static int __devinit ste_ff_vibra_probe(struct platform_device *pdev)
+{
+	struct vibra_info *vinfo;
+	int ret;
+
+	vinfo = kmalloc(sizeof *vinfo, GFP_KERNEL);
+	if (!vinfo) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	vinfo->idev = input_allocate_device();
+	if (!vinfo->idev) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		ret = -ENOMEM;
+		goto exit_vinfo_free;
+	}
+
+	vinfo->idev->name = "ste-ff-vibra";
+	vinfo->idev->dev.parent = pdev->dev.parent;
+	vinfo->idev->open = ste_ff_vibra_open;
+	vinfo->idev->close = ste_ff_vibra_close;
+	INIT_WORK(&vinfo->vibra_work, vibra_play_work);
+	__set_bit(FF_RUMBLE, vinfo->idev->ffbit);
+
+	ret = input_ff_create_memless(vinfo->idev, NULL, vibra_play);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to create memless device\n");
+		goto exit_idev_free;
+	}
+
+	ret = input_register_device(vinfo->idev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register input device\n");
+		goto exit_destroy_memless;
+	}
+
+	input_set_drvdata(vinfo->idev, vinfo);
+	platform_set_drvdata(pdev, vinfo);
+	return 0;
+
+exit_destroy_memless:
+	input_ff_destroy(vinfo->idev);
+exit_idev_free:
+	input_free_device(vinfo->idev);
+exit_vinfo_free:
+	kfree(vinfo);
+	return ret;
+}
+
+static int __devexit ste_ff_vibra_remove(struct platform_device *pdev)
+{
+	struct vibra_info *vinfo = platform_get_drvdata(pdev);
+
+	/*
+	 * Function device_release() will call input_dev_release()
+	 * which will free ff and input device. No need to call
+	 * input_ff_destroy() and input_free_device() explicitly.
+	 */
+	input_unregister_device(vinfo->idev);
+	kfree(vinfo);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver ste_ff_vibra_driver = {
+	.driver = {
+		.name = "ste_ff_vibra",
+		.owner = THIS_MODULE,
+	},
+	.probe	= ste_ff_vibra_probe,
+	.remove	= __devexit_p(ste_ff_vibra_remove)
+};
+
+static int __init ste_ff_vibra_init(void)
+{
+	return platform_driver_register(&ste_ff_vibra_driver);
+}
+module_init(ste_ff_vibra_init);
+
+static void __exit ste_ff_vibra_exit(void)
+{
+	platform_driver_unregister(&ste_ff_vibra_driver);
+}
+module_exit(ste_ff_vibra_exit);
+
+MODULE_AUTHOR("Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com>");
+MODULE_DESCRIPTION("STE Force Feedback Vibrator Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/input/touchscreen/bu21013_ts.c b/drivers/input/touchscreen/bu21013_ts.c
index f2d03c06c2d..d6ae75865e4 100644
--- a/drivers/input/touchscreen/bu21013_ts.c
+++ b/drivers/input/touchscreen/bu21013_ts.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) ST-Ericsson SA 2010
+ * Copyright (C) ST-Ericsson SA 2009
  * Author: Naveen Kumar G <naveen.gaddipati@stericsson.com> for ST-Ericsson
  * License terms:GNU General Public License (GPL) version 2
  */
@@ -12,13 +12,14 @@
 #include <linux/input.h>
 #include <linux/input/bu21013.h>
 #include <linux/slab.h>
+#include <linux/clk.h>
 #include <linux/regulator/consumer.h>
 #include <linux/module.h>
 
 #define PEN_DOWN_INTR	0
-#define MAX_FINGERS	2
 #define RESET_DELAY	30
-#define PENUP_TIMEOUT	(10)
+#define PENUP_TIMEOUT	2 /* 2msecs */
+#define SCALE_FACTOR	1000
 #define DELTA_MIN	16
 #define MASK_BITS	0x03
 #define SHIFT_8		8
@@ -131,7 +132,7 @@
 #define BU21013_NUMBER_OF_X_SENSORS	(6)
 #define BU21013_NUMBER_OF_Y_SENSORS	(11)
 
-#define DRIVER_TP	"bu21013_tp"
+#define DRIVER_TP	"bu21013_ts"
 
 /**
  * struct bu21013_ts_data - touch panel data structure
@@ -142,6 +143,12 @@
  * @in_dev: pointer to the input device structure
  * @intr_pin: interrupt pin value
  * @regulator: pointer to the Regulator used for touch screen
+ * @enable: variable to indicate the enable/disable of touch screen
+ * @ext_clk_enable: true if running on ext clk
+ * @ext_clk_state: Saved state for suspend/resume of ext clk
+ * @factor_x: x scale factor
+ * @factor_y: y scale factor
+ * @tpclk: pointer to clock structure
  *
  * Touch panel device data structure
  */
@@ -149,12 +156,226 @@ struct bu21013_ts_data {
 	struct i2c_client *client;
 	wait_queue_head_t wait;
 	bool touch_stopped;
-	const struct bu21013_platform_device *chip;
+	struct bu21013_platform_device *chip;
 	struct input_dev *in_dev;
 	unsigned int intr_pin;
 	struct regulator *regulator;
+	bool enable;
+	bool ext_clk_enable;
+	bool ext_clk_state;
+	unsigned int factor_x;
+	unsigned int factor_y;
+	struct clk *tpclk;
 };
 
+static int bu21013_init_chip(struct bu21013_ts_data *data, bool on_ext_clk);
+
+/**
+ * bu21013_ext_clk() - enable/disable the external clock
+ * @pdata: touch screen data
+ * @enable: enable external clock
+ * @reconfig: reconfigure chip upon external clock off.
+ *
+ * This function used to enable or disable the external clock and possible
+ * reconfigure hw.
+ */
+static int bu21013_ext_clk(struct bu21013_ts_data *pdata, bool enable,
+			   bool reconfig)
+{
+	int retval = 0;
+
+	if (!pdata->tpclk || pdata->ext_clk_enable == enable)
+		return retval;
+
+	if (enable) {
+		pdata->ext_clk_enable = true;
+		clk_enable(pdata->tpclk);
+		retval = bu21013_init_chip(pdata, true);
+	} else {
+		pdata->ext_clk_enable = false;
+		if (reconfig)
+			retval = bu21013_init_chip(pdata, false);
+		clk_disable(pdata->tpclk);
+	}
+	return retval;
+}
+
+/**
+ * bu21013_enable() - enable the touch driver event
+ * @pdata: touch screen data
+ *
+ * This function used to enable the driver and returns integer
+ */
+static int bu21013_enable(struct bu21013_ts_data *pdata)
+{
+	int retval;
+
+	if (pdata->regulator)
+		regulator_enable(pdata->regulator);
+
+	if (pdata->chip->cs_en) {
+		retval = pdata->chip->cs_en(pdata->chip->cs_pin);
+		if (retval < 0) {
+			dev_err(&pdata->client->dev, "enable hw failed\n");
+			return retval;
+		}
+	}
+
+	if (pdata->ext_clk_state)
+		retval = bu21013_ext_clk(pdata, true, true);
+	else
+		retval = bu21013_init_chip(pdata, false);
+
+	if (retval < 0) {
+		dev_err(&pdata->client->dev, "enable hw failed\n");
+		return retval;
+	}
+	pdata->touch_stopped = false;
+	enable_irq(pdata->chip->irq);
+
+	return 0;
+}
+
+/**
+ * bu21013_disable() - disable the touch driver event
+ * @pdata: touch screen data
+ *
+ * This function used to disable the driver and returns integer
+ */
+static void bu21013_disable(struct bu21013_ts_data *pdata)
+{
+	pdata->touch_stopped = true;
+
+	pdata->ext_clk_state = pdata->ext_clk_enable;
+	(void) bu21013_ext_clk(pdata, false, false);
+
+	disable_irq(pdata->chip->irq);
+	if (pdata->chip->cs_dis)
+		pdata->chip->cs_dis(pdata->chip->cs_pin);
+	if (pdata->regulator)
+		regulator_disable(pdata->regulator);
+}
+
+/**
+ * bu21013_show_attr_enable() - show the touch screen controller status
+ * @dev: pointer to device structure
+ * @attr: pointer to device attribute
+ * @buf: parameter buffer
+ *
+ * This funtion is used to show whether the touch screen is enabled or
+ * disabled
+ */
+static ssize_t bu21013_show_attr_enable(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct bu21013_ts_data *pdata = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", pdata->enable);
+}
+
+/**
+ * bu21013_store_attr_enable() - Enable/Disable the touchscreen.
+ * @dev: pointer to device structure
+ * @attr: pointer to device attribute
+ * @buf: parameter buffer
+ * @count: number of parameters
+ *
+ * This funtion is used to enable or disable the touch screen controller.
+ */
+static ssize_t bu21013_store_attr_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int ret = 0;
+	unsigned long val;
+
+	struct bu21013_ts_data *pdata = dev_get_drvdata(dev);
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if ((val != 0) && (val != 1))
+		return -EINVAL;
+
+	if (pdata->enable != val) {
+		pdata->enable = val ? true : false;
+		if (pdata->enable) {
+			ret = bu21013_enable(pdata);
+			if (ret < 0)
+				return ret;
+		} else
+			bu21013_disable(pdata);
+	}
+	return count;
+}
+
+/**
+ * bu21013_show_attr_extclk() - shows the external clock status
+ * @dev: pointer to device structure
+ * @attr: pointer to device attribute
+ * @buf: parameter buffer
+ *
+ * This funtion is used to show whether the external clock for the touch
+ * screen is enabled or disabled.
+ */
+static ssize_t bu21013_show_attr_extclk(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct bu21013_ts_data *pdata = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", pdata->ext_clk_enable);
+}
+
+/**
+ * bu21013_store_attr_extclk() - Enable/Disable the external clock
+ * for the tocuh screen controller.
+ * @dev: pointer to device structure
+ * @attr: pointer to device attribute
+ * @buf: parameter buffer
+ * @count: number of parameters
+ *
+ * This funtion is used enabled or disable the external clock for the touch
+ * screen controller.
+ */
+static ssize_t bu21013_store_attr_extclk(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int retval = 0;
+	struct bu21013_ts_data *pdata = dev_get_drvdata(dev);
+	unsigned long val;
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if ((val != 0) && (val != 1))
+		return -EINVAL;
+
+	if (pdata->chip->has_ext_clk) {
+		if (pdata->enable)
+			retval = bu21013_ext_clk(pdata, val, true);
+		else
+			pdata->ext_clk_state = val;
+		if (retval < 0)
+			return retval;
+	}
+	return count;
+}
+
+static DEVICE_ATTR(enable, S_IWUSR | S_IRUGO,
+		bu21013_show_attr_enable, bu21013_store_attr_enable);
+
+static DEVICE_ATTR(ext_clk, S_IWUSR | S_IRUGO,
+		bu21013_show_attr_extclk, bu21013_store_attr_extclk);
+
+
+static struct attribute *bu21013_attribute[] = {
+		&dev_attr_enable.attr,
+		&dev_attr_ext_clk.attr,
+		NULL,
+};
+
+static struct attribute_group bu21013_attr_group = {
+		.attrs = bu21013_attribute,
+};
+
+
 /**
  * bu21013_read_block_data(): read the touch co-ordinates
  * @data: bu21013_ts_data structure pointer
@@ -204,12 +425,14 @@ static int bu21013_do_touch_report(struct bu21013_ts_data *data)
 	if (!has_x_sensors || !has_y_sensors)
 		return 0;
 
-	for (i = 0; i < MAX_FINGERS; i++) {
+	for (i = 0; i < 2; i++) {
 		const u8 *p = &buf[4 * i + 3];
 		unsigned int x = p[0] << SHIFT_2 | (p[1] & MASK_BITS);
 		unsigned int y = p[2] << SHIFT_2 | (p[3] & MASK_BITS);
 		if (x == 0 || y == 0)
 			continue;
+		x = x * data->factor_x / SCALE_FACTOR;
+		y = y * data->factor_y / SCALE_FACTOR;
 		pos_x[finger_down_count] = x;
 		pos_y[finger_down_count] = y;
 		finger_down_count++;
@@ -217,21 +440,21 @@ static int bu21013_do_touch_report(struct bu21013_ts_data *data)
 
 	if (finger_down_count) {
 		if (finger_down_count == 2 &&
-		    (abs(pos_x[0] - pos_x[1]) < DELTA_MIN ||
-		     abs(pos_y[0] - pos_y[1]) < DELTA_MIN)) {
+				(abs(pos_x[0] - pos_x[1]) < DELTA_MIN ||
+				abs(pos_y[0] - pos_y[1]) < DELTA_MIN))
 			return 0;
-		}
 
 		for (i = 0; i < finger_down_count; i++) {
-			if (data->chip->x_flip)
-				pos_x[i] = data->chip->touch_x_max - pos_x[i];
-			if (data->chip->y_flip)
-				pos_y[i] = data->chip->touch_y_max - pos_y[i];
-
-			input_report_abs(data->in_dev,
-					 ABS_MT_POSITION_X, pos_x[i]);
-			input_report_abs(data->in_dev,
-					 ABS_MT_POSITION_Y, pos_y[i]);
+			if (data->chip->portrait && data->chip->x_flip)
+				pos_x[i] = data->chip->x_max_res - pos_x[i];
+			if (data->chip->portrait && data->chip->y_flip)
+				pos_y[i] = data->chip->y_max_res - pos_y[i];
+			input_report_abs(data->in_dev, ABS_MT_TOUCH_MAJOR,
+						max(pos_x[i], pos_y[i]));
+			input_report_abs(data->in_dev, ABS_MT_POSITION_X,
+								pos_x[i]);
+			input_report_abs(data->in_dev, ABS_MT_POSITION_Y,
+								pos_y[i]);
 			input_mt_sync(data->in_dev);
 		}
 	} else
@@ -261,24 +484,23 @@ static irqreturn_t bu21013_gpio_irq(int irq, void *device_data)
 			dev_err(&i2c->dev, "bu21013_do_touch_report failed\n");
 			return IRQ_NONE;
 		}
-
 		data->intr_pin = data->chip->irq_read_val();
 		if (data->intr_pin == PEN_DOWN_INTR)
 			wait_event_timeout(data->wait, data->touch_stopped,
-					   msecs_to_jiffies(2));
+					msecs_to_jiffies(PENUP_TIMEOUT));
 	} while (!data->intr_pin && !data->touch_stopped);
-
 	return IRQ_HANDLED;
 }
 
 /**
  * bu21013_init_chip() - power on sequence for the bu21013 controller
  * @data: device structure pointer
+ * @on_ext_clk: Run on external clock
  *
  * This function is used to power on
  * the bu21013 controller and returns integer.
  */
-static int bu21013_init_chip(struct bu21013_ts_data *data)
+static int bu21013_init_chip(struct bu21013_ts_data *data, bool on_ext_clk)
 {
 	int retval;
 	struct i2c_client *i2c = data->client;
@@ -297,28 +519,24 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_SENSOR_0_7 reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_SENSOR_8_15_REG,
 						BU21013_SENSORS_EN_8_15);
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_SENSOR_8_15 reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_SENSOR_16_23_REG,
 						BU21013_SENSORS_EN_16_23);
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_SENSOR_16_23 reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_POS_MODE1_REG,
 				(BU21013_POS_MODE1_0 | BU21013_POS_MODE1_1));
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_POS_MODE1 reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_POS_MODE2_REG,
 			(BU21013_POS_MODE2_ZERO | BU21013_POS_MODE2_AVG1 |
 			BU21013_POS_MODE2_AVG2 | BU21013_POS_MODE2_EN_RAW |
@@ -327,8 +545,7 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_POS_MODE2 reg write failed\n");
 		return retval;
 	}
-
-	if (data->chip->ext_clk)
+	if (on_ext_clk)
 		retval = i2c_smbus_write_byte_data(i2c, BU21013_CLK_MODE_REG,
 			(BU21013_CLK_MODE_EXT | BU21013_CLK_MODE_CALIB));
 	else
@@ -338,21 +555,18 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_CLK_MODE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_IDLE_REG,
 				(BU21013_IDLET_0 | BU21013_IDLE_INTERMIT_EN));
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_IDLE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_INT_MODE_REG,
 						BU21013_INT_MODE_LEVEL);
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_INT_MODE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_FILTER_REG,
 						(BU21013_DELTA_0_6 |
 							BU21013_FILTER_EN));
@@ -367,14 +581,12 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_TH_ON reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_TH_OFF_REG,
 				BU21013_TH_OFF_4 | BU21013_TH_OFF_3);
 	if (retval < 0) {
 		dev_err(&i2c->dev, "BU21013_TH_OFF reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_GAIN_REG,
 					(BU21013_GAIN_0 | BU21013_GAIN_1));
 	if (retval < 0) {
@@ -388,7 +600,6 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_OFFSET_MODE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_XY_EDGE_REG,
 				(BU21013_X_EDGE_0 | BU21013_X_EDGE_2 |
 				BU21013_Y_EDGE_1 | BU21013_Y_EDGE_3));
@@ -396,7 +607,6 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		dev_err(&i2c->dev, "BU21013_XY_EDGE reg write failed\n");
 		return retval;
 	}
-
 	retval = i2c_smbus_write_byte_data(i2c, BU21013_DONE_REG,
 							BU21013_DONE);
 	if (retval < 0) {
@@ -404,25 +614,15 @@ static int bu21013_init_chip(struct bu21013_ts_data *data)
 		return retval;
 	}
 
-	return 0;
+	data->factor_x = (data->chip->x_max_res * SCALE_FACTOR /
+					data->chip->touch_x_max);
+	data->factor_y = (data->chip->y_max_res * SCALE_FACTOR /
+					data->chip->touch_y_max);
+	return retval;
 }
 
 /**
- * bu21013_free_irq() - frees IRQ registered for touchscreen
- * @bu21013_data: device structure pointer
- *
- * This function signals interrupt thread to stop processing and
- * frees interrupt.
- */
-static void bu21013_free_irq(struct bu21013_ts_data *bu21013_data)
-{
-	bu21013_data->touch_stopped = true;
-	wake_up(&bu21013_data->wait);
-	free_irq(bu21013_data->chip->irq, bu21013_data);
-}
-
-/**
- * bu21013_probe() - initializes the i2c-client touchscreen driver
+ * bu21013_probe() - initialzes the i2c-client touchscreen driver
  * @client: i2c client structure pointer
  * @id: i2c device id pointer
  *
@@ -432,11 +632,11 @@ static void bu21013_free_irq(struct bu21013_ts_data *bu21013_data)
 static int __devinit bu21013_probe(struct i2c_client *client,
 					const struct i2c_device_id *id)
 {
+	int retval;
 	struct bu21013_ts_data *bu21013_data;
 	struct input_dev *in_dev;
-	const struct bu21013_platform_device *pdata =
+	struct bu21013_platform_device *pdata =
 					client->dev.platform_data;
-	int error;
 
 	if (!i2c_check_functionality(client->adapter,
 					I2C_FUNC_SMBUS_BYTE_DATA)) {
@@ -446,53 +646,72 @@ static int __devinit bu21013_probe(struct i2c_client *client,
 
 	if (!pdata) {
 		dev_err(&client->dev, "platform data not defined\n");
-		return -EINVAL;
+		retval = -EINVAL;
+		return retval;
 	}
 
 	bu21013_data = kzalloc(sizeof(struct bu21013_ts_data), GFP_KERNEL);
-	in_dev = input_allocate_device();
-	if (!bu21013_data || !in_dev) {
+	if (!bu21013_data) {
 		dev_err(&client->dev, "device memory alloc failed\n");
-		error = -ENOMEM;
-		goto err_free_mem;
+		retval = -ENOMEM;
+		return retval;
+	}
+	/* allocate input device */
+	in_dev = input_allocate_device();
+	if (!in_dev) {
+		dev_err(&client->dev, "input device memory alloc failed\n");
+		retval = -ENOMEM;
+		goto err_alloc;
 	}
 
 	bu21013_data->in_dev = in_dev;
 	bu21013_data->chip = pdata;
 	bu21013_data->client = client;
 
-	bu21013_data->regulator = regulator_get(&client->dev, "V-TOUCH");
+	bu21013_data->regulator = regulator_get(&client->dev, "avdd");
 	if (IS_ERR(bu21013_data->regulator)) {
-		dev_err(&client->dev, "regulator_get failed\n");
-		error = PTR_ERR(bu21013_data->regulator);
-		goto err_free_mem;
+		dev_warn(&client->dev, "regulator_get failed\n");
+		bu21013_data->regulator = NULL;
 	}
-
-	error = regulator_enable(bu21013_data->regulator);
-	if (error < 0) {
-		dev_err(&client->dev, "regulator enable failed\n");
-		goto err_put_regulator;
-	}
-
-	bu21013_data->touch_stopped = false;
-	init_waitqueue_head(&bu21013_data->wait);
+	if (bu21013_data->regulator)
+		regulator_enable(bu21013_data->regulator);
 
 	/* configure the gpio pins */
 	if (pdata->cs_en) {
-		error = pdata->cs_en(pdata->cs_pin);
-		if (error < 0) {
+		retval = pdata->cs_en(pdata->cs_pin);
+		if (retval < 0) {
 			dev_err(&client->dev, "chip init failed\n");
-			goto err_disable_regulator;
+			goto err_init_cs;
+		}
+	}
+
+	if (pdata->has_ext_clk) {
+		bu21013_data->tpclk = clk_get(&client->dev, NULL);
+		if (IS_ERR(bu21013_data->tpclk)) {
+			dev_warn(&client->dev, "get extern clock failed\n");
+			bu21013_data->tpclk = NULL;
+		}
+	}
+
+	if (pdata->enable_ext_clk && bu21013_data->tpclk) {
+		retval = clk_enable(bu21013_data->tpclk);
+		if (retval < 0) {
+			dev_err(&client->dev, "clock enable failed\n");
+			goto err_ext_clk;
 		}
+		bu21013_data->ext_clk_enable = true;
 	}
 
 	/* configure the touch panel controller */
-	error = bu21013_init_chip(bu21013_data);
-	if (error) {
+	retval = bu21013_init_chip(bu21013_data, bu21013_data->ext_clk_enable);
+	if (retval < 0) {
 		dev_err(&client->dev, "error in bu21013 config\n");
-		goto err_cs_disable;
+		goto err_init_config;
 	}
 
+	init_waitqueue_head(&bu21013_data->wait);
+	bu21013_data->touch_stopped = false;
+
 	/* register the device to input subsystem */
 	in_dev->name = DRIVER_TP;
 	in_dev->id.bustype = BUS_I2C;
@@ -503,44 +722,63 @@ static int __devinit bu21013_probe(struct i2c_client *client,
 	__set_bit(EV_ABS, in_dev->evbit);
 
 	input_set_abs_params(in_dev, ABS_MT_POSITION_X, 0,
-						pdata->touch_x_max, 0, 0);
+						pdata->x_max_res, 0, 0);
 	input_set_abs_params(in_dev, ABS_MT_POSITION_Y, 0,
-						pdata->touch_y_max, 0, 0);
+						pdata->y_max_res, 0, 0);
+	input_set_abs_params(in_dev, ABS_MT_TOUCH_MAJOR, 0,
+			max(pdata->x_max_res , pdata->y_max_res), 0, 0);
 	input_set_drvdata(in_dev, bu21013_data);
-
-	error = request_threaded_irq(pdata->irq, NULL, bu21013_gpio_irq,
-				     IRQF_TRIGGER_FALLING | IRQF_SHARED,
-				     DRIVER_TP, bu21013_data);
-	if (error) {
+	retval = input_register_device(in_dev);
+	if (retval)
+		goto err_input_register;
+
+	retval = request_threaded_irq(pdata->irq, NULL, bu21013_gpio_irq,
+					(IRQF_TRIGGER_FALLING | IRQF_SHARED),
+					DRIVER_TP, bu21013_data);
+	if (retval) {
 		dev_err(&client->dev, "request irq %d failed\n", pdata->irq);
-		goto err_cs_disable;
+		goto err_init_irq;
 	}
+	bu21013_data->enable = true;
+	i2c_set_clientdata(client, bu21013_data);
 
-	error = input_register_device(in_dev);
-	if (error) {
-		dev_err(&client->dev, "failed to register input device\n");
-		goto err_free_irq;
+	/* sysfs implementation for dynamic enable/disable the input event */
+	retval = sysfs_create_group(&client->dev.kobj,	&bu21013_attr_group);
+	if (retval) {
+		dev_err(&client->dev, "failed to create sysfs entries\n");
+		goto err_sysfs_create;
 	}
 
-	device_init_wakeup(&client->dev, pdata->wakeup);
-	i2c_set_clientdata(client, bu21013_data);
-
-	return 0;
+	return retval;
 
-err_free_irq:
-	bu21013_free_irq(bu21013_data);
-err_cs_disable:
-	pdata->cs_dis(pdata->cs_pin);
-err_disable_regulator:
-	regulator_disable(bu21013_data->regulator);
-err_put_regulator:
-	regulator_put(bu21013_data->regulator);
-err_free_mem:
-	input_free_device(in_dev);
+err_sysfs_create:
+	free_irq(pdata->irq, bu21013_data);
+	i2c_set_clientdata(client, NULL);
+err_init_irq:
+	input_unregister_device(bu21013_data->in_dev);
+err_input_register:
+	wake_up(&bu21013_data->wait);
+err_init_config:
+	if (bu21013_data->tpclk) {
+		if (bu21013_data->ext_clk_enable)
+			clk_disable(bu21013_data->tpclk);
+		clk_put(bu21013_data->tpclk);
+	}
+err_ext_clk:
+	if (pdata->cs_dis)
+		pdata->cs_dis(pdata->cs_pin);
+err_init_cs:
+	if (bu21013_data->regulator) {
+		regulator_disable(bu21013_data->regulator);
+		regulator_put(bu21013_data->regulator);
+	}
+	input_free_device(bu21013_data->in_dev);
+err_alloc:
 	kfree(bu21013_data);
 
-	return error;
+	return retval;
 }
+
 /**
  * bu21013_remove() - removes the i2c-client touchscreen driver
  * @client: i2c client structure pointer
@@ -552,19 +790,24 @@ static int __devexit bu21013_remove(struct i2c_client *client)
 {
 	struct bu21013_ts_data *bu21013_data = i2c_get_clientdata(client);
 
-	bu21013_free_irq(bu21013_data);
-
+	bu21013_data->touch_stopped = true;
+	sysfs_remove_group(&client->dev.kobj, &bu21013_attr_group);
+	wake_up(&bu21013_data->wait);
+	free_irq(bu21013_data->chip->irq, bu21013_data);
 	bu21013_data->chip->cs_dis(bu21013_data->chip->cs_pin);
-
 	input_unregister_device(bu21013_data->in_dev);
 
-	regulator_disable(bu21013_data->regulator);
-	regulator_put(bu21013_data->regulator);
-
+	if (bu21013_data->tpclk) {
+		if (bu21013_data->ext_clk_enable)
+			clk_disable(bu21013_data->tpclk);
+		clk_put(bu21013_data->tpclk);
+	}
+	if (bu21013_data->regulator) {
+		regulator_disable(bu21013_data->regulator);
+		regulator_put(bu21013_data->regulator);
+	}
 	kfree(bu21013_data);
 
-	device_init_wakeup(&client->dev, false);
-
 	return 0;
 }
 
@@ -579,15 +822,8 @@ static int __devexit bu21013_remove(struct i2c_client *client)
 static int bu21013_suspend(struct device *dev)
 {
 	struct bu21013_ts_data *bu21013_data = dev_get_drvdata(dev);
-	struct i2c_client *client = bu21013_data->client;
 
-	bu21013_data->touch_stopped = true;
-	if (device_may_wakeup(&client->dev))
-		enable_irq_wake(bu21013_data->chip->irq);
-	else
-		disable_irq(bu21013_data->chip->irq);
-
-	regulator_disable(bu21013_data->regulator);
+	bu21013_disable(bu21013_data);
 
 	return 0;
 }
@@ -602,29 +838,8 @@ static int bu21013_suspend(struct device *dev)
 static int bu21013_resume(struct device *dev)
 {
 	struct bu21013_ts_data *bu21013_data = dev_get_drvdata(dev);
-	struct i2c_client *client = bu21013_data->client;
-	int retval;
 
-	retval = regulator_enable(bu21013_data->regulator);
-	if (retval < 0) {
-		dev_err(&client->dev, "bu21013 regulator enable failed\n");
-		return retval;
-	}
-
-	retval = bu21013_init_chip(bu21013_data);
-	if (retval < 0) {
-		dev_err(&client->dev, "bu21013 controller config failed\n");
-		return retval;
-	}
-
-	bu21013_data->touch_stopped = false;
-
-	if (device_may_wakeup(&client->dev))
-		disable_irq_wake(bu21013_data->chip->irq);
-	else
-		enable_irq(bu21013_data->chip->irq);
-
-	return 0;
+	return bu21013_enable(bu21013_data);
 }
 
 static const struct dev_pm_ops bu21013_dev_pm_ops = {
diff --git a/drivers/input/touchscreen/synaptics_i2c_rmi.c b/drivers/input/touchscreen/synaptics_i2c_rmi.c
new file mode 100644
index 00000000000..5729602cbb6
--- /dev/null
+++ b/drivers/input/touchscreen/synaptics_i2c_rmi.c
@@ -0,0 +1,675 @@
+/* drivers/input/keyboard/synaptics_i2c_rmi.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/earlysuspend.h>
+#include <linux/hrtimer.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/synaptics_i2c_rmi.h>
+
+static struct workqueue_struct *synaptics_wq;
+
+struct synaptics_ts_data {
+	uint16_t addr;
+	struct i2c_client *client;
+	struct input_dev *input_dev;
+	int use_irq;
+	bool has_relative_report;
+	struct hrtimer timer;
+	struct work_struct  work;
+	uint16_t max[2];
+	int snap_state[2][2];
+	int snap_down_on[2];
+	int snap_down_off[2];
+	int snap_up_on[2];
+	int snap_up_off[2];
+	int snap_down[2];
+	int snap_up[2];
+	uint32_t flags;
+	int reported_finger_count;
+	int8_t sensitivity_adjust;
+	int (*power)(int on);
+	struct early_suspend early_suspend;
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void synaptics_ts_early_suspend(struct early_suspend *h);
+static void synaptics_ts_late_resume(struct early_suspend *h);
+#endif
+
+static int synaptics_init_panel(struct synaptics_ts_data *ts)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0xff, 0x10); /* page select = 0x10 */
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed for page select\n");
+		goto err_page_select_failed;
+	}
+	ret = i2c_smbus_write_byte_data(ts->client, 0x41, 0x04); /* Set "No Clip Z" */
+	if (ret < 0)
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed for No Clip Z\n");
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0x44,
+					ts->sensitivity_adjust);
+	if (ret < 0)
+		pr_err("synaptics_ts: failed to set Sensitivity Adjust\n");
+
+err_page_select_failed:
+	ret = i2c_smbus_write_byte_data(ts->client, 0xff, 0x04); /* page select = 0x04 */
+	if (ret < 0)
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed for page select\n");
+	ret = i2c_smbus_write_byte_data(ts->client, 0xf0, 0x81); /* normal operation, 80 reports per second */
+	if (ret < 0)
+		printk(KERN_ERR "synaptics_ts_resume: i2c_smbus_write_byte_data failed\n");
+	return ret;
+}
+
+static void synaptics_ts_work_func(struct work_struct *work)
+{
+	int i;
+	int ret;
+	int bad_data = 0;
+	struct i2c_msg msg[2];
+	uint8_t start_reg;
+	uint8_t buf[15];
+	struct synaptics_ts_data *ts = container_of(work, struct synaptics_ts_data, work);
+	int buf_len = ts->has_relative_report ? 15 : 13;
+
+	msg[0].addr = ts->client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &start_reg;
+	start_reg = 0x00;
+	msg[1].addr = ts->client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = buf_len;
+	msg[1].buf = buf;
+
+	/* printk("synaptics_ts_work_func\n"); */
+	for (i = 0; i < ((ts->use_irq && !bad_data) ? 1 : 10); i++) {
+		ret = i2c_transfer(ts->client->adapter, msg, 2);
+		if (ret < 0) {
+			printk(KERN_ERR "synaptics_ts_work_func: i2c_transfer failed\n");
+			bad_data = 1;
+		} else {
+			/* printk("synaptics_ts_work_func: %x %x %x %x %x %x" */
+			/*        " %x %x %x %x %x %x %x %x %x, ret %d\n", */
+			/*        buf[0], buf[1], buf[2], buf[3], */
+			/*        buf[4], buf[5], buf[6], buf[7], */
+			/*        buf[8], buf[9], buf[10], buf[11], */
+			/*        buf[12], buf[13], buf[14], ret); */
+			if ((buf[buf_len - 1] & 0xc0) != 0x40) {
+				printk(KERN_WARNING "synaptics_ts_work_func:"
+				       " bad read %x %x %x %x %x %x %x %x %x"
+				       " %x %x %x %x %x %x, ret %d\n",
+				       buf[0], buf[1], buf[2], buf[3],
+				       buf[4], buf[5], buf[6], buf[7],
+				       buf[8], buf[9], buf[10], buf[11],
+				       buf[12], buf[13], buf[14], ret);
+				if (bad_data)
+					synaptics_init_panel(ts);
+				bad_data = 1;
+				continue;
+			}
+			bad_data = 0;
+			if ((buf[buf_len - 1] & 1) == 0) {
+				/* printk("read %d coordinates\n", i); */
+				break;
+			} else {
+				int pos[2][2];
+				int f, a;
+				int base;
+				/* int x = buf[3] | (uint16_t)(buf[2] & 0x1f) << 8; */
+				/* int y = buf[5] | (uint16_t)(buf[4] & 0x1f) << 8; */
+				int z = buf[1];
+				int w = buf[0] >> 4;
+				int finger = buf[0] & 7;
+
+				/* int x2 = buf[3+6] | (uint16_t)(buf[2+6] & 0x1f) << 8; */
+				/* int y2 = buf[5+6] | (uint16_t)(buf[4+6] & 0x1f) << 8; */
+				/* int z2 = buf[1+6]; */
+				/* int w2 = buf[0+6] >> 4; */
+				/* int finger2 = buf[0+6] & 7; */
+
+				/* int dx = (int8_t)buf[12]; */
+				/* int dy = (int8_t)buf[13]; */
+				int finger2_pressed;
+
+				/* printk("x %4d, y %4d, z %3d, w %2d, F %d, 2nd: x %4d, y %4d, z %3d, w %2d, F %d, dx %4d, dy %4d\n", */
+				/*	x, y, z, w, finger, */
+				/*	x2, y2, z2, w2, finger2, */
+				/*	dx, dy); */
+
+				base = 2;
+				for (f = 0; f < 2; f++) {
+					uint32_t flip_flag = SYNAPTICS_FLIP_X;
+					for (a = 0; a < 2; a++) {
+						int p = buf[base + 1];
+						p |= (uint16_t)(buf[base] & 0x1f) << 8;
+						if (ts->flags & flip_flag)
+							p = ts->max[a] - p;
+						if (ts->flags & SYNAPTICS_SNAP_TO_INACTIVE_EDGE) {
+							if (ts->snap_state[f][a]) {
+								if (p <= ts->snap_down_off[a])
+									p = ts->snap_down[a];
+								else if (p >= ts->snap_up_off[a])
+									p = ts->snap_up[a];
+								else
+									ts->snap_state[f][a] = 0;
+							} else {
+								if (p <= ts->snap_down_on[a]) {
+									p = ts->snap_down[a];
+									ts->snap_state[f][a] = 1;
+								} else if (p >= ts->snap_up_on[a]) {
+									p = ts->snap_up[a];
+									ts->snap_state[f][a] = 1;
+								}
+							}
+						}
+						pos[f][a] = p;
+						base += 2;
+						flip_flag <<= 1;
+					}
+					base += 2;
+					if (ts->flags & SYNAPTICS_SWAP_XY)
+						swap(pos[f][0], pos[f][1]);
+				}
+				if (z) {
+					input_report_abs(ts->input_dev, ABS_X, pos[0][0]);
+					input_report_abs(ts->input_dev, ABS_Y, pos[0][1]);
+				}
+				input_report_abs(ts->input_dev, ABS_PRESSURE, z);
+				input_report_abs(ts->input_dev, ABS_TOOL_WIDTH, w);
+				input_report_key(ts->input_dev, BTN_TOUCH, finger);
+				finger2_pressed = finger > 1 && finger != 7;
+				input_report_key(ts->input_dev, BTN_2, finger2_pressed);
+				if (finger2_pressed) {
+					input_report_abs(ts->input_dev, ABS_HAT0X, pos[1][0]);
+					input_report_abs(ts->input_dev, ABS_HAT0Y, pos[1][1]);
+				}
+
+				if (!finger)
+					z = 0;
+				input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, z);
+				input_report_abs(ts->input_dev, ABS_MT_WIDTH_MAJOR, w);
+				input_report_abs(ts->input_dev, ABS_MT_POSITION_X, pos[0][0]);
+				input_report_abs(ts->input_dev, ABS_MT_POSITION_Y, pos[0][1]);
+				input_mt_sync(ts->input_dev);
+				if (finger2_pressed) {
+					input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, z);
+					input_report_abs(ts->input_dev, ABS_MT_WIDTH_MAJOR, w);
+					input_report_abs(ts->input_dev, ABS_MT_POSITION_X, pos[1][0]);
+					input_report_abs(ts->input_dev, ABS_MT_POSITION_Y, pos[1][1]);
+					input_mt_sync(ts->input_dev);
+				} else if (ts->reported_finger_count > 1) {
+					input_report_abs(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0);
+					input_report_abs(ts->input_dev, ABS_MT_WIDTH_MAJOR, 0);
+					input_mt_sync(ts->input_dev);
+				}
+				ts->reported_finger_count = finger;
+				input_sync(ts->input_dev);
+			}
+		}
+	}
+	if (ts->use_irq)
+		enable_irq(ts->client->irq);
+}
+
+static enum hrtimer_restart synaptics_ts_timer_func(struct hrtimer *timer)
+{
+	struct synaptics_ts_data *ts = container_of(timer, struct synaptics_ts_data, timer);
+	/* printk("synaptics_ts_timer_func\n"); */
+
+	queue_work(synaptics_wq, &ts->work);
+
+	hrtimer_start(&ts->timer, ktime_set(0, 12500000), HRTIMER_MODE_REL);
+	return HRTIMER_NORESTART;
+}
+
+static irqreturn_t synaptics_ts_irq_handler(int irq, void *dev_id)
+{
+	struct synaptics_ts_data *ts = dev_id;
+
+	/* printk("synaptics_ts_irq_handler\n"); */
+	disable_irq_nosync(ts->client->irq);
+	queue_work(synaptics_wq, &ts->work);
+	return IRQ_HANDLED;
+}
+
+static int synaptics_ts_probe(
+	struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct synaptics_ts_data *ts;
+	uint8_t buf0[4];
+	uint8_t buf1[8];
+	struct i2c_msg msg[2];
+	int ret = 0;
+	uint16_t max_x, max_y;
+	int fuzz_x, fuzz_y, fuzz_p, fuzz_w;
+	struct synaptics_i2c_rmi_platform_data *pdata;
+	unsigned long irqflags;
+	int inactive_area_left;
+	int inactive_area_right;
+	int inactive_area_top;
+	int inactive_area_bottom;
+	int snap_left_on;
+	int snap_left_off;
+	int snap_right_on;
+	int snap_right_off;
+	int snap_top_on;
+	int snap_top_off;
+	int snap_bottom_on;
+	int snap_bottom_off;
+	uint32_t panel_version;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		printk(KERN_ERR "synaptics_ts_probe: need I2C_FUNC_I2C\n");
+		ret = -ENODEV;
+		goto err_check_functionality_failed;
+	}
+
+	ts = kzalloc(sizeof(*ts), GFP_KERNEL);
+	if (ts == NULL) {
+		ret = -ENOMEM;
+		goto err_alloc_data_failed;
+	}
+	INIT_WORK(&ts->work, synaptics_ts_work_func);
+	ts->client = client;
+	i2c_set_clientdata(client, ts);
+	pdata = client->dev.platform_data;
+	if (pdata)
+		ts->power = pdata->power;
+	if (ts->power) {
+		ret = ts->power(1);
+		if (ret < 0) {
+			printk(KERN_ERR "synaptics_ts_probe power on failed\n");
+			goto err_power_failed;
+		}
+	}
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0xf4, 0x01); /* device command = reset */
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed\n");
+		/* fail? */
+	}
+	{
+		int retry = 10;
+		while (retry-- > 0) {
+			ret = i2c_smbus_read_byte_data(ts->client, 0xe4);
+			if (ret >= 0)
+				break;
+			msleep(100);
+		}
+	}
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: Product Major Version %x\n", ret);
+	panel_version = ret << 8;
+	ret = i2c_smbus_read_byte_data(ts->client, 0xe5);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: Product Minor Version %x\n", ret);
+	panel_version |= ret;
+
+	ret = i2c_smbus_read_byte_data(ts->client, 0xe3);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: product property %x\n", ret);
+
+	if (pdata) {
+		while (pdata->version > panel_version)
+			pdata++;
+		ts->flags = pdata->flags;
+		ts->sensitivity_adjust = pdata->sensitivity_adjust;
+		irqflags = pdata->irqflags;
+		inactive_area_left = pdata->inactive_left;
+		inactive_area_right = pdata->inactive_right;
+		inactive_area_top = pdata->inactive_top;
+		inactive_area_bottom = pdata->inactive_bottom;
+		snap_left_on = pdata->snap_left_on;
+		snap_left_off = pdata->snap_left_off;
+		snap_right_on = pdata->snap_right_on;
+		snap_right_off = pdata->snap_right_off;
+		snap_top_on = pdata->snap_top_on;
+		snap_top_off = pdata->snap_top_off;
+		snap_bottom_on = pdata->snap_bottom_on;
+		snap_bottom_off = pdata->snap_bottom_off;
+		fuzz_x = pdata->fuzz_x;
+		fuzz_y = pdata->fuzz_y;
+		fuzz_p = pdata->fuzz_p;
+		fuzz_w = pdata->fuzz_w;
+	} else {
+		irqflags = 0;
+		inactive_area_left = 0;
+		inactive_area_right = 0;
+		inactive_area_top = 0;
+		inactive_area_bottom = 0;
+		snap_left_on = 0;
+		snap_left_off = 0;
+		snap_right_on = 0;
+		snap_right_off = 0;
+		snap_top_on = 0;
+		snap_top_off = 0;
+		snap_bottom_on = 0;
+		snap_bottom_off = 0;
+		fuzz_x = 0;
+		fuzz_y = 0;
+		fuzz_p = 0;
+		fuzz_w = 0;
+	}
+
+	ret = i2c_smbus_read_byte_data(ts->client, 0xf0);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: device control %x\n", ret);
+
+	ret = i2c_smbus_read_byte_data(ts->client, 0xf1);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_byte_data failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: interrupt enable %x\n", ret);
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0xf1, 0); /* disable interrupt */
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed\n");
+		goto err_detect_failed;
+	}
+
+	msg[0].addr = ts->client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = buf0;
+	buf0[0] = 0xe0;
+	msg[1].addr = ts->client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = 8;
+	msg[1].buf = buf1;
+	ret = i2c_transfer(ts->client->adapter, msg, 2);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_transfer failed\n");
+		goto err_detect_failed;
+	}
+	printk(KERN_INFO "synaptics_ts_probe: 0xe0: %x %x %x %x %x %x %x %x\n",
+	       buf1[0], buf1[1], buf1[2], buf1[3],
+	       buf1[4], buf1[5], buf1[6], buf1[7]);
+
+	ret = i2c_smbus_write_byte_data(ts->client, 0xff, 0x10); /* page select = 0x10 */
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_write_byte_data failed for page select\n");
+		goto err_detect_failed;
+	}
+	ret = i2c_smbus_read_word_data(ts->client, 0x02);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_word_data failed\n");
+		goto err_detect_failed;
+	}
+	ts->has_relative_report = !(ret & 0x100);
+	printk(KERN_INFO "synaptics_ts_probe: Sensor properties %x\n", ret);
+	ret = i2c_smbus_read_word_data(ts->client, 0x04);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_word_data failed\n");
+		goto err_detect_failed;
+	}
+	ts->max[0] = max_x = (ret >> 8 & 0xff) | ((ret & 0x1f) << 8);
+	ret = i2c_smbus_read_word_data(ts->client, 0x06);
+	if (ret < 0) {
+		printk(KERN_ERR "i2c_smbus_read_word_data failed\n");
+		goto err_detect_failed;
+	}
+	ts->max[1] = max_y = (ret >> 8 & 0xff) | ((ret & 0x1f) << 8);
+	if (ts->flags & SYNAPTICS_SWAP_XY)
+		swap(max_x, max_y);
+
+	ret = synaptics_init_panel(ts); /* will also switch back to page 0x04 */
+	if (ret < 0) {
+		printk(KERN_ERR "synaptics_init_panel failed\n");
+		goto err_detect_failed;
+	}
+
+	ts->input_dev = input_allocate_device();
+	if (ts->input_dev == NULL) {
+		ret = -ENOMEM;
+		printk(KERN_ERR "synaptics_ts_probe: Failed to allocate input device\n");
+		goto err_input_dev_alloc_failed;
+	}
+	ts->input_dev->name = "synaptics-rmi-touchscreen";
+	set_bit(EV_SYN, ts->input_dev->evbit);
+	set_bit(EV_KEY, ts->input_dev->evbit);
+	set_bit(BTN_TOUCH, ts->input_dev->keybit);
+	set_bit(BTN_2, ts->input_dev->keybit);
+	set_bit(EV_ABS, ts->input_dev->evbit);
+	inactive_area_left = inactive_area_left * max_x / 0x10000;
+	inactive_area_right = inactive_area_right * max_x / 0x10000;
+	inactive_area_top = inactive_area_top * max_y / 0x10000;
+	inactive_area_bottom = inactive_area_bottom * max_y / 0x10000;
+	snap_left_on = snap_left_on * max_x / 0x10000;
+	snap_left_off = snap_left_off * max_x / 0x10000;
+	snap_right_on = snap_right_on * max_x / 0x10000;
+	snap_right_off = snap_right_off * max_x / 0x10000;
+	snap_top_on = snap_top_on * max_y / 0x10000;
+	snap_top_off = snap_top_off * max_y / 0x10000;
+	snap_bottom_on = snap_bottom_on * max_y / 0x10000;
+	snap_bottom_off = snap_bottom_off * max_y / 0x10000;
+	fuzz_x = fuzz_x * max_x / 0x10000;
+	fuzz_y = fuzz_y * max_y / 0x10000;
+	ts->snap_down[!!(ts->flags & SYNAPTICS_SWAP_XY)] = -inactive_area_left;
+	ts->snap_up[!!(ts->flags & SYNAPTICS_SWAP_XY)] = max_x + inactive_area_right;
+	ts->snap_down[!(ts->flags & SYNAPTICS_SWAP_XY)] = -inactive_area_top;
+	ts->snap_up[!(ts->flags & SYNAPTICS_SWAP_XY)] = max_y + inactive_area_bottom;
+	ts->snap_down_on[!!(ts->flags & SYNAPTICS_SWAP_XY)] = snap_left_on;
+	ts->snap_down_off[!!(ts->flags & SYNAPTICS_SWAP_XY)] = snap_left_off;
+	ts->snap_up_on[!!(ts->flags & SYNAPTICS_SWAP_XY)] = max_x - snap_right_on;
+	ts->snap_up_off[!!(ts->flags & SYNAPTICS_SWAP_XY)] = max_x - snap_right_off;
+	ts->snap_down_on[!(ts->flags & SYNAPTICS_SWAP_XY)] = snap_top_on;
+	ts->snap_down_off[!(ts->flags & SYNAPTICS_SWAP_XY)] = snap_top_off;
+	ts->snap_up_on[!(ts->flags & SYNAPTICS_SWAP_XY)] = max_y - snap_bottom_on;
+	ts->snap_up_off[!(ts->flags & SYNAPTICS_SWAP_XY)] = max_y - snap_bottom_off;
+	printk(KERN_INFO "synaptics_ts_probe: max_x %d, max_y %d\n", max_x, max_y);
+	printk(KERN_INFO "synaptics_ts_probe: inactive_x %d %d, inactive_y %d %d\n",
+	       inactive_area_left, inactive_area_right,
+	       inactive_area_top, inactive_area_bottom);
+	printk(KERN_INFO "synaptics_ts_probe: snap_x %d-%d %d-%d, snap_y %d-%d %d-%d\n",
+	       snap_left_on, snap_left_off, snap_right_on, snap_right_off,
+	       snap_top_on, snap_top_off, snap_bottom_on, snap_bottom_off);
+	input_set_abs_params(ts->input_dev, ABS_X, -inactive_area_left, max_x + inactive_area_right, fuzz_x, 0);
+	input_set_abs_params(ts->input_dev, ABS_Y, -inactive_area_top, max_y + inactive_area_bottom, fuzz_y, 0);
+	input_set_abs_params(ts->input_dev, ABS_PRESSURE, 0, 255, fuzz_p, 0);
+	input_set_abs_params(ts->input_dev, ABS_TOOL_WIDTH, 0, 15, fuzz_w, 0);
+	input_set_abs_params(ts->input_dev, ABS_HAT0X, -inactive_area_left, max_x + inactive_area_right, fuzz_x, 0);
+	input_set_abs_params(ts->input_dev, ABS_HAT0Y, -inactive_area_top, max_y + inactive_area_bottom, fuzz_y, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_X, -inactive_area_left, max_x + inactive_area_right, fuzz_x, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_POSITION_Y, -inactive_area_top, max_y + inactive_area_bottom, fuzz_y, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_TOUCH_MAJOR, 0, 255, fuzz_p, 0);
+	input_set_abs_params(ts->input_dev, ABS_MT_WIDTH_MAJOR, 0, 15, fuzz_w, 0);
+	/* ts->input_dev->name = ts->keypad_info->name; */
+	ret = input_register_device(ts->input_dev);
+	if (ret) {
+		printk(KERN_ERR "synaptics_ts_probe: Unable to register %s input device\n", ts->input_dev->name);
+		goto err_input_register_device_failed;
+	}
+	if (client->irq) {
+		ret = request_irq(client->irq, synaptics_ts_irq_handler, irqflags, client->name, ts);
+		if (ret == 0) {
+			ret = i2c_smbus_write_byte_data(ts->client, 0xf1, 0x01); /* enable abs int */
+			if (ret)
+				free_irq(client->irq, ts);
+		}
+		if (ret == 0)
+			ts->use_irq = 1;
+		else
+			dev_err(&client->dev, "request_irq failed\n");
+	}
+	if (!ts->use_irq) {
+		hrtimer_init(&ts->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+		ts->timer.function = synaptics_ts_timer_func;
+		hrtimer_start(&ts->timer, ktime_set(1, 0), HRTIMER_MODE_REL);
+	}
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ts->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ts->early_suspend.suspend = synaptics_ts_early_suspend;
+	ts->early_suspend.resume = synaptics_ts_late_resume;
+	register_early_suspend(&ts->early_suspend);
+#endif
+
+	printk(KERN_INFO "synaptics_ts_probe: Start touchscreen %s in %s mode\n", ts->input_dev->name, ts->use_irq ? "interrupt" : "polling");
+
+	return 0;
+
+err_input_register_device_failed:
+	input_free_device(ts->input_dev);
+
+err_input_dev_alloc_failed:
+err_detect_failed:
+err_power_failed:
+	kfree(ts);
+err_alloc_data_failed:
+err_check_functionality_failed:
+	return ret;
+}
+
+static int synaptics_ts_remove(struct i2c_client *client)
+{
+	struct synaptics_ts_data *ts = i2c_get_clientdata(client);
+	unregister_early_suspend(&ts->early_suspend);
+	if (ts->use_irq)
+		free_irq(client->irq, ts);
+	else
+		hrtimer_cancel(&ts->timer);
+	input_unregister_device(ts->input_dev);
+	kfree(ts);
+	return 0;
+}
+
+static int synaptics_ts_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	int ret;
+	struct synaptics_ts_data *ts = i2c_get_clientdata(client);
+
+	if (ts->use_irq)
+		disable_irq(client->irq);
+	else
+		hrtimer_cancel(&ts->timer);
+	ret = cancel_work_sync(&ts->work);
+	if (ret && ts->use_irq) /* if work was pending disable-count is now 2 */
+		enable_irq(client->irq);
+	ret = i2c_smbus_write_byte_data(ts->client, 0xf1, 0); /* disable interrupt */
+	if (ret < 0)
+		printk(KERN_ERR "synaptics_ts_suspend: i2c_smbus_write_byte_data failed\n");
+
+	ret = i2c_smbus_write_byte_data(client, 0xf0, 0x86); /* deep sleep */
+	if (ret < 0)
+		printk(KERN_ERR "synaptics_ts_suspend: i2c_smbus_write_byte_data failed\n");
+	if (ts->power) {
+		ret = ts->power(0);
+		if (ret < 0)
+			printk(KERN_ERR "synaptics_ts_resume power off failed\n");
+	}
+	return 0;
+}
+
+static int synaptics_ts_resume(struct i2c_client *client)
+{
+	int ret;
+	struct synaptics_ts_data *ts = i2c_get_clientdata(client);
+
+	if (ts->power) {
+		ret = ts->power(1);
+		if (ret < 0)
+			printk(KERN_ERR "synaptics_ts_resume power on failed\n");
+	}
+
+	synaptics_init_panel(ts);
+
+	if (ts->use_irq)
+		enable_irq(client->irq);
+
+	if (!ts->use_irq)
+		hrtimer_start(&ts->timer, ktime_set(1, 0), HRTIMER_MODE_REL);
+	else
+		i2c_smbus_write_byte_data(ts->client, 0xf1, 0x01); /* enable abs int */
+
+	return 0;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void synaptics_ts_early_suspend(struct early_suspend *h)
+{
+	struct synaptics_ts_data *ts;
+	ts = container_of(h, struct synaptics_ts_data, early_suspend);
+	synaptics_ts_suspend(ts->client, PMSG_SUSPEND);
+}
+
+static void synaptics_ts_late_resume(struct early_suspend *h)
+{
+	struct synaptics_ts_data *ts;
+	ts = container_of(h, struct synaptics_ts_data, early_suspend);
+	synaptics_ts_resume(ts->client);
+}
+#endif
+
+static const struct i2c_device_id synaptics_ts_id[] = {
+	{ SYNAPTICS_I2C_RMI_NAME, 0 },
+	{ }
+};
+
+static struct i2c_driver synaptics_ts_driver = {
+	.probe		= synaptics_ts_probe,
+	.remove		= synaptics_ts_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
+	.suspend	= synaptics_ts_suspend,
+	.resume		= synaptics_ts_resume,
+#endif
+	.id_table	= synaptics_ts_id,
+	.driver = {
+		.name	= SYNAPTICS_I2C_RMI_NAME,
+	},
+};
+
+static int __devinit synaptics_ts_init(void)
+{
+	synaptics_wq = create_singlethread_workqueue("synaptics_wq");
+	if (!synaptics_wq)
+		return -ENOMEM;
+	return i2c_add_driver(&synaptics_ts_driver);
+}
+
+static void __exit synaptics_ts_exit(void)
+{
+	i2c_del_driver(&synaptics_ts_driver);
+	if (synaptics_wq)
+		destroy_workqueue(synaptics_wq);
+}
+
+module_init(synaptics_ts_init);
+module_exit(synaptics_ts_exit);
+
+MODULE_DESCRIPTION("Synaptics Touchscreen Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index ff4b8cfda58..5183a2d4fd5 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -50,6 +50,14 @@ config LEDS_LM3530
 	  controlled manually or using PWM input or using ambient
 	  light automatically.
 
+config LEDS_AB5500
+	tristate "HVLED driver for AB5500"
+	depends on AB5500_CORE
+	help
+	  This option enables support for the HVLED in AB5500
+	  multi function device. Currently Ab5500 v1.0 chip leds
+	  are supported.
+
 config LEDS_LOCOMO
 	tristate "LED Support for Locomo device"
 	depends on LEDS_CLASS
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index 890481cb09f..59a569b376e 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -10,6 +10,7 @@ obj-$(CONFIG_LEDS_ATMEL_PWM)		+= leds-atmel-pwm.o
 obj-$(CONFIG_LEDS_BD2802)		+= leds-bd2802.o
 obj-$(CONFIG_LEDS_LOCOMO)		+= leds-locomo.o
 obj-$(CONFIG_LEDS_LM3530)		+= leds-lm3530.o
+obj-$(CONFIG_LEDS_AB5500)		+= leds-ab5500.o
 obj-$(CONFIG_LEDS_MIKROTIK_RB532)	+= leds-rb532.o
 obj-$(CONFIG_LEDS_S3C24XX)		+= leds-s3c24xx.o
 obj-$(CONFIG_LEDS_NET48XX)		+= leds-net48xx.o
diff --git a/drivers/leds/leds-ab5500.c b/drivers/leds/leds-ab5500.c
new file mode 100644
index 00000000000..294551b1962
--- /dev/null
+++ b/drivers/leds/leds-ab5500.c
@@ -0,0 +1,811 @@
+/*
+ * leds-ab5500.c - driver for High Voltage (HV) LED in ST-Ericsson AB5500 chip
+ *
+ * Copyright (C) 2011 ST-Ericsson SA.
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Shreshtha Kumar SAHU <shreshthakumar.sahu@stericsson.com>
+ */
+
+/*
+ * Driver for HVLED in ST-Ericsson AB5500 analog baseband controller
+ *
+ * This chip can drive upto 3 leds, of upto 40mA of led sink current.
+ * These leds can be programmed to blink between two intensities with
+ * fading delay of half, one or two seconds.
+ *
+ * Leds can be controlled via sysfs entries in
+ *	"/sys/class/leds/< red | green | blue >"
+ *
+ * For each led,
+ *
+ * Modes of operation:
+ *  - manual:	echo 0 > fade_auto (default, no auto blinking)
+ *  - auto:	echo 1 > fade_auto
+ *
+ * Soft scaling delay between two intensities:
+ *  - 1/2 sec:	echo 1 > fade_delay
+ *  - 1 sec:	echo 2 > fade_delay
+ *  - 2 sec:	echo 3 > fade_delay
+ *
+ * Possible sequence of operation:
+ *  - continuous glow: set brightness (brt)
+ *  - blink between LED_OFF and LED_FULL:
+ *	set fade delay -> set fade auto
+ *  - blink between previous two brightness (only for LED-1):
+ *	set brt1 -> set brt2 -> set fade auto
+ *
+ * Delay can be set in any step, its affect will be seen on switching mode.
+ *
+ * Note: Blink/Fade feature is supported in AB5500 v2 onwards
+ *
+ */
+
+#include <linux/leds.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/input.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+#include <linux/leds-ab5500.h>
+#include <linux/types.h>
+
+#include <mach/hardware.h>
+
+#define AB5500LED_NAME		"ab5500-leds"
+#define AB5500_LED_MAX		0x03
+
+/* Register offsets */
+#define AB5500_LED_REG_ENABLE	0x03
+#define AB5500_LED_FADE_CTRL	0x0D
+
+/* LED-0 Register Addr. Offsets */
+#define AB5500_LED0_PWM_DUTY	0x01
+#define AB5500_LED0_PWMFREQ	0x02
+#define AB5500_LED0_SINKCTL	0x0A
+#define AB5500_LED0_FADE_HI	0x11
+#define AB5500_LED0_FADE_LO	0x17
+
+/* LED-1 Register Addr. Offsets */
+#define AB5500_LED1_PWM_DUTY	0x05
+#define AB5500_LED1_PWMFREQ	0x06
+#define AB5500_LED1_SINKCTL	0x0B
+#define AB5500_LED1_FADE_HI	0x13
+#define AB5500_LED1_FADE_LO	0x19
+
+/* LED-2 Register Addr. Offsets */
+#define AB5500_LED2_PWM_DUTY	0x08
+#define AB5500_LED2_PWMFREQ	0x09
+#define AB5500_LED2_SINKCTL	0x0C
+#define AB5500_LED2_FADE_HI	0x15
+#define AB5500_LED2_FADE_LO	0x1B
+
+/* led-0/1/2 enable bit */
+#define AB5500_LED_ENABLE_MASK	0x04
+
+/* led intensity */
+#define AB5500_LED_INTENSITY_OFF	0x0
+#define AB5500_LED_INTENSITY_MAX	0x3FF
+#define AB5500_LED_INTENSITY_STEP	(AB5500_LED_INTENSITY_MAX/LED_FULL)
+
+/* pwm frequency */
+#define AB5500_LED_PWMFREQ_MAX		0x0F	/* 373.39 @sysclk=26MHz */
+#define AB5500_LED_PWMFREQ_SHIFT	4
+
+/* LED sink current control */
+#define AB5500_LED_SINKCURR_MAX		0x0F	/* 40mA MAX */
+#define AB5500_LED_SINKCURR_SHIFT	4
+
+/* fade Control shift and masks */
+#define AB5500_FADE_DELAY_SHIFT		0x00
+#define AB5500_FADE_MODE_MASK		0x80
+#define AB5500_FADE_DELAY_MASK		0x03
+#define AB5500_FADE_START_MASK		0x04
+#define AB5500_FADE_ON_MASK		0x70
+#define AB5500_LED_FADE_ENABLE(ledid)	(0x40 >> (ledid))
+
+struct ab5500_led {
+	u8 id;
+	u8 max_current;
+	u16 brt_val;
+	u16 fade_hi;
+	u16 fade_lo;
+	bool led_on;
+	struct led_classdev led_cdev;
+	struct work_struct led_work;
+};
+
+struct ab5500_hvleds {
+	struct mutex lock;
+	struct device *dev;
+	struct ab5500_hvleds_platform_data *pdata;
+	struct ab5500_led leds[AB5500_HVLEDS_MAX];
+	bool hw_fade;
+	bool fade_auto;
+	enum ab5500_fade_delay fade_delay;
+};
+
+static u8 ab5500_led_pwmduty_reg[AB5500_LED_MAX] = {
+			AB5500_LED0_PWM_DUTY,
+			AB5500_LED1_PWM_DUTY,
+			AB5500_LED2_PWM_DUTY,
+};
+
+static u8 ab5500_led_pwmfreq_reg[AB5500_LED_MAX] = {
+			AB5500_LED0_PWMFREQ,
+			AB5500_LED1_PWMFREQ,
+			AB5500_LED2_PWMFREQ,
+};
+
+static u8 ab5500_led_sinkctl_reg[AB5500_LED_MAX] = {
+			AB5500_LED0_SINKCTL,
+			AB5500_LED1_SINKCTL,
+			AB5500_LED2_SINKCTL
+};
+
+static u8 ab5500_led_fade_hi_reg[AB5500_LED_MAX] = {
+			AB5500_LED0_FADE_HI,
+			AB5500_LED1_FADE_HI,
+			AB5500_LED2_FADE_HI,
+};
+
+static u8 ab5500_led_fade_lo_reg[AB5500_LED_MAX] = {
+			AB5500_LED0_FADE_LO,
+			AB5500_LED1_FADE_LO,
+			AB5500_LED2_FADE_LO,
+};
+
+#define to_led(_x)	container_of(_x, struct ab5500_led, _x)
+
+static inline struct ab5500_hvleds *led_to_hvleds(struct ab5500_led *led)
+{
+	return container_of(led, struct ab5500_hvleds, leds[led->id]);
+}
+
+static int ab5500_led_enable(struct ab5500_hvleds *hvleds,
+		unsigned int led_id)
+{
+	int ret;
+
+	ret = abx500_mask_and_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_pwmduty_reg[led_id],
+			AB5500_LED_ENABLE_MASK,
+			AB5500_LED_ENABLE_MASK);
+	if (ret < 0)
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+				ab5500_led_pwmduty_reg[led_id], ret);
+
+	return ret;
+
+}
+
+static int ab5500_led_start_manual(struct ab5500_hvleds *hvleds)
+{
+	int ret;
+
+	mutex_lock(&hvleds->lock);
+
+	ret = abx500_mask_and_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			AB5500_LED_FADE_CTRL, AB5500_FADE_START_MASK,
+			AB5500_FADE_START_MASK);
+	if (ret < 0)
+		dev_err(hvleds->dev, "update reg 0x%x failed - %d\n",
+				AB5500_LED_FADE_CTRL, ret);
+
+	mutex_unlock(&hvleds->lock);
+
+	return ret;
+}
+
+static int ab5500_led_disable(struct ab5500_hvleds *hvleds,
+		unsigned int led_id)
+{
+	int ret;
+
+	ret = abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_pwmduty_reg[led_id] - 1, 0);
+	ret |= abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_pwmduty_reg[led_id], 0);
+	if (ret < 0)
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			ab5500_led_pwmduty_reg[led_id], ret);
+
+	return ret;
+}
+
+static int ab5500_led_pwmduty_write(struct ab5500_hvleds *hvleds,
+			unsigned int led_id, u16 val)
+{
+	int ret;
+	u8 val_lsb = val & 0xFF;
+	u8 val_msb = (val & 0x300) >> 8;
+
+	mutex_lock(&hvleds->lock);
+
+	dev_dbg(hvleds->dev, "ab5500-leds: reg[%d] w val = %d\n"
+		"reg[%d] w val = %d\n",
+		ab5500_led_pwmduty_reg[led_id] - 1, val_lsb,
+		ab5500_led_pwmduty_reg[led_id], val_msb);
+
+	ret = abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_pwmduty_reg[led_id] - 1, val_lsb);
+	ret |= abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_pwmduty_reg[led_id], val_msb);
+	if (ret < 0)
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			ab5500_led_pwmduty_reg[led_id], ret);
+
+	mutex_unlock(&hvleds->lock);
+
+	return ret;
+}
+
+static int ab5500_led_pwmfreq_write(struct ab5500_hvleds *hvleds,
+			unsigned int led_id, u8 val)
+{
+	int ret;
+
+	val = (val & 0x0F) << AB5500_LED_PWMFREQ_SHIFT;
+
+	mutex_lock(&hvleds->lock);
+
+	dev_dbg(hvleds->dev, "ab5500-leds: reg[%d] w val=%d\n",
+			ab5500_led_pwmfreq_reg[led_id], val);
+
+	ret = abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_pwmfreq_reg[led_id], val);
+	if (ret < 0)
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			ab5500_led_pwmfreq_reg[led_id], ret);
+
+	mutex_unlock(&hvleds->lock);
+
+	return ret;
+}
+
+static int ab5500_led_sinkctl_write(struct ab5500_hvleds *hvleds,
+			unsigned int led_id, u8 val)
+{
+	int ret;
+
+	if (val > AB5500_LED_SINKCURR_MAX)
+		val = AB5500_LED_SINKCURR_MAX;
+
+	val = (val << AB5500_LED_SINKCURR_SHIFT);
+
+	dev_dbg(hvleds->dev, "ab5500-leds: reg[%d] w val=%d\n",
+			ab5500_led_sinkctl_reg[led_id], val);
+
+	mutex_lock(&hvleds->lock);
+
+	ret = abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_sinkctl_reg[led_id], val);
+	if (ret < 0)
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			ab5500_led_sinkctl_reg[led_id], ret);
+
+	mutex_unlock(&hvleds->lock);
+
+	return ret;
+}
+
+static int ab5500_led_fade_write(struct ab5500_hvleds *hvleds,
+			unsigned int led_id, bool on, u16 val)
+{
+	int ret;
+	int val_lsb = val & 0xFF;
+	int val_msb = (val & 0x300) >> 8;
+	u8 *fade_reg;
+
+	if (on)
+		fade_reg = ab5500_led_fade_hi_reg;
+	else
+		fade_reg = ab5500_led_fade_lo_reg;
+
+	dev_dbg(hvleds->dev, "ab5500-leds: reg[%d] w val = %d\n"
+		"reg[%d] w val = %d\n",
+		fade_reg[led_id] - 1, val_lsb,
+		fade_reg[led_id], val_msb);
+
+	mutex_lock(&hvleds->lock);
+
+	ret = abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			fade_reg[led_id] - 1, val_lsb);
+	ret |= abx500_set_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			fade_reg[led_id], val_msb);
+	if (ret < 0)
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			fade_reg[led_id], ret);
+
+	mutex_unlock(&hvleds->lock);
+
+	return ret;
+}
+
+static int ab5500_led_sinkctl_read(struct ab5500_hvleds *hvleds,
+			unsigned int led_id)
+{
+	int ret;
+	u8 val;
+
+	mutex_lock(&hvleds->lock);
+
+	ret = abx500_get_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			ab5500_led_sinkctl_reg[led_id], &val);
+	if (ret < 0) {
+		dev_err(hvleds->dev, "reg[%d] r failed: %d\n",
+			ab5500_led_sinkctl_reg[led_id], ret);
+		mutex_unlock(&hvleds->lock);
+		return ret;
+	}
+
+	val = (val & 0xF0) >> AB5500_LED_SINKCURR_SHIFT;
+
+	mutex_unlock(&hvleds->lock);
+
+	return val;
+}
+
+static void ab5500_led_brightness_set(struct led_classdev *led_cdev,
+			enum led_brightness brt_val)
+{
+	struct ab5500_led *led = to_led(led_cdev);
+
+	/* adjust LED_FULL to 10bit range */
+	brt_val &= LED_FULL;
+	led->brt_val = brt_val * AB5500_LED_INTENSITY_STEP;
+
+	schedule_work(&led->led_work);
+}
+
+static void ab5500_led_work(struct work_struct *led_work)
+{
+	struct ab5500_led *led = to_led(led_work);
+	struct ab5500_hvleds *hvleds = led_to_hvleds(led);
+
+	if (led->led_on == true) {
+		ab5500_led_pwmduty_write(hvleds, led->id, led->brt_val);
+		if (hvleds->hw_fade && led->brt_val) {
+			ab5500_led_enable(hvleds, led->id);
+			ab5500_led_start_manual(hvleds);
+		}
+	}
+}
+
+static ssize_t ab5500_led_show_current(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	int led_curr = 0;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct ab5500_led *led = to_led(led_cdev);
+	struct ab5500_hvleds *hvleds = led_to_hvleds(led);
+
+	led_curr = ab5500_led_sinkctl_read(hvleds, led->id);
+
+	if (led_curr < 0)
+		return led_curr;
+
+	return sprintf(buf, "%d\n", led_curr);
+}
+
+static ssize_t ab5500_led_store_current(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t len)
+{
+	int ret;
+	unsigned long led_curr;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct ab5500_led *led = to_led(led_cdev);
+	struct ab5500_hvleds *hvleds = led_to_hvleds(led);
+
+	if (strict_strtoul(buf, 0, &led_curr))
+		return -EINVAL;
+
+	if (led_curr > led->max_current)
+		led_curr = led->max_current;
+
+	ret = ab5500_led_sinkctl_write(hvleds, led->id, led_curr);
+	if (ret < 0)
+		return ret;
+
+	return len;
+}
+
+static ssize_t ab5500_led_store_fade_auto(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t len)
+{
+	int ret;
+	u8 fade_ctrl = 0;
+	unsigned long fade_auto;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct ab5500_led *led = to_led(led_cdev);
+	struct ab5500_hvleds *hvleds = led_to_hvleds(led);
+
+	if (strict_strtoul(buf, 0, &fade_auto))
+		return -EINVAL;
+
+	if (fade_auto > 1) {
+		dev_err(hvleds->dev, "invalid mode\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&hvleds->lock);
+
+	ret = abx500_get_register_interruptible(
+			hvleds->dev, AB5500_BANK_LED,
+			AB5500_LED_FADE_CTRL, &fade_ctrl);
+	if (ret < 0) {
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+				AB5500_LED_FADE_CTRL, ret);
+		goto unlock_and_return;
+	}
+
+	/* manual mode */
+	if (fade_auto == false) {
+		fade_ctrl &= ~(AB5500_LED_FADE_ENABLE(led->id));
+		if (!(fade_ctrl & AB5500_FADE_ON_MASK))
+			fade_ctrl = 0;
+
+		ret = ab5500_led_disable(hvleds, led->id);
+		if (ret < 0)
+			goto unlock_and_return;
+	} else {
+		/* set led auto enable bit */
+		fade_ctrl |= AB5500_FADE_MODE_MASK;
+		fade_ctrl |= AB5500_LED_FADE_ENABLE(led->id);
+
+		/* set fade delay */
+		fade_ctrl &= ~AB5500_FADE_DELAY_MASK;
+		fade_ctrl |= hvleds->fade_delay << AB5500_FADE_DELAY_SHIFT;
+
+		/* set fade start manual */
+		fade_ctrl |= AB5500_FADE_START_MASK;
+
+		/* enble corresponding led */
+		ret = ab5500_led_enable(hvleds, led->id);
+		if (ret < 0)
+			goto unlock_and_return;
+
+	}
+
+	ret = abx500_set_register_interruptible(
+				hvleds->dev, AB5500_BANK_LED,
+				AB5500_LED_FADE_CTRL, fade_ctrl);
+	if (ret < 0) {
+		dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+			       AB5500_LED_FADE_CTRL, ret);
+		goto unlock_and_return;
+	}
+
+	hvleds->fade_auto = fade_auto;
+
+	ret = len;
+
+unlock_and_return:
+	mutex_unlock(&hvleds->lock);
+
+	return ret;
+}
+
+static ssize_t ab5500_led_show_fade_auto(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct ab5500_led *led = to_led(led_cdev);
+	struct ab5500_hvleds *hvleds = led_to_hvleds(led);
+
+	return sprintf(buf, "%d\n", hvleds->fade_auto);
+}
+
+static ssize_t ab5500_led_store_fade_delay(struct device *dev,
+			struct device_attribute *attr,
+			const char *buf, size_t len)
+{
+	unsigned long fade_delay;
+	struct led_classdev *led_cdev = dev_get_drvdata(dev);
+	struct ab5500_led *led = to_led(led_cdev);
+	struct ab5500_hvleds *hvleds = led_to_hvleds(led);
+
+	if (strict_strtoul(buf, 0, &fade_delay))
+		return -EINVAL;
+
+	if (fade_delay > AB5500_FADE_DELAY_TWOSEC) {
+		dev_err(hvleds->dev, "invalid mode\n");
+		return -EINVAL;
+	}
+
+	hvleds->fade_delay = fade_delay;
+
+	return len;
+}
+
+/* led class device attributes */
+static DEVICE_ATTR(led_current, S_IRUGO | S_IWUGO,
+	       ab5500_led_show_current, ab5500_led_store_current);
+static DEVICE_ATTR(fade_auto, S_IRUGO | S_IWUGO,
+	       ab5500_led_show_fade_auto, ab5500_led_store_fade_auto);
+static DEVICE_ATTR(fade_delay, S_IRUGO | S_IWUGO,
+	       NULL, ab5500_led_store_fade_delay);
+
+static int ab5500_led_init_registers(struct ab5500_hvleds *hvleds)
+{
+	int ret = 0;
+	unsigned int led_id;
+
+	/*  fade - manual : dur mid : pwm duty mid */
+	if (!hvleds->hw_fade) {
+		ret = abx500_set_register_interruptible(
+				hvleds->dev, AB5500_BANK_LED,
+				AB5500_LED_REG_ENABLE, true);
+		if (ret < 0) {
+			dev_err(hvleds->dev, "reg[%d] w failed: %d\n",
+					AB5500_LED_REG_ENABLE, ret);
+			return ret;
+		}
+	}
+
+	for (led_id = 0; led_id < AB5500_HVLEDS_MAX; led_id++) {
+		if (hvleds->leds[led_id].led_on == false)
+			continue;
+
+		ret = ab5500_led_sinkctl_write(
+				hvleds, led_id,
+				hvleds->leds[led_id].max_current);
+		if (ret < 0)
+			return ret;
+
+		if (hvleds->hw_fade) {
+			ret = ab5500_led_pwmfreq_write(
+					hvleds, led_id,
+					AB5500_LED_PWMFREQ_MAX / 2);
+			if (ret < 0)
+				return ret;
+
+			/* fade high intensity */
+			ret = ab5500_led_fade_write(
+					hvleds, led_id, true,
+					hvleds->leds[led_id].fade_hi);
+			if (ret < 0)
+				return ret;
+
+			/* fade low intensity */
+			ret = ab5500_led_fade_write(
+					hvleds, led_id, false,
+					hvleds->leds[led_id].fade_lo);
+			if (ret < 0)
+				return ret;
+		}
+
+		/* init led off */
+		ret |= ab5500_led_pwmduty_write(
+				hvleds, led_id, AB5500_LED_INTENSITY_OFF);
+		if (ret < 0)
+			return ret;
+	}
+
+	return ret;
+}
+
+static int ab5500_led_register_leds(struct device *dev,
+			struct ab5500_hvleds_platform_data *pdata,
+			struct ab5500_hvleds *hvleds)
+{
+	int i_led;
+	int ret = 0;
+	struct ab5500_led_conf *pled;
+	struct ab5500_led *led;
+
+	hvleds->dev = dev;
+	hvleds->pdata = pdata;
+
+	if (abx500_get_chip_id(dev) == AB5500_2_0)
+		hvleds->hw_fade = true;
+	else
+		hvleds->hw_fade = false;
+
+	for (i_led = 0; i_led < AB5500_HVLEDS_MAX; i_led++) {
+		pled = &pdata->leds[i_led];
+		led = &hvleds->leds[i_led];
+
+		INIT_WORK(&led->led_work, ab5500_led_work);
+
+		led->id = pled->led_id;
+		led->max_current = pled->max_current;
+		led->led_on = pled->led_on;
+		led->led_cdev.name = pled->name;
+		led->led_cdev.brightness_set = ab5500_led_brightness_set;
+
+		/* Provide interface only for enabled LEDs */
+		if (led->led_on == false)
+			continue;
+
+		if (hvleds->hw_fade) {
+			led->fade_hi = (pled->fade_hi & LED_FULL);
+			led->fade_hi *= AB5500_LED_INTENSITY_STEP;
+			led->fade_lo = (pled->fade_lo & LED_FULL);
+			led->fade_lo *= AB5500_LED_INTENSITY_STEP;
+		}
+
+		ret = led_classdev_register(dev, &led->led_cdev);
+		if (ret < 0) {
+			dev_err(dev, "Register led class failed: %d\n", ret);
+			goto bailout1;
+		}
+
+		ret = device_create_file(led->led_cdev.dev,
+						&dev_attr_led_current);
+		if (ret < 0) {
+			dev_err(dev, "sysfs device creation failed: %d\n", ret);
+			goto bailout2;
+		}
+
+		if (hvleds->hw_fade) {
+			ret = device_create_file(led->led_cdev.dev,
+					&dev_attr_fade_auto);
+			if (ret < 0) {
+				dev_err(dev, "sysfs device "
+					"creation failed: %d\n", ret);
+				goto bailout3;
+			}
+
+			ret = device_create_file(led->led_cdev.dev,
+					&dev_attr_fade_delay);
+			if (ret < 0) {
+				dev_err(dev, "sysfs device "
+					"creation failed: %d\n", ret);
+				goto bailout4;
+			}
+		}
+	}
+
+	return ret;
+	for (; i_led >= 0; i_led--) {
+		if (hvleds->leds[i_led].led_on == false)
+			continue;
+
+		if (hvleds->hw_fade) {
+			device_remove_file(hvleds->leds[i_led].led_cdev.dev,
+					&dev_attr_fade_delay);
+bailout4:
+			device_remove_file(hvleds->leds[i_led].led_cdev.dev,
+					&dev_attr_fade_auto);
+		}
+bailout3:
+		device_remove_file(hvleds->leds[i_led].led_cdev.dev,
+				&dev_attr_led_current);
+bailout2:
+		led_classdev_unregister(&hvleds->leds[i_led].led_cdev);
+bailout1:
+		cancel_work_sync(&hvleds->leds[i_led].led_work);
+	}
+	return ret;
+}
+
+static int __devinit ab5500_hvleds_probe(struct platform_device *pdev)
+{
+	struct ab5500_hvleds_platform_data *pdata = pdev->dev.platform_data;
+	struct ab5500_hvleds *hvleds = NULL;
+	int ret = 0, i;
+
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "platform data required\n");
+		ret = -ENODEV;
+		goto err_out;
+	}
+
+	hvleds = kzalloc(sizeof(struct ab5500_hvleds), GFP_KERNEL);
+	if (hvleds == NULL) {
+		ret = -ENOMEM;
+		goto err_out;
+	}
+
+	mutex_init(&hvleds->lock);
+
+	/* init leds data and register led_classdev */
+	ret = ab5500_led_register_leds(&pdev->dev, pdata, hvleds);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "leds registration failed\n");
+		goto err_out;
+	}
+
+	/* init device registers and set initial led current */
+	ret = ab5500_led_init_registers(hvleds);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "reg init failed: %d\n", ret);
+		goto err_reg_init;
+	}
+
+	if (hvleds->hw_fade)
+		dev_info(&pdev->dev, "v2 enabled\n");
+	else
+		dev_info(&pdev->dev, "v1 enabled\n");
+
+	return ret;
+
+err_reg_init:
+	for (i = 0; i < AB5500_HVLEDS_MAX; i++) {
+		struct ab5500_led *led = &hvleds->leds[i];
+
+		if (led->led_on == false)
+			continue;
+
+		device_remove_file(led->led_cdev.dev, &dev_attr_led_current);
+		if (hvleds->hw_fade) {
+			device_remove_file(led->led_cdev.dev,
+					&dev_attr_fade_auto);
+			device_remove_file(led->led_cdev.dev,
+					&dev_attr_fade_delay);
+		}
+		led_classdev_unregister(&led->led_cdev);
+		cancel_work_sync(&led->led_work);
+	}
+err_out:
+	kfree(hvleds);
+	return ret;
+}
+
+static int __devexit ab5500_hvleds_remove(struct platform_device *pdev)
+{
+	struct ab5500_hvleds *hvleds = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < AB5500_HVLEDS_MAX; i++) {
+		struct ab5500_led *led = &hvleds->leds[i];
+
+		if (led->led_on == false)
+			continue;
+
+		device_remove_file(led->led_cdev.dev, &dev_attr_led_current);
+		if (hvleds->hw_fade) {
+			device_remove_file(led->led_cdev.dev,
+					&dev_attr_fade_auto);
+			device_remove_file(led->led_cdev.dev,
+					&dev_attr_fade_delay);
+		}
+		led_classdev_unregister(&led->led_cdev);
+		cancel_work_sync(&led->led_work);
+	}
+	kfree(hvleds);
+	return 0;
+}
+
+static struct platform_driver ab5500_hvleds_driver = {
+	.driver   = {
+		   .name = AB5500LED_NAME,
+		   .owner = THIS_MODULE,
+	},
+	.probe    = ab5500_hvleds_probe,
+	.remove   = __devexit_p(ab5500_hvleds_remove),
+};
+
+static int __init ab5500_hvleds_module_init(void)
+{
+	return platform_driver_register(&ab5500_hvleds_driver);
+}
+
+static void __exit ab5500_hvleds_module_exit(void)
+{
+	platform_driver_unregister(&ab5500_hvleds_driver);
+}
+
+module_init(ab5500_hvleds_module_init);
+module_exit(ab5500_hvleds_module_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Shreshtha Kumar SAHU <shreshthakumar.sahu@stericsson.com>");
+MODULE_DESCRIPTION("Driver for AB5500 HVLED");
+
diff --git a/drivers/leds/leds-lm3530.c b/drivers/leds/leds-lm3530.c
index 968fd5fef4f..41aed6c89ab 100644
--- a/drivers/leds/leds-lm3530.c
+++ b/drivers/leds/leds-lm3530.c
@@ -19,6 +19,7 @@
 #include <linux/types.h>
 #include <linux/regulator/consumer.h>
 #include <linux/module.h>
+#include <linux/gpio.h>
 
 #define LM3530_LED_DEV "lcd-backlight"
 #define LM3530_NAME "lm3530-led"
@@ -101,6 +102,7 @@ static struct lm3530_mode_map mode_map[] = {
  * @mode: mode of operation - manual, ALS, PWM
  * @regulator: regulator
  * @brighness: previous brightness value
+ * @hw_en_gpio: GPIO line for LM3530 HWEN
  * @enable: regulator is enabled
  */
 struct lm3530_data {
@@ -110,6 +112,7 @@ struct lm3530_data {
 	enum lm3530_mode mode;
 	struct regulator *regulator;
 	enum led_brightness brightness;
+	int hw_en_gpio;
 	bool enable;
 };
 
@@ -151,7 +154,7 @@ static int lm3530_init_registers(struct lm3530_data *drvdata)
 	u8 als_imp_sel = 0;
 	u8 brightness;
 	u8 reg_val[LM3530_REG_MAX];
-	u8 zones[LM3530_ALS_ZB_MAX];
+	u8 zones[LM3530_ALS_ZB_MAX] = {0};
 	u32 als_vmin, als_vmax, als_vstep;
 	struct lm3530_platform_data *pdata = drvdata->pdata;
 	struct i2c_client *client = drvdata->client;
@@ -230,6 +233,8 @@ static int lm3530_init_registers(struct lm3530_data *drvdata)
 	reg_val[13] = LM3530_DEF_ZT_4;	/* LM3530_ALS_Z4T_REG */
 
 	if (!drvdata->enable) {
+		if (drvdata->hw_en_gpio != LM3530_NO_HWEN_GPIO)
+			gpio_set_value(drvdata->hw_en_gpio, 1);
 		ret = regulator_enable(drvdata->regulator);
 		if (ret) {
 			dev_err(&drvdata->client->dev,
@@ -294,6 +299,8 @@ static void lm3530_brightness_set(struct led_classdev *led_cdev,
 			if (err)
 				dev_err(&drvdata->client->dev,
 					"Disable regulator failed\n");
+			if (drvdata->hw_en_gpio != LM3530_NO_HWEN_GPIO)
+				gpio_set_value(drvdata->hw_en_gpio, 0);
 			drvdata->enable = false;
 		}
 		break;
@@ -397,6 +404,7 @@ static int __devinit lm3530_probe(struct i2c_client *client,
 	drvdata->client = client;
 	drvdata->pdata = pdata;
 	drvdata->brightness = LED_OFF;
+	drvdata->hw_en_gpio = pdata->hw_en_gpio;
 	drvdata->enable = false;
 	drvdata->led_dev.name = LM3530_LED_DEV;
 	drvdata->led_dev.brightness_set = lm3530_brightness_set;
@@ -404,6 +412,15 @@ static int __devinit lm3530_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, drvdata);
 
+	if (gpio_is_valid(drvdata->hw_en_gpio)) {
+		err = gpio_request_one(drvdata->hw_en_gpio, GPIOF_OUT_INIT_HIGH,
+				"lm3530_hw_en");
+		if (err < 0) {
+			dev_err(&client->dev, "lm3530 hw_en gpio failed: %d\n", err);
+			goto err_gpio_request;
+		}
+	}
+
 	drvdata->regulator = regulator_get(&client->dev, "vin");
 	if (IS_ERR(drvdata->regulator)) {
 		dev_err(&client->dev, "regulator get failed\n");
@@ -443,6 +460,10 @@ err_class_register:
 err_reg_init:
 	regulator_put(drvdata->regulator);
 err_regulator_get:
+	if (gpio_is_valid(drvdata->hw_en_gpio))
+		gpio_free(drvdata->hw_en_gpio);
+err_gpio_request:
+	i2c_set_clientdata(client, NULL);
 	kfree(drvdata);
 err_out:
 	return err;
@@ -457,6 +478,8 @@ static int __devexit lm3530_remove(struct i2c_client *client)
 	if (drvdata->enable)
 		regulator_disable(drvdata->regulator);
 	regulator_put(drvdata->regulator);
+	if (gpio_is_valid(drvdata->hw_en_gpio))
+		gpio_free(drvdata->hw_en_gpio);
 	led_classdev_unregister(&drvdata->led_dev);
 	kfree(drvdata);
 	return 0;
diff --git a/drivers/leds/leds-lp5521.c b/drivers/leds/leds-lp5521.c
index 410a723b869..2f667071278 100644
--- a/drivers/leds/leds-lp5521.c
+++ b/drivers/leds/leds-lp5521.c
@@ -355,7 +355,12 @@ static int lp5521_do_store_load(struct lp5521_engine *engine,
 	while ((offset < len - 1) && (i < LP5521_PROGRAM_LENGTH)) {
 		/* separate sscanfs because length is working only for %s */
 		ret = sscanf(buf + offset, "%2s%n ", c, &nrchars);
-		if (ret != 2)
+		/*
+		 * Execution of a %n directive does not always
+		 * increment the assignment count returned at
+		 * completion of execution.so ret need not be 2
+		 */
+		if ((ret != 1) && (ret != 2))
 			goto fail;
 		ret = sscanf(c, "%2x", &cmd);
 		if (ret != 1)
@@ -787,6 +792,7 @@ static int __devinit lp5521_probe(struct i2c_client *client,
 	ret = lp5521_read(client, LP5521_REG_R_CURRENT, &buf);
 	if (buf != LP5521_REG_R_CURR_DEFAULT) {
 		dev_err(&client->dev, "error in resetting chip\n");
+		ret = -EIO;
 		goto fail2;
 	}
 	usleep_range(10000, 20000);
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
index 3ed92f34bd4..9c5f7136b37 100644
--- a/drivers/leds/leds-pwm.c
+++ b/drivers/leds/leds-pwm.c
@@ -27,9 +27,60 @@ struct led_pwm_data {
 	struct led_classdev	cdev;
 	struct pwm_device	*pwm;
 	unsigned int 		active_low;
+	unsigned int		lth_brightness;
 	unsigned int		period;
+	unsigned int		dutycycle_steps;
+	unsigned int		period_steps;
 };
 
+static int led_pwm_blink_set(struct led_classdev *led_cdev,
+	unsigned long *delay_on, unsigned long *delay_off)
+{
+	struct led_pwm_data *led_dat =
+		container_of(led_cdev, struct led_pwm_data, cdev);
+	int dutycycle_ms, period_sec;
+	int dutycycle, period;
+	/*
+	 * If both the delays are zero set some sensible delay
+	 */
+	if (*delay_on == 0 && *delay_off == 0) {
+		*delay_on = 500;
+		*delay_off = 500;
+	}
+	/*
+	 * calculate the duty cycle from on and off time
+	 */
+	dutycycle_ms = ((*delay_on * 1000)/(*delay_on + *delay_off));
+	/*
+	 * convert calculated value to write into the PWM out register
+	 */
+	if (led_dat->dutycycle_steps)
+		dutycycle = ((dutycycle_ms * led_dat->dutycycle_steps)/1000);
+	else
+		dutycycle = (dutycycle_ms/1000);
+	/*
+	 * calculate period from on and off time(msec)
+	 */
+	period_sec = ((*delay_on + *delay_off)/1000);
+	/*
+	 * convert calculated value to write into the PWM out register
+	 */
+	if (led_dat->period_steps) {
+		if ((*delay_on + *delay_off) == 500)
+			period = led_dat->period_steps;
+		else
+			period = led_dat->period_steps - period_sec;
+	}
+	else
+		period = period_sec;
+	/*
+	 * configure the PWM registers and enable blink functionality
+	 */
+	pwm_config_blink(led_dat->pwm, dutycycle, period);
+	pwm_blink_ctrl(led_dat->pwm, 1);
+	return 0;
+}
+
 static void led_pwm_set(struct led_classdev *led_cdev,
 	enum led_brightness brightness)
 {
@@ -42,7 +93,10 @@ static void led_pwm_set(struct led_classdev *led_cdev,
 		pwm_config(led_dat->pwm, 0, period);
 		pwm_disable(led_dat->pwm);
 	} else {
-		pwm_config(led_dat->pwm, brightness * period / max, period);
+		brightness = led_dat->lth_brightness + (brightness *
+			(led_dat->period - led_dat->lth_brightness) / max);
+		pwm_config(led_dat->pwm, brightness, led_dat->period);
+
 		pwm_enable(led_dat->pwm);
 	}
 }
@@ -79,8 +133,13 @@ static int led_pwm_probe(struct platform_device *pdev)
 		led_dat->cdev.default_trigger = cur_led->default_trigger;
 		led_dat->active_low = cur_led->active_low;
 		led_dat->period = cur_led->pwm_period_ns;
+		led_dat->lth_brightness = cur_led->lth_brightness *
+		      (cur_led->pwm_period_ns / cur_led->max_brightness);
+		led_dat->dutycycle_steps = cur_led->dutycycle_steps;
+		led_dat->period_steps = cur_led->period_steps;
 		led_dat->cdev.brightness_set = led_pwm_set;
 		led_dat->cdev.brightness = LED_OFF;
+		led_dat->cdev.blink_set = led_pwm_blink_set;
 		led_dat->cdev.max_brightness = cur_led->max_brightness;
 		led_dat->cdev.flags |= LED_CORE_SUSPENDRESUME;
 
diff --git a/drivers/media/radio/CG2900/Makefile b/drivers/media/radio/CG2900/Makefile
new file mode 100755
index 00000000000..60b12dd9c35
--- /dev/null
+++ b/drivers/media/radio/CG2900/Makefile
@@ -0,0 +1,12 @@
+#
+# Makefile for the CG2900 FM Radio Driver
+#
+
+radio_cg2900-objs := radio-cg2900.o cg2900_fm_api.o cg2900_fm_driver.o
+
+obj-$(CONFIG_RADIO_CG2900) += radio_cg2900.o
+
+ccflags-y :=                                    \
+        -Idrivers/staging/cg2900/include \
+
+
diff --git a/drivers/media/radio/CG2900/cg2900_fm_api.c b/drivers/media/radio/CG2900/cg2900_fm_api.c
new file mode 100644
index 00000000000..f73f3ef39ee
--- /dev/null
+++ b/drivers/media/radio/CG2900/cg2900_fm_api.c
@@ -0,0 +1,3389 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux FM Host API's for ST-Ericsson FM Chip.
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#include <linux/firmware.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <linux/kthread.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include "cg2900_fm_driver.h"
+
+#define CG2910_FM_LUT_INFO_FILE "FM_FW_CG2910_1_0_P1_4_lut_info.fw"
+#define CG2910_FM_PROG_INFO_FILE "FM_FW_CG2910_1_0_P1_4_prog_info.fw"
+#define CG2910_LUT_IDX		0
+#define CG2910_PROG_IDX		1
+#define CG2910_MAX_FILES_DL	2
+
+#define CG2900_FM_BT_SRC_COEFF_INFO_FILE "cg2900_fm_bt_src_coeff_info.fw"
+#define CG2900_FM_EXT_SRC_COEFF_INFO_FILE "cg2900_fm_ext_src_coeff_info.fw"
+#define CG2900_FM_FM_COEFF_INFO_FILE	"cg2900_fm_fm_coeff_info.fw"
+#define CG2900_FM_FM_PROG_INFO_FILE	"cg2900_fm_fm_prog_info.fw"
+#define CG2900_FM_LINE_BUFFER_LENGTH	128
+#define CG2900_FM_FILENAME_MAX		128
+#define FW_FILE_PARAM_LEN				3
+/* RDS Tx PTY set to Other music */
+#define OTHER_MUSIC			15
+#define DEFAULT_AUDIO_DEVIATION 0x1AA9
+#define DEFAULT_NOTIFICATION_HOLD_OFF_TIME 0x000A
+
+/* Specific chip version data */
+#define CG2900_PG1_REV			0x0101
+#define CG2900_PG2_REV			0x0200
+#define CG2900_PG1_SPECIAL_REV  0x0700
+#define CG2905_PG1_1_REV		0x1805
+#define CG2910_PG1_REV			0x1004
+#define CG2910_PG2_REV			0x1008
+
+static bool fm_rds_status;
+static bool fm_prev_rds_status;
+static u16 program_identification_code;
+static u16 default_program_identification_code = 0x1234;
+static u16 program_type_code;
+static u16 default_program_type_code = OTHER_MUSIC;
+static char program_service[MAX_PSN_SIZE];
+static char default_program_service[MAX_PSN_SIZE] = "FM-Xmit ";
+static char radio_text[MAX_RT_SIZE];
+static char default_radio_text[MAX_RT_SIZE] = "Default Radio Text "
+    "Default Radio Text Default Radio Text Default";
+static bool a_b_flag;
+u8 fm_event;
+static struct mutex rds_mutex;
+struct cg2900_fm_rds_buf fm_rds_buf[MAX_RDS_BUFFER][MAX_RDS_GROUPS];
+struct cg2900_fm_rds_info fm_rds_info;
+static enum cg2900_fm_state fm_state;
+static enum cg2900_fm_mode fm_mode;
+static struct cg2900_version_info version_info;
+
+/**
+ * cg2900_fm_get_one_line_of_text()- Get One line of text from a file.
+ *
+ * Replacement function for stdio function fgets.This function extracts one
+ * line of text from input file.
+ *
+ * @wr_buffer: Buffer to copy text to.
+ * @max_nbr_of_bytes: Max number of bytes to read, i.e. size of rd_buffer.
+ * @rd_buffer: Data to parse.
+ * @bytes_copied: Number of bytes copied to wr_buffer.
+ *
+ * Returns:
+ *   Pointer to next data to read.
+ */
+static char *cg2900_fm_get_one_line_of_text(
+			char *wr_buffer,
+			int max_nbr_of_bytes,
+			char *rd_buffer,
+			int *bytes_copied
+			)
+{
+	char *curr_wr = wr_buffer;
+	char *curr_rd = rd_buffer;
+	char in_byte;
+
+	*bytes_copied = 0;
+
+	do {
+		*curr_wr = *curr_rd;
+		in_byte = *curr_wr;
+		curr_wr++;
+		curr_rd++;
+		(*bytes_copied)++;
+	} while ((*bytes_copied <= max_nbr_of_bytes) && (in_byte != '\0')
+		 && (in_byte != '\n'));
+	*curr_wr = '\0';
+	return curr_rd;
+}
+
+/**
+ * cg2900_fm_get_file_to_load() - Parse info file and find correct target file.
+ *
+ * @fw:	Firmware structure containing file data.
+ * @file_name: (out) Pointer to name of requested file.
+ *
+ * Returns:
+ *   True,  if target file was found,
+ *   False, otherwise.
+ */
+static bool cg2900_fm_get_file_to_load(
+			const struct firmware *fw,
+			char **file_name
+			)
+{
+	char *line_buffer;
+	char *curr_file_buffer;
+	int bytes_left_to_parse = fw->size;
+	int bytes_read = 0;
+	bool file_found = false;
+
+	curr_file_buffer = (char *)&(fw->data[0]);
+
+	line_buffer = kmalloc(CG2900_FM_LINE_BUFFER_LENGTH,
+				GFP_KERNEL);
+
+	if (line_buffer == NULL) {
+		FM_ERR_REPORT("Failed to allocate:"
+			      "file_name 0x%X, line_buffer 0x%X",
+			      (unsigned int)file_name,
+			      (unsigned int)line_buffer);
+		goto error;
+	}
+
+	while (!file_found) {
+		/* Get one line of text from the file to parse */
+		curr_file_buffer =
+		    cg2900_fm_get_one_line_of_text(line_buffer,
+					min
+					(CG2900_FM_LINE_BUFFER_LENGTH,
+					 (int)(fw->size -
+						bytes_read)),
+						curr_file_buffer,
+						&bytes_read);
+
+		bytes_left_to_parse -= bytes_read;
+		if (bytes_left_to_parse <= 0) {
+			/* End of file => Leave while loop */
+			FM_ERR_REPORT("Reached end of file."
+				      "No file found!");
+			break;
+		}
+
+		/*
+		 * Check if the line of text is a comment
+		 * or not, comments begin with '#'
+		 */
+		if (*line_buffer != '#') {
+			u32 hci_rev = 0;
+			u32 lmp_sub = 0;
+
+			FM_DEBUG_REPORT("Found a valid line <%s>",
+					line_buffer);
+
+			/*
+			 * Check if we can find the correct
+			 * HCI revision and LMP subversion
+			 * as well as a file name in the text line
+			 * Store the filename if the actual file can
+			 * be found in the file system
+			 */
+			if (sscanf(line_buffer, "%x%x%s",
+				   (unsigned int *)&hci_rev,
+				   (unsigned int *)&lmp_sub,
+				   *file_name) == FW_FILE_PARAM_LEN
+			    && hci_rev == version_info.revision
+			    && lmp_sub == version_info.sub_version) {
+				FM_INFO_REPORT("File name = %s "
+					       "HCI Revision"
+					       "= 0x%04X LMP "
+					       "Subversion = 0x%04X",
+					       *file_name,
+					       (unsigned int)hci_rev,
+					       (unsigned int)lmp_sub);
+
+				/*
+				 * Name has already been stored above.
+				 * Nothing more to do
+				 */
+				file_found = true;
+			} else {
+		/*Zero the name buffer so  it is clear to next read*/
+				memset(*file_name, 0x00,
+				       CG2900_FM_FILENAME_MAX);
+			}
+		}
+	}
+	kfree(line_buffer);
+error:
+	return file_found;
+}
+
+
+/**
+ * cg2910_fm_load_firmware() - Loads the FM lut and
+ * Program firmware files for CG2910
+ *
+ * @device: Pointer to char device requesting the operation.
+ *
+ * Returns:
+ *  0, if firmware download is successful
+ *  -ENOENT, file not found.
+ *  -ENOMEM, out of memory
+ */
+static int cg2910_fm_load_firmware(
+			struct device *device
+			)
+{
+	int err;
+	bool file_found;
+	int result = 0;
+	const struct firmware *fm_fw_info[2];
+	const struct firmware *fm_firmware[2];
+	char *fm_fw_file_name = NULL;
+	int loopi = 0;
+
+	FM_INFO_REPORT("+cg2910_fm_load_firmware");
+	fm_fw_info[CG2910_LUT_IDX] = NULL;
+	fm_fw_info[CG2910_PROG_IDX] = NULL;
+	fm_firmware[CG2910_LUT_IDX] = NULL;
+	fm_firmware[CG2910_PROG_IDX] = NULL;
+
+	/* Open fm_fw_info lut file. */
+	err = request_firmware(&fm_fw_info[CG2910_LUT_IDX],
+			CG2910_FM_LUT_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2910_fm_load_firmware: "
+				"Couldn't get fm_fw_info lut file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Open fm_fw_info prog file. */
+	err = request_firmware(&fm_fw_info[CG2910_PROG_IDX],
+			CG2910_FM_PROG_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2910_fm_load_firmware: "
+				"Couldn't get fm_fw_info prog file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	fm_fw_file_name = kmalloc(CG2900_FM_FILENAME_MAX,
+			GFP_KERNEL);
+	if (fm_fw_file_name == NULL) {
+		FM_ERR_REPORT("cg2910_fm_load_firmware: "
+				"Couldn't allocate memory for "
+				"fm_fw_file_name");
+		result = -ENOMEM;
+		goto error;
+	}
+
+	/* Put a loop for downloading lut and prog */
+	for (loopi = 0; loopi < CG2910_MAX_FILES_DL; loopi++) {
+		/*
+		 * Now we have the fm_fw_info file. See if we can
+		 * find the right fm_fw_file_name file as well
+		 */
+		file_found = cg2900_fm_get_file_to_load(fm_fw_info[loopi],
+				&fm_fw_file_name);
+
+		if (!file_found) {
+			FM_ERR_REPORT("cg2910_fm_load_firmware: "
+					"Couldn't find fm_fw_file_name file!! "
+					"Major error!!!");
+			result = -ENOENT;
+			goto error;
+		}
+
+		/*
+		 * OK. Now it is time to download the firmware
+		 * First download lut file & then prog
+		 */
+		err = request_firmware(&fm_firmware[loopi],
+				fm_fw_file_name, device);
+		if (err < 0) {
+			FM_ERR_REPORT("cg2910_fm_load_firmware: "
+					"Couldn't get fm_firmware"
+					" file, err = %d", err);
+			result = -ENOENT;
+			goto error;
+		}
+
+		FM_INFO_REPORT("cg2910_fm_load_firmware: "
+				"Downloading %s of %d bytes",
+				fm_fw_file_name, fm_firmware[loopi]->size);
+		if (fmd_send_fm_firmware((u8 *) fm_firmware[loopi]->data,
+				fm_firmware[loopi]->size)) {
+			FM_ERR_REPORT("cg2910_fm_load_firmware: Error in "
+					"downloading %s", fm_fw_file_name);
+			result = -ENOENT;
+			goto error;
+		}
+	}
+
+error:
+	/* Release fm_fw_info lut and prog file */
+	if (fm_fw_info[CG2910_LUT_IDX])
+		release_firmware(fm_fw_info[CG2910_LUT_IDX]);
+	if (fm_fw_info[CG2910_PROG_IDX])
+		release_firmware(fm_fw_info[CG2910_PROG_IDX]);
+
+	if (fm_firmware[CG2910_LUT_IDX])
+		release_firmware(fm_firmware[CG2910_LUT_IDX]);
+	if (fm_firmware[CG2910_PROG_IDX])
+		release_firmware(fm_firmware[CG2910_PROG_IDX]);
+
+	/* Free Allocated memory */
+	kfree(fm_fw_file_name);
+	FM_DEBUG_REPORT("-cg2910_fm_load_firmware: returning %d",
+			result);
+	return result;
+}
+
+/**
+ * cg2900_fm_load_firmware() - Loads the FM Coeffecients and F/W file(s)
+ * for CG2900
+ * @device: Pointer to char device requesting the operation.
+ *
+ * Returns:
+ *  0, if firmware download is successful
+ *  -ENOENT, file not found.
+ *  -ENOMEM, out of memory
+ */
+static int cg2900_fm_load_firmware(
+			struct device *device
+			)
+{
+	int err;
+	bool file_found;
+	int result = 0;
+	const struct firmware *bt_src_coeff_info;
+	const struct firmware *ext_src_coeff_info;
+	const struct firmware *fm_coeff_info;
+	const struct firmware *fm_prog_info;
+	char *bt_src_coeff_file_name = NULL;
+	char *ext_src_coeff_file_name = NULL;
+	char *fm_coeff_file_name = NULL;
+	char *fm_prog_file_name = NULL;
+
+	FM_INFO_REPORT("+cg2900_fm_load_firmware");
+
+	/* Open bt_src_coeff info file. */
+	err = request_firmware(&bt_src_coeff_info,
+			       CG2900_FM_BT_SRC_COEFF_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get bt_src_coeff info file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/*
+	 * Now we have the bt_src_coeff info file.
+	 * See if we can find the right bt_src_coeff file as well
+	 */
+	bt_src_coeff_file_name = kmalloc(CG2900_FM_FILENAME_MAX,
+				GFP_KERNEL);
+	if (bt_src_coeff_file_name == NULL) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't allocate memory for "
+			      "bt_src_coeff_file_name");
+		release_firmware(bt_src_coeff_info);
+		result = -ENOMEM;
+		goto error;
+	}
+	file_found = cg2900_fm_get_file_to_load(bt_src_coeff_info,
+					     &bt_src_coeff_file_name);
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(bt_src_coeff_info);
+
+	if (!file_found) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't find bt_src_coeff file!! "
+			      "Major error!!!");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Open ext_src_coeff info file. */
+	err = request_firmware(&ext_src_coeff_info,
+			       CG2900_FM_EXT_SRC_COEFF_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get ext_src_coeff_info info file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/*
+	 * Now we have the ext_src_coeff info file. See if we can
+	 * find the right ext_src_coeff file as well
+	 */
+	ext_src_coeff_file_name = kmalloc(CG2900_FM_FILENAME_MAX,
+				GFP_KERNEL);
+	if (ext_src_coeff_file_name == NULL) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't allocate memory for "
+			      "ext_src_coeff_file_name");
+		release_firmware(ext_src_coeff_info);
+		result = -ENOMEM;
+		goto error;
+	}
+	file_found = cg2900_fm_get_file_to_load(ext_src_coeff_info,
+					     &ext_src_coeff_file_name);
+
+	/* Now we are finished with the ext_src_coeff info file */
+	release_firmware(ext_src_coeff_info);
+
+	if (!file_found) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't find ext_src_coeff_info "
+			      "file!!! Major error!");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Open fm_coeff info file. */
+	err = request_firmware(&fm_coeff_info,
+			       CG2900_FM_FM_COEFF_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get fm_coeff info file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/*
+	 * Now we have the fm_coeff_info info file.
+	 * See if we can find the right fm_coeff_info file as well
+	 */
+	fm_coeff_file_name = kmalloc(CG2900_FM_FILENAME_MAX,
+				GFP_KERNEL);
+	if (fm_coeff_file_name == NULL) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+						"Couldn't allocate memory for "
+						"fm_coeff_file_name");
+		release_firmware(fm_coeff_info);
+		result = -ENOMEM;
+		goto error;
+	}
+	file_found = cg2900_fm_get_file_to_load(fm_coeff_info,
+					     &fm_coeff_file_name);
+
+	/* Now we are finished with the fm_coeff info file */
+	release_firmware(fm_coeff_info);
+
+	if (!file_found) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't find fm_coeff file!!! "
+			      "Major error!");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Open fm_prog info file. */
+	err = request_firmware(&fm_prog_info,
+				CG2900_FM_FM_PROG_INFO_FILE, device);
+	if (err) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get fm_prog_info info file");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/*
+	 * Now we have the fm_prog info file.
+	 * See if we can find the right fm_prog file as well
+	 */
+	fm_prog_file_name = kmalloc(CG2900_FM_FILENAME_MAX,
+				GFP_KERNEL);
+	if (fm_prog_file_name == NULL) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+						"Couldn't allocate memory for "
+						"fm_prog_file_name");
+		release_firmware(fm_prog_info);
+		result = -ENOMEM;
+		goto error;
+	}
+	file_found = cg2900_fm_get_file_to_load(fm_prog_info,
+					     &fm_prog_file_name);
+
+	/* Now we are finished with fm_prog patch info file */
+	release_firmware(fm_prog_info);
+
+	if (!file_found) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't find fm_prog_info file!!! "
+			      "Major error!");
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* OK. Now it is time to download the firmware */
+	err = request_firmware(&bt_src_coeff_info,
+			       bt_src_coeff_file_name, device);
+	if (err < 0) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get bt_src_coeff file, err = %d", err);
+		result = -ENOENT;
+		goto error;
+	}
+
+	FM_INFO_REPORT("cg2900_fm_load_firmware: Downloading %s of %d bytes",
+		       bt_src_coeff_file_name, bt_src_coeff_info->size);
+	if (fmd_send_fm_firmware((u8 *) bt_src_coeff_info->data,
+				 bt_src_coeff_info->size)) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: Error in "
+				"downloading %s", bt_src_coeff_file_name);
+		release_firmware(bt_src_coeff_info);
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(bt_src_coeff_info);
+	err = request_firmware(&ext_src_coeff_info,
+			       ext_src_coeff_file_name, device);
+	if (err < 0) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get ext_src_coeff file, err = %d", err);
+		result = -ENOENT;
+		goto error;
+	}
+
+	FM_INFO_REPORT("cg2900_fm_load_firmware: Downloading %s of %d bytes",
+		       ext_src_coeff_file_name, ext_src_coeff_info->size);
+	if (fmd_send_fm_firmware((u8 *) ext_src_coeff_info->data,
+				 ext_src_coeff_info->size)) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: Error in "
+				"downloading %s", ext_src_coeff_file_name);
+		release_firmware(ext_src_coeff_info);
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(ext_src_coeff_info);
+
+	err = request_firmware(&fm_coeff_info, fm_coeff_file_name, device);
+	if (err < 0) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get fm_coeff file, err = %d", err);
+		result = -ENOENT;
+		goto error;
+	}
+
+	FM_INFO_REPORT("cg2900_fm_load_firmware: Downloading %s of %d bytes",
+		       fm_coeff_file_name, fm_coeff_info->size);
+	if (fmd_send_fm_firmware((u8 *) fm_coeff_info->data,
+				 fm_coeff_info->size)) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: Error in "
+				"downloading %s", fm_coeff_file_name);
+		release_firmware(fm_coeff_info);
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(fm_coeff_info);
+
+	err = request_firmware(&fm_prog_info, fm_prog_file_name, device);
+	if (err < 0) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: "
+			      "Couldn't get fm_prog file, err = %d", err);
+		result = -ENOENT;
+		goto error;
+	}
+
+	FM_INFO_REPORT("cg2900_fm_load_firmware: Downloading %s of %d bytes",
+		       fm_prog_file_name, fm_prog_info->size);
+	if (fmd_send_fm_firmware((u8 *) fm_prog_info->data,
+		fm_prog_info->size)) {
+		FM_ERR_REPORT("cg2900_fm_load_firmware: Error in "
+				"downloading %s", fm_prog_file_name);
+		release_firmware(fm_prog_info);
+		result = -ENOENT;
+		goto error;
+	}
+
+	/* Now we are finished with the bt_src_coeff info file */
+	release_firmware(fm_prog_info);
+
+error:
+	/* Free Allocated memory */
+	if (bt_src_coeff_file_name != NULL)
+		kfree(bt_src_coeff_file_name);
+	if (ext_src_coeff_file_name != NULL)
+		kfree(ext_src_coeff_file_name);
+	if (fm_coeff_file_name != NULL)
+		kfree(fm_coeff_file_name);
+	if (fm_prog_file_name != NULL)
+		kfree(fm_prog_file_name);
+	FM_DEBUG_REPORT("-cg2900_fm_load_firmware: returning %d",
+			result);
+	return result;
+}
+
+/**
+ * cg2900_fm_transmit_rds_groups()- Transmits the RDS Groups.
+ *
+ * Stores the RDS Groups in Chip's buffer and each group is
+ * transmitted every 87.6 ms.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise
+ */
+static int cg2900_fm_transmit_rds_groups(void)
+{
+	int result = 0;
+	u16 group_position = 0;
+	u8 block1[2];
+	u8 block2[2];
+	u8 block3[2];
+	u8 block4[2];
+	int index1 = 0;
+	int index2 = 0;
+	int group_0B_count = 0;
+	int group_2A_count = 0;
+
+	FM_INFO_REPORT("cg2900_fm_transmit_rds_groups");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_transmit_rds_groups: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	while (group_position < 20 && result == 0) {
+		if (group_position < 4) {
+			/* Transmit PSN in Group 0B */
+			block1[0] = program_identification_code;
+			block1[1] = program_identification_code >> 8;
+			/* M/S bit set to Music */
+			if (group_0B_count % 4 == 0) {
+				/* Manipulate DI bit */
+				block2[0] =
+				    (0x08 | ((program_type_code & 0x07)
+					     << 5))
+				    + group_0B_count;
+			} else {
+				block2[0] =
+				    (0x0C | ((program_type_code & 0x07)
+					     << 5))
+				    + group_0B_count;
+			}
+			block2[1] =
+			    0x08 | ((program_type_code & 0x18) >> 3);
+			block3[0] = program_identification_code;
+			block3[1] = program_identification_code >> 8;
+			block4[0] = program_service[index1 + 1];
+			block4[1] = program_service[index1 + 0];
+			index1 += 2;
+			group_0B_count++;
+		} else {
+			/* Transmit RT in Group 2A */
+			block1[0] = program_identification_code;
+			block1[1] = program_identification_code >> 8;
+			if (a_b_flag)
+				block2[0] = (0x10 |
+					     ((program_type_code & 0x07)
+					      << 5)) + group_2A_count;
+			else
+				block2[0] = (0x00 |
+					     ((program_type_code & 0x07)
+					      << 5)) + group_2A_count;
+			block2[1] = 0x20 | ((program_type_code & 0x18)
+					    >> 3);
+			block3[0] = radio_text[index2 + 1];
+			block3[1] = radio_text[index2 + 0];
+			block4[0] = radio_text[index2 + 3];
+			block4[1] = radio_text[index2 + 2];
+			index2 += 4;
+			group_2A_count++;
+		}
+		FM_DEBUG_REPORT("%02x%02x "
+				"%02x%02x "
+				"%02x%02x "
+				"%02x%02x ",
+				block1[1], block1[0],
+				block2[1], block2[0],
+				block3[1], block3[0],
+				block4[1], block4[0]);
+		result = fmd_tx_set_group(
+				group_position,
+				block1,
+				block2,
+				block3,
+				block4);
+		group_position++;
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_transmit_rds_groups: "
+				      "fmd_tx_set_group failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			break;
+		}
+	}
+	a_b_flag = !a_b_flag;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_transmit_rds_groups: returning %d",
+			result);
+	return result;
+}
+
+/**
+ * cg2900_fm_check_rds_status()- Checks whether RDS was On previously
+ *
+ * This method is called on receiving interrupt for Seek Completion,
+ * Scan completion and Block Scan completion. It will check whether RDS
+ * was forcefully disabled before the above operations started and if the
+ * previous RDS state was true, then RDS will be enabled back
+ */
+static void cg2900_fm_check_rds_status(void)
+{
+	FM_INFO_REPORT("cg2900_fm_check_rds_status");
+	if (fm_prev_rds_status) {
+		/* Restart RDS if it was active previously */
+		cg2900_fm_rds_on();
+		fm_prev_rds_status = false;
+	}
+}
+
+/**
+ * cg2900_fm_driver_callback()- Callback function indicating the event.
+ *
+ * This callback function is called on receiving irpt_CommandSucceeded,
+ * irpt_CommandFailed, irpt_bufferFull, etc from FM chip.
+ * @event: event for which the callback function was caled
+ * from FM Driver.
+ * @event_successful: Signifying whether the event is called from FM Driver
+ * on receiving irpt_OperationSucceeded or irpt_OperationFailed.
+ */
+static void cg2900_fm_driver_callback(
+			u8 event,
+			bool event_successful
+			)
+{
+	struct sk_buff *skb;
+
+	FM_INFO_REPORT("cg2900_fm_driver_callback: "
+			"event = %02x, event_successful = %x",
+			event, event_successful);
+
+	switch (event) {
+	case FMD_EVENT_GEN_POWERUP:
+		FM_DEBUG_REPORT("FMD_EVENT_GEN_POWERUP");
+		break;
+	case FMD_EVENT_ANTENNA_STATUS_CHANGED:
+		FM_DEBUG_REPORT("FMD_EVENT_ANTENNA_STATUS_CHANGED");
+		break;
+	case FMD_EVENT_FREQUENCY_CHANGED:
+		FM_DEBUG_REPORT("FMD_EVENT_FREQUENCY_CHANGED ");
+		break;
+	case FMD_EVENT_SEEK_STOPPED:
+		FM_DEBUG_REPORT("FMD_EVENT_SEEK_STOPPED");
+		skb = alloc_skb(SKB_FM_INTERRUPT_DATA,
+			GFP_KERNEL);
+		if (!skb) {
+			FM_ERR_REPORT("cg2900_fm_driver_callback: "
+					"Unable to Allocate Memory");
+			return;
+		}
+		skb->data[0] = CG2900_EVENT_SCAN_CANCELLED;
+		skb->data[1] = event_successful;
+		skb_queue_tail(&fm_interrupt_queue, skb);
+		wake_up_poll_queue();
+		break;
+	case FMD_EVENT_SEEK_COMPLETED:
+		FM_DEBUG_REPORT("FMD_EVENT_SEEK_COMPLETED");
+		cg2900_fm_check_rds_status();
+		skb = alloc_skb(SKB_FM_INTERRUPT_DATA,
+			GFP_KERNEL);
+		if (!skb) {
+			FM_ERR_REPORT("cg2900_fm_driver_callback: "
+					"Unable to Allocate Memory");
+			return;
+		}
+		skb->data[0] = CG2900_EVENT_SEARCH_CHANNEL_FOUND;
+		skb->data[1] = event_successful;
+		skb_queue_tail(&fm_interrupt_queue, skb);
+		wake_up_poll_queue();
+		break;
+	case FMD_EVENT_SCAN_BAND_COMPLETED:
+		FM_DEBUG_REPORT("FMD_EVENT_SCAN_BAND_COMPLETED");
+		cg2900_fm_check_rds_status();
+		skb = alloc_skb(SKB_FM_INTERRUPT_DATA,
+			GFP_KERNEL);
+		if (!skb) {
+			FM_ERR_REPORT("cg2900_fm_driver_callback: "
+					"Unable to Allocate Memory");
+			return;
+		}
+		skb->data[0] = CG2900_EVENT_SCAN_CHANNELS_FOUND;
+		skb->data[1] = event_successful;
+		skb_queue_tail(&fm_interrupt_queue, skb);
+		wake_up_poll_queue();
+		break;
+	case FMD_EVENT_BLOCK_SCAN_COMPLETED:
+		FM_DEBUG_REPORT("FMD_EVENT_BLOCK_SCAN_COMPLETED");
+		cg2900_fm_check_rds_status();
+		skb = alloc_skb(SKB_FM_INTERRUPT_DATA,
+			GFP_KERNEL);
+		if (!skb) {
+			FM_ERR_REPORT("cg2900_fm_driver_callback: "
+					"Unable to Allocate Memory");
+			return;
+		}
+		skb->data[0] = CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND;
+		skb->data[1] = event_successful;
+		skb_queue_tail(&fm_interrupt_queue, skb);
+		wake_up_poll_queue();
+		break;
+	case FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE:
+		FM_DEBUG_REPORT("FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE");
+		break;
+	case FMD_EVENT_RDSGROUP_RCVD:
+		FM_DEBUG_REPORT("FMD_EVENT_RDSGROUP_RCVD");
+		/*
+		* Release the rds semaphore, poll queue
+		* will be woken-up in rds callback
+		*/
+		fmd_set_rds_sem();
+		break;
+	case FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE:
+		FM_ERR_REPORT(
+			"FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE");
+		skb = alloc_skb(SKB_FM_INTERRUPT_DATA,
+			GFP_KERNEL);
+		if (!skb) {
+			FM_ERR_REPORT("cg2900_fm_driver_callback: "
+					"Unable to Allocate Memory");
+			return;
+		}
+		skb->data[0] = CG2900_EVENT_MONO_STEREO_TRANSITION;
+		skb->data[1] = event_successful;
+		skb_queue_tail(&fm_interrupt_queue, skb);
+		wake_up_poll_queue();
+		break;
+	default:
+		FM_INFO_REPORT("cg2900_fm_driver_callback: "
+			      "Unknown event = %x", event);
+		break;
+	}
+}
+
+/**
+ * cg2900_fm_rds_callback()- Function to retrieve the RDS groups.
+ *
+ * This is called when the chip has received enough RDS groups
+ * so an interrupt irpt_BufferFull is generated to read the groups.
+ */
+static void cg2900_fm_rds_callback(void)
+{
+	u8 index = 0;
+	u16 rds_local_buf_count;
+	int result;
+	struct sk_buff *skb;
+
+	FM_INFO_REPORT("cg2900_fm_rds_callback");
+
+	/*
+	 * Wait till interrupt is RDS Buffer
+	 * full interrupt is received
+	 */
+	fmd_get_rds_sem();
+
+	if (!fm_rds_status)
+		return;
+
+	/* RDS Data available, Read the Groups */
+	mutex_lock(&rds_mutex);
+	result = fmd_int_bufferfull(&rds_local_buf_count);
+
+	if (0 != result)
+		goto error;
+
+	while (index < rds_local_buf_count) {
+		/*
+		 * Status are in reverse order because of Endianness
+		 * of status byte received from chip
+		 */
+		result = fmd_rx_get_low_level_rds_groups(
+			index,
+			&fm_rds_buf[fm_rds_info.rds_head][index].block1,
+			&fm_rds_buf[fm_rds_info.rds_head][index].block2,
+			&fm_rds_buf[fm_rds_info.rds_head][index].block3,
+			&fm_rds_buf[fm_rds_info.rds_head][index].block4,
+			&fm_rds_buf[fm_rds_info.rds_head][index].status2,
+			&fm_rds_buf[fm_rds_info.rds_head][index].status1,
+			&fm_rds_buf[fm_rds_info.rds_head][index].status4,
+			&fm_rds_buf[fm_rds_info.rds_head][index].status3);
+		FM_INFO_REPORT("%04x %04x %04x %04x %02x %02x %02x %02x",
+			fm_rds_buf[fm_rds_info.rds_head][index].block1,
+			fm_rds_buf[fm_rds_info.rds_head][index].block2,
+			fm_rds_buf[fm_rds_info.rds_head][index].block3,
+			fm_rds_buf[fm_rds_info.rds_head][index].block4,
+			fm_rds_buf[fm_rds_info.rds_head][index].status1,
+			fm_rds_buf[fm_rds_info.rds_head][index].status2,
+			fm_rds_buf[fm_rds_info.rds_head][index].status3,
+			fm_rds_buf[fm_rds_info.rds_head][index].status4);
+
+		if (0 != result)
+
+			goto error;
+
+		if (!fm_rds_status)
+			return;
+
+		index++;
+	}
+	fm_rds_info.rds_head++;
+	if (fm_rds_info.rds_head == MAX_RDS_BUFFER)
+		fm_rds_info.rds_head = 0;
+
+	/* Queue the RDS event */
+	skb = alloc_skb(SKB_FM_INTERRUPT_DATA,
+		GFP_KERNEL);
+	if (!skb) {
+		FM_ERR_REPORT("cg2900_fm_rds_callback: "
+				"Unable to Allocate Memory");
+		goto error;
+	}
+	skb->data[0] = CG2900_EVENT_RDS_EVENT;
+	skb->data[1] = true;
+	skb_queue_tail(&fm_interrupt_queue, skb);
+
+	/* Wake up the poll queue */
+	wake_up_poll_queue();
+error:
+	mutex_unlock(&rds_mutex);
+}
+
+int cg2900_fm_init(void)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_init");
+
+	if (CG2900_FM_STATE_DEINITIALIZED != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_init: Already Initialized");
+		result = -EINVAL;
+		goto error;
+	}
+
+	mutex_init(&rds_mutex);
+
+	memset(&fm_rds_info, 0, sizeof(struct cg2900_fm_rds_info));
+	memset(&version_info, 0, sizeof(struct cg2900_version_info));
+	memset(
+			fm_rds_buf,
+			0,
+			sizeof(struct cg2900_fm_rds_buf) *
+			MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+
+	/* Initalize the Driver */
+	if (fmd_init() != 0) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Register the callback */
+	if (fmd_register_callback(
+		(fmd_radio_cb) cg2900_fm_driver_callback) != 0) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* initialize global variables */
+	fm_event = CG2900_EVENT_NO_EVENT;
+	fm_state = CG2900_FM_STATE_INITIALIZED;
+	fm_mode = CG2900_FM_IDLE_MODE;
+	fm_prev_rds_status = false;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_init: returning %d",
+			result);
+	return result;
+
+}
+
+int cg2900_fm_deinit(void)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_deinit");
+
+	if (CG2900_FM_STATE_INITIALIZED != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_deinit: Already de-Initialized");
+		result = -EINVAL;
+		goto error;
+	}
+	fmd_exit();
+	mutex_destroy(&rds_mutex);
+	fm_state = CG2900_FM_STATE_DEINITIALIZED;
+	fm_mode = CG2900_FM_IDLE_MODE;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_deinit: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_switch_on(
+			struct device *device
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_switch_on");
+
+	if (CG2900_FM_STATE_INITIALIZED != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Enable FM IP */
+	FM_DEBUG_REPORT("cg2900_fm_switch_on: " "Sending FM IP Enable");
+
+	if (fmd_send_fm_ip_enable()) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			      "Error in fmd_send_fm_ip_enable");
+		result = -EINVAL;
+		goto error;
+	}
+
+	if (version_info.revision == CG2910_PG1_REV
+			|| version_info.revision == CG2910_PG2_REV
+			|| version_info.revision == CG2905_PG1_1_REV) {
+		/* Now Download CG2910 lut and program Firmware files */
+		if (cg2910_fm_load_firmware(device) != 0) {
+			FM_ERR_REPORT("cg2900_fm_switch_on: "
+				"Error in downloading firmware for CG2910/05");
+			result = -EINVAL;
+			goto error;
+		}
+	} else if (version_info.revision == CG2900_PG1_REV
+			|| version_info.revision == CG2900_PG2_REV
+			|| version_info.revision == CG2900_PG1_SPECIAL_REV) {
+		/* Now Download the Coefficient Files and FM Firmware */
+		if (cg2900_fm_load_firmware(device) != 0) {
+			FM_ERR_REPORT("cg2900_fm_switch_on: "
+					"Error in downloading firmware for CG2900");
+			result = -EINVAL;
+			goto error;
+		}
+	} else {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+				"Unsupported Chip revision");
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Power up FM */
+	result = fmd_power_up();
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Switch Mode To Idle */
+	result = fmd_set_mode(FMD_MODE_IDLE);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			"fmd_set_mode failed %x",
+			(unsigned int)result);
+		result =  -EINVAL;
+		goto error;
+	}
+
+	fm_state = CG2900_FM_STATE_SWITCHED_ON;
+	fm_mode = CG2900_FM_IDLE_MODE;
+	memset(&fm_rds_info, 0,
+		sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+		sizeof(struct cg2900_fm_rds_buf) *
+		MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_switch_on: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_switch_off(void)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_switch_off");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state &&
+		CG2900_FM_STATE_STAND_BY != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_switch_off: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Stop the RDS Thread if it is running */
+	if (fm_rds_status) {
+		fm_rds_status = false;
+		fmd_stop_rds_thread();
+	}
+	if (CG2900_FM_STATE_STAND_BY == fm_state) {
+		/* Power up FM */
+		result = fmd_power_up();
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_switch_off: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		} else
+			fm_state = CG2900_FM_STATE_SWITCHED_ON;
+	}
+	if (fmd_send_fm_ip_disable()) {
+		FM_ERR_REPORT("cg2900_fm_switch_off: "
+			      "Problem in fmd_send_fm_ip_"
+			      "disable");
+		result = -EINVAL;
+		goto error;
+	}
+	if (0 == result) {
+		fm_state = CG2900_FM_STATE_INITIALIZED;
+		fm_mode = CG2900_FM_IDLE_MODE;
+		memset(&fm_rds_info, 0,
+			sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+			sizeof(struct cg2900_fm_rds_buf) *
+			MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+		/* Remove all Interrupts from the queue */
+		skb_queue_purge(&fm_interrupt_queue);
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_switch_off: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_standby(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_standby");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_standby: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	memset(&fm_rds_info, 0,
+		sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+		sizeof(struct cg2900_fm_rds_buf) *
+		MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	/* Remove all Interrupts from the queue */
+	skb_queue_purge(&fm_interrupt_queue);
+	result = fmd_goto_standby();
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_standby: "
+			      "FMLGotoStandby failed, "
+			      "err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	fm_state = CG2900_FM_STATE_STAND_BY;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_standby: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_power_up_from_standby(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_power_up_from_standby");
+
+	if (CG2900_FM_STATE_STAND_BY != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_power_up_from_standby: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Power up FM */
+	result = fmd_power_up();
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_power_up_from_standby: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	} else {
+		fm_state = CG2900_FM_STATE_SWITCHED_ON;
+		if (CG2900_FM_TX_MODE == fm_mode) {
+			/* Enable the PA */
+			result = fmd_tx_set_pa(true);
+			if (0 != result) {
+				FM_ERR_REPORT
+				    ("cg2900_fm_power_up_from_standby:"
+				     " fmd_tx_set_pa " "failed %d",
+				     (unsigned int)result);
+				result = -EINVAL;
+				goto error;
+			}
+		}
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_power_up_from_standby: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_rx_default_settings(
+			u32 freq,
+			u8 band,
+			u8 grid,
+			bool enable_rds,
+			bool enable_stereo
+			)
+{
+	int result;
+	u8 vol_in_percentage;
+
+	FM_INFO_REPORT("cg2900_fm_set_rx_default_settings: freq = %d Hz, "
+		       "band = %d, grid = %d, RDS = %d, Stereo Mode = %d",
+		       freq, band, grid, enable_rds, enable_stereo);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state &&
+		CG2900_FM_STATE_STAND_BY != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_STATE_STAND_BY == fm_state) {
+		/* Power up FM */
+		result = fmd_power_up();
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		} else
+			fm_state = CG2900_FM_STATE_SWITCHED_ON;
+	}
+	fm_mode = CG2900_FM_RX_MODE;
+
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending Set mode to Rx");
+	result = fmd_set_mode(FMD_MODE_RX);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_set_mode failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Grid */
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending fmd_rx_set_grid ");
+	result = fmd_rx_set_grid(grid);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_rx_set_grid failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Band */
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending Set fmd_set_freq_range");
+	result = fmd_set_freq_range(band);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_set_freq_range failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Frequency */
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending Set fmd_rx_set_frequency");
+	result = fmd_rx_set_frequency(
+			freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_rx_set_frequency failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"SetFrequency interrupt received, "
+			"Sending Set fmd_rx_set_stereo_mode");
+
+	if (enable_stereo) {
+		/* Set the Stereo Blending mode */
+		result = fmd_rx_set_stereo_mode(
+			FMD_STEREOMODE_BLENDING);
+	} else {
+		/* Set the Mono mode */
+		result = fmd_rx_set_stereo_mode(
+			FMD_STEREOMODE_MONO);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "fmd_rx_set_stereo_mode "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	if (enable_stereo) {
+		/* Set the Stereo Blending RSSI control */
+		result = fmd_rx_set_stereo_ctrl_blending_rssi(
+			STEREO_BLENDING_MIN_RSSI,
+			STEREO_BLENDING_MAX_RSSI);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			"fmd_rx_set_stereo_ctrl_blending_rssi "
+			"failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set RDS Group rejection Off */
+	result = fmd_rx_set_rds_group_rejection(
+		FMD_RDS_GROUP_REJECTION_OFF);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			"fmd_rx_set_rds_group_rejection "
+			"failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Remove all Interrupt from the queue */
+	skb_queue_purge(&fm_interrupt_queue);
+
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: "
+			"Sending Set rds");
+
+	if (enable_rds) {
+		/* Enable RDS */
+		a_b_flag = false;
+		result = cg2900_fm_rds_on();
+	} else {
+		/* Disable RDS */
+		result = cg2900_fm_rds_off();
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rx_default_settings: "
+			      "cg2900_fm_rds_on "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Currently, not supported for CG2905/10 */
+	if (version_info.revision == CG2900_PG1_REV
+			|| version_info.revision == CG2900_PG2_REV
+			|| version_info.revision == CG2900_PG1_SPECIAL_REV) {
+		/* Set the Analog Out Volume to Max */
+		vol_in_percentage = (u8)
+			(((u16) (MAX_ANALOG_VOLUME) * 100)
+			/ MAX_ANALOG_VOLUME);
+		result = fmd_set_volume(vol_in_percentage);
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_switch_on: "
+					"FMRSetVolume failed %x",
+					(unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		}
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_rx_default_settings: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_tx_default_settings(
+			u32 freq,
+			u8 band,
+			u8 grid,
+			bool enable_rds,
+			bool enable_stereo
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_tx_default_settings: freq = %d Hz, "
+		       "band = %d, grid = %d, RDS = %d, Stereo Mode = %d",
+		       freq, band, grid, enable_rds, enable_stereo);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state &&
+		CG2900_FM_STATE_STAND_BY != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_STATE_STAND_BY == fm_state) {
+		/* Power up FM */
+		result = fmd_power_up();
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_power_up failed %x",
+			      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		} else
+			fm_state = CG2900_FM_STATE_SWITCHED_ON;
+	}
+	fm_mode = CG2900_FM_TX_MODE;
+	if (fm_rds_status) {
+		fm_rds_status = false;
+		fmd_stop_rds_thread();
+		memset(&fm_rds_info, 0,
+			sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+			sizeof(struct cg2900_fm_rds_buf) *
+			MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+		/* Give 50 ms delay to exit the RDS thread */
+		schedule_timeout_interruptible(msecs_to_jiffies(50));
+	}
+	/* Remove all Interrupt from the queue */
+	skb_queue_purge(&fm_interrupt_queue);
+
+	/* Switch To Tx mode */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending Set mode to Tx");
+	result = fmd_set_mode(FMD_MODE_TX);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_set_mode failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Sets the Limiter Values */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending fmd_limiter_setcontrol");
+	result = fmd_limiter_setcontrol(
+				DEFAULT_AUDIO_DEVIATION,
+				DEFAULT_NOTIFICATION_HOLD_OFF_TIME);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_limiter_setcontrol failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Grid */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending fmd_tx_set_grid ");
+	result = fmd_tx_set_grid(grid);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_set_grid failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Band */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending fmd_tx_set_freq_range");
+	result = fmd_tx_set_freq_range(band);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_set_freq_range failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Band */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending fmd_tx_set_preemphasis");
+	result = fmd_tx_set_preemphasis(FMD_EMPHASIS_75US);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			      "fmd_tx_set_preemphasis failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Set the Frequency */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending Set fmd_tx_set_frequency");
+	result = fmd_tx_set_frequency(
+				freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_switch_on: "
+			      "fmd_tx_set_frequency failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"SetFrequency interrupt received, "
+			"Sending Set fmd_tx_enable_stereo_mode");
+
+	/* Set the Stereo mode */
+	result = fmd_tx_enable_stereo_mode(enable_stereo);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_enable_stereo_mode "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending Set fmd_tx_set_pa");
+
+	/* Enable the PA */
+	result = fmd_tx_set_pa(true);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_set_pa "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"set PA interrupt received, "
+			"Sending Set fmd_tx_set_signal_strength");
+
+	/* Set the Signal Strength to Max */
+	result = fmd_tx_set_signal_strength(
+				MAX_POWER_LEVEL);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "fmd_tx_set_signal_strength "
+			      "failed %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Enable Tx RDS  */
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: "
+			"Sending Set cg2900_fm_tx_rds");
+	result = cg2900_fm_tx_rds(enable_rds);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_tx_default_settings: "
+			      "cg2900_fm_tx_rds "
+			      "failed %x", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_tx_default_settings: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_grid(
+			u8 grid
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_grid: Grid = %d", grid);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_grid: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_set_grid(grid);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_grid: "
+			      "fmd_rx_set_grid failed");
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_grid: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_band(
+			u8 band
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_band: Band = %d", band);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_band: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_set_freq_range(band);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_band: "
+			      "fmd_set_freq_range failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_band: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_search_up_freq(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_search_up_freq");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_search_up_freq: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (fm_rds_status) {
+		/* Stop RDS if it is active */
+		result = cg2900_fm_rds_off();
+		fm_prev_rds_status = true;
+	} else {
+		memset(&fm_rds_info, 0,
+			sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+			sizeof(struct cg2900_fm_rds_buf) *
+			MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+		/* Remove all Interrupts from the queue */
+		skb_queue_purge(&fm_interrupt_queue);
+	}
+	result = fmd_rx_seek(CG2900_DIR_UP);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_search_up_freq: "
+			      "Error Code %d", (unsigned int)result);
+		cg2900_fm_check_rds_status();
+		result = -EINVAL;
+		goto error;
+	}
+	result = 0;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_search_up_freq: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_search_down_freq(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_search_down_freq");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_search_down_freq: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (fm_rds_status) {
+		/* Stop RDS if it is active */
+		result = cg2900_fm_rds_off();
+		fm_prev_rds_status = true;
+	} else {
+		memset(&fm_rds_info, 0,
+			sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+			sizeof(struct cg2900_fm_rds_buf) *
+			MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+		/* Remove all Interrupts from the queue */
+		skb_queue_purge(&fm_interrupt_queue);
+	}
+	result = fmd_rx_seek(CG2900_DIR_DOWN);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_search_down_freq: "
+			      "Error Code %d", (unsigned int)result);
+		cg2900_fm_check_rds_status();
+		result = -EINVAL;
+		goto error;
+	}
+	result = 0;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_search_down_freq: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_start_band_scan(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_start_band_scan");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_start_band_scan: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (fm_rds_status) {
+		/* Stop RDS if it is active */
+		result = cg2900_fm_rds_off();
+		fm_prev_rds_status = true;
+	} else {
+		memset(&fm_rds_info, 0,
+			sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+			sizeof(struct cg2900_fm_rds_buf) *
+			MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+		/* Remove all Interrupts from the queue */
+		skb_queue_purge(&fm_interrupt_queue);
+	}
+	result = fmd_rx_scan_band(DEFAULT_CHANNELS_TO_SCAN);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_start_band_scan: "
+			      "Error Code %d", (unsigned int)result);
+		cg2900_fm_check_rds_status();
+		result = -EINVAL;
+		goto error;
+	}
+	result = 0;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_start_band_scan: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_stop_scan(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_stop_scan");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_stop_scan: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_stop_seeking();
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_stop_scan: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	memset(&fm_rds_info, 0,
+	       sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+	       sizeof(struct cg2900_fm_rds_buf) *
+	       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	result = 0;
+	if (fm_prev_rds_status) {
+		/* Restart RDS if it was active earlier */
+		cg2900_fm_rds_on();
+		fm_prev_rds_status = false;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_stop_scan: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_scan_result(
+			u16 *num_of_scanfreq,
+			u32 *scan_freq,
+			u32 *scan_freq_rssi_level
+			)
+{
+	int result;
+	u32 cnt;
+	u32 index;
+	u32 minfreq;
+	u32 maxfreq;
+	u16 channels[3];
+	u16 rssi[3];
+	u8 freq_range;
+	u8 max_channels = 0;
+
+	FM_INFO_REPORT("cg2900_fm_get_scan_result");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_scan_result: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_get_freq_range(&freq_range);
+
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_get_freq_range_properties(
+				freq_range,
+				&minfreq,
+				&maxfreq);
+
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_rx_get_max_channels_to_scan(&max_channels);
+
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* In 1 iteration we can retreive max 3 channels */
+	cnt = (max_channels / 3) + 1;
+	while ((cnt--) && (result == 0)) {
+		/*
+		 * Get all channels, including empty ones.
+		 * In 1 iteration at max 3 channels can be found.
+		 */
+		result = fmd_rx_get_scan_band_info(cnt * 3,
+						   num_of_scanfreq,
+						   channels, rssi);
+		if (0 == result) {
+			index = cnt * 3;
+			/* Convert Freq to Hz from channel number */
+			scan_freq[index] = (minfreq +
+					channels[0] *
+					CHANNEL_FREQ_CONVERTER_MHZ) *
+					FREQUENCY_CONVERTOR_KHZ_HZ;
+			scan_freq_rssi_level[index] = rssi[0];
+			/* Convert Freq to Hz from channel number */
+			scan_freq[index + 1] = (minfreq +
+						channels[1] *
+						CHANNEL_FREQ_CONVERTER_MHZ) *
+						FREQUENCY_CONVERTOR_KHZ_HZ;
+			scan_freq_rssi_level[index + 1] = rssi[1];
+			/* Check if we donot overwrite the array */
+			if (cnt < (max_channels / 3)) {
+				/* Convert Freq to Hz from channel number */
+				scan_freq[index + 2] = (minfreq +
+					channels[2] *
+					CHANNEL_FREQ_CONVERTER_MHZ) *
+					FREQUENCY_CONVERTOR_KHZ_HZ;
+				scan_freq_rssi_level[index + 2]
+				    = rssi[2];
+			}
+		}
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_scan_result: returning %d",
+			result);
+	return result;
+
+}
+
+int cg2900_fm_start_block_scan(
+			u32 start_freq,
+			u32 end_freq
+			)
+{
+	int result;
+	u8 antenna;
+
+	FM_INFO_REPORT("cg2900_fm_start_block_scan");
+
+	FM_DEBUG_REPORT("cg2900_fm_start_block_scan: Start Freq = %d, "
+		"End Freq = %d", start_freq, end_freq);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_start_block_scan: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (fm_rds_status) {
+		/* Stop RDS if it is active */
+		result = cg2900_fm_rds_off();
+		fm_prev_rds_status = true;
+	} else {
+		memset(&fm_rds_info, 0,
+			sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+			sizeof(struct cg2900_fm_rds_buf) *
+			MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+		/* Remove all Interrupts from the queue */
+		skb_queue_purge(&fm_interrupt_queue);
+	}
+	result = fmd_get_antenna(
+			&antenna);
+	result = fmd_block_scan(
+			start_freq/FREQUENCY_CONVERTOR_KHZ_HZ,
+			end_freq/FREQUENCY_CONVERTOR_KHZ_HZ,
+			antenna);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_start_block_scan: "
+			"Error Code %d", (unsigned int)result);
+		cg2900_fm_check_rds_status();
+		result = -EINVAL;
+		goto error;
+	}
+	result = 0;
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_start_block_scan: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_block_scan_result(
+			u16 *num_of_scanchan,
+			u16 *scan_freq_rssi_level
+			)
+{
+	int result = 0;
+	u32 cnt;
+	u32 index;
+	u16 rssi[6];
+
+	FM_INFO_REPORT("cg2900_fm_get_block_scan_result");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_block_scan_result: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	cnt = 33;
+	while ((cnt--) && (result == 0)) {
+		/* Get all channels, including empty ones */
+		result = fmd_get_block_scan_result(
+						      cnt * 6,
+						      num_of_scanchan,
+						      rssi);
+		if (0 == result) {
+			index = cnt * 6;
+			scan_freq_rssi_level[index]
+			    = rssi[0];
+			scan_freq_rssi_level[index + 1]
+			    = rssi[1];
+			scan_freq_rssi_level[index + 2]
+			    = rssi[2];
+			scan_freq_rssi_level[index + 3]
+			    = rssi[3];
+			scan_freq_rssi_level[index + 4]
+			    = rssi[4];
+			scan_freq_rssi_level[index + 5]
+			    = rssi[5];
+		}
+	}
+	if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_block_scan_result:"
+				" Sending Set fmd_tx_set_pa");
+
+		/* Enable the PA */
+		result = fmd_tx_set_pa(true);
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_get_block_scan_result:"
+				      " fmd_tx_set_pa "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		}
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_block_scan_result: returning %d",
+			result);
+	return result;
+
+}
+
+int cg2900_fm_tx_rds(
+			bool enable_rds
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_rds: enable_rds = %d", enable_rds);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_rds: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (enable_rds) {
+		/* Set the Tx Buffer Size */
+		result = fmd_tx_buffer_set_size(
+						MAX_RDS_GROUPS - 2);
+	if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_tx_rds: "
+				      "fmd_tx_buffer_set_size "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		} else {
+			result = fmd_tx_set_rds(true);
+		}
+
+	if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_tx_rds: "
+				      "fmd_tx_set_rds "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		}
+		program_identification_code =
+		    default_program_identification_code;
+		program_type_code = default_program_type_code;
+		memcpy(program_service,
+			    default_program_service,
+			    MAX_PSN_SIZE);
+		memcpy(radio_text,
+			    default_radio_text, MAX_RT_SIZE);
+		radio_text[strlen(radio_text)] = 0x0D;
+		cg2900_fm_transmit_rds_groups();
+		result = 0;
+	} else {
+		result = fmd_tx_set_rds(false);
+
+	if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_tx_rds: "
+				      "fmd_tx_set_rds "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		}
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_rds: returning %d",
+			result);
+
+	return result;
+}
+
+int cg2900_fm_tx_set_pi_code(
+			u16 pi_code
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_pi_code: PI = %04x", pi_code);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pi_code: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	program_identification_code = pi_code;
+	result = cg2900_fm_transmit_rds_groups();
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_pi_code: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_pty_code(
+			u16 pty_code
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_pty_code: PTY = %04x", pty_code);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pty_code: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	program_type_code = pty_code;
+	result = cg2900_fm_transmit_rds_groups();
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_pty_code: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_program_station_name(
+			char *psn,
+			u8 len
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_program_station_name: PSN = %s",
+			psn);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_program_station_name: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (len < (MAX_PSN_SIZE - 1)) {
+		int count = len;
+		while (count < (MAX_PSN_SIZE - 1))
+			psn[count++] = ' ';
+	}
+	memcpy(program_service, psn, MAX_PSN_SIZE);
+	result = cg2900_fm_transmit_rds_groups();
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_program_station_name: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_radio_text(
+			char *rt,
+			u8 len
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_radio_text: RT = %s", rt);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_radio_text: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	rt[len] = 0x0D;
+	memcpy(radio_text, rt, len + 1);
+
+	result = cg2900_fm_transmit_rds_groups();
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_radio_text: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_get_rds_deviation(
+			u16 *deviation
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_rds_deviation");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_rds_deviation: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_rds_deviation(deviation);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_rds_deviation: "
+			      "fmd_tx_get_rds_deviation failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_rds_deviation: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_rds_deviation(
+			u16 deviation
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_rds_deviation: deviation = %d",
+		       deviation);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_rds_deviation: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_set_rds_deviation(deviation);
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_tx_set_rds_deviation: "
+			      "fmd_tx_set_rds_deviation failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_rds_deviation: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_get_pilot_tone_status(
+			bool *enable
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_pilot_tone_status");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_pilot_tone_status: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_stereo_mode(enable);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_pilot_tone_status: "
+			"fmd_tx_get_stereo_mode failed %d",
+			result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_pilot_tone_status: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_pilot_tone_status(
+			bool enable
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_pilot_tone_status: enable = %d",
+			enable);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pilot_tone_status: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_enable_stereo_mode(enable);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pilot_tone_status: "
+			      "fmd_tx_enable_stereo_mode failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_pilot_tone_status: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_get_pilot_deviation(
+			u16 *deviation
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_pilot_deviation");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_pilot_deviation: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_pilot_deviation(deviation);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_pilot_deviation: "
+			      "fmd_tx_get_pilot_deviation failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_pilot_deviation: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_pilot_deviation(
+			u16 deviation
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_pilot_deviation: deviation = %d",
+		       deviation);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pilot_deviation: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_set_pilot_deviation(deviation);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_pilot_deviation: "
+			      "fmd_tx_set_pilot_deviation failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_pilot_deviation: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_get_preemphasis(
+			u8 *preemphasis
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_preemphasis");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_preemphasis: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_preemphasis(preemphasis);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_preemphasis: "
+			      "fmd_tx_get_preemphasis failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_preemphasis: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_preemphasis(
+			u8 preemphasis
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_preemphasis: preemphasis = %d",
+		       preemphasis);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_preemphasis: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_set_preemphasis(preemphasis);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_preemphasis: "
+			      "fmd_tx_set_preemphasis failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_preemphasis: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_rx_set_deemphasis(
+			u8 deemphasis
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_rx_set_deemphasis: deemphasis = %02x",
+		deemphasis);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_rx_set_deemphasis: "
+			"Invalid state of FM Driver = %d", fm_state);
+			result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_set_deemphasis(deemphasis);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rx_set_deemphasis: "
+			"fmd_rx_set_deemphasis failed %d",
+			result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_rx_set_deemphasis: returning %d", result);
+	return result;
+}
+
+int cg2900_fm_tx_get_power_level(
+			u16 *power_level
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_get_power_level");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_power_level: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_get_signal_strength(power_level);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_get_power_level: "
+			      "fmd_tx_get_signal_strength failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_get_power_level: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_tx_set_power_level(
+			u16 power_level
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_tx_set_power_level: power_level = %d",
+		       power_level);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_power_level: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_tx_set_signal_strength(power_level);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_tx_set_power_level: "
+			      "fmd_tx_set_preemphasis failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_tx_set_power_level: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_audio_balance(
+			s8 balance
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_audio_balance, balance = %d", balance);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_audio_balance: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_set_balance(balance);
+	if (0 != result) {
+		FM_ERR_REPORT("FMRSetAudioBalance : "
+			      "Failed in fmd_set_balance, err = %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_audio_balance: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_volume(
+			u8 vol_level
+			)
+{
+	int result;
+	u8 vol_in_percentage;
+
+	FM_INFO_REPORT("cg2900_fm_set_volume: Volume Level = %d", vol_level);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_volume: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	vol_in_percentage =
+	    (u8) (((u16) (vol_level) * 100) / MAX_ANALOG_VOLUME);
+	result = fmd_set_volume(vol_in_percentage);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_increase_volume: "
+			      "FMRSetVolume failed, err = %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_volume: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_volume(
+			u8 *vol_level
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_get_volume");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_volume: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*vol_level = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_get_volume(vol_level);
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_volume: returning %d, VolLevel = %d",
+			result, *vol_level);
+	return result;
+}
+
+int cg2900_fm_rds_off(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_rds_off");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_rds_off: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	memset(&fm_rds_info, 0,
+	       sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+	       sizeof(struct cg2900_fm_rds_buf) *
+	       MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	result = fmd_rx_set_rds(FMD_SWITCH_OFF_RDS);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rds_off: fmd_rx_set_rds failed, "
+			"err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	if (fm_rds_status) {
+		/* Stop the RDS Thread */
+		FM_DEBUG_REPORT("cg2900_fm_rds_off: "
+				"Stopping RDS Thread");
+		fmd_stop_rds_thread();
+		fm_rds_status = false;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_rds_off: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_rds_on(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_rds_on");
+	if (fm_rds_status) {
+		result = 0;
+		FM_DEBUG_REPORT("cg2900_fm_rds_on: rds is on "
+			"return result = %d", result);
+		return result;
+	}
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_rds_on: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	FM_DEBUG_REPORT("cg2900_fm_rds_on:"
+			" Sending fmd_rx_buffer_set_size");
+	result = fmd_rx_buffer_set_size(MAX_RDS_GROUPS);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rds_on: fmd_rx_buffer_set_size"
+			"failed, err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	FM_DEBUG_REPORT("cg2900_fm_rds_on: Sending "
+			"fmd_rx_buffer_set_threshold");
+	result = fmd_rx_buffer_set_threshold(MAX_RDS_GROUPS - 1);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rds_on: fmd_rx_buffer_set_threshold "
+			"failed, err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	FM_DEBUG_REPORT("cg2900_fm_rds_on: Sending fmd_rx_set_rds");
+	result = fmd_rx_set_rds(FMD_SWITCH_ON_RDS_ENHANCED_MODE);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_rds_on: fmd_rx_set_rds failed, "
+			"err = %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Start the RDS Thread to read the RDS Buffers */
+	fm_rds_status = true;
+	memset(&fm_rds_info, 0,
+		sizeof(struct cg2900_fm_rds_info));
+	memset(fm_rds_buf, 0,
+		sizeof(struct cg2900_fm_rds_buf) *
+		MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+	fmd_start_rds_thread(cg2900_fm_rds_callback);
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_rds_on: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_rds_status(
+			bool *rds_status
+			)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_get_rds_status");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_rds_status: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_rds_status: "
+				"fmd_rx_get_rds");
+		result = fmd_rx_get_rds(rds_status);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_rds_status: "
+				"fmd_tx_get_rds");
+		result = fmd_tx_get_rds(rds_status);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_rds_status: "
+			      "fmd_get_rds failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_rds_status: returning %d, "
+			"rds_status = %d", result,
+			*rds_status);
+	return result;
+}
+
+int cg2900_fm_mute(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_mute");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_mute: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+
+	/* Mute Analog DAC */
+	result = fmd_set_mute(true);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_mute: "
+			      "fmd_set_mute failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Mute Ext Src */
+	result = fmd_ext_set_mute(true);
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_mute: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_unmute(void)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_unmute");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_unmute: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Unmute Analog DAC */
+	result = fmd_set_mute(false);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_mute: "
+			      "fmd_set_mute failed %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Unmute Ext Src */
+	result = fmd_ext_set_mute(false);
+	if (0 != result) {
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_unmute: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_frequency(
+			u32 *freq
+			)
+{
+	int result = 0;
+	u32 currentFreq = 0;
+
+	FM_INFO_REPORT("cg2900_fm_get_frequency");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_frequency: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*freq = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_frequency: "
+				"fmd_rx_get_frequency");
+		result = fmd_rx_get_frequency(
+					      (u32 *) &currentFreq);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_frequency: "
+				"fmd_tx_get_frequency");
+		result = fmd_tx_get_frequency(
+					      (u32 *) &currentFreq);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_frequency: "
+			      "fmd_rx_get_frequency failed %d",
+			      (unsigned int)result);
+		*freq = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	/* Convert To Hz */
+	*freq = currentFreq * FREQUENCY_CONVERTOR_KHZ_HZ;
+	FM_DEBUG_REPORT("cg2900_fm_get_frequency: "
+			"Current Frequency = %d Hz", *freq);
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_frequency: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_frequency(
+			u32 new_freq
+			)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_set_frequency, new_freq = %d",
+			(int)new_freq);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_frequency: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	/* Check if RDS needs to be disabled before Setting Frequency */
+	if (fm_rds_status) {
+		/* Stop RDS if it is active */
+		result = cg2900_fm_rds_off();
+		fm_prev_rds_status = true;
+	} else {
+		memset(&fm_rds_info, 0,
+			sizeof(struct cg2900_fm_rds_info));
+		memset(fm_rds_buf, 0,
+			sizeof(struct cg2900_fm_rds_buf) *
+			MAX_RDS_BUFFER * MAX_RDS_GROUPS);
+		/* Remove all Interrupts from the queue */
+		skb_queue_purge(&fm_interrupt_queue);
+	}
+
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_frequency: "
+				"fmd_rx_set_frequency");
+		result = fmd_rx_set_frequency(
+				new_freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_frequency: "
+				"fmd_tx_set_frequency");
+		result = fmd_tx_set_frequency(
+				new_freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	}
+	if (fm_prev_rds_status) {
+		/* Restart RDS if it was active earlier */
+		cg2900_fm_rds_on();
+		fm_prev_rds_status = false;
+	}
+	if (result != 0) {
+		FM_ERR_REPORT("cg2900_fm_set_frequency: "
+			      "fmd_rx_set_frequency failed %x",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+	if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_frequency:"
+				" Sending Set" "fmd_tx_set_pa");
+
+		/* Enable the PA */
+		result = fmd_tx_set_pa(true);
+		if (0 != result) {
+			FM_ERR_REPORT("cg2900_fm_set_frequency:"
+				      " fmd_tx_set_pa "
+				      "failed %d",
+				      (unsigned int)result);
+			result = -EINVAL;
+			goto error;
+		}
+		result = 0;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_frequency: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_signal_strength(
+			u16 *signal_strength
+			)
+{
+	int result = 0;
+
+	FM_INFO_REPORT("cg2900_fm_get_signal_strength");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_signal_strength: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*signal_strength = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_signal_strength: "
+				"fmd_rx_get_signal_strength");
+		result = fmd_rx_get_signal_strength(
+						    signal_strength);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_signal_strength: "
+				"fmd_tx_get_signal_strength");
+		result = fmd_tx_get_signal_strength(
+						    signal_strength);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_signal_strength: "
+			      "Error Code %d", (unsigned int)result);
+		*signal_strength = 0;
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_signal_strength: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_af_update_get_result(
+			u16 *af_update_rssi
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_af_update_get_result");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_af_update_get_result: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*af_update_rssi = 0;
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_get_af_update_result(af_update_rssi);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_af_update_get_result: "
+			      "Error Code %d", (unsigned int)result);
+		*af_update_rssi = 0;
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_af_update_get_result: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_af_update_start(
+			u32 af_freq
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_af_update_start");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_af_update_start: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_af_update_start(
+					af_freq / FREQUENCY_CONVERTOR_KHZ_HZ);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_af_update_start: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_af_update_start: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_af_switch_get_result(
+			u16 *af_switch_conclusion
+			)
+{
+	int result;
+	u16 af_rssi;
+	u16 af_pi;
+
+	FM_INFO_REPORT("cg2900_fm_af_switch_get_result");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_af_switch_get_result: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_get_af_switch_results(
+					      af_switch_conclusion,
+					      &af_rssi, &af_pi);
+
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_af_switch_get_result: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+	FM_DEBUG_REPORT("cg2900_fm_af_switch_get_result: "
+			"AF Switch conclusion = %d "
+			"AF Switch RSSI level = %d "
+			"AF Switch PI code = %d ",
+			*af_switch_conclusion, af_rssi, af_pi);
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_af_switch_get_result: returning %d",
+			result);
+	return result;
+
+}
+
+int cg2900_fm_af_switch_start(
+			u32 af_switch_freq,
+			u16 af_switch_pi
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_af_switch_start");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_af_switch_start: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_rx_af_switch_start(
+				af_switch_freq / FREQUENCY_CONVERTOR_KHZ_HZ,
+				af_switch_pi);
+
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_af_switch_start: "
+			      "Error Code %d", (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_af_switch_start: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_mode(
+			u8 *cur_mode
+			)
+{
+	int result = 0;
+	bool stereo_mode;
+
+	FM_INFO_REPORT("cg2900_fm_get_mode");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_mode: "
+			"Invalid state of FM Driver = %d", fm_state);
+		*cur_mode = CG2900_MODE_MONO;
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_mode: "
+				"fmd_rx_get_stereo_mode");
+		result = fmd_rx_get_stereo_mode(cur_mode);
+		FM_DEBUG_REPORT("cg2900_fm_get_mode: cur_mode = %x", *cur_mode);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_get_mode: "
+				"fmd_tx_get_stereo_mode");
+		result = fmd_tx_get_stereo_mode(&stereo_mode);
+		if (stereo_mode)
+			*cur_mode = CG2900_MODE_STEREO;
+		else
+			*cur_mode = CG2900_MODE_MONO;
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_mode: "
+			      "fmd_get_stereo_mode failed, "
+			      "Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_mode: returning %d, mode = %d",
+			result, *cur_mode);
+	return result;
+}
+
+int cg2900_fm_set_mode(
+			u8 mode
+			)
+{
+	int result = 0;
+	bool enable_stereo_mode = false;
+
+	FM_INFO_REPORT("cg2900_fm_set_mode: mode = %d", mode);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_set_mode: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	if (CG2900_FM_RX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_mode: "
+				"fmd_rx_set_stereo_mode");
+		result = fmd_rx_set_stereo_mode(mode);
+	} else if (CG2900_FM_TX_MODE == fm_mode) {
+		FM_DEBUG_REPORT("cg2900_fm_set_mode: "
+				"fmd_tx_set_stereo_mode");
+		if (mode == CG2900_MODE_STEREO)
+			enable_stereo_mode = true;
+		result =
+		    fmd_tx_enable_stereo_mode(
+					      enable_stereo_mode);
+	}
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_mode: "
+			      "fmd_rx_set_stereo_mode failed, "
+			      "Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_mode: returning %d",
+		result);
+	return result;
+}
+
+int cg2900_fm_select_antenna(
+			u8 antenna
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_select_antenna: Antenna = %d", antenna);
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_select_antenna: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_set_antenna(antenna);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_select_antenna: "
+			      "fmd_set_antenna failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_select_antenna: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_antenna(
+			u8 *antenna
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_get_antenna");
+
+	if (CG2900_FM_STATE_SWITCHED_ON != fm_state) {
+		FM_ERR_REPORT("cg2900_fm_get_antenna: "
+			"Invalid state of FM Driver = %d", fm_state);
+		result = -EINVAL;
+		goto error;
+	}
+	result = fmd_get_antenna(antenna);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_antenna: "
+			      "fmd_get_antenna failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_antenna: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_get_rssi_threshold(
+			u16 *rssi_thresold
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_get_rssi_threshold");
+
+	result = fmd_rx_get_stop_level(rssi_thresold);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_get_rssi_threshold: "
+			      "fmd_rx_get_stop_level failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_get_rssi_threshold: returning %d",
+			result);
+	return result;
+}
+
+int cg2900_fm_set_rssi_threshold(
+			u16 rssi_thresold
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_rssi_threshold: "
+		       "RssiThresold = %d", rssi_thresold);
+
+	result = fmd_rx_set_stop_level(rssi_thresold);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_rssi_threshold: "
+			      "fmd_rx_set_stop_level failed, Error Code %d",
+			      (unsigned int)result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_rssi_threshold: returning %d",
+			result);
+	return result;
+}
+
+void cg2900_fm_set_chip_version(
+			u16 revision,
+			u16 sub_version
+			)
+{
+	version_info.revision = revision;
+	version_info.sub_version = sub_version;
+}
+
+int cg2900_fm_set_test_tone_generator(
+			u8 test_tone_status
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_set_test_tone_generator: "
+		"test_tone_status = %02x", test_tone_status);
+
+	result = fmd_set_test_tone_generator_status(test_tone_status);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_set_test_tone_generator: "
+			"fmd_set_test_tone_generator_status failed"
+			", Error Code %d", result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_set_test_tone_generator: returning %d",
+		result);
+	return result;
+}
+
+int cg2900_fm_test_tone_connect(
+			u8 left_audio_mode,
+			u8 right_audio_mode
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_test_tone_connect: "
+		"left_audio_mode = %02x right_audio_mode = %02x",
+		left_audio_mode, right_audio_mode);
+
+	result = fmd_test_tone_connect(left_audio_mode, right_audio_mode);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_test_tone_connect: "
+			"fmd_set_test_tone_connect failed, Error Code %d",
+			result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_test_tone_connect: returning %d",
+		result);
+	return result;
+}
+
+int cg2900_fm_test_tone_set_params(
+			u8 tone_gen,
+			u16 frequency,
+			u16 volume,
+			u16 phase_offset,
+			u16 dc,
+			u8 waveform
+			)
+{
+	int result;
+
+	FM_INFO_REPORT("cg2900_fm_test_tone_set_params: "
+		"tone_gen = %02x frequency = %04x "
+		"volume = %04x phase_offset = %04x "
+		"dc offset = %04x waveform = %02x",
+		tone_gen, frequency,
+		volume, phase_offset,
+		dc, waveform);
+
+	result = fmd_test_tone_set_params(
+				tone_gen,
+				frequency,
+				volume,
+				phase_offset,
+				dc,
+				waveform);
+	if (0 != result) {
+		FM_ERR_REPORT("cg2900_fm_test_tone_set_params: "
+			"fmd_test_tone_set_params failed, Error Code %d",
+			result);
+		result = -EINVAL;
+		goto error;
+	}
+
+error:
+	FM_DEBUG_REPORT("cg2900_fm_test_tone_set_params: returning %d",
+		result);
+	return result;
+}
+
+MODULE_AUTHOR("Hemant Gupta");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/radio/CG2900/cg2900_fm_api.h b/drivers/media/radio/CG2900/cg2900_fm_api.h
new file mode 100644
index 00000000000..ec9e6e86f77
--- /dev/null
+++ b/drivers/media/radio/CG2900/cg2900_fm_api.h
@@ -0,0 +1,1077 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux FM Host API's for ST-Ericsson FM Chip.
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#ifndef CG2900_FM_API_H
+#define CG2900_FM_API_H
+
+#include <linux/device.h>
+#include <linux/skbuff.h>
+
+/* Callback function to receive RDS Data. */
+typedef void  (*cg2900_fm_rds_cb)(void);
+
+extern struct sk_buff_head		fm_interrupt_queue;
+
+/**
+ * struct cg2900_fm_rds_buf - RDS Group Receiving Structure
+ *
+ * @block1: RDS Block A
+ * @block2: RDS Block B
+ * @block3: RDS Block C
+ * @block4: RDS Block D
+ * @status1: Status of received RDS Block A
+ * @status2: Status of received RDS Block B
+ * @status3: Status of received RDS Block C
+ * @status4: Status of received RDS Block D
+ *
+ * Structure for receiving the RDS Group from FM Chip.
+ */
+struct cg2900_fm_rds_buf {
+	u16 block1;
+	u16 block2;
+	u16 block3;
+	u16 block4;
+	u8 status1;
+	u8 status2;
+	u8 status3;
+	u8 status4;
+};
+
+/**
+ * struct cg2900_fm_rds_info - RDS Information Structure
+ *
+ * @rds_head: RDS Queue Head for storing next valid data.
+ * @rds_tail: RDS Queue Tail for retreiving next valid data.
+ * @rds_group_sent: Number of RDS Groups sent to Application.
+ * @rds_block_sent: Number of RDS Blocks sent to Application.
+ *
+ * Structure for storing the RDS data queue information.
+ */
+struct cg2900_fm_rds_info {
+	u8 rds_head;
+	u8 rds_tail;
+	u8 rds_group_sent;
+	u8 rds_block_sent;
+};
+
+/**
+ * struct cg2900_version_info - Chip HCI Version Info
+ *
+ * @revision:	Revision of the controller, e.g. to indicate that it is
+ *			a CG2900 controller.
+ * @sub_version:	Subversion of the controller, e.g. to indicate a certain
+ *			tape-out of the controller.
+ *
+ * Structure for storing the HCI Version Information of the Controller.
+ */
+struct cg2900_version_info {
+	u16 revision;
+	u16 sub_version;
+};
+
+/**
+ * enum cg2900_fm_state - States of FM Driver.
+ *
+ * @CG2900_FM_STATE_DEINITIALIZED: FM driver is not initialized.
+ * @CG2900_FM_STATE_INITIALIZED: FM driver is initialized.
+ * @CG2900_FM_STATE_SWITCHED_ON: FM driver is switched on and in active state.
+ * @CG2900_FM_STATE_STAND_BY: FM Radio is switched on but not in active state.
+ *
+ * Various states of FM Driver.
+ */
+enum cg2900_fm_state {
+	CG2900_FM_STATE_DEINITIALIZED,
+	CG2900_FM_STATE_INITIALIZED,
+	CG2900_FM_STATE_SWITCHED_ON,
+	CG2900_FM_STATE_STAND_BY
+};
+
+/**
+ * enum cg2900_fm_mode - FM Driver Command state .
+ *
+ * @CG2900_FM_IDLE_MODE: FM Radio is in Idle Mode.
+ * @CG2900_FM_RX_MODE: FM Radio is configured in Rx mode.
+ * @CG2900_FM_TX_MODE: FM Radio is configured in Tx mode.
+ *
+ * Various Modes of the FM Radio.
+ */
+enum cg2900_fm_mode {
+	CG2900_FM_IDLE_MODE,
+	CG2900_FM_RX_MODE,
+	CG2900_FM_TX_MODE
+};
+
+/**
+ * enum cg2900_fm_band - Various Frequency band supported.
+ *
+ * @CG2900_FM_BAND_US_EU: European / US Band.
+ * @CG2900_FM_BAND_JAPAN: Japan Band.
+ * @CG2900_FM_BAND_CHINA: China Band.
+ * @CG2900_FM_BAND_CUSTOM: Custom Band.
+ *
+ * Various Frequency band supported.
+ */
+enum cg2900_fm_band {
+	CG2900_FM_BAND_US_EU,
+	CG2900_FM_BAND_JAPAN,
+	CG2900_FM_BAND_CHINA,
+	CG2900_FM_BAND_CUSTOM
+};
+
+/**
+ * enum cg2900_fm_grid - Various Frequency grids supported.
+ *
+ * @CG2900_FM_GRID_50: 50 kHz spacing.
+ * @CG2900_FM_GRID_100: 100 kHz spacing.
+ * @CG2900_FM_GRID_200: 200 kHz spacing.
+ *
+ * Various Frequency grids supported.
+ */
+enum cg2900_fm_grid {
+	CG2900_FM_GRID_50,
+	CG2900_FM_GRID_100,
+	CG2900_FM_GRID_200
+};
+
+/**
+ * enum cg2900_fm_event - Various Events reported by FM API layer.
+ *
+ * @CG2900_EVENT_NO_EVENT: No Event.
+ * @CG2900_EVENT_SEARCH_CHANNEL_FOUND: Seek operation is completed.
+ * @CG2900_EVENT_SCAN_CHANNELS_FOUND: Band Scan is completed.
+ * @CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND: Block Scan is completed.
+ * @CG2900_EVENT_SCAN_CANCELLED: Scan/Seek is cancelled.
+ * @CG2900_EVENT_MONO_STEREO_TRANSITION: Mono/Stereo Transition has taken place.
+ * @CG2900_EVENT_DEVICE_RESET: CG2900 has been reset by some other IP.
+ * @CG2900_EVENT_RDS_EVENT: RDS data interrupt has been received from chip.
+ *
+ * Various Events reported by FM API layer.
+ */
+enum cg2900_fm_event {
+	CG2900_EVENT_NO_EVENT,
+	CG2900_EVENT_SEARCH_CHANNEL_FOUND,
+	CG2900_EVENT_SCAN_CHANNELS_FOUND,
+	CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND,
+	CG2900_EVENT_SCAN_CANCELLED,
+	CG2900_EVENT_MONO_STEREO_TRANSITION,
+	CG2900_EVENT_DEVICE_RESET,
+	CG2900_EVENT_RDS_EVENT
+};
+
+/**
+ * enum cg2900_fm_direction - Directions used while seek.
+ *
+ * @CG2900_DIR_DOWN: Search in downwards direction.
+ * @CG2900_DIR_UP: Search in upwards direction.
+ *
+ * Directions used while seek.
+ */
+enum cg2900_fm_direction {
+	CG2900_DIR_DOWN,
+	CG2900_DIR_UP
+};
+
+/**
+ * enum cg2900_fm_stereo_mode - Stereo Modes.
+ *
+ * @CG2900_MODE_MONO: Mono Mode.
+ * @CG2900_MODE_STEREO: Stereo Mode.
+ *
+ * Stereo Modes.
+ */
+enum cg2900_fm_stereo_mode {
+	CG2900_MODE_MONO,
+	CG2900_MODE_STEREO
+};
+
+#define CG2900_FM_DEFAULT_RSSI_THRESHOLD			100
+#define MAX_RDS_BUFFER					10
+#define MAX_RDS_GROUPS					22
+#define MIN_ANALOG_VOLUME				0
+#define MAX_ANALOG_VOLUME				20
+#define NUM_OF_RDS_BLOCKS				4
+#define RDS_BLOCK_MASK					0x1C
+#define RDS_ERROR_STATUS_MASK				0x03
+#define RDS_UPTO_TWO_BITS_CORRECTED			0x01
+#define RDS_UPTO_FIVE_BITS_CORRECTED			0x02
+#define MAX_RT_SIZE					65
+#define MAX_PSN_SIZE					9
+#define DEFAULT_CHANNELS_TO_SCAN			32
+#define MAX_CHANNELS_TO_SCAN				99
+#define MAX_CHANNELS_FOR_BLOCK_SCAN			198
+#define SKB_FM_INTERRUPT_DATA				2
+
+extern u8 fm_event;
+extern struct cg2900_fm_rds_buf fm_rds_buf[MAX_RDS_BUFFER][MAX_RDS_GROUPS];
+extern struct cg2900_fm_rds_info fm_rds_info;
+
+/**
+ * cg2900_fm_init()- Initializes FM Radio.
+ *
+ * Initializes the Variables and structures required for FM Driver.
+ * It also registers the callback to receive the events for command
+ * completion, etc
+ *
+ * Returns:
+ *   0,  if Initialization successful
+ *   -EINVAL, otherwise.
+ */
+int cg2900_fm_init(void);
+
+/**
+ * cg2900_fm_deinit()- De-initializes FM Radio.
+ *
+ * De-initializes the Variables and structures required for FM Driver.
+ *
+ * Returns:
+ *	 0, if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_deinit(void);
+
+/**
+ * cg2900_fm_switch_on()- Start up procedure of the FM radio.
+ *
+ * @device: Character device requesting the operation.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_switch_on(
+			struct device *device
+			);
+
+/**
+ * cg2900_fm_switch_off()- Switches off FM radio
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_switch_off(void);
+
+/**
+ * cg2900_fm_standby()- Makes the FM Radio Go in Standby mode.
+ *
+ * The FM Radio memorizes the the last state, i.e. Volume, last
+ * tuned station, etc that helps in resuming quickly to previous state.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_standby(void);
+
+/**
+ * cg2900_fm_power_up_from_standby()- Power Up FM Radio from Standby mode.
+ *
+ * It retruns the FM radio to the same state as it was before
+ * going to Standby.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_power_up_from_standby(void);
+
+/**
+ * cg2900_fm_set_rx_default_settings()- Loads FM Rx Default Settings.
+ *
+ * @freq: Frequency in Hz to be set on the FM Radio.
+ * @band: Band To be Set.
+ * (0: US/EU, 1: Japan, 2: China, 3: Custom)
+ * @grid: Grid specifying Spacing.
+ * (0: 50 KHz, 1: 100 KHz, 2: 200 Khz)
+ * @enable_rds: Flag indicating enable or disable rds transmission.
+ * @enable_stereo: Flag indicating enable or disable stereo mode.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_rx_default_settings(
+			u32 freq,
+			u8 band,
+			u8 grid,
+			bool enable_rds,
+			bool enable_stereo
+			);
+
+/**
+ * cg2900_fm_set_tx_default_settings()- Loads FM Tx Default Settings.
+ *
+ * @freq: Frequency in Hz to be set on the FM Radio.
+ * @band: Band To be Set.
+ * (0: US/EU, 1: Japan, 2: China, 3: Custom)
+ * @grid: Grid specifying Spacing.
+ * (0: 50 KHz, 1: 100 KHz, 2: 200 Khz)
+ * @enable_rds: Flag indicating enable or disable rds transmission.
+ * @enable_stereo: Flag indicating enable or disable stereo mode.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_tx_default_settings(
+			u32 freq,
+			u8 band,
+			u8 grid,
+			bool enable_rds,
+			bool enable_stereo
+			);
+
+/**
+ * cg2900_fm_set_grid()- Sets the Grid on the FM Radio.
+ *
+ * @grid: Grid specifying Spacing.
+ * (0: 50 KHz,1: 100 KHz,2: 200 Khz)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_grid(
+			u8 grid
+			);
+
+/**
+ * cg2900_fm_set_band()- Sets the Band on the FM Radio.
+ *
+ * @band: Band specifying Region.
+ * (0: US_EU,1: Japan,2: China,3: Custom)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_band(
+			u8 band
+			);
+
+/**
+ * cg2900_fm_search_up_freq()- seek Up.
+ *
+ * Searches the next available station in Upward Direction
+ * starting from the Current freq.
+ *
+ * If the operation is started successfully, the chip will generate the
+ * irpt_OperationSucced. interrupt when the operation is completed
+ * and will tune to the next available frequency.
+ * If no station is found, the chip is still tuned to the original station
+ * before starting the search
+ * Till the interrupt is received, no more API's should be called
+ * except cg2900_fm_stop_scan
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_search_up_freq(void);
+
+/**
+ * cg2900_fm_search_down_freq()- seek Down.
+ *
+ * Searches the next available station in Downward Direction
+ * starting from the Current freq.
+ *
+ * If the operation is started successfully, the chip will generate
+ * the irpt_OperationSucced. interrupt when the operation is completed.
+ * and will tune to the next available frequency. If no station is found,
+ * the chip is still tuned to the original station before starting the search.
+ * Till the interrupt is received, no more API's should be called
+ * except cg2900_fm_stop_scan.
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_search_down_freq(void);
+
+/**
+ * cg2900_fm_start_band_scan()- Band Scan.
+ *
+ * Searches for available Stations in the entire Band starting from
+ * current freq.
+ * If the operation is started successfully, the chip will generate
+ * the irpt_OperationSucced. interrupt when the operation is completed.
+ * After completion the chip will still be tuned the original station before
+ * starting the Scan. on reception of interrupt, the host should call the AP
+ * cg2900_fm_get_scan_result() to retrieve the Stations and corresponding
+ * RSSI of stations found in the Band.
+ * Till the interrupt is received, no more API's should be called
+ * except cg2900_fm_stop_scan, cg2900_fm_switch_off, cg2900_fm_standby and
+ * cg2900_fm_get_frequency.
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_start_band_scan(void);
+
+/**
+ * cg2900_fm_stop_scan()- Stops an active ongoing seek or Band Scan.
+ *
+ * If the operation is started successfully, the chip will generate the
+ * irpt_OperationSucced interrupt when the operation is completed.
+ * Till the interrupt is received, no more API's should be called.
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_stop_scan(void);
+
+/**
+ * cg2900_fm_get_scan_result()- Retreives Band Scan Result
+ *
+ * Retrieves the Scan Band Results of the stations found and
+ * the corressponding RSSI values of the stations.
+ * @num_of_scanfreq: (out) Number of Stations found
+ * during Scanning.
+ * @scan_freq: (out) Frequency of Stations in Hz
+ * found during Scanning.
+ * @scan_freq_rssi_level: (out) RSSI level of Stations
+ * found during Scanning.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_scan_result(
+			u16 *num_of_scanfreq,
+			u32 *scan_freq,
+			u32 *scan_freq_rssi_level
+			);
+
+/**
+ * cg2900_fm_start_block_scan()- Block Scan.
+ *
+ * Searches for RSSI level of all the channels between the start and stop
+ * channels. If the operation is started successfully, the chip will generate
+ * the irpt_OperationSucced interrupt when the operation is completed.
+ * After completion the chip will still be tuned the original station before
+ * starting the Scan. On reception of interrupt, the host should call the AP
+ * cg2900_fm_get_block_scan_result() to retrieve the RSSI of channels.
+ * Till the interrupt is received, no more API's should be called from Host
+ * except cg2900_fm_stop_scan, cg2900_fm_switch_off, cg2900_fm_standby and
+ * cg2900_fm_get_frequency.
+ * @start_freq: Start channel block scan Frequency.
+ * @end_freq:  End channel block scan Frequency
+ *
+ * Returns:
+ *	 0,  if operation started successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_start_block_scan(
+			u32 start_freq,
+			u32 end_freq
+			);
+
+/**
+ * cg2900_fm_get_scan_result()- Retreives Band Scan Result
+ *
+ * Retrieves the Scan Band Results of the stations found and
+ * the corressponding RSSI values of the stations.
+ * @num_of_scanchan: (out) Number of Stations found
+ * during Scanning.
+ * @scan_freq_rssi_level: (out) RSSI level of Stations
+ * found during Scanning.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_block_scan_result(
+			u16 *num_of_scanchan,
+			u16 *scan_freq_rssi_level
+			);
+
+/**
+ * cg2900_fm_tx_get_rds_deviation()- Gets RDS Deviation.
+ *
+ * Retrieves the RDS Deviation level set for FM Tx.
+ * @deviation: (out) Rds Deviation.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_rds_deviation(
+			u16 *deviation
+			);
+
+/**
+ * cg2900_fm_tx_set_rds_deviation()- Sets RDS Deviation.
+ *
+ * Sets the RDS Deviation level on FM Tx.
+ * @deviation: Rds Deviation to set on FM Tx.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_rds_deviation(
+			u16 deviation
+			);
+
+/**
+ * cg2900_fm_tx_set_pi_code()- Sets PI code for RDS Transmission.
+ *
+ * Sets the Program Identification code to be transmitted.
+ * @pi_code: PI code to be transmitted.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_pi_code(
+			u16 pi_code
+			);
+
+/**
+ * cg2900_fm_tx_set_pty_code()- Sets PTY code for RDS Transmission.
+ *
+ * Sets the Program Type code to be transmitted.
+ * @pty_code: PTY code to be transmitted.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_pty_code(
+			u16 pty_code
+			);
+
+/**
+ * cg2900_fm_tx_set_program_station_name()- Sets PSN for RDS Transmission.
+ *
+ * Sets the Program Station Name to be transmitted.
+ * @psn: Program Station Name to be transmitted.
+ * @len: Length of Program Station Name to be transmitted.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_program_station_name(
+			char *psn,
+			u8 len
+			);
+
+/**
+ * cg2900_fm_tx_set_radio_text()- Sets RT for RDS Transmission.
+ *
+ * Sets the radio text to be transmitted.
+ * @rt: Radio Text to be transmitted.
+ * @len: Length of Radio Text to be transmitted.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_radio_text(
+			char *rt,
+			u8 len
+			);
+
+/**
+ * cg2900_fm_tx_get_rds_deviation()- Gets Pilot Tone status
+ *
+ * Gets the current status of pilot tone for FM Tx.
+ * @enable: (out) Flag indicating Pilot Tone is enabled or disabled.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_pilot_tone_status(
+			bool *enable
+			);
+
+/**
+ * cg2900_fm_tx_set_pilot_tone_status()- Enables/Disables Pilot Tone.
+ *
+ * Enables or disables the pilot tone for FM Tx.
+ * @enable: Flag indicating enabling or disabling Pilot Tone.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_pilot_tone_status(
+			bool enable
+			);
+
+/**
+ * cg2900_fm_tx_get_pilot_deviation()- Gets Pilot Deviation.
+ *
+ * Retrieves the Pilot Tone Deviation level set for FM Tx.
+ * @deviation: (out) Pilot Tone Deviation.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_pilot_deviation(
+			u16 *deviation
+			);
+
+/**
+ * cg2900_fm_tx_set_pilot_deviation()- Sets Pilot Deviation.
+ *
+ * Sets the Pilot Tone Deviation level on FM Tx.
+ * @deviation: Pilot Tone Deviation to set.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_pilot_deviation(
+			u16 deviation
+			);
+
+/**
+ * cg2900_fm_tx_get_preemphasis()- Gets Pre-emhasis level.
+ *
+ * Retrieves the Preemphasis level set for FM Tx.
+ * @preemphasis: (out) Preemphasis level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_preemphasis(
+			u8 *preemphasis
+			);
+
+/**
+ * cg2900_fm_tx_set_preemphasis()- Sets Pre-emhasis level.
+ *
+ * Sets the Preemphasis level on FM Tx.
+ * @preemphasis: Preemphasis level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_preemphasis(
+			u8 preemphasis
+			);
+
+/**
+ * cg2900_fm_tx_get_power_level()- Gets Power level.
+ *
+ * Retrieves the Power level set for FM Tx.
+ * @power_level: (out) Power level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_get_power_level(
+			u16 *power_level
+			);
+
+/**
+ * cg2900_fm_tx_set_power_level()- Sets Power level.
+ *
+ * Sets the Power level for FM Tx.
+ * @power_level: Power level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_set_power_level(
+			u16 power_level
+			);
+
+/**
+ * cg2900_fm_tx_rds()- Enable or disable Tx RDS.
+ *
+ * Enable or disable RDS transmission.
+ * @enable_rds: Flag indicating enabling or disabling RDS.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_tx_rds(
+			bool enable_rds
+			);
+
+/**
+ * cg2900_fm_set_audio_balance()- Sets Audio Balance.
+ *
+ * @balance: Audio Balnce to be Set in Percentage.
+ * (-100: Right Mute.... 0: Both on.... 100: Left Mute)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_audio_balance(
+			s8 balance
+			);
+
+/**
+ * cg2900_fm_set_volume()- Sets the Analog Out Gain of FM Chip.
+ *
+ * @vol_level: Volume Level to be set on Tuner (0-20).
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_volume(
+			u8 vol_level
+			);
+
+/**
+ * cg2900_fm_get_volume()- Gets the currently set Analog Out Gain of FM Chip.
+ *
+ * @vol_level: (out)Volume Level set on Tuner (0-20).
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_volume(
+			u8 *vol_level
+			);
+
+/**
+ * cg2900_fm_rds_off()- Disables the RDS decoding algorithm in FM chip
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_rds_off(void);
+
+/**
+ * cg2900_fm_rds_on()- Enables the RDS decoding algorithm in FM chip
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_rds_on(void);
+
+/**
+ * cg2900_fm_get_rds_status()- Retrieves the status whether RDS is enabled or not
+ *
+ * @rds_status: (out) Status of RDS
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_rds_status(
+			bool *rds_status
+			);
+
+/**
+ * cg2900_fm_mute()- Mutes the Audio output from FM Chip
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_mute(void);
+
+/**
+ * cg2900_fm_unmute()- Unmutes the Audio output from FM Chip
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_unmute(void);
+
+/**
+ * cg2900_fm_get_frequency()- Gets the Curently tuned Frequency on FM Radio
+ *
+ * @freq: (out) Frequency in Hz set on the FM Radio.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_frequency(
+			u32 *freq
+			);
+
+/**
+ * cg2900_fm_set_frequency()- Sets the frequency on FM Radio
+ *
+ * @new_freq: Frequency in Hz to be set on the FM Radio.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_frequency(
+			u32 new_freq
+			);
+
+/**
+ * cg2900_fm_get_signal_strength()- Gets the RSSI level.
+ *
+ * @signal_strength: (out) RSSI level of the currently
+ * tuned frequency.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_signal_strength(
+			u16 *signal_strength
+			);
+
+/**
+ * cg2900_fm_get_af_updat()- Retrives results of AF Update
+ *
+ * @af_update_rssi: (out) RSSI level of the Alternative frequency.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_af_update_get_result(
+			u16 *af_update_rssi
+			);
+
+
+/**
+ * cg2900_fm_af_update_start()- PErforms AF Update.
+ *
+ * @af_freq: AF frequency in Hz whose RSSI is to be retrived.
+ * tuned frequency.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+
+int cg2900_fm_af_update_start(
+			u32 af_freq
+			);
+
+/**
+ * cg2900_fm_af_switch_get_result()- Retrives the AF switch result.
+ *
+ * @af_switch_conclusion: (out) Conclusion of the AF Switch.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_af_switch_get_result(
+			u16 *af_switch_conclusion
+			);
+
+/**
+ * cg2900_fm_af_switch_start()- PErforms AF switch.
+ *
+ * @af_switch_freq: Alternate Frequency in Hz to be switched.
+ * @af_switch_pi: picode of the Alternative frequency.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_af_switch_start(
+			u32 af_switch_freq,
+			u16 af_switch_pi
+			);
+
+/**
+ * cg2900_fm_get_mode()- Gets the mode of the Radio tuner.
+ *
+ * @cur_mode: (out) Current mode set on FM Radio
+ * (0: Stereo, 1: Mono, 2: Blending, 3: Switching).
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_mode(
+			u8 *cur_mode
+			);
+
+/**
+ * cg2900_fm_set_mode()- Sets the mode on the Radio tuner.
+ *
+ * @mode: mode to be set on FM Radio
+ * (0: Stereo, 1: Mono, 2: Blending, 3: Switching.)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_mode(
+			u8 mode
+			);
+
+/**
+ * cg2900_fm_select_antenna()- Selects the Antenna of the Radio tuner.
+ *
+ * @antenna: (0: Embedded, 1: Wired.)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_select_antenna(
+			u8 antenna
+			);
+
+/**
+ * cg2900_fm_get_antenna()- Retreives the currently selected antenna.
+ *
+ * @antenna: out (0: Embedded, 1: Wired.)
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_antenna(
+			u8 *antenna
+			);
+
+/**
+ * cg2900_fm_get_rssi_threshold()- Gets the rssi threshold currently
+ *
+ * set on FM radio.
+ * @rssi_thresold: (out) Current rssi threshold set.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_get_rssi_threshold(
+			u16 *rssi_thresold
+			);
+
+/**
+ * cg2900_fm_set_rssi_threshold()- Sets the rssi threshold to be used during
+ *
+ * Band Scan and seek Stations
+ * @rssi_thresold: rssi threshold to be set.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_rssi_threshold(
+			u16 rssi_thresold
+			);
+
+/**
+ * cg2900_handle_device_reset()- Handle The reset of Device
+ */
+void cg2900_handle_device_reset(void);
+
+/**
+ * wake_up_poll_queue()- Wakes up the Task waiting on Poll Queue.
+ * This function is called when Scan Band or seek has completed.
+ */
+void wake_up_poll_queue(void);
+
+/**
+ * void cg2900_fm_set_chip_version()- Sets the Version of the Controller.
+ *
+ * This function is used to update the Chip Version information at time
+ * of intitialization of FM driver.
+ * @revision:	Revision of the controller, e.g. to indicate that it is
+ *			a CG2900 controller.
+ * @sub_version:	Subversion of the controller, e.g. to indicate a certain
+ *			tape-out of the controller.
+ */
+void cg2900_fm_set_chip_version(
+			u16 revision,
+			u16 sub_version
+			);
+
+/**
+ * cg2900_fm_rx_set_deemphasis()- Sets de-emhasis level.
+ *
+ * Sets the Deemphasis level on FM Rx.
+ * @deemphasis: Deemphasis level.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_rx_set_deemphasis(
+			u8 deemphasis
+			);
+
+/**
+ * cg2900_fm_set_test_tone_generator()- Sets the Test Tone Generator.
+ *
+ * This function is used to enable/disable the Internal Tone Generator of
+ * CG2900.
+ * @test_tone_status: Status of tone generator.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_set_test_tone_generator(
+			u8 test_tone_status
+			);
+
+
+/**
+ * cg2900_fm_test_tone_connect()- Connect Audio outputs/inputs.
+ *
+ * This function connects the audio outputs/inputs of the Internal Tone
+ * Generator of CG2900.
+ * @left_audio_mode: Left Audio Output Mode.
+ * @right_audio_mode: Right Audio Output Mode.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_test_tone_connect(
+			u8 left_audio_mode,
+			u8 right_audio_mode
+			);
+
+/**
+ * cg2900_fm_test_tone_set_params()- Sets the Test Tone Parameters.
+ *
+ * This function is used to set the parameters of the Internal Tone Generator of
+ * CG2900.
+ * @tone_gen: Tone to be configured (Tone 1 or Tone 2)
+ * @frequency: Frequency of the tone.
+ * @volume: Volume of the tone.
+ * @phase_offset: Phase offset of the tone.
+ * @dc: DC to add to tone.
+ * @waveform: Waveform to generate, sine or pulse.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int cg2900_fm_test_tone_set_params(
+			u8 tone_gen,
+			u16 frequency,
+			u16 volume,
+			u16 phase_offset,
+			u16 dc,
+			u8 waveform
+			);
+
+#endif /* CG2900_FM_API_H */
diff --git a/drivers/media/radio/CG2900/cg2900_fm_driver.c b/drivers/media/radio/CG2900/cg2900_fm_driver.c
new file mode 100644
index 00000000000..627d0f2d674
--- /dev/null
+++ b/drivers/media/radio/CG2900/cg2900_fm_driver.c
@@ -0,0 +1,5017 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux FM Driver for CG2900 FM Chip
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#include <linux/device.h>
+#include <linux/time.h>
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <asm-generic/errno-base.h>
+#include "cg2900.h"
+#include "cg2900_fm_driver.h"
+
+/*
+ * Macro for printing the HCI Packet received from Protocol Driver
+ * to FM Driver.
+ */
+#define CG2900_HEX_READ_PACKET_DUMP \
+	if (cg2900_fm_debug_level == FM_HCI_PACKET_LOGS) \
+		fmd_hexdump('<', skb->data, skb->len);
+
+/* Macro for printing the HCI Packet sent to Protocol Driver from FM Driver */
+#define CG2900_HEX_WRITE_PACKET_DUMP \
+	if (cg2900_fm_debug_level == FM_HCI_PACKET_LOGS) \
+		fmd_hexdump('>', send_buffer, num_bytes);
+
+/* Converts the given value to ASCII format*/
+#define ASCVAL(x)(((x) <= 9) ? (x) + '0' : (x) - 10 + 'a')
+
+/* The receive Packet's 1st byte indicates the packet length */
+#define FM_GET_PKT_LEN(__data) __data[0]
+
+/* The receive Packet's following bytes are the actual data. */
+#define FM_GET_RSP_PKT_ADDR(__data) (&__data[1])
+
+/*
+ * LSB consists of shifting the command Id by 3 bits
+ * to left and ORING with the number of parameters of the
+ * command.
+ */
+#define FM_CMD_GET_LSB(__cmd_id, __num_param) \
+		((u8)(((__cmd_id << 3) & 0x00FF) | __num_param))
+
+/* MSB consists of shifting the command Id by 5 bits to right. */
+#define FM_CMD_GET_MSB(__cmd_id) \
+		((u8)(__cmd_id >> 5))
+
+/*
+ * Command id is mapped as shifting the MSB 5 bits to left and
+ * ORING with LSB shifted 3 bits to right.
+ */
+#define FM_GET_CMD_ID(__data) \
+		((u16)((__data[2] << 5) | __data[1] >> 3))
+
+/*
+ * Number of parameters in the response packet are the last 3 bits
+ * of the 1st byte of the received packet.
+ */
+#define FM_GET_NUM_PARAMS(__data) \
+		((u16)((__data[1] & 0x07)))
+
+/* Function Id is mapped to the 1st byte of the received packet */
+#define FM_GET_FUNCTION_ID(__data) __data[0]
+
+/*
+ * Block Id of the FM Firmware downloaded is mapped to the
+ *  2nd byte of the received packet.
+ */
+#define FM_GET_BLOCK_ID(__data) __data[1]
+
+/* Status of the received packet is mapped to the 4th byte. */
+#define FM_GET_STATUS(__data) __data[3]
+
+/*
+ * For PG1 of CG2900, the FM Interrupt is mapped
+ * to the 3rd and 4th byte of the received packet.
+ */
+#define FM_GET_PGI_INTERRUPT(__data) \
+		((u16)(__data[3] << 8 | __data[2]))
+
+/*
+ * For PG2 of CG2900, the FM Interrupt is mapped
+ * to the 5th and 6th byte of the received packet.
+ */
+#define FM_GET_PG2_INTERRUPT(__data) \
+		((u16)(__data[5] << 8 | __data[4]))
+
+#define FM_GET_NUM_RDS_GRPS(__data) __data[0]
+
+/* Response buffer starts from the 4th byte if the response buffer */
+#define FM_GET_RSP_BUFFER_ADDR(__data) (&__data[3])
+
+/* FM Function buffer starts from the 5th byte if the response buffer */
+#define FM_GET_FUNCTION_ADDR(__data) (&__data[4])
+
+/*
+ * Maximum time for chip to respond including the Command
+ * Competion interrupts for some commands. This time has been
+ * adjusted to cater to increased communication time with chip
+ * when debug level is set to 4.
+ */
+#define MAX_RESPONSE_TIME_IN_MS	5000
+
+/* Byte per word */
+#define WORD_LENGTH				2
+
+/* Byte Offset Counter */
+#define COUNTER					1
+
+/* Binary shift offset for one byte */
+#define SHIFT_OFFSET			8
+
+/*
+ * enum fmd_gocmd_t - FM Driver Command state.
+ *
+ * @FMD_STATE_NONE: FM Driver in Idle state
+ * @FMD_STATE_MODE: FM Driver in setmode state
+ * @FMD_STATE_FREQUENCY: FM Driver in Set frequency state.
+ * @FMD_STATE_PA: FM Driver in SetPA state.
+ * @FMD_STATE_PA_LEVEL: FM Driver in Setpalevl state.
+ * @FMD_STATE_ANTENNA: FM Driver in Setantenna state
+ * @FMD_STATE_MUTE: FM Driver in Setmute state
+ * @FMD_STATE_SEEK: FM Driver in seek mode
+ * @FMD_STATE_SEEK_STOP: FM Driver in seek stop level state.
+ * @FMD_STATE_SCAN_BAND: FM Driver in Scanband mode
+ * @FMD_STATE_TX_SET_CTRL: FM Driver in RDS control state
+ * @FMD_STATE_TX_SET_THRSHLD: FM Driver in RDS threshld state
+ * @FMD_STATE_GEN_POWERUP: FM Driver in Power UP state.
+ * @FMD_STATE_SELECT_REF_CLK: FM Driver in Select Reference clock state.
+ * @FMD_STATE_SET_REF_CLK_PLL: FM Driver in Set Reference Freq state.
+ * @FMD_STATE_BLOCK_SCAN: FM Driver in Block Scan state.
+ * @FMD_STATE_AF_UPDATE: FM Driver in AF Update State.
+ * @FMD_STATE_AF_SWITCH: FM Driver in AF Switch State.
+ * @FMD_STATE_LAST: Last State of FM Driver
+ *
+ * Various states of the FM driver.
+ */
+enum fmd_gocmd {
+	FMD_STATE_NONE,
+	FMD_STATE_MODE,
+	FMD_STATE_FREQUENCY,
+	FMD_STATE_PA,
+	FMD_STATE_PA_LEVEL,
+	FMD_STATE_ANTENNA,
+	FMD_STATE_MUTE,
+	FMD_STATE_SEEK,
+	FMD_STATE_SEEK_STOP,
+	FMD_STATE_SCAN_BAND,
+	FMD_STATE_TX_SET_CTRL,
+	FMD_STATE_TX_SET_THRSHLD,
+	FMD_STATE_GEN_POWERUP,
+	FMD_STATE_SELECT_REF_CLK,
+	FMD_STATE_SET_REF_CLK_PLL,
+	FMD_STATE_BLOCK_SCAN,
+	FMD_STATE_AF_UPDATE,
+	FMD_STATE_AF_SWITCH,
+	FMD_STATE_LAST
+};
+
+/**
+ * struct fmd_rdsgroup_t - Rds group structure.
+ *
+ * @block: Array for RDS Block(s) received.
+ * @status: Array of Status of corresponding RDS block(s).
+ *
+ * It stores the value and status of a particular RDS group
+ * received.
+ */
+struct fmd_rds_group {
+	u16 block[NUM_OF_RDS_BLOCKS];
+	u8  status[NUM_OF_RDS_BLOCKS];
+};
+
+/**
+ * struct fmd_states_info - Main FM state info structure.
+ *
+ * @fmd_initialized: Flag indicating FM Driver is initialized or not
+ * @rx_freq_range: Receiver freq range
+ * @rx_volume:  Receiver volume level
+ * @rx_antenna: Receiver Antenna
+ * @rx_seek_stop_level:  RDS seek stop Level
+ * @rx_rds_on:  Receiver RDS ON
+ * @rx_stereo_mode: Receiver Stereo mode
+ * @max_channels_to_scan: Maximum Number of channels to Scan.
+ * @tx_freq_range: Transmitter freq Range
+ * @tx_preemphasis: Transmitter Pre emphiasis level
+ * @tx_stereo_mode: Transmitter stero mode
+ * @tx_rds_on:  Enable RDS
+ * @tx_pilot_dev: PIlot freq deviation
+ * @tx_rds_dev:  RDS deviation
+ * @tx_strength:  TX Signal Stregnth
+ * @irq_index:  Index where last interrupt is added to Interrupt queue
+ * @interrupt_available_for_processing:  Flag indicating if interrupt is
+ * available for processing or not.
+ * @interrupt_queue: Circular Queue to store the received interrupt from chip.
+ * @gocmd:  Command which is in progress.
+ * @mode:  Current Mode of FM Radio.
+ * @rds_group:  Array of RDS group Buffer
+ * @callback: Callback registered by upper layers.
+ */
+struct fmd_states_info {
+	bool		fmd_initialized;
+	u8			rx_freq_range;
+	u8			rx_volume;
+	u8			rx_antenna;
+	u16			rx_seek_stop_level;
+	bool		rx_rds_on;
+	u8			rx_stereo_mode;
+	u8			tx_freq_range;
+	u8			tx_preemphasis;
+	bool		tx_stereo_mode;
+	u8			max_channels_to_scan;
+	bool		tx_rds_on;
+	u16			tx_pilot_dev;
+	u16			tx_rds_dev;
+	u16			tx_strength;
+	u8			irq_index;
+	bool		interrupt_available_for_processing;
+	u16			interrupt_queue[MAX_COUNT_OF_IRQS];
+	enum fmd_gocmd gocmd;
+	enum fmd_mode mode;
+	struct fmd_rds_group	rds_group[MAX_RDS_GROUPS];
+	fmd_radio_cb callback;
+};
+
+/**
+ * struct fmd_data - Main structure for FM data exchange.
+ *
+ * @cmd_id: Command Id of the command being exchanged.
+ * @num_parameters: Number of parameters
+ * @parameters: FM data parameters.
+ */
+struct fmd_data {
+	u32 cmd_id;
+	u16 num_parameters;
+	u8 parameters[MAX_RESP_SIZE];
+};
+
+static struct fmd_states_info fmd_state_info;
+static struct fmd_data fmd_data;
+static struct semaphore cmd_sem;
+static struct semaphore rds_sem;
+static struct semaphore interrupt_sem;
+static struct task_struct *rds_thread_task;
+static struct task_struct *irq_thread_task;
+static struct device *cg2900_fm_dev;
+static struct mutex write_mutex;
+static struct mutex send_cmd_mutex;
+static spinlock_t fmd_spinlock;
+static spinlock_t fmd_spinlock_read;
+
+/* Debug Level
+ * 1: Only Error Logs
+ * 2: Info Logs
+ * 3: Debug Logs
+ * 4: HCI Logs
+ */
+unsigned short cg2900_fm_debug_level = FM_ERROR_LOGS;
+EXPORT_SYMBOL(cg2900_fm_debug_level);
+
+static cg2900_fm_rds_cb cb_rds_func;
+static bool rds_thread_required;
+static bool irq_thread_required;
+
+static char event_name[FMD_EVENT_LAST_ELEMENT][MAX_NAME_SIZE] = {
+	"FMD_EVENT_OPERATION_COMPLETED",
+	"FMD_EVENT_ANTENNA_STATUS_CHANGED",
+	"FMD_EVENT_FREQUENCY_CHANGED",
+	"FMD_EVENT_SEEK_COMPLETED",
+	"FMD_EVENT_SCAN_BAND_COMPLETED",
+	"FMD_EVENT_BLOCK_SCAN_COMPLETED",
+	"FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE",
+	"FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE",
+	"FMD_EVENT_SEEK_STOPPED",
+	"FMD_EVENT_GEN_POWERUP",
+	"FMD_EVENT_RDSGROUP_RCVD",
+};
+
+static char interrupt_name[MAX_COUNT_OF_IRQS][MAX_NAME_SIZE] = {
+	"IRPT_OPERATION_SUCCEEDED",
+	"IRPT_OPERATION_FAILED",
+	"IRPT_NOT_DEFINED",
+	"IRPT_RX_BUFFER_FULL_OR_TX_BUFFER_EMPTY",
+	"IRPT_RX_SIGNAL_QUALITY_LOW_OR_TX_MUTE_STATUS_CHANGED",
+	"IRPT_MONO_STEREO_TRANSITION",
+	"IRPT_RX_RDS_SYNC_FOUND_OR_TX_INPUT_OVERDRIVE",
+	"IRPT_RDS_SYNC_LOST",
+	"IRPT_PI_CODE_CHANGED",
+	"IRPT_REQUESTED_BLOCK_AVAILABLE",
+	"IRPT_NOT_DEFINED",
+	"IRPT_NOT_DEFINED",
+	"IRPT_NOT_DEFINED",
+	"IRPT_NOT_DEFINED",
+	"IRPT_WARM_BOOT_READY",
+	"IRPT_COLD_BOOT_READY",
+};
+
+
+static void fmd_hexdump(
+			char prompt,
+			u8 *buffer,
+			int num_bytes
+			);
+static u8 fmd_get_event(
+			enum fmd_gocmd	gocmd
+			);
+static void fmd_event_name(
+			u8 event,
+			char *event_name
+			);
+static char *fmd_get_fm_function_name(
+			u8 fm_function
+			);
+static void fmd_interrupt_name(
+			u16 interrupt,
+			char *interrupt_name
+			);
+static void fmd_add_interrupt_to_queue(
+			u16 interrupt
+			);
+static void fmd_process_interrupt(
+			u16 interrupt
+			);
+static void fmd_callback(
+			u8 event,
+			bool event_successful
+			);
+static int fmd_rx_frequency_to_channel(
+			u32 freq,
+			u16 *channel
+			);
+static int fmd_rx_channel_to_frequency(
+			u16 channel_number,
+			u32 *frequency
+			);
+static int fmd_tx_frequency_to_channel(
+			u32 freq,
+			u16 *channel
+			);
+static int fmd_tx_channel_to_frequency(
+			u16 channel_number,
+			u32 *frequency
+			);
+static bool fmd_go_cmd_busy(void);
+static int fmd_send_cmd_and_read_resp(
+			const u16 cmd_id,
+			const u16 num_parameters,
+			const u16 *parameters,
+			u16 *resp_num_parameters,
+			u16 *resp_parameters
+			);
+static int fmd_send_cmd(
+			const u16 cmd_id,
+			const u16 num_parameters,
+			const u16 *parameters
+			);
+static int fmd_read_resp(
+			u16 *cmd_id,
+			u16 *num_parameters,
+			u16 *parameters
+			);
+static void fmd_process_fm_function(
+			u8 *packet_buffer
+			);
+static int fmd_write_file_block(
+			u32 file_block_id,
+			u8 *file_block,
+			u16 file_block_length
+			);
+static void fmd_receive_data(
+			u16 packet_length,
+			u8 *packet_buffer
+			);
+static int fmd_rds_thread(
+			void *data
+			);
+static void fmd_start_irq_thread(void);
+static void fmd_stop_irq_thread(void);
+static int fmd_irq_thread(
+			void *data
+			);
+static int fmd_send_packet(
+			u16 num_bytes,
+			u8 *send_buffer
+			);
+static int fmd_get_cmd_sem(void);
+static void fmd_set_cmd_sem(void);
+static void fmd_get_interrupt_sem(void);
+static void fmd_set_interrupt_sem(void);
+static bool fmd_driver_init(void);
+static void fmd_driver_exit(void);
+
+/* structure declared in time.h */
+struct timespec time_spec;
+
+
+/**
+ * fmd_hexdump() - Displays the HCI Data Bytes exchanged with FM Chip.
+ *
+ * @prompt: Prompt signifying the direction '<' for Rx '>' for Tx
+ * @buffer: Buffer to be displayed.
+ * @num_bytes: Number of bytes of the buffer.
+ */
+ static void fmd_hexdump(
+			char prompt,
+			u8 *buffer,
+			int num_bytes
+			)
+{
+	int i;
+	u8 tmp_val;
+	struct timespec time;
+	static u8 pkt_write[MAX_BUFFER_SIZE], *pkt_ptr;
+
+	getnstimeofday(&time);
+	sprintf(pkt_write, "\n[%08x:%08x] [%04x] %c",
+		(unsigned int)time.tv_sec,
+		(unsigned int)time.tv_nsec,
+		num_bytes, prompt);
+
+	pkt_ptr = pkt_write + strlen(pkt_write);
+	if (buffer == NULL)
+		return;
+
+	/* Copy the buffer only if the input buffer is not NULL */
+	for (i = 0; i < num_bytes; i++) {
+		*pkt_ptr++ = ' ';
+		tmp_val = buffer[i] >> 4;
+		*pkt_ptr++ = ASCVAL(tmp_val);
+		tmp_val = buffer[i] & 0x0F;
+		*pkt_ptr++ = ASCVAL(tmp_val);
+		if (i > 20) {
+			/* Print only 20 bytes at max */
+			break;
+		}
+	}
+	*pkt_ptr++ = '\0';
+	FM_HEX_REPORT("%s", pkt_write);
+}
+
+/**
+ * fmd_get_event() - Returns the Event based on FM Driver State.
+ *
+ * @gocmd: Pending FM Command
+ *
+ * Returns: Corresponding Event
+ */
+static u8 fmd_get_event(
+			enum fmd_gocmd gocmd
+			)
+{
+	u8 event = FMD_EVENT_OPERATION_COMPLETED;
+	switch (gocmd) {
+	case FMD_STATE_ANTENNA:
+		event = FMD_EVENT_ANTENNA_STATUS_CHANGED;
+		break;
+	case FMD_STATE_FREQUENCY:
+		event = FMD_EVENT_FREQUENCY_CHANGED;
+		break;
+	case FMD_STATE_SEEK:
+		event = FMD_EVENT_SEEK_COMPLETED;
+		break;
+	case FMD_STATE_SCAN_BAND:
+		event = FMD_EVENT_SCAN_BAND_COMPLETED;
+		break;
+	case FMD_STATE_BLOCK_SCAN:
+		event = FMD_EVENT_BLOCK_SCAN_COMPLETED;
+		break;
+	case FMD_STATE_AF_UPDATE:
+	/* Drop Down */
+	case FMD_STATE_AF_SWITCH:
+		event = FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE;
+		break;
+	case FMD_STATE_SEEK_STOP:
+		event = FMD_EVENT_SEEK_STOPPED;
+		break;
+	default:
+		event = FMD_EVENT_OPERATION_COMPLETED;
+		break;
+	}
+	return event;
+}
+
+/**
+ * fmd_event_name() - Converts the event to a displayable string.
+ *
+ * @event: Event that has occurred.
+ * @eventname: (out) Buffer to store event name.
+ */
+static void fmd_event_name(
+			u8 event,
+			char *eventname
+			)
+{
+	if (eventname == NULL) {
+		FM_ERR_REPORT("fmd_event_name: Output Buffer is NULL");
+		return;
+	}
+	if (event < FMD_EVENT_LAST_ELEMENT)
+		strcpy(eventname, event_name[event]);
+	else
+		strcpy(eventname, "FMD_EVENT_UNKNOWN");
+}
+
+/**
+ * fmd_get_fm_function_name() - Returns the FM Fucntion name.
+ *
+ * @fm_function: Function whose name is to be retrieved.
+ *
+ * Returns FM Function Name.
+ */
+static char *fmd_get_fm_function_name(
+			u8 fm_function
+			)
+{
+	switch (fm_function) {
+	case FM_FUNCTION_ENABLE:
+		return "FM_FUNCTION_ENABLE";
+		break;
+	case FM_FUNCTION_DISABLE:
+		return "FM_FUNCTION_DISABLE";
+		break;
+	case FM_FUNCTION_RESET:
+		return "FM_FUNCTION_RESET";
+		break;
+	case FM_FUNCTION_WRITE_COMMAND:
+		return "FM_FUNCTION_WRITE_COMMAND";
+		break;
+	case FM_FUNCTION_SET_INT_MASK_ALL:
+		return "FM_FUNCTION_SET_INT_MASK_ALL";
+		break;
+	case FM_FUNCTION_GET_INT_MASK_ALL:
+		return "FM_FUNCTION_GET_INT_MASK_ALL";
+		break;
+	case FM_FUNCTION_SET_INT_MASK:
+		return "FM_FUNCTION_SET_INT_MASK";
+		break;
+	case FM_FUNCTION_GET_INT_MASK:
+		return "FM_FUNCTION_GET_INT_MASK";
+		break;
+	case FM_FUNCTION_FIRMWARE_DOWNLOAD:
+		return "FM_FUNCTION_FIRMWARE_DOWNLOAD";
+		break;
+	default:
+		return "FM_FUNCTION_UNKNOWN";
+		break;
+	}
+}
+
+/**
+ * fmd_interrupt_name() - Converts the interrupt to a displayable string.
+ *
+ * @interrupt: interrupt received from FM Chip
+ * @interruptname: (out) Buffer to store interrupt name.
+ */
+static void fmd_interrupt_name(
+			u16 interrupt,
+			char *interruptname
+			)
+{
+	int index;
+
+	if (interruptname == NULL) {
+		FM_ERR_REPORT("fmd_interrupt_name: Output Buffer is NULL!!!");
+		return;
+	}
+	/* Convert Interrupt to Bit */
+	for (index = 0; index < MAX_COUNT_OF_IRQS; index++) {
+		if (interrupt & (1 << index)) {
+			/* Match found, break the loop */
+			break;
+		}
+	}
+	if (index < MAX_COUNT_OF_IRQS)
+		strcpy(interruptname, interrupt_name[index]);
+	else
+		strcpy(interruptname, "IRPT_UNKNOWN");
+}
+
+/**
+ * fmd_add_interrupt_to_queue() - Add interrupt to IRQ Queue.
+ *
+ * @interrupt: interrupt received from FM Chip
+ */
+static void fmd_add_interrupt_to_queue(
+			u16 interrupt
+			)
+{
+	FM_DEBUG_REPORT("fmd_add_interrupt_to_queue : "
+			"Interrupt Received = %04x", (u16) interrupt);
+
+	/* Reset the index if it reaches the array limit */
+	if (fmd_state_info.irq_index > MAX_COUNT_OF_IRQS - 1) {
+		spin_lock(&fmd_spinlock);
+		fmd_state_info.irq_index = 0;
+		spin_unlock(&fmd_spinlock);
+	}
+
+	spin_lock(&fmd_spinlock);
+	fmd_state_info.interrupt_queue[fmd_state_info.irq_index] = interrupt;
+	fmd_state_info.irq_index++;
+	spin_unlock(&fmd_spinlock);
+	if (!fmd_state_info.interrupt_available_for_processing) {
+		spin_lock(&fmd_spinlock);
+		fmd_state_info.interrupt_available_for_processing = true;
+		spin_unlock(&fmd_spinlock);
+		fmd_set_interrupt_sem();
+	}
+}
+
+/**
+ * fmd_process_interrupt() - Processes the Interrupt.
+ *
+ * This function processes the interrupt received from FM Chip
+ * and calls the corresponding callback registered by upper layers with
+ * proper parameters.
+ * @interrupt: interrupt received from FM Chip
+ */
+static void fmd_process_interrupt(
+			u16 interrupt
+			)
+{
+	char irpt_name[MAX_NAME_SIZE];
+
+	fmd_interrupt_name(interrupt, irpt_name);
+	FM_DEBUG_REPORT("%s", irpt_name);
+	if ((interrupt & IRPT_OPERATION_SUCCEEDED) |
+		(interrupt & IRPT_OPERATION_FAILED)) {
+		bool event_status = (interrupt & IRPT_OPERATION_SUCCEEDED);
+		u8 event = fmd_get_event(fmd_state_info.gocmd);
+
+		switch (fmd_state_info.gocmd) {
+		case FMD_STATE_MODE:
+			/* Mode has been changed. */
+		case FMD_STATE_MUTE:
+			/* FM radio is Muter or Unmuted */
+		case FMD_STATE_PA:
+			/* Power Amplifier has been enabled/disabled */
+		case FMD_STATE_PA_LEVEL:
+			/* Power Amplifier Level has been changed. */
+		case FMD_STATE_SELECT_REF_CLK:
+			/* Reference Clock has been selected. */
+		case FMD_STATE_SET_REF_CLK_PLL:
+			/* Reference Clock frequency has been changed. */
+		case FMD_STATE_TX_SET_CTRL:
+			/* Tx Control has been set. */
+		case FMD_STATE_TX_SET_THRSHLD:
+			/* Tx Threashold has been set. */
+			/* Set State to None and set the waiting semaphore. */
+			fmd_state_info.gocmd = FMD_STATE_NONE;
+			fmd_set_cmd_sem();
+			break;
+		case FMD_STATE_ANTENNA:
+			/* Antenna status has been changed. */
+		case FMD_STATE_SEEK_STOP:
+			/* Band scan, seek or block scan has completed. */
+		case FMD_STATE_AF_UPDATE:
+			/* AF Update has completed. */
+		case FMD_STATE_AF_SWITCH:
+			/* AF Switch has completed. */
+		case FMD_STATE_FREQUENCY:
+			/* Frequency has been changed. */
+			/*
+			 * Set State to None, set the waiting semaphore,
+			 * and inform upper layer.
+			 */
+			fmd_state_info.gocmd = FMD_STATE_NONE;
+			fmd_set_cmd_sem();
+			fmd_callback(
+				event,
+				event_status);
+			break;
+		case FMD_STATE_SEEK:
+			/* Seek has completed. */
+		case FMD_STATE_SCAN_BAND:
+			/* Band scan has completed. */
+		case FMD_STATE_BLOCK_SCAN:
+			/* Block scan has completed. */
+			/*
+			 * Set State to None. No need to set the
+			 * semaphore since this is an asyncronous event.
+			 */
+			fmd_state_info.gocmd = FMD_STATE_NONE;
+			/* Inform Upper layer. */
+			fmd_callback(event,	event_status);
+			break;
+		default:
+			/* Do Nothing */
+			FM_ERR_REPORT("Default %s case of "\
+				"interrupt processing", event_status ? \
+				"Success" : "Failed");
+			break;
+		}
+	}
+
+	if (interrupt & IRPT_RX_BUFFERFULL_TX_BUFFEREMPTY) {
+		/*
+		 * RDS Buffer Full or RDS Buffer Empty
+		 * interrupt received from chip, indicating
+		 * that RDS data is available if chip
+		 * is in Rx mode or RDS data can be send
+		 * to chip in case of Tx mode. Inform the
+		 * upper layers about this interrupt.
+		 */
+		fmd_callback(
+		     FMD_EVENT_RDSGROUP_RCVD,
+		     true);
+	}
+
+	if (interrupt & IRPT_RX_MONO_STEREO_TRANSITION) {
+		/*
+		 * Mono Stereo Transition interrupt
+		 * received from chip, inform the
+		 * upper layers about it.
+		 */
+		fmd_callback(
+		     FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE,
+		     true);
+	}
+
+	if ((interrupt & IRPT_COLD_BOOT_READY) |
+		(interrupt &  IRPT_WARM_BOOT_READY)) {
+		switch (fmd_state_info.gocmd) {
+		case FMD_STATE_GEN_POWERUP:
+			/*
+			 * Cold Boot/ Warm Boot Interrupt received from
+			 * chip, indicating transition from
+			 * power off/standby state to active state.
+			 * Inform the upper layers about it.
+			 */
+			fmd_callback(
+				FMD_EVENT_GEN_POWERUP,
+				true);
+			/* Set State to None and set the waiting semaphore. */
+			fmd_state_info.gocmd = FMD_STATE_NONE;
+			fmd_set_cmd_sem();
+			break;
+		default:
+			/* Do Nothing */
+			break;
+		}
+	}
+}
+
+/**
+ * fmd_callback() - Callback function for upper layers.
+ *
+ * Callback function that calls the registered callback of upper
+ * layers with proper parameters.
+ * @event: event for which the callback function was called
+ * from FM Driver.
+ * @event_successful: Signifying whether the event is called from FM
+ * Driver on receiving irpt_Operation_Succeeded or irpt_Operation_Failed.
+ */
+static void fmd_callback(
+			u8 event,
+			bool event_successful
+			)
+{
+	char event_name_string[MAX_NAME_SIZE];
+
+	fmd_event_name(event, event_name_string);
+
+	FM_DEBUG_REPORT("%s %x, %d", event_name_string,
+		(unsigned int)event , (unsigned int)event_successful);
+
+	if (fmd_state_info.callback)
+		fmd_state_info.callback(
+				event,
+				event_successful);
+}
+
+/**
+ * fmd_rx_frequency_to_channel() - Converts Rx frequency to channel number.
+ *
+ * Converts the Frequency in kHz to corresponding Channel number.
+ * This is used for FM Rx.
+ * @freq: Frequency in kHz.
+ * @channel: Channel Number corresponding to the given Frequency.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ *
+ */
+static int fmd_rx_frequency_to_channel(
+			u32 freq,
+			u16 *channel
+			)
+{
+	u8	range;
+	int	result;
+	u32	min_freq;
+	u32	max_freq;
+
+	if (channel == NULL) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_get_freq_range(
+			&range);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_get_freq_range_properties(
+			range,
+			&min_freq,
+			&max_freq);
+
+	if (result != 0)
+		goto error;
+
+	if (freq > max_freq)
+		freq = max_freq;
+	else if (freq < min_freq)
+		freq = min_freq;
+
+	/*
+	 * Frequency in kHz needs to be divided with 50 kHz to get
+	 * channel number for all FM Bands
+	 */
+	*channel = (u16)((freq - min_freq) / CHANNEL_FREQ_CONVERTER_MHZ);
+	result = 0;
+error:
+	return result;
+}
+
+/**
+ * fmd_rx_channel_to_frequency() - Converts Rx Channel number to frequency.
+ *
+ * Converts the Channel Number to corresponding Frequency in kHz.
+ * This is used for FM Rx.
+ * @channel_number: Channel Number to be converted.
+ * @frequency: Frequency corresponding to the corresponding channel in kHz.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ *
+ */
+static int fmd_rx_channel_to_frequency(
+			u16 channel_number,
+			u32 *frequency
+			)
+{
+	u8	range;
+	int	result;
+	u32	min_freq;
+	u32	max_freq;
+
+	if (frequency == NULL) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_get_freq_range(
+			&range);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_get_freq_range_properties(
+			range,
+			&min_freq,
+			&max_freq);
+
+	if (result != 0)
+		goto error;
+
+	/*
+	 * Channel Number needs to be multiplied with 50 kHz to get
+	 * frequency in kHz for all FM Bands
+	 */
+	*frequency = min_freq + (channel_number * CHANNEL_FREQ_CONVERTER_MHZ);
+
+error:
+	return result;
+}
+
+/**
+ * fmd_tx_frequency_to_channel() - Converts Tx frequency to channel number.
+ *
+ * Converts the Frequency in kHz to corresponding Channel number.
+ * This is used for FM Tx.
+ * @freq: Frequency in kHz.
+ * @channel: (out)Channel Number corresponding to the given Frequency.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ */
+static int fmd_tx_frequency_to_channel(
+			u32 freq,
+			u16 *channel
+			)
+{
+	u8	range;
+	int	result;
+	u32	min_freq;
+	u32	max_freq;
+
+	if (channel == NULL) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_tx_get_freq_range(
+			&range);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_get_freq_range_properties(
+			range,
+			&min_freq,
+			&max_freq);
+
+	if (result != 0)
+		goto error;
+
+	if (freq > max_freq)
+		freq = max_freq;
+	else if (freq < min_freq)
+		freq = min_freq;
+
+	/*
+	 * Frequency in kHz needs to be divided with 50 kHz to get
+	 * channel number for all FM Bands
+	 */
+	*channel = (u16)((freq - min_freq) / CHANNEL_FREQ_CONVERTER_MHZ);
+	result = 0;
+error:
+	return result;
+}
+
+/**
+ * fmd_tx_channel_to_frequency() - Converts Tx Channel number to frequency.
+ *
+ * Converts the Channel Number to corresponding Frequency in kHz.
+ * This is used for FM Tx.
+ * @channel_number: Channel Number to be converted.
+ * @frequency: Frequency corresponding to the corresponding channel
+ * in kHz.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ */
+static int fmd_tx_channel_to_frequency(
+			u16 channel_number,
+			u32 *frequency
+			)
+{
+	u8	range;
+	int	result;
+	u32	min_freq;
+	u32	max_freq;
+
+	if (frequency == NULL) {
+		result = -EINVAL;
+		goto error;
+	}
+
+	result = fmd_tx_get_freq_range(
+			&range);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_get_freq_range_properties(
+			range,
+			&min_freq,
+			&max_freq);
+
+	if (result != 0)
+		goto error;
+
+	/*
+	 * Channel Number needs to be multiplied with 50 kHz to get
+	 * frequency in kHz for all FM Bands
+	 */
+	*frequency = min_freq + (channel_number * CHANNEL_FREQ_CONVERTER_MHZ);
+
+	if (*frequency > max_freq)
+		*frequency = max_freq;
+	else if (*frequency < min_freq)
+		*frequency = min_freq;
+
+	result = 0;
+error:
+	return result;
+}
+
+/**
+ * fmd_go_cmd_busy() - Function to check if FM Driver is busy or idle
+ *
+ * Returns:
+ *   false if FM Driver is Idle
+ *   true otherwise
+ */
+static bool fmd_go_cmd_busy(void)
+{
+	return (fmd_state_info.gocmd != FMD_STATE_NONE);
+}
+
+/**
+ * fmd_read_cb() - Handle Received Data
+ *
+ * This function handles data received from connectivity protocol driver.
+ * @dev: Device receiving data.
+ * @skb: Buffer with data coming form device.
+ */
+static void fmd_read_cb(
+			struct cg2900_user_data *dev,
+			struct sk_buff *skb
+			)
+{
+	FM_INFO_REPORT("fmd_read_cb");
+
+	if (skb->data == NULL || skb->len == 0)
+		return;
+
+	spin_lock(&fmd_spinlock_read);
+	CG2900_HEX_READ_PACKET_DUMP;
+	/*
+	 * The first byte is length of bytes following bytes
+	 * Rest of the bytes are the actual data
+	 */
+	fmd_receive_data(
+					FM_GET_PKT_LEN(skb->data),
+					FM_GET_RSP_PKT_ADDR(skb->data));
+
+	kfree_skb(skb);
+	spin_unlock(&fmd_spinlock_read);
+}
+
+/**
+ * fmd_receive_data() - Processes the FM data received from device.
+ *
+ * @packet_length: Length of received Data Packet
+ * @packet_buffer: Received Data buffer.
+ */
+static void fmd_receive_data(
+			u16 packet_length,
+			u8 *packet_buffer
+			)
+{
+	if (packet_buffer == NULL) {
+		FM_ERR_REPORT("fmd_receive_data: Buffer = NULL");
+		return;
+	}
+
+	if (packet_length == FM_PG1_INTERRUPT_EVENT_LEN &&
+		packet_buffer[0] == FM_CATENA_OPCODE &&
+		packet_buffer[1] == FM_EVENT_ID) {
+		/* PG 1.0 interrupt Handling */
+		u16 interrupt = FM_GET_PGI_INTERRUPT(packet_buffer);
+		FM_DEBUG_REPORT("interrupt = %04x",
+			(unsigned int)interrupt);
+		fmd_add_interrupt_to_queue(interrupt);
+	} else if (packet_length == FM_PG2_INTERRUPT_EVENT_LEN &&
+		packet_buffer[0] == FM_SUCCESS_STATUS &&
+		packet_buffer[1] == FM_CATENA_OPCODE &&
+		packet_buffer[2] == FM_EVENT &&
+		packet_buffer[3] == FM_EVENT_ID) {
+		/* PG 2.0 interrupt Handling */
+		u16 interrupt = FM_GET_PG2_INTERRUPT(packet_buffer);
+		FM_DEBUG_REPORT("interrupt = %04x",
+			(unsigned int)interrupt);
+		fmd_add_interrupt_to_queue(interrupt);
+	} else if (packet_buffer[0] == FM_SUCCESS_STATUS &&
+		packet_buffer[1] == FM_CATENA_OPCODE &&
+		packet_buffer[2] == FM_WRITE) {
+		/* Command Complete or RDS Data Handling */
+		u8 fm_status = FM_GET_STATUS(packet_buffer);;
+		switch (fm_status) {
+		case FM_CMD_STATUS_CMD_SUCCESS:
+			fmd_process_fm_function(
+				FM_GET_FUNCTION_ADDR(packet_buffer));
+			break;
+		case FM_CMD_STATUS_HCI_ERR_HW_FAILURE:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_HCI_ERR_HW_FAILURE");
+			break;
+		case FM_CMD_STATUS_HCI_ERR_INVALID_PARAMETERS:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_HCI_ERR_INVALID_PARAMETERS");
+			break;
+		case FM_CMD_STATUS_IP_UNINIT:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_IP_UNINIT");
+			break;
+		case FM_CMD_STATUS_HCI_ERR_UNSPECIFIED_ERROR:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_HCI_ERR_UNSPECIFIED_ERROR");
+			break;
+		case FM_CMD_STATUS_HCI_ERR_CMD_DISALLOWED:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_HCI_ERR_CMD_DISALLOWED");
+			break;
+		case FM_CMD_STATUS_WRONG_SEQ_NUM:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_WRONG_SEQ_NUM");
+			break;
+		case FM_CMD_STATUS_UNKNOWN_FILE_TYPE:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_UNKNOWN_FILE_TYPE");
+			break;
+		case FM_CMD_STATUS_FILE_VERSION_MISMATCH:
+			FM_DEBUG_REPORT(
+				"FM_CMD_STATUS_FILE_VERSION_MISMATCH");
+			break;
+		default:
+			FM_DEBUG_REPORT(
+				"Unknown Status = %02x", fm_status);
+			break;
+		}
+	}
+}
+
+/**
+ * fmd_reset_cb() - Reset callback fuction.
+ *
+ * @dev: CPD device reseting.
+ */
+static void fmd_reset_cb(struct cg2900_user_data *dev)
+{
+	FM_INFO_REPORT("fmd_reset_cb: Device Reset");
+	spin_lock(&fmd_spinlock_read);
+	cg2900_handle_device_reset();
+	spin_unlock(&fmd_spinlock_read);
+}
+
+/**
+ * fmd_rds_thread() - Thread for receiving RDS data from Chip.
+ *
+ * @data: Data beng passed as parameter on starting the thread.
+ */
+static int fmd_rds_thread(
+			void *data
+			)
+{
+	FM_INFO_REPORT("fmd_rds_thread Created Successfully");
+	while (rds_thread_required) {
+		if (cb_rds_func)
+			cb_rds_func();
+		/* Give 100 ms for context switching */
+		schedule_timeout_interruptible(msecs_to_jiffies(100));
+	}
+	/* Always signal the rds_sem semaphore before exiting */
+	fmd_set_rds_sem();
+	FM_DEBUG_REPORT("fmd_rds_thread Exiting!!!");
+	return 0;
+}
+
+/**
+ * fmd_start_irq_thread() - Function for starting Interrupt Thread.
+ */
+static void fmd_start_irq_thread(void)
+{
+	FM_INFO_REPORT("fmd_start_irq_thread");
+	irq_thread_task = kthread_create(fmd_irq_thread, NULL, "irq_thread");
+	if (IS_ERR(irq_thread_task)) {
+		FM_ERR_REPORT("fmd_start_irq_thread: "
+			"Unable to Create irq_thread");
+		irq_thread_task = NULL;
+		return;
+	}
+	wake_up_process(irq_thread_task);
+}
+
+/**
+ * fmd_stop_irq_thread() - Function for stopping Interrupt Thread.
+ */
+static void fmd_stop_irq_thread(void)
+{
+	FM_INFO_REPORT("fmd_stop_irq_thread");
+	kthread_stop(irq_thread_task);
+	irq_thread_task = NULL;
+	FM_DEBUG_REPORT("-fmd_stop_irq_thread");
+}
+
+/**
+ * fmd_irq_thread() - Thread for processing Interrupts received from Chip.
+ *
+ * @data: Data being passed as parameter on starting the thread.
+ */
+
+static int fmd_irq_thread(
+			void *data
+			)
+{
+	int index;
+
+	FM_INFO_REPORT("fmd_irq_thread Created Successfully");
+
+	while (irq_thread_required) {
+		if (!fmd_state_info.interrupt_available_for_processing) {
+			FM_DEBUG_REPORT("fmd_irq_thread: Waiting on irq sem "
+				"interrupt_available_for_processing = %d "
+				"fmd_state_info.fmd_initialized = %d",
+			fmd_state_info.interrupt_available_for_processing,
+			fmd_state_info.fmd_initialized);
+			fmd_get_interrupt_sem();
+			FM_DEBUG_REPORT("fmd_irq_thread: Waiting on irq sem "
+				"interrupt_available_for_processing = %d "
+				"fmd_state_info.fmd_initialized = %d",
+			fmd_state_info.interrupt_available_for_processing,
+			fmd_state_info.fmd_initialized);
+		}
+		index = 0;
+
+		if (fmd_state_info.interrupt_available_for_processing) {
+			while (index < MAX_COUNT_OF_IRQS) {
+				if (fmd_state_info.interrupt_queue[index]
+				    != IRPT_INVALID) {
+					FM_DEBUG_REPORT("fmd_process_interrupt "
+						"Interrupt = %04x",
+						fmd_state_info.
+						interrupt_queue[index]);
+					fmd_process_interrupt(
+					fmd_state_info.interrupt_queue[index]);
+					fmd_state_info.interrupt_queue[index]
+					    = IRPT_INVALID;
+				}
+				index++;
+			}
+		}
+		fmd_state_info.interrupt_available_for_processing = false;
+		schedule_timeout_interruptible(msecs_to_jiffies(100));
+	}
+	FM_DEBUG_REPORT("fmd_irq_thread Exiting!!!");
+	return 0;
+}
+
+/**
+ * fmd_send_packet() - Sends the FM HCI Packet to the CG2900 Protocol Driver.
+ *
+ * @num_bytes: Number of bytes of Data to be sent including
+ * Channel Identifier (08)
+ * @send_buffer: Buffer containing the Data to be sent to Chip.
+ *
+ * Returns:
+ * 0, If packet was sent successfully to
+ * CG2900 Protocol Driver, otherwise the corresponding error.
+ * -EINVAL If parameters are not valid.
+ * -EIO If there is an Input/Output Error.
+ */
+static int fmd_send_packet(
+			u16 num_bytes,
+			u8 *send_buffer
+			)
+{
+	int err;
+	struct sk_buff *skb;
+	struct cg2900_user_data *pf_data;
+
+	FM_INFO_REPORT("fmd_send_packet");
+
+	if (send_buffer == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (!cg2900_fm_dev) {
+		FM_ERR_REPORT("fmd_send_packet: No FM device registered");
+		err = -EIO;
+		goto error;
+	}
+
+	pf_data = dev_get_platdata(cg2900_fm_dev);
+	if (!pf_data->opened) {
+		FM_ERR_REPORT("fmd_send_packet: FM channel is not opened");
+		err = -EIO;
+		goto error;
+	}
+
+	mutex_lock(&write_mutex);
+	CG2900_HEX_WRITE_PACKET_DUMP;
+
+	skb = pf_data->alloc_skb(num_bytes, GFP_KERNEL);
+	if (!skb) {
+		FM_ERR_REPORT("fmd_send_packet:Couldn't " \
+			"allocate sk_buff with length %d", num_bytes);
+		err = -EIO;
+		goto error;
+	}
+
+	/*
+	 * Copy the buffer removing the FM Header as this
+	 * would be done by Protocol Driver
+	 */
+	memcpy(skb_put(skb, num_bytes), send_buffer, num_bytes);
+
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		FM_ERR_REPORT("fmd_send_packet: "
+		"Failed to send(%d) bytes using "
+		"cg2900_write, err = %d",
+		num_bytes, err);
+		kfree(skb);
+		err = -EIO;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	mutex_unlock(&write_mutex);
+	FM_DEBUG_REPORT("fmd_send_packet returning %d", err);
+	return err;
+}
+
+/**
+ * fmd_get_cmd_sem() - Block on Command Semaphore.
+ *
+ * This is required to ensure Flow Control in FM Driver.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ETIME if timeout occurs.
+ */
+static int fmd_get_cmd_sem(void)
+{
+	int ret_val;
+
+	FM_INFO_REPORT("fmd_get_cmd_sem");
+
+	ret_val = down_timeout(&cmd_sem,
+		msecs_to_jiffies(MAX_RESPONSE_TIME_IN_MS));
+
+	if (ret_val)
+		FM_ERR_REPORT("fmd_get_cmd_sem: down_timeout "
+			      "returned error = %d", ret_val);
+
+	return ret_val;
+}
+
+/**
+ * fmd_set_cmd_sem() - Unblock on Command Semaphore.
+ *
+ * This is required to ensure Flow Control in FM Driver.
+ */
+static void fmd_set_cmd_sem(void)
+{
+	FM_DEBUG_REPORT("fmd_set_cmd_sem");
+
+	up(&cmd_sem);
+}
+
+/**
+ * fmd_get_interrupt_sem() - Block on Interrupt Semaphore.
+ *
+ * Till Interrupt is received, Interrupt Task is blocked.
+ */
+static void fmd_get_interrupt_sem(void)
+{
+	int ret_val;
+
+	FM_DEBUG_REPORT("fmd_get_interrupt_sem");
+
+	ret_val = down_killable(&interrupt_sem);
+
+	if (ret_val)
+		FM_ERR_REPORT("fmd_get_interrupt_sem: down_killable "
+			"returned error = %d", ret_val);
+}
+
+/**
+ * fmd_set_interrupt_sem() - Unblock on Interrupt Semaphore.
+ *
+ * on receiving  Interrupt, Interrupt Task is un-blocked.
+ */
+static void fmd_set_interrupt_sem(void)
+{
+	FM_DEBUG_REPORT("fmd_set_interrupt_sem");
+	up(&interrupt_sem);
+}
+
+/**
+ * fmd_driver_init()- Initializes the Mutex, Semaphore, etc for FM Driver.
+ *
+ * It also registers FM Driver with the Protocol Driver.
+ *
+ * Returns:
+ *   true if initialization is successful
+ *   false if initiialization fails.
+ */
+static bool fmd_driver_init(void)
+{
+	bool ret_val;
+	struct cg2900_rev_data rev_data;
+	struct cg2900_user_data *pf_data;
+	int err;
+
+	FM_INFO_REPORT("fmd_driver_init");
+
+	if (!cg2900_fm_dev) {
+		FM_ERR_REPORT("No device registered");
+		ret_val = false;
+		goto error;
+	}
+
+	/* Initialize the semaphores */
+	sema_init(&cmd_sem, 0);
+	sema_init(&rds_sem, 0);
+	sema_init(&interrupt_sem, 0);
+	cb_rds_func = NULL;
+	rds_thread_required = false;
+	irq_thread_required = true;
+
+	pf_data = dev_get_platdata(cg2900_fm_dev);
+
+	/* Create Mutex For Reading and Writing */
+	spin_lock_init(&fmd_spinlock_read);
+	mutex_init(&write_mutex);
+	mutex_init(&send_cmd_mutex);
+	spin_lock_init(&fmd_spinlock);
+	fmd_start_irq_thread();
+
+	/* Open the FM channel */
+	err = pf_data->open(pf_data);
+	if (err) {
+		FM_ERR_REPORT("fmd_driver_init: "
+			"Couldn't open FM channel. Either chip is not connected"
+			" or Protocol Driver is not initialized");
+		ret_val = false;
+		goto error;
+	}
+
+	if (!pf_data->get_local_revision(pf_data, &rev_data)) {
+		FM_DEBUG_REPORT("No revision data available");
+		ret_val = false;
+		goto error;
+	}
+
+	FM_DEBUG_REPORT("Read revision data revision %04x "
+		   "sub_version %04x",
+		   rev_data.revision, rev_data.sub_version);
+	cg2900_fm_set_chip_version(rev_data.revision, rev_data.sub_version);
+	ret_val = true;
+
+error:
+	FM_DEBUG_REPORT("fmd_driver_init: Returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * fmd_driver_exit() - Deinitializes the mutex, semaphores, etc.
+ *
+ * It also deregisters FM Driver with the Protocol Driver.
+ *
+ */
+static void fmd_driver_exit(void)
+{
+	struct cg2900_user_data *pf_data;
+
+	FM_INFO_REPORT("fmd_driver_exit");
+	irq_thread_required = false;
+	mutex_destroy(&write_mutex);
+	mutex_destroy(&send_cmd_mutex);
+	fmd_stop_irq_thread();
+	/* Close the FM channel */
+	pf_data = dev_get_platdata(cg2900_fm_dev);
+	if (pf_data->opened)
+		pf_data->close(pf_data);
+}
+
+/**
+ * fmd_send_cmd_and_read_resp() - Send command and read response.
+ *
+ * This function sends the HCI Command to Protocol Driver and
+ * Reads back the Response Packet.
+ * @cmd_id: Command Id to be sent to FM Chip.
+ * @num_parameters: Number of parameters of the command sent.
+ * @parameters: Buffer containing the Buffer to be sent.
+ * @resp_num_parameters: (out) Number of paramters of the response packet.
+ * @resp_parameters: (out) Buffer of the response packet.
+ *
+ * Returns:
+ *   0: If the command is sent successfully and the
+ *   response received is also correct.
+ *   -EINVAL: If the received response is not correct.
+ *   -EIO: If there is an input/output error.
+ *   -EINVAL: If parameters are not valid.
+ */
+static int fmd_send_cmd_and_read_resp(
+			const u16 cmd_id,
+			const u16 num_parameters,
+			const u16 *parameters,
+			u16 *resp_num_parameters,
+			u16 *resp_parameters
+			)
+{
+	int result;
+	u16 read_cmd_id = CMD_ID_NONE;
+
+	FM_INFO_REPORT("fmd_send_cmd_and_read_resp");
+
+	mutex_lock(&send_cmd_mutex);
+	result = fmd_send_cmd(
+			cmd_id,
+			num_parameters,
+			parameters);
+
+	if (result != 0)
+		goto error;
+
+	result = fmd_read_resp(
+			&read_cmd_id,
+			resp_num_parameters,
+			resp_parameters);
+
+	if (result != 0)
+		goto error;
+
+	/*
+	 * Check that the response belongs to the sent command
+	 */
+	if (read_cmd_id != cmd_id)
+		result = -EINVAL;
+
+error:
+	mutex_unlock(&send_cmd_mutex);
+	FM_DEBUG_REPORT("fmd_send_cmd_and_read_resp: "
+		"returning %d", result);
+	return result;
+}
+
+/**
+ * fmd_send_cmd() - This function sends the HCI Command
+ * to Protocol Driver.
+ *
+ * @cmd_id: Command Id to be sent to FM Chip.
+ * @num_parameters: Number of parameters of the command sent.
+ * @parameters: Buffer containing the Buffer to be sent.
+ *
+ * Returns:
+ *   0: If the command is sent successfully to Lower Layers.
+ *   -EIO: If there is an input/output error.
+ *   -EINVAL: If parameters are not valid.
+ */
+static int fmd_send_cmd(
+			const u16 cmd_id ,
+			const u16 num_parameters,
+			const u16 *parameters
+			)
+{
+	/*
+	 * Total Length includes 6 bytes HCI Header
+	 * and remaining bytes depending on number of paramters.
+	 */
+	u16 total_length = num_parameters * sizeof(u16) + FM_HCI_CMD_HEADER_LEN;
+	/*
+	 * Parameter Length includes 5 bytes HCI Header
+	 * and remaining bytes depending on number of paramters.
+	 */
+	u16 param_length = num_parameters * sizeof(u16) + FM_HCI_CMD_PARAM_LEN;
+	u8 *fm_data = kmalloc(total_length, GFP_KERNEL);
+	int err = -EINVAL;
+
+	FM_INFO_REPORT("fmd_send_cmd");
+
+	if (fm_data == NULL) {
+		err = -EIO;
+		goto error;
+	}
+
+	if (num_parameters && parameters == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	/* HCI encapsulation */
+	fm_data[0] = param_length;
+	fm_data[1] = FM_CATENA_OPCODE;
+	fm_data[2] = FM_WRITE;
+	fm_data[3] = FM_FUNCTION_WRITE_COMMAND;
+	fm_data[4] = FM_CMD_GET_LSB(cmd_id, num_parameters);
+	fm_data[5] = FM_CMD_GET_MSB(cmd_id);
+
+	memcpy(
+			(fm_data + FM_HCI_CMD_HEADER_LEN),
+			(void *)parameters,
+			num_parameters * sizeof(u16));
+
+	/* Send the Packet */
+	err = fmd_send_packet(total_length , fm_data);
+
+error:
+	kfree(fm_data);
+	FM_DEBUG_REPORT("fmd_send_cmd: "
+		"returning %d", err);
+	return err;
+}
+
+/**
+ * fmd_read_resp() - This function reads the response packet of the previous
+ * command sent to FM Chip and copies it to the buffer provided as parameter.
+ *
+ * @cmd_id: (out) Command Id received from FM Chip.
+ * @num_parameters: (out) Number of paramters of the response packet.
+ * @parameters: (out) Buffer of the response packet.
+ *
+ * Returns:
+ *   0: If the response buffer is copied successfully.
+ *   -EINVAL: If parameters are not valid.
+ *   -ETIME: Otherwise
+ */
+static int fmd_read_resp(
+			u16 *cmd_id,
+			u16 *num_parameters,
+			u16 *parameters
+			)
+{
+	int err;
+	int param_offset = 0;
+	int byte_offset = 0;
+	FM_INFO_REPORT("fmd_read_resp");
+
+	/* Wait till response of the command is received */
+	if (fmd_get_cmd_sem()) {
+		err = -ETIME;
+		goto error;
+	}
+
+	/* Check if the parameters are valid */
+	if (cmd_id == NULL || (fmd_data.num_parameters &&
+		(num_parameters == NULL || parameters == NULL))) {
+		err =  -EINVAL;
+		goto error;
+	}
+
+	/* Fill the arguments */
+	*cmd_id = fmd_data.cmd_id;
+	if (fmd_data.num_parameters) {
+		*num_parameters = fmd_data.num_parameters;
+		while (param_offset <
+				(*num_parameters * sizeof(u16)) / WORD_LENGTH) {
+			parameters[param_offset] =
+				(u16)(fmd_data.parameters[byte_offset])
+						& 0x00ff;
+			parameters[param_offset] |=
+				((u16)(fmd_data.parameters[byte_offset + COUNTER])
+						& 0x00ff) << SHIFT_OFFSET;
+			byte_offset = byte_offset + WORD_LENGTH;
+			param_offset++;
+		}
+	}
+
+	err = 0;
+
+error:
+	FM_DEBUG_REPORT("fmd_read_resp: "
+		"returning %d", err);
+	return err;
+}
+
+/**
+ * fmd_process_fm_function() - Process FM Function.
+ *
+ * This function processes the Response buffer received
+ * from lower layers for the FM function and performs the necessary action to
+ * parse the same.
+ * @packet_buffer: Received Buffer.
+ */
+static void fmd_process_fm_function(
+			u8 *packet_buffer
+			)
+{
+	u8 fm_function_id;
+	u8 block_id;
+	int count = 0;
+
+	if (packet_buffer == NULL)
+		return;
+
+	fm_function_id = FM_GET_FUNCTION_ID(packet_buffer);
+	switch (fm_function_id) {
+	case FM_FUNCTION_ENABLE:
+	case FM_FUNCTION_DISABLE:
+	case FM_FUNCTION_RESET:
+		FM_DEBUG_REPORT(
+			"fmd_process_fm_function: "
+			"command success received for %s",
+			fmd_get_fm_function_name(fm_function_id));
+		/* Release the semaphore since response is received */
+		fmd_set_cmd_sem();
+		break;
+	case FM_FUNCTION_WRITE_COMMAND:
+		FM_DEBUG_REPORT(
+			"fmd_process_fm_function: "
+			"command success received for %s",
+			fmd_get_fm_function_name(fm_function_id));
+
+		fmd_data.cmd_id = FM_GET_CMD_ID(packet_buffer);
+		fmd_data.num_parameters =
+			FM_GET_NUM_PARAMS(packet_buffer);
+
+		FM_DEBUG_REPORT(
+			"fmd_process_fm_function: "
+			"Cmd Id = 0x%04x, Num Of Parms = %02x",
+			fmd_data.cmd_id, fmd_data.num_parameters);
+
+		if (fmd_data.num_parameters) {
+			while (count <
+				(fmd_data.num_parameters * sizeof(u16))) {
+				fmd_data.parameters[count] =
+				*(FM_GET_RSP_BUFFER_ADDR(packet_buffer) + count);
+				count++;
+			}
+		}
+
+		/* Release the semaphore since response is received */
+		fmd_set_cmd_sem();
+		break;
+	case FM_FUNCTION_FIRMWARE_DOWNLOAD:
+		block_id = FM_GET_BLOCK_ID(packet_buffer);
+		FM_DEBUG_REPORT(
+			"fmd_process_fm_function: "
+			"command success received for %s"
+			"block id = %02x",
+			fmd_get_fm_function_name(fm_function_id),
+			block_id);
+		/* Release the semaphore since response is received */
+		fmd_set_cmd_sem();
+		break;
+	default:
+		FM_ERR_REPORT(
+			"fmd_process_fm_function: "
+			"default case: command success received for %s",
+			fmd_get_fm_function_name(fm_function_id));
+		break;
+	}
+}
+
+/**
+ * fmd_write_file_block() - download firmware.
+ *
+ * This Function adds the header for downloading
+ * the firmware and coeffecient files and sends it to Protocol Driver.
+ * @file_block_id: Block ID of the F/W to be transmitted to FM Chip
+ * @file_block: Buffer containing the bytes to be sent.
+ * @file_block_length: Size of the Firmware buffer.
+ *
+ * Returns:
+ *   0: If there is no error.
+ *   -EINVAL: If parameters are not valid.
+ *   -ETIME: Otherwise
+ */
+static int fmd_write_file_block(
+			u32 file_block_id,
+			u8 *file_block,
+			u16 file_block_length
+			)
+{
+	int err;
+
+	FM_INFO_REPORT("fmd_write_file_block");
+	if (file_block == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	mutex_lock(&send_cmd_mutex);
+	file_block[0] = file_block_length + FM_HCI_WRITE_FILE_BLK_PARAM_LEN;
+	file_block[1] = FM_CATENA_OPCODE;
+	file_block[2] = FM_WRITE;
+	file_block[3] = FM_FUNCTION_FIRMWARE_DOWNLOAD;
+	file_block[4] = file_block_id;
+	/* Send the Packet */
+	err = fmd_send_packet(
+				file_block_length +
+				FM_HCI_WRITE_FILE_BLK_HEADER_LEN,
+				file_block);
+
+	/* wait till response comes */
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+
+error:
+	mutex_unlock(&send_cmd_mutex);
+	FM_DEBUG_REPORT("fmd_write_file_block: "
+		"returning %d", err);
+	return err;
+}
+
+int fmd_init(void)
+{
+	int err;
+
+	if (!fmd_driver_init()) {
+		err = -EIO;
+		goto error;
+	}
+
+	memset(&fmd_state_info, 0, sizeof(fmd_state_info));
+	fmd_state_info.fmd_initialized = true;
+	fmd_state_info.gocmd = FMD_STATE_NONE;
+	fmd_state_info.mode = FMD_MODE_IDLE;
+	fmd_state_info.callback = NULL;
+	fmd_state_info.rx_freq_range = FMD_FREQRANGE_EUROAMERICA;
+	fmd_state_info.rx_stereo_mode = FMD_STEREOMODE_BLENDING;
+	fmd_state_info.rx_volume = MAX_ANALOG_VOLUME;
+	fmd_state_info.rx_antenna = FMD_ANTENNA_EMBEDDED;
+	fmd_state_info.rx_rds_on = false;
+	fmd_state_info.rx_seek_stop_level = DEFAULT_RSSI_THRESHOLD;
+	fmd_state_info.tx_freq_range = FMD_FREQRANGE_EUROAMERICA;
+	fmd_state_info.tx_preemphasis = FMD_EMPHASIS_75US;
+	fmd_state_info.tx_pilot_dev = DEFAULT_PILOT_DEVIATION;
+	fmd_state_info.tx_rds_dev = DEFAULT_RDS_DEVIATION;
+	fmd_state_info.tx_strength = MAX_POWER_LEVEL;
+	fmd_state_info.max_channels_to_scan = DEFAULT_CHANNELS_TO_SCAN;
+	fmd_state_info.tx_stereo_mode = true;
+	fmd_state_info.irq_index = 0;
+	spin_lock_init(&fmd_spinlock);
+	err = 0;
+
+error:
+	FM_DEBUG_REPORT("fmd_init returning = %d", err);
+	return err;
+}
+
+void fmd_exit(void)
+{
+	fmd_set_interrupt_sem();
+	fmd_driver_exit();
+	memset(&fmd_state_info, 0, sizeof(fmd_state_info));
+}
+
+int fmd_register_callback(
+			fmd_radio_cb callback
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	fmd_state_info.callback = callback;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_version(
+			u16 *version
+			)
+{
+	int err;
+	int io_result;
+	u16  response_count;
+	u16  response_data[CMD_GET_VERSION_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (version == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_GET_VERSION,
+			CMD_GET_VERSION_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	memcpy(version,
+		response_data,
+		sizeof(u16) * CMD_GET_VERSION_RSP_PARAM_LEN);
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_mode(
+			u8 mode
+			)
+{
+	int err;
+	u16 parameters[CMD_GOTO_MODE_PARAM_LEN];
+    int io_result;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (mode > FMD_MODE_TX) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = mode;
+
+	fmd_state_info.gocmd = FMD_STATE_MODE;
+	FM_ERR_REPORT("Sending Set Mode");
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_GOTO_MODE,
+			CMD_GOTO_MODE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem()) {
+		err = -ETIME;
+		goto error;
+	}
+	fmd_state_info.mode = mode;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_freq_range_properties(
+			u8 range,
+			u32 *min_freq,
+			u32 *max_freq
+			)
+{
+	int err;
+
+	if (min_freq == NULL || max_freq == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	switch (range) {
+	case FMD_FREQRANGE_EUROAMERICA:
+		*min_freq =  FMD_EU_US_MIN_FREQ_IN_KHZ;
+		*max_freq = FMD_EU_US_MAX_FREQ_IN_KHZ;
+		break;
+	case FMD_FREQRANGE_JAPAN:
+		*min_freq = FMD_JAPAN_MIN_FREQ_IN_KHZ;
+		*max_freq = FMD_JAPAN_MAX_FREQ_IN_KHZ;
+		break;
+	case FMD_FREQRANGE_CHINA:
+		*min_freq =  FMD_CHINA_MIN_FREQ_IN_KHZ;
+		*max_freq = FMD_CHINA_MAX_FREQ_IN_KHZ;
+		break;
+	default:
+		*min_freq =  FMD_EU_US_MIN_FREQ_IN_KHZ;
+		*max_freq = FMD_EU_US_MAX_FREQ_IN_KHZ;
+		break;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_antenna(
+			u8 antenna
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_ANTENNA_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (antenna > FMD_ANTENNA_WIRED) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = antenna;
+
+	fmd_state_info.gocmd = FMD_STATE_ANTENNA;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SET_ANTENNA,
+			CMD_SET_ANTENNA_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.rx_antenna = antenna;
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_antenna(
+			u8 *antenna
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	*antenna = fmd_state_info.rx_antenna;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_freq_range(
+			u8 range
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TN_SET_BAND_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = range;
+	parameters[1] = FMD_MIN_CHANNEL_NUMBER;
+	parameters[2] = FMD_MAX_CHANNEL_NUMBER;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_TN_SET_BAND,
+			CMD_TN_SET_BAND_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+	fmd_state_info.rx_freq_range = range;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_freq_range(
+			u8 *range
+			)
+{
+	int err;
+
+	if (range == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	*range = fmd_state_info.rx_freq_range;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_grid(
+			u8 grid
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TN_SET_GRID_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (grid > FMD_GRID_200KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = grid;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_TN_SET_GRID,
+			CMD_TN_SET_GRID_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_frequency(
+			u32 freq
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (freq > FMD_EU_US_MAX_FREQ_IN_KHZ ||
+		freq < FMD_CHINA_MIN_FREQ_IN_KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_rx_frequency_to_channel(
+			freq,
+			&parameters[0]);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_FREQUENCY;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_TUNE_SET_CHANNEL,
+			CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_frequency(
+			u32 *freq
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16 response_data[CMD_SP_TUNE_GET_CHANNEL_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (freq == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+				CMD_FMR_SP_TUNE_GET_CHANNEL,
+				CMD_SP_TUNE_GET_CHANNEL_PARAM_LEN,
+				NULL,
+				&response_count,
+				response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	io_result = fmd_rx_channel_to_frequency(
+			response_data[0], /* 1st byte is the Frequency */
+			freq);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_stereo_mode(
+			u8 mode
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_RP_STEREO_SET_MODE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (mode > FMD_STEREOMODE_BLENDING) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = mode;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_RP_STEREO_SET_MODE,
+			CMD_RP_STEREO_SET_MODE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.rx_stereo_mode = mode;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_stereo_ctrl_blending_rssi(
+			u16 min_rssi,
+			u16 max_rssi)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_RP_STEREO_SET_CONTROL_BLENDING_RSSI_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = min_rssi;
+	parameters[1] = max_rssi;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_RP_STEREO_SET_CONTROL_BLENDING_RSSI,
+			CMD_RP_STEREO_SET_CONTROL_BLENDING_RSSI_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_stereo_mode(
+			u8 *mode
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16 response_data[CMD_RP_GET_STATE_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (mode == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+				CMD_FMR_RP_GET_STATE,
+				CMD_RP_GET_STATE_PARAM_LEN,
+				NULL,
+				&response_count,
+				response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	/* 2nd element of response is stereo signal */
+	*mode = response_data[1];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_signal_strength(
+			u16 *strength
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16 response_data[CMD_RP_GET_RSSI_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (strength == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_RP_GET_RSSI,
+			CMD_RP_GET_RSSI_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	*strength = response_data[0]; /* 1st byte is the signal strength */
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_stop_level(
+			u16 stoplevel
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	fmd_state_info.rx_seek_stop_level = stoplevel;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_stop_level(
+			u16 *stop_level
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (stop_level == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*stop_level = fmd_state_info.rx_seek_stop_level;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_seek(
+			bool upwards
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_SEARCH_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (upwards)
+		parameters[0] = 0x0000;
+	else
+		parameters[0] = 0x0001;
+	parameters[1] = fmd_state_info.rx_seek_stop_level;
+	parameters[2] = DEFAULT_PEAK_NOISE_VALUE;
+	parameters[3] = DEFAULT_AVERAGE_NOISE_MAX_VALUE;
+	fmd_state_info.gocmd = FMD_STATE_SEEK;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_SEARCH_START,
+			CMD_SP_SEARCH_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_scan_band(
+			u8 max_channels_to_scan
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_SCAN_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (max_channels_to_scan > MAX_CHANNELS_TO_SCAN) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = max_channels_to_scan;
+	parameters[1] = fmd_state_info.rx_seek_stop_level;
+	parameters[2] = DEFAULT_PEAK_NOISE_VALUE;
+	parameters[3] = DEFAULT_AVERAGE_NOISE_MAX_VALUE;
+
+	fmd_state_info.gocmd = FMD_STATE_SCAN_BAND;
+	fmd_state_info.max_channels_to_scan = max_channels_to_scan;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_SCAN_START,
+			CMD_SP_SCAN_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_max_channels_to_scan(
+			u8 *max_channels_to_scan
+			)
+{
+	int err;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (max_channels_to_scan == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*max_channels_to_scan = fmd_state_info.max_channels_to_scan;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_scan_band_info(
+			u32 index,
+			u16 *num_channels,
+			u16 *channels,
+			u16 *rssi
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_SCAN_GET_RESULT_PARAM_LEN];
+	u16 response_count;
+	u16 response_data[CMD_SP_SCAN_GET_RESULT_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (num_channels == NULL || rssi == NULL || channels == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = index;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_SCAN_GET_RESULT,
+			CMD_SP_SCAN_GET_RESULT_PARAM_LEN,
+			parameters,
+			&response_count,
+			response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	/* 1st byte indicates number of channels found */
+	*num_channels = response_data[0];
+	/* 2nd byte indicates 1st channel number */
+	channels[0] = response_data[1];
+	/* 3rd byte indicates RSSI of corresponding channel */
+	rssi[0] = response_data[2];
+	/* 4th byte indicates 2nd channel number */
+	channels[1] = response_data[3];
+	/* 5th byte indicates RSSI of corresponding channel */
+	rssi[1] = response_data[4];
+	/* 6th byte indicates 3rd channel number */
+	channels[2] = response_data[5];
+	/* 7th byte indicates RSSI of corresponding channel */
+	rssi[2] = response_data[6];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_block_scan(
+			u32 start_freq,
+			u32 stop_freq,
+			u8 antenna
+			)
+{
+	u16 start_channel;
+	u16 stop_channel;
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_BLOCK_SCAN_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (antenna > FMD_ANTENNA_WIRED) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (start_freq > FMD_EU_US_MAX_FREQ_IN_KHZ ||
+		start_freq < FMD_CHINA_MIN_FREQ_IN_KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (stop_freq > FMD_EU_US_MAX_FREQ_IN_KHZ ||
+		stop_freq < FMD_CHINA_MIN_FREQ_IN_KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	/* Convert the start frequency to corresponsing channel */
+	switch (fmd_state_info.mode) {
+	case FMD_MODE_RX:
+		io_result = fmd_rx_frequency_to_channel(
+					start_freq,
+					&start_channel);
+		break;
+	case FMD_MODE_TX:
+		io_result = fmd_tx_frequency_to_channel(
+					start_freq,
+					&start_channel);
+		break;
+	default:
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	/* Convert the end frequency to corresponsing channel */
+	switch (fmd_state_info.mode) {
+	case FMD_MODE_RX:
+		io_result = fmd_rx_frequency_to_channel(
+					stop_freq,
+					&stop_channel);
+		break;
+	case FMD_MODE_TX:
+		io_result = fmd_tx_frequency_to_channel(
+					stop_freq,
+					&stop_channel);
+		break;
+	default:
+		err = -EINVAL;
+		goto error;
+	}
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	parameters[0] = start_channel;
+	parameters[1] = stop_channel;
+	parameters[2] = antenna;
+
+	fmd_state_info.gocmd = FMD_STATE_BLOCK_SCAN;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_BLOCK_SCAN_START,
+			CMD_SP_BLOCK_SCAN_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_block_scan_result(
+			u32 index,
+			u16 *num_channels,
+			u16 *rssi
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_BLOCK_SCAN_GET_RESULT_PARAM_LEN];
+	u16 response_count;
+	u16 response_data[CMD_SP_BLOCK_SCAN_GET_RESULT_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (num_channels == NULL || rssi == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = index;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_BLOCK_SCAN_GET_RESULT,
+			CMD_SP_BLOCK_SCAN_GET_RESULT_PARAM_LEN,
+			parameters,
+			&response_count,
+			response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	/*
+	 * Response packet has 1st byte as the number
+	 * of channels, and the remaining 6 bytes as
+	 * rssi values of the channels.
+	 */
+	*num_channels = response_data[0];
+	rssi[0] = response_data[1];
+	rssi[1] = response_data[2];
+	rssi[2] = response_data[3];
+	rssi[3] = response_data[4];
+	rssi[4] = response_data[5];
+	rssi[5] = response_data[6];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_stop_seeking(void)
+{
+	int err;
+	int io_result;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (!(fmd_state_info.gocmd == FMD_STATE_SEEK ||
+		fmd_state_info.gocmd == FMD_STATE_SCAN_BAND ||
+		fmd_state_info.gocmd == FMD_STATE_BLOCK_SCAN)) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_SEEK_STOP;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_STOP,
+			CMD_SP_STOP_PARAM_LEN,
+			NULL,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_af_update_start(
+			u32 freq
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SP_AF_UPDATE_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_rx_frequency_to_channel(
+			freq,
+			&parameters[0]);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_AF_UPDATE;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_AF_UPDATE_START,
+			CMD_SP_AF_UPDATE_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_af_update_result(
+			u16 *af_level
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16 response_data[CMD_SP_AF_UPDATE_GET_RESULT_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (af_level == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_AF_UPDATE_GET_RESULT,
+			CMD_SP_AF_UPDATE_GET_RESULT_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (io_result != 0)	{
+		err = io_result;
+		goto error;
+	}
+
+	/*
+	 * 1st byte of response packet is the
+	 * RSSI of the AF Frequency.
+	 */
+	*af_level = response_data[0];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_af_switch_start(
+			u32 freq,
+			u16 picode
+			)
+{
+
+	int err;
+	int io_result;
+	u16  parameters[CMD_SP_AF_SWITCH_START_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_rx_frequency_to_channel(
+			freq,
+			&parameters[0]);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	parameters[1] = picode;
+	parameters[2] = 0xFFFF; /* PI Mask */
+	parameters[3] = fmd_state_info.rx_seek_stop_level;
+	parameters[4] = 0x0000; /* Unmute when AF's PI matches expected PI */
+
+	fmd_state_info.gocmd = FMD_STATE_AF_SWITCH;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_AF_SWITCH_START,
+			CMD_SP_AF_SWITCH_START_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_af_switch_results(
+			u16 *afs_conclusion,
+			u16 *afs_level,
+			u16 *afs_pi
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16 response_data[CMD_SP_AF_SWITCH_GET_RESULT_RWSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (afs_conclusion == NULL ||
+		afs_level == NULL ||
+		afs_pi == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_SP_AF_SWITCH_GET_RESULT,
+			CMD_SP_AF_SWITCH_GET_RESULT_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (io_result != 0)	{
+		err = io_result;
+		goto error;
+	}
+
+	*afs_conclusion = response_data[0];
+	*afs_level = response_data[1];
+	*afs_pi = response_data[2];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_rds(
+			bool *on
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (on == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*on = fmd_state_info.rx_rds_on;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_buffer_set_size(
+			u8 size
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_DP_BUFFER_SET_SIZE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (size > MAX_RDS_GROUPS) {
+		err = -EIO;
+		goto error;
+	}
+
+	parameters[0] = size;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_DP_BUFFER_SET_SIZE,
+			CMD_DP_BUFFER_SET_SIZE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_buffer_set_threshold(
+			u8 threshold
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_DP_BUFFER_SET_THRESHOLD_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (threshold > MAX_RDS_GROUPS) {
+		err = -EIO;
+		goto error;
+	}
+
+	parameters[0] = threshold;
+
+	io_result = fmd_send_cmd_and_read_resp(
+		CMD_FMR_DP_BUFFER_SET_THRESHOLD,
+		CMD_DP_BUFFER_SET_THRESHOLD_PARAM_LEN,
+		parameters,
+		NULL,
+		NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_rds(
+			u8 on_off_state
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_DP_SET_CONTROL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	switch (on_off_state) {
+	case FMD_SWITCH_ON_RDS_SIMULATOR:
+		parameters[0] = 0xFFFF;
+		break;
+	case FMD_SWITCH_OFF_RDS:
+	default:
+		parameters[0] = 0x0000;
+		fmd_state_info.rx_rds_on = false;
+		break;
+	case FMD_SWITCH_ON_RDS:
+		parameters[0] = 0x0001;
+		fmd_state_info.rx_rds_on = true;
+		break;
+	case FMD_SWITCH_ON_RDS_ENHANCED_MODE:
+		parameters[0] = 0x0002;
+		fmd_state_info.rx_rds_on = true;
+		break;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_DP_SET_CONTROL,
+			CMD_DP_SET_CONTROL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_rds_group_rejection(
+			u8 on_off_state
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_DP_SET_GROUP_REJECTION_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (on_off_state == FMD_RDS_GROUP_REJECTION_ON)
+		parameters[0] = 0x0001;
+	else if (on_off_state == FMD_RDS_GROUP_REJECTION_OFF)
+		parameters[0] = 0x0000;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_DP_SET_GROUP_REJECTION,
+			CMD_DP_SET_GROUP_REJECTION_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_get_low_level_rds_groups(
+			u8 index,
+			u16 *block1,
+			u16 *block2,
+			u16 *block3,
+			u16 *block4,
+			u8 *status1,
+			u8 *status2,
+			u8 *status3,
+			u8 *status4
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (block1 == NULL ||
+		block2 == NULL ||
+		block3 == NULL ||
+		block4 == NULL ||
+		status1 == NULL ||
+		status2 == NULL ||
+		status3 == NULL ||
+		status4 == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*block1  = fmd_state_info.rds_group[index].block[0];
+	*block2  = fmd_state_info.rds_group[index].block[1];
+	*block3  = fmd_state_info.rds_group[index].block[2];
+	*block4  = fmd_state_info.rds_group[index].block[3];
+	*status1 = fmd_state_info.rds_group[index].status[0];
+	*status2 = fmd_state_info.rds_group[index].status[1];
+	*status3 = fmd_state_info.rds_group[index].status[2];
+	*status4 = fmd_state_info.rds_group[index].status[3];
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_rx_set_deemphasis(
+			u8 deemphasis
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_RP_SET_DEEMPHASIS_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	switch (deemphasis)	{
+	case FMD_EMPHASIS_50US:
+		parameters[0] = FMD_EMPHASIS_50US;
+		break;
+
+	case FMD_EMPHASIS_75US:
+		parameters[0] = FMD_EMPHASIS_75US;
+		break;
+
+	case FMD_EMPHASIS_NONE:
+	default:
+		parameters[0] = FMD_EMPHASIS_NONE;
+		break;
+
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_RP_SET_DEEMPHASIS,
+			CMD_RP_SET_DEEMPHASIS_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_pa(
+			bool on
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_PA_SET_MODE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (on)
+		parameters[0] = 0x0001;
+	else
+		parameters[0] = 0x0000;
+
+	fmd_state_info.gocmd = FMD_STATE_PA;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_PA_SET_MODE,
+			CMD_PA_SET_MODE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_signal_strength(
+			u16 strength
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_PA_SET_CONTROL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if ((strength > MAX_POWER_LEVEL)
+		|| (strength < MIN_POWER_LEVEL)) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = strength;
+
+	fmd_state_info.gocmd = FMD_STATE_PA_LEVEL;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_PA_SET_CONTROL,
+			CMD_PA_SET_CONTROL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_strength = strength;
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_signal_strength(
+			u16 *strength
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (strength == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*strength = fmd_state_info.tx_strength;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_freq_range(
+			u8 range
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TN_SET_BAND_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (range > FMD_FREQRANGE_CHINA) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = range;
+	parameters[1] = FMD_MIN_CHANNEL_NUMBER;
+	parameters[2] = FMD_MAX_CHANNEL_NUMBER;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_TN_SET_BAND,
+			CMD_TN_SET_BAND_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_freq_range = range;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_freq_range(
+			u8 *range
+			)
+{
+	int err;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (range == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*range = fmd_state_info.tx_freq_range;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_grid(
+			u8 grid
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TN_SET_GRID_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (grid > FMD_GRID_200KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = grid;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_TN_SET_GRID,
+			CMD_TN_SET_GRID_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_preemphasis(
+			u8 preemphasis
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_RP_SET_PREEMPHASIS_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	switch (preemphasis)	{
+	case FMD_EMPHASIS_50US:
+		parameters[0] = FMD_EMPHASIS_50US;
+		break;
+	case FMD_EMPHASIS_75US:
+	default:
+		parameters[0] = FMD_EMPHASIS_75US;
+		break;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_RP_SET_PREEMPHASIS,
+			CMD_RP_SET_PREEMPHASIS_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_preemphasis = preemphasis;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_preemphasis(
+			u8 *preemphasis
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (preemphasis == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*preemphasis = fmd_state_info.tx_preemphasis;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_frequency(
+			u32 freq
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (freq > FMD_EU_US_MAX_FREQ_IN_KHZ ||
+		freq < FMD_CHINA_MIN_FREQ_IN_KHZ) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_tx_frequency_to_channel(
+			freq,
+			&parameters[0]);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_FREQUENCY;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_SP_TUNE_SET_CHANNEL,
+			CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_frequency(
+			u32 *freq
+			)
+{
+	int err;
+	int io_result;
+	u16 response_count;
+	u16  response_data[CMD_SP_TUNE_GET_CHANNEL_RSP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (freq == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_SP_TUNE_GET_CHANNEL,
+			CMD_SP_TUNE_GET_CHANNEL_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	io_result = fmd_tx_channel_to_frequency(
+			response_data[0], /* 1st byte is the Frequency */
+			freq);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_enable_stereo_mode(
+			bool enable_stereo_mode
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_RP_STEREO_SET_MODE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = enable_stereo_mode;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_RP_STEREO_SET_MODE,
+			CMD_RP_STEREO_SET_MODE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_stereo_mode = enable_stereo_mode;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_stereo_mode(
+			bool *stereo_mode
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (stereo_mode == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*stereo_mode = fmd_state_info.tx_stereo_mode;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_pilot_deviation(
+			u16 deviation
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_RP_SET_PILOT_DEVIATION_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (deviation > MAX_PILOT_DEVIATION) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = deviation;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_RP_SET_PILOT_DEVIATION,
+			CMD_RP_SET_PILOT_DEVIATION_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_pilot_dev = deviation;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_pilot_deviation(
+			u16 *deviation
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (deviation == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*deviation = fmd_state_info.tx_pilot_dev;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_rds_deviation(
+			u16 deviation
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_RP_SET_RDS_DEVIATION_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (deviation > MAX_RDS_DEVIATION) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = deviation;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_RP_SET_RDS_DEVIATION,
+			CMD_RP_SET_RDS_DEVIATION_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_rds_dev = deviation;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_get_rds_deviation(
+			u16 *deviation
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (deviation == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*deviation = fmd_state_info.tx_rds_dev;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_rds(
+			bool on
+			)
+{
+	int err;
+	int io_result;
+	u16  parameters[CMD_DP_SET_CONTROL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (on)
+		parameters[0] = 0x0001;
+	else
+		parameters[0] = 0x0000;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_DP_SET_CONTROL,
+			CMD_DP_SET_CONTROL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.tx_rds_on = on;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_set_group(
+			u16 position,
+			u8  *block1,
+			u8  *block2,
+			u8  *block3,
+			u8  *block4
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_DP_BUFFER_SET_GROUP_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (block1 == NULL ||
+		block2 == NULL ||
+		block3 == NULL ||
+		block4 == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = position;
+	memcpy(&parameters[1], block1, sizeof(u16));
+	memcpy(&parameters[2], block2, sizeof(u16));
+	memcpy(&parameters[3], block3, sizeof(u16));
+	memcpy(&parameters[4], block4, sizeof(u16));
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_DP_BUFFER_SET_GROUP,
+			CMD_DP_BUFFER_SET_GROUP_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_tx_buffer_set_size(
+			u16 buffer_size
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_DP_BUFFER_SET_SIZE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = buffer_size;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_DP_BUFFER_SET_SIZE,
+			CMD_DP_BUFFER_SET_SIZE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+
+}
+
+int fmd_tx_get_rds(
+			bool *on
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (on == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*on = fmd_state_info.tx_rds_on;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_balance(
+			s8 balance
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_BALANCE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	/* Convert balance from percentage to chip number */
+	parameters[0] = (((s16)balance) * FMD_MAX_BALANCE) / 100;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_AUP_SET_BALANCE,
+			CMD_SET_BALANCE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_volume(
+			u8 volume
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_VOLUME_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	/* Convert volume from percentage to chip number */
+	parameters[0] = (((u16)volume) * FMD_MAX_VOLUME) / 100;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_AUP_SET_VOLUME,
+			CMD_SET_VOLUME_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	fmd_state_info.rx_volume = volume;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_get_volume(
+			u8 *volume
+			)
+{
+	int err;
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (volume == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	*volume = fmd_state_info.rx_volume;
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_mute(
+			bool mute_on
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_MUTE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (!mute_on)
+		parameters[0] = 0x0000;
+	else
+		parameters[0] = 0x0001;
+	parameters[1] = 0x0001;
+
+	fmd_state_info.gocmd = FMD_STATE_MUTE;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_AUP_SET_MUTE,
+			CMD_SET_MUTE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_ext_set_mute(
+			bool mute_on
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_EXT_SET_MUTE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (!mute_on)
+		parameters[0] = 0x0000;
+	else
+		parameters[0] = 0x0001;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_AUP_EXT_SET_MUTE,
+			CMD_EXT_SET_MUTE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_power_up(void)
+{
+	int err;
+	int io_result;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	fmd_state_info.gocmd = FMD_STATE_GEN_POWERUP;
+	FM_ERR_REPORT("Sending Gen Power Up");
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_POWERUP,
+			CMD_POWERUP_PARAM_LEN,
+			NULL,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_goto_standby(void)
+{
+	int err;
+	int io_result;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_GOTO_STANDBY,
+			CMD_GOTO_STANDBY_PARAM_LEN,
+			NULL,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_goto_power_down(void)
+{
+	int err;
+	int io_result;
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_GOTO_POWERDOWN,
+			CMD_GOTO_POWERDOWN_PARAM_LEN,
+			NULL,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_select_ref_clk(
+			u16 ref_clk
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SELECT_REFERENCE_CLOCK_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = ref_clk;
+
+	fmd_state_info.gocmd = FMD_STATE_SELECT_REF_CLK;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_SELECT_REFERENCE_CLOCK,
+			CMD_SELECT_REFERENCE_CLOCK_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_set_ref_clk_pll(
+			u16 freq
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_SET_REFERENCE_CLOCK_PLL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	parameters[0] = freq;
+
+	fmd_state_info.gocmd = FMD_STATE_SET_REF_CLK_PLL;
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_GEN_SET_REFERENCE_CLOCK_PLL,
+			CMD_SET_REFERENCE_CLOCK_PLL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		fmd_state_info.gocmd = FMD_STATE_NONE;
+		err = io_result;
+		goto error;
+	}
+
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+	else
+		err = 0;
+
+error:
+	return err;
+}
+
+int fmd_send_fm_ip_enable(void)
+{
+	int err;
+	u8 fm_ip_enable_cmd[CMD_IP_ENABLE_CMD_LEN];
+
+	mutex_lock(&send_cmd_mutex);
+	fm_ip_enable_cmd[0] = CMD_IP_ENABLE_PARAM_LEN;
+	fm_ip_enable_cmd[1] = FM_CATENA_OPCODE;
+	fm_ip_enable_cmd[2] = FM_WRITE ;
+	fm_ip_enable_cmd[3] = FM_FUNCTION_ENABLE;
+
+	/* Send the Packet */
+	err = fmd_send_packet(
+				CMD_IP_ENABLE_CMD_LEN,
+				fm_ip_enable_cmd);
+
+	/* Check the ErrorCode */
+	if (err != 0)
+		goto error;
+
+	/* wait till response comes */
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+
+error:
+	mutex_unlock(&send_cmd_mutex);
+	return err;
+}
+
+int fmd_send_fm_ip_disable(void)
+{
+	int err;
+	u8 fm_ip_disable_cmd[CMD_IP_DISABLE_CMD_LEN];
+
+	mutex_lock(&send_cmd_mutex);
+	fm_ip_disable_cmd[0] = CMD_IP_DISABLE_PARAM_LEN;
+	fm_ip_disable_cmd[1] = FM_CATENA_OPCODE;
+	fm_ip_disable_cmd[2] = FM_WRITE ;
+	fm_ip_disable_cmd[3] = FM_FUNCTION_DISABLE;
+
+	/* Send the Packet */
+	err = fmd_send_packet(
+				CMD_IP_DISABLE_CMD_LEN,
+				fm_ip_disable_cmd);
+
+	/* Check the ErrorCode */
+	if (err != 0)
+		goto error;
+
+	/* wait till response comes */
+	if (fmd_get_cmd_sem())
+		err = -ETIME;
+
+error:
+	mutex_unlock(&send_cmd_mutex);
+	return err;
+}
+
+int fmd_send_fm_firmware(
+			u8 *fw_buffer,
+			u16 fw_size
+			)
+{
+	int err = -EINVAL;
+	u16 bytes_to_write = ST_WRITE_FILE_BLK_SIZE -
+				FM_HCI_WRITE_FILE_BLK_PARAM_LEN;
+	u16 bytes_remaining = fw_size;
+	u8 fm_firmware_data[ST_WRITE_FILE_BLK_SIZE + FM_HCI_CMD_HEADER_LEN];
+	u32 block_id = 0;
+
+	if (fw_buffer == NULL) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	while (bytes_remaining > 0) {
+		if (bytes_remaining <
+			(ST_WRITE_FILE_BLK_SIZE -
+			 FM_HCI_WRITE_FILE_BLK_PARAM_LEN))
+			bytes_to_write = bytes_remaining;
+
+		/*
+		 * Five bytes of HCI Header for FM Firmware
+		 * so shift the firmware data by 5 bytes
+		 */
+		memcpy(
+			fm_firmware_data + FM_HCI_WRITE_FILE_BLK_HEADER_LEN,
+			fw_buffer, bytes_to_write);
+		err = fmd_write_file_block(
+				block_id,
+				fm_firmware_data,
+				bytes_to_write);
+		if (err) {
+			FM_DEBUG_REPORT("fmd_send_fm_firmware: "
+				"Failed to download %d Block "
+				"error = %d", (unsigned int)block_id, err);
+			goto error;
+		}
+		/*
+		 * Increment the Block Id by 1, since one
+		 * block is successfully transmitted
+		 * to the chip.
+		 */
+		block_id++;
+		/*
+		 * Increment the next firmware buffer equal
+		 * to the number of bytes transmitted.
+		 */
+		fw_buffer += bytes_to_write;
+		/*
+		 * Decrement the number of bytes remaining
+		 * equal to number of bytes transmitted successfully.
+		 */
+		bytes_remaining -= bytes_to_write;
+
+		if (block_id == ST_MAX_NUMBER_OF_FILE_BLOCKS)
+			block_id = 0;
+	}
+
+error:
+	return err;
+}
+
+int fmd_int_bufferfull(
+			u16 *number_of_rds_groups
+			)
+{
+	u16 response_count;
+	u16 response_data[CMD_DP_BUFFER_GET_GROUP_COUNT_PARAM_LEN];
+	u16 index = 0;
+	u16 rds_group_count;
+	u8 result = -ENOEXEC;
+	struct fmd_rds_group rds_group;
+
+	if (!fmd_state_info.rx_rds_on)
+		goto error;
+
+	/* get group count*/
+	result = fmd_send_cmd_and_read_resp(
+			CMD_FMR_DP_BUFFER_GET_GROUP_COUNT,
+			CMD_DP_BUFFER_GET_GROUP_COUNT_PARAM_LEN,
+			NULL,
+			&response_count,
+			response_data);
+
+	if (result != 0)
+		goto error;
+
+	/* read RDS groups */
+	rds_group_count = FM_GET_NUM_RDS_GRPS(response_data);
+	if (rds_group_count > MAX_RDS_GROUPS)
+		rds_group_count = MAX_RDS_GROUPS;
+
+	*number_of_rds_groups = rds_group_count;
+
+	if (rds_group_count) {
+		FM_DEBUG_REPORT("rds_group_count = %d", rds_group_count);
+		while (rds_group_count-- && fmd_state_info.rx_rds_on) {
+			result = fmd_send_cmd_and_read_resp(
+					CMD_FMR_DP_BUFFER_GET_GROUP,
+					CMD_DP_BUFFER_GET_GROUP_PARAM_LEN,
+					NULL,
+					&response_count,
+					(u16 *)&rds_group);
+
+			if (result != 0)
+				goto error;
+
+			if (fmd_state_info.rx_rds_on)
+				fmd_state_info.rds_group[index++] = rds_group;
+		}
+	}
+error:
+	return result;
+}
+
+void fmd_start_rds_thread(
+			cg2900_fm_rds_cb cb_func
+			)
+{
+	FM_INFO_REPORT("fmd_start_rds_thread");
+	cb_rds_func = cb_func;
+	rds_thread_required = true;
+	rds_thread_task = kthread_create(fmd_rds_thread, NULL, "rds_thread");
+	if (IS_ERR(rds_thread_task)) {
+		FM_ERR_REPORT("fmd_start_rds_thread: "
+			      "Unable to Create rds_thread");
+		rds_thread_task = NULL;
+		rds_thread_required = false;
+		return;
+	}
+	wake_up_process(rds_thread_task);
+}
+
+void fmd_stop_rds_thread(void)
+{
+	FM_INFO_REPORT("fmd_stop_rds_thread");
+	/* In case thread is waiting, set the rds sem */
+	fmd_set_rds_sem();
+	/* Re-initialize RDS Semaphore to zero */
+	sema_init(&rds_sem, 0);
+	cb_rds_func = NULL;
+	rds_thread_required = false;
+	/* Wait for RDS thread to exit gracefully */
+	fmd_get_rds_sem();
+
+	if (rds_thread_task)
+		rds_thread_task = NULL;
+}
+
+void fmd_get_rds_sem(void)
+{
+	int ret_val;
+
+	FM_DEBUG_REPORT("fmd_get_rds_sem");
+	ret_val = down_killable(&rds_sem);
+
+	if (ret_val)
+		FM_ERR_REPORT("fmd_get_rds_sem: down_killable "
+			"returned error = %d", ret_val);
+}
+
+void fmd_set_rds_sem(void)
+{
+	FM_DEBUG_REPORT("fmd_set_rds_sem");
+	up(&rds_sem);
+}
+
+int fmd_set_dev(struct device *dev)
+{
+	struct cg2900_user_data *pf_data;
+
+	FM_DEBUG_REPORT("fmd_set_dev");
+
+	if (dev && cg2900_fm_dev) {
+		FM_ERR_REPORT("Only one FM device supported");
+		return -EACCES;
+	}
+
+	cg2900_fm_dev = dev;
+
+	if (!dev)
+		return 0;
+
+	pf_data = dev_get_platdata(dev);
+	pf_data->dev = dev;
+	pf_data->read_cb = fmd_read_cb;
+	pf_data->reset_cb = fmd_reset_cb;
+
+	return 0;
+}
+
+int fmd_set_test_tone_generator_status(
+			u8 test_tone_status
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TST_TONE_ENABLE_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (test_tone_status > FMD_TST_TONE_ON_WO_SRC) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = test_tone_status;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_TST_TONE_ENABLE,
+			CMD_TST_TONE_ENABLE_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_test_tone_connect(
+			u8 left_audio_mode,
+			u8 right_audio_mode
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TST_TONE_CONNECT_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (left_audio_mode > FMD_TST_TONE_AUDIO_TONE_SUM ||
+		right_audio_mode > FMD_TST_TONE_AUDIO_TONE_SUM) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = left_audio_mode;
+	parameters[1] = right_audio_mode;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_TST_TONE_CONNECT,
+			CMD_TST_TONE_CONNECT_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_test_tone_set_params(
+			u8 tone_gen,
+			u16 frequency,
+			u16 volume,
+			u16 phase_offset,
+			u16 dc,
+			u8 waveform
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_TST_TONE_SET_PARAMS_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (tone_gen > FMD_TST_TONE_2 ||
+		waveform > FMD_TST_TONE_PULSE ||
+		frequency > 0x7FFF ||
+		volume > 0x7FFF) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = tone_gen;
+	parameters[1] = frequency;
+	parameters[2] = volume;
+	parameters[3] = phase_offset;
+	parameters[4] = dc;
+	parameters[5] = waveform;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_TST_TONE_SET_PARAMS,
+			CMD_TST_TONE_SET_PARAMS_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+int fmd_limiter_setcontrol(
+			u16 audio_deviation,
+			u16 notification_hold_off_time
+			)
+{
+	int err;
+	int io_result;
+	u16 parameters[CMD_FMT_RP_LIMITER_SETCONTROL_PARAM_LEN];
+
+	if (fmd_go_cmd_busy()) {
+		err = -EBUSY;
+		goto error;
+	}
+
+	if (!fmd_state_info.fmd_initialized) {
+		err = -ENOEXEC;
+		goto error;
+	}
+
+	if (audio_deviation < MIN_AUDIO_DEVIATION ||
+		audio_deviation > MAX_AUDIO_DEVIATION ||
+		notification_hold_off_time > 0x7FFF) {
+		err = -EINVAL;
+		goto error;
+	}
+
+	parameters[0] = audio_deviation;
+	parameters[1] = notification_hold_off_time;
+
+	io_result = fmd_send_cmd_and_read_resp(
+			CMD_FMT_RP_LIMITER_SETCONTROL,
+			CMD_FMT_RP_LIMITER_SETCONTROL_PARAM_LEN,
+			parameters,
+			NULL,
+			NULL);
+
+	if (io_result != 0) {
+		err = io_result;
+		goto error;
+	}
+
+	err = 0;
+
+error:
+	return err;
+}
+
+MODULE_AUTHOR("Hemant Gupta");
+MODULE_LICENSE("GPL v2");
+
+module_param(cg2900_fm_debug_level, ushort, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(cg2900_fm_debug_level, "cg2900_fm_debug_level: "
+		" *1: Only Error Logs* "
+		" 2: Info Logs "
+		" 3: Debug Logs "
+		" 4: HCI Logs");
+
diff --git a/drivers/media/radio/CG2900/cg2900_fm_driver.h b/drivers/media/radio/CG2900/cg2900_fm_driver.h
new file mode 100644
index 00000000000..62703fa4690
--- /dev/null
+++ b/drivers/media/radio/CG2900/cg2900_fm_driver.h
@@ -0,0 +1,1856 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux FM Driver for CG2900 FM Chip
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#ifndef _FMDRIVER_H_
+#define _FMDRIVER_H_
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/uaccess.h>
+#include <linux/semaphore.h>
+#include <linux/version.h>
+#include <linux/kthread.h>
+#include <linux/kernel.h>
+#include <linux/timer.h>
+#include <linux/mutex.h>
+#include "cg2900_fm_api.h"
+
+/* structure declared in cg2900_fm_driver.c */
+extern struct timespec time_spec;
+
+/* module_param declared in cg2900_fm_driver.c */
+extern unsigned short cg2900_fm_debug_level;
+
+/**
+ * enum fmd_debug_levels - FM Driver Debug Levels.
+ *
+ * @FM_NO_LOGS: No Logs are displayed.
+ * @FM_ERROR_LOGS: Only Error Logs are displayed.
+ * @FM_INFO_LOGS: Function Entry logs are displayed.
+ * @FM_DEBUG_LOGS: Full debugging support.
+ * @FM_HCI_PACKET_LOGS: HCI Packet Sent/received to/by
+ * FM Driver are displayed.
+ *
+ * Various debug levels for FM Driver.
+ */
+enum fmd_debug_levels {
+	FM_NO_LOGS,
+	FM_ERROR_LOGS,
+	FM_INFO_LOGS,
+	FM_DEBUG_LOGS,
+	FM_HCI_PACKET_LOGS
+};
+
+#define FM_HEX_REPORT(fmt, arg...) \
+	if (cg2900_fm_debug_level == FM_HCI_PACKET_LOGS) { \
+		printk(KERN_INFO fmt "\r\n" , ## arg); \
+	}
+
+#define FM_DEBUG_REPORT(fmt, arg...) \
+	if (cg2900_fm_debug_level > FM_INFO_LOGS && \
+			cg2900_fm_debug_level < FM_HCI_PACKET_LOGS) { \
+		getnstimeofday(&time_spec); \
+		printk(KERN_INFO "\n[%08x:%08x] " \
+		"CG2900_FM_Driver: " fmt "\r\n" , \
+		(unsigned int)time_spec.tv_sec, \
+		(unsigned int)time_spec.tv_nsec, ## arg); \
+	}
+
+#define FM_INFO_REPORT(fmt, arg...)    \
+	if (cg2900_fm_debug_level > FM_ERROR_LOGS && \
+			cg2900_fm_debug_level < FM_HCI_PACKET_LOGS) { \
+		getnstimeofday(&time_spec); \
+		printk(KERN_INFO "\n[%08x:%08x] " \
+		"CG2900_FM_Driver: " fmt "\r\n" , \
+		(unsigned int)time_spec.tv_sec, \
+		(unsigned int)time_spec.tv_nsec, ## arg); \
+	}
+
+#define FM_ERR_REPORT(fmt, arg...)               \
+	if (cg2900_fm_debug_level >= FM_ERROR_LOGS) { \
+		getnstimeofday(&time_spec); \
+		printk(KERN_ERR "\n[%08x:%08x] " \
+		"CG2900_FM_Driver: " fmt "\r\n" , \
+		(unsigned int)time_spec.tv_sec, \
+		(unsigned int)time_spec.tv_nsec, ## arg); \
+	}
+
+#define MAX_COUNT_OF_IRQS				16
+#define MAX_BUFFER_SIZE					512
+#define MAX_NAME_SIZE					100
+/* Maximum size of parsable data in bytes, received from CG2900 FM IP */
+#define MAX_RESP_SIZE					20
+/* Minimum Power level for CG2900. The value is in units of dBuV */
+#define MIN_POWER_LEVEL					88
+/* Maximum Power level for CG2900. The value is in units of dBuV */
+#define MAX_POWER_LEVEL					123
+/* Minimum RDS Deviation value for CG2900. The value is in units of 10 Hz */
+#define MIN_RDS_DEVIATION				0
+/* Default RDS Deviation value for CG2900. The value is in units of 10 Hz */
+#define DEFAULT_RDS_DEVIATION				200
+/* Maximum RDS Deviation value for CG2900. The value is in units of 10 Hz */
+#define MAX_RDS_DEVIATION				750
+#define FMD_EU_US_MIN_FREQ_IN_KHZ			87500
+#define FMD_EU_US_MAX_FREQ_IN_KHZ			108000
+#define FMD_JAPAN_MIN_FREQ_IN_KHZ			76000
+#define FMD_JAPAN_MAX_FREQ_IN_KHZ			90000
+#define FMD_CHINA_MIN_FREQ_IN_KHZ			70000
+#define FMD_CHINA_MAX_FREQ_IN_KHZ			108000
+#define FMD_MIN_CHANNEL_NUMBER				0
+#define FMD_MAX_CHANNEL_NUMBER				760
+/*
+ * Maximum supported balance for CG2900. This is just a hexadecimal number
+ * with no units.
+ */
+#define FMD_MAX_BALANCE					0x7FFF
+/*
+ * Maximum supported volume for CG2900. This is just a hexadecimal number
+ * with no units.
+ */
+#define FMD_MAX_VOLUME					0x7FFF
+/* Minimum Program Identification value as per RDS specification */
+#define MIN_PI_VALUE					0x0000
+/* Maximum Program Identification value as per RDS specification */
+#define MAX_PI_VALUE					0xFFFF
+/* Minimum Program Type code value as per RDS specification */
+#define MIN_PTY_VALUE					0
+/* Maximum Program Type code value as per RDS specification */
+#define MAX_PTY_VALUE					31
+/* Minimum Pilot Deviation value for CG2900. The value is in units of 10 Hz */
+#define MIN_PILOT_DEVIATION				0
+/* Default Pilot Deviation value for CG2900. The value is in units of 10 Hz */
+#define DEFAULT_PILOT_DEVIATION				675
+/* Maximum Pilot Deviation value for CG2900. The value is in units of 10 Hz */
+#define MAX_PILOT_DEVIATION				1000
+/*
+ * Default RSSI Threshold for a channel to be considered valid for CG2900.
+ * This is just a hexadecimal number with no units.
+ */
+#define DEFAULT_RSSI_THRESHOLD				0x0100
+/*
+ * Default Peak Noise level for a channel to be considered valid for CG2900.
+ * This is just a hexadecimal number with no units.
+ */
+#define DEFAULT_PEAK_NOISE_VALUE			0x0035
+/* Defines the RF level (at the antenna pin) at which the stereo blending
+ * function will stop limiting the channel separation */
+#define STEREO_BLENDING_MIN_RSSI			0x0005
+/* Defines the RF level (at the antenna pin) at which the stereo blending
+ * function will start limiting the channel separation */
+#define STEREO_BLENDING_MAX_RSSI			0x0100
+/*
+ * Default Average Noise level for a channel to be considered valid for CG2900.
+ * This is just a hexadecimal number with no units.
+ */
+#define DEFAULT_AVERAGE_NOISE_MAX_VALUE			0x0030
+/*
+ * Minimum Audio Deviation Level, as per CG2900 FM User Manual.
+ * This is units of 10 Hz.
+ */
+#define MIN_AUDIO_DEVIATION						0x157C
+/*
+ * Maximum Audio Deviation Level, as per CG2900 FM UserManual.
+ * This is units of 10 Hz.
+ */
+#define MAX_AUDIO_DEVIATION						0x3840
+#define FREQUENCY_CONVERTOR_KHZ_HZ			1000
+#define CHANNEL_FREQ_CONVERTER_MHZ			50
+/* Interrupt(s) for CG2900 */
+#define IRPT_INVALID					0x0000
+#define IRPT_OPERATION_SUCCEEDED			0x0001
+#define IRPT_OPERATION_FAILED				0x0002
+#define IRPT_RX_BUFFERFULL_TX_BUFFEREMPTY		0x0008
+#define IRPT_RX_SIGNAL_QUALITYLOW_MUTE_STATUS_CHANGED	0x0010
+#define IRPT_RX_MONO_STEREO_TRANSITION			0x0020
+#define IRPT_TX_OVERMODULATION				0x0030
+#define IRPT_RX_RDS_SYNCFOUND_TX_OVERDRIVE		0x0040
+#define IRPT_RDS_SYNC_LOST				0x0080
+#define IRPT_PI_CODE_CHANGED				0x0100
+#define IRPT_REQUESTED_BLOCK_AVAILABLE			0x0200
+#define IRPT_BUFFER_CLEARED				0x2000
+#define IRPT_WARM_BOOT_READY				0x4000
+#define IRPT_COLD_BOOT_READY				0x8000
+/* FM Commands Id */
+#define CMD_ID_NONE					0x0000
+#define CMD_AUP_EXT_SET_MUTE				0x01E2
+#define CMD_AUP_SET_BALANCE				0x0042
+#define CMD_AUP_SET_MUTE				0x0062
+#define CMD_AUP_SET_VOLUME				0x0022
+#define CMD_FMR_DP_BUFFER_GET_GROUP			0x0303
+#define CMD_FMR_DP_BUFFER_GET_GROUP_COUNT		0x0323
+#define CMD_FMR_DP_BUFFER_SET_SIZE			0x0343
+#define CMD_FMR_DP_BUFFER_SET_THRESHOLD			0x06C3
+#define CMD_FMR_DP_SET_CONTROL				0x02A3
+#define CMD_FMR_DP_SET_GROUP_REJECTION		0x0543
+#define CMD_FMR_RP_GET_RSSI				0x0083
+#define CMD_FMR_RP_GET_STATE				0x0063
+#define CMD_FMR_RP_STEREO_SET_MODE			0x0123
+#define CMD_FMR_RP_STEREO_SET_CONTROL_BLENDING_RSSI		0x0143
+#define CMD_FMR_SET_ANTENNA				0x0663
+#define CMD_FMR_SP_AF_SWITCH_GET_RESULT			0x0603
+#define CMD_FMR_SP_AF_SWITCH_START			0x04A3
+#define CMD_FMR_SP_AF_UPDATE_GET_RESULT			0x0483
+#define CMD_FMR_SP_AF_UPDATE_START			0x0463
+#define CMD_FMR_SP_BLOCK_SCAN_GET_RESULT		0x06A3
+#define CMD_FMR_SP_BLOCK_SCAN_START			0x0683
+#define CMD_FMR_SP_SCAN_GET_RESULT			0x0423
+#define CMD_FMR_SP_SCAN_START				0x0403
+#define CMD_FMR_SP_SEARCH_START				0x03E3
+#define CMD_FMR_SP_STOP					0x0383
+#define CMD_FMR_SP_TUNE_GET_CHANNEL			0x03A3
+#define CMD_FMR_SP_TUNE_SET_CHANNEL			0x03C3
+#define CMD_FMR_TN_SET_BAND				0x0023
+#define CMD_FMR_TN_SET_GRID				0x0043
+#define CMD_FMR_RP_SET_DEEMPHASIS			0x00C3
+#define CMD_FMT_DP_BUFFER_GET_POSITION			0x0204
+#define CMD_FMT_DP_BUFFER_SET_GROUP			0x0244
+#define CMD_FMT_DP_BUFFER_SET_SIZE			0x0224
+#define CMD_FMT_DP_BUFFER_SET_THRESHOLD			0x0284
+#define CMD_FMT_DP_SET_CONTROL				0x0264
+#define CMD_FMT_PA_SET_CONTROL				0x01A4
+#define CMD_FMT_PA_SET_MODE				0x01E4
+#define CMD_FMT_RP_SET_PILOT_DEVIATION			0x02A4
+#define CMD_FMT_RP_SET_PREEMPHASIS			0x00C4
+#define CMD_FMT_RP_SET_RDS_DEVIATION			0x0344
+#define CMD_FMT_RP_STEREO_SET_MODE			0x0164
+#define CMD_FMT_SP_TUNE_GET_CHANNEL			0x0184
+#define CMD_FMT_SP_TUNE_SET_CHANNEL			0x0064
+#define CMD_FMT_TN_SET_BAND				0x0024
+#define CMD_FMT_TN_SET_GRID				0x0044
+#define CMD_GEN_GET_MODE				0x0021
+#define CMD_GEN_GET_REGISTER_VALUE			0x00E1
+#define CMD_GEN_GET_VERSION				0x00C1
+#define CMD_GEN_GOTO_MODE				0x0041
+#define CMD_GEN_GOTO_POWERDOWN				0x0081
+#define CMD_GEN_GOTO_STANDBY				0x0061
+#define CMD_GEN_POWERUP					0x0141
+#define CMD_GEN_SELECT_REFERENCE_CLOCK			0x0201
+#define CMD_GEN_SET_REFERENCE_CLOCK			0x0161
+#define CMD_GEN_SET_REFERENCE_CLOCK_PLL			0x01A1
+#define CMD_GEN_SET_REGISTER_VALUE			0x0101
+#define CMD_TST_TONE_ENABLE				0x0027
+#define CMD_TST_TONE_CONNECT				0x0047
+#define CMD_TST_TONE_SET_PARAMS				0x0067
+#define CMD_FMT_RP_LIMITER_SETCONTROL				0x01C4
+
+/* FM Command Id Parameter Length */
+#define CMD_GET_VERSION_PARAM_LEN			0
+#define CMD_GET_VERSION_RSP_PARAM_LEN			7
+#define CMD_GOTO_MODE_PARAM_LEN				1
+#define CMD_SET_ANTENNA_PARAM_LEN			1
+#define CMD_TN_SET_BAND_PARAM_LEN			3
+#define CMD_TN_SET_GRID_PARAM_LEN			1
+#define CMD_SP_TUNE_SET_CHANNEL_PARAM_LEN		1
+#define CMD_SP_TUNE_GET_CHANNEL_PARAM_LEN		0
+#define CMD_SP_TUNE_GET_CHANNEL_RSP_PARAM_LEN		1
+#define CMD_RP_STEREO_SET_MODE_PARAM_LEN		1
+#define CMD_RP_STEREO_SET_CONTROL_BLENDING_RSSI_PARAM_LEN	2
+#define CMD_RP_GET_RSSI_PARAM_LEN			0
+#define CMD_RP_GET_RSSI_RSP_PARAM_LEN			1
+#define CMD_RP_GET_STATE_PARAM_LEN			0
+#define CMD_RP_GET_STATE_RSP_PARAM_LEN			2
+#define CMD_SP_SEARCH_START_PARAM_LEN			4
+#define CMD_SP_SCAN_START_PARAM_LEN			4
+#define CMD_SP_SCAN_GET_RESULT_PARAM_LEN		1
+#define CMD_SP_SCAN_GET_RESULT_RSP_PARAM_LEN		7
+#define CMD_SP_BLOCK_SCAN_START_PARAM_LEN		3
+#define CMD_SP_BLOCK_SCAN_GET_RESULT_PARAM_LEN		1
+#define CMD_SP_BLOCK_SCAN_GET_RESULT_RSP_PARAM_LEN	7
+#define CMD_SP_STOP_PARAM_LEN				0
+#define CMD_SP_AF_UPDATE_START_PARAM_LEN		1
+#define CMD_SP_AF_UPDATE_GET_RESULT_PARAM_LEN		0
+#define CMD_SP_AF_UPDATE_GET_RESULT_RSP_PARAM_LEN	1
+#define CMD_SP_AF_SWITCH_START_PARAM_LEN		5
+#define CMD_SP_AF_SWITCH_GET_RESULT_PARAM_LEN		0
+#define CMD_SP_AF_SWITCH_GET_RESULT_RWSP_PARAM_LEN	3
+#define CMD_DP_BUFFER_SET_SIZE_PARAM_LEN		1
+#define CMD_DP_BUFFER_SET_THRESHOLD_PARAM_LEN		1
+#define CMD_DP_SET_CONTROL_PARAM_LEN			1
+#define CMD_DP_SET_GROUP_REJECTION_PARAM_LEN		1
+#define CMD_PA_SET_MODE_PARAM_LEN			1
+#define CMD_PA_SET_CONTROL_PARAM_LEN			1
+#define CMD_RP_SET_PREEMPHASIS_PARAM_LEN		1
+#define CMD_RP_SET_DEEMPHASIS_PARAM_LEN			1
+#define CMD_RP_SET_PILOT_DEVIATION_PARAM_LEN		1
+#define CMD_RP_SET_RDS_DEVIATION_PARAM_LEN		1
+#define CMD_DP_BUFFER_SET_GROUP_PARAM_LEN		5
+#define CMD_SET_BALANCE_PARAM_LEN			1
+#define CMD_SET_VOLUME_PARAM_LEN			1
+#define CMD_SET_MUTE_PARAM_LEN				2
+#define CMD_EXT_SET_MUTE_PARAM_LEN			1
+#define CMD_POWERUP_PARAM_LEN				0
+#define CMD_GOTO_STANDBY_PARAM_LEN			0
+#define CMD_GOTO_POWERDOWN_PARAM_LEN			0
+#define CMD_SELECT_REFERENCE_CLOCK_PARAM_LEN		1
+#define CMD_SET_REFERENCE_CLOCK_PLL_PARAM_LEN		1
+#define CMD_DP_BUFFER_GET_GROUP_COUNT_PARAM_LEN		0
+#define CMD_DP_BUFFER_GET_GROUP_PARAM_LEN		0
+#define CMD_IP_ENABLE_CMD_LEN				4
+#define CMD_IP_ENABLE_PARAM_LEN				3
+#define CMD_IP_DISABLE_CMD_LEN				4
+#define CMD_IP_DISABLE_PARAM_LEN			3
+#define CMD_TST_TONE_ENABLE_PARAM_LEN			1
+#define CMD_TST_TONE_CONNECT_PARAM_LEN			2
+#define CMD_TST_TONE_SET_PARAMS_PARAM_LEN		6
+#define CMD_FMT_RP_LIMITER_SETCONTROL_PARAM_LEN		2
+
+/* FM HCI Command and event specific */
+#define FM_WRITE					0x00
+#define FM_READ						0x01
+#define FM_CATENA_OPCODE				0xFE
+#define HCI_CMD_FM					0xFD50
+#define HCI_CMD_VS_WRITE_FILE_BLOCK			0xFC2E
+#define FM_EVENT_ID					0x15
+#define FM_SUCCESS_STATUS				0x00
+#define FM_EVENT					0x01
+#define HCI_COMMAND_COMPLETE_EVENT			0x0E
+#define HCI_VS_DBG_EVENT				0xFF
+#define ST_WRITE_FILE_BLK_SIZE				254
+#define ST_MAX_NUMBER_OF_FILE_BLOCKS			256
+#define FM_PG1_INTERRUPT_EVENT_LEN			0x04
+#define FM_PG2_INTERRUPT_EVENT_LEN			0x06
+#define FM_HCI_CMD_HEADER_LEN				6
+#define FM_HCI_CMD_PARAM_LEN				5
+#define FM_HCI_WRITE_FILE_BLK_HEADER_LEN		5
+#define FM_HCI_WRITE_FILE_BLK_PARAM_LEN			4
+#define HCI_PACKET_INDICATOR_CMD			0x01
+#define HCI_PACKET_INDICATOR_EVENT			0x04
+#define HCI_PACKET_INDICATOR_FM_CMD_EVT			0x08
+/* FM Functions specific to CG2900 */
+#define FM_FUNCTION_ENABLE				0x00
+#define FM_FUNCTION_DISABLE				0x01
+#define FM_FUNCTION_RESET				0x02
+#define FM_FUNCTION_WRITE_COMMAND			0x10
+#define FM_FUNCTION_SET_INT_MASK_ALL			0x20
+#define FM_FUNCTION_GET_INT_MASK_ALL			0x21
+#define FM_FUNCTION_SET_INT_MASK			0x22
+#define FM_FUNCTION_GET_INT_MASK			0x23
+#define FM_FUNCTION_FIRMWARE_DOWNLOAD			0x30
+/* Command succeeded */
+#define FM_CMD_STATUS_CMD_SUCCESS			0x00
+/* HCI_ERR_HW_FAILURE when no response from the IP */
+#define FM_CMD_STATUS_HCI_ERR_HW_FAILURE		0x03
+/* HCI_ERR_INVALID_PARAMETERS. */
+#define FM_CMD_STATUS_HCI_ERR_INVALID_PARAMETERS	0x12
+/* When the host tries to send a command to an IP that hasn't been
+ * initialized.
+ */
+#define FM_CMD_STATUS_IP_UNINIT				0x15
+/* HCI_ERR_UNSPECIFIED_ERROR: any other error */
+#define FM_CMD_STATUS_HCI_ERR_UNSPECIFIED_ERROR		0x1F
+/* HCI_ERR_CMD_DISALLOWED when the host asks for an unauthorized operation
+ * (FM state transition for instance)
+ */
+#define FM_CMD_STATUS_HCI_ERR_CMD_DISALLOWED		0x0C
+/* Wrong sequence number for FM FW download command */
+#define FM_CMD_STATUS_WRONG_SEQ_NUM			0xF1
+/* Unknown file type for FM FW download command */
+#define FM_CMD_STATUS_UNKNOWN_FILE_TYPE			0xF2
+/* File version mismatch for FM FW download command */
+#define FM_CMD_STATUS_FILE_VERSION_MISMATCH		0xF3
+
+
+/**
+ * enum fmd_event - Events received.
+ *
+ * @FMD_EVENT_OPERATION_COMPLETED: Previous operation has been completed.
+ * @FMD_EVENT_ANTENNA_STATUS_CHANGED: Antenna has been changed.
+ * @FMD_EVENT_FREQUENCY_CHANGED: Frequency has been changed.
+ * @FMD_EVENT_SEEK_COMPLETED: Seek operation has completed.
+ * @FMD_EVENT_SCAN_BAND_COMPLETED: Band Scan completed.
+ * @FMD_EVENT_BLOCK_SCAN_COMPLETED: Block Scan completed.
+ * @FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE: Af Update or AF Switch is complete.
+ * @FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE: Mono stereo transition is
+ * completed.
+ * @FMD_EVENT_SEEK_STOPPED: Previous Seek/Band Scan/ Block Scan operation is
+ * stopped.
+ * @FMD_EVENT_GEN_POWERUP: FM IP Powerup has been powered up.
+ * @FMD_EVENT_RDSGROUP_RCVD: RDS Groups Full interrupt.
+ * @FMD_EVENT_LAST_ELEMENT: Last event, used for keeping count of
+ * number of events.
+ *
+ * Various events received from FM driver for Upper Layer(s) processing.
+ */
+enum fmd_event {
+	FMD_EVENT_OPERATION_COMPLETED,
+	FMD_EVENT_ANTENNA_STATUS_CHANGED,
+	FMD_EVENT_FREQUENCY_CHANGED,
+	FMD_EVENT_SEEK_COMPLETED,
+	FMD_EVENT_SCAN_BAND_COMPLETED,
+	FMD_EVENT_BLOCK_SCAN_COMPLETED,
+	FMD_EVENT_AF_UPDATE_SWITCH_COMPLETE,
+	FMD_EVENT_MONO_STEREO_TRANSITION_COMPLETE,
+	FMD_EVENT_SEEK_STOPPED,
+	FMD_EVENT_GEN_POWERUP,
+	FMD_EVENT_RDSGROUP_RCVD,
+	FMD_EVENT_LAST_ELEMENT
+};
+
+/**
+ * enum fmd_mode - FM Driver Modes.
+ *
+ * @FMD_MODE_IDLE: FM Driver in Idle mode.
+ * @FMD_MODE_RX: FM Driver in Rx mode.
+ * @FMD_MODE_TX: FM Driver in Tx mode.
+ *
+ * Various Modes of FM Radio.
+ */
+enum fmd_mode {
+	FMD_MODE_IDLE,
+	FMD_MODE_RX,
+	FMD_MODE_TX
+};
+
+/**
+ * enum fmd_antenna - Antenna selection.
+ *
+ * @FMD_ANTENNA_EMBEDDED: Embedded Antenna.
+ * @FMD_ANTENNA_WIRED: Wired Antenna.
+ *
+ * Antenna to be used for FM Radio.
+ */
+enum fmd_antenna {
+	FMD_ANTENNA_EMBEDDED,
+	FMD_ANTENNA_WIRED
+};
+
+/**
+ * enum fmd_grid - Grid used on FM Radio.
+ *
+ * @FMD_GRID_50KHZ: 50  kHz grid spacing.
+ * @FMD_GRID_100KHZ: 100  kHz grid spacing.
+ * @FMD_GRID_200KHZ: 200  kHz grid spacing.
+ *
+ * Spacing used on FM Radio.
+ */
+enum fmd_grid {
+	FMD_GRID_50KHZ,
+	FMD_GRID_100KHZ,
+	FMD_GRID_200KHZ
+};
+
+/**
+ * enum fmd_emphasis - De-emphasis/Pre-emphasis level.
+ *
+ * @FMD_EMPHASIS_NONE: De-emphasis Disabled.
+ * @FMD_EMPHASIS_50US: 50 us de-emphasis/pre-emphasis level.
+ * @FMD_EMPHASIS_75US: 75 us de-emphasis/pre-emphasis level.
+ *
+ * De-emphasis/Pre-emphasis level used on FM Radio.
+ */
+enum fmd_emphasis {
+	FMD_EMPHASIS_NONE = 0,
+	FMD_EMPHASIS_50US = 1,
+	FMD_EMPHASIS_75US = 2
+};
+
+/**
+ * enum fmd_freq_range - Frequency range.
+ *
+ * @FMD_FREQRANGE_EUROAMERICA: EU/US Range (87.5 - 108 MHz).
+ * @FMD_FREQRANGE_JAPAN: Japan Range (76 - 90 MHz).
+ * @FMD_FREQRANGE_CHINA: China Range (70 - 108 MHz).
+ *
+ * Various Frequency range(s) supported by FM Radio.
+ */
+enum fmd_freq_range {
+	FMD_FREQRANGE_EUROAMERICA,
+	FMD_FREQRANGE_JAPAN,
+	FMD_FREQRANGE_CHINA
+};
+
+/**
+ * enum fmd_stereo_mode - FM Driver Stereo Modes.
+ *
+ * @FMD_STEREOMODE_OFF: Streo Blending Off.
+ * @FMD_STEREOMODE_MONO: Mono Mode.
+ * @FMD_STEREOMODE_BLENDING: Blending Mode.
+ *
+ * Various Stereo Modes of FM Radio.
+ */
+enum fmd_stereo_mode {
+	FMD_STEREOMODE_OFF,
+	FMD_STEREOMODE_MONO,
+	FMD_STEREOMODE_BLENDING
+};
+
+/**
+ * enum fmd_pilot_tone - Pilot Tone Selection
+ *
+ * @FMD_PILOT_TONE_DISABLED: Pilot Tone to be disabled.
+ * @FMD_PILOT_TONE_ENABLED: Pilot Tone to be enabled.
+ *
+ * Pilot Tone to be enabled or disabled.
+ */
+enum fmd_pilot_tone {
+	FMD_PILOT_TONE_DISABLED,
+	FMD_PILOT_TONE_ENABLED
+};
+
+/**
+ * enum fmd_output - Output of Sample Rate Converter.
+ *
+ * @FMD_OUTPUT_DISABLED: Sample Rate converter in disabled.
+ * @FMD_OUTPUT_I2S: I2S Output from Sample rate converter.
+ * @FMD_OUTPUT_PARALLEL: Parallel output from sample rate converter.
+ *
+ * Sample Rate Converter's output to be set on Connectivity Controller.
+ */
+enum fmd_output {
+	FMD_OUTPUT_DISABLED,
+	FMD_OUTPUT_I2S,
+	FMD_OUTPUT_PARALLEL
+};
+
+/**
+ * enum fmd_input - Audio Input to Sample Rate Converter.
+ *
+ * @FMD_INPUT_ANALOG: Selects the ADC's as audio source
+ * @FMD_INPUT_DIGITAL: Selects Digital Input as audio source.
+ *
+ * Audio Input source for Sample Rate Converter.
+ */
+enum fmd_input {
+	FMD_INPUT_ANALOG,
+	FMD_INPUT_DIGITAL
+};
+
+/**
+ * enum fmd_rds_mode - RDS Mode to be selected for FM Rx.
+ *
+ * @FMD_SWITCH_OFF_RDS: RDS Decoding disabled in FM Chip.
+ * @FMD_SWITCH_ON_RDS: RDS Decoding enabled in FM Chip.
+ * @FMD_SWITCH_ON_RDS_ENHANCED_MODE: Enhanced RDS Mode.
+ * @FMD_SWITCH_ON_RDS_SIMULATOR: RDS Simulator switched on in FM Chip.
+ *
+ * RDS Mode to be selected for FM Rx.
+ */
+enum fmd_rds_mode {
+	FMD_SWITCH_OFF_RDS,
+	FMD_SWITCH_ON_RDS,
+	FMD_SWITCH_ON_RDS_ENHANCED_MODE,
+	FMD_SWITCH_ON_RDS_SIMULATOR
+};
+
+/**
+ * enum fmd_rds_group_rejection_mode - RDS Group Rejection
+ * to be selected for FM Rx.
+ *
+ * @FMD_RDS_GROUP_REJECTION_ON: Group rejection is enabled in FM Chip.
+ * @FMD_RDS_GROUP_REJECTION_OFF: Group rejection is disabled in FM Chip.
+ *
+ * RDS Group rejection to be selected for FM Rx.
+ */
+enum fmd_rds_group_rejection_mode {
+	FMD_RDS_GROUP_REJECTION_ON,
+	FMD_RDS_GROUP_REJECTION_OFF
+};
+
+/**
+ * enum fmd_tst_tone_status - Test Tone Generator Status.
+ *
+ * @FMD_TST_TONE_OFF: Test Tone Generator is off.
+ * @FMD_TST_TONE_ON_W_SRC: Test Tone Gen. is on with Sample Rate Conversion.
+ * @FMD_TST_TONE_ON_WO_SRC: Test Tone Gen. is on without Sample Rate Conversion.
+ *
+ * Test Tone Generator status to be set.
+ */
+enum fmd_tst_tone_status {
+	FMD_TST_TONE_OFF,
+	FMD_TST_TONE_ON_W_SRC,
+	FMD_TST_TONE_ON_WO_SRC
+};
+
+/**
+ * enum fmd_tst_tone_audio_mode - Test Tone Generator Audio Output/Input Mode.
+ *
+ * @FMD_TST_TONE_AUDIO_NORMAL: Normal Audio.
+ * @FMD_TST_TONE_AUDIO_ZERO: Zero.
+ * @FMD_TST_TONE_AUDIO_TONE_1: Tone 1.
+ * @FMD_TST_TONE_AUDIO_TONE_2: Tone 2.
+ * @FMD_TST_TONE_AUDIO_TONE_SUM: Sum of Tone 1 and Tone 2.
+ *
+ * Test Tone Generator Audio Output/Input Modes.
+ */
+enum fmd_tst_tone_audio_mode {
+	FMD_TST_TONE_AUDIO_NORMAL,
+	FMD_TST_TONE_AUDIO_ZERO,
+	FMD_TST_TONE_AUDIO_TONE_1,
+	FMD_TST_TONE_AUDIO_TONE_2,
+	FMD_TST_TONE_AUDIO_TONE_SUM
+};
+
+/**
+ * enum fmd_tst_tone - Test Tone of Internal Tone Generator.
+ *
+ * @FMD_TST_TONE_1: Test Tone 1
+ * @FMD_TST_TONE_2: Test Tone 2
+ *
+ * Test Tone.
+ */
+enum fmd_tst_tone {
+	FMD_TST_TONE_1,
+	FMD_TST_TONE_2
+};
+
+/**
+ * enum fmd_tst_tone_waveform - Test Tone Waveform of Internal Tone Generator.
+ *
+ * @FMD_TST_TONE_SINE: Sine wave
+ * @FMD_TST_TONE_PULSE: Pulse wave
+ *
+ * Test Tone waveform.
+ */
+enum fmd_tst_tone_waveform {
+	FMD_TST_TONE_SINE,
+	FMD_TST_TONE_PULSE
+};
+
+/* Callback function to receive radio events. */
+typedef void(*fmd_radio_cb)(
+		u8 event,
+		bool event_successful
+		);
+
+/**
+ * fmd_init() - Initialize the FM Driver internal structures.
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -EIO, if there is an error.
+ */
+int fmd_init(void);
+
+/**
+ * fmd_exit() - De-initialize the FM Driver.
+ */
+void fmd_exit(void);
+
+/**
+ * fmd_register_callback() - Function to register callback function.
+ *
+ * This function registers the callback function provided by upper layers.
+ * @callback: Fmradio call back Function pointer
+ *
+ * Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_register_callback(
+			fmd_radio_cb callback
+			);
+
+/**
+ * fmd_get_version() - Retrieves the FM HW and FW version.
+ *
+ * @version: (out) Version Array
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameters are not valid.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_get_version(
+			u16 *version
+			);
+
+/**
+ * fmd_set_mode() - Starts a transition to the given mode.
+ *
+ * @mode: Transition mode
+ *
+ *  Returns:
+ *	 0,  if set mode done successfully.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_mode(
+			u8 mode
+			);
+
+/**
+ * fmd_get_freq_range_properties() - Retrieves Freq Range Properties.
+ *
+ * @range: range of freq
+ * @min_freq: (out) Minimum Frequency of the Band in kHz.
+ * @max_freq: (out) Maximum Frequency of the Band in kHz
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EINVAL, if parameter is invalid.
+ */
+int fmd_get_freq_range_properties(
+			u8 range,
+			u32  *min_freq,
+			u32  *max_freq
+			);
+
+/**
+ * fmd_set_antenna() - Selects the antenna to be used in receive mode.
+ *
+ * embedded - Selects the embedded antenna, wired- Selects the wired antenna.
+ * @antenna: Antenna Type
+ *
+ *  Returns:
+ *	 0,  if set antenna done successfully.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_antenna(
+			u8 antenna
+			);
+
+/**
+ * fmd_get_antenna() - Retrieves the currently used antenna type.
+ *
+ * @antenna: (out) Antenna Selected on FM Radio.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_get_antenna(
+			u8 *antenna
+			);
+
+/**
+ * fmd_set_freq_range() - Sets the FM band.
+ *
+ * @range: freq range
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_freq_range(
+			u8 range
+			);
+
+/**
+ * fmd_get_freq_range() - Gets the FM band currently in use.
+ *
+ * @range: (out) Frequency Range set on FM Radio.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ */
+int fmd_get_freq_range(
+			u8 *range
+			);
+
+/**
+ * fmd_rx_set_grid() - Sets the tuning grid.
+ *
+ * @grid: Tuning grid size
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_set_grid(
+			u8 grid
+			);
+
+/**
+ * fmd_rx_set_frequency() - Sets the FM Channel.
+ *
+ * @freq: Frequency to Set in Khz
+ *
+ *  Returns:
+ *	 0,  if set frequency done successfully.
+ *	 -EINVAL,  if parameters are invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_set_frequency(
+			u32 freq
+			);
+
+/**
+ * fmd_rx_get_frequency() - Gets the currently used FM Channel.
+ *
+ * @freq: (out) Current Frequency set on FM Radio.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL,  if parameters are invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_get_frequency(
+			u32  *freq
+			);
+
+/**
+ * fmd_rx_set_stereo_mode() - Sets the stereomode functionality.
+ *
+ * @mode: FMD_STEREOMODE_MONO, FMD_STEREOMODE_STEREO and
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_set_stereo_mode(
+			u8 mode
+			);
+
+/**
+ * fmd_rx_set_stereo_ctrl_blending_rssi() - Sets the stereo blending control setting.
+ *
+ * @min_rssi: Defines the RF level (at the antenna pin) at which the stereo blending
+ *			function will stop limiting the channel separation
+ * @max_rssi: Defines the RF level (at the antenna pin) at which the stereo blending
+ *			function will start limiting the channel separation.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_set_stereo_ctrl_blending_rssi(
+			u16 min_rssi,
+			u16 max_rssi
+			);
+
+/**
+ * fmd_rx_get_stereo_mode() - Gets the currently used FM mode.
+ *
+ * FMD_STEREOMODE_MONO, FMD_STEREOMODE_STEREO and
+ * FMD_STEREOMODE_AUTO.
+ * @mode: (out) Mode set on FM Radio, stereo or mono.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_get_stereo_mode(
+			u8 *mode
+			);
+
+/**
+ * fmd_rx_get_signal_strength() - Gets the RSSI level of current frequency.
+ *
+ * @strength: (out) RSSI level of current channel.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_get_signal_strength(
+			u16  *strength
+			);
+
+/**
+ * fmd_rx_set_stop_level() - Sets the FM Rx Seek stop level.
+ *
+ * @stoplevel: seek stop level
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_set_stop_level(
+			u16 stoplevel
+			);
+
+/**
+ * fmd_rx_get_stop_level() - Gets the current FM Rx Seek stop level.
+ *
+ * @stoplevel: (out) RSSI Threshold set on FM Radio.
+ *
+ *  Returns:
+ *	 0,  if no error.
+ *	 -EINVAL, if parameter is invalid.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_get_stop_level(
+			u16  *stoplevel
+			);
+
+/**
+ * fmd_rx_seek() - Perform FM Seek.
+ *
+ * Starts searching relative to the actual channel with
+ * a specific direction, stop.
+ *  level and optional noise levels
+ * @upwards: scan up
+ *
+ *  Returns:
+ *	 0,  if seek started successfully.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -EBUSY, if FM Driver is not in idle state.
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_seek(
+			bool upwards
+			);
+
+/**
+ * fmd_rx_stop_seeking() - Stops a currently active seek or scan band.
+ *
+ *  Returns:
+ *	 0,  if stop seek done successfully.
+ *	 -ENOEXEC, if preconditions are violated.
+ *	 -ENOEXEC, if FM Driver is
+ * not currently in Seek or Scan State..
+ *	 -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_stop_seeking(void);
+
+/**
+ * fmd_rx_af_update_start() - Perform AF update.
+ *
+ * This is used to switch to a shortly tune to a AF freq,
+ * measure its RSSI and tune back to the original frequency.
+ * @freq: Alternative frequncy in KHz to be set for AF updation.
+ *
+ *  Returns:
+ * -EBUSY, if FM Driver is not in idle state.
+ * 0,  if no error.
+ * -ENOEXEC, if preconditions are violated.
+ */
+int fmd_rx_af_update_start(
+			 u32 freq
+			 );
+
+/**
+ * fmd_rx_get_af_update_result() - Retrive result of AF update.
+ *
+ * Retrive the RSSI level of the Alternative frequency.
+ * @af_level: RSSI level of the Alternative frequency.
+ *
+ * Returns:
+ * -EBUSY, if FM Driver is not in idle state.
+ * 0,  if no error.
+ * -EINVAL, if parameter is invalid.
+ * -ENOEXEC, if preconditions are violated.
+ */
+int fmd_rx_get_af_update_result(
+			 u16 *af_level
+			 );
+
+/**
+ * fmd_af_switch_start() -Performs AF switch.
+ *
+ * @freq: Frequency to Set in Khz.
+ * @picode:programable id,unique for each station.
+ *
+ *  Returns:
+ * -EBUSY, if FM Driver is not in idle state.
+ * 0,  if no error and if AF switch started successfully.
+ * -ENOEXEC, if preconditions are violated.
+ */
+int fmd_rx_af_switch_start(
+			u32 freq,
+			u16  picode
+			);
+
+/**
+ * fmd_rx_get_af_switch_results() -Retrieves the results of AF Switch.
+ *
+ * @afs_conclusion: Conclusion of AF switch.
+ * @afs_level: RSSI level of the Alternative frequnecy.
+ * @afs_pi: PI code of the alternative channel (if found).
+ *
+ * Returns:
+ * -EBUSY, if FM Driver is not in idle state.
+ * 0,  if no error.
+ * -EINVAL, if parameter is invalid.
+ * -ENOEXEC, if preconditions are violated.
+ */
+int fmd_rx_get_af_switch_results(
+			 u16 *afs_conclusion,
+			 u16 *afs_level,
+			 u16 *afs_pi
+			 );
+
+/**
+ * fmd_rx_scan_band() - Starts Band Scan.
+ *
+ * Starts scanning the active band for the strongest
+ * channels above a threshold.
+ * @max_channels_to_scan: Maximum number of channels to scan.
+ *
+ * Returns:
+ *   0,  if scan band started successfully.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_scan_band(
+			u8 max_channels_to_scan
+			);
+
+/**
+ * fmd_rx_get_max_channels_to_scan() - Retreives the maximum channels.
+ *
+ * Retrieves the maximum number of channels that can be found during
+ * band scann.
+ * @max_channels_to_scan: (out) Maximum number of channels to scan.
+ *
+ * Returns:
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if parameter is invalid.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_get_max_channels_to_scan(
+			u8 *max_channels_to_scan
+			);
+
+/**
+ * fmd_rx_get_scan_band_info() - Retrieves Channels found during scanning.
+ *
+ * Retrieves the scanned active band
+ * for the strongest channels above a threshold.
+ * @index: (out) Index value to retrieve the channels.
+ * @numchannels: (out) Number of channels found during Band Scan.
+ * @channels: (out) Channels found during band scan.
+ * @rssi: (out) Rssi of channels found during Band scan.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_get_scan_band_info(
+			u32 index,
+			u16  *numchannels,
+			u16  *channels,
+			u16  *rssi
+			);
+
+/**
+ * fmd_block_scan() - Starts Block Scan.
+ *
+ * Starts block scan for retriving the RSSI level of channels
+ * in the given block.
+ * @start_freq: Starting frequency of the block from where scanning has
+ * to be started.
+ * @stop_freq: End frequency of the block to be scanned.
+ * @antenna: Antenna to be used during scanning.
+ *
+ * Returns:
+ *   0,  if scan band started successfully.
+ *   -EINVAL,  if parameters are invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_block_scan(
+			u32 start_freq,
+			u32 stop_freq,
+			u8 antenna
+			);
+
+/**
+ * fmd_get_block_scan_result() - Retrieves RSSI Level of channels.
+ *
+ * Retrieves the RSSI level of the channels in the block.
+ * @index: (out) Index value to retrieve the channels.
+ * @numchannels: (out) Number of channels found during Band Scan.
+ * @rssi: (out) Rssi of channels found during Band scan.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_get_block_scan_result(
+			u32 index,
+			u16 *numchannels,
+			u16 *rssi
+			);
+
+/**
+ * fmd_rx_get_rds() - Gets the current status of RDS transmission.
+ *
+ * @on: (out) RDS status
+ *
+ *  Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_get_rds(
+			bool  *on
+			);
+
+/**
+ * fmd_rx_buffer_set_size() - Sets the number of groups that the data buffer.
+ * can contain and clears the buffer.
+ *
+ * @size: buffer size
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_buffer_set_size(
+			u8 size
+			);
+
+/**
+ * fmd_rx_buffer_set_threshold() - RDS Buffer Threshold level in FM Chip.
+ *
+ * Sets the group number at which the RDS buffer full interrupt must be
+ * generated. The interrupt will be set after reception of the group.
+ * @threshold: threshold level.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_buffer_set_threshold(
+			u8 threshold
+			);
+
+/**
+ * fmd_rx_set_rds() - Enables or disables demodulation of RDS data.
+ *
+ * @on_off_state : Rx Set ON/OFF control
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_rx_set_rds(
+			u8 on_off_state
+			);
+
+/**
+ * fmd_rx_set_rds_group_rejection() - Enables or disables group rejection
+ * in case groups with erroneous blocks are received.
+ *
+ * @on_off_state : Rx Group Rejection ON /OFF control
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+
+int fmd_rx_set_rds_group_rejection(
+			u8 on_off_state
+			);
+
+/**
+ * fmd_rx_get_low_level_rds_groups() - Gets Low level RDS group data.
+ *
+ * @index: RDS group index
+ * @block1: (out) RDS Block 1
+ * @block2: (out) RDS Block 2
+ * @block3: (out) RDS Block 3
+ * @block4: (out) RDS Block 4
+ * @status1: (out) RDS data status 1
+ * @status2: (out) RDS data status 2
+ * @status3: (out) RDS data status 3
+ * @status4: (out) RDS data status 4
+ *
+ *  Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_rx_get_low_level_rds_groups(
+			u8 index,
+			u16  *block1,
+			u16  *block2,
+			u16  *block3,
+			u16  *block4,
+			u8  *status1,
+			u8  *status2,
+			u8  *status3,
+			u8  *status4
+			);
+
+/**
+ * fmd_tx_set_pa() - Enables or disables the Power Amplifier.
+ *
+ * @on: Power Amplifier current state to set
+ *
+ * Returns:
+ *   0,  if set Power Amplifier done successfully.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_pa(
+			bool on
+			);
+
+/**
+ * fmd_tx_set_signal_strength() - Sets the RF-level of the output FM signal.
+ *
+ * @strength: Signal strength to be set for FM Tx in dBuV.
+ *
+ * Returns:
+ *   0,  if set RSSI Level done successfully.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_signal_strength(
+			u16 strength
+			);
+
+/**
+ * fmd_tx_get_signal_strength() - Retrieves current RSSI of FM Tx.
+ *
+ * @strength: (out) Strength of signal being transmitted in dBuV.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL, if parameter is invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_signal_strength(
+			u16  *strength
+			);
+
+/**
+ * fmd_tx_set_freq_range() - Sets the FM band and specifies the custom band.
+ *
+ * @range: Freq range to set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL, if parameter is invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_freq_range(
+			u8 range
+			);
+
+/**
+ * fmd_tx_get_freq_range() - Gets the FM band currently in use.
+ *
+ * @range: (out) Frequency Range set on Fm Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ */
+int fmd_tx_get_freq_range(
+			u8 *range
+			);
+
+/**
+ * fmd_tx_set_grid() - Sets the tuning grid size.
+ *
+ * @grid: FM Grid (50 Khz, 100 Khz, 200 Khz) to be set for FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL, if parameter is invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_grid(
+			u8 grid
+			);
+
+/**
+ * fmd_tx_get_grid() - Gets the current tuning grid size.
+ *
+ * @grid: (out) FM Grid (50 Khz, 100 Khz, 200 Khz) currently set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_grid(
+			u8 *grid
+			);
+
+/**
+ * fmd_tx_set_preemphasis() - Sets the Preemphasis characteristic of the Tx.
+ *
+ * @preemphasis: Pre-emphasis level to be set for FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_preemphasis(
+			u8 preemphasis
+			);
+
+/**
+ * fmd_tx_get_preemphasis() - Gets the currently used Preemphasis char of th FM Tx.
+ *
+ * @preemphasis: (out) Preemphasis Level used for FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_preemphasis(
+			u8 *preemphasis
+			);
+
+/**
+ * fmd_tx_set_frequency() - Sets the FM Channel for Tx.
+ *
+ * @freq: Freq to be set for transmission.
+ *
+ * Returns:
+ *   0,  if set frequency done successfully.
+ *   -EINVAL,  if parameters are invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_frequency(
+			u32 freq
+			);
+
+/**
+ * fmd_rx_get_frequency() - Gets the currently used Channel for Tx.
+ *
+ * @freq: (out) Frequency set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL,  if parameters are invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_get_frequency(
+			u32  *freq
+			);
+
+/**
+ * fmd_tx_enable_stereo_mode() - Sets Stereo mode state for TX.
+ *
+ * @enable_stereo_mode: Flag indicating enabling or disabling Stereo mode.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_enable_stereo_mode(
+			bool enable_stereo_mode
+			);
+
+/**
+ * fmd_tx_get_stereo_mode() - Gets the currently used FM Tx stereo mode.
+ *
+ * @stereo_mode: (out) Stereo Mode state set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -EINVAL, if parameter is invalid.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_stereo_mode(
+			bool  *stereo_mode
+			);
+
+/**
+ * fmd_tx_set_pilot_deviation() - Sets pilot deviation in HZ
+ *
+ * @deviation: Pilot deviation in HZ to set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_pilot_deviation(
+			u16 deviation
+			);
+
+/**
+ * fmd_tx_get_pilot_deviation() - Retrieves the current pilot deviation.
+ *
+ * @deviation: (out) Pilot deviation set on FM Tx.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_pilot_deviation(
+			u16  *deviation
+			);
+
+/**
+ * fmd_tx_set_rds_deviation() - Sets Rds deviation in HZ.
+ *
+ * @deviation: RDS deviation in HZ.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_rds_deviation(
+			u16 deviation
+			);
+
+/**
+ * fmd_tx_get_rds_deviation() - Retrieves the current Rds deviation.
+ *
+ * @deviation: (out) RDS deviation currently set.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_rds_deviation(
+			u16  *deviation
+			);
+
+/**
+ * fmd_tx_set_rds() - Enables or disables RDS transmission for Tx.
+ *
+ * @on: Boolean - RDS ON
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_rds(
+			bool on
+			);
+
+/**
+ * fmd_rx_get_rds() - Gets the current status of RDS transmission for FM Tx.
+ *
+ * @on: (out) Rds enabled or disabled.
+ *
+ *Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_tx_get_rds(
+			bool  *on
+			);
+
+/**
+ * fmd_tx_set_group() - Programs a grp on a certain position in the RDS buffer.
+ *
+ * @position: RDS group position
+ * @block1: Data to be transmitted in Block 1
+ * @block2: Data to be transmitted in Block 2
+ * @block3: Data to be transmitted in Block 3
+ * @block4: Data to be transmitted in Block 4
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if parameters are invalid.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_set_group(
+			u16 position,
+			u8  *block1,
+			u8  *block2,
+			u8  *block3,
+			u8  *block4
+			);
+
+/**
+ * fmd_tx_buffer_set_size() - Controls the size of the RDS buffer in groups.
+ *
+ * @buffer_size: RDS buffer size.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_tx_buffer_set_size(
+			u16 buffer_size
+			);
+
+/**
+ * fmd_set_volume() - Sets the receive audio volume.
+ *
+ * @volume: Audio volume level
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_volume(
+			u8 volume
+			);
+
+/**
+ * fmd_get_volume() - Retrives the current audio volume.
+ *
+ * @volume: Analog Volume level.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if parameter is invalid.
+ *   -EBUSY, if FM Driver is not in idle state.
+ */
+int fmd_get_volume(
+			u8 *volume
+			);
+
+/**
+ * fmd_set_balance() - Controls the receiver audio balance.
+ *
+ * @balance: Audio balance level
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_balance(
+			s8 balance
+			);
+
+/**
+ * fmd_set_mute() - Enables or disables muting of the analog audio(DAC).
+ *
+ * @mute_on: bool of mute on
+ *
+ * Returns:
+ *   0,  if mute done successfully.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_mute(
+			bool mute_on
+			);
+
+/**
+ * fmd_ext_set_mute() - Enables or disables muting of the audio channel.
+ *
+ * @mute_on: bool to Mute
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_ext_set_mute(
+			bool mute_on
+			);
+
+/**
+ * fmd_power_up() - Puts the system in Powerup state.
+ *
+ * Returns:
+ *   0,  if power up command sent successfully to chip.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_power_up(void);
+
+/**
+ * fmd_goto_standby() - Puts the system in standby mode.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_goto_standby(void);
+
+/**
+ * fmd_goto_power_down() - Puts the system in Powerdown mode.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_goto_power_down(void);
+
+/**
+ * fmd_select_ref_clk() - Selects the FM reference clock.
+ *
+ * @ref_clk: Ref Clock.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_select_ref_clk(
+			u16 ref_clk
+			);
+
+/**
+ * fmd_set_ref_clk_pll() - Sets the freq of Referece Clock.
+ *
+ * Sets frequency and offset correction properties of the external
+ * reference clock of the PLL
+ * @freq: PLL Frequency/ 2 in kHz.
+ *
+ * Returns:
+ *   0,  if no error.
+ *   -ENOEXEC, if preconditions are violated.
+ *   -EBUSY, if FM Driver is not in idle state.
+ *   -EINVAL, if wrong response received from chip.
+ */
+int fmd_set_ref_clk_pll(
+			u16 freq
+			);
+
+/**
+ * fmd_send_fm_ip_enable()- Enables the FM IP.
+ *
+ * Returns:
+ *	 0: If there is no error.
+ *	 -ETIME: Otherwise
+ */
+int fmd_send_fm_ip_enable(void);
+
+/**
+ * fmd_send_fm_ip_disable()- Disables the FM IP.
+ *
+ * Returns:
+ *	 0, If there is no error.
+ *	 -ETIME: Otherwise
+ */
+int fmd_send_fm_ip_disable(void);
+
+/**
+ * fmd_send_fm_firmware() - Send the FM Firmware File to Device.
+ *
+ * @fw_buffer: Firmware to be downloaded.
+ * @fw_size: Size of firmware to be downloaded.
+ *
+ * Returns:
+ *	 0, If there is no error.
+ *	 -ETIME: Otherwise
+ */
+int fmd_send_fm_firmware(
+			u8 *fw_buffer,
+			u16 fw_size
+			);
+
+/**
+ * fmd_int_bufferfull() - RDS Groups availabe for reading by Host.
+ *
+ * Gets the number of groups that are available in the
+ * buffer. This function is called in RX mode to read RDS groups.
+ * @number_of_rds_groups: Number of RDS groups ready to
+ * be read from the Host.
+ *
+ * Returns:
+ *	 0, If there is no error.
+ *	 corresponding error Otherwise
+ */
+int fmd_int_bufferfull(
+			u16 *number_of_rds_groups
+			);
+
+/**
+ * fmd_start_rds_thread() - Starts the RDS Thread for receiving RDS Data.
+ *
+ * This is started by Application when it wants to receive RDS Data.
+ * @cb_func: Callback function for receiving RDS Data
+ */
+void fmd_start_rds_thread(
+			cg2900_fm_rds_cb cb_func
+			);
+/**
+ * fmd_stop_rds_thread() - Stops the RDS Thread when Application does not
+ * want to receive RDS.
+ */
+void fmd_stop_rds_thread(void);
+
+/**
+ * fmd_get_rds_sem() - Block on RDS Semaphore.
+ * Till irpt_BufferFull is received, RDS Task is blocked.
+ */
+void fmd_get_rds_sem(void);
+
+/**
+ * fmd_set_rds_sem() - Unblock on RDS Semaphore.
+ * on receiving  irpt_BufferFull, RDS Task is un-blocked.
+ */
+void fmd_set_rds_sem(void);
+
+/**
+ * fmd_set_dev() - Set FM device.
+ *
+ * @dev: FM Device
+ *
+ * Returns:
+ *	 0, If there is no error.
+ *	 corresponding error Otherwise
+ */
+int fmd_set_dev(
+			struct device *dev
+			);
+
+/**
+ * fmd_set_test_tone_generator_status()- Sets the Test Tone Generator.
+ *
+ * This function is used to enable/disable the Internal Tone Generator of
+ * CG2900.
+ * @test_tone_status: Status of tone generator.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int fmd_set_test_tone_generator_status(
+			u8 test_tone_status
+			);
+
+/**
+ * fmd_test_tone_connect()- Connect Audio outputs/inputs.
+ *
+ * This function connects the audio outputs/inputs of the
+ * Internal Tone Generator of CG2900.
+ * @left_audio_mode: Left Audio Output Mode.
+ * @right_audio_mode: Right Audio Output Mode.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int fmd_test_tone_connect(
+			u8 left_audio_mode,
+			u8 right_audio_mode
+			);
+
+/**
+ * fmd_test_tone_set_params()- Sets the Test Tone Parameters.
+ *
+ * This function is used to set the parameters of
+ * the Internal Tone Generator of CG2900.
+ * @tone_gen: Tone to be configured (Tone 1 or Tone 2)
+ * @frequency: Frequency of the tone.
+ * @volume: Volume of the tone.
+ * @phase_offset: Phase offset of the tone.
+ * @dc: DC to add to tone.
+ * @waveform: Waveform to generate, sine or pulse.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int fmd_test_tone_set_params(
+			u8 tone_gen,
+			u16 frequency,
+			u16 volume,
+			u16 phase_offset,
+			u16 dc,
+			u8 waveform
+			);
+
+/**
+ * fmd_rx_set_deemphasis()- Connect Audio outputs/inputs.
+ *
+ * This function sets the de-emphasis filter to the
+ * specified de-empahsis level.
+ * @deemphasis: De-emphasis level to set.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int fmd_rx_set_deemphasis(
+			u8 deemphasis
+			);
+
+/**
+ * fmd_limiter_setcontrol()- Sets the Limiter Controls.
+ *
+ * This function sets the limiter control.
+ * @audio_deviation: Limiting level of Audio Deviation.
+ * @notification_hold_off_time: Minimum time between
+ * two limiting interrupts.
+ *
+ * Returns:
+ *	 0,  if operation completed successfully.
+ *	 -EINVAL, otherwise.
+ */
+int fmd_limiter_setcontrol(
+			u16 audio_deviation,
+			u16 notification_hold_off_time
+			);
+
+#endif /* _FMDRIVER_H_  */
diff --git a/drivers/media/radio/CG2900/radio-cg2900.c b/drivers/media/radio/CG2900/radio-cg2900.c
new file mode 100644
index 00000000000..9ccb4e6b85d
--- /dev/null
+++ b/drivers/media/radio/CG2900/radio-cg2900.c
@@ -0,0 +1,3024 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Linux Wrapper for V4l2 FM Driver for CG2900.
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2
+ */
+
+#include<linux/init.h>
+#include<linux/videodev2.h>
+#include<media/v4l2-ioctl.h>
+#include<media/v4l2-common.h>
+#include<linux/module.h>
+#include <linux/platform_device.h>
+#include<linux/string.h>
+#include<linux/wait.h>
+#include"cg2900.h"
+#include"cg2900_fm_driver.h"
+
+#define RADIO_CG2900_VERSION KERNEL_VERSION(1, 1, 0)
+#define BANNER "ST-Ericsson FM Radio Card driver v1.1.0"
+
+#define FMR_HZ_TO_MHZ_CONVERTER				1000000
+#define FMR_EU_US_LOW_FREQ_IN_MHZ			87.5
+#define FMR_EU_US_HIGH_FREQ_IN_MHZ			108
+#define FMR_JAPAN_LOW_FREQ_IN_MHZ			76
+#define FMR_JAPAN_HIGH_FREQ_IN_MHZ			90
+#define FMR_CHINA_LOW_FREQ_IN_MHZ			70
+#define FMR_CHINA_HIGH_FREQ_IN_MHZ			108
+#define FMR_MAX_BLOCK_SCAN_CHANNELS			198
+#define FMR_CHINA_GRID_IN_HZ				50000
+#define FMR_EUROPE_GRID_IN_HZ				100000
+#define FMR_USA_GRID_IN_HZ				200000
+#define FMR_AF_SWITCH_DATA_SIZE				2
+#define FMR_BLOCK_SCAN_DATA_SIZE			2
+#define FMR_GET_INTERRUPT_DATA_SIZE			2
+#define FMR_TEST_TONE_CONNECT_DATA_SIZE			2
+#define FMR_TEST_TONE_SET_PARAMS_DATA_SIZE		6
+
+/* freq in Hz to V4l2 freq (units of 62.5Hz) */
+#define HZ_TO_V4L2(X)  (2*(X)/125)
+/* V4l2 freq (units of 62.5Hz) to freq in Hz */
+#define V4L2_TO_HZ(X)  (((X)*125)/(2))
+
+static int cg2900_open(
+			struct file *file
+			);
+static int cg2900_release(
+			struct file *file
+			);
+static ssize_t cg2900_read(
+			struct file *file,
+			char __user *data,
+			size_t count,
+			loff_t *pos
+			);
+static unsigned int cg2900_poll(
+			struct file *file,
+			struct poll_table_struct *wait
+			);
+static int vidioc_querycap(
+			struct file *file,
+			void *priv,
+			struct v4l2_capability *query_caps
+			);
+static int vidioc_get_tuner(
+			struct file *file,
+			void *priv,
+			struct v4l2_tuner *tuner
+			);
+static int vidioc_set_tuner(
+			struct file *file,
+			void *priv,
+			struct v4l2_tuner *tuner
+			);
+static int vidioc_get_modulator(
+			struct file *file,
+			void *priv,
+			struct v4l2_modulator *modulator
+			);
+static int vidioc_set_modulator(
+			struct file *file,
+			void *priv,
+			struct v4l2_modulator *modulator
+			);
+static int vidioc_get_frequency(
+			struct file *file,
+			void *priv,
+			struct v4l2_frequency *freq
+			);
+static int vidioc_set_frequency(
+			struct file *file,
+			void *priv,
+			struct v4l2_frequency *freq
+			);
+static int vidioc_query_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_queryctrl *query_ctrl
+			);
+static int vidioc_get_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_control *ctrl
+			);
+static int vidioc_set_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_control *ctrl
+			);
+static int vidioc_get_ext_ctrls(
+			struct file *file,
+			void *priv,
+			struct v4l2_ext_controls *ext_ctrl
+			);
+static int vidioc_set_ext_ctrls(
+			struct file *file,
+			void *priv,
+			struct v4l2_ext_controls *ext_ctrl
+			);
+static int vidioc_set_hw_freq_seek(
+			struct file *file,
+			void *priv,
+			struct v4l2_hw_freq_seek *freq_seek
+			);
+static int vidioc_get_audio(
+			struct file *file,
+			void *priv,
+			struct v4l2_audio *audio
+			);
+static int vidioc_set_audio(
+			struct file *file,
+			void *priv,
+			struct v4l2_audio *audio
+			);
+static int vidioc_get_input(
+			struct file *filp,
+			void *priv,
+			unsigned int *input
+			);
+static int vidioc_set_input(
+			struct file *filp,
+			void *priv,
+			unsigned int input
+			);
+static void cg2900_convert_err_to_v4l2(
+			char status_byte,
+			char *out_byte
+			);
+static int cg2900_map_event_to_v4l2(
+			u8 fm_event
+			);
+
+static u32 freq_low;
+static u32 freq_high;
+
+/* Module Parameters */
+static int radio_nr = -1;
+static int grid;
+static int band;
+
+/* cg2900_poll_queue - Main Wait Queue for polling (Scan/Seek) */
+static wait_queue_head_t cg2900_poll_queue;
+
+struct sk_buff_head		fm_interrupt_queue;
+
+/**
+ * enum fm_seek_status - Seek status of FM Radio.
+ *
+ * @FMR_SEEK_NONE: No seek in progress.
+ * @FMR_SEEK_IN_PROGRESS: Seek is in progress.
+ *
+ * Seek status of FM Radio.
+ */
+enum fm_seek_status {
+	FMR_SEEK_NONE,
+	FMR_SEEK_IN_PROGRESS
+};
+
+/**
+ * enum fm_power_state - Power states of FM Radio.
+ *
+ * @FMR_SWITCH_OFF: FM Radio is switched off.
+ * @FMR_SWITCH_ON: FM Radio is switched on.
+ * @FMR_STANDBY: FM Radio in standby state.
+ *
+ * Power states of FM Radio.
+ */
+enum fm_power_state {
+	FMR_SWITCH_OFF,
+	FMR_SWITCH_ON,
+	FMR_STANDBY
+};
+
+/**
+ * struct cg2900_device - Stores FM Device Info.
+ *
+ * @state: state of FM Radio
+ * @muted: FM Radio Mute/Unmute status
+ * @seekstatus: seek status
+ * @rx_rds_enabled: Rds enable/disable status for FM Rx
+ * @tx_rds_enabled: Rds enable/disable status for FM Tx
+ * @rx_stereo_status: Stereo Mode status for FM Rx
+ * @tx_stereo_status: Stereo Mode status for FM Tx
+ * @volume: Analog Volume Gain of FM Radio
+ * @rssi_threshold: rssi Thresold set on FM Radio
+ * @frequency: Frequency tuned on FM Radio in V4L2 Format
+ * @audiopath: Audio Balance
+ * @wait_on_read_queue: Flag for waiting on read queue.
+ * @fm_mode: Enum for storing the current FM Mode.
+ *
+ * FM Driver Information Structure.
+ */
+struct cg2900_device {
+	u8 state;
+	u8 muted;
+	u8 seekstatus;
+	bool rx_rds_enabled;
+	bool tx_rds_enabled;
+	bool rx_stereo_status;
+	bool tx_stereo_status;
+	int volume;
+	u16 rssi_threshold;
+	u32 frequency;
+	u32 audiopath;
+	bool wait_on_read_queue;
+	enum cg2900_fm_mode fm_mode;
+};
+
+/* Global Structure to store the maintain FM Driver device info */
+static struct cg2900_device cg2900_device;
+
+/* V4l2 File Operation Structure */
+static const struct v4l2_file_operations cg2900_fops = {
+	.owner = THIS_MODULE,
+	.open = cg2900_open,
+	.release = cg2900_release,
+	.read = cg2900_read,
+	.poll = cg2900_poll,
+	.ioctl = video_ioctl2,
+};
+
+/* V4L2 IOCTL Operation Structure */
+static const struct v4l2_ioctl_ops cg2900_ioctl_ops = {
+	.vidioc_querycap = vidioc_querycap,
+	.vidioc_g_tuner = vidioc_get_tuner,
+	.vidioc_s_tuner = vidioc_set_tuner,
+	.vidioc_g_modulator = vidioc_get_modulator,
+	.vidioc_s_modulator = vidioc_set_modulator,
+	.vidioc_g_frequency = vidioc_get_frequency,
+	.vidioc_s_frequency = vidioc_set_frequency,
+	.vidioc_queryctrl = vidioc_query_ctrl,
+	.vidioc_g_ctrl = vidioc_get_ctrl,
+	.vidioc_s_ctrl = vidioc_set_ctrl,
+	.vidioc_g_ext_ctrls = vidioc_get_ext_ctrls,
+	.vidioc_s_ext_ctrls = vidioc_set_ext_ctrls,
+	.vidioc_s_hw_freq_seek = vidioc_set_hw_freq_seek,
+	.vidioc_g_audio = vidioc_get_audio,
+	.vidioc_s_audio = vidioc_set_audio,
+	.vidioc_g_input = vidioc_get_input,
+	.vidioc_s_input = vidioc_set_input,
+};
+
+/* V4L2 Video Device Structure */
+static struct video_device cg2900_video_device = {
+	.name = "STE CG2900 FM Rx/Tx Radio",
+	.fops = &cg2900_fops,
+	.ioctl_ops = &cg2900_ioctl_ops,
+	.release = video_device_release_empty,
+};
+
+static u16 no_of_scan_freq;
+static u16 no_of_block_scan_freq;
+static u32 scanfreq_rssi_level[MAX_CHANNELS_TO_SCAN];
+static u16 block_scan_rssi_level[MAX_CHANNELS_FOR_BLOCK_SCAN];
+static u32 scanfreq[MAX_CHANNELS_TO_SCAN];
+static struct mutex fm_mutex;
+static spinlock_t fm_spinlock;
+static int users;
+
+/**
+ * vidioc_querycap()- Query FM Driver Capabilities.
+ *
+ * This function is used to query the capabilities of the
+ * FM Driver. This function is called when the application issues the IOCTL
+ * VIDIOC_QUERYCAP.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @query_caps: v4l2_capability structure.
+ *
+ * Returns: 0
+ */
+static int vidioc_querycap(
+			struct file *file,
+			void *priv,
+			struct v4l2_capability *query_caps
+			)
+{
+	FM_INFO_REPORT("vidioc_querycap");
+	memset(
+			query_caps,
+			0,
+			sizeof(*query_caps)
+			);
+	strlcpy(
+			query_caps->driver,
+			"CG2900 Driver",
+			sizeof(query_caps->driver)
+			);
+	strlcpy(
+			query_caps->card,
+			"CG2900 FM Radio",
+			sizeof(query_caps->card)
+			);
+	strcpy(
+			query_caps->bus_info,
+			"platform"
+			);
+	query_caps->version = RADIO_CG2900_VERSION;
+	query_caps->capabilities =
+			V4L2_CAP_TUNER |
+			V4L2_CAP_MODULATOR |
+			V4L2_CAP_RADIO  |
+			V4L2_CAP_READWRITE |
+			V4L2_CAP_RDS_CAPTURE |
+			V4L2_CAP_HW_FREQ_SEEK |
+			V4L2_CAP_RDS_OUTPUT;
+	FM_DEBUG_REPORT("vidioc_querycap returning 0");
+	return 0;
+}
+
+/**
+ * vidioc_get_tuner()- Get FM Tuner Features.
+ *
+ * This function is used to get the tuner features.
+ * This function is called when the application issues the IOCTL
+ * VIDIOC_G_TUNER
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @tuner: v4l2_tuner structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_tuner(
+			struct file *file,
+			void *priv,
+			struct v4l2_tuner *tuner
+			)
+{
+	int status = 0;
+	u8 mode;
+	bool rds_enabled;
+	u16 rssi;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_get_tuner");
+
+	if (tuner->index > 0) {
+		FM_ERR_REPORT("vidioc_get_tuner: Only 1 tuner supported");
+		goto error;
+	}
+
+	memset(tuner, 0, sizeof(*tuner));
+	strcpy(tuner->name, "CG2900 FM Receiver");
+	tuner->type = V4L2_TUNER_RADIO;
+	tuner->rangelow = HZ_TO_V4L2(freq_low);
+	tuner->rangehigh = HZ_TO_V4L2(freq_high);
+	tuner->capability =
+			V4L2_TUNER_CAP_LOW /* Frequency steps = 1/16 kHz */
+			| V4L2_TUNER_CAP_STEREO	/* Can receive stereo */
+			| V4L2_TUNER_CAP_RDS;	/* Supports RDS Capture  */
+
+	if (cg2900_device.fm_mode == CG2900_FM_RX_MODE) {
+
+		status = cg2900_fm_get_mode(&mode);
+
+		FM_DEBUG_REPORT("vidioc_get_tuner: mode = %x, ", mode);
+
+		if (0 != status) {
+			/* Get mode API failed, set mode to mono */
+			tuner->audmode = V4L2_TUNER_MODE_MONO;
+			tuner->rxsubchans = V4L2_TUNER_SUB_MONO;
+			goto error;
+		}
+
+		switch (mode) {
+		case CG2900_MODE_STEREO:
+			tuner->audmode = V4L2_TUNER_MODE_STEREO;
+			tuner->rxsubchans = V4L2_TUNER_SUB_STEREO;
+			break;
+		case CG2900_MODE_MONO:
+		default:
+			tuner->audmode = V4L2_TUNER_MODE_MONO;
+			tuner->rxsubchans = V4L2_TUNER_SUB_MONO;
+			break;
+		}
+
+		status = cg2900_fm_get_rds_status(&rds_enabled);
+
+		if (0 != status) {
+			tuner->rxsubchans &= ~V4L2_TUNER_SUB_RDS;
+			goto error;
+		}
+
+		if (rds_enabled)
+			tuner->rxsubchans |= V4L2_TUNER_SUB_RDS;
+		else
+			tuner->rxsubchans &= ~V4L2_TUNER_SUB_RDS;
+	} else {
+		tuner->audmode = V4L2_TUNER_MODE_MONO;
+		tuner->rxsubchans = V4L2_TUNER_SUB_MONO;
+	}
+
+	if (cg2900_device.fm_mode == CG2900_FM_RX_MODE) {
+		status = cg2900_fm_get_signal_strength(&rssi);
+
+		if (0 != status) {
+			tuner->signal = 0;
+			goto error;
+		}
+		tuner->signal = rssi;
+	} else {
+		tuner->signal = 0;
+	}
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_get_tuner: returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_tuner()- Set FM Tuner Features.
+ *
+ * This function is used to set the tuner features.
+ * It also sets the default FM Rx settings.
+ * This function is called when the application issues the IOCTL
+ * VIDIOC_S_TUNER
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @tuner: v4l2_tuner structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_tuner(
+			struct file *file,
+			void *priv,
+			struct v4l2_tuner *tuner
+			)
+{
+	bool rds_status = false;
+	bool stereo_status = false;
+	int status = 0;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_set_tuner");
+	if (tuner->index != 0) {
+		FM_ERR_REPORT("vidioc_set_tuner: Only 1 tuner supported");
+		goto error;
+	}
+
+	if (cg2900_device.fm_mode != CG2900_FM_RX_MODE) {
+		/*
+		 * FM Rx mode should be configured
+		 * as earlier mode was not FM Rx
+		 */
+		if (CG2900_FM_BAND_US_EU == band) {
+			freq_low = FMR_EU_US_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		} else if (CG2900_FM_BAND_JAPAN == band) {
+			freq_low = FMR_JAPAN_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_JAPAN_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		} else if (CG2900_FM_BAND_CHINA == band) {
+			freq_low = FMR_CHINA_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_CHINA_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		}
+		cg2900_device.fm_mode = CG2900_FM_RX_MODE;
+		cg2900_device.rx_rds_enabled =
+		    (tuner->rxsubchans & V4L2_TUNER_SUB_RDS) ?
+		    true : false;
+		if (tuner->rxsubchans & V4L2_TUNER_SUB_STEREO)
+			stereo_status = true;
+		else if (tuner->rxsubchans & V4L2_TUNER_SUB_MONO)
+			stereo_status = false;
+		cg2900_device.rx_stereo_status = stereo_status;
+		status = cg2900_fm_set_rx_default_settings(freq_low,
+				band,
+				grid,
+				cg2900_device.rx_rds_enabled,
+				cg2900_device.rx_stereo_status);
+
+		if (0 != status) {
+			FM_ERR_REPORT("vidioc_set_tuner: "
+				"cg2900_fm_set_rx_default_settings returned "
+				" %d", status);
+			goto error;
+		}
+		status = cg2900_fm_set_rssi_threshold(
+				cg2900_device.rssi_threshold);
+		if (0 != status) {
+			FM_ERR_REPORT("vidioc_set_tuner: "
+					"cg2900_fm_set_rssi_threshold returned "
+					" %d", status);
+			goto error;
+		}
+	} else {
+		/*
+		 * Mode was FM Rx only, change the RDS settings or stereo mode
+		 * if they are changed by application
+		 */
+		rds_status = (tuner->rxsubchans & V4L2_TUNER_SUB_RDS) ?
+		    true : false;
+		if (tuner->rxsubchans & V4L2_TUNER_SUB_STEREO)
+			stereo_status = true;
+		else if (tuner->rxsubchans & V4L2_TUNER_SUB_MONO)
+			stereo_status = false;
+		if (stereo_status != cg2900_device.rx_stereo_status) {
+			cg2900_device.rx_stereo_status = stereo_status;
+			if (stereo_status)
+				status =
+				    cg2900_fm_set_mode(
+						FMD_STEREOMODE_BLENDING);
+			else
+				status = cg2900_fm_set_mode(
+						FMD_STEREOMODE_MONO);
+
+			if (0 != status) {
+				FM_ERR_REPORT("vidioc_set_tuner: "
+						"cg2900_fm_set_mode returned "
+						" %d", status);
+				goto error;
+			}
+		}
+		if (rds_status != cg2900_device.rx_rds_enabled) {
+			cg2900_device.rx_rds_enabled = rds_status;
+			if (rds_status)
+				status = cg2900_fm_rds_on();
+			else
+				status = cg2900_fm_rds_off();
+
+			if (0 != status) {
+				FM_ERR_REPORT("vidioc_set_tuner: "
+						"cg2900_fm_rds returned "
+						" %d", status);
+				goto error;
+			}
+		}
+	}
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_set_tuner: returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_modulator()- Get FM Modulator Features.
+ *
+ * This function is used to get the modulator features.
+ * This function is called when the application issues the IOCTL
+ * VIDIOC_G_MODULATOR
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @modulator: v4l2_modulator structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_modulator(
+			struct file *file,
+			void *priv,
+			struct v4l2_modulator *modulator
+			)
+{
+	int status = 0;
+	bool rds_enabled;
+	u8 mode;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_get_modulator");
+
+	if (modulator->index > 0) {
+		FM_ERR_REPORT("vidioc_get_modulator: Only 1 "
+			      "modulator supported");
+		goto error;
+	}
+
+	memset(modulator, 0, sizeof(*modulator));
+	strcpy(modulator->name, "CG2900 FM Transmitter");
+	modulator->rangelow = freq_low;
+	modulator->rangehigh = freq_high;
+	modulator->capability = V4L2_TUNER_CAP_NORM /* Freq steps = 1/16 kHz */
+	    | V4L2_TUNER_CAP_STEREO	/* Can receive stereo */
+	    | V4L2_TUNER_CAP_RDS;	/* Supports RDS Capture  */
+
+	if (cg2900_device.fm_mode == CG2900_FM_TX_MODE) {
+		status = cg2900_fm_get_mode(&mode);
+		FM_DEBUG_REPORT("vidioc_get_modulator: mode = %x", mode);
+		if (0 != status) {
+			/* Get mode API failed, set mode to mono */
+			modulator->txsubchans = V4L2_TUNER_SUB_MONO;
+			goto error;
+		}
+		switch (mode) {
+			/* Stereo */
+		case CG2900_MODE_STEREO:
+			modulator->txsubchans = V4L2_TUNER_SUB_STEREO;
+			break;
+			/* Mono */
+		case CG2900_MODE_MONO:
+			modulator->txsubchans = V4L2_TUNER_SUB_MONO;
+			break;
+			/* Switching or Blending, set mode as Stereo */
+		default:
+			modulator->txsubchans = V4L2_TUNER_SUB_STEREO;
+		}
+		status = cg2900_fm_get_rds_status(&rds_enabled);
+		if (0 != status) {
+			modulator->txsubchans &= ~V4L2_TUNER_SUB_RDS;
+			goto error;
+		}
+		if (rds_enabled)
+			modulator->txsubchans |= V4L2_TUNER_SUB_RDS;
+		else
+			modulator->txsubchans &= ~V4L2_TUNER_SUB_RDS;
+	} else
+		modulator->txsubchans = V4L2_TUNER_SUB_MONO;
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_get_modulator: returning %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_modulator()- Set FM Modulator Features.
+ *
+ * This function is used to set the Modulaotr features.
+ * It also sets the default FM Tx settings.
+ * This function is called when the application issues the IOCTL
+ * VIDIOC_S_MODULATOR
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @modulator: v4l2_modulator structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_modulator(
+			struct file *file,
+			void *priv,
+			struct v4l2_modulator *modulator
+			)
+{
+	bool rds_status = false;
+	bool stereo_status = false;
+	int status = 0;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_set_modulator");
+	if (modulator->index != 0) {
+		FM_ERR_REPORT("vidioc_set_modulator: Only 1 "
+			      "modulator supported");
+		goto error;
+	}
+
+	if (cg2900_device.fm_mode != CG2900_FM_TX_MODE) {
+		/*
+		 * FM Tx mode should be configured as
+		 * earlier mode was not FM Tx
+		 */
+		if (band == CG2900_FM_BAND_US_EU) {
+			freq_low = FMR_EU_US_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		} else if (band == CG2900_FM_BAND_JAPAN) {
+			freq_low = FMR_JAPAN_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_JAPAN_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		} else if (band == CG2900_FM_BAND_CHINA) {
+			freq_low = FMR_CHINA_LOW_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+			freq_high = FMR_CHINA_HIGH_FREQ_IN_MHZ *
+			    FMR_HZ_TO_MHZ_CONVERTER;
+		}
+		cg2900_device.fm_mode = CG2900_FM_TX_MODE;
+		cg2900_device.rx_rds_enabled = false;
+		cg2900_device.tx_rds_enabled =
+		    (modulator->txsubchans & V4L2_TUNER_SUB_RDS) ?
+		    true : false;
+		if (modulator->txsubchans & V4L2_TUNER_SUB_STEREO)
+			stereo_status = true;
+		else if (modulator->txsubchans & V4L2_TUNER_SUB_MONO)
+			stereo_status = false;
+		cg2900_device.tx_stereo_status = stereo_status;
+
+		status = cg2900_fm_set_tx_default_settings(freq_low,
+					       band,
+					       grid,
+					       cg2900_device.tx_rds_enabled,
+					       cg2900_device.
+					       tx_stereo_status);
+
+		if (0 != status) {
+			FM_ERR_REPORT("vidioc_set_modulator: "
+				"cg2900_fm_set_tx_default_settings returned "
+				" %d", status);
+			goto error;
+		}
+	} else {
+		/*
+		 * Mode was FM Tx only, change the RDS settings or stereo mode
+		 * if they are changed by application
+		 */
+		rds_status = (modulator->txsubchans & V4L2_TUNER_SUB_RDS) ?
+		    true : false;
+		if (modulator->txsubchans & V4L2_TUNER_SUB_STEREO)
+			stereo_status = true;
+		else if (modulator->txsubchans & V4L2_TUNER_SUB_MONO)
+			stereo_status = false;
+		if (stereo_status != cg2900_device.tx_stereo_status) {
+			cg2900_device.tx_stereo_status = stereo_status;
+			status = cg2900_fm_set_mode(stereo_status);
+			if (0 != status) {
+				FM_ERR_REPORT("vidioc_set_modulator: "
+						"cg2900_fm_set_mode returned "
+						" %d", status);
+				goto error;
+			}
+		}
+		if (rds_status != cg2900_device.tx_rds_enabled) {
+			cg2900_device.tx_rds_enabled = rds_status;
+			status = cg2900_fm_tx_rds(rds_status);
+			if (0 != status) {
+				FM_ERR_REPORT("vidioc_set_modulator: "
+						"cg2900_fm_tx_rds returned "
+						" %d", status);
+				goto error;
+			}
+		}
+	}
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_set_modulator: returning %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_frequency()- Get the Current FM Frequnecy.
+ *
+ * This function is used to get the currently tuned
+ * frequency on FM Radio. This function is called when the application
+ * issues the IOCTL VIDIOC_G_FREQUENCY
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @freq: v4l2_frequency structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_frequency(
+			struct file *file,
+			void *priv,
+			struct v4l2_frequency *freq
+			)
+{
+	int status;
+	u32 frequency;
+	int ret_val = -EINVAL;
+	struct sk_buff *skb;
+
+	FM_INFO_REPORT("vidioc_get_frequency: Status = %d",
+			cg2900_device.seekstatus);
+
+	status = cg2900_fm_get_frequency(&frequency);
+
+	if (0 != status) {
+		freq->frequency = cg2900_device.frequency;
+		goto error;
+	}
+
+	if (cg2900_device.seekstatus == FMR_SEEK_IN_PROGRESS) {
+		if (skb_queue_empty(&fm_interrupt_queue)) {
+			/* No Interrupt, bad case */
+			FM_ERR_REPORT("vidioc_get_frequency: "
+				"No Interrupt to read");
+			fm_event = CG2900_EVENT_NO_EVENT;
+			goto error;
+		}
+		spin_lock(&fm_spinlock);
+		skb = skb_dequeue(&fm_interrupt_queue);
+		spin_unlock(&fm_spinlock);
+		if (!skb) {
+			/* No Interrupt, bad case */
+			FM_ERR_REPORT("vidioc_get_frequency: "
+				"No Interrupt to read");
+			fm_event = CG2900_EVENT_NO_EVENT;
+			goto error;
+		}
+		fm_event = (u8)skb->data[0];
+		FM_DEBUG_REPORT("vidioc_get_frequency: Interrupt = %x",
+				fm_event);
+		/*  Check if seek is finished or not */
+		if (CG2900_EVENT_SEARCH_CHANNEL_FOUND == fm_event) {
+			/* seek is finished */
+			spin_lock(&fm_spinlock);
+			cg2900_device.frequency = HZ_TO_V4L2(frequency);
+			freq->frequency = cg2900_device.frequency;
+			cg2900_device.seekstatus = FMR_SEEK_NONE;
+			fm_event = CG2900_EVENT_NO_EVENT;
+			kfree_skb(skb);
+			spin_unlock(&fm_spinlock);
+		} else {
+			/* Some other interrupt, queue it back */
+			spin_lock(&fm_spinlock);
+			skb_queue_head(&fm_interrupt_queue, skb);
+			spin_unlock(&fm_spinlock);
+		}
+	} else {
+		spin_lock(&fm_spinlock);
+		cg2900_device.frequency = HZ_TO_V4L2(frequency);
+		freq->frequency = cg2900_device.frequency;
+		spin_unlock(&fm_spinlock);
+	}
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_get_frequency: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_frequency()- Set the FM Frequnecy.
+ *
+ * This function is used to set the frequency
+ * on FM Radio. This function is called when the application
+ * issues the IOCTL VIDIOC_S_FREQUENCY
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @freq: v4l2_frequency structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_frequency(
+			struct file *file,
+			void *priv,
+			struct v4l2_frequency *freq
+			)
+{
+	u32 frequency = freq->frequency;
+	u32 freq_low, freq_high;
+	int status;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_set_frequency: Frequency = "
+			"%d ", V4L2_TO_HZ(frequency));
+
+	/*  Check which band is set currently */
+	switch (band) {
+	case CG2900_FM_BAND_US_EU:
+		freq_low = FMR_EU_US_LOW_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		freq_high = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		break;
+
+	case CG2900_FM_BAND_CHINA:
+		freq_low = FMR_CHINA_LOW_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		freq_high = FMR_CHINA_HIGH_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		break;
+
+	case CG2900_FM_BAND_JAPAN:
+		freq_low = FMR_JAPAN_LOW_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		freq_high = FMR_JAPAN_HIGH_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		break;
+
+	default:
+		/* Set to US_MAX and CHINA_MIN band */
+		freq_low = FMR_CHINA_LOW_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+		freq_high = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+			FMR_HZ_TO_MHZ_CONVERTER;
+	}
+
+	/* Check if the frequency set is out of current band */
+	if ((V4L2_TO_HZ(frequency) < freq_low) ||
+		(V4L2_TO_HZ(frequency) > freq_high))
+		goto error;
+
+	spin_lock(&fm_spinlock);
+	fm_event = CG2900_EVENT_NO_EVENT;
+	no_of_scan_freq = 0;
+	spin_unlock(&fm_spinlock);
+
+	cg2900_device.seekstatus = FMR_SEEK_NONE;
+	cg2900_device.frequency = frequency;
+	status = cg2900_fm_set_frequency(V4L2_TO_HZ(frequency));
+
+	if (0 != status)
+		goto error;
+
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_set_frequency: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_query_ctrl()- Query the FM Driver control features.
+ *
+ * This function is used to query the control features on FM Radio.
+ * This function is called when the application
+ * issues the IOCTL VIDIOC_QUERYCTRL
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @query_ctrl: v4l2_queryctrl structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_query_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_queryctrl *query_ctrl
+			)
+{
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_query_ctrl");
+	/* Check which control is requested */
+	switch (query_ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		FM_DEBUG_REPORT("vidioc_query_ctrl:  V4L2_CID_AUDIO_MUTE");
+		query_ctrl->type = V4L2_CTRL_TYPE_BOOLEAN;
+		query_ctrl->minimum = 0;
+		query_ctrl->maximum = 1;
+		query_ctrl->step = 1;
+		query_ctrl->default_value = 0;
+		query_ctrl->flags = 0;
+		strncpy(query_ctrl->name, "CG2900 Mute", 32);
+		ret_val = 0;
+		break;
+
+	case V4L2_CID_AUDIO_VOLUME:
+		FM_DEBUG_REPORT("vidioc_query_ctrl: V4L2_CID_AUDIO_VOLUME");
+
+		strncpy(query_ctrl->name, "CG2900 Volume", 32);
+		query_ctrl->minimum = MIN_ANALOG_VOLUME;
+		query_ctrl->maximum = MAX_ANALOG_VOLUME;
+		query_ctrl->step = 1;
+		query_ctrl->default_value = MAX_ANALOG_VOLUME;
+		query_ctrl->flags = 0;
+		query_ctrl->type = V4L2_CTRL_TYPE_INTEGER;
+		ret_val = 0;
+		break;
+
+	case V4L2_CID_AUDIO_BALANCE:
+		FM_DEBUG_REPORT("vidioc_query_ctrl:   V4L2_CID_AUDIO_BALANCE ");
+		strncpy(query_ctrl->name, "CG2900 Audio Balance", 32);
+		query_ctrl->type = V4L2_CTRL_TYPE_INTEGER;
+		query_ctrl->minimum = 0x0000;
+		query_ctrl->maximum = 0xFFFF;
+		query_ctrl->step = 0x0001;
+		query_ctrl->default_value = 0x0000;
+		query_ctrl->flags = 0;
+		ret_val = 0;
+		break;
+
+	case V4L2_CID_AUDIO_BASS:
+		FM_DEBUG_REPORT("vidioc_query_ctrl: "
+				"V4L2_CID_AUDIO_BASS (unsupported)");
+		break;
+
+	case V4L2_CID_AUDIO_TREBLE:
+		FM_DEBUG_REPORT("vidioc_query_ctrl: "
+				"V4L2_CID_AUDIO_TREBLE (unsupported)");
+		break;
+
+	default:
+		FM_DEBUG_REPORT("vidioc_query_ctrl: "
+				"--> unsupported id = %x", query_ctrl->id);
+		break;
+	}
+
+	FM_DEBUG_REPORT("vidioc_query_ctrl: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_ctrl()- Get the value of a particular Control.
+ *
+ * This function is used to get the value of a
+ * particular control from the FM Driver. This function is called
+ * when the application issues the IOCTL VIDIOC_G_CTRL
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @ctrl: v4l2_control structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_control *ctrl
+			)
+{
+	int status;
+	u8 value;
+	u16 rssi;
+	u8 antenna;
+	u16 conclusion;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_get_ctrl");
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_VOLUME:
+		status = cg2900_fm_get_volume(&value);
+		if (0 == status) {
+			ctrl->value = value;
+			cg2900_device.volume = value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_AUDIO_MUTE:
+		ctrl->value = cg2900_device.muted;
+		ret_val = 0;
+		break;
+	case V4L2_CID_AUDIO_BALANCE:
+		ctrl->value = cg2900_device.audiopath;
+		ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_RSSI_THRESHOLD:
+		ctrl->value = cg2900_device.rssi_threshold;
+		ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_SELECT_ANTENNA:
+		status = cg2900_fm_get_antenna(&antenna);
+		FM_DEBUG_REPORT("vidioc_get_ctrl: Antenna = %x", antenna);
+		if (0 == status) {
+			ctrl->value = antenna;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_GET_RESULT:
+		status = cg2900_fm_af_update_get_result(&rssi);
+		FM_DEBUG_REPORT("vidioc_get_ctrl: AF RSSI Level = %x", rssi);
+		if (0 == status) {
+			ctrl->value = rssi;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_GET_RESULT:
+		status = cg2900_fm_af_switch_get_result(&conclusion);
+		FM_DEBUG_REPORT("vidioc_get_ctrl: AF Switch conclusion = %x",
+				conclusion);
+		if (0 != status)
+			break;
+		if (conclusion == 0) {
+			ctrl->value = conclusion;
+			FM_DEBUG_REPORT("vidioc_get_ctrl: "
+					"AF Switch conclusion = %d",
+					ctrl->value);
+			ret_val = 0;
+		} else {
+			/*
+			 * Convert positive error code returned by chip
+			 * into negative error codes to be in line with linux.
+			 */
+			ctrl->value = -conclusion;
+			FM_ERR_REPORT("vidioc_get_ctrl: "
+			"AF-Switch failed with value %d", ctrl->value);
+			ret_val = 0;
+		}
+		break;
+	default:
+		FM_DEBUG_REPORT("vidioc_get_ctrl: "
+				"unsupported (id = %x)", (int)ctrl->id);
+		ret_val = -EINVAL;
+	}
+	FM_DEBUG_REPORT("vidioc_get_ctrl: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_ctrl()- Set the value of a particular Control.
+ *
+ * This function is used to set the value of a
+ * particular control from the FM Driver. This function is called when the
+ * application issues the IOCTL VIDIOC_S_CTRL
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @ctrl: v4l2_control structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -ERANGE when the parameter is out of range.
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_ctrl(
+			struct file *file,
+			void *priv,
+			struct v4l2_control *ctrl
+			)
+{
+	int status;
+	int ret_val = -EINVAL;
+	FM_INFO_REPORT("vidioc_set_ctrl");
+	/* Check which control is requested */
+	switch (ctrl->id) {
+	case V4L2_CID_AUDIO_MUTE:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_AUDIO_MUTE, "
+				"value = %d", ctrl->value);
+		if (ctrl->value > 1 && ctrl->value < 0) {
+			ret_val =  -ERANGE;
+			break;
+		}
+
+		if (ctrl->value) {
+			FM_DEBUG_REPORT("vidioc_set_ctrl: Ctrl_Id = "
+					"V4L2_CID_AUDIO_MUTE, "
+					"Muting the Radio");
+			status = cg2900_fm_mute();
+		} else {
+			FM_DEBUG_REPORT("vidioc_set_ctrl: "
+					"Ctrl_Id = V4L2_CID_AUDIO_MUTE, "
+					"UnMuting the Radio");
+			status = cg2900_fm_unmute();
+		}
+		if (0 == status) {
+			cg2900_device.muted = ctrl->value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_AUDIO_VOLUME:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_AUDIO_VOLUME, "
+				"value = %d", ctrl->value);
+		if (ctrl->value > MAX_ANALOG_VOLUME &&
+				ctrl->value < MIN_ANALOG_VOLUME) {
+			ret_val = -ERANGE;
+			break;
+		}
+		status = cg2900_fm_set_volume(ctrl->value);
+		if (0 == status) {
+			cg2900_device.volume = ctrl->value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_AUDIO_BALANCE:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_AUDIO_BALANCE, "
+				"value = %d", ctrl->value);
+		status = cg2900_fm_set_audio_balance(ctrl->value);
+		if (0 == status) {
+			cg2900_device.audiopath = ctrl->value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_CG2900_RADIO_CHIP_STATE:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_CHIP_STATE, "
+				"value = %d", ctrl->value);
+		if (V4L2_CG2900_RADIO_STANDBY == ctrl->value)
+			status = cg2900_fm_standby();
+		else if (V4L2_CG2900_RADIO_POWERUP == ctrl->value)
+			status = cg2900_fm_power_up_from_standby();
+		else
+			break;
+		if (0 != status)
+			break;
+		if (V4L2_CG2900_RADIO_STANDBY == ctrl->value)
+			cg2900_device.state = FMR_STANDBY;
+		else if (V4L2_CG2900_RADIO_POWERUP == ctrl->value)
+			cg2900_device.state = FMR_SWITCH_ON;
+		ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_SELECT_ANTENNA:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_SELECT_ANTENNA, "
+				"value = %d", ctrl->value);
+		status = cg2900_fm_select_antenna(ctrl->value);
+		if (0 == status)
+			ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_BANDSCAN:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_BANDSCAN, "
+				"value = %d", ctrl->value);
+		if (V4L2_CG2900_RADIO_BANDSCAN_START == ctrl->value) {
+			cg2900_device.seekstatus = FMR_SEEK_IN_PROGRESS;
+			no_of_scan_freq = 0;
+			status = cg2900_fm_start_band_scan();
+		} else if (V4L2_CG2900_RADIO_BANDSCAN_STOP == ctrl->value) {
+			status = cg2900_fm_stop_scan();
+			cg2900_device.seekstatus = FMR_SEEK_NONE;
+		} else
+			break;
+		if (0 == status)
+			ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_RSSI_THRESHOLD:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_RSSI_THRESHOLD "
+				"= %d", ctrl->value);
+		status = cg2900_fm_set_rssi_threshold(ctrl->value);
+		if (0 == status) {
+			cg2900_device.rssi_threshold = ctrl->value;
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_START:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"V4L2_CID_CG2900_RADIO_RDS_AF_UPDATE_START "
+				"freq = %d Hz", ctrl->value);
+		status = cg2900_fm_af_update_start(ctrl->value);
+		if (0 == status)
+			ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_TEST_TONE_GENERATOR_SET_STATUS:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+			"V4L2_CID_CG2900_RADIO_TEST_TONE_GENERATOR_SET_STATUS "
+			"state = %d ", ctrl->value);
+		if (ctrl->value < V4L2_CG2900_RADIO_TEST_TONE_GEN_OFF ||
+			ctrl->value >
+			V4L2_CG2900_RADIO_TEST_TONE_GENERATOR_ON_WO_SRC) {
+			FM_ERR_REPORT("Invalid parameter = %d", ctrl->value);
+			break;
+		}
+		status = cg2900_fm_set_test_tone_generator(ctrl->value);
+		if (0 == status)
+			ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_TUNE_DEEMPHASIS:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+			"V4L2_CID_CG2900_RADIO_TUNE_DEEMPHASIS, "
+			"Value = %d", ctrl->value);
+
+		if ((V4L2_CG2900_RADIO_DEEMPHASIS_DISABLED >
+			ctrl->value) ||
+			(V4L2_CG2900_RADIO_DEEMPHASIS_75_uS <
+			ctrl->value)) {
+			FM_ERR_REPORT("Unsupported deemphasis = %d",
+			ctrl->value);
+			break;
+		}
+
+		switch (ctrl->value) {
+		case V4L2_CG2900_RADIO_DEEMPHASIS_50_uS:
+			ctrl->value = FMD_EMPHASIS_50US;
+			break;
+		case V4L2_CG2900_RADIO_DEEMPHASIS_75_uS:
+			ctrl->value = FMD_EMPHASIS_75US;
+			break;
+		case V4L2_CG2900_RADIO_DEEMPHASIS_DISABLED:
+			/* Drop Down */
+		default:
+			ctrl->value = FMD_EMPHASIS_NONE;
+			break;
+		}
+		status = cg2900_fm_rx_set_deemphasis(ctrl->value);
+
+		if (0 == status)
+			ret_val = 0;
+		break;
+	default:
+		FM_DEBUG_REPORT("vidioc_set_ctrl: "
+				"unsupported (id = %x)", ctrl->id);
+	}
+	FM_DEBUG_REPORT("vidioc_set_ctrl: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_ext_ctrls()- Get the values of a particular control.
+ *
+ * This function is used to get the value of a
+ * particular control from the FM Driver. This is used when the data to
+ * be received is more than 1 paramter. This function is called when the
+ * application issues the IOCTL VIDIOC_G_EXT_CTRLS
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @ext_ctrl: v4l2_ext_controls structure.
+ *
+ * Returns:
+ *   0 when no error
+ *  -ENOSPC: when there is no space to copy the data into the buffer provided
+ * by application.
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_ext_ctrls(
+			struct file *file,
+			void *priv,
+			struct v4l2_ext_controls *ext_ctrl
+			)
+{
+	u32 *dest_buffer;
+	int index = 0;
+	int count = 0;
+	int ret_val = -EINVAL;
+	int status;
+	struct sk_buff *skb;
+	u8 mode;
+	s8 interrupt_success;
+	int *fm_interrupt_buffer;
+
+	FM_INFO_REPORT("vidioc_get_ext_ctrls: Id = %04x,"
+			"ext_ctrl->ctrl_class = %04x",
+			ext_ctrl->controls->id,
+			ext_ctrl->ctrl_class);
+
+	if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_FM_TX &&
+	    ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+		FM_ERR_REPORT("vidioc_get_ext_ctrls: Unsupported "
+			      "ctrl_class = %04x", ext_ctrl->ctrl_class);
+		goto error;
+	}
+
+	switch (ext_ctrl->controls->id) {
+	case V4L2_CID_CG2900_RADIO_BANDSCAN_GET_RESULTS:
+		if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+			FM_ERR_REPORT("vidioc_get_ext_ctrls:  "
+			"V4L2_CID_CG2900_RADIO_BANDSCAN_GET_RESULTS "
+			"Unsupported ctrl_class = %04x",
+			ext_ctrl->ctrl_class);
+			break;
+		}
+		if (cg2900_device.seekstatus ==
+			FMR_SEEK_IN_PROGRESS) {
+			spin_lock(&fm_spinlock);
+			skb = skb_dequeue(&fm_interrupt_queue);
+			spin_unlock(&fm_spinlock);
+			if (!skb) {
+				/* No Interrupt, bad case */
+				FM_ERR_REPORT("No Interrupt to read");
+				fm_event = CG2900_EVENT_NO_EVENT;
+				break;
+			}
+			fm_event = (u8)skb->data[0];
+			FM_DEBUG_REPORT(
+				"V4L2_CID_CG2900_RADIO"
+				"_BANDSCAN_GET_RESULTS: "
+				"fm_event = %x", fm_event);
+			if (fm_event ==
+				CG2900_EVENT_SCAN_CHANNELS_FOUND) {
+				/* Check to get Scan Result */
+				status =
+					cg2900_fm_get_scan_result
+					(&no_of_scan_freq, scanfreq,
+					scanfreq_rssi_level);
+				if (0 != status) {
+					FM_ERR_REPORT
+						("vidioc_get_ext_ctrls: "
+						"cg2900_fm_get_scan_"
+						"result: returned %d",
+						status);
+					kfree_skb(skb);
+					break;
+				}
+				kfree_skb(skb);
+			} else {
+				/* Some other interrupt, Queue it back */
+				spin_lock(&fm_spinlock);
+				skb_queue_head(&fm_interrupt_queue, skb);
+				spin_unlock(&fm_spinlock);
+			}
+		}
+		FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+				"SeekStatus = %x, GlobalEvent = %x, "
+				"numchannels = %x",
+				cg2900_device.seekstatus,
+				fm_event, no_of_scan_freq);
+
+		if (ext_ctrl->controls->size == 0 &&
+		    ext_ctrl->controls->string == NULL) {
+			if (cg2900_device.seekstatus ==
+			    FMR_SEEK_IN_PROGRESS &&
+			    CG2900_EVENT_SCAN_CHANNELS_FOUND
+			    == fm_event) {
+				spin_lock(&fm_spinlock);
+				ext_ctrl->controls->size =
+				    no_of_scan_freq;
+				cg2900_device.seekstatus
+				    = FMR_SEEK_NONE;
+				fm_event =
+				    CG2900_EVENT_NO_EVENT;
+				spin_unlock(&fm_spinlock);
+				return -ENOSPC;
+			}
+		} else if (ext_ctrl->controls->string != NULL) {
+			dest_buffer =
+			    (u32 *) ext_ctrl->controls->string;
+			while (index < no_of_scan_freq) {
+				*(dest_buffer + count + 0) =
+				    HZ_TO_V4L2(scanfreq[index]);
+				*(dest_buffer + count + 1) =
+				    scanfreq_rssi_level[index];
+				count += 2;
+				index++;
+			}
+			ret_val = 0;
+		}
+		break;
+	case V4L2_CID_CG2900_RADIO_BLOCKSCAN_GET_RESULTS:
+		if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+			FM_ERR_REPORT("vidioc_get_ext_ctrls:  "
+			"V4L2_CID_CG2900_RADIO_BLOCKSCAN"
+			"_GET_RESULTS "
+			"Unsupported ctrl_class = %04x",
+			ext_ctrl->ctrl_class);
+			break;
+		}
+		if (cg2900_device.seekstatus == FMR_SEEK_IN_PROGRESS) {
+			spin_lock(&fm_spinlock);
+			skb = skb_dequeue(&fm_interrupt_queue);
+			spin_unlock(&fm_spinlock);
+			if (!skb) {
+				/* No Interrupt, bad case */
+				FM_ERR_REPORT("No Interrupt to read");
+				fm_event = CG2900_EVENT_NO_EVENT;
+				break;
+			}
+			fm_event = (u8)skb->data[0];
+			FM_DEBUG_REPORT(
+				"V4L2_CID_CG2900_RADIO_BLOCKSCAN"
+				"GET_RESULTS: "
+				"fm_event = %x", fm_event);
+			if (fm_event ==
+				CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND) {
+				/* Check for BlockScan Result */
+				status =
+					cg2900_fm_get_block_scan_result
+					(&no_of_block_scan_freq,
+					block_scan_rssi_level);
+				if (0 != status) {
+					FM_ERR_REPORT
+						("vidioc_get_ext_ctrls: "
+						"cg2900_fm_get_block_scan_"
+						"result: returned %d",
+						status);
+					kfree_skb(skb);
+					break;
+				}
+				kfree_skb(skb);
+			} else {
+				/* Some other interrupt,
+				Queue it back */
+				spin_lock(&fm_spinlock);
+				skb_queue_head(&fm_interrupt_queue, skb);
+				spin_unlock(&fm_spinlock);
+			}
+		}
+		FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+				"SeekStatus = %x, GlobalEvent = %x, "
+				"numchannels = %x",
+				cg2900_device.seekstatus,
+				fm_event, no_of_block_scan_freq);
+		if (ext_ctrl->controls->size == 0 &&
+		    ext_ctrl->controls->string == NULL) {
+			if (cg2900_device.seekstatus ==
+			    FMR_SEEK_IN_PROGRESS &&
+			    CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND
+			    == fm_event) {
+				spin_lock(&fm_spinlock);
+				ext_ctrl->controls->size =
+				    no_of_block_scan_freq;
+				cg2900_device.seekstatus
+				    = FMR_SEEK_NONE;
+				fm_event =
+				    CG2900_EVENT_NO_EVENT;
+				spin_unlock(&fm_spinlock);
+				return -ENOSPC;
+			}
+		} else if (ext_ctrl->controls->size >=
+			   no_of_block_scan_freq &&
+			   ext_ctrl->controls->string != NULL) {
+			dest_buffer =
+			    (u32 *) ext_ctrl->controls->string;
+			while (index < no_of_block_scan_freq) {
+				*(dest_buffer + index) =
+				    block_scan_rssi_level
+				    [index];
+				index++;
+			}
+			ret_val = 0;
+			return ret_val;
+		}
+		break;
+	case V4L2_CID_RDS_TX_DEVIATION:
+		FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+				"V4L2_CID_RDS_TX_DEVIATION");
+		if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+			FM_ERR_REPORT("Invalid Ctrl Class = %x",
+				      ext_ctrl->ctrl_class);
+			break;
+		}
+		status = cg2900_fm_tx_get_rds_deviation((u16 *) &
+						     ext_ctrl->
+						     controls->value);
+		if (status == 0)
+			ret_val = 0;
+		break;
+	case V4L2_CID_PILOT_TONE_ENABLED:
+		FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+				"V4L2_CID_PILOT_TONE_ENABLED");
+		if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+			FM_ERR_REPORT("Invalid Ctrl Class = %x",
+				      ext_ctrl->ctrl_class);
+			break;
+		}
+		status = cg2900_fm_tx_get_pilot_tone_status(
+				(bool *)&ext_ctrl->controls->value);
+		if (status == 0)
+			ret_val = 0;
+		break;
+	case V4L2_CID_PILOT_TONE_DEVIATION:
+		FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+				"V4L2_CID_PILOT_TONE_DEVIATION");
+		if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+			FM_ERR_REPORT("Invalid Ctrl Class = %x",
+				      ext_ctrl->ctrl_class);
+			break;
+		}
+		status = cg2900_fm_tx_get_pilot_deviation(
+				(u16 *)&ext_ctrl->controls->value);
+		if (status == 0)
+			ret_val = 0;
+		break;
+	case V4L2_CID_TUNE_PREEMPHASIS:
+		FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+				"V4L2_CID_TUNE_PREEMPHASIS");
+		if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+			FM_ERR_REPORT("Invalid Ctrl Class = %x",
+				      ext_ctrl->ctrl_class);
+			break;
+		}
+		status = cg2900_fm_tx_get_preemphasis(
+				(u8 *)&ext_ctrl->controls->value);
+		if (status == 0)
+			ret_val = 0;
+		break;
+	case V4L2_CID_TUNE_POWER_LEVEL:
+		FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+				"V4L2_CID_TUNE_POWER_LEVEL");
+		if (V4L2_CTRL_CLASS_FM_TX != ext_ctrl->ctrl_class) {
+			FM_ERR_REPORT("Invalid Ctrl Class = %x",
+				      ext_ctrl->ctrl_class);
+			break;
+		}
+		status = cg2900_fm_tx_get_power_level(
+				(u16 *)&ext_ctrl->controls->value);
+		if (status == 0)
+			ret_val = 0;
+		break;
+	case V4L2_CID_CG2900_RADIO_GET_INTERRUPT:
+		if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+			FM_ERR_REPORT("vidioc_get_ext_ctrls:  "
+			"V4L2_CID_CG2900_RADIO_GET_INTERRUPT "
+			"Unsupported ctrl_class = %04x",
+			ext_ctrl->ctrl_class);
+			break;
+		}
+		if (ext_ctrl->controls->size != FMR_GET_INTERRUPT_DATA_SIZE ||
+			ext_ctrl->controls->string == NULL) {
+			FM_ERR_REPORT("vidioc_get_ext_ctrls:  "
+			"V4L2_CID_CG2900_RADIO_GET_INTERRUPT "
+			"Invalid parameters, ext_ctrl->controls->size = %x "
+			"ext_ctrl->controls->string = %08x",
+			ext_ctrl->controls->size,
+			(unsigned int)ext_ctrl->controls->string);
+			ret_val = -ENOSPC;
+			break;
+		}
+		spin_lock(&fm_spinlock);
+		skb = skb_dequeue(&fm_interrupt_queue);
+		spin_unlock(&fm_spinlock);
+		if (!skb) {
+			/* No Interrupt, bad case */
+			FM_ERR_REPORT("V4L2_CID_CG2900_RADIO_GET_INTERRUPT: "
+				"No Interrupt to read");
+			fm_event = CG2900_EVENT_NO_EVENT;
+			break;
+		}
+		fm_event = (u8)skb->data[0];
+		interrupt_success = (s8)skb->data[1];
+		FM_DEBUG_REPORT("vidioc_get_ctrl: Interrupt = %x "
+				"interrupt_success = %x",
+				fm_event, interrupt_success);
+		fm_interrupt_buffer =
+		    (int *) ext_ctrl->controls->string;
+		/* Interrupt that has occurred */
+		*fm_interrupt_buffer = cg2900_map_event_to_v4l2(fm_event);
+
+		/* Interrupt success or failed */
+		if (interrupt_success) {
+			/* Interrupt Success, return 0 */
+			*(fm_interrupt_buffer + 1) = 0;
+		} else {
+			spin_lock(&fm_spinlock);
+			no_of_scan_freq = 0;
+			no_of_block_scan_freq = 0;
+			spin_unlock(&fm_spinlock);
+			cg2900_device.seekstatus = FMR_SEEK_NONE;
+			/* Clear the Interrupt flag */
+			fm_event = CG2900_EVENT_NO_EVENT;
+			kfree_skb(skb);
+			/* Interrupt Success, return negative error */
+			*(fm_interrupt_buffer + 1) = -1;
+			FM_ERR_REPORT("vidioc_get_ext_ctrls: Interrupt = %d "
+				"failed with reason = %d",
+				(*fm_interrupt_buffer),
+				(*(fm_interrupt_buffer + 1)));
+			/*
+			 * Update return value, so that application
+			 * can read the event failure reason.
+			 */
+			ret_val = 0;
+			break;
+		}
+
+		if (CG2900_EVENT_MONO_STEREO_TRANSITION
+			== fm_event) {
+			/*
+			 * In case of Mono/Stereo Interrupt,
+			 * get the current value from chip
+			 */
+			status = cg2900_fm_get_mode(&mode);
+			cg2900_device.rx_stereo_status = (bool)mode;
+			/* Clear the Interrupt flag */
+			fm_event = CG2900_EVENT_NO_EVENT;
+			kfree_skb(skb);
+		} else if (CG2900_EVENT_SCAN_CANCELLED ==
+			fm_event) {
+			/* Scan/Search cancelled by User */
+			spin_lock(&fm_spinlock);
+			no_of_scan_freq = 0;
+			no_of_block_scan_freq = 0;
+			spin_unlock(&fm_spinlock);
+			cg2900_device.seekstatus = FMR_SEEK_NONE;
+			/* Clear the Interrupt flag */
+			fm_event = CG2900_EVENT_NO_EVENT;
+			kfree_skb(skb);
+		} else {
+				/* Queue the interrupt back
+					for later dequeuing */
+				FM_DEBUG_REPORT("V4L2_CID_CG2900"
+					"_RADIO_GET_INTERRUPT: "
+					"Queuing the interrupt"
+					"again to head of list");
+				spin_lock(&fm_spinlock);
+				skb_queue_head(&fm_interrupt_queue, skb);
+				spin_unlock(&fm_spinlock);
+		}
+		ret_val = 0;
+		break;
+	default:
+		FM_DEBUG_REPORT("vidioc_get_ext_ctrls: "
+				"unsupported (id = %x)",
+				ext_ctrl->controls->id);
+	}
+
+error:
+	FM_DEBUG_REPORT("vidioc_get_ext_ctrls: returning = %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_set_ext_ctrls()- Set the values of a particular control.
+ *
+ * This function is used to set the value of a
+ * particular control on the FM Driver. This is used when the data to
+ * be set is more than 1 paramter. This function is called when the
+ * application issues the IOCTL VIDIOC_S_EXT_CTRLS
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @ext_ctrl: v4l2_ext_controls structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -ENOSPC: when there is no space to copy the data into the buffer provided
+ * by application.
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_ext_ctrls(
+			struct file *file,
+			void *priv,
+			struct v4l2_ext_controls *ext_ctrl
+			)
+{
+	int ret_val = -EINVAL;
+	int status;
+
+	FM_INFO_REPORT("vidioc_set_ext_ctrls: Id = %04x, ctrl_class = %04x",
+			ext_ctrl->controls->id, ext_ctrl->ctrl_class);
+
+	if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_FM_TX &&
+	    ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+		FM_ERR_REPORT("vidioc_set_ext_ctrls: Unsupported "
+			      "ctrl_class = %04x", ext_ctrl->ctrl_class);
+		goto error;
+	}
+
+	switch (ext_ctrl->controls->id) {
+	case V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START:
+		{
+			u32 af_switch_freq;
+			u16 af_switch_pi;
+			u32 *af_switch_buf;
+
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+
+			if (ext_ctrl->controls->size !=
+				FMR_AF_SWITCH_DATA_SIZE ||
+				ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+
+			af_switch_buf = (u32 *) ext_ctrl->controls->string;
+			af_switch_freq = V4L2_TO_HZ(*af_switch_buf);
+			af_switch_pi = *(af_switch_buf + 1);
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+				"V4L2_CID_CG2900_RADIO_RDS_AF_SWITCH_START: "
+				"AF Switch Freq =%d Hz AF Switch PI = %04x",
+				(int)af_switch_freq, af_switch_pi);
+
+			if (af_switch_freq < (FMR_CHINA_LOW_FREQ_IN_MHZ
+				* FMR_HZ_TO_MHZ_CONVERTER) ||
+				af_switch_freq > (FMR_CHINA_HIGH_FREQ_IN_MHZ
+				* FMR_HZ_TO_MHZ_CONVERTER)) {
+				FM_ERR_REPORT("Invalid Freq = %04x",
+					af_switch_freq);
+				break;
+			}
+
+			status = cg2900_fm_af_switch_start(
+					af_switch_freq,
+					af_switch_pi);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_DEVIATION:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_DEVIATION, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+
+			if (ext_ctrl->controls->value <= MIN_RDS_DEVIATION &&
+			    ext_ctrl->controls->value > MAX_RDS_DEVIATION) {
+				FM_ERR_REPORT("Invalid RDS Deviation = %02x",
+					ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_rds_deviation(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_PI:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_PI, PI = %04x",
+					ext_ctrl->controls->value);
+
+			if (ext_ctrl->controls->value <= MIN_PI_VALUE &&
+			    ext_ctrl->controls->value > MAX_PI_VALUE) {
+				FM_ERR_REPORT("Invalid PI = %04x",
+					ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_pi_code(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_PTY:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_PTY, PTY = %d",
+					ext_ctrl->controls->value);
+
+			if (ext_ctrl->controls->value < MIN_PTY_VALUE &&
+			    ext_ctrl->controls->value > MAX_PTY_VALUE) {
+				FM_ERR_REPORT("Invalid PTY = %02x",
+					      ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_pty_code(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_PS_NAME:
+		{
+			if (ext_ctrl->controls->size > MAX_PSN_SIZE
+			    || ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("Invalid PSN");
+				break;
+			}
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_PS_NAME, "
+					"PSN = %s, Len = %x",
+					ext_ctrl->controls->string,
+					ext_ctrl->controls->size);
+
+			status = cg2900_fm_tx_set_program_station_name(
+					ext_ctrl->controls->string,
+					ext_ctrl->controls->size);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_RDS_TX_RADIO_TEXT:
+		{
+			if (ext_ctrl->controls->size >= MAX_RT_SIZE
+			    || ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("Invalid RT");
+				break;
+			}
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_RDS_TX_RADIO_TEXT, "
+					"RT = %s, Len = %x",
+					ext_ctrl->controls->string,
+					ext_ctrl->controls->size);
+
+			status = cg2900_fm_tx_set_radio_text(
+					ext_ctrl->controls->string,
+					ext_ctrl->controls->size);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_PILOT_TONE_ENABLED:
+		{
+			bool enable;
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_PILOT_TONE_ENABLED, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+
+			if (FMD_PILOT_TONE_ENABLED ==
+				ext_ctrl->controls->value)
+				enable = true;
+			else if (FMD_PILOT_TONE_DISABLED ==
+				ext_ctrl->controls->value)
+				enable = false;
+			else {
+				FM_ERR_REPORT("Unsupported Value = %d",
+					      ext_ctrl->controls->value);
+				break;
+			}
+			status = cg2900_fm_tx_set_pilot_tone_status(enable);
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_PILOT_TONE_DEVIATION:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_PILOT_TONE_DEVIATION, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+
+			if (ext_ctrl->controls->value <= MIN_PILOT_DEVIATION &&
+			    ext_ctrl->controls->value > MAX_PILOT_DEVIATION) {
+				FM_ERR_REPORT("Invalid Pilot Deviation = %02x",
+					      ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_pilot_deviation(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_TUNE_PREEMPHASIS:
+		{
+			u8 preemphasis;
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_TUNE_PREEMPHASIS, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+
+			if ((V4L2_PREEMPHASIS_50_uS >
+					ext_ctrl->controls->value) ||
+					(V4L2_PREEMPHASIS_75_uS <
+					ext_ctrl->controls->value)) {
+				FM_ERR_REPORT("Unsupported Preemphasis = %d",
+					      ext_ctrl->controls->value);
+				break;
+			}
+
+			if (V4L2_PREEMPHASIS_50_uS ==
+				ext_ctrl->controls->value) {
+				preemphasis = FMD_EMPHASIS_50US;
+			} else if (V4L2_PREEMPHASIS_75_uS ==
+				ext_ctrl->controls->value) {
+				preemphasis = FMD_EMPHASIS_75US;
+			}
+
+			status = cg2900_fm_tx_set_preemphasis(preemphasis);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_TUNE_POWER_LEVEL:
+		{
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_TUNE_POWER_LEVEL, "
+					"Value = %d",
+					ext_ctrl->controls->value);
+			if (ext_ctrl->controls->value < MIN_POWER_LEVEL &&
+			    ext_ctrl->controls->value > MAX_POWER_LEVEL) {
+				FM_ERR_REPORT("Invalid Power Level = %02x",
+					      ext_ctrl->controls->value);
+				break;
+			}
+
+			status = cg2900_fm_tx_set_power_level(
+					ext_ctrl->controls->value);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_CG2900_RADIO_BLOCKSCAN_START:
+		{
+			u32 start_freq;
+			u32 end_freq;
+			u32 *block_scan_buf;
+			u32 current_grid;
+			u32 low_freq;
+			u32 high_freq;
+			u32 result_freq;
+			u8  no_of_block_scan_channels;
+
+			/* V4L2 Initial check */
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_BLOCKSCAN_START "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+
+			if (ext_ctrl->controls->size !=
+				FMR_BLOCK_SCAN_DATA_SIZE ||
+				ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_BLOCKSCAN_START "
+				"Invalid Parameters");
+				break;
+			}
+
+			/* Check for current grid */
+			if (grid == CG2900_FM_GRID_50)
+				current_grid = FMR_CHINA_GRID_IN_HZ;
+			else if (grid == CG2900_FM_GRID_100)
+				current_grid = FMR_EUROPE_GRID_IN_HZ;
+			else
+				current_grid = FMR_USA_GRID_IN_HZ;
+
+			/* Check for current band */
+			if (band == CG2900_FM_BAND_US_EU) {
+				low_freq = FMR_EU_US_LOW_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+				high_freq = FMR_EU_US_HIGH_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+
+			} else if (band == CG2900_FM_BAND_JAPAN) {
+				low_freq = FMR_JAPAN_LOW_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+				high_freq = FMR_JAPAN_HIGH_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+
+			} else {
+				low_freq = FMR_CHINA_LOW_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+				high_freq = FMR_CHINA_HIGH_FREQ_IN_MHZ *
+					FMR_HZ_TO_MHZ_CONVERTER;
+			}
+
+			/* V4L2 Extended control */
+
+			block_scan_buf = (u32 *)ext_ctrl->controls->string;
+			start_freq = V4L2_TO_HZ(*block_scan_buf);
+			end_freq = V4L2_TO_HZ(*(block_scan_buf + 1));
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+					"V4L2_CID_CG2900_RADIO_"
+					"BLOCKSCAN_START: "
+					"Start Freq = %d Hz "
+					"End Freq = %d Hz",
+					(int)start_freq,
+					(int)end_freq);
+
+			result_freq = end_freq - start_freq;
+			no_of_block_scan_channels =
+				(u8)(result_freq / current_grid);
+
+			/* Frequency Check */
+			if (end_freq < start_freq) {
+				FM_ERR_REPORT("Start Freq (%d Hz) "
+					" > End Freq (%d Hz)",
+					(int)start_freq,
+					(int)end_freq);
+				break;
+			}
+
+			if ((start_freq < low_freq) ||
+				(start_freq > high_freq)) {
+				FM_ERR_REPORT("Out of Band Freq: "
+					"Start Freq = %d Hz",
+					(int)start_freq);
+				break;
+			}
+
+			if ((end_freq < low_freq) ||
+				(end_freq > high_freq)) {
+				FM_ERR_REPORT("Out of Band Freq: "
+					"End Freq = %d Hz",
+					(int)end_freq);
+				break;
+			}
+
+			/* Maximum allowed block scan range */
+			if (FMR_MAX_BLOCK_SCAN_CHANNELS <
+				no_of_block_scan_channels) {
+				FM_ERR_REPORT("No of channels (%x)"
+					"exceeds Max Block Scan (%x)",
+					no_of_block_scan_channels,
+					FMR_MAX_BLOCK_SCAN_CHANNELS);
+				break;
+			}
+
+			status = cg2900_fm_start_block_scan(
+					start_freq,
+					end_freq);
+			if (0 == status) {
+				cg2900_device.seekstatus =
+					FMR_SEEK_IN_PROGRESS;
+				ret_val = 0;
+			}
+			break;
+		}
+	case V4L2_CID_CG2900_RADIO_TEST_TONE_CONNECT:
+		{
+			u8 left_audio_mode;
+			u8 right_audio_mode;
+			u8 *test_tone_connect_buf;
+
+			/* V4L2 Initial check */
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+					"V4L2_CID_CG2900_RADIO_"
+					"TEST_TONE_CONNECT "
+					"Unsupported ctrl_class = %04x",
+					ext_ctrl->ctrl_class);
+				break;
+			}
+
+			if (ext_ctrl->controls->size !=
+				FMR_TEST_TONE_CONNECT_DATA_SIZE ||
+				ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+					"V4L2_CID_CG2900_RADIO_TEST"
+					"_TONE_CONNECT "
+					"Invalid Parameters");
+				break;
+			}
+
+			/* V4L2 Extended control */
+			test_tone_connect_buf =
+				(u8 *)ext_ctrl->controls->string;
+			left_audio_mode = *test_tone_connect_buf;
+			right_audio_mode = *(test_tone_connect_buf + 1);
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+				"V4L2_CID_CG2900_RADIO_TEST_TONE_CONNECT"
+				"left_audio_mode Freq = %02x"
+				"right_audio_modeFreq = %02x",
+				left_audio_mode,
+				right_audio_mode);
+
+			/* Range Check */
+			if (left_audio_mode > \
+			V4L2_CG2900_RADIO_TEST_TONE_TONE_SUM) {
+				FM_ERR_REPORT("Invalid Value of "
+					"left_audio_mode (%02x) ",
+					left_audio_mode);
+				break;
+			}
+
+			if (right_audio_mode > \
+			V4L2_CG2900_RADIO_TEST_TONE_TONE_SUM) {
+				FM_ERR_REPORT("Invalid Value of "
+					"right_audio_mode (%02x) ",
+					left_audio_mode);
+				break;
+			}
+
+			status = cg2900_fm_test_tone_connect(
+					left_audio_mode,
+					right_audio_mode);
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	case V4L2_CID_CG2900_RADIO_TEST_TONE_SET_PARAMS:
+		{
+			u8 tone_gen;
+			u16 frequency;
+			u16 volume;
+			u16 phase_offset;
+			u16 dc;
+			u8 waveform;
+			u16 *test_tone_set_params_buf;
+
+			/* V4L2 Initial check */
+			if (ext_ctrl->ctrl_class != V4L2_CTRL_CLASS_USER) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"V4L2_CID_CG2900_RADIO_TEST_TONE_SET_PARAMS "
+				"Unsupported ctrl_class = %04x",
+				ext_ctrl->ctrl_class);
+				break;
+			}
+
+			if (ext_ctrl->controls->size !=
+				FMR_TEST_TONE_SET_PARAMS_DATA_SIZE ||
+				ext_ctrl->controls->string == NULL) {
+				FM_ERR_REPORT("vidioc_set_ext_ctrls:  "
+				"FMR_TEST_TONE_SET_PARAMS_DATA_SIZE "
+				"Invalid Parameters");
+				break;
+			}
+
+			/* V4L2 Extended control */
+			test_tone_set_params_buf = \
+			(u16 *)ext_ctrl->controls->string;
+
+			tone_gen = (u8)(*test_tone_set_params_buf);
+			frequency = *(test_tone_set_params_buf + 1);
+			volume = *(test_tone_set_params_buf + 2);
+			phase_offset = *(test_tone_set_params_buf + 3);
+			dc = *(test_tone_set_params_buf + 4);
+			waveform = (u8)(*(test_tone_set_params_buf + 5));
+
+			FM_DEBUG_REPORT("vidioc_set_ext_ctrls: "
+				"V4L2_CID_CG2900_RADIO_TEST_TONE_SET_PARAMS"
+				"tone_gen = %02x frequency = %04x"
+				"volume = %04x phase_offset = %04x"
+				"dc = %04x waveform = %02x",
+				tone_gen, frequency,
+				volume, phase_offset,
+				dc, waveform);
+
+			/* Range Check */
+			if (tone_gen > FMD_TST_TONE_2) {
+				FM_ERR_REPORT("Invalid Value of "
+					"tone_gen (%02x) ",
+					tone_gen);
+				break;
+			}
+
+			if (waveform > FMD_TST_TONE_PULSE) {
+				FM_ERR_REPORT("Invalid Value of "
+					"waveform (%02x) ",
+					waveform);
+				break;
+			}
+
+			if (frequency > 0x7FFF) {
+				FM_ERR_REPORT("Invalid Value of "
+					"frequency (%04x) ",
+					frequency);
+				break;
+			}
+
+			if (volume > 0x7FFF) {
+				FM_ERR_REPORT("Invalid Value of "
+					"volume (%04x) ",
+					volume);
+				break;
+			}
+
+			status = cg2900_fm_test_tone_set_params(
+					tone_gen,
+					frequency,
+					volume,
+					phase_offset,
+					dc,
+					waveform);
+
+			if (0 == status)
+				ret_val = 0;
+			break;
+		}
+	default:
+		{
+			FM_ERR_REPORT("vidioc_set_ext_ctrls: "
+				      "Unsupported Id = %04x",
+				      ext_ctrl->controls->id);
+		}
+	}
+error:
+	return ret_val;
+}
+
+/**
+ * vidioc_set_hw_freq_seek()- seek Up/Down Frequency.
+ *
+ * This function is used to start seek
+ * on the FM Radio. Direction if seek is as inicated by the parameter
+ * inside the v4l2_hw_freq_seek structure. This function is called when the
+ * application issues the IOCTL VIDIOC_S_HW_FREQ_SEEK
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @freq_seek: v4l2_hw_freq_seek structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_hw_freq_seek(
+			struct file *file,
+			void *priv,
+			struct v4l2_hw_freq_seek *freq_seek
+			)
+{
+	int status;
+	int ret_val = -EINVAL;
+
+	FM_INFO_REPORT("vidioc_set_hw_freq_seek");
+
+	FM_DEBUG_REPORT("vidioc_set_hw_freq_seek: Status = %x, "
+			"Upwards = %x, Wrap Around = %x",
+			cg2900_device.seekstatus,
+			freq_seek->seek_upward, freq_seek->wrap_around);
+
+	if (cg2900_device.seekstatus == FMR_SEEK_IN_PROGRESS) {
+		FM_ERR_REPORT("vidioc_set_hw_freq_seek: "
+				"VIDIOC_S_HW_FREQ_SEEK, "
+				"freq_seek in progress");
+		goto error;
+	}
+
+	spin_lock(&fm_spinlock);
+	fm_event = CG2900_EVENT_NO_EVENT;
+	no_of_scan_freq = 0;
+	spin_unlock(&fm_spinlock);
+
+	if (CG2900_DIR_UP == freq_seek->seek_upward)
+		status = cg2900_fm_search_up_freq();
+	else if (CG2900_DIR_DOWN == freq_seek->seek_upward)
+		status = cg2900_fm_search_down_freq();
+	else
+		goto error;
+
+	if (0 != status)
+		goto error;
+
+	cg2900_device.seekstatus = FMR_SEEK_IN_PROGRESS;
+	ret_val = 0;
+
+error:
+	FM_DEBUG_REPORT("vidioc_set_hw_freq_seek: returning = %d",
+			ret_val);
+	return ret_val;
+}
+
+/**
+ * vidioc_get_audio()- Get Audio features of FM Driver.
+ *
+ * This function is used to get the audio features of FM Driver.
+ * This function is imlemented as a dumy function.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @audio: (out) v4l2_audio structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_audio(
+			struct file *file,
+			void *priv,
+			struct v4l2_audio *audio
+			)
+{
+	FM_INFO_REPORT("vidioc_get_audio");
+	strcpy(audio->name, "");
+	audio->capability = 0;
+	audio->mode = 0;
+	return 0;
+}
+
+/**
+ * vidioc_set_audio()- Set Audio features of FM Driver.
+ *
+ * This function is used to set the audio features of FM Driver.
+ * This function is imlemented as a dumy function.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @audio: v4l2_audio structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_audio(
+			struct file *file,
+			void *priv,
+			struct v4l2_audio *audio
+			)
+{
+	FM_INFO_REPORT("vidioc_set_audio");
+	if (audio->index != 0)
+		return -EINVAL;
+	return 0;
+}
+
+/**
+ * vidioc_get_input()- Get the Input Value
+ *
+ * This function is used to get the Input.
+ * This function is imlemented as a dumy function.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @input: (out) Value to be stored.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_get_input(
+			struct file *file,
+			void *priv,
+			unsigned int *input
+			)
+{
+	FM_INFO_REPORT("vidioc_get_input");
+	*input = 0;
+	return 0;
+}
+
+/**
+ * vidioc_set_input()- Set the input value.
+ *
+ * This function is used to set input.
+ * This function is imlemented as a dumy function.
+ *
+ * @file: File structure.
+ * @priv: Previous data of file structure.
+ * @input: Value to set
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int vidioc_set_input(
+			struct file *file,
+			void *priv,
+			unsigned int input
+			)
+{
+	FM_INFO_REPORT("vidioc_set_input");
+	if (input != 0)
+		return -EINVAL;
+	return 0;
+}
+
+/**
+ * cg2900_convert_err_to_v4l2()- Convert Error Bits to V4L2 RDS format.
+ *
+ * This function converts the error bits in RDS Block
+ * as received from Chip into V4L2 RDS data specification.
+ *
+ * @status_byte: The status byte as received in RDS Group for
+ * particular RDS Block
+ * @out_byte: byte to store the modified byte with the err bits
+ * alligned as per V4L2 RDS Specifications.
+ */
+static void cg2900_convert_err_to_v4l2(
+			char status_byte,
+			char *out_byte
+			)
+{
+	if ((status_byte & RDS_ERROR_STATUS_MASK) == RDS_ERROR_STATUS_MASK) {
+		/* Uncorrectable Block */
+		*out_byte = (*out_byte | V4L2_RDS_BLOCK_ERROR);
+	} else if (((status_byte & RDS_UPTO_TWO_BITS_CORRECTED)
+		    == RDS_UPTO_TWO_BITS_CORRECTED) ||
+		   ((status_byte & RDS_UPTO_FIVE_BITS_CORRECTED)
+		    == RDS_UPTO_FIVE_BITS_CORRECTED)) {
+		/* Corrected Bits in Block */
+		*out_byte = (*out_byte | V4L2_RDS_BLOCK_CORRECTED);
+	}
+}
+
+/**
+ * cg2900_map_event_to_v4l2()- Maps cg2900 event to v4l2 events .
+ *
+ * This function maps cg2900 events to corresponding v4l2 events.
+ *
+ * @event: This contains the cg2900 event to be converted.
+ *
+ * Returns: Corresponding V4L2 events.
+ */
+static int cg2900_map_event_to_v4l2(
+			u8 event
+			)
+{
+	switch (event) {
+	case CG2900_EVENT_MONO_STEREO_TRANSITION:
+		return V4L2_CG2900_RADIO_INTERRUPT_MONO_STEREO_TRANSITION;
+	case CG2900_EVENT_SEARCH_CHANNEL_FOUND:
+		return V4L2_CG2900_RADIO_INTERRUPT_SEARCH_COMPLETED;
+	case CG2900_EVENT_SCAN_CHANNELS_FOUND:
+		return V4L2_CG2900_RADIO_INTERRUPT_BAND_SCAN_COMPLETED;
+	case CG2900_EVENT_BLOCK_SCAN_CHANNELS_FOUND:
+		return V4L2_CG2900_RADIO_INTERRUPT_BLOCK_SCAN_COMPLETED;
+	case CG2900_EVENT_SCAN_CANCELLED:
+		return V4L2_CG2900_RADIO_INTERRUPT_SCAN_CANCELLED;
+	case CG2900_EVENT_DEVICE_RESET:
+		return V4L2_CG2900_RADIO_INTERRUPT_DEVICE_RESET;
+	case CG2900_EVENT_RDS_EVENT:
+		return V4L2_CG2900_RADIO_INTERRUPT_RDS_RECEIVED;
+	default:
+		return V4L2_CG2900_RADIO_INTERRUPT_UNKNOWN;
+	}
+}
+
+/**
+ * cg2900_open()- This function nitializes and switches on FM.
+ *
+ * This is called when the application opens the character device.
+ *
+ * @file: File structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int cg2900_open(
+			struct file *file
+			)
+{
+	int status;
+	int ret_val = -EINVAL;
+	struct video_device *vdev = video_devdata(file);
+
+	mutex_lock(&fm_mutex);
+	users++;
+	FM_INFO_REPORT("cg2900_open: users = %d", users);
+
+	if (users > 1) {
+		FM_INFO_REPORT("cg2900_open: FM already switched on!!!");
+		ret_val = 0;
+		/*
+		 * No need to perform the initialization and switch on FM
+		 * since it is already done during the first open call to
+		 * this driver.
+		 */
+		goto done;
+	}
+
+	status = cg2900_fm_init();
+	if (0 != status)
+		goto init_error;
+
+	FM_DEBUG_REPORT("cg2900_open: Switching on FM");
+	status = cg2900_fm_switch_on(&(vdev->dev));
+	if (0 != status)
+		goto switch_on_error;
+
+	cg2900_device.state = FMR_SWITCH_ON;
+	cg2900_device.frequency = HZ_TO_V4L2(freq_low);
+	cg2900_device.rx_rds_enabled = false;
+	cg2900_device.muted = false;
+	cg2900_device.audiopath = 0;
+	cg2900_device.seekstatus = FMR_SEEK_NONE;
+	cg2900_device.rssi_threshold = CG2900_FM_DEFAULT_RSSI_THRESHOLD;
+	fm_event = CG2900_EVENT_NO_EVENT;
+	no_of_scan_freq = 0;
+	cg2900_device.fm_mode = CG2900_FM_IDLE_MODE;
+	ret_val = 0;
+	goto done;
+
+switch_on_error:
+	cg2900_fm_deinit();
+init_error:
+	users--;
+done:
+	mutex_unlock(&fm_mutex);
+	FM_DEBUG_REPORT("cg2900_open: returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * cg2900_release()- This function switches off FM.
+ *
+ * This function switches off FM and releases the resources.
+ * This is called when the application closes the character
+ * device.
+ *
+ * @file: File structure.
+ *
+ * Returns:
+ *   0 when no error
+ *   -EINVAL: otherwise
+ */
+static int cg2900_release(
+			struct file *file
+			)
+{
+	int status;
+	int ret_val = -EINVAL;
+
+	mutex_lock(&fm_mutex);
+
+	FM_INFO_REPORT("cg2900_release");
+	if (users <= 0) {
+		FM_ERR_REPORT("cg2900_release: No users registered "
+			      "with FM Driver");
+		goto done;
+	}
+
+	users--;
+	FM_INFO_REPORT("cg2900_release: users = %d", users);
+
+	if (0 == users) {
+		FM_DEBUG_REPORT("cg2900_release: Switching Off FM");
+		status = cg2900_fm_switch_off();
+		status = cg2900_fm_deinit();
+		if (0 != status)
+			goto done;
+
+		cg2900_device.state = FMR_SWITCH_OFF;
+		cg2900_device.frequency = 0;
+		cg2900_device.rx_rds_enabled = false;
+		cg2900_device.muted = false;
+		cg2900_device.seekstatus = FMR_SEEK_NONE;
+		fm_event = CG2900_EVENT_NO_EVENT;
+		no_of_scan_freq = 0;
+	}
+	ret_val = 0;
+
+done:
+	mutex_unlock(&fm_mutex);
+	FM_DEBUG_REPORT("cg2900_release: returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * cg2900_read()- This function is invoked when the application
+ * calls read() to receive RDS Data.
+ *
+ * @file: File structure.
+ * @data: buffer provided by application for receving the data.
+ * @count: Number of bytes that application wants to read from driver
+ * @pos: offset
+ *
+ * Returns:
+ *   Number of bytes copied to the user buffer
+ *   -EFAULT: If there is problem in copying data to buffer supplied
+ *    by application
+ *   -EIO: If the number of bytes to be read are not a multiple of
+ *    struct  v4l2_rds_data.
+ *   -EAGAIN: More than 22 blocks requested to be read or read
+ *    was called in non blocking mode and no data was available for reading.
+ *   -EINTR: If read was interrupted by a signal before data was avaialble.
+ *   0 when no data available for reading.
+ */
+static ssize_t cg2900_read(
+			struct file *file,
+			char __user *data,
+			size_t count, loff_t *pos
+			)
+{
+	int current_rds_grp;
+	int index = 0;
+	int blocks_to_read;
+	struct v4l2_rds_data rdsbuf[MAX_RDS_GROUPS * NUM_OF_RDS_BLOCKS];
+	struct v4l2_rds_data *rdslocalbuf = rdsbuf;
+	struct sk_buff *skb;
+
+	FM_INFO_REPORT("cg2900_read");
+
+	blocks_to_read = (count / sizeof(struct v4l2_rds_data));
+
+	if (!cg2900_device.rx_rds_enabled) {
+		/* Remove all Interrupts from the queue */
+		skb_queue_purge(&fm_interrupt_queue);
+		FM_INFO_REPORT("cg2900_read: returning 0");
+		return 0;
+	}
+
+	if (count % sizeof(struct v4l2_rds_data) != 0) {
+		FM_ERR_REPORT("cg2900_read: Invalid Number of bytes %x "
+			      "requested to read", count);
+		return -EIO;
+	}
+
+	if (blocks_to_read > MAX_RDS_GROUPS * NUM_OF_RDS_BLOCKS) {
+		FM_ERR_REPORT("cg2900_read: Too many blocks(%d) "
+			      "requested to be read", blocks_to_read);
+		return -EAGAIN;
+	}
+
+	current_rds_grp = fm_rds_info.rds_group_sent;
+
+	if ((fm_rds_info.rds_head == fm_rds_info.rds_tail) ||
+	    (fm_rds_buf[fm_rds_info.rds_tail]
+	     [current_rds_grp].block1 == 0x0000)) {
+		/* Remove all Interrupts from the queue */
+		skb_queue_purge(&fm_interrupt_queue);
+		FM_INFO_REPORT("cg2900_read: returning 0");
+		return 0;
+	}
+
+	spin_lock(&fm_spinlock);
+	while (index < blocks_to_read) {
+		/* Check which Block needs to be transferred next */
+		switch (fm_rds_info.rds_block_sent % NUM_OF_RDS_BLOCKS) {
+		case 0:
+			(rdslocalbuf + index)->lsb =
+			fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block1;
+			(rdslocalbuf + index)->msb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block1 >> 8;
+			(rdslocalbuf + index)->block =
+			    (fm_rds_buf[fm_rds_info.rds_tail]
+			     [current_rds_grp].status1
+			     & RDS_BLOCK_MASK) >> 2;
+			cg2900_convert_err_to_v4l2(
+				fm_rds_buf[fm_rds_info.rds_tail]
+				[current_rds_grp].status1,
+				&(rdslocalbuf + index)->block);
+			break;
+		case 1:
+			(rdslocalbuf + index)->lsb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block2;
+			(rdslocalbuf + index)->msb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block2 >> 8;
+			(rdslocalbuf + index)->block =
+			    (fm_rds_buf[fm_rds_info.rds_tail]
+			     [current_rds_grp].status2
+			     & RDS_BLOCK_MASK) >> 2;
+			cg2900_convert_err_to_v4l2(
+				fm_rds_buf[fm_rds_info.rds_tail]
+				[current_rds_grp].status2,
+				&(rdslocalbuf + index)->block);
+			break;
+		case 2:
+			(rdslocalbuf + index)->lsb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block3;
+			(rdslocalbuf + index)->msb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block3 >> 8;
+			(rdslocalbuf + index)->block =
+			    (fm_rds_buf[fm_rds_info.rds_tail]
+			     [current_rds_grp].status3
+			     & RDS_BLOCK_MASK) >> 2;
+			cg2900_convert_err_to_v4l2(
+				fm_rds_buf[fm_rds_info.rds_tail]
+				[current_rds_grp].status3,
+				&(rdslocalbuf + index)->block);
+			break;
+		case 3:
+			(rdslocalbuf + index)->lsb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block4;
+			(rdslocalbuf + index)->msb =
+			    fm_rds_buf[fm_rds_info.rds_tail]
+			    [current_rds_grp].block4 >> 8;
+			(rdslocalbuf + index)->block =
+			    (fm_rds_buf[fm_rds_info.rds_tail]
+			     [current_rds_grp].status4
+			     & RDS_BLOCK_MASK) >> 2;
+			cg2900_convert_err_to_v4l2(
+				fm_rds_buf[fm_rds_info.rds_tail]
+				[current_rds_grp].status4,
+				&(rdslocalbuf + index)->block);
+			current_rds_grp++;
+			if (current_rds_grp == MAX_RDS_GROUPS) {
+				fm_rds_info.rds_tail++;
+				current_rds_grp = 0;
+				/* Dequeue Rds Interrupt here */
+				skb = skb_dequeue(&fm_interrupt_queue);
+				if (!skb) {
+					/* No Interrupt, bad case */
+					FM_ERR_REPORT("cg2900_read: "
+					"skb is NULL. Major error");
+					spin_unlock(&fm_spinlock);
+					return 0;
+				}
+				fm_event = (u8)skb->data[0];
+				if (fm_event != CG2900_EVENT_RDS_EVENT) {
+					/* RDS interrupt not found */
+					FM_ERR_REPORT("cg2900_read:"
+					"RDS interrupt not found"
+					"for de-queuing."
+					"fm_event = %x", fm_event);
+					/* Queue the event back */
+					skb_queue_head(&fm_interrupt_queue,
+						skb);
+					spin_unlock(&fm_spinlock);
+					return 0;
+				}
+				kfree_skb(skb);
+			}
+			break;
+		default:
+			FM_ERR_REPORT("Invalid RDS Group!!!");
+			spin_unlock(&fm_spinlock);
+			return 0;
+		}
+		index++;
+		fm_rds_info.rds_block_sent++;
+		if (fm_rds_info.rds_block_sent == NUM_OF_RDS_BLOCKS)
+			fm_rds_info.rds_block_sent = 0;
+
+		if (!cg2900_device.rx_rds_enabled) {
+			/* Remove all Interrupts from the queue */
+			skb_queue_purge(&fm_interrupt_queue);
+			FM_INFO_REPORT("cg2900_read: returning 0");
+			spin_unlock(&fm_spinlock);
+			return 0;
+		}
+	}
+	/* Update the RDS Group Count Sent to Application */
+	fm_rds_info.rds_group_sent = current_rds_grp;
+	if (fm_rds_info.rds_tail == MAX_RDS_BUFFER)
+		fm_rds_info.rds_tail = 0;
+
+	spin_unlock(&fm_spinlock);
+	if (copy_to_user(data, rdslocalbuf, count)) {
+		FM_ERR_REPORT("cg2900_read: Error "
+			      "in copying, returning");
+		return -EFAULT;
+	}
+	return count;
+}
+
+/**
+ * cg2900_poll()- Check if the operation is complete or not.
+ *
+ * This function is invoked by application on calling poll() and is used to
+ * wait till the any FM interrupt is received from the chip.
+ * The application decides to read the corresponding data depending on FM
+ * interrupt.
+ *
+ * @file: File structure.
+ * @wait: poll table
+ *
+ * Returns:
+ *   POLLRDNORM|POLLIN	whenever FM interrupt has occurred.
+ *   0			whenever the call times out.
+ */
+static unsigned int cg2900_poll(
+			struct file *file,
+			struct poll_table_struct *wait
+			)
+{
+	int ret_val = 0;
+
+	FM_INFO_REPORT("cg2900_poll");
+
+	/* Check if we have some data in queue already */
+	if (skb_queue_empty(&fm_interrupt_queue)) {
+		FM_DEBUG_REPORT("cg2900_poll: Interrupt Queue Empty, waiting");
+		/* No Interrupt, wait for it to occur */
+		poll_wait(file, &cg2900_poll_queue, wait);
+	}
+	/* Check if we now have interrupt to read in queue */
+	if (skb_queue_empty(&fm_interrupt_queue))
+		goto done;
+
+	ret_val = POLLIN | POLLRDNORM;
+
+done:
+	FM_DEBUG_REPORT("poll_wait returning %d", ret_val);
+	return ret_val;
+}
+
+/**
+ * radio_cg2900_probe()- This function registers FM Driver with V4L2 Driver.
+ *
+ * This function is called whenever the driver is probed by the device system,
+ * i.e. when a CG2900 controller has connected. It registers the FM Driver with
+ * Video4Linux as a character device.
+ *
+ * @pdev: Platform device.
+ *
+ * Returns:
+ *   0 on success
+ * -EINVAL on error
+ */
+static int __devinit radio_cg2900_probe(
+			struct platform_device *pdev
+			)
+{
+	int err;
+
+	FM_INFO_REPORT(BANNER);
+
+	err = fmd_set_dev(&pdev->dev);
+	if (err) {
+		FM_ERR_REPORT("Could not set device %s", pdev->name);
+		return err;
+	}
+
+	radio_nr = 0;
+	grid = CG2900_FM_GRID_100;
+	band = CG2900_FM_BAND_US_EU;
+	FM_INFO_REPORT("radio_cg2900_probe: radio_nr= %d.", radio_nr);
+
+	/* Initialize the parameters */
+	if (video_register_device(
+				&cg2900_video_device,
+				VFL_TYPE_RADIO,
+				radio_nr) == -1) {
+		FM_ERR_REPORT("radio_cg2900_probe: video_register_device err");
+		return -EINVAL;
+	}
+	mutex_init(&fm_mutex);
+	spin_lock_init(&fm_spinlock);
+	init_waitqueue_head(&cg2900_poll_queue);
+	skb_queue_head_init(&fm_interrupt_queue);
+	users = 0;
+	return 0;
+}
+
+/**
+ * radio_cg2900_remove()- This function removes the FM Driver.
+ *
+ * This function is called whenever the driver is removed by the device system,
+ * i.e. when a CG2900 controller has disconnected. It unregisters the FM Driver
+ * from Video4Linux.
+ *
+ * @pdev: Platform device.
+ *
+ * Returns: 0 on success
+ */
+static int __devexit radio_cg2900_remove(
+			struct platform_device *pdev
+			)
+{
+	FM_INFO_REPORT("radio_cg2900_remove");
+	/* Wake up the poll queue since we are now exiting */
+	wake_up_poll_queue();
+	/* Give some time for application to exit the poll thread */
+	schedule_timeout_interruptible(msecs_to_jiffies(500));
+
+	/* Try to Switch Off FM in case it is still switched on */
+	cg2900_fm_switch_off();
+	cg2900_fm_deinit();
+	skb_queue_purge(&fm_interrupt_queue);
+	mutex_destroy(&fm_mutex);
+	video_unregister_device(&cg2900_video_device);
+	fmd_set_dev(NULL);
+	return 0;
+}
+
+static struct platform_driver radio_cg2900_driver = {
+	.driver = {
+		.name	= "cg2900-fm",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= radio_cg2900_probe,
+	.remove	= __devexit_p(radio_cg2900_remove),
+};
+
+/**
+ * radio_cg2900_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init radio_cg2900_init(void)
+{
+	FM_INFO_REPORT("radio_cg2900_init");
+	return platform_driver_register(&radio_cg2900_driver);
+}
+
+/**
+ * radio_cg2900_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit radio_cg2900_exit(void)
+{
+	FM_INFO_REPORT("radio_cg2900_exit");
+	platform_driver_unregister(&radio_cg2900_driver);
+}
+
+void wake_up_poll_queue(void)
+{
+	FM_INFO_REPORT("wake_up_poll_queue");
+	wake_up_interruptible(&cg2900_poll_queue);
+}
+
+void cg2900_handle_device_reset(void)
+{
+	struct sk_buff *skb;
+	FM_INFO_REPORT("cg2900_handle_device_reset");
+	skb = alloc_skb(SKB_FM_INTERRUPT_DATA, GFP_KERNEL);
+	if (!skb) {
+		FM_ERR_REPORT("cg2900_handle_device_reset: "
+			"Unable to Allocate Memory");
+		return;
+	}
+	skb->data[0] = CG2900_EVENT_DEVICE_RESET;
+	skb->data[1] = true;
+	skb_queue_tail(&fm_interrupt_queue, skb);
+	wake_up_poll_queue();
+}
+
+module_init(radio_cg2900_init);
+module_exit(radio_cg2900_exit);
+MODULE_AUTHOR("Hemant Gupta");
+MODULE_LICENSE("GPL v2");
+
+module_param(radio_nr, int, S_IRUGO);
+
+module_param(grid, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(grid, "Grid:"
+			"0=50 kHz"
+			"*1=100 kHz*"
+			"2=200 kHz");
+
+module_param(band, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(band, "Band:"
+			"*0=87.5-108 MHz*"
+			"1=76-90 MHz"
+			"2=70-108 MHz");
+
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index 8db2d7f4b52..3fadf6aff35 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -161,6 +161,22 @@ config RADIO_WL1273
 	  To compile this driver as a module, choose M here: the
 	  module will be called radio-wl1273.
 
+config RADIO_CG2900
+	tristate "ST-Ericsson CG2900 FM Radio support"
+	depends on CG2900 && VIDEO_V4L2
+	---help---
+	  Choose Y here if you have one of these FM radio cards. This is a BT,
+	  FM and GPS combo chip controlled via HCI.
+
+	  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 radio-CG2900.
+
+
 # TI's ST based wl128x FM radio
 source "drivers/media/radio/wl128x/Kconfig"
 
diff --git a/drivers/media/radio/Makefile b/drivers/media/radio/Makefile
index ca8c7d134b9..8435dabcb3c 100644
--- a/drivers/media/radio/Makefile
+++ b/drivers/media/radio/Makefile
@@ -28,5 +28,6 @@ obj-$(CONFIG_RADIO_TEF6862) += tef6862.o
 obj-$(CONFIG_RADIO_TIMBERDALE) += radio-timb.o
 obj-$(CONFIG_RADIO_WL1273) += radio-wl1273.o
 obj-$(CONFIG_RADIO_WL128X) += wl128x/
+obj-$(CONFIG_RADIO_CG2900) += CG2900/
 
 ccflags-y += -Isound
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 11e44386fa9..825673af5f3 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -331,6 +331,17 @@ config MFD_TC3589X
 	  additional drivers must be enabled in order to use the
 	  functionality of the device.
 
+config MFD_TC35892
+	bool "Support Toshiba TC35892"
+	depends on I2C=y && GENERIC_HARDIRQS
+	select MFD_CORE
+	help
+	  Support for the Toshiba TC35892 I/O Expander.
+
+	  This driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the
+	  functionality of the device.
+
 config MFD_TMIO
 	bool
 	default n
@@ -359,6 +370,27 @@ config MFD_TC6393XB
 	help
 	  Support for Toshiba Mobile IO Controller TC6393XB
 
+config AB5500_CORE
+	bool "ST-Ericsson AB5500 Mixed Signal Circuit core functions"
+	select MFD_CORE
+	depends on GENERIC_HARDIRQS && ABX500_CORE
+	help
+	  Select this to enable the AB5500 Mixed Signal IC core
+	  functionality. This connects to a AB5500 chip on the I2C bus via
+	  the Power and Reset Management Unit (PRCMU). It exposes a number
+	  of symbols needed for dependent devices to read and write
+	  registers and subscribe to events from this multi-functional IC.
+	  This is needed to use other features of the AB5500 such as
+	  battery-backed RTC, charging control, Regulators, LEDs, vibrator,
+	  system power and temperature, power management and ALSA sound.
+
+config AB5500_GPADC
+	bool "AB5500 GPADC driver"
+	depends on AB5500_CORE
+	default y
+	help
+	  AB5500 GPADC driver used to convert battery/usb voltage.
+
 config PMIC_DA903X
 	bool "Dialog Semiconductor DA9030/DA9034 PMIC Support"
 	depends on I2C=y
@@ -678,7 +710,7 @@ config AB8500_CORE
 
 config AB8500_I2C_CORE
 	bool "AB8500 register access via PRCMU I2C"
-	depends on AB8500_CORE && MFD_DB8500_PRCMU
+	depends on AB8500_CORE
 	default y
 	help
 	  This enables register access to the AB8500 chip via PRCMU I2C.
@@ -686,6 +718,14 @@ config AB8500_I2C_CORE
 	  the I2C bus is connected to the Power Reset
 	  and Mangagement Unit, PRCMU.
 
+config AB8500_DENC
+	bool "AB8500_DENC driver support(CVBS)"
+	depends on AB8500_CORE
+	help
+	  Select this option to add driver support for analog TV out through
+	  AB8500.
+
+
 config AB8500_DEBUG
        bool "Enable debug info via debugfs"
        depends on AB8500_CORE && DEBUG_FS
@@ -696,10 +736,10 @@ config AB8500_DEBUG
 
 config AB8500_GPADC
 	bool "AB8500 GPADC driver"
-	depends on AB8500_CORE && REGULATOR_AB8500
+	depends on AB8500_CORE
 	default y
 	help
-	  AB8500 GPADC driver used to convert Acc and battery/ac/usb voltage
+	  AB8500 GPADC driver used to convert Acc and battery/ac/usb voltage.
 
 config MFD_DB8500_PRCMU
 	bool "ST-Ericsson DB8500 Power Reset Control Management Unit"
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 05fa538c5ef..cfed0402931 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -2,6 +2,7 @@
 # Makefile for multifunction miscellaneous devices
 #
 
+obj-$(CONFIG_AB5500_CORE)	+= ab5500-core.o ab5500-power.o
 88pm860x-objs			:= 88pm860x-core.o 88pm860x-i2c.o
 obj-$(CONFIG_MFD_88PM860X)	+= 88pm860x.o
 obj-$(CONFIG_MFD_SM501)		+= sm501.o
@@ -19,6 +20,7 @@ obj-$(CONFIG_MFD_STMPE)		+= stmpe.o
 obj-$(CONFIG_STMPE_I2C)		+= stmpe-i2c.o
 obj-$(CONFIG_STMPE_SPI)		+= stmpe-spi.o
 obj-$(CONFIG_MFD_TC3589X)	+= tc3589x.o
+obj-$(CONFIG_MFD_TC35892)	+= tc35892.o
 obj-$(CONFIG_MFD_T7L66XB)	+= t7l66xb.o tmio_core.o
 obj-$(CONFIG_MFD_TC6387XB)	+= tc6387xb.o tmio_core.o
 obj-$(CONFIG_MFD_TC6393XB)	+= tc6393xb.o tmio_core.o
@@ -91,11 +93,13 @@ obj-$(CONFIG_AB5500_CORE)	+= ab5500-core.o
 obj-$(CONFIG_AB5500_DEBUG)	+= ab5500-debugfs.o
 obj-$(CONFIG_AB8500_CORE)	+= ab8500-core.o ab8500-sysctrl.o
 obj-$(CONFIG_AB8500_DEBUG)	+= ab8500-debugfs.o
+obj-$(CONFIG_AB8500_DENC)	+= ab8500-denc.o
 obj-$(CONFIG_AB8500_GPADC)	+= ab8500-gpadc.o
 obj-$(CONFIG_MFD_DB8500_PRCMU)	+= db8500-prcmu.o
 # ab8500-i2c need to come after db8500-prcmu (which provides the channel)
 obj-$(CONFIG_AB8500_I2C_CORE)	+= ab8500-i2c.o
 obj-$(CONFIG_MFD_DB5500_PRCMU)	+= db5500-prcmu.o
+obj-$(CONFIG_AB5500_GPADC)	+= ab5500-gpadc.o
 obj-$(CONFIG_MFD_TIMBERDALE)    += timberdale.o
 obj-$(CONFIG_PMIC_ADP5520)	+= adp5520.o
 obj-$(CONFIG_LPC_SCH)		+= lpc_sch.o
diff --git a/drivers/mfd/ab5500-core.c b/drivers/mfd/ab5500-core.c
index 54d0fe40845..e8ae5d945e4 100644
--- a/drivers/mfd/ab5500-core.c
+++ b/drivers/mfd/ab5500-core.c
@@ -991,6 +991,74 @@ static struct mfd_cell ab5500_devs[AB5500_NUM_DEVICES] = {
 			},
 		},
 	},
+	[AB5500_DEVID_TEMPMON] = {
+		.name = "abx500-temp",
+		.id = AB5500_DEVID_TEMPMON,
+		.num_resources = 1,
+		.resources = (struct resource[]) {
+			{
+				.name   = "ABX500_TEMP_WARM",
+				.flags  = IORESOURCE_IRQ,
+				.start  = AB5500_IRQ(2, 2),
+				.end    = AB5500_IRQ(2, 2),
+			},
+		},
+	},
+	[AB5500_DEVID_ACCDET] = {
+		.name = "ab5500-acc-det",
+		.id = AB5500_DEVID_ACCDET,
+		.num_resources = 8,
+		.resources = (struct resource[]) {
+			{
+				.name	= "acc_detedt22db_rising",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(2, 7),
+				.end	= AB5500_IRQ(2, 7),
+			},
+				{
+				.name	= "acc_detedt21db_falling",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(2, 6),
+				.end	= AB5500_IRQ(2, 6),
+			},
+			{
+				.name	= "acc_detedt21db_rising",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(2, 5),
+				.end	= AB5500_IRQ(2, 5),
+			},
+			{
+				.name	= "acc_detedt3db_falling",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(3, 4),
+				.end	= AB5500_IRQ(3, 4),
+			},
+			{
+				.name	= "acc_detedt3db_rising",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(3, 3),
+				.end	= AB5500_IRQ(3, 3),
+			},
+			{
+				.name	= "acc_detedt1db_falling",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(3, 2),
+				.end	= AB5500_IRQ(3, 2),
+			},
+			{
+				.name	= "acc_detedt1db_rising",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(3, 1),
+				.end	= AB5500_IRQ(3, 1),
+			},
+			{
+				.name	= "acc_detedt22db_falling",
+				.flags	= IORESOURCE_IRQ,
+				.start	= AB5500_IRQ(3, 0),
+				.end	= AB5500_IRQ(3, 0),
+			},
+		},
+	},
 };
 
 /*
@@ -1301,6 +1369,10 @@ static const struct ab_family_id ids[] __initdata = {
 		.id = AB5500_1_1,
 		.name = "1.1"
 	},
+	{
+		.id = AB5500_2_0,
+		.name = "2.0"
+	},
 	/* Terminator */
 	{
 		.id = 0x00,
diff --git a/drivers/mfd/ab5500-gpadc.c b/drivers/mfd/ab5500-gpadc.c
new file mode 100644
index 00000000000..fe05ffbadfd
--- /dev/null
+++ b/drivers/mfd/ab5500-gpadc.c
@@ -0,0 +1,1256 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Vijaya Kumar K <vijay.kilari@stericsson.com>
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+
+/*
+ * Manual mode ADC registers
+ */
+#define AB5500_GPADC_MANUAL_STAT_REG		0x1F
+#define AB5500_GPADC_MANDATAL_REG		0x21
+#define AB5500_GPADC_MANDATAH_REG		0x20
+#define AB5500_GPADC_MANUAL_MUX_CTRL		0x22
+#define AB5500_GPADC_MANUAL_MODE_CTRL		0x23
+#define AB5500_GPADC_MANUAL_MODE_CTRL2		0x24
+/*
+ * Auto/Polling mode ADC registers
+ */
+#define AB5500_GPADC_AUTO_VBAT_MAX		0x26
+#define AB5500_GPADC_AUTO_VBAT_MIN_TXON		0x27
+#define AB5500_GPADC_AUTO_VBAT_MIN_NOTX		0x28
+#define AB5500_GPADC_AUTO_VBAT_AVGH		0x29
+#define AB5500_GPADC_AUTO_VBAT_AVGL		0x2A
+#define AB5500_GPADC_AUTO_ICHAR_MAX		0x2B
+#define AB5500_GPADC_AUTO_ICHAR_MIN		0x2C
+#define AB5500_GPADC_AUTO_ICHAR_AVG		0x2D
+#define AB5500_GPADC_AUTO_CTRL2			0x2F
+#define AB5500_GPADC_AUTO_CTRL1			0x30
+#define AB5500_GPADC_AUTO_PWR_CTRL		0x31
+#define AB5500_GPADC_AUTO_TRIG_VBAT_MIN_TXON	0x32
+#define AB5500_GPADC_AUTO_TRIG_VBAT_MIN_NOTX	0x33
+#define AB5500_GPADC_AUTO_TRIG_ADOUT0_CTRL	0x34
+#define AB5500_GPADC_AUTO_TRIG_ADOUT1_CTRL	0x35
+#define AB5500_GPADC_AUTO_TRIG0_MUX_CTRL	0x37
+#define AB5500_GPADC_AUTO_XTALTEMP_CTRL		0x57
+#define AB5500_GPADC_KELVIN_CTRL		0xFE
+
+/* gpadc constants */
+#define AB5500_INT_ADC_TRIG0		0x0
+#define AB5500_INT_ADC_TRIG1		0x1
+#define AB5500_INT_ADC_TRIG2		0x2
+#define AB5500_INT_ADC_TRIG3		0x3
+#define AB5500_INT_ADC_TRIG4		0x4
+#define AB5500_INT_ADC_TRIG5		0x5
+#define AB5500_INT_ADC_TRIG6		0x6
+#define AB5500_INT_ADC_TRIG7		0x7
+
+#define AB5500_GPADC_AUTO_TRIG_INDEX	AB5500_GPADC_AUTO_TRIG0_MUX_CTRL
+#define GPADC_MANUAL_READY		0x01
+#define GPADC_MANUAL_ADOUT0_MASK	0x30
+#define GPADC_MANUAL_ADOUT1_MASK	0xC0
+#define GPADC_MANUAL_ADOUT0_ON		0x10
+#define GPADC_MANUAL_ADOUT1_ON		0x40
+#define MUX_SCALE_GPADC0_MASK		0x08
+#define MUX_SCALE_VBAT_MASK		0x02
+#define MUX_SCALE_45			0x02
+#define MUX_SCALE_BDATA_MASK		0x01
+#define MUX_SCALE_BDATA27		0x00
+#define MUX_SCALE_BDATA18		0x01
+#define MUX_SCALE_ACCDET2_MASK		0x01
+#define MUX_SCALE_ACCDET3_MASK		0x02
+#define GPADC0_SCALE_VOL27		0x00
+#define GPADC0_SCALE_VOL18		0x01
+#define ACCDET2_SCALE_VOL27		0x00
+#define ACCDET3_SCALE_VOL27		0x00
+#define TRIGX_FREQ_MASK			0x07
+#define AUTO_VBAT_MASK			0x10
+#define AUTO_VBAT_ON			0x10
+#define TRIG_VBAT_TXON_ARM_MASK		0x08
+#define TRIG_VBAT_NOTX_ARM_MASK		0x04
+#define TRIGX_ARM_MASK			0x20
+#define TRIGX_ARM			0x20
+#define TRIGX_MUX_SELECT		0x1F
+#define ADC_CAL_OFF_MASK		0x04
+#define ADC_ON_MODE_MASK		0x03
+#define ADC_CAL_ON			0x00
+#define ADC_FULLPWR			0x03
+#define ADC_XTAL_FORCE_MASK		0x80
+#define ADC_XTAL_FORCE_EN		0x80
+#define ADC_XTAL_FORCE_DI		0x00
+#define ADOUT0				0x01
+#define ADOUT1				0x02
+#define MIN_INDEX			0x02
+#define MAX_INDEX			0x03
+#define CTRL_INDEX			0x01
+#define KELVIN_SCALE_VOL45		0x00
+
+/* GPADC constants from AB5500 spec  */
+#define GPADC0_MIN		0
+#define GPADC0_MAX		1800
+#define BTEMP_MIN		0
+#define BTEMP_MAX		1800
+#define BDATA_MIN		0
+#define BDATA_MAX		2750
+#define PCBTEMP_MIN		0
+#define PCBTEMP_MAX		1800
+#define XTALTEMP_MIN		0
+#define XTALTEMP_MAX		1800
+#define DIETEMP_MIN		0
+#define DIETEMP_MAX		1800
+#define VBUS_I_MIN		0
+#define VBUS_I_MAX		1600
+#define VBUS_V_MIN		0
+#define VBUS_V_MAX		20000
+#define ACCDET2_MIN		0
+#define ACCDET2_MAX		2500
+#define ACCDET3_MIN		0
+#define ACCDET3_MAX		2500
+#define VBAT_MIN		2300
+#define VBAT_MAX		4500
+#define BKBAT_MIN		0
+#define BKBAT_MAX		2750
+#define USBID_MIN		0
+#define USBID_MAX		1800
+#define KELVIN_MIN		0
+#define KELVIN_MAX		4500
+#define VIBRA_MIN		0
+#define VIBRA_MAX		4500
+
+/* This is used for calibration */
+#define ADC_RESOLUTION			1023
+#define AUTO_ADC_RESOLUTION		255
+
+/* ADOUT prestart time */
+#define ADOUT0_TRIGX_PRESTART		0x18
+
+enum adc_auto_channels {
+	ADC_INPUT_TRIG0 = 0,
+	ADC_INPUT_TRIG1,
+	ADC_INPUT_TRIG2,
+	ADC_INPUT_TRIG3,
+	ADC_INPUT_TRIG4,
+	ADC_INPUT_TRIG5,
+	ADC_INPUT_TRIG6,
+	ADC_INPUT_TRIG7,
+	ADC_INPUT_VBAT_TXOFF,
+	ADC_INPUT_VBAT_TXON,
+	N_AUTO_TRIGGER
+};
+
+/**
+ * struct adc_auto_trigger - AB5500 GPADC auto trigger
+ * @adc_mux			Mux input
+ * @flag			Status of trigger
+ * @freq			Frequency of conversion
+ * @adout			Adout to pull
+ * @trig_min			trigger minimum value
+ * @trig_max			trigger maximum value
+ * @auto_adc_callback		notification callback
+ */
+struct adc_auto_trigger {
+	u8 auto_mux;
+	u8 flag;
+	u8 freq;
+	u8 adout;
+	u8 trig_min;
+	u8 trig_max;
+	int (*auto_callb)(int mux);
+};
+
+/**
+ * struct ab5500_btemp_interrupts - ab5500 interrupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab5500_adc_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+/**
+ * struct ab5500_gpadc - AB5500 GPADC device information
+ * @chip_id			ABB chip id
+ * @dev:			pointer to the struct device
+ * @node:			a list of AB5500 GPADCs, hence prepared for
+				reentrance
+ * @ab5500_gpadc_complete:	pointer to the struct completion, to indicate
+ *				the completion of gpadc conversion
+ * @ab5500_gpadc_lock:		structure of type mutex
+ * @regu:			pointer to the struct regulator
+ * @irq:			interrupt number that is used by gpadc
+ * @cal_data			array of ADC calibration data structs
+ * @auto_trig			auto trigger channel
+ * @gpadc_trigX_work		work items for trigger channels
+ */
+struct ab5500_gpadc {
+	u8 chip_id;
+	struct device *dev;
+	struct list_head node;
+	struct mutex ab5500_gpadc_lock;
+	struct regulator *regu;
+	int irq;
+	int prev_bdata;
+	spinlock_t gpadc_auto_lock;
+	struct adc_auto_trigger adc_trig[N_AUTO_TRIGGER];
+	struct workqueue_struct *gpadc_wq;
+	struct work_struct gpadc_trig0_work;
+	struct work_struct gpadc_trig1_work;
+	struct work_struct gpadc_trig2_work;
+	struct work_struct gpadc_trig3_work;
+	struct work_struct gpadc_trig4_work;
+	struct work_struct gpadc_trig5_work;
+	struct work_struct gpadc_trig6_work;
+	struct work_struct gpadc_trig7_work;
+	struct work_struct gpadc_trig_vbat_txon_work;
+	struct work_struct gpadc_trig_vbat_txoff_work;
+};
+
+static LIST_HEAD(ab5500_gpadc_list);
+
+struct adc_data {
+	u8 mux;
+	int min;
+	int max;
+	int adout;
+};
+
+#define ADC_DATA(_id, _mux, _min, _max, _adout)	\
+	[_id] = {				\
+		.mux = _mux,		\
+		.min = _min,		\
+		.max = _max,		\
+		.adout = _adout		\
+	}
+
+struct adc_data adc_tab[] = {
+	ADC_DATA(GPADC0_V, 0x00, GPADC0_MIN, GPADC0_MAX, 0),
+	ADC_DATA(BTEMP_BALL, 0x0D, BTEMP_MIN, BTEMP_MAX, ADOUT0),
+	ADC_DATA(BAT_CTRL, 0x0D, BDATA_MIN, BDATA_MAX, 0),
+	ADC_DATA(MAIN_BAT_V, 0x0C, VBAT_MIN, VBAT_MAX, 0),
+	ADC_DATA(MAIN_BAT_V_TXON, 0x0C, VBAT_MIN, VBAT_MAX, 0),
+	ADC_DATA(VBUS_V, 0x10, VBUS_V_MIN, VBUS_V_MAX, 0),
+	ADC_DATA(USB_CHARGER_C,	0x0A, VBUS_I_MIN, VBUS_I_MAX, 0),
+	ADC_DATA(BK_BAT_V, 0x07, BKBAT_MIN, BKBAT_MAX, 0),
+	ADC_DATA(DIE_TEMP, 0x0F, DIETEMP_MIN, DIETEMP_MAX, ADOUT0),
+	ADC_DATA(PCB_TEMP, 0x13, PCBTEMP_MIN, PCBTEMP_MAX, ADOUT0),
+	ADC_DATA(XTAL_TEMP, 0x06, XTALTEMP_MIN, XTALTEMP_MAX, ADOUT0),
+	ADC_DATA(USB_ID, 0x1A, USBID_MIN, USBID_MAX, 0),
+	ADC_DATA(ACC_DETECT2, 0x18, ACCDET2_MIN, ACCDET2_MAX, 0),
+	ADC_DATA(ACC_DETECT3, 0x19, ACCDET3_MIN, ACCDET3_MAX, 0),
+	ADC_DATA(MAIN_BAT_V_TRIG_MIN, 0x0C, VBAT_MIN, VBAT_MAX, 0),
+	ADC_DATA(MAIN_BAT_V_TXON_TRIG_MIN, 0x0C, VBAT_MIN, VBAT_MAX, 0),
+	ADC_DATA(VIBRA_KELVIN, 0x16, VIBRA_MIN, VIBRA_MAX, 0),
+};
+
+/**
+ * ab5500_gpadc_get() - returns a reference to the primary AB5500 GPADC
+ * (i.e. the first GPADC in the instance list)
+ */
+struct ab5500_gpadc *ab5500_gpadc_get(const char *name)
+{
+	struct ab5500_gpadc *gpadc;
+	list_for_each_entry(gpadc, &ab5500_gpadc_list, node) {
+		if (!strcmp(name, dev_name(gpadc->dev)))
+			return gpadc;
+	}
+
+	return ERR_PTR(-ENOENT);
+}
+EXPORT_SYMBOL(ab5500_gpadc_get);
+
+#define CONV(min, max, x)\
+	((min) + ((((max)-(min))*(x))/ADC_RESOLUTION))
+
+static int ab5500_gpadc_ad_to_voltage(struct ab5500_gpadc *gpadc,
+					u8 in, u16 ad_val)
+{
+	int res;
+
+	switch (in) {
+	case VIBRA_KELVIN:
+	case GPADC0_V:
+	case PCB_TEMP:
+	case BTEMP_BALL:
+	case MAIN_BAT_V:
+	case MAIN_BAT_V_TXON:
+	case ACC_DETECT2:
+	case ACC_DETECT3:
+	case VBUS_V:
+	case USB_CHARGER_C:
+	case BK_BAT_V:
+	case XTAL_TEMP:
+	case USB_ID:
+	case BAT_CTRL:
+		res = CONV(adc_tab[in].min, adc_tab[in].max, ad_val);
+		break;
+	case DIE_TEMP:
+		/*
+		 * From the AB5500 product specification
+		 * T(deg cel) = 27 - ((ADCode - 709)/2.4213)
+		 * 27 + 709/2.4213 - ADCode/2.4213
+		 * 320 - (ADCode/2.4213)
+		 */
+		res = 320 - (((unsigned long)ad_val * 10000) / 24213);
+		break;
+	default:
+		dev_err(gpadc->dev,
+			"unknown channel, not possible to convert\n");
+		res = -EINVAL;
+		break;
+	}
+	return res;
+}
+
+/**
+ * ab5500_gpadc_convert() - gpadc conversion
+ * @input:	analog input to be converted to digital data
+ *
+ * This function converts the selected analog i/p to digital
+ * data.
+ */
+int ab5500_gpadc_convert(struct ab5500_gpadc *gpadc, u8 input)
+{
+	int result, ret = -EINVAL;
+	u16 data = 0;
+	u8 looplimit = 0;
+	u8 status = 0;
+	u8 low_data, high_data, adout_mask, adout_val;
+
+	if (!gpadc)
+		return -ENODEV;
+
+	mutex_lock(&gpadc->ab5500_gpadc_lock);
+
+	switch (input) {
+	case MAIN_BAT_V:
+	case MAIN_BAT_V_TXON:
+		/*
+		 * The value of mux scale volatage depends
+		 * on the type of battery
+		 * for LI-ion use MUX_SCALE_35 => 2.3-3.5V
+		 * for LiFePo4 use MUX_SCALE_45 => 2.3-4.5V
+		 * Check type of battery from platform data TODO ???
+		 */
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+			MUX_SCALE_VBAT_MASK, MUX_SCALE_45);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: failed to read status\n");
+			goto out;
+		}
+		break;
+	case BTEMP_BALL:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				MUX_SCALE_BDATA_MASK, MUX_SCALE_BDATA18);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set mux scale\n");
+			goto out;
+		}
+		break;
+	case BAT_CTRL:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				MUX_SCALE_BDATA_MASK, MUX_SCALE_BDATA27);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set mux scale\n");
+			goto out;
+		}
+		break;
+	case XTAL_TEMP:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_AUTO_XTALTEMP_CTRL,
+			ADC_XTAL_FORCE_MASK, ADC_XTAL_FORCE_EN);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set xtaltemp\n");
+			goto out;
+		}
+		break;
+	case GPADC0_V:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				MUX_SCALE_GPADC0_MASK, GPADC0_SCALE_VOL18);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set gpadc0\n");
+			goto out;
+		}
+		break;
+	case ACC_DETECT2:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL2,
+				MUX_SCALE_ACCDET2_MASK, ACCDET2_SCALE_VOL27);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set accdet2\n");
+			goto out;
+		}
+		break;
+	case ACC_DETECT3:
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL2,
+				MUX_SCALE_ACCDET3_MASK, ACCDET3_SCALE_VOL27);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set accdet3\n");
+			goto out;
+		}
+		break;
+	case VIBRA_KELVIN:
+		ret = abx500_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_KELVIN_CTRL,
+				KELVIN_SCALE_VOL45);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set kelv scale\n");
+			goto out;
+		}
+		break;
+	case USB_CHARGER_C:
+	case VBUS_V:
+	case BK_BAT_V:
+	case USB_ID:
+	case PCB_TEMP:
+	case DIE_TEMP:
+		break;
+	default:
+		dev_err(gpadc->dev, "gpadc: Wrong adc\n");
+		goto out;
+		break;
+	}
+	if (adc_tab[input].adout) {
+		adout_mask = adc_tab[input].adout == ADOUT0 ?
+			GPADC_MANUAL_ADOUT0_MASK : GPADC_MANUAL_ADOUT1_MASK;
+		adout_val = adc_tab[input].adout == ADOUT0 ?
+			GPADC_MANUAL_ADOUT0_ON : GPADC_MANUAL_ADOUT1_ON;
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				adout_mask, adout_val);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to set ADOUT\n");
+			goto out;
+		}
+		/* delay required to ramp up voltage on BDATA node */
+		usleep_range(10000, 12000);
+	}
+	ret = abx500_set_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_MANUAL_MUX_CTRL, adc_tab[input].mux);
+	if (ret < 0) {
+		dev_err(gpadc->dev,
+			"gpadc: fail to trigger manual conv\n");
+		goto out;
+	}
+	/* wait for completion of conversion */
+	looplimit = 0;
+	do {
+		msleep(1);
+		ret = abx500_get_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_MANUAL_STAT_REG,
+			&status);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: failed to read status\n");
+			goto out;
+		}
+		if (status & GPADC_MANUAL_READY)
+			break;
+	} while (++looplimit < 2);
+	if (looplimit >= 2) {
+		dev_err(gpadc->dev, "timeout:failed to complete conversion\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/*
+	 * Disable ADOUT for measurement
+	 */
+	if (adc_tab[input].adout) {
+		adout_mask = adc_tab[input].adout == ADOUT0 ?
+			GPADC_MANUAL_ADOUT0_MASK : GPADC_MANUAL_ADOUT1_MASK;
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				adout_mask, 0x0);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to disable ADOUT\n");
+			goto out;
+		}
+	}
+	/*
+	 * Disable XTAL TEMP
+	 */
+	if (input == XTAL_TEMP) {
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_AUTO_XTALTEMP_CTRL,
+			ADC_XTAL_FORCE_MASK, ADC_XTAL_FORCE_DI);
+		if (ret < 0) {
+			dev_err(gpadc->dev,
+				"gpadc: fail to disable xtaltemp\n");
+			goto out;
+		}
+	}
+	/* Read the converted RAW data */
+	ret = abx500_get_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_MANDATAL_REG, &low_data);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: read low data failed\n");
+		goto out;
+	}
+
+	ret = abx500_get_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_MANDATAH_REG, &high_data);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: read high data failed\n");
+		goto out;
+	}
+
+	data = (high_data << 2) | (low_data >> 6);
+	if (input == BAT_CTRL || input == BTEMP_BALL) {
+		/*
+		 * TODO: Re-check with h/w team
+		 * discard null or value < 5, as there is some error
+		 * in conversion
+		 */
+		if (data < 5)
+			data = gpadc->prev_bdata;
+		else
+			gpadc->prev_bdata = data;
+	}
+	result = ab5500_gpadc_ad_to_voltage(gpadc, input, data);
+
+	mutex_unlock(&gpadc->ab5500_gpadc_lock);
+	return result;
+
+out:
+	mutex_unlock(&gpadc->ab5500_gpadc_lock);
+	dev_err(gpadc->dev,
+		"gpadc: Failed to AD convert channel %d\n", input);
+	return ret;
+}
+EXPORT_SYMBOL(ab5500_gpadc_convert);
+
+/**
+ * ab5500_gpadc_program_auto() - gpadc conversion auto conversion
+ * @trig_index:	Generic trigger channel for conversion
+ *
+ * This function program the auto trigger channel
+ */
+static int ab5500_gpadc_program_auto(struct ab5500_gpadc *gpadc, int trig)
+{
+	int ret;
+	u8 adout;
+#define MIN_INDEX	0x02
+#define MAX_INDEX	0x03
+#define CTRL_INDEX	0x01
+
+	ret = abx500_set_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2) + MIN_INDEX,
+			gpadc->adc_trig[trig].trig_min);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to program min\n");
+		return ret;
+	}
+	ret = abx500_set_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2) + MAX_INDEX,
+			gpadc->adc_trig[trig].trig_max);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to program max\n");
+		return ret;
+	}
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+		AB5500_BANK_ADC, AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2),
+		TRIGX_MUX_SELECT, gpadc->adc_trig[trig].auto_mux);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to select mux\n");
+		return ret;
+	}
+	if (gpadc->adc_trig[trig].adout) {
+		adout = gpadc->adc_trig[trig].adout == ADOUT0 ?
+				gpadc->adc_trig[trig].adout << 6 :
+				gpadc->adc_trig[trig].adout << 5;
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2) + CTRL_INDEX,
+			adout, adout);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to program adout\n");
+			return ret;
+		}
+	}
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_TRIG_INDEX + (trig << 2) + CTRL_INDEX,
+			TRIGX_FREQ_MASK, gpadc->adc_trig[trig].freq);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to program freq\n");
+		return ret;
+	}
+	return ret;
+
+}
+
+#define TRIG_V(trigval, min, max) \
+	((((trigval) - (min)) * AUTO_ADC_RESOLUTION) / ((max) - (min)))
+
+static int ab5500_gpadc_vbat_auto_conf(struct ab5500_gpadc *gpadc,
+					struct adc_auto_input *in)
+{
+	int trig_min, ret;
+	u8 trig_reg, trig_arm;
+
+	/* Scale mux voltage */
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC,
+			AB5500_GPADC_MANUAL_MODE_CTRL,
+			MUX_SCALE_VBAT_MASK, MUX_SCALE_45);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: failed to set vbat scale\n");
+		return ret;
+	}
+
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC,
+			AB5500_GPADC_AUTO_CTRL1,
+			AUTO_VBAT_MASK, AUTO_VBAT_ON);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: failed to set vbat on\n");
+		return ret;
+	}
+
+	trig_min = TRIG_V(in->min, adc_tab[in->mux].min, adc_tab[in->mux].max);
+
+	if (in->mux == MAIN_BAT_V_TRIG_MIN) {
+		trig_reg = AB5500_GPADC_AUTO_TRIG_VBAT_MIN_NOTX;
+		trig_arm = TRIG_VBAT_NOTX_ARM_MASK;
+	} else {
+		trig_reg = AB5500_GPADC_AUTO_TRIG_VBAT_MIN_TXON;
+		trig_arm = TRIG_VBAT_TXON_ARM_MASK;
+	}
+	ret = abx500_set_register_interruptible(gpadc->dev, AB5500_BANK_ADC,
+			trig_reg, trig_min);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: fail to program vbat min\n");
+		return ret;
+	}
+	/*
+	 * arm the trigger
+	 */
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+		AB5500_BANK_ADC, AB5500_GPADC_AUTO_CTRL1, trig_arm, trig_arm);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: failed to trig vbat\n");
+		return ret;
+	}
+	return ret;
+}
+/**
+ * ab5500_gpadc_convert_auto() - gpadc conversion
+ * @auto_input:	input trigger for conversion
+ *
+ * This function converts the selected channel from
+ * analog to digital data in auto mode
+ */
+
+int ab5500_gpadc_convert_auto(struct ab5500_gpadc *gpadc,
+				struct adc_auto_input *in)
+{
+	int ret, trig;
+	unsigned long flags;
+
+	if (!gpadc)
+		return -ENODEV;
+	mutex_lock(&gpadc->ab5500_gpadc_lock);
+
+	if (in->mux == MAIN_BAT_V_TXON_TRIG_MIN) {
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		if (gpadc->adc_trig[ADC_INPUT_VBAT_TXON].flag == true) {
+			spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+			ret = -EBUSY;
+			dev_err(gpadc->dev, "gpadc: Auto vbat txon busy");
+			goto out;
+		}
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+
+		ret = ab5500_gpadc_vbat_auto_conf(gpadc, in);
+		if (ret < 0)
+			goto out;
+
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXON].auto_mux = in->mux;
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXON].auto_callb =
+				in->auto_adc_callback;
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXON].flag = true;
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+	} else if (in->mux == MAIN_BAT_V_TRIG_MIN) {
+
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		if (gpadc->adc_trig[ADC_INPUT_VBAT_TXOFF].flag == true) {
+			spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+			ret = -EBUSY;
+			dev_err(gpadc->dev, "gpadc: Auto vbat busy");
+			goto out;
+		}
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+
+		ret = ab5500_gpadc_vbat_auto_conf(gpadc, in);
+		if (ret < 0)
+			goto out;
+
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXOFF].auto_mux = in->mux;
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXOFF].auto_callb =
+				in->auto_adc_callback;
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		gpadc->adc_trig[ADC_INPUT_VBAT_TXOFF].flag = true;
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+	} else {
+		/*
+		 * check if free trigger is available
+		 */
+		trig = ADC_INPUT_TRIG0;
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		while (gpadc->adc_trig[trig].flag == true &&
+					trig <= ADC_INPUT_TRIG7)
+			trig++;
+
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+		if (trig > ADC_INPUT_TRIG7) {
+			ret = -EBUSY;
+			dev_err(gpadc->dev, "gpadc: no free channel\n");
+			goto out;
+		}
+		switch (in->mux) {
+		case MAIN_BAT_V:
+			/*
+			 * The value of mux scale volatage depends
+			 * on the type of battery
+			 * for LI-ion use MUX_SCALE_35 => 2.3-3.5V
+			 * for LiFePo4 use MUX_SCALE_45 => 2.3-4.5V
+			 * Check type of battery from platform data TODO ???
+			 */
+			ret = abx500_mask_and_set_register_interruptible(
+				gpadc->dev,
+				AB5500_BANK_ADC, AB5500_GPADC_MANUAL_MODE_CTRL,
+				MUX_SCALE_VBAT_MASK, MUX_SCALE_45);
+			if (ret < 0) {
+				dev_err(gpadc->dev,
+					"gpadc: failed to read status\n");
+				goto out;
+			}
+
+		case BTEMP_BALL:
+			ret = abx500_set_register_interruptible(
+				gpadc->dev, AB5500_BANK_ADC,
+				AB5500_GPADC_AUTO_TRIG_ADOUT0_CTRL,
+				ADOUT0_TRIGX_PRESTART);
+			if (ret < 0) {
+				dev_err(gpadc->dev,
+					"gpadc: failed to set prestart\n");
+				goto out;
+			}
+
+		case ACC_DETECT2:
+		case ACC_DETECT3:
+		case VBUS_V:
+		case USB_CHARGER_C:
+		case BK_BAT_V:
+		case PCB_TEMP:
+		case USB_ID:
+		case BAT_CTRL:
+			gpadc->adc_trig[trig].trig_min =
+			(u8)TRIG_V(in->min, adc_tab[in->mux].min,
+					adc_tab[in->mux].max);
+			gpadc->adc_trig[trig].trig_max =
+			(u8)TRIG_V(in->max, adc_tab[in->mux].min,
+					adc_tab[in->mux].max);
+			gpadc->adc_trig[trig].adout =
+				adc_tab[in->mux].adout;
+			break;
+		case DIE_TEMP:
+			/*
+			 * From the AB5500 product specification
+			 * T(deg_cel) = 27 - (ADCode - 709)/2.4213)
+			 * ADCode = 709 + (2.4213 * (27 - T))
+			 * Auto trigger min/max level is of 8bit precision.
+			 * Hence use AB5500_GPADC_MANDATAH_REG value
+			 * obtained by 2 bit right shift of ADCode.
+			 */
+			gpadc->adc_trig[trig].trig_min =
+				(709 + ((24213 * (27 - in->min))/10000))>>2;
+			gpadc->adc_trig[trig].trig_max =
+				(709 + ((24213 * (27 - in->max))/10000))>>2;
+			gpadc->adc_trig[trig].adout =
+				adc_tab[in->mux].adout;
+			break;
+		default:
+			dev_err(gpadc->dev, "Unknow GPADC request\n");
+			break;
+		}
+		gpadc->adc_trig[trig].freq = in->freq;
+		gpadc->adc_trig[trig].auto_mux =
+			adc_tab[in->mux].mux;
+		gpadc->adc_trig[trig].auto_callb = in->auto_adc_callback;
+
+		ret = ab5500_gpadc_program_auto(gpadc, trig);
+		if (ret < 0) {
+			dev_err(gpadc->dev,
+					"gpadc: fail to program auto ch\n");
+			goto out;
+		}
+		ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+				AB5500_BANK_ADC,
+				AB5500_GPADC_AUTO_TRIG_INDEX + (trig * 4),
+				TRIGX_ARM_MASK, TRIGX_ARM);
+		if (ret < 0) {
+			dev_err(gpadc->dev, "gpadc: fail to trigger\n");
+			goto out;
+		}
+		spin_lock_irqsave(&gpadc->gpadc_auto_lock, flags);
+		gpadc->adc_trig[trig].flag = true;
+		spin_unlock_irqrestore(&gpadc->gpadc_auto_lock, flags);
+	}
+out:
+	mutex_unlock(&gpadc->ab5500_gpadc_lock);
+	return ret;
+
+}
+EXPORT_SYMBOL(ab5500_gpadc_convert_auto);
+
+/* sysfs interface for GPADC0 */
+static ssize_t ab5500_gpadc0_get(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	int voltage;
+	struct ab5500_gpadc *gpadc = dev_get_drvdata(dev);
+
+	voltage = ab5500_gpadc_convert(gpadc, GPADC0_V);
+
+	return sprintf(buf, "%d\n", voltage);
+}
+static DEVICE_ATTR(adc0volt, 0644, ab5500_gpadc0_get, NULL);
+
+static void ab5500_gpadc_trigx_work(struct ab5500_gpadc *gp, int trig)
+{
+	unsigned long flags;
+	if (gp->adc_trig[trig].auto_callb != NULL) {
+		gp->adc_trig[trig].auto_callb(gp->adc_trig[trig].auto_mux);
+		spin_lock_irqsave(&gp->gpadc_auto_lock, flags);
+		gp->adc_trig[trig].flag = false;
+		spin_unlock_irqrestore(&gp->gpadc_auto_lock, flags);
+	} else {
+		dev_err(gp->dev, "Unknown trig for %d\n", trig);
+	}
+}
+/**
+ * ab5500_gpadc_trig0_work() - work item for trig0 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 0 auto conversion.
+ */
+static void ab5500_gpadc_trig0_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig0_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG0);
+}
+
+/**
+ * ab5500_gpadc_trig1_work() - work item for trig1 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig1 auto conversion.
+ */
+static void ab5500_gpadc_trig1_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig1_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG1);
+}
+
+/**
+ * ab5500_gpadc_trig2_work() - work item for trig2 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 2 auto conversion.
+ */
+static void ab5500_gpadc_trig2_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig2_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG2);
+}
+
+/**
+ * ab5500_gpadc_trig3_work() - work item for trig3 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 3 auto conversion.
+ */
+static void ab5500_gpadc_trig3_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig3_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG3);
+}
+
+/**
+ * ab5500_gpadc_trig4_work() - work item for trig4 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 4 auto conversion.
+ */
+static void ab5500_gpadc_trig4_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig4_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG4);
+}
+
+/**
+ * ab5500_gpadc_trig5_work() - work item for trig5 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 5 auto conversion.
+ */
+static void ab5500_gpadc_trig5_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig5_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG5);
+}
+
+/**
+ * ab5500_gpadc_trig6_work() - work item for trig6 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 6 auto conversion.
+ */
+static void ab5500_gpadc_trig6_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig6_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG6);
+}
+
+/**
+ * ab5500_gpadc_trig7_work() - work item for trig7 auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for trig 7 auto conversion.
+ */
+static void ab5500_gpadc_trig7_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig7_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_TRIG7);
+}
+
+/**
+ * ab5500_gpadc_vbat_txon_work() - work item for vbat_txon trigger auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for vbat_txon trigger auto adc.
+ */
+static void ab5500_gpadc_vbat_txon_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig_vbat_txon_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_VBAT_TXON);
+}
+
+/**
+ * ab5500_gpadc_vbat_txoff_work() - work item for vbat_txoff trigger auto adc
+ * @irq:	irq number
+ * @work:	work pointer
+ *
+ * This is a work handler for vbat_txoff trigger auto adc.
+ */
+static void ab5500_gpadc_vbat_txoff_work(struct work_struct *work)
+{
+	struct ab5500_gpadc *gpadc = container_of(work,
+		struct ab5500_gpadc, gpadc_trig_vbat_txoff_work);
+	ab5500_gpadc_trigx_work(gpadc, ADC_INPUT_VBAT_TXOFF);
+}
+
+/**
+ * ab5500_adc_trigx_handler() - isr for auto gpadc conversion trigger
+ * @irq:	irq number
+ * @data:	pointer to the data passed during request irq
+ *
+ * This is a interrupt service routine for auto gpadc conversion.
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_adc_trigx_handler(int irq, void *_gpadc)
+{
+	struct ab5500_platform_data *plat;
+	struct ab5500_gpadc *gpadc = _gpadc;
+	int dev_irq;
+
+	plat = dev_get_platdata(gpadc->dev->parent);
+	dev_irq = irq - plat->irq.base;
+
+	switch (dev_irq) {
+	case AB5500_INT_ADC_TRIG0:
+		dev_dbg(gpadc->dev, "Trigger 0 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig0_work);
+		break;
+	case AB5500_INT_ADC_TRIG1:
+		dev_dbg(gpadc->dev, "Trigger 1 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig1_work);
+		break;
+	case AB5500_INT_ADC_TRIG2:
+		dev_dbg(gpadc->dev, "Trigger 2 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig2_work);
+		break;
+	case AB5500_INT_ADC_TRIG3:
+		dev_dbg(gpadc->dev, "Trigger 3 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig3_work);
+		break;
+	case AB5500_INT_ADC_TRIG4:
+		dev_dbg(gpadc->dev, "Trigger 4 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig4_work);
+		break;
+	case AB5500_INT_ADC_TRIG5:
+		dev_dbg(gpadc->dev, "Trigger 5 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig5_work);
+		break;
+	case AB5500_INT_ADC_TRIG6:
+		dev_dbg(gpadc->dev, "Trigger 6 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig6_work);
+		break;
+	case AB5500_INT_ADC_TRIG7:
+		dev_dbg(gpadc->dev, "Trigger 7 received\n");
+		queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig7_work);
+		break;
+	default:
+		dev_dbg(gpadc->dev, "unknown trigx handler input\n");
+		break;
+	}
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_adc_vbat_txon_handler() - isr for auto vbat_txon conversion trigger
+ * @irq:	irq number
+ * @data:	pointer to the data passed during request irq
+ *
+ * This is a interrupt service routine for auto vbat_txon conversion
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_adc_vbat_txon_handler(int irq, void *_gpadc)
+{
+	struct ab5500_gpadc *gpadc = _gpadc;
+
+	queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig_vbat_txon_work);
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_adc_vbat_txoff_handler() - isr for auto vbat_txoff conversion trigger
+ * @irq:	irq number
+ * @data:	pointer to the data passed during request irq
+ *
+ * This is a interrupt service routine for auto vbat_txoff conversion
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_adc_vbat_txoff_handler(int irq, void *_gpadc)
+{
+	struct ab5500_gpadc *gpadc = _gpadc;
+
+	queue_work(gpadc->gpadc_wq, &gpadc->gpadc_trig_vbat_txoff_work);
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_gpadc_configuration() - function for gpadc conversion
+ * @irq:	irq number
+ * @data:	pointer to the data passed during request irq
+ *
+ * This function configures the gpadc
+ */
+static int ab5500_gpadc_configuration(struct ab5500_gpadc *gpadc)
+{
+	int ret;
+	ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+			AB5500_BANK_ADC, AB5500_GPADC_AUTO_CTRL2,
+			ADC_CAL_OFF_MASK | ADC_ON_MODE_MASK,
+			ADC_CAL_ON | ADC_FULLPWR);
+	return ret;
+}
+
+/* ab5500 btemp driver interrupts and their respective isr */
+static struct ab5500_adc_interrupts ab5500_adc_irq[] = {
+	{"TRIGGER-0", ab5500_adc_trigx_handler},
+	{"TRIGGER-1", ab5500_adc_trigx_handler},
+	{"TRIGGER-2", ab5500_adc_trigx_handler},
+	{"TRIGGER-3", ab5500_adc_trigx_handler},
+	{"TRIGGER-4", ab5500_adc_trigx_handler},
+	{"TRIGGER-5", ab5500_adc_trigx_handler},
+	{"TRIGGER-6", ab5500_adc_trigx_handler},
+	{"TRIGGER-7", ab5500_adc_trigx_handler},
+	{"TRIGGER-VBAT-TXON", ab5500_adc_vbat_txon_handler},
+	{"TRIGGER-VBAT", ab5500_adc_vbat_txoff_handler},
+};
+
+static int __devinit ab5500_gpadc_probe(struct platform_device *pdev)
+{
+	int ret, irq, i, j;
+	struct ab5500_gpadc *gpadc;
+
+	gpadc = kzalloc(sizeof(struct ab5500_gpadc), GFP_KERNEL);
+	if (!gpadc) {
+		dev_err(&pdev->dev, "Error: No memory\n");
+		return -ENOMEM;
+	}
+	gpadc->dev = &pdev->dev;
+	mutex_init(&gpadc->ab5500_gpadc_lock);
+	spin_lock_init(&gpadc->gpadc_auto_lock);
+
+	/* Register interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_adc_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_adc_irq[i].name);
+		ret = request_threaded_irq(irq, NULL, ab5500_adc_irq[i].isr,
+			IRQF_NO_SUSPEND,
+			ab5500_adc_irq[i].name, gpadc);
+
+		if (ret) {
+			dev_err(gpadc->dev, "failed to request %s IRQ %d: %d\n"
+				, ab5500_adc_irq[i].name, irq, ret);
+			goto fail_irq;
+		}
+		dev_dbg(gpadc->dev, "Requested %s IRQ %d: %d\n",
+			ab5500_adc_irq[i].name, irq, ret);
+	}
+
+	/* Get Chip ID of the ABB ASIC  */
+	ret = abx500_get_chip_id(gpadc->dev);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "failed to get chip ID\n");
+		goto fail_irq;
+	}
+	gpadc->chip_id = (u8) ret;
+
+	/* Create a work queue for gpadc auto */
+	gpadc->gpadc_wq =
+		create_singlethread_workqueue("ab5500_gpadc_wq");
+	if (gpadc->gpadc_wq == NULL) {
+		dev_err(gpadc->dev, "failed to create work queue\n");
+		goto fail_irq;
+	}
+
+	INIT_WORK(&gpadc->gpadc_trig0_work, ab5500_gpadc_trig0_work);
+	INIT_WORK(&gpadc->gpadc_trig1_work, ab5500_gpadc_trig1_work);
+	INIT_WORK(&gpadc->gpadc_trig2_work, ab5500_gpadc_trig2_work);
+	INIT_WORK(&gpadc->gpadc_trig3_work, ab5500_gpadc_trig3_work);
+	INIT_WORK(&gpadc->gpadc_trig4_work, ab5500_gpadc_trig4_work);
+	INIT_WORK(&gpadc->gpadc_trig5_work, ab5500_gpadc_trig5_work);
+	INIT_WORK(&gpadc->gpadc_trig6_work, ab5500_gpadc_trig6_work);
+	INIT_WORK(&gpadc->gpadc_trig7_work, ab5500_gpadc_trig7_work);
+	INIT_WORK(&gpadc->gpadc_trig_vbat_txon_work,
+				ab5500_gpadc_vbat_txon_work);
+	INIT_WORK(&gpadc->gpadc_trig_vbat_txoff_work,
+				ab5500_gpadc_vbat_txoff_work);
+
+	for (j = 0; j < N_AUTO_TRIGGER; j++)
+		gpadc->adc_trig[j].flag = false;
+
+	ret = ab5500_gpadc_configuration(gpadc);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "gpadc: configuration failed\n");
+		goto free_wq;
+	}
+
+	ret = device_create_file(gpadc->dev, &dev_attr_adc0volt);
+	if (ret < 0) {
+		dev_err(gpadc->dev, "File device creation failed: %d\n", ret);
+		ret = -ENODEV;
+		goto fail_sysfs;
+	}
+	list_add_tail(&gpadc->node, &ab5500_gpadc_list);
+
+	platform_set_drvdata(pdev, gpadc);
+
+	return 0;
+fail_sysfs:
+free_wq:
+	destroy_workqueue(gpadc->gpadc_wq);
+fail_irq:
+	for (i = i - 1; i >= 0; i--) {
+		irq = platform_get_irq_byname(pdev, ab5500_adc_irq[i].name);
+		free_irq(irq, gpadc);
+	}
+	kfree(gpadc);
+	gpadc = NULL;
+	return ret;
+}
+
+static int __devexit ab5500_gpadc_remove(struct platform_device *pdev)
+{
+	int i, irq;
+	struct ab5500_gpadc *gpadc = platform_get_drvdata(pdev);
+
+	device_remove_file(gpadc->dev, &dev_attr_adc0volt);
+
+	/* remove this gpadc entry from the list */
+	list_del(&gpadc->node);
+	/* Disable interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_adc_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_adc_irq[i].name);
+		free_irq(irq, gpadc);
+	}
+	/* Flush work */
+	flush_workqueue(gpadc->gpadc_wq);
+
+	/* Delete the work queue */
+	destroy_workqueue(gpadc->gpadc_wq);
+
+	kfree(gpadc);
+	gpadc = NULL;
+	return 0;
+}
+
+static struct platform_driver ab5500_gpadc_driver = {
+	.probe = ab5500_gpadc_probe,
+	.remove = __devexit_p(ab5500_gpadc_remove),
+	.driver = {
+		.name = "ab5500-adc",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_gpadc_init(void)
+{
+	return platform_driver_register(&ab5500_gpadc_driver);
+}
+
+static void __exit ab5500_gpadc_exit(void)
+{
+	platform_driver_unregister(&ab5500_gpadc_driver);
+}
+
+subsys_initcall_sync(ab5500_gpadc_init);
+module_exit(ab5500_gpadc_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Vijaya Kumar K");
+MODULE_ALIAS("platform:ab5500_adc");
+MODULE_DESCRIPTION("AB5500 GPADC driver");
diff --git a/drivers/mfd/ab5500-power.c b/drivers/mfd/ab5500-power.c
new file mode 100644
index 00000000000..9474c32809b
--- /dev/null
+++ b/drivers/mfd/ab5500-power.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/signal.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+
+static struct device *dev;
+
+/* STARTUP */
+#define AB5500_SYSPOR_CONTROL	0x30
+
+/* VINT IO I2C CLOCK */
+#define AB5500_RTC_VINT		0x01
+
+int ab5500_clock_rtc_enable(int num, bool enable)
+{
+	/* RTC_CLK{0,1,2} are bits {4,3,2}, active low */
+	u8 mask = BIT(4 - num);
+	u8 value = enable ? 0 : mask;
+
+	/* Don't allow RTC_CLK0 to be controlled. */
+	if (num < 1 || num > 2)
+		return -EINVAL;
+
+	if (!dev)
+		return -EAGAIN;
+
+	return abx500_mask_and_set(dev, AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP,
+				   AB5500_RTC_VINT, mask, value);
+}
+
+static void ab5500_power_off(void)
+{
+	sigset_t old;
+	sigset_t all;
+
+	sigfillset(&all);
+
+	if (!sigprocmask(SIG_BLOCK, &all, &old)) {
+		/* Clear dbb_on */
+		int ret = abx500_set(dev, AB5500_BANK_STARTUP,
+				     AB5500_SYSPOR_CONTROL, 0);
+		WARN_ON(ret);
+	}
+}
+
+static int __devinit ab5500_power_probe(struct platform_device *pdev)
+{
+	struct ab5500_platform_data *plat = dev_get_platdata(pdev->dev.parent);
+
+	dev = &pdev->dev;
+
+	if (plat->pm_power_off)
+		pm_power_off = ab5500_power_off;
+
+	return 0;
+}
+
+static int __devexit ab5500_power_remove(struct platform_device *pdev)
+{
+	struct ab5500_platform_data *plat = dev_get_platdata(pdev->dev.parent);
+
+	if (plat->pm_power_off)
+		pm_power_off = NULL;
+	dev = NULL;
+
+	return 0;
+}
+
+static struct platform_driver ab5500_power_driver = {
+	.driver = {
+		.name = "ab5500-power",
+		.owner = THIS_MODULE,
+	},
+	.probe = ab5500_power_probe,
+	.remove = __devexit_p(ab5500_power_remove),
+};
+
+static int __init ab8500_sysctrl_init(void)
+{
+	return platform_driver_register(&ab5500_power_driver);
+}
+
+subsys_initcall(ab8500_sysctrl_init);
+
+MODULE_DESCRIPTION("AB5500 power driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index 1f08704f7ae..8137ac22816 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -100,6 +100,9 @@
 #define AB9540_MODEM_CTRL2_REG			0x23
 #define AB9540_MODEM_CTRL2_SWDBBRSTN_BIT	BIT(2)
 
+static bool no_bm; /* No battery management */
+module_param(no_bm, bool, S_IRUGO);
+
 /*
  * Map interrupt numbers to the LATCH and MASK register offsets, Interrupt
  * numbers are indexed into this array with (num / 8). The interupts are
@@ -257,6 +260,7 @@ static struct abx500_ops ab8500_ops = {
 	.mask_and_set_register = ab8500_mask_and_set_register,
 	.event_registers_startup_state_get = NULL,
 	.startup_irq_enabled = NULL,
+	.dump_all_banks = ab8500_dump_all_banks,
 };
 
 static void ab8500_irq_lock(struct irq_data *data)
@@ -354,6 +358,7 @@ static irqreturn_t ab8500_irq(int irq, void *dev)
 			int line = i * 8 + bit;
 
 			handle_nested_irq(ab8500->irq_base + line);
+			ab8500_debug_register_interrupt(line);
 			value &= ~(1 << bit);
 		} while (value);
 	}
@@ -746,7 +751,7 @@ static struct resource __devinitdata ab8500_usb_resources[] = {
 
 static struct resource __devinitdata ab8500_temp_resources[] = {
 	{
-		.name  = "AB8500_TEMP_WARM",
+		.name  = "ABX500_TEMP_WARM",
 		.start = AB8500_INT_TEMP_WARM,
 		.end   = AB8500_INT_TEMP_WARM,
 		.flags = IORESOURCE_IRQ,
@@ -768,6 +773,9 @@ static struct mfd_cell __devinitdata abx500_common_devs[] = {
 		.name = "ab8500-regulator",
 	},
 	{
+		.name = "ab8500-regulator-debug",
+	},
+	{
 		.name = "ab8500-gpadc",
 		.num_resources = ARRAY_SIZE(ab8500_gpadc_resources),
 		.resources = ab8500_gpadc_resources,
@@ -778,26 +786,6 @@ static struct mfd_cell __devinitdata abx500_common_devs[] = {
 		.resources = ab8500_rtc_resources,
 	},
 	{
-		.name = "ab8500-charger",
-		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
-		.resources = ab8500_charger_resources,
-	},
-	{
-		.name = "ab8500-btemp",
-		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
-		.resources = ab8500_btemp_resources,
-	},
-	{
-		.name = "ab8500-fg",
-		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
-		.resources = ab8500_fg_resources,
-	},
-	{
-		.name = "ab8500-chargalg",
-		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
-		.resources = ab8500_chargalg_resources,
-	},
-	{
 		.name = "ab8500-acc-det",
 		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
 		.resources = ab8500_av_acc_detect_resources,
@@ -815,20 +803,12 @@ static struct mfd_cell __devinitdata abx500_common_devs[] = {
 		.name = "ab8500-pwm",
 		.id = 1,
 	},
-	{
-		.name = "ab8500-pwm",
-		.id = 2,
-	},
-	{
-		.name = "ab8500-pwm",
-		.id = 3,
-	},
 	{ .name = "ab8500-leds", },
 	{
 		.name = "ab8500-denc",
 	},
 	{
-		.name = "ab8500-temp",
+		.name = "abx500-temp",
 		.num_resources = ARRAY_SIZE(ab8500_temp_resources),
 		.resources = ab8500_temp_resources,
 	},
@@ -860,6 +840,29 @@ static struct mfd_cell __devinitdata ab9540_devs[] = {
 	},
 };
 
+static struct mfd_cell __devinitdata ab8500_bm_devs[] = {
+	{
+		.name = "ab8500-charger",
+		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
+		.resources = ab8500_charger_resources,
+	},
+	{
+		.name = "ab8500-btemp",
+		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
+		.resources = ab8500_btemp_resources,
+	},
+	{
+		.name = "ab8500-fg",
+		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
+		.resources = ab8500_fg_resources,
+	},
+	{
+		.name = "ab8500-chargalg",
+		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
+		.resources = ab8500_chargalg_resources,
+	},
+};
+
 static ssize_t show_chip_id(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
@@ -1130,6 +1133,15 @@ int __devinit ab8500_init(struct ab8500 *ab8500, enum ab8500_version version)
 	if (ret)
 		goto out_freeirq;
 
+	if (!no_bm) {
+		/* Add battery management devices */
+		ret = mfd_add_devices(ab8500->dev, 0, ab8500_bm_devs,
+				      ARRAY_SIZE(ab8500_bm_devs), NULL,
+				      ab8500->irq_base);
+		if (ret)
+			dev_err(ab8500->dev, "error adding bm devices\n");
+	}
+
 	if (is_ab9540(ab8500))
 		ret = sysfs_create_group(&ab8500->dev->kobj,
 					&ab9540_attr_group);
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 9a0211aa889..7b912afd664 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -4,6 +4,72 @@
  * Author: Mattias Wallin <mattias.wallin@stericsson.com> for ST-Ericsson.
  * License Terms: GNU General Public License v2
  */
+/*
+ * AB8500 register access
+ * ======================
+ *
+ * read:
+ * # echo BANK  >  <debugfs>/ab8500/register-bank
+ * # echo ADDR  >  <debugfs>/ab8500/register-address
+ * # cat <debugfs>/ab8500/register-value
+ *
+ * write:
+ * # echo BANK  >  <debugfs>/ab8500/register-bank
+ * # echo ADDR  >  <debugfs>/ab8500/register-address
+ * # echo VALUE >  <debugfs>/ab8500/register-value
+ *
+ * read all registers from a bank:
+ * # echo BANK  >  <debugfs>/ab8500/register-bank
+ * # cat <debugfs>/ab8500/all-bank-register
+ *
+ * BANK   target AB8500 register bank
+ * ADDR   target AB8500 register address
+ * VALUE  decimal or 0x-prefixed hexadecimal
+ *
+ *
+ * User Space notification on AB8500 IRQ
+ * =====================================
+ *
+ * Allows user space entity to be notified when target AB8500 IRQ occurs.
+ * When subscribed, a sysfs entry is created in ab8500.i2c platform device.
+ * One can pool this file to get target IRQ occurence information.
+ *
+ * subscribe to an AB8500 IRQ:
+ * # echo IRQ  >  <debugfs>/ab8500/irq-subscribe
+ *
+ * unsubscribe from an AB8500 IRQ:
+ * # echo IRQ  >  <debugfs>/ab8500/irq-unsubscribe
+ *
+ *
+ * AB8500 register formated read/write access
+ * ==========================================
+ *
+ * Read:  read data, data>>SHIFT, data&=MASK, output data
+ *        [0xABCDEF98] shift=12 mask=0xFFF => 0x00000CDE
+ * Write: read data, data &= ~(MASK<<SHIFT), data |= (VALUE<<SHIFT), write data
+ *        [0xABCDEF98] shift=12 mask=0xFFF value=0x123 => [0xAB123F98]
+ *
+ * Usage:
+ * # echo "CMD [OPTIONS] BANK ADRESS [VALUE]" > $debugfs/ab8500/hwreg
+ *
+ * CMD      read      read access
+ *          write     write access
+ *
+ * BANK     target reg bank
+ * ADDRESS  target reg address
+ * VALUE    (write) value to be updated
+ *
+ * OPTIONS
+ *  -d|-dec            (read) output in decimal
+ *  -h|-hexa           (read) output in 0x-hexa (default)
+ *  -l|-w|-b           32bit (default), 16bit or 8bit reg access
+ *  -m|-mask MASK      0x-hexa mask (default 0xFFFFFFFF)
+ *  -s|-shift SHIFT    bit shift value (read:left, write:right)
+ *  -o|-offset OFFSET  address offset to add to ADDRESS value
+ *
+ * Warning: bit shift operation is applied to bit-mask.
+ * Warning: bit shift direction depends on read or right command.
+ */
 
 #include <linux/seq_file.h>
 #include <linux/uaccess.h>
@@ -11,13 +77,29 @@
 #include <linux/module.h>
 #include <linux/debugfs.h>
 #include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/kobject.h>
+#include <linux/slab.h>
 
 #include <linux/mfd/abx500.h>
-#include <linux/mfd/abx500/ab8500.h>
+#include <linux/mfd/abx500/ab8500-gpadc.h>
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/string.h>
+#include <linux/ctype.h>
+#endif
 
 static u32 debug_bank;
 static u32 debug_address;
 
+static int irq_first;
+static int irq_last;
+static u32 *irq_count;
+static int num_irqs;
+
+static struct device_attribute **dev_attr;
+static char **event_name;
+
 /**
  * struct ab8500_reg_range
  * @first: the first address of the range
@@ -42,15 +124,35 @@ struct ab8500_i2c_ranges {
 	const struct ab8500_reg_range *range;
 };
 
+/* hwreg- "mask" and "shift" entries ressources */
+struct hwreg_cfg {
+	u32  bank;      /* target bank */
+	u32  addr;      /* target address */
+	uint fmt;       /* format */
+	uint mask;      /* read/write mask, applied before any bit shift */
+	int  shift;     /* bit shift (read:right shift, write:left shift */
+};
+/* fmt bit #0: 0=hexa, 1=dec */
+#define REG_FMT_DEC(c) ((c)->fmt & 0x1)
+#define REG_FMT_HEX(c) (!REG_FMT_DEC(c))
+
+static struct hwreg_cfg hwreg_cfg = {
+	.addr = 0,			/* default: invalid phys addr */
+	.fmt = 0,			/* default: 32bit access, hex output */
+	.mask = 0xFFFFFFFF,	/* default: no mask */
+	.shift = 0,			/* default: no bit shift */
+};
+
 #define AB8500_NAME_STRING "ab8500"
-#define AB8500_NUM_BANKS 22
+#define AB8500_ADC_NAME_STRING "gpadc"
+#define AB8500_NUM_BANKS 24
 
 #define AB8500_REV_REG 0x80
 
 static struct ab8500_i2c_ranges debug_ranges[AB8500_NUM_BANKS] = {
 	[0x0] = {
 		.num_ranges = 0,
-		.range = 0,
+		.range = NULL,
 	},
 	[AB8500_SYS_CTRL1_BLOCK] = {
 		.num_ranges = 3,
@@ -215,7 +317,7 @@ static struct ab8500_i2c_ranges debug_ranges[AB8500_NUM_BANKS] = {
 		},
 	},
 	[AB8500_CHARGER] = {
-		.num_ranges = 8,
+		.num_ranges = 9,
 		.range = (struct ab8500_reg_range[]) {
 			{
 				.first = 0x00,
@@ -249,6 +351,10 @@ static struct ab8500_i2c_ranges debug_ranges[AB8500_NUM_BANKS] = {
 				.first = 0xC0,
 				.last = 0xC2,
 			},
+			{
+				.first = 0xf5,
+				.last =	0xf6,
+			},
 		},
 	},
 	[AB8500_GAS_GAUGE] = {
@@ -268,6 +374,24 @@ static struct ab8500_i2c_ranges debug_ranges[AB8500_NUM_BANKS] = {
 			},
 		},
 	},
+	[AB8500_DEVELOPMENT] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x00,
+				.last = 0x00,
+			},
+		},
+	},
+	[AB8500_DEBUG] = {
+		.num_ranges = 1,
+		.range = (struct ab8500_reg_range[]) {
+			{
+				.first = 0x05,
+				.last = 0x07,
+			},
+		},
+	},
 	[AB8500_AUDIO] = {
 		.num_ranges = 1,
 		.range = (struct ab8500_reg_range[]) {
@@ -354,15 +478,30 @@ static struct ab8500_i2c_ranges debug_ranges[AB8500_NUM_BANKS] = {
 	},
 };
 
-static int ab8500_registers_print(struct seq_file *s, void *p)
+static irqreturn_t ab8500_debug_handler(int irq, void *data)
 {
-	struct device *dev = s->private;
-	unsigned int i;
-	u32 bank = debug_bank;
+	char buf[16];
+	struct kobject *kobj = (struct kobject *)data;
+	unsigned int irq_abb = irq - irq_first;
 
-	seq_printf(s, AB8500_NAME_STRING " register values:\n");
+	if (irq_abb < num_irqs)
+		irq_count[irq_abb]++;
+	/*
+	 * This makes it possible to use poll for events (POLLPRI | POLLERR)
+	 * from userspace on sysfs file named <irq-nr>
+	 */
+	sprintf(buf, "%d", irq);
+	sysfs_notify(kobj, NULL, buf);
+
+	return IRQ_HANDLED;
+}
+
+/* Prints to seq_file or log_buf */
+static int ab8500_registers_print(struct device *dev, u32 bank,
+				struct seq_file *s)
+{
+	unsigned int i;
 
-	seq_printf(s, " bank %u:\n", bank);
 	for (i = 0; i < debug_ranges[bank].num_ranges; i++) {
 		u32 reg;
 
@@ -379,22 +518,42 @@ static int ab8500_registers_print(struct seq_file *s, void *p)
 				return err;
 			}
 
-			err = seq_printf(s, "  [%u/0x%02X]: 0x%02X\n", bank,
-				reg, value);
-			if (err < 0) {
-				dev_err(dev, "seq_printf overflow\n");
-				/* Error is not returned here since
-				 * the output is wanted in any case */
-				return 0;
+			if (s) {
+				err = seq_printf(s, "  [%u/0x%02X]: 0x%02X\n",
+					bank, reg, value);
+				if (err < 0) {
+					dev_err(dev,
+					"seq_printf overflow bank=%d reg=%d\n",
+						bank, reg);
+					/* Error is not returned here since
+					 * the output is wanted in any case */
+					return 0;
+				}
+			} else {
+				printk(KERN_INFO" [%u/0x%02X]: 0x%02X\n", bank,
+					reg, value);
 			}
 		}
 	}
 	return 0;
 }
 
+static int ab8500_print_bank_registers(struct seq_file *s, void *p)
+{
+	struct device *dev = s->private;
+	u32 bank = debug_bank;
+
+	seq_printf(s, AB8500_NAME_STRING " register values:\n");
+
+	seq_printf(s, " bank %u:\n", bank);
+
+	ab8500_registers_print(dev, bank, s);
+	return 0;
+}
+
 static int ab8500_registers_open(struct inode *inode, struct file *file)
 {
-	return single_open(file, ab8500_registers_print, inode->i_private);
+	return single_open(file, ab8500_print_bank_registers, inode->i_private);
 }
 
 static const struct file_operations ab8500_registers_fops = {
@@ -405,6 +564,64 @@ static const struct file_operations ab8500_registers_fops = {
 	.owner = THIS_MODULE,
 };
 
+static int ab8500_print_all_banks(struct seq_file *s, void *p)
+{
+	struct device *dev = s->private;
+	unsigned int i;
+	int err;
+
+	seq_printf(s, AB8500_NAME_STRING " register values:\n");
+
+	for (i = 1; i < AB8500_NUM_BANKS; i++) {
+		err = seq_printf(s, " bank %u:\n", i);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow, bank=%d\n", i);
+
+		ab8500_registers_print(dev, i, s);
+	}
+	return 0;
+}
+
+/* Dump registers to kernel log */
+void ab8500_dump_all_banks(struct device *dev)
+{
+	unsigned int i;
+
+	printk(KERN_INFO"ab8500 register values:\n");
+
+	for (i = 1; i < AB8500_NUM_BANKS; i++) {
+		printk(KERN_INFO" bank %u:\n", i);
+		ab8500_registers_print(dev, i, NULL);
+	}
+}
+
+static int ab8500_all_banks_open(struct inode *inode, struct file *file)
+{
+	struct seq_file *s;
+	int err;
+
+	err = single_open(file, ab8500_print_all_banks, inode->i_private);
+	if (!err) {
+		/* Default buf size in seq_read is not enough */
+		s = (struct seq_file *)file->private_data;
+		s->size = (PAGE_SIZE * 2);
+		s->buf = kmalloc(s->size, GFP_KERNEL);
+		if (!s->buf) {
+			single_release(inode, file);
+			err = -ENOMEM;
+		}
+	}
+	return err;
+}
+
+static const struct file_operations ab8500_all_banks_fops = {
+	.open = ab8500_all_banks_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
 static int ab8500_bank_print(struct seq_file *s, void *p)
 {
 	return seq_printf(s, "%d\n", debug_bank);
@@ -515,10 +732,761 @@ static ssize_t ab8500_val_write(struct file *file,
 		printk(KERN_ERR "abx500_set_reg failed %d, %d", err, __LINE__);
 		return -EINVAL;
 	}
+	return count;
+}
+
+/*
+ * Interrupt status
+ */
+static u32 num_interrupts[AB8500_MAX_NR_IRQS];
+static int num_interrupt_lines;
+
+void ab8500_debug_register_interrupt(int line)
+{
+	if (line < num_interrupt_lines)
+		num_interrupts[line]++;
+}
+
+static int ab8500_interrupts_print(struct seq_file *s, void *p)
+{
+	int line;
+
+	seq_printf(s, "irq:  number of\n");
+
+	for (line = 0; line < num_interrupt_lines; line++)
+		seq_printf(s, "%3i:  %6i\n", line, num_interrupts[line]);
+
+	return 0;
+}
+
+static int ab8500_interrupts_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_interrupts_print, inode->i_private);
+}
+
+/*
+ * - HWREG DB8500 formated routines
+ */
+static int ab8500_hwreg_print(struct seq_file *s, void *d)
+{
+	struct device *dev = s->private;
+	int ret;
+	u8 regvalue;
+
+	ret = abx500_get_register_interruptible(dev,
+		(u8)hwreg_cfg.bank, (u8)hwreg_cfg.addr, &regvalue);
+	if (ret < 0) {
+		dev_err(dev, "abx500_get_reg fail %d, %d\n",
+			ret, __LINE__);
+		return -EINVAL;
+	}
+
+	if (hwreg_cfg.shift >= 0)
+		regvalue >>= hwreg_cfg.shift;
+	else
+		regvalue <<= -hwreg_cfg.shift;
+	regvalue &= hwreg_cfg.mask;
+
+	if (REG_FMT_DEC(&hwreg_cfg))
+		seq_printf(s, "%d\n", regvalue);
+	else
+		seq_printf(s, "0x%02X\n", regvalue);
+	return 0;
+}
+
+static int ab8500_hwreg_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_hwreg_print, inode->i_private);
+}
+
+static int ab8500_gpadc_bat_ctrl_print(struct seq_file *s, void *p)
+{
+	int bat_ctrl_raw;
+	int bat_ctrl_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	bat_ctrl_raw = ab8500_gpadc_read_raw(gpadc, BAT_CTRL);
+	bat_ctrl_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+			BAT_CTRL, bat_ctrl_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			bat_ctrl_convert, bat_ctrl_raw);
+}
+
+static int ab8500_gpadc_bat_ctrl_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_bat_ctrl_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_bat_ctrl_fops = {
+	.open = ab8500_gpadc_bat_ctrl_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_btemp_ball_print(struct seq_file *s, void *p)
+{
+	int btemp_ball_raw;
+	int btemp_ball_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	btemp_ball_raw = ab8500_gpadc_read_raw(gpadc, BTEMP_BALL);
+	btemp_ball_convert = ab8500_gpadc_ad_to_voltage(gpadc, BTEMP_BALL,
+			btemp_ball_raw);
+
+	return seq_printf(s,
+			"%d,0x%X\n", btemp_ball_convert, btemp_ball_raw);
+}
+
+static int ab8500_gpadc_btemp_ball_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_btemp_ball_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_btemp_ball_fops = {
+	.open = ab8500_gpadc_btemp_ball_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_main_charger_v_print(struct seq_file *s, void *p)
+{
+	int main_charger_v_raw;
+	int main_charger_v_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	main_charger_v_raw = ab8500_gpadc_read_raw(gpadc, MAIN_CHARGER_V);
+	main_charger_v_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+			MAIN_CHARGER_V, main_charger_v_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			main_charger_v_convert, main_charger_v_raw);
+}
+
+static int ab8500_gpadc_main_charger_v_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_main_charger_v_print,
+			inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_main_charger_v_fops = {
+	.open = ab8500_gpadc_main_charger_v_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_acc_detect1_print(struct seq_file *s, void *p)
+{
+	int acc_detect1_raw;
+	int acc_detect1_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	acc_detect1_raw = ab8500_gpadc_read_raw(gpadc, ACC_DETECT1);
+	acc_detect1_convert = ab8500_gpadc_ad_to_voltage(gpadc, ACC_DETECT1,
+			acc_detect1_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			acc_detect1_convert, acc_detect1_raw);
+}
+
+static int ab8500_gpadc_acc_detect1_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_acc_detect1_print,
+			inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_acc_detect1_fops = {
+	.open = ab8500_gpadc_acc_detect1_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_acc_detect2_print(struct seq_file *s, void *p)
+{
+	int acc_detect2_raw;
+	int acc_detect2_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	acc_detect2_raw = ab8500_gpadc_read_raw(gpadc, ACC_DETECT2);
+	acc_detect2_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+	    ACC_DETECT2, acc_detect2_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			acc_detect2_convert, acc_detect2_raw);
+}
+
+static int ab8500_gpadc_acc_detect2_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_acc_detect2_print,
+	    inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_acc_detect2_fops = {
+	.open = ab8500_gpadc_acc_detect2_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_aux1_print(struct seq_file *s, void *p)
+{
+	int aux1_raw;
+	int aux1_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	aux1_raw = ab8500_gpadc_read_raw(gpadc, ADC_AUX1);
+	aux1_convert = ab8500_gpadc_ad_to_voltage(gpadc, ADC_AUX1,
+			aux1_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			aux1_convert, aux1_raw);
+}
+
+static int ab8500_gpadc_aux1_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_aux1_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_aux1_fops = {
+	.open = ab8500_gpadc_aux1_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_aux2_print(struct seq_file *s, void *p)
+{
+	int aux2_raw;
+	int aux2_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	aux2_raw = ab8500_gpadc_read_raw(gpadc, ADC_AUX2);
+	aux2_convert = ab8500_gpadc_ad_to_voltage(gpadc, ADC_AUX2,
+			aux2_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			aux2_convert, aux2_raw);
+}
+
+static int ab8500_gpadc_aux2_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_aux2_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_aux2_fops = {
+	.open = ab8500_gpadc_aux2_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_main_bat_v_print(struct seq_file *s, void *p)
+{
+	int main_bat_v_raw;
+	int main_bat_v_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	main_bat_v_raw = ab8500_gpadc_read_raw(gpadc, MAIN_BAT_V);
+	main_bat_v_convert = ab8500_gpadc_ad_to_voltage(gpadc, MAIN_BAT_V,
+			main_bat_v_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			main_bat_v_convert, main_bat_v_raw);
+}
+
+static int ab8500_gpadc_main_bat_v_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_main_bat_v_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_main_bat_v_fops = {
+	.open = ab8500_gpadc_main_bat_v_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_vbus_v_print(struct seq_file *s, void *p)
+{
+	int vbus_v_raw;
+	int vbus_v_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	vbus_v_raw = ab8500_gpadc_read_raw(gpadc, VBUS_V);
+	vbus_v_convert = ab8500_gpadc_ad_to_voltage(gpadc, VBUS_V,
+			vbus_v_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			vbus_v_convert, vbus_v_raw);
+}
+
+static int ab8500_gpadc_vbus_v_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_vbus_v_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_vbus_v_fops = {
+	.open = ab8500_gpadc_vbus_v_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_main_charger_c_print(struct seq_file *s, void *p)
+{
+	int main_charger_c_raw;
+	int main_charger_c_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	main_charger_c_raw = ab8500_gpadc_read_raw(gpadc, MAIN_CHARGER_C);
+	main_charger_c_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+			MAIN_CHARGER_C, main_charger_c_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			main_charger_c_convert, main_charger_c_raw);
+}
+
+static int ab8500_gpadc_main_charger_c_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_main_charger_c_print,
+			inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_main_charger_c_fops = {
+	.open = ab8500_gpadc_main_charger_c_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_usb_charger_c_print(struct seq_file *s, void *p)
+{
+	int usb_charger_c_raw;
+	int usb_charger_c_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	usb_charger_c_raw = ab8500_gpadc_read_raw(gpadc, USB_CHARGER_C);
+	usb_charger_c_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+	    USB_CHARGER_C, usb_charger_c_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			usb_charger_c_convert, usb_charger_c_raw);
+}
+
+static int ab8500_gpadc_usb_charger_c_open(struct inode *inode,
+		struct file *file)
+{
+	return single_open(file, ab8500_gpadc_usb_charger_c_print,
+	    inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_usb_charger_c_fops = {
+	.open = ab8500_gpadc_usb_charger_c_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_bk_bat_v_print(struct seq_file *s, void *p)
+{
+	int bk_bat_v_raw;
+	int bk_bat_v_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	bk_bat_v_raw = ab8500_gpadc_read_raw(gpadc, BK_BAT_V);
+	bk_bat_v_convert = ab8500_gpadc_ad_to_voltage(gpadc,
+			BK_BAT_V, bk_bat_v_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			bk_bat_v_convert, bk_bat_v_raw);
+}
+
+static int ab8500_gpadc_bk_bat_v_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_bk_bat_v_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_bk_bat_v_fops = {
+	.open = ab8500_gpadc_bk_bat_v_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int ab8500_gpadc_die_temp_print(struct seq_file *s, void *p)
+{
+	int die_temp_raw;
+	int die_temp_convert;
+	struct ab8500_gpadc *gpadc;
+
+	gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
+	die_temp_raw = ab8500_gpadc_read_raw(gpadc, DIE_TEMP);
+	die_temp_convert = ab8500_gpadc_ad_to_voltage(gpadc, DIE_TEMP,
+			die_temp_raw);
+
+	return seq_printf(s, "%d,0x%X\n",
+			die_temp_convert, die_temp_raw);
+}
+
+static int ab8500_gpadc_die_temp_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_gpadc_die_temp_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_gpadc_die_temp_fops = {
+	.open = ab8500_gpadc_die_temp_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+/*
+ * return length of an ASCII numerical value, 0 is string is not a
+ * numerical value.
+ * string shall start at value 1st char.
+ * string can be tailed with \0 or space or newline chars only.
+ * value can be decimal or hexadecimal (prefixed 0x or 0X).
+ */
+static int strval_len(char *b)
+{
+	char *s = b;
+	if ((*s == '0') && ((*(s+1) == 'x') || (*(s+1) == 'X'))) {
+		s += 2;
+		for (; *s && (*s != ' ') && (*s != '\n'); s++) {
+			if (!isxdigit(*s))
+				return 0;
+		}
+	} else {
+		if (*s == '-')
+			s++;
+		for (; *s && (*s != ' ') && (*s != '\n'); s++) {
+			if (!isdigit(*s))
+				return 0;
+		}
+	}
+	return (int) (s-b);
+}
+
+/*
+ * parse hwreg input data.
+ * update global hwreg_cfg only if input data syntax is ok.
+ */
+static ssize_t hwreg_common_write(char *b, struct hwreg_cfg *cfg,
+		struct device *dev)
+{
+	uint write, val = 0;
+	struct hwreg_cfg loc = {
+		.bank = 0,          /* default: invalid phys addr */
+		.addr = 0,          /* default: invalid phys addr */
+		.fmt = 0,           /* default: 32bit access, hex output */
+		.mask = 0xFFFFFFFF, /* default: no mask */
+		.shift = 0,         /* default: no bit shift */
+	};
+
+	/* read or write ? */
+	if (!strncmp(b, "read ", 5)) {
+		write = 0;
+		b += 5;
+	} else if (!strncmp(b, "write ", 6)) {
+		write = 1;
+		b += 6;
+	} else
+		return -EINVAL;
+
+	/* OPTIONS -l|-w|-b -s -m -o */
+	while ((*b == ' ') || (*b == '-')) {
+		if (*(b-1) != ' ') {
+			b++;
+			continue;
+		}
+		if ((!strncmp(b, "-d ", 3)) ||
+				(!strncmp(b, "-dec ", 5))) {
+			b += (*(b+2) == ' ') ? 3 : 5;
+			loc.fmt |= (1<<0);
+		} else if ((!strncmp(b, "-h ", 3)) ||
+				(!strncmp(b, "-hex ", 5))) {
+			b += (*(b+2) == ' ') ? 3 : 5;
+			loc.fmt &= ~(1<<0);
+		} else if ((!strncmp(b, "-m ", 3)) ||
+				(!strncmp(b, "-mask ", 6))) {
+			b += (*(b+2) == ' ') ? 3 : 6;
+			if (strval_len(b) == 0)
+				return -EINVAL;
+			loc.mask = simple_strtoul(b, &b, 0);
+		} else if ((!strncmp(b, "-s ", 3)) ||
+				(!strncmp(b, "-shift ", 7))) {
+			b += (*(b+2) == ' ') ? 3 : 7;
+			if (strval_len(b) == 0)
+				return -EINVAL;
+			loc.shift = simple_strtol(b, &b, 0);
+		} else {
+			return -EINVAL;
+		}
+	}
+	/* get arg BANK and ADDRESS */
+	if (strval_len(b) == 0)
+		return -EINVAL;
+	loc.bank = simple_strtoul(b, &b, 0);
+	while (*b == ' ')
+		b++;
+	if (strval_len(b) == 0)
+		return -EINVAL;
+	loc.addr = simple_strtoul(b, &b, 0);
+
+	if (write) {
+		while (*b == ' ')
+			b++;
+		if (strval_len(b) == 0)
+			return -EINVAL;
+		val = simple_strtoul(b, &b, 0);
+	}
+
+	/* args are ok, update target cfg (mainly for read) */
+	*cfg = loc;
+
+#ifdef ABB_HWREG_DEBUG
+	pr_warn("HWREG request: %s, %s, addr=0x%08X, mask=0x%X, shift=%d"
+			"value=0x%X\n", (write) ? "write" : "read",
+			REG_FMT_DEC(cfg) ? "decimal" : "hexa",
+			cfg->addr, cfg->mask, cfg->shift, val);
+#endif
+
+	if (write) {
+		u8  regvalue;
+		int ret = abx500_get_register_interruptible(dev,
+			(u8)cfg->bank, (u8)cfg->addr, &regvalue);
+		if (ret < 0) {
+			dev_err(dev, "abx500_get_reg fail %d, %d\n",
+				ret, __LINE__);
+			return -EINVAL;
+		}
+
+		if (cfg->shift >= 0) {
+			regvalue &= ~(cfg->mask << (cfg->shift));
+			val = (val & cfg->mask) << (cfg->shift);
+		} else {
+			regvalue &= ~(cfg->mask >> (-cfg->shift));
+			val = (val & cfg->mask) >> (-cfg->shift);
+		}
+		val = val | regvalue;
+
+		ret = abx500_set_register_interruptible(dev,
+			(u8)cfg->bank, (u8)cfg->addr, (u8)val);
+		if (ret < 0) {
+			pr_err("abx500_set_reg failed %d, %d", ret, __LINE__);
+			return -EINVAL;
+		}
+
+	}
+	return 0;
+}
+
+static ssize_t ab8500_hwreg_write(struct file *file,
+	const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	char buf[128];
+	int buf_size, ret;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	/* get args and process */
+	ret = hwreg_common_write(buf, &hwreg_cfg, dev);
+	return (ret) ? ret : buf_size;
+}
+
+/*
+ * - irq subscribe/unsubscribe stuff
+ */
+static int ab8500_subscribe_unsubscribe_print(struct seq_file *s, void *p)
+{
+	seq_printf(s, "%d\n", irq_first);
+
+	return 0;
+}
+
+static int ab8500_subscribe_unsubscribe_open(struct inode *inode,
+	struct file *file)
+{
+	return single_open(file, ab8500_subscribe_unsubscribe_print,
+		inode->i_private);
+}
+
+/*
+ * Userspace should use poll() on this file. When an event occur
+ * the blocking poll will be released.
+ */
+static ssize_t show_irq(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	unsigned long name;
+	unsigned int irq_index;
+	int err;
+
+	err = strict_strtoul(attr->attr.name, 0, &name);
+	if (err)
+		return err;
+
+	irq_index = name - irq_first;
+	if (irq_index >= num_irqs)
+		return -EINVAL;
+	else
+		return sprintf(buf, "%u\n", irq_count[irq_index]);
+}
+
+static ssize_t ab8500_subscribe_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	char buf[32];
+	int buf_size;
+	unsigned long user_val;
+	int err;
+	unsigned int irq_index;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	err = strict_strtoul(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	if (user_val < irq_first) {
+		dev_err(dev, "debugfs error input < %d\n", irq_first);
+		return -EINVAL;
+	}
+	if (user_val > irq_last) {
+		dev_err(dev, "debugfs error input > %d\n", irq_last);
+		return -EINVAL;
+	}
+
+	irq_index = user_val - irq_first;
+	if (irq_index >= num_irqs)
+		return -EINVAL;
+
+	/*
+	 * This will create a sysfs file named <irq-nr> which userspace can
+	 * use to select or poll and get the AB8500 events
+	 */
+	dev_attr[irq_index] = kmalloc(sizeof(struct device_attribute),
+		GFP_KERNEL);
+	event_name[irq_index] = kmalloc(buf_size, GFP_KERNEL);
+	sprintf(event_name[irq_index], "%lu", user_val);
+	dev_attr[irq_index]->show = show_irq;
+	dev_attr[irq_index]->store = NULL;
+	dev_attr[irq_index]->attr.name = event_name[irq_index];
+	dev_attr[irq_index]->attr.mode = S_IRUGO;
+	err = sysfs_create_file(&dev->kobj, &dev_attr[irq_index]->attr);
+	if (err < 0) {
+		printk(KERN_ERR "sysfs_create_file failed %d\n", err);
+		return err;
+	}
+
+	err = request_threaded_irq(user_val, NULL, ab8500_debug_handler,
+		IRQF_SHARED | IRQF_NO_SUSPEND, "ab8500-debug", &dev->kobj);
+	if (err < 0) {
+		printk(KERN_ERR "request_threaded_irq failed %d, %lu\n",
+			err, user_val);
+		sysfs_remove_file(&dev->kobj, &dev_attr[irq_index]->attr);
+		return err;
+	}
+
+	return buf_size;
+}
+
+static ssize_t ab8500_unsubscribe_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	struct device *dev = ((struct seq_file *)(file->private_data))->private;
+	char buf[32];
+	int buf_size;
+	unsigned long user_val;
+	int err;
+	unsigned int irq_index;
+
+	/* Get userspace string and assure termination */
+	buf_size = min(count, (sizeof(buf)-1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	err = strict_strtoul(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	if (user_val < irq_first) {
+		dev_err(dev, "debugfs error input < %d\n", irq_first);
+		return -EINVAL;
+	}
+	if (user_val > irq_last) {
+		dev_err(dev, "debugfs error input > %d\n", irq_last);
+		return -EINVAL;
+	}
+
+	irq_index = user_val - irq_first;
+	if (irq_index >= num_irqs)
+		return -EINVAL;
+
+	/* Set irq count to 0 when unsubscribe */
+	irq_count[irq_index] = 0;
+
+	if (dev_attr[irq_index])
+		sysfs_remove_file(&dev->kobj, &dev_attr[irq_index]->attr);
+
+
+	free_irq(user_val, &dev->kobj);
+	kfree(event_name[irq_index]);
+	kfree(dev_attr[irq_index]);
 
 	return count;
 }
 
+/*
+ * - several deubgfs nodes fops
+ */
+
 static const struct file_operations ab8500_bank_fops = {
 	.open = ab8500_bank_open,
 	.write = ab8500_bank_write,
@@ -546,64 +1514,231 @@ static const struct file_operations ab8500_val_fops = {
 	.owner = THIS_MODULE,
 };
 
+static const struct file_operations ab8500_interrupts_fops = {
+	.open = ab8500_interrupts_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ab8500_subscribe_fops = {
+	.open = ab8500_subscribe_unsubscribe_open,
+	.write = ab8500_subscribe_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ab8500_unsubscribe_fops = {
+	.open = ab8500_subscribe_unsubscribe_open,
+	.write = ab8500_unsubscribe_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static const struct file_operations ab8500_hwreg_fops = {
+	.open = ab8500_hwreg_open,
+	.write = ab8500_hwreg_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
 static struct dentry *ab8500_dir;
-static struct dentry *ab8500_reg_file;
-static struct dentry *ab8500_bank_file;
-static struct dentry *ab8500_address_file;
-static struct dentry *ab8500_val_file;
+static struct dentry *ab8500_gpadc_dir;
 
 static int __devinit ab8500_debug_probe(struct platform_device *plf)
 {
+	struct dentry *file;
+	int ret = -ENOMEM;
+	struct ab8500 *ab8500;
 	debug_bank = AB8500_MISC;
 	debug_address = AB8500_REV_REG & 0x00FF;
 
+	ab8500 = dev_get_drvdata(plf->dev.parent);
+	num_irqs = ab8500->mask_size;
+
+	irq_count = kzalloc(sizeof(*irq_count)*num_irqs, GFP_KERNEL);
+	if (!irq_count)
+		return -ENOMEM;
+
+	dev_attr = kzalloc(sizeof(*dev_attr)*num_irqs,GFP_KERNEL);
+	if (!dev_attr)
+		goto out_freeirq_count;
+
+	event_name = kzalloc(sizeof(*event_name)*num_irqs, GFP_KERNEL);
+	if (!event_name)
+		goto out_freedev_attr;
+
+	irq_first = platform_get_irq_byname(plf, "IRQ_FIRST");
+	if (irq_first < 0) {
+		dev_err(&plf->dev, "First irq not found, err %d\n",
+				irq_first);
+		ret = irq_first;
+		goto out_freeevent_name;
+	}
+
+	irq_last = platform_get_irq_byname(plf, "IRQ_LAST");
+	if (irq_last < 0) {
+		dev_err(&plf->dev, "Last irq not found, err %d\n",
+				irq_last);
+		ret = irq_last;
+                goto out_freeevent_name;
+	}
+
 	ab8500_dir = debugfs_create_dir(AB8500_NAME_STRING, NULL);
 	if (!ab8500_dir)
-		goto exit_no_debugfs;
+		goto err;
+
+	ab8500_gpadc_dir = debugfs_create_dir(AB8500_ADC_NAME_STRING,
+	    ab8500_dir);
+	if (!ab8500_gpadc_dir)
+		goto err;
+
+	file = debugfs_create_file("all-bank-registers", S_IRUGO,
+	    ab8500_dir, &plf->dev, &ab8500_registers_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("all-banks", S_IRUGO,
+	    ab8500_dir, &plf->dev, &ab8500_all_banks_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("register-bank", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_bank_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("register-address", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_address_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("register-value", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_val_fops);
+	if (!file)
+		goto err;
 
-	ab8500_reg_file = debugfs_create_file("all-bank-registers",
-		S_IRUGO, ab8500_dir, &plf->dev, &ab8500_registers_fops);
-	if (!ab8500_reg_file)
-		goto exit_destroy_dir;
+	if (is_ab8500(ab8500))
+		num_interrupt_lines = AB8500_NR_IRQS;
+	else if (is_ab8505(ab8500))
+		num_interrupt_lines = AB8505_NR_IRQS;
+	else if (is_ab9540(ab8500))
+		num_interrupt_lines = AB9540_NR_IRQS;
 
-	ab8500_bank_file = debugfs_create_file("register-bank",
-		(S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev, &ab8500_bank_fops);
-	if (!ab8500_bank_file)
-		goto exit_destroy_reg;
+	file = debugfs_create_file("interrupts", (S_IRUGO),
+	    ab8500_dir, &plf->dev, &ab8500_interrupts_fops);
+	if (!file)
+		goto err;
 
-	ab8500_address_file = debugfs_create_file("register-address",
-		(S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev,
-		&ab8500_address_fops);
-	if (!ab8500_address_file)
-		goto exit_destroy_bank;
+	file = debugfs_create_file("irq-subscribe", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_subscribe_fops);
+	if (!file)
+		goto err;
 
-	ab8500_val_file = debugfs_create_file("register-value",
-		(S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev, &ab8500_val_fops);
-	if (!ab8500_val_file)
-		goto exit_destroy_address;
+	file = debugfs_create_file("irq-unsubscribe", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_unsubscribe_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("hwreg", (S_IRUGO | S_IWUGO),
+	    ab8500_dir, &plf->dev, &ab8500_hwreg_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("bat_ctrl", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_bat_ctrl_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("btemp_ball", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_btemp_ball_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("main_charger_v", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_charger_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("acc_detect1", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_acc_detect1_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("acc_detect2", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_acc_detect2_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("adc_aux1", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_aux1_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("adc_aux2", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_aux2_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("main_bat_v", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_bat_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("vbus_v", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_vbus_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("main_charger_c", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_main_charger_c_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("usb_charger_c", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_usb_charger_c_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("bk_bat_v", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_bk_bat_v_fops);
+	if (!file)
+		goto err;
+
+	file = debugfs_create_file("die_temp", (S_IRUGO | S_IWUGO),
+	    ab8500_gpadc_dir, &plf->dev, &ab8500_gpadc_die_temp_fops);
+	if (!file)
+		goto err;
 
 	return 0;
 
-exit_destroy_address:
-	debugfs_remove(ab8500_address_file);
-exit_destroy_bank:
-	debugfs_remove(ab8500_bank_file);
-exit_destroy_reg:
-	debugfs_remove(ab8500_reg_file);
-exit_destroy_dir:
-	debugfs_remove(ab8500_dir);
-exit_no_debugfs:
+err:
+	if (ab8500_dir)
+		debugfs_remove_recursive(ab8500_dir);
 	dev_err(&plf->dev, "failed to create debugfs entries.\n");
-	return -ENOMEM;
+out_freeevent_name:
+	kfree(event_name);
+out_freedev_attr:
+	kfree(dev_attr);
+out_freeirq_count:
+	kfree(irq_count);
+
+	return ret;
 }
 
 static int __devexit ab8500_debug_remove(struct platform_device *plf)
 {
-	debugfs_remove(ab8500_val_file);
-	debugfs_remove(ab8500_address_file);
-	debugfs_remove(ab8500_bank_file);
-	debugfs_remove(ab8500_reg_file);
-	debugfs_remove(ab8500_dir);
+	debugfs_remove_recursive(ab8500_dir);
+	kfree(event_name);
+	kfree(dev_attr);
+	kfree(irq_count);
 
 	return 0;
 }
diff --git a/drivers/mfd/ab8500-denc.c b/drivers/mfd/ab8500-denc.c
new file mode 100644
index 00000000000..17efee62110
--- /dev/null
+++ b/drivers/mfd/ab8500-denc.c
@@ -0,0 +1,539 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson AB8500 DENC base driver
+ *
+ * Author: Marcel Tunnissen <marcel.tuennissen@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/debugfs.h>
+#include <linux/list.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/err.h>
+#include <linux/uaccess.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab8500.h>
+#include <linux/mfd/ab8500/denc-regs.h>
+#include <linux/mfd/ab8500/denc.h>
+
+#define AB8500_NAME      "ab8500"
+#define AB8500_DENC_NAME "ab8500_denc"
+
+struct device_usage {
+	struct list_head list;
+	struct platform_device *pdev;
+	bool taken;
+};
+static LIST_HEAD(device_list);
+
+/* To get rid of the extra bank parameter: */
+#define AB8500_REG_BANK_NR(__reg)	((0xff00 & (__reg)) >> 8)
+static inline u8 ab8500_rreg(struct device *dev, u32 reg)
+{
+	u8 val;
+	if (abx500_get_register_interruptible(dev, AB8500_REG_BANK_NR(reg),
+								reg, &val) < 0)
+		return 0;
+	else
+		return val;
+}
+
+static inline int ab8500_wreg(struct device *dev, u32 reg, u8 val)
+{
+	return abx500_set_register_interruptible(dev, AB8500_REG_BANK_NR(reg),
+								reg, val);
+}
+
+/* Only use in the macro below: */
+static inline int _ab8500_wreg_fld(struct device *dev, u32 reg, u8 val,
+							u8 mask, u8 shift)
+{
+	int ret;
+	u8 org_val;
+
+	ret = abx500_get_register_interruptible(dev, AB8500_REG_BANK_NR(reg),
+								reg, &org_val);
+	if (ret < 0)
+		return ret;
+	else
+		ab8500_wreg(dev, reg,
+				(org_val & ~mask) | ((val << shift) & mask));
+	return 0;
+}
+
+#define ab8500_wr_fld(__d, __reg, __fld, __val)				\
+	_ab8500_wreg_fld(__d, __reg, __val, __reg##_##__fld##_MASK,	\
+						__reg##_##__fld##_SHIFT)
+
+#define ab8500_set_fld(__cur_val, __reg, __fld, __val)			\
+	(((__cur_val) & ~__reg##_##__fld##_MASK) |			\
+	(((__val) << __reg##_##__fld##_SHIFT) & __reg##_##__fld##_MASK))
+
+#define AB8500_DENC_TRACE(__pd)	dev_dbg(&(__pd)->dev, "%s\n", __func__)
+
+#ifdef CONFIG_DEBUG_FS
+static struct dentry *debugfs_ab8500_denc_dir;
+static struct dentry *debugfs_ab8500_dump_regs_file;
+static void ab8500_denc_conf_ddr(struct platform_device *pdev);
+static int debugfs_ab8500_open_file(struct inode *inode, struct file *file);
+static ssize_t debugfs_ab8500_dump_regs(struct file *file, char __user *buf,
+						size_t count, loff_t *f_pos);
+
+static const struct file_operations debugfs_ab8500_dump_regs_fops = {
+	.owner = THIS_MODULE,
+	.open  = debugfs_ab8500_open_file,
+	.read  = debugfs_ab8500_dump_regs,
+};
+#endif /* CONFIG_DEBUG_FS */
+
+static int __devinit ab8500_denc_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct ab8500_platform_data *ab8500_pdata =
+		dev_get_platdata(pdev->dev.parent);
+	struct ab8500_denc_platform_data *pdata;
+	struct device_usage *device_data;
+
+	AB8500_DENC_TRACE(pdev);
+
+	if (ab8500_pdata == NULL) {
+		dev_err(&pdev->dev, "AB8500 platform data missing\n");
+		return -EINVAL;
+	}
+
+	pdata = ab8500_pdata->denc;
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "Denc platform data missing\n");
+		return -EINVAL;
+	}
+
+	device_data = kzalloc(sizeof(struct device_usage), GFP_KERNEL);
+	if (!device_data) {
+		dev_err(&pdev->dev, "Failed to allocate device data\n");
+		return -ENOMEM;
+	}
+	device_data->pdev = pdev;
+	list_add_tail(&device_data->list, &device_list);
+
+#ifdef CONFIG_DEBUG_FS
+	debugfs_ab8500_denc_dir = debugfs_create_dir(pdev->name, NULL);
+	debugfs_ab8500_dump_regs_file = debugfs_create_file(
+			"dumpregs", S_IRUGO,
+			debugfs_ab8500_denc_dir, &pdev->dev,
+			&debugfs_ab8500_dump_regs_fops
+		);
+#endif /* CONFIG_DEBUG_FS */
+	return ret;
+}
+
+static int __devexit ab8500_denc_remove(struct platform_device *pdev)
+{
+	struct list_head *element;
+	struct device_usage *device_data;
+
+	AB8500_DENC_TRACE(pdev);
+
+#ifdef CONFIG_DEBUG_FS
+	debugfs_remove(debugfs_ab8500_dump_regs_file);
+	debugfs_remove(debugfs_ab8500_denc_dir);
+#endif /* CONFIG_DEBUG_FS */
+
+	list_for_each(element, &device_list) {
+		device_data = list_entry(element, struct device_usage, list);
+		if (device_data->pdev == pdev) {
+			list_del(element);
+			kzfree(device_data);
+		}
+	}
+
+	return 0;
+}
+
+static struct platform_driver ab8500_denc_driver = {
+	.probe	= ab8500_denc_probe,
+	.remove = ab8500_denc_remove,
+	.driver = {
+		.name	= "ab8500-denc",
+	},
+};
+
+static void setup_27mhz(struct platform_device *pdev, bool enable)
+{
+	u8 data = ab8500_rreg(&pdev->dev, AB8500_SYS_ULP_CLK_CONF);
+
+	AB8500_DENC_TRACE(pdev);
+	/* TODO: check if this field needs to be set */
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, CLK_27MHZ_PD_ENA,
+									true);
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, CLK_27MHZ_BUF_ENA,
+									enable);
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, TVOUT_CLK_INV,
+									false);
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, TVOUT_CLK_DE_IN,
+									false);
+	data = ab8500_set_fld(data, AB8500_SYS_ULP_CLK_CONF, CLK_27MHZ_STRE,
+									1);
+	ab8500_wreg(&pdev->dev, AB8500_SYS_ULP_CLK_CONF, data);
+
+	data = ab8500_rreg(&pdev->dev, AB8500_SYS_CLK_CTRL);
+	data = ab8500_set_fld(data, AB8500_SYS_CLK_CTRL, TVOUT_CLK_VALID,
+									enable);
+	data = ab8500_set_fld(data, AB8500_SYS_CLK_CTRL, TVOUT_PLL_ENA,
+									enable);
+	ab8500_wreg(&pdev->dev, AB8500_SYS_CLK_CTRL, data);
+}
+
+static u32 map_tv_std(enum ab8500_denc_TV_std std)
+{
+	switch (std) {
+	case TV_STD_PAL_BDGHI:
+		return AB8500_DENC_CONF0_STD_PAL_BDGHI;
+	case TV_STD_PAL_N:
+		return AB8500_DENC_CONF0_STD_PAL_N;
+	case TV_STD_PAL_M:
+		return AB8500_DENC_CONF0_STD_PAL_M;
+	case TV_STD_NTSC_M:
+		return AB8500_DENC_CONF0_STD_NTSC_M;
+	default:
+		return 0;
+	}
+}
+
+static u32 map_cr_filter(enum ab8500_denc_cr_filter_bandwidth bw)
+{
+	switch (bw) {
+	case TV_CR_NTSC_LOW_DEF_FILTER:
+		return AB8500_DENC_CONF1_FLT_1_1MHZ;
+	case TV_CR_PAL_LOW_DEF_FILTER:
+		return AB8500_DENC_CONF1_FLT_1_3MHZ;
+	case TV_CR_NTSC_HIGH_DEF_FILTER:
+		return AB8500_DENC_CONF1_FLT_1_6MHZ;
+	case TV_CR_PAL_HIGH_DEF_FILTER:
+		return AB8500_DENC_CONF1_FLT_1_9MHZ;
+	default:
+		return 0;
+	}
+}
+
+static u32 map_phase_rst_mode(enum ab8500_denc_phase_reset_mode mode)
+{
+	switch (mode) {
+	case TV_PHASE_RST_MOD_DISABLE:
+		return AB8500_DENC_CONF8_PH_RST_MODE_DISABLED;
+	case TV_PHASE_RST_MOD_FROM_PHASE_BUF:
+		return AB8500_DENC_CONF8_PH_RST_MODE_UPDATE_FROM_PHASE_BUF;
+	case TV_PHASE_RST_MOD_FROM_INC_DFS:
+		return AB8500_DENC_CONF8_PH_RST_MODE_UPDATE_FROM_INC_DFS;
+	case TV_PHASE_RST_MOD_RST:
+		return AB8500_DENC_CONF8_PH_RST_MODE_RESET;
+	default:
+		return 0;
+	}
+}
+
+static u32 map_plug_time(enum ab8500_denc_plug_time time)
+{
+	switch (time) {
+	case TV_PLUG_TIME_0_5S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_0_5S;
+	case TV_PLUG_TIME_1S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_1S;
+	case TV_PLUG_TIME_1_5S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_1_5S;
+	case TV_PLUG_TIME_2S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_2S;
+	case TV_PLUG_TIME_2_5S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_2_5S;
+	case TV_PLUG_TIME_3S:
+		return AB8500_TVOUT_CTRL_PLUG_TV_TIME_3S;
+	default:
+		return 0;
+	}
+}
+
+struct platform_device *ab8500_denc_get_device(void)
+{
+	struct list_head *element;
+	struct device_usage *device_data;
+
+	pr_debug("%s\n", __func__);
+	list_for_each(element, &device_list) {
+		device_data = list_entry(element, struct device_usage, list);
+		if (!device_data->taken) {
+			device_data->taken = true;
+			return device_data->pdev;
+		}
+	}
+	return NULL;
+}
+EXPORT_SYMBOL(ab8500_denc_get_device);
+
+void ab8500_denc_put_device(struct platform_device *pdev)
+{
+	struct list_head *element;
+	struct device_usage *device_data;
+
+	AB8500_DENC_TRACE(pdev);
+	list_for_each(element, &device_list) {
+		device_data = list_entry(element, struct device_usage, list);
+		if (device_data->pdev == pdev)
+			device_data->taken = false;
+	}
+}
+EXPORT_SYMBOL(ab8500_denc_put_device);
+
+void ab8500_denc_reset(struct platform_device *pdev, bool hard)
+{
+	AB8500_DENC_TRACE(pdev);
+	if (hard) {
+		u8 data = ab8500_rreg(&pdev->dev, AB8500_CTRL3);
+		/* reset start */
+		ab8500_wreg(&pdev->dev, AB8500_CTRL3,
+			ab8500_set_fld(data, AB8500_CTRL3, RESET_DENC_N, 0)
+		);
+		/* reset done */
+		ab8500_wreg(&pdev->dev, AB8500_CTRL3,
+			ab8500_set_fld(data, AB8500_CTRL3, RESET_DENC_N, 1)
+		);
+	} else {
+		ab8500_wr_fld(&pdev->dev, AB8500_DENC_CONF6, SOFT_RESET, 1);
+		mdelay(10);
+	}
+}
+EXPORT_SYMBOL(ab8500_denc_reset);
+
+void ab8500_denc_power_up(struct platform_device *pdev)
+{
+	setup_27mhz(pdev, true);
+}
+EXPORT_SYMBOL(ab8500_denc_power_up);
+
+void ab8500_denc_power_down(struct platform_device *pdev)
+{
+	setup_27mhz(pdev, false);
+}
+EXPORT_SYMBOL(ab8500_denc_power_down);
+
+void ab8500_denc_conf(struct platform_device *pdev,
+						struct ab8500_denc_conf *conf)
+{
+	u8 data;
+
+	AB8500_DENC_TRACE(pdev);
+
+	ab8500_wreg(&pdev->dev, AB8500_DENC_CONF0,
+		AB8500_VAL2REG(AB8500_DENC_CONF0, STD, map_tv_std(conf->TV_std))
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF0, SYNC,
+			conf->test_pattern ? AB8500_DENC_CONF0_SYNC_AUTO_TEST :
+					AB8500_DENC_CONF0_SYNC_F_BASED_SLAVE
+		)
+	);
+	ab8500_wreg(&pdev->dev, AB8500_DENC_CONF1,
+		AB8500_VAL2REG(AB8500_DENC_CONF1, BLK_LI,
+						!conf->partial_blanking)
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF1, FLT,
+						map_cr_filter(conf->cr_filter))
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF1, CO_KI, conf->suppress_col)
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF1, SETUP_MAIN,
+						conf->black_level_setup)
+		/* TODO: handle cc field: set to 0 now */
+	);
+
+	data = ab8500_rreg(&pdev->dev, AB8500_DENC_CONF2);
+	data = ab8500_set_fld(data, AB8500_DENC_CONF2, N_INTRL,
+						conf->progressive);
+	ab8500_wreg(&pdev->dev, AB8500_DENC_CONF2, data);
+
+	ab8500_wreg(&pdev->dev, AB8500_DENC_CONF8,
+		AB8500_VAL2REG(AB8500_DENC_CONF8, PH_RST_MODE,
+				map_phase_rst_mode(conf->phase_reset_mode))
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF8, VAL_422_MUX,
+						conf->act_output)
+		|
+		AB8500_VAL2REG(AB8500_DENC_CONF8, BLK_ALL,
+						conf->blank_all)
+	);
+	data = ab8500_rreg(&pdev->dev, AB8500_TVOUT_CTRL);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, DAC_CTRL0,
+							conf->dac_enable);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, DAC_CTRL1,
+							conf->act_dc_output);
+	ab8500_wreg(&pdev->dev, AB8500_TVOUT_CTRL, data);
+
+	/* no support for DDR in early versions */
+	if (AB8500_REG2VAL(AB8500_REV, FULL_MASK,
+				ab8500_rreg(&pdev->dev, AB8500_REV)) > 0)
+		ab8500_denc_conf_ddr(pdev);
+}
+EXPORT_SYMBOL(ab8500_denc_conf);
+
+void ab8500_denc_conf_plug_detect(struct platform_device *pdev,
+						bool enable, bool load_RC,
+						enum ab8500_denc_plug_time time)
+{
+	u8 data;
+
+	AB8500_DENC_TRACE(pdev);
+	data = ab8500_rreg(&pdev->dev, AB8500_TVOUT_CTRL);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, TV_PLUG_ON,   enable);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, TV_LOAD_RC,   load_RC);
+	data = ab8500_set_fld(data, AB8500_TVOUT_CTRL, PLUG_TV_TIME,
+							map_plug_time(time));
+	ab8500_wreg(&pdev->dev, AB8500_TVOUT_CTRL, data);
+}
+EXPORT_SYMBOL(ab8500_denc_conf_plug_detect);
+
+void ab8500_denc_mask_int_plug_det(struct platform_device *pdev, bool plug,
+								bool unplug)
+{
+	u8 data = ab8500_rreg(&pdev->dev, AB8500_IT_MASK1);
+
+	AB8500_DENC_TRACE(pdev);
+	data = ab8500_set_fld(data, AB8500_IT_MASK1, PLUG_TV_DET,   plug);
+	data = ab8500_set_fld(data, AB8500_IT_MASK1, UNPLUG_TV_DET, unplug);
+	ab8500_wreg(&pdev->dev, AB8500_IT_MASK1, data);
+}
+EXPORT_SYMBOL(ab8500_denc_mask_int_plug_det);
+
+static void ab8500_denc_conf_ddr(struct platform_device *pdev)
+{
+	struct ab8500_platform_data *core_pdata;
+	struct ab8500_denc_platform_data *denc_pdata;
+
+	AB8500_DENC_TRACE(pdev);
+	core_pdata = dev_get_platdata(pdev->dev.parent);
+	denc_pdata = core_pdata->denc;
+	ab8500_wreg(&pdev->dev, AB8500_TVOUT_CTRL2,
+		AB8500_VAL2REG(AB8500_TVOUT_CTRL2,
+					DENC_DDR, denc_pdata->ddr_enable) |
+		AB8500_VAL2REG(AB8500_TVOUT_CTRL2, SWAP_DDR_DATA_IN,
+					denc_pdata->ddr_little_endian));
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int debugfs_ab8500_open_file(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+#define DEBUG_BUF_SIZE 900
+
+#define AB8500_GPIO_DIR5                0x1014
+#define AB8500_GPIO_DIR5_35_SHIFT       2
+#define AB8500_GPIO_DIR5_35_MASK        (1 << AB8500_GPIO_DIR5_35_SHIFT)
+#define AB8500_GPIO_OUT5                0x1024
+#define AB8500_GPIO_OUT5_35_SHIFT       2
+#define AB8500_GPIO_OUT5_35_MASK        (1 << AB8500_GPIO_OUT5_35_SHIFT)
+#define AB8500_GPIO_OUT5_35_VIDEO       0
+#define AB8500_GPIO_OUT5_35_AUDIO       1
+#define AB8500_GPIO_NPUD5                0x1034
+#define AB8500_GPIO_NPUD5_35_SHIFT       2
+#define AB8500_GPIO_NPUD5_35_MASK        (1 << AB8500_GPIO_NPUD5_35_SHIFT)
+#define AB8500_GPIO_NPUD5_35_ACTIVE      0
+#define AB8500_GPIO_NPUD5_35_INACTIVE    1
+
+static ssize_t debugfs_ab8500_dump_regs(struct file *file, char __user *buf,
+						size_t count, loff_t *f_pos)
+{
+	int ret = 0;
+	size_t data_size = 0;
+	char   buffer[DEBUG_BUF_SIZE];
+	struct device *dev = file->private_data;
+
+	data_size += sprintf(buffer + data_size,
+		"AB8500 DENC registers:\n"
+		"------Regulators etc ----------\n"
+		"CTRL3         : 0x%04x = 0x%02x\n"
+		"SYSULPCLK_CONF: 0x%04x = 0x%02x\n"
+		"SYSCLK_CTRL   : 0x%04x = 0x%02x\n"
+		"REGU_MISC1    : 0x%04x = 0x%02x\n"
+		"VAUX12_REGU   : 0x%04x = 0x%02x\n"
+		"VAUX1_SEL1    : 0x%04x = 0x%02x\n"
+		"------TVout only --------------\n"
+		"DENC_CONF0    : 0x%04x = 0x%02x\n"
+		"DENC_CONF1    : 0x%04x = 0x%02x\n"
+		"DENC_CONF2    : 0x%04x = 0x%02x\n"
+		"DENC_CONF6    : 0x%04x = 0x%02x\n"
+		"DENC_CONF8    : 0x%04x = 0x%02x\n"
+		"TVOUT_CTRL    : 0x%04x = 0x%02x\n"
+		"TVOUT_CTRL2   : 0x%04x = 0x%02x\n"
+		"IT_MASK1      : 0x%04x = 0x%02x\n"
+		"------AV connector-------------\n"
+		"GPIO_DIR5     : 0x%04x = 0x%02x\n"
+		"GPIO_OUT5     : 0x%04x = 0x%02x\n"
+		"GPIO_NPUD5    : 0x%04x = 0x%02x\n"
+		,
+		AB8500_CTRL3,            ab8500_rreg(dev, AB8500_CTRL3),
+		AB8500_SYS_ULP_CLK_CONF, ab8500_rreg(dev,
+						AB8500_SYS_ULP_CLK_CONF),
+		AB8500_SYS_CLK_CTRL,     ab8500_rreg(dev, AB8500_SYS_CLK_CTRL),
+		AB8500_REGU_MISC1,       ab8500_rreg(dev, AB8500_REGU_MISC1),
+		AB8500_VAUX12_REGU,      ab8500_rreg(dev, AB8500_VAUX12_REGU),
+		AB8500_VAUX1_SEL,        ab8500_rreg(dev, AB8500_VAUX1_SEL),
+		AB8500_DENC_CONF0,       ab8500_rreg(dev, AB8500_DENC_CONF0),
+		AB8500_DENC_CONF1,       ab8500_rreg(dev, AB8500_DENC_CONF1),
+		AB8500_DENC_CONF2,       ab8500_rreg(dev, AB8500_DENC_CONF2),
+		AB8500_DENC_CONF6,       ab8500_rreg(dev, AB8500_DENC_CONF6),
+		AB8500_DENC_CONF8,       ab8500_rreg(dev, AB8500_DENC_CONF8),
+		AB8500_TVOUT_CTRL,       ab8500_rreg(dev, AB8500_TVOUT_CTRL),
+		AB8500_TVOUT_CTRL2,      ab8500_rreg(dev, AB8500_TVOUT_CTRL2),
+		AB8500_IT_MASK1,         ab8500_rreg(dev, AB8500_IT_MASK1),
+		AB8500_GPIO_DIR5,        ab8500_rreg(dev, AB8500_GPIO_DIR5),
+		AB8500_GPIO_OUT5,        ab8500_rreg(dev, AB8500_GPIO_OUT5),
+		AB8500_GPIO_NPUD5,       ab8500_rreg(dev, AB8500_GPIO_NPUD5)
+	);
+	if (data_size >= DEBUG_BUF_SIZE) {
+		printk(KERN_EMERG "AB8500 DENC: Buffer overrun\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* check if read done */
+	if (*f_pos > data_size)
+		goto out;
+
+	if (*f_pos + count > data_size)
+		count = data_size - *f_pos;
+
+	if (copy_to_user(buf, buffer + *f_pos, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+out:
+	return ret;
+}
+#endif /* CONFIG_DEBUG_FS */
+
+/* Module init */
+static int __init ab8500_denc_init(void)
+{
+	return platform_driver_register(&ab8500_denc_driver);
+}
+module_init(ab8500_denc_init);
+
+static void __exit ab8500_denc_exit(void)
+{
+	platform_driver_unregister(&ab8500_denc_driver);
+}
+module_exit(ab8500_denc_exit);
+
+MODULE_AUTHOR("Marcel Tunnissen <marcel.tuennissen@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson AB8500 DENC driver");
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
index c39fc716e1d..d06f4826619 100644
--- a/drivers/mfd/ab8500-gpadc.c
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -12,6 +12,7 @@
 #include <linux/interrupt.h>
 #include <linux/spinlock.h>
 #include <linux/delay.h>
+#include <linux/pm_runtime.h>
 #include <linux/platform_device.h>
 #include <linux/completion.h>
 #include <linux/regulator/consumer.h>
@@ -82,6 +83,11 @@
 /* This is used to not lose precision when dividing to get gain and offset */
 #define CALIB_SCALE			1000
 
+/* Time in ms before disabling regulator */
+#define GPADC_AUDOSUSPEND_DELAY		1
+
+#define CONVERSION_TIME			500 /* ms */
+
 enum cal_channels {
 	ADC_INPUT_VMAIN = 0,
 	ADC_INPUT_BTEMP,
@@ -102,10 +108,10 @@ struct adc_cal_data {
 
 /**
  * struct ab8500_gpadc - AB8500 GPADC device information
- * @chip_id			ABB chip id
  * @dev:			pointer to the struct device
  * @node:			a list of AB8500 GPADCs, hence prepared for
 				reentrance
+ * @parent:			pointer to the struct ab8500
  * @ab8500_gpadc_complete:	pointer to the struct completion, to indicate
  *				the completion of gpadc conversion
  * @ab8500_gpadc_lock:		structure of type mutex
@@ -114,9 +120,9 @@ struct adc_cal_data {
  * @cal_data			array of ADC calibration data structs
  */
 struct ab8500_gpadc {
-	u8 chip_id;
 	struct device *dev;
 	struct list_head node;
+	struct ab8500 *parent;
 	struct completion ab8500_gpadc_complete;
 	struct mutex ab8500_gpadc_lock;
 	struct regulator *regu;
@@ -282,8 +288,9 @@ int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 		return -ENODEV;
 
 	mutex_lock(&gpadc->ab8500_gpadc_lock);
+
 	/* Enable VTVout LDO this is required for GPADC */
-	regulator_enable(gpadc->regu);
+	pm_runtime_get_sync(gpadc->dev);
 
 	/* Check if ADC is not busy, lock and proceed */
 	do {
@@ -332,7 +339,7 @@ int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 			EN_BUF | EN_ICHAR);
 		break;
 	case BTEMP_BALL:
-		if (gpadc->chip_id >= AB8500_CUT3P0) {
+		if (!is_ab8500_2p0_or_earlier(gpadc->parent)) {
 			/* Turn on btemp pull-up on ABB 3.0 */
 			ret = abx500_mask_and_set_register_interruptible(
 				gpadc->dev,
@@ -344,7 +351,7 @@ int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 		  * Delay might be needed for ABB8500 cut 3.0, if not, remove
 		  * when hardware will be availible
 		  */
-			msleep(1);
+			mdelay(1);
 			break;
 		}
 		/* Intentional fallthrough */
@@ -367,7 +374,8 @@ int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 		goto out;
 	}
 	/* wait for completion of conversion */
-	if (!wait_for_completion_timeout(&gpadc->ab8500_gpadc_complete, 2*HZ)) {
+	if (!wait_for_completion_timeout(&gpadc->ab8500_gpadc_complete,
+					 msecs_to_jiffies(CONVERSION_TIME))) {
 		dev_err(gpadc->dev,
 			"timeout: didn't receive GPADC conversion interrupt\n");
 		ret = -EINVAL;
@@ -397,8 +405,10 @@ int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel)
 		dev_err(gpadc->dev, "gpadc_conversion: disable gpadc failed\n");
 		goto out;
 	}
-	/* Disable VTVout LDO this is required for GPADC */
-	regulator_disable(gpadc->regu);
+
+	pm_runtime_mark_last_busy(gpadc->dev);
+	pm_runtime_put_autosuspend(gpadc->dev);
+
 	mutex_unlock(&gpadc->ab8500_gpadc_lock);
 
 	return (high_data << 8) | low_data;
@@ -412,7 +422,9 @@ out:
 	 */
 	(void) abx500_set_register_interruptible(gpadc->dev, AB8500_GPADC,
 		AB8500_GPADC_CTRL1_REG, DIS_GPADC);
-	regulator_disable(gpadc->regu);
+
+	pm_runtime_put(gpadc->dev);
+
 	mutex_unlock(&gpadc->ab8500_gpadc_lock);
 	dev_err(gpadc->dev,
 		"gpadc_conversion: Failed to AD convert channel %d\n", channel);
@@ -571,6 +583,28 @@ static void ab8500_gpadc_read_calibration_data(struct ab8500_gpadc *gpadc)
 		gpadc->cal_data[ADC_INPUT_VBAT].offset);
 }
 
+static int ab8500_gpadc_runtime_suspend(struct device *dev)
+{
+	struct ab8500_gpadc *gpadc = dev_get_drvdata(dev);
+
+	regulator_disable(gpadc->regu);
+	return 0;
+}
+
+static int ab8500_gpadc_runtime_resume(struct device *dev)
+{
+	struct ab8500_gpadc *gpadc = dev_get_drvdata(dev);
+
+	regulator_enable(gpadc->regu);
+	return 0;
+}
+
+static int ab8500_gpadc_runtime_idle(struct device *dev)
+{
+	pm_runtime_suspend(dev);
+	return 0;
+}
+
 static int __devinit ab8500_gpadc_probe(struct platform_device *pdev)
 {
 	int ret = 0;
@@ -591,6 +625,7 @@ static int __devinit ab8500_gpadc_probe(struct platform_device *pdev)
 	}
 
 	gpadc->dev = &pdev->dev;
+	gpadc->parent = dev_get_drvdata(pdev->dev.parent);
 	mutex_init(&gpadc->ab8500_gpadc_lock);
 
 	/* Initialize completion used to notify completion of conversion */
@@ -606,14 +641,6 @@ static int __devinit ab8500_gpadc_probe(struct platform_device *pdev)
 		goto fail;
 	}
 
-	/* Get Chip ID of the ABB ASIC  */
-	ret = abx500_get_chip_id(gpadc->dev);
-	if (ret < 0) {
-		dev_err(gpadc->dev, "failed to get chip ID\n");
-		goto fail_irq;
-	}
-	gpadc->chip_id = (u8) ret;
-
 	/* VTVout LDO used to power up ab8500-GPADC */
 	gpadc->regu = regulator_get(&pdev->dev, "vddadc");
 	if (IS_ERR(gpadc->regu)) {
@@ -621,6 +648,16 @@ static int __devinit ab8500_gpadc_probe(struct platform_device *pdev)
 		dev_err(gpadc->dev, "failed to get vtvout LDO\n");
 		goto fail_irq;
 	}
+
+	platform_set_drvdata(pdev, gpadc);
+
+	regulator_enable(gpadc->regu);
+
+	pm_runtime_set_autosuspend_delay(gpadc->dev, GPADC_AUDOSUSPEND_DELAY);
+	pm_runtime_use_autosuspend(gpadc->dev);
+	pm_runtime_set_active(gpadc->dev);
+	pm_runtime_enable(gpadc->dev);
+
 	ab8500_gpadc_read_calibration_data(gpadc);
 	list_add_tail(&gpadc->node, &ab8500_gpadc_list);
 	dev_dbg(gpadc->dev, "probe success\n");
@@ -641,19 +678,34 @@ static int __devexit ab8500_gpadc_remove(struct platform_device *pdev)
 	list_del(&gpadc->node);
 	/* remove interrupt  - completion of Sw ADC conversion */
 	free_irq(gpadc->irq, gpadc);
-	/* disable VTVout LDO that is being used by GPADC */
-	regulator_put(gpadc->regu);
+
+	pm_runtime_get_sync(gpadc->dev);
+	pm_runtime_disable(gpadc->dev);
+
+	regulator_disable(gpadc->regu);
+
+	pm_runtime_set_suspended(gpadc->dev);
+
+	pm_runtime_put_noidle(gpadc->dev);
+
 	kfree(gpadc);
 	gpadc = NULL;
 	return 0;
 }
 
+static const struct dev_pm_ops ab8500_gpadc_pm_ops = {
+	SET_RUNTIME_PM_OPS(ab8500_gpadc_runtime_suspend,
+			   ab8500_gpadc_runtime_resume,
+			   ab8500_gpadc_runtime_idle)
+};
+
 static struct platform_driver ab8500_gpadc_driver = {
 	.probe = ab8500_gpadc_probe,
 	.remove = __devexit_p(ab8500_gpadc_remove),
 	.driver = {
 		.name = "ab8500-gpadc",
 		.owner = THIS_MODULE,
+		.pm = &ab8500_gpadc_pm_ops,
 	},
 };
 
diff --git a/drivers/mfd/ab8500-i2c.c b/drivers/mfd/ab8500-i2c.c
index b83045f102b..5ee90fd125e 100644
--- a/drivers/mfd/ab8500-i2c.c
+++ b/drivers/mfd/ab8500-i2c.c
@@ -13,6 +13,7 @@
 #include <linux/mfd/abx500/ab8500.h>
 #include <linux/mfd/dbx500-prcmu.h>
 
+
 static int ab8500_i2c_write(struct ab8500 *ab8500, u16 addr, u8 data)
 {
 	int ret;
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
index c28d4eb1eff..d5865d41514 100644
--- a/drivers/mfd/ab8500-sysctrl.c
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -7,12 +7,114 @@
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/reboot.h>
+#include <linux/signal.h>
+#include <linux/power_supply.h>
 #include <linux/mfd/abx500.h>
 #include <linux/mfd/abx500/ab8500.h>
 #include <linux/mfd/abx500/ab8500-sysctrl.h>
+#include <linux/time.h>
+#include <linux/hwmon.h>
 
 static struct device *sysctrl_dev;
 
+void ab8500_power_off(void)
+{
+	struct ab8500_platform_data *plat;
+	struct timespec ts;
+	sigset_t old;
+	sigset_t all;
+	static char *pss[] = {"ab8500_ac", "ab8500_usb"};
+	int i;
+	bool charger_present = false;
+	union power_supply_propval val;
+	struct power_supply *psy;
+	int ret;
+
+	/*
+	 * If we have a charger connected and we're powering off,
+	 * reboot into charge-only mode.
+	 */
+
+	for (i = 0; i < ARRAY_SIZE(pss); i++) {
+		psy = power_supply_get_by_name(pss[i]);
+		if (!psy)
+			continue;
+
+		ret = psy->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &val);
+
+		if (!ret && val.intval) {
+			charger_present = true;
+			break;
+		}
+	}
+
+	if (!charger_present)
+		goto shutdown;
+
+	/* Check if battery is known */
+	psy = power_supply_get_by_name("ab8500_btemp");
+	if (psy) {
+		ret = psy->get_property(psy, POWER_SUPPLY_PROP_TECHNOLOGY,
+					&val);
+		if (!ret && val.intval != POWER_SUPPLY_TECHNOLOGY_UNKNOWN) {
+			printk(KERN_INFO
+			       "Charger \"%s\" is connected with known battery."
+			       " Rebooting.\n",
+			       pss[i]);
+			machine_restart("charging");
+		}
+	}
+
+shutdown:
+	sigfillset(&all);
+
+	plat = dev_get_platdata(sysctrl_dev->parent);
+		getnstimeofday(&ts);
+	if (!sigprocmask(SIG_BLOCK, &all, &old)) {
+		if (ts.tv_sec == 0 ||
+			(ts.tv_sec - plat->thermal_set_time_sec >
+				plat->thermal_time_out))
+			plat->thermal_power_off_pending = false;
+		if (!plat->thermal_power_off_pending) {
+			(void)ab8500_sysctrl_set(AB8500_STW4500CTRL1,
+				AB8500_STW4500CTRL1_SWOFF |
+				AB8500_STW4500CTRL1_SWRESET4500N);
+			(void)sigprocmask(SIG_SETMASK, &old, NULL);
+		} else {
+			(void)ab8500_sysctrl_set(AB8500_STW4500CTRL1,
+				AB8500_STW4500CTRL1_THDB8500SWOFF |
+				AB8500_STW4500CTRL1_SWRESET4500N);
+			(void)sigprocmask(SIG_SETMASK, &old, NULL);
+		}
+	}
+}
+
+static int ab8500_notifier_call(struct notifier_block *this,
+				unsigned long val, void *data)
+{
+	struct ab8500_platform_data *plat;
+	static struct timespec ts;
+	if (sysctrl_dev == NULL)
+		return -EAGAIN;
+
+	plat = dev_get_platdata(sysctrl_dev->parent);
+	if (val) {
+		getnstimeofday(&ts);
+		plat->thermal_set_time_sec = ts.tv_sec;
+		plat->thermal_power_off_pending = true;
+	} else {
+		plat->thermal_set_time_sec = 0;
+		plat->thermal_power_off_pending = false;
+	}
+	return 0;
+}
+
+static struct notifier_block ab8500_notifier = {
+	.notifier_call = ab8500_notifier_call,
+};
+
 static inline bool valid_bank(u8 bank)
 {
 	return ((bank == AB8500_SYS_CTRL1_BLOCK) ||
@@ -33,6 +135,7 @@ int ab8500_sysctrl_read(u16 reg, u8 *value)
 	return abx500_get_register_interruptible(sysctrl_dev, bank,
 		(u8)(reg & 0xFF), value);
 }
+EXPORT_SYMBOL(ab8500_sysctrl_read);
 
 int ab8500_sysctrl_write(u16 reg, u8 mask, u8 value)
 {
@@ -48,10 +151,42 @@ int ab8500_sysctrl_write(u16 reg, u8 mask, u8 value)
 	return abx500_mask_and_set_register_interruptible(sysctrl_dev, bank,
 		(u8)(reg & 0xFF), mask, value);
 }
+EXPORT_SYMBOL(ab8500_sysctrl_write);
 
 static int __devinit ab8500_sysctrl_probe(struct platform_device *pdev)
 {
+	struct ab8500_platform_data *plat;
+	struct ab8500_sysctrl_platform_data *pdata;
+
 	sysctrl_dev = &pdev->dev;
+	plat = dev_get_platdata(pdev->dev.parent);
+	if (plat->pm_power_off)
+		pm_power_off = ab8500_power_off;
+	hwmon_notifier_register(&ab8500_notifier);
+
+	pdata = plat->sysctrl;
+
+	if (pdata) {
+		int ret;
+		int i;
+		int j;
+		for (i = AB8500_SYSCLKREQ1RFCLKBUF;
+				i <= AB8500_SYSCLKREQ8RFCLKBUF; i++) {
+			j = i - AB8500_SYSCLKREQ1RFCLKBUF;
+			ret = ab8500_sysctrl_write(i, 0xff,
+					pdata->initial_req_buf_config[j]);
+			dev_dbg(&pdev->dev,
+					"Setting SysClkReq%dRfClkBuf 0x%X\n",
+					j + 1,
+					pdata->initial_req_buf_config[j]);
+			if (ret < 0) {
+				dev_err(&pdev->dev,
+					"unable to set sysClkReq%dRfClkBuf: "
+					"%d\n", j + 1, ret);
+			}
+		}
+	}
+
 	return 0;
 }
 
diff --git a/drivers/mfd/abx500-core.c b/drivers/mfd/abx500-core.c
index 7ce65f49480..9818afba251 100644
--- a/drivers/mfd/abx500-core.c
+++ b/drivers/mfd/abx500-core.c
@@ -153,6 +153,22 @@ int abx500_startup_irq_enabled(struct device *dev, unsigned int irq)
 }
 EXPORT_SYMBOL(abx500_startup_irq_enabled);
 
+void abx500_dump_all_banks(void)
+{
+	struct abx500_ops *ops;
+	struct device dummy_child = {0};
+	struct abx500_device_entry *dev_entry;
+
+	list_for_each_entry(dev_entry, &abx500_list, list) {
+		dummy_child.parent = dev_entry->dev;
+		ops = &dev_entry->ops;
+
+		if ((ops != NULL) && (ops->dump_all_banks != NULL))
+			ops->dump_all_banks(&dummy_child);
+	}
+}
+EXPORT_SYMBOL(abx500_dump_all_banks);
+
 MODULE_AUTHOR("Mattias Wallin <mattias.wallin@stericsson.com>");
 MODULE_DESCRIPTION("ABX500 core driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/db5500-prcmu-regs.h b/drivers/mfd/db5500-prcmu-regs.h
new file mode 100644
index 00000000000..0428b5e95ae
--- /dev/null
+++ b/drivers/mfd/db5500-prcmu-regs.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ */
+
+#ifndef __MACH_PRCMU_REGS_DB5500_H
+#define __MACH_PRCMU_REGS_DB5500_H
+
+#define BITS(_start, _end) ((BIT(_end) - BIT(_start)) + BIT(_end))
+
+#define PRCM_TCR		0x1C8
+#define PRCM_TCR_TENSEL_MASK	BITS(0, 7)
+#define PRCM_TCR_STOP_TIMERS	BIT(16)
+#define PRCM_TCR_DOZE_MODE	BIT(17)
+
+/* PRCMU HW semaphore */
+#define PRCM_SEM 0x400
+#define PRCM_SEM_PRCM_SEM BIT(0)
+
+#define DB5500_PRCM_ACLK_MGT		0x004
+#define DB5500_PRCM_SVACLK_MGT		0x008
+#define DB5500_PRCM_SIACLK_MGT		0x00C
+#define DB5500_PRCM_SGACLK_MGT		0x014
+#define DB5500_PRCM_UARTCLK_MGT		0x018
+#define DB5500_PRCM_MSP02CLK_MGT	0x01C
+#define DB5500_PRCM_I2CCLK_MGT		0x020
+#define DB5500_PRCM_SDMMCCLK_MGT	0x024
+#define DB5500_PRCM_PER1CLK_MGT		0x02C
+#define DB5500_PRCM_PER2CLK_MGT		0x030
+#define DB5500_PRCM_PER3CLK_MGT		0x034
+#define DB5500_PRCM_PER5CLK_MGT		0x038
+#define DB5500_PRCM_PER6CLK_MGT		0x03C
+#define DB5500_PRCM_IRDACLK_MGT		0x040
+#define DB5500_PRCM_PWMCLK_MGT		0x044
+#define DB5500_PRCM_SPARE1CLK_MGT	0x048
+#define DB5500_PRCM_IRRCCLK_MGT		0x04C
+#define DB5500_PRCM_HDMICLK_MGT		0x058
+#define DB5500_PRCM_APEATCLK_MGT	0x05C
+#define DB5500_PRCM_APETRACECLK_MGT	0x060
+#define DB5500_PRCM_MCDECLK_MGT		0x064
+#define DB5500_PRCM_DSIALTCLK_MGT	0x06C
+#define DB5500_PRCM_DMACLK_MGT		0x074
+#define DB5500_PRCM_B2R2CLK_MGT		0x078
+#define DB5500_PRCM_TVCLK_MGT		0x07C
+#define DB5500_PRCM_RNGCLK_MGT		0x284
+
+#define PRCM_CLK_MGT_CLKPLLDIV_MASK	BITS(0, 4)
+#define PRCM_CLK_MGT_CLKPLLDIV_SHIFT	0
+#define PRCM_CLK_MGT_CLKPLLSW_MASK	BITS(5, 7)
+#define PRCM_CLK_MGT_CLKEN		BIT(8)
+
+#define PRCM_ARM_IT1_CLEAR 0x48C
+#define PRCM_ARM_IT1_VAL 0x494
+
+/* CPU mailbox registers */
+#define PRCM_MBOX_CPU_VAL 0x0FC
+#define PRCM_MBOX_CPU_SET 0x100
+
+/* System reset register */
+#define PRCM_APE_SOFTRST 0x228
+
+/* PRCMU clock/PLL/reset registers */
+#define PRCM_PLLDSI_FREQ	0x500
+#define PRCM_PLLDSI_ENABLE	0x504
+#define PRCM_PLLDSI_LOCKP	0x508
+#define PRCM_DSI_PLLOUT_SEL	0x530
+#define PRCM_DSITVCLK_DIV	0x52C
+#define PRCM_APE_RESETN_SET	0x1E4
+#define PRCM_APE_RESETN_CLR	0x1E8
+
+/* CLKOUTx SEL0 settings */
+#define CLKOUT_SEL0_REF_CLK  0x01 /* 0b 0001 */
+#define CLKOUT_SEL0_RTC_CLK0 0x02 /* 0b 0010 */
+#define CLKOUT_SEL0_ULP_CLK  0x04 /* 0b 0100 */
+#define CLKOUT_SEL0_SEL_CLK  0x08 /* 0b 1000 */
+
+/* CLKOUTx SEL settings */
+#define CLKOUT_SEL_STATIC0     0x0001 /* 0b 00 0000 0001 */
+#define CLKOUT_SEL_REFCLK      0x0002 /* 0b 00 0000 0010 */
+#define CLKOUT_SEL_ULPCLK      0x0004 /* 0b 00 0000 0100 */
+#define CLKOUT_SEL_ARMCLK      0x0008 /* 0b 00 0000 1000 */
+#define CLKOUT_SEL_SYSACC0CLK  0x0010 /* 0b 00 0001 0000 */
+#define CLKOUT_SEL_SOC0PLLCLK  0x0020 /* 0b 00 0010 0000 */
+#define CLKOUT_SEL_SOC1PLLCLK  0x0040 /* 0b 00 0100 0000 */
+#define CLKOUT_SEL_DDRPLLCLK   0x0080 /* 0b 00 1000 0000 */
+#define CLKOUT_SEL_TVCLK       0x0100 /* 0b 01 0000 0000 */
+#define CLKOUT_SEL_IRDACLK     0x0200 /* 0b 10 0000 0000 */
+
+/* CLKOUTx dividers */
+#define CLKOUT_DIV_2   0x00 /* 0b 000 */
+#define CLKOUT_DIV_4   0x01 /* 0b 001 */
+#define CLKOUT_DIV_8   0x02 /* 0b 010 */
+#define CLKOUT_DIV_16  0x03 /* 0b 011 */
+#define CLKOUT_DIV_32  0x04 /* 0b 100 */
+#define CLKOUT_DIV_64  0x05 /* 0b 101 */
+/* Values 0x06 and 0x07 will also set the CLKOUTx divider to 64. */
+
+/* PRCM_CLKOCR CLKOUTx Control registers */
+#define PRCM_CLKOCR  0x1CC
+#define PRCM_CLKOCR_CLKOUT0_SEL0_SHIFT  0
+#define PRCM_CLKOCR_CLKOUT0_SEL0_MASK   BITS(0, 3)
+#define PRCM_CLKOCR_CLKOUT0_SEL_SHIFT   4
+#define PRCM_CLKOCR_CLKOUT0_SEL_MASK    BITS(4, 13)
+#define PRCM_CLKOCR_CLKOUT1_SEL0_SHIFT  16
+#define PRCM_CLKOCR_CLKOUT1_SEL0_MASK   BITS(16, 19)
+#define PRCM_CLKOCR_CLKOUT1_SEL_SHIFT   20
+#define PRCM_CLKOCR_CLKOUT1_SEL_MASK    BITS(20, 29)
+
+/* PRCM_CLKODIV CLKOUTx Dividers */
+#define PRCM_CLKODIV  0x188
+#define PRCM_CLKODIV_CLKOUT0_DIV_SHIFT  0
+#define PRCM_CLKODIV_CLKOUT0_DIV_MASK   BITS(0, 2)
+#define PRCM_CLKODIV_CLKOUT1_DIV_SHIFT  16
+#define PRCM_CLKODIV_CLKOUT1_DIV_MASK   BITS(16, 18)
+
+#define PRCM_MMIP_LS_CLAMP_SET 0x420
+#define PRCM_MMIP_LS_CLAMP_CLR 0x424
+#define PRCM_DDR_SUBSYS_APE_MINBW 0x438
+
+/* Miscellaneous unit registers */
+#define PRCM_DSI_SW_RESET 0x324
+#define PRCM_RESOUTN_SET_OFFSET 0x214
+#define PRCM_RESOUTN_CLR_OFFSET 0x218
+
+/* APE - Modem Registers */
+#define PRCM_HOSTACCESS_REQ	0x334
+/* APE - Modem register bit maipulation */
+#define PRCM_HOSTACCESS_REQ_BIT BIT(0)
+#define PRCM_APE_ACK		0x49c
+#define PRCM_APE_ACK_BIT	0x01
+
+/* Watchdog - mtimer registers */
+#define PRCM_TIMER0_RTOS_COMP1_OFFSET	0x4C
+#define PRCM_TIMER0_RTOS_COUNTER_OFFSET	0x40
+#define PRCM_TIMER0_IRQ_EN_SET_OFFSET	0x70
+#define PRCM_TIMER0_IRQ_EN_CLR_OFFSET	0x6C
+#define PRCM_TIMER0_IRQ_RTOS1_SET	0x08
+#define PRCM_TIMER0_IRQ_RTOS1_CLR	0x08
+
+#endif
diff --git a/drivers/mfd/db5500-prcmu.c b/drivers/mfd/db5500-prcmu.c
index bb115b2f04e..b106632d03c 100644
--- a/drivers/mfd/db5500-prcmu.c
+++ b/drivers/mfd/db5500-prcmu.c
@@ -19,12 +19,21 @@
 #include <linux/irq.h>
 #include <linux/jiffies.h>
 #include <linux/bitops.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/core.h>
+#include <linux/regulator/db5500-prcmu.h>
+#include <linux/regulator/machine.h>
 #include <linux/interrupt.h>
 #include <linux/mfd/dbx500-prcmu.h>
 #include <mach/hardware.h>
 #include <mach/irqs.h>
 #include <mach/db5500-regs.h>
-#include "dbx500-prcmu-regs.h"
+#include <mach/prcmu-debug.h>
+
+#include "db5500-prcmu-regs.h"
+
+#define PRCMU_FW_VERSION_OFFSET 0xA4
+#define PRCM_SW_RST_REASON (tcdm_base + 0xFF8) /* 2 bytes */
 
 #define _PRCM_MB_HEADER (tcdm_base + 0xFE8)
 #define PRCM_REQ_MB0_HEADER (_PRCM_MB_HEADER + 0x0)
@@ -64,6 +73,52 @@
 #define PRCM_ACK_MB6 (tcdm_base + 0xF0C)
 #define PRCM_ACK_MB7 (tcdm_base + 0xF08)
 
+/* Share info */
+#define PRCM_SHARE_INFO (tcdm_base + 0xEC8)
+
+#define PRCM_SHARE_INFO_HOTDOG (PRCM_SHARE_INFO + 62)
+
+/* Mailbox 0 REQs */
+#define PRCM_REQ_MB0_AP_POWER_STATE    (PRCM_REQ_MB0 + 0x0)
+#define PRCM_REQ_MB0_ULP_CLOCK_STATE   (PRCM_REQ_MB0 + 0x1)
+#define PRCM_REQ_MB0_AP_PLL_STATE      (PRCM_REQ_MB0 + 0x2)
+#define PRCM_REQ_MB0_DDR_STATE         (PRCM_REQ_MB0 + 0x3)
+#define PRCM_REQ_MB0_ESRAM0_STATE      (PRCM_REQ_MB0 + 0x4)
+#define PRCM_REQ_MB0_WAKEUP_DBB        (PRCM_REQ_MB0 + 0x8)
+#define PRCM_REQ_MB0_WAKEUP_ABB        (PRCM_REQ_MB0 + 0xC)
+
+/* Mailbox 0 ACKs */
+#define PRCM_ACK_MB0_AP_PWRSTTR_STATUS (PRCM_ACK_MB0 + 0x0)
+#define PRCM_ACK_MB0_READ_POINTER      (PRCM_ACK_MB0 + 0x1)
+#define PRCM_ACK_MB0_WAKEUP_0_DBB      (PRCM_ACK_MB0 + 0x4)
+#define PRCM_ACK_MB0_WAKEUP_0_ABB      (PRCM_ACK_MB0 + 0x8)
+#define PRCM_ACK_MB0_WAKEUP_1_DBB      (PRCM_ACK_MB0 + 0x28)
+#define PRCM_ACK_MB0_WAKEUP_1_ABB      (PRCM_ACK_MB0 + 0x2C)
+#define PRCM_ACK_MB0_EVENT_ABB_NUMBERS 20
+
+/* Request mailbox 1 fields. */
+#define PRCM_REQ_MB1_ARM_OPP			(PRCM_REQ_MB1 + 0x0)
+#define PRCM_REQ_MB1_APE_OPP			(PRCM_REQ_MB1 + 0x1)
+
+/* Mailbox 1 ACKs */
+#define PRCM_ACK_MB1_CURRENT_ARM_OPP	(PRCM_ACK_MB1 + 0x0)
+#define PRCM_ACK_MB1_CURRENT_APE_OPP	(PRCM_ACK_MB1 + 0x1)
+#define PRCM_ACK_MB1_ARM_VOLT_STATUS	(PRCM_ACK_MB1 + 0x2)
+#define PRCM_ACK_MB1_APE_VOLT_STATUS	(PRCM_ACK_MB1 + 0x3)
+
+/* Mailbox 2 REQs */
+#define PRCM_REQ_MB2_EPOD_CLIENT (PRCM_REQ_MB2 + 0x0)
+#define PRCM_REQ_MB2_EPOD_STATE  (PRCM_REQ_MB2 + 0x1)
+#define PRCM_REQ_MB2_CLK_CLIENT  (PRCM_REQ_MB2 + 0x2)
+#define PRCM_REQ_MB2_CLK_STATE   (PRCM_REQ_MB2 + 0x3)
+#define PRCM_REQ_MB2_PLL_CLIENT  (PRCM_REQ_MB2 + 0x4)
+#define PRCM_REQ_MB2_PLL_STATE   (PRCM_REQ_MB2 + 0x5)
+
+/* Mailbox 2 ACKs */
+#define PRCM_ACK_MB2_EPOD_STATUS (PRCM_ACK_MB2 + 0x2)
+#define PRCM_ACK_MB2_CLK_STATUS  (PRCM_ACK_MB2 + 0x6)
+#define PRCM_ACK_MB2_PLL_STATUS  (PRCM_ACK_MB2 + 0xA)
+
 enum mb_return_code {
 	RC_SUCCESS,
 	RC_FAIL,
@@ -71,12 +126,58 @@ enum mb_return_code {
 
 /* Mailbox 0 headers. */
 enum mb0_header {
-	/* request */
-	RMB0H_PWR_STATE_TRANS = 1,
-	RMB0H_WAKE_UP_CFG,
-	RMB0H_RD_WAKE_UP_ACK,
 	/* acknowledge */
-	AMB0H_WAKE_UP = 1,
+	MB0H_WAKE_UP = 0,
+	/* request */
+	MB0H_PWR_STATE_TRANS,
+	MB0H_WAKE_UP_CFG,
+	MB0H_RD_WAKE_UP_ACK,
+};
+
+/* Mailbox 1 headers.*/
+enum mb1_header {
+	MB1H_ARM_OPP = 1,
+	MB1H_APE_OPP,
+	MB1H_ARM_APE_OPP,
+};
+
+/* Mailbox 2 headers. */
+enum mb2_header {
+	MB2H_EPOD_REQUEST = 1,
+	MB2H_CLK_REQUEST,
+	MB2H_PLL_REQUEST,
+};
+
+/* Mailbox 3 headers. */
+enum mb3_header {
+	MB3H_REFCLK_REQUEST = 1,
+};
+
+enum sysclk_state {
+	SYSCLK_OFF,
+	SYSCLK_ON,
+};
+
+/* Mailbox 4 headers */
+enum mb4_header {
+	MB4H_CFG_HOTDOG = 7,
+	MB4H_CFG_HOTMON = 8,
+	MB4H_CFG_HOTPERIOD = 10,
+	MB4H_CGF_MODEM_RESET = 13,
+	MB4H_CGF_A9WDOG_EN_PREBARK = 14,
+	MB4H_CGF_A9WDOG_EN_NOPREBARK = 15,
+	MB4H_CGF_A9WDOG_DIS = 16,
+};
+
+/* Mailbox 4 ACK headers */
+enum mb4_ack_header {
+	MB4H_ACK_CFG_HOTDOG = 5,
+	MB4H_ACK_CFG_HOTMON = 6,
+	MB4H_ACK_CFG_HOTPERIOD = 8,
+	MB4H_ACK_CFG_MODEM_RESET = 11,
+	MB4H_ACK_CGF_A9WDOG_EN_PREBARK = 12,
+	MB4H_ACK_CGF_A9WDOG_EN_NOPREBARK = 13,
+	MB4H_ACK_CGF_A9WDOG_DIS = 14,
 };
 
 /* Mailbox 5 headers. */
@@ -85,6 +186,69 @@ enum mb5_header {
 	MB5H_I2C_READ,
 };
 
+enum db5500_arm_opp {
+	DB5500_ARM_100_OPP = 1,
+	DB5500_ARM_50_OPP,
+	DB5500_ARM_EXT_OPP,
+};
+
+enum db5500_ape_opp {
+	DB5500_APE_100_OPP = 1,
+	DB5500_APE_50_OPP
+};
+
+enum epod_state {
+	EPOD_OFF,
+	EPOD_ON,
+};
+enum epod_onoffret_state {
+	EPOD_OOR_OFF,
+	EPOD_OOR_RET,
+	EPOD_OOR_ON,
+};
+enum db5500_prcmu_pll {
+	DB5500_PLL_SOC0,
+	DB5500_PLL_SOC1,
+	DB5500_PLL_DDR,
+	DB5500_NUM_PLL_ID,
+};
+
+enum db5500_prcmu_clk {
+	DB5500_MSP1CLK,
+	DB5500_CDCLK,
+	DB5500_IRDACLK,
+	DB5500_TVCLK,
+	DB5500_NUM_CLK_CLIENTS,
+};
+
+enum on_off_ret {
+	OFF_ST,
+	RET_ST,
+	ON_ST,
+};
+
+enum db5500_ap_pwr_state {
+	DB5500_AP_SLEEP = 2,
+	DB5500_AP_DEEP_SLEEP,
+	DB5500_AP_IDLE,
+};
+
+/* Request mailbox 3 fields */
+#define PRCM_REQ_MB3_REFCLK_MGT		(PRCM_REQ_MB3 + 0x0)
+
+/* Ack. mailbox 3 fields */
+#define PRCM_ACK_MB3_REFCLK_REQ		(PRCM_ACK_MB3 + 0x0)
+
+
+/* Request mailbox 4 fields */
+#define PRCM_REQ_MB4_HOTDOG_THRESHOLD           (PRCM_REQ_MB4 + 32)
+#define PRCM_REQ_MB4_HOT_PERIOD                 (PRCM_REQ_MB4 + 34)
+#define PRCM_REQ_MB4_HOTMON_LOW                 (PRCM_REQ_MB4 + 36)
+#define PRCM_REQ_MB4_HOTMON_HIGH                (PRCM_REQ_MB4 + 38)
+
+/* Ack. mailbox 4 field */
+#define PRCM_ACK_MB4_REQUESTS                   (PRCM_ACK_MB4 + 0x0)
+
 /* Request mailbox 5 fields. */
 #define PRCM_REQ_MB5_I2C_SLAVE (PRCM_REQ_MB5 + 0)
 #define PRCM_REQ_MB5_I2C_REG (PRCM_REQ_MB5 + 1)
@@ -105,11 +269,12 @@ enum mb5_header {
 #define PRCMU_RESET_DSIPLL			0x00004000
 #define PRCMU_UNCLAMP_DSIPLL			0x00400800
 
-/* HDMI CLK MGT PLLSW=001 (PLLSOC0), PLLDIV=0x8, = 50 Mhz*/
-#define PRCMU_DSI_CLOCK_SETTING			0x00000128
+/* HDMI CLK MGT PLLSW=001 (PLLSOC0), PLLDIV=0xC, = 33.33 Mhz*/
+#define PRCMU_DSI_CLOCK_SETTING			0x0000012C
 /* TVCLK_MGT PLLSW=001 (PLLSOC0) PLLDIV=0x13, = 19.05 MHZ */
 #define PRCMU_DSI_LP_CLOCK_SETTING		0x00000135
-#define PRCMU_PLLDSI_FREQ_SETTING		0x00020121
+/* PRCM_PLLDSI_FREQ R=4, N=1, D= 0x65 */
+#define PRCMU_PLLDSI_FREQ_SETTING		0x00040165
 #define PRCMU_DSI_PLLOUT_SEL_SETTING		0x00000002
 #define PRCMU_ENABLE_ESCAPE_CLOCK_DIV		0x03000201
 #define PRCMU_DISABLE_ESCAPE_CLOCK_DIV		0x00000101
@@ -125,13 +290,176 @@ enum mb5_header {
 #define PRCMU_PLLDSI_LOCKP_LOCKED		0x3
 
 /*
+ * Wakeups/IRQs
+ */
+
+#define WAKEUP_BIT_RTC BIT(0)
+#define WAKEUP_BIT_RTT0 BIT(1)
+#define WAKEUP_BIT_RTT1 BIT(2)
+#define WAKEUP_BIT_CD_IRQ BIT(3)
+#define WAKEUP_BIT_SRP_TIM BIT(4)
+#define WAKEUP_BIT_APE_REQ BIT(5)
+#define WAKEUP_BIT_USB BIT(6)
+#define WAKEUP_BIT_ABB BIT(7)
+#define WAKEUP_BIT_LOW_POWER_AUDIO BIT(8)
+#define WAKEUP_BIT_TEMP_SENSOR_LOW BIT(9)
+#define WAKEUP_BIT_ARM BIT(10)
+#define WAKEUP_BIT_AC_WAKE_ACK BIT(11)
+#define WAKEUP_BIT_TEMP_SENSOR_HIGH BIT(12)
+#define WAKEUP_BIT_MODEM_SW_RESET_REQ BIT(20)
+#define WAKEUP_BIT_GPIO0 BIT(23)
+#define WAKEUP_BIT_GPIO1 BIT(24)
+#define WAKEUP_BIT_GPIO2 BIT(25)
+#define WAKEUP_BIT_GPIO3 BIT(26)
+#define WAKEUP_BIT_GPIO4 BIT(27)
+#define WAKEUP_BIT_GPIO5 BIT(28)
+#define WAKEUP_BIT_GPIO6 BIT(29)
+#define WAKEUP_BIT_GPIO7 BIT(30)
+#define WAKEUP_BIT_AC_REL_ACK BIT(30)
+
+/*
+ * This vector maps irq numbers to the bits in the bit field used in
+ * communication with the PRCMU firmware.
+ *
+ * The reason for having this is to keep the irq numbers contiguous even though
+ * the bits in the bit field are not. (The bits also have a tendency to move
+ * around, to further complicate matters.)
+ */
+#define IRQ_INDEX(_name) ((IRQ_DB5500_PRCMU_##_name) - IRQ_DB5500_PRCMU_BASE)
+#define IRQ_ENTRY(_name)[IRQ_INDEX(_name)] = (WAKEUP_BIT_##_name)
+static u32 prcmu_irq_bit[NUM_DB5500_PRCMU_WAKEUPS] = {
+	IRQ_ENTRY(RTC),
+	IRQ_ENTRY(RTT0),
+	IRQ_ENTRY(RTT1),
+	IRQ_ENTRY(CD_IRQ),
+	IRQ_ENTRY(SRP_TIM),
+	IRQ_ENTRY(APE_REQ),
+	IRQ_ENTRY(USB),
+	IRQ_ENTRY(ABB),
+	IRQ_ENTRY(LOW_POWER_AUDIO),
+	IRQ_ENTRY(TEMP_SENSOR_LOW),
+	IRQ_ENTRY(TEMP_SENSOR_HIGH),
+	IRQ_ENTRY(ARM),
+	IRQ_ENTRY(AC_WAKE_ACK),
+	IRQ_ENTRY(MODEM_SW_RESET_REQ),
+	IRQ_ENTRY(GPIO0),
+	IRQ_ENTRY(GPIO1),
+	IRQ_ENTRY(GPIO2),
+	IRQ_ENTRY(GPIO3),
+	IRQ_ENTRY(GPIO4),
+	IRQ_ENTRY(GPIO5),
+	IRQ_ENTRY(GPIO6),
+	IRQ_ENTRY(GPIO7),
+	IRQ_ENTRY(AC_REL_ACK),
+};
+
+#define VALID_WAKEUPS (BIT(NUM_PRCMU_WAKEUP_INDICES) - 1)
+#define WAKEUP_ENTRY(_name)[PRCMU_WAKEUP_INDEX_##_name] = (WAKEUP_BIT_##_name)
+static u32 prcmu_wakeup_bit[NUM_PRCMU_WAKEUP_INDICES] = {
+	WAKEUP_ENTRY(RTC),
+	WAKEUP_ENTRY(RTT0),
+	WAKEUP_ENTRY(RTT1),
+	WAKEUP_ENTRY(CD_IRQ),
+	WAKEUP_ENTRY(USB),
+	WAKEUP_ENTRY(ABB),
+	WAKEUP_ENTRY(ARM)
+};
+
+/*
  * mb0_transfer - state needed for mailbox 0 communication.
- * @lock:		The transaction lock.
+ * @lock                The transaction lock.
+ * @dbb_irqs_lock       lock used for (un)masking DBB wakeup interrupts
+ * @mask_work:          Work structure used for (un)masking wakeup interrupts.
+ * @ac_wake_lock:	mutex to lock modem_req and modem_rel
+ * @req:                Request data that need to persist between requests.
  */
 static struct {
 	spinlock_t lock;
+	spinlock_t dbb_irqs_lock;
+	struct work_struct mask_work;
+	struct mutex ac_wake_lock;
+	struct {
+		u32 dbb_irqs;
+		u32 dbb_wakeups;
+		u32 abb_events;
+	} req;
 } mb0_transfer;
 
+
+/*
+ * mb1_transfer - state needed for mailbox 1 communication.
+ * @lock:	The transaction lock.
+ * @work:	The transaction completion structure.
+ * @req_arm_opp Requested arm opp
+ * @req_ape_opp Requested ape opp
+ * @ack:	Reply ("acknowledge") data.
+ */
+static struct {
+	struct mutex lock;
+	struct completion work;
+	u8 req_arm_opp;
+	u8 req_ape_opp;
+	struct {
+		u8 header;
+		u8 arm_opp;
+		u8 ape_opp;
+		u8 arm_voltage_st;
+		u8 ape_voltage_st;
+	} ack;
+} mb1_transfer;
+
+/*
+ * mb2_transfer - state needed for mailbox 2 communication.
+ * @lock:      The transaction lock.
+ * @work:      The transaction completion structure.
+ * @req:       Request data that need to persist between requests.
+ * @ack:       Reply ("acknowledge") data.
+ */
+static struct {
+	struct mutex lock;
+	struct completion work;
+	struct {
+		u8 epod_st[DB5500_NUM_EPOD_ID];
+		u8 pll_st[DB5500_NUM_PLL_ID];
+	} req;
+	struct {
+		u8 header;
+		u8 status;
+	} ack;
+} mb2_transfer;
+
+/*
+ * mb3_transfer - state needed for mailbox 3 communication.
+ * @sysclk_lock:	A lock used to handle concurrent sysclk requests.
+ * @sysclk_work:	Work structure used for sysclk requests.
+ * @req_st:		Requested clock state.
+ * @ack:		Acknowledgement data
+ */
+static struct {
+	struct mutex sysclk_lock;
+	struct completion sysclk_work;
+	enum sysclk_state req_st;
+	struct {
+		u8 header;
+		u8 status;
+	} ack;
+} mb3_transfer;
+
+/*
+ * mb4_transfer - state needed for mailbox 4 communication.
+ * @lock:       The transaction lock.
+ * @work:       The transaction completion structure.
+ * @ack:        Acknowledgement data
+ */
+static struct {
+	struct mutex lock;
+	struct completion work;
+	struct {
+		u8 header;
+		u8 status;
+	} ack;
+} mb4_transfer;
+
 /*
  * mb5_transfer - state needed for mailbox 5 communication.
  * @lock:	The transaction lock.
@@ -148,9 +476,825 @@ static struct {
 	} ack;
 } mb5_transfer;
 
-/* PRCMU TCDM base IO address. */
+/* Spinlocks */
+static DEFINE_SPINLOCK(clkout_lock);
+
+/* PRCMU TCDM base IO address */
 static __iomem void *tcdm_base;
 
+/* PRCMU MTIMER base IO address */
+static __iomem void *mtimer_base;
+
+struct clk_mgt {
+	unsigned int offset;
+	u32 pllsw;
+	u32 div;
+	bool scalable;
+	bool force50;
+};
+
+/* PRCMU Firmware Details */
+static struct {
+	u16 board;
+	u8 fw_version;
+	u8 api_version;
+} prcmu_version;
+
+static struct {
+	u32 timeout;
+	bool enabled;
+} a9wdog_timer;
+
+static DEFINE_SPINLOCK(clk_mgt_lock);
+
+#define CLK_MGT_ENTRY(_name, _scalable)[PRCMU_##_name] = {	\
+	.offset = DB5500_PRCM_##_name##_MGT,			\
+	.scalable = _scalable,					\
+}
+
+static struct clk_mgt clk_mgt[PRCMU_NUM_REG_CLOCKS] = {
+	CLK_MGT_ENTRY(SGACLK, true),
+	CLK_MGT_ENTRY(UARTCLK, false),
+	CLK_MGT_ENTRY(MSP02CLK, false),
+	CLK_MGT_ENTRY(I2CCLK, false),
+	[PRCMU_SDMMCCLK] {
+		.offset		= DB5500_PRCM_SDMMCCLK_MGT,
+		.force50	= true,
+		.scalable	= false,
+
+	},
+	[PRCMU_SPARE1CLK] {
+		.offset		= DB5500_PRCM_SPARE1CLK_MGT,
+		.force50	= true,
+		.scalable	= false,
+
+	},
+	CLK_MGT_ENTRY(PER1CLK, false),
+	CLK_MGT_ENTRY(PER2CLK, true),
+	CLK_MGT_ENTRY(PER3CLK, true),
+	CLK_MGT_ENTRY(PER5CLK, false),	/* used for SPI */
+	CLK_MGT_ENTRY(PER6CLK, true),
+	CLK_MGT_ENTRY(PWMCLK, false),
+	CLK_MGT_ENTRY(IRDACLK, false),
+	CLK_MGT_ENTRY(IRRCCLK, false),
+	CLK_MGT_ENTRY(HDMICLK, false),
+	CLK_MGT_ENTRY(APEATCLK, false),
+	CLK_MGT_ENTRY(APETRACECLK, true),
+	CLK_MGT_ENTRY(MCDECLK, true),
+	CLK_MGT_ENTRY(DSIALTCLK, false),
+	CLK_MGT_ENTRY(DMACLK, true),
+	CLK_MGT_ENTRY(B2R2CLK, true),
+	CLK_MGT_ENTRY(TVCLK, false),
+	CLK_MGT_ENTRY(RNGCLK, false),
+	CLK_MGT_ENTRY(SIACLK, false),
+	CLK_MGT_ENTRY(SVACLK, false),
+	CLK_MGT_ENTRY(ACLK, true),
+};
+
+static atomic_t modem_req_state = ATOMIC_INIT(0);
+
+bool db5500_prcmu_is_modem_requested(void)
+{
+	return (atomic_read(&modem_req_state) != 0);
+}
+
+/**
+ * prcmu_modem_req - APE requests Modem to wake up
+ *
+ * Whenever APE wants to send message to the modem, it will have to call this
+ * function to make sure that modem is awake.
+ */
+void prcmu_modem_req(void)
+{
+	u32 val;
+
+	mutex_lock(&mb0_transfer.ac_wake_lock);
+
+	val = readl(_PRCMU_BASE + PRCM_HOSTACCESS_REQ);
+	if (val & PRCM_HOSTACCESS_REQ_BIT)
+		goto unlock_and_return;
+
+	writel((val | PRCM_HOSTACCESS_REQ_BIT),
+		(_PRCMU_BASE + PRCM_HOSTACCESS_REQ));
+	atomic_set(&modem_req_state, 1);
+
+unlock_and_return:
+	mutex_unlock(&mb0_transfer.ac_wake_lock);
+
+}
+
+/**
+ * prcmu_modem_rel - APE has no more messages to send and hence releases modem.
+ *
+ * APE to Modem communication is initiated by modem_req and once the
+ * communication is completed, APE sends modem_rel to complete the protocol.
+ */
+void prcmu_modem_rel(void)
+{
+	u32 val;
+
+	mutex_lock(&mb0_transfer.ac_wake_lock);
+
+	val = readl(_PRCMU_BASE + PRCM_HOSTACCESS_REQ);
+	if (!(val & PRCM_HOSTACCESS_REQ_BIT))
+		goto unlock_and_return;
+
+	writel((val & ~PRCM_HOSTACCESS_REQ_BIT),
+		(_PRCMU_BASE + PRCM_HOSTACCESS_REQ));
+
+	atomic_set(&modem_req_state, 0);
+
+unlock_and_return:
+	mutex_unlock(&mb0_transfer.ac_wake_lock);
+}
+
+/**
+ * prcm_ape_ack - send an acknowledgement to modem
+ *
+ * On ape receiving ape_req, APE will have to acknowledge for the interrupt
+ * received. This function will send the acknowledgement by writing to the
+ * prcmu register and an interrupt is trigerred to modem.
+ */
+void prcmu_ape_ack(void)
+{
+	writel(PRCM_APE_ACK_BIT, (_PRCMU_BASE + PRCM_APE_ACK));
+}
+
+/**
+ * db5500_prcmu_modem_reset - Assert a Reset on modem
+ *
+ * This function will assert a reset request to the modem. Prior to that
+ * PRCM_HOSTACCESS_REQ must be '0'.
+ */
+void db5500_prcmu_modem_reset(void)
+{
+	mutex_lock(&mb4_transfer.lock);
+
+	/* PRCM_HOSTACCESS_REQ = 0, before asserting a reset */
+	prcmu_modem_rel();
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+		cpu_relax();
+
+	writeb(MB4H_CGF_MODEM_RESET, PRCM_REQ_MB4_HEADER);
+	writel(MBOX_BIT(4), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
+	wait_for_completion(&mb4_transfer.work);
+	if (mb4_transfer.ack.status != RC_SUCCESS ||
+			mb4_transfer.ack.header != MB4H_CGF_MODEM_RESET)
+		printk(KERN_ERR,
+				"ACK not received for modem reset interrupt\n");
+	mutex_unlock(&mb4_transfer.lock);
+}
+
+/**
+ * prcmu_config_clkout - Configure one of the programmable clock outputs.
+ * @clkout:	The CLKOUT number (0 or 1).
+ * @source:	Clock source.
+ * @div:	The divider to be applied.
+ *
+ * Configures one of the programmable clock outputs (CLKOUTs).
+ */
+int prcmu_config_clkout(u8 clkout, u8 source, u8 div)
+{
+	static bool configured[2] = {false, false};
+	int r = 0;
+	unsigned long flags;
+	u32 sel_val;
+	u32 div_val;
+	u32 sel_bits;
+	u32 div_bits;
+	u32 sel_mask;
+	u32 div_mask;
+	u8 sel0 = CLKOUT_SEL0_SEL_CLK;
+	u16 sel = 0;
+
+	BUG_ON(clkout > DB5500_CLKOUT1);
+	BUG_ON(source > DB5500_CLKOUT_IRDACLK);
+	BUG_ON(div > 7);
+
+	switch (source) {
+	case DB5500_CLKOUT_REF_CLK_SEL0:
+		sel0 = CLKOUT_SEL0_REF_CLK;
+		break;
+	case DB5500_CLKOUT_RTC_CLK0_SEL0:
+		sel0 = CLKOUT_SEL0_RTC_CLK0;
+		break;
+	case DB5500_CLKOUT_ULP_CLK_SEL0:
+		sel0 = CLKOUT_SEL0_ULP_CLK;
+		break;
+	case DB5500_CLKOUT_STATIC0:
+		sel = CLKOUT_SEL_STATIC0;
+		break;
+	case DB5500_CLKOUT_REFCLK:
+		sel = CLKOUT_SEL_REFCLK;
+		break;
+	case DB5500_CLKOUT_ULPCLK:
+		sel = CLKOUT_SEL_ULPCLK;
+		break;
+	case DB5500_CLKOUT_ARMCLK:
+		sel = CLKOUT_SEL_ARMCLK;
+		break;
+	case DB5500_CLKOUT_SYSACC0CLK:
+		sel = CLKOUT_SEL_SYSACC0CLK;
+		break;
+	case DB5500_CLKOUT_SOC0PLLCLK:
+		sel = CLKOUT_SEL_SOC0PLLCLK;
+		break;
+	case DB5500_CLKOUT_SOC1PLLCLK:
+		sel = CLKOUT_SEL_SOC1PLLCLK;
+		break;
+	case DB5500_CLKOUT_DDRPLLCLK:
+		sel = CLKOUT_SEL_DDRPLLCLK;
+		break;
+	case DB5500_CLKOUT_TVCLK:
+		sel = CLKOUT_SEL_TVCLK;
+		break;
+	case DB5500_CLKOUT_IRDACLK:
+		sel = CLKOUT_SEL_IRDACLK;
+		break;
+	}
+
+	switch (clkout) {
+	case DB5500_CLKOUT0:
+		sel_mask = PRCM_CLKOCR_CLKOUT0_SEL0_MASK |
+			PRCM_CLKOCR_CLKOUT0_SEL_MASK;
+		sel_bits = ((sel0 << PRCM_CLKOCR_CLKOUT0_SEL0_SHIFT) |
+			(sel << PRCM_CLKOCR_CLKOUT0_SEL_SHIFT));
+		div_mask = PRCM_CLKODIV_CLKOUT0_DIV_MASK;
+		div_bits = div << PRCM_CLKODIV_CLKOUT0_DIV_SHIFT;
+		break;
+	case DB5500_CLKOUT1:
+		sel_mask = PRCM_CLKOCR_CLKOUT1_SEL0_MASK |
+			PRCM_CLKOCR_CLKOUT1_SEL_MASK;
+		sel_bits = ((sel0 << PRCM_CLKOCR_CLKOUT1_SEL0_SHIFT) |
+			(sel << PRCM_CLKOCR_CLKOUT1_SEL_SHIFT));
+		div_mask = PRCM_CLKODIV_CLKOUT1_DIV_MASK;
+		div_bits = div << PRCM_CLKODIV_CLKOUT1_DIV_SHIFT;
+		break;
+	}
+
+	spin_lock_irqsave(&clkout_lock, flags);
+
+	if (configured[clkout]) {
+		r = -EINVAL;
+		goto unlock_and_return;
+	}
+
+	sel_val = readl(_PRCMU_BASE + PRCM_CLKOCR);
+	writel((sel_bits | (sel_val & ~sel_mask)),
+		(_PRCMU_BASE + PRCM_CLKOCR));
+
+	div_val = readl(_PRCMU_BASE + PRCM_CLKODIV);
+	writel((div_bits | (div_val & ~div_mask)),
+		(_PRCMU_BASE + PRCM_CLKODIV));
+
+	configured[clkout] = true;
+
+unlock_and_return:
+	spin_unlock_irqrestore(&clkout_lock, flags);
+
+	return r;
+}
+
+static int request_sysclk(bool enable)
+{
+	int r;
+
+	r = 0;
+	mutex_lock(&mb3_transfer.sysclk_lock);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(3))
+		cpu_relax();
+
+	if (enable)
+		mb3_transfer.req_st = SYSCLK_ON;
+	else
+		mb3_transfer.req_st = SYSCLK_OFF;
+
+	writeb(mb3_transfer.req_st, (PRCM_REQ_MB3_REFCLK_MGT));
+
+	writeb(MB3H_REFCLK_REQUEST, (PRCM_REQ_MB3_HEADER));
+	writel(MBOX_BIT(3), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
+
+	/*
+	 * The firmware only sends an ACK if we want to enable the
+	 * SysClk, and it succeeds.
+	 */
+	if (!wait_for_completion_timeout(&mb3_transfer.sysclk_work,
+			msecs_to_jiffies(20000))) {
+		pr_err("prcmu: %s timed out (20 s) waiting for a reply.\n",
+			__func__);
+		r = -EIO;
+		WARN(1, "Failed to set sysclk");
+		goto unlock_and_return;
+	}
+
+	if ((mb3_transfer.ack.header != MB3H_REFCLK_REQUEST) ||
+			(mb3_transfer.ack.status != mb3_transfer.req_st)) {
+		r = -EIO;
+	}
+
+unlock_and_return:
+	mutex_unlock(&mb3_transfer.sysclk_lock);
+
+	return r;
+}
+
+static int request_timclk(bool enable)
+{
+	u32 val = (PRCM_TCR_DOZE_MODE | PRCM_TCR_TENSEL_MASK);
+
+	if (!enable)
+		val |= PRCM_TCR_STOP_TIMERS;
+	writel(val, _PRCMU_BASE + PRCM_TCR);
+
+	return 0;
+}
+
+static int request_clk(u8 clock, bool enable)
+{
+	int r = 0;
+
+	BUG_ON(clock >= DB5500_NUM_CLK_CLIENTS);
+
+	mutex_lock(&mb2_transfer.lock);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(2))
+		cpu_relax();
+
+	/* fill in mailbox */
+	writeb(clock, PRCM_REQ_MB2_CLK_CLIENT);
+	writeb(enable, PRCM_REQ_MB2_CLK_STATE);
+
+	writeb(MB2H_CLK_REQUEST, PRCM_REQ_MB2_HEADER);
+
+	writel(MBOX_BIT(2), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
+	if (!wait_for_completion_timeout(&mb2_transfer.work,
+		msecs_to_jiffies(20000))) {
+		pr_err("prcmu: request_clk() failed.\n");
+		r = -EIO;
+		WARN(1, "Failed in request_clk");
+		goto unlock_and_return;
+	}
+	if (mb2_transfer.ack.status != RC_SUCCESS ||
+		mb2_transfer.ack.header != MB2H_CLK_REQUEST)
+		r = -EIO;
+
+unlock_and_return:
+	mutex_unlock(&mb2_transfer.lock);
+	return r;
+}
+
+static int request_reg_clock(u8 clock, bool enable)
+{
+	u32 val;
+	unsigned long flags;
+
+	WARN_ON(!clk_mgt[clock].offset);
+
+	spin_lock_irqsave(&clk_mgt_lock, flags);
+
+	/* Grab the HW semaphore. */
+	while ((readl(_PRCMU_BASE + PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
+		cpu_relax();
+
+	val = readl(_PRCMU_BASE + clk_mgt[clock].offset);
+	if (enable) {
+		val |= (PRCM_CLK_MGT_CLKEN | clk_mgt[clock].pllsw);
+	} else {
+		clk_mgt[clock].pllsw = (val & PRCM_CLK_MGT_CLKPLLSW_MASK);
+		val &= ~(PRCM_CLK_MGT_CLKEN | PRCM_CLK_MGT_CLKPLLSW_MASK);
+	}
+	writel(val, (_PRCMU_BASE + clk_mgt[clock].offset));
+
+	/* Release the HW semaphore. */
+	writel(0, _PRCMU_BASE + PRCM_SEM);
+
+	spin_unlock_irqrestore(&clk_mgt_lock, flags);
+
+	return 0;
+}
+
+/*
+ * request_pll() - Request for a pll to be enabled or disabled.
+ * @pll:        The pll for which the request is made.
+ * @enable:     Whether the clock should be enabled (true) or disabled (false).
+ *
+ * This function should only be used by the clock implementation.
+ * Do not use it from any other place!
+ */
+static int request_pll(u8 pll, bool enable)
+{
+	int r = 0;
+
+	BUG_ON(pll >= DB5500_NUM_PLL_ID);
+	mutex_lock(&mb2_transfer.lock);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(2))
+		cpu_relax();
+
+	mb2_transfer.req.pll_st[pll] = enable;
+
+	/* fill in mailbox */
+	writeb(pll, PRCM_REQ_MB2_PLL_CLIENT);
+	writeb(mb2_transfer.req.pll_st[pll], PRCM_REQ_MB2_PLL_STATE);
+
+	writeb(MB2H_PLL_REQUEST, PRCM_REQ_MB2_HEADER);
+
+	writel(MBOX_BIT(2), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
+	if (!wait_for_completion_timeout(&mb2_transfer.work,
+		msecs_to_jiffies(20000))) {
+		pr_err("prcmu: set_pll() failed.\n");
+		r = -EIO;
+		WARN(1, "Failed to set pll");
+		goto unlock_and_return;
+	}
+	if (mb2_transfer.ack.status != RC_SUCCESS ||
+		mb2_transfer.ack.header != MB2H_PLL_REQUEST)
+		r = -EIO;
+
+unlock_and_return:
+	mutex_unlock(&mb2_transfer.lock);
+
+	return r;
+}
+
+/**
+ * db5500_prcmu_request_clock() - Request for a clock to be enabled or disabled.
+ * @clock:      The clock for which the request is made.
+ * @enable:     Whether the clock should be enabled (true) or disabled (false).
+ *
+ * This function should only be used by the clock implementation.
+ * Do not use it from any other place!
+ */
+int db5500_prcmu_request_clock(u8 clock, bool enable)
+{
+	/* MSP1 & CD clocks are handled by FW */
+	if (clock == PRCMU_MSP1CLK)
+		return request_clk(DB5500_MSP1CLK, enable);
+	else if (clock == PRCMU_CDCLK)
+		return request_clk(DB5500_CDCLK, enable);
+	else if (clock == PRCMU_IRDACLK)
+		return request_clk(DB5500_IRDACLK, enable);
+	else if (clock < PRCMU_NUM_REG_CLOCKS)
+		return request_reg_clock(clock, enable);
+	else if (clock == PRCMU_TIMCLK)
+		return request_timclk(enable);
+	else if (clock == PRCMU_PLLSOC0)
+		return request_pll(DB5500_PLL_SOC0, enable);
+	else if (clock == PRCMU_PLLSOC1)
+		return request_pll(DB5500_PLL_SOC1, enable);
+	else if (clock == PRCMU_PLLDDR)
+		return request_pll(DB5500_PLL_DDR, enable);
+	else if (clock == PRCMU_SYSCLK)
+		return request_sysclk(enable);
+	else
+		return -EINVAL;
+}
+
+/* This function should only be called while mb0_transfer.lock is held. */
+static void config_wakeups(void)
+{
+	static u32 last_dbb_events;
+	static u32 last_abb_events;
+	u32 dbb_events;
+	u32 abb_events;
+
+	dbb_events = mb0_transfer.req.dbb_irqs | mb0_transfer.req.dbb_wakeups;
+
+	abb_events = mb0_transfer.req.abb_events;
+
+	if ((dbb_events == last_dbb_events) && (abb_events == last_abb_events))
+		return;
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
+		cpu_relax();
+
+	writel(dbb_events, PRCM_REQ_MB0_WAKEUP_DBB);
+	writel(abb_events, PRCM_REQ_MB0_WAKEUP_ABB);
+	writeb(MB0H_WAKE_UP_CFG, PRCM_REQ_MB0_HEADER);
+	writel(MBOX_BIT(0), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
+
+	last_dbb_events = dbb_events;
+	last_abb_events = abb_events;
+}
+
+int db5500_prcmu_config_esram0_deep_sleep(u8 state)
+{
+	unsigned long flags;
+
+	if ((state > ESRAM0_DEEP_SLEEP_STATE_RET) ||
+	    (state < ESRAM0_DEEP_SLEEP_STATE_OFF))
+		return -EINVAL;
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	if (state == ESRAM0_DEEP_SLEEP_STATE_RET)
+		writeb(RET_ST, PRCM_REQ_MB0_ESRAM0_STATE);
+	else
+		writeb(OFF_ST, PRCM_REQ_MB0_ESRAM0_STATE);
+
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+
+	return 0;
+}
+
+int db5500_prcmu_set_power_state(u8 state, bool keep_ulp_clk, bool keep_ap_pll)
+{
+	int r = 0;
+	unsigned long flags;
+
+	/* Deep Idle is not supported in DB5500 */
+	BUG_ON((state < PRCMU_AP_SLEEP) || (state >= PRCMU_AP_DEEP_IDLE));
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
+		cpu_relax();
+
+	switch (state) {
+	case PRCMU_AP_IDLE:
+		writeb(DB5500_AP_IDLE, PRCM_REQ_MB0_AP_POWER_STATE);
+		/* TODO: Can be high latency */
+		writeb(DDR_PWR_STATE_UNCHANGED, PRCM_REQ_MB0_DDR_STATE);
+		break;
+	case PRCMU_AP_SLEEP:
+		writeb(DB5500_AP_SLEEP, PRCM_REQ_MB0_AP_POWER_STATE);
+		break;
+	case PRCMU_AP_DEEP_SLEEP:
+		writeb(DB5500_AP_DEEP_SLEEP, PRCM_REQ_MB0_AP_POWER_STATE);
+		break;
+	default:
+		r = -EINVAL;
+		goto unlock_return;
+	}
+	writeb((keep_ap_pll ? 1 : 0), PRCM_REQ_MB0_AP_PLL_STATE);
+	writeb((keep_ulp_clk ? 1 : 0), PRCM_REQ_MB0_ULP_CLOCK_STATE);
+
+	writeb(MB0H_PWR_STATE_TRANS, PRCM_REQ_MB0_HEADER);
+	writel(MBOX_BIT(0), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
+
+unlock_return:
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+
+	return r;
+}
+
+u8 db5500_prcmu_get_power_state_result(void)
+{
+	u8 status = readb_relaxed(PRCM_ACK_MB0_AP_PWRSTTR_STATUS);
+
+	/*
+	 * Callers expect all the status values to match 8500.  Adjust for
+	 * PendingReq_Er (0x2b).
+	 */
+	if (status == 0x2b)
+		status = PRCMU_PRCMU2ARMPENDINGIT_ER;
+
+	return status;
+}
+
+void db5500_prcmu_enable_wakeups(u32 wakeups)
+{
+	unsigned long flags;
+	u32 bits;
+	int i;
+
+	BUG_ON(wakeups != (wakeups & VALID_WAKEUPS));
+
+	for (i = 0, bits = 0; i < NUM_PRCMU_WAKEUP_INDICES; i++) {
+		if (wakeups & BIT(i)) {
+			if (prcmu_wakeup_bit[i] == 0)
+				WARN(1, "WAKEUP NOT SUPPORTED");
+			else
+				bits |= prcmu_wakeup_bit[i];
+		}
+	}
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	mb0_transfer.req.dbb_wakeups = bits;
+	config_wakeups();
+
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+}
+
+void db5500_prcmu_config_abb_event_readout(u32 abb_events)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	mb0_transfer.req.abb_events = abb_events;
+	config_wakeups();
+
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+}
+
+void db5500_prcmu_get_abb_event_buffer(void __iomem **buf)
+{
+	if (readb(PRCM_ACK_MB0_READ_POINTER) & 1)
+		*buf = (PRCM_ACK_MB0_WAKEUP_1_ABB);
+	else
+		*buf = (PRCM_ACK_MB0_WAKEUP_0_ABB);
+}
+
+/* This function should be called with lock */
+static int mailbox4_request(u8 mb4_request, u8 ack_request)
+{
+	int ret = 0;
+
+	writeb(mb4_request, PRCM_REQ_MB4_HEADER);
+	writel(MBOX_BIT(4), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+
+	if (!wait_for_completion_timeout(&mb4_transfer.work,
+		msecs_to_jiffies(20000))) {
+		pr_err("prcmu: MB4 request %d failed", mb4_request);
+		ret = -EIO;
+		WARN(1, "prcmu: failed mb4 request");
+		goto failed;
+	}
+
+	if (mb4_transfer.ack.header != ack_request ||
+		mb4_transfer.ack.status != RC_SUCCESS)
+		ret = -EIO;
+failed:
+	return ret;
+}
+
+int db5500_prcmu_get_hotdog(void)
+{
+	return readw(PRCM_SHARE_INFO_HOTDOG);
+}
+
+int db5500_prcmu_config_hotdog(u8 threshold)
+{
+	int r = 0;
+
+	mutex_lock(&mb4_transfer.lock);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+		cpu_relax();
+
+	writew(threshold, PRCM_REQ_MB4_HOTDOG_THRESHOLD);
+	r = mailbox4_request(MB4H_CFG_HOTDOG, MB4H_ACK_CFG_HOTDOG);
+
+	mutex_unlock(&mb4_transfer.lock);
+
+	return r;
+}
+
+int db5500_prcmu_config_hotmon(u8 low, u8 high)
+{
+	int r = 0;
+
+	mutex_lock(&mb4_transfer.lock);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+		cpu_relax();
+
+	writew(low, PRCM_REQ_MB4_HOTMON_LOW);
+	writew(high, PRCM_REQ_MB4_HOTMON_HIGH);
+
+	r = mailbox4_request(MB4H_CFG_HOTMON, MB4H_ACK_CFG_HOTMON);
+
+	mutex_unlock(&mb4_transfer.lock);
+
+	return r;
+}
+
+static int config_hot_period(u16 val)
+{
+	int r = 0;
+
+	mutex_lock(&mb4_transfer.lock);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+		cpu_relax();
+
+	writew(val, PRCM_REQ_MB4_HOT_PERIOD);
+	r = mailbox4_request(MB4H_CFG_HOTPERIOD, MB4H_ACK_CFG_HOTPERIOD);
+
+	mutex_unlock(&mb4_transfer.lock);
+
+	return r;
+}
+
+/*
+ * period in milli seconds
+ */
+int db5500_prcmu_start_temp_sense(u16 period)
+{
+	if (period == 0xFFFF)
+		return -EINVAL;
+
+	return config_hot_period(period);
+}
+
+int db5500_prcmu_stop_temp_sense(void)
+{
+	return config_hot_period(0xFFFF);
+}
+
+static int prcmu_a9wdog(u8 req, u8 ack)
+{
+	int r = 0;
+
+	mutex_lock(&mb4_transfer.lock);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(4))
+		cpu_relax();
+
+	r = mailbox4_request(req, ack);
+
+	mutex_unlock(&mb4_transfer.lock);
+
+	return r;
+}
+
+static void prcmu_a9wdog_set_interrupt(bool enable)
+{
+	if (enable) {
+		writel(PRCM_TIMER0_IRQ_RTOS1_SET,
+			(mtimer_base + PRCM_TIMER0_IRQ_EN_SET_OFFSET));
+	} else {
+		writel(PRCM_TIMER0_IRQ_RTOS1_CLR,
+			(mtimer_base + PRCM_TIMER0_IRQ_EN_CLR_OFFSET));
+	}
+}
+
+static void prcmu_a9wdog_set_timeout(u32 timeout)
+{
+	u32 comp_timeout;
+
+	comp_timeout = readl(mtimer_base + PRCM_TIMER0_RTOS_COUNTER_OFFSET) +
+			timeout;
+	writel(comp_timeout, mtimer_base + PRCM_TIMER0_RTOS_COMP1_OFFSET);
+}
+
+int db5500_prcmu_config_a9wdog(u8 num, bool sleep_auto_off)
+{
+	/*
+	 * Sleep auto off feature is not supported. Resume and
+	 * suspend will be handled by watchdog driver.
+         */
+	return 0;
+}
+
+int db5500_prcmu_enable_a9wdog(u8 id)
+{
+	int r = 0;
+
+	if (a9wdog_timer.enabled)
+		return -EPERM;
+
+	prcmu_a9wdog_set_interrupt(true);
+
+	r = prcmu_a9wdog(MB4H_CGF_A9WDOG_EN_PREBARK,
+			MB4H_ACK_CGF_A9WDOG_EN_PREBARK);
+	if (!r)
+		a9wdog_timer.enabled = true;
+	else
+		prcmu_a9wdog_set_interrupt(false);
+
+	return r;
+}
+
+int db5500_prcmu_disable_a9wdog(u8 id)
+{
+	if (!a9wdog_timer.enabled)
+		return -EPERM;
+
+	prcmu_a9wdog_set_interrupt(false);
+
+	a9wdog_timer.enabled = false;
+
+	return prcmu_a9wdog(MB4H_CGF_A9WDOG_DIS,
+			MB4H_ACK_CGF_A9WDOG_DIS);
+}
+
+int db5500_prcmu_kick_a9wdog(u8 id)
+{
+	int r = 0;
+
+	if (a9wdog_timer.enabled)
+		prcmu_a9wdog_set_timeout(a9wdog_timer.timeout);
+	else
+		r = -EPERM;
+
+	return r;
+}
+
+int db5500_prcmu_load_a9wdog(u8 id, u32 timeout)
+{
+	if (a9wdog_timer.enabled)
+		return -EPERM;
+
+	prcmu_a9wdog_set_timeout(timeout);
+	a9wdog_timer.timeout = timeout;
+
+	return 0;
+}
+
 /**
  * db5500_prcmu_abb_read() - Read register value(s) from the ABB.
  * @slave:	The I2C slave address.
@@ -170,14 +1314,14 @@ int db5500_prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size)
 
 	mutex_lock(&mb5_transfer.lock);
 
-	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
 		cpu_relax();
 	writeb(slave, PRCM_REQ_MB5_I2C_SLAVE);
 	writeb(reg, PRCM_REQ_MB5_I2C_REG);
 	writeb(size, PRCM_REQ_MB5_I2C_SIZE);
 	writeb(MB5H_I2C_READ, PRCM_REQ_MB5_HEADER);
 
-	writel(MBOX_BIT(5), PRCM_MBOX_CPU_SET);
+	writel(MBOX_BIT(5), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb5_transfer.work);
 
 	r = 0;
@@ -211,7 +1355,7 @@ int db5500_prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
 
 	mutex_lock(&mb5_transfer.lock);
 
-	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(5))
 		cpu_relax();
 	writeb(slave, PRCM_REQ_MB5_I2C_SLAVE);
 	writeb(reg, PRCM_REQ_MB5_I2C_REG);
@@ -219,7 +1363,7 @@ int db5500_prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
 	memcpy_toio(PRCM_REQ_MB5_I2C_DATA, value, size);
 	writeb(MB5H_I2C_WRITE, PRCM_REQ_MB5_HEADER);
 
-	writel(MBOX_BIT(5), PRCM_MBOX_CPU_SET);
+	writel(MBOX_BIT(5), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb5_transfer.work);
 
 	if ((mb5_transfer.ack.header == MB5H_I2C_WRITE) &&
@@ -233,42 +1377,385 @@ int db5500_prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size)
 	return r;
 }
 
+/**
+ * db5500_prcmu_set_arm_opp - set the appropriate ARM OPP
+ * @opp: The new ARM operating point to which transition is to be made
+ * Returns: 0 on success, non-zero on failure
+ *
+ * This function sets the the operating point of the ARM.
+ */
+int db5500_prcmu_set_arm_opp(u8 opp)
+{
+	int r;
+	u8 db5500_opp;
+
+	r = 0;
+
+	switch (opp) {
+	case ARM_EXTCLK:
+		db5500_opp = DB5500_ARM_EXT_OPP;
+		break;
+	case ARM_50_OPP:
+		db5500_opp = DB5500_ARM_50_OPP;
+		break;
+	case ARM_100_OPP:
+		db5500_opp = DB5500_ARM_100_OPP;
+		break;
+	default:
+		pr_err("prcmu: %s() received wrong opp value: %d\n",
+				__func__, opp);
+		r = -EINVAL;
+		goto bailout;
+	}
+
+	mutex_lock(&mb1_transfer.lock);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
+		cpu_relax();
+
+	writeb(MB1H_ARM_OPP, PRCM_REQ_MB1_HEADER);
+
+	writeb(db5500_opp, PRCM_REQ_MB1_ARM_OPP);
+	writel(MBOX_BIT(1), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
+
+	if (!wait_for_completion_timeout(&mb1_transfer.work,
+		msecs_to_jiffies(20000))) {
+		r = -EIO;
+		WARN(1, "prcmu: failed to set arm opp");
+		goto unlock_and_return;
+	}
+
+	if (mb1_transfer.ack.header != MB1H_ARM_OPP ||
+		(mb1_transfer.ack.arm_opp != db5500_opp) ||
+		(mb1_transfer.ack.arm_voltage_st != RC_SUCCESS))
+		r = -EIO;
+
+unlock_and_return:
+	mutex_unlock(&mb1_transfer.lock);
+bailout:
+	if (!r)
+		prcmu_debug_arm_opp_log(opp);
+	return r;
+}
+
+static void __init prcmu_ape_clocks_init(void)
+{
+	u8 opp = db5500_prcmu_get_ape_opp();
+	unsigned long flags;
+	int i;
+
+	WARN(opp != APE_100_OPP, "%s: Initial APE OPP (%u) not 100%%?\n",
+	     __func__, opp);
+
+	for (i = 0; i < PRCMU_NUM_REG_CLOCKS; i++) {
+		struct clk_mgt *clkmgt = &clk_mgt[i];
+		u32 clkval;
+		u32 div;
+
+		if (!clkmgt->scalable && !clkmgt->force50)
+			continue;
+
+		spin_lock_irqsave(&clk_mgt_lock, flags);
+
+		clkval = readl(_PRCMU_BASE + clkmgt->offset);
+		div = clkval & PRCM_CLK_MGT_CLKPLLDIV_MASK;
+		div >>= PRCM_CLK_MGT_CLKPLLDIV_SHIFT;
+
+		if (clkmgt->force50) {
+			div *= 2;
+
+			clkval &= ~PRCM_CLK_MGT_CLKPLLDIV_MASK;
+			clkval |= div << PRCM_CLK_MGT_CLKPLLDIV_SHIFT;
+			writel(clkval, _PRCMU_BASE + clkmgt->offset);
+
+			spin_unlock_irqrestore(&clk_mgt_lock, flags);
+			continue;
+		}
+
+		spin_unlock_irqrestore(&clk_mgt_lock, flags);
+
+		clkmgt->div = div;
+		if (!div)
+			pr_err("%s: scalable clock at offset %#x has zero divisor\n",
+			       __func__, clkmgt->offset);
+	}
+}
+
+static void prcmu_ape_clocks_scale(u8 opp)
+{
+	unsigned long irqflags;
+	unsigned int i;
+	u32 clkval;
+
+	/*
+	 * Note: calling printk() under the following lock can cause lock
+	 * recursion via clk_enable() for the console UART!
+	 */
+	spin_lock_irqsave(&clk_mgt_lock, irqflags);
+
+	/* take a lock on HW (HWSEM)*/
+	while ((readl(_PRCMU_BASE + PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
+		cpu_relax();
+
+	for (i = 0; i < PRCMU_NUM_REG_CLOCKS; i++) {
+		u32 divval;
+
+		if (!clk_mgt[i].scalable)
+			continue;
+
+		clkval = readl(_PRCMU_BASE + clk_mgt[i].offset);
+		divval = clk_mgt[i].div;
+
+		pr_debug("PRCMU: reg %#x prev clk = 0x%x stored div = 0x%x\n",
+			 clk_mgt[i].offset, clkval, divval);
+
+		if (opp == DB5500_APE_50_OPP)
+			divval *= 2;
+
+		clkval &= ~PRCM_CLK_MGT_CLKPLLDIV_MASK;
+		clkval |= divval << PRCM_CLK_MGT_CLKPLLDIV_SHIFT;
+
+		pr_debug("PRCMU: wr 0x%x in reg 0x%x\n",
+			 clkval, clk_mgt[i].offset);
+
+		writel(clkval, _PRCMU_BASE + clk_mgt[i].offset);
+	}
+
+	/* release lock */
+	writel(0, (_PRCMU_BASE + PRCM_SEM));
+
+	spin_unlock_irqrestore(&clk_mgt_lock, irqflags);
+}
+/* Divide the frequency of certain clocks by 2 for APE_50_PARTLY_25_OPP. */
+static void request_even_slower_clocks(bool enable)
+{
+	void __iomem *clock_reg[] = {
+		(_PRCMU_BASE + DB5500_PRCM_ACLK_MGT),
+		(_PRCMU_BASE + DB5500_PRCM_DMACLK_MGT)
+	};
+	unsigned long flags;
+	unsigned int i;
+
+	spin_lock_irqsave(&clk_mgt_lock, flags);
+
+	/* Grab the HW semaphore. */
+	while ((readl(_PRCMU_BASE + PRCM_SEM) & PRCM_SEM_PRCM_SEM) != 0)
+		cpu_relax();
+
+	for (i = 0; i < ARRAY_SIZE(clock_reg); i++) {
+		u32 val;
+		u32 div;
+
+		val = readl(clock_reg[i]);
+		div = (val & PRCM_CLK_MGT_CLKPLLDIV_MASK);
+		if (enable) {
+			if ((div <= 1) || (div > 15)) {
+				pr_err("prcmu: Bad clock divider %d in %s\n",
+					div, __func__);
+				goto unlock_and_return;
+			}
+			div <<= 1;
+		} else {
+			if (div <= 2)
+				goto unlock_and_return;
+			div >>= 1;
+		}
+		val = ((val & ~PRCM_CLK_MGT_CLKPLLDIV_MASK) |
+			(div & PRCM_CLK_MGT_CLKPLLDIV_MASK));
+		writel(val, clock_reg[i]);
+	}
+
+unlock_and_return:
+	/* Release the HW semaphore. */
+	writel(0, _PRCMU_BASE + PRCM_SEM);
+
+	spin_unlock_irqrestore(&clk_mgt_lock, flags);
+}
+int db5500_prcmu_set_ape_opp(u8 opp)
+{
+	int ret = 0;
+	u8 db5500_opp;
+	if (opp == mb1_transfer.req_ape_opp)
+		return 0;
+
+	switch (opp) {
+	case APE_100_OPP:
+		db5500_opp = DB5500_APE_100_OPP;
+		break;
+	case APE_50_OPP:
+	case APE_50_PARTLY_25_OPP:
+		db5500_opp = DB5500_APE_50_OPP;
+		break;
+	default:
+		pr_err("prcmu: %s() received wrong opp value: %d\n",
+				__func__, opp);
+		ret = -EINVAL;
+		goto bailout;
+	}
+
+	mutex_lock(&mb1_transfer.lock);
+	if (mb1_transfer.req_ape_opp == APE_50_PARTLY_25_OPP)
+		request_even_slower_clocks(false);
+	if ((opp != APE_100_OPP) && (mb1_transfer.req_ape_opp != APE_100_OPP))
+		goto skip_message;
+
+	prcmu_ape_clocks_scale(db5500_opp);
+
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
+		cpu_relax();
+
+	writeb(MB1H_APE_OPP, PRCM_REQ_MB1_HEADER);
+	writeb(db5500_opp, PRCM_REQ_MB1_APE_OPP);
+	writel(MBOX_BIT(1), (_PRCMU_BASE + PRCM_MBOX_CPU_SET));
+
+	if (!wait_for_completion_timeout(&mb1_transfer.work,
+		msecs_to_jiffies(20000))) {
+		ret = -EIO;
+		WARN(1, "prcmu: failed to set ape opp to %u", opp);
+		goto unlock_and_return;
+	}
+
+	if (mb1_transfer.ack.header != MB1H_APE_OPP ||
+		(mb1_transfer.ack.ape_opp != db5500_opp) ||
+		(mb1_transfer.ack.arm_voltage_st != RC_SUCCESS))
+		ret = -EIO;
+
+skip_message:
+	if ((!ret && (opp == APE_50_PARTLY_25_OPP)) ||
+		(ret && (mb1_transfer.req_ape_opp == APE_50_PARTLY_25_OPP)))
+			request_even_slower_clocks(true);
+	if (!ret)
+		mb1_transfer.req_ape_opp = opp;
+unlock_and_return:
+	mutex_unlock(&mb1_transfer.lock);
+bailout:
+	return ret;
+}
+
+int db5500_prcmu_get_ape_opp(void)
+{
+	u8 opp = readb(PRCM_ACK_MB1_CURRENT_APE_OPP);
+
+	switch (opp) {
+	case DB5500_APE_100_OPP:
+		return APE_100_OPP;
+	case DB5500_APE_50_OPP:
+		return APE_50_OPP;
+	default:
+		pr_err("prcmu: %s() read unknown opp value: %d\n",
+				__func__, opp);
+		return APE_100_OPP;
+	}
+}
+
+int db5500_prcmu_get_ddr_opp(void)
+{
+	return readb(_PRCMU_BASE + PRCM_DDR_SUBSYS_APE_MINBW);
+}
+
+int db5500_prcmu_set_ddr_opp(u8 opp)
+{
+	if (opp != DDR_100_OPP && opp != DDR_50_OPP)
+		return -EINVAL;
+
+	writeb(opp, _PRCMU_BASE + PRCM_DDR_SUBSYS_APE_MINBW);
+
+	return 0;
+}
+
+/**
+ * db5500_prcmu_get_arm_opp - get the current ARM OPP
+ *
+ * Returns: the current ARM OPP
+ */
+int db5500_prcmu_get_arm_opp(void)
+{
+	u8 opp = readb(PRCM_ACK_MB1_CURRENT_ARM_OPP);
+
+	switch (opp) {
+	case DB5500_ARM_EXT_OPP:
+		return ARM_EXTCLK;
+	case DB5500_ARM_50_OPP:
+		return ARM_50_OPP;
+	case DB5500_ARM_100_OPP:
+		return ARM_100_OPP;
+	default:
+		pr_err("prcmu: %s() read unknown opp value: %d\n",
+				__func__, opp);
+		return ARM_100_OPP;
+	}
+}
+
+int prcmu_resetout(u8 resoutn, u8 state)
+{
+	int offset;
+	int pin = -1;
+
+	offset = state > 0 ? PRCM_RESOUTN_SET_OFFSET : PRCM_RESOUTN_CLR_OFFSET;
+
+	switch (resoutn) {
+	case 0:
+		pin = PRCMU_RESOUTN0_PIN;
+		break;
+	case 1:
+		pin = PRCMU_RESOUTN1_PIN;
+		break;
+	case 2:
+		pin = PRCMU_RESOUTN2_PIN;
+	default:
+		break;
+	}
+
+	if (pin > 0)
+		writel(pin, _PRCMU_BASE + offset);
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
 int db5500_prcmu_enable_dsipll(void)
 {
 	int i;
+	int ret = 0;
 
 	/* Enable DSIPLL_RESETN resets */
-	writel(PRCMU_RESET_DSIPLL, PRCM_APE_RESETN_CLR);
+	writel(PRCMU_RESET_DSIPLL, _PRCMU_BASE + PRCM_APE_RESETN_CLR);
 	/* Unclamp DSIPLL in/out */
-	writel(PRCMU_UNCLAMP_DSIPLL, PRCM_MMIP_LS_CLAMP_CLR);
+	writel(PRCMU_UNCLAMP_DSIPLL, _PRCMU_BASE + PRCM_MMIP_LS_CLAMP_CLR);
 	/* Set DSI PLL FREQ */
-	writel(PRCMU_PLLDSI_FREQ_SETTING, PRCM_PLLDSI_FREQ);
+	writel(PRCMU_PLLDSI_FREQ_SETTING, _PRCMU_BASE + PRCM_PLLDSI_FREQ);
 	writel(PRCMU_DSI_PLLOUT_SEL_SETTING,
-		PRCM_DSI_PLLOUT_SEL);
+		_PRCMU_BASE + PRCM_DSI_PLLOUT_SEL);
 	/* Enable Escape clocks */
-	writel(PRCMU_ENABLE_ESCAPE_CLOCK_DIV, PRCM_DSITVCLK_DIV);
+	writel(PRCMU_ENABLE_ESCAPE_CLOCK_DIV, _PRCMU_BASE + PRCM_DSITVCLK_DIV);
 
 	/* Start DSI PLL */
-	writel(PRCMU_ENABLE_PLLDSI, PRCM_PLLDSI_ENABLE);
+	writel(PRCMU_ENABLE_PLLDSI, _PRCMU_BASE + PRCM_PLLDSI_ENABLE);
 	/* Reset DSI PLL */
-	writel(PRCMU_DSI_RESET_SW, PRCM_DSI_SW_RESET);
+	writel(PRCMU_DSI_RESET_SW, _PRCMU_BASE + PRCM_DSI_SW_RESET);
 	for (i = 0; i < 10; i++) {
-		if ((readl(PRCM_PLLDSI_LOCKP) &
+		if ((readl(_PRCMU_BASE + PRCM_PLLDSI_LOCKP) &
 			PRCMU_PLLDSI_LOCKP_LOCKED) == PRCMU_PLLDSI_LOCKP_LOCKED)
 			break;
 		udelay(100);
 	}
+
+	if ((readl(_PRCMU_BASE + PRCM_PLLDSI_LOCKP) &
+			PRCMU_PLLDSI_LOCKP_LOCKED)
+					!= PRCMU_PLLDSI_LOCKP_LOCKED)
+		ret = -EIO;
 	/* Release DSIPLL_RESETN */
-	writel(PRCMU_RESET_DSIPLL, PRCM_APE_RESETN_SET);
-	return 0;
+	writel(PRCMU_RESET_DSIPLL, _PRCMU_BASE + PRCM_APE_RESETN_SET);
+	return ret;
 }
 
 int db5500_prcmu_disable_dsipll(void)
 {
 	/* Disable dsi pll */
-	writel(PRCMU_DISABLE_PLLDSI, PRCM_PLLDSI_ENABLE);
+	writel(PRCMU_DISABLE_PLLDSI, _PRCMU_BASE + PRCM_PLLDSI_ENABLE);
 	/* Disable  escapeclock */
-	writel(PRCMU_DISABLE_ESCAPE_CLOCK_DIV, PRCM_DSITVCLK_DIV);
+	writel(PRCMU_DISABLE_ESCAPE_CLOCK_DIV, _PRCMU_BASE + PRCM_DSITVCLK_DIV);
 	return 0;
 }
 
@@ -276,27 +1763,150 @@ int db5500_prcmu_set_display_clocks(void)
 {
 	/* HDMI and TVCLK Should be handled somewhere else */
 	/* PLLDIV=8, PLLSW=2, CLKEN=1 */
-	writel(PRCMU_DSI_CLOCK_SETTING, PRCM_HDMICLK_MGT);
+	writel(PRCMU_DSI_CLOCK_SETTING, _PRCMU_BASE + DB5500_PRCM_HDMICLK_MGT);
 	/* PLLDIV=14, PLLSW=2, CLKEN=1 */
-	writel(PRCMU_DSI_LP_CLOCK_SETTING, PRCM_TVCLK_MGT);
+	writel(PRCMU_DSI_LP_CLOCK_SETTING, _PRCMU_BASE + DB5500_PRCM_TVCLK_MGT);
 	return 0;
 }
 
+u32 db5500_prcmu_read(unsigned int reg)
+{
+	return readl_relaxed(_PRCMU_BASE + reg);
+}
+
+void db5500_prcmu_write(unsigned int reg, u32 value)
+{
+	writel_relaxed(value, _PRCMU_BASE + reg);
+}
+
+void db5500_prcmu_write_masked(unsigned int reg, u32 mask, u32 value)
+{
+	u32 val;
+
+	val = readl_relaxed(_PRCMU_BASE + reg);
+	val = (val & ~mask) | (value & mask);
+	writel_relaxed(val, _PRCMU_BASE + reg);
+}
+
+/**
+ * db5500_prcmu_system_reset - System reset
+ *
+ * Saves the reset reason code and then sets the APE_SOFTRST register which
+ * fires an interrupt to fw
+ */
+void db5500_prcmu_system_reset(u16 reset_code)
+{
+	writew(reset_code, PRCM_SW_RST_REASON);
+	writel(1, _PRCMU_BASE + PRCM_APE_SOFTRST);
+}
+
+/**
+ * db5500_prcmu_get_reset_code - Retrieve SW reset reason code
+ *
+ * Retrieves the reset reason code stored by prcmu_system_reset() before
+ * last restart.
+ */
+u16 db5500_prcmu_get_reset_code(void)
+{
+	return readw(PRCM_SW_RST_REASON);
+}
+
 static void ack_dbb_wakeup(void)
 {
 	unsigned long flags;
 
 	spin_lock_irqsave(&mb0_transfer.lock, flags);
 
-	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(0))
 		cpu_relax();
 
-	writeb(RMB0H_RD_WAKE_UP_ACK, PRCM_REQ_MB0_HEADER);
-	writel(MBOX_BIT(0), PRCM_MBOX_CPU_SET);
+	writeb(MB0H_RD_WAKE_UP_ACK, PRCM_REQ_MB0_HEADER);
+	writel(MBOX_BIT(0), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
 
 	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
 }
 
+int db5500_prcmu_set_epod(u16 epod, u8 epod_state)
+{
+	int r = 0;
+	bool ram_retention = false;
+
+	/* check argument */
+	BUG_ON(epod < DB5500_EPOD_ID_BASE);
+	BUG_ON(epod_state > EPOD_STATE_ON);
+	BUG_ON((epod - DB5500_EPOD_ID_BASE) >= DB5500_NUM_EPOD_ID);
+
+	if (epod == DB5500_EPOD_ID_ESRAM12)
+		ram_retention = true;
+
+	/* check argument */
+	BUG_ON(epod_state == EPOD_STATE_RAMRET && !ram_retention);
+
+	/* get lock */
+	mutex_lock(&mb2_transfer.lock);
+
+	/* wait for mailbox */
+	while (readl(_PRCMU_BASE + PRCM_MBOX_CPU_VAL) & MBOX_BIT(2))
+		cpu_relax();
+
+	/* Retention is allowed only for ESRAM12 */
+	if (epod  == DB5500_EPOD_ID_ESRAM12) {
+		switch (epod_state) {
+		case EPOD_STATE_ON:
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_OOR_ON;
+			break;
+		case EPOD_STATE_OFF:
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_OOR_OFF;
+			break;
+		case EPOD_STATE_RAMRET:
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_OOR_RET;
+			break;
+		default:
+			r = -EINVAL;
+			goto unlock_and_return;
+			break;
+		}
+	} else {
+		if (epod_state == EPOD_STATE_ON)
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_ON;
+		else if (epod_state == EPOD_STATE_OFF)
+			mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE] =
+				EPOD_OFF;
+		else {
+			r = -EINVAL;
+			goto unlock_and_return;
+		}
+	}
+	/* fill in mailbox */
+	writeb((epod - DB5500_EPOD_ID_BASE), PRCM_REQ_MB2_EPOD_CLIENT);
+	writeb(mb2_transfer.req.epod_st[epod - DB5500_EPOD_ID_BASE],
+		PRCM_REQ_MB2_EPOD_STATE);
+
+	writeb(MB2H_EPOD_REQUEST, PRCM_REQ_MB2_HEADER);
+
+	writel(MBOX_BIT(2), _PRCMU_BASE + PRCM_MBOX_CPU_SET);
+
+	if (!wait_for_completion_timeout(&mb2_transfer.work,
+		msecs_to_jiffies(20000))) {
+		pr_err("prcmu: set_epod() failed.\n");
+		r = -EIO;
+		WARN(1, "Failed to set epod");
+		goto unlock_and_return;
+	}
+
+	if (mb2_transfer.ack.status != RC_SUCCESS ||
+		mb2_transfer.ack.header != MB2H_EPOD_REQUEST)
+		r = -EIO;
+
+unlock_and_return:
+	mutex_unlock(&mb2_transfer.lock);
+	return r;
+}
+
 static inline void print_unknown_header_warning(u8 n, u8 header)
 {
 	pr_warning("prcmu: Unknown message header (%d) in mailbox %d.\n",
@@ -306,11 +1916,31 @@ static inline void print_unknown_header_warning(u8 n, u8 header)
 static bool read_mailbox_0(void)
 {
 	bool r;
+	u32 ev;
+	unsigned int n;
+
 	u8 header;
 
 	header = readb(PRCM_ACK_MB0_HEADER);
 	switch (header) {
-	case AMB0H_WAKE_UP:
+	case MB0H_WAKE_UP:
+		if (readb(PRCM_ACK_MB0_READ_POINTER) & 1)
+			ev = readl(PRCM_ACK_MB0_WAKEUP_1_DBB);
+		else
+			ev = readl(PRCM_ACK_MB0_WAKEUP_0_DBB);
+
+		prcmu_debug_register_mbox0_event(ev,
+						 (mb0_transfer.req.dbb_irqs |
+						  mb0_transfer.req.dbb_wakeups));
+
+		ev &= mb0_transfer.req.dbb_irqs;
+
+		for (n = 0; n < NUM_DB5500_PRCMU_WAKEUPS; n++) {
+			if (ev & prcmu_irq_bit[n]) {
+				if (n != IRQ_INDEX(ABB))
+					generic_handle_irq(IRQ_DB5500_PRCMU_BASE + n);
+			}
+		}
 		r = true;
 		break;
 	default:
@@ -318,31 +1948,123 @@ static bool read_mailbox_0(void)
 		r = false;
 		break;
 	}
-	writel(MBOX_BIT(0), PRCM_ARM_IT1_CLR);
+	writel(MBOX_BIT(0), _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
 	return r;
 }
 
 static bool read_mailbox_1(void)
 {
-	writel(MBOX_BIT(1), PRCM_ARM_IT1_CLR);
+	u8 header;
+	bool do_complete = true;
+
+	header = mb1_transfer.ack.header = readb(PRCM_ACK_MB1_HEADER);
+
+	switch (header) {
+	case MB1H_ARM_OPP:
+		mb1_transfer.ack.arm_opp = readb(PRCM_ACK_MB1_CURRENT_ARM_OPP);
+		mb1_transfer.ack.arm_voltage_st =
+			readb(PRCM_ACK_MB1_ARM_VOLT_STATUS);
+		break;
+	case MB1H_APE_OPP:
+		mb1_transfer.ack.ape_opp = readb(PRCM_ACK_MB1_CURRENT_APE_OPP);
+		mb1_transfer.ack.ape_voltage_st =
+			readb(PRCM_ACK_MB1_APE_VOLT_STATUS);
+		break;
+	case MB1H_ARM_APE_OPP:
+		mb1_transfer.ack.ape_opp = readb(PRCM_ACK_MB1_CURRENT_APE_OPP);
+		mb1_transfer.ack.ape_voltage_st =
+			readb(PRCM_ACK_MB1_APE_VOLT_STATUS);
+		break;
+	default:
+		print_unknown_header_warning(1, header);
+		do_complete = false;
+		break;
+	}
+
+	writel(MBOX_BIT(1), _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
+
+	if (do_complete)
+		complete(&mb1_transfer.work);
+
 	return false;
 }
 
 static bool read_mailbox_2(void)
 {
-	writel(MBOX_BIT(2), PRCM_ARM_IT1_CLR);
+	u8 header;
+
+	header = readb(PRCM_ACK_MB2_HEADER);
+	mb2_transfer.ack.header = header;
+	switch (header) {
+	case MB2H_EPOD_REQUEST:
+		mb2_transfer.ack.status = readb(PRCM_ACK_MB2_EPOD_STATUS);
+		break;
+	case MB2H_CLK_REQUEST:
+		mb2_transfer.ack.status = readb(PRCM_ACK_MB2_CLK_STATUS);
+		break;
+	case MB2H_PLL_REQUEST:
+		mb2_transfer.ack.status = readb(PRCM_ACK_MB2_PLL_STATUS);
+		break;
+	default:
+		writel(MBOX_BIT(2), _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
+		pr_err("prcmu: Wrong ACK received for MB2 request \n");
+		return false;
+		break;
+	}
+	writel(MBOX_BIT(2), _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
+	complete(&mb2_transfer.work);
 	return false;
 }
 
 static bool read_mailbox_3(void)
 {
-	writel(MBOX_BIT(3), PRCM_ARM_IT1_CLR);
+	u8 header;
+
+	header = readb(PRCM_ACK_MB3_HEADER);
+	mb3_transfer.ack.header = header;
+	switch (header) {
+	case MB3H_REFCLK_REQUEST:
+		mb3_transfer.ack.status = readb(PRCM_ACK_MB3_REFCLK_REQ);
+		writel(MBOX_BIT(3), _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
+		complete(&mb3_transfer.sysclk_work);
+		break;
+	default:
+		writel(MBOX_BIT(3), _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
+		pr_err("prcmu: wrong MB3 header\n");
+		break;
+	}
+
 	return false;
 }
 
 static bool read_mailbox_4(void)
 {
-	writel(MBOX_BIT(4), PRCM_ARM_IT1_CLR);
+	u8 header;
+	bool do_complete = true;
+
+	header = readb(PRCM_ACK_MB4_HEADER);
+	mb4_transfer.ack.header = header;
+	switch (header) {
+	case MB4H_ACK_CFG_HOTDOG:
+	case MB4H_ACK_CFG_HOTMON:
+	case MB4H_ACK_CFG_HOTPERIOD:
+	case MB4H_ACK_CFG_MODEM_RESET:
+	case MB4H_ACK_CGF_A9WDOG_EN_PREBARK:
+	case MB4H_ACK_CGF_A9WDOG_EN_NOPREBARK:
+	case MB4H_ACK_CGF_A9WDOG_DIS:
+		mb4_transfer.ack.status = readb(PRCM_ACK_MB4_REQUESTS);
+		break;
+	default:
+		print_unknown_header_warning(4, header);
+		do_complete = false;
+		break;
+	}
+
+	writel(MBOX_BIT(4), (_PRCMU_BASE + PRCM_ARM_IT1_CLEAR));
+
+	if (do_complete)
+		complete(&mb4_transfer.work);
+
 	return false;
 }
 
@@ -363,19 +2085,19 @@ static bool read_mailbox_5(void)
 		print_unknown_header_warning(5, header);
 		break;
 	}
-	writel(MBOX_BIT(5), PRCM_ARM_IT1_CLR);
+	writel(MBOX_BIT(5), _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
 	return false;
 }
 
 static bool read_mailbox_6(void)
 {
-	writel(MBOX_BIT(6), PRCM_ARM_IT1_CLR);
+	writel(MBOX_BIT(6), _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
 	return false;
 }
 
 static bool read_mailbox_7(void)
 {
-	writel(MBOX_BIT(7), PRCM_ARM_IT1_CLR);
+	writel(MBOX_BIT(7), _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
 	return false;
 }
 
@@ -396,7 +2118,7 @@ static irqreturn_t prcmu_irq_handler(int irq, void *data)
 	u8 n;
 	irqreturn_t r;
 
-	bits = (readl(PRCM_ARM_IT1_VAL) & ALL_MBOX_BITS);
+	bits = (readl(_PRCMU_BASE + PRCM_ARM_IT1_VAL) & ALL_MBOX_BITS);
 	if (unlikely(!bits))
 		return IRQ_NONE;
 
@@ -406,6 +2128,7 @@ static irqreturn_t prcmu_irq_handler(int irq, void *data)
 			bits -= MBOX_BIT(n);
 			if (read_mailbox[n]())
 				r = IRQ_WAKE_THREAD;
+			prcmu_debug_register_interrupt(n);
 		}
 	}
 	return r;
@@ -413,39 +2136,271 @@ static irqreturn_t prcmu_irq_handler(int irq, void *data)
 
 static irqreturn_t prcmu_irq_thread_fn(int irq, void *data)
 {
+	u32 ev;
+
+	/*
+	 * ABB needs to be handled before the wakeup because
+	 * the ping/pong buffers for ABB events could change
+	 * after we acknowledge the wakeup.
+	 */
+	if (readb(PRCM_ACK_MB0_READ_POINTER) & 1)
+		ev = readl(PRCM_ACK_MB0_WAKEUP_1_DBB);
+	else
+		ev = readl(PRCM_ACK_MB0_WAKEUP_0_DBB);
+
+	ev &= mb0_transfer.req.dbb_irqs;
+	if (ev & WAKEUP_BIT_ABB)
+		handle_nested_irq(IRQ_DB5500_PRCMU_ABB);
+
 	ack_dbb_wakeup();
+
 	return IRQ_HANDLED;
 }
 
+static void prcmu_mask_work(struct work_struct *work)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mb0_transfer.lock, flags);
+
+	config_wakeups();
+
+	spin_unlock_irqrestore(&mb0_transfer.lock, flags);
+}
+
+static void prcmu_irq_mask(struct irq_data *d)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mb0_transfer.dbb_irqs_lock, flags);
+
+	mb0_transfer.req.dbb_irqs &= ~prcmu_irq_bit[d->irq - IRQ_DB5500_PRCMU_BASE];
+
+	spin_unlock_irqrestore(&mb0_transfer.dbb_irqs_lock, flags);
+	schedule_work(&mb0_transfer.mask_work);
+}
+
+static void prcmu_irq_unmask(struct irq_data *d)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mb0_transfer.dbb_irqs_lock, flags);
+
+	mb0_transfer.req.dbb_irqs |= prcmu_irq_bit[d->irq - IRQ_DB5500_PRCMU_BASE];
+
+	spin_unlock_irqrestore(&mb0_transfer.dbb_irqs_lock, flags);
+	schedule_work(&mb0_transfer.mask_work);
+}
+
+static void noop(struct irq_data *d)
+{
+}
+
+static struct irq_chip prcmu_irq_chip = {
+	.name           = "prcmu",
+	.irq_disable    = prcmu_irq_mask,
+	.irq_ack        = noop,
+	.irq_mask       = prcmu_irq_mask,
+	.irq_unmask     = prcmu_irq_unmask,
+};
+
 void __init db5500_prcmu_early_init(void)
 {
+	unsigned int i;
+	void *tcpm_base = ioremap_nocache(U5500_PRCMU_TCPM_BASE, SZ_4K);
+
+	if (tcpm_base != NULL) {
+		int version_high, version_low;
+
+		version_high = readl(tcpm_base + PRCMU_FW_VERSION_OFFSET);
+		version_low = readl(tcpm_base + PRCMU_FW_VERSION_OFFSET + 4);
+		prcmu_version.board = (version_high >> 24) & 0xFF;
+		prcmu_version.fw_version = version_high & 0xFF;
+		prcmu_version.api_version = version_low & 0xFF;
+
+		pr_info("PRCMU Firmware Version: 0x%x\n",
+			prcmu_version.fw_version);
+		pr_info("PRCMU API Version: 0x%x\n",
+			prcmu_version.api_version);
+
+		iounmap(tcpm_base);
+	}
+
 	tcdm_base = __io_address(U5500_PRCMU_TCDM_BASE);
+	mtimer_base = __io_address(U5500_MTIMER_BASE);
 	spin_lock_init(&mb0_transfer.lock);
+	spin_lock_init(&mb0_transfer.dbb_irqs_lock);
+	mutex_init(&mb0_transfer.ac_wake_lock);
+	mutex_init(&mb1_transfer.lock);
+	init_completion(&mb1_transfer.work);
+	mutex_init(&mb2_transfer.lock);
+	init_completion(&mb2_transfer.work);
+	mutex_init(&mb3_transfer.sysclk_lock);
+	init_completion(&mb3_transfer.sysclk_work);
+	mutex_init(&mb4_transfer.lock);
+	init_completion(&mb4_transfer.work);
 	mutex_init(&mb5_transfer.lock);
 	init_completion(&mb5_transfer.work);
+
+	INIT_WORK(&mb0_transfer.mask_work, prcmu_mask_work);
+
+	/* Initalize irqs. */
+	for (i = 0; i < NUM_DB5500_PRCMU_WAKEUPS; i++) {
+		unsigned int irq;
+
+		irq = IRQ_DB5500_PRCMU_BASE + i;
+		irq_set_chip_and_handler(irq, &prcmu_irq_chip,
+					 handle_simple_irq);
+		if (irq == IRQ_DB5500_PRCMU_ABB)
+			irq_set_nested_thread(irq, true);
+		set_irq_flags(irq, IRQF_VALID);
+	}
+	prcmu_ape_clocks_init();
+}
+
+/*
+ * Power domain switches (ePODs) modeled as regulators for the DB5500 SoC
+ */
+static struct regulator_consumer_supply db5500_vape_consumers[] = {
+	REGULATOR_SUPPLY("v-ape", NULL),
+	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.0"),
+	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.1"),
+	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.2"),
+	REGULATOR_SUPPLY("v-i2c", "nmk-i2c.3"),
+	REGULATOR_SUPPLY("vcore", "sdi0"),
+	REGULATOR_SUPPLY("vcore", "sdi1"),
+	REGULATOR_SUPPLY("vcore", "sdi2"),
+	REGULATOR_SUPPLY("vcore", "sdi3"),
+	REGULATOR_SUPPLY("vcore", "sdi4"),
+	REGULATOR_SUPPLY("v-uart", "uart0"),
+	REGULATOR_SUPPLY("v-uart", "uart1"),
+	REGULATOR_SUPPLY("v-uart", "uart2"),
+	REGULATOR_SUPPLY("v-uart", "uart3"),
+	REGULATOR_SUPPLY("v-ape", "db5500-keypad"),
+};
+
+static struct regulator_consumer_supply db5500_sga_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.0"),
+	REGULATOR_SUPPLY("v-mali", NULL),
+};
+
+static struct regulator_consumer_supply db5500_hva_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.1"),
+	REGULATOR_SUPPLY("v-hva", NULL),
+};
+
+static struct regulator_consumer_supply db5500_sia_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.2"),
+	REGULATOR_SUPPLY("v-sia", "mmio_camera"),
+};
+
+static struct regulator_consumer_supply db5500_disp_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.3"),
+	REGULATOR_SUPPLY("vsupply", "b2r2_bus"),
+	REGULATOR_SUPPLY("vsupply", "mcde"),
+};
+
+static struct regulator_consumer_supply db5500_esram12_consumers[] = {
+	REGULATOR_SUPPLY("debug", "reg-virt-consumer.4"),
+	REGULATOR_SUPPLY("v-esram12", "mcde"),
+	REGULATOR_SUPPLY("esram12", "hva"),
+};
+
+#define DB5500_REGULATOR_SWITCH(lower, upper)                           \
+[DB5500_REGULATOR_SWITCH_##upper] = {                                   \
+	.constraints = {                                                \
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS,              \
+	},                                                              \
+	.consumer_supplies      = db5500_##lower##_consumers,            \
+	.num_consumer_supplies  = ARRAY_SIZE(db5500_##lower##_consumers),\
 }
 
+#define DB5500_REGULATOR_SWITCH_VAPE(lower, upper)			\
+[DB5500_REGULATOR_SWITCH_##upper] = {					\
+	.supply_regulator = "db5500-vape",				\
+	.constraints = {						\
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS,		\
+	},								\
+	.consumer_supplies      = db5500_##lower##_consumers,		\
+	.num_consumer_supplies  = ARRAY_SIZE(db5500_##lower##_consumers),\
+}									\
+
+static struct regulator_init_data db5500_regulators[DB5500_NUM_REGULATORS] = {
+	[DB5500_REGULATOR_VAPE] = {
+		.constraints = {
+			.valid_ops_mask	= REGULATOR_CHANGE_STATUS,
+		},
+		.consumer_supplies	= db5500_vape_consumers,
+		.num_consumer_supplies	= ARRAY_SIZE(db5500_vape_consumers),
+	},
+	DB5500_REGULATOR_SWITCH_VAPE(sga, SGA),
+	DB5500_REGULATOR_SWITCH_VAPE(hva, HVA),
+	DB5500_REGULATOR_SWITCH_VAPE(sia, SIA),
+	DB5500_REGULATOR_SWITCH_VAPE(disp, DISP),
+	/*
+	 * ESRAM12 is put in retention by the firmware when VAPE is
+	 * turned off so there's no need to hold VAPE.
+	 */
+	DB5500_REGULATOR_SWITCH(esram12, ESRAM12),
+};
+
+static struct mfd_cell db5500_prcmu_devs[] = {
+	{
+		.name = "db5500-prcmu-regulators",
+		.platform_data = &db5500_regulators,
+		.pdata_size = sizeof(db5500_regulators),
+	},
+	{
+		.name = "cpufreq-u5500",
+	},
+};
+
 /**
  * prcmu_fw_init - arch init call for the Linux PRCMU fw init logic
  *
  */
-int __init db5500_prcmu_init(void)
+static int __init db5500_prcmu_probe(struct platform_device *pdev)
 {
-	int r = 0;
+	int err = 0;
 
 	if (ux500_is_svp() || !cpu_is_u5500())
 		return -ENODEV;
 
 	/* Clean up the mailbox interrupts after pre-kernel code. */
-	writel(ALL_MBOX_BITS, PRCM_ARM_IT1_CLR);
+	writel(ALL_MBOX_BITS, _PRCMU_BASE + PRCM_ARM_IT1_CLEAR);
 
-	r = request_threaded_irq(IRQ_DB5500_PRCMU1, prcmu_irq_handler,
-		prcmu_irq_thread_fn, 0, "prcmu", NULL);
-	if (r < 0) {
+	err = request_threaded_irq(IRQ_DB5500_PRCMU1, prcmu_irq_handler,
+		prcmu_irq_thread_fn, IRQF_NO_SUSPEND, "prcmu", NULL);
+	if (err < 0) {
 		pr_err("prcmu: Failed to allocate IRQ_DB5500_PRCMU1.\n");
-		return -EBUSY;
+		err = -EBUSY;
+		goto no_irq_return;
 	}
-	return 0;
+
+	err = mfd_add_devices(&pdev->dev, 0, db5500_prcmu_devs,
+			      ARRAY_SIZE(db5500_prcmu_devs), NULL,
+			      0);
+
+	if (err)
+		pr_err("prcmu: Failed to add subdevices\n");
+	else
+		pr_info("DB5500 PRCMU initialized\n");
+
+no_irq_return:
+	return err;
+
+}
+
+static struct platform_driver db5500_prcmu_driver = {
+	.driver = {
+		.name = "db5500-prcmu",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init db5500_prcmu_init(void)
+{
+	return platform_driver_probe(&db5500_prcmu_driver, db5500_prcmu_probe);
 }
 
 arch_initcall(db5500_prcmu_init);
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index 5be32489714..7c26c41a7ef 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -30,11 +30,13 @@
 #include <linux/mfd/dbx500-prcmu.h>
 #include <linux/regulator/db8500-prcmu.h>
 #include <linux/regulator/machine.h>
+#include <linux/mfd/abx500.h>
 #include <asm/hardware/gic.h>
 #include <mach/hardware.h>
 #include <mach/irqs.h>
 #include <mach/db8500-regs.h>
 #include <mach/id.h>
+#include <mach/prcmu-debug.h>
 #include "dbx500-prcmu-regs.h"
 
 /* Offset for the firmware version within the TCPM */
@@ -70,6 +72,8 @@
 
 #define PRCM_SW_RST_REASON 0xFF8 /* 2 bytes */
 
+#define PRCM_TCDM_VOICE_CALL_FLAG 0xDD4 /* 4 bytes */
+
 #define _PRCM_MBOX_HEADER		0xFE8 /* 16 bytes */
 #define PRCM_MBOX_HEADER_REQ_MB0	(_PRCM_MBOX_HEADER + 0x0)
 #define PRCM_MBOX_HEADER_REQ_MB1	(_PRCM_MBOX_HEADER + 0x1)
@@ -214,10 +218,8 @@
 #define PRCM_REQ_MB5_I2C_HW_BITS	(PRCM_REQ_MB5 + 0x1)
 #define PRCM_REQ_MB5_I2C_REG		(PRCM_REQ_MB5 + 0x2)
 #define PRCM_REQ_MB5_I2C_VAL		(PRCM_REQ_MB5 + 0x3)
-#define PRCMU_I2C_WRITE(slave) \
-	(((slave) << 1) | (cpu_is_u8500v2() ? BIT(6) : 0))
-#define PRCMU_I2C_READ(slave) \
-	(((slave) << 1) | BIT(0) | (cpu_is_u8500v2() ? BIT(6) : 0))
+#define PRCMU_I2C_WRITE(slave) (((slave) << 1) | BIT(6))
+#define PRCMU_I2C_READ(slave) (((slave) << 1) | BIT(0) | BIT(6))
 #define PRCMU_I2C_STOP_EN		BIT(3)
 
 /* Mailbox 5 ACKs */
@@ -424,6 +426,13 @@ static DEFINE_SPINLOCK(clkout_lock);
 /* Global var to runtime determine TCDM base for v2 or v1 */
 static __iomem void *tcdm_base;
 
+/*
+ * Copies of the startup values of the reset status register and the SW reset
+ * code.
+ */
+static u32 reset_status_copy;
+static u16 reset_code_copy;
+
 struct clk_mgt {
 	void __iomem *reg;
 	u32 pllsw;
@@ -637,6 +646,26 @@ void db8500_prcmu_write_masked(unsigned int reg, u32 mask, u32 value)
 	spin_unlock_irqrestore(&prcmu_lock, flags);
 }
 
+/*
+ * Dump AB8500 registers, PRCMU registers and PRCMU data memory
+ * on critical errors.
+ */
+static void db8500_prcmu_debug_dump(const char *func,
+				bool dump_prcmu, bool dump_abb)
+{
+	printk(KERN_DEBUG"%s: timeout\n", func);
+
+	/* Dump AB8500 registers */
+	if (dump_abb)
+		abx500_dump_all_banks();
+
+	/* Dump prcmu registers and data memory */
+	if (dump_prcmu) {
+		prcmu_debug_dump_regs();
+		prcmu_debug_dump_data_mem();
+	}
+}
+
 struct prcmu_fw_version *prcmu_get_fw_version(void)
 {
 	return fw_info.valid ? &fw_info.version : NULL;
@@ -648,6 +677,11 @@ bool prcmu_has_arm_maxopp(void)
 		PRCM_AVS_ISMODEENABLE_MASK) == PRCM_AVS_ISMODEENABLE_MASK;
 }
 
+void db8500_prcmu_vc(bool enable)
+{
+	writel((enable ? 0xF : 0), (tcdm_base + PRCM_TCDM_VOICE_CALL_FLAG));
+}
+
 /**
  * prcmu_get_boot_status - PRCMU boot status checking
  * Returns: the current PRCMU boot status
@@ -1049,7 +1083,7 @@ int db8500_prcmu_set_ddr_opp(u8 opp)
 	if (opp < DDR_100_OPP || opp > DDR_25_OPP)
 		return -EINVAL;
 	/* Changing the DDR OPP can hang the hardware pre-v21 */
-	if (cpu_is_u8500v20_or_later() && !cpu_is_u8500v20())
+	if (!cpu_is_u8500v20())
 		writeb(opp, PRCM_DDR_SUBSYS_APE_MINBW);
 
 	return 0;
@@ -1111,12 +1145,14 @@ unlock_and_return:
 int db8500_prcmu_set_ape_opp(u8 opp)
 {
 	int r = 0;
+	u8 prcmu_opp_req;
 
 	if (opp == mb1_transfer.ape_opp)
 		return 0;
 
 	mutex_lock(&mb1_transfer.lock);
 
+	/* Exit APE_50_PARTLY_25_OPP */
 	if (mb1_transfer.ape_opp == APE_50_PARTLY_25_OPP)
 		request_even_slower_clocks(false);
 
@@ -1126,20 +1162,22 @@ int db8500_prcmu_set_ape_opp(u8 opp)
 	while (readl(PRCM_MBOX_CPU_VAL) & MBOX_BIT(1))
 		cpu_relax();
 
+	prcmu_opp_req  = (opp == APE_50_PARTLY_25_OPP) ? APE_50_OPP : opp;
+
 	writeb(MB1H_ARM_APE_OPP, (tcdm_base + PRCM_MBOX_HEADER_REQ_MB1));
 	writeb(ARM_NO_CHANGE, (tcdm_base + PRCM_REQ_MB1_ARM_OPP));
-	writeb(((opp == APE_50_PARTLY_25_OPP) ? APE_50_OPP : opp),
-		(tcdm_base + PRCM_REQ_MB1_APE_OPP));
+	writeb(prcmu_opp_req, (tcdm_base + PRCM_REQ_MB1_APE_OPP));
 
 	writel(MBOX_BIT(1), PRCM_MBOX_CPU_SET);
 	wait_for_completion(&mb1_transfer.work);
 
 	if ((mb1_transfer.ack.header != MB1H_ARM_APE_OPP) ||
-		(mb1_transfer.ack.ape_opp != opp))
+		(mb1_transfer.ack.ape_opp != prcmu_opp_req))
 		r = -EIO;
 
 skip_message:
 	if ((!r && (opp == APE_50_PARTLY_25_OPP)) ||
+		/* Set APE_50_PARTLY_25_OPP back in case new opp failed */
 		(r && (mb1_transfer.ape_opp == APE_50_PARTLY_25_OPP)))
 		request_even_slower_clocks(true);
 	if (!r)
@@ -1322,6 +1360,7 @@ int db8500_prcmu_set_epod(u16 epod_id, u8 epod_state)
 		pr_err("prcmu: %s timed out (20 s) waiting for a reply.\n",
 			__func__);
 		r = -EIO;
+		db8500_prcmu_debug_dump(__func__, true, true);
 		goto unlock_and_return;
 	}
 
@@ -1416,6 +1455,7 @@ static int request_sysclk(bool enable)
 		pr_err("prcmu: %s timed out (20 s) waiting for a reply.\n",
 			__func__);
 		r = -EIO;
+		db8500_prcmu_debug_dump(__func__, true, true);
 	}
 
 	mutex_unlock(&mb3_transfer.sysclk_lock);
@@ -2190,6 +2230,7 @@ int prcmu_abb_read(u8 slave, u8 reg, u8 *value, u8 size)
 		pr_err("prcmu: %s timed out (20 s) waiting for a reply.\n",
 			__func__);
 		r = -EIO;
+		db8500_prcmu_debug_dump(__func__, true, false);
 	} else {
 		r = ((mb5_transfer.ack.status == I2C_RD_OK) ? 0 : -EIO);
 	}
@@ -2240,6 +2281,7 @@ int prcmu_abb_write_masked(u8 slave, u8 reg, u8 *value, u8 *mask, u8 size)
 		pr_err("prcmu: %s timed out (20 s) waiting for a reply.\n",
 			__func__);
 		r = -EIO;
+		db8500_prcmu_debug_dump(__func__, true, false);
 	} else {
 		r = ((mb5_transfer.ack.status == I2C_WR_OK) ? 0 : -EIO);
 	}
@@ -2287,6 +2329,7 @@ retry:
 
 	if (!wait_for_completion_timeout(&mb0_transfer.ac_wake_work,
 			msecs_to_jiffies(5000))) {
+		db8500_prcmu_debug_dump(__func__, true, true);
 		pr_crit("prcmu: %s timed out (5 s) waiting for a reply.\n",
 			__func__);
 		goto unlock_and_return;
@@ -2309,6 +2352,7 @@ retry:
 		if (wait_for_completion_timeout(&mb0_transfer.ac_wake_work,
 				msecs_to_jiffies(5000)))
 			goto retry;
+		db8500_prcmu_debug_dump(__func__, true, true);
 		pr_crit("prcmu: %s timed out (5 s) waiting for AC_SLEEP_ACK.\n",
 			__func__);
 	}
@@ -2335,6 +2379,7 @@ void prcmu_ac_sleep_req()
 
 	if (!wait_for_completion_timeout(&mb0_transfer.ac_wake_work,
 			msecs_to_jiffies(5000))) {
+		db8500_prcmu_debug_dump(__func__, true, true);
 		pr_crit("prcmu: %s timed out (5 s) waiting for a reply.\n",
 			__func__);
 	}
@@ -2370,7 +2415,17 @@ void db8500_prcmu_system_reset(u16 reset_code)
  */
 u16 db8500_prcmu_get_reset_code(void)
 {
-	return readw(tcdm_base + PRCM_SW_RST_REASON);
+	return reset_code_copy;
+}
+
+/**
+ * db8500_prcmu_get_reset_status - Retrieve reset status
+ *
+ * Retrieves the value of the reset status register as read at startup.
+ */
+u32 db8500_prcmu_get_reset_status(void)
+{
+	return reset_status_copy;
 }
 
 /**
@@ -2437,6 +2492,13 @@ static bool read_mailbox_0(void)
 		if (ev & WAKEUP_BIT_SYSCLK_OK)
 			complete(&mb3_transfer.sysclk_work);
 
+		prcmu_debug_register_mbox0_event(ev,
+						 (mb0_transfer.req.dbb_irqs |
+						  mb0_transfer.req.dbb_wakeups |
+						  WAKEUP_BIT_AC_WAKE_ACK |
+						  WAKEUP_BIT_AC_SLEEP_ACK |
+						  WAKEUP_BIT_SYSCLK_OK));
+
 		ev &= mb0_transfer.req.dbb_irqs;
 
 		for (n = 0; n < NUM_PRCMU_WAKEUPS; n++) {
@@ -2561,6 +2623,7 @@ static irqreturn_t prcmu_irq_handler(int irq, void *data)
 			bits -= MBOX_BIT(n);
 			if (read_mailbox[n]())
 				r = IRQ_WAKE_THREAD;
+			prcmu_debug_register_interrupt(n);
 		}
 	}
 	return r;
@@ -2646,29 +2709,38 @@ static char *fw_project_name(u8 project)
 void __init db8500_prcmu_early_init(void)
 {
 	unsigned int i;
-	if (cpu_is_u8500v2()) {
-		void *tcpm_base = ioremap_nocache(U8500_PRCMU_TCPM_BASE, SZ_4K);
-
-		if (tcpm_base != NULL) {
-			u32 version;
-			version = readl(tcpm_base + PRCMU_FW_VERSION_OFFSET);
-			fw_info.version.project = version & 0xFF;
-			fw_info.version.api_version = (version >> 8) & 0xFF;
-			fw_info.version.func_version = (version >> 16) & 0xFF;
-			fw_info.version.errata = (version >> 24) & 0xFF;
-			fw_info.valid = true;
-			pr_info("PRCMU firmware: %s, version %d.%d.%d\n",
-				fw_project_name(fw_info.version.project),
-				(version >> 8) & 0xFF, (version >> 16) & 0xFF,
-				(version >> 24) & 0xFF);
-			iounmap(tcpm_base);
-		}
+	void *tcpm_base = ioremap_nocache(U8500_PRCMU_TCPM_BASE, SZ_4K);
+	void __iomem *sec_base;
+
+	if (tcpm_base != NULL) {
+		u32 version;
+		version = readl(tcpm_base + PRCMU_FW_VERSION_OFFSET);
+		fw_info.version.project = version & 0xFF;
+		fw_info.version.api_version = (version >> 8) & 0xFF;
+		fw_info.version.func_version = (version >> 16) & 0xFF;
+		fw_info.version.errata = (version >> 24) & 0xFF;
+		fw_info.valid = true;
+		pr_info("PRCMU firmware: %s, version %d.%d.%d\n",
+			fw_project_name(fw_info.version.project),
+			(version >> 8) & 0xFF, (version >> 16) & 0xFF,
+			(version >> 24) & 0xFF);
+		iounmap(tcpm_base);
+	}
 
-		tcdm_base = __io_address(U8500_PRCMU_TCDM_BASE);
-	} else {
-		pr_err("prcmu: Unsupported chip version\n");
-		BUG();
+	tcdm_base = __io_address(U8500_PRCMU_TCDM_BASE);
+
+	/*
+	 * Copy the value of the reset status register and if needed also
+	 * the software reset code.
+	 */
+	sec_base = ioremap_nocache(U8500_PRCMU_SEC_BASE, SZ_4K);
+	if (sec_base != NULL) {
+		reset_status_copy = readl(sec_base +
+			DB8500_SEC_PRCM_RESET_STATUS);
+		iounmap(sec_base);
 	}
+	if (reset_status_copy & DB8500_SEC_PRCM_RESET_STATUS_APE_SOFTWARE_RESET)
+		reset_code_copy = readw(tcdm_base + PRCM_SW_RST_REASON);
 
 	spin_lock_init(&mb0_transfer.lock);
 	spin_lock_init(&mb0_transfer.dbb_irqs_lock);
@@ -2734,6 +2806,7 @@ static struct regulator_consumer_supply db8500_vape_consumers[] = {
 	REGULATOR_SUPPLY("vcore", "uart2"),
 	REGULATOR_SUPPLY("v-ape", "nmk-ske-keypad.0"),
 	REGULATOR_SUPPLY("v-hsi", "ste_hsi.0"),
+	REGULATOR_SUPPLY("vddvario", "smsc911x.0"),
 };
 
 static struct regulator_consumer_supply db8500_vsmps2_consumers[] = {
@@ -2743,7 +2816,7 @@ static struct regulator_consumer_supply db8500_vsmps2_consumers[] = {
 };
 
 static struct regulator_consumer_supply db8500_b2r2_mcde_consumers[] = {
-	REGULATOR_SUPPLY("vsupply", "b2r2_bus"),
+	REGULATOR_SUPPLY("vsupply", "b2r2_core"),
 	REGULATOR_SUPPLY("vsupply", "mcde"),
 };
 
@@ -2962,9 +3035,6 @@ static int __init db8500_prcmu_probe(struct platform_device *pdev)
 {
 	int err = 0;
 
-	if (ux500_is_svp())
-		return -ENODEV;
-
 	init_prcm_registers();
 
 	/* Clean up the mailbox interrupts after pre-kernel code. */
@@ -2978,8 +3048,7 @@ static int __init db8500_prcmu_probe(struct platform_device *pdev)
 		goto no_irq_return;
 	}
 
-	if (cpu_is_u8500v20_or_later())
-		prcmu_config_esram0_deep_sleep(ESRAM0_DEEP_SLEEP_STATE_RET);
+	prcmu_config_esram0_deep_sleep(ESRAM0_DEEP_SLEEP_STATE_RET);
 
 	err = mfd_add_devices(&pdev->dev, 0, db8500_prcmu_devs,
 			      ARRAY_SIZE(db8500_prcmu_devs), NULL,
diff --git a/drivers/mfd/dbx500-prcmu-regs.h b/drivers/mfd/dbx500-prcmu-regs.h
index 3a0bf91d778..0835a57dac1 100644
--- a/drivers/mfd/dbx500-prcmu-regs.h
+++ b/drivers/mfd/dbx500-prcmu-regs.h
@@ -34,6 +34,9 @@
 #define PRCM_PER5CLK_MGT	PRCM_CLK_MGT(0x038)
 #define PRCM_PER6CLK_MGT	PRCM_CLK_MGT(0x03C)
 #define PRCM_PER7CLK_MGT	PRCM_CLK_MGT(0x040)
+#define PRCM_PWMCLK_MGT		PRCM_CLK_MGT(0x044) /* for DB5500 */
+#define PRCM_IRDACLK_MGT	PRCM_CLK_MGT(0x048) /* for DB5500 */
+#define PRCM_IRRCCLK_MGT	PRCM_CLK_MGT(0x04C) /* for DB5500 */
 #define PRCM_LCDCLK_MGT		PRCM_CLK_MGT(0x044)
 #define PRCM_BMLCLK_MGT		PRCM_CLK_MGT(0x04C)
 #define PRCM_HSITXCLK_MGT	PRCM_CLK_MGT(0x050)
@@ -124,7 +127,6 @@
 #define PRCM_ITSTATUS4		(_PRCMU_BASE + 0x168)
 #define PRCM_ITSTATUS5		(_PRCMU_BASE + 0x484)
 #define PRCM_ITCLEAR5		(_PRCMU_BASE + 0x488)
-#define PRCM_ARMIT_MASKXP70_IT	(_PRCMU_BASE + 0x1018)
 
 /* System reset register */
 #define PRCM_APE_SOFTRST	(_PRCMU_BASE + 0x228)
@@ -247,4 +249,17 @@
 /* System reset register */
 #define PRCM_APE_SOFTRST		(_PRCMU_BASE + 0x228)
 
+/* Secure read-only registers */
+#define DB8500_SEC_PRCM_RESET_STATUS 0x03C
+#define DB8500_SEC_PRCM_RESET_STATUS_A9_CPU0_WATCHDOG_RESET	BIT(0)
+#define DB8500_SEC_PRCM_RESET_STATUS_A9_CPU1_WATCHDOG_RESET	BIT(1)
+#define DB8500_SEC_PRCM_RESET_STATUS_APE_SOFTWARE_RESET		BIT(2)
+#define DB8500_SEC_PRCM_RESET_STATUS_APE_RESET			BIT(3)
+#define DB8500_SEC_PRCM_RESET_STATUS_SECURE_WATCHDOG		BIT(4)
+#define DB8500_SEC_PRCM_RESET_STATUS_POWER_ON_RESET		BIT(5)
+#define DB8500_SEC_PRCM_RESET_STATUS_A9_RESTART			BIT(6)
+#define DB8500_SEC_PRCM_RESET_STATUS_APE_RESTART		BIT(7)
+#define DB8500_SEC_PRCM_RESET_STATUS_MODEM_SOFTWARE_RESET	BIT(8)
+#define DB8500_SEC_PRCM_RESET_STATUS_BOOT_ENGI			BIT(16)
+
 #endif /* __DB8500_PRCMU_REGS_H */
diff --git a/drivers/mfd/stmpe.c b/drivers/mfd/stmpe.c
index 2dd8d49cb30..6b8f9417c00 100644
--- a/drivers/mfd/stmpe.c
+++ b/drivers/mfd/stmpe.c
@@ -772,7 +772,7 @@ static irqreturn_t stmpe_irq(int irq, void *data)
 	ret = stmpe_block_read(stmpe, israddr, num, isr);
 	if (ret < 0)
 		return IRQ_NONE;
-
+back:
 	for (i = 0; i < num; i++) {
 		int bank = num - i - 1;
 		u8 status = isr[i];
@@ -794,6 +794,22 @@ static irqreturn_t stmpe_irq(int irq, void *data)
 		stmpe_reg_write(stmpe, israddr + i, clear);
 	}
 
+	/*
+	   It may happen that on the first status read interrupt
+	   sources may not showup, so read one more time.
+	 */
+	ret = stmpe_block_read(stmpe, israddr, num, isr);
+	if (ret >= 0) {
+		for (i = 0; i < num; i++) {
+			int bank = num - i - 1;
+			u8 status = isr[i];
+
+			status &= stmpe->ier[bank];
+			if (status)
+				goto back;
+		}
+	}
+
 	return IRQ_HANDLED;
 }
 
diff --git a/drivers/mfd/tc35892.c b/drivers/mfd/tc35892.c
new file mode 100644
index 00000000000..91211f29623
--- /dev/null
+++ b/drivers/mfd/tc35892.c
@@ -0,0 +1,503 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License, version 2
+ * Author: Hanumath Prasad <hanumath.prasad@stericsson.com> for ST-Ericsson
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/tc35892.h>
+
+#define TC35892_CLKMODE_MODCTL_SLEEP		0x0
+#define TC35892_CLKMODE_MODCTL_OPERATION	(1 << 0)
+
+/**
+ * tc35892_reg_read() - read a single TC35892 register
+ * @tc35892:	Device to read from
+ * @reg:	Register to read
+ */
+int tc35892_reg_read(struct tc35892 *tc35892, u8 reg)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(tc35892->i2c, reg);
+	if (ret < 0)
+		dev_err(tc35892->dev, "failed to read reg %#x: %d\n",
+			reg, ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_reg_read);
+
+/**
+ * tc35892_reg_read() - write a single TC35892 register
+ * @tc35892:	Device to write to
+ * @reg:	Register to read
+ * @data:	Value to write
+ */
+int tc35892_reg_write(struct tc35892 *tc35892, u8 reg, u8 data)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(tc35892->i2c, reg, data);
+	if (ret < 0)
+		dev_err(tc35892->dev, "failed to write reg %#x: %d\n",
+			reg, ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_reg_write);
+
+/**
+ * tc35892_block_read() - read multiple TC35892 registers
+ * @tc35892:	Device to read from
+ * @reg:	First register
+ * @length:	Number of registers
+ * @values:	Buffer to write to
+ */
+int tc35892_block_read(struct tc35892 *tc35892, u8 reg, u8 length, u8 *values)
+{
+	int ret;
+
+	ret = i2c_smbus_read_i2c_block_data(tc35892->i2c, reg, length, values);
+	if (ret < 0)
+		dev_err(tc35892->dev, "failed to read regs %#x: %d\n",
+			reg, ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_block_read);
+
+/**
+ * tc35892_block_write() - write multiple TC35892 registers
+ * @tc35892:	Device to write to
+ * @reg:	First register
+ * @length:	Number of registers
+ * @values:	Values to write
+ */
+int tc35892_block_write(struct tc35892 *tc35892, u8 reg, u8 length,
+			const u8 *values)
+{
+	int ret;
+
+	ret = i2c_smbus_write_i2c_block_data(tc35892->i2c, reg, length,
+					     values);
+	if (ret < 0)
+		dev_err(tc35892->dev, "failed to write regs %#x: %d\n",
+			reg, ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_block_write);
+
+/**
+ * tc35892_set_bits() - set the value of a bitfield in a TC35892 register
+ * @tc35892:	Device to write to
+ * @reg:	Register to write
+ * @mask:	Mask of bits to set
+ * @values:	Value to set
+ */
+int tc35892_set_bits(struct tc35892 *tc35892, u8 reg, u8 mask, u8 val)
+{
+	int ret;
+
+	mutex_lock(&tc35892->lock);
+
+	ret = tc35892_reg_read(tc35892, reg);
+	if (ret < 0)
+		goto out;
+
+	ret &= ~mask;
+	ret |= val;
+
+	ret = tc35892_reg_write(tc35892, reg, ret);
+
+out:
+	mutex_unlock(&tc35892->lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_set_bits);
+
+static struct resource gpio_resources[] = {
+	{
+		.start	= TC35892_INT_GPIIRQ,
+		.end	= TC35892_INT_GPIIRQ,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
+static struct mfd_cell tc35892_devs[] = {
+	{
+		.name		= "tc35892-gpio",
+		.num_resources	= ARRAY_SIZE(gpio_resources),
+		.resources	= &gpio_resources[0],
+	},
+};
+
+static irqreturn_t tc35892_irq(int irq, void *data)
+{
+	struct tc35892 *tc35892 = data;
+	int status;
+
+again:
+	status = tc35892_reg_read(tc35892, TC35892_IRQST);
+	if (status < 0)
+		return IRQ_NONE;
+
+	while (status) {
+		int bit = __ffs(status);
+
+		handle_nested_irq(tc35892->irq_base + bit);
+		status &= ~(1 << bit);
+	}
+
+	/*
+	 * A dummy read or write (to any register) appears to be necessary to
+	 * have the last interrupt clear (for example, GPIO IC write) take
+	 * effect. In such a case, recheck for any interrupt which is still
+	 * pending.
+	 */
+	status = tc35892_reg_read(tc35892, TC35892_IRQST);
+	if (status)
+		goto again;
+
+	return IRQ_HANDLED;
+}
+
+static void tc35892_irq_dummy(unsigned int irq)
+{
+	/* No mask/unmask at this level */
+}
+
+static struct irq_chip tc35892_irq_chip = {
+	.name	= "tc35892",
+	.irq_mask	= tc35892_irq_dummy,
+	.irq_unmask	= tc35892_irq_dummy,
+};
+
+static int tc35892_irq_init(struct tc35892 *tc35892)
+{
+	int base = tc35892->irq_base;
+	int irq;
+
+	for (irq = base; irq < base + TC35892_NR_INTERNAL_IRQS; irq++) {
+		irq_set_chip_data(irq, tc35892);
+		irq_set_chip_and_handler(irq, &tc35892_irq_chip,
+					 handle_edge_irq);
+		irq_set_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+		set_irq_flags(irq, IRQF_VALID);
+#else
+		set_irq_noprobe(irq);
+#endif
+	}
+
+	return 0;
+}
+
+static void tc35892_irq_remove(struct tc35892 *tc35892)
+{
+	int base = tc35892->irq_base;
+	int irq;
+
+	for (irq = base; irq < base + TC35892_NR_INTERNAL_IRQS; irq++) {
+#ifdef CONFIG_ARM
+		set_irq_flags(irq, 0);
+#endif
+		irq_set_chip_and_handler(irq, NULL, NULL);
+		irq_set_chip_data(irq, NULL);
+	}
+}
+
+static int tc35892_chip_init(struct tc35892 *tc35892)
+{
+	int manf, ver, ret;
+
+	manf = tc35892_reg_read(tc35892, TC35892_MANFCODE);
+	if (manf < 0)
+		return manf;
+
+	ver = tc35892_reg_read(tc35892, TC35892_VERSION);
+	if (ver < 0)
+		return ver;
+
+	if (manf != TC35892_MANFCODE_MAGIC) {
+		dev_err(tc35892->dev, "unknown manufacturer: %#x\n", manf);
+		return -EINVAL;
+	}
+
+	dev_info(tc35892->dev, "manufacturer: %#x, version: %#x\n", manf, ver);
+
+	/*
+	 * Put everything except the IRQ module into reset;
+	 * also spare the GPIO module for any pin initialization
+	 * done during pre-kernel boot
+	 */
+	ret = tc35892_reg_write(tc35892, TC35892_RSTCTRL,
+				TC35892_RSTCTRL_TIMRST
+				| TC35892_RSTCTRL_ROTRST
+				| TC35892_RSTCTRL_KBDRST);
+	if (ret < 0)
+		return ret;
+
+	/* Clear the reset interrupt. */
+	return tc35892_reg_write(tc35892, TC35892_RSTINTCLR, 0x1);
+}
+
+static int __devinit tc35892_probe(struct i2c_client *i2c,
+				   const struct i2c_device_id *id)
+{
+	struct tc35892_platform_data *pdata = i2c->dev.platform_data;
+	struct tc35892 *tc35892;
+	int ret;
+
+	if (!i2c_check_functionality(i2c->adapter, I2C_FUNC_SMBUS_BYTE_DATA
+				     | I2C_FUNC_SMBUS_I2C_BLOCK))
+		return -EIO;
+
+	tc35892 = kzalloc(sizeof(struct tc35892), GFP_KERNEL);
+	if (!tc35892)
+		return -ENOMEM;
+
+	mutex_init(&tc35892->lock);
+
+	tc35892->dev = &i2c->dev;
+	tc35892->i2c = i2c;
+	tc35892->pdata = pdata;
+	tc35892->irq_base = pdata->irq_base;
+	tc35892->num_gpio = id->driver_data;
+
+	i2c_set_clientdata(i2c, tc35892);
+
+	ret = tc35892_chip_init(tc35892);
+	if (ret)
+		goto out_free;
+
+	ret = tc35892_irq_init(tc35892);
+	if (ret)
+		goto out_free;
+
+	ret = request_threaded_irq(tc35892->i2c->irq, NULL, tc35892_irq,
+				   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+				   "tc35892", tc35892);
+	if (ret) {
+		dev_err(tc35892->dev, "failed to request IRQ: %d\n", ret);
+		goto out_removeirq;
+	}
+
+	ret = mfd_add_devices(tc35892->dev, -1, tc35892_devs,
+			      ARRAY_SIZE(tc35892_devs), NULL,
+			      tc35892->irq_base);
+	if (ret) {
+		dev_err(tc35892->dev, "failed to add children\n");
+		goto out_freeirq;
+	}
+
+	return 0;
+
+out_freeirq:
+	free_irq(tc35892->i2c->irq, tc35892);
+out_removeirq:
+	tc35892_irq_remove(tc35892);
+out_free:
+	kfree(tc35892);
+	return ret;
+}
+
+static int __devexit tc35892_remove(struct i2c_client *client)
+{
+	struct tc35892 *tc35892 = i2c_get_clientdata(client);
+
+	mfd_remove_devices(tc35892->dev);
+
+	free_irq(tc35892->i2c->irq, tc35892);
+	tc35892_irq_remove(tc35892);
+
+	kfree(tc35892);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+static u32 sleep_regs[] = {
+	TC35892_IOPC0_L,
+	TC35892_IOPC0_H,
+	TC35892_IOPC1_L,
+	TC35892_IOPC1_H,
+	TC35892_IOPC2_L,
+	TC35892_IOPC2_H,
+	TC35892_DRIVE0_L,
+	TC35892_DRIVE0_H,
+	TC35892_DRIVE1_L,
+	TC35892_DRIVE1_H,
+	TC35892_DRIVE2_L,
+	TC35892_DRIVE2_H,
+	TC35892_DRIVE3,
+	TC35892_GPIODATA0,
+	TC35892_GPIOMASK0,
+	TC35892_GPIODATA1,
+	TC35892_GPIOMASK1,
+	TC35892_GPIODATA2,
+	TC35892_GPIOMASK2,
+	TC35892_GPIODIR0,
+	TC35892_GPIODIR1,
+	TC35892_GPIODIR2,
+	TC35892_GPIOIE0,
+	TC35892_GPIOIE1,
+	TC35892_GPIOIE2,
+	TC35892_RSTCTRL,
+	TC35892_CLKCFG,
+};
+
+static u8 sleep_regs_val[] = {
+	0x00,		/* TC35892_IOPC0_L */
+	0x00,		/* TC35892_IOPC0_H */
+	0x00,		/* TC35892_IOPC1_L */
+	0x00,		/* TC35892_IOPC1_H */
+	0x00,		/* TC35892_IOPC2_L */
+	0x00,		/* TC35892_IOPC2_H */
+	0xff,		/* TC35892_DRIVE0_L */
+	0xff,		/* TC35892_DRIVE0_H */
+	0xff,		/* TC35892_DRIVE1_L */
+	0xff,		/* TC35892_DRIVE1_H */
+	0xff,		/* TC35892_DRIVE2_L */
+	0xff,		/* TC35892_DRIVE2_H */
+	0x0f,		/* TC35892_DRIVE3 */
+	0x80,		/* TC35892_GPIODATA0 */
+	0x80,		/* TC35892_GPIOMASK0 */
+	0x80,		/* TC35892_GPIODATA1 */
+	0x80,		/* TC35892_GPIOMASK1 */
+	0x06,		/* TC35892_GPIODATA2 */
+	0x06,		/* TC35892_GPIOMASK2 */
+	0xf0,		/* TC35892_GPIODIR0 */
+	0xe0,		/* TC35892_GPIODIR1 */
+	0xee,		/* TC35892_GPIODIR2 */
+	0x0f,		/* TC35892_GPIOIE0 */
+	0x1f,		/* TC35892_GPIOIE1 */
+	0x11,		/* TC35892_GPIOIE2 */
+	0x0f,		/* TC35892_RSTCTRL */
+	0xb0		/* TC35892_CLKCFG */
+
+};
+
+static u8 sleep_regs_backup[ARRAY_SIZE(sleep_regs)];
+
+static int tc35892_suspend(struct device *dev)
+{
+	struct tc35892 *tc35892 = dev_get_drvdata(dev);
+	struct i2c_client *client = tc35892->i2c;
+	int ret = 0;
+	int i, j;
+	int val;
+
+	/* Put the system to sleep mode */
+	if (!device_may_wakeup(&client->dev)) {
+		for (i = 0; i < ARRAY_SIZE(sleep_regs); i++) {
+			val = tc35892_reg_read(tc35892,
+					       sleep_regs[i]);
+			if (val < 0)
+				goto out;
+
+			sleep_regs_backup[i] = (u8) (val & 0xff);
+		}
+
+		for (i = 0; i < ARRAY_SIZE(sleep_regs); i++) {
+			ret = tc35892_reg_write(tc35892,
+					  sleep_regs[i],
+					  sleep_regs_val[i]);
+			if (ret < 0)
+				goto fail;
+
+		}
+
+		ret = tc35892_reg_write(tc35892,
+					TC35892_CLKMODE,
+					TC35892_CLKMODE_MODCTL_SLEEP);
+	}
+out:
+	return ret;
+fail:
+	for (j = 0; j <= i; j++) {
+		ret = tc35892_reg_write(tc35892,
+					sleep_regs[i],
+					sleep_regs_backup[i]);
+		if (ret < 0)
+			break;
+	}
+	return ret;
+}
+
+static int tc35892_resume(struct device *dev)
+{
+	struct tc35892 *tc35892 = dev_get_drvdata(dev);
+	struct i2c_client *client = tc35892->i2c;
+	int ret = 0;
+	int i;
+
+	/* Enable the system into operation */
+	if (!device_may_wakeup(&client->dev))
+	{
+		ret = tc35892_reg_write(tc35892,
+					TC35892_CLKMODE,
+					TC35892_CLKMODE_MODCTL_OPERATION);
+		if (ret < 0)
+			goto out;
+
+		for (i = ARRAY_SIZE(sleep_regs) - 1; i >= 0; i--) {
+			ret = tc35892_reg_write(tc35892,
+						sleep_regs[i],
+						sleep_regs_backup[i]);
+			/* Not much to do here if we fail */
+			if (ret < 0)
+				break;
+		}
+	}
+out:
+	return ret;
+}
+
+static const struct dev_pm_ops tc35892_dev_pm_ops = {
+	.suspend = tc35892_suspend,
+	.resume  = tc35892_resume,
+};
+#endif
+
+static const struct i2c_device_id tc35892_id[] = {
+	{ "tc35892", 24 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tc35892_id);
+
+static struct i2c_driver tc35892_driver = {
+	.driver.name	= "tc35892",
+	.driver.owner	= THIS_MODULE,
+#ifdef CONFIG_PM
+	.driver.pm      = &tc35892_dev_pm_ops,
+#endif
+	.probe		= tc35892_probe,
+	.remove		= __devexit_p(tc35892_remove),
+	.id_table	= tc35892_id,
+};
+
+static int __init tc35892_init(void)
+{
+	return i2c_add_driver(&tc35892_driver);
+}
+subsys_initcall(tc35892_init);
+
+static void __exit tc35892_exit(void)
+{
+	i2c_del_driver(&tc35892_driver);
+}
+module_exit(tc35892_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TC35892 MFD core driver");
+MODULE_AUTHOR("Hanumath Prasad, Rabin Vincent");
diff --git a/drivers/mfd/tc3589x.c b/drivers/mfd/tc3589x.c
index de979742c6f..0e79fe2d214 100644
--- a/drivers/mfd/tc3589x.c
+++ b/drivers/mfd/tc3589x.c
@@ -358,16 +358,114 @@ static int __devexit tc3589x_remove(struct i2c_client *client)
 }
 
 #ifdef CONFIG_PM
+
+static u32 sleep_regs[] = {
+	TC3589x_IOPC0_L,
+	TC3589x_IOPC0_H,
+	TC3589x_IOPC1_L,
+	TC3589x_IOPC1_H,
+	TC3589x_IOPC2_L,
+	TC3589x_IOPC2_H,
+	TC3589x_DRIVE0_L,
+	TC3589x_DRIVE0_H,
+	TC3589x_DRIVE1_L,
+	TC3589x_DRIVE1_H,
+	TC3589x_DRIVE2_L,
+	TC3589x_DRIVE2_H,
+	TC3589x_DRIVE3,
+	TC3589x_GPIODATA0,
+	TC3589x_GPIOMASK0,
+	TC3589x_GPIODATA1,
+	TC3589x_GPIOMASK1,
+	TC3589x_GPIODATA2,
+	TC3589x_GPIOMASK2,
+	TC3589x_GPIODIR0,
+	TC3589x_GPIODIR1,
+	TC3589x_GPIODIR2,
+	TC3589x_GPIOIE0,
+	TC3589x_GPIOIE1,
+	TC3589x_GPIOIE2,
+	TC3589x_RSTCTRL,
+	TC3589x_CLKCFG,
+};
+
+static u8 sleep_regs_val[] = {
+	0x00,		/* TC3589x_IOPC0_L */
+	0x00,		/* TC3589x_IOPC0_H */
+	0x00,		/* TC3589x_IOPC1_L */
+	0x00,		/* TC3589x_IOPC1_H */
+	0x00,		/* TC3589x_IOPC2_L */
+	0x00,		/* TC3589x_IOPC2_H */
+	0xff,		/* TC3589x_DRIVE0_L */
+	0xff,		/* TC3589x_DRIVE0_H */
+	0xff,		/* TC3589x_DRIVE1_L */
+	0xff,		/* TC3589x_DRIVE1_H */
+	0xff,		/* TC3589x_DRIVE2_L */
+	0xff,		/* TC3589x_DRIVE2_H */
+	0x0f,		/* TC3589x_DRIVE3 */
+	0x80,		/* TC3589x_GPIODATA0 */
+	0x80,		/* TC3589x_GPIOMASK0 */
+	0x80,		/* TC3589x_GPIODATA1 */
+	0x80,		/* TC3589x_GPIOMASK1 */
+	0x06,		/* TC3589x_GPIODATA2 */
+	0x06,		/* TC3589x_GPIOMASK2 */
+	0xf0,		/* TC3589x_GPIODIR0 */
+	0xe0,		/* TC3589x_GPIODIR1 */
+	0xee,		/* TC3589x_GPIODIR2 */
+	0x0f,		/* TC3589x_GPIOIE0 */
+	0x1f,		/* TC3589x_GPIOIE1 */
+	0x11,		/* TC3589x_GPIOIE2 */
+	0x0f,		/* TC3589x_RSTCTRL */
+	0xb0		/* TC3589x_CLKCFG */
+
+};
+
+static u8 sleep_regs_backup[ARRAY_SIZE(sleep_regs)];
+
 static int tc3589x_suspend(struct device *dev)
 {
 	struct tc3589x *tc3589x = dev_get_drvdata(dev);
 	struct i2c_client *client = tc3589x->i2c;
 	int ret = 0;
+	int i, j;
+	int val;
+
+	/* Put the system to sleep mode */
+	if (!device_may_wakeup(&client->dev)) {
+		for (i = 0; i < ARRAY_SIZE(sleep_regs); i++) {
+			val = tc3589x_reg_read(tc3589x,
+					       sleep_regs[i]);
+			if (val < 0)
+				goto out;
+
+			sleep_regs_backup[i] = (u8) (val & 0xff);
+		}
 
-	/* put the system to sleep mode */
-	if (!device_may_wakeup(&client->dev))
-		ret = tc3589x_reg_write(tc3589x, TC3589x_CLKMODE,
-				TC3589x_CLKMODE_MODCTL_SLEEP);
+		for (i = 0; i < ARRAY_SIZE(sleep_regs); i++) {
+			ret = tc3589x_reg_write(tc3589x,
+					  sleep_regs[i],
+					  sleep_regs_val[i]);
+			if (ret < 0)
+				goto fail;
+
+		}
+
+		ret = tc3589x_reg_write(tc3589x,
+					TC3589x_CLKMODE,
+					TC3589x_CLKMODE_MODCTL_SLEEP);
+	} else {
+		enable_irq_wake(client->irq);
+	}
+out:
+	return ret;
+fail:
+	for (j = 0; j <= i; j++) {
+		ret = tc3589x_reg_write(tc3589x,
+					sleep_regs[i],
+					sleep_regs_backup[i]);
+		if (ret < 0)
+			break;
+	}
 
 	return ret;
 }
@@ -377,12 +475,29 @@ static int tc3589x_resume(struct device *dev)
 	struct tc3589x *tc3589x = dev_get_drvdata(dev);
 	struct i2c_client *client = tc3589x->i2c;
 	int ret = 0;
+	int i;
 
-	/* enable the system into operation */
+	/* Enable the system into operation */
 	if (!device_may_wakeup(&client->dev))
-		ret = tc3589x_reg_write(tc3589x, TC3589x_CLKMODE,
-				TC3589x_CLKMODE_MODCTL_OPERATION);
-
+	{
+		ret = tc3589x_reg_write(tc3589x,
+					TC3589x_CLKMODE,
+					TC3589x_CLKMODE_MODCTL_OPERATION);
+		if (ret < 0)
+			goto out;
+
+		for (i = ARRAY_SIZE(sleep_regs) - 1; i >= 0; i--) {
+			ret = tc3589x_reg_write(tc3589x,
+						sleep_regs[i],
+						sleep_regs_backup[i]);
+			/* Not much to do here if we fail */
+			if (ret < 0)
+				break;
+		}
+	} else {
+		disable_irq_wake(client->irq);
+	}
+out:
 	return ret;
 }
 
diff --git a/drivers/mfd/tps6105x.c b/drivers/mfd/tps6105x.c
index a293b978e27..d7b9e0c60ea 100644
--- a/drivers/mfd/tps6105x.c
+++ b/drivers/mfd/tps6105x.c
@@ -195,6 +195,7 @@ static int __devinit tps6105x_probe(struct i2c_client *client,
 	return 0;
 
 fail:
+	i2c_set_clientdata(client, NULL);
 	kfree(tps6105x);
 	return ret;
 }
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index c7795096d43..4fbe2c3f0f9 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -451,6 +451,20 @@ config ARM_CHARLCD
 	  line and the Linux version on the second line, but that's
 	  still useful.
 
+config STE_TRACE_MODEM
+	tristate "DB8500 trace Modem"
+	depends on ARCH_U8500
+	default n
+	help
+	 Select this option to enable modem tracing by APE
+
+config DBX500_MLOADER
+	tristate "Modem firmware loader for db8500"
+	default n
+	depends on UX500_SOC_DB8500 || UX500_SOC_DB5500
+	help
+	  Provides a user interface to load modem firmware on dbx500 SOCs
+
 config BMP085
 	tristate "BMP085 digital pressure sensor"
 	depends on I2C && SYSFS
@@ -461,6 +475,40 @@ config BMP085
 	  To compile this driver as a module, choose M here: the
 	  module will be called bmp085.
 
+config DISPDEV
+	bool "Display overlay device"
+	depends on FB_MCDE
+	default n
+	help
+	  This driver provides a way to use a second overlay for a display (in
+	  addition to the framebuffer). The device allows for registration of
+	  userspace buffers to be used with the overlay.
+
+config COMPDEV
+	bool "Display composition device"
+	depends on FB_MCDE && HWMEM
+	default n
+	help
+	  This driver provides a way to use several overlays for a display.
+	  This driver replaces the use of the framebuffer The device allows
+	  for posting userspace buffers to be used with the overlays.
+
+config CLONEDEV
+	bool "Display cloning device"
+	depends on FB_MCDE && HWMEM && COMPDEV
+	default n
+	help
+	  This driver provides a way to clone content between two compdev
+	  devices.
+
+config CLONEDEV_DEBUG
+	bool "Display cloning device debug"
+	depends on CLONEDEV
+	default n
+	help
+	  This driver provides a way to clone content between two compdev
+	  devices.
+
 config PCH_PHUB
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) PHUB"
 	depends on PCI
@@ -481,6 +529,30 @@ config PCH_PHUB
 	  To compile this driver as a module, choose M here: the module will
 	  be called pch_phub.
 
+config HWMEM
+	bool "Hardware memory driver"
+	default n
+	help
+	  This driver provides a way to allocate contiguous system memory which
+	  can be used by hardware. It also enables accessing hwmem allocated
+	  memory buffers through a secure id which can be shared across processes.
+
+config U5500_MBOX
+	bool "Mailbox support"
+	depends on (UX500_SOC_DB5500 && U5500_MODEM_IRQ)
+	default y
+	help
+	  Add support for U5500 mailbox communication with modem side
+
+config U8500_SIM_DETECT
+	bool "Sim hot swap detection support"
+	depends on (MODEM && UX500_SOC_DB8500)
+	default n
+	help
+	  Add support for sim hot swap detection support in U8500.Driver
+	  basically wakes up the modem if its sleeping when sim hot plug
+	  in/out has happened.
+
 config USB_SWITCH_FSA9480
 	tristate "FSA9480 USB Switch"
 	depends on I2C
@@ -498,6 +570,7 @@ config MAX8997_MUIC
 	  Maxim MAX8997 PMIC.
 	  The MAX8997 MUIC is a USB port accessory detector and switch.
 
+source "drivers/misc/Kconfig.stm"
 source "drivers/misc/c2port/Kconfig"
 source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
@@ -506,4 +579,5 @@ source "drivers/misc/ti-st/Kconfig"
 source "drivers/misc/lis3lv02d/Kconfig"
 source "drivers/misc/carma/Kconfig"
 source "drivers/misc/altera-stapl/Kconfig"
+source "drivers/misc/modem_audio/Kconfig"
 endmenu
diff --git a/drivers/misc/Kconfig.stm b/drivers/misc/Kconfig.stm
new file mode 100644
index 00000000000..d509c85c79f
--- /dev/null
+++ b/drivers/misc/Kconfig.stm
@@ -0,0 +1,120 @@
+menuconfig STM_TRACE
+	bool "STM MIPI Trace driver"
+	depends on ARCH_U8500
+	help
+	  Simple System Trace Module driver. It allows to use and configure the
+	  STM, either from kernel space, or from user space.
+
+if STM_TRACE
+
+config STM_NUMBER_OF_CHANNEL
+	int
+	default 512 if ARCH_U8500
+	default 256
+	help
+	  Number Max of channels always a multiple of 256
+
+config STM_DEFAULT_MASTERS_MODES
+	hex "channel mode"
+	default 0xffffffff
+	help
+	  Default config for enabling hardware mode tracing
+
+config STM_PRINTK
+	bool "printk support"
+	depends on STM_TRACE
+	help
+	  Duplicate printk output on STM printk channel & activate stm_printk
+
+config STM_PRINTK_CHANNEL
+	int "printk channel"
+	range 0 255
+	depends on STM_PRINTK
+	default 255
+	help
+	  STM printk channel number
+
+config STM_FTRACE
+	bool "functions tracing"
+	depends on FTRACE
+	default y
+	help
+	  Output function tracing on STM dedicated channel
+
+config STM_FTRACE_CHANNEL
+	int "ftrace channel"
+	range 0 255
+	depends on STM_FTRACE
+	default 254
+	help
+	  STM ftrace channel number
+
+config STM_CTX_SWITCH
+	bool "Context switch tracing"
+	depends on CONTEXT_SWITCH_TRACER
+	default y
+	help
+	  Output scheduler context switch on STM dedicated channel
+
+config STM_CTX_SWITCH_CHANNEL
+	int "Context switch channel"
+	range 0 255
+	depends on STM_CTX_SWITCH
+	default 253
+	help
+	  STM Context switch channel number
+
+config STM_WAKEUP
+	bool "Scheduler wakeup tracing"
+	depends on CONTEXT_SWITCH_TRACER
+	default y
+	help
+	  Output scheduler wakeup on STM dedicated channel
+
+config STM_WAKEUP_CHANNEL
+	int "Wakeup channel"
+	range 0 255
+	depends on STM_WAKEUP
+	default 252
+	help
+	  STM scheduler wakeup channel number
+
+config STM_STACK_TRACE
+	bool "Stack tracing"
+	depends on STACKTRACE
+	default y
+	help
+	  Output stack tracing on STM dedicated channel
+
+config STM_STACK_TRACE_CHANNEL
+	int "Stack trace channel"
+	range 0 255
+	depends on STM_STACK_TRACE
+	default 251
+	help
+	  STM stack trace channel number
+
+config STM_TRACE_PRINTK
+	bool "trace printk & binary printk support"
+	depends on TRACING
+	default y
+	help
+	  Duplicate trace printk output on STM printk channel
+
+config STM_TRACE_PRINTK_CHANNEL
+	int "trace_printk channel"
+	range 0 255
+	depends on TRACING
+	default 250
+	help
+	  STM trace_printk channel number
+
+config STM_TRACE_BPRINTK_CHANNEL
+	int "trace_bprintk channel"
+	range 0 255
+	depends on TRACING
+	default 249
+	help
+	  STM trace binary printk channel number
+
+endif
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 3e1d80106f0..fc02851cbe8 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -46,6 +46,16 @@ obj-y				+= ti-st/
 obj-$(CONFIG_AB8500_PWM)	+= ab8500-pwm.o
 obj-y				+= lis3lv02d/
 obj-y				+= carma/
+obj-$(CONFIG_STM_TRACE) 	+= stm.o
+obj-$(CONFIG_HWMEM)             += hwmem/
+obj-$(CONFIG_DISPDEV)		+= dispdev/
+obj-$(CONFIG_COMPDEV)		+= compdev/
+obj-$(CONFIG_CLONEDEV)		+= clonedev/
+obj-$(CONFIG_STE_TRACE_MODEM) 	+= db8500-modem-trace.o
+obj-$(CONFIG_DBX500_MLOADER) 	+= dbx500-mloader.o
+obj-$(CONFIG_U5500_MBOX)	+= mbox.o mbox_channels-db5500.o
+obj-$(CONFIG_U8500_SIM_DETECT)	+= sim_detect.o
 obj-$(CONFIG_USB_SWITCH_FSA9480) += fsa9480.o
 obj-$(CONFIG_ALTERA_STAPL)	+=altera-stapl/
 obj-$(CONFIG_MAX8997_MUIC)	+= max8997-muic.o
+obj-y				+= modem_audio/
diff --git a/drivers/misc/ab8500-pwm.c b/drivers/misc/ab8500-pwm.c
index d7a9aa14e5d..9d864e4db5a 100644
--- a/drivers/misc/ab8500-pwm.c
+++ b/drivers/misc/ab8500-pwm.c
@@ -8,10 +8,11 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/pwm.h>
+#include <linux/clk.h>
 #include <linux/mfd/abx500.h>
 #include <linux/mfd/abx500/ab8500.h>
 #include <linux/module.h>
-
+#include <linux/mfd/ab8500/pwmleds.h>
 /*
  * PWM Out generators
  * Bank: 0x10
@@ -19,6 +20,11 @@
 #define AB8500_PWM_OUT_CTRL1_REG	0x60
 #define AB8500_PWM_OUT_CTRL2_REG	0x61
 #define AB8500_PWM_OUT_CTRL7_REG	0x66
+#define AB8505_PWM_OUT_BLINK_CTRL1_REG  0x68
+#define AB8505_PWM_OUT_BLINK_CTRL4_REG  0x6B
+#define AB8505_PWM_OUT_BLINK_CTRL_DUTYBIT 4
+#define AB8505_PWM_OUT_BLINK_DUTYMASK (0x0F << AB8505_PWM_OUT_BLINK_CTRL_DUTYBIT)
+
 
 /* backlight driver constants */
 #define ENABLE_PWM			1
@@ -27,12 +33,73 @@
 struct pwm_device {
 	struct device *dev;
 	struct list_head node;
+	struct clk *clk;
 	const char *label;
 	unsigned int pwm_id;
+	unsigned int num_pwm;
+	unsigned int blink_en;
+	struct ab8500 *parent;
+	bool clk_enabled;
 };
 
 static LIST_HEAD(pwm_list);
 
+int pwm_config_blink(struct pwm_device *pwm, int duty_ns, int period_ns)
+{
+	int ret;
+	unsigned int value;
+	u8 reg;
+	if ((!is_ab8505(pwm->parent)) || (!pwm->blink_en)) {
+		dev_err(pwm->dev, "setting blinking for this "
+					"device not supported\n");
+		return -EINVAL;
+	}
+	/*
+	 * get the period value that is to be written to
+	 * AB8500_PWM_OUT_BLINK_CTRL1 REGS[0:2]
+	 */
+	value = period_ns & 0x07;
+	/*
+	 * get blink duty value to be written to
+	 * AB8500_PWM_OUT_BLINK_CTRL REGS[7:4]
+	 */
+	value |= ((duty_ns << AB8505_PWM_OUT_BLINK_CTRL_DUTYBIT) &
+					AB8505_PWM_OUT_BLINK_DUTYMASK);
+
+	reg = AB8505_PWM_OUT_BLINK_CTRL1_REG + (pwm->pwm_id - 1);
+
+	ret = abx500_set_register_interruptible(pwm->dev, AB8500_MISC,
+			reg, (u8)value);
+	if (ret < 0)
+		dev_err(pwm->dev, "%s: Failed to config PWM blink,Error %d\n",
+							pwm->label, ret);
+	return ret;
+}
+EXPORT_SYMBOL(pwm_config_blink);
+
+int pwm_blink_ctrl(struct pwm_device *pwm , int enable)
+{
+	int ret;
+
+	if ((!is_ab8505(pwm->parent)) || (!pwm->blink_en)) {
+		dev_err(pwm->dev, "setting blinking for this "
+					"device not supported\n");
+		return -EINVAL;
+	}
+	/*
+	 * Enable/disable blinking feature for corresponding PWMOUT
+	 * channel depending on value of enable.
+	 */
+	ret = abx500_mask_and_set_register_interruptible(pwm->dev,
+			AB8500_MISC, AB8505_PWM_OUT_BLINK_CTRL4_REG,
+			1 << (pwm->pwm_id-1), enable << (pwm->pwm_id-1));
+	if (ret < 0)
+		dev_err(pwm->dev, "%s: Failed to control PWM blink,Error %d\n",
+							pwm->label, ret);
+	return ret;
+}
+EXPORT_SYMBOL(pwm_blink_ctrl);
+
 int pwm_config(struct pwm_device *pwm, int duty_ns, int period_ns)
 {
 	int ret = 0;
@@ -67,11 +134,19 @@ int pwm_enable(struct pwm_device *pwm)
 {
 	int ret;
 
+	if (!pwm->clk_enabled) {
+		ret = clk_enable(pwm->clk);
+		if (ret < 0) {
+			dev_err(pwm->dev, "failed to enable clock\n");
+			return ret;
+		}
+		pwm->clk_enabled = true;
+	}
 	ret = abx500_mask_and_set_register_interruptible(pwm->dev,
 				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
-				1 << (pwm->pwm_id-1), ENABLE_PWM);
+				1 << (pwm->pwm_id-1), 1 << (pwm->pwm_id-1));
 	if (ret < 0)
-		dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
+		dev_err(pwm->dev, "%s: Failed to enable PWM, Error %d\n",
 							pwm->label, ret);
 	return ret;
 }
@@ -84,9 +159,27 @@ void pwm_disable(struct pwm_device *pwm)
 	ret = abx500_mask_and_set_register_interruptible(pwm->dev,
 				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
 				1 << (pwm->pwm_id-1), DISABLE_PWM);
+	/*
+	 * Workaround to set PWM in disable.
+	 * If enable bit is not toggled the PWM might output 50/50 duty cycle
+	 * even though it should be disabled
+	 */
+	ret &= abx500_mask_and_set_register_interruptible(pwm->dev,
+				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
+				1 << (pwm->pwm_id-1),
+				ENABLE_PWM << (pwm->pwm_id-1));
+	ret &= abx500_mask_and_set_register_interruptible(pwm->dev,
+				AB8500_MISC, AB8500_PWM_OUT_CTRL7_REG,
+				1 << (pwm->pwm_id-1), DISABLE_PWM);
+
 	if (ret < 0)
 		dev_err(pwm->dev, "%s: Failed to disable PWM, Error %d\n",
 							pwm->label, ret);
+	if (pwm->clk_enabled) {
+		clk_disable(pwm->clk);
+		pwm->clk_enabled = false;
+	}
+
 	return;
 }
 EXPORT_SYMBOL(pwm_disable);
@@ -94,7 +187,6 @@ EXPORT_SYMBOL(pwm_disable);
 struct pwm_device *pwm_request(int pwm_id, const char *label)
 {
 	struct pwm_device *pwm;
-
 	list_for_each_entry(pwm, &pwm_list, node) {
 		if (pwm->pwm_id == pwm_id) {
 			pwm->label = label;
@@ -113,30 +205,131 @@ void pwm_free(struct pwm_device *pwm)
 }
 EXPORT_SYMBOL(pwm_free);
 
+static ssize_t store_blink_status(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct pwm_device *pwm;
+	unsigned long val;
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+	list_for_each_entry(pwm, &pwm_list, node) {
+		if (pwm->pwm_id == val)
+			break;
+		else {
+			/* check if PWM ID is valid*/
+			if (val > pwm->num_pwm) {
+				dev_err(pwm->dev, "Invalid PWM ID\n");
+				return -EINVAL;
+			}
+		}
+	}
+	if ((!is_ab8505(pwm->parent)) || (!pwm->blink_en)) {
+		dev_err(pwm->dev, "setting blinking for this "
+					"device not supported\n");
+		return -EINVAL;
+	}
+	/*Disable blink functionlity */
+	pwm_blink_ctrl(pwm, 0);
+	return count;
+}
+
+static DEVICE_ATTR(disable_blink, S_IWUGO, NULL, store_blink_status);
+
+static struct attribute *pwmled_attributes[] = {
+	&dev_attr_disable_blink.attr,
+	NULL
+};
+
+static const struct attribute_group pwmled_attr_group = {
+	.attrs = pwmled_attributes,
+};
+
 static int __devinit ab8500_pwm_probe(struct platform_device *pdev)
 {
+	struct ab8500 *parent = dev_get_drvdata(pdev->dev.parent);
+	struct ab8500_platform_data *plat = dev_get_platdata(parent->dev);
+	struct ab8500_pwmled_platform_data *pdata;
 	struct pwm_device *pwm;
+	int ret = 0 , i;
+
+	/* get pwmled specific platform data */
+	if (!plat->pwmled) {
+		dev_err(&pdev->dev, "no pwm platform data supplied\n");
+		return -EINVAL;
+	}
+	pdata = plat->pwmled;
 	/*
 	 * Nothing to be done in probe, this is required to get the
 	 * device which is required for ab8500 read and write
 	 */
-	pwm = kzalloc(sizeof(struct pwm_device), GFP_KERNEL);
+	pwm = kzalloc(((sizeof(struct pwm_device)) * pdata->num_pwm),
+						 GFP_KERNEL);
 	if (pwm == NULL) {
 		dev_err(&pdev->dev, "failed to allocate memory\n");
 		return -ENOMEM;
 	}
-	pwm->dev = &pdev->dev;
-	pwm->pwm_id = pdev->id;
-	list_add_tail(&pwm->node, &pwm_list);
+	for (i = 0; i < pdata->num_pwm; i++) {
+		pwm[i].dev = &pdev->dev;
+		pwm[i].parent = parent;
+		pwm[i].blink_en = pdata->leds[i].blink_en;
+		pwm[i].pwm_id = pdata->leds[i].pwm_id;
+		pwm[i].num_pwm = pdata->num_pwm;
+		list_add_tail(&pwm[i].node, &pwm_list);
+	}
+	for (i = 0; i < pdata->num_pwm; i++) {
+		/*Implement sysfs only if blink is enabled*/
+		if ((is_ab8505(pwm[i].parent)) && (pwm[i].blink_en)) {
+			/* sysfs implementation to disable the blink */
+			ret = sysfs_create_group(&pdev->dev.kobj,
+							&pwmled_attr_group);
+			if (ret) {
+				dev_err(&pdev->dev, "failed to create"
+						" sysfs entries\n");
+				goto fail;
+			}
+			break;
+		}
+	}
+	pwm->clk = clk_get(pwm->dev, NULL);
+	if (IS_ERR(pwm->clk)) {
+		dev_err(pwm->dev, "clock request failed\n");
+		ret = PTR_ERR(pwm->clk);
+		goto err_clk;
+	}
 	platform_set_drvdata(pdev, pwm);
+	pwm->clk_enabled = false;
 	dev_dbg(pwm->dev, "pwm probe successful\n");
-	return 0;
+	return ret;
+
+err_clk:
+	for (i = 0; i < pdata->num_pwm; i++) {
+		if ((is_ab8505(pwm[i].parent)) && (pwm[i].blink_en)) {
+			sysfs_remove_group(&pdev->dev.kobj,
+				&pwmled_attr_group);
+			break;
+		}
+	}
+fail:
+	list_del(&pwm->node);
+	kfree(pwm);
+	return ret;
 }
 
 static int __devexit ab8500_pwm_remove(struct platform_device *pdev)
 {
 	struct pwm_device *pwm = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < pwm->num_pwm; i++) {
+		if ((is_ab8505(pwm[i].parent)) && (pwm[i].blink_en)) {
+			sysfs_remove_group(&pdev->dev.kobj,
+				&pwmled_attr_group);
+			break;
+		}
+	}
 	list_del(&pwm->node);
+	clk_put(pwm->clk);
 	dev_dbg(&pdev->dev, "pwm driver removed\n");
 	kfree(pwm);
 	return 0;
diff --git a/drivers/misc/bh1780gli.c b/drivers/misc/bh1780gli.c
index 54f6f39f990..1035cb37695 100644
--- a/drivers/misc/bh1780gli.c
+++ b/drivers/misc/bh1780gli.c
@@ -18,11 +18,17 @@
  * this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #include <linux/i2c.h>
+#include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/mutex.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/module.h>
+#include <linux/regulator/consumer.h>
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
 
 #define BH1780_REG_CONTROL	0x80
 #define BH1780_REG_PARTID	0x8A
@@ -40,11 +46,20 @@
 
 struct bh1780_data {
 	struct i2c_client *client;
+	struct regulator *regulator;
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	struct early_suspend early_suspend;
+#endif
 	int power_state;
 	/* lock for sysfs operations */
 	struct mutex lock;
 };
 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void bh1780_early_suspend(struct early_suspend *ddata);
+static void bh1780_late_resume(struct early_suspend *ddata);
+#endif
+
 static int bh1780_write(struct bh1780_data *ddata, u8 reg, u8 val, char *msg)
 {
 	int ret = i2c_smbus_write_byte_data(ddata->client, reg, val);
@@ -72,6 +87,9 @@ static ssize_t bh1780_show_lux(struct device *dev,
 	struct bh1780_data *ddata = platform_get_drvdata(pdev);
 	int lsb, msb;
 
+	if (ddata->power_state == BH1780_POFF)
+		return -EINVAL;
+
 	lsb = bh1780_read(ddata, BH1780_REG_DLOW, "DLOW");
 	if (lsb < 0)
 		return lsb;
@@ -89,13 +107,9 @@ static ssize_t bh1780_show_power_state(struct device *dev,
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct bh1780_data *ddata = platform_get_drvdata(pdev);
-	int state;
-
-	state = bh1780_read(ddata, BH1780_REG_CONTROL, "CONTROL");
-	if (state < 0)
-		return state;
 
-	return sprintf(buf, "%d\n", state & BH1780_POWMASK);
+	/* we already maintain a sw state */
+	return sprintf(buf, "%d\n", ddata->power_state);
 }
 
 static ssize_t bh1780_store_power_state(struct device *dev,
@@ -104,7 +118,7 @@ static ssize_t bh1780_store_power_state(struct device *dev,
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct bh1780_data *ddata = platform_get_drvdata(pdev);
-	unsigned long val;
+	long val;
 	int error;
 
 	error = strict_strtoul(buf, 0, &val);
@@ -114,15 +128,25 @@ static ssize_t bh1780_store_power_state(struct device *dev,
 	if (val < BH1780_POFF || val > BH1780_PON)
 		return -EINVAL;
 
+	if (ddata->power_state == val)
+		return count;
+
 	mutex_lock(&ddata->lock);
 
+	if (ddata->power_state == BH1780_POFF)
+		regulator_enable(ddata->regulator);
+
 	error = bh1780_write(ddata, BH1780_REG_CONTROL, val, "CONTROL");
 	if (error < 0) {
 		mutex_unlock(&ddata->lock);
+		regulator_disable(ddata->regulator);
 		return error;
 	}
 
-	msleep(BH1780_PON_DELAY);
+	if (val == BH1780_POFF)
+		regulator_disable(ddata->regulator);
+
+	mdelay(BH1780_PON_DELAY);
 	ddata->power_state = val;
 	mutex_unlock(&ddata->lock);
 
@@ -131,7 +155,7 @@ static ssize_t bh1780_store_power_state(struct device *dev,
 
 static DEVICE_ATTR(lux, S_IRUGO, bh1780_show_lux, NULL);
 
-static DEVICE_ATTR(power_state, S_IWUSR | S_IRUGO,
+static DEVICE_ATTR(power_state, S_IWUGO | S_IRUGO,
 		bh1780_show_power_state, bh1780_store_power_state);
 
 static struct attribute *bh1780_attributes[] = {
@@ -153,21 +177,42 @@ static int __devinit bh1780_probe(struct i2c_client *client,
 
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE)) {
 		ret = -EIO;
-		goto err_op_failed;
+		return ret;
 	}
 
 	ddata = kzalloc(sizeof(struct bh1780_data), GFP_KERNEL);
 	if (ddata == NULL) {
+		dev_err(&client->dev, "failed to alloc ddata\n");
 		ret = -ENOMEM;
-		goto err_op_failed;
+		return ret;
 	}
 
 	ddata->client = client;
 	i2c_set_clientdata(client, ddata);
 
+	ddata->regulator = regulator_get(&client->dev, "vcc");
+	if (IS_ERR(ddata->regulator)) {
+		dev_err(&client->dev, "failed to get regulator\n");
+		ret = PTR_ERR(ddata->regulator);
+		goto free_ddata;
+	}
+
+	regulator_enable(ddata->regulator);
+
 	ret = bh1780_read(ddata, BH1780_REG_PARTID, "PART ID");
-	if (ret < 0)
-		goto err_op_failed;
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to read part ID\n");
+		goto disable_regulator;
+	}
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	ddata->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1;
+	ddata->early_suspend.suspend = bh1780_early_suspend;
+	ddata->early_suspend.resume = bh1780_late_resume;
+	register_early_suspend(&ddata->early_suspend);
+#endif
+
+	regulator_disable(ddata->regulator);
+	ddata->power_state = BH1780_POFF;
 
 	dev_info(&client->dev, "Ambient Light Sensor, Rev : %d\n",
 			(ret & BH1780_REVMASK));
@@ -175,12 +220,17 @@ static int __devinit bh1780_probe(struct i2c_client *client,
 	mutex_init(&ddata->lock);
 
 	ret = sysfs_create_group(&client->dev.kobj, &bh1780_attr_group);
-	if (ret)
-		goto err_op_failed;
+	if (ret) {
+		dev_err(&client->dev, "failed to create sysfs group\n");
+		goto put_regulator;
+	}
 
 	return 0;
-
-err_op_failed:
+disable_regulator:
+	regulator_disable(ddata->regulator);
+put_regulator:
+	regulator_put(ddata->regulator);
+free_ddata:
 	kfree(ddata);
 	return ret;
 }
@@ -196,50 +246,106 @@ static int __devexit bh1780_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int bh1780_suspend(struct device *dev)
+#if defined(CONFIG_HAS_EARLYSUSPEND) || defined(CONFIG_PM)
+static int bh1780_do_suspend(struct bh1780_data *ddata)
 {
-	struct bh1780_data *ddata;
-	int state, ret;
-	struct i2c_client *client = to_i2c_client(dev);
+	int ret = 0;
 
-	ddata = i2c_get_clientdata(client);
-	state = bh1780_read(ddata, BH1780_REG_CONTROL, "CONTROL");
-	if (state < 0)
-		return state;
+	mutex_lock(&ddata->lock);
 
-	ddata->power_state = state & BH1780_POWMASK;
+	if (ddata->power_state == BH1780_POFF)
+		goto unlock;
 
-	ret = bh1780_write(ddata, BH1780_REG_CONTROL, BH1780_POFF,
-				"CONTROL");
+	ret = bh1780_write(ddata, BH1780_REG_CONTROL, BH1780_POFF, "CONTROL");
 
 	if (ret < 0)
-		return ret;
+		goto unlock;
 
-	return 0;
+	if (ddata->regulator)
+		regulator_disable(ddata->regulator);
+unlock:
+	mutex_unlock(&ddata->lock);
+	return ret;
 }
 
-static int bh1780_resume(struct device *dev)
+static int bh1780_do_resume(struct bh1780_data *ddata)
 {
-	struct bh1780_data *ddata;
-	int state, ret;
-	struct i2c_client *client = to_i2c_client(dev);
+	int ret = 0;
 
-	ddata = i2c_get_clientdata(client);
-	state = ddata->power_state;
-	ret = bh1780_write(ddata, BH1780_REG_CONTROL, state,
-				"CONTROL");
+	mutex_lock(&ddata->lock);
+
+	if (ddata->power_state == BH1780_POFF)
+		goto unlock;
 
+	if (ddata->regulator)
+		regulator_enable(ddata->regulator);
+
+	ret = bh1780_write(ddata, BH1780_REG_CONTROL,
+					ddata->power_state, "CONTROL");
+
+unlock:
+	mutex_unlock(&ddata->lock);
+	return ret;
+}
+#endif
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#ifdef CONFIG_PM
+static int bh1780_suspend(struct device *dev)
+{
+	struct bh1780_data *ddata = dev_get_drvdata(dev);
+	int ret = 0;
+
+	ret = bh1780_do_suspend(ddata);
 	if (ret < 0)
-		return ret;
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device\n");
 
-	return 0;
+	return ret;
 }
+
+static int bh1780_resume(struct device *dev)
+{
+	struct bh1780_data *ddata = dev_get_drvdata(dev);
+	int ret = 0;
+
+	ret = bh1780_do_resume(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device\n");
+
+	return ret;
+}
+
 static SIMPLE_DEV_PM_OPS(bh1780_pm, bh1780_suspend, bh1780_resume);
 #define BH1780_PMOPS (&bh1780_pm)
+#endif /* CONFIG_PM */
 #else
 #define BH1780_PMOPS NULL
-#endif /* CONFIG_PM */
+static void bh1780_early_suspend(struct early_suspend *data)
+{
+	struct bh1780_data *ddata =
+		container_of(data, struct bh1780_data, early_suspend);
+	int ret;
+
+	ret = bh1780_do_suspend(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while suspending the device\n");
+}
+
+static void bh1780_late_resume(struct early_suspend *data)
+{
+	struct bh1780_data *ddata =
+		container_of(data, struct bh1780_data, early_suspend);
+	int ret;
+
+	ret = bh1780_do_resume(ddata);
+	if (ret < 0)
+		dev_err(&ddata->client->dev,
+				"Error while resuming the device\n");
+}
+#endif /*!CONFIG_HAS_EARLYSUSPEND */
 
 static const struct i2c_device_id bh1780_id[] = {
 	{ "bh1780", 0 },
@@ -252,7 +358,9 @@ static struct i2c_driver bh1780_driver = {
 	.id_table	= bh1780_id,
 	.driver = {
 		.name = "bh1780",
+#if (!defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM))
 		.pm	= BH1780_PMOPS,
+#endif
 	},
 };
 
diff --git a/drivers/misc/clonedev/Makefile b/drivers/misc/clonedev/Makefile
new file mode 100644
index 00000000000..f84859dd3ee
--- /dev/null
+++ b/drivers/misc/clonedev/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_CLONEDEV) += clonedev.o
+
+ifdef CONFIG_CLONEDEV_DEBUG
+EXTRA_CFLAGS += -DDEBUG
+endif
diff --git a/drivers/misc/clonedev/clonedev.c b/drivers/misc/clonedev/clonedev.c
new file mode 100644
index 00000000000..d3b770fd324
--- /dev/null
+++ b/drivers/misc/clonedev/clonedev.c
@@ -0,0 +1,312 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Device for display cloning on external output.
+ *
+ * Author: Per-Daniel Olsson <per-daniel.olsson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/ioctl.h>
+
+#include <linux/clonedev.h>
+
+#include <linux/compdev.h>
+#include <linux/mm.h>
+#include <video/mcde.h>
+
+static LIST_HEAD(dev_list);
+static DEFINE_MUTEX(dev_list_lock);
+
+struct clonedev {
+	struct mutex lock;
+	struct miscdevice mdev;
+	struct list_head list;
+	bool open;
+	struct compdev *src_compdev;
+	struct compdev *dst_compdev;
+	bool overlay_case;
+	struct compdev_size dst_size;
+	struct compdev_scene_info s_info;
+};
+
+static void best_fit(struct compdev_rect *src_rect,
+		struct compdev_size *dst_size,
+		struct compdev_img *img)
+{
+	/* aspect ratio in 26.6 fixed point */
+	int aspect = 1;
+	int dst_w;
+	int dst_h;
+
+	if (img->rotation == COMPDEV_ROT_90_CCW ||
+			img->rotation == COMPDEV_ROT_270_CCW)
+		aspect = (src_rect->height << 6) / src_rect->width;
+	else
+		aspect = (src_rect->width << 6) / src_rect->height;
+
+	dst_w = aspect * dst_size->height >> 6;
+	dst_h = dst_size->height;
+	img->dst_rect.y = 0;
+
+	if (dst_w > dst_size->width) {
+		/*
+		 * Destination rectangle too wide.
+		 * Clamp to image width. Keep aspect ratio.
+		 */
+		dst_h = (dst_size->width << 6) / aspect;
+		dst_w = dst_size->width;
+	}
+
+	/* center the image */
+	if (dst_w < dst_size->width) {
+		int offset = (dst_size->width - dst_w) / 2;
+		img->dst_rect.x = offset;
+	}
+
+	if (dst_h < dst_size->height) {
+		int offset = (dst_size->height - dst_h) / 2;
+		img->dst_rect.y = offset;
+	}
+
+	img->dst_rect.width = dst_w;
+	img->dst_rect.height = dst_h;
+}
+
+static int clonedev_open(struct inode *inode, struct file *file)
+{
+	struct clonedev *cd = NULL;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry(cd, &dev_list, list)
+		if (cd->mdev.minor == iminor(inode))
+			break;
+
+	if (&cd->list == &dev_list) {
+		mutex_unlock(&dev_list_lock);
+		return -ENODEV;
+	}
+
+	if (cd->open) {
+		mutex_unlock(&dev_list_lock);
+		return -EBUSY;
+	}
+
+	cd->open = true;
+
+	mutex_unlock(&dev_list_lock);
+
+	file->private_data = cd;
+
+	return 0;
+}
+
+static int clonedev_release(struct inode *inode, struct file *file)
+{
+	struct clonedev *cd = NULL;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry(cd, &dev_list, list)
+		if (cd->mdev.minor == iminor(inode))
+			break;
+	mutex_unlock(&dev_list_lock);
+
+	if (&cd->list == &dev_list)
+		return -ENODEV;
+
+	cd->open = false;
+	return 0;
+}
+
+static long clonedev_ioctl(struct file *file,
+		unsigned int cmd,
+		unsigned long arg)
+{
+	int ret;
+	struct clonedev *cd = (struct clonedev *)file->private_data;
+
+	mutex_lock(&cd->lock);
+
+	switch (cmd) {
+	case CLONEDEV_SET_MODE_IOC:
+		/* TODO: Get the user data */
+
+		break;
+
+	default:
+		ret = -ENOSYS;
+	}
+
+	mutex_unlock(&cd->lock);
+
+	return ret;
+}
+
+static const struct file_operations clonedev_fops = {
+	.open = clonedev_open,
+	.release = clonedev_release,
+	.unlocked_ioctl = clonedev_ioctl,
+};
+
+static void init_clonedev(struct clonedev *cd, const char *name)
+{
+	mutex_init(&cd->lock);
+	INIT_LIST_HEAD(&cd->list);
+
+	cd->mdev.minor = MISC_DYNAMIC_MINOR;
+	cd->mdev.name = name;
+	cd->mdev.fops = &clonedev_fops;
+}
+
+static void clonedev_post_buffer_callback(void *data,
+		struct compdev_img *cb_img)
+{
+	struct clonedev *cd = (struct clonedev *)data;
+
+	mutex_lock(&cd->lock);
+
+	if (!cd->overlay_case || (cd->overlay_case &&
+			(cb_img->flags & COMPDEV_OVERLAY_FLAG))) {
+		struct compdev_img img;
+
+		img = *cb_img;
+
+		if (img.flags & COMPDEV_BYPASS_FLAG)
+			img.flags &= ~COMPDEV_BYPASS_FLAG;
+
+		if (cd->overlay_case)
+			img.rotation = cd->s_info.ovly_rotation;
+		else
+			img.rotation = cd->s_info.fb_rotation;
+
+		best_fit(&img.src_rect, &cd->dst_size, &img);
+
+		compdev_post_buffer(cd->dst_compdev, &img);
+	}
+	mutex_unlock(&cd->lock);
+}
+
+static void clonedev_post_scene_info_callback(void *data,
+		struct compdev_scene_info *s_info)
+{
+	struct clonedev *cd = (struct clonedev *)data;
+
+	mutex_lock(&cd->lock);
+	if (s_info->img_count > 1)
+		cd->overlay_case = true;
+	else
+		cd->overlay_case = false;
+
+	cd->s_info = *s_info;
+	cd->s_info.img_count = 1;
+	compdev_post_scene_info(cd->dst_compdev, &cd->s_info);
+	mutex_unlock(&cd->lock);
+}
+
+int clonedev_create(void)
+{
+	int ret;
+	struct clonedev *cd;
+
+	static int counter;
+	char name[10];
+
+	cd = kzalloc(sizeof(struct clonedev), GFP_KERNEL);
+	if (!cd)
+		return -ENOMEM;
+
+	snprintf(name, sizeof(name), "%s%d", CLONEDEV_DEFAULT_DEVICE_PREFIX,
+			counter++);
+	init_clonedev(cd, name);
+
+	ret = misc_register(&cd->mdev);
+	if (ret)
+		goto fail_register_misc;
+	mutex_lock(&dev_list_lock);
+	list_add_tail(&cd->list, &dev_list);
+	mutex_unlock(&dev_list_lock);
+
+	mutex_lock(&cd->lock);
+
+	compdev_get(0, &cd->src_compdev);
+	compdev_get(1, &cd->dst_compdev);
+	compdev_get_size(cd->dst_compdev, &cd->dst_size);
+
+	compdev_register_listener_callbacks(cd->src_compdev, (void *)cd,
+			&clonedev_post_buffer_callback,
+			&clonedev_post_scene_info_callback);
+
+	mutex_unlock(&cd->lock);
+	goto out;
+
+fail_register_misc:
+	kfree(cd);
+out:
+	return ret;
+}
+
+void clonedev_destroy(void)
+{
+	struct clonedev *cd;
+	struct clonedev *tmp;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry_safe(cd, tmp, &dev_list, list) {
+		compdev_put(cd->src_compdev);
+		compdev_put(cd->dst_compdev);
+		compdev_deregister_callbacks(cd->src_compdev);
+		list_del(&cd->list);
+		misc_deregister(&cd->mdev);
+		kfree(cd);
+		break;
+	}
+	mutex_unlock(&dev_list_lock);
+}
+
+static void clonedev_destroy_all(void)
+{
+	struct clonedev *cd;
+	struct clonedev *tmp;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry_safe(cd, tmp, &dev_list, list) {
+		list_del(&cd->list);
+		misc_deregister(&cd->mdev);
+		kfree(cd);
+	}
+	mutex_unlock(&dev_list_lock);
+
+	mutex_destroy(&dev_list_lock);
+}
+
+static int __init clonedev_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	mutex_init(&dev_list_lock);
+
+	return 0;
+}
+module_init(clonedev_init);
+
+static void __exit clonedev_exit(void)
+{
+	clonedev_destroy_all();
+	pr_info("%s\n", __func__);
+}
+module_exit(clonedev_exit);
+
+MODULE_AUTHOR("Per-Daniel Olsson <per-daniel.olsson@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Device for display cloning on external output");
+
diff --git a/drivers/misc/compdev/Makefile b/drivers/misc/compdev/Makefile
new file mode 100644
index 00000000000..b8385848712
--- /dev/null
+++ b/drivers/misc/compdev/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_COMPDEV) += compdev.o
+
+ifdef CONFIG_COMPDEV_DEBUG
+EXTRA_CFLAGS += -DDEBUG
+endif
+
diff --git a/drivers/misc/compdev/compdev.c b/drivers/misc/compdev/compdev.c
new file mode 100644
index 00000000000..d929a02c565
--- /dev/null
+++ b/drivers/misc/compdev/compdev.c
@@ -0,0 +1,1381 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Display overlay compositer device driver
+ *
+ * Author: Anders Bauer <anders.bauer@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * Modified: Per-Daniel Olsson <per-daniel.olsson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/ioctl.h>
+#include <linux/sched.h>
+
+#include <linux/compdev.h>
+#include <linux/hwmem.h>
+#include <linux/mm.h>
+#include <video/mcde_dss.h>
+#include <video/b2r2_blt.h>
+#include <linux/workqueue.h>
+#include <linux/completion.h>
+
+#define BUFFER_CACHE_DEPTH 2
+#define NUM_COMPDEV_BUFS 2
+
+static LIST_HEAD(dev_list);
+static DEFINE_MUTEX(dev_list_lock);
+static int dev_counter;
+
+struct compdev_buffer {
+	struct hwmem_alloc *alloc;
+	enum compdev_ptr_type type;
+	u32 size;
+	u32 paddr; /* if pinned */
+};
+
+struct compdev_img_internal {
+	struct compdev_img img;
+	u32 ref_count;
+};
+
+struct compdev_blt_work {
+	struct work_struct work;
+	struct compdev_img *src_img;
+	struct compdev_img_internal *dst_img;
+	int blt_handle;
+	bool mcde_rotation;
+	struct device *dev;
+};
+
+struct compdev_post_callback_work {
+	struct work_struct work;
+	struct compdev_img *img;
+	post_buffer_callback pb_cb;
+	void *cb_data;
+	struct device *dev;
+};
+
+struct buffer_cache_context {
+	struct compdev_img_internal
+		*img[BUFFER_CACHE_DEPTH];
+	u8 index;
+	u8 unused_counter;
+	struct device *dev;
+};
+
+struct dss_context {
+	struct device *dev;
+	struct mcde_display_device *ddev;
+	struct mcde_overlay *ovly[NUM_COMPDEV_BUFS];
+	struct compdev_buffer ovly_buffer[NUM_COMPDEV_BUFS];
+	struct compdev_size phy_size;
+	enum mcde_display_rotation display_rotation;
+	enum compdev_rotation current_buffer_rotation;
+	int blt_handle;
+	u8 temp_img_count;
+	struct compdev_img_internal *temp_img[NUM_COMPDEV_BUFS];
+	struct buffer_cache_context cache_ctx;
+};
+
+struct compdev {
+	struct mutex lock;
+	struct miscdevice mdev;
+	struct device *dev;
+	struct list_head list;
+	struct dss_context dss_ctx;
+	u16 ref_count;
+	struct workqueue_struct *worker_thread;
+	int dev_index;
+	post_buffer_callback pb_cb;
+	post_scene_info_callback si_cb;
+	struct compdev_scene_info s_info;
+	u8 sync_count;
+	u8 image_count;
+	struct compdev_img *images[NUM_COMPDEV_BUFS];
+	struct completion fence;
+	void *cb_data;
+	bool mcde_rotation;
+};
+
+static struct compdev *compdevs[MAX_NBR_OF_COMPDEVS];
+
+static int compdev_post_buffers_dss(struct dss_context *dss_ctx,
+		struct compdev_img *img1, struct compdev_img *img2);
+
+
+static int compdev_open(struct inode *inode, struct file *file)
+{
+	struct compdev *cd = NULL;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry(cd, &dev_list, list)
+		if (cd->mdev.minor == iminor(inode))
+			break;
+
+	if (&cd->list == &dev_list) {
+		mutex_unlock(&dev_list_lock);
+		return -ENODEV;
+	}
+	mutex_unlock(&dev_list_lock);
+	file->private_data = cd;
+	return 0;
+}
+
+static int disable_overlay(struct mcde_overlay *ovly)
+{
+	struct mcde_overlay_info info;
+
+	mcde_dss_get_overlay_info(ovly, &info);
+	if (info.paddr != 0) {
+		/* Set the pointer to zero to disable the overlay */
+		info.paddr = 0;
+		mcde_dss_apply_overlay(ovly, &info);
+	}
+	return 0;
+}
+
+static int compdev_release(struct inode *inode, struct file *file)
+{
+	struct compdev *cd = NULL;
+	int i;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry(cd, &dev_list, list)
+		if (cd->mdev.minor == iminor(inode))
+			break;
+	mutex_unlock(&dev_list_lock);
+
+	if (&cd->list == &dev_list)
+		return -ENODEV;
+
+	for (i = 0; i < NUM_COMPDEV_BUFS; i++) {
+		disable_overlay(cd->dss_ctx.ovly[i]);
+		if (cd->dss_ctx.ovly_buffer[i].paddr &&
+				cd->dss_ctx.ovly_buffer[i].type ==
+				COMPDEV_PTR_HWMEM_BUF_NAME_OFFSET)
+			hwmem_unpin(cd->dss_ctx.ovly_buffer[i].alloc);
+
+		cd->dss_ctx.ovly_buffer[i].alloc = NULL;
+		cd->dss_ctx.ovly_buffer[i].size = 0;
+		cd->dss_ctx.ovly_buffer[i].paddr = 0;
+	}
+
+	return 0;
+}
+
+static enum mcde_ovly_pix_fmt get_ovly_fmt(enum compdev_fmt fmt)
+{
+	switch (fmt) {
+	default:
+	case COMPDEV_FMT_RGB565:
+		return MCDE_OVLYPIXFMT_RGB565;
+	case COMPDEV_FMT_RGB888:
+		return MCDE_OVLYPIXFMT_RGB888;
+	case COMPDEV_FMT_RGBA8888:
+		return MCDE_OVLYPIXFMT_RGBA8888;
+	case COMPDEV_FMT_RGBX8888:
+		return MCDE_OVLYPIXFMT_RGBX8888;
+	case COMPDEV_FMT_YUV422:
+		return MCDE_OVLYPIXFMT_YCbCr422;
+	}
+}
+
+static int compdev_setup_ovly(struct compdev_img *img,
+		struct compdev_buffer *buffer,
+		struct mcde_overlay *ovly,
+		int z_order,
+		struct dss_context *dss_ctx)
+{
+	int ret = 0;
+	enum hwmem_mem_type memtype;
+	enum hwmem_access access;
+	struct hwmem_mem_chunk mem_chunk;
+	size_t mem_chunk_length = 1;
+	struct hwmem_region rgn = { .offset = 0, .count = 1, .start = 0 };
+	struct mcde_overlay_info info;
+
+	if (img->buf.type == COMPDEV_PTR_HWMEM_BUF_NAME_OFFSET) {
+		buffer->type = COMPDEV_PTR_HWMEM_BUF_NAME_OFFSET;
+		buffer->alloc = hwmem_resolve_by_name(img->buf.hwmem_buf_name);
+		if (IS_ERR(buffer->alloc)) {
+			ret = PTR_ERR(buffer->alloc);
+			dev_warn(dss_ctx->dev,
+				"HWMEM resolve failed, %d\n", ret);
+			goto resolve_failed;
+		}
+
+		hwmem_get_info(buffer->alloc, &buffer->size, &memtype,
+				&access);
+
+		if (!(access & HWMEM_ACCESS_READ) ||
+				memtype != HWMEM_MEM_CONTIGUOUS_SYS) {
+			ret = -EACCES;
+			dev_warn(dss_ctx->dev,
+				"Invalid_mem overlay, %d\n", ret);
+			goto invalid_mem;
+		}
+		ret = hwmem_pin(buffer->alloc, &mem_chunk, &mem_chunk_length);
+		if (ret) {
+			dev_warn(dss_ctx->dev,
+				"Pin failed, %d\n", ret);
+			goto pin_failed;
+		}
+
+		rgn.size = rgn.end = buffer->size;
+		ret = hwmem_set_domain(buffer->alloc, HWMEM_ACCESS_READ,
+			HWMEM_DOMAIN_SYNC, &rgn);
+		if (ret)
+			dev_warn(dss_ctx->dev,
+				"Set domain failed, %d\n", ret);
+
+		buffer->paddr = mem_chunk.paddr;
+	} else if (img->buf.type == COMPDEV_PTR_PHYSICAL) {
+		buffer->type = COMPDEV_PTR_PHYSICAL;
+		buffer->alloc = NULL;
+		buffer->size = img->buf.len;
+		buffer->paddr = img->buf.offset;
+	}
+
+	info.stride = img->pitch;
+	info.fmt = get_ovly_fmt(img->fmt);
+	info.src_x = 0;
+	info.src_y = 0;
+	info.dst_x = img->dst_rect.x;
+	info.dst_y = img->dst_rect.y;
+	info.dst_z = z_order;
+	info.w = img->dst_rect.width;
+	info.h = img->dst_rect.height;
+	info.dirty.x = 0;
+	info.dirty.y = 0;
+	info.dirty.w = img->dst_rect.width;
+	info.dirty.h = img->dst_rect.height;
+	info.paddr = buffer->paddr;
+
+	mcde_dss_apply_overlay(ovly, &info);
+	return ret;
+
+pin_failed:
+invalid_mem:
+	buffer->alloc = NULL;
+	buffer->size = 0;
+	buffer->paddr = 0;
+
+resolve_failed:
+	return ret;
+}
+
+static int compdev_update_rotation(struct dss_context *dss_ctx,
+		enum compdev_rotation rotation)
+{
+	/* Set video mode */
+	struct mcde_video_mode vmode;
+	int ret = 0;
+
+	memset(&vmode, 0, sizeof(struct mcde_video_mode));
+	mcde_dss_get_video_mode(dss_ctx->ddev, &vmode);
+	if ((dss_ctx->display_rotation + rotation) % 180) {
+		vmode.xres = dss_ctx->phy_size.height;
+		vmode.yres = dss_ctx->phy_size.width;
+	} else {
+		vmode.xres = dss_ctx->phy_size.width;
+		vmode.yres = dss_ctx->phy_size.height;
+	}
+
+	/* Set rotation */
+	ret = mcde_dss_set_rotation(dss_ctx->ddev,
+			(dss_ctx->display_rotation + rotation) % 360);
+	if (ret != 0)
+		goto exit;
+
+	ret = mcde_dss_set_video_mode(dss_ctx->ddev, &vmode);
+	if (ret != 0)
+		goto exit;
+
+
+	/* Apply */
+	ret = mcde_dss_apply_channel(dss_ctx->ddev);
+exit:
+	return ret;
+}
+
+static int release_prev_frame(struct dss_context *dss_ctx)
+{
+	int ret = 0;
+	int i;
+
+	/* Handle unpin of previous buffers */
+	for (i = 0; i < NUM_COMPDEV_BUFS; i++) {
+		if (dss_ctx->ovly_buffer[i].type ==
+				COMPDEV_PTR_HWMEM_BUF_NAME_OFFSET &&
+				dss_ctx->ovly_buffer[i].paddr != 0) {
+			hwmem_unpin(dss_ctx->ovly_buffer[i].alloc);
+			hwmem_release(dss_ctx->ovly_buffer[i].alloc);
+		}
+		dss_ctx->ovly_buffer[i].alloc = NULL;
+		dss_ctx->ovly_buffer[i].size = 0;
+		dss_ctx->ovly_buffer[i].paddr = 0;
+	}
+	return ret;
+
+}
+
+static enum b2r2_blt_fmt compdev_to_blt_format(enum compdev_fmt fmt)
+{
+	switch (fmt) {
+	case COMPDEV_FMT_RGBA8888:
+		return B2R2_BLT_FMT_32_BIT_ABGR8888;
+	case COMPDEV_FMT_RGB888:
+		return B2R2_BLT_FMT_24_BIT_RGB888;
+	case COMPDEV_FMT_RGB565:
+		return B2R2_BLT_FMT_16_BIT_RGB565;
+	case COMPDEV_FMT_YUV422:
+		return B2R2_BLT_FMT_CB_Y_CR_Y;
+	case COMPDEV_FMT_YCBCR42XMBN:
+		return B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE;
+	case COMPDEV_FMT_YUV420_SP:
+		return B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR;
+	case COMPDEV_FMT_YVU420_SP:
+		return B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR;
+	case COMPDEV_FMT_YUV420_P:
+		return B2R2_BLT_FMT_YUV420_PACKED_PLANAR;
+	default:
+		return B2R2_BLT_FMT_UNUSED;
+	}
+}
+
+static enum b2r2_blt_transform to_blt_transform
+					(enum compdev_rotation compdev_rot)
+{
+	switch (compdev_rot) {
+	case COMPDEV_ROT_0:
+		return B2R2_BLT_TRANSFORM_NONE;
+	case COMPDEV_ROT_90_CCW:
+		return B2R2_BLT_TRANSFORM_CCW_ROT_90;
+	case COMPDEV_ROT_180:
+		return B2R2_BLT_TRANSFORM_CCW_ROT_180;
+	case COMPDEV_ROT_270_CCW:
+		return B2R2_BLT_TRANSFORM_CCW_ROT_90;
+	default:
+		return B2R2_BLT_TRANSFORM_NONE;
+	}
+}
+
+static u32 get_stride(u32 width, enum compdev_fmt fmt)
+{
+	u32 stride = 0;
+	switch (fmt) {
+	case COMPDEV_FMT_RGB565:
+		stride = width * 2;
+		break;
+	case COMPDEV_FMT_RGB888:
+		stride = width * 3;
+		break;
+	case COMPDEV_FMT_RGBX8888:
+		stride = width * 4;
+		break;
+	case COMPDEV_FMT_RGBA8888:
+		stride = width * 4;
+		break;
+	case COMPDEV_FMT_YUV422:
+		stride = width * 2;
+		break;
+	case COMPDEV_FMT_YCBCR42XMBN:
+	case COMPDEV_FMT_YUV420_SP:
+	case COMPDEV_FMT_YVU420_SP:
+	case COMPDEV_FMT_YUV420_P:
+		stride = width;
+		break;
+	}
+
+	/* The display controller requires 8 byte aligned strides */
+	if (stride % 8)
+		stride += 8 - (stride % 8);
+
+	return stride;
+}
+
+static int alloc_comp_internal_img(enum compdev_fmt fmt,
+		u16 width, u16 height, struct compdev_img_internal **img_pp)
+{
+	struct hwmem_alloc *alloc;
+	int name;
+	u32 size;
+	u32 stride;
+	struct compdev_img_internal *img;
+
+	stride = get_stride(width, fmt);
+	size = stride * height;
+	size = PAGE_ALIGN(size);
+
+	img = kzalloc(sizeof(struct compdev_img_internal), GFP_KERNEL);
+
+	if (!img)
+		return -ENOMEM;
+
+	alloc = hwmem_alloc(size, HWMEM_ALLOC_HINT_WRITE_COMBINE |
+			HWMEM_ALLOC_HINT_UNCACHED,
+			(HWMEM_ACCESS_READ  | HWMEM_ACCESS_WRITE |
+			HWMEM_ACCESS_IMPORT),
+			HWMEM_MEM_CONTIGUOUS_SYS);
+
+	if (IS_ERR(alloc)) {
+		kfree(img);
+		img = NULL;
+		return PTR_ERR(alloc);
+	}
+
+	name = hwmem_get_name(alloc);
+	if (name < 0) {
+		kfree(img);
+		img = NULL;
+		hwmem_release(alloc);
+		return name;
+	}
+
+	img->img.height = height;
+	img->img.width = width;
+	img->img.fmt = fmt;
+	img->img.pitch = stride;
+	img->img.buf.hwmem_buf_name = name;
+	img->img.buf.type = COMPDEV_PTR_HWMEM_BUF_NAME_OFFSET;
+	img->img.buf.offset = 0;
+	img->img.buf.len = size;
+
+	img->ref_count = 1;
+
+	*img_pp = img;
+
+	return 0;
+}
+
+static void free_comp_img_buf(struct compdev_img_internal *img,
+		struct device *dev)
+{
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (img != NULL && img->ref_count) {
+		img->ref_count--;
+		if (img->ref_count == 0) {
+			struct hwmem_alloc *alloc;
+			if (img->img.buf.hwmem_buf_name > 0) {
+				alloc = hwmem_resolve_by_name(
+						img->img.buf.hwmem_buf_name);
+				if (IS_ERR(alloc)) {
+					dev_err(dev, "%s: Error getting Alloc "
+						"from HWMEM\n", __func__);
+					return;
+				}
+				/* Double release needed */
+				hwmem_release(alloc);
+				hwmem_release(alloc);
+			}
+			kfree(img);
+		}
+	}
+}
+
+struct compdev_img_internal *compdev_buffer_cache_get_image(
+		struct buffer_cache_context *cache_ctx, enum compdev_fmt fmt,
+		u16 width, u16 height)
+{
+	int i;
+	struct compdev_img_internal *img = NULL;
+
+	dev_dbg(cache_ctx->dev, "%s\n", __func__);
+
+	/* First check for a cache hit */
+	if (cache_ctx->unused_counter > 0) {
+		u8 active_index = cache_ctx->index;
+		struct compdev_img_internal *temp =
+				cache_ctx->img[active_index];
+		if (temp != NULL && temp->img.fmt == fmt &&
+				temp->img.width == width &&
+				temp->img.height == height) {
+			img = temp;
+			cache_ctx->unused_counter = 0;
+		}
+	}
+	/* Check if there was a cache hit */
+	if (img == NULL) {
+		/* Create new buffers and release old */
+		for (i = 0; i < BUFFER_CACHE_DEPTH; i++) {
+			if (cache_ctx->img[i]) {
+				free_comp_img_buf(cache_ctx->img[i],
+						cache_ctx->dev);
+				cache_ctx->img[i] = NULL;
+			}
+			cache_ctx->index = 0;
+			if (alloc_comp_internal_img(fmt, width, height,
+					&cache_ctx->img[i]))
+				dev_err(cache_ctx->dev,
+						"%s: Allocation error\n",
+						__func__);
+		}
+		img = cache_ctx->img[0];
+	}
+
+	if (img != NULL) {
+		img->ref_count++;
+		cache_ctx->unused_counter = 0;
+		cache_ctx->index++;
+		if (cache_ctx->index >= BUFFER_CACHE_DEPTH)
+			cache_ctx->index = 0;
+	}
+
+	return img;
+}
+
+static void compdev_buffer_cache_mark_frame
+				(struct buffer_cache_context *cache_ctx)
+{
+	if (cache_ctx->unused_counter < 2)
+		cache_ctx->unused_counter++;
+	if (cache_ctx->unused_counter == 2) {
+		int i;
+		for (i = 0; i < BUFFER_CACHE_DEPTH; i++) {
+			if (cache_ctx->img[i]) {
+				free_comp_img_buf(cache_ctx->img[i],
+						cache_ctx->dev);
+				cache_ctx->img[i] = NULL;
+			}
+		}
+	}
+}
+
+static bool check_hw_format(enum compdev_fmt fmt)
+{
+	if (fmt == COMPDEV_FMT_RGB565 ||
+			fmt == COMPDEV_FMT_RGB888 ||
+			fmt == COMPDEV_FMT_RGBA8888 ||
+			fmt == COMPDEV_FMT_RGBX8888 ||
+			fmt == COMPDEV_FMT_YUV422)
+		return true;
+	else
+		return false;
+}
+
+static enum compdev_fmt find_compatible_fmt(enum compdev_fmt fmt, bool rotation)
+{
+	if (!rotation) {
+		switch (fmt) {
+		case COMPDEV_FMT_RGB565:
+		case COMPDEV_FMT_RGB888:
+		case COMPDEV_FMT_RGBA8888:
+		case COMPDEV_FMT_RGBX8888:
+			return fmt;
+		case COMPDEV_FMT_YUV422:
+		case COMPDEV_FMT_YCBCR42XMBN:
+		case COMPDEV_FMT_YUV420_SP:
+		case COMPDEV_FMT_YVU420_SP:
+		case COMPDEV_FMT_YUV420_P:
+			return COMPDEV_FMT_YUV422;
+		default:
+			return COMPDEV_FMT_RGBA8888;
+		}
+	} else {
+		switch (fmt) {
+		case COMPDEV_FMT_RGB565:
+		case COMPDEV_FMT_RGB888:
+		case COMPDEV_FMT_RGBA8888:
+		case COMPDEV_FMT_RGBX8888:
+			return fmt;
+		case COMPDEV_FMT_YUV422:
+		case COMPDEV_FMT_YCBCR42XMBN:
+		case COMPDEV_FMT_YUV420_SP:
+		case COMPDEV_FMT_YVU420_SP:
+		case COMPDEV_FMT_YUV420_P:
+			return COMPDEV_FMT_RGB888;
+		default:
+			return COMPDEV_FMT_RGBA8888;
+		}
+	}
+}
+
+static void compdev_callback_worker_function(struct work_struct *work)
+{
+	struct compdev_post_callback_work *cb_work =
+			(struct compdev_post_callback_work *)work;
+
+	if (cb_work->pb_cb != NULL)
+		cb_work->pb_cb(cb_work->cb_data, cb_work->img);
+}
+static void compdev_blt_worker_function(struct work_struct *work)
+{
+	struct compdev_blt_work *blt_work = (struct compdev_blt_work *)work;
+	struct compdev_img *src_img;
+	struct compdev_img *dst_img;
+	struct b2r2_blt_req req;
+	int req_id;
+
+	dev_dbg(blt_work->dev, "%s\n", __func__);
+
+	src_img = blt_work->src_img;
+	dst_img = &blt_work->dst_img->img;
+
+	memset(&req, 0, sizeof(req));
+	req.size = sizeof(req);
+
+	if (src_img->buf.type == COMPDEV_PTR_PHYSICAL) {
+		req.src_img.buf.type = B2R2_BLT_PTR_PHYSICAL;
+		req.src_img.buf.fd = src_img->buf.fd;
+	} else {
+		struct hwmem_alloc *alloc;
+
+		req.src_img.buf.type = B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET;
+		req.src_img.buf.hwmem_buf_name = src_img->buf.hwmem_buf_name;
+
+		alloc = hwmem_resolve_by_name(src_img->buf.hwmem_buf_name);
+		if (IS_ERR(alloc)) {
+			dev_warn(blt_work->dev,
+				"HWMEM resolve failed\n");
+		}
+		hwmem_set_access(alloc,
+				HWMEM_ACCESS_READ | HWMEM_ACCESS_IMPORT,
+				task_tgid_nr(current));
+		hwmem_release(alloc);
+	}
+	req.src_img.pitch = src_img->pitch;
+	req.src_img.buf.offset = src_img->buf.offset;
+	req.src_img.buf.len = src_img->buf.len;
+	req.src_img.fmt = compdev_to_blt_format(src_img->fmt);
+	req.src_img.width = src_img->width;
+	req.src_img.height = src_img->height;
+
+	req.src_rect.x = src_img->src_rect.x;
+	req.src_rect.y = src_img->src_rect.y;
+	req.src_rect.width = src_img->src_rect.width;
+	req.src_rect.height = src_img->src_rect.height;
+
+	if (dst_img->buf.type == COMPDEV_PTR_PHYSICAL) {
+		req.dst_img.buf.type = B2R2_BLT_PTR_PHYSICAL;
+		req.dst_img.buf.fd = dst_img->buf.fd;
+	} else {
+		req.dst_img.buf.type = B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET;
+		req.dst_img.buf.hwmem_buf_name = dst_img->buf.hwmem_buf_name;
+	}
+	req.dst_img.pitch = dst_img->pitch;
+	req.dst_img.buf.offset = dst_img->buf.offset;
+	req.dst_img.buf.len = dst_img->buf.len;
+	req.dst_img.fmt = compdev_to_blt_format(dst_img->fmt);
+	req.dst_img.width = dst_img->width;
+	req.dst_img.height = dst_img->height;
+
+	if (blt_work->mcde_rotation)
+		req.transform = B2R2_BLT_TRANSFORM_NONE;
+	else
+		req.transform = to_blt_transform(src_img->rotation);
+	req.dst_rect.x = 0;
+	req.dst_rect.y = 0;
+	req.dst_rect.width = src_img->dst_rect.width;
+	req.dst_rect.height = src_img->dst_rect.height;
+
+	req.global_alpha = 0xff;
+	req.flags = B2R2_BLT_FLAG_DITHER;
+
+	req_id = b2r2_blt_request(blt_work->blt_handle, &req);
+
+	if (b2r2_blt_synch(blt_work->blt_handle, req_id) < 0) {
+		dev_err(blt_work->dev,
+				"%s: Could not perform b2r2_blt_synch",
+				__func__);
+	}
+
+	dst_img->src_rect.x = 0;
+	dst_img->src_rect.x = 0;
+	dst_img->src_rect.width = dst_img->width;
+	dst_img->src_rect.height = dst_img->height;
+
+	dst_img->dst_rect.x = src_img->dst_rect.x;
+	dst_img->dst_rect.y = src_img->dst_rect.y;
+	dst_img->dst_rect.width = src_img->dst_rect.width;
+	dst_img->dst_rect.height = src_img->dst_rect.height;
+
+	dst_img->rotation = src_img->rotation;
+}
+
+static int compdev_post_buffer_locked(struct compdev *cd,
+		struct compdev_img *src_img)
+{
+	int ret = 0;
+	int i;
+	bool transform_needed = false;
+	struct compdev_img *resulting_img;
+	struct compdev_blt_work blt_work;
+	struct compdev_post_callback_work cb_work;
+	bool callback_work = false;
+	bool bypass_case = false;
+
+	dev_dbg(cd->dev, "%s\n", __func__);
+
+	/* Free potential temp buffers */
+	for (i = 0; i < cd->dss_ctx.temp_img_count; i++)
+		free_comp_img_buf(cd->dss_ctx.temp_img[i], cd->dev);
+	cd->dss_ctx.temp_img_count = 0;
+
+	/* Check for bypass images */
+	if (src_img->flags & COMPDEV_BYPASS_FLAG)
+		bypass_case = true;
+
+	/* Handle callback */
+	if (cd->pb_cb != NULL) {
+		callback_work = true;
+		INIT_WORK((struct work_struct *)&cb_work,
+				compdev_callback_worker_function);
+		cb_work.img = src_img;
+		cb_work.pb_cb = cd->pb_cb;
+		cb_work.cb_data = cd->cb_data;
+		cb_work.dev = cd->dev;
+		queue_work(cd->worker_thread, (struct work_struct *)&cb_work);
+	}
+
+	if (!bypass_case) {
+		/* Determine if transform is needed */
+		/* First check scaling */
+		if ((src_img->rotation == COMPDEV_ROT_0 ||
+			src_img->rotation == COMPDEV_ROT_180) &&
+			(src_img->src_rect.width != src_img->dst_rect.width ||
+			src_img->src_rect.height != src_img->dst_rect.height))
+			transform_needed = true;
+		else if ((src_img->rotation == COMPDEV_ROT_90_CCW ||
+			src_img->rotation == COMPDEV_ROT_270_CCW) &&
+			(src_img->src_rect.width != src_img->dst_rect.height ||
+			src_img->src_rect.height != src_img->dst_rect.width))
+			transform_needed = true;
+
+		if (!transform_needed && check_hw_format(src_img->fmt) == false)
+			transform_needed = true;
+
+		if (transform_needed) {
+			u16 width = 0;
+			u16 height = 0;
+			enum compdev_fmt fmt;
+
+			INIT_WORK((struct work_struct *)&blt_work,
+					compdev_blt_worker_function);
+
+			if (cd->dss_ctx.blt_handle == 0) {
+				dev_dbg(cd->dev, "%s: B2R2 opened\n", __func__);
+				cd->dss_ctx.blt_handle = b2r2_blt_open();
+				if (cd->dss_ctx.blt_handle < 0) {
+					dev_warn(cd->dev,
+						"%s(%d): Failed to "
+						"open b2r2 device\n",
+						__func__, __LINE__);
+				}
+			}
+			blt_work.blt_handle = cd->dss_ctx.blt_handle;
+			blt_work.src_img = src_img;
+			blt_work.mcde_rotation = cd->mcde_rotation;
+
+			width = src_img->dst_rect.width;
+			height = src_img->dst_rect.height;
+
+			fmt = find_compatible_fmt(src_img->fmt,
+					(!cd->mcde_rotation) &&
+					(src_img->rotation != COMPDEV_ROT_0));
+
+			blt_work.dst_img = compdev_buffer_cache_get_image
+					(&cd->dss_ctx.cache_ctx,
+							fmt, width, height);
+
+			blt_work.dst_img->img.flags = src_img->flags;
+			blt_work.dev = cd->dev;
+
+			queue_work(cd->worker_thread,
+					(struct work_struct *)&blt_work);
+			flush_work_sync((struct work_struct *)&blt_work);
+
+			resulting_img = &blt_work.dst_img->img;
+
+			cd->dss_ctx.temp_img[cd->dss_ctx.temp_img_count] =
+					blt_work.dst_img;
+			cd->dss_ctx.temp_img_count++;
+
+		} else {
+		    resulting_img = src_img;
+		}
+
+		if (!cd->mcde_rotation)
+			resulting_img->rotation = COMPDEV_ROT_0;
+
+		cd->images[cd->image_count] = resulting_img;
+		cd->image_count++;
+
+		/* make sure that a potential callback has returned */
+		if (callback_work)
+			flush_work_sync((struct work_struct *)&cb_work);
+
+		if (cd->sync_count > 1) {
+			cd->sync_count--;
+			mutex_unlock(&cd->lock);
+			/* Wait for fence */
+			wait_for_completion(&cd->fence);
+			mutex_lock(&cd->lock);
+		} else {
+			struct compdev_img *img1 = NULL;
+			struct compdev_img *img2 = NULL;
+
+			if (cd->sync_count)
+				cd->sync_count--;
+
+			img1 = cd->images[0];
+			if (cd->image_count)
+				img2 = cd->images[1];
+
+			/* Do the refresh */
+			compdev_post_buffers_dss(&cd->dss_ctx, img1, img2);
+			compdev_buffer_cache_mark_frame
+						(&cd->dss_ctx.cache_ctx);
+
+			if (cd->s_info.img_count > 1) {
+				/* Releasing fence */
+				complete(&cd->fence);
+			}
+
+			cd->sync_count = 0;
+			cd->image_count = 0;
+			cd->images[0] = NULL;
+			cd->images[1] = NULL;
+		}
+	} else {
+		/* make sure that a potential callback has returned */
+		if (callback_work)
+			flush_work_sync((struct work_struct *)&cb_work);
+	}
+
+	return ret;
+}
+
+static int compdev_post_buffers_dss(struct dss_context *dss_ctx,
+		struct compdev_img *img1, struct compdev_img *img2)
+{
+	int ret = 0;
+	int i = 0;
+
+	struct compdev_img *fb_img = NULL;
+	struct compdev_img *ovly_img = NULL;
+
+	/* Unpin the previous frame */
+	release_prev_frame(dss_ctx);
+
+	/* Set channel rotation */
+	if (img1 != NULL &&
+			(dss_ctx->current_buffer_rotation != img1->rotation)) {
+		if (compdev_update_rotation(dss_ctx, img1->rotation) != 0)
+			dev_warn(dss_ctx->dev,
+				"Failed to update MCDE rotation "
+				"(img1->rotation = %d), %d\n",
+				img1->rotation, ret);
+		else
+			dss_ctx->current_buffer_rotation = img1->rotation;
+	}
+
+	if ((img1 != NULL) && (img1->flags & COMPDEV_OVERLAY_FLAG))
+		ovly_img = img1;
+	else if (img1 != NULL)
+		fb_img = img1;
+
+
+	if ((img2 != NULL) && (img2->flags & COMPDEV_OVERLAY_FLAG))
+		ovly_img = img2;
+	else if (img2 != NULL)
+		fb_img = img2;
+
+	/* Handle buffers */
+	if (fb_img != NULL) {
+		ret = compdev_setup_ovly(fb_img,
+			&dss_ctx->ovly_buffer[i], dss_ctx->ovly[0], 1, dss_ctx);
+		if (ret)
+			dev_warn(dss_ctx->dev,
+				"Failed to setup overlay[%d], %d\n", 0, ret);
+		i++;
+	} else {
+		disable_overlay(dss_ctx->ovly[0]);
+	}
+
+
+	if (ovly_img != NULL) {
+		ret = compdev_setup_ovly(ovly_img,
+			&dss_ctx->ovly_buffer[i], dss_ctx->ovly[1], 0, dss_ctx);
+		if (ret)
+			dev_warn(dss_ctx->dev,
+				"Failed to setup overlay[%d], %d\n", 1, ret);
+	} else {
+		disable_overlay(dss_ctx->ovly[1]);
+	}
+
+	/* Do the display update */
+	mcde_dss_update_overlay(dss_ctx->ovly[0], true);
+
+	return ret;
+}
+
+static int compdev_post_scene_info_locked(struct compdev *cd,
+				struct compdev_scene_info *s_info)
+{
+	int ret = 0;
+
+	dev_dbg(cd->dev, "%s\n", __func__);
+
+	cd->s_info = *s_info;
+	cd->sync_count = cd->s_info.img_count;
+
+	/* always complete the fence in case someone is hanging incorrectly. */
+	complete(&cd->fence);
+	init_completion(&cd->fence);
+
+	/* Handle callback */
+	if (cd->si_cb != NULL) {
+		mutex_unlock(&cd->lock);
+		cd->si_cb(cd->cb_data, s_info);
+		mutex_lock(&cd->lock);
+	}
+	return ret;
+}
+
+
+static int compdev_get_size_locked(struct dss_context *dss_ctx,
+					struct compdev_size *size)
+{
+	int ret = 0;
+	if ((dss_ctx->display_rotation) % 180) {
+		size->height = dss_ctx->phy_size.width;
+		size->width = dss_ctx->phy_size.height;
+	} else {
+		size->height = dss_ctx->phy_size.height;
+		size->width = dss_ctx->phy_size.width;
+	}
+
+	return ret;
+}
+
+static int compdev_get_listener_state_locked(struct compdev *cd,
+				enum compdev_listener_state *state)
+{
+	int ret = 0;
+
+	*state = COMPDEV_LISTENER_OFF;
+	if (cd->pb_cb != NULL)
+		*state = COMPDEV_LISTENER_ON;
+	return ret;
+}
+
+static long compdev_ioctl(struct file *file,
+		unsigned int cmd,
+		unsigned long arg)
+{
+	int ret;
+	struct compdev *cd = (struct compdev *)file->private_data;
+	struct compdev_img img;
+	struct compdev_scene_info s_info;
+
+	mutex_lock(&cd->lock);
+
+	switch (cmd) {
+	case COMPDEV_GET_SIZE_IOC:
+	{
+		struct compdev_size tmp;
+		compdev_get_size_locked(&cd->dss_ctx, &tmp);
+		ret = copy_to_user((void __user *)arg, &tmp,
+							sizeof(tmp));
+		if (ret)
+			ret = -EFAULT;
+	}
+	break;
+	case COMPDEV_GET_LISTENER_STATE_IOC:
+	{
+		enum compdev_listener_state state;
+		compdev_get_listener_state_locked(cd, &state);
+		ret = copy_to_user((void __user *)arg, &state,
+				sizeof(state));
+		if (ret)
+			ret = -EFAULT;
+	}
+	break;
+	case COMPDEV_POST_BUFFER_IOC:
+		memset(&img, 0, sizeof(img));
+		/* Get the user data */
+		if (copy_from_user(&img, (void *)arg, sizeof(img))) {
+			dev_warn(cd->dev,
+				"%s: copy_from_user failed\n",
+				__func__);
+			mutex_unlock(&cd->lock);
+			return -EFAULT;
+		}
+		ret = compdev_post_buffer_locked(cd, &img);
+
+		break;
+	case COMPDEV_POST_SCENE_INFO_IOC:
+		memset(&s_info, 0, sizeof(s_info));
+		/* Get the user data */
+		if (copy_from_user(&s_info, (void *)arg, sizeof(s_info))) {
+			dev_warn(cd->dev,
+				"%s: copy_from_user failed\n",
+				__func__);
+			mutex_unlock(&cd->lock);
+			return -EFAULT;
+		}
+		ret = compdev_post_scene_info_locked(cd, &s_info);
+
+		break;
+
+	default:
+		ret = -ENOSYS;
+	}
+
+	mutex_unlock(&cd->lock);
+
+	return ret;
+}
+
+static const struct file_operations compdev_fops = {
+	.open = compdev_open,
+	.release = compdev_release,
+	.unlocked_ioctl = compdev_ioctl,
+};
+
+static void init_compdev(struct compdev *cd, const char *name)
+{
+	mutex_init(&cd->lock);
+	INIT_LIST_HEAD(&cd->list);
+	init_completion(&cd->fence);
+
+	cd->mdev.minor = MISC_DYNAMIC_MINOR;
+	cd->mdev.name = name;
+	cd->mdev.fops = &compdev_fops;
+	cd->dev = cd->mdev.this_device;
+}
+
+static void init_dss_context(struct dss_context *dss_ctx,
+		struct mcde_display_device *ddev, struct compdev *cd)
+{
+	dss_ctx->ddev = ddev;
+	dss_ctx->dev = cd->dev;
+	memset(&dss_ctx->cache_ctx, 0, sizeof(struct buffer_cache_context));
+	dss_ctx->cache_ctx.dev = dss_ctx->dev;
+}
+
+int compdev_create(struct mcde_display_device *ddev,
+		struct mcde_overlay *parent_ovly, bool mcde_rotation)
+{
+	int ret = 0;
+	int i;
+	struct compdev *cd;
+	struct mcde_video_mode vmode;
+	struct mcde_overlay_info info;
+
+	char name[10];
+
+	if (dev_counter == 0) {
+		for (i = 0; i < MAX_NBR_OF_COMPDEVS; i++)
+			compdevs[i] = NULL;
+	}
+
+	if (dev_counter > MAX_NBR_OF_COMPDEVS)
+		return -ENOMEM;
+
+	cd = kzalloc(sizeof(struct compdev), GFP_KERNEL);
+	if (!cd)
+		return -ENOMEM;
+
+	compdevs[dev_counter] = cd;
+	cd->dev_index = dev_counter;
+
+	snprintf(name, sizeof(name), "%s%d", COMPDEV_DEFAULT_DEVICE_PREFIX,
+			dev_counter++);
+	init_compdev(cd, name);
+
+	init_dss_context(&cd->dss_ctx, ddev, cd);
+
+	mcde_dss_get_video_mode(ddev, &vmode);
+
+	cd->worker_thread = create_workqueue(name);
+	if (!cd->worker_thread) {
+		ret = -ENOMEM;
+		goto fail_workqueue;
+	}
+
+	cd->dss_ctx.ovly[0] = parent_ovly;
+	if (!cd->dss_ctx.ovly[0]) {
+		ret = -ENOMEM;
+		goto fail_create_ovly;
+	}
+
+	for (i = 1; i < NUM_COMPDEV_BUFS; i++) {
+		cd->dss_ctx.ovly[i] = mcde_dss_create_overlay(ddev, &info);
+		if (!cd->dss_ctx.ovly[i]) {
+			ret = -ENOMEM;
+			goto fail_create_ovly;
+		}
+		if (mcde_dss_enable_overlay(cd->dss_ctx.ovly[i]))
+			goto fail_create_ovly;
+		if (disable_overlay(cd->dss_ctx.ovly[i]))
+			goto fail_create_ovly;
+	}
+
+	mcde_dss_get_native_resolution(ddev, &cd->dss_ctx.phy_size.width,
+			&cd->dss_ctx.phy_size.height);
+	cd->dss_ctx.display_rotation = mcde_dss_get_rotation(ddev);
+	cd->dss_ctx.current_buffer_rotation = 0;
+
+	cd->mcde_rotation = mcde_rotation;
+
+	ret = misc_register(&cd->mdev);
+	if (ret)
+		goto fail_register_misc;
+	mutex_lock(&dev_list_lock);
+	list_add_tail(&cd->list, &dev_list);
+	mutex_unlock(&dev_list_lock);
+
+	goto out;
+
+fail_register_misc:
+fail_create_ovly:
+	for (i = 0; i < NUM_COMPDEV_BUFS; i++) {
+		if (cd->dss_ctx.ovly[i])
+			mcde_dss_destroy_overlay(cd->dss_ctx.ovly[i]);
+	}
+fail_workqueue:
+	kfree(cd);
+out:
+	return ret;
+}
+
+
+int compdev_get(int dev_idx, struct compdev **cd_pp)
+{
+	struct compdev *cd;
+	cd = NULL;
+
+	if (dev_idx >= MAX_NBR_OF_COMPDEVS)
+		return -ENOMEM;
+
+	cd = compdevs[dev_idx];
+	if (cd != NULL) {
+		mutex_lock(&cd->lock);
+		cd->ref_count++;
+		mutex_unlock(&cd->lock);
+		*cd_pp = cd;
+		return 0;
+	} else {
+		return -ENOMEM;
+	}
+}
+EXPORT_SYMBOL(compdev_get);
+
+int compdev_put(struct compdev *cd)
+{
+	int ret = 0;
+	if (cd == NULL)
+		return -ENOMEM;
+
+	mutex_lock(&cd->lock);
+	cd->ref_count--;
+	if (cd->ref_count < 0)
+		dev_warn(cd->dev,
+				"%s: Incorrect ref count\n", __func__);
+	mutex_unlock(&cd->lock);
+	return ret;
+}
+EXPORT_SYMBOL(compdev_put);
+
+int compdev_get_size(struct compdev *cd, struct compdev_size *size)
+{
+	int ret = 0;
+	if (cd == NULL)
+		return -ENOMEM;
+
+	mutex_lock(&cd->lock);
+
+	ret = compdev_get_size_locked(&cd->dss_ctx, size);
+
+	mutex_unlock(&cd->lock);
+	return ret;
+}
+EXPORT_SYMBOL(compdev_get_size);
+
+int compdev_get_listener_state(struct compdev *cd,
+	enum compdev_listener_state *listener_state)
+{
+	int ret = 0;
+	if (cd == NULL)
+		return -ENOMEM;
+
+	mutex_lock(&cd->lock);
+
+	ret = compdev_get_listener_state_locked(cd, listener_state);
+
+	mutex_unlock(&cd->lock);
+	return ret;
+}
+EXPORT_SYMBOL(compdev_get_listener_state);
+
+
+int compdev_post_buffer(struct compdev *cd, struct compdev_img *img)
+{
+	int ret = 0;
+	if (cd == NULL)
+		return -ENOMEM;
+
+	mutex_lock(&cd->lock);
+
+	ret = compdev_post_buffer_locked(cd, img);
+
+	mutex_unlock(&cd->lock);
+	return ret;
+}
+EXPORT_SYMBOL(compdev_post_buffer);
+
+int compdev_post_scene_info(struct compdev *cd,
+			struct compdev_scene_info *s_info)
+{
+	int ret = 0;
+	if (cd == NULL)
+		return -ENOMEM;
+
+	mutex_lock(&cd->lock);
+
+	ret = compdev_post_scene_info_locked(cd, s_info);
+
+	mutex_unlock(&cd->lock);
+	return ret;
+}
+EXPORT_SYMBOL(compdev_post_scene_info);
+
+int compdev_register_listener_callbacks(struct compdev *cd, void *data,
+		post_buffer_callback pb_cb, post_scene_info_callback si_cb)
+{
+	int ret = 0;
+	if (cd == NULL)
+		return -ENOMEM;
+	mutex_lock(&cd->lock);
+	cd->cb_data = data;
+	cd->pb_cb = pb_cb;
+	cd->si_cb = si_cb;
+	mutex_unlock(&cd->lock);
+	return ret;
+}
+EXPORT_SYMBOL(compdev_register_listener_callbacks);
+
+int compdev_deregister_callbacks(struct compdev *cd)
+{
+	int ret = 0;
+	if (cd == NULL)
+		return -ENOMEM;
+	mutex_lock(&cd->lock);
+	cd->cb_data = NULL;
+	cd->pb_cb = NULL;
+	cd->si_cb = NULL;
+	mutex_unlock(&cd->lock);
+	return ret;
+}
+EXPORT_SYMBOL(compdev_deregister_callbacks);
+
+void compdev_destroy(struct mcde_display_device *ddev)
+{
+	struct compdev *cd;
+	struct compdev *tmp;
+	int i;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry_safe(cd, tmp, &dev_list, list) {
+		if (cd->dss_ctx.ddev == ddev) {
+			list_del(&cd->list);
+			misc_deregister(&cd->mdev);
+			for (i = 1; i < NUM_COMPDEV_BUFS; i++)
+				mcde_dss_destroy_overlay(cd->dss_ctx.ovly[i]);
+			b2r2_blt_close(cd->dss_ctx.blt_handle);
+
+			release_prev_frame(&cd->dss_ctx);
+
+			/* Free potential temp buffers */
+			for (i = 0; i < cd->dss_ctx.temp_img_count; i++)
+				free_comp_img_buf(cd->dss_ctx.temp_img[i],
+						cd->dev);
+
+			for (i = 0; i < BUFFER_CACHE_DEPTH; i++) {
+				if (cd->dss_ctx.cache_ctx.img[i]) {
+					free_comp_img_buf
+						(cd->dss_ctx.cache_ctx.img[i],
+							cd->dev);
+					cd->dss_ctx.cache_ctx.img[i] = NULL;
+				}
+			}
+
+			destroy_workqueue(cd->worker_thread);
+			kfree(cd);
+			break;
+		}
+	}
+	dev_counter--;
+	mutex_unlock(&dev_list_lock);
+}
+
+static void compdev_destroy_all(void)
+{
+	struct compdev *cd;
+	struct compdev *tmp;
+	int i;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry_safe(cd, tmp, &dev_list, list) {
+		list_del(&cd->list);
+		misc_deregister(&cd->mdev);
+		for (i = 0; i < NUM_COMPDEV_BUFS; i++)
+			mcde_dss_destroy_overlay(cd->dss_ctx.ovly[i]);
+
+		release_prev_frame(&cd->dss_ctx);
+		/* Free potential temp buffers */
+		for (i = 0; i < cd->dss_ctx.temp_img_count; i++)
+			free_comp_img_buf(cd->dss_ctx.temp_img[i], cd->dev);
+
+		for (i = 0; i < BUFFER_CACHE_DEPTH; i++) {
+			if (cd->dss_ctx.cache_ctx.img[i]) {
+				free_comp_img_buf
+					(cd->dss_ctx.cache_ctx.img[i],
+						cd->dev);
+				cd->dss_ctx.cache_ctx.img[i] = NULL;
+			}
+		}
+
+		kfree(cd);
+	}
+	mutex_unlock(&dev_list_lock);
+
+	mutex_destroy(&dev_list_lock);
+}
+
+static int __init compdev_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	mutex_init(&dev_list_lock);
+
+	return 0;
+}
+module_init(compdev_init);
+
+static void __exit compdev_exit(void)
+{
+	compdev_destroy_all();
+	pr_info("%s\n", __func__);
+}
+module_exit(compdev_exit);
+
+MODULE_AUTHOR("Anders Bauer <anders.bauer@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Display overlay device driver");
+
diff --git a/drivers/misc/db8500-modem-trace.c b/drivers/misc/db8500-modem-trace.c
new file mode 100644
index 00000000000..b757b742121
--- /dev/null
+++ b/drivers/misc/db8500-modem-trace.c
@@ -0,0 +1,273 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors: Michel JAOUEN <michel.jaouen@stericsson.com>
+ *          Maxime COQUELIN <maxime.coquelin-nonst@stericsson.com>
+ *				for ST-Ericsson
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/device.h>
+#include <linux/cdev.h>
+#include <linux/poll.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/uaccess.h>
+#include <linux/mman.h>
+#include <linux/db8500-modem-trace.h>
+
+#include <mach/hardware.h>
+
+#define DEVICE_NAME "db8500-modem-trace"
+
+/* activation of this flag triggers an initialization of 2 buffers
+ * 4kbytes , id 0xdeadbeef
+ * and 16Kbytes id 0xfadafada
+ * we assume that platform provides minimum 20Kbytes. */
+
+struct trace {
+	u32 start;
+	u32 end;
+	u32 mdm_base;
+	u32 ape_base;
+	void __iomem *area;
+	/* this spinlock to forbid concurrent access on the same trace buffer */
+	spinlock_t lock;
+	struct device *dev;
+	struct miscdevice misc_dev;
+};
+
+struct trace_modem {
+	u32 phys_addr;
+	u8 filler;
+};
+
+static struct trace *trace_priv;
+
+
+/*  all this definition are linked to modem interface */
+#define MODEM_MARKER 0x88
+/*  free marker is also written on filler */
+#define FREE_MARKER 0xa5
+#define FREE_MARKER_2 0xa5a5
+#define READ_MARKER 0x5a
+
+struct buffer_header {
+	u8 pattern;
+	u8 filler;
+	u16 head_size;
+};
+
+
+static int trace_read(unsigned long arg)
+{
+	struct modem_trace_req req;
+	struct buffer_header *pt;
+	char tmp_char;
+
+	if (copy_from_user(&req, (struct modem_trace_req *)arg,
+				sizeof(struct modem_trace_req)))
+		return -EFAULT;
+
+	/* compute Modem physical address to APE physical address range  */
+	if (req.phys_addr < trace_priv->mdm_base) {
+		dev_err(trace_priv->dev, "MODEM ADDR uncorrect\n");
+		return -EINVAL;
+	}
+	req.phys_addr += trace_priv->ape_base - trace_priv->mdm_base;
+
+	/* check request is in the range and aligned */
+	if ((req.phys_addr % 4 != 0)
+			|| (req.phys_addr < trace_priv->start)
+			|| (req.phys_addr + req.size) >= trace_priv->end) {
+		dev_err(trace_priv->dev, "req out of range %x %x\n",
+			req.phys_addr, req.size);
+		return -EINVAL;
+	}
+
+	/* perform access to memory area  */
+	pt = (struct buffer_header *)((u32)trace_priv->area +
+			req.phys_addr - trace_priv->start);
+
+	/* in case of several request coming on same trace buffer take a
+	 * spinlock */
+	spin_lock(&trace_priv->lock);
+	if (pt->pattern != MODEM_MARKER) {
+		/* pattern and size not matching */
+		dev_err(trace_priv->dev, "req not matching filler %x/%x \
+				or/and pattern %x\n", req.filler, pt->filler,
+				pt->pattern);
+		spin_unlock(&trace_priv->lock);
+		return -EINVAL;
+	}
+	/* mark pattern as read and unlock spin */
+	pt->pattern = READ_MARKER;
+	spin_unlock(&trace_priv->lock);
+
+	req.size -= copy_to_user(req.buff, pt, req.size);
+
+	pt->pattern = FREE_MARKER;
+	pt->filler = FREE_MARKER;
+	tmp_char = MODEM_MARKER;
+
+	/*  Update marker for trace tool */
+	if (copy_to_user(req.buff, &tmp_char, 1))
+		return -EFAULT;
+
+	/*  Update effective written size */
+	if (copy_to_user((struct modem_trace_req *)arg, &req,
+				sizeof(struct modem_trace_req)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int trace_mmapdump(struct file *file, struct vm_area_struct *vma)
+{
+	unsigned long vma_start = vma->vm_start;
+
+	if (vma->vm_flags & VM_WRITE)
+		return -EPERM;
+
+	if ((vma->vm_end - vma->vm_start) <
+			(trace_priv->end - trace_priv->start))
+		return -EINVAL;
+	if (remap_pfn_range(vma,
+				vma_start,
+				trace_priv->start >> PAGE_SHIFT,
+				trace_priv->end - trace_priv->start,
+				vma->vm_page_prot))
+		return -EAGAIN;
+	return 0;
+}
+
+static long trace_ioctl(struct file *filp, unsigned int cmd,
+		unsigned long arg)
+{
+	long ret = 0;
+	void __user *argp = (void __user *)arg;
+	unsigned long size = trace_priv->end-trace_priv->start;
+
+	switch (cmd) {
+	case TM_GET_DUMPINFO:
+		ret = put_user(size, (unsigned long *)argp);
+		break;
+	case TM_TRACE_REQ:
+		ret = trace_read(arg);
+		break;
+
+	default:
+		ret = -EPERM;
+		break;
+	}
+	return ret;
+}
+
+static const struct file_operations trace_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = trace_ioctl,
+	.mmap = trace_mmapdump
+};
+
+static int trace_probe(struct platform_device *pdev)
+{
+	int rv = 0;
+	struct db8500_trace_platform_data *pdata = pdev->dev.platform_data;
+	/* retrieve area descriptor from platform device ressource */
+	struct resource *mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if ((mem->start == 0) && (mem->end == 0)) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	if ((pdata->ape_base == 0) || (pdata->modem_base == 0)) {
+		rv = -EINVAL;
+		goto out;
+	}
+
+	trace_priv = kzalloc(sizeof(*trace_priv), GFP_ATOMIC);
+	if (!trace_priv) {
+		rv = -ENOMEM;
+		goto out;
+	}
+
+	trace_priv->dev = &pdev->dev;
+	trace_priv->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	trace_priv->misc_dev.name = DEVICE_NAME;
+	trace_priv->misc_dev.fops = &trace_fops;
+	trace_priv->area = (void __iomem *)ioremap_nocache(mem->start,
+						resource_size(mem));
+	if (!trace_priv->area) {
+		rv = -ENOMEM;
+		goto outfree;
+	}
+
+	trace_priv->start = mem->start;
+	trace_priv->end = mem->end;
+
+	trace_priv->mdm_base = pdata->modem_base;
+	trace_priv->ape_base = pdata->ape_base;
+
+	/*  spin allowing smp access for reading/writing trace buffer header  */
+	spin_lock_init(&trace_priv->lock);
+
+	rv = misc_register(&trace_priv->misc_dev);
+	if (rv) {
+		dev_err(&pdev->dev, "can't misc_register\n");
+		goto outunmap;
+	}
+
+	return rv;
+
+outunmap:
+	iounmap(trace_priv->area);
+outfree:
+	kfree(trace_priv);
+out:
+	return rv;
+
+}
+
+static int trace_remove(struct platform_device *pdev)
+{
+	int rv = 0;
+
+	if (trace_priv) {
+		rv = misc_deregister(&trace_priv->misc_dev);
+		iounmap(trace_priv->area);
+		kfree(trace_priv);
+	}
+
+	return rv;
+}
+
+static struct platform_driver trace_driver = {
+	.probe = trace_probe,
+	.remove = trace_remove,
+	.driver = {
+		.name = "db8500-modem-trace",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int trace_init(void)
+{
+	platform_driver_register(&trace_driver);
+	return 0;
+}
+static void trace_exit(void)
+{
+	platform_driver_unregister(&trace_driver);
+}
+module_init(trace_init);
+module_exit(trace_exit);
+
+MODULE_AUTHOR("ST-Ericsson");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/dbx500-mloader.c b/drivers/misc/dbx500-mloader.c
new file mode 100644
index 00000000000..408d1a1a29e
--- /dev/null
+++ b/drivers/misc/dbx500-mloader.c
@@ -0,0 +1,288 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Ludovic Barre <ludovic.barre@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/firmware.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/uaccess.h>
+#include <linux/mman.h>
+#include <linux/io.h>
+
+#include <mach/mloader-dbx500.h>
+#include <linux/mloader.h>
+#include <mach/hardware.h>
+
+#define DEVICE_NAME "dbx500_mloader_fw"
+
+struct mloader_priv {
+	struct platform_device *pdev;
+	struct dbx500_mloader_pdata *pdata;
+	struct miscdevice misc_dev;
+	u32 aeras_size;
+	void __iomem *uid_base;
+	u8 size;
+};
+
+static struct mloader_priv *mloader_priv;
+
+static int mloader_fw_send(struct dbx500_ml_fw *fw_info)
+{
+	const struct firmware *fw;
+	unsigned long size;
+	unsigned long phys_start;
+	void *fw_data;
+	void *vaddr;
+	void __iomem *ioaddr;
+	int ret;
+
+	ret = request_firmware(&fw, fw_info->name, &mloader_priv->pdev->dev);
+	if (ret) {
+		dev_err(&mloader_priv->pdev->dev, "request firmware failed\n");
+		goto out;
+	}
+
+	if (fw->size > (fw_info->area->size - fw_info->offset)) {
+		dev_err(&mloader_priv->pdev->dev,
+				"fw:%s is too big for:%s\n",
+				fw_info->name, fw_info->area->name);
+		ret = -EINVAL;
+		goto err_fw;
+	}
+
+	size = PAGE_ALIGN(fw->size);
+	phys_start = fw_info->area->start + fw_info->offset;
+	phys_start &= PAGE_MASK;
+	ioaddr = ioremap(phys_start, size);
+	if (!ioaddr) {
+		dev_err(&mloader_priv->pdev->dev,
+				"failed remap memory region.\n");
+		ret = -EINVAL;
+		goto err_fw;
+	}
+
+	vaddr = ioaddr + (fw_info->offset & ~PAGE_MASK);
+	fw_data = (void *)fw->data;
+	memcpy_toio(vaddr, fw_data, fw->size);
+	iounmap(ioaddr);
+
+err_fw:
+	release_firmware(fw);
+out:
+	return ret;
+}
+
+static int mloader_fw_upload(void)
+{
+	int i, ret;
+	struct dbx500_mloader_pdata *pdata = mloader_priv->pdata;
+
+	for (i = 0; i < pdata->nr_fws; i++) {
+		ret = mloader_fw_send(&pdata->fws[i]);
+		if (ret)
+			goto err;
+	}
+
+	return 0;
+err:
+	dev_err(&mloader_priv->pdev->dev,
+				"Failed to upload %s firmware", pdata->fws[i].name);
+	return ret;
+}
+
+static int mloader_fw_mmapdump(struct file *file, struct vm_area_struct *vma)
+{
+	int i;
+	unsigned long dump_size = 0;
+	unsigned long vma_start = vma->vm_start;
+
+	if (vma->vm_flags & VM_WRITE)
+		return -EPERM;
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_areas ; i++)
+		dump_size += mloader_priv->pdata->areas[i].size;
+
+	if ((vma->vm_end - vma->vm_start) < dump_size)
+		return -EINVAL;
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_areas ; i++) {
+		if (remap_pfn_range(vma,
+			    vma_start,
+			    mloader_priv->pdata->areas[i].start >> PAGE_SHIFT,
+			    mloader_priv->pdata->areas[i].size,
+			    vma->vm_page_prot))
+			return -EAGAIN;
+		vma_start += mloader_priv->pdata->areas[i].size;
+	}
+	return 0;
+}
+
+static void mloader_fw_dumpinfo(struct dump_image *images)
+{
+	u32 offset = 0;
+	int i;
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_areas ; i++) {
+		strncpy(images[i].name,
+				mloader_priv->pdata->areas[i].name, MAX_NAME);
+		images[i].name[MAX_NAME-1] = 0;
+		images[i].offset = offset;
+		images[i].size = mloader_priv->pdata->areas[i].size;
+		offset += mloader_priv->pdata->areas[i].size;
+	}
+}
+
+static long mloader_fw_ioctl(struct file *filp, unsigned int cmd,
+		unsigned long arg)
+{
+	long ret = 0;
+	void __user *argp = (void __user *)arg;
+
+	switch (cmd) {
+	case ML_UPLOAD:
+		ret = mloader_fw_upload();
+		break;
+	case ML_GET_NBIMAGES:
+		ret = put_user(mloader_priv->pdata->nr_areas,
+				(unsigned long __user *)argp);
+		break;
+	case ML_GET_DUMPINFO: {
+		struct dump_image *dump_images;
+		dump_images = kzalloc(mloader_priv->pdata->nr_areas
+				* sizeof(struct dump_image), GFP_ATOMIC);
+		mloader_fw_dumpinfo(dump_images);
+		ret = copy_to_user(argp, (void *) dump_images,
+				mloader_priv->pdata->nr_areas
+				* sizeof(struct dump_image)) ? -EFAULT : 0;
+		kfree(dump_images);
+		break;
+	}
+	case ML_GET_FUSEINFO: {
+		ret = copy_to_user(argp, (void *) mloader_priv->uid_base,
+				mloader_priv->size) ? -EFAULT : 0;
+		break;
+	}
+	default:
+		ret = -EPERM;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct file_operations modem_fw_fops = {
+	.owner			= THIS_MODULE,
+	.unlocked_ioctl		= mloader_fw_ioctl,
+	.mmap			= mloader_fw_mmapdump,
+};
+
+static int __devinit mloader_fw_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	int i;
+	struct resource *res = NULL;
+
+	mloader_priv = kzalloc(sizeof(*mloader_priv), GFP_ATOMIC);
+	if (!mloader_priv) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	mloader_priv->pdev = pdev;
+	mloader_priv->pdata = pdev->dev.platform_data;
+
+	mloader_priv->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	mloader_priv->misc_dev.name = DEVICE_NAME;
+	mloader_priv->misc_dev.fops = &modem_fw_fops;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mloader_priv->size = resource_size(res);
+	mloader_priv->uid_base = ioremap(res->start, mloader_priv->size);
+
+	if (!mloader_priv->uid_base) {
+		   ret = -ENOMEM;
+		   goto err_free_priv;
+	}
+
+	ret = misc_register(&mloader_priv->misc_dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can't misc_register\n");
+		goto err_free_priv;
+	}
+
+	dev_info(&mloader_priv->pdev->dev, "mloader device register\n");
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_areas ; i++) {
+		dev_dbg(&mloader_priv->pdev->dev,
+				"Area:%d (name:%s start:%x size:%x)\n",
+				i, mloader_priv->pdata->areas[i].name,
+				mloader_priv->pdata->areas[i].start,
+				mloader_priv->pdata->areas[i].size);
+	}
+
+	for (i = 0 ; i < mloader_priv->pdata->nr_fws ; i++) {
+		dev_dbg(&mloader_priv->pdev->dev,
+				"Firmware:%d (name:%s offset:%x "
+				"area_name:%s area_start:%x area_size:%x)\n",
+				i, mloader_priv->pdata->fws[i].name,
+				mloader_priv->pdata->fws[i].offset,
+				mloader_priv->pdata->fws[i].area->name,
+				mloader_priv->pdata->fws[i].area->start,
+				mloader_priv->pdata->fws[i].area->size);
+	}
+
+	return ret;
+
+err_free_priv:
+	kfree(mloader_priv);
+out:
+	return ret;
+}
+
+static int __devexit mloader_fw_remove(struct platform_device *pdev)
+{
+	int err;
+
+	err = misc_register(&mloader_priv->misc_dev);
+	if (err < 0)
+		dev_err(&pdev->dev, "can't misc_deregister, %d\n", err);
+
+	kfree(mloader_priv);
+
+	return err;
+}
+
+static struct platform_driver mloader_fw_driver = {
+	.driver.name	= DEVICE_NAME,
+	.driver.owner	= THIS_MODULE,
+	.probe		= mloader_fw_probe,
+	.remove		= __devexit_p(mloader_fw_remove),
+};
+
+static int __init mloader_fw_init(void)
+{
+	return platform_driver_register(&mloader_fw_driver);
+}
+
+static void __exit mloader_fw_exit(void)
+{
+	kfree(mloader_priv);
+	platform_driver_unregister(&mloader_fw_driver);
+}
+
+module_init(mloader_fw_init);
+module_exit(mloader_fw_exit);
+MODULE_DESCRIPTION("ST-Ericsson modem loader firmware");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Ludovic Barre <ludovic.barre@stericsson.com>");
diff --git a/drivers/misc/dispdev/Makefile b/drivers/misc/dispdev/Makefile
new file mode 100644
index 00000000000..11dc7611d26
--- /dev/null
+++ b/drivers/misc/dispdev/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_DISPDEV) += dispdev.o
diff --git a/drivers/misc/dispdev/dispdev.c b/drivers/misc/dispdev/dispdev.c
new file mode 100644
index 00000000000..5413a252d35
--- /dev/null
+++ b/drivers/misc/dispdev/dispdev.c
@@ -0,0 +1,659 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Display output device driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/idr.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/ioctl.h>
+
+#include <linux/dispdev.h>
+#include <linux/hwmem.h>
+#include <video/mcde_dss.h>
+
+#define DENSITY_CHECK (16)
+#define MAX_BUFFERS 4
+
+static LIST_HEAD(dev_list);
+static DEFINE_MUTEX(dev_list_lock);
+
+enum buffer_state {
+	BUF_UNUSED = 0,
+	BUF_QUEUED,
+	BUF_ACTIVATED,
+/*TODO:waitfordone	BUF_DEACTIVATED,*/
+	BUF_FREE,
+	BUF_DEQUEUED,
+};
+
+struct dispdev_buffer {
+	struct hwmem_alloc *alloc;
+	u32 size;
+	enum buffer_state state;
+	u32 paddr; /* if pinned */
+};
+
+struct dispdev {
+	bool open;
+	struct mutex lock;
+	struct miscdevice mdev;
+	struct list_head list;
+	struct mcde_display_device *ddev;
+	struct mcde_overlay *ovly;
+	struct mcde_overlay *parent_ovly;
+	struct dispdev_config config;
+	bool overlay;
+	struct dispdev_buffer buffers[MAX_BUFFERS];
+	wait_queue_head_t waitq_dq;
+	/*
+	 * For the rotation use case
+	 * buffers_need_update is used to ensure that a set_config that
+	 * changes width or height is followed by a unregister_buffer.
+	 */
+	bool buffers_need_update;
+	/*
+	 * For the overlay startup use case.
+	 * first_update is used to handle the first update after a set_config.
+	 * In this case a queue_buffer will arrive after set_config and not a
+	 * unregister_buffer as in the rotation use case.
+	 */
+	bool first_update;
+	char name[sizeof(DISPDEV_DEFAULT_DEVICE_PREFIX) + 3];
+};
+
+static int find_buf(struct dispdev *dd, enum buffer_state state)
+{
+	int i;
+	for (i = 0; i < MAX_BUFFERS; i++)
+		if (dd->buffers[i].state == state)
+			return i;
+	return -1;
+}
+
+int dispdev_open(struct inode *inode, struct file *file)
+{
+	int ret;
+	struct dispdev *dd = NULL;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry(dd, &dev_list, list)
+		if (dd->mdev.minor == iminor(inode))
+			break;
+
+	if (&dd->list == &dev_list) {
+		mutex_unlock(&dev_list_lock);
+		return -ENODEV;
+	}
+
+	if (dd->open) {
+		mutex_unlock(&dev_list_lock);
+		return -EBUSY;
+	}
+
+	dd->open = true;
+
+	mutex_unlock(&dev_list_lock);
+
+	ret = mcde_dss_enable_overlay(dd->ovly);
+	if (ret)
+		return ret;
+
+	file->private_data = dd;
+
+	return 0;
+}
+
+int dispdev_release(struct inode *inode, struct file *file)
+{
+	int i;
+	struct dispdev *dd = NULL;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry(dd, &dev_list, list)
+		if (dd->mdev.minor == iminor(inode))
+			break;
+	mutex_unlock(&dev_list_lock);
+
+	if (&dd->list == &dev_list)
+		return -ENODEV;
+
+	/* TODO: Make sure it waits for completion */
+	mcde_dss_disable_overlay(dd->ovly);
+	for (i = 0; i < MAX_BUFFERS; i++) {
+		if (dd->buffers[i].paddr)
+			hwmem_unpin(dd->buffers[i].alloc);
+		if (dd->buffers[i].alloc)
+			hwmem_release(dd->buffers[i].alloc);
+		dd->buffers[i].alloc = NULL;
+		dd->buffers[i].state = BUF_UNUSED;
+		dd->buffers[i].size = 0;
+		dd->buffers[i].paddr = 0;
+	}
+	dd->open = false;
+	wake_up(&dd->waitq_dq);
+	return 0;
+}
+
+static enum mcde_ovly_pix_fmt get_ovly_fmt(enum dispdev_fmt fmt)
+{
+	switch (fmt) {
+	default:
+	case DISPDEV_FMT_RGB565:
+		return MCDE_OVLYPIXFMT_RGB565;
+	case DISPDEV_FMT_RGB888:
+		return MCDE_OVLYPIXFMT_RGB888;
+	case DISPDEV_FMT_RGBA8888:
+		return MCDE_OVLYPIXFMT_RGBA8888;
+	case DISPDEV_FMT_RGBX8888:
+		return MCDE_OVLYPIXFMT_RGBX8888;
+	case DISPDEV_FMT_YUV422:
+		return MCDE_OVLYPIXFMT_YCbCr422;
+	}
+}
+
+static void get_ovly_info(struct dispdev_config *cfg,
+				struct mcde_video_mode *vmode,
+				struct mcde_overlay_info *info, bool overlay)
+{
+	info->paddr = 0;
+	info->stride = cfg->stride;
+	info->fmt = get_ovly_fmt(cfg->format);
+	info->src_x = 0;
+	info->src_y = 0;
+	info->dst_x = cfg->x;
+	info->dst_y = cfg->y;
+	info->dst_z = cfg->z;
+	info->w = cfg->width;
+	info->h = cfg->height;
+	info->dirty.x = 0;
+	info->dirty.y = 0;
+	info->dirty.w = vmode->xres;
+	info->dirty.h = vmode->yres;
+}
+
+static int dispdev_set_config(struct dispdev *dd, struct dispdev_config *cfg)
+{
+	int ret = 0;
+	if (memcmp(&dd->config, cfg, sizeof(struct dispdev_config)) == 0)
+		return 0;
+
+	/*
+	 * Only update MCDE if format, stride, width and height
+	 * is the same. Otherwise just store the new config and update
+	 * MCDE in the next queue buffer. This because the buffer that is
+	 * active can be have the wrong format, width ...
+	 */
+	if (cfg->format == dd->config.format &&
+			cfg->stride == dd->config.stride &&
+			cfg->width == dd->config.width &&
+			cfg->height == dd->config.height) {
+
+		int buf_index;
+		if (!dd->buffers_need_update) {
+			buf_index = find_buf(dd, BUF_ACTIVATED);
+			if (buf_index >= 0) {
+				struct mcde_overlay_info info;
+				struct dispdev_buffer *buf;
+				struct mcde_video_mode vmode;
+
+				buf = &dd->buffers[buf_index];
+				mcde_dss_get_video_mode(dd->ddev, &vmode);
+				get_ovly_info(cfg, &vmode, &info, dd->overlay);
+				info.paddr = buf->paddr;
+				ret = mcde_dss_apply_overlay(dd->ovly, &info);
+				if (!ret)
+					mcde_dss_update_overlay(dd->ovly,
+									false);
+			}
+		}
+	} else {
+		dd->buffers_need_update = true;
+	}
+
+	dd->config = *cfg;
+
+	return ret;
+}
+
+static int dispdev_register_buffer(struct dispdev *dd, s32 hwmem_name)
+{
+	int ret;
+	struct dispdev_buffer *buf;
+	enum hwmem_mem_type memtype;
+	enum hwmem_access access;
+
+	ret = find_buf(dd, BUF_UNUSED);
+	if (ret < 0)
+		return -ENOMEM;
+	buf = &dd->buffers[ret];
+	buf->alloc = hwmem_resolve_by_name(hwmem_name);
+	if (IS_ERR(buf->alloc)) {
+		ret = PTR_ERR(buf->alloc);
+		goto resolve_failed;
+	}
+
+	hwmem_get_info(buf->alloc, &buf->size, &memtype, &access);
+
+	if (!(access & HWMEM_ACCESS_READ) ||
+					memtype != HWMEM_MEM_CONTIGUOUS_SYS) {
+		ret = -EACCES;
+		goto invalid_mem;
+	}
+
+	buf->state = BUF_FREE;
+	goto out;
+invalid_mem:
+	hwmem_release(buf->alloc);
+resolve_failed:
+out:
+	return ret;
+}
+
+static int dispdev_unregister_buffer(struct dispdev *dd, u32 buf_idx)
+{
+	struct dispdev_buffer *buf = &dd->buffers[buf_idx];
+
+	if (buf_idx >= ARRAY_SIZE(dd->buffers))
+		return -EINVAL;
+
+	if (buf->state == BUF_UNUSED)
+		return -EINVAL;
+
+	if (dd->buffers_need_update)
+		dd->buffers_need_update = false;
+
+	if (buf->state == BUF_ACTIVATED) {
+		/* Disable the overlay */
+		struct mcde_overlay_info info;
+		struct mcde_video_mode vmode;
+		/* TODO Wait for frame done */
+		mcde_dss_get_video_mode(dd->ddev, &vmode);
+		get_ovly_info(&dd->config, &vmode, &info, dd->overlay);
+		mcde_dss_apply_overlay(dd->ovly, &info);
+		mcde_dss_update_overlay(dd->ovly, false);
+		hwmem_unpin(dd->buffers[buf_idx].alloc);
+	}
+
+	hwmem_release(buf->alloc);
+	buf->state = BUF_UNUSED;
+	buf->alloc = NULL;
+	buf->size = 0;
+	buf->paddr = 0;
+	dd->first_update = false;
+
+	return 0;
+}
+
+
+/**
+ * @brief Check if the buffer is transparent or black (ARGB = X000)
+ *        Note: Only for ARGB32.
+ *        Worst case: a ~full transparent buffer
+ *        Results: ~2200us @800Mhz for a WVGA screen, with DENSITY_CHECK=8
+ *                 ~520us  @800Mhz for a WVGA screen, with DENSITY_CHECK=16
+ *
+ * @param w witdh
+ * @param h height
+ * @param addr buffer addr
+ *
+ * @return 1 if the buffer is transparent, else 0
+ */
+static int is_transparent(int w, int h, u32 *addr)
+{
+	int i, j;
+	u32 *c, *next_line;
+	u32 sum;
+
+	next_line = addr;
+	sum = 0;
+
+	/* TODO Optimize me */
+	for (j = 0; j < h; j += DENSITY_CHECK) {
+		c = next_line;
+		for (i = 0; i < w; i += DENSITY_CHECK) {
+			sum += ((*c) & 0x00FFFFFF);
+			c += DENSITY_CHECK;
+		}
+		if (sum)
+			return 0; /* Not "transparent" */
+		next_line += (w * DENSITY_CHECK);
+	}
+
+	return 1; /* "Transparent" */
+}
+
+static int dispdev_queue_buffer(struct dispdev *dd,
+					struct dispdev_buffer_info *buffer)
+{
+	int ret, i;
+	struct mcde_overlay_info info;
+	struct hwmem_mem_chunk mem_chunk;
+	size_t mem_chunk_length = 1;
+	struct hwmem_region rgn = { .offset = 0, .count = 1, .start = 0 };
+	struct hwmem_alloc *alloc;
+	struct mcde_video_mode vmode;
+	u32 buf_idx = buffer->buf_idx;
+
+	if (buf_idx >= ARRAY_SIZE(dd->buffers) ||
+				 dd->buffers[buf_idx].state != BUF_DEQUEUED)
+		return -EINVAL;
+
+	alloc = dd->buffers[buf_idx].alloc;
+	mcde_dss_get_video_mode(dd->ddev, &vmode);
+	get_ovly_info(&dd->config, &vmode, &info, dd->overlay);
+	ret = hwmem_pin(alloc, &mem_chunk, &mem_chunk_length);
+	if (ret) {
+		dev_warn(dd->mdev.this_device, "Pin failed, %d\n", ret);
+		return -EINVAL;
+	}
+
+	rgn.size = rgn.end = dd->buffers[buf_idx].size;
+	ret = hwmem_set_domain(alloc, HWMEM_ACCESS_READ,
+		HWMEM_DOMAIN_SYNC, &rgn);
+	if (ret)
+		dev_warn(dd->mdev.this_device, "Set domain failed, %d\n", ret);
+
+	i = find_buf(dd, BUF_ACTIVATED);
+	if (i >= 0) {
+		dd->buffers[i].state = BUF_FREE;
+		wake_up(&dd->waitq_dq);
+	}
+
+	if (!dd->first_update) {
+		dd->first_update = true;
+		dd->buffers_need_update = false;
+	}
+
+	dd->buffers[buf_idx].paddr = mem_chunk.paddr;
+
+	if (buffer->display_update && !dd->buffers_need_update &&
+			dd->config.width == buffer->buf_cfg.width &&
+			dd->config.height == buffer->buf_cfg.height &&
+			dd->config.format == buffer->buf_cfg.format &&
+			dd->config.stride == buffer->buf_cfg.stride) {
+		info.paddr = mem_chunk.paddr;
+		mcde_dss_apply_overlay(dd->ovly, &info);
+		mcde_dss_update_overlay(dd->ovly, false);
+	} else if (buffer->display_update) {
+		dd->buffers_need_update = true;
+	}
+
+	/* Disable the MCDE FB overlay */
+	if ((dd->parent_ovly->state != NULL) &&
+			(dd->ddev->check_transparency)) {
+		dd->ddev->check_transparency--;
+		mcde_dss_get_overlay_info(dd->parent_ovly, &info);
+		if (dd->ddev->check_transparency == 0) {
+			if (is_transparent(info.w, info.h, info.vaddr)) {
+				mcde_dss_disable_overlay(dd->parent_ovly);
+				printk(KERN_INFO "%s Disable overlay\n",
+								__func__);
+			}
+		}
+	}
+
+	dd->buffers[buf_idx].state = BUF_ACTIVATED;
+
+	return 0;
+}
+
+static int dispdev_dequeue_buffer(struct dispdev *dd)
+{
+	int i;
+
+	i = find_buf(dd, BUF_FREE);
+	if (i < 0) {
+		if (find_buf(dd, BUF_ACTIVATED) < 0)
+			return -EINVAL;
+		mutex_unlock(&dd->lock);
+		wait_event(dd->waitq_dq, (i = find_buf(dd, BUF_FREE)) >= 0);
+		mutex_lock(&dd->lock);
+	}
+	hwmem_unpin(dd->buffers[i].alloc);
+	dd->buffers[i].state = BUF_DEQUEUED;
+	dd->buffers[i].paddr = 0;
+
+	return i;
+}
+
+long dispdev_ioctl(struct file *file, unsigned int cmd,
+							unsigned long arg)
+{
+	int ret;
+	struct dispdev *dd = (struct dispdev *)file->private_data;
+
+	mutex_lock(&dd->lock);
+
+	switch (cmd) {
+	case DISPDEV_SET_CONFIG_IOC:
+		{
+			struct dispdev_config cfg;
+			if (copy_from_user(&cfg, (void __user *)arg,
+								sizeof(cfg)))
+				ret = -EFAULT;
+			else
+				ret = dispdev_set_config(dd, &cfg);
+		}
+		break;
+	case DISPDEV_GET_CONFIG_IOC:
+		ret = copy_to_user((void __user *)arg, &dd->config,
+							sizeof(dd->config));
+		if (ret)
+			ret = -EFAULT;
+		break;
+	case DISPDEV_REGISTER_BUFFER_IOC:
+		ret = dispdev_register_buffer(dd, (s32)arg);
+		break;
+	case DISPDEV_UNREGISTER_BUFFER_IOC:
+		ret = dispdev_unregister_buffer(dd, (u32)arg);
+		break;
+	case DISPDEV_QUEUE_BUFFER_IOC:
+	{
+		struct dispdev_buffer_info buffer;
+		if (copy_from_user(&buffer, (void __user *)arg,
+							sizeof(buffer)))
+			ret = -EFAULT;
+		else
+			ret = dispdev_queue_buffer(dd, &buffer);
+		break;
+	}
+	case DISPDEV_DEQUEUE_BUFFER_IOC:
+		ret = dispdev_dequeue_buffer(dd);
+		break;
+	default:
+		ret = -ENOSYS;
+	}
+
+	mutex_unlock(&dd->lock);
+
+	return ret;
+}
+
+static const struct file_operations dispdev_fops = {
+	.open = dispdev_open,
+	.release = dispdev_release,
+	.unlocked_ioctl = dispdev_ioctl,
+};
+
+static void init_dispdev(struct dispdev *dd, struct mcde_display_device *ddev,
+						const char *name, bool overlay)
+{
+	u16 w, h;
+	int rotation;
+
+	mutex_init(&dd->lock);
+	INIT_LIST_HEAD(&dd->list);
+	dd->ddev = ddev;
+	dd->overlay = overlay;
+	mcde_dss_get_native_resolution(ddev, &w, &h);
+	rotation = mcde_dss_get_rotation(ddev);
+
+	if ((rotation == MCDE_DISPLAY_ROT_90_CCW) ||
+			(rotation == MCDE_DISPLAY_ROT_90_CW)) {
+		dd->config.width = h;
+		dd->config.height = w;
+	} else {
+		dd->config.width  = w;
+		dd->config.height = h;
+	}
+	dd->config.format = DISPDEV_FMT_RGB565;
+	dd->config.stride = sizeof(u16) * w;
+	dd->config.x = 0;
+	dd->config.y = 0;
+	dd->config.z = 0;
+	dd->buffers_need_update = false;
+	dd->first_update = false;
+	init_waitqueue_head(&dd->waitq_dq);
+	dd->mdev.minor = MISC_DYNAMIC_MINOR;
+	dd->mdev.name = name;
+	dd->mdev.fops = &dispdev_fops;
+	pr_info("%s: name=%s w=%d, h=%d, fmt=%d, stride=%d\n", __func__, name,
+			dd->config.width, dd->config.height, dd->config.format,
+							dd->config.stride);
+}
+
+int dispdev_create(struct mcde_display_device *ddev, bool overlay,
+					struct mcde_overlay *parent_ovly)
+{
+	int ret = 0;
+	struct dispdev *dd;
+	struct mcde_video_mode vmode;
+	struct mcde_overlay_info info = {0};
+
+	static int counter;
+
+	dd = kzalloc(sizeof(struct dispdev), GFP_KERNEL);
+	if (!dd)
+		return -ENOMEM;
+
+	snprintf(dd->name, sizeof(dd->name), "%s%d",
+		 DISPDEV_DEFAULT_DEVICE_PREFIX, counter++);
+	init_dispdev(dd, ddev, dd->name, overlay);
+
+	if (!overlay) {
+		ret = mcde_dss_enable_display(ddev);
+		if (ret)
+			goto fail_enable_display;
+		mcde_dss_get_video_mode(ddev, &vmode);
+		mcde_dss_try_video_mode(ddev, &vmode);
+		ret = mcde_dss_set_video_mode(ddev, &vmode);
+		if (ret)
+			goto fail_set_video_mode;
+		mcde_dss_set_pixel_format(ddev, info.fmt);
+		mcde_dss_apply_channel(ddev);
+	} else
+		mcde_dss_get_video_mode(ddev, &vmode);
+	get_ovly_info(&dd->config, &vmode, &info, overlay);
+
+	/* Save the MCDE FB overlay */
+	dd->parent_ovly = parent_ovly;
+
+	dd->ovly = mcde_dss_create_overlay(ddev, &info);
+	if (!dd->ovly) {
+		ret = -ENOMEM;
+		goto fail_create_ovly;
+	}
+
+	ret = misc_register(&dd->mdev);
+	if (ret)
+		goto fail_register_misc;
+	mutex_lock(&dev_list_lock);
+	list_add_tail(&dd->list, &dev_list);
+	mutex_unlock(&dev_list_lock);
+
+	goto out;
+
+fail_register_misc:
+	mcde_dss_destroy_overlay(dd->ovly);
+fail_create_ovly:
+	if (!overlay)
+		mcde_dss_disable_display(ddev);
+fail_set_video_mode:
+fail_enable_display:
+	kfree(dd);
+out:
+	return ret;
+}
+
+void dispdev_destroy(struct mcde_display_device *ddev)
+{
+	struct dispdev *dd;
+	struct dispdev *tmp;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry_safe(dd, tmp, &dev_list, list) {
+		if (dd->ddev == ddev) {
+			list_del(&dd->list);
+			misc_deregister(&dd->mdev);
+			mcde_dss_destroy_overlay(dd->ovly);
+			/*
+			 * TODO: Uncomment when DSS has reference
+			 * counting of enable/disable
+			 */
+			/* mcde_dss_disable_display(dd->ddev); */
+			kfree(dd);
+			break;
+		}
+	}
+	mutex_unlock(&dev_list_lock);
+}
+
+static void dispdev_destroy_all(void)
+{
+	struct dispdev *dd;
+	struct dispdev *tmp;
+
+	mutex_lock(&dev_list_lock);
+	list_for_each_entry_safe(dd, tmp, &dev_list, list) {
+		list_del(&dd->list);
+		misc_deregister(&dd->mdev);
+		mcde_dss_destroy_overlay(dd->ovly);
+		/*
+		 * TODO: Uncomment when DSS has reference
+		 * counting of enable/disable
+		 */
+		/* mcde_dss_disable_display(dd->ddev); */
+		kfree(dd);
+	}
+	mutex_unlock(&dev_list_lock);
+
+	mutex_destroy(&dev_list_lock);
+}
+
+static int __init dispdev_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	mutex_init(&dev_list_lock);
+
+	return 0;
+}
+module_init(dispdev_init);
+
+static void __exit dispdev_exit(void)
+{
+	dispdev_destroy_all();
+	pr_info("%s\n", __func__);
+}
+module_exit(dispdev_exit);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Display output device driver");
+
diff --git a/drivers/misc/hwmem/Makefile b/drivers/misc/hwmem/Makefile
new file mode 100644
index 00000000000..c307616a181
--- /dev/null
+++ b/drivers/misc/hwmem/Makefile
@@ -0,0 +1,3 @@
+hwmem-objs := hwmem-main.o hwmem-ioctl.o cache_handler.o contig_alloc.o
+
+obj-$(CONFIG_HWMEM) += hwmem.o
diff --git a/drivers/misc/hwmem/cache_handler.c b/drivers/misc/hwmem/cache_handler.c
new file mode 100644
index 00000000000..e0ab4ee6cf8
--- /dev/null
+++ b/drivers/misc/hwmem/cache_handler.c
@@ -0,0 +1,510 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Cache handler
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/hwmem.h>
+
+#include <asm/pgtable.h>
+
+#include <mach/dcache.h>
+
+#include "cache_handler.h"
+
+#define U32_MAX (~(u32)0)
+
+enum hwmem_alloc_flags cachi_get_cache_settings(
+			enum hwmem_alloc_flags requested_cache_settings);
+void cachi_set_pgprot_cache_options(enum hwmem_alloc_flags cache_settings,
+							pgprot_t *pgprot);
+
+static void sync_buf_pre_cpu(struct cach_buf *buf, enum hwmem_access access,
+						struct hwmem_region *region);
+static void sync_buf_post_cpu(struct cach_buf *buf,
+	enum hwmem_access next_access, struct hwmem_region *next_region);
+
+static void invalidate_cpu_cache(struct cach_buf *buf,
+					struct cach_range *range_2b_used);
+static void clean_cpu_cache(struct cach_buf *buf,
+					struct cach_range *range_2b_used);
+static void flush_cpu_cache(struct cach_buf *buf,
+					struct cach_range *range_2b_used);
+
+static void null_range(struct cach_range *range);
+static void expand_range(struct cach_range *range,
+					struct cach_range *range_2_add);
+/*
+ * Expands range to one of enclosing_range's two edges. The function will
+ * choose which of enclosing_range's edges to expand range to in such a
+ * way that the size of range is minimized. range must be located inside
+ * enclosing_range.
+ */
+static void expand_range_2_edge(struct cach_range *range,
+					struct cach_range *enclosing_range);
+static void shrink_range(struct cach_range *range,
+					struct cach_range *range_2_remove);
+static bool is_non_empty_range(struct cach_range *range);
+static void intersect_range(struct cach_range *range_1,
+		struct cach_range *range_2, struct cach_range *intersection);
+/* Align_up restrictions apply here to */
+static void align_range_up(struct cach_range *range, u32 alignment);
+static u32 range_length(struct cach_range *range);
+static void region_2_range(struct hwmem_region *region, u32 buffer_size,
+						struct cach_range *range);
+
+static void *offset_2_vaddr(struct cach_buf *buf, u32 offset);
+static u32 offset_2_paddr(struct cach_buf *buf, u32 offset);
+
+/* Saturates, might return unaligned values when that happens */
+static u32 align_up(u32 value, u32 alignment);
+static u32 align_down(u32 value, u32 alignment);
+
+/*
+ * Exported functions
+ */
+
+void cach_init_buf(struct cach_buf *buf, enum hwmem_alloc_flags cache_settings,
+								u32 size)
+{
+	buf->vstart = NULL;
+	buf->pstart = 0;
+	buf->size = size;
+
+	buf->cache_settings = cachi_get_cache_settings(cache_settings);
+}
+
+void cach_set_buf_addrs(struct cach_buf *buf, void* vaddr, u32 paddr)
+{
+	bool tmp;
+
+	buf->vstart = vaddr;
+	buf->pstart = paddr;
+
+	if (buf->cache_settings & HWMEM_ALLOC_HINT_CACHED) {
+		/*
+		 * Keep whatever is in the cache. This way we avoid an
+		 * unnecessary synch if CPU is the first user.
+		 */
+		buf->range_in_cpu_cache.start = 0;
+		buf->range_in_cpu_cache.end = buf->size;
+		align_range_up(&buf->range_in_cpu_cache,
+						get_dcache_granularity());
+		buf->range_dirty_in_cpu_cache.start = 0;
+		buf->range_dirty_in_cpu_cache.end = buf->size;
+		align_range_up(&buf->range_dirty_in_cpu_cache,
+						get_dcache_granularity());
+	} else {
+		flush_cpu_dcache(buf->vstart, buf->pstart, buf->size, false,
+									&tmp);
+		drain_cpu_write_buf();
+
+		null_range(&buf->range_in_cpu_cache);
+		null_range(&buf->range_dirty_in_cpu_cache);
+	}
+	null_range(&buf->range_invalid_in_cpu_cache);
+}
+
+void cach_set_pgprot_cache_options(struct cach_buf *buf, pgprot_t *pgprot)
+{
+	cachi_set_pgprot_cache_options(buf->cache_settings, pgprot);
+}
+
+void cach_set_domain(struct cach_buf *buf, enum hwmem_access access,
+			enum hwmem_domain domain, struct hwmem_region *region)
+{
+	struct hwmem_region *__region;
+	struct hwmem_region full_region;
+
+	if (region != NULL) {
+		__region = region;
+	} else {
+		full_region.offset = 0;
+		full_region.count = 1;
+		full_region.start = 0;
+		full_region.end = buf->size;
+		full_region.size = buf->size;
+
+		__region = &full_region;
+	}
+
+	switch (domain) {
+	case HWMEM_DOMAIN_SYNC:
+		sync_buf_post_cpu(buf, access, __region);
+
+		break;
+
+	case HWMEM_DOMAIN_CPU:
+		sync_buf_pre_cpu(buf, access, __region);
+
+		break;
+	}
+}
+
+/*
+ * Local functions
+ */
+
+enum hwmem_alloc_flags __attribute__((weak)) cachi_get_cache_settings(
+			enum hwmem_alloc_flags requested_cache_settings)
+{
+	static const u32 CACHE_ON_FLAGS_MASK = HWMEM_ALLOC_HINT_CACHED |
+		HWMEM_ALLOC_HINT_CACHE_WB | HWMEM_ALLOC_HINT_CACHE_WT |
+		HWMEM_ALLOC_HINT_CACHE_NAOW | HWMEM_ALLOC_HINT_CACHE_AOW |
+				HWMEM_ALLOC_HINT_INNER_AND_OUTER_CACHE |
+					HWMEM_ALLOC_HINT_INNER_CACHE_ONLY;
+	/* We don't know the cache setting so we assume worst case. */
+	static const u32 CACHE_SETTING = HWMEM_ALLOC_HINT_WRITE_COMBINE |
+			HWMEM_ALLOC_HINT_CACHED | HWMEM_ALLOC_HINT_CACHE_WB |
+						HWMEM_ALLOC_HINT_CACHE_AOW |
+					HWMEM_ALLOC_HINT_INNER_AND_OUTER_CACHE;
+
+	if (requested_cache_settings & CACHE_ON_FLAGS_MASK)
+		return CACHE_SETTING;
+	else if (requested_cache_settings & HWMEM_ALLOC_HINT_WRITE_COMBINE ||
+		(requested_cache_settings & HWMEM_ALLOC_HINT_UNCACHED &&
+		 !(requested_cache_settings &
+					HWMEM_ALLOC_HINT_NO_WRITE_COMBINE)))
+		return HWMEM_ALLOC_HINT_WRITE_COMBINE;
+	else if (requested_cache_settings &
+					(HWMEM_ALLOC_HINT_NO_WRITE_COMBINE |
+						HWMEM_ALLOC_HINT_UNCACHED))
+		return 0;
+	else
+		/* Nothing specified, use cached */
+		return CACHE_SETTING;
+}
+
+void __attribute__((weak)) cachi_set_pgprot_cache_options(
+		enum hwmem_alloc_flags cache_settings, pgprot_t *pgprot)
+{
+	if (cache_settings & HWMEM_ALLOC_HINT_CACHED)
+		*pgprot = *pgprot; /* To silence compiler and checkpatch */
+	else if (cache_settings & HWMEM_ALLOC_HINT_WRITE_COMBINE)
+		*pgprot = pgprot_writecombine(*pgprot);
+	else
+		*pgprot = pgprot_noncached(*pgprot);
+}
+
+bool __attribute__((weak)) speculative_data_prefetch(void)
+{
+	/* We don't know so we go with the safe alternative */
+	return true;
+}
+
+static void sync_buf_pre_cpu(struct cach_buf *buf, enum hwmem_access access,
+						struct hwmem_region *region)
+{
+	bool write = access & HWMEM_ACCESS_WRITE;
+	bool read = access & HWMEM_ACCESS_READ;
+
+	if (!write && !read)
+		return;
+
+	if (buf->cache_settings & HWMEM_ALLOC_HINT_CACHED) {
+		struct cach_range region_range;
+
+		region_2_range(region, buf->size, &region_range);
+
+		if (read || (write && buf->cache_settings &
+						HWMEM_ALLOC_HINT_CACHE_WB))
+			/* Perform defered invalidates */
+			invalidate_cpu_cache(buf, &region_range);
+		if (read || (write && buf->cache_settings &
+						HWMEM_ALLOC_HINT_CACHE_AOW))
+			expand_range(&buf->range_in_cpu_cache, &region_range);
+		if (write && buf->cache_settings & HWMEM_ALLOC_HINT_CACHE_WB) {
+			struct cach_range dirty_range_addition;
+
+			if (buf->cache_settings & HWMEM_ALLOC_HINT_CACHE_AOW)
+				dirty_range_addition = region_range;
+			else
+				intersect_range(&buf->range_in_cpu_cache,
+					&region_range, &dirty_range_addition);
+
+			expand_range(&buf->range_dirty_in_cpu_cache,
+							&dirty_range_addition);
+		}
+	}
+	if (buf->cache_settings & HWMEM_ALLOC_HINT_WRITE_COMBINE) {
+		if (write)
+			buf->in_cpu_write_buf = true;
+	}
+}
+
+static void sync_buf_post_cpu(struct cach_buf *buf,
+	enum hwmem_access next_access, struct hwmem_region *next_region)
+{
+	bool write = next_access & HWMEM_ACCESS_WRITE;
+	bool read = next_access & HWMEM_ACCESS_READ;
+	struct cach_range region_range;
+
+	if (!write && !read)
+		return;
+
+	region_2_range(next_region, buf->size, &region_range);
+
+	if (write) {
+		if (speculative_data_prefetch()) {
+			/* Defer invalidate */
+			struct cach_range intersection;
+
+			intersect_range(&buf->range_in_cpu_cache,
+						&region_range, &intersection);
+
+			expand_range(&buf->range_invalid_in_cpu_cache,
+								&intersection);
+
+			clean_cpu_cache(buf, &region_range);
+		} else {
+			flush_cpu_cache(buf, &region_range);
+		}
+	}
+	if (read)
+		clean_cpu_cache(buf, &region_range);
+
+	if (buf->in_cpu_write_buf) {
+		drain_cpu_write_buf();
+
+		buf->in_cpu_write_buf = false;
+	}
+}
+
+static void invalidate_cpu_cache(struct cach_buf *buf, struct cach_range *range)
+{
+	struct cach_range intersection;
+
+	intersect_range(&buf->range_invalid_in_cpu_cache, range,
+								&intersection);
+	if (is_non_empty_range(&intersection)) {
+		bool flushed_everything;
+
+		expand_range_2_edge(&intersection,
+					&buf->range_invalid_in_cpu_cache);
+
+		/*
+		 * Cache handler never uses invalidate to discard data in the
+		 * cache so we can use flush instead which is considerably
+		 * faster for large buffers.
+		 */
+		flush_cpu_dcache(
+				offset_2_vaddr(buf, intersection.start),
+				offset_2_paddr(buf, intersection.start),
+				range_length(&intersection),
+				buf->cache_settings &
+					HWMEM_ALLOC_HINT_INNER_CACHE_ONLY,
+							&flushed_everything);
+
+		if (flushed_everything) {
+			null_range(&buf->range_invalid_in_cpu_cache);
+			null_range(&buf->range_dirty_in_cpu_cache);
+		} else {
+			/*
+			 * No need to shrink range_in_cpu_cache as invalidate
+			 * is only used when we can't keep track of what's in
+			 * the CPU cache.
+			 */
+			shrink_range(&buf->range_invalid_in_cpu_cache,
+								&intersection);
+		}
+	}
+}
+
+static void clean_cpu_cache(struct cach_buf *buf, struct cach_range *range)
+{
+	struct cach_range intersection;
+
+	intersect_range(&buf->range_dirty_in_cpu_cache, range, &intersection);
+	if (is_non_empty_range(&intersection)) {
+		bool cleaned_everything;
+
+		expand_range_2_edge(&intersection,
+					&buf->range_dirty_in_cpu_cache);
+
+		clean_cpu_dcache(
+				offset_2_vaddr(buf, intersection.start),
+				offset_2_paddr(buf, intersection.start),
+				range_length(&intersection),
+				buf->cache_settings &
+					HWMEM_ALLOC_HINT_INNER_CACHE_ONLY,
+							&cleaned_everything);
+
+		if (cleaned_everything)
+			null_range(&buf->range_dirty_in_cpu_cache);
+		else
+			shrink_range(&buf->range_dirty_in_cpu_cache,
+								&intersection);
+	}
+}
+
+static void flush_cpu_cache(struct cach_buf *buf, struct cach_range *range)
+{
+	struct cach_range intersection;
+
+	intersect_range(&buf->range_in_cpu_cache, range, &intersection);
+	if (is_non_empty_range(&intersection)) {
+		bool flushed_everything;
+
+		expand_range_2_edge(&intersection, &buf->range_in_cpu_cache);
+
+		flush_cpu_dcache(
+				offset_2_vaddr(buf, intersection.start),
+				offset_2_paddr(buf, intersection.start),
+				range_length(&intersection),
+				buf->cache_settings &
+					HWMEM_ALLOC_HINT_INNER_CACHE_ONLY,
+							&flushed_everything);
+
+		if (flushed_everything) {
+			if (!speculative_data_prefetch())
+				null_range(&buf->range_in_cpu_cache);
+			null_range(&buf->range_dirty_in_cpu_cache);
+			null_range(&buf->range_invalid_in_cpu_cache);
+		} else {
+			if (!speculative_data_prefetch())
+				shrink_range(&buf->range_in_cpu_cache,
+							 &intersection);
+			shrink_range(&buf->range_dirty_in_cpu_cache,
+								&intersection);
+			shrink_range(&buf->range_invalid_in_cpu_cache,
+								&intersection);
+		}
+	}
+}
+
+static void null_range(struct cach_range *range)
+{
+	range->start = U32_MAX;
+	range->end = 0;
+}
+
+static void expand_range(struct cach_range *range,
+						struct cach_range *range_2_add)
+{
+	range->start = min(range->start, range_2_add->start);
+	range->end = max(range->end, range_2_add->end);
+}
+
+/*
+ * Expands range to one of enclosing_range's two edges. The function will
+ * choose which of enclosing_range's edges to expand range to in such a
+ * way that the size of range is minimized. range must be located inside
+ * enclosing_range.
+ */
+static void expand_range_2_edge(struct cach_range *range,
+					struct cach_range *enclosing_range)
+{
+	u32 space_on_low_side = range->start - enclosing_range->start;
+	u32 space_on_high_side = enclosing_range->end - range->end;
+
+	if (space_on_low_side < space_on_high_side)
+		range->start = enclosing_range->start;
+	else
+		range->end = enclosing_range->end;
+}
+
+static void shrink_range(struct cach_range *range,
+					struct cach_range *range_2_remove)
+{
+	if (range_2_remove->start > range->start)
+		range->end = min(range->end, range_2_remove->start);
+	else
+		range->start = max(range->start, range_2_remove->end);
+
+	if (range->start >= range->end)
+		null_range(range);
+}
+
+static bool is_non_empty_range(struct cach_range *range)
+{
+	return range->end > range->start;
+}
+
+static void intersect_range(struct cach_range *range_1,
+		struct cach_range *range_2, struct cach_range *intersection)
+{
+	intersection->start = max(range_1->start, range_2->start);
+	intersection->end = min(range_1->end, range_2->end);
+
+	if (intersection->start >= intersection->end)
+		null_range(intersection);
+}
+
+/* Align_up restrictions apply here to */
+static void align_range_up(struct cach_range *range, u32 alignment)
+{
+	if (!is_non_empty_range(range))
+		return;
+
+	range->start = align_down(range->start, alignment);
+	range->end = align_up(range->end, alignment);
+}
+
+static u32 range_length(struct cach_range *range)
+{
+	if (is_non_empty_range(range))
+		return range->end - range->start;
+	else
+		return 0;
+}
+
+static void region_2_range(struct hwmem_region *region, u32 buffer_size,
+						struct cach_range *range)
+{
+	/*
+	 * We don't care about invalid regions, instead we limit the region's
+	 * range to the buffer's range. This should work good enough, worst
+	 * case we synch the entire buffer when we get an invalid region which
+	 * is acceptable.
+	 */
+	range->start = region->offset + region->start;
+	range->end = min(region->offset + (region->count * region->size) -
+				(region->size - region->end), buffer_size);
+	if (range->start >= range->end) {
+		null_range(range);
+		return;
+	}
+
+	align_range_up(range, get_dcache_granularity());
+}
+
+static void *offset_2_vaddr(struct cach_buf *buf, u32 offset)
+{
+	return (void *)((u32)buf->vstart + offset);
+}
+
+static u32 offset_2_paddr(struct cach_buf *buf, u32 offset)
+{
+	return buf->pstart + offset;
+}
+
+/* Saturates, might return unaligned values when that happens */
+static u32 align_up(u32 value, u32 alignment)
+{
+	u32 remainder = value % alignment;
+	u32 value_2_add;
+
+	if (remainder == 0)
+		return value;
+
+	value_2_add = alignment - remainder;
+
+	if (value_2_add > U32_MAX - value) /* Will overflow */
+		return U32_MAX;
+
+	return value + value_2_add;
+}
+
+static u32 align_down(u32 value, u32 alignment)
+{
+	u32 remainder = value % alignment;
+	if (remainder == 0)
+		return value;
+
+	return value - remainder;
+}
diff --git a/drivers/misc/hwmem/cache_handler.h b/drivers/misc/hwmem/cache_handler.h
new file mode 100644
index 00000000000..792105196fa
--- /dev/null
+++ b/drivers/misc/hwmem/cache_handler.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Cache handler
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/*
+ * Cache handler can not handle simultaneous execution! The caller has to
+ * ensure such a situation does not occur.
+ */
+
+#ifndef _CACHE_HANDLER_H_
+#define _CACHE_HANDLER_H_
+
+#include <linux/types.h>
+#include <linux/hwmem.h>
+
+/*
+ * To not have to double all datatypes we've used hwmem datatypes. If someone
+ * want's to use cache handler but not hwmem then we'll have to define our own
+ * datatypes.
+ */
+
+struct cach_range {
+	u32 start; /* Inclusive */
+	u32 end; /* Exclusive */
+};
+
+/*
+ * Internal, do not touch!
+ */
+struct cach_buf {
+	void *vstart;
+	u32 pstart;
+	u32 size;
+
+	/* Remaining hints are active */
+	enum hwmem_alloc_flags cache_settings;
+
+	bool in_cpu_write_buf;
+	struct cach_range range_in_cpu_cache;
+	struct cach_range range_dirty_in_cpu_cache;
+	struct cach_range range_invalid_in_cpu_cache;
+};
+
+void cach_init_buf(struct cach_buf *buf,
+			enum hwmem_alloc_flags cache_settings, u32 size);
+
+void cach_set_buf_addrs(struct cach_buf *buf, void* vaddr, u32 paddr);
+
+void cach_set_pgprot_cache_options(struct cach_buf *buf, pgprot_t *pgprot);
+
+void cach_set_domain(struct cach_buf *buf, enum hwmem_access access,
+			enum hwmem_domain domain, struct hwmem_region *region);
+
+#endif /* _CACHE_HANDLER_H_ */
diff --git a/drivers/misc/hwmem/contig_alloc.c b/drivers/misc/hwmem/contig_alloc.c
new file mode 100644
index 00000000000..31533ed5988
--- /dev/null
+++ b/drivers/misc/hwmem/contig_alloc.c
@@ -0,0 +1,571 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Contiguous memory allocator
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>,
+ * Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/module.h>
+#include <linux/vmalloc.h>
+#include <asm/sizes.h>
+
+#define MAX_INSTANCE_NAME_LENGTH 31
+
+struct alloc {
+	struct list_head list;
+
+	bool in_use;
+	phys_addr_t paddr;
+	size_t size;
+};
+
+struct instance {
+	struct list_head list;
+
+	char name[MAX_INSTANCE_NAME_LENGTH + 1];
+
+	phys_addr_t region_paddr;
+	void *region_kaddr;
+	size_t region_size;
+
+	struct list_head alloc_list;
+
+#ifdef CONFIG_DEBUG_FS
+	struct inode *debugfs_inode;
+	int cona_status_free;
+	int cona_status_used;
+	int cona_status_max_cont;
+	int cona_status_max_check;
+	int cona_status_biggest_free;
+	int cona_status_printed;
+#endif /* #ifdef CONFIG_DEBUG_FS */
+};
+
+static LIST_HEAD(instance_list);
+
+static DEFINE_MUTEX(lock);
+
+void *cona_create(const char *name, phys_addr_t region_paddr,
+							size_t region_size);
+void *cona_alloc(void *instance, size_t size);
+void cona_free(void *instance, void *alloc);
+phys_addr_t cona_get_alloc_paddr(void *alloc);
+void *cona_get_alloc_kaddr(void *instance, void *alloc);
+size_t cona_get_alloc_size(void *alloc);
+
+static int init_alloc_list(struct instance *instance);
+static void clean_alloc_list(struct instance *instance);
+static struct alloc *find_free_alloc_bestfit(struct instance *instance,
+								size_t size);
+static struct alloc *split_allocation(struct alloc *alloc,
+							size_t new_alloc_size);
+static phys_addr_t get_alloc_offset(struct instance *instance,
+							struct alloc *alloc);
+
+void *cona_create(const char *name, phys_addr_t region_paddr,
+							size_t region_size)
+{
+	int ret;
+	struct instance *instance;
+	struct vm_struct *vm_area;
+
+	if (region_size == 0)
+		return ERR_PTR(-EINVAL);
+
+	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
+	if (instance == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	memcpy(instance->name, name, MAX_INSTANCE_NAME_LENGTH + 1);
+	/* Truncate name if necessary */
+	instance->name[MAX_INSTANCE_NAME_LENGTH] = '\0';
+	instance->region_paddr = region_paddr;
+	instance->region_size = region_size;
+
+	vm_area = get_vm_area(region_size, VM_IOREMAP);
+	if (vm_area == NULL) {
+		printk(KERN_WARNING "CONA: Failed to allocate %u bytes"
+					" kernel virtual memory", region_size);
+		ret = -ENOMSG;
+		goto vmem_alloc_failed;
+	}
+	instance->region_kaddr = vm_area->addr;
+
+	INIT_LIST_HEAD(&instance->alloc_list);
+	ret = init_alloc_list(instance);
+	if (ret < 0)
+		goto init_alloc_list_failed;
+
+	mutex_lock(&lock);
+	list_add_tail(&instance->list, &instance_list);
+	mutex_unlock(&lock);
+
+	return instance;
+
+init_alloc_list_failed:
+	vm_area = remove_vm_area(instance->region_kaddr);
+	if (vm_area == NULL)
+		printk(KERN_ERR "CONA: Failed to free kernel virtual memory,"
+							" resource leak!\n");
+
+	kfree(vm_area);
+vmem_alloc_failed:
+	kfree(instance);
+
+	return ERR_PTR(ret);
+}
+
+void *cona_alloc(void *instance, size_t size)
+{
+	struct instance *instance_l = (struct instance *)instance;
+	struct alloc *alloc;
+
+	if (size == 0)
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(&lock);
+
+	alloc = find_free_alloc_bestfit(instance_l, size);
+	if (IS_ERR(alloc))
+		goto out;
+	if (size < alloc->size) {
+		alloc = split_allocation(alloc, size);
+		if (IS_ERR(alloc))
+			goto out;
+	} else {
+		alloc->in_use = true;
+	}
+#ifdef CONFIG_DEBUG_FS
+	instance_l->cona_status_max_cont += alloc->size;
+	instance_l->cona_status_max_check =
+					max(instance_l->cona_status_max_check,
+					instance_l->cona_status_max_cont);
+#endif /* #ifdef CONFIG_DEBUG_FS */
+
+out:
+	mutex_unlock(&lock);
+
+	return alloc;
+}
+
+void cona_free(void *instance, void *alloc)
+{
+	struct instance *instance_l = (struct instance *)instance;
+	struct alloc *alloc_l = (struct alloc *)alloc;
+	struct alloc *other;
+
+	mutex_lock(&lock);
+
+	alloc_l->in_use = false;
+
+#ifdef CONFIG_DEBUG_FS
+	instance_l->cona_status_max_cont -= alloc_l->size;
+#endif /* #ifdef CONFIG_DEBUG_FS */
+
+	other = list_entry(alloc_l->list.prev, struct alloc, list);
+	if ((alloc_l->list.prev != &instance_l->alloc_list) &&
+							!other->in_use) {
+		other->size += alloc_l->size;
+		list_del(&alloc_l->list);
+		kfree(alloc_l);
+		alloc_l = other;
+	}
+	other = list_entry(alloc_l->list.next, struct alloc, list);
+	if ((alloc_l->list.next != &instance_l->alloc_list) &&
+							!other->in_use) {
+		alloc_l->size += other->size;
+		list_del(&other->list);
+		kfree(other);
+	}
+
+	mutex_unlock(&lock);
+}
+
+phys_addr_t cona_get_alloc_paddr(void *alloc)
+{
+	return ((struct alloc *)alloc)->paddr;
+}
+
+void *cona_get_alloc_kaddr(void *instance, void *alloc)
+{
+	struct instance *instance_l = (struct instance *)instance;
+
+	return instance_l->region_kaddr + get_alloc_offset(instance_l,
+							(struct alloc *)alloc);
+}
+
+size_t cona_get_alloc_size(void *alloc)
+{
+	return ((struct alloc *)alloc)->size;
+}
+
+static int init_alloc_list(struct instance *instance)
+{
+	/*
+	 * Hack to not get any allocs that cross a 64MiB boundary as B2R2 can't
+	 * handle that.
+	 */
+	int ret;
+	u32 curr_pos = instance->region_paddr;
+	u32 region_end = instance->region_paddr + instance->region_size;
+	u32 next_64mib_boundary = (curr_pos + SZ_64M) & ~(SZ_64M - 1);
+	struct alloc *alloc;
+
+	if (PAGE_SIZE >= SZ_64M) {
+		printk(KERN_WARNING "CONA: PAGE_SIZE >= 64MiB\n");
+		return -ENOMSG;
+	}
+
+	while (next_64mib_boundary < region_end) {
+		if (next_64mib_boundary - curr_pos > PAGE_SIZE) {
+			alloc = kzalloc(sizeof(struct alloc), GFP_KERNEL);
+			if (alloc == NULL) {
+				ret = -ENOMEM;
+				goto error;
+			}
+			alloc->paddr = curr_pos;
+			alloc->size = next_64mib_boundary - curr_pos -
+								PAGE_SIZE;
+			alloc->in_use = false;
+			list_add_tail(&alloc->list, &instance->alloc_list);
+			curr_pos = alloc->paddr + alloc->size;
+		}
+
+		alloc = kzalloc(sizeof(struct alloc), GFP_KERNEL);
+		if (alloc == NULL) {
+			ret = -ENOMEM;
+			goto error;
+		}
+		alloc->paddr = curr_pos;
+		alloc->size = PAGE_SIZE;
+		alloc->in_use = true;
+		list_add_tail(&alloc->list, &instance->alloc_list);
+		curr_pos = alloc->paddr + alloc->size;
+
+#ifdef CONFIG_DEBUG_FS
+		instance->cona_status_max_cont += alloc->size;
+#endif /* #ifdef CONFIG_DEBUG_FS */
+
+		next_64mib_boundary += SZ_64M;
+	}
+
+	alloc = kzalloc(sizeof(struct alloc), GFP_KERNEL);
+	if (alloc == NULL) {
+		ret = -ENOMEM;
+		goto error;
+	}
+	alloc->paddr = curr_pos;
+	alloc->size = region_end - curr_pos;
+	alloc->in_use = false;
+	list_add_tail(&alloc->list, &instance->alloc_list);
+
+	return 0;
+
+error:
+	clean_alloc_list(instance);
+
+	return ret;
+}
+
+static void clean_alloc_list(struct instance *instance)
+{
+	while (list_empty(&instance->alloc_list) == 0) {
+		struct alloc *i = list_first_entry(&instance->alloc_list,
+							struct alloc, list);
+
+		list_del(&i->list);
+
+		kfree(i);
+	}
+}
+
+static struct alloc *find_free_alloc_bestfit(struct instance *instance,
+								size_t size)
+{
+	size_t best_diff = ~(size_t)0;
+	struct alloc *alloc = NULL, *i;
+
+	list_for_each_entry(i, &instance->alloc_list, list) {
+		size_t diff = i->size - size;
+		if (i->in_use || i->size < size)
+			continue;
+		if (diff < best_diff) {
+			alloc = i;
+			best_diff = diff;
+		}
+	}
+
+	return alloc != NULL ? alloc : ERR_PTR(-ENOMEM);
+}
+
+static struct alloc *split_allocation(struct alloc *alloc,
+							size_t new_alloc_size)
+{
+	struct alloc *new_alloc;
+
+	new_alloc = kzalloc(sizeof(struct alloc), GFP_KERNEL);
+	if (new_alloc == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	new_alloc->in_use = true;
+	new_alloc->paddr = alloc->paddr;
+	new_alloc->size = new_alloc_size;
+	alloc->size -= new_alloc_size;
+	alloc->paddr += new_alloc_size;
+
+	list_add_tail(&new_alloc->list, &alloc->list);
+
+	return new_alloc;
+}
+
+static phys_addr_t get_alloc_offset(struct instance *instance,
+							struct alloc *alloc)
+{
+	return alloc->paddr - instance->region_paddr;
+}
+
+/* Debug */
+
+#ifdef CONFIG_DEBUG_FS
+
+static int print_alloc(struct instance *instance, struct alloc *alloc,
+						char **buf, size_t buf_size);
+static int print_alloc_status(struct instance *instance, char **buf,
+						size_t buf_size);
+static struct instance *get_instance_from_file(struct file *file);
+static int debugfs_allocs_read(struct file *filp, char __user *buf,
+						size_t count, loff_t *f_pos);
+
+static const struct file_operations debugfs_allocs_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_allocs_read,
+};
+
+static int print_alloc(struct instance *instance, struct alloc *alloc,
+						char **buf, size_t buf_size)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		size_t buf_size_l;
+		if (i == 0)
+			buf_size_l = 0;
+		else
+			buf_size_l = buf_size;
+
+		if (i == 1) {
+			if (alloc->in_use)
+				instance->cona_status_used += alloc->size;
+			else
+				instance->cona_status_free += alloc->size;
+		}
+
+		if (!alloc->in_use) {
+			instance->cona_status_biggest_free =
+				max((size_t)alloc->size,
+				(size_t)instance->cona_status_biggest_free);
+		}
+
+		ret = snprintf(*buf, buf_size_l, "paddr: %10x\tsize: %10u\t"
+				"in use: %1u\t used: %10u (%dMB)"
+				" \t free: %10u (%dMB)\n",
+				alloc->paddr,
+				alloc->size,
+				alloc->in_use,
+				instance->cona_status_used,
+				instance->cona_status_used/1024/1024,
+				instance->cona_status_free,
+				instance->cona_status_free/1024/1024);
+
+		if (ret < 0)
+			return -ENOMSG;
+		else if (ret + 1 > buf_size)
+			return -EINVAL;
+	}
+
+	*buf += ret;
+
+	return 0;
+}
+
+static int print_alloc_status(struct instance *instance, char **buf,
+							size_t buf_size)
+{
+	int ret;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		size_t buf_size_l;
+		if (i == 0)
+			buf_size_l = 0;
+		else
+			buf_size_l = buf_size;
+
+		ret = snprintf(*buf, buf_size_l, "Overall peak usage:\t%10u "
+				"(%dMB)\nCurrent max usage:\t%10u (%dMB)\n"
+				"Current biggest free:\t%10d (%dMB)\n",
+				instance->cona_status_max_check,
+				instance->cona_status_max_check/1024/1024,
+				instance->cona_status_max_cont,
+				instance->cona_status_max_cont/1024/1024,
+				instance->cona_status_biggest_free,
+				instance->cona_status_biggest_free/1024/1024);
+
+		if (ret < 0)
+			return -ENOMSG;
+		else if (ret + 1 > buf_size)
+			return -EINVAL;
+	}
+
+	*buf += ret;
+
+	return 0;
+}
+
+static struct instance *get_instance_from_file(struct file *file)
+{
+	struct instance *curr_instance;
+
+	list_for_each_entry(curr_instance, &instance_list, list) {
+		if (file->f_dentry->d_inode == curr_instance->debugfs_inode)
+			return curr_instance;
+	}
+
+	return ERR_PTR(-ENOENT);
+}
+
+static int debugfs_allocs_read(struct file *file, char __user *buf,
+						size_t count, loff_t *f_pos)
+{
+	/*
+	 * We assume the supplied buffer and PAGE_SIZE is large enough to hold
+	 * information about at least one alloc, if not no data will be
+	 * returned.
+	 */
+
+	int ret;
+	struct instance *instance;
+	struct alloc *curr_alloc;
+	char *local_buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	char *local_buf_pos = local_buf;
+	size_t available_space = min((size_t)PAGE_SIZE, count);
+	/* private_data is intialized to NULL in open which I assume is 0. */
+	void **curr_pos = &file->private_data;
+	size_t bytes_read;
+	bool readout_aborted = false;
+
+	if (local_buf == NULL)
+		return -ENOMEM;
+
+	mutex_lock(&lock);
+	instance = get_instance_from_file(file);
+	if (IS_ERR(instance)) {
+		ret = PTR_ERR(instance);
+		goto out;
+	}
+
+	list_for_each_entry(curr_alloc, &instance->alloc_list, list) {
+		phys_addr_t alloc_offset = get_alloc_offset(instance,
+								curr_alloc);
+		if (alloc_offset < (phys_addr_t)*curr_pos)
+			continue;
+
+		ret = print_alloc(instance, curr_alloc, &local_buf_pos,
+				available_space - (size_t)(local_buf_pos -
+				local_buf));
+
+		if (ret == -EINVAL) { /* No more room */
+			readout_aborted = true;
+			break;
+		} else if (ret < 0) {
+			goto out;
+		}
+		/*
+		 * There could be an overflow issue here in the unlikely case
+		 * where the region is placed at the end of the address range
+		 * and the last alloc is 1 byte large. Since this is debug code
+		 * and that case most likely never will happen I've chosen to
+		 * defer fixing it till it happens.
+		 */
+		*curr_pos = (void *)(alloc_offset + 1);
+
+		/* Make sure to also print status if there were any prints */
+		instance->cona_status_printed = false;
+	}
+
+	if (!readout_aborted && !instance->cona_status_printed) {
+		ret = print_alloc_status(instance, &local_buf_pos,
+					available_space -
+					(size_t)(local_buf_pos - local_buf));
+
+		if (ret == -EINVAL) /* No more room */
+			readout_aborted = true;
+		else if (ret < 0)
+			goto out;
+		else
+			instance->cona_status_printed = true;
+	}
+
+	if (!readout_aborted) {
+		instance->cona_status_free = 0;
+		instance->cona_status_used = 0;
+		instance->cona_status_biggest_free = 0;
+	}
+
+	bytes_read = (size_t)(local_buf_pos - local_buf);
+
+	ret = copy_to_user(buf, local_buf, bytes_read);
+	if (ret < 0)
+		goto out;
+
+	ret = bytes_read;
+
+out:
+	kfree(local_buf);
+	mutex_unlock(&lock);
+
+	return ret;
+}
+
+static int __init init_debugfs(void)
+{
+	struct instance *curr_instance;
+	struct dentry *debugfs_root_dir = debugfs_create_dir("cona", NULL);
+
+	mutex_lock(&lock);
+
+	list_for_each_entry(curr_instance, &instance_list, list) {
+		struct dentry *file_dentry;
+		char tmp_str[MAX_INSTANCE_NAME_LENGTH + 7 + 1];
+		tmp_str[0] = '\0';
+		strcat(tmp_str, curr_instance->name);
+		strcat(tmp_str, "_allocs");
+		file_dentry = debugfs_create_file(tmp_str, 0444,
+				debugfs_root_dir, 0, &debugfs_allocs_fops);
+		if (file_dentry != NULL)
+			curr_instance->debugfs_inode = file_dentry->d_inode;
+	}
+
+	mutex_unlock(&lock);
+
+	return 0;
+}
+/*
+ * Must be executed after all instances have been created, hence the
+ * late_initcall.
+ */
+late_initcall(init_debugfs);
+
+#endif /* #ifdef CONFIG_DEBUG_FS */
diff --git a/drivers/misc/hwmem/hwmem-ioctl.c b/drivers/misc/hwmem/hwmem-ioctl.c
new file mode 100644
index 00000000000..e9e50de78bd
--- /dev/null
+++ b/drivers/misc/hwmem/hwmem-ioctl.c
@@ -0,0 +1,532 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Hardware memory driver, hwmem
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/idr.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/mm_types.h>
+#include <linux/hwmem.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+
+static int hwmem_open(struct inode *inode, struct file *file);
+static int hwmem_ioctl_mmap(struct file *file, struct vm_area_struct *vma);
+static int hwmem_release_fop(struct inode *inode, struct file *file);
+static long hwmem_ioctl(struct file *file, unsigned int cmd,
+	unsigned long arg);
+static unsigned long hwmem_get_unmapped_area(struct file *file,
+	unsigned long addr, unsigned long len, unsigned long pgoff,
+	unsigned long flags);
+
+static const struct file_operations hwmem_fops = {
+	.open = hwmem_open,
+	.mmap = hwmem_ioctl_mmap,
+	.unlocked_ioctl = hwmem_ioctl,
+	.release = hwmem_release_fop,
+	.get_unmapped_area = hwmem_get_unmapped_area,
+};
+
+static struct miscdevice hwmem_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "hwmem",
+	.fops = &hwmem_fops,
+};
+
+struct hwmem_file {
+	struct mutex lock;
+	struct idr idr; /* id -> struct hwmem_alloc*, ref counted */
+	struct hwmem_alloc *fd_alloc; /* Ref counted */
+};
+
+static s32 create_id(struct hwmem_file *hwfile, struct hwmem_alloc *alloc)
+{
+	int id, ret;
+
+	while (true) {
+		if (idr_pre_get(&hwfile->idr, GFP_KERNEL) == 0)
+			return -ENOMEM;
+
+		ret = idr_get_new_above(&hwfile->idr, alloc, 1, &id);
+		if (ret == 0)
+			break;
+		else if (ret != -EAGAIN)
+			return -ENOMEM;
+	}
+
+	/*
+	 * IDR always returns the lowest free id so there is no wrapping issue
+	 * because of this.
+	 */
+	if (id >= (s32)1 << (31 - PAGE_SHIFT)) {
+		dev_err(hwmem_device.this_device, "Out of IDs!\n");
+		idr_remove(&hwfile->idr, id);
+		return -ENOMSG;
+	}
+
+	return (s32)id << PAGE_SHIFT;
+}
+
+static void remove_id(struct hwmem_file *hwfile, s32 id)
+{
+	idr_remove(&hwfile->idr, id >> PAGE_SHIFT);
+}
+
+static struct hwmem_alloc *resolve_id(struct hwmem_file *hwfile, s32 id)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = id ? idr_find(&hwfile->idr, id >> PAGE_SHIFT) :
+							hwfile->fd_alloc;
+	if (alloc == NULL)
+		alloc = ERR_PTR(-EINVAL);
+
+	return alloc;
+}
+
+static s32 alloc(struct hwmem_file *hwfile, struct hwmem_alloc_request *req)
+{
+	s32 ret = 0;
+	struct hwmem_alloc *alloc;
+
+	alloc = hwmem_alloc(req->size, req->flags, req->default_access,
+								req->mem_type);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	ret = create_id(hwfile, alloc);
+	if (ret < 0)
+		hwmem_release(alloc);
+
+	return ret;
+}
+
+static int alloc_fd(struct hwmem_file *hwfile, struct hwmem_alloc_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	if (hwfile->fd_alloc)
+		return -EINVAL;
+
+	alloc = hwmem_alloc(req->size, req->flags, req->default_access,
+								req->mem_type);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	hwfile->fd_alloc = alloc;
+
+	return 0;
+}
+
+static int release(struct hwmem_file *hwfile, s32 id)
+{
+	struct hwmem_alloc *alloc;
+
+	if (id == 0)
+		return -EINVAL;
+
+	alloc = resolve_id(hwfile, id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	remove_id(hwfile, id);
+	hwmem_release(alloc);
+
+	return 0;
+}
+
+static int set_cpu_domain(struct hwmem_file *hwfile,
+					struct hwmem_set_domain_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	return hwmem_set_domain(alloc, req->access, HWMEM_DOMAIN_CPU,
+					(struct hwmem_region *)&req->region);
+}
+
+static int set_sync_domain(struct hwmem_file *hwfile,
+					struct hwmem_set_domain_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	return hwmem_set_domain(alloc, req->access, HWMEM_DOMAIN_SYNC,
+					(struct hwmem_region *)&req->region);
+}
+
+static int pin(struct hwmem_file *hwfile, struct hwmem_pin_request *req)
+{
+	int ret;
+	struct hwmem_alloc *alloc;
+	enum hwmem_mem_type mem_type;
+	struct hwmem_mem_chunk mem_chunk;
+	size_t mem_chunk_length = 1;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	hwmem_get_info(alloc, NULL, &mem_type, NULL);
+	if (mem_type != HWMEM_MEM_CONTIGUOUS_SYS)
+		return -EINVAL;
+
+	ret = hwmem_pin(alloc, &mem_chunk, &mem_chunk_length);
+	if (ret < 0)
+		return ret;
+
+	req->phys_addr = mem_chunk.paddr;
+
+	return 0;
+}
+
+static int unpin(struct hwmem_file *hwfile, s32 id)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	hwmem_unpin(alloc);
+
+	return 0;
+}
+
+static int set_access(struct hwmem_file *hwfile,
+		struct hwmem_set_access_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	return hwmem_set_access(alloc, req->access, req->pid);
+}
+
+static int get_info(struct hwmem_file *hwfile,
+		struct hwmem_get_info_request *req)
+{
+	struct hwmem_alloc *alloc;
+
+	alloc = resolve_id(hwfile, req->id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	hwmem_get_info(alloc, &req->size, &req->mem_type, &req->access);
+
+	return 0;
+}
+
+static s32 export(struct hwmem_file *hwfile, s32 id)
+{
+	s32 ret;
+	struct hwmem_alloc *alloc;
+	enum hwmem_access access;
+
+	alloc = resolve_id(hwfile, id);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	/*
+	 * The user could be about to send the buffer to a driver but
+	 * there is a chance the current thread group don't have import rights
+	 * if it gained access to the buffer via a inter-process fd transfer
+	 * (fork, Android binder), if this is the case the driver will not be
+	 * able to resolve the buffer name. To avoid this situation we give the
+	 * current thread group import rights. This will not breach the
+	 * security as the process already has access to the buffer (otherwise
+	 * it would not be able to get here).
+	 */
+	hwmem_get_info(alloc, NULL, NULL, &access);
+
+	ret = hwmem_set_access(alloc, (access | HWMEM_ACCESS_IMPORT),
+							task_tgid_nr(current));
+	if (ret < 0)
+		return ret;
+
+	return hwmem_get_name(alloc);
+}
+
+static s32 import(struct hwmem_file *hwfile, s32 name)
+{
+	s32 ret = 0;
+	struct hwmem_alloc *alloc;
+	enum hwmem_access access;
+
+	alloc = hwmem_resolve_by_name(name);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	/* Check access permissions for process */
+	hwmem_get_info(alloc, NULL, NULL, &access);
+	if (!(access & HWMEM_ACCESS_IMPORT)) {
+		ret = -EPERM;
+		goto error;
+	}
+
+	ret = create_id(hwfile, alloc);
+	if (ret < 0)
+		goto error;
+
+	return ret;
+
+error:
+	hwmem_release(alloc);
+
+	return ret;
+}
+
+static int import_fd(struct hwmem_file *hwfile, s32 name)
+{
+	int ret;
+	struct hwmem_alloc *alloc;
+	enum hwmem_access access;
+
+	if (hwfile->fd_alloc)
+		return -EINVAL;
+
+	alloc = hwmem_resolve_by_name(name);
+	if (IS_ERR(alloc))
+		return PTR_ERR(alloc);
+
+	/* Check access permissions for process */
+	hwmem_get_info(alloc, NULL, NULL, &access);
+	if (!(access & HWMEM_ACCESS_IMPORT)) {
+		ret = -EPERM;
+		goto error;
+	}
+
+	hwfile->fd_alloc = alloc;
+
+	return 0;
+
+error:
+	hwmem_release(alloc);
+
+	return ret;
+}
+
+static int hwmem_open(struct inode *inode, struct file *file)
+{
+	struct hwmem_file *hwfile;
+
+	hwfile = kzalloc(sizeof(struct hwmem_file), GFP_KERNEL);
+	if (hwfile == NULL)
+		return -ENOMEM;
+
+	idr_init(&hwfile->idr);
+	mutex_init(&hwfile->lock);
+	file->private_data = hwfile;
+
+	return 0;
+}
+
+static int hwmem_ioctl_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	int ret;
+	struct hwmem_file *hwfile = (struct hwmem_file *)file->private_data;
+	struct hwmem_alloc *alloc;
+
+	mutex_lock(&hwfile->lock);
+
+	alloc = resolve_id(hwfile, (s32)vma->vm_pgoff << PAGE_SHIFT);
+	if (IS_ERR(alloc)) {
+		ret = PTR_ERR(alloc);
+		goto out;
+	}
+
+	ret = hwmem_mmap(alloc, vma);
+
+out:
+	mutex_unlock(&hwfile->lock);
+
+	return ret;
+}
+
+static int hwmem_release_idr_for_each_wrapper(int id, void *ptr, void *data)
+{
+	hwmem_release((struct hwmem_alloc *)ptr);
+
+	return 0;
+}
+
+static int hwmem_release_fop(struct inode *inode, struct file *file)
+{
+	struct hwmem_file *hwfile = (struct hwmem_file *)file->private_data;
+
+	idr_for_each(&hwfile->idr, hwmem_release_idr_for_each_wrapper, NULL);
+	idr_remove_all(&hwfile->idr);
+	idr_destroy(&hwfile->idr);
+
+	if (hwfile->fd_alloc)
+		hwmem_release(hwfile->fd_alloc);
+
+	mutex_destroy(&hwfile->lock);
+
+	kfree(hwfile);
+
+	return 0;
+}
+
+static long hwmem_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	int ret = -ENOSYS;
+	struct hwmem_file *hwfile = (struct hwmem_file *)file->private_data;
+
+	mutex_lock(&hwfile->lock);
+
+	switch (cmd) {
+	case HWMEM_ALLOC_IOC:
+		{
+			struct hwmem_alloc_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+					sizeof(struct hwmem_alloc_request)))
+				ret = -EFAULT;
+			else
+				ret = alloc(hwfile, &req);
+		}
+		break;
+	case HWMEM_ALLOC_FD_IOC:
+		{
+			struct hwmem_alloc_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+					sizeof(struct hwmem_alloc_request)))
+				ret = -EFAULT;
+			else
+				ret = alloc_fd(hwfile, &req);
+		}
+		break;
+	case HWMEM_RELEASE_IOC:
+		ret = release(hwfile, (s32)arg);
+		break;
+	case HWMEM_SET_CPU_DOMAIN_IOC:
+		{
+			struct hwmem_set_domain_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_set_domain_request)))
+				ret = -EFAULT;
+			else
+				ret = set_cpu_domain(hwfile, &req);
+		}
+		break;
+	case HWMEM_SET_SYNC_DOMAIN_IOC:
+		{
+			struct hwmem_set_domain_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_set_domain_request)))
+				ret = -EFAULT;
+			else
+				ret = set_sync_domain(hwfile, &req);
+		}
+		break;
+	case HWMEM_PIN_IOC:
+		{
+			struct hwmem_pin_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_pin_request)))
+				ret = -EFAULT;
+			else
+				ret = pin(hwfile, &req);
+			if (ret == 0 && copy_to_user((void __user *)arg, &req,
+					sizeof(struct hwmem_pin_request)))
+				ret = -EFAULT;
+		}
+		break;
+	case HWMEM_UNPIN_IOC:
+		ret = unpin(hwfile, (s32)arg);
+		break;
+	case HWMEM_SET_ACCESS_IOC:
+		{
+			struct hwmem_set_access_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_set_access_request)))
+				ret = -EFAULT;
+			else
+				ret = set_access(hwfile, &req);
+		}
+		break;
+	case HWMEM_GET_INFO_IOC:
+		{
+			struct hwmem_get_info_request req;
+			if (copy_from_user(&req, (void __user *)arg,
+				sizeof(struct hwmem_get_info_request)))
+				ret = -EFAULT;
+			else
+				ret = get_info(hwfile, &req);
+			if (ret == 0 && copy_to_user((void __user *)arg, &req,
+					sizeof(struct hwmem_get_info_request)))
+				ret = -EFAULT;
+		}
+		break;
+	case HWMEM_EXPORT_IOC:
+		ret = export(hwfile, (s32)arg);
+		break;
+	case HWMEM_IMPORT_IOC:
+		ret = import(hwfile, (s32)arg);
+		break;
+	case HWMEM_IMPORT_FD_IOC:
+		ret = import_fd(hwfile, (s32)arg);
+		break;
+	}
+
+	mutex_unlock(&hwfile->lock);
+
+	return ret;
+}
+
+static unsigned long hwmem_get_unmapped_area(struct file *file,
+	unsigned long addr, unsigned long len, unsigned long pgoff,
+	unsigned long flags)
+{
+	/*
+	 * pgoff will not be valid as it contains a buffer id (right shifted
+	 * PAGE_SHIFT bits). To not confuse get_unmapped_area we'll not pass
+	 * on file or pgoff.
+	 */
+	return current->mm->get_unmapped_area(NULL, addr, len, 0, flags);
+}
+
+int __init hwmem_ioctl_init(void)
+{
+	if (PAGE_SHIFT < 1 || PAGE_SHIFT > 30 || sizeof(size_t) != 4 ||
+		sizeof(int) > 4 || sizeof(enum hwmem_alloc_flags) != 4 ||
+					sizeof(enum hwmem_access) != 4 ||
+					 sizeof(enum hwmem_mem_type) != 4) {
+		dev_err(hwmem_device.this_device, "PAGE_SHIFT < 1 || PAGE_SHIFT"
+			" > 30 || sizeof(size_t) != 4 || sizeof(int) > 4 ||"
+			" sizeof(enum hwmem_alloc_flags) != 4 || sizeof(enum"
+			" hwmem_access) != 4 || sizeof(enum hwmem_mem_type)"
+								" != 4\n");
+		return -ENOMSG;
+	}
+	if (PAGE_SHIFT > 15)
+		dev_warn(hwmem_device.this_device, "Due to the page size only"
+				" %u id:s per file instance are available\n",
+					((u32)1 << (31 - PAGE_SHIFT)) - 1);
+
+	return misc_register(&hwmem_device);
+}
+
+void __exit hwmem_ioctl_exit(void)
+{
+	misc_deregister(&hwmem_device);
+}
diff --git a/drivers/misc/hwmem/hwmem-main.c b/drivers/misc/hwmem/hwmem-main.c
new file mode 100644
index 00000000000..b91d99bc2be
--- /dev/null
+++ b/drivers/misc/hwmem/hwmem-main.c
@@ -0,0 +1,726 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Hardware memory driver, hwmem
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>,
+ * Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/idr.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/pid.h>
+#include <linux/list.h>
+#include <linux/hwmem.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/kallsyms.h>
+#include <linux/vmalloc.h>
+#include "cache_handler.h"
+
+#define S32_MAX 2147483647
+
+struct hwmem_alloc_threadg_info {
+	struct list_head list;
+
+	struct pid *threadg_pid; /* Ref counted */
+
+	enum hwmem_access access;
+};
+
+struct hwmem_alloc {
+	struct list_head list;
+
+	atomic_t ref_cnt;
+
+	enum hwmem_alloc_flags flags;
+	struct hwmem_mem_type_struct *mem_type;
+
+	void *allocator_hndl;
+	phys_addr_t paddr;
+	void *kaddr;
+	size_t size;
+	s32 name;
+
+	/* Access control */
+	enum hwmem_access default_access;
+	struct list_head threadg_info_list;
+
+	/* Cache handling */
+	struct cach_buf cach_buf;
+
+#ifdef CONFIG_DEBUG_FS
+	/* Debug */
+	void *creator;
+	pid_t creator_tgid;
+#endif /* #ifdef CONFIG_DEBUG_FS */
+};
+
+static struct platform_device *hwdev;
+
+static LIST_HEAD(alloc_list);
+static DEFINE_IDR(global_idr);
+static DEFINE_MUTEX(lock);
+
+static void vm_open(struct vm_area_struct *vma);
+static void vm_close(struct vm_area_struct *vma);
+static struct vm_operations_struct vm_ops = {
+	.open = vm_open,
+	.close = vm_close,
+};
+
+static void kunmap_alloc(struct hwmem_alloc *alloc);
+
+/* Helpers */
+
+static void destroy_alloc_threadg_info(
+		struct hwmem_alloc_threadg_info *info)
+{
+	if (info->threadg_pid)
+		put_pid(info->threadg_pid);
+
+	kfree(info);
+}
+
+static void clean_alloc_threadg_info_list(struct hwmem_alloc *alloc)
+{
+	struct hwmem_alloc_threadg_info *info;
+	struct hwmem_alloc_threadg_info *tmp;
+
+	list_for_each_entry_safe(info, tmp, &(alloc->threadg_info_list),
+									list) {
+		list_del(&info->list);
+		destroy_alloc_threadg_info(info);
+	}
+}
+
+static enum hwmem_access get_access(struct hwmem_alloc *alloc)
+{
+	struct hwmem_alloc_threadg_info *info;
+	struct pid *my_pid;
+	bool found = false;
+
+	my_pid = find_get_pid(task_tgid_nr(current));
+	if (!my_pid)
+		return 0;
+
+	list_for_each_entry(info, &(alloc->threadg_info_list), list) {
+		if (info->threadg_pid == my_pid) {
+			found = true;
+			break;
+		}
+	}
+
+	put_pid(my_pid);
+
+	if (found)
+		return info->access;
+	else
+		return alloc->default_access;
+}
+
+static void clear_alloc_mem(struct hwmem_alloc *alloc)
+{
+	cach_set_domain(&alloc->cach_buf, HWMEM_ACCESS_WRITE,
+						HWMEM_DOMAIN_CPU, NULL);
+
+	memset(alloc->kaddr, 0, alloc->size);
+}
+
+static void destroy_alloc(struct hwmem_alloc *alloc)
+{
+	list_del(&alloc->list);
+
+	if (alloc->name != 0) {
+		idr_remove(&global_idr, alloc->name);
+		alloc->name = 0;
+	}
+
+	clean_alloc_threadg_info_list(alloc);
+
+	kunmap_alloc(alloc);
+
+	if (!IS_ERR_OR_NULL(alloc->allocator_hndl))
+		alloc->mem_type->allocator_api.free(
+					alloc->mem_type->allocator_instance,
+							alloc->allocator_hndl);
+
+	kfree(alloc);
+}
+
+static int kmap_alloc(struct hwmem_alloc *alloc)
+{
+	int ret;
+	pgprot_t pgprot;
+	void *alloc_kaddr;
+
+	alloc_kaddr = alloc->mem_type->allocator_api.get_alloc_kaddr(
+		alloc->mem_type->allocator_instance, alloc->allocator_hndl);
+	if (IS_ERR(alloc_kaddr))
+		return PTR_ERR(alloc_kaddr);
+
+	pgprot = PAGE_KERNEL;
+	cach_set_pgprot_cache_options(&alloc->cach_buf, &pgprot);
+
+	ret = ioremap_page_range((unsigned long)alloc_kaddr,
+		(unsigned long)alloc_kaddr + alloc->size, alloc->paddr, pgprot);
+	if (ret < 0) {
+		dev_warn(&hwdev->dev, "Failed to map %#x - %#x", alloc->paddr,
+						alloc->paddr + alloc->size);
+		return ret;
+	}
+
+	alloc->kaddr = alloc_kaddr;
+
+	return 0;
+}
+
+static void kunmap_alloc(struct hwmem_alloc *alloc)
+{
+	if (alloc->kaddr == NULL)
+		return;
+
+	unmap_kernel_range((unsigned long)alloc->kaddr, alloc->size);
+
+	alloc->kaddr = NULL;
+}
+
+static struct hwmem_mem_type_struct *resolve_mem_type(
+						enum hwmem_mem_type mem_type)
+{
+	unsigned int i;
+	for (i = 0; i < hwmem_num_mem_types; i++) {
+		if (hwmem_mem_types[i].id == mem_type)
+			return &hwmem_mem_types[i];
+	}
+
+	return ERR_PTR(-ENOENT);
+}
+
+/* HWMEM API */
+
+struct hwmem_alloc *hwmem_alloc(size_t size, enum hwmem_alloc_flags flags,
+		enum hwmem_access def_access, enum hwmem_mem_type mem_type)
+{
+	int ret;
+	struct hwmem_alloc *alloc;
+
+	if (hwdev == NULL) {
+		printk(KERN_ERR "HWMEM: Badly configured\n");
+		return ERR_PTR(-ENOMSG);
+	}
+
+	if (size == 0)
+		return ERR_PTR(-EINVAL);
+
+	mutex_lock(&lock);
+
+	size = PAGE_ALIGN(size);
+
+	alloc = kzalloc(sizeof(struct hwmem_alloc), GFP_KERNEL);
+	if (alloc == NULL) {
+		ret = -ENOMEM;
+		goto alloc_alloc_failed;
+	}
+
+	INIT_LIST_HEAD(&alloc->list);
+	atomic_inc(&alloc->ref_cnt);
+	alloc->flags = flags;
+	alloc->default_access = def_access;
+	INIT_LIST_HEAD(&alloc->threadg_info_list);
+#ifdef CONFIG_DEBUG_FS
+	alloc->creator = __builtin_return_address(0);
+	alloc->creator_tgid = task_tgid_nr(current);
+#endif
+	alloc->mem_type = resolve_mem_type(mem_type);
+	if (IS_ERR(alloc->mem_type)) {
+		ret = PTR_ERR(alloc->mem_type);
+		goto resolve_mem_type_failed;
+	}
+
+	alloc->allocator_hndl = alloc->mem_type->allocator_api.alloc(
+				alloc->mem_type->allocator_instance, size);
+	if (IS_ERR(alloc->allocator_hndl)) {
+		ret = PTR_ERR(alloc->allocator_hndl);
+		goto allocator_failed;
+	}
+
+	alloc->paddr = alloc->mem_type->allocator_api.get_alloc_paddr(
+							alloc->allocator_hndl);
+	alloc->size = alloc->mem_type->allocator_api.get_alloc_size(
+							alloc->allocator_hndl);
+
+	cach_init_buf(&alloc->cach_buf, alloc->flags, alloc->size);
+	ret = kmap_alloc(alloc);
+	if (ret < 0)
+		goto kmap_alloc_failed;
+	cach_set_buf_addrs(&alloc->cach_buf, alloc->kaddr, alloc->paddr);
+
+	list_add_tail(&alloc->list, &alloc_list);
+
+	clear_alloc_mem(alloc);
+
+	goto out;
+
+kmap_alloc_failed:
+allocator_failed:
+resolve_mem_type_failed:
+	destroy_alloc(alloc);
+alloc_alloc_failed:
+	alloc = ERR_PTR(ret);
+
+out:
+	mutex_unlock(&lock);
+
+	return alloc;
+}
+EXPORT_SYMBOL(hwmem_alloc);
+
+void hwmem_release(struct hwmem_alloc *alloc)
+{
+	mutex_lock(&lock);
+
+	if (atomic_dec_and_test(&alloc->ref_cnt))
+		destroy_alloc(alloc);
+
+	mutex_unlock(&lock);
+}
+EXPORT_SYMBOL(hwmem_release);
+
+int hwmem_set_domain(struct hwmem_alloc *alloc, enum hwmem_access access,
+		enum hwmem_domain domain, struct hwmem_region *region)
+{
+	mutex_lock(&lock);
+
+	cach_set_domain(&alloc->cach_buf, access, domain, region);
+
+	mutex_unlock(&lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(hwmem_set_domain);
+
+int hwmem_pin(struct hwmem_alloc *alloc, struct hwmem_mem_chunk *mem_chunks,
+							u32 *mem_chunks_length)
+{
+	if (*mem_chunks_length < 1) {
+		*mem_chunks_length = 1;
+		return -ENOSPC;
+	}
+
+	mutex_lock(&lock);
+
+	mem_chunks[0].paddr = alloc->paddr;
+	mem_chunks[0].size = alloc->size;
+	*mem_chunks_length = 1;
+
+	mutex_unlock(&lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(hwmem_pin);
+
+void hwmem_unpin(struct hwmem_alloc *alloc)
+{
+}
+EXPORT_SYMBOL(hwmem_unpin);
+
+static void vm_open(struct vm_area_struct *vma)
+{
+	atomic_inc(&((struct hwmem_alloc *)vma->vm_private_data)->ref_cnt);
+}
+
+static void vm_close(struct vm_area_struct *vma)
+{
+	hwmem_release((struct hwmem_alloc *)vma->vm_private_data);
+}
+
+int hwmem_mmap(struct hwmem_alloc *alloc, struct vm_area_struct *vma)
+{
+	int ret = 0;
+	unsigned long vma_size = vma->vm_end - vma->vm_start;
+	enum hwmem_access access;
+	mutex_lock(&lock);
+
+	access = get_access(alloc);
+
+	/* Check permissions */
+	if ((!(access & HWMEM_ACCESS_WRITE) &&
+				(vma->vm_flags & VM_WRITE)) ||
+			(!(access & HWMEM_ACCESS_READ) &&
+				(vma->vm_flags & VM_READ))) {
+		ret = -EPERM;
+		goto illegal_access;
+	}
+
+	if (vma_size > alloc->size) {
+		ret = -EINVAL;
+		goto illegal_size;
+	}
+
+	/*
+	 * We don't want Linux to do anything (merging etc) with our VMAs as
+	 * the offset is not necessarily valid
+	 */
+	vma->vm_flags |= VM_SPECIAL;
+	cach_set_pgprot_cache_options(&alloc->cach_buf, &vma->vm_page_prot);
+	vma->vm_private_data = (void *)alloc;
+	atomic_inc(&alloc->ref_cnt);
+	vma->vm_ops = &vm_ops;
+
+	ret = remap_pfn_range(vma, vma->vm_start, alloc->paddr >> PAGE_SHIFT,
+		min(vma_size, (unsigned long)alloc->size), vma->vm_page_prot);
+	if (ret < 0)
+		goto map_failed;
+
+	goto out;
+
+map_failed:
+	atomic_dec(&alloc->ref_cnt);
+illegal_size:
+illegal_access:
+
+out:
+	mutex_unlock(&lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(hwmem_mmap);
+
+void *hwmem_kmap(struct hwmem_alloc *alloc)
+{
+	void *ret;
+
+	mutex_lock(&lock);
+
+	ret = alloc->kaddr;
+
+	mutex_unlock(&lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(hwmem_kmap);
+
+void hwmem_kunmap(struct hwmem_alloc *alloc)
+{
+}
+EXPORT_SYMBOL(hwmem_kunmap);
+
+int hwmem_set_access(struct hwmem_alloc *alloc,
+		enum hwmem_access access, pid_t pid_nr)
+{
+	int ret;
+	struct hwmem_alloc_threadg_info *info;
+	struct pid *pid;
+	bool found = false;
+
+	pid = find_get_pid(pid_nr);
+	if (!pid) {
+		ret = -EINVAL;
+		goto error_get_pid;
+	}
+
+	list_for_each_entry(info, &(alloc->threadg_info_list), list) {
+		if (info->threadg_pid == pid) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		info = kmalloc(sizeof(*info), GFP_KERNEL);
+		if (!info) {
+			ret = -ENOMEM;
+			goto error_alloc_info;
+		}
+
+		info->threadg_pid = pid;
+		info->access = access;
+
+		list_add_tail(&(info->list), &(alloc->threadg_info_list));
+	} else {
+		info->access = access;
+	}
+
+	return 0;
+
+error_alloc_info:
+	put_pid(pid);
+error_get_pid:
+	return ret;
+}
+EXPORT_SYMBOL(hwmem_set_access);
+
+void hwmem_get_info(struct hwmem_alloc *alloc, u32 *size,
+	enum hwmem_mem_type *mem_type, enum hwmem_access *access)
+{
+	mutex_lock(&lock);
+
+	if (size != NULL)
+		*size = alloc->size;
+	if (mem_type != NULL)
+		*mem_type = alloc->mem_type->id;
+	if (access != NULL)
+		*access = get_access(alloc);
+
+	mutex_unlock(&lock);
+}
+EXPORT_SYMBOL(hwmem_get_info);
+
+s32 hwmem_get_name(struct hwmem_alloc *alloc)
+{
+	int ret = 0, name;
+
+	mutex_lock(&lock);
+
+	if (alloc->name != 0) {
+		ret = alloc->name;
+		goto out;
+	}
+
+	while (true) {
+		if (idr_pre_get(&global_idr, GFP_KERNEL) == 0) {
+			ret = -ENOMEM;
+			goto pre_get_id_failed;
+		}
+
+		ret = idr_get_new_above(&global_idr, alloc, 1, &name);
+		if (ret == 0)
+			break;
+		else if (ret != -EAGAIN)
+			goto get_id_failed;
+	}
+
+	if (name > S32_MAX) {
+		ret = -ENOMSG;
+		goto overflow;
+	}
+
+	alloc->name = name;
+
+	ret = name;
+	goto out;
+
+overflow:
+	idr_remove(&global_idr, name);
+get_id_failed:
+pre_get_id_failed:
+
+out:
+	mutex_unlock(&lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(hwmem_get_name);
+
+struct hwmem_alloc *hwmem_resolve_by_name(s32 name)
+{
+	struct hwmem_alloc *alloc;
+
+	mutex_lock(&lock);
+
+	alloc = idr_find(&global_idr, name);
+	if (alloc == NULL) {
+		alloc = ERR_PTR(-EINVAL);
+		goto find_failed;
+	}
+	atomic_inc(&alloc->ref_cnt);
+
+	goto out;
+
+find_failed:
+
+out:
+	mutex_unlock(&lock);
+
+	return alloc;
+}
+EXPORT_SYMBOL(hwmem_resolve_by_name);
+
+/* Debug */
+
+#ifdef CONFIG_DEBUG_FS
+
+static int debugfs_allocs_read(struct file *filp, char __user *buf,
+						size_t count, loff_t *f_pos);
+
+static const struct file_operations debugfs_allocs_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_allocs_read,
+};
+
+static int print_alloc(struct hwmem_alloc *alloc, char **buf, size_t buf_size)
+{
+	int ret;
+	char creator[KSYM_SYMBOL_LEN];
+	int i;
+
+	if (sprint_symbol(creator, (unsigned long)alloc->creator) < 0)
+		creator[0] = '\0';
+
+	for (i = 0; i < 2; i++) {
+		size_t buf_size_l;
+		if (i == 0)
+			buf_size_l = 0;
+		else
+			buf_size_l = buf_size;
+
+		ret = snprintf(*buf, buf_size_l,
+			"%#x\n"
+				"\tSize: %u\n"
+				"\tMemory type: %u\n"
+				"\tName: %#x\n"
+				"\tReference count: %i\n"
+				"\tAllocation flags: %#x\n"
+				"\t$ settings: %#x\n"
+				"\tDefault access: %#x\n"
+				"\tPhysical address: %#x\n"
+				"\tKernel virtual address: %#x\n"
+				"\tCreator: %s\n"
+				"\tCreator thread group id: %u\n",
+			(unsigned int)alloc, alloc->size, alloc->mem_type->id,
+			alloc->name, atomic_read(&alloc->ref_cnt),
+			alloc->flags, alloc->cach_buf.cache_settings,
+			alloc->default_access, alloc->paddr,
+			(unsigned int)alloc->kaddr, creator,
+			alloc->creator_tgid);
+		if (ret < 0)
+			return -ENOMSG;
+		else if (ret + 1 > buf_size)
+			return -EINVAL;
+	}
+
+	*buf += ret;
+
+	return 0;
+}
+
+static int debugfs_allocs_read(struct file *file, char __user *buf,
+						size_t count, loff_t *f_pos)
+{
+	/*
+	 * We assume the supplied buffer and PAGE_SIZE is large enough to hold
+	 * information about at least one alloc, if not no data will be
+	 * returned.
+	 */
+
+	int ret;
+	size_t i = 0;
+	struct hwmem_alloc *curr_alloc;
+	char *local_buf = kmalloc(PAGE_SIZE, GFP_KERNEL);
+	char *local_buf_pos = local_buf;
+	size_t available_space = min((size_t)PAGE_SIZE, count);
+	/* private_data is intialized to NULL in open which I assume is 0. */
+	void **curr_pos = &file->private_data;
+	size_t bytes_read;
+
+	if (local_buf == NULL)
+		return -ENOMEM;
+
+	mutex_lock(&lock);
+
+	list_for_each_entry(curr_alloc, &alloc_list, list) {
+		if (i++ < (size_t)*curr_pos)
+			continue;
+
+		ret = print_alloc(curr_alloc, &local_buf_pos, available_space -
+					(size_t)(local_buf_pos - local_buf));
+		if (ret == -EINVAL) /* No more room */
+			break;
+		else if (ret < 0)
+			goto out;
+
+		*curr_pos = (void *)i;
+	}
+
+	bytes_read = (size_t)(local_buf_pos - local_buf);
+
+	ret = copy_to_user(buf, local_buf, bytes_read);
+	if (ret < 0)
+		goto out;
+
+	ret = bytes_read;
+
+out:
+	kfree(local_buf);
+
+	mutex_unlock(&lock);
+
+	return ret;
+}
+
+static void init_debugfs(void)
+{
+	/* Hwmem is never unloaded so dropping the dentrys is ok. */
+	struct dentry *debugfs_root_dir = debugfs_create_dir("hwmem", NULL);
+	(void)debugfs_create_file("allocs", 0444, debugfs_root_dir, 0,
+							&debugfs_allocs_fops);
+}
+
+#endif /* #ifdef CONFIG_DEBUG_FS */
+
+/* Module */
+
+extern int hwmem_ioctl_init(void);
+
+static int __devinit hwmem_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	if (hwdev) {
+		dev_err(&pdev->dev, "Probed multiple times\n");
+		return -EINVAL;
+	}
+
+	hwdev = pdev;
+
+	/*
+	 * No need to flush the caches here. If we can keep track of the cache
+	 * content then none of our memory will be in the caches, if we can't
+	 * keep track of the cache content we always assume all our memory is
+	 * in the caches.
+	 */
+
+	ret = hwmem_ioctl_init();
+	if (ret < 0)
+		dev_warn(&pdev->dev, "Failed to start hwmem-ioctl, continuing"
+								" anyway\n");
+
+#ifdef CONFIG_DEBUG_FS
+	init_debugfs();
+#endif
+
+	dev_info(&pdev->dev, "Probed OK\n");
+
+	return 0;
+}
+
+static struct platform_driver hwmem_driver = {
+	.probe	= hwmem_probe,
+	.driver = {
+		.name	= "hwmem",
+	},
+};
+
+static int __init hwmem_init(void)
+{
+	return platform_driver_register(&hwmem_driver);
+}
+subsys_initcall(hwmem_init);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Hardware memory driver");
+
diff --git a/drivers/misc/mbox.c b/drivers/misc/mbox.c
new file mode 100644
index 00000000000..d884496fa4c
--- /dev/null
+++ b/drivers/misc/mbox.c
@@ -0,0 +1,867 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Stefan Nilsson <stefan.xk.nilsson@stericsson.com> for ST-Ericsson.
+ * Author: Martin Persson <martin.persson@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/*
+ * Mailbox nomenclature:
+ *
+ *       APE           MODEM
+ *           mbox pairX
+ *   ..........................
+ *   .                       .
+ *   .           peer        .
+ *   .     send  ----        .
+ *   .      -->  |  |        .
+ *   .           |  |        .
+ *   .           ----        .
+ *   .                       .
+ *   .           local       .
+ *   .     rec   ----        .
+ *   .           |  | <--    .
+ *   .           |  |        .
+ *   .           ----        .
+ *   .........................
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/platform_device.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/completion.h>
+#include <linux/workqueue.h>
+#include <linux/hrtimer.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/mfd/db5500-prcmu.h>
+#include <mach/mbox-db5500.h>
+#include <mach/reboot_reasons.h>
+
+#define MBOX_NAME "mbox"
+
+#define MBOX_FIFO_DATA        0x000
+#define MBOX_FIFO_ADD         0x004
+#define MBOX_FIFO_REMOVE      0x008
+#define MBOX_FIFO_THRES_FREE  0x00C
+#define MBOX_FIFO_THRES_OCCUP 0x010
+#define MBOX_FIFO_STATUS      0x014
+
+#define MBOX_DISABLE_IRQ 0x4
+#define MBOX_ENABLE_IRQ  0x0
+#define MBOX_LATCH 1
+
+struct mbox_device_info {
+	struct mbox *mbox;
+	struct workqueue_struct *mbox_modem_rel_wq;
+	struct work_struct mbox_modem_rel;
+	struct completion mod_req_ack_work;
+	atomic_t ape_state;
+	atomic_t mod_req;
+	atomic_t mod_reset;
+};
+
+/* Global list of all mailboxes */
+struct hrtimer ape_timer;
+struct hrtimer modem_timer;
+static DEFINE_MUTEX(modem_state_mutex);
+static struct list_head mboxs = LIST_HEAD_INIT(mboxs);
+static struct mbox_device_info *mb;
+
+static enum hrtimer_restart mbox_ape_callback(struct hrtimer *hrtimer)
+{
+	queue_work(mb->mbox_modem_rel_wq, &mb->mbox_modem_rel);
+
+	return HRTIMER_NORESTART;
+}
+
+static enum hrtimer_restart mbox_mod_callback(struct hrtimer *hrtimer)
+{
+	atomic_set(&mb->ape_state, 0);
+	return HRTIMER_NORESTART;
+}
+
+static void mbox_modem_rel_work(struct work_struct *work)
+{
+	mutex_lock(&modem_state_mutex);
+	prcmu_modem_rel();
+	atomic_set(&mb->mod_req, 0);
+	mutex_unlock(&modem_state_mutex);
+}
+
+static void mbox_modem_req(void)
+{
+	mutex_lock(&modem_state_mutex);
+	if (!db5500_prcmu_is_modem_requested()) {
+		prcmu_modem_req();
+		/* TODO: optimize this timeout */
+		if (!wait_for_completion_timeout(&mb->mod_req_ack_work,
+					msecs_to_jiffies(2000)))
+			printk(KERN_ERR "mbox:modem_req_ack timedout(2sec)\n");
+	}
+	atomic_set(&mb->mod_req, 1);
+	mutex_unlock(&modem_state_mutex);
+}
+
+static struct mbox *get_mbox_with_id(u8 id)
+{
+	u8 i;
+	struct list_head *pos = &mboxs;
+	for (i = 0; i <= id; i++)
+		pos = pos->next;
+
+	return (struct mbox *) list_entry(pos, struct mbox, list);
+}
+
+int mbox_send(struct mbox *mbox, u32 mbox_msg, bool block)
+{
+	int res = 0;
+	unsigned long flag;
+
+	if (atomic_read(&mb->mod_reset)) {
+		dev_err(&mbox->pdev->dev,
+				"mbox_send called after modem reset\n");
+		return -EINVAL;
+	}
+	dev_dbg(&(mbox->pdev->dev),
+		"About to buffer 0x%X to mailbox 0x%X."
+		" ri = %d, wi = %d\n",
+		mbox_msg, (u32)mbox, mbox->read_index,
+		mbox->write_index);
+
+	/* Request for modem */
+	if (!db5500_prcmu_is_modem_requested())
+		mbox_modem_req();
+
+	spin_lock_irqsave(&mbox->lock, flag);
+	/* Check if write buffer is full */
+	while (((mbox->write_index + 1) % MBOX_BUF_SIZE) == mbox->read_index) {
+		if (!block) {
+			dev_dbg(&(mbox->pdev->dev),
+			"Buffer full in non-blocking call! "
+			"Returning -ENOMEM!\n");
+			res = -ENOMEM;
+			goto exit;
+		}
+		spin_unlock_irqrestore(&mbox->lock, flag);
+		dev_dbg(&(mbox->pdev->dev),
+			"Buffer full in blocking call! Sleeping...\n");
+		mbox->client_blocked = 1;
+		wait_for_completion(&mbox->buffer_available);
+		dev_dbg(&(mbox->pdev->dev),
+			"Blocking send was woken up! Trying again...\n");
+		spin_lock_irqsave(&mbox->lock, flag);
+	}
+
+	mbox->buffer[mbox->write_index] = mbox_msg;
+	mbox->write_index = (mbox->write_index + 1) % MBOX_BUF_SIZE;
+
+	/*
+	 * Indicate that we want an IRQ as soon as there is a slot
+	 * in the FIFO
+	 */
+	if (atomic_read(&mb->mod_reset)) {
+		dev_err(&mbox->pdev->dev,
+				"modem is in reset state, cannot proceed\n");
+		res  = -EINVAL;
+		goto exit;
+	}
+	writel(MBOX_ENABLE_IRQ, mbox->virtbase_peer + MBOX_FIFO_THRES_FREE);
+
+exit:
+	spin_unlock_irqrestore(&mbox->lock, flag);
+	return res;
+}
+EXPORT_SYMBOL(mbox_send);
+
+#if defined(CONFIG_DEBUG_FS)
+/*
+ * Expected input: <value> <nbr sends>
+ * Example: "echo 0xdeadbeef 4 > mbox-node" sends 0xdeadbeef 4 times
+ */
+static ssize_t mbox_write_fifo(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf,
+			       size_t count)
+{
+	unsigned long mbox_mess;
+	unsigned long nbr_sends;
+	unsigned long i;
+	char int_buf[16];
+	char *token;
+	char *val;
+
+	struct platform_device *pdev = to_platform_device(dev);
+	struct mbox *mbox = platform_get_drvdata(pdev);
+
+	strncpy((char *) &int_buf, buf, sizeof(int_buf));
+	token = (char *) &int_buf;
+
+	/* Parse message */
+	val = strsep(&token, " ");
+	if ((val == NULL) || (strict_strtoul(val, 16, &mbox_mess) != 0))
+		mbox_mess = 0xDEADBEEF;
+
+	val = strsep(&token, " ");
+	if ((val == NULL) || (strict_strtoul(val, 10, &nbr_sends) != 0))
+		nbr_sends = 1;
+
+	dev_dbg(dev, "Will write 0x%lX %ld times using data struct at 0x%X\n",
+		mbox_mess, nbr_sends, (u32) mbox);
+
+	for (i = 0; i < nbr_sends; i++)
+		mbox_send(mbox, mbox_mess, true);
+
+	return count;
+}
+
+static ssize_t mbox_read_fifo(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	int mbox_value;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct mbox *mbox = platform_get_drvdata(pdev);
+
+	if (atomic_read(&mb->mod_reset)) {
+		dev_err(&mbox->pdev->dev, "modem crashed, returning\n");
+		return 0;
+	}
+	if ((readl(mbox->virtbase_local + MBOX_FIFO_STATUS) & 0x7) <= 0)
+		return sprintf(buf, "Mailbox is empty\n");
+
+	mbox_value = readl(mbox->virtbase_local + MBOX_FIFO_DATA);
+	writel(MBOX_LATCH, (mbox->virtbase_local + MBOX_FIFO_REMOVE));
+
+	return sprintf(buf, "0x%X\n", mbox_value);
+}
+
+static DEVICE_ATTR(fifo, S_IWUGO | S_IRUGO, mbox_read_fifo, mbox_write_fifo);
+
+static int mbox_show(struct seq_file *s, void *data)
+{
+	struct list_head *pos;
+	u8 mbox_index = 0;
+
+	list_for_each(pos, &mboxs) {
+		struct mbox *m =
+			(struct mbox *) list_entry(pos, struct mbox, list);
+		if (m == NULL) {
+			seq_printf(s,
+				   "Unable to retrieve mailbox %d\n",
+				   mbox_index);
+			continue;
+		}
+
+		spin_lock(&m->lock);
+		if ((m->virtbase_peer == NULL) || (m->virtbase_local == NULL)) {
+			seq_printf(s, "MAILBOX %d not setup or corrupt\n",
+				   mbox_index);
+			spin_unlock(&m->lock);
+			continue;
+		}
+
+		if (atomic_read(&mb->mod_reset)) {
+			dev_err(&m->pdev->dev, "modem crashed, returning\n");
+			spin_unlock(&m->lock);
+			return 0;
+		}
+		seq_printf(s,
+		"===========================\n"
+		" MAILBOX %d\n"
+		" PEER MAILBOX DUMP\n"
+		"---------------------------\n"
+		"FIFO:                 0x%X (%d)\n"
+		"Free     Threshold:   0x%.2X (%d)\n"
+		"Occupied Threshold:   0x%.2X (%d)\n"
+		"Status:               0x%.2X (%d)\n"
+		"   Free spaces  (ot):    %d (%d)\n"
+		"   Occup spaces (ot):    %d (%d)\n"
+		"===========================\n"
+		" LOCAL MAILBOX DUMP\n"
+		"---------------------------\n"
+		"FIFO:                 0x%.X (%d)\n"
+		"Free     Threshold:   0x%.2X (%d)\n"
+		"Occupied Threshold:   0x%.2X (%d)\n"
+		"Status:               0x%.2X (%d)\n"
+		"   Free spaces  (ot):    %d (%d)\n"
+		"   Occup spaces (ot):    %d (%d)\n"
+		"===========================\n"
+		"write_index: %d\n"
+		"read_index : %d\n"
+		"===========================\n"
+		"\n",
+		mbox_index,
+		readl(m->virtbase_peer + MBOX_FIFO_DATA),
+		readl(m->virtbase_peer + MBOX_FIFO_DATA),
+		readl(m->virtbase_peer + MBOX_FIFO_THRES_FREE),
+		readl(m->virtbase_peer + MBOX_FIFO_THRES_FREE),
+		readl(m->virtbase_peer + MBOX_FIFO_THRES_OCCUP),
+		readl(m->virtbase_peer + MBOX_FIFO_THRES_OCCUP),
+		readl(m->virtbase_peer + MBOX_FIFO_STATUS),
+		readl(m->virtbase_peer + MBOX_FIFO_STATUS),
+		(readl(m->virtbase_peer + MBOX_FIFO_STATUS) >> 4) & 0x7,
+		(readl(m->virtbase_peer + MBOX_FIFO_STATUS) >> 7) & 0x1,
+		(readl(m->virtbase_peer + MBOX_FIFO_STATUS) >> 0) & 0x7,
+		(readl(m->virtbase_peer + MBOX_FIFO_STATUS) >> 3) & 0x1,
+		readl(m->virtbase_local + MBOX_FIFO_DATA),
+		readl(m->virtbase_local + MBOX_FIFO_DATA),
+		readl(m->virtbase_local + MBOX_FIFO_THRES_FREE),
+		readl(m->virtbase_local + MBOX_FIFO_THRES_FREE),
+		readl(m->virtbase_local + MBOX_FIFO_THRES_OCCUP),
+		readl(m->virtbase_local + MBOX_FIFO_THRES_OCCUP),
+		readl(m->virtbase_local + MBOX_FIFO_STATUS),
+		readl(m->virtbase_local + MBOX_FIFO_STATUS),
+		(readl(m->virtbase_local + MBOX_FIFO_STATUS) >> 4) & 0x7,
+		(readl(m->virtbase_local + MBOX_FIFO_STATUS) >> 7) & 0x1,
+		(readl(m->virtbase_local + MBOX_FIFO_STATUS) >> 0) & 0x7,
+		(readl(m->virtbase_local + MBOX_FIFO_STATUS) >> 3) & 0x1,
+		m->write_index, m->read_index);
+		mbox_index++;
+		spin_unlock(&m->lock);
+	}
+
+	return 0;
+}
+
+static int mbox_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, mbox_show, NULL);
+}
+
+static const struct file_operations mbox_operations = {
+	.owner = THIS_MODULE,
+	.open = mbox_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+#endif
+
+static irqreturn_t mbox_irq(int irq, void *arg)
+{
+	u32 mbox_value;
+	int nbr_occup;
+	int nbr_free;
+	struct mbox *mbox = (struct mbox *) arg;
+
+	if (atomic_read(&mb->mod_reset)) {
+		dev_err(&mbox->pdev->dev, "modem in reset state\n");
+		return IRQ_HANDLED;
+	}
+	spin_lock(&mbox->lock);
+
+	dev_dbg(&(mbox->pdev->dev),
+		"mbox IRQ [%d] received. ri = %d, wi = %d\n",
+		irq, mbox->read_index, mbox->write_index);
+
+	/*
+	 * Check if we have any outgoing messages, and if there is space for
+	 * them in the FIFO.
+	 */
+	if (mbox->read_index != mbox->write_index) {
+		/*
+		 * Check by reading FREE for LOCAL since that indicates
+		 * OCCUP for PEER
+		 */
+		nbr_free = (readl(mbox->virtbase_local + MBOX_FIFO_STATUS)
+			    >> 4) & 0x7;
+		dev_dbg(&(mbox->pdev->dev),
+			"Status indicates %d empty spaces in the FIFO!\n",
+			nbr_free);
+
+		while ((nbr_free > 0) &&
+		       (mbox->read_index != mbox->write_index)) {
+			if (atomic_read(&mb->mod_reset)) {
+				dev_err(&mbox->pdev->dev,
+						"modem in reset state\n");
+				goto exit;
+			}
+			/* Write the message and latch it into the FIFO */
+			writel(mbox->buffer[mbox->read_index],
+			       (mbox->virtbase_peer + MBOX_FIFO_DATA));
+			writel(MBOX_LATCH,
+			       (mbox->virtbase_peer + MBOX_FIFO_ADD));
+			dev_dbg(&(mbox->pdev->dev),
+				"Wrote message 0x%X to addr 0x%X\n",
+				mbox->buffer[mbox->read_index],
+				(u32) (mbox->virtbase_peer + MBOX_FIFO_DATA));
+
+			nbr_free--;
+			mbox->read_index =
+				(mbox->read_index + 1) % MBOX_BUF_SIZE;
+		}
+
+		if (atomic_read(&mb->mod_reset)) {
+			dev_err(&mbox->pdev->dev, "modem in reset state\n");
+			goto exit;
+		}
+		/*
+		 * Check if we still want IRQ:s when there is free
+		 * space to send
+		 */
+		if (mbox->read_index != mbox->write_index) {
+			dev_dbg(&(mbox->pdev->dev),
+				"Still have messages to send, but FIFO full. "
+				"Request IRQ again!\n");
+			writel(MBOX_ENABLE_IRQ,
+			       mbox->virtbase_peer + MBOX_FIFO_THRES_FREE);
+		} else {
+			dev_dbg(&(mbox->pdev->dev),
+				"No more messages to send. "
+				"Do not request IRQ again!\n");
+			writel(MBOX_DISABLE_IRQ,
+			       mbox->virtbase_peer + MBOX_FIFO_THRES_FREE);
+		}
+
+		/*
+		 * Check if we can signal any blocked clients that it is OK to
+		 * start buffering again
+		 */
+		if (mbox->client_blocked &&
+		    (((mbox->write_index + 1) % MBOX_BUF_SIZE)
+		     != mbox->read_index)) {
+			dev_dbg(&(mbox->pdev->dev),
+				"Waking up blocked client\n");
+			complete(&mbox->buffer_available);
+			mbox->client_blocked = 0;
+		}
+	}
+
+	/* Start timer and on timer expiry call modem_rel */
+	hrtimer_start(&ape_timer, ktime_set(0, 10*NSEC_PER_MSEC),
+			HRTIMER_MODE_REL);
+
+	if (atomic_read(&mb->mod_reset)) {
+		dev_err(&mbox->pdev->dev, "modem in reset state\n");
+		goto exit;
+	}
+	/* Check if we have any incoming messages */
+	nbr_occup = readl(mbox->virtbase_local + MBOX_FIFO_STATUS) & 0x7;
+	if (nbr_occup == 0)
+		goto exit;
+
+redo:
+	if (mbox->cb == NULL) {
+		dev_dbg(&(mbox->pdev->dev), "No receive callback registered, "
+			"leaving %d incoming messages in fifo!\n", nbr_occup);
+		goto exit;
+	}
+	atomic_set(&mb->ape_state, 1);
+
+	if (atomic_read(&mb->mod_reset)) {
+		dev_err(&mbox->pdev->dev, "modem in reset state\n");
+		goto exit;
+	}
+	/* Read and acknowledge the message */
+	mbox_value = readl(mbox->virtbase_local + MBOX_FIFO_DATA);
+	writel(MBOX_LATCH, (mbox->virtbase_local + MBOX_FIFO_REMOVE));
+
+	/* Notify consumer of new mailbox message */
+	dev_dbg(&(mbox->pdev->dev), "Calling callback for message 0x%X!\n",
+		mbox_value);
+	mbox->cb(mbox_value, mbox->client_data);
+
+	nbr_occup = readl(mbox->virtbase_local + MBOX_FIFO_STATUS) & 0x7;
+
+	if (nbr_occup > 0)
+		goto redo;
+
+	/* Start a timer and timer expiry will be the criteria for sleep */
+	hrtimer_start(&modem_timer, ktime_set(0, 100*MSEC_PER_SEC),
+			HRTIMER_MODE_REL);
+exit:
+	dev_dbg(&(mbox->pdev->dev), "Exit mbox IRQ. ri = %d, wi = %d\n",
+		mbox->read_index, mbox->write_index);
+	spin_unlock(&mbox->lock);
+
+	return IRQ_HANDLED;
+}
+
+static void mbox_shutdown(struct mbox *mbox)
+{
+	if (!mbox->allocated)
+		return;
+#if defined(CONFIG_DEBUG_FS)
+	debugfs_remove(mbox->dentry);
+	device_remove_file(&mbox->pdev->dev, &dev_attr_fifo);
+#endif
+	/* TODO: Need to check if we can write after modem reset */
+	if (!atomic_read(&mb->mod_reset)) {
+		writel(MBOX_DISABLE_IRQ, mbox->virtbase_local +
+				MBOX_FIFO_THRES_OCCUP);
+		writel(MBOX_DISABLE_IRQ, mbox->virtbase_peer +
+				MBOX_FIFO_THRES_FREE);
+	}
+	free_irq(mbox->irq, (void *)mbox);
+	mbox->client_blocked = 0;
+	iounmap(mbox->virtbase_local);
+	iounmap(mbox->virtbase_peer);
+	mbox->cb = NULL;
+	mbox->client_data = NULL;
+	mbox->allocated = false;
+}
+
+/** mbox_state_reset - Reset the mailbox state machine
+ *
+ * This function is called on receiving modem reset interrupt. Reset all
+ * the mailbox state machine, disable irq, cancel timers, shutdown the
+ * mailboxs and re-enable irq's.
+ */
+void mbox_state_reset(void)
+{
+	struct mbox *mbox = mb->mbox;
+
+	/* Common for all mailbox */
+	atomic_set(&mb->mod_reset, 1);
+
+	/* Disable IRQ */
+	disable_irq_nosync(IRQ_DB5500_PRCMU_AC_WAKE_ACK);
+
+	/* Cancel sleep_req timers */
+	hrtimer_cancel(&modem_timer);
+	hrtimer_cancel(&ape_timer);
+
+	/* specific to each mailbox */
+	list_for_each_entry(mbox, &mboxs, list) {
+		mbox_shutdown(mbox);
+	}
+
+	/* Reset mailbox state machine */
+	atomic_set(&mb->mod_req, 0);
+	atomic_set(&mb->ape_state, 0);
+
+	/* Enable irq */
+	enable_irq(IRQ_DB5500_PRCMU_AC_WAKE_ACK);
+}
+
+
+/* Setup is executed once for each mbox pair */
+struct mbox *mbox_setup(u8 mbox_id, mbox_recv_cb_t *mbox_cb, void *priv)
+{
+	struct resource *resource;
+	int res;
+	struct mbox *mbox;
+
+	/*
+	 * set mod_reset flag to '0', clients calling this APE should make sure
+	 * that modem is rebooted after MSR. Mailbox doesnt have any means of
+	 * knowing the boot status of modem.
+	 */
+	atomic_set(&mb->mod_reset, 0);
+
+	mbox = get_mbox_with_id(mbox_id);
+	if (mbox == NULL) {
+		dev_err(&(mbox->pdev->dev), "Incorrect mailbox id: %d!\n",
+			mbox_id);
+		goto exit;
+	}
+
+	/*
+	 * Check if mailbox has been allocated to someone else,
+	 * otherwise allocate it
+	 */
+	if (mbox->allocated) {
+		dev_err(&(mbox->pdev->dev), "Mailbox number %d is busy!\n",
+			mbox_id);
+		mbox = NULL;
+		goto exit;
+	}
+	mbox->allocated = true;
+
+	dev_dbg(&(mbox->pdev->dev), "Initiating mailbox number %d: 0x%X...\n",
+		mbox_id, (u32)mbox);
+
+	mbox->client_data = priv;
+	mbox->cb = mbox_cb;
+
+	/* Get addr for peer mailbox and ioremap it */
+	resource = platform_get_resource_byname(mbox->pdev,
+						IORESOURCE_MEM,
+						"mbox_peer");
+	if (resource == NULL) {
+		dev_err(&(mbox->pdev->dev),
+			"Unable to retrieve mbox peer resource\n");
+		mbox = NULL;
+		goto free_mbox;
+	}
+	dev_dbg(&(mbox->pdev->dev),
+		"Resource name: %s start: 0x%X, end: 0x%X\n",
+		resource->name, resource->start, resource->end);
+	mbox->virtbase_peer = ioremap(resource->start, resource_size(resource));
+	if (!mbox->virtbase_peer) {
+		dev_err(&(mbox->pdev->dev), "Unable to ioremap peer mbox\n");
+		mbox = NULL;
+		goto free_mbox;
+	}
+	dev_dbg(&(mbox->pdev->dev),
+		"ioremapped peer physical: (0x%X-0x%X) to virtual: 0x%X\n",
+		resource->start, resource->end, (u32) mbox->virtbase_peer);
+
+	/* Get addr for local mailbox and ioremap it */
+	resource = platform_get_resource_byname(mbox->pdev,
+						IORESOURCE_MEM,
+						"mbox_local");
+	if (resource == NULL) {
+		dev_err(&(mbox->pdev->dev),
+			"Unable to retrieve mbox local resource\n");
+		mbox = NULL;
+		goto free_map;
+	}
+	dev_dbg(&(mbox->pdev->dev),
+		"Resource name: %s start: 0x%X, end: 0x%X\n",
+		resource->name, resource->start, resource->end);
+	mbox->virtbase_local = ioremap(resource->start, resource_size(resource));
+	if (!mbox->virtbase_local) {
+		dev_err(&(mbox->pdev->dev), "Unable to ioremap local mbox\n");
+		mbox = NULL;
+		goto free_map;
+	}
+	dev_dbg(&(mbox->pdev->dev),
+		"ioremapped local physical: (0x%X-0x%X) to virtual: 0x%X\n",
+		resource->start, resource->end, (u32) mbox->virtbase_peer);
+
+	init_completion(&mbox->buffer_available);
+	mbox->client_blocked = 0;
+
+	/* Get IRQ for mailbox and allocate it */
+	mbox->irq = platform_get_irq_byname(mbox->pdev, "mbox_irq");
+	if (mbox->irq < 0) {
+		dev_err(&(mbox->pdev->dev),
+			"Unable to retrieve mbox irq resource\n");
+		mbox = NULL;
+		goto free_map1;
+	}
+
+	dev_dbg(&(mbox->pdev->dev), "Allocating irq %d...\n", mbox->irq);
+	res = request_threaded_irq(mbox->irq, NULL, mbox_irq,
+			IRQF_NO_SUSPEND | IRQF_ONESHOT,
+			mbox->name, (void *) mbox);
+	if (res < 0) {
+		dev_err(&(mbox->pdev->dev),
+			"Unable to allocate mbox irq %d\n", mbox->irq);
+		mbox = NULL;
+		goto exit;
+	}
+
+	/* check if modem has reset */
+	if (atomic_read(&mb->mod_reset)) {
+		dev_err(&mbox->pdev->dev,
+				"modem is in reset state, cannot proceed\n");
+		mbox = NULL;
+		goto free_irq;
+	}
+	/* Set up mailbox to not launch IRQ on free space in mailbox */
+	writel(MBOX_DISABLE_IRQ, mbox->virtbase_peer + MBOX_FIFO_THRES_FREE);
+
+	/*
+	 * Set up mailbox to launch IRQ on new message if we have
+	 * a callback set. If not, do not raise IRQ, but keep message
+	 * in FIFO for manual retrieval
+	 */
+	if (mbox_cb != NULL)
+		writel(MBOX_ENABLE_IRQ,
+		       mbox->virtbase_local + MBOX_FIFO_THRES_OCCUP);
+	else
+		writel(MBOX_DISABLE_IRQ,
+		       mbox->virtbase_local + MBOX_FIFO_THRES_OCCUP);
+
+#if defined(CONFIG_DEBUG_FS)
+	res = device_create_file(&(mbox->pdev->dev), &dev_attr_fifo);
+	if (res != 0)
+		dev_warn(&(mbox->pdev->dev),
+			 "Unable to create mbox sysfs entry");
+
+	mbox->dentry = debugfs_create_file("mbox", S_IFREG | S_IRUGO, NULL,
+				   NULL, &mbox_operations);
+#endif
+	dev_info(&(mbox->pdev->dev),
+		 "Mailbox driver with index %d initiated!\n", mbox_id);
+
+	return mbox;
+free_irq:
+	free_irq(mbox->irq, (void *)mbox);
+free_map1:
+	iounmap(mbox->virtbase_local);
+free_map:
+	iounmap(mbox->virtbase_peer);
+free_mbox:
+	mbox->client_data = NULL;
+	mbox->cb = NULL;
+exit:
+	return mbox;
+}
+EXPORT_SYMBOL(mbox_setup);
+
+static irqreturn_t mbox_prcmu_mod_req_ack_handler(int irq, void *data)
+{
+	complete(&mb->mod_req_ack_work);
+	return IRQ_HANDLED;
+}
+
+int __init mbox_probe(struct platform_device *pdev)
+{
+	struct mbox *mbox;
+	int res = 0;
+	dev_dbg(&(pdev->dev), "Probing mailbox (pdev = 0x%X)...\n", (u32) pdev);
+
+	mbox = kzalloc(sizeof(struct mbox), GFP_KERNEL);
+	if (mbox == NULL) {
+		dev_err(&pdev->dev,
+				"Could not allocate memory for struct mbox\n");
+		return -ENOMEM;
+	}
+
+	mbox->pdev = pdev;
+	mbox->write_index = 0;
+	mbox->read_index = 0;
+
+	INIT_LIST_HEAD(&(mbox->list));
+	list_add_tail(&(mbox->list), &mboxs);
+
+	sprintf(mbox->name, "%s", MBOX_NAME);
+	spin_lock_init(&mbox->lock);
+
+	platform_set_drvdata(pdev, mbox);
+	mb->mbox = mbox;
+	dev_info(&(pdev->dev), "Mailbox driver loaded\n");
+
+	return res;
+}
+
+static int __exit mbox_remove(struct platform_device *pdev)
+{
+	struct mbox *mbox = platform_get_drvdata(pdev);
+
+	hrtimer_cancel(&ape_timer);
+	hrtimer_cancel(&modem_timer);
+	mbox_shutdown(mbox);
+	list_del(&mbox->list);
+	kfree(mbox);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+int mbox_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct mbox *mbox = platform_get_drvdata(pdev);
+
+	/*
+	 * Nothing to be done for now, once APE-Modem power management is
+	 * in place communication will have to be stopped.
+	 */
+
+	list_for_each_entry(mbox, &mboxs, list) {
+		if (mbox->client_blocked)
+			return -EBUSY;
+	}
+	dev_dbg(dev, "APE_STATE = %d\n", atomic_read(&mb->ape_state));
+	dev_dbg(dev, "MODEM_STATE = %d\n", db5500_prcmu_is_modem_requested());
+	if (atomic_read(&mb->ape_state) || db5500_prcmu_is_modem_requested() ||
+			atomic_read(&mb->mod_req))
+		return -EBUSY;
+	return 0;
+}
+
+int mbox_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct mbox *mbox = platform_get_drvdata(pdev);
+
+	/*
+	 * Nothing to be done for now, once APE-Modem power management is
+	 * in place communication will have to be resumed.
+	 */
+
+	return 0;
+}
+
+static const struct dev_pm_ops mbox_dev_pm_ops = {
+	.suspend_noirq = mbox_suspend,
+	.resume_noirq = mbox_resume,
+};
+#endif
+
+static struct platform_driver mbox_driver = {
+	.remove = __exit_p(mbox_remove),
+	.driver = {
+		.name = MBOX_NAME,
+		.owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm = &mbox_dev_pm_ops,
+#endif
+	},
+};
+
+static int __init mbox_init(void)
+{
+	struct mbox_device_info *mb_di;
+	int err;
+
+	mb_di = kzalloc(sizeof(struct mbox_device_info), GFP_KERNEL);
+	if (mb_di == NULL) {
+		printk(KERN_ERR
+			"mbox:Could not allocate memory for struct mbox_device_info\n");
+		return -ENOMEM;
+	}
+
+	mb_di->mbox_modem_rel_wq = create_singlethread_workqueue(
+			"mbox_modem_rel");
+	if (!mb_di->mbox_modem_rel_wq) {
+		printk(KERN_ERR "mbox:failed to create work queue\n");
+		err = -ENOMEM;
+		goto free_mem;
+	}
+
+	INIT_WORK(&mb_di->mbox_modem_rel, mbox_modem_rel_work);
+
+	hrtimer_init(&ape_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	ape_timer.function = mbox_ape_callback;
+	hrtimer_init(&modem_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	modem_timer.function = mbox_mod_callback;
+
+	atomic_set(&mb_di->ape_state, 0);
+	atomic_set(&mb_di->mod_req, 0);
+	atomic_set(&mb_di->mod_reset, 0);
+
+	err = request_irq(IRQ_DB5500_PRCMU_AC_WAKE_ACK,
+			mbox_prcmu_mod_req_ack_handler,
+			IRQF_NO_SUSPEND, "mod_req_ack", NULL);
+	if (err < 0) {
+		printk(KERN_ERR "mbox:Failed alloc IRQ_PRCMU_CA_SLEEP.\n");
+		goto free_irq;
+	}
+
+	init_completion(&mb_di->mod_req_ack_work);
+	mb = mb_di;
+	return platform_driver_probe(&mbox_driver, mbox_probe);
+free_irq:
+	destroy_workqueue(mb_di->mbox_modem_rel_wq);
+free_mem:
+	kfree(mb_di);
+	return err;
+}
+
+module_init(mbox_init);
+
+void __exit mbox_exit(void)
+{
+	free_irq(IRQ_DB5500_PRCMU_AC_WAKE_ACK, NULL);
+	destroy_workqueue(mb->mbox_modem_rel_wq);
+	platform_driver_unregister(&mbox_driver);
+	kfree(mb);
+}
+
+module_exit(mbox_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MBOX driver");
diff --git a/drivers/misc/mbox_channels-db5500.c b/drivers/misc/mbox_channels-db5500.c
new file mode 100644
index 00000000000..919be308ed4
--- /dev/null
+++ b/drivers/misc/mbox_channels-db5500.c
@@ -0,0 +1,1273 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Mailbox Logical Driver
+ *
+ * Author: Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com> for ST-Ericsson.
+ *         Bibek Basu ,bibek.basu@stericsson.com>
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <asm/mach-types.h>
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <mach/mbox-db5500.h>
+#include <mach/mbox_channels-db5500.h>
+#include <linux/io.h>
+
+/* Defines start sequence number for given mailbox channel */
+#define CHANNEL_START_SEQUENCE_NUMBER	0x80
+
+/* Defines number of channels per mailbox unit */
+#define CHANNELS_PER_MBOX_UNIT		256
+
+/*
+ * This macro builds mbox channel PDU header with following format:
+ * ---------------------------------------------------------------------------
+ * |                |         |          |                                    |
+ * | Sequence nmbr  |  Type   |  Length  | Destination logical channel number |
+ * |                |         |          |                                    |
+ * ---------------------------------------------------------------------------
+ * 31               24        20         16                    0
+ *
+ */
+#define BUILD_HEADER(chan, len, type, seq_no) \
+			((chan) | (((len) & 0xf) << 16) | \
+			(((type) & 0xf) << 20) | ((seq_no) << 24))
+
+/* Returns type from mbox message header */
+#define GET_TYPE(mbox_msg)			(((mbox_msg) >> 20) & 0xf)
+
+/* Returns channel number from mbox message header */
+#define GET_CHANNEL(mbox_msg)			((mbox_msg) & 0xffff)
+
+/* Returns length of payload from mbox message header */
+#define GET_LENGTH(mbox_msg)			(((mbox_msg) >> 16) & 0xf)
+
+/* Returns sequence number from mbox message header */
+#define GET_SEQ_NUMBER(mbox_msg)		(((mbox_msg) >> 24)
+
+enum mbox_msg{
+	MBOX_CLOSE,
+	MBOX_OPEN,
+	MBOX_SEND,
+	MBOX_CAST,
+	MBOX_ACK,
+	MBOX_NAK,
+};
+
+enum mbox_dir {
+	MBOX_TX,
+	MBOX_RX,
+};
+
+struct mbox_channel_mapping {
+	u16		chan_base;
+	u8		mbox_id;
+	enum mbox_dir	direction;
+};
+
+/* This table maps mbox logical channel to mbox id and direction */
+static struct mbox_channel_mapping channel_mappings[] = {
+	{0x500, 2, MBOX_RX}, /* channel 5 maps to mbox 0.1, dsp->app (unsec) */
+	{0x900, 2, MBOX_TX}, /* channel 9 maps to mbox 0.0, app->dsp (unsec) */
+};
+
+/* This table specifies mailbox ids which mbox channels module will use */
+static u8 mbox_ids[] = {
+	2,		/* app <-> dsp (unsec) */
+};
+
+/**
+ * struct mbox_unit_status - current status of mbox unit
+ * @mbox_id :		holds mbox unit identification number
+ * @mbox :		holds mbox pointer after mbox_register() call
+ * @tx_chans :		holds list of open tx mbox channels
+ * @tx_lock:		lock for tx channel
+ * @rx_chans :		holds list of open rx mbox channels
+ * @rx_lock:		lock for rx channel
+ */
+struct mbox_unit_status {
+	u8			mbox_id;
+	struct mbox		*mbox;
+	struct list_head	tx_chans;
+	spinlock_t		tx_lock;
+	struct list_head	rx_chans;
+	spinlock_t		rx_lock;
+};
+
+static struct {
+	struct platform_device	*pdev;
+	struct mbox_unit_status	mbox_unit[ARRAY_SIZE(mbox_ids)];
+} channels;
+
+/* This structure describes pending element for mbox tx channel */
+struct pending_elem {
+	struct list_head	list;
+	u32			*data;
+	u8			length;
+};
+
+struct rx_pending_elem {
+	u32			buffer[MAILBOX_NR_OF_DATAWORDS];
+	u8			length;
+	void			*priv;
+};
+
+struct rx_pending_elem rx_pending[NUM_DSP_BUFFER];
+
+/* This structure holds list of pending elements for mbox tx channel */
+struct tx_channel {
+	struct list_head	pending;
+};
+
+/* Specific status for mbox rx channel */
+struct rx_channel {
+	struct list_head	pending;
+	spinlock_t		lock;
+	u32			buffer[MAILBOX_NR_OF_DATAWORDS];
+	u8			index;
+	u8			length;
+};
+
+/**
+ * struct channel_status - status of mbox channel - common for tx and rx
+ * @list :		holds list of channels registered
+ * @channel :		holds channel number
+ * @state :		holds state of channel
+ * @cb:			holds callback function forr rx channel
+ * @with_ack :		holds if ack is needed
+ * @rx:			holds pointer to rx_channel
+ * @tx :		holds pointer to tx_channel
+ * @receive_wq :	holds pointer to receive workqueue_struct
+ * @cast_wq :		holds pointer to cast workqueue_struct
+ * @open_msg:		holds work_struct for open msg
+ * @receive_msg :	holds work_struct for receive msg
+ * @cast_msg:		holds work_struct for cast msg
+ * @lock:		holds lock for channel
+ */
+struct channel_status {
+	atomic_t rcv_counter;
+	struct list_head	list;
+	u16			channel;
+	int			state;
+	mbox_channel_cb_t	*cb;
+	void			*priv;
+	u8			seq_number;
+	bool			with_ack;
+	struct rx_channel	rx;
+	struct tx_channel	tx;
+	struct workqueue_struct	*receive_wq;
+	struct workqueue_struct	*cast_wq;
+	struct work_struct	open_msg;
+	struct work_struct	receive_msg;
+	struct work_struct	cast_msg;
+	struct mutex		lock;
+};
+
+/* Checks if provided channel number is valid */
+static bool check_channel(u16 channel, enum mbox_dir direction)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(channel_mappings); i++) {
+		if ((channel >= channel_mappings[i].chan_base) &&
+		    (channel < channel_mappings[i].chan_base +
+		    CHANNELS_PER_MBOX_UNIT)) {
+			/* Check if direction of given channel is correct*/
+			if (channel_mappings[i].direction == direction)
+				return true;
+			else
+				break;
+		}
+	}
+	return false;
+}
+
+/* get the tx channel corresponding to the given rx channel */
+static u16 get_tx_channel(u16 channel)
+{
+	int i;
+	int relative_chan = 0;
+	int mbox_id = 0xFF;
+	u16 tx_channel = 0xFF;
+
+	for (i = 0; i < ARRAY_SIZE(channel_mappings); i++) {
+		if ((channel >= channel_mappings[i].chan_base) &&
+				(channel < channel_mappings[i].chan_base +
+				 CHANNELS_PER_MBOX_UNIT)) {
+			/* Check if direction of given channel is correct*/
+			relative_chan = channel - channel_mappings[i].chan_base;
+			mbox_id = channel_mappings[i].mbox_id;
+
+		}
+	}
+
+	for (i = 0; i < ARRAY_SIZE(channel_mappings); i++) {
+		if ((mbox_id == channel_mappings[i].mbox_id) &&
+				(channel_mappings[i].direction == MBOX_TX))
+			tx_channel = channel_mappings[i].chan_base +
+				relative_chan;
+	}
+	return tx_channel;
+}
+
+/* Returns mbox unit id for given mbox channel */
+static int get_mbox_id(u16 channel)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(channel_mappings); i++) {
+		if ((channel >= channel_mappings[i].chan_base) &&
+		    (channel < channel_mappings[i].chan_base +
+		    CHANNELS_PER_MBOX_UNIT)) {
+			return channel_mappings[i].mbox_id;
+		}
+	}
+	/* There is no mbox unit registered for given channel */
+	return -EINVAL;
+}
+
+/* Returns mbox structure saved after mbox_register() call */
+static struct mbox *get_mbox(u16 channel)
+{
+	int i;
+	int mbox_id = get_mbox_id(channel);
+
+	if (mbox_id < 0) {
+		dev_err(&channels.pdev->dev, "couldn't get mbox id\n");
+		return NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(channels.mbox_unit); i++) {
+		if (channels.mbox_unit[i].mbox_id == mbox_id)
+			return channels.mbox_unit[i].mbox;
+	}
+	return NULL;
+}
+
+/* Returns pointer to rx mbox channels list for given mbox unit */
+static struct list_head *get_rx_list(u8 mbox_id)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mbox_ids); i++) {
+		if (channels.mbox_unit[i].mbox_id == mbox_id)
+			return &channels.mbox_unit[i].rx_chans;
+	}
+	return NULL;
+}
+
+/* Returns pointer to tx mbox channels list for given mbox unit */
+static struct list_head *get_tx_list(u8 mbox_id)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(mbox_ids); i++) {
+		if (channels.mbox_unit[i].mbox_id == mbox_id)
+			return &channels.mbox_unit[i].tx_chans;
+	}
+	return NULL;
+}
+
+static int send_pdu(struct channel_status *chan_status, int command,
+		u16 channel)
+{
+	struct mbox *mbox;
+	u32 header = 0;
+	int ret = 0;
+	/* SEND PDU is not supported */
+	if (command == MBOX_SEND) {
+		dev_err(&channels.pdev->dev, "SEND command not implemented\n");
+		ret = -EINVAL;
+		goto exit;
+	}
+	mbox = get_mbox(chan_status->channel);
+	if (mbox == NULL) {
+		dev_err(&channels.pdev->dev, "couldn't get mailbox\n");
+		ret = -ENOSYS;
+		goto exit;
+	}
+	/* For CAST type send all pending messages */
+	if (command == MBOX_CAST) {
+		struct list_head *pos, *n;
+
+		/* Send all pending messages from TX channel */
+		list_for_each_safe(pos, n, &chan_status->tx.pending) {
+			struct pending_elem *pending =
+				list_entry(pos, struct pending_elem, list);
+			int i;
+
+			header = BUILD_HEADER(channel,
+					pending->length,
+					command,
+					chan_status->seq_number);
+
+			ret = mbox_send(mbox, header, true);
+			if (ret < 0) {
+				dev_err(&channels.pdev->dev,
+					"failed to send header, err=%d\n", ret);
+				goto exit;
+			}
+
+			for (i = 0; i < pending->length; i++) {
+				ret = mbox_send(mbox, pending->data[i], true);
+				if (ret < 0) {
+					dev_err(&channels.pdev->dev,
+					"failed to send header, err=%d\n", ret);
+					goto exit;
+				}
+			}
+
+			/* Call client's callback that data is already sent */
+			if (chan_status->cb)
+				chan_status->cb(pending->data, pending->length,
+						chan_status->priv);
+			else
+				dev_err(&channels.pdev->dev,
+					"%s no callback provided:header 0x%x\n",
+					__func__, header);
+
+			/* Increment sequence number */
+			chan_status->seq_number++;
+
+			/* Remove and free element from the list */
+			list_del(&pending->list);
+			kfree(pending);
+		}
+	} else {
+		header = BUILD_HEADER(channel, 0,
+				command, chan_status->seq_number);
+
+		ret = mbox_send(mbox, header, true);
+		if (ret < 0)
+			dev_err(&channels.pdev->dev, "failed to send header\n");
+		/* Increment sequence number */
+		chan_status->seq_number++;
+	}
+
+exit:
+	return ret;
+}
+
+void mbox_handle_receive_msg(struct work_struct *work)
+{
+	struct channel_status *rx_chan = container_of(work,
+			struct channel_status,
+			receive_msg);
+
+	if (!atomic_read(&rx_chan->rcv_counter))
+		return;
+rcv_msg:
+	/* Call client's callback and reset state */
+	if (rx_chan->cb) {
+		static int rx_pending_count;
+		rx_chan->cb(rx_pending[rx_pending_count].buffer,
+				rx_pending[rx_pending_count].length,
+				rx_pending[rx_pending_count].priv);
+		rx_pending_count++;
+		if (rx_pending_count == NUM_DSP_BUFFER)
+			rx_pending_count = 0;
+	} else {
+		dev_err(&channels.pdev->dev,
+				"%s no callback provided\n", __func__);
+	}
+	if (atomic_dec_return(&rx_chan->rcv_counter) > 0)
+		goto rcv_msg;
+
+}
+
+void mbox_handle_open_msg(struct work_struct *work)
+{
+	struct channel_status *tx_chan = container_of(work,
+							struct channel_status,
+							 open_msg);
+	/* Change channel state to OPEN */
+	tx_chan->state = MBOX_OPEN;
+	/* If pending list not empty, start sending data */
+	mutex_lock(&tx_chan->lock);
+	if (!list_empty(&tx_chan->tx.pending))
+		send_pdu(tx_chan, MBOX_CAST, tx_chan->channel);
+	mutex_unlock(&tx_chan->lock);
+}
+
+void mbox_handle_cast_msg(struct work_struct *work)
+{
+	struct channel_status *rx_chan = container_of(work,
+							struct channel_status,
+							 cast_msg);
+	/* Check if channel is opened */
+	if (rx_chan->state == MBOX_CLOSE) {
+		/* Peer sent message to closed channel */
+		dev_err(&channels.pdev->dev,
+			"channel in wrong state\n");
+	}
+}
+
+static bool handle_receive_msg(u32 mbox_msg, struct channel_status *rx_chan)
+{
+	int i;
+	static int rx_pending_count;
+
+	if (rx_chan) {
+		/* Store received data in RX channel buffer */
+		rx_chan->rx.buffer[rx_chan->rx.index++] = mbox_msg;
+
+		/* Check if it's last data of PDU */
+		if (rx_chan->rx.index == rx_chan->rx.length) {
+			for (i = 0; i < MAILBOX_NR_OF_DATAWORDS; i++) {
+				rx_pending[rx_pending_count].buffer[i] =
+					rx_chan->rx.buffer[i];
+			}
+
+			rx_pending[rx_pending_count].length =
+				rx_chan->rx.length;
+			rx_pending[rx_pending_count].priv = rx_chan->priv;
+			rx_chan->rx.index = 0;
+			rx_chan->rx.length = 0;
+			rx_chan->state = MBOX_OPEN;
+			rx_chan->seq_number++;
+			rx_pending_count++;
+			if (rx_pending_count == NUM_DSP_BUFFER)
+				rx_pending_count = 0;
+			atomic_inc(&rx_chan->rcv_counter);
+			queue_work(rx_chan->receive_wq,
+			&rx_chan->receive_msg);
+		}
+		dev_dbg(&channels.pdev->dev, "%s OK\n", __func__);
+
+		return true;
+	}
+	return false;
+}
+
+static void handle_open_msg(u16 channel, u8 mbox_id)
+{
+	struct list_head *tx_list, *pos;
+	struct channel_status *tmp;
+	struct channel_status *tx_chan = NULL;
+	struct mbox_unit_status *mbox_unit;
+	channel = get_tx_channel(channel);
+	dev_dbg(&channels.pdev->dev, "%s mbox_id %d\tchannel %x\n",
+			__func__, mbox_id, channel);
+	/* Get TX channel for given mbox unit */
+	tx_list = get_tx_list(mbox_id);
+	if (tx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid %d\n",
+			mbox_id);
+		return;
+	}
+	mbox_unit = container_of(tx_list, struct mbox_unit_status, tx_chans);
+	/* Search for channel in tx list */
+	spin_lock(&mbox_unit->tx_lock);
+	list_for_each(pos, tx_list) {
+		tmp = list_entry(pos, struct channel_status, list);
+		dev_dbg(&channels.pdev->dev, "tmp->channel=%d\n",
+				tmp->channel);
+		if (tmp->channel == channel)
+			tx_chan = tmp;
+	}
+	spin_unlock(&mbox_unit->tx_lock);
+	if (tx_chan) {
+		schedule_work(&tx_chan->open_msg);
+	} else {
+		/* No tx channel found on the list, allocate new element */
+		tx_chan = kzalloc(sizeof(*tx_chan), GFP_ATOMIC);
+		if (tx_chan == NULL) {
+			dev_err(&channels.pdev->dev,
+				"failed to allocate memory\n");
+			return;
+		}
+
+		/* Fill initial data and add this element to tx list */
+		tx_chan->channel = get_tx_channel(channel);
+		tx_chan->state = MBOX_OPEN;
+		tx_chan->seq_number = CHANNEL_START_SEQUENCE_NUMBER;
+		INIT_LIST_HEAD(&tx_chan->tx.pending);
+		INIT_WORK(&tx_chan->open_msg, mbox_handle_open_msg);
+		INIT_WORK(&tx_chan->cast_msg, mbox_handle_cast_msg);
+		INIT_WORK(&tx_chan->receive_msg, mbox_handle_receive_msg);
+		mutex_init(&tx_chan->lock);
+		spin_lock(&mbox_unit->tx_lock);
+		list_add_tail(&tx_chan->list, tx_list);
+		spin_unlock(&mbox_unit->tx_lock);
+	}
+}
+
+static void handle_cast_msg(u16 channel, struct channel_status *rx_chan,
+						u32 mbox_msg, bool send)
+{
+	dev_dbg(&channels.pdev->dev, " %s\n", __func__);
+	if (rx_chan) {
+		rx_chan->rx.buffer[0] = mbox_msg;
+		rx_chan->with_ack = send;
+		rx_chan->rx.length = GET_LENGTH(rx_chan->rx.buffer[0]);
+		if (rx_chan->rx.length <= MAILBOX_NR_OF_DATAWORDS &&
+				rx_chan->rx.length > 0) {
+			rx_chan->rx.index = 0;
+			rx_chan->state = MBOX_CAST;
+		}
+		queue_work(rx_chan->cast_wq,
+			&rx_chan->cast_msg);
+	} else {
+		/* Channel not found, peer sent wrong message */
+		dev_err(&channels.pdev->dev, "channel %d doesn't exist\n",
+			channel);
+	}
+}
+
+/*
+ * This callback is called whenever mbox unit receives data.
+ * priv parameter holds mbox unit id.
+ */
+static void mbox_cb(u32 mbox_msg, void *priv)
+{
+	u8 mbox_id = *(u8 *)priv;
+	struct list_head *rx_list;
+	u8 type = GET_TYPE(mbox_msg);
+	u16 channel = GET_CHANNEL(mbox_msg);
+	struct mbox_unit_status *mbox_unit;
+	struct list_head *pos;
+	struct channel_status *tmp;
+	struct channel_status *rx_chan = NULL;
+	bool is_Payload = 0;
+
+	dev_dbg(&channels.pdev->dev, "%s type %d\t, mbox_msg %x\n",
+		       __func__, type, mbox_msg);
+
+	/* Get RX channels list for given mbox unit */
+	rx_list = get_rx_list(mbox_id);
+	if (rx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid %d\n",
+				mbox_id);
+		return;
+	}
+
+	mbox_unit = container_of(rx_list, struct mbox_unit_status, rx_chans);
+	/* Search for channel in rx list */
+	spin_lock(&mbox_unit->rx_lock);
+	list_for_each(pos, rx_list) {
+		tmp = list_entry(pos, struct channel_status, list);
+		if (tmp->state == MBOX_SEND ||
+				tmp->state == MBOX_CAST) {
+			/* Received message is payload */
+			is_Payload = 1;
+			rx_chan = tmp;
+		} else
+		if (tmp->channel == channel)
+			rx_chan = tmp;
+	}
+	spin_unlock(&mbox_unit->rx_lock);
+	/* if callback is present for that RX channel */
+	if (rx_chan && rx_chan->cb) {
+		/* If received message is payload this
+		 * function will take care of it
+		 */
+		if ((is_Payload) && (handle_receive_msg(mbox_msg, rx_chan)))
+			return;
+	} else
+		dev_err(&channels.pdev->dev, "callback not present:msg 0x%x "
+				"rx_chan 0x%x\n", mbox_msg, (u32)rx_chan);
+
+	/* Received message is header as no RX channel is in SEND/CAST state */
+	switch (type) {
+	case MBOX_CLOSE:
+		/* Not implemented */
+		break;
+	case MBOX_OPEN:
+		handle_open_msg(channel, mbox_id);
+		break;
+	case MBOX_SEND:
+		/* if callback is present for that RX channel */
+		if (rx_chan && rx_chan->cb)
+			handle_cast_msg(channel, rx_chan, mbox_msg, true);
+		break;
+	case MBOX_CAST:
+		/* if callback is present for that RX channel */
+		if (rx_chan && rx_chan->cb)
+			handle_cast_msg(channel, rx_chan, mbox_msg, false);
+		break;
+	case MBOX_ACK:
+	case MBOX_NAK:
+		/* Not implemented */
+		break;
+	}
+}
+
+/**
+ * mbox_channel_register() - Registers for a channel
+ * @channel:	Channel Number.
+ * @cb:		Pointer to function pointer mbox_channel_cb_t
+ * @priv:	Pointer to private data
+ *
+ * This routine is used to register for a logical channel.
+ * It first does sanity check on the requested channel availability
+ * and parameters. Then it prepares internal entry for the channel.
+ * And send a OPEN request for that channel.
+ */
+int mbox_channel_register(u16 channel, mbox_channel_cb_t *cb, void *priv)
+{
+	struct channel_status *rx_chan;
+	struct list_head *pos, *rx_list;
+	int res = 0;
+	struct mbox_unit_status *mbox_unit;
+
+	dev_dbg(&channels.pdev->dev, " %s channel = %d\n", __func__, channel);
+	/* Check for callback fcn */
+	if (cb == NULL) {
+		dev_err(&channels.pdev->dev,
+			"channel callback missing:channel %d\n", channel);
+		res = -EINVAL;
+		goto exit;
+	}
+
+	/* Check if provided channel number is valid */
+	if (!check_channel(channel, MBOX_RX)) {
+		dev_err(&channels.pdev->dev, "wrong mbox channel number %d\n",
+			channel);
+		res = -EINVAL;
+		goto exit;
+	}
+
+	rx_list = get_rx_list(get_mbox_id(channel));
+	if (rx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid\n");
+		res = -EINVAL;
+		goto exit;
+	}
+
+	mbox_unit = container_of(rx_list, struct mbox_unit_status, rx_chans);
+
+	/* Check if channel is already registered */
+	spin_lock(&mbox_unit->rx_lock);
+	list_for_each(pos, rx_list) {
+		rx_chan = list_entry(pos, struct channel_status, list);
+
+		if (rx_chan->channel == channel) {
+			dev_dbg(&channels.pdev->dev,
+				"channel already registered\n");
+			rx_chan->cb = cb;
+			rx_chan->priv = priv;
+			spin_unlock(&mbox_unit->rx_lock);
+			goto exit;
+		}
+	}
+	spin_unlock(&mbox_unit->rx_lock);
+
+	rx_chan = kzalloc(sizeof(*rx_chan), GFP_KERNEL);
+	if (rx_chan == NULL) {
+		dev_err(&channels.pdev->dev,
+			"couldn't allocate channel status\n");
+		res = -ENOMEM;
+		goto exit;
+	}
+
+	atomic_set(&rx_chan->rcv_counter, 0);
+	/* Fill out newly allocated element and add it to rx list */
+	rx_chan->channel = channel;
+	rx_chan->cb = cb;
+	rx_chan->priv = priv;
+	rx_chan->seq_number = CHANNEL_START_SEQUENCE_NUMBER;
+	mutex_init(&rx_chan->lock);
+	INIT_LIST_HEAD(&rx_chan->rx.pending);
+	rx_chan->cast_wq = create_singlethread_workqueue("mbox_cast_msg");
+	if (!rx_chan->cast_wq) {
+		dev_err(&channels.pdev->dev, "failed to create work queue\n");
+		res = -ENOMEM;
+		goto error_cast_wq;
+	}
+	rx_chan->receive_wq = create_singlethread_workqueue("mbox_receive_msg");
+	if (!rx_chan->receive_wq) {
+		dev_err(&channels.pdev->dev, "failed to create work queue\n");
+		res = -ENOMEM;
+		goto error_recv_wq;
+	}
+	INIT_WORK(&rx_chan->open_msg, mbox_handle_open_msg);
+	INIT_WORK(&rx_chan->cast_msg, mbox_handle_cast_msg);
+	INIT_WORK(&rx_chan->receive_msg, mbox_handle_receive_msg);
+	spin_lock(&mbox_unit->rx_lock);
+	list_add_tail(&rx_chan->list, rx_list);
+	spin_unlock(&mbox_unit->rx_lock);
+
+	mutex_lock(&rx_chan->lock);
+	res = send_pdu(rx_chan, MBOX_OPEN, get_tx_channel(rx_chan->channel));
+	if (res) {
+		dev_err(&channels.pdev->dev, "failed to send OPEN command\n");
+		spin_lock(&mbox_unit->rx_lock);
+		list_del(&rx_chan->list);
+		spin_unlock(&mbox_unit->rx_lock);
+		mutex_unlock(&rx_chan->lock);
+		goto error_send_pdu;
+	} else {
+		rx_chan->seq_number++;
+		rx_chan->state = MBOX_OPEN;
+		mutex_unlock(&rx_chan->lock);
+		return res;
+	}
+error_send_pdu:
+	flush_workqueue(rx_chan->receive_wq);
+error_recv_wq:
+	flush_workqueue(rx_chan->cast_wq);
+error_cast_wq:
+	kfree(rx_chan);
+exit:
+	return res;
+}
+EXPORT_SYMBOL(mbox_channel_register);
+
+/**
+ * mbox_channel_deregister() - DeRegisters for a channel
+ * @channel:	Channel Number.
+ *
+ * This routine is used to deregister for a logical channel.
+ * It first does sanity check on the requested channel availability
+ * and parameters. Then it deletes the channel
+ */
+int mbox_channel_deregister(u16 channel)
+{
+	struct channel_status *rx_chan = NULL;
+	struct list_head *pos, *rx_list;
+	int res = 0;
+	struct mbox_unit_status *mbox_unit;
+
+	dev_dbg(&channels.pdev->dev, " %s channel = %d\n", __func__, channel);
+	/* Check if provided channel number is valid */
+	if (!check_channel(channel, MBOX_RX)) {
+		dev_err(&channels.pdev->dev, "wrong mbox channel number %d\n",
+			channel);
+		res = -EINVAL;
+		goto exit;
+	}
+
+	rx_list = get_rx_list(get_mbox_id(channel));
+	if (rx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid\n");
+		res = -EINVAL;
+		goto exit;
+	}
+
+	mbox_unit = container_of(rx_list, struct mbox_unit_status, rx_chans);
+
+	/* Check if channel is already registered */
+	spin_lock(&mbox_unit->rx_lock);
+	list_for_each(pos, rx_list) {
+		rx_chan = list_entry(pos, struct channel_status, list);
+
+		if (rx_chan->channel == channel) {
+			dev_dbg(&channels.pdev->dev,
+				"channel found\n");
+			rx_chan->cb = NULL;
+		}
+	}
+	list_del(&rx_chan->list);
+	spin_unlock(&mbox_unit->rx_lock);
+	flush_workqueue(rx_chan->cast_wq);
+	flush_workqueue(rx_chan->receive_wq);
+	kfree(rx_chan);
+
+exit:
+	return res;
+}
+EXPORT_SYMBOL(mbox_channel_deregister);
+
+/**
+ * mbox_channel_send() - Send messages
+ * @msg:	Pointer to mbox_channel_msg data structure.
+ *
+ * This routine is used to send messages over the registered logical
+ * TX channel. It first does sanity check on the message paramenters.
+ * It registered channel is not found then it just registers for that
+ * channel. If channel found, it puts the message to the pending list.
+ * If channel is OPEN, it then pushes the message to the mailbox in
+ * FIFO manner from the pending list.
+ */
+int mbox_channel_send(struct mbox_channel_msg *msg)
+{
+	struct list_head *pos, *tx_list;
+	struct channel_status *tmp = NULL;
+	struct channel_status *tx_chan = NULL;
+	struct pending_elem *pending;
+	struct mbox_unit_status *mbox_unit;
+	int res = 0;
+
+	if (msg->length > MAILBOX_NR_OF_DATAWORDS || msg->length == 0) {
+		dev_err(&channels.pdev->dev, "data length incorrect\n");
+		res = -EINVAL;
+		goto exit;
+	}
+
+	if (!check_channel(msg->channel, MBOX_TX)) {
+		dev_err(&channels.pdev->dev, "wrong channel number %d\n",
+			msg->channel);
+		res = -EINVAL;
+		goto exit;
+	}
+
+	tx_list = get_tx_list(get_mbox_id(msg->channel));
+	if (tx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid\n");
+		res = -EINVAL;
+		goto exit;
+	}
+
+	mbox_unit = container_of(tx_list, struct mbox_unit_status, tx_chans);
+
+	spin_lock(&mbox_unit->tx_lock);
+	dev_dbg(&channels.pdev->dev, "send:tx_list=%x\tmbox_unit=%x\n",
+			(u32)tx_list, (u32)mbox_unit);
+	list_for_each(pos, tx_list) {
+		tmp = list_entry(pos, struct channel_status, list);
+		if (tmp->channel == msg->channel)
+			tx_chan = tmp;
+	}
+	spin_unlock(&mbox_unit->tx_lock);
+	/* Allocate pending element and add it to the list */
+	pending = kzalloc(sizeof(*pending), GFP_KERNEL);
+	if (pending == NULL) {
+		dev_err(&channels.pdev->dev,
+			"couldn't allocate memory for pending\n");
+		res = -ENOMEM;
+		goto exit;
+	}
+	pending->data = msg->data;
+	pending->length = msg->length;
+
+	if (tx_chan) {
+		mutex_lock(&tx_chan->lock);
+		list_add_tail(&pending->list, &tx_chan->tx.pending);
+		tx_chan->cb = msg->cb;
+		tx_chan->priv = msg->priv;
+		/* If channel is already opened start sending data */
+		if (tx_chan->state == MBOX_OPEN)
+			send_pdu(tx_chan, MBOX_CAST, tx_chan->channel);
+		/* Stop processing here */
+		mutex_unlock(&tx_chan->lock);
+	} else {
+		/* No channel found on the list, allocate new element */
+		tx_chan = kzalloc(sizeof(*tx_chan), GFP_KERNEL);
+		if (tx_chan == NULL) {
+			dev_err(&channels.pdev->dev,
+					"couldn't allocate memory for \
+					tx_chan\n");
+			res = -ENOMEM;
+			goto exit;
+		}
+		tx_chan->channel = msg->channel;
+		tx_chan->cb = msg->cb;
+		tx_chan->priv = msg->priv;
+		tx_chan->state = MBOX_CLOSE;
+		tx_chan->seq_number = CHANNEL_START_SEQUENCE_NUMBER;
+		INIT_LIST_HEAD(&tx_chan->tx.pending);
+		INIT_WORK(&tx_chan->open_msg, mbox_handle_open_msg);
+		INIT_WORK(&tx_chan->cast_msg, mbox_handle_cast_msg);
+		INIT_WORK(&tx_chan->receive_msg, mbox_handle_receive_msg);
+		mutex_init(&tx_chan->lock);
+		spin_lock(&mbox_unit->tx_lock);
+		list_add_tail(&tx_chan->list, tx_list);
+		spin_unlock(&mbox_unit->tx_lock);
+		mutex_lock(&tx_chan->lock);
+		list_add_tail(&pending->list, &tx_chan->tx.pending);
+		mutex_unlock(&tx_chan->lock);
+	}
+	return 0;
+
+exit:
+	return res;
+}
+EXPORT_SYMBOL(mbox_channel_send);
+
+static void revoke_pending_msgs(struct channel_status *tx_chan)
+{
+	struct list_head *pos, *n;
+	struct pending_elem *pending;
+
+	list_for_each_safe(pos, n, &tx_chan->tx.pending) {
+		pending = list_entry(pos, struct pending_elem, list);
+
+		if (tx_chan->cb)
+			tx_chan->cb(pending->data, pending->length,
+							tx_chan->priv);
+		else
+			dev_err(&channels.pdev->dev,
+				"%s no callback provided\n", __func__);
+		list_del(&pending->list);
+		kfree(pending);
+	}
+}
+
+/**
+ * mbox_channel_revoke_messages() - Revoke pending messages
+ * @channel:	Channel on which action to be taken.
+ *
+ * This routine Clear all pending messages from TX channel
+ * It searches for the channel.Checks if there is pending
+ * messages.Calls if tehre is any registered function. And
+ * deletes the messages for the pending list.
+ */
+int mbox_channel_revoke_messages(u16 channel)
+{
+	struct list_head *pos, *tx_list;
+	struct channel_status *tmp;
+	struct channel_status *tx_chan = NULL;
+	struct mbox_unit_status *mbox_unit;
+	int res = 0;
+
+	if (!check_channel(channel, MBOX_TX)) {
+		dev_err(&channels.pdev->dev,
+			"wrong channel number %d\n", channel);
+		return -EINVAL;
+	}
+
+	tx_list = get_tx_list(get_mbox_id(channel));
+	if (tx_list == NULL) {
+		dev_err(&channels.pdev->dev, "given mbox id is not valid\n");
+		return -EINVAL;
+	}
+
+	mbox_unit = container_of(tx_list, struct mbox_unit_status, tx_chans);
+
+	spin_lock(&mbox_unit->tx_lock);
+	list_for_each(pos, tx_list) {
+		tmp = list_entry(pos, struct channel_status, list);
+		if (tmp->channel == channel)
+			tx_chan = tmp;
+	}
+	spin_unlock(&mbox_unit->tx_lock);
+
+	if (tx_chan) {
+		mutex_lock(&tx_chan->lock);
+		revoke_pending_msgs(tx_chan);
+		mutex_unlock(&tx_chan->lock);
+		dev_dbg(&channels.pdev->dev, "channel %d cleared\n",
+			channel);
+	} else {
+		dev_err(&channels.pdev->dev, "no channel found\n");
+		res = -EINVAL;
+	}
+
+	dev_dbg(&channels.pdev->dev, "%s exiting %d\n", __func__, res);
+	return res;
+}
+EXPORT_SYMBOL(mbox_channel_revoke_messages);
+
+#if defined(CONFIG_DEBUG_FS)
+#define MBOXTEST_DEBUG 1
+#ifdef MBOXTEST_DEBUG
+#define DBG_TEST(x)  x
+#else
+#define DBG_TEST(x)
+#endif
+
+#define MBOX_TEST_MAX_WORDS	3
+#define MBOX_RX_CHAN		0x500
+#define MBOX_TX_RX_CHANNEL_DIFF 0x400
+#define MBOX_MAX_NUM_TRANSFER	30000
+static int registration_done;
+/**
+ * struct mboxtest_data - mbox test via debugfs information
+ * @rx_buff:		Buffer for incomming data
+ * @rx_pointer:		Ptr to actual RX data buff
+ * @tx_buff:		Buffer for outgoing data
+ * @tx_pointer:		Ptr to actual TX data buff
+ * @tx_done:		TX Transfer done indicator
+ * @rx_done:		RX Transfer done indicator
+ * @received:		Received words
+ * @xfer_words:		Num of bytes in actual trf
+ * @xfers:		Number of transfers
+ * @words:		Number of total words
+ * @channel:		Channel test number
+ */
+struct mboxtest_data {
+	unsigned int		*rx_buff;
+	unsigned int		*rx_pointer;
+	unsigned int		*tx_buff;
+	unsigned int		*tx_pointer;
+	struct completion	tx_done;
+	struct completion	rx_done;
+	int			received;
+	int			xfer_words;
+	int			xfers;
+	int			words;
+	int			channel;
+};
+
+static void mboxtest_receive_cb(u32 *data, u32 len, void *arg)
+{
+	struct mboxtest_data *mboxtest = (struct mboxtest_data *) arg;
+	int i;
+
+	printk(KERN_INFO "receive_cb.. data.= 0x%X, len = %d\n",
+								*data, len);
+	for (i = 0; i < len; i++)
+		*(mboxtest->rx_pointer++) = *(data++);
+
+	mboxtest->received += len;
+
+	printk(KERN_INFO "received = %d, words = %d\n",
+				mboxtest->received, mboxtest->words);
+	if (mboxtest->received >= mboxtest->words)
+		complete(&mboxtest->rx_done);
+	dev_dbg(&channels.pdev->dev, "%s exiting\n", __func__);
+}
+
+static void mboxtest_send_cb(u32 *data, u32 len, void *arg)
+{
+	struct mboxtest_data *mboxtest = (struct mboxtest_data *) arg;
+
+	printk(KERN_INFO "send_cb.. data.= 0x%X, len = %d\n",
+							*data, len);
+
+	complete(&mboxtest->tx_done);
+	dev_dbg(&channels.pdev->dev, "kernel:mboxtest_send_cb exiting\n");
+}
+
+static int mboxtest_transmit(struct mboxtest_data *mboxtest)
+{
+	int status = 0;
+	struct mbox_channel_msg msg;
+
+	dev_dbg(&channels.pdev->dev, "%s entering\n", __func__);
+	init_completion(&mboxtest->tx_done);
+
+	msg.channel = mboxtest->channel;
+	msg.data = mboxtest->tx_pointer;
+	msg.length = mboxtest->words;
+	msg.cb = mboxtest_send_cb;
+	msg.priv = mboxtest;
+
+	status = mbox_channel_send(&msg);
+	if (!status) {
+		mboxtest->tx_pointer += mboxtest->xfer_words;
+		wait_for_completion(&mboxtest->tx_done);
+	}
+
+	dev_dbg(&channels.pdev->dev, "%s exiting %d\n",
+			__func__, status);
+	return status;
+}
+
+static int transfer_test(struct mboxtest_data *mboxtest)
+{
+	int status = 0;
+	int len = 0;
+	int i;
+
+	len = mboxtest->words;
+
+	dev_dbg(&channels.pdev->dev, "%s enterring\n", __func__);
+	/* Allocate buffers */
+	mboxtest->rx_buff = kzalloc(sizeof(unsigned int) * len, GFP_KERNEL);
+	if (!mboxtest->rx_buff) {
+		DBG_TEST(printk(KERN_INFO
+			"Cannot allocate mbox rx memory\n"));
+		status = -ENOMEM;
+		goto err1;
+	}
+	memset(mboxtest->rx_buff, '\0', sizeof(unsigned int) * len);
+
+	mboxtest->tx_buff = kzalloc(sizeof(unsigned int) * len, GFP_KERNEL);
+	if (!mboxtest->tx_buff) {
+		DBG_TEST(printk(KERN_INFO
+			"Cannot allocate mbox tx memory\n"));
+		status = -ENOMEM;
+		goto err2;
+	}
+	memset(mboxtest->tx_buff, '\0', sizeof(unsigned int) * len);
+
+	/* Generate data */
+		get_random_bytes((unsigned char *)mboxtest->tx_buff,
+				sizeof(unsigned int) * len);
+	/* Set pointers */
+	mboxtest->tx_pointer = mboxtest->tx_buff;
+	mboxtest->rx_pointer = mboxtest->rx_buff;
+	mboxtest->received = 0;
+	init_completion(&mboxtest->rx_done);
+
+	/* Start tx transfer test transfer */
+	status = mboxtest_transmit(mboxtest);
+	DBG_TEST(printk(KERN_INFO "xfer_words=%d\n",
+				mboxtest->xfer_words));
+	if (!status)
+		wait_for_completion(&mboxtest->rx_done);
+	for (i = 0; i < len; i++)
+		DBG_TEST(printk(KERN_INFO "%d -> TX:0x%X, RX:0x%X\n", i,
+			mboxtest->tx_buff[i], mboxtest->rx_buff[i]));
+
+	dev_dbg(&channels.pdev->dev, "%s exiting %d\n", __func__, status);
+	return status;
+err2:
+	kfree(mboxtest->rx_buff);
+err1:
+	return status;
+}
+
+static int mboxtest_prepare(struct mboxtest_data *mboxtest)
+{
+	int err = 0;
+
+	mboxtest->xfers = MBOX_MAX_NUM_TRANSFER;
+	/* Calculate number of bytes in each transfer */
+	mboxtest->xfer_words = mboxtest->words / mboxtest->xfers;
+
+	/* Trim to maxiumum data words per transfer */
+	if (mboxtest->xfer_words > MBOX_TEST_MAX_WORDS) {
+		DBG_TEST(printk(KERN_INFO "Recalculating xfers ...\n"));
+		mboxtest->xfer_words = MBOX_TEST_MAX_WORDS;
+		if (mboxtest->words % mboxtest->xfer_words)
+			mboxtest->xfers = (mboxtest->words /
+						mboxtest->xfer_words) + 1;
+		else
+			mboxtest->xfers = (mboxtest->words /
+						mboxtest->xfer_words);
+	}
+
+	DBG_TEST(printk(KERN_INFO "Params: chan=0x%X words=%d, xfers=%d\n",
+			mboxtest->channel, mboxtest->words,
+						mboxtest->xfers));
+
+	if (mbox_channel_register(mboxtest->channel,
+			mboxtest_receive_cb, mboxtest)) {
+		DBG_TEST(printk(KERN_INFO "Cannot register mbox channel\n"));
+		err = -ENOMEM;
+		goto err;
+	}
+
+	registration_done = true;
+	return 0;
+err:
+	return err;
+}
+
+struct mboxtest_data mboxtest;
+/*
+ * Expected input: <nbr_channel> <nbr_word>
+ * Example: "echo 500 2"
+ */
+static ssize_t mbox_write_channel(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf,
+			       size_t count)
+{
+	unsigned long nbr_channel;
+	unsigned long nbr_word;
+	char int_buf[16];
+	char *token;
+	char *val;
+
+	strncpy((char *) &int_buf, buf, sizeof(int_buf));
+	token = (char *) &int_buf;
+
+	/* Parse message */
+	val = strsep(&token, " ");
+	if ((val == NULL) || (strict_strtoul(val, 16, &nbr_channel) != 0))
+		nbr_channel = MBOX_RX_CHAN;
+
+	val = strsep(&token, " ");
+	if ((val == NULL) || (strict_strtoul(val, 16, &nbr_word) != 0))
+		nbr_word = 2;
+
+	dev_dbg(dev, "Will setup logical channel %ld\n", nbr_channel);
+	mboxtest.channel = nbr_channel;
+	mboxtest.words = nbr_word;
+
+	if (!registration_done)
+		mboxtest_prepare(&mboxtest);
+	else
+		dev_dbg(&channels.pdev->dev, "already registration done\n");
+
+	return count;
+}
+
+static ssize_t mbox_read_channel(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+
+	unsigned long i;
+	static bool config_done;
+
+	if (!config_done) {
+		config_done = true;
+		mboxtest.channel += MBOX_TX_RX_CHANNEL_DIFF;
+	}
+	dev_dbg(dev, "Will transfer %d words %d times at channel 0x%x\n",
+			mboxtest.words, mboxtest.xfers, mboxtest.channel);
+	for (i = 0; i < mboxtest.xfers; i++)
+		transfer_test(&mboxtest);
+
+	return 1;
+}
+static DEVICE_ATTR(channel, S_IWUGO | S_IRUGO, mbox_read_channel,
+		mbox_write_channel);
+
+#endif
+
+static int __init mbox_channel_probe(struct platform_device *pdev)
+{
+	int i, ret = 0;
+	struct mbox *mbox;
+
+	dev_dbg(&(pdev->dev), "Probing mailbox (pdev = 0x%X)...\n", (u32)pdev);
+
+	/* Register to given mailbox units (ids) */
+	for (i = 0; i < ARRAY_SIZE(mbox_ids); i++) {
+		mbox = mbox_setup(mbox_ids[i], mbox_cb, &mbox_ids[i]);
+		if (mbox == NULL) {
+			dev_err(&(pdev->dev), "Unable to setup mailbox %d\n",
+				mbox_ids[i]);
+			ret = -EBUSY;
+			goto exit;
+		}
+		channels.mbox_unit[i].mbox_id = mbox_ids[i];
+		channels.mbox_unit[i].mbox = mbox;
+		INIT_LIST_HEAD(&channels.mbox_unit[i].rx_chans);
+		INIT_LIST_HEAD(&channels.mbox_unit[i].tx_chans);
+		spin_lock_init(&channels.mbox_unit[i].rx_lock);
+		spin_lock_init(&channels.mbox_unit[i].tx_lock);
+	}
+
+	channels.pdev = pdev;
+
+	dev_dbg(&(pdev->dev), "Mailbox channel driver loaded\n");
+#if defined(CONFIG_DEBUG_FS)
+	ret = device_create_file(&(pdev->dev), &dev_attr_channel);
+	if (ret != 0)
+		dev_warn(&(pdev->dev),
+			 "Unable to create mbox_channel sysfs entry");
+
+
+#endif
+exit:
+	return ret;
+}
+
+static struct platform_driver mbox_channel_driver = {
+	.driver = {
+		.name = "mbox_channel",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init mbox_channel_init(void)
+{
+	if (!machine_is_u5500())
+		return 0;
+
+	platform_device_register_simple("mbox_channel", 0, NULL, 0);
+
+	return platform_driver_probe(&mbox_channel_driver, mbox_channel_probe);
+}
+module_init(mbox_channel_init);
+
+static void __exit mbox_channel_exit(void)
+{
+	platform_driver_unregister(&mbox_channel_driver);
+}
+module_exit(mbox_channel_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MBOX channels driver");
diff --git a/drivers/misc/modem_audio/Kconfig b/drivers/misc/modem_audio/Kconfig
new file mode 100644
index 00000000000..5396868a9de
--- /dev/null
+++ b/drivers/misc/modem_audio/Kconfig
@@ -0,0 +1,6 @@
+config MODEM_AUDIO_DRIVER
+	bool "Modem Audio Driver"
+	depends on (U5500_MBOX && UX500_SOC_DB5500)
+	help
+	  This module is used for read and write data between APE and
+	  Access side in u5500  platform.
diff --git a/drivers/misc/modem_audio/Makefile b/drivers/misc/modem_audio/Makefile
new file mode 100644
index 00000000000..a5c1740ea48
--- /dev/null
+++ b/drivers/misc/modem_audio/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_MODEM_AUDIO_DRIVER)  += mad.o
+
diff --git a/drivers/misc/modem_audio/mad.c b/drivers/misc/modem_audio/mad.c
new file mode 100644
index 00000000000..d31d78ba3f2
--- /dev/null
+++ b/drivers/misc/modem_audio/mad.c
@@ -0,0 +1,506 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2011
+ *
+ * Modem Audio Driver
+ *
+ * Author:Rahul Venkatram <rahul.venkatram@stericsson.com> for ST-Ericsson
+ *	 Haridhar KALVALA<haridhar.kalvala@stericsson.com> for ST-Ericsson
+ *	Amaresh Mulage<amaresh.mulage@stericsson.com> for ST-Ericsson.
+ *
+ * License terms:GNU General Public License (GPLv2)version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/sched.h>
+#include <linux/poll.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/fcntl.h>
+#include <linux/spinlock.h>
+#include <mach/mbox_channels-db5500.h>
+
+MODULE_DESCRIPTION("Modem Audio Driver");
+MODULE_LICENSE("GPLv2");
+
+/**
+ * -----------------------------------------------------
+ * |		|		|			|
+ * | Data[0]	|Data[1]	|Data[2]		|===>Data word 32 bits
+ * -----------------------------------------------------
+ * | MESSAGE	|Data		| Index			|
+ * | TYPE	|length		| number		|===>READ/WRITE message
+ * -----------------------------------------------------
+ * -----------------------------------------------------
+ * | MESSAGE	| DSP SHM addr	| max_no_of_buffers	|===> READ
+ * | TYPE	| to write data	| ||buffersize		|WRITE SETUP message
+ * -----------------------------------------------------
+ */
+
+
+#define MAD_NAME "mad"
+/* Bit mask  */
+#define MASK_UPPER_WORD		0xFFFF
+
+/* channel values for each direction */
+#define CHANNEL_NUM_RX	0x500
+#define CHANNEL_NUM_TX	0x900
+
+/*
+ * Maximum number of datawords which can be sent
+ * in the mailbox each word is 32 bits
+ */
+#define MAX_NR_OF_DATAWORDS	MAILBOX_NR_OF_DATAWORDS
+#define MAX_NUM_RX_BUFF		NUM_DSP_BUFFER
+#define NR_OF_DATAWORDS_REQD_FOR_ACK	1
+
+/**
+ * Message types, must be identical in DSP Side
+ * VCS_MBOX_MSG_WRITE_IF_SETUP : DSP -> ARM
+ * VCS_MBOX_MSG_WRITE_IF_SETUP_ACK : ARM -> DSP
+ * VCS_MBOX_MSG_READ_IF_SETUP :  DSP -> ARM
+ * VCS_MBOX_MSG_READ_IF_SETUP_ACK : ARM -> DSP
+ * VCS_MBOX_MSG_IF_ENC_DATA : ARM -> DSP
+ * VCS_MBOX_MSG_IF_DEC_DATA : DSP -> ARM
+ */
+#define VCS_MBOX_MSG_WRITE_IF_SETUP		0x200
+#define VCS_MBOX_MSG_WRITE_IF_SETUP_ACK		0x201
+#define VCS_MBOX_MSG_READ_IF_SETUP		0x400
+#define VCS_MBOX_MSG_READ_IF_SETUP_ACK		0x401
+#define VCS_MBOX_MSG_IF_ENC_DATA		0x80
+#define VCS_MBOX_MSG_IF_DEC_DATA		0x100
+
+/**
+ * struct mad_data - This structure holds the state of the Modem Audio Driver.
+ *
+ * @dsp_shm_write_ptr : Ptr to the first TX buffer in DSP
+ * @dsp_shm_read_ptr : Ptr to the first RX buffer in DSP
+ * @max_tx_buffs : No. of DSP buffers available to write
+ * @max_rx_buffs : No. of DSP buffers available to read
+ * @write_offset : Size of each buffer in the DSP
+ * @read_offset : Size of each buffer in the DSP
+ * @rx_buff : Buffer for incoming data
+ * @tx_buff : Buffer for outgoing data
+ * @tx_buffer_num : Buffer counter for writing to DSP
+ * @rx_buffer_num : Buffer counter for reading  to DSP
+ * @rx_buffer_read :  Buffer counter for reading from userspace
+ * @data_written : RX data message arrival indicator
+ * @read_setup_msg : flag for opening read data
+ * @readq :  read queue of data message
+ * @lock : lock for r/w  message queue
+ */
+struct mad_data {
+	void __iomem	*dsp_shm_write_ptr;
+	void __iomem	*dsp_shm_read_ptr;
+	int	max_tx_buffs;
+	int	max_rx_buffs;
+	int	write_offset;
+	int	read_offset;
+	u32	*rx_buff;
+	u32	*tx_buff;
+	int	tx_buffer_num;
+	int	rx_buffer_num;
+	int	rx_buffer_read;
+	u32	data_written;
+	bool	read_setup_msg;
+	bool	open_check;
+	wait_queue_head_t  readq;
+	spinlock_t	lock;
+};
+
+static struct mad_data *mad;
+
+static void mad_receive_cb(u32 *data, u32 length, void *priv);
+static int mad_read(struct file *filp, char __user *buff, size_t count,
+		loff_t *offp);
+static int mad_write(struct file *filp, const char __user *buff, size_t count,
+		loff_t *offp);
+static unsigned int mad_select(struct file *filp, poll_table *wait);
+static void mad_send_cb(u32 *data, u32 len, void *arg);
+static int mad_open(struct inode *ino, struct file *filp);
+static int mad_close(struct inode *ino, struct file *filp);
+
+static const struct file_operations mad_fops = {
+	.release = mad_close,
+	.open    = mad_open,
+	.read    = mad_read,
+	.write   = mad_write,
+	.poll    = mad_select,
+	.owner   = THIS_MODULE,
+};
+
+static struct miscdevice mad_dev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name  = MAD_NAME,
+	.fops  = &mad_fops
+};
+
+/**
+ *  mad_send_cb - This function is default callback for send.
+ *  @data -Pointer to the data buffer
+ *  @len  -Data buffer length
+ *  @arg  -Private data pointer associated with test
+ */
+static void mad_send_cb(u32 *data, u32 len, void *arg)
+{
+	dev_dbg(mad_dev.this_device, "%s", __func__);
+}
+
+/**
+ * mad_receive_cb - This callback function is for receiving data from mailbox
+ * @data -Pointer to the data buffer
+ * @len  -length of the Mailbox
+ * @arg  -Private data pointer associated with test
+ */
+static void mad_receive_cb(u32 *data, u32 length, void *priv)
+{
+	struct mad_data *mad = priv;
+	struct mbox_channel_msg msg;
+	u32 ack_to_dsp;
+	unsigned long flags;
+
+	/* setup message for write address */
+	if (*data == VCS_MBOX_MSG_WRITE_IF_SETUP) {
+
+		ack_to_dsp = VCS_MBOX_MSG_WRITE_IF_SETUP_ACK;
+
+		/* if setup message comes again.unmap  */
+		if (mad->dsp_shm_write_ptr != NULL) {
+			iounmap(mad->dsp_shm_write_ptr);
+			mad->dsp_shm_write_ptr = NULL;
+			mad->write_offset = 0;
+			mad->max_tx_buffs = 0;
+		}
+
+		/* convert offset to uint size */
+		mad->write_offset = (data[2] & MASK_UPPER_WORD);
+		mad->max_tx_buffs = (data[2] >> 16);
+
+		mad->dsp_shm_write_ptr =  ioremap(data[1],
+			mad->max_tx_buffs * mad->write_offset);
+		if (mad->dsp_shm_write_ptr == NULL)
+			dev_err(mad_dev.this_device, "incrt write address");
+
+		/* Initialize all buffer numbers */
+		mad->tx_buffer_num = 0;
+
+		/* Send ACK to the DSP */
+		msg.channel = CHANNEL_NUM_TX;
+		msg.data = &ack_to_dsp;
+		msg.length = NR_OF_DATAWORDS_REQD_FOR_ACK;
+		msg.cb = mad_send_cb;
+		msg.priv = mad;
+
+		if (mbox_channel_send(&msg))
+			dev_err(mad_dev.this_device, "%s: can't send data\n",
+					__func__);
+
+	} /* setup message for reading SHM */
+	else if (*data == VCS_MBOX_MSG_READ_IF_SETUP) {
+
+		ack_to_dsp = VCS_MBOX_MSG_READ_IF_SETUP_ACK;
+
+		/* if setup message comes again.unmap  */
+		if (mad->dsp_shm_read_ptr != NULL) {
+			iounmap(mad->dsp_shm_read_ptr);
+			mad->dsp_shm_read_ptr = NULL;
+			mad->read_offset = 0;
+			mad->max_rx_buffs = 0;
+		}
+
+		/*convert offset to uint size*/
+		mad->read_offset	= (data[2] & MASK_UPPER_WORD);
+		mad->max_rx_buffs	= data[2] >> 16;
+
+		mad->dsp_shm_read_ptr =  ioremap(data[1],
+			mad->max_rx_buffs * mad->read_offset);
+
+		/* Initialize all buffer numbers and flags */
+		mad->rx_buffer_num  = 0;
+		mad->rx_buffer_read = 0;
+		mad->data_written   = 0;
+
+		/* Send ACK to the DSP */
+		msg.channel = CHANNEL_NUM_TX;
+		msg.data = &ack_to_dsp;
+		msg.length = NR_OF_DATAWORDS_REQD_FOR_ACK;
+		msg.cb = mad_send_cb;
+		msg.priv = mad;
+
+		if (mbox_channel_send(&msg))
+			dev_err(mad_dev.this_device, "%s: can't send data\n",
+					__func__);
+
+		/* allow read */
+		spin_lock_irqsave(&mad->lock, flags);
+		mad->read_setup_msg = true;
+		spin_unlock_irqrestore(&mad->lock, flags);
+		/* blocked in select() */
+		wake_up_interruptible(&mad->readq);
+
+	} else if (*data == VCS_MBOX_MSG_IF_DEC_DATA) {
+		/*
+		* Check if you have valid message with proper length in message
+		* otherwise Dont care
+		*/
+		if ((data[1] <=  0) || (mad->rx_buff == NULL)
+				|| (mad->dsp_shm_read_ptr == NULL)) {
+			if (mad->rx_buff == NULL)
+				dev_warn(mad_dev.this_device, "%s :MAD closed",
+						__func__);
+			else
+				dev_warn(mad_dev.this_device, "%s :0-len msg",
+						__func__);
+		} else {
+			mad->rx_buff[mad->rx_buffer_num] = data[1];
+			mad->rx_buffer_num++;
+
+			/* store the offset */
+			mad->rx_buff[mad->rx_buffer_num] = data[2];
+
+			if (mad->rx_buffer_num < ((MAX_NUM_RX_BUFF * 2)-1))
+				mad->rx_buffer_num++;
+			else
+				mad->rx_buffer_num = 0;
+
+			spin_lock_irqsave(&mad->lock, flags);
+			mad->data_written++;
+
+			if (mad->data_written > MAX_NUM_RX_BUFF) {
+				dev_warn(mad_dev.this_device,
+						"%s :Read msg overflow = %u\n",
+						__func__ , mad->data_written);
+				/*
+				 * Donot exceed MAX_NUM_RX_BUFF size of buffer
+				 * TO DO overflow control
+				 */
+				mad->data_written = MAX_NUM_RX_BUFF ;
+			}
+			spin_unlock_irqrestore(&mad->lock, flags);
+			wake_up_interruptible(&mad->readq);
+		}
+	} else {
+		/* received Invalid message */
+		dev_err(mad_dev.this_device, "%s : Invalid Msg", __func__);
+	}
+}
+
+static int mad_read(struct file *filp, char __user *buff, size_t count,
+		loff_t *offp)
+{
+	unsigned long flags;
+	unsigned int size = 0;
+	void __iomem *shm_ptr = NULL;
+
+	dev_dbg(mad_dev.this_device, "%s", __func__);
+
+	if (!(mad->data_written > 0)) {
+		if (wait_event_interruptible(mad->readq,
+			((mad->data_written > 0) &&
+			(mad->dsp_shm_read_ptr != NULL))))
+			return -ERESTARTSYS;
+	}
+
+	if (mad->dsp_shm_read_ptr == NULL) {
+		dev_err(mad_dev.this_device, "%s :pointer err", __func__);
+		return -EINVAL ;
+	}
+
+	if (mad->rx_buff[mad->rx_buffer_read] > count) {
+		/*
+		 * Size of message greater than buffer , this shouldnt happen
+		 * It shouldnt come here : we ensured that message size
+		 * smaller that buffer length
+		 */
+		dev_err(mad_dev.this_device, "%s : Incrct length", __func__);
+		return -EFAULT;
+	}
+	size = mad->rx_buff[mad->rx_buffer_read];
+	mad->rx_buff[mad->rx_buffer_read] = 0;
+	mad->rx_buffer_read++;
+	shm_ptr = (u8 *)(mad->dsp_shm_read_ptr +
+			(mad->rx_buff[mad->rx_buffer_read] * mad->read_offset));
+	if (copy_to_user(buff, shm_ptr, size) < 0) {
+		dev_err(mad_dev.this_device, "%s :copy to user", __func__);
+		return -EFAULT;
+	}
+
+	if (mad->rx_buffer_read < ((MAX_NUM_RX_BUFF*2)-1))
+		mad->rx_buffer_read++;
+	else
+		mad->rx_buffer_read = 0;
+
+	spin_lock_irqsave(&mad->lock, flags);
+	mad->data_written--;
+	if (mad->data_written < 0) {
+		/* Means wrong read*/
+		mad->data_written = 0;
+		dev_err(mad_dev.this_device, "%s :data Rcev err", __func__);
+	}
+	spin_unlock_irqrestore(&mad->lock, flags);
+	return size;
+}
+
+static int mad_write(struct file *filp, const char __user *buff, size_t count,
+								loff_t *offp)
+{
+	int retval = 0;
+	void __iomem  *dsp_write_address;
+	struct mbox_channel_msg msg;
+
+	dev_dbg(mad_dev.this_device, "%s", __func__);
+
+	/* check for valid write pointer else skip writing*/
+	if (mad->dsp_shm_write_ptr == NULL) {
+		dev_err(mad_dev.this_device, "%s :Illegal memory", __func__);
+		return -EFAULT;
+	}
+
+	dsp_write_address = (mad->dsp_shm_write_ptr +
+				(mad->tx_buffer_num * mad->write_offset));
+
+	if (copy_from_user(dsp_write_address, buff, count)) {
+		dev_err(mad_dev.this_device, "%s:copy_from_user\n", __func__);
+		return -EFAULT;
+	}
+
+	mad->tx_buff[0] = VCS_MBOX_MSG_IF_ENC_DATA;
+	mad->tx_buff[1] = count;
+	mad->tx_buff[2] = mad->tx_buffer_num;
+
+	if (mad->tx_buffer_num < (mad->max_tx_buffs-1))
+		mad->tx_buffer_num++;
+	else
+		mad->tx_buffer_num = 0;
+
+	msg.channel = CHANNEL_NUM_TX;
+	msg.data = mad->tx_buff;
+	msg.length = MAX_NR_OF_DATAWORDS;
+	msg.cb = mad_send_cb;
+	msg.priv = mad;
+
+	retval = mbox_channel_send(&msg);
+	if (retval) {
+		dev_err(mad_dev.this_device, "%s:can't send data", __func__);
+		return retval;
+	}
+	return count;
+}
+
+static unsigned int mad_select(struct file *filp, poll_table *wait)
+{
+	unsigned int mask = 0;
+	unsigned long flags;
+
+	dev_dbg(mad_dev.this_device, "%s", __func__);
+
+	poll_wait(filp, &mad->readq,  wait);
+	spin_lock_irqsave(&mad->lock, flags);
+
+	if ((true == mad->read_setup_msg) && (mad->data_written > 0))
+		mask |= POLLIN | POLLRDNORM;    /* allow readable */
+	spin_unlock_irqrestore(&mad->lock, flags);
+
+	return mask;
+}
+
+static int mad_open(struct inode *ino, struct file *filp)
+{
+	int	err = 0;
+
+	dev_dbg(mad_dev.this_device, "%s", __func__);
+
+	if (mad->open_check == true) {
+		dev_err(mad_dev.this_device, "%s :Already opened", __func__);
+		return -EFAULT;
+	}
+
+	mad->rx_buff =  kzalloc((MAX_NUM_RX_BUFF*2 *
+				sizeof(mad->rx_buff)), GFP_KERNEL);
+
+	if (mad->rx_buff == NULL) {
+		dev_err(mad_dev.this_device, "%s:RX memory\n", __func__);
+		err = -ENOMEM;
+		goto error;
+	}
+
+	mad->tx_buff =  kzalloc(MAX_NR_OF_DATAWORDS, GFP_KERNEL);
+	if (mad->tx_buff == NULL) {
+		dev_err(mad_dev.this_device, "%s:TX memory\n", __func__);
+		err = -ENOMEM;
+		goto error;
+	}
+
+	/* Init spinlock for critical section access*/
+	spin_lock_init(&mad->lock);
+	init_waitqueue_head(&(mad->readq));
+
+	err = mbox_channel_register(CHANNEL_NUM_RX, mad_receive_cb, mad);
+	if (err) {
+		dev_err(mad_dev.this_device, "%s: register err", __func__);
+		err = -EFAULT;
+		goto error;
+	}
+	mad->open_check = true;
+
+	return 0;
+error:
+	kfree(mad->rx_buff);
+	kfree(mad->tx_buff);
+	return err;
+}
+
+static int mad_close(struct inode *ino, struct file *filp)
+{
+	dev_dbg(mad_dev.this_device, "%s", __func__);
+
+	if (mbox_channel_deregister(CHANNEL_NUM_RX)) {
+		dev_err(mad_dev.this_device, "%s:deregister err", __func__);
+		return -EFAULT;
+	}
+	kfree(mad->rx_buff);
+	kfree(mad->tx_buff);
+	mad->data_written = 0;
+	mad->rx_buffer_num = 0;
+	mad->rx_buffer_read = 0;
+	mad->open_check = false;
+
+	return 0;
+}
+
+static int __init mad_init(void)
+{
+	dev_dbg(mad_dev.this_device, "%s", __func__);
+
+	mad = kzalloc(sizeof(*mad), GFP_KERNEL);
+	if (mad == NULL) {
+		dev_err(mad_dev.this_device, "%s :MAD failed", __func__);
+		return -ENOMEM;
+	}
+
+	return misc_register(&mad_dev);
+}
+module_init(mad_init);
+
+static void  __exit mad_exit(void)
+{
+	dev_dbg(mad_dev.this_device, "%s", __func__);
+
+	 if (mad->dsp_shm_write_ptr != NULL) {
+		 iounmap(mad->dsp_shm_write_ptr);
+		 mad->dsp_shm_write_ptr = NULL;
+	 }
+
+	 if (mad->dsp_shm_read_ptr != NULL) {
+		 iounmap(mad->dsp_shm_read_ptr);
+		 mad->dsp_shm_read_ptr = NULL;
+	 }
+
+	 kfree(mad);
+	 misc_deregister(&mad_dev);
+}
diff --git a/drivers/misc/sim_detect.c b/drivers/misc/sim_detect.c
new file mode 100644
index 00000000000..e67f4fad3db
--- /dev/null
+++ b/drivers/misc/sim_detect.c
@@ -0,0 +1,306 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: BIBEK BASU <bibek.basu@stericsson.com>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/workqueue.h>
+#include <linux/uaccess.h>
+#include <linux/modem/modem_client.h>
+#include <mach/sim_detect.h>
+#include <linux/regulator/consumer.h>
+
+/* time in millisec */
+#define TIMER_DELAY	10
+
+struct sim_detect{
+	struct work_struct	timer_expired;
+	struct device	*dev;
+	struct modem *modem;
+	struct hrtimer timer;
+	struct mutex lock;
+	int voltage;
+	struct regulator *vinvsim_regulator;
+	bool regulator_enabled;
+};
+
+static ssize_t show_voltage(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct sim_detect *data = dev_get_drvdata(dev);
+	int ret, len;
+
+	ret = mutex_lock_interruptible(&data->lock);
+	if (ret < 0)
+		return ret;
+
+	len = sprintf(buf, "%i\n", data->voltage);
+
+	mutex_unlock(&data->lock);
+
+	return len;
+}
+
+static ssize_t write_voltage(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct sim_detect *sim_detect = dev_get_drvdata(dev);
+	long val;
+	int ret;
+
+	/* check input */
+	if (strict_strtol(buf, 0, &val) != 0) {
+		dev_err(dev, "Invalid voltage class configured.\n");
+		return -EINVAL;
+	}
+
+	switch (val) {
+	case -1:
+	case 0:
+	case 1800000:
+	case 3000000:
+		break;
+	default:
+		dev_err(dev, "Invalid voltage class configured.\n");
+		return -EINVAL;
+	}
+
+	/* lock */
+	ret = mutex_lock_interruptible(&sim_detect->lock);
+	if (ret < 0)
+		return ret;
+
+	/* update state */
+	sim_detect->voltage = val;
+
+	/* call regulator */
+	switch (sim_detect->voltage) {
+	case 0:
+		/* SIM voltage is unknown, turn on regulator for 3 V SIM */
+	case 3000000:
+		/* Vinvsim supply is used only for 3 V SIM */
+		if (!sim_detect->regulator_enabled) {
+			ret = regulator_enable(sim_detect->vinvsim_regulator);
+			if (ret) {
+				dev_err(dev, "Failed to enable regulator.\n");
+				goto out_unlock;
+			}
+			sim_detect->regulator_enabled = true;
+		}
+		break;
+	case 1800000:
+	case -1:
+		/* Vbatvsim is used otherwise */
+		if (sim_detect->regulator_enabled) {
+			regulator_disable(sim_detect->vinvsim_regulator);
+			sim_detect->regulator_enabled = false;
+		}
+	}
+
+out_unlock:
+	/* unlock and return */
+	mutex_unlock(&sim_detect->lock);
+
+	return count;
+}
+
+static DEVICE_ATTR(voltage, S_IWUGO | S_IRUGO, show_voltage, write_voltage);
+
+static struct attribute *sim_attributes[] = {
+	&dev_attr_voltage.attr,
+	NULL
+};
+
+static const struct attribute_group sim_attr_group = {
+	.attrs	= sim_attributes,
+};
+
+static void inform_modem_release(struct work_struct *work)
+{
+	struct sim_detect *sim_detect =
+		container_of(work, struct sim_detect, timer_expired);
+
+	/* call Modem Access Framework api to release modem */
+	modem_release(sim_detect->modem);
+}
+
+static enum hrtimer_restart timer_callback(struct hrtimer *timer)
+{
+	struct sim_detect *sim_detect =
+		container_of(timer, struct sim_detect, timer);
+
+	schedule_work(&sim_detect->timer_expired);
+	return HRTIMER_NORESTART;
+}
+
+static irqreturn_t sim_activity_irq(int irq, void *dev)
+{
+	struct sim_detect *sim_detect = dev;
+
+	/* call Modem Access Framework api to acquire modem */
+	modem_request(sim_detect->modem);
+	/* start the timer for 10ms */
+	hrtimer_start(&sim_detect->timer,
+			ktime_set(0, TIMER_DELAY*NSEC_PER_MSEC),
+			HRTIMER_MODE_REL);
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_PM
+/**
+ * sim_detect_suspend() - This routine puts the Sim detect in to sustend state.
+ * @dev:	pointer to device structure.
+ *
+ * This routine checks the current ongoing communication with Modem by
+ * examining the modem_get_usage and work_pending state.
+ * accordingly prevents suspend if modem communication
+ * is on-going.
+ */
+int sim_detect_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct sim_detect *sim_detect = platform_get_drvdata(pdev);
+
+	dev_dbg(&pdev->dev, "%s called...\n", __func__);
+	/* if modem is accessed, event system suspend */
+	if (modem_get_usage(sim_detect->modem)
+			|| work_pending(&sim_detect->timer_expired))
+		return -EBUSY;
+	else
+		return 0;
+}
+
+static const struct dev_pm_ops sim_detect_dev_pm_ops = {
+	.suspend = sim_detect_suspend,
+};
+#endif
+
+
+static int __devinit sim_detect_probe(struct platform_device *pdev)
+{
+	struct sim_detect_platform_data *plat = dev_get_platdata(&pdev->dev);
+	struct sim_detect *sim_detect;
+	int ret;
+
+	sim_detect = kzalloc(sizeof(struct sim_detect), GFP_KERNEL);
+	if (sim_detect == NULL) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	/* initialize data */
+	mutex_init(&sim_detect->lock);
+	sim_detect->voltage = 0;
+
+	sim_detect->dev = &pdev->dev;
+	INIT_WORK(&sim_detect->timer_expired, inform_modem_release);
+	hrtimer_init(&sim_detect->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	sim_detect->timer.function = timer_callback;
+
+	sim_detect->modem = modem_get(sim_detect->dev, "u8500-shrm-modem");
+	if (IS_ERR(sim_detect->modem)) {
+		ret = PTR_ERR(sim_detect->modem);
+		dev_err(sim_detect->dev, "Could not retrieve the modem\n");
+		goto out_free;
+	}
+
+	/* set drvdata */
+	platform_set_drvdata(pdev, sim_detect);
+
+	/* request irq */
+	ret = request_threaded_irq(plat->irq_num,
+		       NULL, sim_activity_irq,
+		       IRQF_TRIGGER_FALLING |
+		       IRQF_TRIGGER_RISING |
+		       IRQF_NO_SUSPEND,
+		       "sim activity", sim_detect);
+	if (ret < 0)
+		goto out_put_modem;
+
+	/* get regulator */
+	sim_detect->regulator_enabled = false;
+	sim_detect->vinvsim_regulator = regulator_get(sim_detect->dev,
+							 "vinvsim");
+	if (IS_ERR(sim_detect->vinvsim_regulator)) {
+		dev_err(&pdev->dev,
+			"Failed to get regulator. (dev_name %s).\n",
+			dev_name(sim_detect->dev));
+		ret = PTR_ERR(sim_detect->vinvsim_regulator);
+		goto out_free_irq;
+	}
+
+	/* register sysfs entry */
+	ret = sysfs_create_group(&pdev->dev.kobj, &sim_attr_group);
+	if (ret != 0) {
+		dev_err(&pdev->dev,
+			"Failed to create attribute group: %d\n", ret);
+		goto out_free_regulator;
+	}
+
+	return 0;
+
+out_free_regulator:
+	regulator_put(sim_detect->vinvsim_regulator);
+out_free_irq:
+	free_irq(plat->irq_num, sim_detect);
+out_put_modem:
+	modem_put(sim_detect->modem);
+	platform_set_drvdata(pdev, NULL);
+out_free:
+	kfree(sim_detect);
+	return ret;
+}
+
+static int __devexit sim_detect_remove(struct platform_device *pdev)
+{
+	struct sim_detect *sim_detect = platform_get_drvdata(pdev);
+
+	sysfs_remove_group(&pdev->dev.kobj, &sim_attr_group);
+	regulator_put(sim_detect->vinvsim_regulator);
+	modem_put(sim_detect->modem);
+	platform_set_drvdata(pdev, NULL);
+	kfree(sim_detect);
+	return 0;
+}
+
+static struct platform_driver sim_detect_driver = {
+	.driver = {
+		.name = "sim-detect",
+		.owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm = &sim_detect_dev_pm_ops,
+#endif
+	},
+	.probe = sim_detect_probe,
+	.remove = __devexit_p(sim_detect_remove),
+};
+
+static int __init sim_detect_init(void)
+{
+	return platform_driver_register(&sim_detect_driver);
+}
+module_init(sim_detect_init);
+
+static void __exit sim_detect_exit(void)
+{
+	platform_driver_unregister(&sim_detect_driver);
+}
+module_exit(sim_detect_exit);
+
+MODULE_AUTHOR("BIBEK BASU <bibek.basu@stericsson.com>");
+MODULE_DESCRIPTION("Detects SIM Hot Swap and wakes modem");
+MODULE_ALIAS("platform:sim-detect");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/misc/stm.c b/drivers/misc/stm.c
new file mode 100644
index 00000000000..33bb26c27ca
--- /dev/null
+++ b/drivers/misc/stm.c
@@ -0,0 +1,850 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ *         Philippe Langlais <philippe.Langlais@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/cdev.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/debugfs.h>
+#include <trace/stm.h>
+
+/* STM Registers */
+#define STM_CR          (stm.virtbase)
+#define STM_MMC         (stm.virtbase + 0x008)
+#define STM_TER         (stm.virtbase + 0x010)
+#define STMPERIPHID0    (stm.virtbase + 0xFC0)
+#define STMPERIPHID1    (stm.virtbase + 0xFC8)
+#define STMPERIPHID2    (stm.virtbase + 0xFD0)
+#define STMPERIPHID3    (stm.virtbase + 0xFD8)
+#define STMPCELLID0     (stm.virtbase + 0xFE0)
+#define STMPCELLID1     (stm.virtbase + 0xFE8)
+#define STMPCELLID2     (stm.virtbase + 0xFF0)
+#define STMPCELLID3     (stm.virtbase + 0xFF8)
+
+#define STM_CLOCK_SHIFT 6
+#define STM_CLOCK_MASK  0x1C0
+
+/* Hardware mode for all sources */
+#define STM_MMC_DEFAULT            CONFIG_STM_DEFAULT_MASTERS_MODES
+
+/* Max number of channels (multiple of 256) */
+#define STM_NUMBER_OF_CHANNEL      CONFIG_STM_NUMBER_OF_CHANNEL
+
+/* # dynamically allocated channel with stm_trace_buffer */
+#define NB_KERNEL_DYNAMIC_CHANNEL  128
+
+static struct stm_device {
+	const struct stm_platform_data *pdata;
+	void __iomem *virtbase;
+	/* Used to register the allocated channels */
+	DECLARE_BITMAP(ch_bitmap, STM_NUMBER_OF_CHANNEL);
+} stm;
+
+volatile struct stm_channel __iomem *stm_channels;
+
+static struct cdev cdev;
+static struct class *stm_class;
+static int stm_major;
+
+static DEFINE_SPINLOCK(lock);
+
+/* Middle value for clock divisor */
+static enum clock_div stm_clockdiv = STM_CLOCK_DIV8;
+
+/* Default value for STM output connection */
+static enum stm_connection_type stm_connection = STM_DEFAULT_CONNECTION;
+
+#define STM_BUFSIZE	256
+struct channel_data {
+	DECLARE_BITMAP(bitmap, STM_NUMBER_OF_CHANNEL);
+	int numero;
+	spinlock_t lock;
+	u8 data_buffer[STM_BUFSIZE];
+};
+
+static u64 stm_printk_buf[1024/sizeof(u64)];
+static arch_spinlock_t stm_buf_lock =
+		(arch_spinlock_t)__ARCH_SPIN_LOCK_UNLOCKED;
+
+static char *mipi60 = "none";
+module_param(mipi60, charp, S_IRUGO);
+MODULE_PARM_DESC(mipi60, "STM Trace to output on probe2 of mipi60 "
+		 "('none' or 'ape' or 'modem')");
+
+static char *mipi34 = "none";
+module_param(mipi34, charp, S_IRUGO);
+MODULE_PARM_DESC(mipi34, "STM Trace to output on mipi34 "
+		 "('none' or 'ape' or 'modem')");
+
+static char *microsd = "none";
+module_param(microsd, charp, S_IRUGO);
+MODULE_PARM_DESC(microsd, "STM Trace to output on SD card connector "
+		 "('none' or 'ape' or 'modem')");
+
+static unsigned int stm_ter;
+module_param(stm_ter, uint, 0);
+MODULE_PARM_DESC(stm_ter, "Value for STM_TER (trace control register). "
+		 "Should be set by user as environment variable stm.stm_ter");
+
+#define IS_APE_ON_MIPI34 (mipi34 && !strcmp(mipi34, "ape"))
+#define IS_APE_ON_MIPI60 (mipi60 && !strcmp(mipi60, "ape"))
+#define IS_APE_ON_MICROSD (microsd && !strcmp(microsd, "ape"))
+#define IS_MODEM_ON_MICROSD (microsd && !strcmp(microsd, "modem"))
+
+static int stm_connection_set(void *data, u64 val);
+
+int stm_alloc_channel(int offset)
+{
+	int channel;
+
+	/* Look for a free channel from offset */
+	do {
+		channel = find_next_zero_bit(stm.ch_bitmap,
+				STM_NUMBER_OF_CHANNEL, offset);
+	} while ((channel < STM_NUMBER_OF_CHANNEL)
+	       && test_and_set_bit(channel, stm.ch_bitmap));
+	return channel;
+}
+EXPORT_SYMBOL(stm_alloc_channel);
+
+void stm_free_channel(int channel)
+{
+	clear_bit(channel, stm.ch_bitmap);
+}
+EXPORT_SYMBOL(stm_free_channel);
+
+static int stm_get_channel(struct channel_data *ch_data, int __user *arg)
+{
+	int channel, err;
+
+	channel = stm_alloc_channel(0);
+	if (channel < STM_NUMBER_OF_CHANNEL) {
+		/* One free found ! */
+		err = put_user(channel, arg);
+		if (err)
+			stm_free_channel(channel);
+		else
+			/* Register it in the context of the file */
+			set_bit(channel, ch_data->bitmap);
+	} else
+		err = -ENOMEM;
+	return err;
+}
+
+static int stm_release_channel(struct channel_data *ch_data, int channel)
+{
+	if ((channel < 0) || (channel >= STM_NUMBER_OF_CHANNEL))
+		return -EINVAL;
+	stm_free_channel(channel);
+	clear_bit(channel, ch_data->bitmap);
+	return 0;
+}
+
+/*
+ * Trace a buffer on a given channel
+ * with auto time stamping on last byte(s) only
+ */
+int stm_trace_buffer_onchannel(int channel,
+			       const void *data, size_t length)
+{
+	int i, mod64;
+	volatile struct stm_channel __iomem *pch;
+
+	if (channel >= STM_NUMBER_OF_CHANNEL || !stm_channels)
+		return 0;
+
+	pch = &stm_channels[channel];
+
+	/*  Align data pointer to u64 & time stamp last byte(s) */
+	mod64 = (int)data & 7;
+	i = length - 8 + mod64;
+	switch (mod64) {
+	case 0:
+		if (i)
+			pch->no_stamp64 = *(u64 *)data;
+		else {
+			pch->stamp64 = *(u64 *)data;
+			return length;
+		}
+		data += 8;
+		break;
+	case 1:
+		pch->no_stamp8 = *(u8 *)data;
+		pch->no_stamp16 = *(u16 *)(data+1);
+		if (i)
+			pch->no_stamp32 = *(u32 *)(data+3);
+		else {
+			pch->stamp32 = *(u32 *)(data+3);
+			return length;
+		}
+		data += 7;
+		break;
+	case 2:
+		pch->no_stamp16 = *(u16 *)data;
+		if (i)
+			pch->no_stamp32 = *(u32 *)(data+2);
+		else {
+			pch->stamp32 = *(u32 *)(data+2);
+			return length;
+		}
+		data += 6;
+		break;
+	case 3:
+		pch->no_stamp8 = *(u8 *)data;
+		if (i)
+			pch->no_stamp32 = *(u32 *)(data+1);
+		else {
+			pch->stamp32 = *(u32 *)(data+1);
+			return length;
+		}
+		data += 5;
+		break;
+	case 4:
+		if (i)
+			pch->no_stamp32 = *(u32 *)data;
+		else {
+			pch->stamp32 = *(u32 *)data;
+			return length;
+		}
+		data += 4;
+		break;
+	case 5:
+		pch->no_stamp8 = *(u8 *)data;
+		if (i)
+			pch->no_stamp16 = *(u16 *)(data+1);
+		else {
+			pch->stamp16 = *(u16 *)(data+1);
+			return length;
+		}
+		data += 3;
+		break;
+	case 6:
+		if (i)
+			pch->no_stamp16 = *(u16 *)data;
+		else {
+			pch->stamp16 = *(u16 *)data;
+			return length;
+		}
+		data += 2;
+		break;
+	case 7:
+		if (i)
+			pch->no_stamp8 = *(u8 *)data;
+		else {
+			pch->stamp8 = *(u8 *)data;
+			return length;
+		}
+		data++;
+		break;
+	}
+	for (;;) {
+		if (i > 8) {
+			pch->no_stamp64 = *(u64 *)data;
+			data += 8;
+			i -= 8;
+		} else if (i == 8) {
+			pch->stamp64 = *(u64 *)data;
+			break;
+		} else if (i > 4) {
+			pch->no_stamp32 = *(u32 *)data;
+			data += 4;
+			i -= 4;
+		} else if (i == 4) {
+			pch->stamp32 = *(u32 *)data;
+			break;
+		} else if (i > 2) {
+			pch->no_stamp16 = *(u16 *)data;
+			data += 2;
+			i -= 2;
+		} else if (i == 2) {
+			pch->stamp16 = *(u16 *)data;
+			break;
+		} else {
+			pch->stamp8 = *(u8 *)data;
+			break;
+		}
+	}
+	return length;
+}
+EXPORT_SYMBOL(stm_trace_buffer_onchannel);
+
+static int stm_open(struct inode *inode, struct file *file)
+{
+	struct channel_data *channel_data;
+	int retval = 0;
+
+	channel_data = kzalloc(sizeof(struct channel_data), GFP_KERNEL);
+	if (channel_data == NULL)
+		return -ENOMEM;
+
+	spin_lock_init(&channel_data->lock);
+	channel_data->numero = -1;   /*  Channel not yet allocated */
+	file->private_data = channel_data;
+
+	/*
+	 * Check if microsd is selected as trace interface
+	 * and enable corresponding pins muxing.
+	 */
+	if (IS_MODEM_ON_MICROSD)
+		retval = stm_connection_set(NULL, STM_STE_MODEM_ON_MICROSD);
+	else if (IS_APE_ON_MICROSD)
+		retval = stm_connection_set(NULL, STM_STE_APE_ON_MICROSD);
+
+	if (retval)
+		pr_alert("stm_open: failed to connect STM output\n");
+
+	return retval;
+}
+
+static int stm_release(struct inode *inode, struct file *file)
+{
+	struct channel_data *channel;
+
+	channel = (struct channel_data *)file->private_data;
+
+	/* Free allocated channel if necessary */
+	if (channel->numero != -1)
+		stm_free_channel(channel->numero);
+
+	bitmap_andnot(stm.ch_bitmap, stm.ch_bitmap,
+			channel->bitmap, STM_NUMBER_OF_CHANNEL);
+
+	kfree(channel);
+	return 0;
+}
+
+static ssize_t stm_write(struct file *file, const char __user *buf,
+		  size_t size, loff_t *off)
+{
+	struct channel_data *channel = file->private_data;
+
+	/*  Alloc channel at first write */
+	if (channel->numero == -1) {
+		channel->numero = stm_alloc_channel(0);
+		if (channel->numero > STM_NUMBER_OF_CHANNEL)
+			return -ENOMEM;
+	}
+
+	if (size > STM_BUFSIZE)
+		size = STM_BUFSIZE;
+
+	spin_lock(&channel->lock);
+
+	if (copy_from_user
+	    (channel->data_buffer, (void __user *) buf, size)) {
+		spin_unlock(&channel->lock);
+		return -EFAULT;
+	}
+	size = stm_trace_buffer_onchannel(channel->numero,
+			channel->data_buffer, size);
+
+	spin_unlock(&channel->lock);
+
+	return size;
+}
+
+static int stm_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	/*
+	 * Don't allow a mapping that covers more than the STM channels
+	 */
+	if ((vma->vm_end - vma->vm_start) >
+	    STM_NUMBER_OF_CHANNEL*sizeof(struct stm_channel))
+		return -EINVAL;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (io_remap_pfn_range(vma, vma->vm_start,
+			stm.pdata->channels_phys_base>>PAGE_SHIFT,
+			STM_NUMBER_OF_CHANNEL*sizeof(struct stm_channel),
+			vma->vm_page_prot))
+		return -EAGAIN;
+
+	return 0;
+}
+
+/* Enable the trace for given sources (bitfield) */
+static void stm_enable_src(unsigned int v)
+{
+	unsigned int cr_val;
+	spin_lock(&lock);
+	cr_val = readl(STM_CR);
+	cr_val &= ~STM_CLOCK_MASK;
+	writel(cr_val|(stm_clockdiv<<STM_CLOCK_SHIFT), STM_CR);
+	/*
+	 * If the kernel argument stm_ter has been set by the boot loader
+	 * all calls to stm_enable_src will be ignored
+	 */
+	v = stm_ter ? stm_ter : v;
+	writel(v, STM_TER);
+	spin_unlock(&lock);
+}
+
+/* Disable all sources */
+static void stm_disable_src(void)
+{
+	writel(0x0, STM_CR);   /* stop clock */
+	writel(0x0, STM_TER);  /* Disable cores */
+}
+
+/* Set clock speed */
+static int stm_set_ckdiv(enum clock_div v)
+{
+	unsigned int val;
+
+	spin_lock(&lock);
+	val = readl(STM_CR);
+	val &= ~STM_CLOCK_MASK;
+	writel(val | ((v << STM_CLOCK_SHIFT) & STM_CLOCK_MASK), STM_CR);
+	spin_unlock(&lock);
+	stm_clockdiv = v;
+
+	return 0;
+}
+
+/* Return the control register */
+static inline unsigned int stm_get_cr(void)
+{
+	return readl(STM_CR);
+}
+
+/*
+ * Set Trace MODE lossless/lossy (Software/Hardware)
+ * each bit represent the corresponding mode of this source
+ */
+static inline void stm_set_modes(unsigned int modes)
+{
+	writel(modes, STM_MMC);
+}
+
+/* Get Trace MODE lossless/lossy (Software/Hardware)
+ * each bit represent the corresponding mode of this source */
+static inline unsigned int stm_get_modes(void)
+{
+	return readl(STM_MMC);
+}
+
+/* Count # of free channels */
+static int stm_nb_free_channels(void)
+{
+	int nb_channels, offset;
+
+	nb_channels = 0;
+	offset = 0;
+	for (;;) {
+		offset = find_next_zero_bit(stm.ch_bitmap,
+				STM_NUMBER_OF_CHANNEL, offset);
+		if (offset == STM_NUMBER_OF_CHANNEL)
+			break;
+		offset++;
+		nb_channels++;
+	}
+	return nb_channels;
+}
+
+static long stm_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	int err = 0;
+	struct channel_data *channel = file->private_data;
+
+	switch (cmd) {
+
+	case STM_CONNECTION:
+		if (stm.pdata->stm_connection)
+			stm.pdata->stm_connection(arg);
+		stm_connection = arg;
+		break;
+
+	case STM_DISABLE:
+		stm_disable_src();
+		break;
+
+	case STM_GET_NB_MAX_CHANNELS:
+		err = put_user(STM_NUMBER_OF_CHANNEL, (unsigned int *)arg);
+		break;
+
+	case STM_GET_NB_FREE_CHANNELS:
+		err = put_user(stm_nb_free_channels(), (unsigned int *)arg);
+		break;
+
+	case STM_GET_CHANNEL_NO:
+		err = put_user(channel->numero, (unsigned int *)arg);
+		break;
+
+	case STM_SET_CLOCK_DIV:
+		err = stm_set_ckdiv((enum clock_div) arg);
+		break;
+
+	case STM_SET_MODE:
+		stm_set_modes(arg);
+		break;
+
+	case STM_GET_MODE:
+		err = put_user(stm_get_modes(), (unsigned int *)arg);
+		break;
+
+	case STM_GET_CTRL_REG:
+		err = put_user(stm_get_cr(), (unsigned int *)arg);
+		break;
+
+	case STM_ENABLE_SRC:
+		stm_enable_src(arg);
+		break;
+
+	case STM_GET_FREE_CHANNEL:
+		err = stm_get_channel(channel, (int *)arg);
+		break;
+
+	case STM_RELEASE_CHANNEL:
+		err = stm_release_channel(channel, arg);
+		break;
+
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	return err;
+}
+
+/*
+ * Trace a buffer on a dynamically allocated channel
+ * with auto time stamping on the first byte(s) only
+ * Dynamic channel number >=
+ *     STM_NUMBER_OF_CHANNEL - NB_KERNEL_DYNAMIC_CHANNEL
+ */
+int stm_trace_buffer(const void *data, size_t length)
+{
+	int channel;
+
+	channel = stm_alloc_channel(STM_NUMBER_OF_CHANNEL
+			- NB_KERNEL_DYNAMIC_CHANNEL);
+	if (channel < STM_NUMBER_OF_CHANNEL) {
+		length = stm_trace_buffer_onchannel(channel, data, length);
+		stm_free_channel(channel);
+		return length;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(stm_trace_buffer);
+
+static const struct file_operations stm_fops = {
+	.owner =		THIS_MODULE,
+	.unlocked_ioctl =	stm_ioctl,
+	.open =			stm_open,
+	.llseek =		no_llseek,
+	.write =		stm_write,
+	.release =		stm_release,
+	.mmap =			stm_mmap,
+};
+
+/*
+ * Init and deinit driver
+ */
+
+static int __devinit stm_probe(struct platform_device *pdev)
+{
+	int retval = 0;
+
+	if (!pdev || !pdev->dev.platform_data)  {
+		pr_alert("No device/platform_data found on STM driver\n");
+		return -ENODEV;
+	}
+
+	stm.pdata = pdev->dev.platform_data;
+
+	cdev_init(&cdev, &stm_fops);
+	cdev.owner = THIS_MODULE;
+
+	stm_channels =
+		ioremap_nocache(stm.pdata->channels_phys_base,
+			STM_NUMBER_OF_CHANNEL*sizeof(*stm_channels));
+	if (stm_channels == NULL) {
+		dev_err(&pdev->dev, "could not remap STM Msg register\n");
+		return -ENODEV;
+	}
+
+	stm.virtbase = ioremap_nocache(stm.pdata->regs_phys_base, SZ_4K);
+	if (stm.virtbase == NULL) {
+		retval = -EIO;
+		dev_err(&pdev->dev, "could not remap STM Register\n");
+		goto err_channels;
+	}
+
+	retval = cdev_add(&cdev, MKDEV(stm_major, 0), 1);
+	if (retval) {
+		dev_err(&pdev->dev, "chardev registration failed\n");
+		goto err_channels;
+	}
+
+	if (IS_ERR(device_create(stm_class, &pdev->dev,
+				 MKDEV(stm_major, 0), NULL, STM_DEV_NAME)))
+		dev_err(&pdev->dev, "can't create device\n");
+
+	/* Check chip IDs if necessary */
+	if (stm.pdata->id_mask) {
+		u32 periph_id, cell_id;
+
+		periph_id = (readb(STMPERIPHID3)<<24) +
+				 (readb(STMPERIPHID2)<<16) +
+				 (readb(STMPERIPHID1)<<8) +
+				 readb(STMPERIPHID0);
+		cell_id = (readb(STMPCELLID3)<<24) +
+				 (readb(STMPCELLID2)<<16) +
+				 (readb(STMPCELLID1)<<8) +
+				 readb(STMPCELLID0);
+		/* Only warns if it isn't a ST-Ericsson supported one */
+		if ((periph_id & stm.pdata->id_mask) != 0x00080dec ||
+		    cell_id != 0xb105f00d) {
+			dev_warn(&pdev->dev, "STM-Trace IC not compatible\n");
+			dev_warn(&pdev->dev, "periph_id=%x\n", periph_id);
+			dev_warn(&pdev->dev, "pcell_id=%x\n", cell_id);
+		}
+	}
+
+	/* Reserve channels if necessary */
+	if (stm.pdata->channels_reserved_sz) {
+		int i;
+
+		for (i = 0; i < stm.pdata->channels_reserved_sz; i++) {
+			set_bit(stm.pdata->channels_reserved[i],
+					stm.ch_bitmap);
+		}
+	}
+	/* Reserve kernel trace channels on demand */
+#ifdef CONFIG_STM_PRINTK
+	set_bit(CONFIG_STM_PRINTK_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_FTRACE
+	set_bit(CONFIG_STM_FTRACE_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_CTX_SWITCH
+	set_bit(CONFIG_STM_CTX_SWITCH_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_WAKEUP
+	set_bit(CONFIG_STM_WAKEUP_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_STACK_TRACE
+	set_bit(CONFIG_STM_STACK_TRACE_CHANNEL, stm.ch_bitmap);
+#endif
+#ifdef CONFIG_STM_TRACE_PRINTK
+	set_bit(CONFIG_STM_TRACE_PRINTK_CHANNEL, stm.ch_bitmap);
+	set_bit(CONFIG_STM_TRACE_BPRINTK_CHANNEL, stm.ch_bitmap);
+#endif
+
+	/* Check kernel's environment parameters first */
+	if (IS_APE_ON_MIPI34)
+		stm_connection = STM_STE_APE_ON_MIPI34_NONE_ON_MIPI60;
+	else if (IS_APE_ON_MIPI60)
+		stm_connection = STM_STE_MODEM_ON_MIPI34_APE_ON_MIPI60;
+
+	/* Apply parameters to driver */
+	if (stm.pdata->stm_connection) {
+		retval = stm.pdata->stm_connection(stm_connection);
+		if (retval) {
+			dev_err(&pdev->dev, "failed to connect STM output\n");
+			goto err_channels;
+		}
+	}
+
+	/* Enable STM Masters given in pdata */
+	if (stm.pdata->masters_enabled)
+		stm_enable_src(stm.pdata->masters_enabled);
+	stm_set_modes(STM_MMC_DEFAULT); /* Set all sources in HW mode */
+
+	dev_info(&pdev->dev, "STM-Trace driver probed successfully\n");
+	stm_printk("STM-Trace driver initialized\n");
+	return 0;
+
+err_channels:
+	iounmap(stm_channels);
+	return retval;
+}
+
+static int __devexit stm_remove(struct platform_device *pdev)
+{
+	device_destroy(stm_class, MKDEV(stm_major, 0));
+	cdev_del(&cdev);
+
+	if (stm.pdata->stm_connection)
+		(void) stm.pdata->stm_connection(STM_DISCONNECT);
+
+	stm_disable_src();
+	iounmap(stm.virtbase);
+	iounmap(stm_channels);
+
+	return 0;
+}
+
+int stm_printk(const char *fmt, ...)
+{
+	int ret;
+	size_t size;
+	va_list args;
+
+	va_start(args, fmt);
+	arch_spin_lock(&stm_buf_lock);
+	size = vscnprintf((char *)stm_printk_buf,
+			sizeof(stm_printk_buf), fmt, args);
+	ret = stm_trace_buffer(stm_printk_buf, size);
+	arch_spin_unlock(&stm_buf_lock);
+	va_end(args);
+	return ret;
+}
+EXPORT_SYMBOL(stm_printk);
+
+/*
+ * Debugfs interface
+ */
+
+static int stm_connection_show(void *data, u64 *val)
+{
+	*val = stm_connection;
+	return 0;
+}
+
+static int stm_connection_set(void *data, u64 val)
+{
+	int retval = 0;
+
+	if (stm.pdata->stm_connection) {
+		stm_connection = val;
+		retval = stm.pdata->stm_connection(val);
+	}
+	return retval;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_connection_fops, stm_connection_show,
+		stm_connection_set, "%llu\n");
+
+static int stm_clockdiv_show(void *data, u64 *val)
+{
+	*val = stm_clockdiv;
+	return 0;
+}
+
+static int stm_clockdiv_set(void *data, u64 val)
+{
+	stm_set_ckdiv(val);
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_clockdiv_fops, stm_clockdiv_show,
+		stm_clockdiv_set, "%llu\n");
+
+static int stm_masters_enable_show(void *data, u64 *val)
+{
+	*val = readl(STM_TER);
+	return 0;
+}
+
+static int stm_masters_enable_set(void *data, u64 val)
+{
+	stm_enable_src(val);
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_masters_enable_fops, stm_masters_enable_show,
+		stm_masters_enable_set, "%08llx\n");
+
+static int stm_masters_modes_show(void *data, u64 *val)
+{
+	*val = stm_get_modes();
+	return 0;
+}
+
+static int stm_masters_modes_set(void *data, u64 val)
+{
+	stm_set_modes(val);
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_masters_modes_fops, stm_masters_modes_show,
+		stm_masters_modes_set, "%08llx\n");
+
+/* Count # of free channels */
+static int stm_free_channels_show(void *data, u64 *val)
+{
+	*val = stm_nb_free_channels();
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(stm_free_channels_fops, stm_free_channels_show,
+		NULL, "%lld\n");
+
+static __init int stm_init_debugfs(void)
+{
+	struct dentry *d_stm;
+
+	d_stm =  debugfs_create_dir(STM_DEV_NAME, NULL);
+	if (!d_stm)
+		return -ENOMEM;
+
+	(void) debugfs_create_file("connection",  S_IRUGO | S_IWUGO, d_stm,
+				NULL, &stm_connection_fops);
+	(void) debugfs_create_file("clockdiv", S_IRUGO | S_IWUGO, d_stm,
+				NULL, &stm_clockdiv_fops);
+	(void) debugfs_create_file("masters_enable", S_IRUGO | S_IWUGO, d_stm,
+				NULL, &stm_masters_enable_fops);
+	(void) debugfs_create_file("masters_modes", S_IRUGO | S_IWUGO, d_stm,
+				NULL, &stm_masters_modes_fops);
+	(void) debugfs_create_file("free_channels", S_IRUGO, d_stm,
+				NULL, &stm_free_channels_fops);
+	return 0;
+}
+fs_initcall(stm_init_debugfs);
+
+static struct platform_driver stm_driver = {
+	.probe = stm_probe,
+	.remove = __devexit_p(stm_remove),
+	.driver = {
+		.name = STM_DEV_NAME,
+		.owner = THIS_MODULE,
+	}
+};
+
+static int __init stm_init(void)
+{
+	int retval;
+	dev_t dev;
+
+	stm_class = class_create(THIS_MODULE, STM_DEV_NAME);
+	if (IS_ERR(stm_class)) {
+		pr_err("stm: can't register stm class\n");
+		return PTR_ERR(stm_class);
+	}
+
+	retval = alloc_chrdev_region(&dev, 0, 1, STM_DEV_NAME);
+	if (retval) {
+		pr_err("stm: can't register character device\n");
+		class_destroy(stm_class);
+		return retval;
+	}
+	stm_major = MAJOR(dev);
+	return platform_driver_register(&stm_driver);
+}
+
+static void __exit stm_exit(void)
+{
+	platform_driver_unregister(&stm_driver);
+	unregister_chrdev_region(MKDEV(stm_major, 0), 1);
+	class_destroy(stm_class);
+}
+
+arch_initcall(stm_init);   /* STM init ASAP need to wait GPIO init */
+module_exit(stm_exit);
+
+MODULE_AUTHOR("Paul Ghaleb - ST Microelectronics");
+MODULE_AUTHOR("Pierre Peiffer - ST-Ericsson");
+MODULE_AUTHOR("Philippe Langlais - ST-Ericsson");
+MODULE_DESCRIPTION("System Trace Module driver");
+MODULE_ALIAS("stm");
+MODULE_ALIAS("stm-trace");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index dabec556ebb..4c1b59aff9d 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -107,6 +107,7 @@ struct mmc_blk_data {
 	unsigned int	part_curr;
 	struct device_attribute force_ro;
 	struct device_attribute power_ro_lock;
+	struct device_attribute power_ro_lock_legacy;
 	int	area_type;
 };
 
@@ -167,6 +168,87 @@ static void mmc_blk_put(struct mmc_blk_data *md)
 	mutex_unlock(&open_lock);
 }
 
+#define EXT_CSD_BOOT_WP_PWR_WP_TEXT "pwr_ro"
+#define EXT_CSD_BOOT_WP_PERM_WP_TEXT "perm_ro"
+#define EXT_CSD_BOOT_WP_WP_DISABLED_TEXT "rw"
+static ssize_t boot_partition_ro_lock_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int ret;
+	struct mmc_blk_data *md = mmc_blk_get(dev_to_disk(dev));
+	struct mmc_card *card = md->queue.card;
+	const char *out_text;
+
+	if (card->ext_csd.boot_ro_lock
+			& EXT_CSD_BOOT_WP_B_PERM_WP_EN)
+		out_text = EXT_CSD_BOOT_WP_PERM_WP_TEXT;
+	else if (card->ext_csd.boot_ro_lock
+			& EXT_CSD_BOOT_WP_B_PWR_WP_EN)
+		out_text = EXT_CSD_BOOT_WP_PWR_WP_TEXT;
+	else
+		out_text = EXT_CSD_BOOT_WP_WP_DISABLED_TEXT;
+
+	ret = snprintf(buf, PAGE_SIZE, "%s\n", out_text);
+
+	return ret;
+}
+
+static ssize_t boot_partition_ro_lock_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	int ret;
+	struct mmc_blk_data *md, *part_md;
+	struct mmc_card *card;
+	u8 set = 0;
+
+	md = mmc_blk_get(dev_to_disk(dev));
+	card = md->queue.card;
+
+	if (!strncmp(buf, EXT_CSD_BOOT_WP_PWR_WP_TEXT,
+				strlen(EXT_CSD_BOOT_WP_PWR_WP_TEXT)))
+		set = EXT_CSD_BOOT_WP_B_PWR_WP_EN;
+	else if (!strncmp(buf, EXT_CSD_BOOT_WP_PERM_WP_TEXT,
+				strlen(EXT_CSD_BOOT_WP_PERM_WP_TEXT)))
+		set = EXT_CSD_BOOT_WP_B_PERM_WP_EN;
+
+	if (set) {
+		mmc_claim_host(card->host);
+
+		ret = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+				EXT_CSD_BOOT_WP,
+				set,
+				card->ext_csd.part_time);
+		if (ret)
+			pr_err("Boot Partition Lock failed: %d", ret);
+		else
+			card->ext_csd.boot_ro_lock = set;
+
+		mmc_release_host(card->host);
+
+		if (!ret) {
+			pr_info("%s: Locking boot partition "
+					"%s",
+					md->disk->disk_name,
+					buf);
+			set_disk_ro(md->disk, 1);
+
+			list_for_each_entry(part_md, &md->part, part)
+				if (part_md->area_type ==
+						MMC_BLK_DATA_AREA_BOOT) {
+					pr_info("%s: Locking boot partition "
+						"%s",
+						part_md->disk->disk_name,
+						buf);
+					set_disk_ro(part_md->disk, 1);
+				}
+		}
+	}
+	ret = count;
+
+	mmc_blk_put(md);
+	return ret;
+}
+
 static ssize_t power_ro_lock_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -1411,6 +1493,14 @@ static int mmc_blk_issue_rq(struct mmc_queue *mq, struct request *req)
 	struct mmc_blk_data *md = mq->data;
 	struct mmc_card *card = md->queue.card;
 
+	/*
+	 * We must make sure we have not claimed the host before
+	 * doing a flush to prevent deadlock, thus we check if
+	 * the host needs a resume first.
+	 */
+	if (mmc_host_needs_resume(card->host))
+		mmc_resume_host_sync(card->host);
+
 	if (req && !mq->mqrq_prev->req)
 		/* claim host only for the first request */
 		mmc_claim_host(card->host);
@@ -1654,9 +1744,12 @@ static void mmc_blk_remove_req(struct mmc_blk_data *md)
 		if (md->disk->flags & GENHD_FL_UP) {
 			device_remove_file(disk_to_dev(md->disk), &md->force_ro);
 			if ((md->area_type & MMC_BLK_DATA_AREA_BOOT) &&
-					card->ext_csd.boot_ro_lockable)
+					card->ext_csd.boot_ro_lockable) {
 				device_remove_file(disk_to_dev(md->disk),
 					&md->power_ro_lock);
+				device_remove_file(disk_to_dev(md->disk),
+					&md->power_ro_lock_legacy);
+			}
 
 			/* Stop new requests from getting into the queue */
 			del_gendisk(md->disk);
@@ -1716,9 +1809,24 @@ static int mmc_add_disk(struct mmc_blk_data *md)
 				&md->power_ro_lock);
 		if (ret)
 			goto power_ro_lock_fail;
+
+		/* Legacy mode */
+		mode = S_IRUGO | S_IWUSR;
+
+		md->power_ro_lock_legacy.show = boot_partition_ro_lock_show;
+		md->power_ro_lock_legacy.store = boot_partition_ro_lock_store;
+		sysfs_attr_init(&md->power_ro_lock_legacy.attr);
+		md->power_ro_lock_legacy.attr.mode = mode;
+		md->power_ro_lock_legacy.attr.name = "ro_lock";
+		ret = device_create_file(disk_to_dev(md->disk),
+				&md->power_ro_lock_legacy);
+		if (ret)
+			goto power_ro_lock_fail_legacy;
 	}
 	return ret;
 
+power_ro_lock_fail_legacy:
+	device_remove_file(disk_to_dev(md->disk), &md->power_ro_lock);
 power_ro_lock_fail:
 	device_remove_file(disk_to_dev(md->disk), &md->force_ro);
 force_ro_fail:
diff --git a/drivers/mmc/card/mmc_test.c b/drivers/mmc/card/mmc_test.c
index 759714ed6be..6622f2e6e05 100644
--- a/drivers/mmc/card/mmc_test.c
+++ b/drivers/mmc/card/mmc_test.c
@@ -1253,6 +1253,130 @@ static int mmc_test_align_multi_read(struct mmc_test_card *test)
 	return 0;
 }
 
+
+/* helper function for various address alignment and sg length alignment */
+static int mmc_test_align_multi(struct mmc_test_card *test, bool do_write,
+				struct scatterlist *sg,
+				u32 *sizes, int sg_len, int offset)
+{
+	int ret, i;
+	unsigned int size;
+	u32 buf_off;
+	u32 sg_size;
+
+	if (test->card->host->max_blk_count == 1)
+		return RESULT_UNSUP_HOST;
+
+	size = PAGE_SIZE * 2;
+	size = min(size, test->card->host->max_req_size);
+	size = min(size, test->card->host->max_seg_size);
+	size = min(size, test->card->host->max_blk_count * 512);
+	size -= offset;
+	size -= size % 512;
+
+	if (size < 1024)
+		return RESULT_UNSUP_HOST;
+
+	for (i = 0, sg_size = 0;
+	     i < sg_len && sg_size + sizes[i] < size; i++)
+		sg_size += sizes[i];
+
+	if (sg_size < size)
+		sizes[i-1] += size - sg_size;
+	sg_len = i;
+
+	sg_init_table(sg, sg_len);
+	for (i = 0, buf_off = offset; i < sg_len; i++) {
+		sg_set_buf(&sg[i], test->buffer + buf_off, sizes[i]);
+		buf_off += sizes[i];
+	}
+
+	ret = mmc_test_transfer(test, sg, sg_len, 0, size/512, 512, do_write);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int mmc_test_align_length_32(struct mmc_test_card *test, bool do_write)
+{
+	u32 sizes[] = {512, 32*1, 32*2, 32*3, 32*4, 32*5, 32*6, 32*7,
+		       32*8, 32*9, 32*10, 32*11, 32*12, 32*13, 2048};
+	struct scatterlist sg[ARRAY_SIZE(sizes)];
+
+	return mmc_test_align_multi(test, do_write, sg, sizes,
+				    ARRAY_SIZE(sg), 0);
+}
+
+static int mmc_test_align_length_4(struct mmc_test_card *test, bool do_write)
+{
+	u32 sizes[] = {512, 4*1, 4*2, 4*3, 4*4, 4*5, 4*6, 4*7,
+		       4*8, 4*9, 520, 1040, 2080};
+	struct scatterlist sg[ARRAY_SIZE(sizes)];
+
+	return mmc_test_align_multi(test, do_write, sg, sizes,
+				    ARRAY_SIZE(sg), 0);
+}
+
+static int mmc_test_align_length_4_write(struct mmc_test_card *test)
+{
+	bool do_write = true;
+	return mmc_test_align_length_4(test, do_write);
+}
+
+static int mmc_test_align_length_4_read(struct mmc_test_card *test)
+{
+	bool do_write = false;
+	return mmc_test_align_length_4(test, do_write);
+}
+
+static int mmc_test_align_length_32_write(struct mmc_test_card *test)
+{
+	bool do_write = true;
+	return mmc_test_align_length_32(test, do_write);
+}
+
+static int mmc_test_align_length_32_read(struct mmc_test_card *test)
+{
+	bool do_write = false;
+	return mmc_test_align_length_32(test, do_write);
+}
+
+/* helper function for testing address alignment */
+static int mmc_test_align_address(struct mmc_test_card *test, bool do_write,
+				  u32 offset)
+{
+	u32 sizes[] = {512, 512, 1024, 1024, 2048};
+	struct scatterlist sg[ARRAY_SIZE(sizes)];
+
+	return mmc_test_align_multi(test, do_write, sg,
+				    sizes, ARRAY_SIZE(sg), offset);
+}
+
+static int mmc_test_align_address_4_write(struct mmc_test_card *test)
+{
+	bool do_write = true;
+	return mmc_test_align_address(test, do_write, 4);
+}
+
+static int mmc_test_align_address_4_read(struct mmc_test_card *test)
+{
+	bool do_write = false;
+	return mmc_test_align_address(test, do_write, 4);
+}
+
+static int mmc_test_align_address_32_write(struct mmc_test_card *test)
+{
+	bool do_write = true;
+	return mmc_test_align_address(test, do_write, 32);
+}
+
+static int mmc_test_align_address_32_read(struct mmc_test_card *test)
+{
+	bool do_write = false;
+	return mmc_test_align_address(test, do_write, 32);
+}
+
 static int mmc_test_xfersize_write(struct mmc_test_card *test)
 {
 	int ret;
@@ -2451,6 +2575,62 @@ static const struct mmc_test_case mmc_test_cases[] = {
 	},
 
 	{
+		.name = "4 bytes aligned sg-element length write",
+		.prepare = mmc_test_prepare_write,
+		.run = mmc_test_align_length_4_write,
+		.cleanup = mmc_test_cleanup,
+	},
+
+	{
+		.name = "4 bytes aligned sg-element length read",
+		.prepare = mmc_test_prepare_read,
+		.run = mmc_test_align_length_4_read,
+		.cleanup = mmc_test_cleanup,
+	},
+
+	{
+		.name = "32 bytes aligned sg-element length write",
+		.prepare = mmc_test_prepare_write,
+		.run = mmc_test_align_length_32_write,
+		.cleanup = mmc_test_cleanup,
+	},
+
+	{
+		.name = "32 bytes aligned sg-element length read",
+		.prepare = mmc_test_prepare_read,
+		.run = mmc_test_align_length_32_read,
+		.cleanup = mmc_test_cleanup,
+	},
+
+	{
+		.name = "4 bytes aligned sg-element address write",
+		.prepare = mmc_test_prepare_write,
+		.run = mmc_test_align_address_4_write,
+		.cleanup = mmc_test_cleanup,
+	},
+
+	{
+		.name = "4 bytes aligned sg-element address read",
+		.prepare = mmc_test_prepare_read,
+		.run = mmc_test_align_address_4_read,
+		.cleanup = mmc_test_cleanup,
+	},
+
+	{
+		.name = "32 bytes aligned sg-element address write",
+		.prepare = mmc_test_prepare_write,
+		.run = mmc_test_align_address_32_write,
+		.cleanup = mmc_test_cleanup,
+	},
+
+	{
+		.name = "32 bytes aligned sg-element address read",
+		.prepare = mmc_test_prepare_read,
+		.run = mmc_test_align_address_32_read,
+		.cleanup = mmc_test_cleanup,
+	},
+
+	{
 		.name = "Correct xfer_size at write (start failure)",
 		.run = mmc_test_xfersize_write,
 	},
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index ba821fe70bc..9cce415f1ff 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -2010,7 +2010,7 @@ void mmc_rescan(struct work_struct *work)
 		container_of(work, struct mmc_host, detect.work);
 	int i;
 
-	if (host->rescan_disable)
+	if (host->rescan_disable || mmc_host_needs_resume(host))
 		return;
 
 	mmc_bus_get(host);
@@ -2273,8 +2273,13 @@ int mmc_suspend_host(struct mmc_host *host)
 	int err = 0;
 
 	cancel_delayed_work(&host->detect);
+	cancel_delayed_work_sync(&host->resume);
 	mmc_flush_scheduled_work();
 
+	/* Skip suspend, if deferred resume were scheduled but not completed. */
+	if (mmc_host_needs_resume(host))
+		return 0;
+
 	err = mmc_cache_ctrl(host, 0);
 	if (err)
 		goto out;
@@ -2300,6 +2305,10 @@ int mmc_suspend_host(struct mmc_host *host)
 			mmc_release_host(host);
 			host->pm_flags = 0;
 			err = 0;
+		} else if (mmc_card_mmc(host->card) ||
+			   mmc_card_sd(host->card)) {
+			host->pm_state |= MMC_HOST_DEFERRED_RESUME |
+					  MMC_HOST_NEEDS_RESUME;
 		}
 	}
 	mmc_bus_put(host);
@@ -2321,6 +2330,12 @@ int mmc_resume_host(struct mmc_host *host)
 {
 	int err = 0;
 
+	if (mmc_host_deferred_resume(host)) {
+		mmc_schedule_delayed_work(&host->resume,
+					  msecs_to_jiffies(3000));
+		return 0;
+	}
+
 	mmc_bus_get(host);
 	if (host->bus_ops && !host->bus_dead) {
 		if (!mmc_card_keep_power(host)) {
@@ -2355,6 +2370,24 @@ int mmc_resume_host(struct mmc_host *host)
 }
 EXPORT_SYMBOL(mmc_resume_host);
 
+void mmc_resume_work(struct work_struct *work)
+{
+	struct mmc_host *host =
+		container_of(work, struct mmc_host, resume.work);
+
+	host->pm_state &= ~MMC_HOST_DEFERRED_RESUME;
+	mmc_resume_host(host);
+	host->pm_state &= ~MMC_HOST_NEEDS_RESUME;
+
+	mmc_detect_change(host, 0);
+}
+
+void mmc_resume_host_sync(struct mmc_host *host)
+{
+	flush_delayed_work_sync(&host->resume);
+}
+EXPORT_SYMBOL(mmc_resume_host_sync);
+
 /* Do the card removal on suspend if card is assumed removeable
  * Do that in pm notifier while userspace isn't yet frozen, so we will be able
    to sync the card.
diff --git a/drivers/mmc/core/core.h b/drivers/mmc/core/core.h
index 3bdafbca354..5796d2d85f4 100644
--- a/drivers/mmc/core/core.h
+++ b/drivers/mmc/core/core.h
@@ -59,6 +59,7 @@ static inline void mmc_delay(unsigned int ms)
 void mmc_rescan(struct work_struct *work);
 void mmc_start_host(struct mmc_host *host);
 void mmc_stop_host(struct mmc_host *host);
+void mmc_resume_work(struct work_struct *work);
 
 int _mmc_detect_card_removed(struct mmc_host *host);
 
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index 91c84c7a182..d87d1152ea2 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -330,6 +330,7 @@ struct mmc_host *mmc_alloc_host(int extra, struct device *dev)
 	spin_lock_init(&host->lock);
 	init_waitqueue_head(&host->wq);
 	INIT_DELAYED_WORK(&host->detect, mmc_rescan);
+	INIT_DELAYED_WORK(&host->resume, mmc_resume_work);
 #ifdef CONFIG_PM
 	host->pm_notify.notifier_call = mmc_pm_notify;
 #endif
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index 032b84791a1..7df4119aaa2 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -19,6 +19,7 @@
 #include <linux/err.h>
 #include <linux/highmem.h>
 #include <linux/log2.h>
+#include <linux/mmc/pm.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/card.h>
 #include <linux/amba/bus.h>
@@ -46,6 +47,7 @@ static unsigned int fmax = 515633;
  * struct variant_data - MMCI variant-specific quirks
  * @clkreg: default value for MCICLOCK register
  * @clkreg_enable: enable value for MMCICLOCK register
+ * @dma_sdio_req_ctrl: enable value for DMAREQCTL register for SDIO write
  * @datalength_bits: number of bits in the MMCIDATALENGTH register
  * @fifosize: number of bytes that can be written when MMCI_TXFIFOEMPTY
  *	      is asserted (likewise for RX)
@@ -56,10 +58,13 @@ static unsigned int fmax = 515633;
  * @blksz_datactrl16: true if Block size is at b16..b30 position in datactrl register
  * @pwrreg_powerup: power up value for MMCIPOWER register
  * @signal_direction: input/out direction of bus signals can be indicated
+ * @non_power_of_2_blksize: true if block sizes can be other than power of two
+ * @pwrreg_ctrl_power: bits in MMCIPOWER register controls ext. power supply
  */
 struct variant_data {
 	unsigned int		clkreg;
 	unsigned int		clkreg_enable;
+	unsigned int		dma_sdio_req_ctrl;
 	unsigned int		datalength_bits;
 	unsigned int		fifosize;
 	unsigned int		fifohalfsize;
@@ -68,6 +73,8 @@ struct variant_data {
 	bool			blksz_datactrl16;
 	u32			pwrreg_powerup;
 	bool			signal_direction;
+	bool			non_power_of_2_blksize;
+	bool			pwrreg_ctrl_power;
 };
 
 static struct variant_data variant_arm = {
@@ -75,6 +82,7 @@ static struct variant_data variant_arm = {
 	.fifohalfsize		= 8 * 4,
 	.datalength_bits	= 16,
 	.pwrreg_powerup		= MCI_PWR_UP,
+	.pwrreg_ctrl_power	= true,
 };
 
 static struct variant_data variant_arm_extended_fifo = {
@@ -82,6 +90,7 @@ static struct variant_data variant_arm_extended_fifo = {
 	.fifohalfsize		= 64 * 4,
 	.datalength_bits	= 16,
 	.pwrreg_powerup		= MCI_PWR_UP,
+	.pwrreg_ctrl_power	= true,
 };
 
 static struct variant_data variant_u300 = {
@@ -99,6 +108,7 @@ static struct variant_data variant_ux500 = {
 	.fifohalfsize		= 8 * 4,
 	.clkreg			= MCI_CLK_ENABLE,
 	.clkreg_enable		= MCI_ST_UX500_HWFCEN,
+	.dma_sdio_req_ctrl	= MCI_ST_DPSM_DMAREQCTL,
 	.datalength_bits	= 24,
 	.sdio			= true,
 	.st_clkdiv		= true,
@@ -111,15 +121,42 @@ static struct variant_data variant_ux500v2 = {
 	.fifohalfsize		= 8 * 4,
 	.clkreg			= MCI_CLK_ENABLE,
 	.clkreg_enable		= MCI_ST_UX500_HWFCEN,
+	.dma_sdio_req_ctrl	= MCI_ST_DPSM_DMAREQCTL,
 	.datalength_bits	= 24,
 	.sdio			= true,
 	.st_clkdiv		= true,
 	.blksz_datactrl16	= true,
 	.pwrreg_powerup		= MCI_PWR_ON,
 	.signal_direction	= true,
+	.non_power_of_2_blksize	= true,
 };
 
 /*
+ * Validate mmc prerequisites
+ */
+static int mmci_validate_data(struct mmci_host *host,
+			      struct mmc_data *data)
+{
+	if (!data)
+		return 0;
+
+	if (!host->variant->non_power_of_2_blksize &&
+	    !is_power_of_2(data->blksz)) {
+		dev_err(mmc_dev(host->mmc),
+			"unsupported block size (%d bytes)\n", data->blksz);
+		return -EINVAL;
+	}
+
+	if (data->sg->offset & 3) {
+		dev_err(mmc_dev(host->mmc),
+			"unsupported alginment (0x%x)\n", data->sg->offset);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
  * This must be called with host->lock held
  */
 static void mmci_write_clkreg(struct mmci_host *host, u32 clk)
@@ -228,6 +265,7 @@ static void mmci_stop_data(struct mmci_host *host)
 	writel(0, host->base + MMCIDATACTRL);
 	mmci_set_mask1(host, 0);
 	host->data = NULL;
+	host->datactrl_reg = 0;
 }
 
 static void mmci_init_sg(struct mmci_host *host, struct mmc_data *data)
@@ -338,10 +376,33 @@ static inline void mmci_dma_release(struct mmci_host *host)
 	host->dma_rx_channel = host->dma_tx_channel = NULL;
 }
 
+static void mmci_dma_data_error(struct mmci_host *host, struct mmc_data *data)
+{
+	dev_err(mmc_dev(host->mmc), "error during DMA transfer!\n");
+	dmaengine_terminate_all(host->dma_current);
+	host->dma_current = NULL;
+	host->dma_desc_current = NULL;
+	data->host_cookie = 0;
+}
+
 static void mmci_dma_unmap(struct mmci_host *host, struct mmc_data *data)
 {
-	struct dma_chan *chan = host->dma_current;
+	struct dma_chan *chan;
 	enum dma_data_direction dir;
+
+	if (data->flags & MMC_DATA_READ) {
+		dir = DMA_FROM_DEVICE;
+		chan = host->dma_rx_channel;
+	} else {
+		dir = DMA_TO_DEVICE;
+		chan = host->dma_tx_channel;
+	}
+
+	dma_unmap_sg(chan->device->dev, data->sg, data->sg_len, dir);
+}
+
+static void mmci_dma_finalize(struct mmci_host *host, struct mmc_data *data)
+{
 	u32 status;
 	int i;
 
@@ -360,19 +421,12 @@ static void mmci_dma_unmap(struct mmci_host *host, struct mmc_data *data)
 	 * contiguous buffers.  On TX, we'll get a FIFO underrun error.
 	 */
 	if (status & MCI_RXDATAAVLBLMASK) {
-		dmaengine_terminate_all(chan);
-		if (!data->error)
-			data->error = -EIO;
-	}
-
-	if (data->flags & MMC_DATA_WRITE) {
-		dir = DMA_TO_DEVICE;
-	} else {
-		dir = DMA_FROM_DEVICE;
+		data->error = -EIO;
+		mmci_dma_data_error(host, data);
 	}
 
 	if (!data->host_cookie)
-		dma_unmap_sg(chan->device->dev, data->sg, data->sg_len, dir);
+		mmci_dma_unmap(host, data);
 
 	/*
 	 * Use of DMA with scatter-gather is impossible.
@@ -382,16 +436,15 @@ static void mmci_dma_unmap(struct mmci_host *host, struct mmc_data *data)
 		dev_err(mmc_dev(host->mmc), "buggy DMA detected. Taking evasive action.\n");
 		mmci_dma_release(host);
 	}
-}
 
-static void mmci_dma_data_error(struct mmci_host *host)
-{
-	dev_err(mmc_dev(host->mmc), "error during DMA transfer!\n");
-	dmaengine_terminate_all(host->dma_current);
+	host->dma_current = NULL;
+	host->dma_desc_current = NULL;
 }
 
-static int mmci_dma_prep_data(struct mmci_host *host, struct mmc_data *data,
-			      struct mmci_host_next *next)
+/* prepares DMA channel and DMA descriptor, returns non-zero on failure */
+static int __mmci_dma_prep_data(struct mmci_host *host, struct mmc_data *data,
+				struct dma_chan **dma_chan,
+				struct dma_async_tx_descriptor **dma_desc)
 {
 	struct variant_data *variant = host->variant;
 	struct dma_slave_config conf = {
@@ -409,16 +462,6 @@ static int mmci_dma_prep_data(struct mmci_host *host, struct mmc_data *data,
 	enum dma_data_direction buffer_dirn;
 	int nr_sg;
 
-	/* Check if next job is already prepared */
-	if (data->host_cookie && !next &&
-	    host->dma_current && host->dma_desc_current)
-		return 0;
-
-	if (!next) {
-		host->dma_current = NULL;
-		host->dma_desc_current = NULL;
-	}
-
 	if (data->flags & MMC_DATA_READ) {
 		conf.direction = DMA_DEV_TO_MEM;
 		buffer_dirn = DMA_FROM_DEVICE;
@@ -433,8 +476,12 @@ static int mmci_dma_prep_data(struct mmci_host *host, struct mmc_data *data,
 	if (!chan)
 		return -EINVAL;
 
-	/* If less than or equal to the fifo size, don't bother with DMA */
-	if (data->blksz * data->blocks <= variant->fifosize)
+	/*
+	 * If less than or equal to the fifo size, don't bother with DMA
+	 * SDIO transfers may not be 4 bytes aligned, fall back to PIO
+	 */
+	if (data->blksz * data->blocks <= variant->fifosize ||
+	    (data->blksz * data->blocks) & 3)
 		return -EINVAL;
 
 	device = chan->device;
@@ -448,29 +495,42 @@ static int mmci_dma_prep_data(struct mmci_host *host, struct mmc_data *data,
 	if (!desc)
 		goto unmap_exit;
 
-	if (next) {
-		next->dma_chan = chan;
-		next->dma_desc = desc;
-	} else {
-		host->dma_current = chan;
-		host->dma_desc_current = desc;
-	}
+	*dma_chan = chan;
+	*dma_desc = desc;
 
 	return 0;
 
  unmap_exit:
-	if (!next)
-		dmaengine_terminate_all(chan);
 	dma_unmap_sg(device->dev, data->sg, data->sg_len, buffer_dirn);
 	return -ENOMEM;
 }
 
-static int mmci_dma_start_data(struct mmci_host *host, unsigned int datactrl)
+static int inline mmci_dma_prep_data(struct mmci_host *host,
+				     struct mmc_data *data)
+{
+	/* Check if next job is already prepared. */
+	if (host->dma_current && host->dma_desc_current)
+		return 0;
+
+	/* No job were prepared thus do it now. */
+	return __mmci_dma_prep_data(host, data, &host->dma_current,
+				    &host->dma_desc_current);
+}
+
+static inline int mmci_dma_prep_next(struct mmci_host *host,
+				     struct mmc_data *data)
+{
+	struct mmci_host_next *nd = &host->next_data;
+	return __mmci_dma_prep_data(host, data, &nd->dma_chan, &nd->dma_desc);
+}
+
+static int mmci_dma_start_data(struct mmci_host *host)
 {
 	int ret;
 	struct mmc_data *data = host->data;
+	struct variant_data *variant = host->variant;
 
-	ret = mmci_dma_prep_data(host, host->data, NULL);
+	ret = mmci_dma_prep_data(host, host->data);
 	if (ret)
 		return ret;
 
@@ -481,10 +541,15 @@ static int mmci_dma_start_data(struct mmci_host *host, unsigned int datactrl)
 	dmaengine_submit(host->dma_desc_current);
 	dma_async_issue_pending(host->dma_current);
 
-	datactrl |= MCI_DPSM_DMAENABLE;
+	host->datactrl_reg |= MCI_DPSM_DMAENABLE;
+
+	/* Some hardware versions need special flags for SDIO DMA write */
+	if (variant->sdio && host->mmc->card && mmc_card_sdio(host->mmc->card)
+	    && (data->flags & MMC_DATA_WRITE))
+		host->datactrl_reg |= variant->dma_sdio_req_ctrl;
 
 	/* Trigger the DMA transfer */
-	writel(datactrl, host->base + MMCIDATACTRL);
+	writel(host->datactrl_reg, host->base + MMCIDATACTRL);
 
 	/*
 	 * Let the MMCI say when the data is ended and it's time
@@ -501,18 +566,14 @@ static void mmci_get_next_data(struct mmci_host *host, struct mmc_data *data)
 	struct mmci_host_next *next = &host->next_data;
 
 	if (data->host_cookie && data->host_cookie != next->cookie) {
-		pr_warning("[%s] invalid cookie: data->host_cookie %d"
+		pr_err("[%s] invalid cookie: data->host_cookie %d"
 		       " host->next_data.cookie %d\n",
 		       __func__, data->host_cookie, host->next_data.cookie);
-		data->host_cookie = 0;
+		BUG();
 	}
 
-	if (!data->host_cookie)
-		return;
-
 	host->dma_desc_current = next->dma_desc;
 	host->dma_current = next->dma_chan;
-
 	next->dma_desc = NULL;
 	next->dma_chan = NULL;
 }
@@ -527,19 +588,18 @@ static void mmci_pre_request(struct mmc_host *mmc, struct mmc_request *mrq,
 	if (!data)
 		return;
 
-	if (data->host_cookie) {
-		data->host_cookie = 0;
+	BUG_ON(data->host_cookie);
+
+	if (mmci_validate_data(host, data))
 		return;
-	}
 
-	/* if config for dma */
-	if (((data->flags & MMC_DATA_WRITE) && host->dma_tx_channel) ||
-	    ((data->flags & MMC_DATA_READ) && host->dma_rx_channel)) {
-		if (mmci_dma_prep_data(host, data, nd))
-			data->host_cookie = 0;
-		else
-			data->host_cookie = ++nd->cookie < 0 ? 1 : nd->cookie;
-	}
+	/*
+	 * Don't prepare DMA if there is no previous request,
+	 * is_first_req is set. Instead, prepare DMA while
+	 * start command is being issued.
+	 */
+	if (!is_first_req && !mmci_dma_prep_next(host, data))
+		data->host_cookie = ++nd->cookie < 0 ? 1 : nd->cookie;
 }
 
 static void mmci_post_request(struct mmc_host *mmc, struct mmc_request *mrq,
@@ -547,29 +607,23 @@ static void mmci_post_request(struct mmc_host *mmc, struct mmc_request *mrq,
 {
 	struct mmci_host *host = mmc_priv(mmc);
 	struct mmc_data *data = mrq->data;
-	struct dma_chan *chan;
-	enum dma_data_direction dir;
 
-	if (!data)
+	if (!data || !data->host_cookie)
 		return;
 
-	if (data->flags & MMC_DATA_READ) {
-		dir = DMA_FROM_DEVICE;
-		chan = host->dma_rx_channel;
-	} else {
-		dir = DMA_TO_DEVICE;
-		chan = host->dma_tx_channel;
-	}
+	mmci_dma_unmap(host, data);
 
+	if (err) {
+		struct mmci_host_next *next = &host->next_data;
+		struct dma_chan *chan;
+		if (data->flags & MMC_DATA_READ)
+			chan = host->dma_rx_channel;
+		else
+			chan = host->dma_tx_channel;
+		dmaengine_terminate_all(chan);
 
-	/* if config for dma */
-	if (chan) {
-		if (err)
-			dmaengine_terminate_all(chan);
-		if (data->host_cookie)
-			dma_unmap_sg(mmc_dev(host->mmc), data->sg,
-				     data->sg_len, dir);
-		mrq->data->host_cookie = 0;
+		next->dma_desc = NULL;
+		next->dma_chan = NULL;
 	}
 }
 
@@ -590,11 +644,20 @@ static inline void mmci_dma_unmap(struct mmci_host *host, struct mmc_data *data)
 {
 }
 
-static inline void mmci_dma_data_error(struct mmci_host *host)
+static inline void mmci_dma_finalize(struct mmci_host *host, struct mmc_data *data)
+{
+}
+
+static inline void mmci_dma_data_error(struct mmci_host *host, struct mmc_data *data)
+{
+}
+
+static inline int mmci_dma_start_data(struct mmci_host *host)
 {
+	return -ENOSYS;
 }
 
-static inline int mmci_dma_start_data(struct mmci_host *host, unsigned int datactrl)
+static inline int mmci_dma_prep_data(struct mmci_host *host, struct mmc_data *data)
 {
 	return -ENOSYS;
 }
@@ -604,10 +667,10 @@ static inline int mmci_dma_start_data(struct mmci_host *host, unsigned int datac
 
 #endif
 
-static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
+static void mmci_setup_datactrl(struct mmci_host *host, struct mmc_data *data)
 {
 	struct variant_data *variant = host->variant;
-	unsigned int datactrl, timeout, irqmask;
+	unsigned int datactrl, timeout;
 	unsigned long long clks;
 	void __iomem *base;
 	int blksz_bits;
@@ -629,7 +692,6 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 	writel(host->size, base + MMCIDATALENGTH);
 
 	blksz_bits = ffs(data->blksz) - 1;
-	BUG_ON(1 << blksz_bits != data->blksz);
 
 	if (variant->blksz_datactrl16)
 		datactrl = MCI_DPSM_ENABLE | (data->blksz << 16);
@@ -641,14 +703,43 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 
 	/* The ST Micro variants has a special bit to enable SDIO */
 	if (variant->sdio && host->mmc->card)
-		if (mmc_card_sdio(host->mmc->card))
+		if (mmc_card_sdio(host->mmc->card)) {
+			/*
+			 * The ST Micro variants has a special bit
+			 * to enable SDIO.
+			 */
+			u32 clk;
+
 			datactrl |= MCI_ST_DPSM_SDIOEN;
 
+			/*
+			 * The ST Micro variant for SDIO write transfer sizes
+			 * less then 8 bytes needs to have clock H/W flow
+			 * control disabled.
+			 */
+			if ((host->size < 8) &&
+			    (data->flags & MMC_DATA_WRITE))
+				clk = host->clk_reg & ~variant->clkreg_enable;
+			else
+				clk = host->clk_reg | variant->clkreg_enable;
+
+			mmci_write_clkreg(host, clk);
+		}
+	host->datactrl_reg = datactrl;
+	writel(datactrl, base + MMCIDATACTRL);
+}
+
+static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
+{
+	unsigned int irqmask;
+	struct variant_data *variant = host->variant;
+	void __iomem *base = host->base;
+
 	/*
 	 * Attempt to use DMA operation mode, if this
 	 * should fail, fall back to PIO mode
 	 */
-	if (!mmci_dma_start_data(host, datactrl))
+	if (!mmci_dma_start_data(host))
 		return;
 
 	/* IRQ mode, map the SG list for CPU reading/writing */
@@ -672,7 +763,6 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 		irqmask = MCI_TXFIFOHALFEMPTYMASK;
 	}
 
-	writel(datactrl, base + MMCIDATACTRL);
 	writel(readl(base + MMCIMASK0) & ~MCI_DATAENDMASK, base + MMCIMASK0);
 	mmci_set_mask1(host, irqmask);
 }
@@ -699,6 +789,14 @@ mmci_start_command(struct mmci_host *host, struct mmc_command *cmd, u32 c)
 	if (/*interrupt*/0)
 		c |= MCI_CPSM_INTERRUPT;
 
+	/*
+	 * For levelshifters we must not use more than 25MHz when
+	 * sending commands.
+	 */
+	host->cclk_desired = host->cclk;
+	if (host->plat->ios_handler && (host->cclk_desired > 25000000))
+		mmci_set_clkreg(host, 25000000);
+
 	host->cmd = cmd;
 
 	writel(cmd->arg, base + MMCIARGUMENT);
@@ -715,8 +813,10 @@ mmci_data_irq(struct mmci_host *host, struct mmc_data *data,
 		u32 remain, success;
 
 		/* Terminate the DMA transfer */
-		if (dma_inprogress(host))
-			mmci_dma_data_error(host);
+		if (dma_inprogress(host)) {
+			mmci_dma_data_error(host, data);
+			mmci_dma_unmap(host, data);
+		}
 
 		/*
 		 * Calculate how far we are into the transfer.  Note that
@@ -755,7 +855,7 @@ mmci_data_irq(struct mmci_host *host, struct mmc_data *data,
 
 	if (status & MCI_DATAEND || data->error) {
 		if (dma_inprogress(host))
-			mmci_dma_unmap(host, data);
+			mmci_dma_finalize(host, data);
 		mmci_stop_data(host);
 
 		if (!data->error)
@@ -789,15 +889,24 @@ mmci_cmd_irq(struct mmci_host *host, struct mmc_command *cmd,
 		cmd->resp[3] = readl(base + MMCIRESPONSE3);
 	}
 
+	/*
+	 * For levelshifters we might have decreased cclk to 25MHz when
+	 * sending commands, then we restore the frequency here.
+	 */
+	if (host->plat->ios_handler && (host->cclk_desired > host->cclk))
+		mmci_set_clkreg(host, host->cclk_desired);
+
 	if (!cmd->data || cmd->error) {
-		if (host->data) {
-			/* Terminate the DMA transfer */
-			if (dma_inprogress(host))
-				mmci_dma_data_error(host);
-			mmci_stop_data(host);
+		/* Terminate the DMA transfer */
+		if (dma_inprogress(host)) {
+			mmci_dma_data_error(host, host->mrq->data);
+			mmci_dma_unmap(host, host->mrq->data);
 		}
+		if (host->data)
+			mmci_stop_data(host);
 		mmci_request_end(host, cmd->mrq);
 	} else if (!(cmd->data->flags & MMC_DATA_READ)) {
+		mmci_setup_datactrl(host, cmd->data);
 		mmci_start_data(host, cmd->data);
 	}
 }
@@ -864,22 +973,6 @@ static int mmci_pio_write(struct mmci_host *host, char *buffer, unsigned int rem
 		count = min(remain, maxcnt);
 
 		/*
-		 * The ST Micro variant for SDIO transfer sizes
-		 * less then 8 bytes should have clock H/W flow
-		 * control disabled.
-		 */
-		if (variant->sdio &&
-		    mmc_card_sdio(host->mmc->card)) {
-			u32 clk;
-			if (count < 8)
-				clk = host->clk_reg & ~variant->clkreg_enable;
-			else
-				clk = host->clk_reg | variant->clkreg_enable;
-
-			mmci_write_clkreg(host, clk);
-		}
-
-		/*
 		 * SDIO especially may want to send something that is
 		 * not divisible by 4 (as opposed to card sectors
 		 * etc), and the FIFO only accept full 32-bit writes.
@@ -1032,13 +1125,12 @@ static void mmci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 {
 	struct mmci_host *host = mmc_priv(mmc);
 	unsigned long flags;
+	bool dmaprep_after_cmd = false;
 
 	WARN_ON(host->mrq != NULL);
 
-	if (mrq->data && !is_power_of_2(mrq->data->blksz)) {
-		dev_err(mmc_dev(mmc), "unsupported block size (%d bytes)\n",
-			mrq->data->blksz);
-		mrq->cmd->error = -EINVAL;
+	mrq->cmd->error = mmci_validate_data(host, mrq->data);
+	if (mrq->cmd->error) {
 		mmc_request_done(mmc, mrq);
 		return;
 	}
@@ -1049,14 +1141,28 @@ static void mmci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 
 	host->mrq = mrq;
 
-	if (mrq->data)
+	if (mrq->data) {
+		dmaprep_after_cmd =
+			(host->variant->clkreg_enable &&
+			 (mrq->data->flags & MMC_DATA_READ)) ||
+			!(mrq->data->flags & MMC_DATA_READ);
 		mmci_get_next_data(host, mrq->data);
-
-	if (mrq->data && mrq->data->flags & MMC_DATA_READ)
-		mmci_start_data(host, mrq->data);
+		if (mrq->data->flags & MMC_DATA_READ) {
+			mmci_setup_datactrl(host, mrq->data);
+			if (!dmaprep_after_cmd)
+				mmci_start_data(host, mrq->data);
+		}
+	}
 
 	mmci_start_command(host, mrq->cmd, 0);
 
+	if (mrq->data && dmaprep_after_cmd) {
+		mmci_dma_prep_data(host, mrq->data);
+
+		if (mrq->data->flags & MMC_DATA_READ)
+			mmci_start_data(host, mrq->data);
+	}
+
 	spin_unlock_irqrestore(&host->lock, flags);
 }
 
@@ -1178,6 +1284,21 @@ static int mmci_get_cd(struct mmc_host *mmc)
 	return status;
 }
 
+static int mmci_sig_volt_switch(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct mmci_host *host = mmc_priv(mmc);
+	int ret = 0;
+
+	if (host->plat->ios_handler) {
+		pm_runtime_get_sync(mmc_dev(mmc));
+		ret = host->plat->ios_handler(mmc_dev(mmc), ios);
+		pm_runtime_mark_last_busy(mmc_dev(mmc));
+		pm_runtime_put_autosuspend(mmc_dev(mmc));
+	}
+
+	return ret;
+}
+
 static irqreturn_t mmci_cd_irq(int irq, void *dev_id)
 {
 	struct mmci_host *host = dev_id;
@@ -1194,6 +1315,7 @@ static const struct mmc_host_ops mmci_ops = {
 	.set_ios	= mmci_set_ios,
 	.get_ro		= mmci_get_ro,
 	.get_cd		= mmci_get_cd,
+	.start_signal_voltage_switch = mmci_sig_volt_switch,
 };
 
 static int __devinit mmci_probe(struct amba_device *dev,
@@ -1321,6 +1443,9 @@ static int __devinit mmci_probe(struct amba_device *dev,
 	mmc->caps = plat->capabilities;
 	mmc->caps2 = plat->capabilities2;
 
+	/* We support these PM capabilities. */
+	mmc->pm_caps = MMC_PM_KEEP_POWER;
+
 	/*
 	 * We can do SGIO
 	 */
@@ -1390,7 +1515,8 @@ static int __devinit mmci_probe(struct amba_device *dev,
 	}
 
 	if ((host->plat->status || host->gpio_cd != -ENOSYS)
-	    && host->gpio_cd_irq < 0)
+		&& host->gpio_cd_irq < 0
+		&& !(mmc->caps & MMC_CAP_NONREMOVABLE))
 		mmc->caps |= MMC_CAP_NEEDS_POLL;
 
 	ret = request_irq(dev->irq[0], mmci_irq, IRQF_SHARED, DRIVER_NAME " (cmd)", host);
@@ -1503,6 +1629,75 @@ static int __devexit mmci_remove(struct amba_device *dev)
 	return 0;
 }
 
+#if defined(CONFIG_SUSPEND) || defined(CONFIG_PM_RUNTIME)
+static int mmci_save(struct amba_device *dev)
+{
+	struct mmc_host *mmc = amba_get_drvdata(dev);
+	unsigned long flags;
+	struct mmc_ios ios;
+	int ret = 0;
+
+	if (mmc) {
+		struct mmci_host *host = mmc_priv(mmc);
+
+		/* Let the ios_handler act on a POWER_OFF to save power. */
+		if (host->plat->ios_handler) {
+			memcpy(&ios, &mmc->ios, sizeof(struct mmc_ios));
+			ios.power_mode = MMC_POWER_OFF;
+			ret = host->plat->ios_handler(mmc_dev(mmc),
+						      &ios);
+			if (ret)
+				return ret;
+		}
+
+		spin_lock_irqsave(&host->lock, flags);
+
+		/*
+		 * Make sure we do not get any interrupts when we disabled the
+		 * clock and the regulator and as well make sure to clear the
+		 * registers for clock and power.
+		 */
+		writel(0, host->base + MMCIMASK0);
+		writel(0, host->base + MMCIPOWER);
+		writel(0, host->base + MMCICLOCK);
+
+		spin_unlock_irqrestore(&host->lock, flags);
+
+		clk_disable(host->clk);
+	}
+
+	return ret;
+}
+
+static int mmci_restore(struct amba_device *dev)
+{
+	struct mmc_host *mmc = amba_get_drvdata(dev);
+	unsigned long flags;
+
+	if (mmc) {
+		struct mmci_host *host = mmc_priv(mmc);
+
+		clk_enable(host->clk);
+
+		spin_lock_irqsave(&host->lock, flags);
+
+		/* Restore registers and re-enable interrupts. */
+		writel(host->clk_reg, host->base + MMCICLOCK);
+		writel(host->pwr_reg, host->base + MMCIPOWER);
+		writel(MCI_IRQENABLE, host->base + MMCIMASK0);
+
+		spin_unlock_irqrestore(&host->lock, flags);
+
+		/* Restore settings done by the ios_handler. */
+		if (host->plat->ios_handler)
+			host->plat->ios_handler(mmc_dev(mmc),
+						&mmc->ios);
+	}
+
+	return 0;
+}
+#endif
+
 #ifdef CONFIG_SUSPEND
 static int mmci_suspend(struct device *dev)
 {
@@ -1511,12 +1706,11 @@ static int mmci_suspend(struct device *dev)
 	int ret = 0;
 
 	if (mmc) {
-		struct mmci_host *host = mmc_priv(mmc);
-
 		ret = mmc_suspend_host(mmc);
 		if (ret == 0) {
 			pm_runtime_get_sync(dev);
-			writel(0, host->base + MMCIMASK0);
+			mmci_save(adev);
+			amba_pclk_disable(adev);
 		}
 	}
 
@@ -1530,9 +1724,8 @@ static int mmci_resume(struct device *dev)
 	int ret = 0;
 
 	if (mmc) {
-		struct mmci_host *host = mmc_priv(mmc);
-
-		writel(MCI_IRQENABLE, host->base + MMCIMASK0);
+		amba_pclk_enable(adev);
+		mmci_restore(adev);
 		pm_runtime_put(dev);
 
 		ret = mmc_resume_host(mmc);
@@ -1542,8 +1735,43 @@ static int mmci_resume(struct device *dev)
 }
 #endif
 
+#ifdef CONFIG_PM_RUNTIME
+static int mmci_runtime_suspend(struct device *dev)
+{
+	struct amba_device *adev = to_amba_device(dev);
+	struct mmc_host *mmc = amba_get_drvdata(adev);
+	int ret = 0;
+
+	if (mmc) {
+		struct mmci_host *host = mmc_priv(mmc);
+		struct variant_data *variant = host->variant;
+		if (!variant->pwrreg_ctrl_power)
+			ret = mmci_save(adev);
+	}
+
+	return ret;
+}
+
+static int mmci_runtime_resume(struct device *dev)
+{
+	struct amba_device *adev = to_amba_device(dev);
+	struct mmc_host *mmc = amba_get_drvdata(adev);
+	int ret = 0;
+
+	if (mmc) {
+		struct mmci_host *host = mmc_priv(mmc);
+		struct variant_data *variant = host->variant;
+		if (!variant->pwrreg_ctrl_power)
+			ret = mmci_restore(adev);
+	}
+
+	return ret;
+}
+#endif
+
 static const struct dev_pm_ops mmci_dev_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(mmci_suspend, mmci_resume)
+	SET_RUNTIME_PM_OPS(mmci_runtime_suspend, mmci_runtime_resume, NULL)
 };
 
 static struct amba_id mmci_ids[] = {
diff --git a/drivers/mmc/host/mmci.h b/drivers/mmc/host/mmci.h
index d437ccf62d6..5a17beafd05 100644
--- a/drivers/mmc/host/mmci.h
+++ b/drivers/mmc/host/mmci.h
@@ -60,6 +60,13 @@
 #define MCI_ST_DPSM_RWMOD	(1 << 10)
 #define MCI_ST_DPSM_SDIOEN	(1 << 11)
 /* Control register extensions in the ST Micro Ux500 versions */
+/*
+ * DMA request control is required for write
+ * if transfer size is not 32 byte aligned.
+ * DMA request control is also needed if the total
+ * transfer size is 32 byte aligned but any of the
+ * sg element lengths are not aligned with 32 byte.
+ */
 #define MCI_ST_DPSM_DMAREQCTL	(1 << 12)
 #define MCI_ST_DPSM_DBOOTMODEEN	(1 << 13)
 #define MCI_ST_DPSM_BUSYMODE	(1 << 14)
@@ -179,8 +186,10 @@ struct mmci_host {
 
 	unsigned int		mclk;
 	unsigned int		cclk;
+	unsigned int		cclk_desired;
 	u32			pwr_reg;
 	u32			clk_reg;
+	u32			datactrl_reg;
 	struct mmci_platform_data *plat;
 	struct variant_data	*variant;
 
diff --git a/drivers/modem/Kconfig b/drivers/modem/Kconfig
new file mode 100644
index 00000000000..be8476ed0f9
--- /dev/null
+++ b/drivers/modem/Kconfig
@@ -0,0 +1,44 @@
+config MODEM
+        bool "Modem Access Framework"
+        default y
+        help
+         Add support for Modem Access Framework. It allows different
+	 platform specific drivers to register modem access mechanisms
+	 and allows transparent access to modem to the client drivers.
+
+	 If unsure, say N.
+
+config MODEM_U5500_MCDD
+	tristate "Modem crash dump detection driver for STE U5500 platform"
+	depends on (UX500_SOC_DB5500 && U5500_MODEM_IRQ && MODEM)
+	default y
+	help
+	 Add support for Modem crash detection
+	 driver for  STE U5500 platform.
+	 And inform userspace.
+
+         If unsure, say N.
+
+config MODEM_U8500
+	bool "Modem Access driver for STE U8500 platform"
+	depends on MODEM
+	default n
+	help
+	 Add support for Modem Access driver on STE U8500 platform which
+	 uses Shared Memroy as IPC mechanism between Modem processor and
+	 Application processor.
+
+	 If unsure, say N.
+
+source "drivers/modem/shrm/Kconfig"
+
+config MODEM_M6718
+	tristate "Modem Access driver for STE M6718 modem"
+	depends on MODEM
+	default n
+	help
+	 Add support for the modem access driver for the M6718 modem.
+
+	 If unsure, say N.
+
+source "drivers/modem/m6718_spi/Kconfig"
diff --git a/drivers/modem/Makefile b/drivers/modem/Makefile
new file mode 100644
index 00000000000..82921988f27
--- /dev/null
+++ b/drivers/modem/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_MODEM)       := modem_access.o
+obj-$(CONFIG_MODEM_U8500) += modem_u8500.o
+obj-$(CONFIG_U8500_SHRM)  += shrm/
+obj-$(CONFIG_MODEM_M6718) += modem_m6718.o
+obj-$(CONFIG_MODEM_M6718_SPI) += m6718_spi/
+obj-$(CONFIG_MODEM_U5500_MCDD)  += mcdd.o
diff --git a/drivers/modem/m6718_spi/Kconfig b/drivers/modem/m6718_spi/Kconfig
new file mode 100644
index 00000000000..f945d24a094
--- /dev/null
+++ b/drivers/modem/m6718_spi/Kconfig
@@ -0,0 +1,83 @@
+#
+# M6718 modem SPI IPC driver kernel configuration
+#
+config MODEM_M6718_SPI
+	tristate "M6718 modem IPC SPI driver"
+	depends on MODEM_M6718
+	default y
+	---help---
+	  If you say Y here, you will enable the M6718 modem IPC SPI driver.
+
+	  If unsure, say Y.
+
+config MODEM_M6718_SPI_DEBUG
+	boolean "Modem driver debug"
+	depends on MODEM_M6718_SPI
+	default N
+	---help---
+	   If you say Y here, you will enable full debug trace from the M6718
+	   modem driver.  This should not be enabled by default.
+
+	   If unsure, say N.
+
+config MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+	boolean "M6718 modem state driver integration"
+	depends on MODEM_M6718_SPI
+	default y
+	---help---
+	  Enables integration of the IPC driver with the modem state driver.
+	  This allows the IPC driver to be notified of changes in modem state
+	  (on, off, reset) and allows the IPC driver to cause modem state
+	  changes if needed.
+
+	  By default this should be enabled.
+
+config MODEM_M6718_SPI_ENABLE_FEATURE_FRAME_DUMP
+	boolean "IPC SPI L1 frame dump"
+	depends on MODEM_M6718_SPI
+	default n
+	---help---
+	  If you say Y here, you will enable dumping of the raw TX and RX frames
+	  by the IPC driver L1.
+
+	  If unsure, say N.
+
+config MODEM_M6718_SPI_ENABLE_FEATURE_LOOPBACK
+	boolean "Modem IPC loopback support"
+	depends on MODEM_M6718_SPI
+	default y
+	---help---
+	  If you say Y here, you will enable the IPC loopback channels/devices.
+
+	  If unsure, say Y.
+
+
+config MODEM_M6718_SPI_ENABLE_FEATURE_VERIFY_FRAMES
+	boolean "Verify loopback frames"
+	depends on MODEM_M6718_SPI
+	default n
+	---help---
+	  This will enabling checking of loopback frames to verify that the data
+	  received is identical to the data sent.
+
+	  If unsure, say N.
+
+config MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	boolean "Modem IPC throughput measurement"
+	depends on MODEM_M6718_SPI
+	default n
+	---help---
+	  If you say Y here, you will enable the IPC link throughput
+	  measurement and reporting.
+
+	  If unsure, say N.
+
+config MODEM_M6718_SPI_SET_THROUGHPUT_FREQUENCY
+	int "Sample rate for throughput measurements (seconds)"
+	default "5"
+	depends on MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	help
+	  The sample frequency for taking IPC SPI link throughput measurements.
+	  Increasing the rate (reducing the time) will increase the accuracy of
+	  the measurements, but will also increase the impact on link and system
+	  performance.
diff --git a/drivers/modem/m6718_spi/Makefile b/drivers/modem/m6718_spi/Makefile
new file mode 100644
index 00000000000..a0a82c30b07
--- /dev/null
+++ b/drivers/modem/m6718_spi/Makefile
@@ -0,0 +1,15 @@
+#
+# Makefile for M6718 SPI driver
+#
+ifeq ($(CONFIG_MODEM_M6718_SPI_DEBUG),y)
+EXTRA_CFLAGS += -DDEBUG
+endif
+
+m6718_modem_spi-objs := modem_driver.o protocol.o util.o queue.o debug.o \
+			netlink.o statemachine.o
+
+ifeq ($(CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE),y)
+m6718_modem_spi-objs += modem_state.o
+endif
+
+obj-$(CONFIG_MODEM_M6718_SPI) += m6718_modem_spi.o
diff --git a/drivers/modem/m6718_spi/debug.c b/drivers/modem/m6718_spi/debug.c
new file mode 100644
index 00000000000..06ee4c34a5a
--- /dev/null
+++ b/drivers/modem/m6718_spi/debug.c
@@ -0,0 +1,482 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010,2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * U9500 <-> M6718 IPC protocol implementation using SPI:
+ *   debug functionality.
+ */
+#include <linux/gpio.h>
+#include <linux/modem/m6718_spi/modem_driver.h>
+#include "modem_debug.h"
+#include "modem_private.h"
+#include "modem_util.h"
+#include "modem_queue.h"
+
+/* name of each state - must match enum ipc_sm_state_id */
+static const char * const sm_state_id_str[] = {
+	"IPC_INIT",
+	"IPC_HALT",
+	"IPC_RESET",
+	"IPC_WAIT_SLAVE_STABLE",
+	"IPC_WAIT_HANDSHAKE_INACTIVE",
+	"IPC_SLW_TX_BOOTREQ",
+	"IPC_ACT_TX_BOOTREQ",
+	"IPC_SLW_RX_BOOTRESP",
+	"IPC_ACT_RX_BOOTRESP",
+	"IPC_IDL",
+	"IPC_SLW_TX_WR_CMD",
+	"IPC_ACT_TX_WR_CMD",
+	"IPC_SLW_TX_WR_DAT",
+	"IPC_ACT_TX_WR_DAT",
+	"IPC_SLW_TX_RD_CMD",
+	"IPC_ACT_TX_RD_CMD",
+	"IPC_SLW_RX_WR_CMD",
+	"IPC_ACT_RX_WR_CMD",
+	"IPC_ACT_RX_WR_DAT",
+};
+
+/* name of each state machine run cause */
+static const char * const sm_run_cause_str[] = {
+	[IPC_SM_RUN_NONE]         = "IPC_SM_RUN_NONE",
+	[IPC_SM_RUN_SLAVE_IRQ]    = "IPC_SM_RUN_SLAVE_IRQ",
+	[IPC_SM_RUN_TFR_COMPLETE] = "IPC_SM_RUN_TFR_COMPLETE",
+	[IPC_SM_RUN_TX_REQ]       = "IPC_SM_RUN_TX_REQ",
+	[IPC_SM_RUN_INIT]         = "IPC_SM_RUN_INIT",
+	[IPC_SM_RUN_ABORT]        = "IPC_SM_RUN_ABORT",
+	[IPC_SM_RUN_COMMS_TMO]    = "IPC_SM_RUN_COMMS_TMO",
+	[IPC_SM_RUN_STABLE_TMO]   = "IPC_SM_RUN_STABLE_TMO",
+	[IPC_SM_RUN_RESET]        = "IPC_SM_RUN_RESET"
+};
+
+
+#if defined DUMP_SPI_TFRS || \
+	defined CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_FRAME_DUMP
+static const char *format_buf(const void *buffer, int len)
+{
+	static char dumpbuf[6000];
+	char *wr = dumpbuf;
+	const char *rd = buffer;
+	int maxlen = min(len, (int)(sizeof(dumpbuf) / 3));
+	int i;
+
+	for (i = 0 ; i < maxlen ; i++) {
+		sprintf(wr, "%02x ", rd[i]);
+		wr += 3;
+	}
+	return dumpbuf;
+}
+#endif
+
+void ipc_dbg_dump_frame(struct device *dev, int linkid,
+	struct ipc_tx_queue *frame, bool tx)
+{
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_FRAME_DUMP
+	if (frame->actual_len == 0)
+		return;
+
+	/*
+	 * Use printk(KERN_DEBUG... directly to ensure these are printed even
+	 * when DEBUG is not defined for this device - we want to be able to
+	 * dump the frames independently from the debug logging.
+	 */
+	printk(KERN_DEBUG "IPC link%d %s %3d %4d bytes:%s\n",
+		linkid, (tx ? "TX" : "RX"), frame->counter, frame->len,
+		format_buf(frame->data, frame->len));
+#endif
+}
+
+void ipc_dbg_dump_spi_tfr(struct ipc_link_context *context)
+{
+#ifdef DUMP_SPI_TFRS
+	struct spi_transfer *tfr = &context->spi_transfer;
+	struct spi_message *msg = &context->spi_message;
+
+	if (tfr->tx_buf != NULL)
+		dev_info(&context->sdev->dev, "link%d TX %4d bytes:%s\n",
+			context->link->id, msg->actual_length,
+			format_buf(tfr->tx_buf, msg->actual_length));
+
+	if (tfr->rx_buf != NULL)
+		dev_info(&context->sdev->dev, "link%d RX %4d bytes:%s\n",
+			context->link->id, msg->actual_length,
+			format_buf(tfr->rx_buf, msg->actual_length));
+#endif
+}
+
+const char *ipc_dbg_state_id(const struct ipc_sm_state *state)
+{
+	if (state == NULL)
+		return "(unknown)";
+	else
+		return sm_state_id_str[state->id];
+}
+
+const char *ipc_dbg_event(u8 event)
+{
+	return sm_run_cause_str[event];
+}
+
+char *ipc_dbg_link_state_str(struct ipc_link_context *context)
+{
+	char *statestr;
+	int ss_pin;
+	int int_pin;
+	int min_free_pc;
+
+	if (context == NULL)
+		return NULL;
+
+	statestr = kmalloc(500, GFP_ATOMIC);
+	if (statestr == NULL)
+		return NULL;
+
+	ss_pin = gpio_get_value(context->link->gpio.ss_pin);
+	int_pin = gpio_get_value(context->link->gpio.int_pin);
+	min_free_pc = context->tx_q_min > 0 ?
+		(context->tx_q_min * 100) / IPC_TX_QUEUE_MAX_SIZE :
+		0;
+
+	sprintf(statestr,
+		"state=%s (for %lus)\n"
+		"ss=%s(%d)\n"
+		"int=%s(%d)\n"
+		"lastevent=%s\n"
+		"lastignored=%s in %s (ignoredinthis=%d)\n"
+		"tx_q_min=%d(%d%%)\n"
+		"tx_q_count=%d\n"
+		"lastcmd=0x%08x (type %d count %d len %d)\n",
+		sm_state_id_str[context->state->id],
+		(jiffies - context->statesince) / HZ,
+		ss_pin == ipc_util_ss_level_active(context) ?
+			"ACTIVE" : "INACTIVE",
+		ss_pin,
+		int_pin == ipc_util_int_level_active(context) ?
+			"ACTIVE" : "INACTIVE",
+		int_pin,
+		sm_run_cause_str[context->lastevent],
+		sm_run_cause_str[context->lastignored],
+		sm_state_id_str[context->lastignored_in],
+		context->lastignored_inthis,
+		context->tx_q_min,
+		min_free_pc,
+		atomic_read(&context->tx_q_count),
+		context->cmd,
+		ipc_util_get_l1_cmd(context->cmd),
+		ipc_util_get_l1_counter(context->cmd),
+		ipc_util_get_l1_length(context->cmd));
+	return statestr;
+}
+
+void ipc_dbg_verify_rx_frame(struct ipc_link_context *context)
+{
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_VERIFY_FRAMES
+	int i;
+	u8 *last;
+	u8 *curr;
+	bool good = true;
+
+	if (context->last_frame == NULL)
+		return;
+
+	if (context->last_frame->actual_len != context->frame->actual_len) {
+		dev_err(&context->sdev->dev,
+			"link %d error: loopback frame length error, "
+			"TX %d RX %d\n",
+			context->link->id,
+			context->last_frame->actual_len,
+			context->frame->actual_len);
+		good = false;
+		goto out;
+	}
+
+	last = (u8 *)context->last_frame->data;
+	curr = (u8 *)context->frame->data;
+
+	/* skip any padding bytes */
+	for (i = 0; i < context->last_frame->actual_len; i++) {
+		if (last[i] != curr[i]) {
+			dev_err(&context->sdev->dev,
+				"link %d bad byte %05d: "
+				"TX %02x RX %02x\n",
+				context->link->id,
+				i,
+				last[i],
+				curr[i]);
+			good = false;
+		}
+	}
+
+out:
+	if (!good)
+		dev_info(&context->sdev->dev,
+			"link %d error: loopback frame verification failed!\n",
+			context->link->id);
+
+	ipc_queue_delete_frame(context->last_frame);
+	context->last_frame = NULL;
+#endif
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int debugfs_linkstate_open(struct inode *inode, struct file *file);
+static int debugfs_linkstate_show(struct seq_file *s, void *data);
+
+static int debugfs_msr_open(struct inode *inode, struct file *file);
+static int debugfs_msr_show(struct seq_file *s, void *data);
+static ssize_t debugfs_msr_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos);
+
+static const struct file_operations debugfs_fops = {
+	.open = debugfs_linkstate_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release
+};
+
+static const struct file_operations debugfs_msr_fops = {
+	.open = debugfs_msr_open,
+	.read = seq_read,
+	.write = debugfs_msr_write,
+	.llseek = seq_lseek,
+	.release = single_release
+};
+
+static int debugfs_linkstate_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, debugfs_linkstate_show, inode->i_private);
+}
+
+static int debugfs_linkstate_show(struct seq_file *s, void *data)
+{
+	struct ipc_link_context *context = s->private;
+	char *statestr;
+
+	if (context == NULL) {
+		seq_printf(s, "invalid context\n");
+		return 0;
+	}
+
+	statestr = ipc_dbg_link_state_str(context);
+	if (statestr == NULL) {
+		seq_printf(s, "unable to get link state string\n");
+		return 0;
+	}
+
+	seq_printf(s, "%s:\n%s", context->link->name, statestr);
+	kfree(statestr);
+	return 0;
+}
+
+static int debugfs_msr_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, debugfs_msr_show, inode->i_private);
+}
+
+static int debugfs_msr_show(struct seq_file *s, void *data)
+{
+	struct ipc_l1_context *context = s->private;
+
+	if (context == NULL) {
+		seq_printf(s, "invalid context\n");
+		return 0;
+	}
+
+	seq_printf(s, "msr %s\n",
+		context->msr_disable ? "disabled" : "enabled");
+	return 0;
+}
+
+static ssize_t debugfs_msr_write(struct file *file,
+			   const char __user *user_buf,
+			   size_t count, loff_t *ppos)
+{
+	char buf[128];
+	int buf_size;
+
+	/* get user space string and assure termination */
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+
+	buf[buf_size] = 0;
+
+	if (buf[0] == '0' || buf[0] == 'd') {
+		pr_info("disabling msr\n");
+		l1_context.msr_disable = true;
+	} else if (buf[0] == '1' || buf[0] == 'e') {
+		pr_info("enabling msr\n");
+		l1_context.msr_disable = false;
+	} else {
+		pr_info("unknown request\n");
+	}
+
+	return buf_size;
+}
+#endif /* CONFIG_DEBUG_FS */
+
+void ipc_dbg_debugfs_init(void)
+{
+#ifdef CONFIG_DEBUG_FS
+	/* create debugfs directory entry for ipc in debugfs root */
+	l1_context.debugfsdir = debugfs_create_dir("modemipc", NULL);
+	l1_context.debugfs_silentreset =
+		debugfs_create_file("msrenable", S_IRUSR | S_IWUSR,
+			l1_context.debugfsdir, &l1_context, &debugfs_msr_fops);
+	if (l1_context.debugfs_silentreset == NULL)
+		pr_err("failed to create debugfs MSR control file\n");
+#endif
+}
+
+void ipc_dbg_debugfs_link_init(struct ipc_link_context *context)
+{
+#ifdef CONFIG_DEBUG_FS
+	context->debugfsfile = NULL;
+	context->lastevent = IPC_SM_RUN_NONE;
+	context->lastignored = IPC_SM_RUN_NONE;
+	context->lastignored_in = IPC_SM_IDL;
+	context->lastignored_inthis = false;
+	context->tx_q_min = IPC_TX_QUEUE_MAX_SIZE;
+	context->statesince = 0;
+
+	if (l1_context.debugfsdir != NULL) {
+		context->debugfsfile =
+			debugfs_create_file(context->link->name, S_IRUGO,
+				l1_context.debugfsdir, context, &debugfs_fops);
+		if (context->debugfsfile == NULL)
+			dev_err(&context->sdev->dev,
+				"link %d: failed to create debugfs file %s\n",
+				context->link->id,
+				context->link->name);
+	}
+#endif
+}
+
+void ipc_dbg_ignoring_event(struct ipc_link_context *context, u8 event)
+{
+#ifdef CONFIG_DEBUG_FS
+	context->lastignored = event;
+	context->lastignored_in = context->state->id;
+	context->lastignored_inthis = true;
+#endif
+}
+
+void ipc_dbg_handling_event(struct ipc_link_context *context, u8 event)
+{
+#ifdef CONFIG_DEBUG_FS
+	context->lastevent = event;
+	context->lastignored_inthis = false;
+#endif
+}
+
+void ipc_dbg_entering_state(struct ipc_link_context *context)
+{
+#ifdef CONFIG_DEBUG_FS
+	context->statesince = jiffies;
+#endif
+}
+
+void ipc_dbg_enter_idle(struct ipc_link_context *context)
+{
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	context->idl_idle_enter = jiffies;
+#endif
+}
+
+void ipc_dbg_exit_idle(struct ipc_link_context *context)
+{
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	context->idl_idle_total += jiffies - context->idl_idle_enter;
+#endif
+}
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+static int measure_usage(struct ipc_link_context *context)
+{
+	unsigned long now = jiffies;
+	unsigned long idle;
+	unsigned long total;
+
+	if (ipc_util_link_is_idle(context))
+		ipc_dbg_exit_idle(context);
+
+	idle = context->idl_idle_total;
+	total = now - context->idl_measured_at;
+
+	context->idl_measured_at = now;
+	context->idl_idle_total = 0;
+	if (ipc_util_link_is_idle(context))
+		context->idl_idle_enter = now;
+
+	return 100 - ((idle * 100) / total);
+}
+#endif
+
+void ipc_dbg_measure_throughput(unsigned long unused)
+{
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	u32 tx_bps_0, tx_bps_1;
+	u32 rx_bps_0, rx_bps_1;
+	int pc0, pc1;
+
+	tx_bps_0 = tx_bps_1 = 0;
+	rx_bps_0 = rx_bps_1 = 0;
+
+	/* link0 */
+	tx_bps_0 = (l1_context.device_context[0].tx_bytes * 8) /
+		CONFIG_MODEM_M6718_SPI_SET_THROUGHPUT_FREQUENCY;
+	rx_bps_0 = (l1_context.device_context[0].rx_bytes * 8) /
+		CONFIG_MODEM_M6718_SPI_SET_THROUGHPUT_FREQUENCY;
+	l1_context.device_context[0].tx_bytes = 0;
+	l1_context.device_context[0].rx_bytes = 0;
+	pc0 = measure_usage(&l1_context.device_context[0]);
+#if IPC_NBR_SUPPORTED_SPI_LINKS > 0
+	/* link1 */
+	tx_bps_1 = (l1_context.device_context[1].tx_bytes * 8) /
+		CONFIG_MODEM_M6718_SPI_SET_THROUGHPUT_FREQUENCY;
+	rx_bps_1 = (l1_context.device_context[1].rx_bytes * 8) /
+		CONFIG_MODEM_M6718_SPI_SET_THROUGHPUT_FREQUENCY;
+	l1_context.device_context[1].tx_bytes = 0;
+	l1_context.device_context[1].rx_bytes = 0;
+	pc1 = measure_usage(&l1_context.device_context[1]);
+#endif
+
+	pr_info("IPC THROUGHPUT (bit/s): "
+		"link0 TX:%8d RX:%8d %3d%% "
+		"link1 TX:%8d RX:%8d %3d%%\n",
+		tx_bps_0, rx_bps_0, pc0,
+		tx_bps_1, rx_bps_1, pc1);
+
+	/* restart the measurement timer */
+	l1_context.tp_timer.expires = jiffies +
+		(CONFIG_MODEM_M6718_SPI_SET_THROUGHPUT_FREQUENCY * HZ);
+	add_timer(&l1_context.tp_timer);
+#endif
+}
+
+void ipc_dbg_throughput_init(void)
+{
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	pr_info("M6718 IPC throughput measurement interval: %d\n",
+		CONFIG_MODEM_M6718_SPI_SET_THROUGHPUT_FREQUENCY);
+	/* init the throughput measurement timer */
+	init_timer(&l1_context.tp_timer);
+	l1_context.tp_timer.function = ipc_dbg_measure_throughput;
+	l1_context.tp_timer.data = 0;
+#endif
+}
+
+void ipc_dbg_throughput_link_init(struct ipc_link_context *context)
+{
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	context->tx_bytes = 0;
+	context->rx_bytes = 0;
+	context->idl_measured_at = jiffies;
+	context->idl_idle_enter = 0;
+	context->idl_idle_total = 0;
+#endif
+}
+
diff --git a/drivers/modem/m6718_spi/modem_debug.h b/drivers/modem/m6718_spi/modem_debug.h
new file mode 100644
index 00000000000..9a2fa39acb4
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_debug.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Modem IPC driver protocol interface header:
+ *   debug functionality.
+ */
+#ifndef _MODEM_DEBUG_H_
+#define _MODEM_DEBUG_H_
+
+#include "modem_private.h"
+
+void ipc_dbg_dump_frame(struct device *dev, int linkid,
+	struct ipc_tx_queue *frame, bool tx);
+void ipc_dbg_dump_spi_tfr(struct ipc_link_context *context);
+const char *ipc_dbg_state_id(const struct ipc_sm_state *state);
+const char *ipc_dbg_event(u8 event);
+char *ipc_dbg_link_state_str(struct ipc_link_context *context);
+void ipc_dbg_verify_rx_frame(struct ipc_link_context *context);
+
+void ipc_dbg_debugfs_init(void);
+void ipc_dbg_debugfs_link_init(struct ipc_link_context *context);
+
+void ipc_dbg_ignoring_event(struct ipc_link_context *context, u8 event);
+void ipc_dbg_handling_event(struct ipc_link_context *context, u8 event);
+void ipc_dbg_entering_state(struct ipc_link_context *context);
+void ipc_dbg_enter_idle(struct ipc_link_context *context);
+void ipc_dbg_exit_idle(struct ipc_link_context *context);
+void ipc_dbg_measure_throughput(unsigned long unused);
+void ipc_dbg_throughput_init(void);
+void ipc_dbg_throughput_link_init(struct ipc_link_context *context);
+
+#endif /* _MODEM_DEBUG_H_ */
diff --git a/drivers/modem/m6718_spi/modem_driver.c b/drivers/modem/m6718_spi/modem_driver.c
new file mode 100644
index 00000000000..8086e97aa7c
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_driver.c
@@ -0,0 +1,292 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *   based on modem_shrm_driver.c
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * SPI driver implementing the M6718 inter-processor communication protocol.
+ */
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/spi/spi.h>
+#include <linux/modem/modem_client.h>
+#include <linux/modem/m6718_spi/modem_driver.h>
+#include <linux/modem/m6718_spi/modem_net.h>
+#include <linux/modem/m6718_spi/modem_char.h>
+#include "modem_protocol.h"
+
+#ifdef CONFIG_PHONET
+static void phonet_rcv_tasklet_func(unsigned long);
+static struct tasklet_struct phonet_rcv_tasklet;
+#endif
+
+static struct modem_spi_dev modem_driver_data = {
+	.dev = NULL,
+	.ndev = NULL,
+	.modem = NULL,
+	.isa_context = NULL,
+	.netdev_flag_up = 0
+};
+
+/**
+ * modem_m6718_spi_receive() - Receive a frame from L1 physical layer
+ * @sdev:	pointer to spi device structure
+ * @channel:	L2 mux channel id
+ * @len:	frame data length
+ * @data:	pointer to frame data
+ *
+ * This function is called from the driver L1 physical transport layer. It
+ * copies the frame data to the receive queue for the channel on which the data
+ * was received.
+ *
+ * Special handling is given to slave-loopback channels where the data is simply
+ * sent back to the modem on the same channel.
+ *
+ * Special handling is given to the ISI channel when PHONET is enabled - the
+ * phonet tasklet is scheduled in order to pump the received data through the
+ * net device interface.
+ */
+int modem_m6718_spi_receive(struct spi_device *sdev, u8 channel,
+	u32 len, void *data)
+{
+	u32 size = 0;
+	int ret = 0;
+	int idx;
+	u8 *psrc;
+	u32 writeptr;
+	struct message_queue *q;
+	struct isa_device_context *isadev;
+
+	dev_dbg(&sdev->dev, "L2 received frame from L1: channel %d len %d\n",
+		channel, len);
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_LOOPBACK
+	if (channel == MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK0 ||
+		channel == MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK1) {
+		/* data received on slave loopback channel - loop it back */
+		modem_m6718_spi_send(&modem_driver_data, channel, len, data);
+		return 0;
+	}
+#endif
+
+	/* find the isa device index for this L2 channel */
+	idx = modem_get_cdev_index(channel);
+	if (idx < 0) {
+		dev_err(&sdev->dev, "failed to get isa device index\n");
+		return idx;
+	}
+	isadev = &modem_driver_data.isa_context->isadev[idx];
+	q = &isadev->dl_queue;
+
+	spin_lock(&q->update_lock);
+
+	/* verify message can be contained in buffer */
+	writeptr = q->writeptr;
+	ret = modem_isa_queue_msg(q, len);
+	if (ret >= 0) {
+		/* memcopy RX data */
+		if ((writeptr + len) >= q->size) {
+			psrc = (u8 *)data;
+			size = q->size - writeptr;
+			/* copy first part of msg */
+			memcpy((q->fifo_base + writeptr), psrc, size);
+			psrc += size;
+			/* copy second part of msg at the top of fifo */
+			memcpy(q->fifo_base, psrc, (len - size));
+		} else {
+			memcpy((q->fifo_base + writeptr), data, len);
+		}
+	}
+	spin_unlock(&q->update_lock);
+
+	if (ret < 0) {
+		dev_err(&sdev->dev, "failed to queue frame!");
+		return ret;
+	}
+
+#ifdef CONFIG_PHONET
+	if (channel == MODEM_M6718_SPI_CHN_ISI &&
+		modem_driver_data.netdev_flag_up)
+		tasklet_schedule(&phonet_rcv_tasklet);
+#endif
+	return ret;
+}
+EXPORT_SYMBOL_GPL(modem_m6718_spi_receive);
+
+static void phonet_rcv_tasklet_func(unsigned long unused)
+{
+	ssize_t result;
+
+	dev_dbg(modem_driver_data.dev, "receiving frames for phonet\n");
+	/* continue receiving while there are frames in the queue */
+	for (;;) {
+		result = modem_net_receive(modem_driver_data.ndev);
+		if (result == 0) {
+			dev_dbg(modem_driver_data.dev,
+				"queue is empty, finished receiving\n");
+			break;
+		}
+		if (result < 0) {
+			dev_err(modem_driver_data.dev,
+				"failed to receive frame from queue!\n");
+			break;
+		}
+	}
+}
+
+static int spi_probe(struct spi_device *sdev)
+{
+	int result = 0;
+
+	spi_set_drvdata(sdev, &modem_driver_data);
+
+	if (modem_protocol_probe(sdev) != 0) {
+		dev_err(&sdev->dev,
+			"failed to initialise link protocol\n");
+		result = -ENODEV;
+		goto rollback;
+	}
+
+	/*
+	 * Since we can have multiple spi links for the same modem, only
+	 * initialise the modem data and char/net interfaces once.
+	 */
+	if (modem_driver_data.dev == NULL) {
+		modem_driver_data.dev = &sdev->dev;
+		modem_driver_data.modem =
+			modem_get(modem_driver_data.dev, "m6718");
+		if (modem_driver_data.modem == NULL) {
+			dev_err(&sdev->dev,
+				"failed to retrieve modem description\n");
+			result = -ENODEV;
+			goto rollback_protocol_init;
+		}
+
+		result = modem_isa_init(&modem_driver_data);
+		if (result < 0) {
+			dev_err(&sdev->dev,
+				"failed to initialise char interface\n");
+			goto rollback_modem_get;
+		}
+
+		result = modem_net_init(&modem_driver_data);
+		if (result < 0) {
+			dev_err(&sdev->dev,
+				"failed to initialse net interface\n");
+			goto rollback_isa_init;
+		}
+
+#ifdef CONFIG_PHONET
+		tasklet_init(&phonet_rcv_tasklet, phonet_rcv_tasklet_func, 0);
+#endif
+	}
+	return result;
+
+rollback_isa_init:
+	modem_isa_exit(&modem_driver_data);
+rollback_modem_get:
+	modem_put(modem_driver_data.modem);
+rollback_protocol_init:
+	modem_protocol_exit();
+rollback:
+	return result;
+}
+
+static int __exit spi_remove(struct spi_device *sdev)
+{
+	modem_protocol_exit();
+	modem_net_exit(&modem_driver_data);
+	modem_isa_exit(&modem_driver_data);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/**
+ * spi_suspend() - This routine puts the IPC driver in to suspend state.
+ * @sdev: pointer to spi device structure.
+ * @mesg: pm operation
+ *
+ * This routine checks the current ongoing communication with modem
+ * and prevents suspend if modem communication is on-going.
+ */
+static int spi_suspend(struct spi_device *sdev, pm_message_t mesg)
+{
+	bool busy;
+	int ret = -EBUSY;
+
+	dev_dbg(&sdev->dev, "suspend called\n");
+	busy = modem_protocol_is_busy(sdev);
+	if (busy) {
+		dev_warn(&sdev->dev, "suspend failed (protocol busy)\n");
+		return -EBUSY;
+	}
+	ret = modem_protocol_suspend(sdev);
+	if (ret) {
+		dev_warn(&sdev->dev, "suspend failed, (protocol suspend))\n");
+		return ret;
+	}
+	ret = modem_net_suspend(modem_driver_data.ndev);
+	if (ret) {
+		dev_warn(&sdev->dev, "suspend failed, (netdev suspend)\n");
+		return ret;
+	}
+	return 0;
+}
+
+/**
+ * spi_resume() - This routine resumes the IPC driver from suspend state.
+ * @sdev: pointer to spi device structure
+ */
+static int spi_resume(struct spi_device *sdev)
+{
+	int ret;
+
+	dev_dbg(&sdev->dev, "resume called\n");
+	ret = modem_protocol_resume(sdev);
+	if (ret) {
+		dev_warn(&sdev->dev, "resume failed, (protocol resume))\n");
+		return ret;
+	}
+	ret = modem_net_resume(modem_driver_data.ndev);
+	if (ret) {
+		dev_warn(&sdev->dev, "resume failed, (netdev resume))\n");
+		return ret;
+	}
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+static struct spi_driver spi_driver = {
+	.driver = {
+		.name = "spimodem",
+		.bus  = &spi_bus_type,
+		.owner = THIS_MODULE
+	},
+	.probe = spi_probe,
+	.remove = __exit_p(spi_remove),
+#ifdef CONFIG_PM
+	.suspend = spi_suspend,
+	.resume = spi_resume,
+#endif
+};
+
+static int __init m6718_spi_driver_init(void)
+{
+	pr_info("M6718 modem driver initialising\n");
+	modem_protocol_init();
+	return spi_register_driver(&spi_driver);
+}
+module_init(m6718_spi_driver_init);
+
+static void __exit m6718_spi_driver_exit(void)
+{
+	pr_debug("M6718 modem SPI IPC driver exit\n");
+	spi_unregister_driver(&spi_driver);
+}
+module_exit(m6718_spi_driver_exit);
+
+MODULE_AUTHOR("Chris Blair <chris.blair@stericsson.com>");
+MODULE_DESCRIPTION("M6718 modem IPC SPI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/modem/m6718_spi/modem_netlink.h b/drivers/modem/m6718_spi/modem_netlink.h
new file mode 100644
index 00000000000..19e123d9b12
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_netlink.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Modem IPC driver protocol interface header:
+ *   netlink related functionality.
+ */
+#ifndef _MODEM_NETLINK_H_
+#define _MODEM_NETLINK_H_
+
+#include "modem_protocol.h"
+
+bool ipc_create_netlink_socket(struct ipc_link_context *context);
+void ipc_broadcast_modem_online(struct ipc_link_context *context);
+void ipc_broadcast_modem_reset(struct ipc_link_context *context);
+
+#endif /* _MODEM_NETLINK_H_ */
diff --git a/drivers/modem/m6718_spi/modem_private.h b/drivers/modem/m6718_spi/modem_private.h
new file mode 100644
index 00000000000..a4de4ba9e93
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_private.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *   based on shrm_driver.h
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Modem IPC driver protocol interface header:
+ *   private data
+ */
+#ifndef _MODEM_PRIVATE_H_
+#define _MODEM_PRIVATE_H_
+
+#include <linux/kernel.h>
+#include <linux/spi/spi.h>
+#include <linux/atomic.h>
+#include <linux/interrupt.h>
+#include <linux/timer.h>
+#include <linux/list.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include "modem_protocol.h"
+#include "modem_statemachine.h"
+
+#define IPC_DRIVER_VERSION       (0x02) /* APE protocol version */
+#define IPC_DRIVER_MODEM_MIN_VER (0x02) /* version required from modem */
+
+#define IPC_NBR_SUPPORTED_SPI_LINKS (2)
+#define IPC_LINK_COMMON (0)
+#define IPC_LINK_AUDIO  (1)
+
+#define IPC_TX_QUEUE_MAX_SIZE (1024*1024)
+
+#define IPC_L1_HDR_SIZE (4)
+#define IPC_L2_HDR_SIZE (4)
+
+/* tx queue item (frame) */
+struct ipc_tx_queue {
+	struct list_head node;
+	int actual_len;
+	int len;
+	void *data;
+	int counter;
+};
+
+/* context structure for an spi link */
+struct ipc_link_context {
+	struct modem_m6718_spi_link_platform_data *link;
+	struct spi_device *sdev;
+	atomic_t suspended;
+	atomic_t gpio_configured;
+	atomic_t state_int;
+	spinlock_t sm_lock;
+	spinlock_t tx_q_update_lock;
+	atomic_t tx_q_count;
+	int tx_q_free;
+	struct list_head tx_q;
+	int tx_frame_counter;
+	const struct ipc_sm_state *state;
+	u32 cmd;
+	struct ipc_tx_queue *frame;
+	struct spi_message spi_message;
+	struct spi_transfer spi_transfer;
+	struct timer_list comms_timer;
+	struct timer_list slave_stable_timer;
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_VERIFY_FRAMES
+	struct ipc_tx_queue *last_frame;
+#endif
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	u32 tx_bytes;
+	u32 rx_bytes;
+	unsigned long idl_measured_at;
+	unsigned long idl_idle_enter;
+	unsigned long idl_idle_total;
+#endif
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debugfsfile;
+	u8 lastevent;
+	u8 lastignored;
+	enum ipc_sm_state_id lastignored_in;
+	bool lastignored_inthis;
+	int tx_q_min;
+	unsigned long statesince;
+#endif
+};
+
+/* context structure for the spi driver */
+struct ipc_l1_context {
+	bool init_done;
+	atomic_t boot_sync_done;
+	struct ipc_link_context device_context[IPC_NBR_SUPPORTED_SPI_LINKS];
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	struct timer_list tp_timer;
+#endif
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debugfsdir;
+	struct dentry *debugfs_silentreset;
+	bool msr_disable;
+#endif
+};
+
+extern struct ipc_l1_context l1_context;
+
+#endif /* _MODEM_PRIVATE_H_ */
diff --git a/drivers/modem/m6718_spi/modem_protocol.h b/drivers/modem/m6718_spi/modem_protocol.h
new file mode 100644
index 00000000000..751dcba1087
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_protocol.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *   based on shrm_driver.h
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Modem IPC driver protocol interface header.
+ */
+#ifndef _MODEM_PROTOCOL_H_
+#define _MODEM_PROTOCOL_H_
+
+#include <linux/spi/spi.h>
+
+void modem_protocol_init(void);
+int modem_protocol_probe(struct spi_device *sdev);
+void modem_protocol_exit(void);
+bool modem_protocol_is_busy(struct spi_device *sdev);
+bool modem_protocol_channel_is_open(u8 channel);
+int modem_protocol_suspend(struct spi_device *sdev);
+int modem_protocol_resume(struct spi_device *sdev);
+
+#endif /* _MODEM_PROTOCOL_H_ */
diff --git a/drivers/modem/m6718_spi/modem_queue.h b/drivers/modem/m6718_spi/modem_queue.h
new file mode 100644
index 00000000000..62604129945
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_queue.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Modem IPC driver protocol interface header:
+ *   queue functionality.
+ */
+#ifndef _MODEM_QUEUE_H_
+#define _MODEM_QUEUE_H_
+
+void ipc_queue_init(struct ipc_link_context *context);
+void ipc_queue_delete_frame(struct ipc_tx_queue *frame);
+struct ipc_tx_queue *ipc_queue_new_frame(struct ipc_link_context *link_context,
+	u32 l2_length);
+bool ipc_queue_is_empty(struct ipc_link_context *context);
+int ipc_queue_push_frame(struct ipc_link_context *link_context, u8 l2_header,
+	u32 l2_length, void *l2_data);
+struct ipc_tx_queue *ipc_queue_get_frame(struct ipc_link_context *context);
+void ipc_queue_reset(struct ipc_link_context *context);
+
+#endif /* _MODEM_QUEUE_H_ */
diff --git a/drivers/modem/m6718_spi/modem_state.c b/drivers/modem/m6718_spi/modem_state.c
new file mode 100644
index 00000000000..47376934bcb
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_state.c
@@ -0,0 +1,1300 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Derek Morton <derek.morton@stericsson.com>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Power state driver for M6718 MODEM
+ */
+
+/* define DEBUG to enable debug logging */
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/timer.h>
+#include <linux/gpio/nomadik.h>
+#include <plat/pincfg.h>
+#include <linux/workqueue.h>
+#include <linux/list.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include "modem_state.h"
+
+/*
+ * To enable this driver add a struct platform_device in the board
+ * configuration file (e.g. board-*.c) with name="modemstate"
+ * optionally specify dev.initname="m6718" to define the driver
+ * name as it will appear in the file system.
+ * e.g.
+ * static struct platform_device modem_state_device =
+ * {
+ *    .name = "modemstate",
+ *    .dev =
+ *    {
+ *       .init_name = "m6718" // Name that will appear in FS
+ *    },
+ *    .num_resources = ARRAY_SIZE(modem_state_resources),
+ *    .resource = modem_state_resources
+ * };
+ *
+ * This driver uses gpio pins which should be specified as resources *
+ * e.g.
+ * static struct resource modem_state_resources[] =  .......
+ * Output pins are specified as IORESOURCE_IO
+ * Currently supported Output pins are:
+ * onkey_pin
+ * reset_pin
+ * vbat_pin
+ * Input pins are specified as IORESOURCE_IRQ
+ * Currently supported input pins are:
+ * rsthc_pin
+ * rstext_pin
+ * crash_pin
+ * Currently only the start value is used as the gpio pin number but
+ * end should also be specified as the gpio pin number in case gpio ranges
+ * are used in the future.
+ * e.g. if gpio 161 is used as the onkey pin
+ * {
+ *    .start = 161,
+ *    .end = 161,
+ *    .name = "onkey_pin",
+ *    .flags = IORESOURCE_IO,
+ * },
+ */
+
+struct modem_state_dev {
+	int onkey_pin;
+	int rsthc_pin;
+	int rstext_pin;
+	int crash_pin;
+	int reset_pin;
+	int vbat_pin;
+	int power_state;
+	int irq_state;
+	int busy;
+	struct timer_list onkey_timer;
+	struct timer_list reset_timer;
+	struct timer_list onkey_debounce_timer;
+	struct timer_list vbat_off_timer;
+	struct timer_list busy_timer;
+	spinlock_t lock;
+	struct device *dev;
+	struct workqueue_struct *workqueue;
+	struct work_struct wq_rsthc;
+	struct work_struct wq_rstext;
+	struct work_struct wq_crash;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debugfsdir;
+	struct dentry *debugfs_debug;
+#endif
+};
+
+struct callback_list {
+	struct list_head node;
+	int (*callback) (unsigned long);
+	unsigned long data;
+};
+LIST_HEAD(callback_list);
+
+static char *modem_state_str[] = {
+	"off",
+	"reset",
+	"crash",
+	"on",
+	/*
+	 * Add new states before error and update enum modem_states
+	 * in modem_state.h
+	 */
+	"error"
+};
+
+static struct modem_state_dev *modem_state;
+
+static void set_on_config(struct modem_state_dev *msdev)
+{
+	if (msdev->crash_pin)
+		nmk_config_pin(PIN_CFG(msdev->crash_pin, GPIO) |
+			       PIN_INPUT_PULLDOWN, false);
+	if (msdev->rstext_pin)
+		nmk_config_pin(PIN_CFG(msdev->rstext_pin, GPIO) |
+			       PIN_INPUT_PULLDOWN, false);
+	if (msdev->rsthc_pin)
+		nmk_config_pin(PIN_CFG(msdev->rsthc_pin, GPIO) |
+			       PIN_INPUT_PULLDOWN, false);
+	if (msdev->reset_pin)
+		nmk_config_pin(PIN_CFG(msdev->reset_pin, GPIO) |
+			       PIN_OUTPUT_HIGH, false);
+}
+
+static void set_off_config(struct modem_state_dev *msdev)
+{
+	if (msdev->crash_pin)
+		nmk_config_pin(PIN_CFG(msdev->crash_pin, GPIO) |
+			       PIN_INPUT_PULLDOWN, false);
+	if (msdev->rstext_pin)
+		nmk_config_pin(PIN_CFG(msdev->rstext_pin, GPIO) |
+			       PIN_OUTPUT_LOW, false);
+	if (msdev->rsthc_pin)
+		nmk_config_pin(PIN_CFG(msdev->rsthc_pin, GPIO) | PIN_OUTPUT_LOW,
+			       false);
+	if (msdev->reset_pin)
+		nmk_config_pin(PIN_CFG(msdev->reset_pin, GPIO) |
+			       PIN_OUTPUT_HIGH, false);
+}
+
+static void enable_irq_all(struct modem_state_dev *msdev)
+{
+	if (msdev->rsthc_pin) {
+		enable_irq(GPIO_TO_IRQ(msdev->rsthc_pin));
+		if ((0 > enable_irq_wake(GPIO_TO_IRQ(msdev->rsthc_pin))))
+			dev_err(msdev->dev,
+				"Request for wake on pin %d failed\n",
+				msdev->rsthc_pin);
+	}
+	if (msdev->rstext_pin) {
+		enable_irq(GPIO_TO_IRQ(msdev->rstext_pin));
+		if ((0 > enable_irq_wake(GPIO_TO_IRQ(msdev->rstext_pin))))
+			dev_err(msdev->dev,
+				"Request for wake on pin %d failed\n",
+				msdev->rstext_pin);
+	}
+	if (msdev->crash_pin) {
+		enable_irq(GPIO_TO_IRQ(msdev->crash_pin));
+		if ((0 > enable_irq_wake(GPIO_TO_IRQ(msdev->crash_pin))))
+			dev_err(msdev->dev,
+				"Request for wake on pin %d failed\n",
+				msdev->crash_pin);
+	}
+}
+
+static void disable_irq_all(struct modem_state_dev *msdev)
+{
+	if (msdev->rsthc_pin) {
+		disable_irq_wake(GPIO_TO_IRQ(msdev->rsthc_pin));
+		disable_irq(GPIO_TO_IRQ(msdev->rsthc_pin));
+	}
+	if (msdev->rstext_pin) {
+		disable_irq_wake(GPIO_TO_IRQ(msdev->rstext_pin));
+		disable_irq(GPIO_TO_IRQ(msdev->rstext_pin));
+	}
+	if (msdev->crash_pin) {
+		disable_irq_wake(GPIO_TO_IRQ(msdev->crash_pin));
+		disable_irq(GPIO_TO_IRQ(msdev->crash_pin));
+	}
+}
+
+/*
+ * These functions which access GPIO must only be called
+ * with spinlock enabled.
+ */
+
+/*
+ * Toggle ONKEY pin high then low to turn modem on or off. Modem expects
+ * ONKEY line to be pulled low then high. GPIO needs to be driven high then
+ * low as logic is inverted through a transistor.
+ */
+static void toggle_modem_power(struct modem_state_dev *msdev)
+{
+	dev_info(msdev->dev, "Modem power toggle\n");
+	msdev->busy = 1;
+	gpio_set_value(msdev->onkey_pin, 1);
+	msdev->onkey_timer.data = (unsigned long)msdev;
+	/* Timeout of at least 1 second */
+	mod_timer(&msdev->onkey_timer, jiffies + (1 * HZ) + 1);
+}
+
+/* Modem is forced into reset when its reset line is pulled low */
+/* Drive GPIO low then high to reset modem */
+static void modem_reset(struct modem_state_dev *msdev)
+{
+	dev_info(msdev->dev, "Modem reset\n");
+	msdev->busy = 1;
+	gpio_set_value(msdev->reset_pin, 0);
+	msdev->reset_timer.data = (unsigned long)msdev;
+	/* Wait a couple of Jiffies */
+	mod_timer(&msdev->reset_timer, jiffies + 2);
+}
+
+static void modem_vbat_set_value(struct modem_state_dev *msdev, int vbat_val)
+{
+	switch (vbat_val) {
+	case 0:
+		msdev->power_state = 0;
+		dev_info(msdev->dev, "Modem vbat off\n");
+		gpio_set_value(msdev->vbat_pin, vbat_val);
+		if (1 == msdev->irq_state) {
+			msdev->irq_state = 0;
+			disable_irq_all(msdev);
+			set_off_config(msdev);
+		}
+		break;
+	case 1:
+		dev_info(msdev->dev, "Modem vbat on\n");
+		if (0 == msdev->irq_state) {
+			msdev->irq_state = 1;
+			set_on_config(msdev);
+			enable_irq_all(msdev);
+		}
+		gpio_set_value(msdev->vbat_pin, vbat_val);
+		break;
+	default:
+		return;
+		break;
+	}
+}
+
+static void modem_power_on(struct modem_state_dev *msdev)
+{
+	int rsthc = gpio_get_value(msdev->rsthc_pin);
+	msdev->power_state = 1;
+	del_timer(&msdev->vbat_off_timer);
+	if (rsthc == 0) {
+		modem_vbat_set_value(msdev, 1);
+		toggle_modem_power(msdev);
+	}
+}
+
+static void modem_power_off(struct modem_state_dev *msdev)
+{
+	int rsthc = gpio_get_value(msdev->rsthc_pin);
+
+	msdev->power_state = 0;
+	if (rsthc == 1) {
+		toggle_modem_power(msdev);
+		/* Cut power to modem after 10 seconds */
+		msdev->vbat_off_timer.data = (unsigned long)msdev;
+		mod_timer(&msdev->vbat_off_timer, jiffies + (10 * HZ));
+	}
+}
+/* End of functions requiring spinlock */
+
+static void call_callbacks(void)
+{
+	struct callback_list *item;
+
+	list_for_each_entry(item, &callback_list, node)
+		item->callback(item->data);
+}
+
+static int get_modem_state(struct modem_state_dev *msdev)
+{
+	int state;
+	unsigned long flags;
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	if (0 == gpio_get_value(msdev->rsthc_pin))
+		state = MODEM_STATE_OFF;
+	else if (0 == gpio_get_value(msdev->rstext_pin))
+		state = MODEM_STATE_RESET;
+	else if (1 == gpio_get_value(msdev->crash_pin))
+		state = MODEM_STATE_CRASH;
+	else
+		state = MODEM_STATE_ON;
+	spin_unlock_irqrestore(&msdev->lock, flags);
+
+	return state;
+}
+
+/* modempower read handler */
+static ssize_t modem_state_power_get(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	int rsthc;
+	int power_state;
+	unsigned long flags;
+	struct modem_state_dev *msdev =
+		platform_get_drvdata(to_platform_device(dev));
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	rsthc = gpio_get_value(msdev->rsthc_pin);
+	power_state = msdev->power_state;
+	spin_unlock_irqrestore(&msdev->lock, flags);
+
+	return sprintf(buf, "state=%d, expected=%d\n", rsthc, power_state);
+}
+
+/*
+ * modempower write handler
+ * Write '0' to /sys/devices/platform/modemstate/modempower to turn modem off
+ * Write '1' to /sys/devices/platform/modemstate/modempower to turn modem on
+ */
+static ssize_t modem_state_power_set(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	unsigned long flags;
+	int ret = count;
+	struct modem_state_dev *msdev =
+		platform_get_drvdata(to_platform_device(dev));
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	if (msdev->busy) {
+		ret = -EAGAIN;
+	} else if (count > 0) {
+		switch (buf[0]) {
+		case '0':
+			modem_power_off(msdev);
+			break;
+		case '1':
+			modem_power_on(msdev);
+			break;
+		default:
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&msdev->lock, flags);
+	return ret;
+}
+
+/* reset read handler */
+static ssize_t modem_state_reset_get(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	int rstext;
+	struct modem_state_dev *msdev =
+		platform_get_drvdata(to_platform_device(dev));
+
+	/* No need for spinlocks here as there is only 1 value */
+	rstext = gpio_get_value(msdev->rstext_pin);
+
+	return sprintf(buf, "state=%d\n", rstext);
+}
+
+/* reset write handler */
+/* Write '1' to /sys/devices/platform/modemstate/reset to reset modem */
+static ssize_t modem_state_reset_set(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t count)
+{
+	unsigned long flags;
+	int ret = count;
+	struct modem_state_dev *msdev =
+		platform_get_drvdata(to_platform_device(dev));
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	if (msdev->busy) {
+		ret = -EAGAIN;
+	} else if (count > 0) {
+		if (buf[0] == '1')
+			modem_reset(msdev);
+	}
+	spin_unlock_irqrestore(&msdev->lock, flags);
+
+	return ret;
+}
+
+/* crash read handler */
+static ssize_t modem_state_crash_get(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	int crash;
+	struct modem_state_dev *msdev =
+		platform_get_drvdata(to_platform_device(dev));
+
+	/* No need for spinlocks here as there is only 1 value */
+	crash = gpio_get_value(msdev->crash_pin);
+
+	return sprintf(buf, "state=%d\n", crash);
+}
+
+/* state read handler */
+static ssize_t modem_state_state_get(struct device *dev,
+				     struct device_attribute *attr, char *buf)
+{
+	int state;
+	struct modem_state_dev *msdev =
+		platform_get_drvdata(to_platform_device(dev));
+
+	state = get_modem_state(msdev);
+	if (state > MODEM_STATE_END_MARKER)
+		state = MODEM_STATE_END_MARKER;
+
+	return sprintf(buf, "%s\n", modem_state_str[state]);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static int modem_state_debug_get(struct seq_file *s, void *data)
+{
+	int onkey;
+	int rsthc;
+	int rstext;
+	int reset;
+	int crash;
+	int vbat;
+	unsigned long flags;
+	struct modem_state_dev *msdev = s->private;
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	onkey = gpio_get_value(msdev->onkey_pin);
+	rsthc = gpio_get_value(msdev->rsthc_pin);
+	rstext = gpio_get_value(msdev->rstext_pin);
+	reset = gpio_get_value(msdev->reset_pin);
+	crash = gpio_get_value(msdev->crash_pin);
+	vbat = gpio_get_value(msdev->vbat_pin);
+	spin_unlock_irqrestore(&msdev->lock, flags);
+
+	seq_printf(s, "onkey=%d, rsthc=%d, rstext=%d, "
+			"reset=%d, crash=%d, vbat=%d\n",
+			onkey, rsthc, rstext, reset, crash, vbat);
+	return 0;
+}
+
+/*
+ * debug write handler
+ * Write o['0'|'1'] to /sys/devices/platform/modemstate/debug to set
+ * onkey line low or high.
+ * Write r['0'|'1'] to /sys/devices/platform/modemstate/debug to set
+ * reset line low or high.
+ * Write v['0'|'1'] to /sys/devices/platform/modemstate/debug to set
+ * vbat line low or high.
+ */
+static ssize_t modem_state_debug_set(struct file *file,
+	const char __user *user_buf,
+	size_t count,
+	loff_t *ppos)
+{
+	unsigned long flags;
+	int bufsize;
+	char buf[128];
+
+	bufsize = min(count, sizeof(buf) - 1);
+	if (copy_from_user(buf, user_buf, bufsize))
+		return -EFAULT;
+	buf[bufsize] = 0;
+
+	spin_lock_irqsave(&modem_state->lock, flags);
+	if (modem_state->busy) {
+		return -EAGAIN;
+	} else if (count > 1) {
+		switch (buf[1]) {
+		case '0':	/* fallthrough */
+		case '1':
+			switch (buf[0]) {
+			case 'o':
+				gpio_set_value(modem_state->onkey_pin,
+					buf[1] - '0');
+				break;
+			case 'r':
+				gpio_set_value(modem_state->reset_pin,
+					buf[1] - '0');
+				break;
+			case 'v':
+				gpio_set_value(modem_state->vbat_pin,
+				buf[1] - '0');
+				break;
+			default:
+				break;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&modem_state->lock, flags);
+
+	return bufsize;
+}
+
+static int modem_state_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, modem_state_debug_get, inode->i_private);
+}
+#endif /* CONFIG_DEBUG_FS */
+
+static DEVICE_ATTR(modempower, S_IRUSR | S_IWUSR,
+		   modem_state_power_get, modem_state_power_set);
+static DEVICE_ATTR(reset, S_IRUSR | S_IWUSR,
+		   modem_state_reset_get, modem_state_reset_set);
+static DEVICE_ATTR(crash, S_IRUSR, modem_state_crash_get, NULL);
+static DEVICE_ATTR(state, S_IRUSR, modem_state_state_get, NULL);
+
+static struct attribute *modemstate_attributes[] = {
+	&dev_attr_modempower.attr,
+	&dev_attr_reset.attr,
+	&dev_attr_crash.attr,
+	&dev_attr_state.attr,
+	NULL
+};
+
+static struct attribute_group modemstate_attr_group = {
+	.attrs = modemstate_attributes,
+	.name = "modemstate"
+};
+
+#ifdef CONFIG_DEBUG_FS
+static const struct file_operations debugfs_debug_fops = {
+	.open = modem_state_debug_open,
+	.read = seq_read,
+	.write = modem_state_debug_set,
+	.llseek = seq_lseek,
+	.release = single_release
+};
+#endif
+
+static void sysfs_notify_rsthc(struct modem_state_dev *msdev)
+{
+	sysfs_notify(&msdev->dev->kobj, NULL, dev_attr_modempower.attr.name);
+	sysfs_notify(&msdev->dev->kobj, NULL, dev_attr_state.attr.name);
+}
+
+static void sysfs_notify_rstext(struct modem_state_dev *msdev)
+{
+	sysfs_notify(&msdev->dev->kobj, NULL, dev_attr_reset.attr.name);
+	sysfs_notify(&msdev->dev->kobj, NULL, dev_attr_state.attr.name);
+}
+
+static void sysfs_notify_crash(struct modem_state_dev *msdev)
+{
+	sysfs_notify(&msdev->dev->kobj, NULL, dev_attr_crash.attr.name);
+	sysfs_notify(&msdev->dev->kobj, NULL, dev_attr_state.attr.name);
+}
+
+static void wq_rsthc(struct work_struct *work)
+{
+	unsigned long flags;
+	int rsthc;
+	struct modem_state_dev *msdev =
+		container_of(work, struct modem_state_dev, wq_rsthc);
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	rsthc = gpio_get_value(msdev->rsthc_pin);
+	dev_dbg(msdev->dev, "RSTHC interrupt detected, rsthc=%d\n", rsthc);
+	if (msdev->power_state == rsthc) {
+		if (!rsthc) {
+			/* Modem has turned off, and we were expecting it to.
+			   turn vbat to the modem off now */
+			del_timer(&msdev->vbat_off_timer);
+			modem_vbat_set_value(msdev, 0);
+		}
+	} else {
+		dev_dbg(msdev->dev,
+			 "Modem power state is %d, expected %d\n", rsthc,
+			 msdev->power_state);
+		dev_dbg(msdev->dev,
+			"Attempting to change modem power state "
+			"in 2 seconds\n");
+
+		msdev->onkey_debounce_timer.data = (unsigned long)msdev;
+		/* Wait > 2048ms due to debounce timer */
+		mod_timer(&msdev->onkey_debounce_timer,
+			jiffies + ((2050 * HZ) / 1000));
+	}
+	spin_unlock_irqrestore(&msdev->lock, flags);
+
+	call_callbacks();
+	sysfs_notify_rsthc(msdev);
+}
+
+static void wq_rstext(struct work_struct *work)
+{
+	struct modem_state_dev *msdev =
+		container_of(work, struct modem_state_dev, wq_rstext);
+
+	dev_dbg(msdev->dev, "RSTEXT interrupt detected, rstext=%d\n",
+		 gpio_get_value(msdev->rstext_pin));
+
+	call_callbacks();
+	sysfs_notify_rstext(msdev);
+}
+
+static void wq_crash(struct work_struct *work)
+{
+	struct modem_state_dev *msdev =
+		container_of(work, struct modem_state_dev, wq_rstext);
+
+	dev_dbg(msdev->dev, "modem crash interrupt detected. crash=%d\n",
+		 gpio_get_value(msdev->crash_pin));
+
+	call_callbacks();
+	sysfs_notify_crash(msdev);
+}
+
+/* Populate device structure used by the driver */
+static int modem_state_dev_init(struct platform_device *pdev,
+				struct modem_state_dev *msdev)
+{
+	int err = 0;
+	struct resource *r;
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IO, "onkey_pin");
+	if (r == NULL) {
+		err = -ENXIO;
+		dev_err(&pdev->dev,
+			"Could not get GPIO number for onkey pin\n");
+		goto err_resource;
+	}
+	msdev->onkey_pin = r->start;
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IO, "reset_pin");
+	if (r == NULL) {
+		err = -ENXIO;
+		dev_err(&pdev->dev,
+			"Could not get GPIO number for reset pin\n");
+		goto err_resource;
+	}
+	msdev->reset_pin = r->start;
+
+	r = platform_get_resource_byname(pdev, IORESOURCE_IO, "vbat_pin");
+	if (r == NULL) {
+		err = -ENXIO;
+		dev_err(&pdev->dev, "Could not get GPIO number for vbat pin\n");
+		goto err_resource;
+	}
+	msdev->vbat_pin = r->start;
+
+	msdev->rsthc_pin = platform_get_irq_byname(pdev, "rsthc_pin");
+	if (msdev->rsthc_pin < 0) {
+		err = msdev->rsthc_pin;
+		dev_err(&pdev->dev,
+			"Could not get GPIO number for rsthc pin\n");
+		goto err_resource;
+	}
+
+	msdev->rstext_pin = platform_get_irq_byname(pdev, "rstext_pin");
+	if (msdev->rstext_pin < 0) {
+		err = msdev->rstext_pin;
+		dev_err(&pdev->dev,
+			"Could not get GPIO number for retext pin\n");
+		goto err_resource;
+	}
+
+	msdev->crash_pin = platform_get_irq_byname(pdev, "crash_pin");
+	if (msdev->crash_pin < 0) {
+		err = msdev->crash_pin;
+		dev_err(&pdev->dev,
+			"Could not get GPIO number for crash pin\n");
+		goto err_resource;
+	}
+err_resource:
+	return err;
+}
+
+/* IRQ handlers */
+
+/* Handlers for rsthc (modem power off indication) IRQ */
+static irqreturn_t rsthc_irq(int irq, void *dev)
+{
+	struct modem_state_dev *msdev = (struct modem_state_dev *)dev;
+
+	/* check it's our interrupt */
+	if (irq != GPIO_TO_IRQ(msdev->rsthc_pin)) {
+		dev_err(msdev->dev, "Spurious RSTHC irq\n");
+		return IRQ_NONE;
+	}
+
+	queue_work(msdev->workqueue, &msdev->wq_rsthc);
+	return IRQ_HANDLED;
+}
+
+/* Handlers for rstext (modem reset indication) IRQ */
+static irqreturn_t rstext_irq(int irq, void *dev)
+{
+	struct modem_state_dev *msdev = (struct modem_state_dev *)dev;
+
+	/* check it's our interrupt */
+	if (irq != GPIO_TO_IRQ(msdev->rstext_pin)) {
+		dev_err(msdev->dev, "Spurious RSTEXT irq\n");
+		return IRQ_NONE;
+	}
+
+	queue_work(msdev->workqueue, &msdev->wq_rstext);
+	return IRQ_HANDLED;
+}
+
+/* Handlers for modem crash indication IRQ */
+static irqreturn_t crash_irq(int irq, void *dev)
+{
+	struct modem_state_dev *msdev = (struct modem_state_dev *)dev;
+
+	/* check it's our interrupt */
+	if (irq != GPIO_TO_IRQ(msdev->crash_pin)) {
+		dev_err(msdev->dev, "Spurious modem crash irq\n");
+		return IRQ_NONE;
+	}
+
+	queue_work(msdev->workqueue, &msdev->wq_crash);
+	return IRQ_HANDLED;
+}
+
+static int request_irq_pin(int pin, irq_handler_t handler, unsigned long flags,
+			   struct modem_state_dev *msdev)
+{
+	int err = 0;
+	if (pin) {
+		err = request_irq(GPIO_TO_IRQ(pin), handler, flags,
+				  dev_name(msdev->dev), msdev);
+		if (err == 0) {
+			err = enable_irq_wake(GPIO_TO_IRQ(pin));
+			if (err < 0) {
+				dev_err(msdev->dev,
+					"Request for wake on pin %d failed\n",
+					pin);
+				free_irq(GPIO_TO_IRQ(pin), NULL);
+			}
+		} else {
+			dev_err(msdev->dev,
+				"Request for irq on pin %d failed\n", pin);
+		}
+	}
+	return err;
+}
+
+static void free_irq_pin(int pin)
+{
+	disable_irq_wake(GPIO_TO_IRQ(pin));
+	free_irq(GPIO_TO_IRQ(pin), NULL);
+}
+
+static int request_irq_all(struct modem_state_dev *msdev)
+{
+	int err;
+
+	err = request_irq_pin(msdev->rsthc_pin, rsthc_irq,
+				IRQF_TRIGGER_RISING |
+				IRQF_TRIGGER_FALLING |
+				IRQF_NO_SUSPEND, msdev);
+	if (err < 0)
+		goto err_rsthc_irq_req;
+
+	err = request_irq_pin(msdev->rstext_pin, rstext_irq,
+				IRQF_TRIGGER_RISING |
+				IRQF_TRIGGER_FALLING |
+				IRQF_NO_SUSPEND, msdev);
+	if (err < 0)
+		goto err_rstext_irq_req;
+
+	err = request_irq_pin(msdev->crash_pin, crash_irq,
+				IRQF_TRIGGER_RISING |
+				IRQF_TRIGGER_FALLING |
+				IRQF_NO_SUSPEND, msdev);
+	if (err < 0)
+		goto err_crash_irq_req;
+
+	return 0;
+
+err_crash_irq_req:
+	free_irq_pin(msdev->rstext_pin);
+err_rstext_irq_req:
+	free_irq_pin(msdev->rsthc_pin);
+err_rsthc_irq_req:
+	return err;
+}
+
+/* Configure GPIO used by the driver */
+static int modem_state_gpio_init(struct platform_device *pdev,
+				 struct modem_state_dev *msdev)
+{
+	int err = 0;
+
+	/* Reserve gpio pins */
+	if (msdev->onkey_pin != 0) {
+		err = gpio_request(msdev->onkey_pin, dev_name(msdev->dev));
+		if (err < 0) {
+			dev_err(&pdev->dev, "Request for onkey pin failed\n");
+			goto err_onkey_req;
+		}
+	}
+	if (msdev->reset_pin != 0) {
+		err = gpio_request(msdev->reset_pin, dev_name(msdev->dev));
+		if (err < 0) {
+			dev_err(&pdev->dev, "Request for reset pin failed\n");
+			goto err_reset_req;
+		}
+	}
+	if (msdev->rsthc_pin != 0) {
+		err = gpio_request(msdev->rsthc_pin, dev_name(msdev->dev));
+		if (err < 0) {
+			dev_err(&pdev->dev, "Request for rsthc pin failed\n");
+			goto err_rsthc_req;
+		}
+	}
+	if (msdev->rstext_pin != 0) {
+		err = gpio_request(msdev->rstext_pin, dev_name(msdev->dev));
+		if (err < 0) {
+			dev_err(&pdev->dev, "Request for rstext pin failed\n");
+			goto err_rstext_req;
+		}
+	}
+	if (msdev->crash_pin != 0) {
+		err = gpio_request(msdev->crash_pin, dev_name(msdev->dev));
+		if (err < 0) {
+			dev_err(&pdev->dev, "Request for crash pin failed\n");
+			goto err_crash_req;
+		}
+	}
+	if (msdev->vbat_pin != 0) {
+		err = gpio_request(msdev->vbat_pin, dev_name(msdev->dev));
+		if (err < 0) {
+			dev_err(&pdev->dev, "Request for vbat pin failed\n");
+			goto err_vbat_req;
+		}
+	}
+
+	/* Set initial pin config */
+	set_on_config(msdev);
+	if (msdev->onkey_pin)
+		nmk_config_pin(PIN_CFG(msdev->onkey_pin, GPIO) |
+			       PIN_OUTPUT_LOW, false);
+	if (msdev->vbat_pin)
+		nmk_config_pin(PIN_CFG(msdev->vbat_pin, GPIO) | PIN_OUTPUT_HIGH,
+			       false);
+
+	/* Configure IRQs for GPIO pins */
+	err = request_irq_all(msdev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "Request for irqs failed, err = %d\n", err);
+		goto err_irq_req;
+	}
+	msdev->irq_state = 1;
+
+	/* Save current modem state */
+	msdev->power_state = gpio_get_value(msdev->rsthc_pin);
+
+	return 0;
+
+err_irq_req:
+	gpio_free(msdev->vbat_pin);
+err_vbat_req:
+	gpio_free(msdev->crash_pin);
+err_crash_req:
+	gpio_free(msdev->rstext_pin);
+err_rstext_req:
+	gpio_free(msdev->rsthc_pin);
+err_rsthc_req:
+	gpio_free(msdev->reset_pin);
+err_reset_req:
+	gpio_free(msdev->onkey_pin);
+err_onkey_req:
+	return err;
+}
+
+/* Timer handlers */
+
+static void modem_power_timeout(unsigned long data)
+{
+	unsigned long flags;
+	struct modem_state_dev *msdev = (struct modem_state_dev *)data;
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	if (msdev->busy)
+		msdev->busy = 0;
+	else
+		dev_err(msdev->dev,
+			"onkey timer expired and busy flag not set\n");
+
+	gpio_set_value(msdev->onkey_pin, 0);
+	spin_unlock_irqrestore(&msdev->lock, flags);
+}
+
+static void modem_reset_timeout(unsigned long data)
+{
+	unsigned long flags;
+	struct modem_state_dev *msdev = (struct modem_state_dev *)data;
+
+	spin_lock_irqsave(&modem_state->lock, flags);
+	if (msdev->busy)
+		msdev->busy = 0;
+	else
+		dev_err(msdev->dev,
+			"reset timer expired and busy flag not set\n");
+
+	gpio_set_value(msdev->reset_pin, 1);
+	spin_unlock_irqrestore(&modem_state->lock, flags);
+}
+
+static void modem_onkey_debounce_timeout(unsigned long data)
+{
+	unsigned long flags;
+	struct modem_state_dev *msdev = (struct modem_state_dev *)data;
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	if (msdev->busy) {
+		dev_info(msdev->dev,
+			 "Delayed onkey change aborted. "
+			 "Another action in progress\n");
+	} else {
+		if (gpio_get_value(msdev->rsthc_pin) != msdev->power_state) {
+			if (0 == msdev->power_state)
+				modem_power_off(msdev);
+			else
+				modem_power_on(msdev);
+		}
+	}
+	spin_unlock_irqrestore(&msdev->lock, flags);
+}
+
+static void modem_vbat_off_timeout(unsigned long data)
+{
+	struct modem_state_dev *msdev = (struct modem_state_dev *)data;
+	unsigned long flags;
+	spin_lock_irqsave(&msdev->lock, flags);
+	if (0 == msdev->power_state)
+		modem_vbat_set_value(msdev, 0);
+	spin_unlock_irqrestore(&msdev->lock, flags);
+}
+
+static void modem_busy_on_timeout(unsigned long data)
+{
+	unsigned long flags;
+	struct modem_state_dev *msdev = (struct modem_state_dev *)data;
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	if (msdev->busy) {
+		mod_timer(&msdev->busy_timer, jiffies + 1);
+	} else {
+		msdev->busy_timer.function = NULL;
+		modem_power_on(msdev);
+	}
+	spin_unlock_irqrestore(&msdev->lock, flags);
+}
+
+static void modem_busy_off_timeout(unsigned long data)
+{
+	unsigned long flags;
+	struct modem_state_dev *msdev = (struct modem_state_dev *)data;
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	if (msdev->busy) {
+		mod_timer(&msdev->busy_timer, jiffies + 1);
+	} else {
+		msdev->busy_timer.function = NULL;
+		modem_power_off(msdev);
+	}
+	spin_unlock_irqrestore(&msdev->lock, flags);
+}
+
+static void modem_busy_reset_timeout(unsigned long data)
+{
+	unsigned long flags;
+	struct modem_state_dev *msdev = (struct modem_state_dev *)data;
+
+	spin_lock_irqsave(&msdev->lock, flags);
+	if (msdev->busy) {
+		mod_timer(&msdev->busy_timer, jiffies + 1);
+	} else {
+		msdev->busy_timer.function = NULL;
+		modem_reset(msdev);
+	}
+	spin_unlock_irqrestore(&msdev->lock, flags);
+}
+
+#ifdef DEBUG
+static int callback_test(unsigned long data)
+{
+	struct modem_state_dev *msdev = (struct modem_state_dev *)data;
+	dev_info(msdev->dev, "Test callback. Modem state is %s\n",
+		 modem_state_to_str(modem_state_get_state()));
+	return 0;
+}
+#endif
+
+/* Exported functions */
+
+void modem_state_power_on(void)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&modem_state->lock, flags);
+	if (modem_state->busy) {
+		/*
+		 * Ignore on request if turning off is queued,
+		 * cancel any queued reset request
+		 */
+		if (modem_busy_reset_timeout ==
+		    modem_state->busy_timer.function) {
+			del_timer_sync(&modem_state->busy_timer);
+			modem_state->busy_timer.function = NULL;
+		}
+		if (NULL == modem_state->busy_timer.function) {
+			modem_state->busy_timer.function =
+			    modem_busy_on_timeout;
+			modem_state->busy_timer.data =
+			    (unsigned long)modem_state;
+			mod_timer(&modem_state->busy_timer, jiffies + 1);
+		}
+	} else {
+		modem_power_on(modem_state);
+	}
+	spin_unlock_irqrestore(&modem_state->lock, flags);
+}
+
+void modem_state_power_off(void)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&modem_state->lock, flags);
+	if (modem_state->busy) {
+		/*
+		 * Prioritize off request if others are queued.
+		 * Must turn modem off if system is shutting down
+		 */
+		if (NULL != modem_state->busy_timer.function)
+			del_timer_sync(&modem_state->busy_timer);
+
+		modem_state->busy_timer.function = modem_busy_off_timeout;
+		modem_state->busy_timer.data = (unsigned long)modem_state;
+		mod_timer(&modem_state->busy_timer, jiffies + 1);
+	} else {
+		modem_power_off(modem_state);
+	}
+	spin_unlock_irqrestore(&modem_state->lock, flags);
+}
+
+void modem_state_force_reset(void)
+{
+	unsigned long flags;
+	spin_lock_irqsave(&modem_state->lock, flags);
+	if (modem_state->busy) {
+		/* Ignore reset request if turning on or off is queued */
+		if (NULL == modem_state->busy_timer.function) {
+			modem_state->busy_timer.function =
+			    modem_busy_reset_timeout;
+			modem_state->busy_timer.data =
+			    (unsigned long)modem_state;
+			mod_timer(&modem_state->busy_timer, jiffies + 1);
+		}
+	} else {
+		modem_reset(modem_state);
+	}
+	spin_unlock_irqrestore(&modem_state->lock, flags);
+}
+
+int modem_state_get_state(void)
+{
+	return get_modem_state(modem_state);
+}
+
+char *modem_state_to_str(int state)
+{
+	if (state > MODEM_STATE_END_MARKER)
+		state = MODEM_STATE_END_MARKER;
+
+	return modem_state_str[state];
+}
+
+int modem_state_register_callback(int (*callback) (unsigned long),
+				  unsigned long data)
+{
+	struct callback_list *item;
+	unsigned long flags;
+
+	if (NULL == modem_state)
+		return -EAGAIN;
+
+	if (NULL == callback)
+		return -EINVAL;
+
+	item = kzalloc(sizeof(*item), GFP_KERNEL);
+	if (NULL == item) {
+		dev_err(modem_state->dev,
+			"Could not allocate memory for struct callback_list\n");
+		return -ENOMEM;
+	}
+	item->callback = callback;
+	item->data = data;
+
+	spin_lock_irqsave(&modem_state->lock, flags);
+	list_add_tail(&item->node, &callback_list);
+	spin_unlock_irqrestore(&modem_state->lock, flags);
+
+	return 0;
+}
+
+int modem_state_remove_callback(int (*callback) (unsigned long))
+{
+	struct callback_list *iterator;
+	struct callback_list *item;
+	unsigned long flags;
+	int ret = -ENXIO;
+
+	if (NULL == callback)
+		return -EINVAL;
+
+	spin_lock_irqsave(&modem_state->lock, flags);
+	list_for_each_entry_safe(iterator, item, &callback_list, node) {
+		if (callback == item->callback) {
+			list_del(&item->node);
+			kfree(item);
+			ret = 0;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&modem_state->lock, flags);
+
+	return ret;
+}
+
+#ifdef CONFIG_PM
+int modem_state_suspend(struct device *dev)
+{
+	struct modem_state_dev *msdev =
+		platform_get_drvdata(to_platform_device(dev));
+
+	if (msdev->busy) {
+		dev_info(dev, "Driver is busy\n");
+		return -EBUSY;
+	} else {
+		return 0;
+	}
+}
+
+int modem_state_resume(struct device *dev)
+{
+	return 0;
+}
+#endif
+
+static int __devinit modem_state_probe(struct platform_device *pdev)
+{
+	int err = 0;
+
+	dev_info(&pdev->dev, "Starting probe\n");
+
+	modem_state = kzalloc(sizeof(struct modem_state_dev), GFP_KERNEL);
+	if (NULL == modem_state) {
+		dev_err(&pdev->dev,
+			"Could not allocate memory for modem_state_dev\n");
+		return -ENOMEM;
+	}
+	modem_state->dev = &pdev->dev;
+
+	spin_lock_init(&modem_state->lock);
+
+	INIT_WORK(&modem_state->wq_rsthc, wq_rsthc);
+	INIT_WORK(&modem_state->wq_rstext, wq_rstext);
+	INIT_WORK(&modem_state->wq_crash, wq_crash);
+	modem_state->workqueue =
+		create_singlethread_workqueue(dev_name(&pdev->dev));
+	if (modem_state->workqueue == NULL) {
+		dev_err(&pdev->dev, "Failed to create workqueue\n");
+		goto err_queue;
+	}
+
+	err = modem_state_dev_init(pdev, modem_state);
+	if (err != 0) {
+		dev_err(&pdev->dev, "Could not initialize device structure\n");
+		goto err_dev;
+	}
+
+	init_timer(&modem_state->onkey_timer);
+	init_timer(&modem_state->reset_timer);
+	init_timer(&modem_state->onkey_debounce_timer);
+	init_timer(&modem_state->vbat_off_timer);
+	init_timer(&modem_state->busy_timer);
+	modem_state->onkey_timer.function = modem_power_timeout;
+	modem_state->reset_timer.function = modem_reset_timeout;
+	modem_state->onkey_debounce_timer.function =
+	    modem_onkey_debounce_timeout;
+	modem_state->vbat_off_timer.function = modem_vbat_off_timeout;
+	modem_state->busy_timer.function = NULL;
+
+	platform_set_drvdata(pdev, modem_state);
+
+	err = modem_state_gpio_init(pdev, modem_state);
+	if (err != 0) {
+		dev_err(&pdev->dev, "Could not initialize GPIO\n");
+		goto err_gpio;
+	}
+
+	if (sysfs_create_group(&pdev->dev.kobj, &modemstate_attr_group) < 0) {
+		dev_err(&pdev->dev, "failed to create sysfs nodes\n");
+		goto err_sysfs;
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	modem_state->debugfsdir = debugfs_create_dir("modemstate", NULL);
+	modem_state->debugfs_debug = debugfs_create_file("debug",
+					S_IRUGO | S_IWUGO,
+					modem_state->debugfsdir,
+					modem_state,
+					&debugfs_debug_fops);
+#endif
+
+#ifdef DEBUG
+	modem_state_register_callback(callback_test,
+				      (unsigned long)modem_state);
+#endif
+	return 0;
+
+err_sysfs:
+err_gpio:
+err_dev:
+	destroy_workqueue(modem_state->workqueue);
+err_queue:
+	kfree(modem_state);
+	return err;
+}
+
+static int __devexit modem_state_remove(struct platform_device *pdev)
+{
+	struct modem_state_dev *msdev = platform_get_drvdata(pdev);
+
+	sysfs_remove_group(&pdev->dev.kobj, &modemstate_attr_group);
+	destroy_workqueue(msdev->workqueue);
+	kfree(msdev);
+	return 0;
+}
+
+static void modem_state_shutdown(struct platform_device *pdev)
+{
+	/*
+	 * Trigger software shutdown of the modem and then wait until
+	 * modem-off state is detected. If the modem does not power off
+	 * when requested power will be removed and we will detect the
+	 * modem-off state that way.
+	 */
+	modem_state_power_off();
+	if (MODEM_STATE_OFF != modem_state_get_state())
+		dev_alert(&pdev->dev, "Waiting for modem to power down\n");
+	while (MODEM_STATE_OFF != modem_state_get_state())
+		cond_resched();
+}
+
+#ifdef CONFIG_PM
+static const struct dev_pm_ops modem_state_dev_pm_ops = {
+	.suspend_noirq = modem_state_suspend,
+	.resume_noirq = modem_state_resume,
+};
+#endif
+
+static struct platform_driver modem_state_driver = {
+	.probe = modem_state_probe,
+	.remove = __devexit_p(modem_state_remove),
+	.shutdown = modem_state_shutdown,
+	.driver = {
+		   .name = "modemstate",
+		   .owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		   .pm = &modem_state_dev_pm_ops,
+#endif
+		   },
+};
+
+static int __init modem_state_init(void)
+{
+#ifdef DEBUG
+	printk(KERN_ALERT "Modem state driver init\n");
+#endif
+	return platform_driver_probe(&modem_state_driver, modem_state_probe);
+}
+
+static void __exit modem_state_exit(void)
+{
+	platform_driver_unregister(&modem_state_driver);
+}
+
+module_init(modem_state_init);
+module_exit(modem_state_exit);
+
+MODULE_AUTHOR("Derek Morton");
+MODULE_DESCRIPTION("M6718 modem power state driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/modem/m6718_spi/modem_state.h b/drivers/modem/m6718_spi/modem_state.h
new file mode 100644
index 00000000000..a2f1d9fbe3e
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_state.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Derek Morton <derek.morton@stericsson.com>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Power state driver for M6718 MODEM
+ */
+#ifndef MODEM_STATE_H
+#define MODEM_STATE_H
+
+enum modem_states {
+	MODEM_STATE_OFF,
+	MODEM_STATE_RESET,
+	MODEM_STATE_CRASH,
+	MODEM_STATE_ON,
+	/*
+	 * Add new states before end marker and update modem_state_str[]
+	 * in modem_state.c
+	 */
+	MODEM_STATE_END_MARKER
+};
+
+void modem_state_power_on(void);
+void modem_state_power_off(void);
+void modem_state_force_reset(void);
+int modem_state_get_state(void);
+char *modem_state_to_str(int state);
+
+/* Callbacks will be running in tasklet context */
+int modem_state_register_callback(int (*callback) (unsigned long),
+	unsigned long data);
+int modem_state_remove_callback(int (*callback) (unsigned long));
+
+#endif
diff --git a/drivers/modem/m6718_spi/modem_statemachine.h b/drivers/modem/m6718_spi/modem_statemachine.h
new file mode 100644
index 00000000000..6a2a32cad3a
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_statemachine.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Modem IPC driver protocol interface header:
+ *   statemachine functionality.
+ */
+#ifndef _MODEM_STATEMACHINE_H_
+#define _MODEM_STATEMACHINE_H_
+
+#include <linux/kernel.h>
+
+/* valid states for the driver state machine */
+enum ipc_sm_state_id {
+	IPC_SM_INIT,
+	IPC_SM_HALT,
+	IPC_SM_RESET,
+	IPC_SM_WAIT_SLAVE_STABLE,
+	IPC_SM_WAIT_HANDSHAKE_INACTIVE,
+	IPC_SM_SLW_TX_BOOTREQ,
+	IPC_SM_ACT_TX_BOOTREQ,
+	IPC_SM_SLW_RX_BOOTRESP,
+	IPC_SM_ACT_RX_BOOTRESP,
+	IPC_SM_IDL,
+	IPC_SM_SLW_TX_WR_CMD,
+	IPC_SM_ACT_TX_WR_CMD,
+	IPC_SM_SLW_TX_WR_DAT,
+	IPC_SM_ACT_TX_WR_DAT,
+	IPC_SM_SLW_TX_RD_CMD,
+	IPC_SM_ACT_TX_RD_CMD,
+	IPC_SM_SLW_RX_WR_CMD,
+	IPC_SM_ACT_RX_WR_CMD,
+	IPC_SM_ACT_RX_WR_DAT,
+	IPC_SM_STATE_ID_NBR
+};
+
+/* state machine trigger causes events */
+#define IPC_SM_RUN_NONE         (0x00)
+#define IPC_SM_RUN_SLAVE_IRQ    (0x01)
+#define IPC_SM_RUN_TFR_COMPLETE (0x02)
+#define IPC_SM_RUN_TX_REQ       (0x04)
+#define IPC_SM_RUN_INIT         (0x08)
+#define IPC_SM_RUN_ABORT        (0x10)
+#define IPC_SM_RUN_COMMS_TMO    (0x20)
+#define IPC_SM_RUN_STABLE_TMO   (0x40)
+#define IPC_SM_RUN_RESET        (0x80)
+
+struct ipc_link_context; /* forward declaration */
+
+typedef u8 (*ipc_sm_enter_func)(u8 event, struct ipc_link_context *context);
+typedef const struct ipc_sm_state *(*ipc_sm_exit_func)(u8 event,
+					struct ipc_link_context *context);
+
+struct ipc_sm_state {
+	enum ipc_sm_state_id id;
+	ipc_sm_enter_func enter;
+	ipc_sm_exit_func exit;
+	u8 events;
+};
+
+const struct ipc_sm_state *ipc_sm_idle_state(struct ipc_link_context *context);
+const struct ipc_sm_state *ipc_sm_init_state(struct ipc_link_context *context);
+const struct ipc_sm_state *ipc_sm_state(u8 id);
+bool ipc_sm_valid_for_state(u8 event, const struct ipc_sm_state *state);
+
+void ipc_sm_kick(u8 event, struct ipc_link_context *context);
+
+#endif /* _MODEM_STATEMACHINE_H_ */
diff --git a/drivers/modem/m6718_spi/modem_util.h b/drivers/modem/m6718_spi/modem_util.h
new file mode 100644
index 00000000000..2d9e2e39abc
--- /dev/null
+++ b/drivers/modem/m6718_spi/modem_util.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Modem IPC driver protocol interface header:
+ *   utility functionality.
+ */
+#ifndef _MODEM_UTIL_H_
+#define _MODEM_UTIL_H_
+
+#include <linux/kernel.h>
+#include "modem_private.h"
+
+bool ipc_util_channel_is_loopback(u8 channel);
+
+u32 ipc_util_make_l2_header(u8 channel, u32 len);
+u8 ipc_util_get_l2_channel(u32 hdr);
+u32 ipc_util_get_l2_length(u32 hdr);
+u32 ipc_util_make_l1_header(u8 cmd, u8 counter, u32 len);
+u8 ipc_util_get_l1_cmd(u32 hdr);
+u8 ipc_util_get_l1_counter(u32 hdr);
+u32 ipc_util_get_l1_length(u32 hdr);
+u8 ipc_util_get_l1_bootresp_ver(u32 bootresp);
+
+int ipc_util_ss_level_active(struct ipc_link_context *context);
+int ipc_util_ss_level_inactive(struct ipc_link_context *context);
+int ipc_util_int_level_active(struct ipc_link_context *context);
+int ipc_util_int_level_inactive(struct ipc_link_context *context);
+
+void ipc_util_deactivate_ss(struct ipc_link_context *context);
+void ipc_util_activate_ss(struct ipc_link_context *context);
+void ipc_util_activate_ss_with_tmo(struct ipc_link_context *context);
+
+bool ipc_util_int_is_active(struct ipc_link_context *context);
+
+bool ipc_util_link_is_idle(struct ipc_link_context *context);
+
+void ipc_util_start_slave_stable_timer(struct ipc_link_context *context);
+
+void ipc_util_spi_message_prepare(struct ipc_link_context *link_context,
+	void *tx_buf, void *rx_buf, int len);
+void ipc_util_spi_message_init(struct ipc_link_context *link_context,
+	void (*complete)(void *));
+
+bool ipc_util_link_gpio_request(struct ipc_link_context *context,
+	irqreturn_t (*irqhnd)(int, void *));
+bool ipc_util_link_gpio_config(struct ipc_link_context *context);
+bool ipc_util_link_gpio_unconfig(struct ipc_link_context *context);
+
+bool ipc_util_link_is_suspended(struct ipc_link_context *context);
+void ipc_util_suspend_link(struct ipc_link_context *context);
+void ipc_util_resume_link(struct ipc_link_context *context);
+
+#endif /* _MODEM_UTIL_H_ */
diff --git a/drivers/modem/m6718_spi/netlink.c b/drivers/modem/m6718_spi/netlink.c
new file mode 100644
index 00000000000..253b19162b1
--- /dev/null
+++ b/drivers/modem/m6718_spi/netlink.c
@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010,2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *   based on shrm_protocol.c
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * U9500 <-> M6718 IPC protocol implementation using SPI:
+ *   netlink related functionality
+ */
+#include <linux/netlink.h>
+#include <linux/spi/spi.h>
+#include <linux/modem/m6718_spi/modem_net.h>
+#include <linux/modem/m6718_spi/modem_char.h>
+#include "modem_protocol.h"
+#include "modem_private.h"
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+#include "modem_state.h"
+#endif
+
+static struct sock *netlink_sk;
+struct modem_spi_dev *modem_dev;
+
+#define MAX_PAYLOAD 1024
+
+/*
+ * Netlink broadcast message values: this must correspond to those values
+ * expected by userspace for the appropriate message.
+ */
+enum netlink_msg_id {
+	NETLINK_MODEM_RESET = 1,
+	NETLINK_MODEM_QUERY_STATE,
+	NETLINK_USER_REQUEST_MODEM_RESET,
+	NETLINK_MODEM_STATUS_ONLINE,
+	NETLINK_MODEM_STATUS_OFFLINE
+};
+
+static void netlink_multicast_tasklet(unsigned long data)
+{
+	struct sk_buff *skb;
+	struct nlmsghdr *nlh;
+	enum netlink_msg_id nlmsg = (enum netlink_msg_id)data;
+
+	if (netlink_sk == NULL) {
+		pr_err("could not send multicast, no socket\n");
+		return;
+	}
+
+	/* prepare netlink message */
+	skb = alloc_skb(NLMSG_SPACE(MAX_PAYLOAD), GFP_ATOMIC);
+	if (!skb) {
+		pr_err("failed to allocate socket buffer\n");
+		return;
+	}
+
+	if (nlmsg == NETLINK_MODEM_RESET)
+		modem_isa_reset(modem_dev);
+
+	nlh = (struct nlmsghdr *)skb->data;
+	nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD);
+	nlh->nlmsg_pid = 0;  /* from kernel */
+	nlh->nlmsg_flags = 0;
+	*(int *)NLMSG_DATA(nlh) = nlmsg;
+	skb_put(skb, MAX_PAYLOAD);
+	/* sender is in group 1<<0 */
+	NETLINK_CB(skb).pid = 0;  /* from kernel */
+	/* to mcast group 1<<0 */
+	NETLINK_CB(skb).dst_group = 1;
+
+	/* multicast the message to all listening processes */
+	pr_debug("sending netlink multicast message %d\n", nlmsg);
+	netlink_broadcast(netlink_sk, skb, 0, 1, GFP_ATOMIC);
+
+}
+
+static void send_unicast(int dst_pid)
+{
+	struct sk_buff *skb;
+	struct nlmsghdr *nlh;
+
+	if (netlink_sk == NULL) {
+		pr_err("could not send unicast, no socket\n");
+		return;
+	}
+
+	/* prepare the message for unicast */
+	skb = alloc_skb(NLMSG_SPACE(MAX_PAYLOAD), GFP_KERNEL);
+	if (!skb) {
+		pr_err("failed to allocate socket buffer\n");
+		return;
+	}
+
+	nlh = (struct nlmsghdr *)skb->data;
+	nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD);
+	nlh->nlmsg_pid = 0;  /* from kernel */
+	nlh->nlmsg_flags = 0;
+
+	if (modem_m6718_spi_is_boot_done()) {
+		pr_debug("sending netlink unicast message %d\n",
+			NETLINK_MODEM_STATUS_ONLINE);
+		*(int *)NLMSG_DATA(nlh) = NETLINK_MODEM_STATUS_ONLINE;
+	} else {
+		pr_debug("sending netlink unicast message %d\n",
+			NETLINK_MODEM_STATUS_OFFLINE);
+		*(int *)NLMSG_DATA(nlh) = NETLINK_MODEM_STATUS_OFFLINE;
+	}
+
+	skb_put(skb, MAX_PAYLOAD);
+	/* sender is in group 1<<0 */
+	NETLINK_CB(skb).pid = 0;  /* from kernel */
+	NETLINK_CB(skb).dst_group = 0;
+
+	/* unicast the message to the querying process */
+	netlink_unicast(netlink_sk, skb, dst_pid, MSG_DONTWAIT);
+}
+
+static void netlink_receive(struct sk_buff *skb)
+{
+	struct nlmsghdr *nlh = NULL;
+	int msg;
+
+	nlh = (struct nlmsghdr *)skb->data;
+	msg = *((int *)(NLMSG_DATA(nlh)));
+	switch (msg) {
+	case NETLINK_MODEM_QUERY_STATE:
+		send_unicast(nlh->nlmsg_pid);
+		break;
+	case NETLINK_USER_REQUEST_MODEM_RESET:
+		pr_info("user requested modem reset!\n");
+#ifdef CONFIG_DEBUG_FS
+		if (l1_context.msr_disable) {
+			pr_info("MSR is disabled, ignoring reset request\n");
+			break;
+		}
+#endif
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+		modem_state_force_reset();
+#else
+		pr_err("modestate integration is not enabled in IPC, "
+			"unable to reset modem\n");
+#endif
+		break;
+	default:
+		pr_debug("ignoring invalid netlink message\n");
+		break;
+	}
+}
+
+bool ipc_create_netlink_socket(struct ipc_link_context *context)
+{
+	if (netlink_sk != NULL)
+		return true;
+
+	netlink_sk = netlink_kernel_create(NULL, NETLINK_MODEM, 1,
+		netlink_receive, NULL, THIS_MODULE);
+	if (netlink_sk == NULL) {
+		dev_err(&context->sdev->dev,
+			"failed to create netlink socket\n");
+		return false;
+	}
+	modem_dev = spi_get_drvdata(context->sdev);
+	return true;
+}
+
+DECLARE_TASKLET(modem_online_tasklet, netlink_multicast_tasklet,
+	NETLINK_MODEM_STATUS_ONLINE);
+DECLARE_TASKLET(modem_reset_tasklet, netlink_multicast_tasklet,
+	NETLINK_MODEM_RESET);
+
+void ipc_broadcast_modem_online(struct ipc_link_context *context)
+{
+	dev_info(&context->sdev->dev, "broadcast modem online event!\n");
+	tasklet_schedule(&modem_online_tasklet);
+}
+
+void ipc_broadcast_modem_reset(struct ipc_link_context *context)
+{
+	dev_info(&context->sdev->dev, "broadcast modem reset event!\n");
+	tasklet_schedule(&modem_reset_tasklet);
+}
+
diff --git a/drivers/modem/m6718_spi/protocol.c b/drivers/modem/m6718_spi/protocol.c
new file mode 100644
index 00000000000..38e9190e397
--- /dev/null
+++ b/drivers/modem/m6718_spi/protocol.c
@@ -0,0 +1,429 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010,2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * U9500 <-> M6718 IPC protocol implementation using SPI.
+ */
+#include <linux/modem/m6718_spi/modem_driver.h>
+#include "modem_protocol.h"
+#include "modem_private.h"
+#include "modem_util.h"
+#include "modem_queue.h"
+#include "modem_debug.h"
+#include "modem_netlink.h"
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+#include <linux/workqueue.h>
+#include "modem_state.h"
+
+#define MODEM_STATE_REGISTER_TMO_MS  (500)
+#endif
+
+#ifdef WORKAROUND_DUPLICATED_IRQ
+#include <linux/amba/pl022.h>
+#endif
+
+struct l2mux_channel {
+	u8 open:1;
+	u8 link:7;
+};
+
+/* valid open L2 mux channels */
+static const struct l2mux_channel channels[255] = {
+	[MODEM_M6718_SPI_CHN_ISI] = {
+		.open = true,
+		.link = IPC_LINK_COMMON
+	},
+	[MODEM_M6718_SPI_CHN_AUDIO] = {
+		.open = true,
+		.link = IPC_LINK_AUDIO
+	},
+	[MODEM_M6718_SPI_CHN_MASTER_LOOPBACK0] = {
+		.open = true,
+		.link = IPC_LINK_COMMON
+	},
+	[MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK0] = {
+		.open = true,
+		.link = IPC_LINK_COMMON
+	},
+	[MODEM_M6718_SPI_CHN_MASTER_LOOPBACK1] = {
+		.open = true,
+		.link = IPC_LINK_AUDIO
+	},
+	[MODEM_M6718_SPI_CHN_SLAVE_LOOPBACK1] = {
+		.open = true,
+		.link = IPC_LINK_AUDIO
+	}
+};
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+static void modem_state_reg_wq(struct work_struct *work);
+static DECLARE_DELAYED_WORK(modem_state_reg_work, modem_state_reg_wq);
+#endif
+
+/* the spi driver context */
+struct ipc_l1_context l1_context = {
+#ifdef CONFIG_DEBUG_FS
+	.msr_disable = false,
+#endif
+	.init_done = false
+};
+
+bool modem_protocol_channel_is_open(u8 channel)
+{
+	return channels[channel].open;
+}
+
+void modem_comms_timeout(unsigned long data)
+{
+	ipc_sm_kick(IPC_SM_RUN_COMMS_TMO, (struct ipc_link_context *)data);
+}
+
+void slave_stable_timeout(unsigned long data)
+{
+	ipc_sm_kick(IPC_SM_RUN_STABLE_TMO, (struct ipc_link_context *)data);
+}
+
+/**
+ * modem_protocol_init() - initialise the IPC protocol
+ *
+ * Initialises the IPC protocol in preparation for use. After this is called
+ * the protocol is ready to be probed for each link to be supported.
+ */
+void modem_protocol_init(void)
+{
+	pr_info("M6718 IPC protocol initialising version %02x\n",
+		IPC_DRIVER_VERSION);
+
+	atomic_set(&l1_context.boot_sync_done, 0);
+	ipc_dbg_debugfs_init();
+	ipc_dbg_throughput_init();
+	l1_context.init_done = true;
+	ipc_dbg_measure_throughput(0);
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+	schedule_delayed_work(&modem_state_reg_work, 0);
+#endif
+}
+
+/**
+ * modem_m6718_spi_send() - send a frame using the IPC protocol
+ * @modem_spi_dev: pointer to modem driver information structure
+ * @channel:       L2 channel to send on
+ * @len:           length of data to send
+ * @data:          pointer to buffer containing data
+ *
+ * Check that the requested channel is supported and open, queue a frame
+ * containing the data on the appropriate link and ensure the state machine
+ * is running to start the transfer.
+ */
+int modem_m6718_spi_send(struct modem_spi_dev *modem_spi_dev, u8 channel,
+	u32 len, void *data)
+{
+	int err;
+	struct ipc_link_context *context;
+
+	if (!channels[channel].open) {
+		dev_err(modem_spi_dev->dev,
+			"error: invalid channel (%d), discarding frame\n",
+			channel);
+		return -EINVAL;
+	}
+
+	context = &l1_context.device_context[channels[channel].link];
+	if (context->state == NULL || context->state->id == IPC_SM_HALT) {
+		static unsigned long linkfail_warn_time;
+		if (printk_timed_ratelimit(&linkfail_warn_time, 60 * 1000))
+			dev_err(modem_spi_dev->dev,
+				"error: link %d for ch %d is not available, "
+				"discarding frames\n",
+				channels[channel].link, channel);
+		return -ENODEV;
+	}
+
+	err = ipc_queue_push_frame(context, channel, len, data);
+	if (err < 0)
+		return err;
+
+	if (ipc_util_link_is_idle(context)) {
+		dev_dbg(modem_spi_dev->dev,
+			"link %d is idle, kicking\n", channels[channel].link);
+		ipc_sm_kick(IPC_SM_RUN_TX_REQ, context);
+	} else {
+		dev_dbg(modem_spi_dev->dev,
+			"link %d is already running\n", channels[channel].link);
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(modem_m6718_spi_send);
+
+/**
+ * modem_m6718_spi_is_boot_done() - check if boot handshake with modem is done
+ */
+bool modem_m6718_spi_is_boot_done(void)
+{
+	return atomic_read(&l1_context.boot_sync_done);
+}
+EXPORT_SYMBOL_GPL(modem_m6718_spi_is_boot_done);
+
+/**
+ * modem_protocol_is_busy() - check if the protocol is currently active
+ * @sdev: pointer to spi_device for link to check
+ *
+ * Checks each of the IPC links to see if they are inactive: this means they
+ * can be in either IDLE or INIT states. If any of the links are not idle then
+ * true is returned to indicate that the protocol is busy.
+ */
+bool modem_protocol_is_busy(struct spi_device *sdev)
+{
+	int i;
+
+	for (i = 0; i < IPC_NBR_SUPPORTED_SPI_LINKS; i++)
+		switch (l1_context.device_context[i].state->id) {
+		case IPC_SM_IDL:
+		case IPC_SM_INIT:
+		case IPC_SM_WAIT_SLAVE_STABLE:
+			/* not busy; continue checking */
+			break;
+		default:
+			dev_info(&sdev->dev, "link %d is busy\n", i);
+			return true;
+		}
+	return false;
+}
+
+int modem_protocol_suspend(struct spi_device *sdev)
+{
+	struct modem_m6718_spi_link_platform_data *link =
+		sdev->dev.platform_data;
+	struct ipc_link_context *context;
+	int link_id;
+
+	if (link == NULL) {
+		/* platform data missing in board config? */
+		dev_err(&sdev->dev, "error: no platform data for link!\n");
+		return -ENODEV;
+	}
+
+	link_id = link->id;
+	context = &l1_context.device_context[link_id];
+
+	if (link_id >= IPC_NBR_SUPPORTED_SPI_LINKS) {
+		dev_err(&sdev->dev,
+			"link %d error: too many links! (max %d)\n",
+			link->id, IPC_NBR_SUPPORTED_SPI_LINKS);
+		return -ENODEV;
+	}
+
+	ipc_util_suspend_link(context);
+	return 0;
+}
+
+int modem_protocol_resume(struct spi_device *sdev)
+{
+	struct modem_m6718_spi_link_platform_data *link =
+		sdev->dev.platform_data;
+	struct ipc_link_context *context;
+	int link_id;
+
+	if (link == NULL) {
+		/* platform data missing in board config? */
+		dev_err(&sdev->dev, "error: no platform data for link!\n");
+		return -ENODEV;
+	}
+
+	link_id = link->id;
+	context = &l1_context.device_context[link_id];
+
+	if (link_id >= IPC_NBR_SUPPORTED_SPI_LINKS) {
+		dev_err(&sdev->dev,
+			"link %d error: too many links! (max %d)\n",
+			link->id, IPC_NBR_SUPPORTED_SPI_LINKS);
+		return -ENODEV;
+	}
+
+	ipc_util_resume_link(context);
+
+	/*
+	 * If the resume event was an interrupt from the slave then the event
+	 * is pending and we need to service it now.
+	 */
+	if (ipc_util_int_is_active(context)) {
+		dev_dbg(&sdev->dev,
+			"link %d: slave-ready is pending after resume\n",
+			link_id);
+		ipc_sm_kick(IPC_SM_RUN_SLAVE_IRQ, context);
+	}
+	return 0;
+}
+
+static void spi_tfr_complete(void *context)
+{
+	ipc_sm_kick(IPC_SM_RUN_TFR_COMPLETE,
+		(struct ipc_link_context *)context);
+}
+
+static irqreturn_t slave_ready_irq(int irq, void *dev)
+{
+	struct ipc_link_context *context = (struct ipc_link_context *)dev;
+	struct modem_m6718_spi_link_platform_data *link = context->link;
+	struct spi_device *sdev = context->sdev;
+
+	if (irq != GPIO_TO_IRQ(link->gpio.int_pin)) {
+		dev_err(&sdev->dev,
+			"link %d error: spurious slave irq!", link->id);
+		return IRQ_NONE;
+	}
+
+#ifdef WORKAROUND_DUPLICATED_IRQ
+	if (pl022_tfr_in_progress(sdev)) {
+		dev_warn(&sdev->dev,
+			"link %d warning: slave irq while transfer "
+			"is active! discarding event\n", link->id);
+		return IRQ_HANDLED;
+	}
+#endif
+	ipc_sm_kick(IPC_SM_RUN_SLAVE_IRQ, context);
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+static int modem_state_callback(unsigned long unused)
+{
+	int modem_state = modem_state_get_state();
+	struct ipc_link_context *contexts = l1_context.device_context;
+	u8 i;
+
+	pr_info("M6718 IPC protocol modemstate reports modem is %s\n",
+		modem_state_to_str(modem_state));
+
+	switch (modem_state) {
+	case MODEM_STATE_ON:
+		/*
+		 * Modem is on, ensure each link is configured and trigger
+		 * a state change on link0 to begin handshake.
+		 */
+		for (i = 0; i < IPC_NBR_SUPPORTED_SPI_LINKS; i++)
+			ipc_util_link_gpio_config(&contexts[i]);
+		ipc_sm_kick(IPC_SM_RUN_INIT, &contexts[0]);
+		break;
+	case MODEM_STATE_OFF:
+	case MODEM_STATE_RESET:
+	case MODEM_STATE_CRASH:
+		/* force all links to reset */
+		for (i = 0; i < IPC_NBR_SUPPORTED_SPI_LINKS; i++)
+			ipc_sm_kick(IPC_SM_RUN_RESET, &contexts[i]);
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+static void modem_state_reg_wq(struct work_struct *work)
+{
+	if (modem_state_register_callback(modem_state_callback, 0) == -EAGAIN) {
+		pr_info("M6718 IPC protocol failed to register with "
+			"modemstate, will retry\n");
+		schedule_delayed_work(&modem_state_reg_work,
+			(MODEM_STATE_REGISTER_TMO_MS * HZ) / 1000);
+	} else {
+		pr_info("M6718 IPC protocol registered with modemstate\n");
+	}
+}
+#endif
+
+int modem_protocol_probe(struct spi_device *sdev)
+{
+	struct modem_m6718_spi_link_platform_data *link =
+		sdev->dev.platform_data;
+	struct ipc_link_context *context;
+	int link_id;
+
+	if (link == NULL) {
+		/* platform data missing in board config? */
+		dev_err(&sdev->dev, "error: no platform data for link!\n");
+		return -ENODEV;
+	}
+
+	link_id = link->id;
+	context = &l1_context.device_context[link_id];
+
+	if (link_id >= IPC_NBR_SUPPORTED_SPI_LINKS) {
+		dev_err(&sdev->dev,
+			"link %d error: too many links! (max %d)\n",
+			link->id, IPC_NBR_SUPPORTED_SPI_LINKS);
+		return -ENODEV;
+	}
+
+	dev_info(&sdev->dev,
+		"link %d: registering SPI link bus:%d cs:%d\n",
+		link->id, sdev->master->bus_num, sdev->chip_select);
+
+	/* update spi device with correct word size for our device */
+	sdev->bits_per_word = 16;
+	spi_setup(sdev);
+
+	/* init link context */
+	context->link = link;
+	context->sdev = sdev;
+	ipc_util_resume_link(context);
+	atomic_set(&context->gpio_configured, 0);
+	atomic_set(&context->state_int,
+		ipc_util_int_level_inactive(context));
+	spin_lock_init(&context->sm_lock);
+	context->state = ipc_sm_init_state(context);
+	ipc_util_spi_message_init(context, spi_tfr_complete);
+	init_timer(&context->comms_timer);
+	context->comms_timer.function = modem_comms_timeout;
+	context->comms_timer.data = (unsigned long)context;
+	init_timer(&context->slave_stable_timer);
+	context->slave_stable_timer.function = slave_stable_timeout;
+	context->slave_stable_timer.data = (unsigned long)context;
+
+	if (!ipc_util_link_gpio_request(context, slave_ready_irq))
+		return -ENODEV;
+	if (!ipc_util_link_gpio_config(context))
+		return -ENODEV;
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_VERIFY_FRAMES
+	context->last_frame = NULL;
+#endif
+
+	ipc_queue_init(context);
+	ipc_dbg_debugfs_link_init(context);
+	ipc_dbg_throughput_link_init(context);
+	ipc_create_netlink_socket(context);
+
+	/*
+	 * For link0 (the handshake link) we force a state transition now so
+	 * that it prepares for boot sync.
+	 */
+	if (link->id == 0)
+		ipc_sm_kick(IPC_SM_RUN_INIT, context);
+
+	/*
+	 * unlikely but possible: for links other than 0, check if handshake is
+	 * already complete by the time this link is probed - if so we force a
+	 * state transition since the one issued by the handshake exit actions
+	 * will have been ignored.
+	 */
+	if (link->id > 0 && atomic_read(&l1_context.boot_sync_done)) {
+		dev_dbg(&sdev->dev,
+			"link %d: boot sync is done, kicking state machine\n",
+			link->id);
+		ipc_sm_kick(IPC_SM_RUN_INIT, context);
+	}
+	return 0;
+}
+
+void modem_protocol_exit(void)
+{
+	int i;
+
+	pr_info("M6718 IPC protocol exit\n");
+	for (i = 0; i < IPC_NBR_SUPPORTED_SPI_LINKS; i++)
+		ipc_util_link_gpio_unconfig(&l1_context.device_context[i]);
+}
diff --git a/drivers/modem/m6718_spi/queue.c b/drivers/modem/m6718_spi/queue.c
new file mode 100644
index 00000000000..fe23ac36736
--- /dev/null
+++ b/drivers/modem/m6718_spi/queue.c
@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010,2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * U9500 <-> M6718 IPC protocol implementation using SPI:
+ *   TX queue functionality.
+ */
+#include <linux/modem/m6718_spi/modem_driver.h>
+#include "modem_util.h"
+
+#define FRAME_LENGTH_ALIGN (4)
+#define MAX_FRAME_COUNTER (256)
+
+void ipc_queue_init(struct ipc_link_context *context)
+{
+	spin_lock_init(&context->tx_q_update_lock);
+	atomic_set(&context->tx_q_count, 0);
+	context->tx_q_free = IPC_TX_QUEUE_MAX_SIZE;
+	INIT_LIST_HEAD(&context->tx_q);
+	context->tx_frame_counter = 0;
+}
+
+void ipc_queue_delete_frame(struct ipc_tx_queue *frame)
+{
+	kfree(frame);
+}
+
+struct ipc_tx_queue *ipc_queue_new_frame(struct ipc_link_context *link_context,
+	u32 l2_length)
+{
+	struct ipc_tx_queue *frame;
+	u32 padded_len = l2_length;
+
+	/* frame length padded to alignment boundary */
+	if (padded_len % FRAME_LENGTH_ALIGN)
+		padded_len += (FRAME_LENGTH_ALIGN -
+				(padded_len % FRAME_LENGTH_ALIGN));
+
+	dev_dbg(&link_context->sdev->dev,
+		"link %d: new frame: length %d, padded to %d\n",
+		link_context->link->id, l2_length, padded_len);
+
+	frame = kzalloc(sizeof(*frame) + padded_len, GFP_ATOMIC);
+	if (frame == NULL) {
+		dev_err(&link_context->sdev->dev,
+			"link %d error: failed to allocate frame\n",
+			link_context->link->id);
+		return NULL;
+	}
+
+	frame->actual_len = l2_length;
+	frame->len = padded_len;
+	frame->data = frame + 1;
+	return frame;
+}
+
+bool ipc_queue_is_empty(struct ipc_link_context *context)
+{
+	unsigned long flags;
+	bool empty;
+
+	spin_lock_irqsave(&context->tx_q_update_lock, flags);
+	empty = list_empty(&context->tx_q);
+	spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
+
+	return empty;
+}
+
+int ipc_queue_push_frame(struct ipc_link_context *context, u8 channel,
+	u32 length, void *data)
+{
+	u32 l2_hdr;
+	unsigned long flags;
+	struct ipc_tx_queue *frame;
+	int *tx_frame_counter = &context->tx_frame_counter;
+	int qcount;
+
+	/*
+	 * Max queue size is only approximate so we allow it to go a few bytes
+	 * over the limit
+	 */
+	if (context->tx_q_free < length) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: tx queue full, wanted %d free %d\n",
+			context->link->id,
+			length,
+			context->tx_q_free);
+		return -EAGAIN;
+	}
+
+	frame = ipc_queue_new_frame(context, length + IPC_L2_HDR_SIZE);
+	if (frame == NULL)
+		return -ENOMEM;
+
+	/* create l2 header and copy to pdu buffer */
+	l2_hdr = ipc_util_make_l2_header(channel, length);
+	*(u32 *)frame->data = l2_hdr;
+
+	/* copy the l2 sdu into the pdu buffer after the header */
+	memcpy(frame->data + IPC_L2_HDR_SIZE, data, length);
+
+	spin_lock_irqsave(&context->tx_q_update_lock, flags);
+	frame->counter = *tx_frame_counter;
+	*tx_frame_counter = (*tx_frame_counter + 1) % MAX_FRAME_COUNTER;
+	list_add_tail(&frame->node, &context->tx_q);
+	qcount = atomic_add_return(1, &context->tx_q_count);
+	/* tx_q_free could go negative here */
+	context->tx_q_free -= frame->len;
+#ifdef CONFIG_DEBUG_FS
+	context->tx_q_min = min(context->tx_q_free, context->tx_q_min);
+#endif
+	spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: push tx frame %d: %08x (ch %d len %d), "
+		"new count %d, new free %d\n",
+		context->link->id,
+		frame->counter,
+		l2_hdr,
+		ipc_util_get_l2_channel(l2_hdr),
+		ipc_util_get_l2_length(l2_hdr),
+		qcount,
+		context->tx_q_free);
+	return 0;
+}
+
+struct ipc_tx_queue *ipc_queue_get_frame(struct ipc_link_context *context)
+{
+	unsigned long flags;
+	struct ipc_tx_queue *frame;
+	int qcount;
+
+	spin_lock_irqsave(&context->tx_q_update_lock, flags);
+	frame = list_first_entry(&context->tx_q, struct ipc_tx_queue, node);
+	list_del(&frame->node);
+	qcount = atomic_sub_return(1, &context->tx_q_count);
+	context->tx_q_free += frame->len;
+	spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: get tx frame %d, new count %d, "
+		"new free %d\n",
+		context->link->id, frame->counter, qcount, context->tx_q_free);
+	return frame;
+}
+
+void ipc_queue_reset(struct ipc_link_context *context)
+{
+	unsigned long flags;
+	struct ipc_tx_queue *frame;
+	int qcount;
+
+	spin_lock_irqsave(&context->tx_q_update_lock, flags);
+	qcount = atomic_read(&context->tx_q_count);
+	while (qcount != 0) {
+		frame = list_first_entry(&context->tx_q,
+				struct ipc_tx_queue, node);
+		list_del(&frame->node);
+		ipc_queue_delete_frame(frame);
+		qcount = atomic_sub_return(1, &context->tx_q_count);
+	}
+	spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
+}
diff --git a/drivers/modem/m6718_spi/statemachine.c b/drivers/modem/m6718_spi/statemachine.c
new file mode 100644
index 00000000000..bb047fcee18
--- /dev/null
+++ b/drivers/modem/m6718_spi/statemachine.c
@@ -0,0 +1,1089 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010,2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * U9500 <-> M6718 IPC protocol implementation using SPI.
+ *   state machine definition and functionality.
+ */
+#include <linux/modem/m6718_spi/modem_driver.h>
+#include "modem_statemachine.h"
+#include "modem_util.h"
+#include "modem_netlink.h"
+#include "modem_debug.h"
+#include "modem_queue.h"
+#include "modem_protocol.h"
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+#include "modem_state.h"
+#endif
+
+#define CMD_BOOTREQ  (1)
+#define CMD_BOOTRESP (2)
+#define CMD_WRITE    (3)
+#define CMD_READ     (4)
+
+static u8 sm_init_enter(u8 event, struct ipc_link_context *context)
+{
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+	/* if modem is off un-configure the IPC GPIO pins for low-power */
+	if (modem_state_get_state() == MODEM_STATE_OFF) {
+		dev_info(&context->sdev->dev,
+			"link %d: modem is off, un-configuring GPIO\n",
+			context->link->id);
+		ipc_util_link_gpio_unconfig(context);
+	}
+#endif
+	/* nothing more to do until an event happens */
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_init_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	bool int_active = false;
+
+	/*
+	 * For reset event just re-enter init in case the modem has
+	 * powered off - we need to reconfigure our GPIO pins
+	 */
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_INIT);
+
+	/* re-sample link INT pin */
+	int_active = ipc_util_int_is_active(context);
+	atomic_set(&context->state_int, int_active);
+
+	dev_info(&context->sdev->dev,
+		"link %d: link initialised; SS:INACTIVE(%d) INT:%s(%d)\n",
+		context->link->id,
+		ipc_util_ss_level_inactive(context),
+		int_active ? "ACTIVE" : "INACTIVE",
+		int_active ? ipc_util_int_level_active(context) :
+				ipc_util_int_level_inactive(context));
+
+	/* handshake is only on link 0 */
+	if (context->link->id == 0) {
+		if (!int_active) {
+			dev_info(&context->sdev->dev,
+				"link %d: slave INT signal is inactive\n",
+				context->link->id);
+			/* start boot handshake */
+			return ipc_sm_state(IPC_SM_SLW_TX_BOOTREQ);
+		} else {
+			/* wait for slave INT signal to stabilise inactive */
+			return ipc_sm_state(IPC_SM_WAIT_SLAVE_STABLE);
+		}
+	} else {
+		dev_info(&context->sdev->dev,
+			"link %d: boot sync not needed, going idle\n",
+			context->link->id);
+		return ipc_sm_state(IPC_SM_IDL);
+	}
+}
+
+static u8 sm_wait_slave_stable_enter(u8 event, struct ipc_link_context *context)
+{
+	static unsigned long printk_warn_time;
+	if (printk_timed_ratelimit(&printk_warn_time, 60 * 1000))
+		dev_info(&context->sdev->dev,
+			"link %d: waiting for stable inactive slave INT\n",
+			context->link->id);
+	ipc_util_start_slave_stable_timer(context);
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_wait_slave_stable_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	if (!ipc_util_int_is_active(context)) {
+		dev_info(&context->sdev->dev,
+			"link %d: slave INT signal is stable inactive\n",
+			context->link->id);
+		return ipc_sm_state(IPC_SM_SLW_TX_BOOTREQ);
+	} else {
+		return ipc_sm_state(IPC_SM_WAIT_SLAVE_STABLE);
+	}
+}
+
+static u8 sm_wait_handshake_inactive_enter(u8 event,
+	struct ipc_link_context *context)
+{
+	dev_info(&context->sdev->dev,
+		"link %d: waiting for stable inactive slave INT\n",
+		context->link->id);
+	ipc_util_start_slave_stable_timer(context);
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_wait_handshake_inactive_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	int i;
+
+	if (!ipc_util_int_is_active(context)) {
+		dev_info(&context->sdev->dev,
+			"link %d: slave INT signal is inactive, going idle\n",
+			context->link->id);
+
+		/* modem sync is done */
+		atomic_inc(&l1_context.boot_sync_done);
+		ipc_broadcast_modem_online(context);
+
+		/*
+		 * Kick the state machine for any initialised links - skip link0
+		 * since this link has just completed handshake
+		 */
+		for (i = 1; i < IPC_NBR_SUPPORTED_SPI_LINKS; i++)
+			if (l1_context.device_context[i].state != NULL) {
+				dev_dbg(&context->sdev->dev,
+					"link %d has already been probed, "
+					"kicking state machine\n", i);
+				ipc_sm_kick(IPC_SM_RUN_INIT,
+					&l1_context.device_context[i]);
+			}
+		return ipc_sm_state(IPC_SM_IDL);
+	} else {
+		return ipc_sm_state(IPC_SM_WAIT_HANDSHAKE_INACTIVE);
+	}
+}
+
+static u8 sm_idl_enter(u8 event, struct ipc_link_context *context)
+{
+	ipc_util_deactivate_ss(context);
+	ipc_dbg_enter_idle(context);
+
+	/* check if tx queue contains items */
+	if (atomic_read(&context->tx_q_count) > 0) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: tx queue contains items\n",
+			context->link->id);
+		return IPC_SM_RUN_TX_REQ;
+	}
+
+	/* check if modem has already requested transaction start */
+	if (atomic_read(&context->state_int)) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: slave has already signalled ready\n",
+			context->link->id);
+		return IPC_SM_RUN_SLAVE_IRQ;
+	}
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: going idle\n", context->link->id);
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_idl_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	ipc_dbg_exit_idle(context);
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+	else if (event == IPC_SM_RUN_TX_REQ)
+		return ipc_sm_state(IPC_SM_SLW_TX_WR_CMD);
+	else if (event == IPC_SM_RUN_SLAVE_IRQ)
+		return ipc_sm_state(IPC_SM_SLW_TX_RD_CMD);
+	else
+		return ipc_sm_state(IPC_SM_HALT);
+}
+
+static u8 sm_slw_tx_wr_cmd_enter(u8 event, struct ipc_link_context *context)
+{
+	struct ipc_tx_queue *frame;
+
+	/* get the frame from the head of the tx queue */
+	if (ipc_queue_is_empty(context)) {
+		dev_err(&context->sdev->dev,
+			"link %d error: tx queue is empty!\n",
+			context->link->id);
+		return IPC_SM_RUN_ABORT;
+	}
+	frame = ipc_queue_get_frame(context);
+	ipc_dbg_dump_frame(&context->sdev->dev, context->link->id, frame, true);
+
+	context->cmd = ipc_util_make_l1_header(CMD_WRITE, frame->counter,
+		frame->len);
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: TX FRAME cmd %08x (type %d counter %d len %d)\n",
+		context->link->id,
+		context->cmd,
+		ipc_util_get_l1_cmd(context->cmd),
+		ipc_util_get_l1_counter(context->cmd),
+		ipc_util_get_l1_length(context->cmd));
+
+	ipc_util_spi_message_prepare(context, &context->cmd,
+		NULL, IPC_L1_HDR_SIZE);
+	context->frame = frame;
+
+	/* slave might already have signalled ready to transmit */
+	if (atomic_read(&context->state_int)) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: slave has already signalled ready\n",
+			context->link->id);
+		ipc_util_activate_ss(context);
+		return IPC_SM_RUN_SLAVE_IRQ;
+	} else {
+		ipc_util_activate_ss_with_tmo(context);
+		return IPC_SM_RUN_NONE;
+	}
+}
+
+static const struct ipc_sm_state *sm_slw_tx_wr_cmd_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+	else if (event == IPC_SM_RUN_COMMS_TMO)
+		return ipc_sm_state(IPC_SM_HALT);
+	else
+		return ipc_sm_state(IPC_SM_ACT_TX_WR_CMD);
+}
+
+static u8 sm_act_tx_wr_cmd_enter(u8 event, struct ipc_link_context *context)
+{
+	int err;
+
+	/* slave is ready - start the spi transfer */
+	dev_dbg(&context->sdev->dev,
+		"link %d: starting spi tfr\n", context->link->id);
+	err = spi_async(context->sdev, &context->spi_message);
+	if (err < 0) {
+		dev_err(&context->sdev->dev,
+			"link %d error: spi tfr start failed, error %d\n",
+			context->link->id, err);
+		return IPC_SM_RUN_ABORT;
+	}
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_act_tx_wr_cmd_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+	else
+		return ipc_sm_state(IPC_SM_SLW_TX_WR_DAT);
+}
+
+static u8 sm_slw_tx_wr_dat_enter(u8 event, struct ipc_link_context *context)
+{
+	/* prepare to transfer the frame tx data */
+	ipc_util_spi_message_prepare(context, context->frame->data,
+		NULL, context->frame->len);
+
+	/* slave might already have signalled ready to transmit */
+	if (atomic_read(&context->state_int)) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: slave has already signalled ready\n",
+			context->link->id);
+		ipc_util_activate_ss(context);
+		return IPC_SM_RUN_SLAVE_IRQ;
+	} else {
+		ipc_util_activate_ss_with_tmo(context);
+		return IPC_SM_RUN_NONE;
+	}
+}
+
+static const struct ipc_sm_state *sm_slw_tx_wr_dat_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+	else if (event == IPC_SM_RUN_COMMS_TMO)
+		return ipc_sm_state(IPC_SM_HALT);
+	else
+		return ipc_sm_state(IPC_SM_ACT_TX_WR_DAT);
+}
+
+static u8 sm_act_tx_wr_dat_enter(u8 event, struct ipc_link_context *context)
+{
+	int err;
+
+	/* slave is ready - start the spi transfer */
+	dev_dbg(&context->sdev->dev,
+		"link %d: starting spi tfr\n", context->link->id);
+	err = spi_async(context->sdev, &context->spi_message);
+	if (err < 0) {
+		dev_err(&context->sdev->dev,
+			"link %d error: spi tfr start failed, error %d\n",
+			context->link->id, err);
+		return IPC_SM_RUN_ABORT;
+	}
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_act_tx_wr_dat_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	/* frame is sent, increment link tx counter */
+	context->tx_bytes += context->frame->actual_len;
+#endif
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_VERIFY_FRAMES
+	{
+		u8 channel;
+
+		channel = ipc_util_get_l2_channel(*(u32 *)context->frame->data);
+		if (ipc_util_channel_is_loopback(channel)) {
+			context->last_frame = context->frame;
+		} else {
+			ipc_queue_delete_frame(context->frame);
+			context->frame = NULL;
+		}
+	}
+#else
+	/* free the sent frame */
+	ipc_queue_delete_frame(context->frame);
+	context->frame = NULL;
+#endif
+	return ipc_sm_state(IPC_SM_SLW_TX_RD_CMD);
+}
+
+static u8 sm_slw_tx_rd_cmd_enter(u8 event, struct ipc_link_context *context)
+{
+	context->cmd = ipc_util_make_l1_header(CMD_READ, 0, 0);
+	dev_dbg(&context->sdev->dev,
+		"link %d: cmd %08x (type %d)\n",
+		context->link->id,
+		context->cmd,
+		ipc_util_get_l1_cmd(context->cmd));
+
+	/* prepare the spi message to transfer */
+	ipc_util_spi_message_prepare(context, &context->cmd,
+		NULL, IPC_L1_HDR_SIZE);
+
+	/* check if the slave requested this transaction */
+	if (event == IPC_SM_RUN_SLAVE_IRQ) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: slave initiated transaction, continue\n",
+			context->link->id);
+		ipc_util_activate_ss(context);
+		return IPC_SM_RUN_SLAVE_IRQ;
+	} else {
+		/* slave might already have signalled ready to transmit */
+		if (atomic_read(&context->state_int)) {
+			dev_dbg(&context->sdev->dev,
+				"link %d: slave has already signalled ready\n",
+				context->link->id);
+			ipc_util_activate_ss(context);
+			return IPC_SM_RUN_SLAVE_IRQ;
+		} else {
+			ipc_util_activate_ss_with_tmo(context);
+			return IPC_SM_RUN_NONE;
+		}
+	}
+}
+
+static const struct ipc_sm_state *sm_slw_tx_rd_cmd_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+	else if (event == IPC_SM_RUN_COMMS_TMO)
+		return ipc_sm_state(IPC_SM_HALT);
+	else
+		return ipc_sm_state(IPC_SM_ACT_TX_RD_CMD);
+}
+
+static u8 sm_act_tx_rd_cmd_enter(u8 event, struct ipc_link_context *context)
+{
+	int err;
+
+	/* slave is ready - start the spi transfer */
+	dev_dbg(&context->sdev->dev,
+		"link %d: starting spi tfr\n", context->link->id);
+	err = spi_async(context->sdev, &context->spi_message);
+	if (err < 0) {
+		dev_err(&context->sdev->dev,
+			"link %d error: spi tfr start failed, error %d\n",
+			context->link->id, err);
+		return IPC_SM_RUN_ABORT;
+	}
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_act_tx_rd_cmd_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+	else
+		return ipc_sm_state(IPC_SM_SLW_RX_WR_CMD);
+}
+
+static u8 sm_slw_rx_wr_cmd_enter(u8 event, struct ipc_link_context *context)
+{
+	/* prepare to receive MESSAGE WRITE frame header */
+	ipc_util_spi_message_prepare(context, NULL,
+		&context->cmd, IPC_L1_HDR_SIZE);
+
+	/* slave might already have signalled ready to transmit */
+	if (atomic_read(&context->state_int)) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: slave has already signalled ready\n",
+			context->link->id);
+		ipc_util_activate_ss(context);
+		return IPC_SM_RUN_SLAVE_IRQ;
+	} else {
+		ipc_util_activate_ss_with_tmo(context);
+		return IPC_SM_RUN_NONE;
+	}
+}
+
+static const struct ipc_sm_state *sm_slw_rx_wr_cmd_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+	else if (event == IPC_SM_RUN_COMMS_TMO)
+		return ipc_sm_state(IPC_SM_HALT);
+	else
+		return ipc_sm_state(IPC_SM_ACT_RX_WR_CMD);
+}
+
+static u8 sm_act_rx_wr_cmd_enter(u8 event, struct ipc_link_context *context)
+{
+	int err;
+
+	/* slave is ready - start the spi transfer */
+	dev_dbg(&context->sdev->dev,
+		"link %d: starting spi tfr\n", context->link->id);
+	err = spi_async(context->sdev, &context->spi_message);
+	if (err < 0) {
+		dev_err(&context->sdev->dev,
+			"link %d error: spi tfr start failed, error %d\n",
+			context->link->id, err);
+		return IPC_SM_RUN_ABORT;
+	}
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_act_rx_wr_cmd_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	u8 cmd_type = ipc_util_get_l1_cmd(context->cmd);
+	int counter = ipc_util_get_l1_counter(context->cmd);
+	int length  = ipc_util_get_l1_length(context->cmd);
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: RX HEADER %08x (type %d counter %d length %d)\n",
+		context->link->id,
+		context->cmd,
+		cmd_type,
+		counter,
+		length);
+
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+
+	if (cmd_type == CMD_WRITE) {
+		/* slave has data to send - allocate a frame to hold it */
+		context->frame = ipc_queue_new_frame(context, length);
+		if (context->frame == NULL)
+			return ipc_sm_state(IPC_SM_IDL);
+
+		context->frame->counter = counter;
+		ipc_util_spi_message_prepare(context, NULL,
+			context->frame->data, context->frame->len);
+		return ipc_sm_state(IPC_SM_ACT_RX_WR_DAT);
+	} else {
+		if (cmd_type != 0)
+			dev_err(&context->sdev->dev,
+				"link %d error: received invalid frame type %x "
+				"(%08x)! assuming TRANSACTION_END...\n",
+				context->link->id,
+				cmd_type,
+				context->cmd);
+
+		/* slave has no data to send */
+		dev_dbg(&context->sdev->dev,
+			"link %d: slave has no data to send\n",
+			context->link->id);
+		return ipc_sm_state(IPC_SM_IDL);
+	}
+}
+
+static u8 sm_act_rx_wr_dat_enter(u8 event, struct ipc_link_context *context)
+{
+	int err;
+
+	/* assume slave is still ready - prepare and start the spi transfer */
+	ipc_util_spi_message_prepare(context, NULL,
+		context->frame->data, context->frame->len);
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: starting spi tfr\n", context->link->id);
+	err = spi_async(context->sdev, &context->spi_message);
+	if (err < 0) {
+		dev_err(&context->sdev->dev,
+			"link %d error: spi tfr start failed, error %d\n",
+			context->link->id, err);
+		return IPC_SM_RUN_ABORT;
+	}
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_act_rx_wr_dat_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	u32           frame_hdr;
+	unsigned char l2_header;
+	unsigned int  l2_length;
+	u8            *l2_data;
+
+	if (event == IPC_SM_RUN_RESET)
+		return ipc_sm_state(IPC_SM_RESET);
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: RX PAYLOAD %d bytes\n",
+		context->link->id, context->frame->len);
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+	/* frame is received, increment link rx counter */
+	context->rx_bytes += context->frame->len;
+#endif
+	/* decode L2 header */
+	frame_hdr = *(u32 *)context->frame->data;
+	l2_header = ipc_util_get_l2_channel(frame_hdr);
+	l2_length = ipc_util_get_l2_length(frame_hdr);
+	l2_data   = (u8 *)context->frame->data + IPC_L2_HDR_SIZE;
+
+	context->frame->actual_len = l2_length + IPC_L2_HDR_SIZE;
+	ipc_dbg_dump_frame(&context->sdev->dev, context->link->id,
+		context->frame, false);
+
+	if (l2_length > (context->frame->len - 4)) {
+		dev_err(&context->sdev->dev,
+			"link %d: suspicious frame: L1 len %d L2 len %d\n",
+			context->link->id, context->frame->len, l2_length);
+	}
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: L2 PDU decode: header 0x%08x channel %d length %d "
+		"data[%02x%02x%02x...]\n",
+		context->link->id, frame_hdr, l2_header, l2_length,
+		l2_data[0], l2_data[1], l2_data[2]);
+
+	if (ipc_util_channel_is_loopback(l2_header))
+		ipc_dbg_verify_rx_frame(context);
+
+	/* pass received frame up to L2mux layer */
+	if (!modem_protocol_channel_is_open(l2_header)) {
+		dev_err(&context->sdev->dev,
+			"link %d error: received frame on invalid channel %d, "
+			"frame discarded\n",
+			context->link->id, l2_header);
+	} else {
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_THROUGHPUT_MEASUREMENT
+		/*
+		 * Discard loopback frames if we are taking throughput
+		 * measurements - we'll be loading the links and so will likely
+		 * overload the buffers.
+		 */
+		if (!ipc_util_channel_is_loopback(l2_header))
+#endif
+			modem_m6718_spi_receive(context->sdev,
+				l2_header, l2_length, l2_data);
+	}
+
+	/* data is copied by L2mux so free the frame here */
+	ipc_queue_delete_frame(context->frame);
+	context->frame = NULL;
+
+	/* check tx queue for content */
+	if (!ipc_queue_is_empty(context)) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: tx queue not empty\n", context->link->id);
+		return ipc_sm_state(IPC_SM_SLW_TX_WR_CMD);
+	} else {
+		dev_dbg(&context->sdev->dev,
+			"link %d: tx queue empty\n", context->link->id);
+		return ipc_sm_state(IPC_SM_SLW_TX_RD_CMD);
+	}
+}
+
+static u8 sm_halt_enter(u8 event, struct ipc_link_context *context)
+{
+	dev_err(&context->sdev->dev,
+		"link %d error: HALTED\n", context->link->id);
+
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_MODEM_STATE
+	/*
+	 * Force modem reset, this will cause a reset event from the modemstate
+	 * driver which will reset the links. If debugfs is enabled then there
+	 * is a userspace file which controls whether MSR is enabled or not.
+	 */
+#ifdef CONFIG_DEBUG_FS
+	if (l1_context.msr_disable) {
+		dev_info(&context->sdev->dev,
+			"link %d: MSR is disabled by user, "
+			"not requesting modem reset\n", context->link->id);
+		return IPC_SM_RUN_RESET;
+	}
+#endif
+	modem_state_force_reset();
+#endif
+	return IPC_SM_RUN_RESET;
+}
+
+static const struct ipc_sm_state *sm_halt_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	return ipc_sm_state(IPC_SM_RESET);
+}
+
+static u8 sm_reset_enter(u8 event, struct ipc_link_context *context)
+{
+	dev_err(&context->sdev->dev,
+		"link %d resetting\n", context->link->id);
+
+	if (context->link->id == 0)
+		ipc_broadcast_modem_reset(context);
+
+	ipc_util_deactivate_ss(context);
+	ipc_queue_reset(context);
+	if (context->frame != NULL) {
+		ipc_queue_delete_frame(context->frame);
+		context->frame = NULL;
+	}
+#ifdef CONFIG_MODEM_M6718_SPI_ENABLE_FEATURE_VERIFY_FRAMES
+	if (context->last_frame != NULL) {
+		ipc_queue_delete_frame(context->last_frame);
+		context->last_frame = NULL;
+	}
+#endif
+	dev_dbg(&context->sdev->dev,
+		"link %d reset completed\n", context->link->id);
+
+	return IPC_SM_RUN_RESET;
+}
+
+static const struct ipc_sm_state *sm_reset_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	return ipc_sm_state(IPC_SM_INIT);
+}
+
+static u8 sm_slw_tx_bootreq_enter(u8 event, struct ipc_link_context *context)
+{
+	dev_info(&context->sdev->dev,
+		"link %d: waiting for boot sync\n", context->link->id);
+
+	ipc_util_activate_ss(context);
+	context->cmd = ipc_util_make_l1_header(CMD_BOOTREQ, 0,
+		IPC_DRIVER_VERSION);
+	dev_dbg(&context->sdev->dev,
+		"link %d: TX HEADER cmd %08x (type %x)\n",
+		context->link->id,
+		context->cmd,
+		ipc_util_get_l1_cmd(context->cmd));
+	ipc_util_spi_message_prepare(context, &context->cmd,
+		NULL, IPC_L1_HDR_SIZE);
+
+	/* wait now for the slave to indicate ready... */
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_slw_tx_bootreq_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	return ipc_sm_state(IPC_SM_ACT_TX_BOOTREQ);
+}
+
+static u8 sm_act_tx_bootreq_enter(u8 event, struct ipc_link_context *context)
+{
+	int err;
+
+	/* slave is ready - start the spi transfer */
+	dev_dbg(&context->sdev->dev,
+		"link %d: starting spi tfr\n", context->link->id);
+	err = spi_async(context->sdev, &context->spi_message);
+	if (err < 0) {
+		dev_err(&context->sdev->dev,
+			"link %d error: spi tfr start failed, error %d\n",
+			context->link->id, err);
+		return IPC_SM_RUN_ABORT;
+	}
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_act_tx_bootreq_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	return ipc_sm_state(IPC_SM_SLW_RX_BOOTRESP);
+}
+
+static u8 sm_slw_rx_bootresp_enter(u8 event, struct ipc_link_context *context)
+{
+	/* prepare to receive BOOTRESP frame header */
+	ipc_util_spi_message_prepare(context, NULL,
+		&context->cmd, IPC_L1_HDR_SIZE);
+
+	/* slave might already have signalled ready to transmit */
+	if (atomic_read(&context->state_int)) {
+		dev_dbg(&context->sdev->dev,
+			"link %d: slave has already signalled ready\n",
+			context->link->id);
+		ipc_util_activate_ss(context);
+		return IPC_SM_RUN_SLAVE_IRQ;
+	} else {
+		ipc_util_activate_ss_with_tmo(context);
+		return IPC_SM_RUN_NONE;
+	}
+}
+
+static const struct ipc_sm_state *sm_slw_rx_bootresp_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	if (event == IPC_SM_RUN_COMMS_TMO) {
+		/*
+		 * Modem timeout: was it really ready or just noise?
+		 * Revert to waiting for handshake to start.
+		 */
+		ipc_util_deactivate_ss(context);
+		return ipc_sm_state(IPC_SM_SLW_TX_BOOTREQ);
+	} else {
+		return ipc_sm_state(IPC_SM_ACT_RX_BOOTRESP);
+	}
+}
+
+static u8 sm_act_rx_bootresp_enter(u8 event, struct ipc_link_context *context)
+{
+	int err;
+
+	/* slave is ready - start the spi transfer */
+	dev_dbg(&context->sdev->dev,
+		"link %d: starting spi tfr\n", context->link->id);
+	err = spi_async(context->sdev, &context->spi_message);
+	if (err < 0) {
+		dev_err(&context->sdev->dev,
+			"link %d error: spi tfr start failed, error %d\n",
+			context->link->id, err);
+		return IPC_SM_RUN_ABORT;
+	}
+	return IPC_SM_RUN_NONE;
+}
+
+static const struct ipc_sm_state *sm_act_rx_bootresp_exit(u8 event,
+	struct ipc_link_context *context)
+{
+	u8 cmd_type = ipc_util_get_l1_cmd(context->cmd);
+	u8 modem_ver;
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: RX HEADER %08x (type %d)\n",
+		context->link->id, context->cmd, cmd_type);
+
+	if (cmd_type == CMD_BOOTRESP) {
+		modem_ver = ipc_util_get_l1_bootresp_ver(context->cmd);
+
+		dev_info(&context->sdev->dev,
+			"link %d: boot sync done; "
+			"APE version %02x, MODEM version %02x\n",
+			context->link->id, IPC_DRIVER_VERSION, modem_ver);
+
+		/* check for minimum required modem version */
+		if (modem_ver != IPC_DRIVER_MODEM_MIN_VER) {
+			dev_warn(&context->sdev->dev,
+				"link %d warning: modem version mismatch! "
+				"required version is %02x\n",
+				context->link->id,
+				IPC_DRIVER_MODEM_MIN_VER);
+		}
+
+		return ipc_sm_state(IPC_SM_WAIT_HANDSHAKE_INACTIVE);
+	} else {
+		/* invalid response... this is not our slave */
+		dev_err(&context->sdev->dev,
+			"link %d error: expected %x (BOOTRESP), received %x.\n",
+			context->link->id,
+			CMD_BOOTRESP,
+			cmd_type);
+		return ipc_sm_state(IPC_SM_HALT);
+	}
+}
+
+/* the driver protocol state machine */
+static const struct ipc_sm_state state_machine[IPC_SM_STATE_ID_NBR] = {
+	[IPC_SM_INIT] = {
+		.id     = IPC_SM_INIT,
+		.enter  = sm_init_enter,
+		.exit   = sm_init_exit,
+		.events = IPC_SM_RUN_INIT | IPC_SM_RUN_RESET
+	},
+	[IPC_SM_HALT] = {
+		.id     = IPC_SM_HALT,
+		.enter  = sm_halt_enter,
+		.exit   = sm_halt_exit,
+		.events = IPC_SM_RUN_RESET
+	},
+	[IPC_SM_RESET] = {
+		.id     = IPC_SM_RESET,
+		.enter  = sm_reset_enter,
+		.exit   = sm_reset_exit,
+		.events = IPC_SM_RUN_RESET
+	},
+	[IPC_SM_WAIT_SLAVE_STABLE] = {
+		.id     = IPC_SM_WAIT_SLAVE_STABLE,
+		.enter  = sm_wait_slave_stable_enter,
+		.exit   = sm_wait_slave_stable_exit,
+		.events = IPC_SM_RUN_STABLE_TMO
+	},
+	[IPC_SM_WAIT_HANDSHAKE_INACTIVE] = {
+		.id     = IPC_SM_WAIT_HANDSHAKE_INACTIVE,
+		.enter  = sm_wait_handshake_inactive_enter,
+		.exit   = sm_wait_handshake_inactive_exit,
+		.events = IPC_SM_RUN_STABLE_TMO
+	},
+	[IPC_SM_SLW_TX_BOOTREQ] = {
+		.id     = IPC_SM_SLW_TX_BOOTREQ,
+		.enter  = sm_slw_tx_bootreq_enter,
+		.exit   = sm_slw_tx_bootreq_exit,
+		.events = IPC_SM_RUN_SLAVE_IRQ
+	},
+	[IPC_SM_ACT_TX_BOOTREQ] = {
+		.id     = IPC_SM_ACT_TX_BOOTREQ,
+		.enter  = sm_act_tx_bootreq_enter,
+		.exit   = sm_act_tx_bootreq_exit,
+		.events = IPC_SM_RUN_TFR_COMPLETE
+	},
+	[IPC_SM_SLW_RX_BOOTRESP] = {
+		.id     = IPC_SM_SLW_RX_BOOTRESP,
+		.enter  = sm_slw_rx_bootresp_enter,
+		.exit   = sm_slw_rx_bootresp_exit,
+		.events = IPC_SM_RUN_SLAVE_IRQ | IPC_SM_RUN_COMMS_TMO
+	},
+	[IPC_SM_ACT_RX_BOOTRESP] = {
+		.id     = IPC_SM_ACT_RX_BOOTRESP,
+		.enter  = sm_act_rx_bootresp_enter,
+		.exit   = sm_act_rx_bootresp_exit,
+		.events = IPC_SM_RUN_TFR_COMPLETE
+	},
+	[IPC_SM_IDL] = {
+		.id     = IPC_SM_IDL,
+		.enter  = sm_idl_enter,
+		.exit   = sm_idl_exit,
+		.events = IPC_SM_RUN_SLAVE_IRQ | IPC_SM_RUN_TX_REQ |
+				IPC_SM_RUN_RESET
+	},
+	[IPC_SM_SLW_TX_WR_CMD] = {
+		.id     = IPC_SM_SLW_TX_WR_CMD,
+		.enter  = sm_slw_tx_wr_cmd_enter,
+		.exit   = sm_slw_tx_wr_cmd_exit,
+		.events = IPC_SM_RUN_SLAVE_IRQ | IPC_SM_RUN_COMMS_TMO |
+				IPC_SM_RUN_RESET
+	},
+	[IPC_SM_ACT_TX_WR_CMD] = {
+		.id     = IPC_SM_ACT_TX_WR_CMD,
+		.enter  = sm_act_tx_wr_cmd_enter,
+		.exit   = sm_act_tx_wr_cmd_exit,
+		.events = IPC_SM_RUN_TFR_COMPLETE | IPC_SM_RUN_RESET
+	},
+	[IPC_SM_SLW_TX_WR_DAT] = {
+		.id     = IPC_SM_SLW_TX_WR_DAT,
+		.enter  = sm_slw_tx_wr_dat_enter,
+		.exit   = sm_slw_tx_wr_dat_exit,
+		.events = IPC_SM_RUN_SLAVE_IRQ | IPC_SM_RUN_COMMS_TMO |
+				IPC_SM_RUN_RESET
+	},
+	[IPC_SM_ACT_TX_WR_DAT] = {
+		.id     = IPC_SM_ACT_TX_WR_DAT,
+		.enter  = sm_act_tx_wr_dat_enter,
+		.exit   = sm_act_tx_wr_dat_exit,
+		.events = IPC_SM_RUN_TFR_COMPLETE | IPC_SM_RUN_RESET
+	},
+	[IPC_SM_SLW_TX_RD_CMD] = {
+		.id     = IPC_SM_SLW_TX_RD_CMD,
+		.enter  = sm_slw_tx_rd_cmd_enter,
+		.exit   = sm_slw_tx_rd_cmd_exit,
+		.events = IPC_SM_RUN_SLAVE_IRQ | IPC_SM_RUN_COMMS_TMO |
+				IPC_SM_RUN_RESET
+	},
+	[IPC_SM_ACT_TX_RD_CMD] = {
+		.id     = IPC_SM_ACT_TX_RD_CMD,
+		.enter  = sm_act_tx_rd_cmd_enter,
+		.exit   = sm_act_tx_rd_cmd_exit,
+		.events = IPC_SM_RUN_TFR_COMPLETE | IPC_SM_RUN_RESET
+	},
+	[IPC_SM_SLW_RX_WR_CMD] = {
+		.id     = IPC_SM_SLW_RX_WR_CMD,
+		.enter  = sm_slw_rx_wr_cmd_enter,
+		.exit   = sm_slw_rx_wr_cmd_exit,
+		.events = IPC_SM_RUN_SLAVE_IRQ | IPC_SM_RUN_COMMS_TMO |
+				IPC_SM_RUN_RESET
+	},
+	[IPC_SM_ACT_RX_WR_CMD] = {
+		.id     = IPC_SM_ACT_RX_WR_CMD,
+		.enter  = sm_act_rx_wr_cmd_enter,
+		.exit   = sm_act_rx_wr_cmd_exit,
+		.events = IPC_SM_RUN_TFR_COMPLETE | IPC_SM_RUN_RESET
+	},
+	[IPC_SM_ACT_RX_WR_DAT] = {
+		.id     = IPC_SM_ACT_RX_WR_DAT,
+		.enter  = sm_act_rx_wr_dat_enter,
+		.exit   = sm_act_rx_wr_dat_exit,
+		.events = IPC_SM_RUN_TFR_COMPLETE | IPC_SM_RUN_RESET
+	},
+};
+
+const struct ipc_sm_state *ipc_sm_idle_state(struct ipc_link_context *context)
+{
+	return ipc_sm_state(IPC_SM_IDL);
+}
+
+const struct ipc_sm_state *ipc_sm_init_state(struct ipc_link_context *context)
+{
+	return ipc_sm_state(IPC_SM_INIT);
+}
+
+const struct ipc_sm_state *ipc_sm_state(u8 id)
+{
+	BUG_ON(id >= IPC_SM_STATE_ID_NBR);
+	return &state_machine[id];
+}
+
+bool ipc_sm_valid_for_state(u8 event, const struct ipc_sm_state *state)
+{
+	return (state->events & event) == event;
+}
+
+static void state_machine_run(struct ipc_link_context *context, u8 event)
+{
+	struct modem_m6718_spi_link_platform_data *link = context->link;
+	struct spi_device *sdev = context->sdev;
+	const struct ipc_sm_state *cur_state = context->state;
+
+	/* some sanity checking */
+	if (context == NULL || link == NULL || cur_state == NULL) {
+		pr_err("M6718 IPC protocol error: "
+			"inconsistent driver state, ignoring event\n");
+		return;
+	}
+
+	dev_dbg(&sdev->dev, "link %d: RUNNING in %s (%s)\n", link->id,
+		ipc_dbg_state_id(cur_state), ipc_dbg_event(event));
+
+	/* valid trigger event for current state? */
+	if (!ipc_sm_valid_for_state(event, cur_state)) {
+		dev_dbg(&sdev->dev,
+			"link %d: ignoring invalid event\n", link->id);
+		ipc_dbg_ignoring_event(context, event);
+		return;
+	}
+	ipc_dbg_handling_event(context, event);
+
+	/* run machine while state entry functions trigger new changes */
+	do {
+		if (event == IPC_SM_RUN_SLAVE_IRQ &&
+			!ipc_util_int_is_active(context)) {
+			dev_err(&sdev->dev,
+				"link %d error: slave is not ready! (%s)",
+				link->id,
+				ipc_dbg_state_id(cur_state));
+		}
+
+		if (event == IPC_SM_RUN_ABORT) {
+			dev_err(&sdev->dev,
+				"link %d error: abort event\n", link->id);
+			/* reset state to idle */
+			context->state = ipc_sm_idle_state(context);
+			break;
+		} else {
+			/* exit current state */
+			dev_dbg(&sdev->dev, "link %d: exit %s (%s)\n",
+				link->id, ipc_dbg_state_id(cur_state),
+				ipc_dbg_event(event));
+			cur_state = cur_state->exit(event, context);
+			context->state = cur_state;
+		}
+
+		/* reset state of slave irq to prepare for next event */
+		if (event == IPC_SM_RUN_SLAVE_IRQ)
+			atomic_set(&context->state_int, 0);
+
+		/* enter new state */
+		dev_dbg(&sdev->dev, "link %d: enter %s (%s)\n", link->id,
+			ipc_dbg_state_id(cur_state), ipc_dbg_event(event));
+		event = context->state->enter(event, context);
+		ipc_dbg_entering_state(context);
+	} while (event != IPC_SM_RUN_NONE);
+
+	dev_dbg(&sdev->dev, "link %d: STOPPED in %s\n", link->id,
+		ipc_dbg_state_id(cur_state));
+}
+
+void ipc_sm_kick(u8 event, struct ipc_link_context *context)
+{
+	unsigned long flags;
+	struct modem_m6718_spi_link_platform_data *link = context->link;
+	struct spi_device *sdev = context->sdev;
+	struct spi_message *msg = &context->spi_message;
+	u8 i;
+
+	spin_lock_irqsave(&context->sm_lock, flags);
+	switch (event) {
+	case IPC_SM_RUN_SLAVE_IRQ:
+		dev_dbg(&sdev->dev,
+			"link %d EVENT: slave-ready irq\n", link->id);
+		del_timer(&context->comms_timer);
+		atomic_set(&context->state_int,
+			ipc_util_int_is_active(context));
+		break;
+
+	case IPC_SM_RUN_TFR_COMPLETE:
+		dev_dbg(&sdev->dev,
+			"link %d EVENT: spi tfr complete (status %d len %d)\n",
+			link->id, msg->status, msg->actual_length);
+		ipc_dbg_dump_spi_tfr(context);
+		break;
+
+	case IPC_SM_RUN_COMMS_TMO:
+	{
+		char *statestr;
+		struct ipc_link_context *contexts = l1_context.device_context;
+
+		statestr = ipc_dbg_link_state_str(context);
+		dev_err(&sdev->dev,
+			"link %d EVENT: modem comms timeout (%s)!\n",
+			link->id, ipc_dbg_state_id(context->state));
+		if (statestr != NULL) {
+			dev_err(&sdev->dev, "%s", statestr);
+			kfree(statestr);
+		}
+
+		/* cancel all link timeout timers except this one */
+		for (i = 0; i < IPC_NBR_SUPPORTED_SPI_LINKS; i++)
+			if (contexts[i].link->id != link->id)
+				del_timer(&contexts[i].comms_timer);
+		break;
+	}
+
+	case IPC_SM_RUN_STABLE_TMO:
+		dev_dbg(&sdev->dev,
+			"link %d EVENT: slave-stable timeout\n", link->id);
+		break;
+
+	case IPC_SM_RUN_RESET:
+		dev_dbg(&sdev->dev,
+			"link %d EVENT: reset\n", link->id);
+		del_timer(&context->comms_timer);
+		break;
+
+	default:
+		break;
+	}
+
+	if (!ipc_util_link_is_suspended(context))
+		state_machine_run(context, event);
+	else
+		dev_dbg(&sdev->dev,
+			"link %d is suspended, waiting for resume\n", link->id);
+	spin_unlock_irqrestore(&context->sm_lock, flags);
+}
diff --git a/drivers/modem/m6718_spi/util.c b/drivers/modem/m6718_spi/util.c
new file mode 100644
index 00000000000..9026f4427dd
--- /dev/null
+++ b/drivers/modem/m6718_spi/util.c
@@ -0,0 +1,281 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010,2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * U9500 <-> M6718 IPC protocol implementation using SPI:
+ *   utility functions.
+ */
+#include <linux/gpio.h>
+#include <linux/modem/m6718_spi/modem_driver.h>
+#include "modem_util.h"
+
+#define MODEM_COMMS_TMO_MS  (5000) /* 0 == no timeout */
+#define SLAVE_STABLE_TMO_MS (1000)
+
+#define DRIVER_NAME "ipcspi" /* name used when reserving gpio pins */
+
+
+bool ipc_util_channel_is_loopback(u8 channel)
+{
+	return channel == MODEM_M6718_SPI_CHN_MASTER_LOOPBACK0 ||
+		channel == MODEM_M6718_SPI_CHN_MASTER_LOOPBACK1;
+}
+
+u32 ipc_util_make_l2_header(u8 channel, u32 len)
+{
+	return ((channel & 0xf) << 28) | (len & 0x000fffff);
+}
+
+u8 ipc_util_get_l2_channel(u32 hdr)
+{
+	return hdr >> 28;
+}
+
+u32 ipc_util_get_l2_length(u32 hdr)
+{
+	return hdr & 0x000fffff;
+}
+
+u32 ipc_util_make_l1_header(u8 cmd, u8 counter, u32 len)
+{
+	return (cmd << 28) |
+		((counter & 0x000000ff) << 20) |
+		(len & 0x000fffff);
+}
+
+u8 ipc_util_get_l1_cmd(u32 hdr)
+{
+	return hdr >> 28;
+}
+
+u8 ipc_util_get_l1_counter(u32 hdr)
+{
+	return (hdr >> 20) & 0x000000ff;
+}
+
+u32 ipc_util_get_l1_length(u32 hdr)
+{
+	return hdr & 0x000fffff;
+}
+
+u8 ipc_util_get_l1_bootresp_ver(u32 bootresp)
+{
+	return bootresp & 0x000000ff;
+}
+
+int ipc_util_ss_level_active(struct ipc_link_context *context)
+{
+	return context->link->gpio.ss_active == 0 ? 0 : 1;
+}
+
+int ipc_util_ss_level_inactive(struct ipc_link_context *context)
+{
+	return !ipc_util_ss_level_active(context);
+}
+
+int ipc_util_int_level_active(struct ipc_link_context *context)
+{
+	return context->link->gpio.int_active == 0 ? 0 : 1;
+}
+
+int ipc_util_int_level_inactive(struct ipc_link_context *context)
+{
+	return !ipc_util_int_level_active(context);
+}
+
+void ipc_util_deactivate_ss(struct ipc_link_context *context)
+{
+	gpio_set_value(context->link->gpio.ss_pin,
+		ipc_util_ss_level_inactive(context));
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: deactivated SS\n", context->link->id);
+}
+
+void ipc_util_activate_ss(struct ipc_link_context *context)
+{
+	gpio_set_value(context->link->gpio.ss_pin,
+		ipc_util_ss_level_active(context));
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: activated SS\n", context->link->id);
+}
+
+void ipc_util_activate_ss_with_tmo(struct ipc_link_context *context)
+{
+	gpio_set_value(context->link->gpio.ss_pin,
+		ipc_util_ss_level_active(context));
+
+#if MODEM_COMMS_TMO_MS == 0
+	dev_dbg(&context->sdev->dev,
+		 "link %d: activated SS (timeout is disabled)\n",
+		 context->link->id);
+#else
+	context->comms_timer.expires = jiffies +
+		((MODEM_COMMS_TMO_MS * HZ) / 1000);
+	add_timer(&context->comms_timer);
+
+	dev_dbg(&context->sdev->dev,
+		"link %d: activated SS with timeout\n", context->link->id);
+#endif
+}
+
+bool ipc_util_int_is_active(struct ipc_link_context *context)
+{
+	return gpio_get_value(context->link->gpio.int_pin) ==
+		ipc_util_int_level_active(context);
+}
+
+bool ipc_util_link_is_idle(struct ipc_link_context *context)
+{
+	if (context->state == NULL)
+		return false;
+
+	switch (context->state->id) {
+	case IPC_SM_IDL:
+		return true;
+	default:
+		return false;
+	}
+}
+
+void ipc_util_start_slave_stable_timer(struct ipc_link_context *context)
+{
+	context->slave_stable_timer.expires =
+		jiffies + ((SLAVE_STABLE_TMO_MS * HZ) / 1000);
+	add_timer(&context->slave_stable_timer);
+}
+
+void ipc_util_spi_message_prepare(struct ipc_link_context *link_context,
+	void *tx_buf, void *rx_buf, int len)
+{
+	struct spi_transfer *tfr = &link_context->spi_transfer;
+	struct spi_message *msg = &link_context->spi_message;
+
+	tfr->tx_buf = tx_buf;
+	tfr->rx_buf = rx_buf;
+	tfr->len    = len;
+	msg->context = link_context;
+}
+
+void ipc_util_spi_message_init(struct ipc_link_context *link_context,
+	void (*complete)(void *))
+{
+	struct spi_message *msg = &link_context->spi_message;
+	struct spi_transfer *tfr = &link_context->spi_transfer;
+
+	tfr->bits_per_word = 16;
+
+	/* common init of transfer - use default from board device */
+	tfr->cs_change = 0;
+	tfr->speed_hz = 0;
+	tfr->delay_usecs = 0;
+
+	/* common init of message */
+	spi_message_init(msg);
+	msg->spi = link_context->sdev;
+	msg->complete = complete;
+	spi_message_add_tail(tfr, msg);
+}
+
+bool ipc_util_link_gpio_request(struct ipc_link_context *context,
+	irqreturn_t (*irqhnd)(int, void*))
+{
+	struct spi_device *sdev = context->sdev;
+	struct modem_m6718_spi_link_platform_data *link = context->link;
+	unsigned long irqflags;
+
+	if (gpio_request(link->gpio.ss_pin, DRIVER_NAME) < 0) {
+		dev_err(&sdev->dev,
+			"link %d error: failed to get gpio %d for SS pin\n",
+			link->id,
+			link->gpio.ss_pin);
+		return false;
+	}
+	if (gpio_request(link->gpio.int_pin, DRIVER_NAME) < 0) {
+		dev_err(&sdev->dev,
+			"link %d error: failed to get gpio %d for INT pin\n",
+			link->id,
+			link->gpio.int_pin);
+		return false;
+	}
+
+	if (ipc_util_int_level_active(context) == 1)
+		irqflags = IRQF_TRIGGER_RISING;
+	else
+		irqflags = IRQF_TRIGGER_FALLING;
+
+	if (request_irq(GPIO_TO_IRQ(link->gpio.int_pin),
+			irqhnd,
+			irqflags,
+			DRIVER_NAME,
+			context) < 0) {
+		dev_err(&sdev->dev,
+			"link %d error: could not get irq %d\n",
+			link->id, GPIO_TO_IRQ(link->gpio.int_pin));
+		return false;
+	}
+	return true;
+}
+
+bool ipc_util_link_gpio_config(struct ipc_link_context *context)
+{
+	struct spi_device *sdev = context->sdev;
+	struct modem_m6718_spi_link_platform_data *link = context->link;
+
+	if (atomic_read(&context->gpio_configured) == 1)
+		return true;
+
+	dev_dbg(&sdev->dev, "link %d: configuring GPIO\n", link->id);
+
+	ipc_util_deactivate_ss(context);
+	gpio_direction_input(link->gpio.int_pin);
+	if (enable_irq_wake(GPIO_TO_IRQ(link->gpio.int_pin)) < 0) {
+		dev_err(&sdev->dev,
+			"link %d error: failed to enable wake on INT\n",
+			link->id);
+		return false;
+	}
+
+	atomic_set(&context->state_int, gpio_get_value(link->gpio.int_pin));
+	atomic_set(&context->gpio_configured, 1);
+	return true;
+}
+
+bool ipc_util_link_gpio_unconfig(struct ipc_link_context *context)
+{
+	struct spi_device *sdev = context->sdev;
+	struct modem_m6718_spi_link_platform_data *link = context->link;
+
+	if (atomic_read(&context->gpio_configured) == 0)
+		return true;
+
+	dev_dbg(&sdev->dev, "link %d: un-configuring GPIO\n", link->id);
+
+	/* SS: output anyway, just make sure it is low */
+	gpio_set_value(link->gpio.ss_pin, 0);
+
+	/* INT: disable system-wake, reconfigure as output-low */
+	disable_irq_wake(GPIO_TO_IRQ(link->gpio.int_pin));
+	gpio_direction_output(link->gpio.int_pin, 0);
+	atomic_set(&context->gpio_configured, 0);
+	return true;
+}
+
+bool ipc_util_link_is_suspended(struct ipc_link_context *context)
+{
+	return atomic_read(&context->suspended) == 1;
+}
+
+void ipc_util_suspend_link(struct ipc_link_context *context)
+{
+	atomic_set(&context->suspended, 1);
+}
+
+void ipc_util_resume_link(struct ipc_link_context *context)
+{
+	atomic_set(&context->suspended, 0);
+}
diff --git a/drivers/modem/mcdd.c b/drivers/modem/mcdd.c
new file mode 100644
index 00000000000..d291944e810
--- /dev/null
+++ b/drivers/modem/mcdd.c
@@ -0,0 +1,190 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Modem Crash Detection Driver
+ *
+ * Author:Bibek Basu <bibek.basu@stericsson.com> for ST-Ericsson
+ *
+ * License terms:GNU General Public License (GPLv2)version 2
+ */
+
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/miscdevice.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/sched.h>
+#include <linux/poll.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+
+#define MCDD_INTERRUPT_CLEAR (1 << 13)
+#define MODEM_CRASH_EVT  1
+
+struct mcdd_data {
+	bool	modem_event;
+	u32   event_type;
+	wait_queue_head_t  readq;
+	spinlock_t	lock;
+	void __iomem *remap_intcon;
+	struct device *dev;
+	struct miscdevice misc_dev;
+};
+
+static struct mcdd_data *mcdd;
+
+static irqreturn_t mcdd_interrupt_cb(int irq, void *dev)
+{
+	writel(MCDD_INTERRUPT_CLEAR, (u32 *)mcdd->remap_intcon);
+	spin_lock(&mcdd->lock);
+	mcdd->modem_event = true;
+	mcdd->event_type = MODEM_CRASH_EVT;
+	spin_unlock(&mcdd->lock);
+	wake_up_interruptible(&mcdd->readq);
+	return IRQ_HANDLED;
+}
+
+static unsigned int mcdd_select(struct file *filp, poll_table *wait)
+{
+	unsigned int mask = 0;
+	unsigned long flags;
+
+	poll_wait(filp, &mcdd->readq,  wait);
+	spin_lock_irqsave(&mcdd->lock, flags);
+
+	if (mcdd->modem_event == true) {
+		mask |= POLLPRI;
+		mcdd->modem_event = false;
+	}
+	spin_unlock_irqrestore(&mcdd->lock, flags);
+
+	return mask;
+}
+
+static int mcdd_open(struct inode *ino, struct file *filp)
+{
+	/* Do nothing */
+	return 0;
+}
+
+ssize_t mcdd_read(struct file *filp, char __user *buff, size_t size, loff_t *t)
+{
+	if (copy_to_user(buff, &mcdd->event_type, size))
+		return -EFAULT;
+	return 0;
+};
+
+static const struct file_operations mcdd_fops = {
+	.open    = mcdd_open,
+	.poll    = mcdd_select,
+	.read    = mcdd_read,
+	.owner   = THIS_MODULE,
+};
+
+static int __devinit u5500_mcdd_probe(struct platform_device *pdev)
+{
+	struct resource *resource;
+	int ret = 0;
+	int irq;
+
+	mcdd = kzalloc(sizeof(*mcdd), GFP_KERNEL);
+	if (!mcdd) {
+		dev_err(&pdev->dev, "Memory Allocation Failed");
+		return -ENOMEM;
+	}
+	mcdd->dev = &pdev->dev;
+	mcdd->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	mcdd->misc_dev.name = "mcdd";
+	mcdd->misc_dev.fops = &mcdd_fops;
+	spin_lock_init(&mcdd->lock);
+	init_waitqueue_head(&(mcdd->readq));
+
+	/* Get addr for mcdd crash interrupt reset register and ioremap it */
+	resource = platform_get_resource_byname(pdev,
+						IORESOURCE_MEM,
+						"mcdd_intreset_addr");
+	if (resource == NULL) {
+		dev_err(&pdev->dev,
+			"Unable to retrieve mcdd_intreset_addr resource\n");
+		goto exit_free;
+	}
+	mcdd->remap_intcon = ioremap(resource->start, resource_size(resource));
+	if (!mcdd->remap_intcon) {
+		dev_err(&pdev->dev, "Unable to ioremap intcon mbox1\n");
+		ret = -EINVAL;
+		goto exit_free;
+	}
+
+	/* Get IRQ for mcdd mbox interrupt and allocate it */
+	irq = platform_get_irq_byname(pdev, "mcdd_mbox_irq");
+	if (irq < 0) {
+		dev_err(&pdev->dev,
+			"Unable to retrieve mcdd mbox irq resource\n");
+		goto exit_unmap;
+	}
+
+	ret = request_threaded_irq(irq, NULL,
+			mcdd_interrupt_cb, IRQF_NO_SUSPEND | IRQF_ONESHOT,
+			"mcdd", &mcdd);
+	if (ret < 0) {
+		dev_err(&pdev->dev,
+				"Could not allocate irq %d,error %d\n",
+				 irq, ret);
+		goto exit_unmap;
+	}
+
+	ret = misc_register(&mcdd->misc_dev);
+	if (ret) {
+		dev_err(&pdev->dev, "can't misc-register\n");
+		goto exit_unmap;
+	}
+	dev_info(&pdev->dev, "mcdd driver registration done\n");
+	return 0;
+
+exit_unmap:
+	iounmap(mcdd->remap_intcon);
+exit_free:
+	kfree(mcdd);
+	return ret;
+}
+
+static int u5500_mcdd_remove(struct platform_device *pdev)
+{
+	int ret = 0;
+
+	if (mcdd) {
+		iounmap(mcdd->remap_intcon);
+		ret = misc_deregister(&mcdd->misc_dev);
+		kfree(mcdd);
+	}
+	return ret;
+}
+
+static struct platform_driver u5500_mcdd_driver = {
+	.driver = {
+		.name = "u5500-mcdd-modem",
+		.owner = THIS_MODULE,
+	},
+	.probe = u5500_mcdd_probe,
+	.remove = __devexit_p(u5500_mcdd_remove),
+};
+
+static int __init mcdd_init(void)
+{
+	return platform_driver_register(&u5500_mcdd_driver);
+}
+module_init(mcdd_init);
+
+static void __exit mcdd_exit(void)
+{
+	platform_driver_unregister(&u5500_mcdd_driver);
+}
+module_exit(mcdd_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("BIBEK BASU <bibek.basu@stericsson.com>");
+MODULE_DESCRIPTION("Modem Dump Detection Driver");
+MODULE_ALIAS("mcdd driver");
diff --git a/drivers/modem/modem_access.c b/drivers/modem/modem_access.c
new file mode 100644
index 00000000000..2bd32957ae2
--- /dev/null
+++ b/drivers/modem/modem_access.c
@@ -0,0 +1,417 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com>
+ *
+ * Heavily adapted from Regulator framework.
+ * Provides mechanisms for registering platform specific access
+ * mechanisms for modem.
+ * Also, exposes APIs for gettng/releasing the access and even
+ * query the access status, and the modem usage status.
+ */
+#include <linux/module.h>
+#include <linux/modem/modem.h>
+#include <linux/modem/modem_client.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+
+static DEFINE_MUTEX(modem_list_mutex);
+static LIST_HEAD(modem_list);
+
+struct modem {
+	struct device *dev;
+	struct list_head list;
+	char *modem_name;
+	struct device_attribute dev_attr;
+	struct modem_dev *mdev;
+	atomic_t use;
+};
+
+static const char *mdev_get_name(struct modem_dev *mdev)
+{
+	if (mdev->desc->name)
+		return mdev->desc->name;
+	else
+		return "";
+}
+
+static int _modem_is_requested(struct modem_dev *mdev)
+{
+	/* If we don't know then assume that the modem is always on */
+	if (!mdev->desc->ops->is_requested)
+		return 0;
+
+	return mdev->desc->ops->is_requested(mdev);
+}
+
+/**
+ * modem_is_requested - check if modem access is requested
+ * @modem: modem device
+ *
+ * Checks whether modem is accessed or not by querying
+ * the underlying platform specific modem access
+ * implementation.
+ */
+int modem_is_requested(struct modem *modem)
+{
+	int ret;
+
+	mutex_lock(&modem->mdev->mutex);
+	ret = _modem_is_requested(modem->mdev);
+	mutex_unlock(&modem->mdev->mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL(modem_is_requested);
+
+static int _modem_request(struct modem_dev *mdev)
+{
+	int ret;
+
+	if (++mdev->use_count == 1) {
+		ret = _modem_is_requested(mdev);
+		if (ret == 0)
+			mdev->desc->ops->request(mdev);
+	}
+
+	return 0;
+}
+
+/**
+ * modem_request - Request access the modem
+ * @modem: modem device
+ *
+ * API to access the modem. It keeps a client
+ * specific check on whether the particular modem
+ * requested is accessed or not.
+ */
+void modem_request(struct modem *modem)
+{
+	struct modem_dev *mdev = modem->mdev;
+	int ret = 0;
+
+
+	mutex_lock(&mdev->mutex);
+	if (atomic_read(&modem->use) == 1) {
+		mutex_unlock(&mdev->mutex);
+		return;
+	}
+	ret = _modem_request(mdev);
+	if (ret == 0)
+		atomic_set(&modem->use, 1);
+	mutex_unlock(&mdev->mutex);
+}
+EXPORT_SYMBOL(modem_request);
+
+static int _modem_release(struct modem_dev *mdev)
+{
+	if (WARN(mdev->use_count <= 0,
+				"unbalanced releases for %s\n",
+				mdev_get_name(mdev)))
+		return -EIO;
+
+	if (--mdev->use_count == 0)
+		mdev->desc->ops->release(mdev);
+
+	return 0;
+}
+
+/**
+ * modem_release - Release access to modem
+ * @modem: modem device
+ *
+ * Releases accesss to the modem. It keeps a client
+ * specific check on whether a particular modem
+ * is released or not.
+ */
+void modem_release(struct modem *modem)
+{
+	struct modem_dev *mdev = modem->mdev;
+	int ret = 0;
+
+	mutex_lock(&mdev->mutex);
+	if (atomic_read(&modem->use) == 0) {
+		mutex_unlock(&mdev->mutex);
+		return;
+	}
+	ret = _modem_release(mdev);
+	if (ret == 0)
+		atomic_set(&modem->use, 0);
+	mutex_unlock(&mdev->mutex);
+}
+EXPORT_SYMBOL(modem_release);
+
+/**
+ * modem_get_usage - Check if particular client is using modem
+ * @modem: modem device
+ *
+ * Checks whether the particular client is using access to modem.
+ * This API could be used by client drivers in making their
+ * suspend decisions.
+ */
+int modem_get_usage(struct modem *modem)
+{
+	return atomic_read(&modem->use);
+}
+EXPORT_SYMBOL(modem_get_usage);
+
+static struct modem *create_modem(struct modem_dev *mdev,
+		struct device *dev,
+		const char *id)
+{
+	struct modem *modem;
+
+	modem = kzalloc(sizeof(*modem), GFP_KERNEL);
+	if (modem == NULL)
+		return NULL;
+
+	mutex_lock(&mdev->mutex);
+	modem->mdev = mdev;
+	modem->dev = dev;
+	list_add(&modem->list, &mdev->client_list);
+
+	mutex_unlock(&mdev->mutex);
+	return modem;
+
+}
+
+static struct modem *_modem_get(struct device *dev, const char *id,
+		int exclusive)
+{
+	struct modem_dev *mdev_ptr;
+	struct modem *modem = ERR_PTR(-ENODEV);
+	int ret;
+
+	if (id == NULL) {
+		pr_err("modem_get with no identifier\n");
+		return modem;
+	}
+
+	mutex_lock(&modem_list_mutex);
+	list_for_each_entry(mdev_ptr, &modem_list, modem_list) {
+		if (strcmp(mdev_get_name(mdev_ptr), id) == 0)
+			goto found;
+	}
+
+	goto out;
+
+found:
+	if (!try_module_get(mdev_ptr->owner))
+		goto out;
+
+	modem = create_modem(mdev_ptr, dev, id);
+	if (modem == NULL) {
+		modem = ERR_PTR(-ENOMEM);
+		module_put(mdev_ptr->owner);
+	}
+
+	mdev_ptr->open_count++;
+	ret = _modem_is_requested(mdev_ptr);
+	if (ret)
+		mdev_ptr->use_count = 1;
+	else
+		mdev_ptr->use_count = 0;
+
+out:
+	mutex_unlock(&modem_list_mutex);
+	return modem;
+
+}
+
+/**
+ * modem_get - Get reference to a particular platform specific modem
+ * @dev: device
+ * @id: modem device name
+ *
+ * Get reference to a particular modem device.
+ */
+struct modem *modem_get(struct device *dev, const char *id)
+{
+	return _modem_get(dev, id, 0);
+}
+EXPORT_SYMBOL(modem_get);
+
+/**
+ * modem_put - Release reference to a modem device
+ * @modem: modem device
+ *
+ * Release reference to a modem device.
+ */
+void modem_put(struct modem *modem)
+{
+	struct modem_dev *mdev;
+
+	if (modem == NULL || IS_ERR(modem))
+		return;
+
+	mutex_lock(&modem_list_mutex);
+	mdev = modem->mdev;
+
+	list_del(&modem->list);
+	kfree(modem);
+
+	mdev->open_count--;
+
+	module_put(mdev->owner);
+	mutex_unlock(&modem_list_mutex);
+}
+EXPORT_SYMBOL(modem_put);
+
+static ssize_t modem_print_state(char *buf, int state)
+{
+	if (state > 0)
+		return sprintf(buf, "accessed\n");
+	else if (state == 0)
+		return sprintf(buf, "released\n");
+	else
+		return sprintf(buf, "unknown\n");
+}
+
+static ssize_t modem_state_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct modem_dev *mdev = dev_get_drvdata(dev);
+	ssize_t ret;
+
+	mutex_lock(&mdev->mutex);
+	ret = modem_print_state(buf, _modem_is_requested(mdev));
+	mutex_unlock(&mdev->mutex);
+
+	return ret;
+}
+static DEVICE_ATTR(state, 0444, modem_state_show, NULL);
+
+static ssize_t modem_use_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct modem_dev *mdev = dev_get_drvdata(dev);
+	struct modem *mod;
+	size_t size = 0;
+
+	list_for_each_entry(mod, &mdev->client_list, list) {
+		if (mod->dev != NULL)
+			size += sprintf((buf + size), "%s (%d)\n",
+				dev_name(mod->dev), atomic_read(&mod->use));
+		else
+			size += sprintf((buf + size), "unknown (%d)\n",
+				atomic_read(&mod->use));
+	}
+	size += sprintf((buf + size), "\n");
+
+	return size;
+}
+static DEVICE_ATTR(use, 0444, modem_use_show, NULL);
+
+static ssize_t modem_name_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct modem_dev *mdev = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", mdev_get_name(mdev));
+}
+static DEVICE_ATTR(name, 0444, modem_name_show, NULL);
+
+static ssize_t modem_num_active_users_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct modem_dev *mdev = dev_get_drvdata(dev);
+	return sprintf(buf, "%d\n", mdev->use_count);
+}
+static DEVICE_ATTR(num_active_users, 0444, modem_num_active_users_show, NULL);
+
+static int add_modem_attributes(struct modem_dev *mdev)
+{
+	struct device      *dev = &mdev->dev;
+	struct modem_ops   *ops = mdev->desc->ops;
+	int                status = 0;
+
+	status = device_create_file(dev, &dev_attr_use);
+	if (status < 0)
+		return status;
+
+	status = device_create_file(dev, &dev_attr_name);
+	if (status < 0)
+		return status;
+
+	status = device_create_file(dev, &dev_attr_num_active_users);
+	if (status < 0)
+		return status;
+
+	if (ops->is_requested) {
+		status = device_create_file(dev, &dev_attr_state);
+		if (status < 0)
+			return status;
+	}
+
+	return 0;
+}
+
+/**
+ * modem_register - register a modem
+ * @modem_desc: - description for modem
+ * @dev:        - device
+ * @driver_data:- driver specific data
+ *
+ * Register a modem with the modem access framework, so that
+ * it could be used by client drivers for accessing the
+ * modem.
+ */
+struct modem_dev *modem_register(struct modem_desc *modem_desc,
+		struct device *dev,
+		void *driver_data)
+{
+	static atomic_t modem_no = ATOMIC_INIT(0);
+	struct modem_dev *mdev;
+	int ret;
+
+	if (modem_desc == NULL)
+		return ERR_PTR(-EINVAL);
+
+	if (modem_desc->name == NULL || modem_desc->ops == NULL)
+		return ERR_PTR(-EINVAL);
+
+	mdev = kzalloc(sizeof(struct modem_dev), GFP_KERNEL);
+	if (mdev == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_lock(&modem_list_mutex);
+
+	mutex_init(&mdev->mutex);
+	mdev->modem_data = driver_data;
+	mdev->owner = modem_desc->owner;
+	mdev->desc = modem_desc;
+	INIT_LIST_HEAD(&mdev->client_list);
+	INIT_LIST_HEAD(&mdev->modem_list);
+	BLOCKING_INIT_NOTIFIER_HEAD(&mdev->notifier);
+
+	/* mdev->dev.class = &modem_class;*/
+	mdev->dev.parent = dev;
+	dev_set_name(&mdev->dev, "modem.%d", atomic_inc_return(&modem_no) - 1);
+	ret = device_register(&mdev->dev);
+	if (ret != 0)
+		goto clean;
+
+	dev_set_drvdata(&mdev->dev, mdev);
+
+	ret = add_modem_attributes(mdev);
+	if (ret < 0)
+		goto backoff;
+
+	list_add(&mdev->modem_list, &modem_list);
+
+out:
+	mutex_unlock(&modem_list_mutex);
+	return mdev;
+
+backoff:
+	device_unregister(&mdev->dev);
+	mdev = ERR_PTR(ret);
+	goto out;
+
+clean:
+	kfree(mdev);
+	mdev = ERR_PTR(ret);
+	goto out;
+}
+EXPORT_SYMBOL(modem_register);
diff --git a/drivers/modem/modem_m6718.c b/drivers/modem/modem_m6718.c
new file mode 100644
index 00000000000..5e457c16003
--- /dev/null
+++ b/drivers/modem/modem_m6718.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Chris Blair <chris.blair@stericsson.com>
+ *   based on modem_u8500.c
+ *
+ * Platform driver implementing access mechanisms to the M6718 modem.
+ */
+#include <linux/modem/modem.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+
+static void modem_m6718_request(struct modem_dev *mdev)
+{
+	/* nothing to do - modem will wake when data is sent */
+}
+
+static void modem_m6718_release(struct modem_dev *mdev)
+{
+	/* nothing to do - modem does not need to be requested/released */
+}
+
+static int modem_m6718_is_requested(struct modem_dev *mdev)
+{
+	return 0;
+}
+
+static struct modem_ops modem_m6718_ops = {
+	.request = modem_m6718_request,
+	.release = modem_m6718_release,
+	.is_requested = modem_m6718_is_requested,
+};
+
+static struct modem_desc modem_m6718_desc = {
+	.name   = "m6718",
+	.id     = 0,
+	.ops    = &modem_m6718_ops,
+	.owner  = THIS_MODULE,
+};
+
+static int __devinit modem_m6718_probe(struct platform_device *pdev)
+{
+	struct modem_dev *mdev;
+	int err;
+
+	mdev = modem_register(&modem_m6718_desc, &pdev->dev,
+			NULL);
+	if (IS_ERR(mdev)) {
+		err = PTR_ERR(mdev);
+		dev_err(&pdev->dev, "failed to register %s: err %i\n",
+			modem_m6718_desc.name, err);
+	}
+
+	return 0;
+}
+
+static int __devexit modem_m6718_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver modem_m6718_driver = {
+	.driver = {
+		.name = "modem-m6718",
+		.owner = THIS_MODULE,
+	},
+	.probe = modem_m6718_probe,
+	.remove = __devexit_p(modem_m6718_remove),
+};
+
+static int __init modem_m6718_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&modem_m6718_driver);
+	if (ret < 0) {
+		printk(KERN_ERR "modem_m6718: platform driver reg failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void __exit modem_m6718_exit(void)
+{
+	platform_driver_unregister(&modem_m6718_driver);
+}
+
+module_init(modem_m6718_init);
+module_exit(modem_m6718_exit);
+
+MODULE_AUTHOR("Chris Blair <chris.blair@stericsson.com>");
+MODULE_DESCRIPTION("M6718 modem access driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/modem/modem_u8500.c b/drivers/modem/modem_u8500.c
new file mode 100644
index 00000000000..39951995e8e
--- /dev/null
+++ b/drivers/modem/modem_u8500.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com>
+ *
+ * Platform driver implementing access mechanisms to modem
+ * on U8500 which uses Shared Memroy as IPC between Application
+ * Processor and Modem processor.
+ */
+#include <linux/module.h>
+#include <linux/modem/modem.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/mfd/dbx500-prcmu.h>
+
+static void u8500_modem_request(struct modem_dev *mdev)
+{
+	prcmu_ac_wake_req();
+}
+
+static void u8500_modem_release(struct modem_dev *mdev)
+{
+	prcmu_ac_sleep_req();
+}
+
+static int u8500_modem_is_requested(struct modem_dev *mdev)
+{
+	return prcmu_is_ac_wake_requested();
+}
+
+static struct modem_ops u8500_modem_ops = {
+	.request = u8500_modem_request,
+	.release = u8500_modem_release,
+	.is_requested = u8500_modem_is_requested,
+};
+
+static struct modem_desc u8500_modem_desc = {
+	.name   = "u8500-shrm-modem",
+	.id     = 0,
+	.ops    = &u8500_modem_ops,
+	.owner  = THIS_MODULE,
+};
+
+
+static int __devinit u8500_modem_probe(struct platform_device *pdev)
+{
+	struct modem_dev *mdev;
+	int err;
+
+	mdev = modem_register(&u8500_modem_desc, &pdev->dev,
+			NULL);
+	if (IS_ERR(mdev)) {
+		err = PTR_ERR(mdev);
+		pr_err("failed to register %s: err %i\n",
+				u8500_modem_desc.name, err);
+	}
+
+	return 0;
+}
+
+static int __devexit u8500_modem_remove(struct platform_device *pdev)
+{
+
+	return 0;
+}
+
+static struct platform_driver u8500_modem_driver = {
+	.driver = {
+		.name = "u8500-modem",
+		.owner = THIS_MODULE,
+	},
+	.probe = u8500_modem_probe,
+	.remove = __devexit_p(u8500_modem_remove),
+};
+
+static int __init u8500_modem_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&u8500_modem_driver);
+	if (ret < 0) {
+		printk(KERN_ERR "u8500_modem: platform driver reg failed\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void __exit u8500_modem_exit(void)
+{
+	platform_driver_unregister(&u8500_modem_driver);
+}
+
+arch_initcall(u8500_modem_init);
diff --git a/drivers/modem/shrm/Kconfig b/drivers/modem/shrm/Kconfig
new file mode 100644
index 00000000000..465c8bb10a1
--- /dev/null
+++ b/drivers/modem/shrm/Kconfig
@@ -0,0 +1,43 @@
+#
+# SHM HW kernel configuration
+#
+config U8500_SHRM
+	bool "U8500 SHRM hardware driver"
+	depends on ARCH_U8500 && PHONET && MODEM_U8500
+	default Y
+	---help---
+	  If you say Y here, you will enable the STN8500 SHM hardware driver.
+
+	  If unsure, say N.
+choice
+	prompt "Modem Image Version"
+	depends on U8500_SHRM
+	default SHRM_V1_UPDATES_VERSION
+
+	config SHRM_V1_UPDATES_VERSION
+	depends on U8500_SHRM
+	bool "SHRM V1 UPDATES"
+	help
+	 Modem Images with V1 Updates
+
+endchoice
+
+config U8500_SHRM_LOOP_BACK
+	bool "U8500 SHRM loopback"
+	depends on U8500_SHRM
+	default n
+	---help---
+	  If you say Y here, you will enable the shm loopback
+
+	  If unsure, say N.
+
+config U8500_SHRM_MODEM_SILENT_RESET
+	bool "U8500 SHRM Modem Silent Reset"
+	depends on U8500_SHRM
+	default n
+	---help---
+	  If you say Y here, you will enable the modem silent reset feature
+
+	  If unsure, say N.
+
+
diff --git a/drivers/modem/shrm/Makefile b/drivers/modem/shrm/Makefile
new file mode 100644
index 00000000000..8115c24920b
--- /dev/null
+++ b/drivers/modem/shrm/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for SHRM drivers
+#
+
+ifdef CONFIG_PHONET
+u8500_shrm-objs := 	modem_shrm_driver.o shrm_fifo.o shrm_protocol.o
+else
+u8500_shrm-objs := 	shrm_driver.o shrm_fifo.o shrm_protocol.o
+endif
+
+obj-$(CONFIG_U8500_SHRM)	+= u8500_shrm.o
diff --git a/drivers/modem/shrm/modem_shrm_driver.c b/drivers/modem/shrm/modem_shrm_driver.c
new file mode 100644
index 00000000000..f46b86bd22e
--- /dev/null
+++ b/drivers/modem/shrm/modem_shrm_driver.c
@@ -0,0 +1,670 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/uaccess.h>
+#include <asm/atomic.h>
+#include <linux/io.h>
+#include <linux/skbuff.h>
+#ifdef CONFIG_HIGH_RES_TIMERS
+#include <linux/hrtimer.h>
+static struct hrtimer timer;
+#endif
+#include <linux/if_ether.h>
+#include <linux/netdevice.h>
+#include <linux/phonet.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_config.h>
+#include <linux/modem/shrm/shrm_net.h>
+#include <linux/modem/shrm/shrm.h>
+
+#include <mach/isa_ioctl.h>
+/* debug functionality */
+#define ISA_DEBUG 0
+
+#define PHONET_TASKLET
+#define MAX_RCV_LEN	2048
+
+static void do_phonet_rcv_tasklet(unsigned long unused);
+struct tasklet_struct phonet_rcv_tasklet;
+
+/**
+ * audio_receive() - Receive audio channel completion callback
+ * @shrm:	pointer to shrm device information structure
+ * @data:	message pointer
+ * @n_bytes:	message size
+ * @l2_header:	L2 header/device ID 2->audio, 5->audio_loopback
+ *
+ * This fucntion is called from the audio receive handler. Copies the audio
+ * message from the FIFO to the AUDIO queue. The message is later copied from
+ * this queue to the user buffer through the char or net interface read
+ * operation.
+ */
+static int audio_receive(struct shrm_dev *shrm, void *data,
+					u32 n_bytes, u8 l2_header)
+{
+	u32 size = 0;
+	int ret = 0;
+	int idx;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *audiodev;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	idx = shrm_get_cdev_index(l2_header);
+	if (idx < 0) {
+		dev_err(shrm->dev, "failed to get index\n");
+		return idx;
+	}
+	audiodev = &shrm->isa_context->isadev[idx];
+	q = &audiodev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	spin_unlock(&q->update_lock);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding a msg to message queue failed");
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * common_receive() - Receive common channel completion callback
+ * @shrm:	pointer to the shrm device information structure
+ * @data:	message pointer
+ * @n_bytes:	message size
+ * @l2_header:	L2 header / device ID
+ *
+ * This function is called from the receive handler to copy the respective
+ * ISI, RPC, SECURITY message to its respective queue. The message is then
+ * copied from queue to the user buffer on char net interface read operation.
+ */
+static int common_receive(struct shrm_dev *shrm, void *data,
+					u32 n_bytes, u8 l2_header)
+{
+	u32 size = 0;
+	int ret = 0;
+	int idx;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *isa_dev;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	idx = shrm_get_cdev_index(l2_header);
+	if (idx < 0) {
+		dev_err(shrm->dev, "failed to get index\n");
+		return idx;
+	}
+	isa_dev = &shrm->isa_context->isadev[idx];
+	q = &isa_dev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		dev_dbg(shrm->dev, "Inside Loop Back\n");
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	spin_unlock(&q->update_lock);
+	if (ret < 0) {
+		dev_err(shrm->dev, "Adding a msg to message queue failed");
+		return ret;
+	}
+
+
+	if (l2_header == ISI_MESSAGING) {
+		if (shrm->netdev_flag_up) {
+			dev_dbg(shrm->dev,
+				"scheduling the phonet tasklet from %s!\n",
+				__func__);
+			tasklet_schedule(&phonet_rcv_tasklet);
+		}
+		dev_dbg(shrm->dev,
+				"Out of phonet tasklet %s!!!\n", __func__);
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * rx_common_l2msg_handler() - common channel receive handler
+ * @l2_header:		L2 header
+ * @msg:		pointer to the receive buffer
+ * @length:		length of the msg to read
+ * @shrm:		pointer to shrm device information structure
+ *
+ * This function is called to receive the message from CaMsgPendingNotification
+ * interrupt handler.
+ */
+static void rx_common_l2msg_handler(u8 l2_header,
+				 void *msg, u32 length,
+				 struct shrm_dev *shrm)
+{
+	int ret = 0;
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	ret = common_receive(shrm, msg, length, l2_header);
+	if (ret < 0)
+		dev_err(shrm->dev,
+			"common receive with l2 header %d failed\n", l2_header);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+/**
+ * rx_audio_l2msg_handler() - audio channel receive handler
+ * @l2_header:		L2 header
+ * @msg:		pointer to the receive buffer
+ * @length:		length of the msg to read
+ * @shrm:		pointer to shrm device information structure
+ *
+ * This function is called to receive the message from CaMsgPendingNotification
+ * interrupt handler.
+ */
+static void rx_audio_l2msg_handler(u8 l2_header,
+				void *msg, u32 length,
+				struct shrm_dev *shrm)
+{
+	int ret = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	ret = audio_receive(shrm, msg, length, l2_header);
+	if (ret < 0)
+		dev_err(shrm->dev, "audio receive failed\n");
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static int __init shm_initialise_irq(struct shrm_dev *shrm)
+{
+	int err = 0;
+
+	err  = shrm_protocol_init(shrm,
+			rx_common_l2msg_handler, rx_audio_l2msg_handler);
+	if (err < 0) {
+		dev_err(shrm->dev, "SHM Protocol Init Failure\n");
+		return err;
+	}
+
+	err = request_irq(shrm->ca_wake_irq,
+			ca_wake_irq_handler, IRQF_TRIGGER_RISING,
+				 "ca_wake-up", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"Unable to allocate shm tx interrupt line\n");
+		free_irq(shrm->ca_wake_irq, shrm);
+		return err;
+	}
+
+	err = request_irq(shrm->ac_read_notif_0_irq,
+		ac_read_notif_0_irq_handler, 0,
+		"ac_read_notif_0", shrm);
+
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ac_read_notif_0_irq interrupt line\n");
+		goto irq_err1;
+	}
+
+	err = request_irq(shrm->ac_read_notif_1_irq,
+		ac_read_notif_1_irq_handler, 0,
+		"ac_read_notif_1", shrm);
+
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ac_read_notif_1_irq interrupt line\n");
+		goto irq_err2;
+	}
+
+	err = request_irq(shrm->ca_msg_pending_notif_0_irq,
+		 ca_msg_pending_notif_0_irq_handler, 0,
+		"ca_msg_pending_notif_0", shrm);
+
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ca_msg_pending_notif_0_irq line\n");
+		goto irq_err3;
+	}
+
+	err = request_irq(shrm->ca_msg_pending_notif_1_irq,
+		 ca_msg_pending_notif_1_irq_handler, 0,
+		"ca_msg_pending_notif_1", shrm);
+
+	if (err < 0) {
+		dev_err(shrm->dev,
+			"error ca_msg_pending_notif_1_irq interrupt line\n");
+		goto irq_err4;
+	}
+	return err;
+irq_err4:
+	free_irq(shrm->ca_msg_pending_notif_0_irq, shrm);
+irq_err3:
+	free_irq(shrm->ac_read_notif_1_irq, shrm);
+irq_err2:
+	free_irq(shrm->ac_read_notif_0_irq, shrm);
+irq_err1:
+	free_irq(shrm->ca_wake_irq, shrm);
+	return err;
+}
+
+static void free_shm_irq(struct shrm_dev *shrm)
+{
+	free_irq(shrm->ca_wake_irq, shrm);
+	free_irq(shrm->ac_read_notif_0_irq, shrm);
+	free_irq(shrm->ac_read_notif_1_irq, shrm);
+	free_irq(shrm->ca_msg_pending_notif_0_irq, shrm);
+	free_irq(shrm->ca_msg_pending_notif_1_irq, shrm);
+}
+
+
+
+#ifdef CONFIG_HIGH_RES_TIMERS
+static enum hrtimer_restart callback(struct hrtimer *timer)
+{
+	return HRTIMER_NORESTART;
+}
+#endif
+
+void do_phonet_rcv_tasklet(unsigned long unused)
+{
+	ssize_t ret;
+	struct shrm_dev *shrm = (struct shrm_dev *)unused;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	for (;;) {
+		ret = shrm_net_receive(shrm->ndev);
+		if (ret == 0) {
+			dev_dbg(shrm->dev, "len is zero, queue empty\n");
+			break;
+		}
+		if (ret < 0) {
+			dev_err(shrm->dev, "len < 0 !!! error!!!\n");
+			break;
+		}
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static int shrm_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct resource *res;
+	struct shrm_dev *shrm = NULL;
+
+	shrm = kzalloc(sizeof(struct shrm_dev), GFP_KERNEL);
+	if (shrm == NULL) {
+		dev_err(&pdev->dev,
+			"Could not allocate memory for struct shm_dev\n");
+		return -ENOMEM;
+	}
+
+	shrm->dev = &pdev->dev;
+	shrm->modem = modem_get(shrm->dev, "u8500-shrm-modem");
+	if (shrm->modem == NULL) {
+		dev_err(shrm->dev, " Could not retrieve the modem.\n");
+		err = -ENODEV;
+		goto rollback_intr;
+	}
+
+	/* initialise the SHM */
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(shrm->dev,
+				"Unable to map Ca Wake up interrupt\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_wake_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map APE_Read_notif_common IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ac_read_notif_0_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 2);
+
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map APE_Read_notif_audio IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ac_read_notif_1_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 3);
+
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map Cmt_msg_pending_notif_common IRQbase\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_msg_pending_notif_0_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 4);
+
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map Cmt_msg_pending_notif_audio IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_msg_pending_notif_1_irq = res->start;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	if (!res) {
+		dev_err(shrm->dev,
+				"Could not get SHM IO memory information\n");
+		err = -ENODEV;
+		goto rollback_intr;
+	}
+	shrm->intr_base = (void __iomem *)ioremap_nocache(res->start,
+					res->end - res->start + 1);
+	if (!(shrm->intr_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ape_common_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_APE_COMMON_BASE;
+	shrm->ape_common_fifo_base =
+		(void __iomem *)ioremap_nocache(
+					U8500_SHM_FIFO_APE_COMMON_BASE,
+					SHM_FIFO_0_SIZE);
+	shrm->ape_common_fifo_size = (SHM_FIFO_0_SIZE)/4;
+
+	if (!(shrm->ape_common_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ape_common_fifo_base;
+	}
+	shrm->cmt_common_fifo_base_phy =
+		(u32 *)U8500_SHM_FIFO_CMT_COMMON_BASE;
+	shrm->cmt_common_fifo_base =
+		(void __iomem *)ioremap_nocache(
+			U8500_SHM_FIFO_CMT_COMMON_BASE, SHM_FIFO_0_SIZE);
+	shrm->cmt_common_fifo_size = (SHM_FIFO_0_SIZE)/4;
+
+	if (!(shrm->cmt_common_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_cmt_common_fifo_base;
+	}
+	shrm->ape_audio_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_APE_AUDIO_BASE;
+	shrm->ape_audio_fifo_base =
+		(void __iomem *)ioremap_nocache(U8500_SHM_FIFO_APE_AUDIO_BASE,
+							SHM_FIFO_1_SIZE);
+	shrm->ape_audio_fifo_size = (SHM_FIFO_1_SIZE)/4;
+
+	if (!(shrm->ape_audio_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ape_audio_fifo_base;
+	}
+	shrm->cmt_audio_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_CMT_AUDIO_BASE;
+	shrm->cmt_audio_fifo_base =
+		(void __iomem *)ioremap_nocache(U8500_SHM_FIFO_CMT_AUDIO_BASE,
+							SHM_FIFO_1_SIZE);
+	shrm->cmt_audio_fifo_size = (SHM_FIFO_1_SIZE)/4;
+
+	if (!(shrm->cmt_audio_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_cmt_audio_fifo_base;
+	}
+	shrm->ac_common_shared_wptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_0_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_common_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ac_common_shared_wptr;
+	}
+	shrm->ac_common_shared_rptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_0_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_common_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ca_common_shared_wptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_0_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_common_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ca_common_shared_rptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_0_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_common_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ac_audio_shared_wptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_1_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_audio_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ac_audio_shared_rptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_1_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_audio_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ca_audio_shared_wptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_1_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_audio_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+	shrm->ca_audio_shared_rptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_1_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_audio_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+	if (isa_init(shrm) != 0) {
+		dev_err(shrm->dev, "Driver Initialization Error\n");
+		err = -EBUSY;
+	}
+	/* install handlers and tasklets */
+	if (shm_initialise_irq(shrm)) {
+		dev_err(shrm->dev,
+				"shm error in interrupt registration\n");
+		goto rollback_irq;
+	}
+#ifdef CONFIG_HIGH_RES_TIMERS
+	hrtimer_init(&timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	timer.function = callback;
+	hrtimer_start(&timer, ktime_set(0, 2*NSEC_PER_MSEC), HRTIMER_MODE_REL);
+#endif
+	err = shrm_register_netdev(shrm);
+	if (err < 0)
+		goto rollback_irq;
+
+	tasklet_init(&phonet_rcv_tasklet, do_phonet_rcv_tasklet, 0);
+	phonet_rcv_tasklet.data = (unsigned long)shrm;
+
+	platform_set_drvdata(pdev, shrm);
+
+	return err;
+rollback_irq:
+	free_shm_irq(shrm);
+rollback_map:
+	iounmap(shrm->ac_common_shared_wptr);
+	iounmap(shrm->ac_common_shared_rptr);
+	iounmap(shrm->ca_common_shared_wptr);
+	iounmap(shrm->ca_common_shared_rptr);
+	iounmap(shrm->ac_audio_shared_wptr);
+	iounmap(shrm->ac_audio_shared_rptr);
+	iounmap(shrm->ca_audio_shared_wptr);
+	iounmap(shrm->ca_audio_shared_rptr);
+rollback_ac_common_shared_wptr:
+	iounmap(shrm->cmt_audio_fifo_base);
+rollback_cmt_audio_fifo_base:
+	iounmap(shrm->ape_audio_fifo_base);
+rollback_ape_audio_fifo_base:
+	iounmap(shrm->cmt_common_fifo_base);
+rollback_cmt_common_fifo_base:
+	iounmap(shrm->ape_common_fifo_base);
+rollback_ape_common_fifo_base:
+	iounmap(shrm->intr_base);
+rollback_intr:
+	kfree(shrm);
+	return err;
+}
+
+static int __exit shrm_remove(struct platform_device *pdev)
+{
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+
+	free_shm_irq(shrm);
+	iounmap(shrm->intr_base);
+	iounmap(shrm->ape_common_fifo_base);
+	iounmap(shrm->cmt_common_fifo_base);
+	iounmap(shrm->ape_audio_fifo_base);
+	iounmap(shrm->cmt_audio_fifo_base);
+	iounmap(shrm->ac_common_shared_wptr);
+	iounmap(shrm->ac_common_shared_rptr);
+	iounmap(shrm->ca_common_shared_wptr);
+	iounmap(shrm->ca_common_shared_rptr);
+	iounmap(shrm->ac_audio_shared_wptr);
+	iounmap(shrm->ac_audio_shared_rptr);
+	iounmap(shrm->ca_audio_shared_wptr);
+	iounmap(shrm->ca_audio_shared_rptr);
+	shrm_unregister_netdev(shrm);
+	isa_exit(shrm);
+	kfree(shrm);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+/**
+ * u8500_shrm_suspend() - This routine puts the SHRM in to sustend state.
+ * @dev:	pointer to device structure.
+ *
+ * This routine checks the current ongoing communication with Modem by
+ * examining the ca_wake state and prevents suspend if modem communication
+ * is on-going.
+ * If ca_wake = 1 (high), modem comm. is on-going; don't suspend
+ * If ca_wake = 0 (low), no comm. with modem on-going.Allow suspend
+ */
+int u8500_shrm_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+	int err;
+
+	dev_dbg(&pdev->dev, "%s called...\n", __func__);
+	dev_dbg(&pdev->dev, "ca_wake_req_state = %x\n",
+						get_ca_wake_req_state());
+
+	/* if ca_wake_req is high, prevent system suspend */
+	if (!get_ca_wake_req_state()) {
+		err = shrm_suspend_netdev(shrm->ndev);
+		return err;
+	} else
+		return -EBUSY;
+}
+
+/**
+ * u8500_shrm_resume() - This routine resumes the SHRM from suspend state.
+ * @dev:	pointer to device structure
+ *
+ * This routine restore back the current state of the SHRM
+ */
+int u8500_shrm_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+	int err;
+
+	dev_dbg(&pdev->dev, "%s called...\n", __func__);
+	err = shrm_resume_netdev(shrm->ndev);
+
+	return err;
+}
+
+static const struct dev_pm_ops shrm_dev_pm_ops = {
+	.suspend_noirq = u8500_shrm_suspend,
+	.resume_noirq = u8500_shrm_resume,
+};
+#endif
+
+static struct platform_driver shrm_driver = {
+	.remove = __exit_p(shrm_remove),
+	.driver = {
+		.name = "u8500_shrm",
+		.owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm = &shrm_dev_pm_ops,
+#endif
+	},
+};
+
+static int __init shrm_driver_init(void)
+{
+	return platform_driver_probe(&shrm_driver, shrm_probe);
+}
+
+static void __exit shrm_driver_exit(void)
+{
+	platform_driver_unregister(&shrm_driver);
+}
+
+module_init(shrm_driver_init);
+module_exit(shrm_driver_exit);
+
+MODULE_AUTHOR("Biju Das, Kumar Sanghvi, Arun Murthy");
+MODULE_DESCRIPTION("Shared Memory Modem Driver Interface");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/modem/shrm/shrm_driver.c b/drivers/modem/shrm/shrm_driver.c
new file mode 100644
index 00000000000..11540831f95
--- /dev/null
+++ b/drivers/modem/shrm/shrm_driver.c
@@ -0,0 +1,1439 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#define DEBUG
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/smp_lock.h>
+#include <linux/poll.h>
+#include <linux/mutex.h>
+#include <linux/uaccess.h>
+#include <asm/atomic.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_config.h>
+#include <linux/modem/shrm/shrm.h>
+
+#include <mach/isa_ioctl.h>
+
+
+#ifdef CONFIG_HIGH_RES_TIMERS
+#include <linux/hrtimer.h>
+static struct hrtimer timer;
+#endif
+
+
+#define NAME "IPC_ISA"
+#define ISA_DEVICES 4
+/**debug functionality*/
+#define ISA_DEBUG 0
+
+#define ISI_MESSAGING (0)
+#define RPC_MESSAGING (1)
+#define AUDIO_MESSAGING (2)
+#define SECURITY_MESSAGING (3)
+
+#define SIZE_OF_FIFO (512*1024)
+
+static u8 message_fifo[4][SIZE_OF_FIFO];
+
+static u8 wr_isi_msg[10*1024];
+static u8 wr_rpc_msg[10*1024];
+static u8 wr_sec_msg[10*1024];
+static u8 wr_audio_msg[10*1024];
+
+/* global data */
+/*
+ * int major:This variable is exported to user as module_param to specify
+ * major number at load time
+ */
+static int major;
+module_param(major, int, 0);
+MODULE_PARM_DESC(major, "Major device number");
+/* global fops mutex */
+static DEFINE_MUTEX(isa_lock);
+rx_cb common_rx;
+rx_cb audio_rx;
+
+
+static int isi_receive(struct shrm_dev *shrm, void *data, u32 n_bytes);
+static int rpc_receive(struct shrm_dev *shrm, void *data, u32 n_bytes);
+static int audio_receive(struct shrm_dev *shrm, void *data, u32 n_bytes);
+static int security_receive(struct shrm_dev *shrm,
+						void *data, u32 n_bytes);
+
+static void rx_common_l2msg_handler(u8 l2_header,
+				 void *msg, u32 length,
+				 struct shrm_dev *shrm)
+{
+	int ret = 0;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+	u8 *pdata;
+#endif
+     dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	switch (l2_header) {
+	case ISI_MESSAGING:
+		ret = isi_receive(shrm, msg, length);
+		if (ret < 0)
+			dev_err(shrm->dev, "isi receive failed\n");
+		break;
+	case RPC_MESSAGING:
+		ret  = rpc_receive(shrm, msg, length);
+		if (ret < 0)
+			dev_err(shrm->dev, "rpc receive failed\n");
+		break;
+	case SECURITY_MESSAGING:
+		ret = security_receive(shrm, msg, length);
+		if (ret < 0)
+			dev_err(shrm->dev,
+					"security receive failed\n");
+		break;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+	case COMMMON_LOOPBACK_MESSAGING:
+		pdata = (u8 *)msg;
+		if ((*pdata == 0x50) || (*pdata == 0xAF)) {
+			ret = isi_receive(shrm, msg, length);
+			if (ret < 0)
+				dev_err(shrm->dev, "isi receive failed\n");
+		} else if ((*pdata == 0x0A) || (*pdata == 0xF5)) {
+			ret = rpc_receive(shrm, msg, length);
+			if (ret < 0)
+				dev_err(shrm->dev, "rpc receive failed\n");
+		} else if ((*pdata == 0xFF) || (*pdata == 0x00)) {
+			ret = security_receive(shrm, msg, length);
+			if (ret < 0)
+				dev_err(shrm->dev,
+						"security receive failed\n");
+		}
+		break;
+#endif
+	default:
+		break;
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static void rx_audio_l2msg_handler(u8 l2_header,
+				void *msg, u32 length,
+				struct shrm_dev *shrm)
+{
+	int ret = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	audio_receive(shrm, msg, length);
+	if (ret < 0)
+		dev_err(shrm->dev, "audio receive failed\n");
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static int __init shm_initialise_irq(struct shrm_dev *shrm)
+{
+	int err = 0;
+
+	shrm_protocol_init(shrm,
+			rx_common_l2msg_handler, rx_audio_l2msg_handler);
+
+	err = request_irq(shrm->ca_wake_irq,
+			ca_wake_irq_handler, IRQF_TRIGGER_RISING,
+				 "ca_wake-up", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"Unable to allocate shm tx interrupt line\n");
+		return err;
+	}
+
+	err = request_irq(shrm->ac_read_notif_0_irq,
+			ac_read_notif_0_irq_handler, 0,
+			"ac_read_notif_0", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ac_read_notif_0_irq interrupt line\n");
+		goto irq_err1;
+	}
+
+	err = request_irq(shrm->ac_read_notif_1_irq,
+			ac_read_notif_1_irq_handler, 0,
+			"ac_read_notif_1", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error ac_read_notif_1_irq interrupt line\n");
+		goto irq_err2;
+	}
+
+	err = request_irq(shrm->ca_msg_pending_notif_0_irq,
+			 ca_msg_pending_notif_0_irq_handler, 0,
+			"ca_msg_pending_notif_0", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+				"error  ca_msg_pending_notif_0_irq line\n");
+		goto irq_err3;
+	}
+
+	err = request_irq(shrm->ca_msg_pending_notif_1_irq,
+			 ca_msg_pending_notif_1_irq_handler, 0,
+			"ca_msg_pending_notif_1", shrm);
+	if (err < 0) {
+		dev_err(shrm->dev,
+			"error ca_msg_pending_notif_1_irq interrupt line\n");
+		goto irq_err4;
+	}
+
+	return err;
+
+irq_err4:
+	free_irq(shrm->ca_msg_pending_notif_0_irq, shrm);
+irq_err3:
+	free_irq(shrm->ac_read_notif_1_irq, shrm);
+irq_err2:
+	free_irq(shrm->ac_read_notif_0_irq, shrm);
+irq_err1:
+	free_irq(shrm->ca_wake_irq, shrm);
+	return err;
+}
+
+static void free_shm_irq(struct shrm_dev *shrm)
+{
+	free_irq(shrm->ca_wake_irq, shrm);
+	free_irq(shrm->ac_read_notif_0_irq, shrm);
+	free_irq(shrm->ac_read_notif_1_irq, shrm);
+	free_irq(shrm->ca_msg_pending_notif_0_irq, shrm);
+	free_irq(shrm->ca_msg_pending_notif_1_irq, shrm);
+}
+
+/**
+ * create_queue() - To create FIFO for Tx and Rx message buffering.
+ * @q: message queue.
+ * @devicetype: device type 0-isi,1-rpc,2-audio,3-security.
+ *
+ * This function creates a FIFO buffer of n_bytes size using
+ * dma_alloc_coherent(). It also initializes all queue handling
+ * locks, queue management pointers. It also initializes message list
+ * which occupies this queue.
+ *
+ * It return -ENOMEM in case of no memory.
+ */
+static int create_queue(struct message_queue *q, u32 devicetype,
+					struct shrm_dev *shrm)
+{
+	q->fifo_base = (u8 *)&message_fifo[devicetype];
+	q->size = SIZE_OF_FIFO;
+	q->readptr = 0;
+	q->writeptr = 0;
+	q->no = 0;
+	q->shrm = shrm;
+	spin_lock_init(&q->update_lock);
+	INIT_LIST_HEAD(&q->msg_list);
+	init_waitqueue_head(&q->wq_readable);
+	atomic_set(&q->q_rp, 0);
+
+	return 0;
+}
+/**
+ * delete_queue() - To delete FIFO and assiciated memory.
+ * @q: message queue
+ *
+ * This function deletes FIFO created using create_queue() function.
+ * It resets queue management pointers.
+ */
+static void delete_queue(struct message_queue *q)
+{
+	q->size = 0;
+	q->readptr = 0;
+	q->writeptr = 0;
+}
+
+/**
+ * add_msg_to_queue() - Add a message inside inside queue
+ *
+ * @q: message queue
+ * @size: size in bytes
+ *
+ * This function tries to allocate n_bytes of size in FIFO q.
+ * It returns negative number when no memory can be allocated
+ * currently.
+ */
+int add_msg_to_queue(struct message_queue *q, u32 size)
+{
+	struct queue_element *new_msg = NULL;
+	struct shrm_dev *shrm = q->shrm;
+
+	dev_dbg(shrm->dev, "%s IN q->writeptr=%d\n",
+						__func__, q->writeptr);
+	new_msg = kmalloc(sizeof(struct queue_element),
+						 GFP_KERNEL|GFP_ATOMIC);
+
+	if (new_msg == NULL) {
+		dev_err(shrm->dev, "memory overflow inside while(1)\n");
+		return -ENOMEM;
+	}
+	new_msg->offset = q->writeptr;
+	new_msg->size = size;
+	new_msg->no = q->no++;
+
+	/* check for overflow condition */
+	if (q->readptr <= q->writeptr) {
+		if (((q->writeptr-q->readptr) + size) >= q->size) {
+			dev_err(shrm->dev, "Buffer overflow !!\n");
+			BUG_ON(((q->writeptr-q->readptr) + size) >= q->size);
+		}
+	} else {
+		if ((q->writeptr + size) >= q->readptr) {
+			dev_err(shrm->dev, "Buffer overflow !!\n");
+			BUG_ON((q->writeptr + size) >= q->readptr);
+		}
+	}
+	q->writeptr = (q->writeptr + size) % q->size;
+	if (list_empty(&q->msg_list)) {
+		list_add_tail(&new_msg->entry, &q->msg_list);
+		/* There can be 2 blocking calls read  and another select */
+
+		atomic_set(&q->q_rp, 1);
+		wake_up_interruptible(&q->wq_readable);
+	} else
+		list_add_tail(&new_msg->entry, &q->msg_list);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+}
+
+/**
+ * remove_msg_from_queue() - To remove a message from the msg queue.
+ *
+ * @q: message queue
+ *
+ * This function delets a message from the message list associated with message
+ * queue q and also updates read ptr.
+ * If the message list is empty, then, event is set to block the select and
+ * read calls of the paricular queue.
+ *
+ * The message list is FIFO style and message is always added to tail and
+ * removed from head.
+ */
+
+int remove_msg_from_queue(struct message_queue *q)
+{
+	struct queue_element *old_msg = NULL;
+	struct shrm_dev *shrm = q->shrm;
+	struct list_head *msg;
+
+	dev_dbg(shrm->dev, "%s IN q->readptr %d\n",
+					__func__, q->readptr);
+
+	list_for_each(msg, &q->msg_list) {
+		old_msg = list_entry(msg, struct queue_element, entry);
+		if (old_msg == NULL) {
+			dev_err(shrm->dev, ":no message found\n");
+			return -EFAULT;
+		}
+		break;
+	}
+	list_del(msg);
+	q->readptr = (q->readptr + old_msg->size) % q->size;
+	if (list_empty(&q->msg_list)) {
+		dev_dbg(shrm->dev, "List is empty setting RP= 0\n");
+		atomic_set(&q->q_rp, 0);
+	}
+	kfree(old_msg);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+}
+
+/**
+ * get_size_of_new_msg() - retrieve new message from message list
+ *
+ * @q: message queue
+ *
+ * This function will retrieve most recent message from the corresponding
+ * queue list. New message is always retrieved from head side.
+ * It returns new message no, offset if FIFO and size.
+ */
+int get_size_of_new_msg(struct message_queue *q)
+{
+	struct queue_element *new_msg = NULL;
+	struct list_head *msg_list;
+	struct shrm_dev *shrm = q->shrm;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	spin_lock_bh(&q->update_lock);
+	list_for_each(msg_list, &q->msg_list) {
+		new_msg = list_entry(msg_list, struct queue_element, entry);
+		if (new_msg == NULL) {
+			spin_unlock_bh(&q->update_lock);
+			dev_err(shrm->dev, "no message found\n");
+			return -1;
+		}
+		break;
+	}
+	spin_unlock_bh(&q->update_lock);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return new_msg->size;
+}
+
+/**
+ * isi_receive() - Rx Completion callback
+ *
+ * @data:message pointer
+ * @n_bytes:message size
+ *
+ * This function is a callback to indicate ISI message reception is complete.
+ * It updates Writeptr of the Fifo
+ */
+static int isi_receive(struct shrm_dev *shrm,
+					void *data, u32 n_bytes)
+{
+	u32 size = 0;
+	int ret = 0;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *isidev = &shrm->isa_context->isadev[0];
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	q = &isidev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		dev_dbg(shrm->dev, "Inside Loop Back\n");
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding msg to message queue failed\n");
+	spin_unlock(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * rpc_receive() - Rx Completion callback
+ *
+ * @data:message pointer
+ * @n_bytes:message size
+ *
+ * This function is a callback to indicate RPC message reception is complete.
+ * It updates Writeptr of the Fifo
+ */
+static int rpc_receive(struct shrm_dev *shrm,
+					void *data, u32 n_bytes)
+{
+	u32 size = 0;
+	int ret = 0;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *rpcdev = &shrm->isa_context->isadev[1];
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	q = &rpcdev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+
+	ret = add_msg_to_queue(q, n_bytes);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding msg to message queue failed\n");
+	spin_unlock(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * audio_receive() - Rx Completion callback
+ *
+ * @data:message pointer
+ * @n_bytes:message size
+ *
+ * This function is a callback to indicate audio message reception is complete.
+ * It updates Writeptr of the Fifo
+ */
+static int audio_receive(struct shrm_dev *shrm,
+					void *data, u32 n_bytes)
+{
+	u32 size = 0;
+	int ret = 0;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *audiodev = &shrm->isa_context->isadev[2];
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	q = &audiodev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding msg to message queue failed\n");
+	spin_unlock(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * security_receive() - Rx Completion callback
+ *
+ * @data:message pointer
+ * @n_bytes: message size
+ *
+ * This function is a callback to indicate security message reception
+ * is complete.It updates Writeptr of the Fifo
+ */
+static int security_receive(struct shrm_dev *shrm,
+					void *data, u32 n_bytes)
+{
+	u32 size = 0;
+	int ret = 0;
+	u8 *psrc;
+	struct message_queue *q;
+	struct isadev_context *secdev = &shrm->isa_context->isadev[3];
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	q = &secdev->dl_queue;
+	spin_lock(&q->update_lock);
+	/* Memcopy RX data first */
+	if ((q->writeptr+n_bytes) >= q->size) {
+		psrc = (u8 *)data;
+		size = (q->size-q->writeptr);
+		/* Copy First Part of msg */
+		memcpy((q->fifo_base+q->writeptr), psrc, size);
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		memcpy(q->fifo_base, psrc, (n_bytes-size));
+	} else {
+		memcpy((q->fifo_base+q->writeptr), data, n_bytes);
+	}
+	ret = add_msg_to_queue(q, n_bytes);
+	if (ret < 0)
+		dev_err(shrm->dev, "Adding msg to message queue failed\n");
+	spin_unlock(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+
+/**
+ * isa_select() - Select Interface
+ *
+ * @filp:file descriptor pointer
+ * @wait:poll_table_struct pointer
+ *
+ * This function is used to perform non-blocking read operations. It allows
+ * a process to determine whether it can read from one or more open files
+ * without blocking. These calls can also block a process until any of a
+ * given set of file descriptors becomes available for reading.
+ * If a file is ready to read, POLLIN | POLLRDNORM bitmask is returned.
+ * The driver method is called whenever the user-space program performs a select
+ * system call involving a file descriptor associated with the driver.
+ */
+static u32 isa_select(struct file *filp,
+				struct poll_table_struct *wait)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	u32 mask = 0;
+	u32 m = iminor(filp->f_path.dentry->d_inode);
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (isadev->device_id != m)
+			return -1;
+	q = &isadev->dl_queue;
+	poll_wait(filp, &q->wq_readable, wait);
+	if (atomic_read(&q->q_rp) == 1)
+		mask = POLLIN | POLLRDNORM;
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return mask;
+}
+
+/**
+ * isa_read() - Read from device
+ *
+ * @filp:file descriptor
+ * @buf:user buffer pointer
+ * @len:size of requested data transfer
+ * @ppos:not used
+ *
+ * This function is called whenever user calls read() system call.
+ * It reads a oldest message from queue and copies it into user buffer and
+ * returns its size.
+ * If there is no message present in queue, then it blocks until new data is
+ * available.
+ */
+ssize_t isa_read(struct file *filp, char __user *buf,
+				size_t len, loff_t *ppos)
+{
+	struct isadev_context *isadev = (struct isadev_context *)
+							filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	char *psrc;
+	u32 msgsize;
+	u32 size = 0;
+	int ret = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (len <= 0)
+		return -EFAULT;
+	q = &isadev->dl_queue;
+
+	spin_lock_bh(&q->update_lock);
+	if (list_empty(&q->msg_list)) {
+		spin_unlock_bh(&q->update_lock);
+		if (wait_event_interruptible(q->wq_readable,
+				atomic_read(&q->q_rp) == 1)) {
+			return -ERESTARTSYS;
+		}
+	} else
+		spin_unlock_bh(&q->update_lock);
+
+	msgsize = get_size_of_new_msg(q);
+	if ((q->readptr+msgsize) >= q->size) {
+		dev_dbg(shrm->dev, "Inside Loop Back\n");
+		psrc = (char *)buf;
+		size = (q->size-q->readptr);
+		/* Copy First Part of msg */
+		if (copy_to_user(psrc,
+				(u8 *)(q->fifo_base+q->readptr),
+				size)) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+		psrc += size;
+		/* Copy Second Part of msg at the top of fifo */
+		if (copy_to_user(psrc,
+				(u8 *)(q->fifo_base),
+				(msgsize-size))) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+	} else {
+		if (copy_to_user(buf,
+				(u8 *)(q->fifo_base+q->readptr),
+				msgsize)) {
+			dev_err(shrm->dev, "copy_to_user failed\n");
+			return -EFAULT;
+		}
+	}
+
+	spin_lock_bh(&q->update_lock);
+	ret = remove_msg_from_queue(q);
+	if (ret < 0) {
+		dev_err(shrm->dev,
+				"Removing msg from message queue failed\n");
+		msgsize = ret;
+	}
+	spin_unlock_bh(&q->update_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return msgsize;
+}
+/**
+ * isa_write() - Write to device
+ *
+ * @filp:file descriptor
+ * @buf:user buffer pointer
+ * @len:size of requested data transfer
+ * @ppos:not used
+ *
+ * This function is called whenever user calls write() system call.
+ * It checks if there is space available in queue, and copies the message
+ * inside queue. If there is no space, it blocks until space becomes available.
+ * It also schedules transfer thread to transmit the newly added message.
+ */
+static ssize_t isa_write(struct file *filp, const char __user *buf,
+				 size_t len, loff_t *ppos)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct message_queue *q;
+	int err, ret;
+	void *addr = 0;
+	u8 l2_header = 0;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	if (len <= 0)
+		return -EFAULT;
+	q = &isadev->dl_queue;
+
+	switch (isadev->device_id) {
+	case ISI_MESSAGING:
+		dev_dbg(shrm->dev, "ISI\n");
+		addr = (void *)wr_isi_msg;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+		dev_dbg(shrm->dev, "Loopback\n");
+		l2_header = COMMON_LOOPBACK_MESSAGING;
+#else
+		l2_header = isadev->device_id;
+#endif
+		break;
+	case RPC_MESSAGING:
+		dev_dbg(shrm->dev, "RPC\n");
+		addr = (void *)wr_rpc_msg;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+		l2_header = COMMON_LOOPBACK_MESSAGING;
+#else
+		l2_header = isadev->device_id;
+#endif
+		break;
+	case AUDIO_MESSAGING:
+		dev_dbg(shrm->dev, "Audio\n");
+		addr = (void *)wr_audio_msg;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+		l2_header = AUDIO_LOOPBACK_MESSAGING;
+#else
+		l2_header = isadev->device_id;
+#endif
+
+		break;
+	case SECURITY_MESSAGING:
+		dev_dbg(shrm->dev, "Security\n");
+		addr = (void *)wr_sec_msg;
+#ifdef CONFIG_U8500_SHRM_LOOP_BACK
+		l2_header = COMMON_LOOPBACK_MESSAGING;
+#else
+		l2_header = isadev->device_id;
+#endif
+		break;
+	default:
+		dev_dbg(shrm->dev, "Wrong device\n");
+		return -EFAULT;
+	}
+
+	if (copy_from_user(addr, buf, len)) {
+		dev_err(shrm->dev, "copy_from_user failed\n");
+		return -EFAULT;
+	}
+
+	/* Write msg to Fifo */
+	if (isadev->device_id == 2) {
+		mutex_lock(&shrm->isa_context->tx_audio_mutex);
+		err = shm_write_msg(shrm, l2_header, addr, len);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		mutex_unlock(&shrm->isa_context->tx_audio_mutex);
+	} else {
+		spin_lock_bh(&shrm->isa_context->common_tx);
+		err = shm_write_msg(shrm, l2_header, addr, len);
+		if (!err)
+			ret = len;
+		else
+			ret = err;
+		spin_unlock_bh(&shrm->isa_context->common_tx);
+	}
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return ret;
+}
+
+/**
+ * isa_ioctl() - To handle different ioctl commands supported by driver.
+ *
+ * @inode: structure is used by the kernel internally to represent files
+ * @filp:file descriptor pointer
+ * @cmd:ioctl command
+ * @arg:input param
+ *
+ * Following ioctls are supported by this driver.
+ * DLP_IOCTL_ALLOCATE_BUFFER - To allocate buffer for new uplink message.
+ * This ioctl is called with required message size. It returns offset for
+ * the allocates space in the queue. DLP_IOCTL_PUT_MESSAGE -  To indicate
+ * new uplink message available in queuq for  transmission. Message is copied
+ * from offset location returned by previous ioctl before calling this ioctl.
+ * DLP_IOCTL_GET_MESSAGE - To check if any downlink message is available in
+ * queue. It returns  offset for new message inside queue.
+ * DLP_IOCTL_DEALLOCATE_BUFFER - To deallocate any buffer allocate for
+ * downlink message once the message is copied. Message is copied from offset
+ * location returned by previous ioctl before calling this ioctl.
+ */
+static int isa_ioctl(struct inode *inode, struct file *filp,
+				unsigned cmd, unsigned long arg)
+{
+	int err = 0;
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	u32 m = iminor(inode);
+
+	if (isadev->device_id != m)
+		return -1;
+
+	switch (cmd) {
+	case DLP_IOC_ALLOCATE_BUFFER:
+		dev_dbg(shrm->dev, "DLP_IOC_ALLOCATE_BUFFER\n");
+		break;
+	case DLP_IOC_PUT_MESSAGE:
+		dev_dbg(shrm->dev, "DLP_IOC_PUT_MESSAGE\n");
+		break;
+	case DLP_IOC_GET_MESSAGE:
+		dev_dbg(shrm->dev, "DLP_IOC_GET_MESSAGE\n");
+		break;
+	case DLP_IOC_DEALLOCATE_BUFFER:
+		dev_dbg(shrm->dev, "DLP_IOC_DEALLOCATE_BUFFER\n");
+		break;
+	default:
+		dev_dbg(shrm->dev, "Unknown IOCTL\n");
+		err = -1;
+		break;
+	}
+	return err;
+}
+/**
+ * isa_mmap() - Maps kernel queue memory to user space.
+ *
+ * @filp:file descriptor pointer
+ * @vma:virtual area memory structure.
+ *
+ * This function maps kernel FIFO into user space. This function
+ * shall be called twice to map both uplink and downlink buffers.
+ */
+static int isa_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+
+	u32	m = iminor(filp->f_path.dentry->d_inode);
+	dev_dbg(shrm->dev, "%s %dIN\n", __func__, m);
+
+	isadev = (struct isadev_context *)filp->private_data;
+	return 0;
+}
+
+/**
+ * isa_close() - Close device file
+ *
+ * @inode:structure is used by the kernel internally to represent files
+ * @filp:device file descriptor
+ *
+ * This function deletes structues associated with this file, deletes
+ * queues, flushes and destroys workqueus and closes this file.
+ * It also unregisters itself from l2mux driver.
+ */
+static int isa_close(struct inode *inode, struct file *filp)
+{
+	struct isadev_context *isadev = filp->private_data;
+	struct shrm_dev *shrm = isadev->dl_queue.shrm;
+	struct isa_driver_context *isa_context = shrm->isa_context;
+	u8 m;
+
+	mutex_lock(&isa_lock);
+	m = iminor(filp->f_path.dentry->d_inode);
+	dev_dbg(shrm->dev, "%s IN %d", __func__, m);
+
+	if (atomic_dec_and_test(&isa_context->is_open[m])) {
+		atomic_inc(&isa_context->is_open[m]);
+		dev_err(shrm->dev, "Device not opened yet\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	atomic_set(&isa_context->is_open[m], 1);
+
+	dev_dbg(shrm->dev, "isadev->device_id %d", isadev->device_id);
+	dev_dbg(shrm->dev, "Closed %d device\n", m);
+
+	if (m == ISI_MESSAGING)
+		dev_dbg(shrm->dev, "Closed ISI_MESSAGING Device\n");
+	else if (m == RPC_MESSAGING)
+		dev_dbg(shrm->dev, "Closed RPC_MESSAGING Device\n");
+	else if (m == AUDIO_MESSAGING)
+		dev_dbg(shrm->dev, "Closed AUDIO_MESSAGING Device\n");
+	else if (m == SECURITY_MESSAGING)
+		dev_dbg(shrm->dev, "Closed SECURITY_MESSAGING Device\n");
+	else
+		dev_dbg(shrm->dev, NAME ":No such device present\n");
+
+	mutex_unlock(&isa_lock);
+	return 0;
+}
+/**
+ * isa_open() -  Open device file
+ *
+ * @inode: structure is used by the kernel internally to represent files
+ * @filp: device file descriptor
+ *
+ * This function performs initialization tasks needed to open SHM channel.
+ * Following tasks are performed.
+ * -return if device is already opened
+ * -create uplink FIFO
+ * -create downlink FIFO
+ * -init delayed workqueue thread
+ * -register to l2mux driver
+ */
+static int isa_open(struct inode *inode, struct file *filp)
+{
+	int err = 0;
+	u8 m;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context = container_of(
+			inode->i_cdev,
+			struct isa_driver_context,
+			cdev);
+	struct shrm_dev *shrm = isa_context->isadev->dl_queue.shrm;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (get_boot_state() != BOOT_DONE) {
+		dev_err(shrm->dev, "Boot is not done\n");
+		return -EBUSY;
+	}
+	mutex_lock(&isa_lock);
+	m = iminor(inode);
+
+	if ((m != ISI_MESSAGING) && (m != RPC_MESSAGING) &&
+		(m != AUDIO_MESSAGING) && (m != SECURITY_MESSAGING)) {
+		dev_err(shrm->dev, "No such device present\n");
+		mutex_unlock(&isa_lock);
+		return -ENODEV;
+	}
+	if (!atomic_dec_and_test(&isa_context->is_open[m])) {
+		atomic_inc(&isa_context->is_open[m]);
+		dev_err(shrm->dev, "Device already opened\n");
+		mutex_unlock(&isa_lock);
+		return -EBUSY;
+	}
+
+	if (m == ISI_MESSAGING)
+		dev_dbg(shrm->dev, "Open ISI_MESSAGING Device\n");
+	else if (m == RPC_MESSAGING)
+		dev_dbg(shrm->dev, "Open RPC_MESSAGING Device\n");
+	else if (m == AUDIO_MESSAGING)
+		dev_dbg(shrm->dev, "Open AUDIO_MESSAGING Device\n");
+	else if (m == SECURITY_MESSAGING)
+		dev_dbg(shrm->dev, "Open SECURITY_MESSAGING Device\n");
+	else
+		dev_dbg(shrm->dev, ":No such device present\n");
+
+	isadev = &isa_context->isadev[m];
+	if (filp != NULL)
+		filp->private_data = isadev;
+
+	mutex_unlock(&isa_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return err;
+}
+
+const struct file_operations isa_fops = {
+	.owner	 = THIS_MODULE,
+	.open	 = isa_open,
+	.release = isa_close,
+	.ioctl	 = isa_ioctl,
+	.mmap	 = isa_mmap,
+	.read	 = isa_read,
+	.write	 = isa_write,
+	.poll	 = isa_select,
+};
+
+/**
+ * isa_init() - module insertion function
+ *
+ * This function registers module as a character driver using
+ * register_chrdev_region() or alloc_chrdev_region. It adds this
+ * driver to system using cdev_add() call. Major number is dynamically
+ * allocated using alloc_chrdev_region() by default or left to user to specify
+ * it during load time. For this variable major is used as module_param
+ * Nodes to be created using
+ * mknod /dev/isi c $major 0
+ * mknod /dev/rpc c $major 1
+ * mknod /dev/audio c $major 2
+ * mknod /dev/sec c $major 3
+ */
+int isa_init(struct shrm_dev *shrm)
+{
+	dev_t	dev_id;
+	int	retval, no_dev;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context;
+
+	isa_context = kzalloc(sizeof(struct isa_driver_context),
+							GFP_KERNEL);
+	shrm->isa_context = isa_context;
+	if (isa_context == NULL) {
+		dev_err(shrm->dev, "Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+
+	if (major) {
+		dev_id = MKDEV(major, 0);
+		retval = register_chrdev_region(dev_id, ISA_DEVICES, NAME);
+	} else {
+		retval = alloc_chrdev_region(&dev_id, 0, ISA_DEVICES, NAME);
+		major = MAJOR(dev_id);
+	}
+
+	dev_dbg(shrm->dev, "major %d\n", major);
+
+	cdev_init(&isa_context->cdev, &isa_fops);
+	isa_context->cdev.owner = THIS_MODULE;
+	retval = cdev_add(&isa_context->cdev, dev_id, ISA_DEVICES);
+	if (retval) {
+		dev_err(shrm->dev, "Failed to add char device\n");
+		return retval;
+	}
+
+	for (no_dev = 0; no_dev < ISA_DEVICES; no_dev++)
+		atomic_set(&isa_context->is_open[no_dev], 1);
+
+	isa_context->isadev = kzalloc(sizeof
+					(struct isadev_context)*ISA_DEVICES,
+					GFP_KERNEL);
+	if (isa_context->isadev == NULL) {
+		dev_err(shrm->dev, "Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+	for (no_dev = 0; no_dev < ISA_DEVICES; no_dev++) {
+		isadev = &isa_context->isadev[no_dev];
+		isadev->device_id = no_dev;
+		retval = create_queue(&isadev->dl_queue,
+					isadev->device_id, shrm);
+		if (retval < 0) {
+			dev_err(shrm->dev, "create dl_queue failed\n");
+			delete_queue(&isadev->dl_queue);
+			kfree(isadev);
+			return retval;
+		}
+	}
+	mutex_init(&isa_context->tx_audio_mutex);
+	spin_lock_init(&isa_context->common_tx);
+
+	dev_err(shrm->dev, "SHRM char driver added\n");
+
+	return retval;
+}
+
+void isa_exit(struct shrm_dev *shrm)
+{
+	int	no_dev;
+	struct isadev_context *isadev;
+	struct isa_driver_context *isa_context = shrm->isa_context;
+	dev_t dev_id = MKDEV(major, 0);
+
+	for (no_dev = 0; no_dev < ISA_DEVICES; no_dev++) {
+		isadev = &isa_context->isadev[no_dev];
+		delete_queue(&isadev->dl_queue);
+		kfree(isadev);
+	}
+
+	cdev_del(&isa_context->cdev);
+	unregister_chrdev_region(dev_id, ISA_DEVICES);
+	kfree(isa_context);
+
+	dev_err(shrm->dev, "SHRM char driver removed\n");
+}
+
+#ifdef CONFIG_HIGH_RES_TIMERS
+static enum hrtimer_restart callback(struct hrtimer *timer)
+{
+	return HRTIMER_NORESTART;
+}
+#endif
+
+
+static int __init shrm_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct resource *res;
+	struct shrm_dev *shrm = NULL;
+
+	if (pdev == NULL)  {
+		dev_err(shrm->dev,
+			"No device/platform_data found on shm device\n");
+		return -ENODEV;
+	}
+
+
+	shrm = kzalloc(sizeof(struct shrm_dev), GFP_KERNEL);
+	if (shrm == NULL) {
+		dev_err(shrm->dev,
+			"Could not allocate memory for struct shm_dev\n");
+		return -ENOMEM;
+	}
+	shrm->dev = &pdev->dev;
+
+	/* initialise the SHM */
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(shrm->dev, "Unable to map Ca Wake up interrupt\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_wake_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map APE_Read_notif_common IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ac_read_notif_0_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 2);
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map APE_Read_notif_audio IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ac_read_notif_1_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 3);
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map Cmt_msg_pending_notif_common IRQbase\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_msg_pending_notif_0_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 4);
+	if (!res) {
+		dev_err(shrm->dev,
+			"Unable to map Cmt_msg_pending_notif_audio IRQ base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+	shrm->ca_msg_pending_notif_1_irq = res->start;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(shrm->dev,
+				"Could not get SHM IO memory information\n");
+		err = -ENODEV;
+		goto rollback_intr;
+	}
+
+	shrm->intr_base = (void __iomem *)ioremap_nocache(res->start,
+					res->end - res->start + 1);
+
+	if (!(shrm->intr_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_intr;
+	}
+
+	shrm->ape_common_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_APE_COMMON_BASE;
+	shrm->ape_common_fifo_base =
+		(void __iomem *)ioremap_nocache(
+					U8500_SHM_FIFO_APE_COMMON_BASE,
+					SHM_FIFO_0_SIZE);
+	shrm->ape_common_fifo_size = (SHM_FIFO_0_SIZE)/4;
+
+	if (!(shrm->ape_common_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ape_common_fifo_base;
+	}
+
+	shrm->cmt_common_fifo_base_phy =
+		(u32 *)U8500_SHM_FIFO_CMT_COMMON_BASE;
+
+	shrm->cmt_common_fifo_base =
+		(void __iomem *)ioremap_nocache(
+			U8500_SHM_FIFO_CMT_COMMON_BASE, SHM_FIFO_0_SIZE);
+	shrm->cmt_common_fifo_size = (SHM_FIFO_0_SIZE)/4;
+
+	if (!(shrm->cmt_common_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_cmt_common_fifo_base;
+	}
+
+	shrm->ape_audio_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_APE_AUDIO_BASE;
+	shrm->ape_audio_fifo_base =
+		(void __iomem *)ioremap_nocache(U8500_SHM_FIFO_APE_AUDIO_BASE,
+							SHM_FIFO_1_SIZE);
+	shrm->ape_audio_fifo_size = (SHM_FIFO_1_SIZE)/4;
+
+	if (!(shrm->ape_audio_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ape_audio_fifo_base;
+	}
+
+	shrm->cmt_audio_fifo_base_phy =
+			(u32 *)U8500_SHM_FIFO_CMT_AUDIO_BASE;
+	shrm->cmt_audio_fifo_base =
+		(void __iomem *)ioremap_nocache(U8500_SHM_FIFO_CMT_AUDIO_BASE,
+							SHM_FIFO_1_SIZE);
+	shrm->cmt_audio_fifo_size = (SHM_FIFO_1_SIZE)/4;
+
+	if (!(shrm->cmt_audio_fifo_base)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_cmt_audio_fifo_base;
+	}
+
+	shrm->ac_common_shared_wptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_0_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_common_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_ac_common_shared_wptr;
+	}
+
+	shrm->ac_common_shared_rptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_0_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_common_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ca_common_shared_wptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_0_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_common_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+	shrm->ca_common_shared_rptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_0_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_common_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ac_audio_shared_wptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_1_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_audio_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ac_audio_shared_rptr =
+		(void __iomem *)ioremap(SHM_ACFIFO_1_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ac_audio_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ca_audio_shared_wptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_1_WRITE_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_audio_shared_wptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	shrm->ca_audio_shared_rptr =
+		(void __iomem *)ioremap(SHM_CAFIFO_1_READ_AMCU, SHM_PTR_SIZE);
+
+	if (!(shrm->ca_audio_shared_rptr)) {
+		dev_err(shrm->dev, "Unable to map register base\n");
+		err = -EBUSY;
+		goto rollback_map;
+	}
+
+
+	if (isa_init(shrm) != 0) {
+		dev_err(shrm->dev, "Driver Initialization Error\n");
+		err = -EBUSY;
+	}
+	/* install handlers and tasklets */
+	if (shm_initialise_irq(shrm)) {
+		dev_err(shrm->dev, "shm error in interrupt registration\n");
+		goto rollback_irq;
+	}
+
+#ifdef CONFIG_HIGH_RES_TIMERS
+	hrtimer_init(&timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	timer.function = callback;
+
+	hrtimer_start(&timer, ktime_set(0, 2*NSEC_PER_MSEC), HRTIMER_MODE_REL);
+#endif
+
+	return err;
+
+rollback_irq:
+	free_shm_irq(shrm);
+rollback_map:
+	iounmap(shrm->ac_common_shared_wptr);
+	iounmap(shrm->ac_common_shared_rptr);
+	iounmap(shrm->ca_common_shared_wptr);
+	iounmap(shrm->ca_common_shared_rptr);
+	iounmap(shrm->ac_audio_shared_wptr);
+	iounmap(shrm->ac_audio_shared_rptr);
+	iounmap(shrm->ca_audio_shared_wptr);
+	iounmap(shrm->ca_audio_shared_rptr);
+rollback_ac_common_shared_wptr:
+	iounmap(shrm->cmt_audio_fifo_base);
+rollback_cmt_audio_fifo_base:
+	iounmap(shrm->ape_audio_fifo_base);
+rollback_ape_audio_fifo_base:
+	iounmap(shrm->cmt_common_fifo_base);
+rollback_cmt_common_fifo_base:
+	iounmap(shrm->ape_common_fifo_base);
+rollback_ape_common_fifo_base:
+	iounmap(shrm->intr_base);
+rollback_intr:
+	kfree(shrm);
+	return err;
+}
+
+static int __exit shrm_remove(struct platform_device *pdev)
+{
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+
+	free_shm_irq(shrm);
+	iounmap(shrm->intr_base);
+	iounmap(shrm->ape_common_fifo_base);
+	iounmap(shrm->cmt_common_fifo_base);
+	iounmap(shrm->ape_audio_fifo_base);
+	iounmap(shrm->cmt_audio_fifo_base);
+	iounmap(shrm->ac_common_shared_wptr);
+	iounmap(shrm->ac_common_shared_rptr);
+	iounmap(shrm->ca_common_shared_wptr);
+	iounmap(shrm->ca_common_shared_rptr);
+	iounmap(shrm->ac_audio_shared_wptr);
+	iounmap(shrm->ac_audio_shared_rptr);
+	iounmap(shrm->ca_audio_shared_wptr);
+	iounmap(shrm->ca_audio_shared_rptr);
+	kfree(shrm);
+	isa_exit(shrm);
+
+	return 0;
+}
+#ifdef CONFIG_PM
+
+/**
+ * u8500_shrm_suspend() - This routine puts the SHRM in to sustend state.
+ * @pdev: platform device.
+ *
+ * This routine checks the current ongoing communication with Modem by
+ * examining the ca_wake state and prevents suspend if modem communication
+ * is on-going.
+ * If ca_wake = 1 (high), modem comm. is on-going; don't suspend
+ * If ca_wake = 0 (low), no comm. with modem on-going.Allow suspend
+ */
+int u8500_shrm_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+
+	dev_dbg(shrm->dev, "%s called...\n", __func__);
+	dev_dbg(shrm->dev, "\n ca_wake_req_state = %x\n",
+					get_ca_wake_req_state());
+	/* if ca_wake_req is high, prevent system suspend */
+	if (get_ca_wake_req_state())
+		return -EBUSY;
+	else
+		return 0;
+}
+
+/**
+ * u8500_shrm_resume() - This routine resumes the SHRM from sustend state.
+ * @pdev: platform device.
+ *
+ * This routine restore back the current state of the SHRM
+ */
+int u8500_shrm_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct shrm_dev *shrm = platform_get_drvdata(pdev);
+
+	dev_dbg(shrm->dev, "%s called...\n", __func__);
+	/* TODO:
+	 * As of now, no state save takes place in suspend.
+	 * So, nothing to restore in resume.
+	 * Simply return as of now.
+	 * State saved in suspend should be restored here.
+	 */
+
+	return 0;
+}
+
+static const struct dev_pm_ops shrm_dev_pm_ops = {
+	.suspend = u8500_shrm_suspend,
+	.resume = u8500_shrm_resume,
+};
+#endif
+
+static struct platform_driver shrm_driver = {
+	.remove = __exit_p(shrm_remove),
+	.driver = {
+		.name = "u8500_shrm",
+		.owner = THIS_MODULE,
+#ifdef CONFIG_PM
+		.pm = &shrm_dev_pm_ops,
+#endif
+	},
+};
+
+static int __init shrm_driver_init(void)
+{
+	return platform_driver_probe(&shrm_driver, shrm_probe);
+}
+
+static void __exit shrm_driver_exit(void)
+{
+	platform_driver_unregister(&shrm_driver);
+}
+
+module_init(shrm_driver_init);
+module_exit(shrm_driver_exit);
+
+MODULE_AUTHOR("Biju Das");
+MODULE_DESCRIPTION("Shared Memory Modem Driver Interface");
+MODULE_LICENSE("GPL");
diff --git a/drivers/modem/shrm/shrm_fifo.c b/drivers/modem/shrm/shrm_fifo.c
new file mode 100644
index 00000000000..5fe9f3c5724
--- /dev/null
+++ b/drivers/modem/shrm/shrm_fifo.c
@@ -0,0 +1,837 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghavi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/modem/shrm/shrm.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_net.h>
+#include <linux/mfd/dbx500-prcmu.h>
+
+#define L1_BOOT_INFO_REQ	1
+#define L1_BOOT_INFO_RESP	2
+#define L1_NORMAL_MSG		3
+#define L1_HEADER_MASK		28
+#define L1_MAPID_MASK		0xF0000000
+#define CONFIG_OFFSET		8
+#define COUNTER_OFFSET		20
+#define L2_HEADER_SIZE		4
+#define L2_HEADER_OFFSET	24
+#define MASK_0_15_BIT		0xFF
+#define MASK_16_31_BIT		0xFF00
+#define MASK_16_27_BIT		0xFFF0000
+#define MASK_0_39_BIT		0xFFFFF
+#define MASK_40_55_BIT		0xFF00000
+#define MASK_8_16_BIT           0x0000FF00
+#define MSG_LEN_OFFSET          16
+#define SHRM_VER                2
+#define ca_ist_inactivity_timer 25 /*25ms */
+#define ca_csc_inactivity_timer 25 /*25ms */
+
+static u8 msg_audio_counter;
+static u8 msg_common_counter;
+
+struct fifo_write_params ape_shm_fifo_0;
+struct fifo_write_params ape_shm_fifo_1;
+struct fifo_read_params cmt_shm_fifo_0;
+struct fifo_read_params cmt_shm_fifo_1;
+
+
+static u8 cmt_read_notif_0_send;
+static u8 cmt_read_notif_1_send;
+
+void shm_fifo_init(struct shrm_dev *shrm)
+{
+	ape_shm_fifo_0.writer_local_wptr	= 0;
+	ape_shm_fifo_0.writer_local_rptr	= 0;
+	*((u32 *)shrm->ac_common_shared_wptr) = 0;
+	*((u32 *)shrm->ac_common_shared_rptr) = 0;
+	ape_shm_fifo_0.shared_wptr		= 0;
+	ape_shm_fifo_0.shared_rptr		= 0;
+	ape_shm_fifo_0.availablesize = shrm->ape_common_fifo_size;
+	ape_shm_fifo_0.end_addr_fifo    = shrm->ape_common_fifo_size;
+	ape_shm_fifo_0.fifo_virtual_addr = shrm->ape_common_fifo_base;
+	spin_lock_init(&ape_shm_fifo_0.fifo_update_lock);
+
+
+	cmt_shm_fifo_0.reader_local_rptr	= 0;
+	cmt_shm_fifo_0.reader_local_wptr	= 0;
+	cmt_shm_fifo_0.shared_wptr	=
+			*((u32 *)shrm->ca_common_shared_wptr);
+	cmt_shm_fifo_0.shared_rptr	=
+			*((u32 *)shrm->ca_common_shared_rptr);
+	cmt_shm_fifo_0.availablesize	= shrm->cmt_common_fifo_size;
+	cmt_shm_fifo_0.end_addr_fifo	= shrm->cmt_common_fifo_size;
+	cmt_shm_fifo_0.fifo_virtual_addr = shrm->cmt_common_fifo_base;
+
+	ape_shm_fifo_1.writer_local_wptr	= 0;
+	ape_shm_fifo_1.writer_local_rptr	= 0;
+	ape_shm_fifo_1.shared_wptr		= 0;
+	ape_shm_fifo_1.shared_rptr		= 0;
+	*((u32 *)shrm->ac_audio_shared_wptr) = 0;
+	*((u32 *)shrm->ac_audio_shared_rptr) = 0;
+	ape_shm_fifo_1.availablesize = shrm->ape_audio_fifo_size;
+	ape_shm_fifo_1.end_addr_fifo    = shrm->ape_audio_fifo_size;
+	ape_shm_fifo_1.fifo_virtual_addr = shrm->ape_audio_fifo_base;
+	spin_lock_init(&ape_shm_fifo_1.fifo_update_lock);
+
+	cmt_shm_fifo_1.reader_local_rptr	= 0;
+	cmt_shm_fifo_1.reader_local_wptr	= 0;
+	cmt_shm_fifo_1.shared_wptr		=
+			*((u32 *)shrm->ca_audio_shared_wptr);
+	cmt_shm_fifo_1.shared_rptr		=
+			*((u32 *)shrm->ca_audio_shared_rptr);
+	cmt_shm_fifo_1.availablesize	= shrm->cmt_audio_fifo_size;
+	cmt_shm_fifo_1.end_addr_fifo	= shrm->cmt_audio_fifo_size;
+	cmt_shm_fifo_1.fifo_virtual_addr = shrm->cmt_audio_fifo_base;
+	msg_audio_counter = 0;
+	msg_common_counter = 0;
+}
+
+u8 read_boot_info_req(struct shrm_dev *shrm,
+				u32 *config,
+				u32 *version)
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_0;
+	u32 *msg;
+	u32 header = 0;
+	u8 msgtype;
+
+	/* Read L1 header read content of reader_local_rptr */
+	msg = (u32 *)
+		(fifo->reader_local_rptr + fifo->fifo_virtual_addr);
+	header = *msg;
+	msgtype = (header & L1_MAPID_MASK) >> L1_MSG_MAPID_OFFSET;
+	if (msgtype != L1_BOOT_INFO_REQ) {
+		dev_err(shrm->dev, "Read_Boot_Info_Req Fatal ERROR\n");
+		dev_err(shrm->dev, "Received msgtype is %d\n", msgtype);
+		dev_info(shrm->dev, "Initiating a modem reset\n");
+		queue_kthread_work(&shrm->shm_ac_wake_kw,
+				&shrm->shm_mod_reset_req);
+		return 0;
+	}
+	*config = (header >> CONFIG_OFFSET) & MASK_0_15_BIT;
+	*version = header & MASK_0_15_BIT;
+	fifo->reader_local_rptr += 1;
+
+	return 1;
+}
+
+void write_boot_info_resp(struct shrm_dev *shrm, u32 config,
+							u32 version)
+{
+	struct fifo_write_params *fifo = &ape_shm_fifo_0;
+	u32 *msg;
+	u8 msg_length;
+	version = SHRM_VER;
+
+	spin_lock_bh(&fifo->fifo_update_lock);
+	/* Read L1 header read content of reader_local_rptr */
+	msg = (u32 *)
+		(fifo->writer_local_wptr+fifo->fifo_virtual_addr);
+	if (version < 1)	{
+		*msg = ((L1_BOOT_INFO_RESP << L1_MSG_MAPID_OFFSET) |
+				((config << CONFIG_OFFSET) & MASK_16_31_BIT)
+				| (version & MASK_0_15_BIT));
+		msg_length = 1;
+	} else {
+		*msg = ((L1_BOOT_INFO_RESP << L1_MSG_MAPID_OFFSET) |
+			((0x8 << MSG_LEN_OFFSET) & MASK_16_27_BIT) |
+			((config << CONFIG_OFFSET) & MASK_8_16_BIT)|
+			version);
+		msg++;
+		*msg = ca_ist_inactivity_timer;
+		msg++;
+		*msg = ca_csc_inactivity_timer;
+		msg_length = L1_NORMAL_MSG;
+	}
+	fifo->writer_local_wptr += msg_length;
+	fifo->availablesize -= msg_length;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+/**
+ * shm_write_msg_to_fifo() - write message to FIFO
+ * @shrm:	pointer to shrm device information structure
+ * @channel:	audio or common channel
+ * @l2header:	L2 header or device ID
+ * @addr:	pointer to write buffer address
+ * @length:	length of mst to write
+ *
+ * Function Which Writes the data into Fifo in IPC zone
+ * It is called from shm_write_msg. This function will copy the msg
+ * from the kernel buffer to FIFO. There are 4 kernel buffers from where
+ * the data is to copied to FIFO one for each of the messages ISI, RPC,
+ * AUDIO and SECURITY. ISI, RPC and SECURITY messages are pushed to FIFO
+ * in commmon channel and AUDIO message is pushed onto audio channel FIFO.
+ */
+int shm_write_msg_to_fifo(struct shrm_dev *shrm, u8 channel,
+				u8 l2header, void *addr, u32 length)
+{
+	struct fifo_write_params *fifo = NULL;
+	u32 l1_header = 0, l2_header = 0;
+	u32 requiredsize;
+	u32 size = 0;
+	u32 *msg;
+	u8 *src;
+
+	if (channel == COMMON_CHANNEL)
+		fifo = &ape_shm_fifo_0;
+	else if (channel == AUDIO_CHANNEL)
+		fifo = &ape_shm_fifo_1;
+	else {
+		dev_err(shrm->dev, "invalid channel\n");
+		return -EINVAL;
+	}
+
+	/* L2 size in 32b */
+	requiredsize = ((length + 3) / 4);
+	/* Add size of L1 & L2 header */
+	requiredsize += 2;
+
+	/* if availablesize = or < requiredsize then error */
+	if (fifo->availablesize <= requiredsize) {
+		/* Fatal ERROR - should never happens */
+		dev_dbg(shrm->dev, "wr_wptr= %x\n",
+					fifo->writer_local_wptr);
+		dev_dbg(shrm->dev, "wr_rptr= %x\n",
+					fifo->writer_local_rptr);
+		dev_dbg(shrm->dev, "shared_wptr= %x\n",
+						fifo->shared_wptr);
+		dev_dbg(shrm->dev, "shared_rptr= %x\n",
+						fifo->shared_rptr);
+		dev_dbg(shrm->dev, "availsize= %x\n",
+						fifo->availablesize);
+		dev_dbg(shrm->dev, "end__fifo= %x\n",
+				fifo->end_addr_fifo);
+		dev_warn(shrm->dev, "Modem is busy, please wait."
+				" c_cnt = %d; a_cnt = %d\n", msg_common_counter,
+				msg_audio_counter);
+		if (channel == COMMON_CHANNEL) {
+			dev_warn(shrm->dev,
+					"Modem is lagging behind in reading."
+					"Stopping n/w dev queue\n");
+			shrm_stop_netdev(shrm->ndev);
+		}
+
+		return -EAGAIN;
+	}
+
+	if (channel == COMMON_CHANNEL) {
+		/* build L1 header */
+		l1_header = ((L1_NORMAL_MSG << L1_MSG_MAPID_OFFSET) |
+				(((msg_common_counter++) << COUNTER_OFFSET)
+				 & MASK_40_55_BIT) |
+				((length + L2_HEADER_SIZE) & MASK_0_39_BIT));
+	} else if (channel == AUDIO_CHANNEL) {
+		/* build L1 header */
+		l1_header = ((L1_NORMAL_MSG << L1_MSG_MAPID_OFFSET) |
+				(((msg_audio_counter++) << COUNTER_OFFSET)
+				 & MASK_40_55_BIT) |
+				((length + L2_HEADER_SIZE) & MASK_0_39_BIT));
+	}
+
+	/*
+	 * Need to take care race condition for fifo->availablesize
+	 * & fifo->writer_local_rptr with Ac_Read_notification interrupt.
+	 * One option could be use stack variable for LocalRptr and recompute
+	 * fifo->availablesize,based on flag enabled in the
+	 * Ac_read_notification
+	 */
+	l2_header = ((l2header << L2_HEADER_OFFSET) |
+					((length) & MASK_0_39_BIT));
+	spin_lock_bh(&fifo->fifo_update_lock);
+	/* Check Local Rptr is less than or equal to Local WPtr */
+	if (fifo->writer_local_rptr <= fifo->writer_local_wptr) {
+		msg = (u32 *)
+			(fifo->fifo_virtual_addr+fifo->writer_local_wptr);
+
+		/* check enough place bewteen writer_local_wptr & end of FIFO */
+		if ((fifo->end_addr_fifo-fifo->writer_local_wptr) >=
+							requiredsize) {
+			/* Add L1 header and L2 header */
+			*msg = l1_header;
+			msg++;
+			*msg = l2_header;
+			msg++;
+
+			/* copy the l2 message in 1 memcpy */
+			memcpy((void *)msg, addr, length);
+			/* UpdateWptr */
+			fifo->writer_local_wptr += requiredsize;
+			fifo->availablesize -= requiredsize;
+			fifo->writer_local_wptr %= fifo->end_addr_fifo;
+		} else {
+			/*
+			 * message is split between and of FIFO and beg of FIFO
+			 * copy first part from writer_local_wptr to end of FIFO
+			 */
+			size = fifo->end_addr_fifo-fifo->writer_local_wptr;
+
+			if (size == 1) {
+				/* Add L1 header */
+				*msg = l1_header;
+				msg++;
+				/* UpdateWptr */
+				fifo->writer_local_wptr = 0;
+				fifo->availablesize -= size;
+				/*
+				 * copy second part from beg of FIFO
+				 * with remaining part of msg
+				 */
+				msg =	(u32 *)
+						fifo->fifo_virtual_addr;
+				*msg = l2_header;
+				msg++;
+
+				/* copy the l3 message in 1 memcpy */
+				memcpy((void *)msg, addr, length);
+				/* UpdateWptr */
+				fifo->writer_local_wptr +=
+							requiredsize-size;
+				fifo->availablesize -=
+							(requiredsize-size);
+			} else if (size == 2) {
+				/* Add L1 header and L2 header */
+				*msg = l1_header;
+				msg++;
+				*msg = l2_header;
+				msg++;
+
+				/* UpdateWptr */
+				fifo->writer_local_wptr = 0;
+				fifo->availablesize -= size;
+
+				/*
+				 * copy second part from beg of FIFO
+				 * with remaining part of msg
+				 */
+				msg =	(u32 *)
+						fifo->fifo_virtual_addr;
+				/* copy the l3 message in 1 memcpy */
+				memcpy((void *)msg, addr, length);
+
+				/* UpdateWptr */
+				fifo->writer_local_wptr +=
+							requiredsize-size;
+				fifo->availablesize -=
+							(requiredsize-size);
+			} else {
+				/* Add L1 header and L2 header */
+				*msg = l1_header;
+				msg++;
+				*msg = l2_header;
+				msg++;
+
+				/* copy the l2 message in 1 memcpy */
+				memcpy((void *)msg, addr, (size-2)*4);
+
+
+				/* UpdateWptr */
+				fifo->writer_local_wptr = 0;
+				fifo->availablesize -= size;
+
+				/*
+				 * copy second part from beg of FIFO
+				 * with remaining part of msg
+				 */
+				msg = (u32 *)fifo->fifo_virtual_addr;
+				src = (u8 *)addr+((size - 2) * 4);
+				memcpy((void *)msg, src,
+						(length-((size - 2) * 4)));
+
+				/* UpdateWptr */
+				fifo->writer_local_wptr +=
+							requiredsize-size;
+				fifo->availablesize -=
+							(requiredsize-size);
+			}
+
+		}
+	} else {
+		/* writer_local_rptr > writer_local_wptr */
+		msg = (u32 *)
+			(fifo->fifo_virtual_addr+fifo->writer_local_wptr);
+		/* Add L1 header and L2 header */
+		*msg = l1_header;
+		msg++;
+		*msg = l2_header;
+		msg++;
+		/*
+		 * copy message possbile between writer_local_wptr up
+		 * to writer_local_rptr copy the l3 message in 1 memcpy
+		 */
+		memcpy((void *)msg, addr, length);
+
+		/* UpdateWptr */
+		fifo->writer_local_wptr += requiredsize;
+		fifo->availablesize -= requiredsize;
+
+	}
+	spin_unlock_bh(&fifo->fifo_update_lock);
+	return length;
+}
+
+/**
+ * read_one_l2msg_common() - read message from common channel
+ * @shrm:	pointer to shrm device information structure
+ * @l2_msg:	pointer to the read L2 message buffer
+ * @len:	message length
+ *
+ * This function read one message from the FIFO  and returns l2 header type
+ */
+u8 read_one_l2msg_common(struct shrm_dev *shrm,
+			u8 *l2_msg, u32 *len)
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_0;
+
+	u32 *msg;
+	u32 l1_header = 0;
+	u32 l2_header = 0;
+	u32 length;
+	u8 msgtype;
+	u32 msg_size;
+	u32 size = 0;
+
+	/* Read L1 header read content of reader_local_rptr */
+	msg = (u32 *)
+		(fifo->reader_local_rptr+fifo->fifo_virtual_addr);
+	l1_header = *msg++;
+	msgtype = (l1_header & 0xF0000000) >> L1_HEADER_MASK;
+
+	if (msgtype != L1_NORMAL_MSG) {
+		/* Fatal ERROR - should never happens */
+		dev_info(shrm->dev, "wr_wptr= %x\n",
+						fifo->reader_local_wptr);
+		dev_info(shrm->dev, "wr_rptr= %x\n",
+						fifo->reader_local_rptr);
+		dev_info(shrm->dev, "shared_wptr= %x\n",
+						fifo->shared_wptr);
+		dev_info(shrm->dev, "shared_rptr= %x\n",
+						fifo->shared_rptr);
+		dev_info(shrm->dev, "availsize= %x\n",
+						fifo->availablesize);
+		dev_info(shrm->dev, "end_fifo= %x\n",
+						fifo->end_addr_fifo);
+		/* Fatal ERROR - should never happens */
+		dev_crit(shrm->dev, "Fatal ERROR - should never happen\n");
+		dev_info(shrm->dev, "Initiating a modem reset\n");
+		queue_kthread_work(&shrm->shm_ac_wake_kw,
+				&shrm->shm_mod_reset_req);
+	 }
+	if (fifo->reader_local_rptr == (fifo->end_addr_fifo-1)) {
+		l2_header = (*((u32 *)fifo->fifo_virtual_addr));
+		length = l2_header & MASK_0_39_BIT;
+	} else {
+		/* Read L2 header,Msg size & content of reader_local_rptr */
+		l2_header = *msg;
+		length = l2_header & MASK_0_39_BIT;
+	}
+
+	*len = length;
+	msg_size = ((length + 3) / 4);
+	msg_size += 2;
+
+	if (fifo->reader_local_rptr + msg_size <=
+						fifo->end_addr_fifo) {
+		/* Skip L2 header */
+		msg++;
+
+		/* read msg between reader_local_rptr and end of FIFO */
+		memcpy((void *)l2_msg, (void *)msg, length);
+		/* UpdateLocalRptr */
+		fifo->reader_local_rptr += msg_size;
+		fifo->reader_local_rptr %= fifo->end_addr_fifo;
+	} else {
+		/*
+		 * msg split between end of FIFO and beg copy first
+		 * part of msg read msg between reader_local_rptr
+		 * and end of FIFO
+		 */
+		size = fifo->end_addr_fifo-fifo->reader_local_rptr;
+		if (size == 1) {
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			/* Skip L2 header */
+			msg++;
+			memcpy((void *)l2_msg, (void *)(msg), length);
+		} else if (size == 2) {
+			/* Skip L2 header */
+			msg++;
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			memcpy((void *)l2_msg,
+						(void *)(msg), length);
+		} else {
+			/* Skip L2 header */
+			msg++;
+			memcpy((void *)l2_msg, (void *)msg, ((size - 2) * 4));
+			/* copy second part of msg */
+			l2_msg += ((size - 2) * 4);
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			memcpy((void *)l2_msg, (void *)(msg),
+						(length-((size - 2) * 4)));
+		}
+		fifo->reader_local_rptr =
+			(fifo->reader_local_rptr+msg_size) %
+				fifo->end_addr_fifo;
+	}
+	return (l2_header>>L2_HEADER_OFFSET) & MASK_0_15_BIT;
+ }
+
+u8 read_remaining_messages_common()
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_0;
+	/*
+	 * There won't be any Race condition reader_local_rptr &
+	 * fifo->reader_local_wptr with CaMsgpending Notification Interrupt
+	 */
+	return ((fifo->reader_local_rptr != fifo->reader_local_wptr) ? 1 : 0);
+}
+
+u8 read_one_l2msg_audio(struct shrm_dev *shrm,
+				u8 *l2_msg, u32 *len)
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_1;
+
+	u32 *msg;
+	u32 l1_header = 0;
+	u32 l2_header = 0;
+	u32 length;
+	u8 msgtype;
+	u32 msg_size;
+	u32 size = 0;
+
+	/* Read L1 header read content of reader_local_rptr */
+	 msg = (u32 *)
+			(fifo->reader_local_rptr+fifo->fifo_virtual_addr);
+	 l1_header = *msg++;
+	 msgtype = (l1_header & 0xF0000000) >> L1_HEADER_MASK;
+
+	if (msgtype != L1_NORMAL_MSG) {
+		/* Fatal ERROR - should never happens */
+		dev_info(shrm->dev, "wr_local_wptr= %x\n",
+						fifo->reader_local_wptr);
+		dev_info(shrm->dev, "wr_local_rptr= %x\n",
+						fifo->reader_local_rptr);
+		dev_info(shrm->dev, "shared_wptr= %x\n",
+						fifo->shared_wptr);
+		dev_info(shrm->dev, "shared_rptr= %x\n",
+						fifo->shared_rptr);
+		dev_info(shrm->dev, "availsize=%x\n",
+						fifo->availablesize);
+		dev_info(shrm->dev, "end_fifo= %x\n",
+						fifo->end_addr_fifo);
+		dev_info(shrm->dev, "Received msgtype is %d\n", msgtype);
+		/* Fatal ERROR - should never happens */
+		dev_crit(shrm->dev, "Fatal ERROR - should never happen\n");
+		dev_info(shrm->dev, "Initiating a modem reset\n");
+		queue_kthread_work(&shrm->shm_ac_wake_kw,
+				&shrm->shm_mod_reset_req);
+	 }
+	if (fifo->reader_local_rptr == (fifo->end_addr_fifo-1)) {
+		l2_header = (*((u32 *)fifo->fifo_virtual_addr));
+		length = l2_header & MASK_0_39_BIT;
+	} else {
+		/* Read L2 header,Msg size & content of reader_local_rptr */
+		l2_header = *msg;
+		length = l2_header & MASK_0_39_BIT;
+	}
+
+	*len = length;
+	msg_size = ((length + 3) / 4);
+	msg_size += 2;
+
+	if (fifo->reader_local_rptr + msg_size <=
+						fifo->end_addr_fifo) {
+		/* Skip L2 header */
+		msg++;
+		/* read msg between reader_local_rptr and end of FIFO */
+		memcpy((void *)l2_msg, (void *)msg, length);
+		/* UpdateLocalRptr */
+		fifo->reader_local_rptr += msg_size;
+		fifo->reader_local_rptr %= fifo->end_addr_fifo;
+	} else {
+
+		/*
+		 * msg split between end of FIFO and beg
+		 * copy first part of msg
+		 * read msg between reader_local_rptr and end of FIFO
+		 */
+		size = fifo->end_addr_fifo-fifo->reader_local_rptr;
+		if (size == 1) {
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			/* Skip L2 header */
+			msg++;
+			memcpy((void *)l2_msg, (void *)(msg), length);
+		} else if (size == 2) {
+			/* Skip L2 header */
+			msg++;
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			memcpy((void *)l2_msg, (void *)(msg), length);
+		} else {
+			/* Skip L2 header */
+			msg++;
+			memcpy((void *)l2_msg, (void *)msg, ((size - 2) * 4));
+			/* copy second part of msg */
+			l2_msg += ((size - 2) * 4);
+			msg = (u32 *)(fifo->fifo_virtual_addr);
+			memcpy((void *)l2_msg, (void *)(msg),
+						(length-((size - 2) * 4)));
+		}
+		fifo->reader_local_rptr =
+			(fifo->reader_local_rptr+msg_size) %
+			fifo->end_addr_fifo;
+
+	}
+	return (l2_header>>L2_HEADER_OFFSET) & MASK_0_15_BIT;
+ }
+
+u8 read_remaining_messages_audio()
+{
+	struct fifo_read_params *fifo = &cmt_shm_fifo_1;
+
+	return ((fifo->reader_local_rptr != fifo->reader_local_wptr) ?
+									1 : 0);
+}
+
+u8 is_the_only_one_unread_message(struct shrm_dev *shrm,
+						u8 channel, u32 length)
+{
+	struct fifo_write_params *fifo = NULL;
+	u32 messagesize = 0;
+	u8 is_only_one_unread_msg = 0;
+
+	if (channel == COMMON_CHANNEL)
+		fifo = &ape_shm_fifo_0;
+	else /* channel = AUDIO_CHANNEL */
+		fifo = &ape_shm_fifo_1;
+
+	/* L3 size in 32b */
+	messagesize = ((length + 3) / 4);
+	/* Add size of L1 & L2 header */
+	messagesize += 2;
+	/*
+	 * possibility of race condition with Ac Read notification interrupt.
+	 * need to check ?
+	 */
+	if (fifo->writer_local_wptr > fifo->writer_local_rptr)
+		is_only_one_unread_msg =
+			((fifo->writer_local_rptr + messagesize) ==
+			fifo->writer_local_wptr) ? 1 : 0;
+	else
+		/* Msg split between end of fifo and starting of Fifo */
+		is_only_one_unread_msg =
+			(((fifo->writer_local_rptr + messagesize) %
+			fifo->end_addr_fifo) == fifo->writer_local_wptr) ?
+									1 : 0;
+
+	return is_only_one_unread_msg;
+}
+
+void update_ca_common_local_wptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update CA common reader local write pointer with the
+	 * shared write pointer
+	 */
+	struct fifo_read_params *fifo = &cmt_shm_fifo_0;
+
+	fifo->shared_wptr =
+		(*((u32 *)shrm->ca_common_shared_wptr));
+	fifo->reader_local_wptr = fifo->shared_wptr;
+}
+
+void update_ca_audio_local_wptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update CA audio reader local write pointer with the
+	 * shared write pointer
+	 */
+	struct fifo_read_params *fifo = &cmt_shm_fifo_1;
+
+	fifo->shared_wptr =
+		(*((u32 *)shrm->ca_audio_shared_wptr));
+	fifo->reader_local_wptr = fifo->shared_wptr;
+}
+
+void update_ac_common_local_rptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update AC common writer local read pointer with the
+	 * shared read pointer
+	 */
+	struct fifo_write_params *fifo;
+	u32 free_space = 0;
+
+	fifo = &ape_shm_fifo_0;
+
+	spin_lock_bh(&fifo->fifo_update_lock);
+	fifo->shared_rptr =
+		(*((u32 *)shrm->ac_common_shared_rptr));
+
+	if (fifo->shared_rptr >= fifo->writer_local_rptr)
+		free_space =
+			(fifo->shared_rptr-fifo->writer_local_rptr);
+	else {
+		free_space =
+			(fifo->end_addr_fifo-fifo->writer_local_rptr);
+		free_space += fifo->shared_rptr;
+	}
+
+	/* Chance of race condition of below variables with write_msg */
+	fifo->availablesize += free_space;
+	fifo->writer_local_rptr = fifo->shared_rptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void update_ac_audio_local_rptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update AC audio writer local read pointer with the
+	 * shared read pointer
+	 */
+	struct fifo_write_params *fifo;
+	u32 free_space = 0;
+
+	fifo = &ape_shm_fifo_1;
+	spin_lock_bh(&fifo->fifo_update_lock);
+	fifo->shared_rptr =
+		(*((u32 *)shrm->ac_audio_shared_rptr));
+
+	if (fifo->shared_rptr >= fifo->writer_local_rptr)
+		free_space =
+			(fifo->shared_rptr-fifo->writer_local_rptr);
+	else {
+		free_space =
+			(fifo->end_addr_fifo-fifo->writer_local_rptr);
+		free_space += fifo->shared_rptr;
+	}
+
+	/* Chance of race condition of below variables with write_msg */
+	fifo->availablesize += free_space;
+	fifo->writer_local_rptr = fifo->shared_rptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void update_ac_common_shared_wptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update AC common shared write pointer with the
+	 * local write pointer
+	 */
+	struct fifo_write_params *fifo;
+
+	fifo = &ape_shm_fifo_0;
+	spin_lock_bh(&fifo->fifo_update_lock);
+	/* Update shared pointer fifo offset of the IPC zone */
+	(*((u32 *)shrm->ac_common_shared_wptr)) =
+						fifo->writer_local_wptr;
+
+	fifo->shared_wptr = fifo->writer_local_wptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void update_ac_audio_shared_wptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update AC audio shared write pointer with the
+	 * local write pointer
+	 */
+	struct fifo_write_params *fifo;
+
+	fifo = &ape_shm_fifo_1;
+	spin_lock_bh(&fifo->fifo_update_lock);
+	/* Update shared pointer fifo offset of the IPC zone */
+	(*((u32 *)shrm->ac_audio_shared_wptr)) =
+						fifo->writer_local_wptr;
+	fifo->shared_wptr = fifo->writer_local_wptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void update_ca_common_shared_rptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update CA common shared read pointer with the
+	 * local read pointer
+	 */
+	struct fifo_read_params *fifo;
+
+	fifo = &cmt_shm_fifo_0;
+
+	/* Update shared pointer fifo offset of the IPC zone */
+	(*((u32 *)shrm->ca_common_shared_rptr)) =
+						fifo->reader_local_rptr;
+	fifo->shared_rptr = fifo->reader_local_rptr;
+}
+
+void update_ca_audio_shared_rptr(struct shrm_dev *shrm)
+{
+	/*
+	 * update CA audio shared read pointer with the
+	 * local read pointer
+	 */
+	struct fifo_read_params *fifo;
+
+	fifo = &cmt_shm_fifo_1;
+
+	/* Update shared pointer fifo offset of the IPC zone */
+	(*((u32 *)shrm->ca_audio_shared_rptr)) =
+						fifo->reader_local_rptr;
+	fifo->shared_rptr = fifo->reader_local_rptr;
+}
+
+void get_reader_pointers(u8 channel_type, u32 *reader_local_rptr,
+				u32 *reader_local_wptr, u32 *shared_rptr)
+{
+	struct fifo_read_params *fifo = NULL;
+
+	if (channel_type == COMMON_CHANNEL)
+		fifo = &cmt_shm_fifo_0;
+	else /* channel_type = AUDIO_CHANNEL */
+		fifo = &cmt_shm_fifo_1;
+
+	*reader_local_rptr = fifo->reader_local_rptr;
+	*reader_local_wptr = fifo->reader_local_wptr;
+	*shared_rptr = fifo->shared_rptr;
+}
+
+void get_writer_pointers(u8 channel_type, u32 *writer_local_rptr,
+			 u32 *writer_local_wptr, u32 *shared_wptr)
+{
+	struct fifo_write_params *fifo = NULL;
+
+	if (channel_type == COMMON_CHANNEL)
+		fifo = &ape_shm_fifo_0;
+	else /* channel_type = AUDIO_CHANNEL */
+		fifo = &ape_shm_fifo_1;
+
+	spin_lock_bh(&fifo->fifo_update_lock);
+	*writer_local_rptr = fifo->writer_local_rptr;
+	*writer_local_wptr = fifo->writer_local_wptr;
+	*shared_wptr = fifo->shared_wptr;
+	spin_unlock_bh(&fifo->fifo_update_lock);
+}
+
+void set_ca_msg_0_read_notif_send(u8 val)
+{
+	cmt_read_notif_0_send = val;
+}
+
+u8 get_ca_msg_0_read_notif_send(void)
+{
+	return cmt_read_notif_0_send;
+}
+
+void set_ca_msg_1_read_notif_send(u8 val)
+{
+	cmt_read_notif_1_send = val;
+}
+
+u8 get_ca_msg_1_read_notif_send(void)
+{
+	return cmt_read_notif_1_send;
+}
diff --git a/drivers/modem/shrm/shrm_protocol.c b/drivers/modem/shrm/shrm_protocol.c
new file mode 100644
index 00000000000..f029f4e0cfa
--- /dev/null
+++ b/drivers/modem/shrm/shrm_protocol.c
@@ -0,0 +1,1546 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+#include <linux/netlink.h>
+#include <linux/kthread.h>
+#include <linux/modem/shrm/shrm.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_net.h>
+#include <linux/modem/modem_client.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/mfd/abx500.h>
+#include <mach/reboot_reasons.h>
+#include <mach/suspend.h>
+#include <mach/prcmu-debug.h>
+
+#define L2_HEADER_ISI		0x0
+#define L2_HEADER_RPC		0x1
+#define L2_HEADER_AUDIO		0x2
+#define L2_HEADER_SECURITY	0x3
+#define L2_HEADER_COMMON_SIMPLE_LOOPBACK	0xC0
+#define L2_HEADER_COMMON_ADVANCED_LOOPBACK	0xC1
+#define L2_HEADER_AUDIO_SIMPLE_LOOPBACK		0x80
+#define L2_HEADER_AUDIO_ADVANCED_LOOPBACK	0x81
+#define L2_HEADER_CIQ		0xC3
+#define L2_HEADER_RTC_CALIBRATION		0xC8
+#define MAX_PAYLOAD 1024
+#define MOD_STUCK_TIMEOUT	6
+#define FIFO_FULL_TIMEOUT	1
+#define PRCM_MOD_AWAKE_STATUS_PRCM_MOD_COREPD_AWAKE	BIT(0)
+#define PRCM_MOD_AWAKE_STATUS_PRCM_MOD_AAPD_AWAKE	BIT(1)
+#define PRCM_MOD_AWAKE_STATUS_PRCM_MOD_VMODEM_OFF_ISO	BIT(2)
+#define PRCM_MOD_PURESET	BIT(0)
+#define PRCM_MOD_SW_RESET	BIT(1)
+
+#define PRCM_HOSTACCESS_REQ	0x334
+#define PRCM_MOD_AWAKE_STATUS	0x4A0
+#define PRCM_MOD_RESETN_VAL	0x204
+
+static u8 boot_state = BOOT_INIT;
+static u8 recieve_common_msg[8*1024];
+static u8 recieve_audio_msg[8*1024];
+static received_msg_handler rx_common_handler;
+static received_msg_handler rx_audio_handler;
+static struct hrtimer timer;
+static struct hrtimer mod_stuck_timer_0;
+static struct hrtimer mod_stuck_timer_1;
+static struct hrtimer fifo_full_timer;
+struct sock *shrm_nl_sk;
+
+static char shrm_common_tx_state = SHRM_SLEEP_STATE;
+static char shrm_common_rx_state = SHRM_SLEEP_STATE;
+static char shrm_audio_tx_state = SHRM_SLEEP_STATE;
+static char shrm_audio_rx_state = SHRM_SLEEP_STATE;
+
+static atomic_t ac_sleep_disable_count = ATOMIC_INIT(0);
+static atomic_t ac_msg_pend_1 = ATOMIC_INIT(0);
+static atomic_t mod_stuck = ATOMIC_INIT(0);
+static atomic_t fifo_full = ATOMIC_INIT(0);
+static struct shrm_dev *shm_dev;
+
+/* Spin lock and tasklet declaration */
+DECLARE_TASKLET(shm_ca_0_tasklet, shm_ca_msgpending_0_tasklet, 0);
+DECLARE_TASKLET(shm_ca_1_tasklet, shm_ca_msgpending_1_tasklet, 0);
+DECLARE_TASKLET(shm_ac_read_0_tasklet, shm_ac_read_notif_0_tasklet, 0);
+DECLARE_TASKLET(shm_ac_read_1_tasklet, shm_ac_read_notif_1_tasklet, 0);
+
+static DEFINE_MUTEX(ac_state_mutex);
+
+static DEFINE_SPINLOCK(ca_common_lock);
+static DEFINE_SPINLOCK(ca_audio_lock);
+static DEFINE_SPINLOCK(ca_wake_req_lock);
+static DEFINE_SPINLOCK(boot_lock);
+static DEFINE_SPINLOCK(mod_stuck_lock);
+static DEFINE_SPINLOCK(start_timer_lock);
+
+enum shrm_nl {
+	SHRM_NL_MOD_RESET = 1,
+	SHRM_NL_MOD_QUERY_STATE,
+	SHRM_NL_USER_MOD_RESET,
+	SHRM_NL_STATUS_MOD_ONLINE,
+	SHRM_NL_STATUS_MOD_OFFLINE,
+};
+
+static int check_modem_in_reset(void);
+
+void shm_print_dbg_info_work(struct kthread_work *work)
+{
+	abx500_dump_all_banks();
+	prcmu_debug_dump_regs();
+	prcmu_debug_dump_data_mem();
+}
+
+void shm_mod_reset_req_work(struct kthread_work *work)
+{
+	unsigned long flags;
+
+	/* update the boot_state */
+	spin_lock_irqsave(&boot_lock, flags);
+	if (boot_state != BOOT_DONE) {
+		dev_info(shm_dev->dev, "Modem in reset state\n");
+		spin_unlock_irqrestore(&boot_lock, flags);
+		return;
+	}
+	boot_state = BOOT_UNKNOWN;
+	wmb();
+	spin_unlock_irqrestore(&boot_lock, flags);
+	prcmu_modem_reset();
+}
+
+static void shm_ac_sleep_req_work(struct kthread_work *work)
+{
+	mutex_lock(&ac_state_mutex);
+	if (atomic_read(&ac_sleep_disable_count) == 0)
+		modem_release(shm_dev->modem);
+	mutex_unlock(&ac_state_mutex);
+}
+
+static void shm_ac_wake_req_work(struct kthread_work *work)
+{
+	mutex_lock(&ac_state_mutex);
+	modem_request(shm_dev->modem);
+	mutex_unlock(&ac_state_mutex);
+}
+
+static u32 get_host_accessport_val(void)
+{
+	u32 prcm_hostaccess;
+	u32 status;
+	u32 reset_stats;
+
+	status = (prcmu_read(PRCM_MOD_AWAKE_STATUS) & 0x03);
+	reset_stats = (prcmu_read(PRCM_MOD_RESETN_VAL) & 0x03);
+	prcm_hostaccess = prcmu_read(PRCM_HOSTACCESS_REQ);
+	wmb();
+	prcm_hostaccess = ((prcm_hostaccess & 0x01) &&
+		(status == (PRCM_MOD_AWAKE_STATUS_PRCM_MOD_AAPD_AWAKE |
+			    PRCM_MOD_AWAKE_STATUS_PRCM_MOD_COREPD_AWAKE)) &&
+		(reset_stats == (PRCM_MOD_SW_RESET | PRCM_MOD_PURESET)));
+
+	return prcm_hostaccess;
+}
+
+static enum hrtimer_restart shm_fifo_full_timeout(struct hrtimer *timer)
+{
+	queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+					&shm_dev->shm_mod_reset_req);
+	queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+					&shm_dev->shm_print_dbg_info);
+	return HRTIMER_NORESTART;
+}
+
+static enum hrtimer_restart shm_mod_stuck_timeout(struct hrtimer *timer)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mod_stuck_lock, flags);
+	/* Check MSR is already in progress */
+	if (shm_dev->msr_flag || boot_state == BOOT_UNKNOWN ||
+			atomic_read(&mod_stuck) || atomic_read(&fifo_full)) {
+		spin_unlock_irqrestore(&mod_stuck_lock, flags);
+		return HRTIMER_NORESTART;
+	}
+	atomic_set(&mod_stuck, 1);
+	spin_unlock_irqrestore(&mod_stuck_lock, flags);
+	dev_err(shm_dev->dev, "No response from modem, timeout %dsec\n",
+			MOD_STUCK_TIMEOUT);
+	dev_err(shm_dev->dev, "APE initiating MSR\n");
+	queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+					&shm_dev->shm_mod_reset_req);
+	queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+					&shm_dev->shm_print_dbg_info);
+	return HRTIMER_NORESTART;
+}
+
+static enum hrtimer_restart callback(struct hrtimer *timer)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ca_wake_req_lock, flags);
+	if (((shrm_common_rx_state == SHRM_IDLE) ||
+				(shrm_common_rx_state == SHRM_SLEEP_STATE))
+			&& ((shrm_common_tx_state == SHRM_IDLE) ||
+				(shrm_common_tx_state == SHRM_SLEEP_STATE))
+			&& ((shrm_audio_rx_state == SHRM_IDLE)  ||
+				(shrm_audio_rx_state == SHRM_SLEEP_STATE))
+			&& ((shrm_audio_tx_state == SHRM_IDLE)  ||
+				(shrm_audio_tx_state == SHRM_SLEEP_STATE))) {
+
+		shrm_common_rx_state = SHRM_SLEEP_STATE;
+		shrm_audio_rx_state = SHRM_SLEEP_STATE;
+		shrm_common_tx_state = SHRM_SLEEP_STATE;
+		shrm_audio_tx_state = SHRM_SLEEP_STATE;
+
+		queue_kthread_work(&shm_dev->shm_ac_sleep_kw,
+				&shm_dev->shm_ac_sleep_req);
+
+	}
+	spin_unlock_irqrestore(&ca_wake_req_lock, flags);
+
+	return HRTIMER_NORESTART;
+}
+
+int nl_send_multicast_message(int msg, gfp_t gfp_mask)
+{
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh = NULL;
+	int err;
+
+	/* prepare netlink message */
+	skb = alloc_skb(NLMSG_SPACE(MAX_PAYLOAD), gfp_mask);
+	if (!skb) {
+		dev_err(shm_dev->dev, "%s:alloc_skb failed\n", __func__);
+		err = -ENOMEM;
+		goto out;
+	}
+
+	nlh = (struct nlmsghdr *)skb->data;
+	nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD);
+	dev_dbg(shm_dev->dev, "nlh->nlmsg_len = %d\n", nlh->nlmsg_len);
+
+	nlh->nlmsg_pid = 0;  /* from kernel */
+	nlh->nlmsg_flags = 0;
+	*(int *)NLMSG_DATA(nlh) = msg;
+	skb_put(skb, MAX_PAYLOAD);
+	/* sender is in group 1<<0 */
+	NETLINK_CB(skb).pid = 0;  /* from kernel */
+	/* to mcast group 1<<0 */
+	NETLINK_CB(skb).dst_group = 1;
+
+	/*multicast the message to all listening processes*/
+	err = netlink_broadcast(shrm_nl_sk, skb, 0, 1, gfp_mask);
+	dev_dbg(shm_dev->dev, "ret val from nl-multicast = %d\n", err);
+
+out:
+	return err;
+}
+
+static void nl_send_unicast_message(int dst_pid)
+{
+	struct sk_buff *skb = NULL;
+	struct nlmsghdr *nlh = NULL;
+	int err;
+	int bt_state;
+	unsigned long flags;
+
+	dev_info(shm_dev->dev, "Sending unicast message\n");
+
+	/* prepare the NL message for unicast */
+	skb = alloc_skb(NLMSG_SPACE(MAX_PAYLOAD), GFP_KERNEL);
+	if (!skb) {
+		dev_err(shm_dev->dev, "%s:alloc_skb failed\n", __func__);
+		return;
+	}
+
+	nlh = (struct nlmsghdr *)skb->data;
+	nlh->nlmsg_len = NLMSG_SPACE(MAX_PAYLOAD);
+	dev_dbg(shm_dev->dev, "nlh->nlmsg_len = %d\n", nlh->nlmsg_len);
+
+	nlh->nlmsg_pid = 0;  /* from kernel */
+	nlh->nlmsg_flags = 0;
+
+	spin_lock_irqsave(&boot_lock, flags);
+	bt_state = boot_state;
+	spin_unlock_irqrestore(&boot_lock, flags);
+
+	if (bt_state == BOOT_DONE)
+		*(int *)NLMSG_DATA(nlh) = SHRM_NL_STATUS_MOD_ONLINE;
+	else
+		*(int *)NLMSG_DATA(nlh) = SHRM_NL_STATUS_MOD_OFFLINE;
+
+	skb_put(skb, MAX_PAYLOAD);
+	/* sender is in group 1<<0 */
+	NETLINK_CB(skb).pid = 0;  /* from kernel */
+	NETLINK_CB(skb).dst_group = 0;
+
+	/*unicast the message to the querying processes*/
+	err = netlink_unicast(shrm_nl_sk, skb, dst_pid, MSG_DONTWAIT);
+	dev_dbg(shm_dev->dev, "ret val from nl-unicast = %d\n", err);
+}
+
+
+static int check_modem_in_reset(void)
+{
+	u8 bt_state;
+	unsigned long flags;
+
+	spin_lock_irqsave(&boot_lock, flags);
+	bt_state = boot_state;
+	spin_unlock_irqrestore(&boot_lock, flags);
+
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+	if (bt_state != BOOT_UNKNOWN)
+		return 0;
+	else
+		return -ENODEV;
+#else
+	/*
+	 * this check won't be applicable and won't work correctly
+	 * if modem-silent-feature is not enabled
+	 * so, simply return 0
+	 */
+	return 0;
+#endif
+}
+
+void shm_ca_msgpending_0_tasklet(unsigned long tasklet_data)
+{
+	struct shrm_dev *shrm = (struct shrm_dev *)tasklet_data;
+	u32 reader_local_rptr;
+	u32 reader_local_wptr;
+	u32 shared_rptr;
+	u32 config = 0, version = 0;
+	unsigned long flags;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	/* Interprocess locking */
+	spin_lock(&ca_common_lock);
+
+	/* Update_reader_local_wptr with shared_wptr */
+	update_ca_common_local_wptr(shrm);
+	get_reader_pointers(COMMON_CHANNEL, &reader_local_rptr,
+				&reader_local_wptr, &shared_rptr);
+
+	set_ca_msg_0_read_notif_send(0);
+
+	if (boot_state == BOOT_DONE) {
+		shrm_common_rx_state = SHRM_PTR_FREE;
+
+		if (reader_local_rptr != shared_rptr)
+			ca_msg_read_notification_0(shrm);
+		if (reader_local_rptr != reader_local_wptr)
+			receive_messages_common(shrm);
+		get_reader_pointers(COMMON_CHANNEL, &reader_local_rptr,
+				&reader_local_wptr, &shared_rptr);
+		if (reader_local_rptr == reader_local_wptr)
+			shrm_common_rx_state = SHRM_IDLE;
+	} else {
+		/* BOOT phase.only a BOOT_RESP should be in FIFO */
+		if (boot_state != BOOT_INFO_SYNC) {
+			if (!read_boot_info_req(shrm, &config, &version)) {
+				dev_err(shrm->dev,
+						"Unable to read boot state\n");
+				return;
+			}
+			/* SendReadNotification */
+			ca_msg_read_notification_0(shrm);
+			/*
+			 * Check the version number before
+			 * sending Boot info response
+			 */
+
+			/* send MsgPending notification */
+			write_boot_info_resp(shrm, config, version);
+			spin_lock_irqsave(&boot_lock, flags);
+			boot_state = BOOT_INFO_SYNC;
+			spin_unlock_irqrestore(&boot_lock, flags);
+			dev_info(shrm->dev, "BOOT_INFO_SYNC\n");
+			queue_kthread_work(&shrm->shm_common_ch_wr_kw,
+					&shrm->send_ac_msg_pend_notify_0);
+		} else {
+			ca_msg_read_notification_0(shrm);
+			dev_info(shrm->dev,
+				"BOOT_INFO_SYNC\n");
+		}
+	}
+	/* Interprocess locking */
+	spin_unlock(&ca_common_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_ca_msgpending_1_tasklet(unsigned long tasklet_data)
+{
+	struct shrm_dev *shrm = (struct shrm_dev *)tasklet_data;
+	u32 reader_local_rptr;
+	u32 reader_local_wptr;
+	u32 shared_rptr;
+
+	/*
+	 * This function is called when CaMsgPendingNotification Trigerred
+	 * by CMU. It means that CMU has wrote a message into Ca Audio FIFO
+	 */
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown\n",
+				__func__);
+		return;
+	}
+
+	/* Interprocess locking */
+	spin_lock(&ca_audio_lock);
+
+	/* Update_reader_local_wptr(with shared_wptr) */
+	update_ca_audio_local_wptr(shrm);
+	get_reader_pointers(AUDIO_CHANNEL, &reader_local_rptr,
+					&reader_local_wptr, &shared_rptr);
+
+	set_ca_msg_1_read_notif_send(0);
+
+	if (boot_state != BOOT_DONE) {
+		dev_err(shrm->dev, "Boot Error\n");
+		return;
+	}
+	shrm_audio_rx_state = SHRM_PTR_FREE;
+	/* Check we already read the message */
+	if (reader_local_rptr != shared_rptr)
+		ca_msg_read_notification_1(shrm);
+	if (reader_local_rptr != reader_local_wptr)
+		receive_messages_audio(shrm);
+
+	get_reader_pointers(AUDIO_CHANNEL, &reader_local_rptr,
+			&reader_local_wptr, &shared_rptr);
+	if (reader_local_rptr == reader_local_wptr)
+		shrm_audio_rx_state = SHRM_IDLE;
+
+	 /* Interprocess locking */
+	spin_unlock(&ca_audio_lock);
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_ac_read_notif_0_tasklet(unsigned long tasklet_data)
+{
+	struct shrm_dev *shrm = (struct shrm_dev *)tasklet_data;
+	u32 writer_local_rptr;
+	u32 writer_local_wptr;
+	u32 shared_wptr;
+	unsigned long flags;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	/* Update writer_local_rptrwith shared_rptr */
+	update_ac_common_local_rptr(shrm);
+	get_writer_pointers(COMMON_CHANNEL, &writer_local_rptr,
+				&writer_local_wptr, &shared_wptr);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown\n",
+				__func__);
+		return;
+	}
+
+	if (boot_state == BOOT_INFO_SYNC) {
+		/* BOOT_RESP sent by APE has been received by CMT */
+		spin_lock_irqsave(&boot_lock, flags);
+		boot_state = BOOT_DONE;
+		spin_unlock_irqrestore(&boot_lock, flags);
+		dev_info(shrm->dev, "IPC_ISA BOOT_DONE\n");
+
+		if (shrm->msr_flag) {
+			shrm_start_netdev(shrm->ndev);
+			shrm->msr_flag = 0;
+
+			/* multicast that modem is online */
+			nl_send_multicast_message(SHRM_NL_STATUS_MOD_ONLINE,
+					GFP_ATOMIC);
+		}
+
+	} else if (boot_state == BOOT_DONE) {
+		if (writer_local_rptr != writer_local_wptr) {
+			shrm_common_tx_state = SHRM_PTR_FREE;
+			queue_kthread_work(&shrm->shm_common_ch_wr_kw,
+					&shrm->send_ac_msg_pend_notify_0);
+		} else {
+			shrm_common_tx_state = SHRM_IDLE;
+			shrm_restart_netdev(shrm->ndev);
+		}
+	} else {
+		dev_err(shrm->dev, "Invalid boot state\n");
+	}
+	/* start timer here */
+	hrtimer_start(&timer, ktime_set(0, 25*NSEC_PER_MSEC),
+			HRTIMER_MODE_REL);
+	atomic_dec(&ac_sleep_disable_count);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_ac_read_notif_1_tasklet(unsigned long tasklet_data)
+{
+	struct shrm_dev *shrm = (struct shrm_dev *)tasklet_data;
+	u32 writer_local_rptr;
+	u32 writer_local_wptr;
+	u32 shared_wptr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown\n",
+				__func__);
+		return;
+	}
+
+	/* Update writer_local_rptr(with shared_rptr) */
+	update_ac_audio_local_rptr(shrm);
+	get_writer_pointers(AUDIO_CHANNEL, &writer_local_rptr,
+				&writer_local_wptr, &shared_wptr);
+	if (boot_state != BOOT_DONE) {
+		dev_err(shrm->dev, "Error Case in boot state\n");
+		return;
+	}
+	if (writer_local_rptr != writer_local_wptr) {
+		shrm_audio_tx_state = SHRM_PTR_FREE;
+		queue_kthread_work(&shrm->shm_audio_ch_wr_kw,
+				&shrm->send_ac_msg_pend_notify_1);
+	} else {
+		shrm_audio_tx_state = SHRM_IDLE;
+	}
+	/* start timer here */
+	hrtimer_start(&timer, ktime_set(0, 25*NSEC_PER_MSEC),
+			HRTIMER_MODE_REL);
+	atomic_dec(&ac_sleep_disable_count);
+	atomic_dec(&ac_msg_pend_1);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_ca_sleep_req_work(struct kthread_work *work)
+{
+	u8 bt_state;
+	unsigned long flags;
+
+	dev_dbg(shm_dev->dev, "%s:IRQ_PRCMU_CA_SLEEP\n", __func__);
+
+	spin_lock_irqsave(&boot_lock, flags);
+	bt_state = boot_state;
+	spin_unlock_irqrestore(&boot_lock, flags);
+
+	local_irq_save(flags);
+	preempt_disable();
+	if (bt_state != BOOT_DONE) {
+		dev_err(shm_dev->dev, "%s:Modem state reset or unknown\n",
+				__func__);
+		preempt_enable();
+		local_irq_restore(flags);
+		return;
+	}
+	shrm_common_rx_state = SHRM_IDLE;
+	shrm_audio_rx_state =  SHRM_IDLE;
+
+	if (!get_host_accessport_val()) {
+		dev_err(shm_dev->dev, "%s: host_accessport is low\n", __func__);
+		queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+				&shm_dev->shm_mod_reset_req);
+		preempt_enable();
+		local_irq_restore(flags);
+		return;
+	}
+	writel((1<<GOP_CA_WAKE_ACK_BIT),
+		shm_dev->intr_base + GOP_SET_REGISTER_BASE);
+	preempt_enable();
+	local_irq_restore(flags);
+
+	hrtimer_start(&timer, ktime_set(0, 10*NSEC_PER_MSEC),
+			HRTIMER_MODE_REL);
+	suspend_unblock_sleep();
+	atomic_dec(&ac_sleep_disable_count);
+}
+
+void shm_ca_wake_req_work(struct kthread_work *work)
+{
+	unsigned long flags;
+	struct shrm_dev *shrm = container_of(work,
+			struct shrm_dev, shm_ca_wake_req);
+
+	/* initialize the FIFO Variables */
+	if (boot_state == BOOT_INIT)
+		shm_fifo_init(shrm);
+
+	mutex_lock(&ac_state_mutex);
+	modem_request(shrm->modem);
+	mutex_unlock(&ac_state_mutex);
+
+	local_irq_save(flags);
+	preempt_disable();
+	/* send ca_wake_ack_interrupt to CMU */
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown\n",
+			__func__);
+		preempt_enable();
+		local_irq_restore(flags);
+		return;
+	}
+
+	if (!get_host_accessport_val()) {
+		dev_err(shrm->dev, "%s: host_accessport is low\n", __func__);
+		queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+				&shm_dev->shm_mod_reset_req);
+		preempt_enable();
+		local_irq_restore(flags);
+	}
+
+	/* send ca_wake_ack_interrupt to CMU */
+	writel((1<<GOP_CA_WAKE_ACK_BIT),
+			shm_dev->intr_base + GOP_SET_REGISTER_BASE);
+	preempt_enable();
+	local_irq_restore(flags);
+}
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+static int shrm_modem_reset_sequence(void)
+{
+	int err;
+	unsigned long flags;
+
+	hrtimer_cancel(&timer);
+	hrtimer_cancel(&mod_stuck_timer_0);
+	hrtimer_cancel(&mod_stuck_timer_1);
+	hrtimer_cancel(&fifo_full_timer);
+	atomic_set(&mod_stuck, 0);
+	atomic_set(&fifo_full, 0);
+	tasklet_disable_nosync(&shm_ac_read_0_tasklet);
+	tasklet_disable_nosync(&shm_ac_read_1_tasklet);
+	tasklet_disable_nosync(&shm_ca_0_tasklet);
+	tasklet_disable_nosync(&shm_ca_1_tasklet);
+
+	/*
+	 * keep the count to 0 so that we can bring down the line
+	 * for normal ac-wake and ac-sleep logic
+	 */
+	atomic_set(&ac_sleep_disable_count, 0);
+	atomic_set(&ac_msg_pend_1, 0);
+
+	/* workaround for MSR */
+	queue_kthread_work(&shm_dev->shm_ac_wake_kw,
+			&shm_dev->shm_ac_wake_req);
+
+	/* reset char device queues */
+	shrm_char_reset_queues(shm_dev);
+
+	/* reset protocol states */
+	shrm_common_tx_state = SHRM_SLEEP_STATE;
+	shrm_common_rx_state = SHRM_SLEEP_STATE;
+	shrm_audio_tx_state = SHRM_SLEEP_STATE;
+	shrm_audio_rx_state = SHRM_SLEEP_STATE;
+
+	/* set the msr flag */
+	shm_dev->msr_flag = 1;
+
+	/* multicast that modem is going to reset */
+	err = nl_send_multicast_message(SHRM_NL_MOD_RESET, GFP_ATOMIC);
+
+	/* reset the boot state */
+	spin_lock_irqsave(&boot_lock, flags);
+	boot_state = BOOT_INIT;
+	spin_unlock_irqrestore(&boot_lock, flags);
+
+	tasklet_enable(&shm_ac_read_0_tasklet);
+	tasklet_enable(&shm_ac_read_1_tasklet);
+	tasklet_enable(&shm_ca_0_tasklet);
+	tasklet_enable(&shm_ca_1_tasklet);
+	/* re-enable irqs */
+	enable_irq(shm_dev->ac_read_notif_0_irq);
+	enable_irq(shm_dev->ac_read_notif_1_irq);
+	enable_irq(shm_dev->ca_msg_pending_notif_0_irq);
+	enable_irq(shm_dev->ca_msg_pending_notif_1_irq);
+	enable_irq(IRQ_PRCMU_CA_WAKE);
+	enable_irq(IRQ_PRCMU_CA_SLEEP);
+
+	return err;
+}
+#endif
+
+static void shrm_modem_reset_callback(unsigned long irq)
+{
+	dev_err(shm_dev->dev, "Received mod_reset_req interrupt\n");
+
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+	{
+		int err;
+		dev_info(shm_dev->dev, "Initiating Modem silent reset\n");
+
+		err = shrm_modem_reset_sequence();
+		if (err)
+			dev_err(shm_dev->dev,
+				"Failed multicast of modem reset\n");
+	}
+#else
+	dev_info(shm_dev->dev, "Modem in reset loop, doing System reset\n");
+
+	/* Call the PRCMU reset API */
+	prcmu_system_reset(SW_RESET_NO_ARGUMENT);
+#endif
+}
+
+DECLARE_TASKLET(shrm_sw_reset_callback, shrm_modem_reset_callback,
+		IRQ_PRCMU_MODEM_SW_RESET_REQ);
+
+static irqreturn_t shrm_prcmu_irq_handler(int irq, void *data)
+{
+	struct shrm_dev *shrm = data;
+	unsigned long flags;
+
+	switch (irq) {
+	case IRQ_PRCMU_CA_WAKE:
+		suspend_block_sleep();
+		if (shrm->msr_flag)
+			atomic_set(&ac_sleep_disable_count, 0);
+		atomic_inc(&ac_sleep_disable_count);
+		queue_kthread_work(&shrm->shm_ca_wake_kw, &shrm->shm_ca_wake_req);
+		break;
+	case IRQ_PRCMU_CA_SLEEP:
+		queue_kthread_work(&shrm->shm_ca_wake_kw, &shrm->shm_ca_sleep_req);
+		break;
+	case IRQ_PRCMU_MODEM_SW_RESET_REQ:
+		/* update the boot_state */
+		spin_lock_irqsave(&boot_lock, flags);
+		boot_state = BOOT_UNKNOWN;
+
+		/*
+		 * put a barrier over here to make sure boot_state is updated
+		 * else, it is seen that some of already executing modem
+		 * irqs or tasklets fail the protocol checks and will ultimately
+		 * try to acces the modem causing system to hang.
+		 * This is particularly seen with user-space initiated modem reset
+		 */
+		wmb();
+		spin_unlock_irqrestore(&boot_lock, flags);
+
+		disable_irq_nosync(shrm->ac_read_notif_0_irq);
+		disable_irq_nosync(shrm->ac_read_notif_1_irq);
+		disable_irq_nosync(shrm->ca_msg_pending_notif_0_irq);
+		disable_irq_nosync(shrm->ca_msg_pending_notif_1_irq);
+		disable_irq_nosync(IRQ_PRCMU_CA_WAKE);
+		disable_irq_nosync(IRQ_PRCMU_CA_SLEEP);
+
+		/* stop network queue */
+		shrm_stop_netdev(shm_dev->ndev);
+
+		tasklet_schedule(&shrm_sw_reset_callback);
+		break;
+	default:
+		dev_err(shrm->dev, "%s: => IRQ %d\n", __func__, irq);
+		return IRQ_NONE;
+	}
+	return IRQ_HANDLED;
+}
+
+static void send_ac_msg_pend_notify_0_work(struct kthread_work *work)
+{
+	unsigned long flags;
+	struct shrm_dev *shrm = container_of(work, struct shrm_dev,
+			send_ac_msg_pend_notify_0);
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	update_ac_common_shared_wptr(shrm);
+
+	mutex_lock(&ac_state_mutex);
+	atomic_inc(&ac_sleep_disable_count);
+	modem_request(shrm->modem);
+	mutex_unlock(&ac_state_mutex);
+
+	spin_lock_irqsave(&start_timer_lock, flags);
+	if (!get_host_accessport_val()) {
+		dev_err(shrm->dev, "%s: host_accessport is low\n", __func__);
+		queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+				&shm_dev->shm_mod_reset_req);
+		spin_unlock_irqrestore(&start_timer_lock, flags);
+		return;
+	}
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		spin_unlock_irqrestore(&start_timer_lock, flags);
+		return;
+	}
+
+	/* Trigger AcMsgPendingNotification to CMU */
+	writel((1<<GOP_COMMON_AC_MSG_PENDING_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_SET_REGISTER_BASE);
+
+	/* timer to detect modem stuck or hang */
+	hrtimer_start(&mod_stuck_timer_0, ktime_set(MOD_STUCK_TIMEOUT, 0),
+			HRTIMER_MODE_REL);
+	spin_unlock_irqrestore(&start_timer_lock, flags);
+	if (shrm_common_tx_state == SHRM_PTR_FREE)
+		shrm_common_tx_state = SHRM_PTR_BUSY;
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+static void send_ac_msg_pend_notify_1_work(struct kthread_work *work)
+{
+	unsigned long flags;
+	struct shrm_dev *shrm = container_of(work, struct shrm_dev,
+			send_ac_msg_pend_notify_1);
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	/* Update shared_wptr with writer_local_wptr) */
+	update_ac_audio_shared_wptr(shrm);
+
+	mutex_lock(&ac_state_mutex);
+	if (!atomic_read(&ac_msg_pend_1)) {
+		atomic_inc(&ac_sleep_disable_count);
+		atomic_inc(&ac_msg_pend_1);
+	}
+	modem_request(shrm->modem);
+	mutex_unlock(&ac_state_mutex);
+
+	spin_lock_irqsave(&start_timer_lock, flags);
+	if (!get_host_accessport_val()) {
+		dev_err(shrm->dev, "%s: host_accessport is low\n", __func__);
+		queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+				&shm_dev->shm_mod_reset_req);
+		spin_unlock_irqrestore(&start_timer_lock, flags);
+		return;
+	}
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		spin_unlock_irqrestore(&start_timer_lock, flags);
+		return;
+	}
+
+	/* Trigger AcMsgPendingNotification to CMU */
+	writel((1<<GOP_AUDIO_AC_MSG_PENDING_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_SET_REGISTER_BASE);
+
+	/* timer to detect modem stuck or hang */
+	hrtimer_start(&mod_stuck_timer_1, ktime_set(MOD_STUCK_TIMEOUT, 0),
+			HRTIMER_MODE_REL);
+	spin_unlock_irqrestore(&start_timer_lock, flags);
+	if (shrm_audio_tx_state == SHRM_PTR_FREE)
+		shrm_audio_tx_state = SHRM_PTR_BUSY;
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void shm_nl_receive(struct sk_buff *skb)
+{
+	struct nlmsghdr *nlh = NULL;
+	int msg;
+
+	dev_dbg(shm_dev->dev, "Received NL msg from user-space\n");
+
+	nlh = (struct nlmsghdr *)skb->data;
+	msg = *((int *)(NLMSG_DATA(nlh)));
+	switch (msg) {
+	case SHRM_NL_MOD_QUERY_STATE:
+		dev_info(shm_dev->dev, "mod-query-state from user-space\n");
+		nl_send_unicast_message(nlh->nlmsg_pid);
+		break;
+
+	case SHRM_NL_USER_MOD_RESET:
+		dev_info(shm_dev->dev, "user-space inited mod-reset-req\n");
+		dev_info(shm_dev->dev, "PCRMU resets modem\n");
+		if (atomic_read(&mod_stuck) || atomic_read(&fifo_full)) {
+			dev_info(shm_dev->dev,
+					"Modem reset already in progress\n");
+			break;
+		}
+		atomic_set(&mod_stuck, 1);
+		queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+					&shm_dev->shm_mod_reset_req);
+		break;
+
+	default:
+		dev_err(shm_dev->dev, "Invalid NL msg from user-space\n");
+		break;
+	};
+}
+
+int shrm_protocol_init(struct shrm_dev *shrm,
+			received_msg_handler common_rx_handler,
+			received_msg_handler audio_rx_handler)
+{
+	int err;
+	struct sched_param param = { .sched_priority = MAX_RT_PRIO-1 };
+
+	shm_dev = shrm;
+	boot_state = BOOT_INIT;
+	dev_info(shrm->dev, "IPC_ISA BOOT_INIT\n");
+	rx_common_handler = common_rx_handler;
+	rx_audio_handler = audio_rx_handler;
+	atomic_set(&ac_sleep_disable_count, 0);
+
+	hrtimer_init(&timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	timer.function = callback;
+	hrtimer_init(&mod_stuck_timer_0, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	mod_stuck_timer_0.function = shm_mod_stuck_timeout;
+	hrtimer_init(&mod_stuck_timer_1, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	mod_stuck_timer_1.function = shm_mod_stuck_timeout;
+	hrtimer_init(&fifo_full_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	fifo_full_timer.function = shm_fifo_full_timeout;
+
+	init_kthread_worker(&shrm->shm_common_ch_wr_kw);
+	shrm->shm_common_ch_wr_kw_task = kthread_run(kthread_worker_fn,
+						     &shrm->shm_common_ch_wr_kw,
+						     "shm_common_channel_irq");
+	if (IS_ERR(shrm->shm_common_ch_wr_kw_task)) {
+		dev_err(shrm->dev, "failed to create work task\n");
+		return -ENOMEM;
+	}
+
+	init_kthread_worker(&shrm->shm_audio_ch_wr_kw);
+	shrm->shm_audio_ch_wr_kw_task = kthread_run(kthread_worker_fn,
+						    &shrm->shm_audio_ch_wr_kw,
+						    "shm_audio_channel_irq");
+	if (IS_ERR(shrm->shm_audio_ch_wr_kw_task)) {
+		dev_err(shrm->dev, "failed to create work task\n");
+		err = -ENOMEM;
+		goto free_kw1;
+	}
+	/* must use the FIFO scheduler as it is realtime sensitive */
+	sched_setscheduler(shrm->shm_audio_ch_wr_kw_task, SCHED_FIFO, &param);
+
+	init_kthread_worker(&shrm->shm_ac_wake_kw);
+	shrm->shm_ac_wake_kw_task = kthread_run(kthread_worker_fn,
+						&shrm->shm_ac_wake_kw,
+						"shm_ac_wake_req");
+	if (IS_ERR(shrm->shm_ac_wake_kw_task)) {
+		dev_err(shrm->dev, "failed to create work task\n");
+		err = -ENOMEM;
+		goto free_kw2;
+	}
+	/* must use the FIFO scheduler as it is realtime sensitive */
+	sched_setscheduler(shrm->shm_ac_wake_kw_task, SCHED_FIFO, &param);
+
+	init_kthread_worker(&shrm->shm_ca_wake_kw);
+	shrm->shm_ca_wake_kw_task = kthread_run(kthread_worker_fn,
+						&shrm->shm_ca_wake_kw,
+						"shm_ca_wake_req");
+	if (IS_ERR(shrm->shm_ca_wake_kw_task)) {
+		dev_err(shrm->dev, "failed to create work task\n");
+		err = -ENOMEM;
+		goto free_kw3;
+	}
+	/* must use the FIFO scheduler as it is realtime sensitive */
+	sched_setscheduler(shrm->shm_ca_wake_kw_task, SCHED_FIFO, &param);
+
+	init_kthread_worker(&shrm->shm_ac_sleep_kw);
+	shrm->shm_ac_sleep_kw_task = kthread_run(kthread_worker_fn,
+						 &shrm->shm_ac_sleep_kw,
+						 "shm_ac_sleep_req");
+	if (IS_ERR(shrm->shm_ac_sleep_kw_task)) {
+		dev_err(shrm->dev, "failed to create work task\n");
+		err = -ENOMEM;
+		goto free_kw4;
+	}
+	init_kthread_worker(&shrm->shm_mod_stuck_kw);
+	shrm->shm_mod_stuck_kw_task = kthread_run(kthread_worker_fn,
+						     &shrm->shm_mod_stuck_kw,
+						     "shm_mod_reset_req");
+	if (IS_ERR(shrm->shm_mod_stuck_kw_task)) {
+		dev_err(shrm->dev, "failed to create work task\n");
+		err = -ENOMEM;
+		goto free_kw5;
+	}
+
+	init_kthread_work(&shrm->send_ac_msg_pend_notify_0,
+			  send_ac_msg_pend_notify_0_work);
+	init_kthread_work(&shrm->send_ac_msg_pend_notify_1,
+			  send_ac_msg_pend_notify_1_work);
+	init_kthread_work(&shrm->shm_ca_wake_req, shm_ca_wake_req_work);
+	init_kthread_work(&shrm->shm_ca_sleep_req, shm_ca_sleep_req_work);
+	init_kthread_work(&shrm->shm_ac_sleep_req, shm_ac_sleep_req_work);
+	init_kthread_work(&shrm->shm_ac_wake_req, shm_ac_wake_req_work);
+	init_kthread_work(&shrm->shm_mod_reset_req, shm_mod_reset_req_work);
+	init_kthread_work(&shrm->shm_print_dbg_info, shm_print_dbg_info_work);
+
+	/* set tasklet data */
+	shm_ca_0_tasklet.data = (unsigned long)shrm;
+	shm_ca_1_tasklet.data = (unsigned long)shrm;
+
+	err = request_irq(IRQ_PRCMU_CA_SLEEP, shrm_prcmu_irq_handler,
+			IRQF_NO_SUSPEND, "ca-sleep", shrm);
+	if (err < 0) {
+		dev_err(shm_dev->dev, "Failed alloc IRQ_PRCMU_CA_SLEEP.\n");
+		goto free_kw6;
+	}
+
+	err = request_irq(IRQ_PRCMU_CA_WAKE, shrm_prcmu_irq_handler,
+		IRQF_NO_SUSPEND, "ca-wake", shrm);
+	if (err < 0) {
+		dev_err(shm_dev->dev, "Failed alloc IRQ_PRCMU_CA_WAKE.\n");
+		goto drop2;
+	}
+
+	err = request_irq(IRQ_PRCMU_MODEM_SW_RESET_REQ, shrm_prcmu_irq_handler,
+			IRQF_NO_SUSPEND, "modem-sw-reset-req", shrm);
+	if (err < 0) {
+		dev_err(shm_dev->dev,
+				"Failed alloc IRQ_PRCMU_MODEM_SW_RESET_REQ.\n");
+		goto drop1;
+	}
+
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+	/* init netlink socket for user-space communication */
+	shrm_nl_sk = netlink_kernel_create(NULL, NETLINK_SHRM, 1,
+			shm_nl_receive, NULL, THIS_MODULE);
+
+	if (!shrm_nl_sk) {
+		dev_err(shm_dev->dev, "netlink socket creation failed\n");
+		goto drop;
+	}
+#endif
+	return 0;
+
+#ifdef CONFIG_U8500_SHRM_MODEM_SILENT_RESET
+drop:
+	free_irq(IRQ_PRCMU_MODEM_SW_RESET_REQ, NULL);
+#endif
+drop1:
+	free_irq(IRQ_PRCMU_CA_WAKE, NULL);
+drop2:
+	free_irq(IRQ_PRCMU_CA_SLEEP, NULL);
+free_kw6:
+	kthread_stop(shrm->shm_mod_stuck_kw_task);
+free_kw5:
+	kthread_stop(shrm->shm_ac_sleep_kw_task);
+free_kw4:
+	kthread_stop(shrm->shm_ca_wake_kw_task);
+free_kw3:
+	kthread_stop(shrm->shm_ac_wake_kw_task);
+free_kw2:
+	kthread_stop(shrm->shm_audio_ch_wr_kw_task);
+free_kw1:
+	kthread_stop(shrm->shm_common_ch_wr_kw_task);
+	return err;
+}
+
+void shrm_protocol_deinit(struct shrm_dev *shrm)
+{
+	free_irq(IRQ_PRCMU_CA_SLEEP, NULL);
+	free_irq(IRQ_PRCMU_CA_WAKE, NULL);
+	free_irq(IRQ_PRCMU_MODEM_SW_RESET_REQ, NULL);
+	flush_kthread_worker(&shrm->shm_common_ch_wr_kw);
+	flush_kthread_worker(&shrm->shm_audio_ch_wr_kw);
+	flush_kthread_worker(&shrm->shm_ac_wake_kw);
+	flush_kthread_worker(&shrm->shm_ca_wake_kw);
+	flush_kthread_worker(&shrm->shm_ac_sleep_kw);
+	flush_kthread_worker(&shrm->shm_mod_stuck_kw);
+	kthread_stop(shrm->shm_common_ch_wr_kw_task);
+	kthread_stop(shrm->shm_audio_ch_wr_kw_task);
+	kthread_stop(shrm->shm_ac_wake_kw_task);
+	kthread_stop(shrm->shm_ca_wake_kw_task);
+	kthread_stop(shrm->shm_ac_sleep_kw_task);
+	kthread_stop(shrm->shm_mod_stuck_kw_task);
+	modem_put(shrm->modem);
+}
+
+int get_ca_wake_req_state(void)
+{
+	return ((atomic_read(&ac_sleep_disable_count) > 0) ||
+			modem_get_usage(shm_dev->modem));
+}
+
+irqreturn_t ca_wake_irq_handler(int irq, void *ctrlr)
+{
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	/* initialize the FIFO Variables */
+	if (boot_state == BOOT_INIT)
+		shm_fifo_init(shrm);
+
+	dev_dbg(shrm->dev, "Inside ca_wake_irq_handler\n");
+
+	/* Clear the interrupt */
+	writel((1 << GOP_CA_WAKE_REQ_BIT),
+				shrm->intr_base + GOP_CLEAR_REGISTER_BASE);
+
+	/* send ca_wake_ack_interrupt to CMU */
+	writel((1 << GOP_CA_WAKE_ACK_BIT),
+		shrm->intr_base + GOP_SET_REGISTER_BASE);
+
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+}
+
+
+irqreturn_t ac_read_notif_0_irq_handler(int irq, void *ctrlr)
+{
+	unsigned long flags;
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+	/* Cancel the modem stuck timer */
+	spin_lock_irqsave(&start_timer_lock, flags);
+	hrtimer_cancel(&mod_stuck_timer_0);
+	spin_unlock_irqrestore(&start_timer_lock, flags);
+	if (atomic_read(&fifo_full)) {
+		atomic_set(&fifo_full, 0);
+		hrtimer_cancel(&fifo_full_timer);
+	}
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	shm_ac_read_0_tasklet.data = (unsigned long)shrm;
+	tasklet_schedule(&shm_ac_read_0_tasklet);
+
+	local_irq_save(flags);
+	preempt_disable();
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		preempt_enable();
+		local_irq_restore(flags);
+		return IRQ_HANDLED;
+	}
+
+	if (!get_host_accessport_val()) {
+		dev_err(shrm->dev, "%s: host_accessport is low\n", __func__);
+		queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+				&shm_dev->shm_mod_reset_req);
+		preempt_enable();
+		local_irq_restore(flags);
+		return IRQ_HANDLED;
+	}
+	/* Clear the interrupt */
+	writel((1 << GOP_COMMON_AC_READ_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_CLEAR_REGISTER_BASE);
+	preempt_enable();
+	local_irq_restore(flags);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+}
+
+irqreturn_t ac_read_notif_1_irq_handler(int irq, void *ctrlr)
+{
+	unsigned long flags;
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN+\n", __func__);
+	/* Cancel the modem stuck timer */
+	spin_lock_irqsave(&start_timer_lock, flags);
+	hrtimer_cancel(&mod_stuck_timer_1);
+	spin_unlock_irqrestore(&start_timer_lock, flags);
+	if (atomic_read(&fifo_full)) {
+		atomic_set(&fifo_full, 0);
+		hrtimer_cancel(&fifo_full_timer);
+	}
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	shm_ac_read_1_tasklet.data = (unsigned long)shrm;
+	tasklet_schedule(&shm_ac_read_1_tasklet);
+
+	local_irq_save(flags);
+	preempt_disable();
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		preempt_enable();
+		local_irq_restore(flags);
+		return IRQ_HANDLED;
+	}
+
+	if (!get_host_accessport_val()) {
+		dev_err(shrm->dev, "%s: host_accessport is low\n", __func__);
+		queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+				&shm_dev->shm_mod_reset_req);
+		preempt_enable();
+		local_irq_restore(flags);
+		return IRQ_HANDLED;
+	}
+	/* Clear the interrupt */
+	writel((1 << GOP_AUDIO_AC_READ_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_CLEAR_REGISTER_BASE);
+	preempt_enable();
+	local_irq_restore(flags);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+}
+
+irqreturn_t ca_msg_pending_notif_0_irq_handler(int irq, void *ctrlr)
+{
+	unsigned long flags;
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	tasklet_schedule(&shm_ca_0_tasklet);
+
+	local_irq_save(flags);
+	preempt_disable();
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		preempt_enable();
+		local_irq_restore(flags);
+		return IRQ_HANDLED;
+	}
+
+	if (!get_host_accessport_val()) {
+		dev_err(shrm->dev, "%s: host_accessport is low\n", __func__);
+		queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+				&shm_dev->shm_mod_reset_req);
+		preempt_enable();
+		local_irq_restore(flags);
+		return IRQ_HANDLED;
+	}
+	/* Clear the interrupt */
+	writel((1 << GOP_COMMON_CA_MSG_PENDING_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_CLEAR_REGISTER_BASE);
+	preempt_enable();
+	local_irq_restore(flags);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+}
+
+irqreturn_t ca_msg_pending_notif_1_irq_handler(int irq, void *ctrlr)
+{
+	unsigned long flags;
+	struct shrm_dev *shrm = ctrlr;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return IRQ_HANDLED;
+	}
+
+	tasklet_schedule(&shm_ca_1_tasklet);
+
+	local_irq_save(flags);
+	preempt_disable();
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		preempt_enable();
+		local_irq_restore(flags);
+		return IRQ_HANDLED;
+	}
+
+	if (!get_host_accessport_val()) {
+		dev_err(shrm->dev, "%s: host_accessport is low\n", __func__);
+		queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+				&shm_dev->shm_mod_reset_req);
+		preempt_enable();
+		local_irq_restore(flags);
+		return IRQ_HANDLED;
+	}
+	/* Clear the interrupt */
+	writel((1<<GOP_AUDIO_CA_MSG_PENDING_NOTIFICATION_BIT),
+			shrm->intr_base+GOP_CLEAR_REGISTER_BASE);
+	preempt_enable();
+	local_irq_restore(flags);
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return IRQ_HANDLED;
+
+}
+
+/**
+ * shm_write_msg() - write message to shared memory
+ * @shrm:	pointer to the shrm device information structure
+ * @l2_header:	L2 header
+ * @addr:	pointer to the message
+ * @length:	length of the message to be written
+ *
+ * This function is called from net or char interface driver write operation.
+ * Prior to calling this function the message is copied from the user space
+ * buffer to the kernel buffer. This function based on the l2 header routes
+ * the message to the respective channel and FIFO. Then makes a call to the
+ * fifo write function where the message is written to the physical device.
+ */
+int shm_write_msg(struct shrm_dev *shrm, u8 l2_header,
+					void *addr, u32 length)
+{
+	u8 channel = 0;
+	int ret;
+
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (boot_state != BOOT_DONE) {
+		dev_err(shrm->dev,
+			"error:after boot done  call this fn, L2Header = %d\n",
+			l2_header);
+		ret = -ENODEV;
+		goto out;
+	}
+
+	if ((l2_header == L2_HEADER_ISI) ||
+			(l2_header == L2_HEADER_RPC) ||
+			(l2_header == L2_HEADER_SECURITY) ||
+			(l2_header == L2_HEADER_COMMON_SIMPLE_LOOPBACK) ||
+			(l2_header == L2_HEADER_COMMON_ADVANCED_LOOPBACK) ||
+			(l2_header == L2_HEADER_CIQ) ||
+			(l2_header == L2_HEADER_RTC_CALIBRATION)) {
+		channel = 0;
+		if (shrm_common_tx_state == SHRM_SLEEP_STATE)
+			shrm_common_tx_state = SHRM_PTR_FREE;
+		else if (shrm_common_tx_state == SHRM_IDLE)
+			shrm_common_tx_state = SHRM_PTR_FREE;
+
+	} else if ((l2_header == L2_HEADER_AUDIO) ||
+			(l2_header == L2_HEADER_AUDIO_SIMPLE_LOOPBACK) ||
+			(l2_header == L2_HEADER_AUDIO_ADVANCED_LOOPBACK)) {
+		if (shrm_audio_tx_state == SHRM_SLEEP_STATE)
+			shrm_audio_tx_state = SHRM_PTR_FREE;
+		else if (shrm_audio_tx_state == SHRM_IDLE)
+			shrm_audio_tx_state = SHRM_PTR_FREE;
+
+		channel = 1;
+	} else {
+		ret = -ENODEV;
+		goto out;
+	}
+	ret = shm_write_msg_to_fifo(shrm, channel, l2_header, addr, length);
+	if (ret < 0) {
+		dev_err(shrm->dev, "write message to fifo failed\n");
+		if (ret == -EAGAIN) {
+			if (!atomic_read(&fifo_full)) {
+				/* Start a timer so as to handle this gently */
+				atomic_set(&fifo_full, 1);
+				hrtimer_start(&fifo_full_timer, ktime_set(
+						FIFO_FULL_TIMEOUT, 0),
+						HRTIMER_MODE_REL);
+			}
+		}
+		return ret;
+	}
+	/*
+	 * notify only if new msg copied is the only unread one
+	 * otherwise it means that reading process is ongoing
+	 */
+	if (is_the_only_one_unread_message(shrm, channel, length)) {
+
+		/* Send Message Pending Noitication to CMT */
+		if (channel == 0)
+			queue_kthread_work(&shrm->shm_common_ch_wr_kw,
+					&shrm->send_ac_msg_pend_notify_0);
+		else
+			queue_kthread_work(&shrm->shm_audio_ch_wr_kw,
+					&shrm->send_ac_msg_pend_notify_1);
+
+	}
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+	return 0;
+
+out:
+	return ret;
+}
+
+void ca_msg_read_notification_0(struct shrm_dev *shrm)
+{
+	unsigned long flags;
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (get_ca_msg_0_read_notif_send() == 0) {
+		update_ca_common_shared_rptr(shrm);
+
+		local_irq_save(flags);
+		preempt_disable();
+		if (check_modem_in_reset()) {
+			dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+					__func__);
+			preempt_enable();
+			local_irq_restore(flags);
+			return;
+		}
+
+		if (!get_host_accessport_val()) {
+			dev_err(shrm->dev, "%s: host_accessport is low\n",
+					__func__);
+			queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+					&shm_dev->shm_mod_reset_req);
+			preempt_enable();
+			local_irq_restore(flags);
+			return;
+		}
+		/* Trigger CaMsgReadNotification to CMU */
+		writel((1 << GOP_COMMON_CA_READ_NOTIFICATION_BIT),
+			shrm->intr_base + GOP_SET_REGISTER_BASE);
+		preempt_enable();
+		local_irq_restore(flags);
+		set_ca_msg_0_read_notif_send(1);
+		shrm_common_rx_state = SHRM_PTR_BUSY;
+	}
+
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+void ca_msg_read_notification_1(struct shrm_dev *shrm)
+{
+	unsigned long flags;
+	dev_dbg(shrm->dev, "%s IN\n", __func__);
+
+	if (get_ca_msg_1_read_notif_send() == 0) {
+		update_ca_audio_shared_rptr(shrm);
+
+		local_irq_save(flags);
+		preempt_disable();
+		if (check_modem_in_reset()) {
+			dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+					__func__);
+			preempt_enable();
+			local_irq_restore(flags);
+			return;
+		}
+
+		if (!get_host_accessport_val()) {
+			dev_err(shrm->dev, "%s: host_accessport is low\n",
+					__func__);
+			queue_kthread_work(&shm_dev->shm_mod_stuck_kw,
+					&shm_dev->shm_mod_reset_req);
+			preempt_enable();
+			local_irq_restore(flags);
+			return;
+		}
+		/* Trigger CaMsgReadNotification to CMU */
+		writel((1<<GOP_AUDIO_CA_READ_NOTIFICATION_BIT),
+			shrm->intr_base+GOP_SET_REGISTER_BASE);
+		preempt_enable();
+		local_irq_restore(flags);
+		set_ca_msg_1_read_notif_send(1);
+		shrm_audio_rx_state = SHRM_PTR_BUSY;
+	}
+	dev_dbg(shrm->dev, "%s OUT\n", __func__);
+}
+
+/**
+ * receive_messages_common - receive common channnel msg from
+ * CMT(Cellular Mobile Terminal)
+ * @shrm:	pointer to shrm device information structure
+ *
+ * The messages sent from CMT to APE are written to the respective FIFO
+ * and an interrupt is triggered by the CMT. This ca message pending
+ * interrupt calls this function. This function sends a read notification
+ * acknowledgement to the CMT and calls the common channel receive handler
+ * where the messsage is copied to the respective(ISI, RPC, SECURIT) queue
+ * based on the message l2 header.
+ */
+void receive_messages_common(struct shrm_dev *shrm)
+{
+	u8 l2_header;
+	u32 len;
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return;
+	}
+
+	l2_header = read_one_l2msg_common(shrm, recieve_common_msg, &len);
+	/* Send Recieve_Call_back to Upper Layer */
+	if (!rx_common_handler) {
+		dev_err(shrm->dev, "common_rx_handler is Null\n");
+		BUG();
+	}
+	(*rx_common_handler)(l2_header, &recieve_common_msg, len,
+					shrm);
+	/* SendReadNotification */
+	ca_msg_read_notification_0(shrm);
+
+	while (read_remaining_messages_common()) {
+		if (check_modem_in_reset()) {
+			dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+					__func__);
+			return;
+		}
+
+		l2_header = read_one_l2msg_common(shrm, recieve_common_msg,
+								&len);
+		/* Send Recieve_Call_back to Upper Layer */
+		(*rx_common_handler)(l2_header,
+					&recieve_common_msg, len,
+					shrm);
+	}
+}
+
+/**
+ * receive_messages_audio() - receive audio message from CMT
+ * @shrm:	pointer to shrm device information structure
+ *
+ * The messages sent from CMT to APE are written to the respective FIFO
+ * and an interrupt is triggered by the CMT. This ca message pending
+ * interrupt calls this function. This function sends a read notification
+ * acknowledgement to the CMT and calls the common channel receive handler
+ * where the messsage is copied to the audio queue.
+ */
+void receive_messages_audio(struct shrm_dev *shrm)
+{
+	u8 l2_header;
+	u32 len;
+
+	if (check_modem_in_reset()) {
+		dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+				__func__);
+		return;
+	}
+
+	l2_header = read_one_l2msg_audio(shrm, recieve_audio_msg, &len);
+	/* Send Recieve_Call_back to Upper Layer */
+
+	if (!rx_audio_handler) {
+		dev_crit(shrm->dev, "audio_rx_handler is Null\n");
+		BUG();
+	}
+	(*rx_audio_handler)(l2_header, &recieve_audio_msg,
+					len, shrm);
+
+	/* SendReadNotification */
+	ca_msg_read_notification_1(shrm);
+	while (read_remaining_messages_audio())	{
+		if (check_modem_in_reset()) {
+			dev_err(shrm->dev, "%s:Modem state reset or unknown.\n",
+					__func__);
+			return;
+		}
+
+		l2_header = read_one_l2msg_audio(shrm,
+						recieve_audio_msg, &len);
+		/* Send Recieve_Call_back to Upper Layer */
+		(*rx_audio_handler)(l2_header,
+					&recieve_audio_msg, len,
+					shrm);
+	}
+}
+
+u8 get_boot_state()
+{
+	return boot_state;
+}
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index a6b8ce11a22..a0f2484368b 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -70,3 +70,8 @@ obj-$(CONFIG_USB_IPHETH)        += usb/
 obj-$(CONFIG_USB_CDC_PHONET)   += usb/
 
 obj-$(CONFIG_HYPERV_NET) += hyperv/
+
+ifdef CONFIG_PHONET
+obj-$(CONFIG_U8500_SHRM) += u8500_shrm.o
+obj-$(CONFIG_MODEM_M6718_SPI) += m6718_modem_net.o
+endif
diff --git a/drivers/net/caif/caif_serial.c b/drivers/net/caif/caif_serial.c
index 8a3054b8481..957363ceae4 100644
--- a/drivers/net/caif/caif_serial.c
+++ b/drivers/net/caif/caif_serial.c
@@ -182,6 +182,7 @@ static void ldisc_receive(struct tty_struct *tty, const u8 *data,
 	 * This is not yet handled.
 	 */
 
+	BUG_ON(ser->dev == NULL);
 
 	/*
 	 * Workaround for garbage at start of transmission,
diff --git a/drivers/net/caif/caif_shmcore.c b/drivers/net/caif/caif_shmcore.c
index 5b2041319a3..fc55bf65f1c 100644
--- a/drivers/net/caif/caif_shmcore.c
+++ b/drivers/net/caif/caif_shmcore.c
@@ -13,6 +13,7 @@
 #include <linux/list.h>
 #include <linux/netdevice.h>
 #include <linux/if_arp.h>
+#include <linux/kthread.h>
 
 #include <net/caif/caif_device.h>
 #include <net/caif/caif_shm.h>
@@ -107,8 +108,13 @@ struct shmdrv_layer {
 	struct workqueue_struct *pshm_tx_workqueue;
 	struct workqueue_struct *pshm_rx_workqueue;
 
+	struct kthread_worker pshm_flow_ctrl_kw;
+	struct task_struct *pshm_flow_ctrl_kw_task;
+
 	struct work_struct shm_tx_work;
 	struct work_struct shm_rx_work;
+	struct kthread_work shm_flow_on_work;
+	struct kthread_work shm_flow_off_work;
 
 	struct sk_buff_head sk_qhead;
 	struct shmdev_layer *pshm_dev;
@@ -126,6 +132,24 @@ static int shm_netdev_close(struct net_device *shm_netdev)
 	return 0;
 }
 
+static void shm_flow_on_work_func(struct kthread_work *work)
+{
+	struct shmdrv_layer *pshm_drv = container_of(work, struct shmdrv_layer, shm_flow_on_work);
+
+	pshm_drv->cfdev.flowctrl
+			(pshm_drv->pshm_dev->pshm_netdev,
+			CAIF_FLOW_ON);
+}
+
+static void shm_flow_off_work_func(struct kthread_work *work)
+{
+	struct shmdrv_layer *pshm_drv = container_of(work, struct shmdrv_layer, shm_flow_off_work);
+
+	pshm_drv->cfdev.flowctrl
+			(pshm_drv->pshm_dev->pshm_netdev,
+			CAIF_FLOW_OFF);
+}
+
 int caif_shmdrv_rx_cb(u32 mbx_msg, void *priv)
 {
 	struct buf_list *pbuf;
@@ -238,11 +262,9 @@ int caif_shmdrv_rx_cb(u32 mbx_msg, void *priv)
 		if ((avail_emptybuff > HIGH_WATERMARK) &&
 					(!pshm_drv->tx_empty_available)) {
 			pshm_drv->tx_empty_available = 1;
+			queue_kthread_work(&pshm_drv->pshm_flow_ctrl_kw,
+					&pshm_drv->shm_flow_on_work);
 			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-			pshm_drv->cfdev.flowctrl
-					(pshm_drv->pshm_dev->pshm_netdev,
-								CAIF_FLOW_ON);
-
 
 			/* Schedule the work queue. if required */
 			if (!work_pending(&pshm_drv->shm_tx_work))
@@ -426,11 +448,8 @@ static void shm_tx_work_func(struct work_struct *tx_work)
 					pshm_drv->tx_empty_available) {
 			/* Update blocking condition. */
 			pshm_drv->tx_empty_available = 0;
-			spin_unlock_irqrestore(&pshm_drv->lock, flags);
-			pshm_drv->cfdev.flowctrl
-					(pshm_drv->pshm_dev->pshm_netdev,
-					CAIF_FLOW_OFF);
-			spin_lock_irqsave(&pshm_drv->lock, flags);
+			queue_kthread_work(&pshm_drv->pshm_flow_ctrl_kw,
+					&pshm_drv->shm_flow_off_work);
 		}
 		/*
 		 * We simply return back to the caller if we do not have space
@@ -503,7 +522,8 @@ static void shm_tx_work_func(struct work_struct *tx_work)
 			pbuf->frames++;
 			pbuf->frm_ofs += frmlen + (frmlen % 32);
 
-		} while (pbuf->frames < SHM_MAX_FRMS_PER_BUF);
+		} while (pbuf->frames < SHM_MAX_FRMS_PER_BUF &&
+				pbuf->frm_ofs < pbuf->len);
 
 		/* Assign buffer as full. */
 		list_add_tail(&pbuf->list, &pshm_drv->tx_full_list);
@@ -562,6 +582,7 @@ int caif_shmcore_probe(struct shmdev_layer *pshm_dev)
 {
 	int result, j;
 	struct shmdrv_layer *pshm_drv = NULL;
+	struct sched_param param = { .sched_priority = MAX_RT_PRIO-1 };
 
 	pshm_dev->pshm_netdev = alloc_netdev(sizeof(struct shmdrv_layer),
 						"cfshm%d", shm_netdev_setup);
@@ -622,11 +643,20 @@ int caif_shmcore_probe(struct shmdev_layer *pshm_dev)
 	INIT_WORK(&pshm_drv->shm_tx_work, shm_tx_work_func);
 	INIT_WORK(&pshm_drv->shm_rx_work, shm_rx_work_func);
 
+	init_kthread_work(&pshm_drv->shm_flow_on_work, shm_flow_on_work_func);
+	init_kthread_work(&pshm_drv->shm_flow_off_work, shm_flow_off_work_func);
+
 	pshm_drv->pshm_tx_workqueue =
 				create_singlethread_workqueue("shm_tx_work");
 	pshm_drv->pshm_rx_workqueue =
 				create_singlethread_workqueue("shm_rx_work");
 
+	init_kthread_worker(&pshm_drv->pshm_flow_ctrl_kw);
+	pshm_drv->pshm_flow_ctrl_kw_task = kthread_run(kthread_worker_fn,
+			&pshm_drv->pshm_flow_ctrl_kw, "pshm_caif_flow_ctrl");
+	/* must use the FIFO scheduler as it is realtime sensitive */
+	sched_setscheduler(pshm_drv->pshm_flow_ctrl_kw_task, SCHED_FIFO, &param);
+
 	for (j = 0; j < NR_TX_BUF; j++) {
 		struct buf_list *tx_buf =
 				kmalloc(sizeof(struct buf_list), GFP_KERNEL);
@@ -744,6 +774,8 @@ void caif_shmcore_remove(struct net_device *pshm_netdev)
 	/* Destroy work queues. */
 	destroy_workqueue(pshm_drv->pshm_tx_workqueue);
 	destroy_workqueue(pshm_drv->pshm_rx_workqueue);
+	flush_kthread_worker(&pshm_drv->pshm_flow_ctrl_kw);
+	kthread_stop(pshm_drv->pshm_flow_ctrl_kw_task);
 
 	unregister_netdev(pshm_netdev);
 }
diff --git a/drivers/net/ethernet/smsc/smsc911x.c b/drivers/net/ethernet/smsc/smsc911x.c
index cd3defb11ff..edbf830a23f 100644
--- a/drivers/net/ethernet/smsc/smsc911x.c
+++ b/drivers/net/ethernet/smsc/smsc911x.c
@@ -33,6 +33,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/crc32.h>
+#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/errno.h>
 #include <linux/etherdevice.h>
@@ -144,6 +145,9 @@ struct smsc911x_data {
 
 	/* regulators */
 	struct regulator_bulk_data supplies[SMSC911X_NUM_SUPPLIES];
+
+	/* clock */
+	struct clk *fsmc_clk;
 };
 
 /* Easy access to information */
@@ -369,7 +373,7 @@ out:
 }
 
 /*
- * enable resources, currently just regulators.
+ * enable resources, regulators & clocks.
  */
 static int smsc911x_enable_resources(struct platform_device *pdev)
 {
@@ -379,9 +383,17 @@ static int smsc911x_enable_resources(struct platform_device *pdev)
 
 	ret = regulator_bulk_enable(ARRAY_SIZE(pdata->supplies),
 			pdata->supplies);
-	if (ret)
+	if (ret) {
 		netdev_err(ndev, "failed to enable regulators %d\n",
 				ret);
+		return ret;
+	}
+
+	if (pdata->fsmc_clk) {
+		ret = clk_enable(pdata->fsmc_clk);
+		if (ret < 0)
+			netdev_err(ndev, "failed to enable clock %d\n", ret);
+	}
 	return ret;
 }
 
@@ -396,6 +408,8 @@ static int smsc911x_disable_resources(struct platform_device *pdev)
 
 	ret = regulator_bulk_disable(ARRAY_SIZE(pdata->supplies),
 			pdata->supplies);
+	if (pdata->fsmc_clk)
+		clk_disable(pdata->fsmc_clk);
 	return ret;
 }
 
@@ -418,9 +432,17 @@ static int smsc911x_request_resources(struct platform_device *pdev)
 	ret = regulator_bulk_get(&pdev->dev,
 			ARRAY_SIZE(pdata->supplies),
 			pdata->supplies);
-	if (ret)
-		netdev_err(ndev, "couldn't get regulators %d\n",
-				ret);
+	if (ret) {
+		netdev_err(ndev, "couldn't get regulators %d\n", ret);
+		return ret;
+	}
+
+	/* Request clock, ignore if not here */
+	pdata->fsmc_clk = clk_get(NULL, "fsmc");
+	if (IS_ERR(pdata->fsmc_clk)) {
+		netdev_warn(ndev, "couldn't get clock %d\n", ret);
+		pdata->fsmc_clk = NULL;
+	}
 	return ret;
 }
 
@@ -436,6 +458,12 @@ static void smsc911x_free_resources(struct platform_device *pdev)
 	/* Free regulators */
 	regulator_bulk_free(ARRAY_SIZE(pdata->supplies),
 			pdata->supplies);
+
+	/* Free clock */
+	if (pdata->fsmc_clk) {
+		clk_put(pdata->fsmc_clk);
+		pdata->fsmc_clk = NULL;
+	}
 }
 
 /* waits for MAC not busy, with timeout.  Only called by smsc911x_mac_read
@@ -2343,6 +2371,7 @@ static int __devinit smsc911x_drv_probe(struct platform_device *pdev)
 	unsigned int intcfg = 0;
 	int res_size, irq_flags;
 	int retval;
+	int to = 100;
 
 	pr_info("Driver version %s\n", SMSC_DRV_VERSION);
 
@@ -2419,6 +2448,18 @@ static int __devinit smsc911x_drv_probe(struct platform_device *pdev)
 	if (pdata->config.shift)
 		pdata->ops = &shifted_smsc911x_ops;
 
+	/* poll the READY bit in PMT_CTRL. Any other access to the device is
+	 * forbidden while this bit isn't set. Try for 100ms
+	 */
+	while (!(smsc911x_reg_read(pdata, PMT_CTRL) & PMT_CTRL_READY_) && --to)
+		udelay(1000);
+
+	if (to == 0) {
+		pr_err("Device not READY in 100ms aborting\n");
+		goto out_0;
+	}
+
+
 	retval = smsc911x_init(dev);
 	if (retval < 0)
 		goto out_disable_resources;
diff --git a/drivers/net/m6718_modem_net.c b/drivers/net/m6718_modem_net.c
new file mode 100644
index 00000000000..f64a775560b
--- /dev/null
+++ b/drivers/net/m6718_modem_net.c
@@ -0,0 +1,333 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
+ *   based on u8500_shrm.c
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * M6718 modem net device interface.
+ */
+#include <linux/if_ether.h>
+#include <linux/netdevice.h>
+#include <linux/phonet.h>
+#include <linux/if_phonet.h>
+#include <linux/if_arp.h>
+#include <net/sock.h>
+#include <net/phonet/phonet.h>
+#include <net/phonet/pep.h>
+#include <linux/modem/m6718_spi/modem_net.h>
+#include <linux/modem/m6718_spi/modem_driver.h>
+#include <linux/modem/m6718_spi/modem_char.h>
+#include <linux/ratelimit.h>
+
+
+/**
+ * modem_net_receive() - receive data and copy to user space buffer
+ * @dev: pointer to the network device structure
+ *
+ * Copy data from ISI queue to the user space buffer.
+ */
+int modem_net_receive(struct net_device *dev)
+{
+	struct sk_buff *skb;
+	struct isa_device_context *isadev;
+	struct message_queue *q;
+	u32 msgsize;
+	u32 size = 0;
+	struct modem_spi_net_dev *net_iface_priv =
+		(struct modem_spi_net_dev *)netdev_priv(dev);
+	struct modem_spi_dev *modem_spi_dev = net_iface_priv->modem_spi_dev;
+
+	isadev = &modem_spi_dev->isa_context->isadev[MODEM_M6718_SPI_CHN_ISI];
+	q = &isadev->dl_queue;
+
+	spin_lock_bh(&q->update_lock);
+	if (list_empty(&q->msg_list)) {
+		spin_unlock_bh(&q->update_lock);
+		dev_dbg(modem_spi_dev->dev, "empty queue!\n");
+		return 0;
+	}
+	spin_unlock_bh(&q->update_lock);
+
+	msgsize = modem_isa_msg_size(q);
+	if (msgsize <= 0)
+		return msgsize;
+
+	/*
+	 * The packet has been retrieved from the transmission
+	 * medium. Build an skb around it, so upper layers can handle it
+	 */
+	skb = dev_alloc_skb(msgsize);
+	if (!skb) {
+		pr_notice_ratelimited("isa rx: low on mem - packet dropped\n");
+		dev->stats.rx_dropped++;
+		return -ENOMEM;
+	}
+
+	if ((q->readptr + msgsize) >= q->size) {
+		size = (q->size - q->readptr);
+		/* copy first part of msg */
+		skb_copy_to_linear_data(skb,
+			(u8 *)(q->fifo_base + q->readptr), size);
+		skb_put(skb, size);
+
+		/* copy second part of msg at the top of fifo */
+		skb_copy_to_linear_data_offset(skb, size,
+				(u8 *)(q->fifo_base), (msgsize - size));
+		skb_put(skb, msgsize - size);
+
+	} else {
+		skb_copy_to_linear_data(skb,
+				(u8 *)(q->fifo_base + q->readptr), msgsize);
+		skb_put(skb, msgsize);
+	}
+
+	spin_lock_bh(&q->update_lock);
+	modem_isa_unqueue_msg(q);
+	spin_unlock_bh(&q->update_lock);
+
+	skb_reset_mac_header(skb);
+	__skb_pull(skb, dev->hard_header_len);
+	/* write metadata and then pass to the receive level */
+	skb->dev = dev;
+	skb->protocol = htons(ETH_P_PHONET);
+	skb->priority = 0;
+	skb->ip_summed = CHECKSUM_UNNECESSARY; /* don't check it */
+	if (likely(netif_rx_ni(skb) == NET_RX_SUCCESS)) {
+		dev->stats.rx_packets++;
+		dev->stats.rx_bytes += msgsize;
+	} else {
+		dev->stats.rx_dropped++;
+	}
+
+	return msgsize;
+}
+EXPORT_SYMBOL_GPL(modem_net_receive);
+
+static int netdev_isa_open(struct net_device *dev)
+{
+	struct modem_spi_net_dev *net_iface_priv =
+			(struct modem_spi_net_dev *)netdev_priv(dev);
+	struct modem_spi_dev *modem_spi_dev = net_iface_priv->modem_spi_dev;
+
+	modem_spi_dev->netdev_flag_up = 1;
+	if (!netif_carrier_ok(dev))
+		netif_carrier_on(dev);
+	netif_wake_queue(dev);
+	return 0;
+}
+
+static int netdev_isa_close(struct net_device *dev)
+{
+	struct modem_spi_net_dev *net_iface_priv =
+			(struct modem_spi_net_dev *)netdev_priv(dev);
+	struct modem_spi_dev *modem_spi_dev = net_iface_priv->modem_spi_dev;
+
+	modem_spi_dev->netdev_flag_up = 0;
+	netif_stop_queue(dev);
+	netif_carrier_off(dev);
+	return 0;
+}
+
+static int netdev_isa_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct if_phonet_req *req = (struct if_phonet_req *)ifr;
+
+	switch (cmd) {
+	case SIOCPNGAUTOCONF:
+		req->ifr_phonet_autoconf.device = PN_DEV_HOST;
+		return 0;
+	}
+	return -ENOIOCTLCMD;
+}
+
+static struct net_device_stats *netdev_isa_stats(struct net_device *dev)
+{
+	return &dev->stats;
+}
+
+/**
+ * netdev_isa_write() - write through the net interface
+ * @skb:	pointer to the socket buffer
+ * @dev:	pointer to the network device structure
+ *
+ * Copies data(ISI message) from the user buffer to the kernel buffer and
+ * schedule transfer thread to transmit the message to the modem via FIFO.
+ */
+static netdev_tx_t netdev_isa_write(struct sk_buff *skb, struct net_device *dev)
+{
+	int err;
+	int retval = 0;
+	struct modem_spi_net_dev *net_iface_priv =
+			(struct modem_spi_net_dev *)netdev_priv(dev);
+	struct modem_spi_dev *modem_spi_dev = net_iface_priv->modem_spi_dev;
+
+	/*
+	 * FIXME:
+	 * U8500 modem requires that Pipe created/enabled Indication should
+	 * be sent from the port corresponding to GPRS socket.
+	 * Also, the U8500 modem does not implement Pipe controller
+	 * which takes care of port manipulations for GPRS traffic.
+	 *
+	 * Now, APE has GPRS socket and the socket for sending
+	 * Indication msgs bound to different ports.
+	 * Phonet stack does not allow an indication msg to be sent
+	 * from GPRS socket, since Phonet stack assumes the presence
+	 * of Pipe controller in modem.
+	 *
+	 * So, due to lack of Pipe controller implementation in the
+	 * U8500 modem, carry out the port manipulation related to
+	 * GPRS traffic here.
+	 * Ideally, it should be done either by Pipe controller in
+	 * modem OR some implementation of Pipe controller on APE side
+	 */
+	if (skb->data[RESOURCE_ID_INDEX] == PN_PIPE) {
+		if ((skb->data[MSG_ID_INDEX] == PNS_PIPE_CREATED_IND) ||
+			(skb->data[MSG_ID_INDEX] == PNS_PIPE_ENABLED_IND) ||
+			(skb->data[MSG_ID_INDEX] == PNS_PIPE_DISABLED_IND))
+			skb->data[SRC_OBJ_INDEX] = skb->data[PIPE_HDL_INDEX];
+	}
+
+	spin_lock_bh(&modem_spi_dev->isa_context->common_tx_lock);
+	err = modem_m6718_spi_send(modem_spi_dev, MODEM_M6718_SPI_CHN_ISI,
+		skb->len, skb->data);
+	if (!err) {
+		dev->stats.tx_packets++;
+		dev->stats.tx_bytes += skb->len;
+		retval = NETDEV_TX_OK;
+		dev_kfree_skb(skb);
+	} else {
+		dev->stats.tx_dropped++;
+		retval = NETDEV_TX_BUSY;
+	}
+	spin_unlock_bh(&modem_spi_dev->isa_context->common_tx_lock);
+
+	return retval;
+}
+
+static const struct net_device_ops modem_netdev_ops = {
+	.ndo_open = netdev_isa_open,
+	.ndo_stop = netdev_isa_close,
+	.ndo_do_ioctl = netdev_isa_ioctl,
+	.ndo_start_xmit = netdev_isa_write,
+	.ndo_get_stats = netdev_isa_stats,
+};
+
+static void net_device_init(struct net_device *dev)
+{
+	struct modem_spi_net_dev *net_iface_priv;
+
+	dev->netdev_ops = &modem_netdev_ops;
+	dev->header_ops = &phonet_header_ops;
+	dev->type = ARPHRD_PHONET;
+	dev->flags = IFF_POINTOPOINT | IFF_NOARP;
+	dev->mtu = PHONET_MAX_MTU;
+	dev->hard_header_len = MODEM_HLEN;
+	dev->addr_len = PHONET_ALEN;
+	dev->tx_queue_len = PN_TX_QUEUE_LEN;
+	dev->destructor = free_netdev;
+	dev->dev_addr[0] = PN_LINK_ADDR;
+	net_iface_priv = netdev_priv(dev);
+	memset(net_iface_priv, 0 , sizeof(struct modem_spi_net_dev));
+}
+
+int modem_net_init(struct modem_spi_dev *modem_spi_dev)
+{
+	struct net_device *nw_device;
+	struct modem_spi_net_dev *net_iface_priv;
+	int err;
+	/*
+	 * keep the same net device name as U8500 to allow userspace clients
+	 * to remain unchanged and use the same interfaces
+	 */
+	char *devname = "shrm%d";
+
+	/* allocate the net device */
+	nw_device = modem_spi_dev->ndev =
+		alloc_netdev(sizeof(struct modem_spi_net_dev),
+			devname, net_device_init);
+	if (nw_device == NULL) {
+		dev_err(modem_spi_dev->dev,
+			"failed to allocate modem net device\n");
+		return -ENOMEM;
+	}
+	err = register_netdev(modem_spi_dev->ndev);
+	if (err) {
+		dev_err(modem_spi_dev->dev,
+			"failed to register modem net device: error %d\n", err);
+		free_netdev(modem_spi_dev->ndev);
+		return -ENODEV;
+	}
+	dev_dbg(modem_spi_dev->dev, "registered modem net device\n");
+
+	net_iface_priv = (struct modem_spi_net_dev *)netdev_priv(nw_device);
+	net_iface_priv->modem_spi_dev = modem_spi_dev;
+	net_iface_priv->iface_num = 0;
+	return err;
+}
+EXPORT_SYMBOL_GPL(modem_net_init);
+
+int modem_net_stop(struct net_device *dev)
+{
+	netif_stop_queue(dev);
+	return 0;
+}
+
+int modem_net_restart(struct net_device *dev)
+{
+	if (netif_queue_stopped(dev))
+		netif_wake_queue(dev);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(modem_net_restart);
+
+int modem_net_start(struct net_device *dev)
+{
+	struct modem_spi_net_dev *net_iface_priv =
+			(struct modem_spi_net_dev *)netdev_priv(dev);
+	struct modem_spi_dev *modem_spi_dev = net_iface_priv->modem_spi_dev;
+
+	if (!netif_carrier_ok(dev))
+		netif_carrier_on(dev);
+	netif_start_queue(dev);
+	modem_spi_dev->netdev_flag_up = 1;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(modem_net_start);
+
+int modem_net_suspend(struct net_device *dev)
+{
+	if (netif_running(dev)) {
+		netif_stop_queue(dev);
+		netif_carrier_off(dev);
+	}
+	netif_device_detach(dev);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(modem_net_suspend);
+
+int modem_net_resume(struct net_device *dev)
+{
+	netif_device_attach(dev);
+	if (netif_running(dev)) {
+		netif_carrier_on(dev);
+		netif_wake_queue(dev);
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(modem_net_resume);
+
+void modem_net_exit(struct modem_spi_dev *modem_spi_dev)
+{
+	if (modem_spi_dev && modem_spi_dev->ndev) {
+		unregister_netdev(modem_spi_dev->ndev);
+		modem_spi_dev->ndev = NULL;
+		dev_dbg(modem_spi_dev->dev, "removed modem net device\n");
+	}
+}
+EXPORT_SYMBOL_GPL(modem_net_exit);
+
+MODULE_AUTHOR("Chris Blair <chris.blair@stericsson.com>");
+MODULE_DESCRIPTION("M6718 modem IPC net device interface");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/net/u8500_shrm.c b/drivers/net/u8500_shrm.c
new file mode 100644
index 00000000000..08597623443
--- /dev/null
+++ b/drivers/net/u8500_shrm.c
@@ -0,0 +1,312 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2009
+ *
+ * Author: Biju Das <biju.das@stericsson.com> for ST-Ericsson
+ * Author: Kumar Sanghvi <kumar.sanghvi@stericsson.com> for ST-Ericsson
+ * Author: Arun Murthy <arun.murthy@stericsson.com> for ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/if_ether.h>
+#include <linux/netdevice.h>
+#include <linux/phonet.h>
+#include <linux/if_phonet.h>
+#include <linux/if_arp.h>
+#include <linux/modem/shrm/shrm_driver.h>
+#include <linux/modem/shrm/shrm_private.h>
+#include <linux/modem/shrm/shrm_config.h>
+#include <linux/modem/shrm/shrm_net.h>
+#include <linux/modem/shrm/shrm.h>
+#include <net/sock.h>
+#include <net/phonet/phonet.h>
+#include <net/phonet/pep.h>
+
+
+/**
+ * shrm_net_receive() - receive data and copy to user space buffer
+ * @dev:	pointer to the network device structure
+ *
+ * Copy data from ISI queue to the user space buffer.
+ */
+int shrm_net_receive(struct net_device *dev)
+{
+	struct sk_buff *skb;
+	struct isadev_context *isadev;
+	struct message_queue *q;
+	u32 msgsize;
+	u32 size = 0;
+	struct shrm_net_iface_priv *net_iface_priv =
+		(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	isadev = &shrm->isa_context->isadev[ISI_MESSAGING];
+	q = &isadev->dl_queue;
+
+	spin_lock_bh(&q->update_lock);
+	if (list_empty(&q->msg_list)) {
+		spin_unlock_bh(&q->update_lock);
+		dev_dbg(shrm->dev, "Empty Shrm queue\n");
+		return 0;
+	}
+	spin_unlock_bh(&q->update_lock);
+
+	msgsize = get_size_of_new_msg(q);
+	if (msgsize <= 0)
+		return msgsize;
+
+	/*
+	 * The packet has been retrieved from the transmission
+	 * medium. Build an skb around it, so upper layers can handle it
+	 */
+	skb = dev_alloc_skb(msgsize);
+	if (!skb) {
+		if (printk_ratelimit())
+			dev_notice(shrm->dev,
+			"isa rx: low on mem - packet dropped\n");
+		dev->stats.rx_dropped++;
+		goto out;
+	}
+
+	if ((q->readptr+msgsize) >= q->size) {
+		size = (q->size-q->readptr);
+		/*Copy First Part of msg*/
+		skb_copy_to_linear_data(skb,
+				(u8 *)(q->fifo_base + q->readptr), size);
+		skb_put(skb, size);
+
+		/*Copy Second Part of msg at the top of fifo*/
+		skb_copy_to_linear_data_offset(skb, size,
+				(u8 *)(q->fifo_base), (msgsize - size));
+		skb_put(skb, msgsize-size);
+
+	} else {
+		skb_copy_to_linear_data(skb,
+				(u8 *)(q->fifo_base+q->readptr), msgsize);
+		skb_put(skb, msgsize);
+	}
+
+	spin_lock_bh(&q->update_lock);
+	remove_msg_from_queue(q);
+	spin_unlock_bh(&q->update_lock);
+
+	skb_reset_mac_header(skb);
+	__skb_pull(skb, dev->hard_header_len);
+	/*Write metadata, and then pass to the receive level*/
+	skb->dev = dev;/*kmalloc(sizeof(struct net_device), GFP_ATOMIC);*/
+	skb->protocol = htons(ETH_P_PHONET);
+	skb->priority = 0;
+	skb->ip_summed = CHECKSUM_UNNECESSARY; /* don't check it */
+	if (likely(netif_rx_ni(skb) == NET_RX_SUCCESS)) {
+		dev->stats.rx_packets++;
+		dev->stats.rx_bytes += msgsize;
+	} else
+		dev->stats.rx_dropped++;
+
+	return msgsize;
+out:
+	return -ENOMEM;
+}
+
+static int netdev_isa_open(struct net_device *dev)
+{
+	struct shrm_net_iface_priv *net_iface_priv =
+			(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	shrm->netdev_flag_up = 1;
+	if (!netif_carrier_ok(dev))
+		netif_carrier_on(dev);
+	netif_wake_queue(dev);
+	return 0;
+}
+
+static int netdev_isa_close(struct net_device *dev)
+{
+	struct shrm_net_iface_priv *net_iface_priv =
+			(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	shrm->netdev_flag_up = 0;
+	netif_stop_queue(dev);
+	netif_carrier_off(dev);
+	return 0;
+}
+
+static int netdev_isa_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	struct if_phonet_req *req = (struct if_phonet_req *)ifr;
+
+	switch (cmd) {
+	case SIOCPNGAUTOCONF:
+		req->ifr_phonet_autoconf.device = PN_DEV_HOST;
+		return 0;
+	}
+	return -ENOIOCTLCMD;
+}
+
+static struct net_device_stats *netdev_isa_stats(struct net_device *dev)
+{
+	return &dev->stats;
+}
+
+/**
+ * netdev_isa_write() - write through the net interface
+ * @skb:	pointer to the socket buffer
+ * @dev:	pointer to the network device structure
+ *
+ * Copies data(ISI message) from the user buffer to the kernel buffer and
+ * schedule transfer thread to transmit the message to the modem via FIFO.
+ */
+static netdev_tx_t netdev_isa_write(struct sk_buff *skb, struct net_device *dev)
+{
+	int err;
+	int retval = 0;
+	struct shrm_net_iface_priv *net_iface_priv =
+			(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	/*
+	 * FIXME:
+	 * U8500 modem requires that Pipe created/enabled Indication should
+	 * be sent from the port corresponding to GPRS socket.
+	 * Also, the U8500 modem does not implement Pipe controller
+	 * which takes care of port manipulations for GPRS traffic.
+	 *
+	 * Now, APE has GPRS socket and the socket for sending
+	 * Indication msgs bound to different ports.
+	 * Phonet stack does not allow an indication msg to be sent
+	 * from GPRS socket, since Phonet stack assumes the presence
+	 * of Pipe controller in modem.
+	 *
+	 * So, due to lack of Pipe controller implementation in the
+	 * U8500 modem, carry out the port manipulation related to
+	 * GPRS traffic here.
+	 * Ideally, it should be done either by Pipe controller in
+	 * modem OR some implementation of Pipe controller on APE side
+	 */
+	if (skb->data[RESOURCE_ID_INDEX] == PN_PIPE) {
+		if ((skb->data[MSG_ID_INDEX] == PNS_PIPE_CREATED_IND) ||
+			(skb->data[MSG_ID_INDEX] == PNS_PIPE_ENABLED_IND) ||
+			(skb->data[MSG_ID_INDEX] == PNS_PIPE_DISABLED_IND))
+			skb->data[SRC_OBJ_INDEX] = skb->data[PIPE_HDL_INDEX];
+	}
+
+	spin_lock_bh(&shrm->isa_context->common_tx);
+	err = shm_write_msg(shrm, ISI_MESSAGING, skb->data,
+			skb->len);
+	if (!err) {
+		dev->stats.tx_packets++;
+		dev->stats.tx_bytes += skb->len;
+		retval = NETDEV_TX_OK;
+		dev_kfree_skb(skb);
+	} else {
+		dev->stats.tx_dropped++;
+		retval = NETDEV_TX_BUSY;
+	}
+	spin_unlock_bh(&shrm->isa_context->common_tx);
+
+	return retval;
+}
+
+static const struct net_device_ops shrm_netdev_ops = {
+	.ndo_open = netdev_isa_open,
+	.ndo_stop = netdev_isa_close,
+	.ndo_do_ioctl = netdev_isa_ioctl,
+	.ndo_start_xmit = netdev_isa_write,
+	.ndo_get_stats = netdev_isa_stats,
+};
+
+static void shm_net_init(struct net_device *dev)
+{
+	struct shrm_net_iface_priv *net_iface_priv;
+
+	dev->netdev_ops = &shrm_netdev_ops;
+	dev->header_ops = &phonet_header_ops;
+	dev->type = ARPHRD_PHONET;
+	dev->flags = IFF_POINTOPOINT | IFF_NOARP;
+	dev->mtu = PHONET_MAX_MTU;
+	dev->hard_header_len = SHRM_HLEN;
+	dev->addr_len = PHONET_ALEN;
+	dev->tx_queue_len = PN_TX_QUEUE_LEN;
+	dev->destructor = free_netdev;
+	dev->dev_addr[0] = PN_LINK_ADDR;
+	net_iface_priv = netdev_priv(dev);
+	memset(net_iface_priv, 0 , sizeof(struct shrm_net_iface_priv));
+}
+
+int shrm_register_netdev(struct shrm_dev *shrm)
+{
+	struct net_device *nw_device;
+	struct shrm_net_iface_priv *net_iface_priv;
+	char *devname = "shrm%d";
+	int err;
+
+	/* allocate the net device */
+	nw_device = shrm->ndev = alloc_netdev(
+			sizeof(struct shrm_net_iface_priv),
+			devname, shm_net_init);
+	if (nw_device == NULL) {
+		dev_err(shrm->dev, "Failed to allocate SHRM Netdev\n");
+		return -ENOMEM;
+	}
+	err = register_netdev(shrm->ndev);
+	if (err) {
+		dev_err(shrm->dev, "Err %i in reg shrm-netdev\n", err);
+		free_netdev(shrm->ndev);
+		return -ENODEV;
+	}
+	dev_info(shrm->dev, "Registered shrm netdev\n");
+
+	net_iface_priv = (struct shrm_net_iface_priv *)netdev_priv(nw_device);
+	net_iface_priv->shrm_device = shrm;
+	net_iface_priv->iface_num = 0;
+
+	return err;
+}
+
+int shrm_stop_netdev(struct net_device *dev)
+{
+	netif_stop_queue(dev);
+	return 0;
+}
+
+int shrm_restart_netdev(struct net_device *dev)
+{
+	if (netif_queue_stopped(dev))
+		netif_wake_queue(dev);
+	return 0;
+}
+
+int shrm_start_netdev(struct net_device *dev)
+{
+	struct shrm_net_iface_priv *net_iface_priv =
+			(struct shrm_net_iface_priv *)netdev_priv(dev);
+	struct shrm_dev *shrm = net_iface_priv->shrm_device;
+
+	if (!netif_carrier_ok(dev))
+		netif_carrier_on(dev);
+	netif_start_queue(dev);
+	shrm->netdev_flag_up = 1;
+	return 0;
+}
+
+int shrm_suspend_netdev(struct net_device *dev)
+{
+	if (netif_running(dev))
+		netif_stop_queue(dev);
+	netif_device_detach(dev);
+	return 0;
+}
+
+int shrm_resume_netdev(struct net_device *dev)
+{
+	netif_device_attach(dev);
+	if (netif_running(dev))
+		netif_wake_queue(dev);
+	return 0;
+}
+
+void shrm_unregister_netdev(struct shrm_dev *shrm)
+{
+	unregister_netdev(shrm->ndev);
+}
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 99dc29f2f2f..56dfe432b74 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -307,4 +307,23 @@ config AB8500_BATTERY_THERM_ON_BATCTRL
 	help
 	  Say Y to enable battery temperature measurements using
 	  thermistor connected on BATCTRL ADC.
+
+config AB8500_9100_LI_ION_BATTERY
+	bool "Enable support of the 9100 Li-ion battery charging"
+	depends on AB8500_BM
+	help
+	  Say Y to enable support of the 9100 Li-ion battery charging.
+
+config AB5500_BM
+	bool "AB5500 Battery Management Driver"
+	depends on AB5500_CORE && AB5500_GPADC && MACH_U5500
+	help
+	  Say Y to include support for AB5500 battery management.
+
+config AB5500_BATTERY_THERM_ON_BATCTRL
+	bool "Thermistor connected on BATCTRL ADC"
+	depends on AB5500_BM
+	help
+	  Say Y to enable battery temperature measurements using
+	  thermistor connected on BATCTRL ADC.
 endif # POWER_SUPPLY
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index b6b243416c0..af27f1d08aa 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -41,6 +41,7 @@ obj-$(CONFIG_CHARGER_TWL4030)	+= twl4030_charger.o
 obj-$(CONFIG_CHARGER_LP8727)	+= lp8727_charger.o
 obj-$(CONFIG_CHARGER_GPIO)	+= gpio-charger.o
 obj-$(CONFIG_CHARGER_MANAGER)	+= charger-manager.o
+obj-$(CONFIG_AB5500_BM)		+= ab5500_charger.o ab5500_btemp.o ab5500_fg.o abx500_chargalg.o
 obj-$(CONFIG_CHARGER_MAX8997)	+= max8997_charger.o
 obj-$(CONFIG_CHARGER_MAX8998)	+= max8998_charger.o
 obj-$(CONFIG_CHARGER_SMB347)	+= smb347-charger.o
diff --git a/drivers/power/ab5500_btemp.c b/drivers/power/ab5500_btemp.c
new file mode 100644
index 00000000000..3709299d6cb
--- /dev/null
+++ b/drivers/power/ab5500_btemp.c
@@ -0,0 +1,994 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Battery temperature driver for ab5500
+ *
+ * License Terms: GNU General Public License v2
+ * Authors:
+ *	Johan Palsson <johan.palsson@stericsson.com>
+ *	Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/completion.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+#include <linux/mfd/abx500/ab5500-bm.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+
+#define BTEMP_THERMAL_LOW_LIMIT		-10
+#define BTEMP_THERMAL_MED_LIMIT		0
+#define BTEMP_THERMAL_HIGH_LIMIT_62	62
+
+#define BTEMP_BATCTRL_CURR_SRC_7UA	7
+#define BTEMP_BATCTRL_CURR_SRC_15UA	15
+#define BTEMP_BATCTRL_CURR_SRC_20UA	20
+
+#define UART_MODE			0x0F
+#define BAT_CUR_SRC			0x1F
+#define RESIS_ID_MODE			0x03
+#define RESET				0x00
+#define ADOUT_10K_PULL_UP		0x07
+
+#define to_ab5500_btemp_device_info(x) container_of((x), \
+	struct ab5500_btemp, btemp_psy);
+
+/**
+ * struct ab5500_btemp_interrupts - ab5500 interrupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab5500_btemp_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+struct ab5500_btemp_events {
+	bool batt_rem;
+	bool usb_conn;
+};
+
+/**
+ * struct ab5500_btemp - ab5500 BTEMP device information
+ * @dev:		Pointer to the structure device
+ * @chip_id:		Chip-Id of the AB5500
+ * @curr_source:	What current source we use, in uA
+ * @bat_temp:		Battery temperature in degree Celcius
+ * @prev_bat_temp	Last dispatched battery temperature
+ * @node:		struct of type list_head
+ * @parent:		Pointer to the struct ab5500
+ * @gpadc:		Pointer to the struct gpadc
+ * @gpadc-auto:		Pointer to the struct adc_auto_input
+ * @pdata:		Pointer to the ab5500_btemp platform data
+ * @bat:		Pointer to the ab5500_bm platform data
+ * @btemp_psy:		Structure for BTEMP specific battery properties
+ * @events:		Structure for information about events triggered
+ * @btemp_wq:		Work queue for measuring the temperature periodically
+ * @btemp_periodic_work:	Work for measuring the temperature periodically
+ */
+struct ab5500_btemp {
+	struct device *dev;
+	u8 chip_id;
+	int curr_source;
+	int bat_temp;
+	int prev_bat_temp;
+	struct list_head node;
+	struct ab5500 *parent;
+	struct ab5500_gpadc *gpadc;
+	struct adc_auto_input *gpadc_auto;
+	struct abx500_btemp_platform_data *pdata;
+	struct abx500_bm_data *bat;
+	struct power_supply btemp_psy;
+	struct ab5500_btemp_events events;
+	struct workqueue_struct *btemp_wq;
+	struct delayed_work btemp_periodic_work;
+};
+
+/* BTEMP power supply properties */
+static enum power_supply_property ab5500_btemp_props[] = {
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_TEMP,
+};
+
+static LIST_HEAD(ab5500_btemp_list);
+
+static int ab5500_btemp_bat_temp_trig(int mux);
+
+struct ab5500_btemp *ab5500_btemp_get(void)
+{
+	struct ab5500_btemp *di;
+	di = list_first_entry(&ab5500_btemp_list, struct ab5500_btemp, node);
+
+	return di;
+}
+
+/**
+ * ab5500_btemp_get_batctrl_temp() - get the temperature
+ * @di:      pointer to the ab5500_btemp structure
+ *
+ * Returns the batctrl temperature in millidegrees
+ */
+int ab5500_btemp_get_batctrl_temp(struct ab5500_btemp *di)
+{
+	return di->bat_temp * 1000;
+}
+
+/**
+ * ab5500_btemp_volt_to_res() - convert batctrl voltage to resistance
+ * @di:		pointer to the ab5500_btemp structure
+ * @volt:	measured batctrl/btemp_ball voltage
+ * @batcrtl:	batctrl/btemp_ball node
+ *
+ * This function returns the battery resistance that is
+ * derived from the BATCTRL/BTEMP_BALL voltage.
+ * Returns value in Ohms.
+ */
+static int ab5500_btemp_volt_to_res(struct ab5500_btemp *di,
+	int volt, bool batctrl)
+{
+	int rbs;
+
+	if (batctrl) {
+		/*
+		 * If the battery has internal NTC, we use the current
+		 * source to calculate the resistance, 7uA or 20uA
+		 */
+		rbs = volt * 1000 / di->curr_source;
+	} else {
+		/*
+		 * BTEMP_BALL is internally
+		 * connected to 1.8V through a 10k resistor
+		 */
+		rbs = (10000 * volt) / (1800 - volt);
+	}
+	return rbs;
+}
+
+/**
+ * ab5500_btemp_read_batctrl_voltage() - measure batctrl voltage
+ * @di:		pointer to the ab5500_btemp structure
+ *
+ * This function returns the voltage on BATCTRL. Returns value in mV.
+ */
+static int ab5500_btemp_read_batctrl_voltage(struct ab5500_btemp *di)
+{
+	int vbtemp;
+	static int prev;
+
+	vbtemp = ab5500_gpadc_convert(di->gpadc, BAT_CTRL);
+	if (vbtemp < 0) {
+		dev_err(di->dev,
+			"%s gpadc conversion failed, using previous value",
+			__func__);
+		return prev;
+	}
+	prev = vbtemp;
+	return vbtemp;
+}
+
+/**
+ * ab5500_btemp_curr_source_enable() - enable/disable batctrl current source
+ * @di:		pointer to the ab5500_btemp structure
+ * @enable:	enable or disable the current source
+ *
+ * Enable or disable the current sources for the BatCtrl AD channel
+ */
+static int ab5500_btemp_curr_source_enable(struct ab5500_btemp *di,
+	bool enable)
+{
+	int ret = 0;
+
+	/* Only do this for batteries with internal NTC */
+	if (di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL && enable) {
+
+		dev_dbg(di->dev, "Set BATCTRL %duA\n", di->curr_source);
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_UART,
+			UART_MODE, RESIS_ID_MODE);
+		if (ret) {
+			dev_err(di->dev,
+				"%s failed setting resistance identification mode\n",
+				__func__);
+			return ret;
+		}
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_URI,
+			BAT_CUR_SRC, BAT_CTRL_15U_ENA);
+		if (ret) {
+			dev_err(di->dev, "%s failed enabling current source\n",
+				__func__);
+			goto disable_curr_source;
+		}
+	} else if (di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL && !enable) {
+		dev_dbg(di->dev, "Disable BATCTRL curr source\n");
+
+		/* Write 0 to the curr bits */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_URI,
+			BAT_CUR_SRC, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s failed disabling current source\n",
+				__func__);
+			goto disable_curr_source;
+		}
+
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_UART,
+			UART_MODE, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s failed disabling force comp\n",
+				__func__);
+		}
+	}
+	return ret;
+disable_curr_source:
+	/* Write 0 to the curr bits */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_URI,
+			BAT_CUR_SRC, RESET);
+	if (ret) {
+		dev_err(di->dev, "%s failed disabling current source\n",
+			__func__);
+	}
+	return ret;
+}
+
+/**
+ * ab5500_btemp_get_batctrl_res() - get battery resistance
+ * @di:		pointer to the ab5500_btemp structure
+ *
+ * This function returns the battery pack identification resistance.
+ * Returns value in Ohms.
+ */
+static int ab5500_btemp_get_batctrl_res(struct ab5500_btemp *di)
+{
+	int ret;
+	int batctrl;
+	int res;
+
+	ret = ab5500_btemp_curr_source_enable(di, true);
+	/* TODO: This delay has to be optimised */
+	msleep(100);
+	if (ret) {
+		dev_err(di->dev, "%s curr source enable failed\n", __func__);
+		return ret;
+	}
+
+	batctrl = ab5500_btemp_read_batctrl_voltage(di);
+	res = ab5500_btemp_volt_to_res(di, batctrl, true);
+
+	ret = ab5500_btemp_curr_source_enable(di, false);
+	if (ret) {
+		dev_err(di->dev, "%s curr source disable failed\n", __func__);
+		return ret;
+	}
+
+	dev_dbg(di->dev, "%s batctrl: %d res: %d ",
+		__func__, batctrl, res);
+
+	return res;
+}
+
+/**
+ * ab5500_btemp_get_btemp_ball_res() - get battery resistance
+ * @di:		pointer to the ab5500_btemp structure
+ *
+ * This function returns the battery pack identification
+ * resistance using resistor pull-up mode. Returns value in Ohms.
+ */
+static int ab5500_btemp_get_btemp_ball_res(struct ab5500_btemp *di)
+{
+	int ret, vntc;
+
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+		AB5500_BANK_FG_BATTCOM_ACC, AB5500_UART,
+		UART_MODE, ADOUT_10K_PULL_UP);
+	if (ret) {
+		dev_err(di->dev,
+			"failed to enable 10k pull up to Vadout\n");
+			return ret;
+	}
+
+	vntc = ab5500_gpadc_convert(di->gpadc, BTEMP_BALL);
+	if (vntc < 0) {
+		dev_err(di->dev, "%s gpadc conversion failed,"
+				" using previous value\n", __func__);
+		return vntc;
+	}
+
+	return ab5500_btemp_volt_to_res(di, vntc, false);
+}
+
+/**
+ * ab5500_btemp_temp_to_res() - temperature to resistance
+ * @di:		pointer to the ab5500_btemp structure
+ * @tbl:	pointer to the resiatance to temperature table
+ * @tbl_size:	size of the resistance to temperature table
+ * @temp:	temperature to calculate the resistance from
+ *
+ * This function returns the battery resistance in ohms
+ * based on temperature.
+ */
+static int ab5500_btemp_temp_to_res(struct ab5500_btemp *di,
+	const struct abx500_res_to_temp *tbl, int tbl_size, int temp)
+{
+	int i, res;
+	/*
+	 * Calculate the formula for the straight line
+	 * Simple interpolation if we are within
+	 * the resistance table limits, extrapolate
+	 * if resistance is outside the limits.
+	 */
+	if (temp < tbl[0].temp)
+		i = 0;
+	else if (temp >= tbl[tbl_size - 1].temp)
+		i = tbl_size - 2;
+	else {
+		i = 0;
+		while (!(temp >= tbl[i].temp &&
+			temp < tbl[i + 1].temp))
+			i++;
+	}
+
+	res = tbl[i].resist + ((tbl[i + 1].resist - tbl[i].resist) *
+		(temp - tbl[i].temp)) / (tbl[i + 1].temp - tbl[i].temp);
+	return res;
+}
+
+/**
+ * ab5500_btemp_temp_to_volt() - temperature to adc voltage
+ * @di:		pointer to the ab5500_btemp structure
+ * @temp:	temperature to calculate the voltage from
+ *
+ * This function returns the adc voltage in millivolts
+ * based on temperature.
+ */
+static int ab5500_btemp_temp_to_volt(struct ab5500_btemp *di, int temp)
+{
+	int res, id;
+
+	id = di->bat->batt_id;
+	res = ab5500_btemp_temp_to_res(di,
+		di->bat->bat_type[id].r_to_t_tbl,
+                di->bat->bat_type[id].n_temp_tbl_elements,
+		temp);
+	/*
+	 * BTEMP_BALL is internally connected to 1.8V
+	 * through a 10k resistor
+	 */
+	return((1800 * res) / (10000 + res));
+}
+
+/**
+ * ab5500_btemp_res_to_temp() - resistance to temperature
+ * @di:		pointer to the ab5500_btemp structure
+ * @tbl:	pointer to the resiatance to temperature table
+ * @tbl_size:	size of the resistance to temperature table
+ * @res:	resistance to calculate the temperature from
+ *
+ * This function returns the battery temperature in degrees Celcius
+ * based on the NTC resistance.
+ */
+static int ab5500_btemp_res_to_temp(struct ab5500_btemp *di,
+	const struct abx500_res_to_temp *tbl, int tbl_size, int res)
+{
+	int i, temp;
+	/*
+	 * Calculate the formula for the straight line
+	 * Simple interpolation if we are within
+	 * the resistance table limits, extrapolate
+	 * if resistance is outside the limits.
+	 */
+	if (res > tbl[0].resist)
+		i = 0;
+	else if (res <= tbl[tbl_size - 1].resist)
+		i = tbl_size - 2;
+	else {
+		i = 0;
+		while (!(res <= tbl[i].resist &&
+			res > tbl[i + 1].resist))
+			i++;
+	}
+
+	temp = tbl[i].temp + ((tbl[i + 1].temp - tbl[i].temp) *
+		(res - tbl[i].resist)) / (tbl[i + 1].resist - tbl[i].resist);
+	return temp;
+}
+
+/**
+ * ab5500_btemp_measure_temp() - measure battery temperature
+ * @di:		pointer to the ab5500_btemp structure
+ *
+ * Returns battery temperature (on success) else the previous temperature
+ */
+static int ab5500_btemp_measure_temp(struct ab5500_btemp *di)
+{
+	int temp, rbat;
+	u8 id;
+
+	id = di->bat->batt_id;
+	if (di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL &&
+			id != BATTERY_UNKNOWN && !di->bat->auto_trig)
+		rbat = ab5500_btemp_get_batctrl_res(di);
+	else
+		rbat = ab5500_btemp_get_btemp_ball_res(di);
+
+	if (rbat < 0) {
+		dev_err(di->dev, "%s failed to get resistance\n", __func__);
+		/*
+		 * Return out-of-range temperature so that
+		 * charging is stopped
+		 */
+		return BTEMP_THERMAL_LOW_LIMIT;
+	}
+
+	temp = ab5500_btemp_res_to_temp(di,
+		di->bat->bat_type[id].r_to_t_tbl,
+		di->bat->bat_type[id].n_temp_tbl_elements, rbat);
+	dev_dbg(di->dev, "Battery temperature is %d\n", temp);
+
+	return temp;
+}
+
+/**
+ * ab5500_btemp_id() - Identify the connected battery
+ * @di:		pointer to the ab5500_btemp structure
+ *
+ * This function will try to identify the battery by reading the ID
+ * resistor. Some brands use a combined ID resistor with a NTC resistor to
+ * both be able to identify and to read the temperature of it.
+ */
+static int ab5500_btemp_id(struct ab5500_btemp *di)
+{
+	int res;
+	u8 i;
+
+	di->curr_source = BTEMP_BATCTRL_CURR_SRC_7UA;
+	di->bat->batt_id = BATTERY_UNKNOWN;
+
+	res =  ab5500_btemp_get_batctrl_res(di);
+	if (res < 0) {
+		dev_err(di->dev, "%s get batctrl res failed\n", __func__);
+		return -ENXIO;
+	}
+
+	/* BATTERY_UNKNOWN is defined on position 0, skip it! */
+	for (i = BATTERY_UNKNOWN + 1; i < di->bat->n_btypes; i++) {
+		if ((res <= di->bat->bat_type[i].resis_high) &&
+			(res >= di->bat->bat_type[i].resis_low)) {
+			dev_dbg(di->dev, "Battery detected on %s"
+				" low %d < res %d < high: %d"
+				" index: %d\n",
+				di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL ?
+				"BATCTRL" : "BATTEMP",
+				di->bat->bat_type[i].resis_low, res,
+				di->bat->bat_type[i].resis_high, i);
+
+			di->bat->batt_id = i;
+			break;
+		}
+	}
+
+	if (di->bat->batt_id == BATTERY_UNKNOWN) {
+		dev_warn(di->dev, "Battery identified as unknown"
+			", resistance %d Ohm\n", res);
+		return -ENXIO;
+	}
+
+	/*
+	 * We only have to change current source if the
+	 * detected type is Type 1, else we use the 7uA source
+	 */
+	if (di->bat->adc_therm == ABx500_ADC_THERM_BATCTRL &&
+			di->bat->batt_id == 1) {
+		dev_dbg(di->dev, "Set BATCTRL current source to 15uA\n");
+		di->curr_source = BTEMP_BATCTRL_CURR_SRC_15UA;
+	}
+
+	return di->bat->batt_id;
+}
+
+/**
+ * ab5500_btemp_periodic_work() - Measuring the temperature periodically
+ * @work:	pointer to the work_struct structure
+ *
+ * Work function for measuring the temperature periodically
+ */
+static void ab5500_btemp_periodic_work(struct work_struct *work)
+{
+	struct ab5500_btemp *di = container_of(work,
+		struct ab5500_btemp, btemp_periodic_work.work);
+
+	di->bat_temp = ab5500_btemp_measure_temp(di);
+
+	if (di->bat_temp != di->prev_bat_temp) {
+		di->prev_bat_temp = di->bat_temp;
+		power_supply_changed(&di->btemp_psy);
+	}
+	di->bat->temp_now = di->bat_temp;
+
+	if (!di->bat->auto_trig) {
+		/* Check for temperature limits */
+		if (di->bat_temp <= BTEMP_THERMAL_LOW_LIMIT) {
+			dev_err(di->dev,
+				"battery temp less than lower threshold\n");
+			power_supply_changed(&di->btemp_psy);
+		} else if (di->bat_temp >= BTEMP_THERMAL_HIGH_LIMIT_62) {
+			dev_err(di->dev,
+				"battery temp greater them max threshold\n");
+			power_supply_changed(&di->btemp_psy);
+		}
+
+		/* Schedule a new measurement */
+		if (di->events.usb_conn)
+			queue_delayed_work(di->btemp_wq,
+				&di->btemp_periodic_work,
+				round_jiffies(di->bat->interval_charging * HZ));
+		else
+			queue_delayed_work(di->btemp_wq,
+				&di->btemp_periodic_work,
+				round_jiffies(di->bat->interval_not_charging * HZ));
+	} else {
+		/* Schedule a new measurement */
+		queue_delayed_work(di->btemp_wq,
+			&di->btemp_periodic_work,
+			round_jiffies(di->bat->interval_charging * HZ));
+	}
+}
+
+/**
+ * ab5500_btemp_batt_removal_handler() - battery removal detected
+ * @irq:       interrupt number
+ * @_di:       void pointer that has to address of ab5500_btemp
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_btemp_batt_removal_handler(int irq, void *_di)
+{
+	struct ab5500_btemp *di = _di;
+	dev_err(di->dev, "Battery removal detected!\n");
+
+	di->events.batt_rem = true;
+	power_supply_changed(&di->btemp_psy);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_btemp_batt_attach_handler() - battery insertion detected
+ * @irq:       interrupt number
+ * @_di:       void pointer that has to address of ab5500_btemp
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_btemp_batt_attach_handler(int irq, void *_di)
+{
+	struct ab5500_btemp *di = _di;
+	dev_err(di->dev, "Battery attached!\n");
+
+	di->events.batt_rem = false;
+	power_supply_changed(&di->btemp_psy);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_btemp_periodic() - Periodic temperature measurements
+ * @di:		pointer to the ab5500_btemp structure
+ * @enable:	enable or disable periodic temperature measurements
+ *
+ * Starts of stops periodic temperature measurements. Periodic measurements
+ * should only be done when a charger is connected.
+ */
+static void ab5500_btemp_periodic(struct ab5500_btemp *di,
+	bool enable)
+{
+	dev_dbg(di->dev, "Enable periodic temperature measurements: %d\n",
+		enable);
+
+	if (enable)
+		queue_delayed_work(di->btemp_wq, &di->btemp_periodic_work, 0);
+	else
+		cancel_delayed_work_sync(&di->btemp_periodic_work);
+}
+
+/**
+ * ab5500_btemp_get_property() - get the btemp properties
+ * @psy:        pointer to the power_supply structure
+ * @psp:        pointer to the power_supply_property structure
+ * @val:        pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the btemp
+ * properties by reading the sysfs files.
+ * online:	presence of the battery
+ * present:	presence of the battery
+ * technology:	battery technology
+ * temp:	battery temperature
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_btemp_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab5500_btemp *di;
+
+	di = to_ab5500_btemp_device_info(psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_PRESENT:
+	case POWER_SUPPLY_PROP_ONLINE:
+		if (di->events.batt_rem)
+			val->intval = 0;
+		else
+			val->intval = 1;
+		break;
+	case POWER_SUPPLY_PROP_TECHNOLOGY:
+		val->intval = di->bat->bat_type[di->bat->batt_id].name;
+		break;
+	case POWER_SUPPLY_PROP_TEMP:
+		if (di->bat->batt_id == BATTERY_UNKNOWN)
+		/*
+		 * In case the battery is not identified, its assumed that
+		 * we are using the power supply and since no monitoring is
+		 * done for the same, a nominal temp is hardocded.
+		 */
+			val->intval = 250;
+		else
+			val->intval = di->bat_temp * 10;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ab5500_btemp_get_ext_psy_data(struct device *dev, void *data)
+{
+	struct power_supply *psy;
+	struct power_supply *ext;
+	struct ab5500_btemp *di;
+	union power_supply_propval ret;
+	int i, j;
+	bool psy_found = false;
+
+	psy = (struct power_supply *)data;
+	ext = dev_get_drvdata(dev);
+	di = to_ab5500_btemp_device_info(psy);
+
+	/*
+	 * For all psy where the name of your driver
+	 * appears in any supplied_to
+	 */
+	for (i = 0; i < ext->num_supplicants; i++) {
+		if (!strcmp(ext->supplied_to[i], psy->name))
+			psy_found = true;
+	}
+
+	if (!psy_found)
+		return 0;
+
+	/* Go through all properties for the psy */
+	for (j = 0; j < ext->num_properties; j++) {
+		enum power_supply_property prop;
+		prop = ext->properties[j];
+
+		if (ext->get_property(ext, prop, &ret))
+			continue;
+
+		switch (prop) {
+		case POWER_SUPPLY_PROP_PRESENT:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_USB:
+				/* USB disconnected */
+				if (!ret.intval && di->events.usb_conn) {
+					di->events.usb_conn = false;
+					if (di->bat->auto_trig)
+						ab5500_btemp_periodic(di,
+							false);
+				}
+				/* USB connected */
+				else if (ret.intval && !di->events.usb_conn) {
+					di->events.usb_conn = true;
+					if (di->bat->auto_trig)
+						ab5500_btemp_periodic(di, true);
+				}
+				break;
+			default:
+				break;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+/**
+ * ab5500_btemp_external_power_changed() - callback for power supply changes
+ * @psy:       pointer to the structure power_supply
+ *
+ * This function is pointing to the function pointer external_power_changed
+ * of the structure power_supply.
+ * This function gets executed when there is a change in the external power
+ * supply to the btemp.
+ */
+static void ab5500_btemp_external_power_changed(struct power_supply *psy)
+{
+	struct ab5500_btemp *di = to_ab5500_btemp_device_info(psy);
+
+	class_for_each_device(power_supply_class, NULL,
+		&di->btemp_psy, ab5500_btemp_get_ext_psy_data);
+}
+
+/* ab5500 btemp driver interrupts and their respective isr */
+static struct ab5500_btemp_interrupts ab5500_btemp_irq[] = {
+	{"BATT_REMOVAL", ab5500_btemp_batt_removal_handler},
+	{"BATT_ATTACH", ab5500_btemp_batt_attach_handler},
+};
+
+static int ab5500_btemp_bat_temp_trig(int mux)
+{
+	struct ab5500_btemp *di = ab5500_btemp_get();
+	int temp = ab5500_btemp_measure_temp(di);
+
+	if (temp < (BTEMP_THERMAL_LOW_LIMIT+1)) {
+		dev_err(di->dev,
+			"battery temp less than lower threshold (-10 deg cel)\n");
+		power_supply_changed(&di->btemp_psy);
+	} else if (temp > (BTEMP_THERMAL_HIGH_LIMIT_62-1)) {
+		dev_err(di->dev, "battery temp greater them max threshold\n");
+		power_supply_changed(&di->btemp_psy);
+	}
+
+	return 0;
+}
+
+static int ab5500_btemp_auto_temp(struct ab5500_btemp *di)
+{
+	struct adc_auto_input *auto_ip;
+	int ret = 0;
+
+	auto_ip = kzalloc(sizeof(struct adc_auto_input), GFP_KERNEL);
+	if (!auto_ip) {
+		dev_err(di->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	auto_ip->mux = BTEMP_BALL;
+	auto_ip->freq = MS500;
+	auto_ip->min = ab5500_btemp_temp_to_volt(di,
+				BTEMP_THERMAL_HIGH_LIMIT_62);
+	auto_ip->max = ab5500_btemp_temp_to_volt(di,
+				BTEMP_THERMAL_LOW_LIMIT);
+	auto_ip->auto_adc_callback = ab5500_btemp_bat_temp_trig;
+	di->gpadc_auto = auto_ip;
+	ret = ab5500_gpadc_convert_auto(di->gpadc, di->gpadc_auto);
+	if (ret)
+		dev_err(di->dev,
+			"failed to set auto trigger for battery temp\n");
+	return ret;
+}
+
+#if defined(CONFIG_PM)
+static int ab5500_btemp_resume(struct platform_device *pdev)
+{
+	struct ab5500_btemp *di = platform_get_drvdata(pdev);
+
+	if (di->events.usb_conn)
+		ab5500_btemp_periodic(di, true);
+
+	return 0;
+}
+
+static int ab5500_btemp_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab5500_btemp *di = platform_get_drvdata(pdev);
+
+	if (di->events.usb_conn)
+		ab5500_btemp_periodic(di, false);
+
+	return 0;
+}
+#else
+#define ab5500_btemp_suspend      NULL
+#define ab5500_btemp_resume       NULL
+#endif
+
+static int __devexit ab5500_btemp_remove(struct platform_device *pdev)
+{
+	struct ab5500_btemp *di = platform_get_drvdata(pdev);
+	int i, irq;
+
+	/* Disable interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_btemp_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_btemp_irq[i].name);
+		free_irq(irq, di);
+	}
+
+	/* Delete the work queue */
+	destroy_workqueue(di->btemp_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->btemp_psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di->gpadc_auto);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit ab5500_btemp_probe(struct platform_device *pdev)
+{
+	int irq, i, ret = 0;
+	struct abx500_bm_plat_data *plat_data;
+
+	struct ab5500_btemp *di =
+		kzalloc(sizeof(struct ab5500_btemp), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	di->gpadc = ab5500_gpadc_get("ab5500-adc.0");
+
+	plat_data = pdev->dev.platform_data;
+	di->pdata = plat_data->btemp;
+	di->bat = plat_data->battery;
+
+	/* get btemp specific platform data */
+	if (!di->pdata) {
+		dev_err(di->dev, "no btemp platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+
+	/* get battery specific platform data */
+	if (!di->bat) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+
+	/* BTEMP supply */
+	di->btemp_psy.name = "ab5500_btemp";
+	di->btemp_psy.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->btemp_psy.properties = ab5500_btemp_props;
+	di->btemp_psy.num_properties = ARRAY_SIZE(ab5500_btemp_props);
+	di->btemp_psy.get_property = ab5500_btemp_get_property;
+	di->btemp_psy.supplied_to = di->pdata->supplied_to;
+	di->btemp_psy.num_supplicants = di->pdata->num_supplicants;
+	di->btemp_psy.external_power_changed =
+		ab5500_btemp_external_power_changed;
+
+
+	/* Create a work queue for the btemp */
+	di->btemp_wq =
+		create_singlethread_workqueue("ab5500_btemp_wq");
+	if (di->btemp_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for measuring temperature periodically */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->btemp_periodic_work,
+		ab5500_btemp_periodic_work);
+
+	/* Get Chip ID of the ABB ASIC  */
+	ret = abx500_get_chip_id(di->dev);
+	if (ret < 0) {
+		dev_err(di->dev, "failed to get chip ID\n");
+		goto free_btemp_wq;
+	}
+	di->chip_id = ret;
+	dev_dbg(di->dev, "ab5500 CID is: 0x%02x\n",
+		di->chip_id);
+
+	/* Identify the battery */
+	if (ab5500_btemp_id(di) < 0)
+		dev_warn(di->dev, "failed to identify the battery\n");
+
+	/* Measure temperature once initially */
+	di->bat_temp = ab5500_btemp_measure_temp(di);
+	di->bat->temp_now = di->bat_temp;
+
+	/* Register BTEMP power supply class */
+	ret = power_supply_register(di->dev, &di->btemp_psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register BTEMP psy\n");
+		goto free_btemp_wq;
+	}
+
+	/* Register interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_btemp_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_btemp_irq[i].name);
+		ret = request_threaded_irq(irq, NULL, ab5500_btemp_irq[i].isr,
+			IRQF_SHARED | IRQF_NO_SUSPEND,
+			ab5500_btemp_irq[i].name, di);
+
+		if (ret) {
+			dev_err(di->dev, "failed to request %s IRQ %d: %d\n"
+				, ab5500_btemp_irq[i].name, irq, ret);
+			goto free_irq;
+		}
+		dev_dbg(di->dev, "Requested %s IRQ %d: %d\n",
+			ab5500_btemp_irq[i].name, irq, ret);
+	}
+
+	if (!di->bat->auto_trig) {
+		/* Schedule monitoring work only if battery type is known */
+		if (di->bat->batt_id != BATTERY_UNKNOWN)
+			queue_delayed_work(di->btemp_wq, &di->btemp_periodic_work, 0);
+	} else {
+		ret = ab5500_btemp_auto_temp(di);
+		if (ret) {
+			dev_err(di->dev,
+				"failed to register auto trigger for battery temp\n");
+			goto free_irq;
+		}
+	}
+
+	platform_set_drvdata(pdev, di);
+	list_add_tail(&di->node, &ab5500_btemp_list);
+
+	dev_info(di->dev, "probe success\n");
+	return ret;
+
+free_irq:
+	power_supply_unregister(&di->btemp_psy);
+
+	/* We also have to free all successfully registered irqs */
+	for (i = i - 1; i >= 0; i--) {
+		irq = platform_get_irq_byname(pdev, ab5500_btemp_irq[i].name);
+		free_irq(irq, di);
+	}
+free_btemp_wq:
+	destroy_workqueue(di->btemp_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab5500_btemp_driver = {
+	.probe = ab5500_btemp_probe,
+	.remove = __devexit_p(ab5500_btemp_remove),
+	.suspend = ab5500_btemp_suspend,
+	.resume = ab5500_btemp_resume,
+	.driver = {
+		.name = "ab5500-btemp",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_btemp_init(void)
+{
+	return platform_driver_register(&ab5500_btemp_driver);
+}
+
+static void __exit ab5500_btemp_exit(void)
+{
+	platform_driver_unregister(&ab5500_btemp_driver);
+}
+
+subsys_initcall_sync(ab5500_btemp_init);
+module_exit(ab5500_btemp_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
+MODULE_ALIAS("platform:ab5500-btemp");
+MODULE_DESCRIPTION("AB5500 battery temperature driver");
diff --git a/drivers/power/ab5500_charger.c b/drivers/power/ab5500_charger.c
new file mode 100644
index 00000000000..b90c51a4f31
--- /dev/null
+++ b/drivers/power/ab5500_charger.c
@@ -0,0 +1,1820 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Charger driver for AB5500
+ *
+ * License Terms: GNU General Public License v2
+ * Authors:
+ *	Johan Palsson <johan.palsson@stericsson.com>
+ *	Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/completion.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+#include <linux/mfd/abx500/ab5500-bm.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+#include <linux/mfd/abx500/ux500_chargalg.h>
+#include <linux/usb/otg.h>
+
+/* Charger constants */
+#define NO_PW_CONN			0
+#define USB_PW_CONN			2
+
+/* HW failure constants */
+#define VBUS_CH_NOK			0x0A
+#define VBUS_OVV_TH			0x06
+
+/* AB5500 Charger constants */
+#define AB5500_USB_LINK_STATUS		0x78
+#define CHARGER_REV_SUP			0x10
+#define SW_EOC				0x40
+#define USB_CHAR_DET			0x02
+#define VBUS_RISING			0x20
+#define VBUS_FALLING			0x40
+#define USB_LINK_UPDATE			0x02
+#define USB_CH_TH_PROT_LOW		0x02
+#define USB_CH_TH_PROT_HIGH		0x01
+#define USB_ID_HOST_DET_ENA_MASK	0x02
+#define USB_ID_HOST_DET_ENA		0x02
+#define USB_ID_DEVICE_DET_ENA_MASK	0x01
+#define USB_ID_DEVICE_DET_ENA		0x01
+#define CHARGER_ISET_IN_1_1A		0x0C
+#define LED_ENABLE			0x01
+#define RESET				0x00
+#define SSW_ENABLE_REBOOT		0x80
+#define SSW_REBOOT_EN			0x40
+#define SSW_CONTROL_AUTOC		0x04
+#define SSW_PSEL_480S			0x00
+
+/* UsbLineStatus register - usb types */
+enum ab5500_charger_link_status {
+	USB_STAT_NOT_CONFIGURED,
+	USB_STAT_STD_HOST_NC,
+	USB_STAT_STD_HOST_C_NS,
+	USB_STAT_STD_HOST_C_S,
+	USB_STAT_HOST_CHG_NM,
+	USB_STAT_HOST_CHG_HS,
+	USB_STAT_HOST_CHG_HS_CHIRP,
+	USB_STAT_DEDICATED_CHG,
+	USB_STAT_ACA_RID_A,
+	USB_STAT_ACA_RID_B,
+	USB_STAT_ACA_RID_C_NM,
+	USB_STAT_ACA_RID_C_HS,
+	USB_STAT_ACA_RID_C_HS_CHIRP,
+	USB_STAT_HM_IDGND,
+	USB_STAT_RESERVED,
+	USB_STAT_NOT_VALID_LINK,
+};
+
+enum ab5500_usb_state {
+	AB5500_BM_USB_STATE_RESET_HS,	/* HighSpeed Reset */
+	AB5500_BM_USB_STATE_RESET_FS,	/* FullSpeed/LowSpeed Reset */
+	AB5500_BM_USB_STATE_CONFIGURED,
+	AB5500_BM_USB_STATE_SUSPEND,
+	AB5500_BM_USB_STATE_RESUME,
+	AB5500_BM_USB_STATE_MAX,
+};
+
+/* VBUS input current limits supported in AB5500 in mA */
+#define USB_CH_IP_CUR_LVL_0P05		50
+#define USB_CH_IP_CUR_LVL_0P09		98
+#define USB_CH_IP_CUR_LVL_0P19		193
+#define USB_CH_IP_CUR_LVL_0P29		290
+#define USB_CH_IP_CUR_LVL_0P38		380
+#define USB_CH_IP_CUR_LVL_0P45		450
+#define USB_CH_IP_CUR_LVL_0P5		500
+#define USB_CH_IP_CUR_LVL_0P6		600
+#define USB_CH_IP_CUR_LVL_0P7		700
+#define USB_CH_IP_CUR_LVL_0P8		800
+#define USB_CH_IP_CUR_LVL_0P9		900
+#define USB_CH_IP_CUR_LVL_1P0		1000
+#define USB_CH_IP_CUR_LVL_1P1		1100
+#define USB_CH_IP_CUR_LVL_1P3		1300
+#define USB_CH_IP_CUR_LVL_1P4		1400
+#define USB_CH_IP_CUR_LVL_1P5		1500
+
+#define to_ab5500_charger_usb_device_info(x) container_of((x), \
+	struct ab5500_charger, usb_chg)
+
+/**
+ * struct ab5500_charger_interrupts - ab5500 interupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab5500_charger_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+struct ab5500_charger_info {
+	int charger_connected;
+	int charger_online;
+	int charger_voltage;
+	int cv_active;
+	bool wd_expired;
+};
+
+struct ab5500_charger_event_flags {
+	bool usb_thermal_prot;
+	bool vbus_ovv;
+	bool usbchargernotok;
+	bool vbus_collapse;
+};
+
+struct ab5500_charger_usb_state {
+	bool usb_changed;
+	int usb_current;
+	enum ab5500_usb_state state;
+	spinlock_t usb_lock;
+};
+
+/**
+ * struct ab5500_charger - ab5500 Charger device information
+ * @dev:		Pointer to the structure device
+ * @chip_id:		Chip-Id of the ab5500
+ * @max_usb_in_curr:	Max USB charger input current
+ * @vbus_detected:	VBUS detected
+ * @vbus_detected_start:
+ *			VBUS detected during startup
+ * @parent:		Pointer to the struct ab5500
+ * @gpadc:		Pointer to the struct gpadc
+ * @pdata:		Pointer to the ab5500_charger platform data
+ * @bat:		Pointer to the ab5500_bm platform data
+ * @flags:		Structure for information about events triggered
+ * @usb_state:		Structure for usb stack information
+ * @usb_chg:		USB charger power supply
+ * @ac:			Structure that holds the AC charger properties
+ * @usb:		Structure that holds the USB charger properties
+ * @charger_wq:		Work queue for the IRQs and checking HW state
+ * @check_hw_failure_work:	Work for checking HW state
+ * @check_usbchgnotok_work:	Work for checking USB charger not ok status
+ * @ac_work:			Work for checking AC charger connection
+ * @detect_usb_type_work:	Work for detecting the USB type connected
+ * @usb_link_status_work:	Work for checking the new USB link status
+ * @usb_state_changed_work:	Work for checking USB state
+ * @check_main_thermal_prot_work:
+ *				Work for checking Main thermal status
+ * @check_usb_thermal_prot_work:
+ *				Work for checking USB thermal status
+ * @ otg:			pointer to struct otg_transceiver, used to
+ *				notify the current during a standard host
+ *				charger.
+ * @nb:				structture of type notifier_block, which has
+ *				a function pointer referenced by usb driver.
+ */
+struct ab5500_charger {
+	struct device *dev;
+	u8 chip_id;
+	int max_usb_in_curr;
+	bool vbus_detected;
+	bool vbus_detected_start;
+	struct ab5500 *parent;
+	struct ab5500_gpadc *gpadc;
+	struct abx500_charger_platform_data *pdata;
+	struct abx500_bm_data *bat;
+	struct ab5500_charger_event_flags flags;
+	struct ab5500_charger_usb_state usb_state;
+	struct ux500_charger usb_chg;
+	struct ab5500_charger_info usb;
+	struct workqueue_struct *charger_wq;
+	struct delayed_work check_hw_failure_work;
+	struct delayed_work check_usbchgnotok_work;
+	struct work_struct detect_usb_type_work;
+	struct work_struct usb_link_status_work;
+	struct work_struct usb_state_changed_work;
+	struct work_struct check_usb_thermal_prot_work;
+	struct otg_transceiver *otg;
+	struct notifier_block nb;
+};
+
+/* USB properties */
+static enum power_supply_property ab5500_charger_usb_props[] = {
+	POWER_SUPPLY_PROP_HEALTH,
+	POWER_SUPPLY_PROP_CURRENT_AVG,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+};
+
+/**
+ * ab5500_charger_get_vbus_voltage() - get vbus voltage
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * This function returns the vbus voltage.
+ * Returns vbus voltage (on success)
+ */
+static int ab5500_charger_get_vbus_voltage(struct ab5500_charger *di)
+{
+	int vch;
+
+	/* Only measure voltage if the charger is connected */
+	if (di->usb.charger_connected) {
+		vch = ab5500_gpadc_convert(di->gpadc, VBUS_V);
+		if (vch < 0)
+			dev_err(di->dev, "%s gpadc conv failed\n", __func__);
+	} else {
+		vch = 0;
+	}
+	return vch;
+}
+
+/**
+ * ab5500_charger_get_usb_current() - get usb charger current
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * This function returns the usb charger current.
+ * Returns usb current (on success) and error code on failure
+ */
+static int ab5500_charger_get_usb_current(struct ab5500_charger *di)
+{
+	int ich;
+
+	/* Only measure current if the charger is online */
+	if (di->usb.charger_online) {
+		ich = ab5500_gpadc_convert(di->gpadc, USB_CHARGER_C);
+		if (ich < 0)
+			dev_err(di->dev, "%s gpadc conv failed\n", __func__);
+	} else {
+		ich = 0;
+	}
+	return ich;
+}
+
+/**
+ * ab5500_charger_detect_chargers() - Detect the connected chargers
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Returns the type of charger connected.
+ * For USB it will not mean we can actually charge from it
+ * but that there is a USB cable connected that we have to
+ * identify. This is used during startup when we don't get
+ * interrupts of the charger detection
+ *
+ * Returns an integer value, that means,
+ * NO_PW_CONN  no power supply is connected
+ * USB_PW_CONN  if the USB power supply is connected
+ */
+static int ab5500_charger_detect_chargers(struct ab5500_charger *di)
+{
+	int result = NO_PW_CONN;
+	int ret;
+	u8 val;
+	/* Check for USB charger */
+	/*
+	 * TODO: Since there are no status register validating by
+	 * reading the IT souce registers
+	 */
+	ret = abx500_get_register_interruptible(di->dev, AB5500_BANK_IT,
+		AB5500_IT_SOURCE8, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return ret;
+	}
+
+	if (val & VBUS_RISING)
+		result |= USB_PW_CONN;
+	else if (val & VBUS_FALLING)
+		result = NO_PW_CONN;
+
+	return result;
+}
+
+/**
+ * ab5500_charger_max_usb_curr() - get the max curr for the USB type
+ * @di:			pointer to the ab5500_charger structure
+ * @link_status:	the identified USB type
+ *
+ * Get the maximum current that is allowed to be drawn from the host
+ * based on the USB type.
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab5500_charger_max_usb_curr(struct ab5500_charger *di,
+	enum ab5500_charger_link_status link_status)
+{
+	int ret = 0;
+
+	switch (link_status) {
+	case USB_STAT_STD_HOST_NC:
+	case USB_STAT_STD_HOST_C_NS:
+	case USB_STAT_STD_HOST_C_S:
+		dev_dbg(di->dev, "USB Type - Standard host is "
+			"detected through USB driver\n");
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P09;
+		break;
+	case USB_STAT_HOST_CHG_HS_CHIRP:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
+		break;
+	case USB_STAT_HOST_CHG_HS:
+	case USB_STAT_ACA_RID_C_HS:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P9;
+		break;
+	case USB_STAT_ACA_RID_A:
+		/*
+		 * Dedicated charger level minus maximum current accessory
+		 * can consume (300mA). Closest level is 1100mA
+		 */
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P1;
+		break;
+	case USB_STAT_ACA_RID_B:
+		/*
+		 * Dedicated charger level minus 120mA (20mA for ACA and
+		 * 100mA for potential accessory). Closest level is 1300mA
+		 */
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P3;
+		break;
+	case USB_STAT_DEDICATED_CHG:
+	case USB_STAT_HOST_CHG_NM:
+	case USB_STAT_ACA_RID_C_HS_CHIRP:
+	case USB_STAT_ACA_RID_C_NM:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_1P5;
+		break;
+	case USB_STAT_RESERVED:
+		/*
+		 * This state is used to indicate that VBUS has dropped below
+		 * the detection level 4 times in a row. This is due to the
+		 * charger output current is set to high making the charger
+		 * voltage collapse. This have to be propagated through to
+		 * chargalg. This is done using the property
+		 * POWER_SUPPLY_PROP_CURRENT_AVG = 1
+		 */
+		di->flags.vbus_collapse = true;
+		dev_dbg(di->dev, "USB Type - USB_STAT_RESERVED "
+			"VBUS has collapsed\n");
+		ret = -1;
+		break;
+	case USB_STAT_HM_IDGND:
+	case USB_STAT_NOT_CONFIGURED:
+	case USB_STAT_NOT_VALID_LINK:
+		dev_err(di->dev, "USB Type - Charging not allowed\n");
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		ret = -ENXIO;
+		break;
+	default:
+		dev_err(di->dev, "USB Type - Unknown\n");
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		ret = -ENXIO;
+		break;
+	};
+
+	dev_dbg(di->dev, "USB Type - 0x%02x MaxCurr: %d",
+		link_status, di->max_usb_in_curr);
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_read_usb_type() - read the type of usb connected
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Detect the type of the plugged USB
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab5500_charger_read_usb_type(struct ab5500_charger *di)
+{
+	int ret;
+	u8 val;
+
+	ret = abx500_get_register_interruptible(di->dev, AB5500_BANK_USB,
+		AB5500_USB_LINE_STATUS, &val);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return ret;
+	}
+
+	/* get the USB type */
+	val = (val & AB5500_USB_LINK_STATUS) >> 3;
+	ret = ab5500_charger_max_usb_curr(di,
+		(enum ab5500_charger_link_status) val);
+
+	return ret;
+}
+
+static int ab5500_charger_voltage_map[] = {
+	3500 ,
+	3525 ,
+	3550 ,
+	3575 ,
+	3600 ,
+	3625 ,
+	3650 ,
+	3675 ,
+	3700 ,
+	3725 ,
+	3750 ,
+	3775 ,
+	3800 ,
+	3825 ,
+	3850 ,
+	3875 ,
+	3900 ,
+	3925 ,
+	3950 ,
+	3975 ,
+	4000 ,
+	4025 ,
+	4050 ,
+	4060 ,
+	4070 ,
+	4080 ,
+	4090 ,
+	4100 ,
+	4110 ,
+	4120 ,
+	4130 ,
+	4140 ,
+	4150 ,
+	4160 ,
+	4170 ,
+	4180 ,
+	4190 ,
+	4200 ,
+	4210 ,
+	4220 ,
+	4230 ,
+	4240 ,
+	4250 ,
+	4260 ,
+	4270 ,
+	4280 ,
+	4290 ,
+	4300 ,
+	4310 ,
+	4320 ,
+	4330 ,
+	4340 ,
+	4350 ,
+	4360 ,
+	4370 ,
+	4380 ,
+	4390 ,
+	4400 ,
+	4410 ,
+	4420 ,
+	4430 ,
+	4440 ,
+	4450 ,
+	4460 ,
+	4470 ,
+	4480 ,
+	4490 ,
+	4500 ,
+	4510 ,
+	4520 ,
+	4530 ,
+	4540 ,
+	4550 ,
+	4560 ,
+	4570 ,
+	4580 ,
+	4590 ,
+	4600 ,
+};
+
+/*
+ * This array maps the raw hex value to charger current used by the ab5500
+ * Values taken from the AB5500 product specification manual
+ */
+static int ab5500_charger_current_map[] = {
+	100 ,
+	200 ,
+	300 ,
+	400 ,
+	500 ,
+	600 ,
+	700 ,
+	800 ,
+	900 ,
+	1000,
+	1100,
+	1200,
+	1300,
+	1400,
+	1500,
+	1500,
+};
+
+static int ab5500_icsr_current_map[] = {
+	50,
+	93,
+	193,
+	290,
+	380,
+	450,
+	500 ,
+	600 ,
+	700 ,
+	800 ,
+	900 ,
+	1000,
+	1100,
+	1300,
+	1400,
+	1500,
+};
+
+static int ab5500_cvrec_voltage_map[] = {
+	3300,
+	3325,
+	3350,
+	3375,
+	3400,
+	3425,
+	3450,
+	3475,
+	3500,
+	3525,
+	3550,
+	3575,
+	3600,
+	3625,
+	3650,
+	3675,
+	3700,
+	3725,
+	3750,
+	3775,
+	3800,
+	3825,
+	3850,
+	3875,
+	3900,
+	3925,
+	4000,
+	4025,
+	4050,
+	4075,
+	4100,
+	4125,
+	4150,
+	4175,
+	4200,
+	4225,
+	4250,
+	4275,
+	4300,
+	4325,
+	4350,
+	4375,
+	4400,
+	4425,
+	4450,
+	4475,
+	4500,
+	4525,
+	4550,
+	4575,
+	4600,
+};
+
+static int ab5500_cvrec_voltage_to_regval(int voltage)
+{
+	int i;
+
+	/* Special case for voltage below 3.3V */
+	if (voltage < ab5500_cvrec_voltage_map[0])
+		return 0;
+
+	for (i = 1; i < ARRAY_SIZE(ab5500_cvrec_voltage_map); i++) {
+		if (voltage < ab5500_cvrec_voltage_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab5500_cvrec_voltage_map) - 1;
+	if (voltage == ab5500_cvrec_voltage_map[i])
+		return i;
+	else
+		return -1;
+}
+
+static int ab5500_voltage_to_regval(int voltage)
+{
+	int i;
+
+	/* Special case for voltage below 3.3V */
+	if (voltage < ab5500_charger_voltage_map[0])
+		return 0;
+
+	for (i = 1; i < ARRAY_SIZE(ab5500_charger_voltage_map); i++) {
+		if (voltage < ab5500_charger_voltage_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab5500_charger_voltage_map) - 1;
+	if (voltage == ab5500_charger_voltage_map[i])
+		return i;
+	else
+		return -1;
+}
+
+static int ab5500_icsr_curr_to_regval(int curr)
+{
+	int i;
+
+	if (curr < ab5500_icsr_current_map[0])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(ab5500_icsr_current_map); i++) {
+		if (curr < ab5500_icsr_current_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab5500_icsr_current_map) - 1;
+	if (curr == ab5500_icsr_current_map[i])
+		return i;
+	else
+		return -1;
+}
+
+static int ab5500_current_to_regval(int curr)
+{
+	int i;
+
+	if (curr < ab5500_charger_current_map[0])
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(ab5500_charger_current_map); i++) {
+		if (curr < ab5500_charger_current_map[i])
+			return i - 1;
+	}
+
+	/* If not last element, return error */
+	i = ARRAY_SIZE(ab5500_charger_current_map) - 1;
+	if (curr == ab5500_charger_current_map[i])
+		return i;
+	else
+		return -1;
+}
+
+/**
+ * ab5500_charger_get_usb_cur() - get usb current
+ * @di:		pointer to the ab5500_charger structre
+ *
+ * The usb stack provides the maximum current that can be drawn from
+ * the standard usb host. This will be in mA.
+ * This function converts current in mA to a value that can be written
+ * to the register. Returns -1 if charging is not allowed
+ */
+static int ab5500_charger_get_usb_cur(struct ab5500_charger *di)
+{
+	switch (di->usb_state.usb_current) {
+	case 50:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		break;
+	case 100:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P09;
+		break;
+	case 200:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P19;
+		break;
+	case 300:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P29;
+		break;
+	case 400:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P38;
+		break;
+	case 500:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P5;
+		break;
+	default:
+		di->max_usb_in_curr = USB_CH_IP_CUR_LVL_0P05;
+		return -1;
+		break;
+	};
+	return 0;
+}
+
+/**
+ * ab5500_charger_set_vbus_in_curr() - set VBUS input current limit
+ * @di:		pointer to the ab5500_charger structure
+ * @ich_in:	charger input current limit
+ *
+ * Sets the current that can be drawn from the USB host
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_set_vbus_in_curr(struct ab5500_charger *di,
+		int ich_in)
+{
+	int ret;
+	int input_curr_index;
+	int min_value;
+
+	/* We should always use to lowest current limit */
+	min_value = min(di->bat->chg_params->usb_curr_max, ich_in);
+
+	input_curr_index = ab5500_icsr_curr_to_regval(min_value);
+	if (input_curr_index < 0) {
+		dev_err(di->dev, "VBUS input current limit too high\n");
+		return -ENXIO;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB5500_BANK_CHG,
+		AB5500_ICSR, input_curr_index);
+	if (ret)
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_usb_en() - enable usb charging
+ * @di:		pointer to the ab5500_charger structure
+ * @enable:	enable/disable flag
+ * @vset:	charging voltage
+ * @ich_out:	charger output current
+ *
+ * Enable/Disable USB charging and turns on/off the charging led respectively.
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_usb_en(struct ux500_charger *charger,
+	int enable, int vset, int ich_out)
+{
+	int ret;
+	int volt_index;
+	int curr_index;
+
+	struct ab5500_charger *di = to_ab5500_charger_usb_device_info(charger);
+
+	if (enable) {
+		/* Check if USB is connected */
+		if (!di->usb.charger_connected) {
+			dev_err(di->dev, "USB charger not connected\n");
+			return -ENXIO;
+		}
+
+		/* Enable USB charging */
+		dev_dbg(di->dev, "Enable USB: %dmV %dmA\n", vset, ich_out);
+
+		volt_index = ab5500_voltage_to_regval(vset);
+		curr_index = ab5500_current_to_regval(ich_out) ;
+
+		/* ChVoltLevel: max voltage upto which battery can be charged */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_VSRC, (u8) volt_index);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		/* current that can be drawn from the usb */
+		ret = ab5500_charger_set_vbus_in_curr(di, ich_out);
+		if (ret) {
+			dev_err(di->dev, "%s setting icsr failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		/* ChOutputCurentLevel: protected output current */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_OCSRV, (u8) curr_index);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		/*
+		 * Battery voltage when charging should be resumed after
+		 * completion of charging
+		 */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_CVREC,
+				ab5500_cvrec_voltage_to_regval(
+			di->bat->bat_type[di->bat->batt_id].recharge_vol));
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+		/*
+		 * Battery temperature:
+		 * Input to the TBDATA register corresponds to the battery
+		 * temperature(temp being multiples of 2)
+		 * In order to obatain the value to be written to this reg
+		 * divide the temperature obtained from gpadc by 2
+		 */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_TBDATA,
+				di->bat->temp_now / 2);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		/* If success power on charging LED indication */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_LEDT, LED_ENABLE);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		/*
+		 * Register DCIOCURRENT is one among the charging watchdog
+		 * rekick sequence, hence irrespective of usb charging this
+		 * register will have to be written.
+		 */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_DCIOCURRENT,
+				RESET);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+
+		di->usb.charger_online = 1;
+	} else {
+		/* ChVoltLevel: max voltage upto which battery can be charged */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_VSRC, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+		/* USBChInputCurr: current that can be drawn from the usb */
+		ret = ab5500_charger_set_vbus_in_curr(di, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s resetting icsr failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+		/* If success power off charging LED indication */
+		ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_LEDT, RESET);
+		if (ret) {
+			dev_err(di->dev, "%s write failed %d\n",
+					__func__, __LINE__);
+			return ret;
+		}
+		di->usb.charger_online = 0;
+		di->usb.wd_expired = false;
+		dev_dbg(di->dev, "%s Disabled USB charging\n", __func__);
+	}
+	power_supply_changed(&di->usb_chg.psy);
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_watchdog_kick() - kick charger watchdog
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Kick charger watchdog
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_watchdog_kick(struct ux500_charger *charger)
+{
+	int ret;
+	struct ab5500_charger *di;
+	int volt_index, curr_index;
+	u8 value = 0;
+
+	/* TODO: update */
+	if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+		di = to_ab5500_charger_usb_device_info(charger);
+	else
+		return -ENXIO;
+
+	ret = abx500_get_register_interruptible(di->dev, AB5500_BANK_STARTUP,
+		AB5500_MCB, &value);
+	if (ret)
+		dev_err(di->dev, "Failed to read!\n");
+
+	value = value | (SSW_ENABLE_REBOOT | SSW_REBOOT_EN |
+			SSW_CONTROL_AUTOC | SSW_PSEL_480S);
+	ret = abx500_set_register_interruptible(di->dev, AB5500_BANK_STARTUP,
+		AB5500_MCB, value);
+	if (ret)
+		dev_err(di->dev, "Failed to kick WD!\n");
+
+	volt_index = ab5500_voltage_to_regval(
+			di->bat->bat_type[di->bat->batt_id].normal_vol_lvl);
+	curr_index = ab5500_current_to_regval(di->max_usb_in_curr);
+
+	/* ChVoltLevel: max voltage upto which battery can be charged */
+	ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_VSRC, (u8) volt_index);
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+
+	/* current that can be drawn from the usb */
+	ret = ab5500_charger_set_vbus_in_curr(di, di->max_usb_in_curr);
+	if (ret) {
+		dev_err(di->dev, "%s setting icsr failed %d\n",
+				__func__, __LINE__);
+		return ret;
+	}
+
+	/* ChOutputCurentLevel: protected output current */
+	ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_OCSRV, (u8) curr_index);
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+
+	/*
+	 * Battery voltage when charging should be resumed after
+	 * completion of charging
+	 */
+	/* Charger_Vrechar[5:0] = '4.025 V' */
+	ret = abx500_set_register_interruptible(di->dev,
+			AB5500_BANK_CHG, AB5500_CVREC,
+			ab5500_cvrec_voltage_to_regval(
+			di->bat->bat_type[di->bat->batt_id].recharge_vol));
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+	/*
+	 * Battery temperature:
+	 * Input to the TBDATA register corresponds to the battery
+	 * temperature(temp being multiples of 2)
+	 * In order to obatain the value to be written to this reg
+	 * divide the temperature obtained from gpadc by 2
+	 */
+	ret = abx500_set_register_interruptible(di->dev,
+				AB5500_BANK_CHG, AB5500_TBDATA,
+				di->bat->temp_now / 2);
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+	/*
+	 * Register DCIOCURRENT is one among the charging watchdog
+	 * rekick sequence, hence irrespective of usb charging this
+	 * register will have to be written.
+	 */
+	ret = abx500_set_register_interruptible(di->dev,
+			AB5500_BANK_CHG, AB5500_DCIOCURRENT,
+			RESET);
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_update_charger_current() - update charger current
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Update the charger output current for the specified charger
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_update_charger_current(struct ux500_charger *charger,
+		int ich_out)
+{
+	int ret = 0;
+	int curr_index;
+	struct ab5500_charger *di;
+
+	if (charger->psy.type == POWER_SUPPLY_TYPE_USB)
+		di = to_ab5500_charger_usb_device_info(charger);
+	else
+		return -ENXIO;
+
+	curr_index = ab5500_current_to_regval(ich_out);
+	if (curr_index < 0) {
+		dev_err(di->dev,
+			"Charger current too high, "
+			"charging not started\n");
+		return -ENXIO;
+	}
+
+	ret = abx500_set_register_interruptible(di->dev, AB5500_BANK_CHG,
+		AB5500_OCSRV, (u8) curr_index);
+	if (ret) {
+		dev_err(di->dev, "%s write failed %d\n", __func__, __LINE__);
+		return ret;
+	}
+
+	return ret;
+}
+
+/**
+ * ab5500_charger_check_hw_failure_work() - check main charger failure
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the main charger status
+ */
+static void ab5500_charger_check_hw_failure_work(struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, check_hw_failure_work.work);
+
+	/* Check if the status bits for HW failure is still active */
+	if (di->flags.vbus_ovv) {
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_USB, AB5500_USB_PHY_STATUS,
+			&reg_value);
+		if (ret < 0) {
+			dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+			return;
+		}
+		if (!(reg_value & VBUS_OVV_TH)) {
+			di->flags.vbus_ovv = false;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+	}
+	/* If we still have a failure, schedule a new check */
+	if (di->flags.vbus_ovv) {
+		queue_delayed_work(di->charger_wq,
+			&di->check_hw_failure_work, round_jiffies(HZ));
+	}
+}
+
+/**
+ * ab5500_charger_detect_usb_type_work() - work to detect USB type
+ * @work:	Pointer to the work_struct structure
+ *
+ * Detect the type of USB plugged
+ */
+void ab5500_charger_detect_usb_type_work(struct work_struct *work)
+{
+	int ret;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, detect_usb_type_work);
+
+	/*
+	 * Since we can't be sure that the events are received
+	 * synchronously, we have the check if is
+	 * connected by reading the status register
+	 */
+	ret = ab5500_charger_detect_chargers(di);
+	if (ret < 0)
+		return;
+
+	if (!(ret & USB_PW_CONN)) {
+		di->vbus_detected = 0;
+		di->usb.charger_connected = 0;
+		power_supply_changed(&di->usb_chg.psy);
+	} else {
+		di->vbus_detected = 1;
+	}
+}
+
+/**
+ * ab5500_charger_usb_link_status_work() - work to detect USB type
+ * @work:	pointer to the work_struct structure
+ *
+ * Detect the type of USB plugged
+ */
+static void ab5500_charger_usb_link_status_work(struct work_struct *work)
+{
+	int ret;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, usb_link_status_work);
+
+	/*
+	 * Since we can't be sure that the events are received
+	 * synchronously, we have the check if  is
+	 * connected by reading the status register
+	 */
+	ret = ab5500_charger_detect_chargers(di);
+	if (ret < 0)
+		return;
+
+	if (!(ret & USB_PW_CONN)) {
+		di->vbus_detected = 0;
+		di->usb.charger_connected = 0;
+		power_supply_changed(&di->usb_chg.psy);
+	} else {
+		di->vbus_detected = 1;
+		ret = ab5500_charger_read_usb_type(di);
+		if (!ret) {
+			/* Update maximum input current */
+			ret = ab5500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr);
+			if (ret)
+				return;
+
+			di->usb.charger_connected = 1;
+			power_supply_changed(&di->usb_chg.psy);
+		} else if (ret == -ENXIO) {
+			/* No valid charger type detected */
+			di->usb.charger_connected = 0;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+	}
+}
+
+static void ab5500_charger_usb_state_changed_work(struct work_struct *work)
+{
+	int ret;
+	unsigned long flags;
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, usb_state_changed_work);
+
+	if (!di->vbus_detected)
+		return;
+
+	spin_lock_irqsave(&di->usb_state.usb_lock, flags);
+	di->usb_state.usb_changed = false;
+	spin_unlock_irqrestore(&di->usb_state.usb_lock, flags);
+
+	/*
+	 * wait for some time until you get updates from the usb stack
+	 * and negotiations are completed
+	 */
+	msleep(250);
+
+	if (di->usb_state.usb_changed)
+		return;
+
+	dev_dbg(di->dev, "%s USB state: 0x%02x mA: %d\n",
+		__func__, di->usb_state.state, di->usb_state.usb_current);
+
+	switch (di->usb_state.state) {
+	case AB5500_BM_USB_STATE_RESET_HS:
+	case AB5500_BM_USB_STATE_RESET_FS:
+	case AB5500_BM_USB_STATE_SUSPEND:
+	case AB5500_BM_USB_STATE_MAX:
+		di->usb.charger_connected = 0;
+		power_supply_changed(&di->usb_chg.psy);
+		break;
+
+	case AB5500_BM_USB_STATE_RESUME:
+		/*
+		 * when suspend->resume there should be delay
+		 * of 1sec for enabling charging
+		 */
+		msleep(1000);
+		/* Intentional fall through */
+	case AB5500_BM_USB_STATE_CONFIGURED:
+		/*
+		 * USB is configured, enable charging with the charging
+		 * input current obtained from USB driver
+		 */
+		if (!ab5500_charger_get_usb_cur(di)) {
+			/* Update maximum input current */
+			ret = ab5500_charger_set_vbus_in_curr(di,
+					di->max_usb_in_curr);
+			if (ret)
+				return;
+
+			di->usb.charger_connected = 1;
+			power_supply_changed(&di->usb_chg.psy);
+		}
+		break;
+
+	default:
+		break;
+	};
+}
+
+/**
+ * ab5500_charger_check_usbchargernotok_work() - check USB chg not ok status
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the USB charger Not OK status
+ */
+static void ab5500_charger_check_usbchargernotok_work(struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+	bool prev_status;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, check_usbchgnotok_work.work);
+
+	/* Check if the status bit for usbchargernotok is still active */
+	ret = abx500_get_register_interruptible(di->dev,
+		AB5500_BANK_USB, AB5500_CHGFSM_CHARGER_DETECT, &reg_value);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return;
+	}
+	prev_status = di->flags.usbchargernotok;
+
+	if (reg_value & VBUS_CH_NOK) {
+		di->flags.usbchargernotok = true;
+		/* Check again in 1sec */
+		queue_delayed_work(di->charger_wq,
+			&di->check_usbchgnotok_work, HZ);
+	} else {
+		di->flags.usbchargernotok = false;
+		di->flags.vbus_collapse = false;
+	}
+
+	if (prev_status != di->flags.usbchargernotok)
+		power_supply_changed(&di->usb_chg.psy);
+}
+
+/**
+ * ab5500_charger_check_usb_thermal_prot_work() - check usb thermal status
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the USB thermal prot status
+ */
+static void ab5500_charger_check_usb_thermal_prot_work(
+	struct work_struct *work)
+{
+	int ret;
+	u8 reg_value;
+
+	struct ab5500_charger *di = container_of(work,
+		struct ab5500_charger, check_usb_thermal_prot_work);
+
+	/* Check if the status bit for usb_thermal_prot is still active */
+	/* TODO: Interrupt source reg 15 bit 4 */
+	ret = abx500_get_register_interruptible(di->dev,
+		AB5500_BANK_USB, AB5500_CHGFSM_USB_BTEMP_CURR_LIM, &reg_value);
+	if (ret < 0) {
+		dev_err(di->dev, "%s ab5500 read failed\n", __func__);
+		return;
+	}
+	if (reg_value & USB_CH_TH_PROT_LOW || reg_value & USB_CH_TH_PROT_HIGH)
+		di->flags.usb_thermal_prot = true;
+	else
+		di->flags.usb_thermal_prot = false;
+
+	power_supply_changed(&di->usb_chg.psy);
+}
+
+/**
+ * ab5500_charger_vbusdetf_handler() - VBUS falling detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_vbusdetf_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "VBUS falling detected\n");
+	queue_work(di->charger_wq, &di->detect_usb_type_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_vbusdetr_handler() - VBUS rising detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_vbusdetr_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	di->vbus_detected = true;
+	dev_dbg(di->dev, "VBUS rising detected\n");
+	queue_work(di->charger_wq, &di->detect_usb_type_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_usblinkstatus_handler() - USB link status has changed
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_usblinkstatus_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "USB link status changed\n");
+
+	if (!di->usb.charger_online)
+		queue_work(di->charger_wq, &di->usb_link_status_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_usbchthprotr_handler() - Die temp is above usb charger
+ * thermal protection threshold
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_usbchthprotr_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev,
+		"Die temp above USB charger thermal protection threshold\n");
+	queue_work(di->charger_wq, &di->check_usb_thermal_prot_work);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_usbchargernotokr_handler() - USB charger not ok detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_usbchargernotokr_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "Not allowed USB charger detected\n");
+	queue_delayed_work(di->charger_wq, &di->check_usbchgnotok_work, 0);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_chwdexp_handler() - Charger watchdog expired
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_chwdexp_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "Charger watchdog expired\n");
+
+	/*
+	 * The charger that was online when the watchdog expired
+	 * needs to be restarted for charging to start again
+	 */
+	if (di->usb.charger_online) {
+		di->usb.wd_expired = true;
+		power_supply_changed(&di->usb_chg.psy);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_vbusovv_handler() - VBUS overvoltage detected
+ * @irq:       interrupt number
+ * @_di:       pointer to the ab5500_charger structure
+ *
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab5500_charger_vbusovv_handler(int irq, void *_di)
+{
+	struct ab5500_charger *di = _di;
+
+	dev_dbg(di->dev, "VBUS overvoltage detected\n");
+	di->flags.vbus_ovv = true;
+	power_supply_changed(&di->usb_chg.psy);
+
+	/* Schedule a new HW failure check */
+	queue_delayed_work(di->charger_wq, &di->check_hw_failure_work, 0);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * ab5500_charger_usb_get_property() - get the usb properties
+ * @psy:        pointer to the power_supply structure
+ * @psp:        pointer to the power_supply_property structure
+ * @val:        pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the usb
+ * properties by reading the sysfs files.
+ * USB properties are online, present and voltage.
+ * online:     usb charging is in progress or not
+ * present:    presence of the usb
+ * voltage:    vbus voltage
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab5500_charger_usb_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab5500_charger *di;
+
+	di = to_ab5500_charger_usb_device_info(psy_to_ux500_charger(psy));
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_HEALTH:
+		if (di->flags.usbchargernotok)
+			val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		else if (di->usb.wd_expired)
+			val->intval = POWER_SUPPLY_HEALTH_DEAD;
+		else if (di->flags.usb_thermal_prot)
+			val->intval = POWER_SUPPLY_HEALTH_OVERHEAT;
+		else if (di->flags.vbus_ovv)
+			val->intval = POWER_SUPPLY_HEALTH_OVERVOLTAGE;
+		else
+			val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		break;
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = di->usb.charger_online;
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		val->intval = di->usb.charger_connected;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		di->usb.charger_voltage = ab5500_charger_get_vbus_voltage(di);
+		val->intval = di->usb.charger_voltage * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = ab5500_charger_get_usb_current(di) * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_AVG:
+		/*
+		 * This property is used to indicate when VBUS has collapsed
+		 * due to too high output current from the USB charger
+		 */
+		if (di->flags.vbus_collapse)
+			val->intval = 1;
+		else
+			val->intval = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/**
+ * ab5500_charger_hw_registers() - Set up charger related registers
+ * @di:		pointer to the ab5500_charger structure
+ *
+ * Set up charger OVV, watchdog and maximum voltage registers as well as
+ * charging of the backup battery
+ */
+static int ab5500_charger_init_hw_registers(struct ab5500_charger *di)
+{
+	int ret = 0;
+
+	/* Enable ID Host and Device detection */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_USB, AB5500_USB_OTG_CTRL,
+			USB_ID_HOST_DET_ENA_MASK, USB_ID_HOST_DET_ENA);
+	if (ret) {
+		dev_err(di->dev, "failed to enable usb charger detection\n");
+		goto out;
+	}
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_USB, AB5500_USB_OTG_CTRL,
+			USB_ID_DEVICE_DET_ENA_MASK, USB_ID_DEVICE_DET_ENA);
+	if (ret) {
+		dev_err(di->dev, "failed to enable usb charger detection\n");
+		goto out;
+	}
+
+	/* Over current protection for reverse supply */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_CHG, AB5500_CREVS, CHARGER_REV_SUP,
+			CHARGER_REV_SUP);
+	if (ret) {
+		dev_err(di->dev,
+			"failed to enable over current protection for reverse supply\n");
+		goto out;
+	}
+
+	/* Enable SW EOC at flatcurrent detection */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_CHG, AB5500_CCTRL, SW_EOC, SW_EOC);
+	if (ret) {
+		dev_err(di->dev,
+			"failed to enable end of charge at flatcurrent detection\n");
+		goto out;
+	}
+out:
+	return ret;
+}
+
+/*
+ * ab5500 charger driver interrupts and their respective isr
+ */
+static struct ab5500_charger_interrupts ab5500_charger_irq[] = {
+	{"VBUS_FALLING", ab5500_charger_vbusdetf_handler},
+	{"VBUS_RISING", ab5500_charger_vbusdetr_handler},
+	{"USB_LINK_UPDATE", ab5500_charger_usblinkstatus_handler},
+	{"USB_CH_TH_PROTECTION", ab5500_charger_usbchthprotr_handler},
+	{"USB_CH_NOT_OK", ab5500_charger_usbchargernotokr_handler},
+	{"OVV", ab5500_charger_vbusovv_handler},
+	/* TODO: Interrupt missing, will be available in cut 2 */
+	/*{"CHG_SW_TIMER_OUT", ab5500_charger_chwdexp_handler},*/
+};
+
+static int ab5500_charger_usb_notifier_call(struct notifier_block *nb,
+		unsigned long event, void *power)
+{
+	struct ab5500_charger *di =
+		container_of(nb, struct ab5500_charger, nb);
+	enum ab5500_usb_state bm_usb_state;
+	unsigned mA = *((unsigned *)power);
+
+	if (event != USB_EVENT_VBUS) {
+		dev_dbg(di->dev, "not a standard host, returning\n");
+		return NOTIFY_DONE;
+	}
+
+	/* TODO: State is fabricate  here. See if charger really needs USB
+	 * state or if mA is enough
+	 */
+	if ((di->usb_state.usb_current == 2) && (mA > 2))
+		bm_usb_state = AB5500_BM_USB_STATE_RESUME;
+	else if (mA == 0)
+		bm_usb_state = AB5500_BM_USB_STATE_RESET_HS;
+	else if (mA == 2)
+		bm_usb_state = AB5500_BM_USB_STATE_SUSPEND;
+	else if (mA >= 8) /* 8, 100, 500 */
+		bm_usb_state = AB5500_BM_USB_STATE_CONFIGURED;
+	else /* Should never occur */
+		bm_usb_state = AB5500_BM_USB_STATE_RESET_FS;
+
+	dev_dbg(di->dev, "%s usb_state: 0x%02x mA: %d\n",
+		__func__, bm_usb_state, mA);
+
+	spin_lock(&di->usb_state.usb_lock);
+	di->usb_state.usb_changed = true;
+	di->usb_state.state = bm_usb_state;
+	di->usb_state.usb_current = mA;
+	spin_unlock(&di->usb_state.usb_lock);
+
+	queue_work(di->charger_wq, &di->usb_state_changed_work);
+
+	return NOTIFY_OK;
+}
+
+#if defined(CONFIG_PM)
+static int ab5500_charger_resume(struct platform_device *pdev)
+{
+	struct ab5500_charger *di = platform_get_drvdata(pdev);
+
+	/* If we still have a HW failure, schedule a new check */
+	if (di->flags.usbchargernotok || di->flags.vbus_ovv) {
+		queue_delayed_work(di->charger_wq,
+			&di->check_hw_failure_work, 0);
+	}
+
+	return 0;
+}
+
+static int ab5500_charger_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab5500_charger *di = platform_get_drvdata(pdev);
+
+	/* Cancel any pending HW failure check */
+	if (delayed_work_pending(&di->check_hw_failure_work))
+		cancel_delayed_work(&di->check_hw_failure_work);
+
+	return 0;
+}
+#else
+#define ab5500_charger_suspend      NULL
+#define ab5500_charger_resume       NULL
+#endif
+
+static int __devexit ab5500_charger_remove(struct platform_device *pdev)
+{
+	struct ab5500_charger *di = platform_get_drvdata(pdev);
+	int i, irq;
+
+	/* Disable USB charging */
+	ab5500_charger_usb_en(&di->usb_chg, false, 0, 0);
+
+	/* Disable interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_charger_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_charger_irq[i].name);
+		free_irq(irq, di);
+	}
+
+	otg_unregister_notifier(di->otg, &di->nb);
+	otg_put_transceiver(di->otg);
+
+	/* Delete the work queue */
+	destroy_workqueue(di->charger_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->usb_chg.psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit ab5500_charger_probe(struct platform_device *pdev)
+{
+	int irq, i, charger_status, ret = 0;
+	struct abx500_bm_plat_data *plat_data;
+
+	struct ab5500_charger *di =
+		kzalloc(sizeof(struct ab5500_charger), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	di->gpadc = ab5500_gpadc_get("ab5500-adc.0");
+
+	/* initialize lock */
+	spin_lock_init(&di->usb_state.usb_lock);
+
+	plat_data = pdev->dev.platform_data;
+	di->pdata = plat_data->charger;
+	di->bat = plat_data->battery;
+
+	/* get charger specific platform data */
+	if (!di->pdata) {
+		dev_err(di->dev, "no charger platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+
+	/* get battery specific platform data */
+	if (!di->bat) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	/* USB supply */
+	/* power_supply base class */
+	di->usb_chg.psy.name = "ab5500_usb";
+	di->usb_chg.psy.type = POWER_SUPPLY_TYPE_USB;
+	di->usb_chg.psy.properties = ab5500_charger_usb_props;
+	di->usb_chg.psy.num_properties = ARRAY_SIZE(ab5500_charger_usb_props);
+	di->usb_chg.psy.get_property = ab5500_charger_usb_get_property;
+	di->usb_chg.psy.supplied_to = di->pdata->supplied_to;
+	di->usb_chg.psy.num_supplicants = di->pdata->num_supplicants;
+	/* ux500_charger sub-class */
+	di->usb_chg.ops.enable = &ab5500_charger_usb_en;
+	di->usb_chg.ops.kick_wd = &ab5500_charger_watchdog_kick;
+	di->usb_chg.ops.update_curr = &ab5500_charger_update_charger_current;
+	di->usb_chg.max_out_volt = ab5500_charger_voltage_map[
+		ARRAY_SIZE(ab5500_charger_voltage_map) - 1];
+	di->usb_chg.max_out_curr = ab5500_charger_current_map[
+		ARRAY_SIZE(ab5500_charger_current_map) - 1];
+
+
+	/* Create a work queue for the charger */
+	di->charger_wq =
+		create_singlethread_workqueue("ab5500_charger_wq");
+	if (di->charger_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for HW failure check */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->check_hw_failure_work,
+		ab5500_charger_check_hw_failure_work);
+	INIT_DELAYED_WORK_DEFERRABLE(&di->check_usbchgnotok_work,
+		ab5500_charger_check_usbchargernotok_work);
+
+	/* Init work for charger detection */
+	INIT_WORK(&di->usb_link_status_work,
+		ab5500_charger_usb_link_status_work);
+	INIT_WORK(&di->detect_usb_type_work,
+		ab5500_charger_detect_usb_type_work);
+
+	INIT_WORK(&di->usb_state_changed_work,
+		ab5500_charger_usb_state_changed_work);
+
+	/* Init work for checking HW status */
+	INIT_WORK(&di->check_usb_thermal_prot_work,
+		ab5500_charger_check_usb_thermal_prot_work);
+
+	/* Get Chip ID of the ABB ASIC  */
+	ret = abx500_get_chip_id(di->dev);
+	if (ret < 0) {
+		dev_err(di->dev, "failed to get chip ID\n");
+		goto free_charger_wq;
+	}
+	di->chip_id = ret;
+	dev_dbg(di->dev, "AB5500 CID is: 0x%02x\n", di->chip_id);
+
+	/* Initialize OVV, and other registers */
+	ret = ab5500_charger_init_hw_registers(di);
+	if (ret) {
+		dev_err(di->dev, "failed to initialize ABB registers\n");
+		goto free_device_info;
+	}
+
+	/* Register USB charger class */
+	ret = power_supply_register(di->dev, &di->usb_chg.psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register USB charger\n");
+		goto free_device_info;
+	}
+
+	di->otg = otg_get_transceiver();
+	if (!di->otg) {
+		dev_err(di->dev, "failed to get otg transceiver\n");
+		goto free_usb;
+	}
+	di->nb.notifier_call = ab5500_charger_usb_notifier_call;
+	ret = otg_register_notifier(di->otg, &di->nb);
+	if (ret) {
+		dev_err(di->dev, "failed to register otg notifier\n");
+		goto put_otg_transceiver;
+	}
+
+	/* Identify the connected charger types during startup */
+	charger_status = ab5500_charger_detect_chargers(di);
+	if (charger_status & USB_PW_CONN) {
+		dev_dbg(di->dev, "VBUS Detect during startup\n");
+		di->vbus_detected = true;
+		di->vbus_detected_start = true;
+		queue_work(di->charger_wq,
+			&di->usb_link_status_work);
+	}
+
+	/* Register interrupts */
+	for (i = 0; i < ARRAY_SIZE(ab5500_charger_irq); i++) {
+		irq = platform_get_irq_byname(pdev, ab5500_charger_irq[i].name);
+		ret = request_threaded_irq(irq, NULL, ab5500_charger_irq[i].isr,
+			IRQF_SHARED | IRQF_NO_SUSPEND,
+			ab5500_charger_irq[i].name, di);
+
+		if (ret != 0) {
+			dev_err(di->dev, "failed to request %s IRQ %d: %d\n"
+				, ab5500_charger_irq[i].name, irq, ret);
+			goto free_irq;
+		}
+		dev_dbg(di->dev, "Requested %s IRQ %d: %d\n",
+			ab5500_charger_irq[i].name, irq, ret);
+	}
+
+	platform_set_drvdata(pdev, di);
+
+	dev_info(di->dev, "probe success\n");
+	return ret;
+
+free_irq:
+	otg_unregister_notifier(di->otg, &di->nb);
+
+	/* We also have to free all successfully registered irqs */
+	for (i = i - 1; i >= 0; i--) {
+		irq = platform_get_irq_byname(pdev, ab5500_charger_irq[i].name);
+		free_irq(irq, di);
+	}
+put_otg_transceiver:
+	otg_put_transceiver(di->otg);
+free_usb:
+	power_supply_unregister(&di->usb_chg.psy);
+free_charger_wq:
+	destroy_workqueue(di->charger_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab5500_charger_driver = {
+	.probe = ab5500_charger_probe,
+	.remove = __devexit_p(ab5500_charger_remove),
+	.suspend = ab5500_charger_suspend,
+	.resume = ab5500_charger_resume,
+	.driver = {
+		.name = "ab5500-charger",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_charger_init(void)
+{
+	return platform_driver_register(&ab5500_charger_driver);
+}
+
+static void __exit ab5500_charger_exit(void)
+{
+	platform_driver_unregister(&ab5500_charger_driver);
+}
+
+subsys_initcall_sync(ab5500_charger_init);
+module_exit(ab5500_charger_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
+MODULE_ALIAS("platform:ab5500-charger");
+MODULE_DESCRIPTION("AB5500 charger management driver");
diff --git a/drivers/power/ab5500_fg.c b/drivers/power/ab5500_fg.c
new file mode 100644
index 00000000000..c74d351bd8b
--- /dev/null
+++ b/drivers/power/ab5500_fg.c
@@ -0,0 +1,1954 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2011
+ *
+ * Main and Back-up battery management driver.
+ *
+ * Note: Backup battery management is required in case of Li-Ion battery and not
+ * for capacitive battery. HREF boards have capacitive battery and hence backup
+ * battery management is not used and the supported code is available in this
+ * driver.
+ *
+ * License Terms: GNU General Public License v2
+ * Authors:
+ *	Johan Palsson <johan.palsson@stericsson.com>
+ *	Karl Komierowski <karl.komierowski@stericsson.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/power_supply.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/delay.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+#include <linux/mfd/abx500/ab5500-bm.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+
+static LIST_HEAD(ab5500_fg_list);
+
+/* U5500 Constants */
+#define FG_ON_MASK			0x04
+#define FG_ON				0x04
+#define FG_ACC_RESET_ON_READ_MASK	0x08
+#define FG_ACC_RESET_ON_READ		0x08
+#define EN_READOUT_MASK			0x01
+#define EN_READOUT			0x01
+#define EN_ACC_RESET_ON_READ		0x08
+#define ACC_RESET_ON_READ		0x08
+#define RESET				0x00
+#define EOC_52_mA			0x04
+#define MILLI_TO_MICRO			1000
+#define FG_LSB_IN_MA			770
+#define QLSB_NANO_AMP_HOURS_X100	5353
+#define SEC_TO_SAMPLE(S)		(S * 4)
+#define NBR_AVG_SAMPLES			20
+#define LOW_BAT_CHECK_INTERVAL		(2 * HZ)
+#define FG_PERIODIC_START_INTERVAL	(250 * HZ)/1000 /* 250 msec */
+
+#define VALID_CAPACITY_SEC		(45 * 60) /* 45 minutes */
+
+#define interpolate(x, x1, y1, x2, y2) \
+	((y1) + ((((y2) - (y1)) * ((x) - (x1))) / ((x2) - (x1))));
+
+#define to_ab5500_fg_device_info(x) container_of((x), \
+	struct ab5500_fg, fg_psy);
+
+/**
+ * struct ab5500_fg_interrupts - ab5500 fg interupts
+ * @name:	name of the interrupt
+ * @isr		function pointer to the isr
+ */
+struct ab5500_fg_interrupts {
+	char *name;
+	irqreturn_t (*isr)(int irq, void *data);
+};
+
+enum ab5500_fg_discharge_state {
+	AB5500_FG_DISCHARGE_INIT,
+	AB5500_FG_DISCHARGE_INITMEASURING,
+	AB5500_FG_DISCHARGE_INIT_RECOVERY,
+	AB5500_FG_DISCHARGE_RECOVERY,
+	AB5500_FG_DISCHARGE_READOUT,
+	AB5500_FG_DISCHARGE_WAKEUP,
+};
+
+static char *discharge_state[] = {
+	"DISCHARGE_INIT",
+	"DISCHARGE_INITMEASURING",
+	"DISCHARGE_INIT_RECOVERY",
+	"DISCHARGE_RECOVERY",
+	"DISCHARGE_READOUT",
+	"DISCHARGE_WAKEUP",
+};
+
+enum ab5500_fg_charge_state {
+	AB5500_FG_CHARGE_INIT,
+	AB5500_FG_CHARGE_READOUT,
+};
+
+static char *charge_state[] = {
+	"CHARGE_INIT",
+	"CHARGE_READOUT",
+};
+
+enum ab5500_fg_calibration_state {
+	AB5500_FG_CALIB_INIT,
+	AB5500_FG_CALIB_WAIT,
+	AB5500_FG_CALIB_END,
+};
+
+struct ab5500_fg_avg_cap {
+	int avg;
+	int samples[NBR_AVG_SAMPLES];
+	__kernel_time_t time_stamps[NBR_AVG_SAMPLES];
+	int pos;
+	int nbr_samples;
+	int sum;
+};
+
+struct ab5500_fg_battery_capacity {
+	int max_mah_design;
+	int max_mah;
+	int mah;
+	int permille;
+	int level;
+	int prev_mah;
+	int prev_percent;
+	int prev_level;
+};
+
+struct ab5500_fg_flags {
+	bool fg_enabled;
+	bool conv_done;
+	bool charging;
+	bool fully_charged;
+	bool low_bat_delay;
+	bool low_bat;
+	bool bat_ovv;
+	bool batt_unknown;
+	bool calibrate;
+};
+
+/**
+ * struct ab5500_fg - ab5500 FG device information
+ * @dev:		Pointer to the structure device
+ * @vbat:		Battery voltage in mV
+ * @vbat_nom:		Nominal battery voltage in mV
+ * @inst_curr:		Instantenous battery current in mA
+ * @avg_curr:		Average battery current in mA
+ * @fg_samples:		Number of samples used in the FG accumulation
+ * @accu_charge:	Accumulated charge from the last conversion
+ * @recovery_cnt:	Counter for recovery mode
+ * @high_curr_cnt:	Counter for high current mode
+ * @init_cnt:		Counter for init mode
+ * @v_to_cap:		capacity based on battery voltage
+ * @recovery_needed:	Indicate if recovery is needed
+ * @high_curr_mode:	Indicate if we're in high current mode
+ * @init_capacity:	Indicate if initial capacity measuring should be done
+ * @calib_state		State during offset calibration
+ * @discharge_state:	Current discharge state
+ * @charge_state:	Current charge state
+ * @flags:		Structure for information about events triggered
+ * @bat_cap:		Structure for battery capacity specific parameters
+ * @avg_cap:		Average capacity filter
+ * @parent:		Pointer to the struct ab5500
+ * @gpadc:		Pointer to the struct gpadc
+ * @gpadc_auto:		Pointer tot he struct adc_auto_input
+ * @pdata:		Pointer to the ab5500_fg platform data
+ * @bat:		Pointer to the ab5500_bm platform data
+ * @fg_psy:		Structure that holds the FG specific battery properties
+ * @fg_wq:		Work queue for running the FG algorithm
+ * @fg_periodic_work:	Work to run the FG algorithm periodically
+ * @fg_low_bat_work:	Work to check low bat condition
+ * @fg_reinit_work:	Work to reset and re-initialize fuel gauge
+ * @fg_work:		Work to run the FG algorithm instantly
+ * @fg_acc_cur_work:	Work to read the FG accumulator
+ * @cc_lock:		Mutex for locking the CC
+ * @node:		struct of type list_head
+ */
+struct ab5500_fg {
+	struct device *dev;
+	int vbat;
+	int vbat_nom;
+	int inst_curr;
+	int avg_curr;
+	int fg_samples;
+	int accu_charge;
+	int recovery_cnt;
+	int high_curr_cnt;
+	int init_cnt;
+	int v_to_cap;
+	bool recovery_needed;
+	bool high_curr_mode;
+	bool init_capacity;
+	enum ab5500_fg_calibration_state calib_state;
+	enum ab5500_fg_discharge_state discharge_state;
+	enum ab5500_fg_charge_state charge_state;
+	struct ab5500_fg_flags flags;
+	struct ab5500_fg_battery_capacity bat_cap;
+	struct ab5500_fg_avg_cap avg_cap;
+	struct ab5500 *parent;
+	struct ab5500_gpadc *gpadc;
+	struct adc_auto_input *gpadc_auto;
+	struct abx500_fg_platform_data *pdata;
+	struct abx500_bm_data *bat;
+	struct power_supply fg_psy;
+	struct workqueue_struct *fg_wq;
+	struct delayed_work fg_periodic_work;
+	struct delayed_work fg_low_bat_work;
+	struct delayed_work fg_reinit_work;
+	struct work_struct fg_work;
+	struct delayed_work fg_acc_cur_work;
+	struct mutex cc_lock;
+	struct list_head node;
+	struct timer_list avg_current_timer;
+};
+
+/* Main battery properties */
+static enum power_supply_property ab5500_fg_props[] = {
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_CURRENT_AVG,
+	POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN,
+	POWER_SUPPLY_PROP_ENERGY_FULL,
+	POWER_SUPPLY_PROP_ENERGY_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
+};
+
+/* Function Prototype */
+static int ab5500_fg_bat_v_trig(int mux);
+
+static int prev_samples, prev_val;
+
+struct ab5500_fg *ab5500_fg_get(void)
+{
+	struct ab5500_fg *di;
+	di = list_first_entry(&ab5500_fg_list, struct ab5500_fg, node);
+
+	return di;
+}
+
+/**
+ * ab5500_fg_is_low_curr() - Low or high current mode
+ * @di:		pointer to the ab5500_fg structure
+ * @curr:	the current to base or our decision on
+ *
+ * Low current mode if the current consumption is below a certain threshold
+ */
+static int ab5500_fg_is_low_curr(struct ab5500_fg *di, int curr)
+{
+	/*
+	 * We want to know if we're in low current mode
+	 */
+	if (curr > -di->bat->fg_params->high_curr_threshold)
+		return true;
+	else
+		return false;
+}
+
+/**
+ * ab5500_fg_add_cap_sample() - Add capacity to average filter
+ * @di:		pointer to the ab5500_fg structure
+ * @sample:	the capacity in mAh to add to the filter
+ *
+ * A capacity is added to the filter and a new mean capacity is calculated and
+ * returned
+ */
+static int ab5500_fg_add_cap_sample(struct ab5500_fg *di, int sample)
+{
+	struct timespec ts;
+	struct ab5500_fg_avg_cap *avg = &di->avg_cap;
+
+	getnstimeofday(&ts);
+
+	do {
+		avg->sum += sample - avg->samples[avg->pos];
+		avg->samples[avg->pos] = sample;
+		avg->time_stamps[avg->pos] = ts.tv_sec;
+		avg->pos++;
+
+		if (avg->pos == NBR_AVG_SAMPLES)
+			avg->pos = 0;
+
+		if (avg->nbr_samples < NBR_AVG_SAMPLES)
+			avg->nbr_samples++;
+
+		/*
+		 * Check the time stamp for each sample. If too old,
+		 * replace with latest sample
+		 */
+	} while (ts.tv_sec - VALID_CAPACITY_SEC > avg->time_stamps[avg->pos]);
+
+	avg->avg = avg->sum / avg->nbr_samples;
+
+	return avg->avg;
+}
+
+/**
+ * ab5500_fg_clear_cap_samples() - Clear average filter
+ * @di:                pointer to the ab5500_fg structure
+ *
+ * The capacity filter is is reset to zero.
+ */
+static void ab5500_fg_clear_cap_samples(struct ab5500_fg *di)
+{
+	int i;
+	struct ab5500_fg_avg_cap *avg = &di->avg_cap;
+
+	avg->pos = 0;
+	avg->nbr_samples = 0;
+	avg->sum = 0;
+	avg->avg = 0;
+
+	for (i = 0; i < NBR_AVG_SAMPLES; i++) {
+		avg->samples[i] = 0;
+		avg->time_stamps[i] = 0;
+	}
+}
+
+
+/**
+ * ab5500_fg_fill_cap_sample() - Fill average filter
+ * @di:		pointer to the ab5500_fg structure
+ * @sample:	the capacity in mAh to fill the filter with
+ *
+ * The capacity filter is filled with a capacity in mAh
+ */
+static void ab5500_fg_fill_cap_sample(struct ab5500_fg *di, int sample)
+{
+	int i;
+	struct timespec ts;
+	struct ab5500_fg_avg_cap *avg = &di->avg_cap;
+
+	getnstimeofday(&ts);
+
+	for (i = 0; i < NBR_AVG_SAMPLES; i++) {
+		avg->samples[i] = sample;
+		avg->time_stamps[i] = ts.tv_sec;
+	}
+
+	avg->pos = 0;
+	avg->nbr_samples = NBR_AVG_SAMPLES;
+	avg->sum = sample * NBR_AVG_SAMPLES;
+	avg->avg = sample;
+}
+
+/**
+ * ab5500_fg_coulomb_counter() - enable coulomb counter
+ * @di:		pointer to the ab5500_fg structure
+ * @enable:	enable/disable
+ *
+ * Enable/Disable coulomb counter.
+ * On failure returns negative value.
+ */
+static int ab5500_fg_coulomb_counter(struct ab5500_fg *di, bool enable)
+{
+	int ret = 0;
+	mutex_lock(&di->cc_lock);
+	if (enable) {
+		/* Power-up the CC */
+		ret = abx500_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			(FG_ON | FG_ACC_RESET_ON_READ));
+		if (ret)
+			goto cc_err;
+
+		di->flags.fg_enabled = true;
+	} else {
+		/* Stop the CC */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			FG_ON_MASK, RESET);
+		if (ret)
+			goto cc_err;
+
+		di->flags.fg_enabled = false;
+
+	}
+	dev_dbg(di->dev, " CC enabled: %d Samples: %d\n",
+		enable, di->fg_samples);
+
+	mutex_unlock(&di->cc_lock);
+
+	return ret;
+cc_err:
+	dev_err(di->dev, "%s Enabling coulomb counter failed\n", __func__);
+	mutex_unlock(&di->cc_lock);
+	return ret;
+}
+
+/**
+ * ab5500_fg_inst_curr() - battery instantaneous current
+ * @di:         pointer to the ab5500_fg structure
+ *
+ * Returns battery instantenous current(on success) else error code
+ */
+static int ab5500_fg_inst_curr(struct ab5500_fg *di)
+{
+	u8 low, high;
+	static int val;
+	int ret = 0;
+	bool fg_off = false;
+
+	if (!di->flags.fg_enabled) {
+		fg_off = true;
+		/* Power-up the CC */
+		ab5500_fg_coulomb_counter(di, true);
+		msleep(250);
+	}
+
+	mutex_lock(&di->cc_lock);
+
+	/* Enable read request */
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+		AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_B,
+		EN_READOUT_MASK, EN_READOUT);
+	if (ret)
+		goto inst_curr_err;
+
+	/* Read CC Sample conversion value Low and high */
+	ret = abx500_get_register_interruptible(di->dev,
+		AB5500_BANK_FG_BATTCOM_ACC,
+		AB5500_FGDIR_READ0,  &low);
+	if (ret < 0)
+		goto inst_curr_err;
+
+	ret = abx500_get_register_interruptible(di->dev,
+		AB5500_BANK_FG_BATTCOM_ACC,
+		AB5500_FGDIR_READ1,  &high);
+	if (ret < 0)
+		goto inst_curr_err;
+
+	/*
+	 * negative value for Discharging
+	 * convert 2's compliment into decimal
+	 */
+	if (high & 0x10)
+		val = (low | (high << 8) | 0xFFFFE000);
+	else
+		val = (low | (high << 8));
+
+	/*
+	 * Convert to unit value in mA
+	 * R(FGSENSE) = 20 mOhm
+	 * Scaling of LSB: This corresponds fro R(FGSENSE) to a current of
+	 * I = Q/t = 192.7 uC * 4 Hz = 0.77mA
+	 */
+	val = (val * 770) / 1000;
+
+	mutex_unlock(&di->cc_lock);
+
+	if (fg_off) {
+		dev_dbg(di->dev, "%s Disable FG\n", __func__);
+		/* Power-off the CC */
+		ab5500_fg_coulomb_counter(di, false);
+	}
+
+	return val;
+
+inst_curr_err:
+	dev_err(di->dev, "%s Get instanst current failed\n", __func__);
+	mutex_unlock(&di->cc_lock);
+	return ret;
+}
+
+static void ab5500_fg_acc_cur_timer_expired(unsigned long data)
+{
+	struct ab5500_fg *di = (struct ab5500_fg *) data;
+	dev_dbg(di->dev, "Avg current timer expired\n");
+
+	/* Trigger execution of the algorithm instantly */
+	queue_delayed_work(di->fg_wq, &di->fg_acc_cur_work, 0);
+}
+
+/**
+ * ab5500_fg_acc_cur_work() - average battery current
+ * @work:	pointer to the work_struct structure
+ *
+ * Updated the average battery current obtained from the
+ * coulomb counter.
+ */
+static void ab5500_fg_acc_cur_work(struct work_struct *work)
+{
+	int val, raw_val, sample;
+	int ret;
+	u8 low, med, high, cnt_low, cnt_high;
+
+	struct ab5500_fg *di = container_of(work,
+		struct ab5500_fg, fg_acc_cur_work.work);
+
+	if (!di->flags.fg_enabled) {
+		/* Power-up the CC */
+		ab5500_fg_coulomb_counter(di, true);
+		msleep(250);
+	}
+	mutex_lock(&di->cc_lock);
+	ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_C,
+			EN_READOUT_MASK, EN_READOUT);
+	if (ret < 0)
+		goto exit;
+	/* If charging read charging registers for accumulated values */
+	if (di->flags.charging) {
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			ACC_RESET_ON_READ, EN_ACC_RESET_ON_READ);
+		if (ret < 0)
+			goto exit;
+		/* Read CC Sample conversion value Low and high */
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_CH0, &low);
+		if (ret < 0)
+			goto exit;
+
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_CH1, &med);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_CH2, &high);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_VAL_COUNT0, &cnt_low);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_VAL_COUNT1, &cnt_high);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			ACC_RESET_ON_READ, RESET);
+		if (ret < 0)
+			goto exit;
+		queue_delayed_work(di->fg_wq, &di->fg_acc_cur_work,
+				di->bat->interval_charging * HZ);
+	} else { /* discharging */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			ACC_RESET_ON_READ, EN_ACC_RESET_ON_READ);
+		if (ret < 0)
+			goto exit;
+		/* Read CC Sample conversion value Low and high */
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_DIS_CH0, &low);
+		if (ret < 0)
+			goto exit;
+
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_DIS_CH1, &med);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_DIS_CH2, &high);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_VAL_COUNT0, &cnt_low);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_get_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC,
+			AB5500_FG_VAL_COUNT1, &cnt_high);
+		if (ret < 0)
+			goto exit;
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			ACC_RESET_ON_READ, RESET);
+		if (ret < 0)
+			goto exit;
+		queue_delayed_work(di->fg_wq, &di->fg_acc_cur_work,
+				di->bat->interval_not_charging * HZ);
+	}
+	di->fg_samples = (cnt_low | (cnt_high << 8));
+	/*
+	 * TODO: Workaround due to the hardware issue that accumulator is not
+	 * reset after setting reset_on_read bit and reading the accumulator
+	 * Registers.
+	 */
+	if (prev_samples > di->fg_samples) {
+		/* overflow has occured */
+		sample = (0xFFFF - prev_samples) + di->fg_samples;
+	} else
+		sample = di->fg_samples - prev_samples;
+	prev_samples = di->fg_samples;
+	di->fg_samples = sample;
+	val = (low | (med << 8) | (high << 16));
+	/*
+	 * TODO: Workaround due to the hardware issue that accumulator is not
+	 * reset after setting reset_on_read bit and reading the accumulator
+	 * Registers.
+	 */
+	if (prev_val > val)
+		raw_val = (0xFFFFFF - prev_val) + val;
+	else
+		raw_val = val - prev_val;
+	prev_val = val;
+	val = raw_val;
+
+	if (di->fg_samples) {
+		di->accu_charge = (val * QLSB_NANO_AMP_HOURS_X100)/100000;
+		di->avg_curr = (val * FG_LSB_IN_MA) / (di->fg_samples * 1000);
+	} else
+		dev_err(di->dev,
+			"samples is zero, using previous calculated average current\n");
+	di->flags.conv_done = true;
+	di->calib_state = AB5500_FG_CALIB_END;
+
+	mutex_unlock(&di->cc_lock);
+
+	queue_work(di->fg_wq, &di->fg_work);
+
+	return;
+exit:
+	dev_err(di->dev,
+		"Failed to read or write gas gauge registers\n");
+	mutex_unlock(&di->cc_lock);
+	queue_work(di->fg_wq, &di->fg_work);
+}
+
+/**
+ * ab5500_fg_bat_voltage() - get battery voltage
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Returns battery voltage(on success) else error code
+ */
+static int ab5500_fg_bat_voltage(struct ab5500_fg *di)
+{
+	int vbat;
+	static int prev;
+
+	vbat = ab5500_gpadc_convert(di->gpadc, MAIN_BAT_V);
+	if (vbat < 0) {
+		dev_err(di->dev,
+			"%s gpadc conversion failed, using previous value\n",
+			__func__);
+		return prev;
+	}
+
+	prev = vbat;
+	return vbat;
+}
+
+/**
+ * ab5500_fg_volt_to_capacity() - Voltage based capacity
+ * @di:		pointer to the ab5500_fg structure
+ * @voltage:	The voltage to convert to a capacity
+ *
+ * Returns battery capacity in per mille based on voltage
+ */
+static int ab5500_fg_volt_to_capacity(struct ab5500_fg *di, int voltage)
+{
+	int i, tbl_size;
+	struct abx500_v_to_cap *tbl;
+	int cap = 0;
+
+	tbl = di->bat->bat_type[di->bat->batt_id].v_to_cap_tbl,
+	tbl_size = di->bat->bat_type[di->bat->batt_id].n_v_cap_tbl_elements;
+
+	for (i = 0; i < tbl_size; ++i) {
+		if (di->vbat < tbl[i].voltage && di->vbat > tbl[i+1].voltage)
+			di->v_to_cap = tbl[i].capacity;
+	}
+
+	for (i = 0; i < tbl_size; ++i) {
+		if (voltage > tbl[i].voltage)
+			break;
+	}
+
+	if ((i > 0) && (i < tbl_size)) {
+		cap = interpolate(voltage,
+			tbl[i].voltage,
+			tbl[i].capacity * 10,
+			tbl[i-1].voltage,
+			tbl[i-1].capacity * 10);
+	} else if (i == 0) {
+		cap = 1000;
+	} else {
+		cap = 0;
+	}
+
+	dev_dbg(di->dev, "%s Vbat: %d, Cap: %d per mille",
+		__func__, voltage, cap);
+
+	return cap;
+}
+
+/**
+ * ab5500_fg_uncomp_volt_to_capacity() - Uncompensated voltage based capacity
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Returns battery capacity based on battery voltage that is not compensated
+ * for the voltage drop due to the load
+ */
+static int ab5500_fg_uncomp_volt_to_capacity(struct ab5500_fg *di)
+{
+	di->vbat = ab5500_fg_bat_voltage(di);
+	return ab5500_fg_volt_to_capacity(di, di->vbat);
+}
+
+/**
+ * ab5500_fg_load_comp_volt_to_capacity() - Load compensated voltage based capacity
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Returns battery capacity based on battery voltage that is load compensated
+ * for the voltage drop
+ */
+static int ab5500_fg_load_comp_volt_to_capacity(struct ab5500_fg *di)
+{
+	int vbat_comp;
+
+	di->inst_curr = ab5500_fg_inst_curr(di);
+	di->vbat = ab5500_fg_bat_voltage(di);
+
+	/* Use Ohms law to get the load compensated voltage */
+	vbat_comp = di->vbat - (di->inst_curr *
+		di->bat->bat_type[di->bat->batt_id].battery_resistance) / 1000;
+
+	dev_dbg(di->dev, "%s Measured Vbat: %dmV,Compensated Vbat %dmV, "
+		"R: %dmOhm, Current: %dmA\n",
+		__func__,
+		di->vbat,
+		vbat_comp,
+		di->bat->bat_type[di->bat->batt_id].battery_resistance,
+		di->inst_curr);
+
+	return ab5500_fg_volt_to_capacity(di, vbat_comp);
+}
+
+/**
+ * ab5500_fg_convert_mah_to_permille() - Capacity in mAh to permille
+ * @di:		pointer to the ab5500_fg structure
+ * @cap_mah:	capacity in mAh
+ *
+ * Converts capacity in mAh to capacity in permille
+ */
+static int ab5500_fg_convert_mah_to_permille(struct ab5500_fg *di, int cap_mah)
+{
+	return (cap_mah * 1000) / di->bat_cap.max_mah_design;
+}
+
+/**
+ * ab5500_fg_convert_permille_to_mah() - Capacity in permille to mAh
+ * @di:		pointer to the ab5500_fg structure
+ * @cap_pm:	capacity in permille
+ *
+ * Converts capacity in permille to capacity in mAh
+ */
+static int ab5500_fg_convert_permille_to_mah(struct ab5500_fg *di, int cap_pm)
+{
+	return cap_pm * di->bat_cap.max_mah_design / 1000;
+}
+
+/**
+ * ab5500_fg_convert_mah_to_uwh() - Capacity in mAh to uWh
+ * @di:		pointer to the ab5500_fg structure
+ * @cap_mah:	capacity in mAh
+ *
+ * Converts capacity in mAh to capacity in uWh
+ */
+static int ab5500_fg_convert_mah_to_uwh(struct ab5500_fg *di, int cap_mah)
+{
+	u64 div_res;
+	u32 div_rem;
+
+	div_res = ((u64) cap_mah) * ((u64) di->vbat_nom);
+	div_rem = do_div(div_res, 1000);
+
+	/* Make sure to round upwards if necessary */
+	if (div_rem >= 1000 / 2)
+		div_res++;
+
+	return (int) div_res;
+}
+
+/**
+ * ab5500_fg_calc_cap_charging() - Calculate remaining capacity while charging
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Return the capacity in mAh based on previous calculated capcity and the FG
+ * accumulator register value. The filter is filled with this capacity
+ */
+static int ab5500_fg_calc_cap_charging(struct ab5500_fg *di)
+{
+	dev_dbg(di->dev, "%s cap_mah %d accu_charge %d\n",
+		__func__,
+		di->bat_cap.mah,
+		di->accu_charge);
+
+	/* Capacity should not be less than 0 */
+	if (di->bat_cap.mah + di->accu_charge > 0)
+		di->bat_cap.mah += di->accu_charge;
+	else
+		di->bat_cap.mah = 0;
+
+	/*
+	 * We force capacity to 100% as long as the algorithm
+	 * reports that it's full.
+	*/
+	if (di->bat_cap.mah >= di->bat_cap.max_mah_design ||
+		di->flags.fully_charged)
+		di->bat_cap.mah = di->bat_cap.max_mah_design;
+
+	ab5500_fg_fill_cap_sample(di, di->bat_cap.mah);
+	di->bat_cap.permille =
+		ab5500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+
+	/* We need to update battery voltage and inst current when charging */
+	di->vbat = ab5500_fg_bat_voltage(di);
+	di->inst_curr = ab5500_fg_inst_curr(di);
+
+	return di->bat_cap.mah;
+}
+
+/**
+ * ab5500_fg_calc_cap_discharge_voltage() - Capacity in discharge with voltage
+ * @di:		pointer to the ab5500_fg structure
+ * @comp:	if voltage should be load compensated before capacity calc
+ *
+ * Return the capacity in mAh based on the battery voltage. The voltage can
+ * either be load compensated or not. This value is added to the filter and a
+ * new mean value is calculated and returned.
+ */
+static int ab5500_fg_calc_cap_discharge_voltage(struct ab5500_fg *di, bool comp)
+{
+	int permille, mah;
+
+	if (comp)
+		permille = ab5500_fg_load_comp_volt_to_capacity(di);
+	else
+		permille = ab5500_fg_uncomp_volt_to_capacity(di);
+
+	mah = ab5500_fg_convert_permille_to_mah(di, permille);
+
+	di->bat_cap.mah = ab5500_fg_add_cap_sample(di, mah);
+	di->bat_cap.permille =
+		ab5500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+
+	return di->bat_cap.mah;
+}
+
+/**
+ * ab5500_fg_calc_cap_discharge_fg() - Capacity in discharge with FG
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Return the capacity in mAh based on previous calculated capcity and the FG
+ * accumulator register value. This value is added to the filter and a
+ * new mean value is calculated and returned.
+ */
+static int ab5500_fg_calc_cap_discharge_fg(struct ab5500_fg *di)
+{
+	int permille_volt, permille;
+
+	dev_dbg(di->dev, "%s cap_mah %d accu_charge %d\n",
+		__func__,
+		di->bat_cap.mah,
+		di->accu_charge);
+
+	/* Capacity should not be less than 0 */
+	if (di->bat_cap.mah + di->accu_charge > 0)
+		di->bat_cap.mah += di->accu_charge;
+	else
+		di->bat_cap.mah = 0;
+
+	if (di->bat_cap.mah >= di->bat_cap.max_mah_design)
+		di->bat_cap.mah = di->bat_cap.max_mah_design;
+
+	/*
+	 * Check against voltage based capacity. It can not be lower
+	 * than what the uncompensated voltage says
+	 */
+	permille = ab5500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+	permille_volt = ab5500_fg_uncomp_volt_to_capacity(di);
+
+	if (permille < permille_volt) {
+		di->bat_cap.permille = permille_volt;
+		di->bat_cap.mah = ab5500_fg_convert_permille_to_mah(di,
+			di->bat_cap.permille);
+
+		dev_dbg(di->dev, "%s voltage based: perm %d perm_volt %d\n",
+			__func__,
+			permille,
+			permille_volt);
+
+		ab5500_fg_fill_cap_sample(di, di->bat_cap.mah);
+	} else {
+		ab5500_fg_fill_cap_sample(di, di->bat_cap.mah);
+		di->bat_cap.permille =
+			ab5500_fg_convert_mah_to_permille(di, di->bat_cap.mah);
+	}
+
+	return di->bat_cap.mah;
+}
+
+/**
+ * ab5500_fg_capacity_level() - Get the battery capacity level
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Get the battery capacity level based on the capacity in percent
+ */
+static int ab5500_fg_capacity_level(struct ab5500_fg *di)
+{
+	int ret, percent;
+
+	percent = di->bat_cap.permille / 10;
+
+	if (percent <= di->bat->cap_levels->critical ||
+		di->flags.low_bat)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_CRITICAL;
+	else if (percent <= di->bat->cap_levels->low)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_LOW;
+	else if (percent <= di->bat->cap_levels->normal)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_NORMAL;
+	else if (percent <= di->bat->cap_levels->high)
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_HIGH;
+	else
+		ret = POWER_SUPPLY_CAPACITY_LEVEL_FULL;
+
+	return ret;
+}
+
+/**
+ * ab5500_fg_check_capacity_limits() - Check if capacity has changed
+ * @di:		pointer to the ab5500_fg structure
+ * @init:	capacity is allowed to go up in init mode
+ *
+ * Check if capacity or capacity limit has changed and notify the system
+ * about it using the power_supply framework
+ */
+static void ab5500_fg_check_capacity_limits(struct ab5500_fg *di, bool init)
+{
+	bool changed = false;
+
+	di->bat_cap.level = ab5500_fg_capacity_level(di);
+
+	if (di->bat_cap.level != di->bat_cap.prev_level) {
+		/*
+		 * We do not allow reported capacity level to go up
+		 * unless we're charging or if we're in init
+		 */
+		if (!(!di->flags.charging && di->bat_cap.level >
+			di->bat_cap.prev_level) || init) {
+			dev_dbg(di->dev, "level changed from %d to %d\n",
+				di->bat_cap.prev_level,
+				di->bat_cap.level);
+			di->bat_cap.prev_level = di->bat_cap.level;
+			changed = true;
+		} else {
+			dev_dbg(di->dev, "level not allowed to go up "
+				"since no charger is connected: %d to %d\n",
+				di->bat_cap.prev_level,
+				di->bat_cap.level);
+		}
+	}
+
+	/*
+	 * If we have received the LOW_BAT IRQ, set capacity to 0 to initiate
+	 * shutdown
+	 */
+	if (di->flags.low_bat) {
+		dev_dbg(di->dev, "Battery low, set capacity to 0\n");
+		di->bat_cap.prev_percent = 0;
+		di->bat_cap.permille = 0;
+		di->bat_cap.prev_mah = 0;
+		di->bat_cap.mah = 0;
+		changed = true;
+	} else if (di->bat_cap.prev_percent != di->bat_cap.permille / 10) {
+		if (di->bat_cap.permille / 10 == 0) {
+			/*
+			 * We will not report 0% unless we've got
+			 * the LOW_BAT IRQ, no matter what the FG
+			 * algorithm says.
+			 */
+			di->bat_cap.prev_percent = 1;
+			di->bat_cap.permille = 1;
+			di->bat_cap.prev_mah = 1;
+			di->bat_cap.mah = 1;
+
+			changed = true;
+		} else if (!(!di->flags.charging &&
+			(di->bat_cap.permille / 10) >
+			di->bat_cap.prev_percent) || init) {
+			/*
+			 * We do not allow reported capacity to go up
+			 * unless we're charging or if we're in init
+			 */
+			dev_dbg(di->dev,
+				"capacity changed from %d to %d (%d)\n",
+				di->bat_cap.prev_percent,
+				di->bat_cap.permille / 10,
+				di->bat_cap.permille);
+			di->bat_cap.prev_percent = di->bat_cap.permille / 10;
+			di->bat_cap.prev_mah = di->bat_cap.mah;
+
+			changed = true;
+		} else {
+			dev_dbg(di->dev, "capacity not allowed to go up since "
+				"no charger is connected: %d to %d (%d)\n",
+				di->bat_cap.prev_percent,
+				di->bat_cap.permille / 10,
+				di->bat_cap.permille);
+		}
+	}
+
+	if (changed)
+		power_supply_changed(&di->fg_psy);
+
+}
+
+static void ab5500_fg_charge_state_to(struct ab5500_fg *di,
+	enum ab5500_fg_charge_state new_state)
+{
+	dev_dbg(di->dev, "Charge state from %d [%s] to %d [%s]\n",
+		di->charge_state,
+		charge_state[di->charge_state],
+		new_state,
+		charge_state[new_state]);
+
+	di->charge_state = new_state;
+}
+
+static void ab5500_fg_discharge_state_to(struct ab5500_fg *di,
+	enum ab5500_fg_charge_state new_state)
+{
+	dev_dbg(di->dev, "Disharge state from %d [%s] to %d [%s]\n",
+		di->discharge_state,
+		discharge_state[di->discharge_state],
+		new_state,
+		discharge_state[new_state]);
+
+	di->discharge_state = new_state;
+}
+
+/**
+ * ab5500_fg_algorithm_charging() - FG algorithm for when charging
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Battery capacity calculation state machine for when we're charging
+ */
+static void ab5500_fg_algorithm_charging(struct ab5500_fg *di)
+{
+	/*
+	 * If we change to discharge mode
+	 * we should start with recovery
+	 */
+	if (di->discharge_state != AB5500_FG_DISCHARGE_INIT_RECOVERY)
+		ab5500_fg_discharge_state_to(di,
+			AB5500_FG_DISCHARGE_INIT_RECOVERY);
+
+	switch (di->charge_state) {
+	case AB5500_FG_CHARGE_INIT:
+		di->fg_samples = SEC_TO_SAMPLE(
+			di->bat->fg_params->accu_charging);
+
+		ab5500_fg_coulomb_counter(di, true);
+		ab5500_fg_charge_state_to(di, AB5500_FG_CHARGE_READOUT);
+
+		break;
+
+	case AB5500_FG_CHARGE_READOUT:
+		/*
+		 * Read the FG and calculate the new capacity
+		 */
+		mutex_lock(&di->cc_lock);
+		if (!di->flags.conv_done) {
+			/* Wasn't the CC IRQ that got us here */
+			mutex_unlock(&di->cc_lock);
+			dev_dbg(di->dev, "%s CC conv not done\n",
+				__func__);
+
+			break;
+		}
+		di->flags.conv_done = false;
+		mutex_unlock(&di->cc_lock);
+
+		ab5500_fg_calc_cap_charging(di);
+
+		break;
+
+	default:
+		break;
+	}
+
+	/* Check capacity limits */
+	ab5500_fg_check_capacity_limits(di, false);
+}
+
+/**
+ * ab5500_fg_algorithm_discharging() - FG algorithm for when discharging
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Battery capacity calculation state machine for when we're discharging
+ */
+static void ab5500_fg_algorithm_discharging(struct ab5500_fg *di)
+{
+	int sleep_time;
+
+	/* If we change to charge mode we should start with init */
+	if (di->charge_state != AB5500_FG_CHARGE_INIT)
+		ab5500_fg_charge_state_to(di, AB5500_FG_CHARGE_INIT);
+
+	switch (di->discharge_state) {
+	case AB5500_FG_DISCHARGE_INIT:
+		/* We use the FG IRQ to work on */
+		di->init_cnt = 0;
+		di->fg_samples = SEC_TO_SAMPLE(di->bat->fg_params->init_timer);
+		ab5500_fg_coulomb_counter(di, true);
+		ab5500_fg_discharge_state_to(di,
+			AB5500_FG_DISCHARGE_INITMEASURING);
+
+		/* Intentional fallthrough */
+	case AB5500_FG_DISCHARGE_INITMEASURING:
+		/*
+		 * Discard a number of samples during startup.
+		 * After that, use compensated voltage for a few
+		 * samples to get an initial capacity.
+		 * Then go to READOUT
+		 */
+		sleep_time = di->bat->fg_params->init_timer;
+
+		/* Discard the first [x] seconds */
+		if (di->init_cnt >
+			di->bat->fg_params->init_discard_time) {
+
+			ab5500_fg_calc_cap_discharge_voltage(di, true);
+
+			ab5500_fg_check_capacity_limits(di, true);
+		}
+
+		di->init_cnt += sleep_time;
+		if (di->init_cnt >
+			di->bat->fg_params->init_total_time) {
+			di->fg_samples = SEC_TO_SAMPLE(
+				di->bat->fg_params->accu_high_curr);
+
+			ab5500_fg_coulomb_counter(di, true);
+			ab5500_fg_discharge_state_to(di,
+				AB5500_FG_DISCHARGE_READOUT);
+		}
+
+		break;
+
+	case AB5500_FG_DISCHARGE_INIT_RECOVERY:
+		di->recovery_cnt = 0;
+		di->recovery_needed = true;
+		ab5500_fg_discharge_state_to(di,
+			AB5500_FG_DISCHARGE_RECOVERY);
+
+		/* Intentional fallthrough */
+
+	case AB5500_FG_DISCHARGE_RECOVERY:
+		sleep_time = di->bat->fg_params->recovery_sleep_timer;
+
+		/*
+		 * We should check the power consumption
+		 * If low, go to READOUT (after x min) or
+		 * RECOVERY_SLEEP if time left.
+		 * If high, go to READOUT
+		 */
+		di->inst_curr = ab5500_fg_inst_curr(di);
+
+		if (ab5500_fg_is_low_curr(di, di->inst_curr)) {
+			if (di->recovery_cnt >
+				di->bat->fg_params->recovery_total_time) {
+				di->fg_samples = SEC_TO_SAMPLE(
+					di->bat->fg_params->accu_high_curr);
+				ab5500_fg_coulomb_counter(di, true);
+				ab5500_fg_discharge_state_to(di,
+					AB5500_FG_DISCHARGE_READOUT);
+				di->recovery_needed = false;
+			} else {
+				queue_delayed_work(di->fg_wq,
+					&di->fg_periodic_work,
+					sleep_time * HZ);
+			}
+			di->recovery_cnt += sleep_time;
+		} else {
+			di->fg_samples = SEC_TO_SAMPLE(
+				di->bat->fg_params->accu_high_curr);
+			ab5500_fg_coulomb_counter(di, true);
+			ab5500_fg_discharge_state_to(di,
+				AB5500_FG_DISCHARGE_READOUT);
+		}
+
+		break;
+
+	case AB5500_FG_DISCHARGE_READOUT:
+		di->inst_curr = ab5500_fg_inst_curr(di);
+
+		if (ab5500_fg_is_low_curr(di, di->inst_curr)) {
+			/* Detect mode change */
+			if (di->high_curr_mode) {
+				di->high_curr_mode = false;
+				di->high_curr_cnt = 0;
+			}
+
+			if (di->recovery_needed) {
+				ab5500_fg_discharge_state_to(di,
+					AB5500_FG_DISCHARGE_RECOVERY);
+
+				queue_delayed_work(di->fg_wq,
+					&di->fg_periodic_work,
+					0);
+
+				break;
+			}
+
+			ab5500_fg_calc_cap_discharge_voltage(di, true);
+		} else {
+			mutex_lock(&di->cc_lock);
+			if (!di->flags.conv_done) {
+				/* Wasn't the CC IRQ that got us here */
+				mutex_unlock(&di->cc_lock);
+				dev_dbg(di->dev, "%s CC conv not done\n",
+					__func__);
+
+				break;
+			}
+			di->flags.conv_done = false;
+			mutex_unlock(&di->cc_lock);
+
+			/* Detect mode change */
+			if (!di->high_curr_mode) {
+				di->high_curr_mode = true;
+				di->high_curr_cnt = 0;
+			}
+
+			di->high_curr_cnt +=
+				di->bat->fg_params->accu_high_curr;
+			if (di->high_curr_cnt >
+				di->bat->fg_params->high_curr_time)
+				di->recovery_needed = true;
+
+			ab5500_fg_calc_cap_discharge_fg(di);
+		}
+
+		ab5500_fg_check_capacity_limits(di, false);
+
+		break;
+
+	case AB5500_FG_DISCHARGE_WAKEUP:
+		ab5500_fg_coulomb_counter(di, true);
+		di->inst_curr = ab5500_fg_inst_curr(di);
+
+		ab5500_fg_calc_cap_discharge_voltage(di, true);
+
+		di->fg_samples = SEC_TO_SAMPLE(
+			di->bat->fg_params->accu_high_curr);
+		/* Re-program number of samples set above */
+		ab5500_fg_coulomb_counter(di, true);
+		ab5500_fg_discharge_state_to(di, AB5500_FG_DISCHARGE_READOUT);
+
+		ab5500_fg_check_capacity_limits(di, false);
+
+		break;
+
+	default:
+		break;
+	}
+}
+
+/**
+ * ab5500_fg_algorithm_calibrate() - Internal columb counter offset calibration
+ * @di:		pointer to the ab5500_fg structure
+ *
+ */
+static void ab5500_fg_algorithm_calibrate(struct ab5500_fg *di)
+{
+	int ret;
+
+	switch (di->calib_state) {
+	case AB5500_FG_CALIB_INIT:
+		dev_dbg(di->dev, "Calibration ongoing...\n");
+		/* TODO: For Cut 1.1 no calibration */
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_CONTROL_A,
+			FG_ACC_RESET_ON_READ_MASK, FG_ACC_RESET_ON_READ);
+		if (ret)
+			goto err;
+		di->calib_state = AB5500_FG_CALIB_WAIT;
+		break;
+	case AB5500_FG_CALIB_END:
+		di->flags.calibrate = false;
+		dev_dbg(di->dev, "Calibration done...\n");
+		queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+		break;
+	case AB5500_FG_CALIB_WAIT:
+		dev_dbg(di->dev, "Calibration WFI\n");
+	default:
+		break;
+	}
+	return;
+err:
+	/* Something went wrong, don't calibrate then */
+	dev_err(di->dev, "failed to calibrate the CC\n");
+	di->flags.calibrate = false;
+	di->calib_state = AB5500_FG_CALIB_INIT;
+	queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+}
+
+/**
+ * ab5500_fg_algorithm() - Entry point for the FG algorithm
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Entry point for the battery capacity calculation state machine
+ */
+static void ab5500_fg_algorithm(struct ab5500_fg *di)
+{
+	if (di->flags.calibrate)
+		ab5500_fg_algorithm_calibrate(di);
+	else {
+		if (di->flags.charging)
+			ab5500_fg_algorithm_charging(di);
+		else
+			ab5500_fg_algorithm_discharging(di);
+	}
+
+	dev_dbg(di->dev, "[FG_DATA] %d %d %d %d %d %d %d %d %d "
+		"%d %d %d %d %d %d %d\n",
+		di->bat_cap.max_mah_design,
+		di->bat_cap.mah,
+		di->bat_cap.permille,
+		di->bat_cap.level,
+		di->bat_cap.prev_mah,
+		di->bat_cap.prev_percent,
+		di->bat_cap.prev_level,
+		di->vbat,
+		di->inst_curr,
+		di->avg_curr,
+		di->accu_charge,
+		di->flags.charging,
+		di->charge_state,
+		di->discharge_state,
+		di->high_curr_mode,
+		di->recovery_needed);
+}
+
+/**
+ * ab5500_fg_periodic_work() - Run the FG state machine periodically
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for periodic work
+ */
+static void ab5500_fg_periodic_work(struct work_struct *work)
+{
+	struct ab5500_fg *di = container_of(work, struct ab5500_fg,
+		fg_periodic_work.work);
+
+	if (di->init_capacity) {
+		/* A dummy read that will return 0 */
+		di->inst_curr = ab5500_fg_inst_curr(di);
+		/* Get an initial capacity calculation */
+		ab5500_fg_calc_cap_discharge_voltage(di, true);
+		ab5500_fg_check_capacity_limits(di, true);
+		di->init_capacity = false;
+		queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+	} else
+		ab5500_fg_algorithm(di);
+}
+
+/**
+ * ab5500_fg_low_bat_work() - Check LOW_BAT condition
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for checking the LOW_BAT condition
+ */
+static void ab5500_fg_low_bat_work(struct work_struct *work)
+{
+	int vbat;
+
+	struct ab5500_fg *di = container_of(work, struct ab5500_fg,
+		fg_low_bat_work.work);
+
+	vbat = ab5500_fg_bat_voltage(di);
+
+	/* Check if LOW_BAT still fulfilled */
+	if (vbat < di->bat->fg_params->lowbat_threshold) {
+		di->flags.low_bat = true;
+		dev_warn(di->dev, "Battery voltage still LOW\n");
+
+		/*
+		 * We need to re-schedule this check to be able to detect
+		 * if the voltage increases again during charging
+		 */
+		queue_delayed_work(di->fg_wq, &di->fg_low_bat_work,
+			round_jiffies(LOW_BAT_CHECK_INTERVAL));
+		power_supply_changed(&di->fg_psy);
+	} else {
+		di->flags.low_bat = false;
+		dev_warn(di->dev, "Battery voltage OK again\n");
+		power_supply_changed(&di->fg_psy);
+	}
+
+	/* This is needed to dispatch LOW_BAT */
+	ab5500_fg_check_capacity_limits(di, false);
+
+	/* Set this flag to check if LOW_BAT IRQ still occurs */
+	di->flags.low_bat_delay = false;
+}
+
+/**
+ * ab5500_fg_instant_work() - Run the FG state machine instantly
+ * @work:	pointer to the work_struct structure
+ *
+ * Work queue function for instant work
+ */
+static void ab5500_fg_instant_work(struct work_struct *work)
+{
+	struct ab5500_fg *di = container_of(work, struct ab5500_fg, fg_work);
+
+	ab5500_fg_algorithm(di);
+}
+
+/**
+ * ab5500_fg_get_property() - get the fg properties
+ * @psy:	pointer to the power_supply structure
+ * @psp:	pointer to the power_supply_property structure
+ * @val:	pointer to the power_supply_propval union
+ *
+ * This function gets called when an application tries to get the
+ * fg properties by reading the sysfs files.
+ * voltage_now:		battery voltage
+ * current_now:		battery instant current
+ * current_avg:		battery average current
+ * charge_full_design:	capacity where battery is considered full
+ * charge_now:		battery capacity in nAh
+ * capacity:		capacity in percent
+ * capacity_level:	capacity level
+ *
+ * Returns error code in case of failure else 0 on success
+ */
+static int ab5500_fg_get_property(struct power_supply *psy,
+	enum power_supply_property psp,
+	union power_supply_propval *val)
+{
+	struct ab5500_fg *di;
+
+	di = to_ab5500_fg_device_info(psy);
+
+	/*
+	 * If battery is identified as unknown and charging of unknown
+	 * batteries is disabled, we always report 100% capacity and
+	 * capacity level UNKNOWN, since we can't calculate
+	 * remaining capacity
+	 */
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		if (di->flags.bat_ovv)
+			val->intval = 47500000;
+		else {
+			di->vbat = ab5500_gpadc_convert
+					(di->gpadc, MAIN_BAT_V);
+			val->intval = di->vbat * 1000;
+		}
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		di->inst_curr = ab5500_fg_inst_curr(di);
+		val->intval = di->inst_curr * 1000;
+		break;
+	case POWER_SUPPLY_PROP_CURRENT_AVG:
+		val->intval = di->avg_curr * 1000;
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN:
+		val->intval = ab5500_fg_convert_mah_to_uwh(di,
+				di->bat_cap.max_mah_design);
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_FULL:
+		val->intval = ab5500_fg_convert_mah_to_uwh(di,
+				di->bat_cap.max_mah);
+		break;
+	case POWER_SUPPLY_PROP_ENERGY_NOW:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = ab5500_fg_convert_mah_to_uwh(di,
+					di->bat_cap.max_mah);
+		else
+			val->intval = ab5500_fg_convert_mah_to_uwh(di,
+					di->bat_cap.prev_mah);
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
+		val->intval = di->bat_cap.max_mah_design;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_FULL:
+		val->intval = di->bat_cap.max_mah;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = di->bat_cap.max_mah;
+		else
+			val->intval = di->bat_cap.prev_mah;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = 100;
+		else
+			val->intval = di->bat_cap.prev_percent;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY_LEVEL:
+		if (di->flags.batt_unknown && !di->bat->chg_unknown_bat)
+			val->intval = POWER_SUPPLY_CAPACITY_LEVEL_UNKNOWN;
+		else
+			val->intval = di->bat_cap.prev_level;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int ab5500_fg_get_ext_psy_data(struct device *dev, void *data)
+{
+	struct power_supply *psy;
+	struct power_supply *ext;
+	struct ab5500_fg *di;
+	union power_supply_propval ret;
+	int i, j;
+	bool psy_found = false;
+
+	psy = (struct power_supply *)data;
+	ext = dev_get_drvdata(dev);
+	di = to_ab5500_fg_device_info(psy);
+
+	/*
+	 * For all psy where the name of your driver
+	 * appears in any supplied_to
+	 */
+	for (i = 0; i < ext->num_supplicants; i++) {
+		if (!strcmp(ext->supplied_to[i], psy->name))
+			psy_found = true;
+	}
+
+	if (!psy_found)
+		return 0;
+
+	/* Go through all properties for the psy */
+	for (j = 0; j < ext->num_properties; j++) {
+		enum power_supply_property prop;
+		prop = ext->properties[j];
+
+		if (ext->get_property(ext, prop, &ret))
+			continue;
+
+		switch (prop) {
+		case POWER_SUPPLY_PROP_STATUS:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				switch (ret.intval) {
+				case POWER_SUPPLY_STATUS_UNKNOWN:
+				case POWER_SUPPLY_STATUS_DISCHARGING:
+				case POWER_SUPPLY_STATUS_NOT_CHARGING:
+					if (!di->flags.charging)
+						break;
+					di->flags.charging = false;
+					di->flags.fully_charged = false;
+					queue_work(di->fg_wq, &di->fg_work);
+					break;
+				case POWER_SUPPLY_STATUS_FULL:
+					if (di->flags.fully_charged)
+						break;
+					di->flags.fully_charged = true;
+					/* Save current capacity as maximum */
+					di->bat_cap.max_mah = di->bat_cap.mah;
+					queue_work(di->fg_wq, &di->fg_work);
+					break;
+				case POWER_SUPPLY_STATUS_CHARGING:
+					if (di->flags.charging)
+						break;
+					di->flags.charging = true;
+					di->flags.fully_charged = false;
+					queue_work(di->fg_wq, &di->fg_work);
+					break;
+				};
+			default:
+				break;
+			};
+			break;
+		case POWER_SUPPLY_PROP_TECHNOLOGY:
+			switch (ext->type) {
+			case POWER_SUPPLY_TYPE_BATTERY:
+				if (ret.intval)
+					di->flags.batt_unknown = false;
+				else
+					di->flags.batt_unknown = true;
+				break;
+			default:
+				break;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+/**
+ * ab5500_fg_init_hw_registers() - Set up FG related registers
+ * @di:		pointer to the ab5500_fg structure
+ *
+ * Set up battery OVV, low battery voltage registers
+ */
+static int ab5500_fg_init_hw_registers(struct ab5500_fg *di)
+{
+	int ret;
+	struct adc_auto_input *auto_ip;
+
+	auto_ip = kzalloc(sizeof(struct adc_auto_input), GFP_KERNEL);
+	if (!auto_ip) {
+		dev_err(di->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	auto_ip->mux = MAIN_BAT_V;
+	auto_ip->freq = MS500;
+	auto_ip->min = di->bat->fg_params->lowbat_threshold;
+	auto_ip->max = di->bat->fg_params->overbat_threshold;
+	auto_ip->auto_adc_callback = ab5500_fg_bat_v_trig;
+	di->gpadc_auto = auto_ip;
+	ret = ab5500_gpadc_convert_auto(di->gpadc, di->gpadc_auto);
+	if (ret)
+		dev_err(di->dev,
+			"failed to set auto trigger for battery votlage\n");
+	/* set End Of Charge current to 247mA */
+	ret = abx500_set_register_interruptible(di->dev,
+			AB5500_BANK_FG_BATTCOM_ACC, AB5500_FG_EOC, EOC_52_mA);
+	return ret;
+}
+
+static int ab5500_fg_bat_v_trig(int mux)
+{
+	struct ab5500_fg *di = ab5500_fg_get();
+
+	di->vbat = ab5500_gpadc_convert(di->gpadc, MAIN_BAT_V);
+
+	/* check if the battery voltage is below low threshold */
+	if (di->vbat < di->bat->fg_params->lowbat_threshold) {
+		dev_warn(di->dev, "Battery voltage is below LOW threshold\n");
+		di->flags.low_bat_delay = true;
+		/*
+		 * Start a timer to check LOW_BAT again after some time
+		 * This is done to avoid shutdown on single voltage dips
+		 */
+		queue_delayed_work(di->fg_wq, &di->fg_low_bat_work,
+			round_jiffies(LOW_BAT_CHECK_INTERVAL));
+		power_supply_changed(&di->fg_psy);
+	}
+	/* check if battery votlage is above OVV */
+	else if (di->vbat > di->bat->fg_params->overbat_threshold) {
+		dev_warn(di->dev, "Battery OVV\n");
+		di->flags.bat_ovv = true;
+
+		power_supply_changed(&di->fg_psy);
+	} else
+		dev_err(di->dev,
+			"Invalid gpadc auto trigger for battery voltage\n");
+
+	kfree(di->gpadc_auto);
+	ab5500_fg_init_hw_registers(di);
+	return 0;
+}
+
+/**
+ * ab5500_fg_external_power_changed() - callback for power supply changes
+ * @psy:       pointer to the structure power_supply
+ *
+ * This function is the entry point of the pointer external_power_changed
+ * of the structure power_supply.
+ * This function gets executed when there is a change in any external power
+ * supply that this driver needs to be notified of.
+ */
+static void ab5500_fg_external_power_changed(struct power_supply *psy)
+{
+	struct ab5500_fg *di = to_ab5500_fg_device_info(psy);
+
+	class_for_each_device(power_supply_class, NULL,
+		&di->fg_psy, ab5500_fg_get_ext_psy_data);
+}
+
+/**
+ * abab5500_fg_reinit_work() - work to reset the FG algorithm
+ * @work:      pointer to the work_struct structure
+ *
+ * Used to reset the current battery capacity to be able to
+ * retrigger a new voltage base capacity calculation. For
+ * test and verification purpose.
+ */
+static void ab5500_fg_reinit_work(struct work_struct *work)
+{
+	struct ab5500_fg *di = container_of(work, struct ab5500_fg,
+			fg_reinit_work.work);
+
+	if (di->flags.calibrate == false) {
+		dev_dbg(di->dev, "Resetting FG state machine to init.\n");
+		ab5500_fg_clear_cap_samples(di);
+		ab5500_fg_calc_cap_discharge_voltage(di, true);
+		ab5500_fg_charge_state_to(di, AB5500_FG_CHARGE_INIT);
+		ab5500_fg_discharge_state_to(di, AB5500_FG_DISCHARGE_INIT);
+		queue_delayed_work(di->fg_wq, &di->fg_periodic_work, 0);
+
+	} else {
+		dev_err(di->dev,
+			"Residual offset calibration ongoing retrying..\n");
+		/* Wait one second until next try*/
+		queue_delayed_work(di->fg_wq, &di->fg_reinit_work,
+							round_jiffies(1));
+	}
+}
+
+/**
+ * ab5500_fg_reinit() - forces FG algorithm to reinitialize with current values
+ *
+ * This function can be used to force the FG algorithm to recalculate a new
+ * voltage based battery capacity.
+ */
+void ab5500_fg_reinit(void)
+{
+	struct ab5500_fg *di = ab5500_fg_get();
+	/* User won't be notified if a null pointer returned. */
+	if (di != NULL)
+		queue_delayed_work(di->fg_wq, &di->fg_reinit_work, 0);
+}
+
+#if defined(CONFIG_PM)
+static int ab5500_fg_resume(struct platform_device *pdev)
+{
+	struct ab5500_fg *di = platform_get_drvdata(pdev);
+
+	/*
+	 * Change state if we're not charging. If we're charging we will wake
+	 * up on the FG IRQ
+	 */
+	if (!di->flags.charging) {
+		ab5500_fg_discharge_state_to(di, AB5500_FG_DISCHARGE_WAKEUP);
+		queue_work(di->fg_wq, &di->fg_work);
+	}
+
+	return 0;
+}
+
+static int ab5500_fg_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	struct ab5500_fg *di = platform_get_drvdata(pdev);
+
+	flush_delayed_work(&di->fg_periodic_work);
+
+	/*
+	 * If the FG is enabled we will disable it before going to suspend
+	 * only if we're not charging
+	 */
+	if (di->flags.fg_enabled && !di->flags.charging)
+		ab5500_fg_coulomb_counter(di, false);
+
+	return 0;
+}
+#else
+#define ab5500_fg_suspend      NULL
+#define ab5500_fg_resume       NULL
+#endif
+
+static int __devexit ab5500_fg_remove(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct ab5500_fg *di = platform_get_drvdata(pdev);
+
+	/* Disable coulomb counter */
+	ret = ab5500_fg_coulomb_counter(di, false);
+	if (ret)
+		dev_err(di->dev, "failed to disable coulomb counter\n");
+
+	destroy_workqueue(di->fg_wq);
+
+	flush_scheduled_work();
+	power_supply_unregister(&di->fg_psy);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di->gpadc_auto);
+	kfree(di);
+	return ret;
+}
+
+static int __devinit ab5500_fg_probe(struct platform_device *pdev)
+{
+	struct abx500_bm_plat_data *plat_data;
+	int ret = 0;
+
+	struct ab5500_fg *di =
+		kzalloc(sizeof(struct ab5500_fg), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	mutex_init(&di->cc_lock);
+
+	/* get parent data */
+	di->dev = &pdev->dev;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	di->gpadc = ab5500_gpadc_get("ab5500-adc.0");
+
+	plat_data = pdev->dev.platform_data;
+	di->pdata = plat_data->fg;
+	di->bat = plat_data->battery;
+
+	/* get fg specific platform data */
+	if (!di->pdata) {
+		dev_err(di->dev, "no fg platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+
+	/* get battery specific platform data */
+	if (!di->bat) {
+		dev_err(di->dev, "no battery platform data supplied\n");
+		ret = -EINVAL;
+		goto free_device_info;
+	}
+	/* powerup fg to start sampling */
+	ab5500_fg_coulomb_counter(di, true);
+
+	di->fg_psy.name = "ab5500_fg";
+	di->fg_psy.type = POWER_SUPPLY_TYPE_BATTERY;
+	di->fg_psy.properties = ab5500_fg_props;
+	di->fg_psy.num_properties = ARRAY_SIZE(ab5500_fg_props);
+	di->fg_psy.get_property = ab5500_fg_get_property;
+	di->fg_psy.supplied_to = di->pdata->supplied_to;
+	di->fg_psy.num_supplicants = di->pdata->num_supplicants;
+	di->fg_psy.external_power_changed = ab5500_fg_external_power_changed;
+
+	di->bat_cap.max_mah_design = MILLI_TO_MICRO *
+		di->bat->bat_type[di->bat->batt_id].charge_full_design;
+
+	di->bat_cap.max_mah = di->bat_cap.max_mah_design;
+
+	di->vbat_nom = di->bat->bat_type[di->bat->batt_id].nominal_voltage;
+
+	di->init_capacity = true;
+
+	ab5500_fg_charge_state_to(di, AB5500_FG_CHARGE_INIT);
+	ab5500_fg_discharge_state_to(di, AB5500_FG_DISCHARGE_INIT);
+
+	/* Create a work queue for running the FG algorithm */
+	di->fg_wq = create_singlethread_workqueue("ab5500_fg_wq");
+	if (di->fg_wq == NULL) {
+		dev_err(di->dev, "failed to create work queue\n");
+		goto free_device_info;
+	}
+
+	/* Init work for running the fg algorithm instantly */
+	INIT_WORK(&di->fg_work, ab5500_fg_instant_work);
+
+	/* Init work for getting the battery accumulated current */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->fg_acc_cur_work,
+			ab5500_fg_acc_cur_work);
+
+	/* Init work for reinitialising the fg algorithm */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->fg_reinit_work,
+			ab5500_fg_reinit_work);
+
+	/* Work delayed Queue to run the state machine */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->fg_periodic_work,
+		ab5500_fg_periodic_work);
+
+	/* Work to check low battery condition */
+	INIT_DELAYED_WORK_DEFERRABLE(&di->fg_low_bat_work,
+		ab5500_fg_low_bat_work);
+
+	list_add_tail(&di->node, &ab5500_fg_list);
+
+	/* Consider battery unknown until we're informed otherwise */
+	di->flags.batt_unknown = true;
+
+	/* Register FG power supply class */
+	ret = power_supply_register(di->dev, &di->fg_psy);
+	if (ret) {
+		dev_err(di->dev, "failed to register FG psy\n");
+		goto free_fg_wq;
+	}
+
+	/* Initialize OVV, and other registers */
+	ret = ab5500_fg_init_hw_registers(di);
+	if (ret) {
+		dev_err(di->dev, "failed to initialize registers\n");
+		goto pow_unreg;
+	}
+
+	di->fg_samples = SEC_TO_SAMPLE(di->bat->fg_params->init_timer);
+
+	/* Initilialize avg current timer */
+	init_timer(&di->avg_current_timer);
+	di->avg_current_timer.function = ab5500_fg_acc_cur_timer_expired;
+	di->avg_current_timer.data = (unsigned long) di;
+	di->avg_current_timer.expires = 60 * HZ;
+	if (!timer_pending(&di->avg_current_timer))
+		add_timer(&di->avg_current_timer);
+	else
+		mod_timer(&di->avg_current_timer, 60 * HZ);
+
+	platform_set_drvdata(pdev, di);
+
+	/* Calibrate the fg first time */
+	di->flags.calibrate = true;
+	di->calib_state = AB5500_FG_CALIB_INIT;
+	/* Run the FG algorithm */
+	queue_delayed_work(di->fg_wq, &di->fg_periodic_work,
+			FG_PERIODIC_START_INTERVAL);
+	queue_delayed_work(di->fg_wq, &di->fg_acc_cur_work,
+			FG_PERIODIC_START_INTERVAL);
+
+	dev_info(di->dev, "probe success\n");
+	return ret;
+
+pow_unreg:
+	power_supply_unregister(&di->fg_psy);
+free_fg_wq:
+	destroy_workqueue(di->fg_wq);
+free_device_info:
+	kfree(di);
+
+	return ret;
+}
+
+static struct platform_driver ab5500_fg_driver = {
+	.probe = ab5500_fg_probe,
+	.remove = __devexit_p(ab5500_fg_remove),
+	.suspend = ab5500_fg_suspend,
+	.resume = ab5500_fg_resume,
+	.driver = {
+		.name = "ab5500-fg",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_fg_init(void)
+{
+	return platform_driver_register(&ab5500_fg_driver);
+}
+
+static void __exit ab5500_fg_exit(void)
+{
+	platform_driver_unregister(&ab5500_fg_driver);
+}
+
+subsys_initcall_sync(ab5500_fg_init);
+module_exit(ab5500_fg_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Johan Palsson, Karl Komierowski");
+MODULE_ALIAS("platform:ab5500-fg");
+MODULE_DESCRIPTION("AB5500 Fuel Gauge driver");
diff --git a/drivers/power/ab8500_btemp.c b/drivers/power/ab8500_btemp.c
index d8bb99394ac..5e5700cf24a 100644
--- a/drivers/power/ab8500_btemp.c
+++ b/drivers/power/ab8500_btemp.c
@@ -83,6 +83,7 @@ struct ab8500_btemp_ranges {
  * @btemp_ranges:	Battery temperature range structure
  * @btemp_wq:		Work queue for measuring the temperature periodically
  * @btemp_periodic_work:	Work for measuring the temperature periodically
+ * @initialized:	True if battery id read.
  */
 struct ab8500_btemp {
 	struct device *dev;
@@ -100,6 +101,7 @@ struct ab8500_btemp {
 	struct ab8500_btemp_ranges btemp_ranges;
 	struct workqueue_struct *btemp_wq;
 	struct delayed_work btemp_periodic_work;
+	bool initialized;
 };
 
 /* BTEMP power supply properties */
@@ -569,6 +571,13 @@ static void ab8500_btemp_periodic_work(struct work_struct *work)
 	struct ab8500_btemp *di = container_of(work,
 		struct ab8500_btemp, btemp_periodic_work.work);
 
+	if (!di->initialized) {
+		di->initialized = true;
+		/* Identify the battery */
+		if (ab8500_btemp_id(di) < 0)
+			dev_warn(di->dev, "failed to identify the battery\n");
+	}
+
 	di->bat_temp = ab8500_btemp_measure_temp(di);
 
 	if (di->bat_temp != di->prev_bat_temp) {
@@ -964,7 +973,7 @@ static int __devinit ab8500_btemp_probe(struct platform_device *pdev)
 {
 	int irq, i, ret = 0;
 	u8 val;
-	struct abx500_bm_plat_data *plat_data;
+	struct ab8500_platform_data *plat_data;
 
 	struct ab8500_btemp *di =
 		kzalloc(sizeof(struct ab8500_btemp), GFP_KERNEL);
@@ -976,8 +985,10 @@ static int __devinit ab8500_btemp_probe(struct platform_device *pdev)
 	di->parent = dev_get_drvdata(pdev->dev.parent);
 	di->gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
 
+	di->initialized = false;
+
 	/* get btemp specific platform data */
-	plat_data = pdev->dev.platform_data;
+	plat_data = dev_get_platdata(di->parent->dev);
 	di->pdata = plat_data->btemp;
 	if (!di->pdata) {
 		dev_err(di->dev, "no btemp platform data supplied\n");
@@ -1017,10 +1028,6 @@ static int __devinit ab8500_btemp_probe(struct platform_device *pdev)
 	INIT_DELAYED_WORK_DEFERRABLE(&di->btemp_periodic_work,
 		ab8500_btemp_periodic_work);
 
-	/* Identify the battery */
-	if (ab8500_btemp_id(di) < 0)
-		dev_warn(di->dev, "failed to identify the battery\n");
-
 	/* Set BTEMP thermal limits. Low and Med are fixed */
 	di->btemp_ranges.btemp_low_limit = BTEMP_THERMAL_LOW_LIMIT;
 	di->btemp_ranges.btemp_med_limit = BTEMP_THERMAL_MED_LIMIT;
diff --git a/drivers/power/ab8500_charger.c b/drivers/power/ab8500_charger.c
index e2b4accbec8..19f62729a0a 100644
--- a/drivers/power/ab8500_charger.c
+++ b/drivers/power/ab8500_charger.c
@@ -29,6 +29,7 @@
 #include <linux/mfd/abx500/ab8500-gpadc.h>
 #include <linux/mfd/abx500/ux500_chargalg.h>
 #include <linux/usb/otg.h>
+#include <asm/mach-types.h>
 
 /* Charger constants */
 #define NO_PW_CONN			0
@@ -77,6 +78,9 @@
 /* Lowest charger voltage is 3.39V -> 0x4E */
 #define LOW_VOLT_REG			0x4E
 
+/* Step up/down delay in us */
+#define STEP_UDELAY			1000
+
 /* UsbLineStatus register - usb types */
 enum ab8500_charger_link_status {
 	USB_STAT_NOT_CONFIGURED,
@@ -934,6 +938,88 @@ static int ab8500_charger_get_usb_cur(struct ab8500_charger *di)
 }
 
 /**
+ * ab8500_charger_set_current() - set charger current
+ * @di:		pointer to the ab8500_charger structure
+ * @ich:	charger current, in mA
+ * @reg:	select what charger register to set
+ *
+ * Set charger current.
+ * There is no state machine in the AB to step up/down the charger
+ * current to avoid dips and spikes on MAIN, VBUS and VBAT when
+ * charging is started. Instead we need to implement
+ * this charger current step-up/down here.
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_set_current(struct ab8500_charger *di,
+	int ich, int reg)
+{
+	int ret, i;
+	int curr_index, prev_curr_index, shift_value;
+	u8 reg_value;
+
+	switch (reg) {
+	case AB8500_MCH_IPT_CURLVL_REG:
+		shift_value = MAIN_CH_INPUT_CURR_SHIFT;
+		curr_index = ab8500_current_to_regval(ich);
+		break;
+	case AB8500_USBCH_IPT_CRNTLVL_REG:
+		shift_value = VBUS_IN_CURR_LIM_SHIFT;
+		curr_index = ab8500_vbus_in_curr_to_regval(ich);
+		break;
+	case AB8500_CH_OPT_CRNTLVL_REG:
+		shift_value = 0;
+		curr_index = ab8500_current_to_regval(ich);
+		break;
+	default:
+		dev_err(di->dev, "%s current register not valid\n", __func__);
+		return -ENXIO;
+	}
+
+	if (curr_index < 0) {
+		dev_err(di->dev, "requested current limit out-of-range\n");
+		return -ENXIO;
+	}
+
+	ret = abx500_get_register_interruptible(di->dev, AB8500_CHARGER,
+		reg, &reg_value);
+	if (ret < 0) {
+		dev_err(di->dev, "%s read failed\n", __func__);
+		return ret;
+	}
+	prev_curr_index = (reg_value >> shift_value);
+
+	/* only update current if it's been changed */
+	if (prev_curr_index == curr_index)
+		return 0;
+
+	dev_dbg(di->dev, "%s set charger current: %d mA for reg: 0x%02x\n",
+		__func__, ich, reg);
+
+	if (prev_curr_index > curr_index) {
+		for (i = prev_curr_index - 1; i >= curr_index; i--) {
+			ret = abx500_set_register_interruptible(di->dev,
+				AB8500_CHARGER, reg, (u8) i << shift_value);
+			if (ret) {
+				dev_err(di->dev, "%s write failed\n", __func__);
+				return ret;
+			}
+			usleep_range(STEP_UDELAY, STEP_UDELAY * 2);
+		}
+	} else {
+		for (i = prev_curr_index + 1; i <= curr_index; i++) {
+			ret = abx500_set_register_interruptible(di->dev,
+				AB8500_CHARGER, reg, (u8) i << shift_value);
+			if (ret) {
+				dev_err(di->dev, "%s write failed\n", __func__);
+				return ret;
+			}
+			usleep_range(STEP_UDELAY, STEP_UDELAY * 2);
+		}
+	}
+	return ret;
+}
+
+/**
  * ab8500_charger_set_vbus_in_curr() - set VBUS input current limit
  * @di:		pointer to the ab8500_charger structure
  * @ich_in:	charger input current limit
@@ -944,8 +1030,6 @@ static int ab8500_charger_get_usb_cur(struct ab8500_charger *di)
 static int ab8500_charger_set_vbus_in_curr(struct ab8500_charger *di,
 		int ich_in)
 {
-	int ret;
-	int input_curr_index;
 	int min_value;
 
 	/* We should always use to lowest current limit */
@@ -964,19 +1048,38 @@ static int ab8500_charger_set_vbus_in_curr(struct ab8500_charger *di,
 		break;
 	}
 
-	input_curr_index = ab8500_vbus_in_curr_to_regval(min_value);
-	if (input_curr_index < 0) {
-		dev_err(di->dev, "VBUS input current limit too high\n");
-		return -ENXIO;
-	}
+	return ab8500_charger_set_current(di, min_value,
+		AB8500_USBCH_IPT_CRNTLVL_REG);
+}
 
-	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
-		AB8500_USBCH_IPT_CRNTLVL_REG,
-		input_curr_index << VBUS_IN_CURR_LIM_SHIFT);
-	if (ret)
-		dev_err(di->dev, "%s write failed\n", __func__);
+/**
+ * ab8500_charger_set_main_in_curr() - set main charger input current
+ * @di:		pointer to the ab8500_charger structure
+ * @ich_in:	input charger current, in mA
+ *
+ * Set main charger input current.
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_set_main_in_curr(struct ab8500_charger *di,
+	int ich_in)
+{
+	return ab8500_charger_set_current(di, ich_in,
+		AB8500_MCH_IPT_CURLVL_REG);
+}
 
-	return ret;
+/**
+ * ab8500_charger_set_output_curr() - set charger output current
+ * @di:		pointer to the ab8500_charger structure
+ * @ich_out:	output charger current, in mA
+ *
+ * Set charger output current.
+ * Returns error code in case of failure else 0(on success)
+ */
+static int ab8500_charger_set_output_curr(struct ab8500_charger *di,
+	int ich_out)
+{
+	return ab8500_charger_set_current(di, ich_out,
+		AB8500_CH_OPT_CRNTLVL_REG);
 }
 
 /**
@@ -1088,18 +1191,19 @@ static int ab8500_charger_ac_en(struct ux500_charger *charger,
 			return ret;
 		}
 		/* MainChInputCurr: current that can be drawn from the charger*/
-		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
-			AB8500_MCH_IPT_CURLVL_REG,
-			input_curr_index << MAIN_CH_INPUT_CURR_SHIFT);
+		ret = ab8500_charger_set_main_in_curr(di,
+			di->bat->chg_params->ac_curr_max);
 		if (ret) {
-			dev_err(di->dev, "%s write failed\n", __func__);
+			dev_err(di->dev, "%s Failed to set MainChInputCurr\n",
+				__func__);
 			return ret;
 		}
 		/* ChOutputCurentLevel: protected output current */
-		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
-			AB8500_CH_OPT_CRNTLVL_REG, (u8) curr_index);
+		ret = ab8500_charger_set_output_curr(di, iset);
 		if (ret) {
-			dev_err(di->dev, "%s write failed\n", __func__);
+			dev_err(di->dev, "%s "
+				"Failed to set ChOutputCurentLevel\n",
+				__func__);
 			return ret;
 		}
 
@@ -1156,12 +1260,11 @@ static int ab8500_charger_ac_en(struct ux500_charger *charger,
 				return ret;
 			}
 
-			ret = abx500_set_register_interruptible(di->dev,
-				AB8500_CHARGER,
-				AB8500_CH_OPT_CRNTLVL_REG, CH_OP_CUR_LVL_0P1);
+			ret = ab8500_charger_set_output_curr(di, 0);
 			if (ret) {
-				dev_err(di->dev,
-					"%s write failed\n", __func__);
+				dev_err(di->dev, "%s "
+					"Failed to set ChOutputCurentLevel\n",
+					__func__);
 				return ret;
 			}
 		} else {
@@ -1264,10 +1367,11 @@ static int ab8500_charger_usb_en(struct ux500_charger *charger,
 			return ret;
 		}
 		/* ChOutputCurentLevel: protected output current */
-		ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
-			AB8500_CH_OPT_CRNTLVL_REG, (u8) curr_index);
+		ret = ab8500_charger_set_output_curr(di, ich_out);
 		if (ret) {
-			dev_err(di->dev, "%s write failed\n", __func__);
+			dev_err(di->dev, "%s "
+				"Failed to set ChOutputCurentLevel\n",
+				__func__);
 			return ret;
 		}
 		/* Check if VBAT overshoot control should be enabled */
@@ -1364,7 +1468,6 @@ static int ab8500_charger_update_charger_current(struct ux500_charger *charger,
 		int ich_out)
 {
 	int ret;
-	int curr_index;
 	struct ab8500_charger *di;
 
 	if (charger->psy.type == POWER_SUPPLY_TYPE_MAINS)
@@ -1374,18 +1477,11 @@ static int ab8500_charger_update_charger_current(struct ux500_charger *charger,
 	else
 		return -ENXIO;
 
-	curr_index = ab8500_current_to_regval(ich_out);
-	if (curr_index < 0) {
-		dev_err(di->dev,
-			"Charger current too high, "
-			"charging not started\n");
-		return -ENXIO;
-	}
-
-	ret = abx500_set_register_interruptible(di->dev, AB8500_CHARGER,
-		AB8500_CH_OPT_CRNTLVL_REG, (u8) curr_index);
+	ret = ab8500_charger_set_output_curr(di, ich_out);
 	if (ret) {
-		dev_err(di->dev, "%s write failed\n", __func__);
+		dev_err(di->dev, "%s "
+			"Failed to set ChOutputCurentLevel\n",
+			__func__);
 		return ret;
 	}
 
@@ -2354,11 +2450,18 @@ static int ab8500_charger_init_hw_registers(struct ab8500_charger *di)
 	}
 
 	/* Backup battery voltage and current */
-	ret = abx500_set_register_interruptible(di->dev,
-		AB8500_RTC,
-		AB8500_RTC_BACKUP_CHG_REG,
-		di->bat->bkup_bat_v |
-		di->bat->bkup_bat_i);
+	if (machine_is_snowball())
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_RTC,
+			AB8500_RTC_BACKUP_CHG_REG,
+			BUP_VCH_SEL_3P1V |
+			BUP_ICH_SEL_150UA);
+	else
+		ret = abx500_set_register_interruptible(di->dev,
+			AB8500_RTC,
+			AB8500_RTC_BACKUP_CHG_REG,
+			di->bat->bkup_bat_v |
+			di->bat->bkup_bat_i);
 	if (ret) {
 		dev_err(di->dev, "failed to setup backup battery charging\n");
 		goto out;
@@ -2534,7 +2637,7 @@ static int __devexit ab8500_charger_remove(struct platform_device *pdev)
 static int __devinit ab8500_charger_probe(struct platform_device *pdev)
 {
 	int irq, i, charger_status, ret = 0;
-	struct abx500_bm_plat_data *plat_data;
+	struct ab8500_platform_data *plat_data;
 
 	struct ab8500_charger *di =
 		kzalloc(sizeof(struct ab8500_charger), GFP_KERNEL);
@@ -2550,7 +2653,8 @@ static int __devinit ab8500_charger_probe(struct platform_device *pdev)
 	spin_lock_init(&di->usb_state.usb_lock);
 
 	/* get charger specific platform data */
-	plat_data = pdev->dev.platform_data;
+	plat_data = dev_get_platdata(di->parent->dev);
+
 	di->pdata = plat_data->charger;
 
 	if (!di->pdata) {
diff --git a/drivers/power/ab8500_fg.c b/drivers/power/ab8500_fg.c
index c22f2f05657..798f5f7cef4 100644
--- a/drivers/power/ab8500_fg.c
+++ b/drivers/power/ab8500_fg.c
@@ -485,8 +485,9 @@ static int ab8500_fg_coulomb_counter(struct ab8500_fg *di, bool enable)
 		di->flags.fg_enabled = true;
 	} else {
 		/* Clear any pending read requests */
-		ret = abx500_set_register_interruptible(di->dev,
-			AB8500_GAS_GAUGE, AB8500_GASG_CC_CTRL_REG, 0);
+		ret = abx500_mask_and_set_register_interruptible(di->dev,
+			AB8500_GAS_GAUGE, AB8500_GASG_CC_CTRL_REG,
+			(RESET_ACCU | READ_REQ), 0);
 		if (ret)
 			goto cc_err;
 
@@ -1404,8 +1405,7 @@ static void ab8500_fg_algorithm_discharging(struct ab8500_fg *di)
 		sleep_time = di->bat->fg_params->init_timer;
 
 		/* Discard the first [x] seconds */
-		if (di->init_cnt >
-			di->bat->fg_params->init_discard_time) {
+		if (di->init_cnt > di->bat->fg_params->init_discard_time) {
 			ab8500_fg_calc_cap_discharge_voltage(di, true);
 
 			ab8500_fg_check_capacity_limits(di, true);
@@ -2446,7 +2446,7 @@ static int __devinit ab8500_fg_probe(struct platform_device *pdev)
 {
 	int i, irq;
 	int ret = 0;
-	struct abx500_bm_plat_data *plat_data;
+	struct ab8500_platform_data *plat_data;
 
 	struct ab8500_fg *di =
 		kzalloc(sizeof(struct ab8500_fg), GFP_KERNEL);
@@ -2461,7 +2461,7 @@ static int __devinit ab8500_fg_probe(struct platform_device *pdev)
 	di->gpadc = ab8500_gpadc_get("ab8500-gpadc.0");
 
 	/* get fg specific platform data */
-	plat_data = pdev->dev.platform_data;
+	plat_data = dev_get_platdata(di->parent->dev);
 	di->pdata = plat_data->fg;
 	if (!di->pdata) {
 		dev_err(di->dev, "no fg platform data supplied\n");
diff --git a/drivers/power/abx500_chargalg.c b/drivers/power/abx500_chargalg.c
index 804b88c760d..032b27d35bf 100644
--- a/drivers/power/abx500_chargalg.c
+++ b/drivers/power/abx500_chargalg.c
@@ -220,6 +220,7 @@ enum maxim_ret {
  */
 struct abx500_chargalg {
 	struct device *dev;
+	struct ab8500 *parent;
 	int charge_status;
 	int eoc_cnt;
 	int rch_cnt;
@@ -1802,7 +1803,7 @@ static int __devexit abx500_chargalg_remove(struct platform_device *pdev)
 
 static int __devinit abx500_chargalg_probe(struct platform_device *pdev)
 {
-	struct abx500_bm_plat_data *plat_data;
+	struct ab8500_platform_data *plat_data;
 	int ret = 0;
 
 	struct abx500_chargalg *di =
@@ -1812,8 +1813,8 @@ static int __devinit abx500_chargalg_probe(struct platform_device *pdev)
 
 	/* get device struct */
 	di->dev = &pdev->dev;
-
-	plat_data = pdev->dev.platform_data;
+	di->parent = dev_get_drvdata(pdev->dev.parent);
+	plat_data = dev_get_platdata(di->parent->dev);
 	di->pdata = plat_data->chargalg;
 	di->bat = plat_data->battery;
 
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 36db5a441eb..7278f1e4141 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -247,6 +247,14 @@ config REGULATOR_AB8500
 	  This driver supports the regulators found on the ST-Ericsson mixed
 	  signal AB8500 PMIC
 
+config REGULATOR_AB8500_EXT
+	bool "ST-Ericsson AB8500 External Regulators"
+	depends on REGULATOR_AB8500
+	default y if REGULATOR_AB8500
+	help
+	  This driver supports the external regulator controls found on the
+	  ST-Ericsson mixed signal AB8500 PMIC
+
 config REGULATOR_DBX500_PRCMU
 	bool
 
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 94b52745e95..b5b90fb232b 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_REGULATOR_88PM8607) += 88pm8607.o
 obj-$(CONFIG_REGULATOR_AAT2870) += aat2870-regulator.o
 obj-$(CONFIG_REGULATOR_AB3100) += ab3100.o
 obj-$(CONFIG_REGULATOR_AB8500)	+= ab8500.o
+obj-$(CONFIG_REGULATOR_AB8500_EXT) += ab8500-ext.o
 obj-$(CONFIG_REGULATOR_AD5398) += ad5398.o
 obj-$(CONFIG_REGULATOR_ANATOP) += anatop-regulator.o
 obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
diff --git a/drivers/regulator/ab5500.c b/drivers/regulator/ab5500.c
new file mode 100644
index 00000000000..99676f7ad8e
--- /dev/null
+++ b/drivers/regulator/ab5500.c
@@ -0,0 +1,650 @@
+/*
+ * Copyright (C) 2011 ST-Ericsson SA
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ *
+ * Based on ab3100.c.
+ *
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+#include <linux/regulator/ab5500.h>
+
+#define AB5500_LDO_VDIGMIC_ST	0x50
+
+#define AB5500_LDO_G_ST		0x78
+#define AB5500_LDO_G_PWR1	0x79
+#define AB5500_LDO_G_PWR0	0x7a
+
+#define AB5500_LDO_H_ST		0x7b
+#define AB5500_LDO_H_PWR1	0x7c
+#define AB5500_LDO_H_PWR0	0x7d
+
+#define AB5500_LDO_K_ST		0x7e
+#define AB5500_LDO_K_PWR1	0x7f
+#define AB5500_LDO_K_PWR0	0x80
+
+#define AB5500_LDO_L_ST		0x81
+#define AB5500_LDO_L_PWR1	0x82
+#define AB5500_LDO_L_PWR0	0x83
+
+/* In SIM bank */
+#define AB5500_SIM_SUP		0x14
+
+#define AB5500_MBIAS1		0x00
+#define AB5500_MBIAS2		0x01
+
+#define AB5500_LDO_MODE_MASK		(0x3 << 4)
+#define AB5500_LDO_MODE_FULLPOWER	(0x3 << 4)
+#define AB5500_LDO_MODE_PWRCTRL		(0x2 << 4)
+#define AB5500_LDO_MODE_LOWPOWER	(0x1 << 4)
+#define AB5500_LDO_MODE_OFF		(0x0 << 4)
+#define AB5500_LDO_VOLT_MASK		0x07
+
+#define AB5500_MBIAS1_ENABLE		(0x1 << 1)
+#define AB5500_MBIAS1_MODE_MASK		(0x1 << 1)
+#define AB5500_MBIAS2_ENABLE		(0x1 << 1)
+#define AB5500_MBIAS2_VOLT_MASK		(0x1 << 2)
+#define AB5500_MBIAS2_MODE_MASK		(0x1 << 1)
+
+struct ab5500_regulator {
+	struct regulator_desc desc;
+	const int *voltages;
+	int num_holes;
+	bool off_is_lowpower;
+	bool enabled;
+	int enable_time;
+	int load_lp_uA;
+	u8 bank;
+	u8 reg;
+	u8 mode;
+	u8 update_mask;
+	u8 update_val_idle;
+	u8 update_val_normal;
+	u8 voltage_mask;
+};
+
+struct ab5500_regulators {
+	struct device *dev;
+	struct ab5500_regulator *regulator[AB5500_NUM_REGULATORS];
+	struct regulator_dev *rdev[AB5500_NUM_REGULATORS];
+};
+
+static int ab5500_regulator_enable_time(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+
+	return r->enable_time; /* microseconds */
+}
+
+static int ab5500_regulator_enable(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	int ret;
+
+	ret = abx500_mask_and_set(ab5500->dev, r->bank, r->reg,
+				  r->update_mask, r->mode);
+	if (ret < 0)
+		return ret;
+
+	r->enabled = true;
+
+	return 0;
+}
+
+static int ab5500_regulator_disable(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	u8 regval;
+	int ret;
+
+	if (r->off_is_lowpower)
+		regval = AB5500_LDO_MODE_LOWPOWER;
+	else
+		regval = AB5500_LDO_MODE_OFF;
+
+	ret = abx500_mask_and_set(ab5500->dev, r->bank, r->reg,
+				  r->update_mask, regval);
+	if (ret < 0)
+		return ret;
+
+	r->enabled = false;
+
+	return 0;
+}
+
+static unsigned int ab5500_regulator_get_mode(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+
+	if (r->mode == r->update_val_idle)
+		return REGULATOR_MODE_IDLE;
+
+	return REGULATOR_MODE_NORMAL;
+}
+
+static unsigned int
+ab5500_regulator_get_optimum_mode(struct regulator_dev *rdev,
+				  int input_uV, int output_uV, int load_uA)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	unsigned int mode;
+
+	if (load_uA <= r->load_lp_uA)
+		mode = REGULATOR_MODE_IDLE;
+	else
+		mode = REGULATOR_MODE_NORMAL;
+
+	return mode;
+}
+
+static int ab5500_regulator_set_mode(struct regulator_dev *rdev,
+				     unsigned int mode)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		r->mode = r->update_val_normal;
+		break;
+	case REGULATOR_MODE_IDLE:
+		r->mode = r->update_val_idle;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (r->enabled)
+		return ab5500_regulator_enable(rdev);
+
+	return 0;
+}
+
+static int ab5500_regulator_is_enabled(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	u8 regval;
+	int err;
+
+	err = abx500_get_register_interruptible(ab5500->dev,
+						r->bank, r->reg, &regval);
+	if (err) {
+		dev_err(rdev_get_dev(rdev), "unable to get register 0x%x\n",
+			r->reg);
+		return err;
+	}
+
+	switch (regval & r->update_mask) {
+	case AB5500_LDO_MODE_PWRCTRL:
+	case AB5500_LDO_MODE_OFF:
+		r->enabled = false;
+		break;
+	case AB5500_LDO_MODE_LOWPOWER:
+		if (r->off_is_lowpower) {
+			r->enabled = false;
+			break;
+		}
+		/* fall through */
+	default:
+		r->enabled = true;
+		break;
+	}
+
+	return r->enabled;
+}
+
+static int
+ab5500_regulator_list_voltage(struct regulator_dev *rdev, unsigned selector)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	unsigned n_voltages = r->desc.n_voltages;
+	int selindex;
+	int i;
+
+	for (i = 0, selindex = 0; selindex < n_voltages; i++) {
+		int voltage = r->voltages[i];
+
+		if (!voltage)
+			continue;
+
+		if (selindex == selector)
+			return voltage;
+
+		selindex++;
+	}
+
+	return -EINVAL;
+}
+
+static int ab5500_regulator_fixed_get_voltage(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+
+	return r->voltages[0];
+}
+
+static int ab5500_regulator_get_voltage(struct regulator_dev *rdev)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	u8 regval;
+	int ret;
+
+	ret = abx500_get_register_interruptible(ab5500->dev,
+						r->bank, r->reg, &regval);
+	if (ret) {
+		dev_warn(rdev_get_dev(rdev),
+			"failed to get regulator value in register "
+			"%02x\n", r->reg);
+		return ret;
+	}
+
+	regval &= r->voltage_mask;
+	if (regval >= r->desc.n_voltages + r->num_holes)
+	       return -EINVAL;
+
+	if (!r->voltages[regval])
+		return -EINVAL;
+
+	return r->voltages[regval];
+}
+
+static int ab5500_get_best_voltage_index(struct ab5500_regulator *r,
+					 int min_uV, int max_uV)
+{
+	unsigned n_voltages = r->desc.n_voltages;
+	int bestmatch = INT_MAX;
+	int bestindex = -EINVAL;
+	int selindex;
+	int i;
+
+	/*
+	 * Locate the minimum voltage fitting the criteria on
+	 * this regulator. The switchable voltages are not
+	 * in strict falling order so we need to check them
+	 * all for the best match.
+	 */
+	for (i = 0, selindex = 0; selindex < n_voltages; i++) {
+		int voltage = r->voltages[i];
+
+		if (!voltage)
+			continue;
+
+		if (voltage <= max_uV &&
+		    voltage >= min_uV &&
+		    voltage < bestmatch) {
+			bestmatch = voltage;
+			bestindex = i;
+		}
+
+		selindex++;
+	}
+
+	return bestindex;
+}
+
+static int ab5500_regulator_set_voltage(struct regulator_dev *rdev,
+					int min_uV, int max_uV,
+					unsigned *selector)
+{
+	struct ab5500_regulators *ab5500 = rdev_get_drvdata(rdev);
+	struct ab5500_regulator *r = ab5500->regulator[rdev_get_id(rdev)];
+	int bestindex;
+
+	bestindex = ab5500_get_best_voltage_index(r, min_uV, max_uV);
+	if (bestindex < 0) {
+		dev_warn(rdev_get_dev(rdev),
+			 "requested %d<=x<=%d uV, out of range!\n",
+			 min_uV, max_uV);
+		return bestindex;
+	}
+
+	*selector = bestindex;
+
+	return abx500_mask_and_set_register_interruptible(ab5500->dev,
+			r->bank, r->reg, r->voltage_mask, bestindex);
+
+}
+
+static struct regulator_ops ab5500_regulator_variable_ops = {
+	.enable		= ab5500_regulator_enable,
+	.disable	= ab5500_regulator_disable,
+	.is_enabled	= ab5500_regulator_is_enabled,
+	.enable_time	= ab5500_regulator_enable_time,
+	.get_voltage	= ab5500_regulator_get_voltage,
+	.set_voltage	= ab5500_regulator_set_voltage,
+	.list_voltage	= ab5500_regulator_list_voltage,
+	.set_mode	= ab5500_regulator_set_mode,
+	.get_mode	= ab5500_regulator_get_mode,
+	.get_optimum_mode = ab5500_regulator_get_optimum_mode,
+};
+
+static struct regulator_ops ab5500_regulator_fixed_ops = {
+	.enable		= ab5500_regulator_enable,
+	.disable	= ab5500_regulator_disable,
+	.is_enabled	= ab5500_regulator_is_enabled,
+	.enable_time	= ab5500_regulator_enable_time,
+	.get_voltage	= ab5500_regulator_fixed_get_voltage,
+	.list_voltage	= ab5500_regulator_list_voltage,
+	.set_mode	= ab5500_regulator_set_mode,
+	.get_mode	= ab5500_regulator_get_mode,
+	.get_optimum_mode = ab5500_regulator_get_optimum_mode,
+};
+
+static const int ab5500_ldo_lg_voltages[] = {
+	[0x00] = 1200000,
+	[0x01] = 0, /* not used */
+	[0x02] = 1500000,
+	[0x03] = 1800000,
+	[0x04] = 0, /* not used */
+	[0x05] = 2500000,
+	[0x06] = 2730000,
+	[0x07] = 2910000,
+};
+
+static const int ab5500_ldo_kh_voltages[] = {
+	[0x00] = 1200000,
+	[0x01] = 1500000,
+	[0x02] = 1800000,
+	[0x03] = 2100000,
+	[0x04] = 2500000,
+	[0x05] = 2750000,
+	[0x06] = 2790000,
+	[0x07] = 2910000,
+};
+
+static const int ab5500_ldo_vdigmic_voltages[] = {
+	[0x00] = 2100000,
+};
+
+static const int ab5500_ldo_sim_voltages[] = {
+	[0x00] = 1875000,
+	[0x01] = 2800000,
+	[0x02] = 2900000,
+};
+
+static const int ab5500_bias2_voltages[] = {
+	[0x00] = 2000000,
+	[0x01] = 2200000,
+};
+
+static const int ab5500_bias1_voltages[] = {
+	[0x00] = 2000000,
+};
+
+static struct ab5500_regulator ab5500_regulators[] = {
+	[AB5500_LDO_L] = {
+		.desc = {
+			.name		= "LDO_L",
+			.id		= AB5500_LDO_L,
+			.ops		= &ab5500_regulator_variable_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ab5500_ldo_lg_voltages) -
+				2,
+		},
+		.bank			= AB5500_BANK_STARTUP,
+		.reg			= AB5500_LDO_L_ST,
+		.voltages		= ab5500_ldo_lg_voltages,
+		.num_holes		= 2, /* 2 register values unused */
+		.enable_time		= 400,
+		.load_lp_uA		= 20000,
+		.mode			= AB5500_LDO_MODE_FULLPOWER,
+		.update_mask		= AB5500_LDO_MODE_MASK,
+		.update_val_normal	= AB5500_LDO_MODE_FULLPOWER,
+		.update_val_idle	= AB5500_LDO_MODE_LOWPOWER,
+		.voltage_mask		= AB5500_LDO_VOLT_MASK,
+	},
+	[AB5500_LDO_G] = {
+		.desc = {
+			.name		= "LDO_G",
+			.id		= AB5500_LDO_G,
+			.ops		= &ab5500_regulator_variable_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.owner		= THIS_MODULE,
+			.n_voltages 	= ARRAY_SIZE(ab5500_ldo_lg_voltages) -
+				2,
+		},
+		.bank			= AB5500_BANK_STARTUP,
+		.reg			= AB5500_LDO_G_ST,
+		.voltages		= ab5500_ldo_lg_voltages,
+		.num_holes		= 2, /* 2 register values unused */
+		.enable_time		= 400,
+		.load_lp_uA		= 20000,
+		.mode			= AB5500_LDO_MODE_FULLPOWER,
+		.update_mask		= AB5500_LDO_MODE_MASK,
+		.update_val_normal	= AB5500_LDO_MODE_FULLPOWER,
+		.update_val_idle	= AB5500_LDO_MODE_LOWPOWER,
+		.voltage_mask		= AB5500_LDO_VOLT_MASK,
+	},
+	[AB5500_LDO_K] = {
+		.desc = {
+			.name		= "LDO_K",
+			.id		= AB5500_LDO_K,
+			.ops		= &ab5500_regulator_variable_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.owner		= THIS_MODULE,
+			.n_voltages 	= ARRAY_SIZE(ab5500_ldo_kh_voltages),
+		},
+		.bank			= AB5500_BANK_STARTUP,
+		.reg			= AB5500_LDO_K_ST,
+		.voltages		= ab5500_ldo_kh_voltages,
+		.enable_time		= 400,
+		.load_lp_uA		= 20000,
+		.mode			= AB5500_LDO_MODE_FULLPOWER,
+		.update_mask		= AB5500_LDO_MODE_MASK,
+		.update_val_normal	= AB5500_LDO_MODE_FULLPOWER,
+		.update_val_idle	= AB5500_LDO_MODE_LOWPOWER,
+		.voltage_mask		= AB5500_LDO_VOLT_MASK,
+	},
+	[AB5500_LDO_H] = {
+		.desc = {
+			.name		= "LDO_H",
+			.id		= AB5500_LDO_H,
+			.ops		= &ab5500_regulator_variable_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ab5500_ldo_kh_voltages),
+		},
+		.bank			= AB5500_BANK_STARTUP,
+		.reg			= AB5500_LDO_H_ST,
+		.voltages		= ab5500_ldo_kh_voltages,
+		.enable_time		= 400,
+		.load_lp_uA		= 20000,
+		.mode			= AB5500_LDO_MODE_FULLPOWER,
+		.update_mask		= AB5500_LDO_MODE_MASK,
+		.update_val_normal	= AB5500_LDO_MODE_FULLPOWER,
+		.update_val_idle	= AB5500_LDO_MODE_LOWPOWER,
+		.voltage_mask		= AB5500_LDO_VOLT_MASK,
+	},
+	[AB5500_LDO_VDIGMIC] = {
+		.desc = {
+			.name		= "LDO_VDIGMIC",
+			.id		= AB5500_LDO_VDIGMIC,
+			.ops		= &ab5500_regulator_fixed_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.owner		= THIS_MODULE,
+			.n_voltages	=
+				ARRAY_SIZE(ab5500_ldo_vdigmic_voltages),
+		},
+		.bank			= AB5500_BANK_STARTUP,
+		.reg			= AB5500_LDO_VDIGMIC_ST,
+		.voltages		= ab5500_ldo_vdigmic_voltages,
+		.enable_time		= 450,
+		.mode			= AB5500_LDO_MODE_FULLPOWER,
+		.update_mask		= AB5500_LDO_MODE_MASK,
+		.update_val_normal	= AB5500_LDO_MODE_FULLPOWER,
+		.update_val_idle	= AB5500_LDO_MODE_LOWPOWER,
+		.voltage_mask		= AB5500_LDO_VOLT_MASK,
+	},
+	[AB5500_LDO_SIM] = {
+		.desc = {
+			.name		= "LDO_SIM",
+			.id		= AB5500_LDO_SIM,
+			.ops		= &ab5500_regulator_variable_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ab5500_ldo_sim_voltages),
+		},
+		.bank			= AB5500_BANK_SIM_USBSIM,
+		.reg			= AB5500_SIM_SUP,
+		.voltages		= ab5500_ldo_sim_voltages,
+		.enable_time		= 1000,
+		.mode			= AB5500_LDO_MODE_FULLPOWER,
+		.update_mask		= AB5500_LDO_MODE_MASK,
+		.update_val_normal	= AB5500_LDO_MODE_FULLPOWER,
+		.update_val_idle	= AB5500_LDO_MODE_LOWPOWER,
+		.voltage_mask		= AB5500_LDO_VOLT_MASK,
+	},
+	[AB5500_BIAS2] = {
+		.desc = {
+			.name		= "MBIAS2",
+			.id		= AB5500_BIAS2,
+			.ops		= &ab5500_regulator_variable_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ab5500_bias2_voltages),
+		},
+		.bank			= AB5500_BANK_AUDIO_HEADSETUSB,
+		.reg			= AB5500_MBIAS2,
+		.voltages		= ab5500_bias2_voltages,
+		.enable_time		= 1000,
+		.mode			= AB5500_MBIAS2_ENABLE,
+		.update_mask		= AB5500_MBIAS2_MODE_MASK,
+		.update_val_normal	= AB5500_MBIAS2_ENABLE,
+		.update_val_idle	= AB5500_MBIAS2_ENABLE,
+		.voltage_mask		= AB5500_MBIAS2_VOLT_MASK,
+	},
+	[AB5500_BIAS1] = {
+		.desc = {
+			.name		= "MBIAS1",
+			.id		= AB5500_BIAS1,
+			.ops		= &ab5500_regulator_fixed_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ab5500_bias1_voltages),
+		},
+		.bank			= AB5500_BANK_AUDIO_HEADSETUSB,
+		.reg			= AB5500_MBIAS1,
+		.voltages		= ab5500_bias1_voltages,
+		.enable_time		= 1000,
+		.mode			= AB5500_MBIAS1_ENABLE,
+		.update_mask		= AB5500_MBIAS1_MODE_MASK,
+		.update_val_normal	= AB5500_MBIAS1_ENABLE,
+		.update_val_idle	= AB5500_MBIAS1_ENABLE,
+	},
+};
+
+
+static int __devinit ab5500_regulator_probe(struct platform_device *pdev)
+{
+	struct ab5500_platform_data *ppdata = pdev->dev.parent->platform_data;
+	struct ab5500_regulator_platform_data *pdata = ppdata->regulator;
+	struct ab5500_regulator_data *regdata;
+	struct ab5500_regulators *ab5500;
+	int err = 0;
+	int i;
+
+	if (!pdata || !pdata->regulator)
+		return -EINVAL;
+
+	ab5500 = kzalloc(sizeof(*ab5500), GFP_KERNEL);
+	if (!ab5500)
+		return -ENOMEM;
+
+	ab5500->dev = &pdev->dev;
+	regdata = pdata->data;
+
+	platform_set_drvdata(pdev, ab5500);
+
+	for (i = 0; i < AB5500_NUM_REGULATORS; i++) {
+		struct ab5500_regulator *regulator = &ab5500_regulators[i];
+		struct regulator_dev *rdev;
+
+		if (regdata)
+			regulator->off_is_lowpower = regdata[i].off_is_lowpower;
+
+		ab5500->regulator[i] = regulator;
+
+		rdev = regulator_register(&regulator->desc, &pdev->dev,
+					  &pdata->regulator[i], ab5500);
+		if (IS_ERR(rdev)) {
+			err = PTR_ERR(rdev);
+			dev_err(&pdev->dev, "failed to register regulator %s err %d\n",
+				regulator->desc.name, err);
+			goto err_unregister;
+		}
+
+		ab5500->rdev[i] = rdev;
+	}
+
+	return 0;
+
+err_unregister:
+	/* remove the already registered regulators */
+	while (--i >= 0)
+		regulator_unregister(ab5500->rdev[i]);
+
+	platform_set_drvdata(pdev, NULL);
+	kfree(ab5500);
+
+	return err;
+}
+
+static int __devexit ab5500_regulators_remove(struct platform_device *pdev)
+{
+	struct ab5500_regulators *ab5500 = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < AB5500_NUM_REGULATORS; i++)
+		regulator_unregister(ab5500->rdev[i]);
+
+	platform_set_drvdata(pdev, NULL);
+	kfree(ab5500);
+
+	return 0;
+}
+
+static struct platform_driver ab5500_regulator_driver = {
+	.driver = {
+		.name  = "ab5500-regulator",
+		.owner = THIS_MODULE,
+	},
+	.probe = ab5500_regulator_probe,
+	.remove = __devexit_p(ab5500_regulators_remove),
+};
+
+static __init int ab5500_regulator_init(void)
+{
+	return platform_driver_register(&ab5500_regulator_driver);
+}
+
+static __exit void ab5500_regulator_exit(void)
+{
+	platform_driver_unregister(&ab5500_regulator_driver);
+}
+
+subsys_initcall(ab5500_regulator_init);
+module_exit(ab5500_regulator_exit);
+
+MODULE_DESCRIPTION("AB5500 Regulator Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:ab5500-regulator");
diff --git a/drivers/regulator/ab8500-debug.c b/drivers/regulator/ab8500-debug.c
new file mode 100644
index 00000000000..f71cc26c135
--- /dev/null
+++ b/drivers/regulator/ab8500-debug.c
@@ -0,0 +1,2083 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson.
+ *
+ * License Terms: GNU General Public License v2
+ */
+
+#include <linux/module.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/debugfs.h>
+#include <linux/platform_device.h>
+#include <linux/kobject.h>
+#include <linux/slab.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab8500.h>
+#include <linux/regulator/ab8500-debug.h>
+#include <linux/io.h>
+
+#include <mach/db8500-regs.h> /* U8500_BACKUPRAM1_BASE */
+#include <mach/hardware.h>
+
+#include "ab8500-debug.h"
+
+/* board profile address - to determine if suspend-force is default */
+#define BOOT_INFO_BACKUPRAM1 (U8500_BACKUPRAM1_BASE + 0xffc)
+#define BOARD_PROFILE_BACKUPRAM1 (0x3)
+
+/* board profile option */
+#define OPTION_BOARD_VERSION_V5X 50
+
+/* for error prints */
+struct device *dev;
+struct platform_device *pdev;
+
+/* setting for suspend force (disabled by default) */
+static bool setting_suspend_force;
+
+/*
+ * regulator states
+ */
+enum ab8500_regulator_state_id {
+	AB8500_REGULATOR_STATE_INIT,
+	AB8500_REGULATOR_STATE_SUSPEND,
+	AB8500_REGULATOR_STATE_SUSPEND_CORE,
+	AB8500_REGULATOR_STATE_RESUME_CORE,
+	AB8500_REGULATOR_STATE_RESUME,
+	AB8500_REGULATOR_STATE_CURRENT,
+	NUM_REGULATOR_STATE
+};
+
+static const char *regulator_state_name[NUM_REGULATOR_STATE] = {
+	[AB8500_REGULATOR_STATE_INIT] = "init",
+	[AB8500_REGULATOR_STATE_SUSPEND] = "suspend",
+	[AB8500_REGULATOR_STATE_SUSPEND_CORE] = "suspend-core",
+	[AB8500_REGULATOR_STATE_RESUME_CORE] = "resume-core",
+	[AB8500_REGULATOR_STATE_RESUME] = "resume",
+	[AB8500_REGULATOR_STATE_CURRENT] = "current",
+};
+
+/*
+ * regulator register definitions
+ */
+enum ab8500_register_id {
+	AB8500_REGU_NOUSE, /* if not defined */
+	AB8500_REGU_REQUEST_CTRL1,
+	AB8500_REGU_REQUEST_CTRL2,
+	AB8500_REGU_REQUEST_CTRL3,
+	AB8500_REGU_REQUEST_CTRL4,
+	AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+	AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+	AB8500_REGU_HW_HP_REQ1_VALID1,
+	AB8500_REGU_HW_HP_REQ1_VALID2,
+	AB8500_REGU_HW_HP_REQ2_VALID1,
+	AB8500_REGU_HW_HP_REQ2_VALID2,
+	AB8500_REGU_SW_HP_REQ_VALID1,
+	AB8500_REGU_SW_HP_REQ_VALID2,
+	AB8500_REGU_SYSCLK_REQ1_VALID,
+	AB8500_REGU_SYSCLK_REQ2_VALID,
+	AB9540_REGU_VAUX4_REQ_VALID,
+	AB8500_REGU_MISC1,
+	AB8500_REGU_OTG_SUPPLY_CTRL,
+	AB8500_REGU_VUSB_CTRL,
+	AB8500_REGU_VAUDIO_SUPPLY,
+	AB8500_REGU_CTRL1_VAMIC,
+	AB8500_REGU_ARM_REGU1,
+	AB8500_REGU_ARM_REGU2,
+	AB8500_REGU_VAPE_REGU,
+	AB8500_REGU_VSMPS1_REGU,
+	AB8500_REGU_VSMPS2_REGU,
+	AB8500_REGU_VSMPS3_REGU,
+	AB8500_REGU_VPLL_VANA_REGU,
+	AB8500_REGU_VREF_DDR,
+	AB8500_REGU_EXT_SUPPLY_REGU,
+	AB8500_REGU_VAUX12_REGU,
+	AB8500_REGU_VRF1_VAUX3_REGU,
+	AB8500_REGU_VARM_SEL1,
+	AB8500_REGU_VARM_SEL2,
+	AB8500_REGU_VARM_SEL3,
+	AB8500_REGU_VAPE_SEL1,
+	AB8500_REGU_VAPE_SEL2,
+	AB8500_REGU_VAPE_SEL3,
+	AB9540_REGU_VAUX4_REQ_CTRL,
+	AB9540_REGU_VAUX4_REGU,
+	AB9540_REGU_VAUX4_SEL,
+	AB8500_REGU_VBB_SEL1,
+	AB8500_REGU_VBB_SEL2,
+	AB8500_REGU_VSMPS1_SEL1,
+	AB8500_REGU_VSMPS1_SEL2,
+	AB8500_REGU_VSMPS1_SEL3,
+	AB8500_REGU_VSMPS2_SEL1,
+	AB8500_REGU_VSMPS2_SEL2,
+	AB8500_REGU_VSMPS2_SEL3,
+	AB8500_REGU_VSMPS3_SEL1,
+	AB8500_REGU_VSMPS3_SEL2,
+	AB8500_REGU_VSMPS3_SEL3,
+	AB8500_REGU_VAUX1_SEL,
+	AB8500_REGU_VAUX2_SEL,
+	AB8500_REGU_VRF1_VAUX3_SEL,
+	AB8500_REGU_CTRL_EXT_SUP,
+	AB8500_REGU_VMOD_REGU,
+	AB8500_REGU_VMOD_SEL1,
+	AB8500_REGU_VMOD_SEL2,
+	AB8500_REGU_CTRL_DISCH,
+	AB8500_REGU_CTRL_DISCH2,
+	AB9540_REGU_CTRL_DISCH3,
+	AB8500_OTHER_SYSCLK_CTRL, /* Other */
+	AB8500_OTHER_VSIM_SYSCLK_CTRL, /* Other */
+	AB8500_OTHER_SYSULPCLK_CTRL1, /* Other */
+	NUM_AB8500_REGISTER
+};
+
+struct ab8500_register {
+	const char *name;
+	u8 bank;
+	u8 addr;
+	u8 unavailable; /* Used to flag when AB doesn't support a register */
+};
+
+static struct ab8500_register
+		ab8500_register[NUM_AB8500_REGISTER] = {
+	[AB8500_REGU_REQUEST_CTRL1] = {
+		.name = "ReguRequestCtrl1",
+		.bank = 0x03,
+		.addr = 0x03,
+	},
+	[AB8500_REGU_REQUEST_CTRL2] = {
+		.name = "ReguRequestCtrl2",
+		.bank = 0x03,
+		.addr = 0x04,
+	},
+	[AB8500_REGU_REQUEST_CTRL3] = {
+		.name = "ReguRequestCtrl3",
+		.bank = 0x03,
+		.addr = 0x05,
+	},
+	[AB8500_REGU_REQUEST_CTRL4] = {
+		.name = "ReguRequestCtrl4",
+		.bank = 0x03,
+		.addr = 0x06,
+	},
+	[AB8500_REGU_SYSCLK_REQ1_HP_VALID1] = {
+		.name = "ReguSysClkReq1HPValid",
+		.bank = 0x03,
+		.addr = 0x07,
+	},
+	[AB8500_REGU_SYSCLK_REQ1_HP_VALID2] = {
+		.name = "ReguSysClkReq1HPValid2",
+		.bank = 0x03,
+		.addr = 0x08,
+	},
+	[AB8500_REGU_HW_HP_REQ1_VALID1] = {
+		.name = "ReguHwHPReq1Valid1",
+		.bank = 0x03,
+		.addr = 0x09,
+	},
+	[AB8500_REGU_HW_HP_REQ1_VALID2] = {
+		.name = "ReguHwHPReq1Valid2",
+		.bank = 0x03,
+		.addr = 0x0a,
+	},
+	[AB8500_REGU_HW_HP_REQ2_VALID1] = {
+		.name = "ReguHwHPReq2Valid1",
+		.bank = 0x03,
+		.addr = 0x0b,
+	},
+	[AB8500_REGU_HW_HP_REQ2_VALID2] = {
+		.name = "ReguHwHPReq2Valid2",
+		.bank = 0x03,
+		.addr = 0x0c,
+	},
+	[AB8500_REGU_SW_HP_REQ_VALID1] = {
+		.name = "ReguSwHPReqValid1",
+		.bank = 0x03,
+		.addr = 0x0d,
+	},
+	[AB8500_REGU_SW_HP_REQ_VALID2] = {
+		.name = "ReguSwHPReqValid2",
+		.bank = 0x03,
+		.addr = 0x0e,
+	},
+	[AB8500_REGU_SYSCLK_REQ1_VALID] = {
+		.name = "ReguSysClkReqValid1",
+		.bank = 0x03,
+		.addr = 0x0f,
+	},
+	[AB8500_REGU_SYSCLK_REQ2_VALID] = {
+		.name = "ReguSysClkReqValid2",
+		.bank = 0x03,
+		.addr = 0x10,
+	},
+	[AB9540_REGU_VAUX4_REQ_VALID] = {
+		.name = "ReguVaux4ReqValid",
+		.bank = 0x03,
+		.addr = 0x11,
+		.unavailable = true, /* ab9540 register */
+	},
+	[AB8500_REGU_MISC1] = {
+		.name = "ReguMisc1",
+		.bank = 0x03,
+		.addr = 0x80,
+	},
+	[AB8500_REGU_OTG_SUPPLY_CTRL] = {
+		.name = "OTGSupplyCtrl",
+		.bank = 0x03,
+		.addr = 0x81,
+	},
+	[AB8500_REGU_VUSB_CTRL] = {
+		.name = "VusbCtrl",
+		.bank = 0x03,
+		.addr = 0x82,
+	},
+	[AB8500_REGU_VAUDIO_SUPPLY] = {
+		.name = "VaudioSupply",
+		.bank = 0x03,
+		.addr = 0x83,
+	},
+	[AB8500_REGU_CTRL1_VAMIC] = {
+		.name = "ReguCtrl1VAmic",
+		.bank = 0x03,
+		.addr = 0x84,
+	},
+	[AB8500_REGU_ARM_REGU1] = {
+		.name = "ArmRegu1",
+		.bank = 0x04,
+		.addr = 0x00,
+	},
+	[AB8500_REGU_ARM_REGU2] = {
+		.name = "ArmRegu2",
+		.bank = 0x04,
+		.addr = 0x01,
+	},
+	[AB8500_REGU_VAPE_REGU] = {
+		.name = "VapeRegu",
+		.bank = 0x04,
+		.addr = 0x02,
+	},
+	[AB8500_REGU_VSMPS1_REGU] = {
+		.name = "Vsmps1Regu",
+		.bank = 0x04,
+		.addr = 0x03,
+	},
+	[AB8500_REGU_VSMPS2_REGU] = {
+		.name = "Vsmps2Regu",
+		.bank = 0x04,
+		.addr = 0x04,
+	},
+	[AB8500_REGU_VSMPS3_REGU] = {
+		.name = "Vsmps3Regu",
+		.bank = 0x04,
+		.addr = 0x05,
+	},
+	[AB8500_REGU_VPLL_VANA_REGU] = {
+		.name = "VpllVanaRegu",
+		.bank = 0x04,
+		.addr = 0x06,
+	},
+	[AB8500_REGU_VREF_DDR] = {
+		.name = "VrefDDR",
+		.bank = 0x04,
+		.addr = 0x07,
+	},
+	[AB8500_REGU_EXT_SUPPLY_REGU] = {
+		.name = "ExtSupplyRegu",
+		.bank = 0x04,
+		.addr = 0x08,
+	},
+	[AB8500_REGU_VAUX12_REGU] = {
+		.name = "Vaux12Regu",
+		.bank = 0x04,
+		.addr = 0x09,
+	},
+	[AB8500_REGU_VRF1_VAUX3_REGU] = {
+		.name = "VRF1Vaux3Regu",
+		.bank = 0x04,
+		.addr = 0x0a,
+	},
+	[AB8500_REGU_VARM_SEL1] = {
+		.name = "VarmSel1",
+		.bank = 0x04,
+		.addr = 0x0b,
+	},
+	[AB8500_REGU_VARM_SEL2] = {
+		.name = "VarmSel2",
+		.bank = 0x04,
+		.addr = 0x0c,
+	},
+	[AB8500_REGU_VARM_SEL3] = {
+		.name = "VarmSel3",
+		.bank = 0x04,
+		.addr = 0x0d,
+	},
+	[AB8500_REGU_VAPE_SEL1] = {
+		.name = "VapeSel1",
+		.bank = 0x04,
+		.addr = 0x0e,
+	},
+	[AB8500_REGU_VAPE_SEL2] = {
+		.name = "VapeSel2",
+		.bank = 0x04,
+		.addr = 0x0f,
+	},
+	[AB8500_REGU_VAPE_SEL3] = {
+		.name = "VapeSel3",
+		.bank = 0x04,
+		.addr = 0x10,
+	},
+	[AB9540_REGU_VAUX4_REQ_CTRL] = {
+		.name = "Vaux4ReqCtrl",
+		.bank = 0x04,
+		.addr = 0x2d,
+		.unavailable = true, /* ab9540 register */
+	},
+	[AB9540_REGU_VAUX4_REGU] = {
+		.name = "Vaux4Regu",
+		.bank = 0x04,
+		.addr = 0x2e,
+		.unavailable = true, /* ab9540 register */
+	},
+	[AB9540_REGU_VAUX4_SEL] = {
+		.name = "Vaux4Sel",
+		.bank = 0x04,
+		.addr = 0x2f,
+		.unavailable = true, /* ab9540 register */
+	},
+	[AB8500_REGU_VBB_SEL1] = {
+		.name = "VBBSel1",
+		.bank = 0x04,
+		.addr = 0x11,
+	},
+	[AB8500_REGU_VBB_SEL2] = {
+		.name = "VBBSel2",
+		.bank = 0x04,
+		.addr = 0x12,
+	},
+	[AB8500_REGU_VSMPS1_SEL1] = {
+		.name = "Vsmps1Sel1",
+		.bank = 0x04,
+		.addr = 0x13,
+	},
+	[AB8500_REGU_VSMPS1_SEL2] = {
+		.name = "Vsmps1Sel2",
+		.bank = 0x04,
+		.addr = 0x14,
+	},
+	[AB8500_REGU_VSMPS1_SEL3] = {
+		.name = "Vsmps1Sel3",
+		.bank = 0x04,
+		.addr = 0x15,
+	},
+	[AB8500_REGU_VSMPS2_SEL1] = {
+		.name = "Vsmps2Sel1",
+		.bank = 0x04,
+		.addr = 0x17,
+	},
+	[AB8500_REGU_VSMPS2_SEL2] = {
+		.name = "Vsmps2Sel2",
+		.bank = 0x04,
+		.addr = 0x18,
+	},
+	[AB8500_REGU_VSMPS2_SEL3] = {
+		.name = "Vsmps2Sel3",
+		.bank = 0x04,
+		.addr = 0x19,
+	},
+	[AB8500_REGU_VSMPS3_SEL1] = {
+		.name = "Vsmps3Sel1",
+		.bank = 0x04,
+		.addr = 0x1b,
+	},
+	[AB8500_REGU_VSMPS3_SEL2] = {
+		.name = "Vsmps3Sel2",
+		.bank = 0x04,
+		.addr = 0x1c,
+	},
+	[AB8500_REGU_VSMPS3_SEL3] = {
+		.name = "Vsmps3Sel3",
+		.bank = 0x04,
+		.addr = 0x1d,
+	},
+	[AB8500_REGU_VAUX1_SEL] = {
+		.name = "Vaux1Sel",
+		.bank = 0x04,
+		.addr = 0x1f,
+	},
+	[AB8500_REGU_VAUX2_SEL] = {
+		.name = "Vaux2Sel",
+		.bank = 0x04,
+		.addr = 0x20,
+	},
+	[AB8500_REGU_VRF1_VAUX3_SEL] = {
+		.name = "VRF1Vaux3Sel",
+		.bank = 0x04,
+		.addr = 0x21,
+	},
+	[AB8500_REGU_CTRL_EXT_SUP] = {
+		.name = "ReguCtrlExtSup",
+		.bank = 0x04,
+		.addr = 0x22,
+	},
+	[AB8500_REGU_VMOD_REGU] = {
+		.name = "VmodRegu",
+		.bank = 0x04,
+		.addr = 0x40,
+	},
+	[AB8500_REGU_VMOD_SEL1] = {
+		.name = "VmodSel1",
+		.bank = 0x04,
+		.addr = 0x41,
+	},
+	[AB8500_REGU_VMOD_SEL2] = {
+		.name = "VmodSel2",
+		.bank = 0x04,
+		.addr = 0x42,
+	},
+	[AB8500_REGU_CTRL_DISCH] = {
+		.name = "ReguCtrlDisch",
+		.bank = 0x04,
+		.addr = 0x43,
+	},
+	[AB8500_REGU_CTRL_DISCH2] = {
+		.name = "ReguCtrlDisch2",
+		.bank = 0x04,
+		.addr = 0x44,
+	},
+	[AB9540_REGU_CTRL_DISCH3] = {
+		.name = "ReguCtrlDisch3",
+		.bank = 0x04,
+		.addr = 0x48,
+		.unavailable = true, /* ab9540 register */
+	},
+	/* Outside regulator banks */
+	[AB8500_OTHER_SYSCLK_CTRL] = {
+		.name = "SysClkCtrl",
+		.bank = 0x02,
+		.addr = 0x0c,
+	},
+	[AB8500_OTHER_VSIM_SYSCLK_CTRL] = {
+		.name = "VsimSysClkCtrl",
+		.bank = 0x02,
+		.addr = 0x33,
+	},
+	[AB8500_OTHER_SYSULPCLK_CTRL1] = {
+		.name = "SysUlpClkCtrl1",
+		.bank = 0x02,
+		.addr = 0x0b,
+	},
+};
+
+struct ab9540_register_update {
+	/* Identity of register to be updated */
+	u8 bank;
+	u8 addr;
+	/* New value for unavailable flag */
+	u8 unavailable;
+};
+
+static const struct ab9540_register_update ab9540_update[] = {
+	/* AB8500 register which is unavailable to AB9540 */
+	/* AB8500_REGU_VREF_DDR */
+	{
+		.bank = 0x04,
+		.addr = 0x07,
+		.unavailable = true,
+	},
+
+	/* Registers which were not available to AB8500 but are on the
+	 * AB9540. */
+	/* AB9540_REGU_VAUX4_REQ_VALID */
+	{
+		.bank = 0x03,
+		.addr = 0x11,
+	},
+	/* AB9540_REGU_VAUX4_REQ_CTRL */
+	{
+		.bank = 0x04,
+		.addr = 0x2d,
+	},
+	/* AB9540_REGU_VAUX4_REGU */
+	{
+		.bank = 0x04,
+		.addr = 0x2e,
+	},
+	/* AB9540_REGU_VAUX4_SEL */
+	{
+		.bank = 0x04,
+		.addr = 0x2f,
+	},
+	/* AB9540_REGU_CTRL_DISCH3 */
+	{
+		.bank = 0x04,
+		.addr = 0x48,
+	},
+};
+
+static void ab9540_registers_update(void)
+{
+	int i;
+	int j;
+
+	for (i = 0; i < NUM_AB8500_REGISTER; i++)
+		for (j = 0; j < ARRAY_SIZE(ab9540_update); j++)
+			if (ab8500_register[i].bank == ab9540_update[j].bank &&
+			    ab8500_register[i].addr == ab9540_update[j].addr) {
+				ab8500_register[i].unavailable =
+					ab9540_update[j].unavailable;
+				break;
+			}
+}
+
+static u8 ab8500_register_state[NUM_REGULATOR_STATE][NUM_AB8500_REGISTER];
+static bool ab8500_register_state_saved[NUM_REGULATOR_STATE];
+static bool ab8500_register_state_save = true;
+
+static int ab8500_regulator_record_state(int state)
+{
+	u8 val;
+	int i;
+	int ret;
+
+	/* check arguments */
+	if ((state > NUM_REGULATOR_STATE) || (state < 0)) {
+		dev_err(dev, "Wrong state specified\n");
+		return -EINVAL;
+	}
+
+	/* record */
+	if (!ab8500_register_state_save)
+		goto exit;
+
+	ab8500_register_state_saved[state] = true;
+
+	for (i = 1; i < NUM_AB8500_REGISTER; i++) {
+		if (ab8500_register[i].unavailable)
+			continue;
+
+		ret = abx500_get_register_interruptible(dev,
+			ab8500_register[i].bank,
+			ab8500_register[i].addr,
+			&val);
+		if (ret < 0) {
+			dev_err(dev, "abx500_get_reg fail %d, %d\n",
+				ret, __LINE__);
+			return -EINVAL;
+		}
+
+		ab8500_register_state[state][i] = val;
+	}
+exit:
+	return 0;
+}
+
+/*
+ * regulator register dump
+ */
+static int ab8500_regulator_dump_print(struct seq_file *s, void *p)
+{
+	struct device *dev = s->private;
+	int state, reg_id, i;
+	int err;
+
+	/* record current state */
+	ab8500_regulator_record_state(AB8500_REGULATOR_STATE_CURRENT);
+
+	/* print dump header */
+	err = seq_printf(s, "ab8500-regulator dump:\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow\n");
+
+	/* print states */
+	for (state = NUM_REGULATOR_STATE - 1; state >= 0; state--) {
+		if (ab8500_register_state_saved[state])
+			err = seq_printf(s, "%16s saved -------",
+				regulator_state_name[state]);
+		else
+			err = seq_printf(s, "%12s not saved -------",
+				regulator_state_name[state]);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+
+		for (i = 0; i < NUM_REGULATOR_STATE; i++) {
+			if (i < state)
+				err = seq_printf(s, "-----");
+			else if (i == state)
+				err = seq_printf(s, "----+");
+			else
+				err = seq_printf(s, "    |");
+			if (err < 0)
+				dev_err(dev, "seq_printf overflow: %i\n",
+					__LINE__);
+		}
+		err = seq_printf(s, "\n");
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	}
+
+	/* print labels */
+	err = seq_printf(s, "\n                       addr\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+
+	/* dump registers */
+	for (reg_id = 1; reg_id < NUM_AB8500_REGISTER; reg_id++) {
+		if (ab8500_register[reg_id].unavailable)
+			continue;
+
+		err = seq_printf(s, "%22s 0x%02x%02x:",
+			ab8500_register[reg_id].name,
+			ab8500_register[reg_id].bank,
+			ab8500_register[reg_id].addr);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				reg_id, __LINE__);
+
+		for (state = 0; state < NUM_REGULATOR_STATE; state++) {
+			err = seq_printf(s, " 0x%02x",
+				ab8500_register_state[state][reg_id]);
+			if (err < 0)
+				dev_err(dev, "seq_printf overflow: %i, %i\n",
+					reg_id, __LINE__);
+		}
+
+		err = seq_printf(s, "\n");
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				reg_id, __LINE__);
+	}
+
+	return 0;
+}
+
+static int ab8500_regulator_dump_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_regulator_dump_print, inode->i_private);
+}
+
+static const struct file_operations ab8500_regulator_dump_fops = {
+	.open = ab8500_regulator_dump_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+/*
+ * regulator_voltage
+ */
+struct regulator_volt {
+	u8 value;
+	int volt;
+};
+
+struct regulator_volt_range {
+	struct regulator_volt start;
+	struct regulator_volt step;
+	struct regulator_volt end;
+};
+
+/*
+ * ab8500_regulator
+ * @name
+ * @update_regid
+ * @update_mask
+ * @update_val[4] {off, on, hw, lp}
+ * @hw_mode_regid
+ * @hw_mode_mask
+ * @hw_mode_val[4] {hp/lp, hp/off, hp, hp}
+ * @hw_valid_regid[4] {sysclkreq1, hw1, hw2, sw}
+ * @hw_valid_mask[4] {sysclkreq1, hw1, hw2, sw}
+ * @vsel_sel_regid
+ * @vsel_sel_mask
+ * @vsel_val[333] {sel1, sel2, sel3, sel3}
+ * @vsel_regid
+ * @vsel_mask
+ * @vsel_range
+ * @vsel_range_len
+ * @unavailable {true/false depending on whether AB supports the regulator}
+ */
+struct ab8500_regulator {
+	const char *name;
+	int update_regid;
+	u8 update_mask;
+	u8 update_val[4];
+	int hw_mode_regid;
+	u8 hw_mode_mask;
+	u8 hw_mode_val[4];
+	int hw_valid_regid[4];
+	u8 hw_valid_mask[4];
+	int vsel_sel_regid;
+	u8 vsel_sel_mask;
+	u8 vsel_sel_val[4];
+	int vsel_regid[3];
+	u8 vsel_mask[3];
+	struct regulator_volt_range const *vsel_range[3];
+	int vsel_range_len[3];
+	u8 unavailable;
+};
+
+static const char *update_val_name[] = {
+	"off",
+	"on ",
+	"hw ",
+	"lp ",
+	" - " /* undefined value */
+};
+
+static const char *hw_mode_val_name[] = {
+	"hp/lp ",
+	"hp/off",
+	"hp    ",
+	"hp    ",
+	"-/-   ", /* undefined value */
+};
+
+/* voltage selection */
+/* AB8500 device - Varm_vsel in 12.5mV steps */
+#define AB8500_VARM_VSEL_MASK 0x3f
+static const struct regulator_volt_range ab8500_varm_vsel[] = {
+	{ {0x00,  700000}, {0x01,   12500}, {0x35, 1362500} },
+	{ {0x36, 1362500}, {0x01,       0}, {0x3f, 1362500} },
+};
+
+/* AB9540 device - Varm_vsel in 6.25mV steps */
+#define AB9540_VARM_VSEL_MASK 0x7f
+static const struct regulator_volt_range ab9540_varm_vsel[] = {
+	{ {0x00,  600000}, {0x01,    6250}, {0x7f, 1393750} },
+};
+
+static const struct regulator_volt_range vape_vmod_vsel[] = {
+	{ {0x00,  700000}, {0x01,   12500}, {0x35, 1362500} },
+	{ {0x36, 1362500}, {0x01,       0}, {0x3f, 1362500} },
+};
+
+/* AB8500 device - Vbbp_vsel and Vbbn_sel in 100mV steps */
+static const struct regulator_volt_range ab8500_vbbp_vsel[] = {
+	{ {0x00,       0}, {0x10,  100000}, {0x40,  400000} },
+	{ {0x50,  400000}, {0x10,       0}, {0x70,  400000} },
+	{ {0x80, -400000}, {0x10,       0}, {0xb0, -400000} },
+	{ {0xc0, -400000}, {0x10,  100000}, {0xf0, -100000} },
+};
+
+static const struct regulator_volt_range ab8500_vbbn_vsel[] = {
+	{ {0x00,       0}, {0x01, -100000}, {0x04, -400000} },
+	{ {0x05, -400000}, {0x01,       0}, {0x07, -400000} },
+	{ {0x08,       0}, {0x01,  100000}, {0x0c,  400000} },
+	{ {0x0d,  400000}, {0x01,       0}, {0x0f,  400000} },
+};
+
+/* AB9540 device - Vbbp_vsel and Vbbn_sel in 50mV steps */
+static const struct regulator_volt_range ab9540_vbbp_vsel[] = {
+	{ {0x00,       0}, {0x10,  -50000}, {0x70, -350000} },
+	{ {0x80,   50000}, {0x10,   50000}, {0xf0,  400000} },
+};
+
+static const struct regulator_volt_range ab9540_vbbn_vsel[] = {
+	{ {0x00,       0}, {0x01,  -50000}, {0x07, -350000} },
+	{ {0x08,   50000}, {0x01,   50000}, {0x0f,  400000} },
+};
+
+static const struct regulator_volt_range vsmps1_vsel[] = {
+	{ {0x00, 1100000}, {0x01,       0}, {0x1f, 1100000} },
+	{ {0x20, 1100000}, {0x01,   12500}, {0x30, 1300000} },
+	{ {0x31, 1300000}, {0x01,       0}, {0x3f, 1300000} },
+};
+
+static const struct regulator_volt_range vsmps2_vsel[] = {
+	{ {0x00, 1800000}, {0x01,       0}, {0x38, 1800000} },
+	{ {0x39, 1800000}, {0x01,   12500}, {0x7f, 1875000} },
+};
+
+static const struct regulator_volt_range vsmps3_vsel[] = {
+	{ {0x00,  700000}, {0x01,   12500}, {0x35, 1363500} },
+	{ {0x36, 1363500}, {0x01,       0}, {0x7f, 1363500} },
+};
+
+/* for Vaux1, Vaux2 and Vaux4 */
+static const struct regulator_volt_range vauxn_vsel[] = {
+	{ {0x00, 1100000}, {0x01,  100000}, {0x04, 1500000} },
+	{ {0x05, 1800000}, {0x01,   50000}, {0x07, 1900000} },
+	{ {0x08, 2500000}, {0x01,       0}, {0x08, 2500000} },
+	{ {0x09, 2650000}, {0x01,   50000}, {0x0c, 2800000} },
+	{ {0x0d, 2900000}, {0x01,  100000}, {0x0e, 3000000} },
+	{ {0x0f, 3300000}, {0x01,       0}, {0x0f, 3300000} },
+};
+
+static const struct regulator_volt_range vaux3_vsel[] = {
+	{ {0x00, 1200000}, {0x01,  300000}, {0x03, 2100000} },
+	{ {0x04, 2500000}, {0x01,  250000}, {0x05, 2750000} },
+	{ {0x06, 2790000}, {0x01,       0}, {0x06, 2790000} },
+	{ {0x07, 2910000}, {0x01,       0}, {0x07, 2910000} },
+};
+
+static const struct regulator_volt_range vrf1_vsel[] = {
+	{ {0x00, 1800000}, {0x10,  200000}, {0x10, 2000000} },
+	{ {0x20, 2150000}, {0x10,       0}, {0x20, 2150000} },
+	{ {0x30, 2500000}, {0x10,       0}, {0x30, 2500000} },
+};
+
+static const struct regulator_volt_range vintcore12_vsel[] = {
+	{ {0x00, 1200000}, {0x08,   25000}, {0x30, 1350000} },
+	{ {0x38, 1350000}, {0x01,       0}, {0x38, 1350000} },
+};
+
+/* regulators */
+static struct ab8500_regulator ab8500_regulator[AB8500_NUM_REGULATORS] = {
+	[AB8500_VARM] = {
+		.name		   = "Varm",
+		.update_regid      = AB8500_REGU_ARM_REGU1,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL1,
+		.hw_mode_mask      = 0x03,
+		.hw_mode_val       = {0x00, 0x01, 0x02, 0x03},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x02,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x02,
+		.vsel_sel_regid    = AB8500_REGU_ARM_REGU1,
+		.vsel_sel_mask     = 0x0c,
+		.vsel_sel_val      = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VARM_SEL1,
+		.vsel_mask[0]      = AB8500_VARM_VSEL_MASK,
+		.vsel_range[0]     = ab8500_varm_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(ab8500_varm_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VARM_SEL2,
+		.vsel_mask[1]      = AB8500_VARM_VSEL_MASK,
+		.vsel_range[1]     = ab8500_varm_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(ab8500_varm_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VARM_SEL3,
+		.vsel_mask[2]      = AB8500_VARM_VSEL_MASK,
+		.vsel_range[2]     = ab8500_varm_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(ab8500_varm_vsel),
+	},
+	[AB8500_VBBP] = {
+		.name		   = "Vbbp",
+		.update_regid      = AB8500_REGU_ARM_REGU2,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x00},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x04,
+		.vsel_sel_regid    = AB8500_REGU_ARM_REGU1,
+		.vsel_sel_mask     = 0x10,
+		.vsel_sel_val      = {0x00, 0x10, 0x00, 0x00},
+		.vsel_regid[0]     = AB8500_REGU_VBB_SEL1,
+		.vsel_mask[0]      = 0xf0,
+		.vsel_range[0]     = ab8500_vbbp_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(ab8500_vbbp_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VBB_SEL2,
+		.vsel_mask[1]      = 0xf0,
+		.vsel_range[1]     = ab8500_vbbp_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(ab8500_vbbp_vsel),
+	},
+	[AB8500_VBBN] = {
+		.name		   = "Vbbn",
+		.update_regid      = AB8500_REGU_ARM_REGU2,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x00},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x04,
+		.vsel_sel_regid    = AB8500_REGU_ARM_REGU1,
+		.vsel_sel_mask     = 0x20,
+		.vsel_sel_val      = {0x00, 0x20, 0x00, 0x00},
+		.vsel_regid[0]     = AB8500_REGU_VBB_SEL1,
+		.vsel_mask[0]      = 0x0f,
+		.vsel_range[0]     = ab8500_vbbn_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(ab8500_vbbn_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VBB_SEL2,
+		.vsel_mask[1]      = 0x0f,
+		.vsel_range[1]     = ab8500_vbbn_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(ab8500_vbbn_vsel),
+	},
+	[AB8500_VAPE] = {
+		.name		   = "Vape",
+		.update_regid      = AB8500_REGU_VAPE_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL1,
+		.hw_mode_mask      = 0x0c,
+		.hw_mode_val       = {0x00, 0x04, 0x08, 0x0c},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x01,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x01,
+		.vsel_sel_regid    = AB8500_REGU_VAPE_REGU,
+		.vsel_sel_mask     = 0x24,
+		.vsel_sel_val      = {0x00, 0x04, 0x20, 0x24},
+		.vsel_regid[0]     = AB8500_REGU_VAPE_SEL1,
+		.vsel_mask[0]      = 0x3f,
+		.vsel_range[0]     = vape_vmod_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vape_vmod_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VAPE_SEL2,
+		.vsel_mask[1]      = 0x3f,
+		.vsel_range[1]     = vape_vmod_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vape_vmod_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VAPE_SEL3,
+		.vsel_mask[2]      = 0x3f,
+		.vsel_range[2]     = vape_vmod_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(vape_vmod_vsel),
+	},
+	[AB8500_VSMPS1] = {
+		.name		   = "Vsmps1",
+		.update_regid      = AB8500_REGU_VSMPS1_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL1,
+		.hw_mode_mask      = 0x30,
+		.hw_mode_val       = {0x00, 0x10, 0x20, 0x30},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x01,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x01,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x01,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x04,
+		.vsel_sel_regid    = AB8500_REGU_VSMPS1_REGU,
+		.vsel_sel_mask     = 0x0c,
+		.vsel_sel_val      = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VSMPS1_SEL1,
+		.vsel_mask[0]      = 0x3f,
+		.vsel_range[0]     = vsmps1_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vsmps1_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VSMPS1_SEL2,
+		.vsel_mask[1]      = 0x3f,
+		.vsel_range[1]     = vsmps1_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vsmps1_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VSMPS1_SEL3,
+		.vsel_mask[2]      = 0x3f,
+		.vsel_range[2]     = vsmps1_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(vsmps1_vsel),
+	},
+	[AB8500_VSMPS2] = {
+		.name		   = "Vsmps2",
+		.update_regid      = AB8500_REGU_VSMPS2_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL1,
+		.hw_mode_mask      = 0xc0,
+		.hw_mode_val       = {0x00, 0x40, 0x80, 0xc0},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x02,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x02,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x02,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x08,
+		.vsel_sel_regid    = AB8500_REGU_VSMPS2_REGU,
+		.vsel_sel_mask     = 0x0c,
+		.vsel_sel_val      = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VSMPS2_SEL1,
+		.vsel_mask[0]      = 0x3f,
+		.vsel_range[0]     = vsmps2_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vsmps2_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VSMPS2_SEL2,
+		.vsel_mask[1]      = 0x3f,
+		.vsel_range[1]     = vsmps2_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vsmps2_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VSMPS2_SEL3,
+		.vsel_mask[2]      = 0x3f,
+		.vsel_range[2]     = vsmps2_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(vsmps2_vsel),
+	},
+	[AB8500_VSMPS3] = {
+		.name		   = "Vsmps3",
+		.update_regid      = AB8500_REGU_VSMPS3_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL2,
+		.hw_mode_mask      = 0x03,
+		.hw_mode_val       = {0x00, 0x01, 0x02, 0x03},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x04,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x04,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x04,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x10,
+		.vsel_sel_regid    = AB8500_REGU_VSMPS3_REGU,
+		.vsel_sel_mask     = 0x0c,
+		.vsel_sel_val      = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VSMPS3_SEL1,
+		.vsel_mask[0]      = 0x7f,
+		.vsel_range[0]     = vsmps3_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vsmps3_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VSMPS3_SEL2,
+		.vsel_mask[1]      = 0x7f,
+		.vsel_range[1]     = vsmps3_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vsmps3_vsel),
+		.vsel_regid[2]     = AB8500_REGU_VSMPS3_SEL3,
+		.vsel_mask[2]      = 0x7f,
+		.vsel_range[2]     = vsmps3_vsel,
+		.vsel_range_len[2] = ARRAY_SIZE(vsmps3_vsel),
+	},
+	[AB8500_VPLL] = {
+		.name		   = "Vpll",
+		.update_regid      = AB8500_REGU_VPLL_VANA_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL2,
+		.hw_mode_mask      = 0x0c,
+		.hw_mode_val       = {0x00, 0x04, 0x08, 0x0c},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x10,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x10,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x10,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x40,
+	},
+	[AB8500_VREFDDR] = {
+		.name		   = "VrefDDR",
+		.update_regid      = AB8500_REGU_VREF_DDR,
+		.update_mask       = 0x01,
+		.update_val        = {0x00, 0x01, 0x00, 0x00},
+	},
+	[AB8500_VMOD] = {
+		.name		   = "Vmod",
+		.update_regid      = AB8500_REGU_VMOD_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_VMOD_REGU,
+		.hw_mode_mask      = 0xc0,
+		.hw_mode_val       = {0x00, 0x40, 0x80, 0xc0},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x08,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID2,
+		.hw_valid_mask[1]  = 0x08,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID2,
+		.hw_valid_mask[2]  = 0x08,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x20,
+		.vsel_sel_regid    = AB8500_REGU_VMOD_REGU,
+		.vsel_sel_mask     = 0x04,
+		.vsel_sel_val      = {0x00, 0x04, 0x00, 0x00},
+		.vsel_regid[0]     = AB8500_REGU_VMOD_SEL1,
+		.vsel_mask[0]      = 0x3f,
+		.vsel_range[0]     = vape_vmod_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vape_vmod_vsel),
+		.vsel_regid[1]     = AB8500_REGU_VMOD_SEL2,
+		.vsel_mask[1]      = 0x3f,
+		.vsel_range[1]     = vape_vmod_vsel,
+		.vsel_range_len[1] = ARRAY_SIZE(vape_vmod_vsel),
+	},
+	[AB8500_VEXTSUPPLY1] = {
+		.name		   = "Vextsupply1",
+		.update_regid      = AB8500_REGU_EXT_SUPPLY_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL2,
+		.hw_mode_mask      = 0xc0,
+		.hw_mode_val       = {0x00, 0x40, 0x80, 0xc0},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x10,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID2,
+		.hw_valid_mask[1]  = 0x01,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID2,
+		.hw_valid_mask[2]  = 0x01,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x04,
+	},
+	[AB8500_VEXTSUPPLY2] = {
+		.name		   = "VextSupply2",
+		.update_regid      = AB8500_REGU_EXT_SUPPLY_REGU,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x0c},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL3,
+		.hw_mode_mask      = 0x03,
+		.hw_mode_val       = {0x00, 0x01, 0x02, 0x03},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x20,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID2,
+		.hw_valid_mask[1]  = 0x02,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID2,
+		.hw_valid_mask[2]  = 0x02,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x08,
+	},
+	[AB8500_VEXTSUPPLY3] = {
+		.name		   = "VextSupply3",
+		.update_regid      = AB8500_REGU_EXT_SUPPLY_REGU,
+		.update_mask       = 0x30,
+		.update_val        = {0x00, 0x10, 0x20, 0x30},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL3,
+		.hw_mode_mask      = 0x0c,
+		.hw_mode_val       = {0x00, 0x04, 0x08, 0x0c},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID2,
+		.hw_valid_mask[0]  = 0x40,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID2,
+		.hw_valid_mask[1]  = 0x04,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID2,
+		.hw_valid_mask[2]  = 0x04,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x10,
+	},
+	[AB8500_VRF1] = {
+		.name		   = "Vrf1",
+		.update_regid      = AB8500_REGU_VRF1_VAUX3_REGU,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x0c},
+		.vsel_regid[0]     = AB8500_REGU_VRF1_VAUX3_SEL,
+		.vsel_mask[0]      = 0x30,
+		.vsel_range[0]     = vrf1_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vrf1_vsel),
+	},
+	[AB8500_VANA] = {
+		.name		   = "Vana",
+		.update_regid      = AB8500_REGU_VPLL_VANA_REGU,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x0c},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL2,
+		.hw_mode_mask      = 0x30,
+		.hw_mode_val       = {0x00, 0x10, 0x20, 0x30},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x08,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x08,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x08,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x20,
+	},
+	[AB8500_VAUX1] = {
+		.name		   = "Vaux1",
+		.update_regid      = AB8500_REGU_VAUX12_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL3,
+		.hw_mode_mask      = 0x30,
+		.hw_mode_val       = {0x00, 0x10, 0x20, 0x30},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x20,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x20,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x20,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID1,
+		.hw_valid_mask[3]  = 0x80,
+		.vsel_regid[0]     = AB8500_REGU_VAUX1_SEL,
+		.vsel_mask[0]      = 0x0f,
+		.vsel_range[0]     = vauxn_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vauxn_vsel),
+	},
+	[AB8500_VAUX2] = {
+		.name		   = "Vaux2",
+		.update_regid      = AB8500_REGU_VAUX12_REGU,
+		.update_mask       = 0x0c,
+		.update_val        = {0x00, 0x04, 0x08, 0x0c},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL3,
+		.hw_mode_mask      = 0xc0,
+		.hw_mode_val       = {0x00, 0x40, 0x80, 0xc0},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x40,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x40,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x40,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x01,
+		.vsel_regid[0]     = AB8500_REGU_VAUX2_SEL,
+		.vsel_mask[0]      = 0x0f,
+		.vsel_range[0]     = vauxn_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vauxn_vsel),
+	},
+	[AB8500_VAUX3] = {
+		.name		   = "Vaux3",
+		.update_regid      = AB8500_REGU_VRF1_VAUX3_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB8500_REGU_REQUEST_CTRL4,
+		.hw_mode_mask      = 0x03,
+		.hw_mode_val       = {0x00, 0x01, 0x02, 0x03},
+		.hw_valid_regid[0] = AB8500_REGU_SYSCLK_REQ1_HP_VALID1,
+		.hw_valid_mask[0]  = 0x80,
+		.hw_valid_regid[1] = AB8500_REGU_HW_HP_REQ1_VALID1,
+		.hw_valid_mask[1]  = 0x80,
+		.hw_valid_regid[2] = AB8500_REGU_HW_HP_REQ2_VALID1,
+		.hw_valid_mask[2]  = 0x80,
+		.hw_valid_regid[3] = AB8500_REGU_SW_HP_REQ_VALID2,
+		.hw_valid_mask[3]  = 0x02,
+		.vsel_regid[0]     = AB8500_REGU_VRF1_VAUX3_SEL,
+		.vsel_mask[0]      = 0x07,
+		.vsel_range[0]     = vaux3_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vaux3_vsel),
+	},
+	[AB9540_VAUX4] = {
+		.name              = "Vaux4",
+		.update_regid      = AB9540_REGU_VAUX4_REGU,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x02, 0x03},
+		.hw_mode_regid     = AB9540_REGU_VAUX4_REQ_CTRL,
+		.hw_mode_mask      = 0x03,
+		.hw_mode_val       = {0x00, 0x01, 0x02, 0x03},
+		.hw_valid_regid[0] = AB9540_REGU_VAUX4_REQ_VALID,
+		.hw_valid_mask[0]  = 0x08,
+		.hw_valid_regid[1] = AB9540_REGU_VAUX4_REQ_VALID,
+		.hw_valid_mask[1]  = 0x04,
+		.hw_valid_regid[2] = AB9540_REGU_VAUX4_REQ_VALID,
+		.hw_valid_mask[2]  = 0x02,
+		.hw_valid_regid[3] = AB9540_REGU_VAUX4_REQ_VALID,
+		.hw_valid_mask[3]  = 0x01,
+		.vsel_regid[0]     = AB9540_REGU_VAUX4_SEL,
+		.vsel_mask[0]      = 0x0f,
+		.vsel_range[0]     = vauxn_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vauxn_vsel),
+		.unavailable       = true, /* AB9540 regulator */
+	},
+	[AB8500_VINTCORE] = {
+		.name		   = "VintCore12",
+		.update_regid      = AB8500_REGU_MISC1,
+		.update_mask       = 0x44,
+		.update_val        = {0x00, 0x04, 0x00, 0x44},
+		.vsel_regid[0]     = AB8500_REGU_MISC1,
+		.vsel_mask[0]      = 0x38,
+		.vsel_range[0]     = vintcore12_vsel,
+		.vsel_range_len[0] = ARRAY_SIZE(vintcore12_vsel),
+	},
+	[AB8500_VTVOUT] = {
+		.name		   = "VTVout",
+		.update_regid      = AB8500_REGU_MISC1,
+		.update_mask       = 0x82,
+		.update_val        = {0x00, 0x02, 0x00, 0x82},
+	},
+	[AB8500_VAUDIO] = {
+		.name		   = "Vaudio",
+		.update_regid      = AB8500_REGU_VAUDIO_SUPPLY,
+		.update_mask       = 0x02,
+		.update_val        = {0x00, 0x02, 0x00, 0x00},
+	},
+	[AB8500_VANAMIC1] = {
+		.name		   = "Vanamic1",
+		.update_regid      = AB8500_REGU_VAUDIO_SUPPLY,
+		.update_mask       = 0x08,
+		.update_val        = {0x00, 0x08, 0x00, 0x00},
+	},
+	[AB8500_VANAMIC2] = {
+		.name		   = "Vanamic2",
+		.update_regid      = AB8500_REGU_VAUDIO_SUPPLY,
+		.update_mask       = 0x10,
+		.update_val        = {0x00, 0x10, 0x00, 0x00},
+	},
+	[AB8500_VDMIC] = {
+		.name		   = "Vdmic",
+		.update_regid      = AB8500_REGU_VAUDIO_SUPPLY,
+		.update_mask       = 0x04,
+		.update_val        = {0x00, 0x04, 0x00, 0x00},
+	},
+	[AB8500_VUSB] = {
+		.name		   = "Vusb",
+		.update_regid      = AB8500_REGU_VUSB_CTRL,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x00, 0x03},
+	},
+	[AB8500_VOTG] = {
+		.name		   = "VOTG",
+		.update_regid      = AB8500_REGU_OTG_SUPPLY_CTRL,
+		.update_mask       = 0x03,
+		.update_val        = {0x00, 0x01, 0x00, 0x03},
+	},
+	[AB8500_VBUSBIS] = {
+		.name		   = "Vbusbis",
+		.update_regid      = AB8500_REGU_OTG_SUPPLY_CTRL,
+		.update_mask       = 0x08,
+		.update_val        = {0x00, 0x08, 0x00, 0x00},
+	},
+};
+
+static void ab9540_regulators_update(void)
+{
+	/* Update unavailable regulators */
+	ab8500_regulator[AB8500_VREFDDR].unavailable = true;
+	ab8500_regulator[AB9540_VAUX4].unavailable = false;
+
+	/* Update regulator characteristics for AB9540 */
+	ab8500_regulator[AB8500_VARM].vsel_mask[0] = AB9540_VARM_VSEL_MASK;
+	ab8500_regulator[AB8500_VARM].vsel_range[0] = ab9540_varm_vsel;
+	ab8500_regulator[AB8500_VARM].vsel_range_len[0] =
+		ARRAY_SIZE(ab9540_varm_vsel);
+	ab8500_regulator[AB8500_VARM].vsel_mask[1] = AB9540_VARM_VSEL_MASK;
+	ab8500_regulator[AB8500_VARM].vsel_range[1] = ab9540_varm_vsel;
+	ab8500_regulator[AB8500_VARM].vsel_range_len[1] =
+		ARRAY_SIZE(ab9540_varm_vsel);
+	ab8500_regulator[AB8500_VARM].vsel_mask[2] = AB9540_VARM_VSEL_MASK;
+	ab8500_regulator[AB8500_VARM].vsel_range[2] = ab9540_varm_vsel;
+	ab8500_regulator[AB8500_VARM].vsel_range_len[2] =
+		ARRAY_SIZE(ab9540_varm_vsel);
+
+	ab8500_regulator[AB8500_VBBP].vsel_range[0] = ab9540_vbbp_vsel;
+	ab8500_regulator[AB8500_VBBP].vsel_range_len[0] =
+		ARRAY_SIZE(ab9540_vbbp_vsel);
+	ab8500_regulator[AB8500_VBBP].vsel_range[1] = ab9540_vbbp_vsel;
+	ab8500_regulator[AB8500_VBBP].vsel_range_len[1] =
+		ARRAY_SIZE(ab9540_vbbp_vsel);
+
+	ab8500_regulator[AB8500_VBBN].vsel_range[0] = ab9540_vbbn_vsel;
+	ab8500_regulator[AB8500_VBBN].vsel_range_len[0] =
+		ARRAY_SIZE(ab9540_vbbn_vsel);
+	ab8500_regulator[AB8500_VBBN].vsel_range[1] = ab9540_vbbn_vsel;
+	ab8500_regulator[AB8500_VBBN].vsel_range_len[1] =
+		ARRAY_SIZE(ab9540_vbbn_vsel);
+}
+
+static int status_state = AB8500_REGULATOR_STATE_CURRENT;
+
+static int _get_voltage(struct regulator_volt_range const *volt_range,
+	u8 value, int *volt)
+{
+	u8 start = volt_range->start.value;
+	u8 end = volt_range->end.value;
+	u8 step = volt_range->step.value;
+
+	/* Check if witin range */
+	if (step == 0) {
+		if (value == start) {
+			*volt = volt_range->start.volt;
+			return 1;
+		}
+	} else {
+		if ((start <= value) && (value <= end)) {
+			if ((value - start) % step != 0)
+				return -EINVAL; /* invalid setting */
+			*volt = volt_range->start.volt
+			     + volt_range->step.volt
+			     *((value - start) / step);
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+static int get_voltage(struct regulator_volt_range const *volt_range,
+	int volt_range_len,
+	u8 value)
+{
+	int volt;
+	int i, ret;
+
+	for (i = 0; i < volt_range_len; i++) {
+		ret = _get_voltage(&volt_range[i], value, &volt);
+		if (ret < 0)
+			break; /* invalid setting */
+		if (ret == 1)
+			return volt; /* successful */
+	}
+
+	return -EINVAL;
+}
+
+static bool get_reg_and_mask(int regid, u8 mask, u8 *val)
+{
+	int ret;
+	u8 t;
+
+	if (!regid)
+		return false;
+
+	ret = abx500_get_register_interruptible(dev,
+						ab8500_register[regid].bank,
+						ab8500_register[regid].addr,
+						&t);
+	if (ret < 0)
+		return false;
+
+	(*val) = t & mask;
+
+	return true;
+}
+
+/* Convert regulator register value to index */
+static bool val2idx(u8 val, u8 *v, int len, int *idx)
+{
+	int i;
+
+	for (i = 0; i < len && v[i] != val; i++);
+
+	if (i == len)
+		return false;
+
+	(*idx) = i;
+	return true;
+}
+
+int ab8500_regulator_debug_read(enum ab8500_regulator_id id,
+				struct ab8500_debug_regulator_status *s)
+{
+	int i;
+	u8 val;
+	bool found;
+	int idx = 0;
+
+	if (id >= AB8500_NUM_REGULATORS)
+		return -EINVAL;
+
+	s->name = (char *)ab8500_regulator[id].name;
+
+	/* read mode */
+	(void) get_reg_and_mask(ab8500_regulator[id].update_regid,
+				ab8500_regulator[id].update_mask,
+				&val);
+
+	(void) val2idx(val, ab8500_regulator[id].update_val,
+		       4, &idx);
+
+	s->mode = (u8) idx;
+
+	/* read hw mode */
+	found = get_reg_and_mask(ab8500_regulator[id].hw_mode_regid,
+				 ab8500_regulator[id].hw_mode_mask,
+				 &val);
+
+	if (found)
+		found = val2idx(val, ab8500_regulator[id].hw_mode_val, 4, &idx);
+
+	if (found)
+		/* +1 since 0 = HWMODE_NONE */
+		s->hwmode = idx + 1;
+	else
+		s->hwmode = AB8500_HWMODE_NONE;
+
+	for (i = 0; i < 4 && found; i++) {
+
+		bool f = get_reg_and_mask(ab8500_regulator[id].hw_valid_regid[i],
+					  ab8500_regulator[id].hw_valid_mask[i],
+					  &val);
+		if (f)
+			s->hwmode_auto[i] = !!val;
+		else
+			s->hwmode_auto[i] = HWM_INVAL;
+	}
+
+	/* read voltage */
+	found = get_reg_and_mask(ab8500_regulator[id].vsel_sel_regid,
+				 ab8500_regulator[id].vsel_sel_mask,
+				 &val);
+	if (found)
+		found = val2idx(val, ab8500_regulator[id].vsel_sel_val,
+				3, &idx);
+
+	if (found && idx < 3)
+		s->volt_selected = idx + 1;
+	else
+		s->volt_selected = 0;
+
+	for (s->volt_len = 0; s->volt_len < 3; s->volt_len++) {
+		int volt;
+		int i = s->volt_len;
+
+		found = get_reg_and_mask(ab8500_regulator[id].vsel_regid[i],
+					 ab8500_regulator[id].vsel_mask[i],
+					 &val);
+		if (!found)
+			break;
+
+		volt = get_voltage(ab8500_regulator[id].vsel_range[i],
+				   ab8500_regulator[id].vsel_range_len[i],
+				   val);
+		s->volt[i] = volt;
+	}
+	return 0;
+}
+
+static int ab8500_regulator_status_print(struct seq_file *s, void *p)
+{
+	struct device *dev = s->private;
+	int id, regid;
+	int i;
+	u8 val;
+	int err;
+
+	/* record current state */
+	ab8500_regulator_record_state(AB8500_REGULATOR_STATE_CURRENT);
+
+	/* check if chosen state is recorded */
+	if (!ab8500_register_state_saved[status_state]) {
+		seq_printf(s, "ab8500-regulator status is not recorded.\n");
+		goto exit;
+	}
+
+	/* print dump header */
+	err = seq_printf(s, "ab8500-regulator status:\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow\n");
+
+	/* print state */
+	for (i = 0; i < NUM_REGULATOR_STATE; i++) {
+		if (i == status_state)
+			err = seq_printf(s, "-> %i. %12s\n",
+				i, regulator_state_name[i]);
+		else
+			err = seq_printf(s, "   %i. %12s\n",
+				i, regulator_state_name[i]);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow\n");
+	}
+
+	/* print labels */
+	err = seq_printf(s,
+	      "+-----------+----+--------------+-------------------------+\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "|       name|man |auto          |voltage                  |\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "+-----------+----+--------------+ +-----------------------+\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "|           |mode|mode  |0|1|2|3| |    1  |    2  |    3  |\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "+-----------+----+------+-+-+-+-+-+-------+-------+-------+\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+
+	/* dump registers */
+	for (id = 0; id < AB8500_NUM_REGULATORS; id++) {
+		if (ab8500_register[id].unavailable ||
+			ab8500_regulator[id].unavailable)
+			continue;
+
+		/* print name */
+		err = seq_printf(s, "|%11s|",
+			ab8500_regulator[id].name);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				id, __LINE__);
+
+		/* print manual mode */
+		regid = ab8500_regulator[id].update_regid;
+		val = ab8500_register_state[status_state][regid]
+		    & ab8500_regulator[id].update_mask;
+		for (i = 0; i < 4; i++) {
+			if (val == ab8500_regulator[id].update_val[i])
+				break;
+		}
+		err = seq_printf(s, "%4s|",
+			update_val_name[i]);
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				id, __LINE__);
+
+		/* print auto mode */
+		regid = ab8500_regulator[id].hw_mode_regid;
+		if (regid) {
+			val = ab8500_register_state[status_state][regid]
+			    & ab8500_regulator[id].hw_mode_mask;
+			for (i = 0; i < 4; i++) {
+				if (val == ab8500_regulator[id].hw_mode_val[i])
+					break;
+			}
+			err = seq_printf(s, "%6s|",
+				hw_mode_val_name[i]);
+		} else {
+			err = seq_printf(s, "      |");
+		}
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				id, __LINE__);
+
+		/* print valid bits */
+		for (i = 0; i < 4; i++) {
+			regid = ab8500_regulator[id].hw_valid_regid[i];
+			if (regid) {
+				val = ab8500_register_state[status_state][regid]
+				    & ab8500_regulator[id].hw_valid_mask[i];
+				if (val)
+					err = seq_printf(s, "1|");
+				else
+					err = seq_printf(s, "0|");
+			} else {
+				err = seq_printf(s, " |");
+			}
+			if (err < 0)
+				dev_err(dev, "seq_printf overflow: %i, %i\n",
+					regid, __LINE__);
+		}
+
+		/* print voltage selection */
+		regid = ab8500_regulator[id].vsel_sel_regid;
+		if (regid) {
+			val = ab8500_register_state[status_state][regid]
+			    & ab8500_regulator[id].vsel_sel_mask;
+			for (i = 0; i < 3; i++) {
+				if (val == ab8500_regulator[id].vsel_sel_val[i])
+					break;
+			}
+			if (i < 3)
+				seq_printf(s, "%i|", i + 1);
+			else
+				seq_printf(s, "-|");
+		} else {
+			seq_printf(s, " |");
+		}
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				regid, __LINE__);
+
+		for (i = 0; i < 3; i++) {
+			int volt;
+
+			regid = ab8500_regulator[id].vsel_regid[i];
+			if (regid) {
+				val = ab8500_register_state[status_state][regid]
+				    & ab8500_regulator[id].vsel_mask[i];
+				volt = get_voltage(
+					ab8500_regulator[id].vsel_range[i],
+					ab8500_regulator[id].vsel_range_len[i],
+					val);
+				seq_printf(s, "%7i|", volt);
+			} else {
+				seq_printf(s, "       |");
+			}
+			if (err < 0)
+				dev_err(dev, "seq_printf overflow: %i, %i\n",
+					regid, __LINE__);
+		}
+
+		err = seq_printf(s, "\n");
+		if (err < 0)
+			dev_err(dev, "seq_printf overflow: %i, %i\n",
+				regid, __LINE__);
+
+	}
+	err = seq_printf(s,
+	      "+-----------+----+------+-+-+-+-+-+-------+-------+-------+\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+	err = seq_printf(s,
+	      "Note! In HW mode, voltage selection is controlled by HW.\n");
+	if (err < 0)
+		dev_err(dev, "seq_printf overflow: %i\n", __LINE__);
+
+
+exit:
+	return 0;
+}
+
+static int ab8500_regulator_status_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	char buf[32];
+	int buf_size;
+	unsigned long user_val;
+	int err;
+
+	/* copy user data */
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	/* convert */
+	err = strict_strtoul(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+
+	/* set suspend force setting */
+	if (user_val > NUM_REGULATOR_STATE) {
+		dev_err(dev, "debugfs error input > number of states\n");
+		return -EINVAL;
+	}
+
+	status_state = user_val;
+
+	return buf_size;
+}
+
+
+static int ab8500_regulator_status_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ab8500_regulator_status_print,
+		inode->i_private);
+}
+
+static const struct file_operations ab8500_regulator_status_fops = {
+	.open = ab8500_regulator_status_open,
+	.write = ab8500_regulator_status_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+#ifdef CONFIG_PM
+
+struct ab8500_force_reg {
+	char *name;
+	u8 bank;
+	u8 addr;
+	u8 mask;
+	u8 val;
+	bool restore;
+	u8 restore_val;
+	u8 unavailable;
+};
+
+static struct ab8500_force_reg ab8500_force_reg[] = {
+	{
+		/*
+		 * SysClkCtrl
+		 * OTP: 0x00, HSI: 0x06, suspend: 0x00/0x07 (value/mask)
+		 * [  2] USBClkEna = disable SysClk path to USB block
+		 * [  1] TVoutClkEna = disable 27Mhz clock to TVout block
+		 * [  0] TVoutPllEna = disable TVout pll
+		 *                     (generate 27Mhz from SysClk)
+		 */
+		.name = "SysClkCtrl",
+		.bank = 0x02,
+		.addr = 0x0c,
+		.mask = 0x07,
+		.val  = 0x00,
+	},
+	{
+		/*
+		 * VsimSysClkCtrl
+		 * OTP: 0x01, HSI: 0x21, suspend: 0x01/0xff (value/mask)
+		 * [  7] VsimSysClkReq8Valid = no connection
+		 * [  6] VsimSysClkReq7Valid = no connection
+		 * [  5] VsimSysClkReq6Valid = no connection
+		 * [  4] VsimSysClkReq5Valid = no connection
+		 * [  3] VsimSysClkReq4Valid = no connection
+		 * [  2] VsimSysClkReq3Valid = no connection
+		 * [  1] VsimSysClkReq2Valid = no connection
+		 * [  0] VsimSysClkReq1Valid = Vsim set by SysClkReq1
+		 */
+		.name = "VsimSysClkCtrl",
+		.bank = 0x02,
+		.addr = 0x33,
+		.mask = 0xff,
+		.val  = 0x01,
+	},
+	{
+		/*
+		 * SysUlpClkCtrl1
+		 * OTP: 0x00, HSI: 0x00, suspend: 0x00/0x0f (value/mask)
+		 * [  3] 4500SysClkReq = inactive
+		 * [  2] UlpClkReq = inactive
+		 * [1:0] SysUlpClkIntSel[1:0] = no internal clock switching.
+		 *                              Internal clock is SysClk.
+		 */
+		.name = "SysUlpClkCtrl1",
+		.bank = 0x02,
+		.addr = 0x0b,
+		.mask = 0x0f,
+		.val  = 0x00,
+	},
+	{
+		/*
+		 * TVoutCtrl
+		 * OTP: N/A, HSI: N/A, suspend: 0x00/0x03 (value/mask)
+		 * [  2]   PlugTvOn = plug/unplug detection disabled
+		 * [1:0]   TvoutDacCtrl[1:0] = "0" forced on DAC input (test)
+		 */
+		.name = "TVoutCtrl",
+		.bank = 0x06,
+		.addr = 0x80,
+		.mask = 0x03,
+		.val  = 0x00,
+	},
+};
+
+static void ab9540_force_reg_update(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ab8500_force_reg); i++) {
+		if (ab8500_force_reg[i].bank == 0x02 &&
+		    ab8500_force_reg[i].addr == 0x0C) {
+			/*
+			 * SysClkCtrl
+			 * OTP: 0x00, HSI: 0x06, suspend: 0x00/0x07 (value/mask)
+			 * [  2] USBClkEna = disable SysClk path to USB block
+			 */
+			ab8500_force_reg[i].mask = 0x04;
+			ab8500_force_reg[i].val  = 0x00;
+		} else if (ab8500_force_reg[i].bank == 0x06 &&
+			   ab8500_force_reg[i].addr == 0x80) {
+			/* TVoutCtrl not supported by AB9540 */
+			ab8500_force_reg[i].unavailable = true;
+		}
+	}
+}
+
+void ab8500_regulator_debug_force(void)
+{
+	int ret, i;
+
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(AB8500_REGULATOR_STATE_SUSPEND);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Failed to record suspend state.\n");
+
+	/* check if registers should be forced */
+	if (!setting_suspend_force)
+		goto exit;
+
+	/*
+	 * Optimize href v2_v50_pwr board for ApSleep/ApDeepSleep
+	 * power consumption measurements
+	 */
+
+	for (i = 0; i < ARRAY_SIZE(ab8500_force_reg); i++) {
+		if (ab8500_force_reg[i].unavailable)
+			continue;
+
+		dev_vdbg(&pdev->dev, "Save and set %s: "
+			"0x%02x, 0x%02x, 0x%02x, 0x%02x.\n",
+			ab8500_force_reg[i].name,
+			ab8500_force_reg[i].bank,
+			ab8500_force_reg[i].addr,
+			ab8500_force_reg[i].mask,
+			ab8500_force_reg[i].val);
+
+		/* assume that register should be restored */
+		ab8500_force_reg[i].restore = true;
+
+		/* get register value before forcing it */
+		ret = abx500_get_register_interruptible(&pdev->dev,
+			ab8500_force_reg[i].bank,
+			ab8500_force_reg[i].addr,
+			&ab8500_force_reg[i].restore_val);
+		if (ret < 0) {
+			dev_err(dev, "Failed to read %s.\n",
+				ab8500_force_reg[i].name);
+			ab8500_force_reg[i].restore = false;
+			break;
+		}
+
+		/* force register value */
+		ret = abx500_mask_and_set_register_interruptible(&pdev->dev,
+			ab8500_force_reg[i].bank,
+			ab8500_force_reg[i].addr,
+			ab8500_force_reg[i].mask,
+			ab8500_force_reg[i].val);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "Failed to write %s.\n",
+				ab8500_force_reg[i].name);
+			ab8500_force_reg[i].restore = false;
+		}
+	}
+
+exit:
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(
+		AB8500_REGULATOR_STATE_SUSPEND_CORE);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Failed to record suspend state.\n");
+
+	return;
+}
+
+void ab8500_regulator_debug_restore(void)
+{
+	int ret, i;
+
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(AB8500_REGULATOR_STATE_RESUME_CORE);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Failed to record resume state.\n");
+	for (i = ARRAY_SIZE(ab8500_force_reg) - 1; i >= 0; i--) {
+		if (ab8500_force_reg[i].unavailable)
+			continue;
+
+		/* restore register value */
+		if (ab8500_force_reg[i].restore) {
+			ret = abx500_mask_and_set_register_interruptible(
+				 &pdev->dev,
+				 ab8500_force_reg[i].bank,
+				 ab8500_force_reg[i].addr,
+				 ab8500_force_reg[i].mask,
+				 ab8500_force_reg[i].restore_val);
+			if (ret < 0)
+				dev_err(&pdev->dev, "Failed to restore %s.\n",
+					ab8500_force_reg[i].name);
+			dev_vdbg(&pdev->dev, "Restore %s: "
+				"0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+				ab8500_force_reg[i].name,
+				ab8500_force_reg[i].bank,
+				ab8500_force_reg[i].addr,
+				ab8500_force_reg[i].mask,
+				ab8500_force_reg[i].restore_val);
+		}
+	}
+
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(AB8500_REGULATOR_STATE_RESUME);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Failed to record resume state.\n");
+
+	return;
+}
+
+#endif
+
+static int ab8500_regulator_suspend_force_show(struct seq_file *s, void *p)
+{
+	/* print suspend standby status */
+	if (setting_suspend_force)
+		return seq_printf(s, "suspend force enabled\n");
+	else
+		return seq_printf(s, "no suspend force\n");
+}
+
+static int ab8500_regulator_suspend_force_write(struct file *file,
+	const char __user *user_buf,
+	size_t count, loff_t *ppos)
+{
+	char buf[32];
+	int buf_size;
+	unsigned long user_val;
+	int err;
+
+	/* copy user data */
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+	buf[buf_size] = 0;
+
+	/* convert */
+	err = strict_strtoul(buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+
+	/* set suspend force setting */
+	if (user_val > 1) {
+		dev_err(dev, "debugfs error input > 1\n");
+		return -EINVAL;
+	}
+
+	if (user_val)
+		setting_suspend_force = true;
+	else
+		setting_suspend_force = false;
+
+	return buf_size;
+}
+
+static int ab8500_regulator_suspend_force_open(struct inode *inode,
+	struct file *file)
+{
+	return single_open(file, ab8500_regulator_suspend_force_show,
+		inode->i_private);
+}
+
+static const struct file_operations ab8500_regulator_suspend_force_fops = {
+	.open = ab8500_regulator_suspend_force_open,
+	.write = ab8500_regulator_suspend_force_write,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static struct dentry *ab8500_regulator_dir;
+static struct dentry *ab8500_regulator_dump_file;
+static struct dentry *ab8500_regulator_status_file;
+static struct dentry *ab8500_regulator_suspend_force_file;
+
+int __devinit ab8500_regulator_debug_init(struct platform_device *plf)
+{
+	void __iomem *boot_info_backupram;
+	int ret;
+	struct ab8500 *ab8500;
+
+	/* setup dev pointers */
+	dev = &plf->dev;
+	pdev = plf;
+
+	/* save state of registers */
+	ret = ab8500_regulator_record_state(AB8500_REGULATOR_STATE_INIT);
+	if (ret < 0)
+		dev_err(&plf->dev, "Failed to record init state.\n");
+
+        ab8500 = dev_get_drvdata(plf->dev.parent);
+	/* Update data structures for AB9540 */
+	if (is_ab9540(ab8500)) {
+		ab9540_registers_update();
+		ab9540_regulators_update();
+		ab9540_force_reg_update();
+	}
+	/* make suspend-force default if board profile is v5x-power */
+	boot_info_backupram = ioremap(BOOT_INFO_BACKUPRAM1, 0x4);
+
+	if (boot_info_backupram) {
+		u8 board_profile;
+		board_profile = readb(
+			boot_info_backupram + BOARD_PROFILE_BACKUPRAM1);
+		dev_dbg(dev, "Board profile is 0x%02x\n", board_profile);
+
+		if (board_profile >= OPTION_BOARD_VERSION_V5X)
+			setting_suspend_force = true;
+
+		iounmap(boot_info_backupram);
+	} else {
+		dev_err(dev, "Failed to read backupram.\n");
+	}
+
+	/* create directory */
+	ab8500_regulator_dir = debugfs_create_dir("ab8500-regulator", NULL);
+	if (!ab8500_regulator_dir)
+		goto exit_no_debugfs;
+
+	/* create "dump" file */
+	ab8500_regulator_dump_file = debugfs_create_file("dump",
+		S_IRUGO, ab8500_regulator_dir, &plf->dev,
+		&ab8500_regulator_dump_fops);
+	if (!ab8500_regulator_dump_file)
+		goto exit_destroy_dir;
+
+	/* create "status" file */
+	ab8500_regulator_status_file = debugfs_create_file("status",
+		S_IRUGO, ab8500_regulator_dir, &plf->dev,
+		&ab8500_regulator_status_fops);
+	if (!ab8500_regulator_status_file)
+		goto exit_destroy_dump_file;
+
+	/*
+	 * create "suspend-force-v5x" file. As indicated by the name, this is
+	 * only applicable for v2_v5x hardware versions.
+	 */
+	ab8500_regulator_suspend_force_file = debugfs_create_file(
+		"suspend-force-v5x",
+		S_IRUGO, ab8500_regulator_dir, &plf->dev,
+		&ab8500_regulator_suspend_force_fops);
+	if (!ab8500_regulator_suspend_force_file)
+		goto exit_destroy_status_file;
+
+	return 0;
+
+exit_destroy_status_file:
+	debugfs_remove(ab8500_regulator_status_file);
+exit_destroy_dump_file:
+	debugfs_remove(ab8500_regulator_dump_file);
+exit_destroy_dir:
+	debugfs_remove(ab8500_regulator_dir);
+exit_no_debugfs:
+	dev_err(&plf->dev, "failed to create debugfs entries.\n");
+	return -ENOMEM;
+}
+
+int __devexit ab8500_regulator_debug_exit(struct platform_device *plf)
+{
+	debugfs_remove(ab8500_regulator_suspend_force_file);
+	debugfs_remove(ab8500_regulator_status_file);
+	debugfs_remove(ab8500_regulator_dump_file);
+	debugfs_remove(ab8500_regulator_dir);
+
+	return 0;
+}
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Bengt Jonsson <bengt.g.jonsson@stericsson.com");
+MODULE_DESCRIPTION("AB8500 Regulator Debug");
+MODULE_ALIAS("platform:ab8500-regulator-debug");
diff --git a/drivers/regulator/ab8500-debug.h b/drivers/regulator/ab8500-debug.h
new file mode 100644
index 00000000000..2b59e556a3f
--- /dev/null
+++ b/drivers/regulator/ab8500-debug.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010-2011
+ *
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson.
+ *
+ * License Terms: GNU General Public License v2
+ */
+
+#ifndef __AB8500_DEBUG_H__
+#define __AB8500_DEBUG_H__
+
+/*
+ * regulator status print
+ */
+enum ab8500_regulator_id {
+	AB8500_VARM,
+	AB8500_VBBP,
+	AB8500_VBBN,
+	AB8500_VAPE,
+	AB8500_VSMPS1,
+	AB8500_VSMPS2,
+	AB8500_VSMPS3,
+	AB8500_VPLL,
+	AB8500_VREFDDR,
+	AB8500_VMOD,
+	AB8500_VEXTSUPPLY1,
+	AB8500_VEXTSUPPLY2,
+	AB8500_VEXTSUPPLY3,
+	AB8500_VRF1,
+	AB8500_VANA,
+	AB8500_VAUX1,
+	AB8500_VAUX2,
+	AB8500_VAUX3,
+	AB9540_VAUX4,		/* Note: AB9540 only */
+	AB8500_VINTCORE,
+	AB8500_VTVOUT,
+	AB8500_VAUDIO,
+	AB8500_VANAMIC1,
+	AB8500_VANAMIC2,
+	AB8500_VDMIC,
+	AB8500_VUSB,
+	AB8500_VOTG,
+	AB8500_VBUSBIS,
+	AB8500_NUM_REGULATORS,
+};
+
+enum ab8500_regulator_mode {
+	AB8500_MODE_OFF = 0,
+	AB8500_MODE_ON,
+	AB8500_MODE_HW,
+	AB8500_MODE_LP
+};
+
+enum ab8500_regulator_hwmode {
+	AB8500_HWMODE_NONE = 0,
+	AB8500_HWMODE_HPLP,
+	AB8500_HWMODE_HPOFF,
+	AB8500_HWMODE_HP,
+	AB8500_HWMODE_HP2,
+};
+
+enum hwmode_auto {
+	HWM_OFF = 0,
+	HWM_ON = 1,
+	HWM_INVAL = 2,
+};
+
+struct ab8500_debug_regulator_status {
+	char *name;
+	enum ab8500_regulator_mode mode;
+	enum ab8500_regulator_hwmode hwmode;
+	enum hwmode_auto hwmode_auto[4];
+	int volt_selected;
+	int volt_len;
+	int volt[4];
+};
+
+int ab8500_regulator_debug_read(enum ab8500_regulator_id id,
+				struct ab8500_debug_regulator_status *s);
+#endif /* __AB8500_DEBUG_H__ */
diff --git a/drivers/regulator/ab8500-ext.c b/drivers/regulator/ab8500-ext.c
new file mode 100644
index 00000000000..8a5064c07fb
--- /dev/null
+++ b/drivers/regulator/ab8500-ext.c
@@ -0,0 +1,451 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Authors: Bengt Jonsson <bengt.g.jonsson@stericsson.com>
+ *
+ * This file is based on drivers/regulator/ab8500.c
+ *
+ * AB8500 external regulators
+ *
+ * ab8500-ext supports the following regulators:
+ * - VextSupply3
+ */
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab8500.h>
+#include <linux/regulator/ab8500.h>
+
+/**
+ * struct ab8500_ext_regulator_info - ab8500 regulator information
+ * @dev: device pointer
+ * @desc: regulator description
+ * @rdev: regulator device
+ * @cfg: regulator configuration (extension of regulator FW configuration)
+ * @is_enabled: status of regulator (on/off)
+ * @fixed_uV: typical voltage (for fixed voltage supplies)
+ * @update_bank: bank to control on/off
+ * @update_reg: register to control on/off
+ * @update_mask: mask to enable/disable and set mode of regulator
+ * @update_val: bits holding the regulator current mode
+ * @update_val_hp: bits to set EN pin active (LPn pin deactive)
+ *                 normally this means high power mode
+ * @update_val_lp: bits to set EN pin active and LPn pin active
+ *                 normally this means low power mode
+ * @update_val_hw: bits to set regulator pins in HW control
+ *                 SysClkReq pins and logic will choose mode
+ */
+struct ab8500_ext_regulator_info {
+	struct device *dev;
+	struct regulator_desc desc;
+	struct regulator_dev *rdev;
+	struct ab8500_ext_regulator_cfg *cfg;
+	bool is_enabled;
+	int fixed_uV;
+	u8 update_bank;
+	u8 update_reg;
+	u8 update_mask;
+	u8 update_val;
+	u8 update_val_hp;
+	u8 update_val_lp;
+	u8 update_val_hw;
+};
+
+static int enable(struct ab8500_ext_regulator_info *info, u8 *regval)
+{
+	int ret;
+
+	*regval = info->update_val;
+
+	/*
+	 * To satisfy both HW high power request and SW request, the regulator
+	 * must be on in high power.
+	 */
+	if (info->cfg && info->cfg->hwreq)
+		*regval = info->update_val_hp;
+
+	ret = abx500_mask_and_set_register_interruptible(info->dev,
+		info->update_bank, info->update_reg,
+		info->update_mask, *regval);
+	if (ret < 0)
+		dev_err(rdev_get_dev(info->rdev),
+			"couldn't set enable bits for regulator\n");
+
+	info->is_enabled = true;
+
+	return ret;
+}
+
+static int ab8500_ext_regulator_enable(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+	u8 regval;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = enable(info, &regval);
+
+	dev_dbg(rdev_get_dev(rdev), "%s-enable (bank, reg, mask, value):"
+		" 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+		info->desc.name, info->update_bank, info->update_reg,
+		info->update_mask, regval);
+
+	return ret;
+}
+
+static int ab8500_ext_regulator_set_suspend_enable(struct regulator_dev *rdev)
+{
+	dev_dbg(rdev_get_dev(rdev), "suspend: ");
+
+	return ab8500_ext_regulator_enable(rdev);
+}
+
+static int disable(struct ab8500_ext_regulator_info *info, u8 *regval)
+{
+	int ret;
+
+	*regval = 0x0;
+
+	/*
+	 * Set the regulator in HW request mode if configured
+	 */
+	if (info->cfg && info->cfg->hwreq)
+		*regval = info->update_val_hw;
+
+	ret = abx500_mask_and_set_register_interruptible(info->dev,
+		info->update_bank, info->update_reg,
+		info->update_mask, *regval);
+	if (ret < 0)
+		dev_err(rdev_get_dev(info->rdev),
+			"couldn't set disable bits for regulator\n");
+
+	info->is_enabled = false;
+
+	return ret;
+}
+
+static int ab8500_ext_regulator_disable(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+	u8 regval;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = disable(info, &regval);
+
+	dev_dbg(rdev_get_dev(rdev), "%s-disable (bank, reg, mask, value):"
+		" 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+		info->desc.name, info->update_bank, info->update_reg,
+		info->update_mask, regval);
+
+	return ret;
+}
+
+static int ab8500_ext_regulator_set_suspend_disable(struct regulator_dev *rdev)
+{
+	dev_dbg(rdev_get_dev(rdev), "suspend: ");
+
+	return ab8500_ext_regulator_disable(rdev);
+}
+
+static int ab8500_ext_regulator_is_enabled(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+	u8 regval;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = abx500_get_register_interruptible(info->dev,
+		info->update_bank, info->update_reg, &regval);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't read 0x%x register\n", info->update_reg);
+		return ret;
+	}
+
+	dev_dbg(rdev_get_dev(rdev), "%s-is_enabled (bank, reg, mask, value):"
+		" 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
+		info->desc.name, info->update_bank, info->update_reg,
+		info->update_mask, regval);
+
+	if (((regval & info->update_mask) == info->update_val_lp) ||
+	    ((regval & info->update_mask) == info->update_val_hp))
+		info->is_enabled = true;
+	else
+		info->is_enabled = false;
+
+	return info->is_enabled;
+}
+
+static int ab8500_ext_regulator_set_mode(struct regulator_dev *rdev,
+					 unsigned int mode)
+{
+	int ret = 0;
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		info->update_val = info->update_val_hp;
+		break;
+	case REGULATOR_MODE_IDLE:
+		info->update_val = info->update_val_lp;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	if (info->is_enabled) {
+		u8 regval;
+
+		ret = enable(info, &regval);
+		if (ret < 0)
+			dev_err(rdev_get_dev(rdev),
+				"Could not set regulator mode.\n");
+
+		dev_dbg(rdev_get_dev(rdev),
+			"%s-set_mode (bank, reg, mask, value): "
+			"0x%x, 0x%x, 0x%x, 0x%x\n",
+			info->desc.name, info->update_bank, info->update_reg,
+			info->update_mask, regval);
+	}
+
+	return ret;
+}
+
+static unsigned int ab8500_ext_regulator_get_mode(struct regulator_dev *rdev)
+{
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	if (info->update_val == info->update_val_hp)
+		ret = REGULATOR_MODE_NORMAL;
+	else if (info->update_val == info->update_val_lp)
+		ret = REGULATOR_MODE_IDLE;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+static int ab8500_ext_fixed_get_voltage(struct regulator_dev *rdev)
+{
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	return info->fixed_uV;
+}
+
+static int ab8500_ext_list_voltage(struct regulator_dev *rdev,
+				   unsigned selector)
+{
+	struct ab8500_ext_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	/* return the uV for the fixed regulators */
+	if (info->fixed_uV)
+		return info->fixed_uV;
+
+	return -EINVAL;
+}
+
+static struct regulator_ops ab8500_ext_regulator_ops = {
+	.enable			= ab8500_ext_regulator_enable,
+	.set_suspend_enable	= ab8500_ext_regulator_set_suspend_enable,
+	.disable		= ab8500_ext_regulator_disable,
+	.set_suspend_disable	= ab8500_ext_regulator_set_suspend_disable,
+	.is_enabled		= ab8500_ext_regulator_is_enabled,
+	.set_mode		= ab8500_ext_regulator_set_mode,
+	.get_mode		= ab8500_ext_regulator_get_mode,
+	.get_voltage		= ab8500_ext_fixed_get_voltage,
+	.list_voltage		= ab8500_ext_list_voltage,
+};
+
+static struct ab8500_ext_regulator_info
+		ab8500_ext_regulator_info[AB8500_NUM_EXT_REGULATORS] = {
+	[AB8500_EXT_SUPPLY1] = {
+		.desc = {
+			.name		= "VEXTSUPPLY1",
+			.ops		= &ab8500_ext_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_EXT_SUPPLY1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1800000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x08,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_hp		= 0x01,
+		.update_val_lp		= 0x03,
+		.update_val_hw		= 0x02,
+	},
+	[AB8500_EXT_SUPPLY2] = {
+		.desc = {
+			.name		= "VEXTSUPPLY2",
+			.ops		= &ab8500_ext_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_EXT_SUPPLY2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1360000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x08,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_hp		= 0x04,
+		.update_val_lp		= 0x0c,
+		.update_val_hw		= 0x08,
+	},
+	[AB8500_EXT_SUPPLY3] = {
+		.desc = {
+			.name		= "VEXTSUPPLY3",
+			.ops		= &ab8500_ext_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_EXT_SUPPLY3,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 3400000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x08,
+		.update_mask		= 0x30,
+		.update_val		= 0x10,
+		.update_val_hp		= 0x10,
+		.update_val_lp		= 0x30,
+		.update_val_hw		= 0x20,
+	},
+};
+
+__devinit int ab8500_ext_regulator_init(struct platform_device *pdev)
+{
+	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
+	struct ab8500_platform_data *ppdata;
+	struct ab8500_regulator_platform_data *pdata;
+	int i, err;
+
+	if (!ab8500) {
+		dev_err(&pdev->dev, "null mfd parent\n");
+		return -EINVAL;
+	}
+	ppdata = dev_get_platdata(ab8500->dev);
+	if (!ppdata) {
+		dev_err(&pdev->dev, "null parent pdata\n");
+		return -EINVAL;
+	}
+
+	pdata = ppdata->regulator;
+	if (!pdata) {
+		dev_err(&pdev->dev, "null pdata\n");
+		return -EINVAL;
+	}
+
+	/* make sure the platform data has the correct size */
+	if (pdata->num_ext_regulator != ARRAY_SIZE(ab8500_ext_regulator_info)) {
+		dev_err(&pdev->dev, "Configuration error: size mismatch.\n");
+		return -EINVAL;
+	}
+
+	/* check for AB8500 2.x */
+	if (is_ab8500_2p0_or_earlier(ab8500)) {
+		struct ab8500_ext_regulator_info *info;
+
+		/* VextSupply3LPn is inverted on AB8500 2.x */
+		info = &ab8500_ext_regulator_info[AB8500_EXT_SUPPLY3];
+		info->update_val = 0x30;
+		info->update_val_hp = 0x30;
+		info->update_val_lp = 0x10;
+	}
+
+	/* register all regulators */
+	for (i = 0; i < ARRAY_SIZE(ab8500_ext_regulator_info); i++) {
+		struct ab8500_ext_regulator_info *info = NULL;
+
+		/* assign per-regulator data */
+		info = &ab8500_ext_regulator_info[i];
+		info->dev = &pdev->dev;
+		info->cfg = (struct ab8500_ext_regulator_cfg *)
+			pdata->ext_regulator[i].driver_data;
+
+		/* register regulator with framework */
+		info->rdev = regulator_register(&info->desc, &pdev->dev,
+				&pdata->ext_regulator[i], info, NULL);
+		if (IS_ERR(info->rdev)) {
+			err = PTR_ERR(info->rdev);
+			dev_err(&pdev->dev, "failed to register regulator %s\n",
+					info->desc.name);
+			/* when we fail, un-register all earlier regulators */
+			while (--i >= 0) {
+				info = &ab8500_ext_regulator_info[i];
+				regulator_unregister(info->rdev);
+			}
+			return err;
+		}
+
+		dev_dbg(rdev_get_dev(info->rdev),
+			"%s-probed\n", info->desc.name);
+	}
+
+	return 0;
+}
+
+__devexit int ab8500_ext_regulator_exit(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ab8500_ext_regulator_info); i++) {
+		struct ab8500_ext_regulator_info *info = NULL;
+		info = &ab8500_ext_regulator_info[i];
+
+		dev_vdbg(rdev_get_dev(info->rdev),
+			"%s-remove\n", info->desc.name);
+
+		regulator_unregister(info->rdev);
+	}
+
+	return 0;
+}
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Bengt Jonsson <bengt.g.jonsson@stericsson.com>");
+MODULE_DESCRIPTION("AB8500 external regulator driver");
+MODULE_ALIAS("platform:ab8500-ext-regulator");
diff --git a/drivers/regulator/ab8500.c b/drivers/regulator/ab8500.c
index c7ee4c15d6f..71328249659 100644
--- a/drivers/regulator/ab8500.c
+++ b/drivers/regulator/ab8500.c
@@ -21,43 +21,55 @@
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
 #include <linux/regulator/ab8500.h>
+#include <linux/mfd/abx500/ab8500-gpio.h> /* for sysclkreq pins */
+#include <mach/id.h>
 
 /**
  * struct ab8500_regulator_info - ab8500 regulator information
  * @dev: device pointer
  * @desc: regulator description
  * @regulator_dev: regulator device
+ * @is_enabled: status of regulator (on/off)
  * @max_uV: maximum voltage (for variable voltage supplies)
  * @min_uV: minimum voltage (for variable voltage supplies)
  * @fixed_uV: typical voltage (for fixed voltage supplies)
+ * @load_lp_uA: maximum load in idle (low power) mode
  * @update_bank: bank to control on/off
  * @update_reg: register to control on/off
- * @update_mask: mask to enable/disable regulator
- * @update_val_enable: bits to enable the regulator in normal (high power) mode
+ * @update_mask: mask to enable/disable and set mode of regulator
+ * @update_val: bits holding the regulator current mode
+ * @update_val_idle: bits to enable the regulator in idle (low power) mode
+ * @update_val_normal: bits to enable the regulator in normal (high power) mode
  * @voltage_bank: bank to control regulator voltage
  * @voltage_reg: register to control regulator voltage
  * @voltage_mask: mask to control regulator voltage
  * @voltages: supported voltage table
  * @voltages_len: number of supported voltages for the regulator
  * @delay: startup/set voltage delay in us
+ * @gpio_pin: ab8500 gpio pin offset number (for sysclkreq regulator only)
  */
 struct ab8500_regulator_info {
 	struct device		*dev;
 	struct regulator_desc	desc;
 	struct regulator_dev	*regulator;
+	bool is_enabled;
 	int max_uV;
 	int min_uV;
 	int fixed_uV;
+	int load_lp_uA;
 	u8 update_bank;
 	u8 update_reg;
 	u8 update_mask;
-	u8 update_val_enable;
+	u8 update_val;
+	u8 update_val_idle;
+	u8 update_val_normal;
 	u8 voltage_bank;
 	u8 voltage_reg;
 	u8 voltage_mask;
 	int const *voltages;
 	int voltages_len;
 	unsigned int delay;
+	enum ab8500_pin gpio_pin;
 };
 
 /* voltage tables for the vauxn/vintcore supplies */
@@ -113,15 +125,17 @@ static int ab8500_regulator_enable(struct regulator_dev *rdev)
 
 	ret = abx500_mask_and_set_register_interruptible(info->dev,
 		info->update_bank, info->update_reg,
-		info->update_mask, info->update_val_enable);
+		info->update_mask, info->update_val);
 	if (ret < 0)
 		dev_err(rdev_get_dev(rdev),
 			"couldn't set enable bits for regulator\n");
 
+	info->is_enabled = true;
+
 	dev_vdbg(rdev_get_dev(rdev),
 		"%s-enable (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
 		info->desc.name, info->update_bank, info->update_reg,
-		info->update_mask, info->update_val_enable);
+		info->update_mask, info->update_val);
 
 	return ret;
 }
@@ -143,6 +157,8 @@ static int ab8500_regulator_disable(struct regulator_dev *rdev)
 		dev_err(rdev_get_dev(rdev),
 			"couldn't set disable bits for regulator\n");
 
+	info->is_enabled = false;
+
 	dev_vdbg(rdev_get_dev(rdev),
 		"%s-disable (bank, reg, mask, value): 0x%x, 0x%x, 0x%x, 0x%x\n",
 		info->desc.name, info->update_bank, info->update_reg,
@@ -151,6 +167,88 @@ static int ab8500_regulator_disable(struct regulator_dev *rdev)
 	return ret;
 }
 
+static unsigned int ab8500_regulator_get_optimum_mode(
+		struct regulator_dev *rdev, int input_uV,
+		int output_uV, int load_uA)
+{
+	unsigned int mode;
+
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	if (load_uA <= info->load_lp_uA)
+		mode = REGULATOR_MODE_IDLE;
+	else
+		mode = REGULATOR_MODE_NORMAL;
+
+	return mode;
+}
+
+static int ab8500_regulator_set_mode(struct regulator_dev *rdev,
+				     unsigned int mode)
+{
+	int ret = 0;
+
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		info->update_val = info->update_val_normal;
+		break;
+	case REGULATOR_MODE_IDLE:
+		info->update_val = info->update_val_idle;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (info->is_enabled) {
+		ret = abx500_mask_and_set_register_interruptible(info->dev,
+			info->update_bank, info->update_reg,
+			info->update_mask, info->update_val);
+		if (ret < 0)
+			dev_err(rdev_get_dev(rdev),
+				"couldn't set regulator mode\n");
+
+		dev_vdbg(rdev_get_dev(rdev),
+			"%s-set_mode (bank, reg, mask, value): "
+			"0x%x, 0x%x, 0x%x, 0x%x\n",
+			info->desc.name, info->update_bank, info->update_reg,
+			info->update_mask, info->update_val);
+	}
+
+	return ret;
+}
+
+static unsigned int ab8500_regulator_get_mode(struct regulator_dev *rdev)
+{
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	if (info->update_val == info->update_val_normal)
+		ret = REGULATOR_MODE_NORMAL;
+	else if (info->update_val == info->update_val_idle)
+		ret = REGULATOR_MODE_IDLE;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
 static int ab8500_regulator_is_enabled(struct regulator_dev *rdev)
 {
 	int ret;
@@ -177,9 +275,11 @@ static int ab8500_regulator_is_enabled(struct regulator_dev *rdev)
 		info->update_mask, regval);
 
 	if (regval & info->update_mask)
-		return true;
+		info->is_enabled = true;
 	else
-		return false;
+		info->is_enabled = false;
+
+	return info->is_enabled;
 }
 
 static int ab8500_list_voltage(struct regulator_dev *rdev, unsigned selector)
@@ -273,8 +373,13 @@ static int ab8500_regulator_set_voltage(struct regulator_dev *rdev,
 
 	*selector = ret;
 
+	/* vintcore register has a different layout */
+	if (info->desc.id == AB8500_LDO_INTCORE)
+		regval = ((u8)ret) << 3;
+	else
+		regval = (u8)ret;
+
 	/* set the registers for the request */
-	regval = (u8)ret;
 	ret = abx500_mask_and_set_register_interruptible(info->dev,
 			info->voltage_bank, info->voltage_reg,
 			info->voltage_mask, regval);
@@ -314,9 +419,12 @@ static int ab8500_regulator_set_voltage_time_sel(struct regulator_dev *rdev,
 	return info->delay;
 }
 
-static struct regulator_ops ab8500_regulator_ops = {
+static struct regulator_ops ab8500_regulator_volt_mode_ops = {
 	.enable		= ab8500_regulator_enable,
 	.disable	= ab8500_regulator_disable,
+	.get_optimum_mode	= ab8500_regulator_get_optimum_mode,
+	.set_mode	= ab8500_regulator_set_mode,
+	.get_mode	= ab8500_regulator_get_mode,
 	.is_enabled	= ab8500_regulator_is_enabled,
 	.get_voltage_sel = ab8500_regulator_get_voltage_sel,
 	.set_voltage	= ab8500_regulator_set_voltage,
@@ -337,16 +445,116 @@ static int ab8500_fixed_get_voltage(struct regulator_dev *rdev)
 	return info->fixed_uV;
 }
 
-static struct regulator_ops ab8500_regulator_fixed_ops = {
+static struct regulator_ops ab8500_regulator_mode_ops = {
 	.enable		= ab8500_regulator_enable,
 	.disable	= ab8500_regulator_disable,
 	.is_enabled	= ab8500_regulator_is_enabled,
+	.get_optimum_mode	= ab8500_regulator_get_optimum_mode,
+	.set_mode		= ab8500_regulator_set_mode,
+	.get_mode		= ab8500_regulator_get_mode,
 	.get_voltage	= ab8500_fixed_get_voltage,
 	.list_voltage	= ab8500_list_voltage,
 	.enable_time	= ab8500_regulator_enable_time,
 	.set_voltage_time_sel = ab8500_regulator_set_voltage_time_sel,
 };
 
+static struct regulator_ops ab8500_regulator_ops = {
+	.enable			= ab8500_regulator_enable,
+	.disable		= ab8500_regulator_disable,
+	.is_enabled		= ab8500_regulator_is_enabled,
+	.get_voltage		= ab8500_fixed_get_voltage,
+	.list_voltage		= ab8500_list_voltage,
+};
+
+static int ab8500_sysclkreq_enable(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = ab8500_gpio_config_select(info->dev, info->gpio_pin, false);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't set sysclkreq pin selection\n");
+		return ret;
+	}
+
+	info->is_enabled = true;
+
+	dev_vdbg(rdev_get_dev(rdev),
+		"%s-enable (gpio_pin, gpio_select): %i, false\n",
+		info->desc.name, info->gpio_pin);
+
+	return ret;
+}
+
+static int ab8500_sysclkreq_disable(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = ab8500_gpio_config_select(info->dev, info->gpio_pin, true);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't set gpio pin selection\n");
+		return ret;
+	}
+
+	info->is_enabled = false;
+
+	dev_vdbg(rdev_get_dev(rdev),
+		"%s-disable (gpio_pin, gpio_select): %i, true\n",
+		info->desc.name, info->gpio_pin);
+
+	return ret;
+}
+
+static int ab8500_sysclkreq_is_enabled(struct regulator_dev *rdev)
+{
+	int ret;
+	struct ab8500_regulator_info *info = rdev_get_drvdata(rdev);
+	bool gpio_select;
+
+	if (info == NULL) {
+		dev_err(rdev_get_dev(rdev), "regulator info null pointer\n");
+		return -EINVAL;
+	}
+
+	ret = ab8500_gpio_config_get_select(info->dev, info->gpio_pin,
+		&gpio_select);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"couldn't read gpio pin selection\n");
+		return ret;
+	}
+
+	info->is_enabled = !gpio_select;
+
+	dev_vdbg(rdev_get_dev(rdev),
+		"%s-is_enabled (gpio_pin, is_enabled): %i, %i\n",
+		info->desc.name, info->gpio_pin, !gpio_select);
+
+	return info->is_enabled;
+}
+
+static struct regulator_ops ab8500_sysclkreq_ops = {
+	.enable		= ab8500_sysclkreq_enable,
+	.disable	= ab8500_sysclkreq_disable,
+	.is_enabled	= ab8500_sysclkreq_is_enabled,
+	.get_voltage	= ab8500_fixed_get_voltage,
+	.list_voltage	= ab8500_list_voltage,
+};
+
+/* AB8500 regulator information */
 static struct ab8500_regulator_info
 		ab8500_regulator_info[AB8500_NUM_REGULATORS] = {
 	/*
@@ -358,7 +566,7 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_AUX1] = {
 		.desc = {
 			.name		= "LDO-AUX1",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUX1,
 			.owner		= THIS_MODULE,
@@ -366,10 +574,13 @@ static struct ab8500_regulator_info
 		},
 		.min_uV			= 1100000,
 		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x09,
 		.update_mask		= 0x03,
-		.update_val_enable	= 0x01,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x1f,
 		.voltage_mask		= 0x0f,
@@ -379,7 +590,7 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_AUX2] = {
 		.desc = {
 			.name		= "LDO-AUX2",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUX2,
 			.owner		= THIS_MODULE,
@@ -387,10 +598,13 @@ static struct ab8500_regulator_info
 		},
 		.min_uV			= 1100000,
 		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x09,
 		.update_mask		= 0x0c,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x20,
 		.voltage_mask		= 0x0f,
@@ -400,7 +614,7 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_AUX3] = {
 		.desc = {
 			.name		= "LDO-AUX3",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUX3,
 			.owner		= THIS_MODULE,
@@ -408,10 +622,13 @@ static struct ab8500_regulator_info
 		},
 		.min_uV			= 1100000,
 		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x0a,
 		.update_mask		= 0x03,
-		.update_val_enable	= 0x01,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
 		.voltage_bank		= 0x04,
 		.voltage_reg		= 0x21,
 		.voltage_mask		= 0x07,
@@ -421,7 +638,7 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_INTCORE] = {
 		.desc = {
 			.name		= "LDO-INTCORE",
-			.ops		= &ab8500_regulator_ops,
+			.ops		= &ab8500_regulator_volt_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_INTCORE,
 			.owner		= THIS_MODULE,
@@ -429,10 +646,13 @@ static struct ab8500_regulator_info
 		},
 		.min_uV			= 1100000,
 		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x80,
 		.update_mask		= 0x44,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x44,
+		.update_val_idle	= 0x44,
+		.update_val_normal	= 0x04,
 		.voltage_bank		= 0x03,
 		.voltage_reg		= 0x80,
 		.voltage_mask		= 0x38,
@@ -448,7 +668,275 @@ static struct ab8500_regulator_info
 	[AB8500_LDO_TVOUT] = {
 		.desc = {
 			.name		= "LDO-TVOUT",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_TVOUT,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.delay			= 500,
+		.fixed_uV		= 2000000,
+		.load_lp_uA		= 1000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x80,
+		.update_mask		= 0x82,
+		.update_val		= 0x02,
+		.update_val_idle	= 0x82,
+		.update_val_normal	= 0x02,
+	},
+	[AB8500_LDO_AUDIO] = {
+		.desc = {
+			.name		= "LDO-AUDIO",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_AUDIO,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 2000000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x02,
+		.update_val		= 0x02,
+	},
+	[AB8500_LDO_ANAMIC1] = {
+		.desc = {
+			.name		= "LDO-ANAMIC1",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_ANAMIC1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 2050000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x08,
+		.update_val		= 0x08,
+	},
+	[AB8500_LDO_ANAMIC2] = {
+		.desc = {
+			.name		= "LDO-ANAMIC2",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_ANAMIC2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 2050000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x10,
+		.update_val		= 0x10,
+	},
+	[AB8500_LDO_DMIC] = {
+		.desc = {
+			.name		= "LDO-DMIC",
+			.ops		= &ab8500_regulator_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_DMIC,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1800000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x83,
+		.update_mask		= 0x04,
+		.update_val		= 0x04,
+	},
+
+	/*
+	 * Regulators with fixed voltage and normal/idle modes
+	 */
+	[AB8500_LDO_ANA] = {
+		.desc = {
+			.name		= "LDO-ANA",
+			.ops		= &ab8500_regulator_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_ANA,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1200000,
+		.load_lp_uA		= 1000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x06,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
+	},
+
+	/*
+	 * SysClkReq regulators
+	 */
+	[AB8500_SYSCLKREQ_2] = {
+		.desc = {
+			.name		= "SYSCLKREQ-2",
+			.ops		= &ab8500_sysclkreq_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_SYSCLKREQ_2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1, /* bogus value */
+		.gpio_pin		= AB8500_PIN_GPIO1,
+	},
+	[AB8500_SYSCLKREQ_4] = {
+		.desc = {
+			.name		= "SYSCLKREQ-4",
+			.ops		= &ab8500_sysclkreq_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_SYSCLKREQ_4,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1, /* bogus value */
+		.gpio_pin		= AB8500_PIN_GPIO3,
+	},
+};
+
+/* AB9540 regulator information */
+static struct ab8500_regulator_info
+		ab9540_regulator_info[AB9540_NUM_REGULATORS] = {
+	/*
+	 * Variable Voltage Regulators
+	 *   name, min mV, max mV,
+	 *   update bank, reg, mask, enable val
+	 *   volt bank, reg, mask, table, table length
+	 */
+	[AB9540_LDO_AUX1] = {
+		.desc = {
+			.name		= "LDO-AUX1",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_AUX1,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+		},
+		.min_uV			= 1100000,
+		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x09,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x1f,
+		.voltage_mask		= 0x0f,
+		.voltages		= ldo_vauxn_voltages,
+		.voltages_len		= ARRAY_SIZE(ldo_vauxn_voltages),
+	},
+	[AB9540_LDO_AUX2] = {
+		.desc = {
+			.name		= "LDO-AUX2",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_AUX2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+		},
+		.min_uV			= 1100000,
+		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x09,
+		.update_mask		= 0x0c,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x20,
+		.voltage_mask		= 0x0f,
+		.voltages		= ldo_vauxn_voltages,
+		.voltages_len		= ARRAY_SIZE(ldo_vauxn_voltages),
+	},
+	[AB9540_LDO_AUX3] = {
+		.desc = {
+			.name		= "LDO-AUX3",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_AUX3,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vaux3_voltages),
+		},
+		.min_uV			= 1100000,
+		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x04,
+		.update_reg		= 0x0a,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x21,
+		.voltage_mask		= 0x07,
+		.voltages		= ldo_vaux3_voltages,
+		.voltages_len		= ARRAY_SIZE(ldo_vaux3_voltages),
+	},
+	[AB9540_LDO_AUX4] = {
+		.desc = {
+			.name		= "LDO-AUX4",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB9540_LDO_AUX4,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vauxn_voltages),
+		},
+		.min_uV			= 1100000,
+		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
+		/* values for Vaux4Regu register */
+		.update_bank		= 0x04,
+		.update_reg		= 0x2e,
+		.update_mask		= 0x03,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
+		/* values for Vaux4SEL register */
+		.voltage_bank		= 0x04,
+		.voltage_reg		= 0x2f,
+		.voltage_mask		= 0x0f,
+		.voltages		= ldo_vauxn_voltages,
+		.voltages_len		= ARRAY_SIZE(ldo_vauxn_voltages),
+	},
+	[AB9540_LDO_INTCORE] = {
+		.desc = {
+			.name		= "LDO-INTCORE",
+			.ops		= &ab8500_regulator_volt_mode_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_LDO_INTCORE,
+			.owner		= THIS_MODULE,
+			.n_voltages	= ARRAY_SIZE(ldo_vintcore_voltages),
+		},
+		.min_uV			= 1100000,
+		.max_uV			= 3300000,
+		.load_lp_uA		= 5000,
+		.update_bank		= 0x03,
+		.update_reg		= 0x80,
+		.update_mask		= 0x44,
+		.update_val		= 0x44,
+		.update_val_idle	= 0x44,
+		.update_val_normal	= 0x04,
+		.voltage_bank		= 0x03,
+		.voltage_reg		= 0x80,
+		.voltage_mask		= 0x38,
+		.voltages		= ldo_vintcore_voltages,
+		.voltages_len		= ARRAY_SIZE(ldo_vintcore_voltages),
+	},
+
+	/*
+	 * Fixed Voltage Regulators
+	 *   name, fixed mV,
+	 *   update bank, reg, mask, enable val
+	 */
+	[AB9540_LDO_TVOUT] = {
+		.desc = {
+			.name		= "LDO-TVOUT",
+			.ops		= &ab8500_regulator_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_TVOUT,
 			.owner		= THIS_MODULE,
@@ -456,17 +944,20 @@ static struct ab8500_regulator_info
 		},
 		.delay			= 10000,
 		.fixed_uV		= 2000000,
+		.load_lp_uA		= 1000,
 		.update_bank		= 0x03,
 		.update_reg		= 0x80,
 		.update_mask		= 0x82,
-		.update_val_enable	= 0x02,
+		.update_val		= 0x02,
+		.update_val_idle	= 0x82,
+		.update_val_normal	= 0x02,
 	},
-	[AB8500_LDO_USB] = {
+	[AB9540_LDO_USB] = {
 		.desc = {
 			.name           = "LDO-USB",
-			.ops            = &ab8500_regulator_fixed_ops,
+			.ops            = &ab8500_regulator_ops,
 			.type           = REGULATOR_VOLTAGE,
-			.id             = AB8500_LDO_USB,
+			.id             = AB9540_LDO_USB,
 			.owner          = THIS_MODULE,
 			.n_voltages     = 1,
 		},
@@ -474,12 +965,14 @@ static struct ab8500_regulator_info
 		.update_bank            = 0x03,
 		.update_reg             = 0x82,
 		.update_mask            = 0x03,
-		.update_val_enable      = 0x01,
+		.update_val		= 0x01,
+		.update_val_idle	= 0x03,
+		.update_val_normal	= 0x01,
 	},
-	[AB8500_LDO_AUDIO] = {
+	[AB9540_LDO_AUDIO] = {
 		.desc = {
 			.name		= "LDO-AUDIO",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_AUDIO,
 			.owner		= THIS_MODULE,
@@ -489,12 +982,12 @@ static struct ab8500_regulator_info
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x02,
-		.update_val_enable	= 0x02,
+		.update_val		= 0x02,
 	},
-	[AB8500_LDO_ANAMIC1] = {
+	[AB9540_LDO_ANAMIC1] = {
 		.desc = {
 			.name		= "LDO-ANAMIC1",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_ANAMIC1,
 			.owner		= THIS_MODULE,
@@ -504,12 +997,12 @@ static struct ab8500_regulator_info
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x08,
-		.update_val_enable	= 0x08,
+		.update_val		= 0x08,
 	},
-	[AB8500_LDO_ANAMIC2] = {
+	[AB9540_LDO_ANAMIC2] = {
 		.desc = {
 			.name		= "LDO-ANAMIC2",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_ANAMIC2,
 			.owner		= THIS_MODULE,
@@ -519,12 +1012,12 @@ static struct ab8500_regulator_info
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x10,
-		.update_val_enable	= 0x10,
+		.update_val		= 0x10,
 	},
-	[AB8500_LDO_DMIC] = {
+	[AB9540_LDO_DMIC] = {
 		.desc = {
 			.name		= "LDO-DMIC",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_DMIC,
 			.owner		= THIS_MODULE,
@@ -534,25 +1027,58 @@ static struct ab8500_regulator_info
 		.update_bank		= 0x03,
 		.update_reg		= 0x83,
 		.update_mask		= 0x04,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x04,
 	},
-	[AB8500_LDO_ANA] = {
+
+	/*
+	 * Regulators with fixed voltage and normal/idle modes
+	 */
+	[AB9540_LDO_ANA] = {
 		.desc = {
 			.name		= "LDO-ANA",
-			.ops		= &ab8500_regulator_fixed_ops,
+			.ops		= &ab8500_regulator_mode_ops,
 			.type		= REGULATOR_VOLTAGE,
 			.id		= AB8500_LDO_ANA,
 			.owner		= THIS_MODULE,
 			.n_voltages	= 1,
 		},
 		.fixed_uV		= 1200000,
+		.load_lp_uA		= 1000,
 		.update_bank		= 0x04,
 		.update_reg		= 0x06,
 		.update_mask		= 0x0c,
-		.update_val_enable	= 0x04,
+		.update_val		= 0x04,
+		.update_val_idle	= 0x0c,
+		.update_val_normal	= 0x04,
 	},
 
-
+	/*
+	 * SysClkReq regulators
+	 */
+	[AB9540_SYSCLKREQ_2] = {
+		.desc = {
+			.name		= "SYSCLKREQ-2",
+			.ops		= &ab8500_sysclkreq_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_SYSCLKREQ_2,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1, /* bogus value */
+		.gpio_pin		= AB8500_PIN_GPIO1,
+	},
+	[AB9540_SYSCLKREQ_4] = {
+		.desc = {
+			.name		= "SYSCLKREQ-4",
+			.ops		= &ab8500_sysclkreq_ops,
+			.type		= REGULATOR_VOLTAGE,
+			.id		= AB8500_SYSCLKREQ_4,
+			.owner		= THIS_MODULE,
+			.n_voltages	= 1,
+		},
+		.fixed_uV		= 1, /* bogus value */
+		.gpio_pin		= AB8500_PIN_GPIO3,
+	},
 };
 
 struct ab8500_reg_init {
@@ -568,13 +1094,13 @@ struct ab8500_reg_init {
 		.mask = _mask,			\
 	}
 
+/* AB8500 register init */
 static struct ab8500_reg_init ab8500_reg_init[] = {
 	/*
 	 * 0x30, VanaRequestCtrl
-	 * 0x0C, VpllRequestCtrl
 	 * 0xc0, VextSupply1RequestCtrl
 	 */
-	REG_INIT(AB8500_REGUREQUESTCTRL2,	0x03, 0x04, 0xfc),
+	REG_INIT(AB8500_REGUREQUESTCTRL2,	0x03, 0x04, 0xf0),
 	/*
 	 * 0x03, VextSupply2RequestCtrl
 	 * 0x0c, VextSupply3RequestCtrl
@@ -641,13 +1167,21 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	REG_INIT(AB8500_REGUSWHPREQVALID2,	0x03, 0x0e, 0x1f),
 	/*
 	 * 0x02, SysClkReq2Valid1
-	 * ...
+	 * 0x04, SysClkReq3Valid1
+	 * 0x08, SysClkReq4Valid1
+	 * 0x10, SysClkReq5Valid1
+	 * 0x20, SysClkReq6Valid1
+	 * 0x40, SysClkReq7Valid1
 	 * 0x80, SysClkReq8Valid1
 	 */
 	REG_INIT(AB8500_REGUSYSCLKREQVALID1,	0x03, 0x0f, 0xfe),
 	/*
 	 * 0x02, SysClkReq2Valid2
-	 * ...
+	 * 0x04, SysClkReq3Valid2
+	 * 0x08, SysClkReq4Valid2
+	 * 0x10, SysClkReq5Valid2
+	 * 0x20, SysClkReq6Valid2
+	 * 0x40, SysClkReq7Valid2
 	 * 0x80, SysClkReq8Valid2
 	 */
 	REG_INIT(AB8500_REGUSYSCLKREQVALID2,	0x03, 0x10, 0xfe),
@@ -672,8 +1206,8 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	 */
 	REG_INIT(AB8500_REGUCTRL1VAMIC,		0x03, 0x84, 0x03),
 	/*
+	 * 0x03, VpllRegu (NOTE! PRCMU register bits)
 	 * 0x0c, VanaRegu
-	 * 0x03, VpllRegu
 	 */
 	REG_INIT(AB8500_VPLLVANAREGU,		0x04, 0x06, 0x0f),
 	/*
@@ -699,10 +1233,6 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	 */
 	REG_INIT(AB8500_VRF1VAUX3REGU,		0x04, 0x0a, 0x03),
 	/*
-	 * 0x3f, Vsmps1Sel1
-	 */
-	REG_INIT(AB8500_VSMPS1SEL1,		0x04, 0x13, 0x3f),
-	/*
 	 * 0x0f, Vaux1Sel
 	 */
 	REG_INIT(AB8500_VAUX1SEL,		0x04, 0x1f, 0x0f),
@@ -735,79 +1265,412 @@ static struct ab8500_reg_init ab8500_reg_init[] = {
 	REG_INIT(AB8500_REGUCTRLDISCH2,		0x04, 0x44, 0x16),
 };
 
+/* Possibility to add debug */
+int __attribute__((weak)) ab8500_regulator_debug_init(
+	struct platform_device *pdev)
+{
+	return 0;
+}
+
+int __attribute__((weak)) ab8500_regulator_debug_exit(
+	struct platform_device *pdev)
+{
+	return 0;
+}
+
+/* AB9540 register init */
+static struct ab8500_reg_init ab9540_reg_init[] = {
+	/*
+	 * 0x03, VarmRequestCtrl
+	 * 0x0c, VapeRequestCtrl
+	 * 0x30, Vsmps1RequestCtrl
+	 * 0xc0, Vsmps2RequestCtrl
+	 */
+	REG_INIT(AB9540_REGUREQUESTCTRL1,	0x03, 0x03, 0xff),
+	/*
+	 * 0x03, Vsmps3RequestCtrl
+	 * 0x0c, VpllRequestCtrl
+	 * 0x30, VanaRequestCtrl
+	 * 0xc0, VextSupply1RequestCtrl
+	 */
+	REG_INIT(AB9540_REGUREQUESTCTRL2,	0x03, 0x04, 0xff),
+	/*
+	 * 0x03, VextSupply2RequestCtrl
+	 * 0x0c, VextSupply3RequestCtrl
+	 * 0x30, Vaux1RequestCtrl
+	 * 0xc0, Vaux2RequestCtrl
+	 */
+	REG_INIT(AB9540_REGUREQUESTCTRL3,	0x03, 0x05, 0xff),
+	/*
+	 * 0x03, Vaux3RequestCtrl
+	 * 0x04, SwHPReq
+	 */
+	REG_INIT(AB9540_REGUREQUESTCTRL4,	0x03, 0x06, 0x07),
+	/*
+	 * 0x01, Vsmps1SysClkReq1HPValid
+	 * 0x02, Vsmps2SysClkReq1HPValid
+	 * 0x04, Vsmps3SysClkReq1HPValid
+	 * 0x08, VanaSysClkReq1HPValid
+	 * 0x10, VpllSysClkReq1HPValid
+	 * 0x20, Vaux1SysClkReq1HPValid
+	 * 0x40, Vaux2SysClkReq1HPValid
+	 * 0x80, Vaux3SysClkReq1HPValid
+	 */
+	REG_INIT(AB9540_REGUSYSCLKREQ1HPVALID1,	0x03, 0x07, 0xff),
+	/*
+	 * 0x01, VapeSysClkReq1HPValid
+	 * 0x02, VarmSysClkReq1HPValid
+	 * 0x04, VbbSysClkReq1HPValid
+	 * 0x08, VmodSysClkReq1HPValid
+	 * 0x10, VextSupply1SysClkReq1HPValid
+	 * 0x20, VextSupply2SysClkReq1HPValid
+	 * 0x40, VextSupply3SysClkReq1HPValid
+	 */
+	REG_INIT(AB9540_REGUSYSCLKREQ1HPVALID2,	0x03, 0x08, 0x7f),
+	/*
+	 * 0x01, Vsmps1HwHPReq1Valid
+	 * 0x02, Vsmps2HwHPReq1Valid
+	 * 0x04, Vsmps3HwHPReq1Valid
+	 * 0x08, VanaHwHPReq1Valid
+	 * 0x10, VpllHwHPReq1Valid
+	 * 0x20, Vaux1HwHPReq1Valid
+	 * 0x40, Vaux2HwHPReq1Valid
+	 * 0x80, Vaux3HwHPReq1Valid
+	 */
+	REG_INIT(AB9540_REGUHWHPREQ1VALID1,	0x03, 0x09, 0xff),
+	/*
+	 * 0x01, VextSupply1HwHPReq1Valid
+	 * 0x02, VextSupply2HwHPReq1Valid
+	 * 0x04, VextSupply3HwHPReq1Valid
+	 * 0x08, VmodHwHPReq1Valid
+	 */
+	REG_INIT(AB9540_REGUHWHPREQ1VALID2,	0x03, 0x0a, 0x0f),
+	/*
+	 * 0x01, Vsmps1HwHPReq2Valid
+	 * 0x02, Vsmps2HwHPReq2Valid
+	 * 0x03, Vsmps3HwHPReq2Valid
+	 * 0x08, VanaHwHPReq2Valid
+	 * 0x10, VpllHwHPReq2Valid
+	 * 0x20, Vaux1HwHPReq2Valid
+	 * 0x40, Vaux2HwHPReq2Valid
+	 * 0x80, Vaux3HwHPReq2Valid
+	 */
+	REG_INIT(AB9540_REGUHWHPREQ2VALID1,	0x03, 0x0b, 0xff),
+	/*
+	 * 0x01, VextSupply1HwHPReq2Valid
+	 * 0x02, VextSupply2HwHPReq2Valid
+	 * 0x04, VextSupply3HwHPReq2Valid
+	 * 0x08, VmodHwHPReq2Valid
+	 */
+	REG_INIT(AB9540_REGUHWHPREQ2VALID2,	0x03, 0x0c, 0x0f),
+	/*
+	 * 0x01, VapeSwHPReqValid
+	 * 0x02, VarmSwHPReqValid
+	 * 0x04, Vsmps1SwHPReqValid
+	 * 0x08, Vsmps2SwHPReqValid
+	 * 0x10, Vsmps3SwHPReqValid
+	 * 0x20, VanaSwHPReqValid
+	 * 0x40, VpllSwHPReqValid
+	 * 0x80, Vaux1SwHPReqValid
+	 */
+	REG_INIT(AB9540_REGUSWHPREQVALID1,	0x03, 0x0d, 0xff),
+	/*
+	 * 0x01, Vaux2SwHPReqValid
+	 * 0x02, Vaux3SwHPReqValid
+	 * 0x04, VextSupply1SwHPReqValid
+	 * 0x08, VextSupply2SwHPReqValid
+	 * 0x10, VextSupply3SwHPReqValid
+	 * 0x20, VmodSwHPReqValid
+	 */
+	REG_INIT(AB9540_REGUSWHPREQVALID2,	0x03, 0x0e, 0x3f),
+	/*
+	 * 0x02, SysClkReq2Valid1
+	 * ...
+	 * 0x80, SysClkReq8Valid1
+	 */
+	REG_INIT(AB9540_REGUSYSCLKREQVALID1,	0x03, 0x0f, 0xfe),
+	/*
+	 * 0x02, SysClkReq2Valid2
+	 * ...
+	 * 0x80, SysClkReq8Valid2
+	 */
+	REG_INIT(AB9540_REGUSYSCLKREQVALID2,	0x03, 0x10, 0xfe),
+	/*
+	 * 0x01, Vaux4SwHPReqValid
+	 * 0x02, Vaux4HwHPReq2Valid
+	 * 0x04, Vaux4HwHPReq1Valid
+	 * 0x08, Vaux4SysClkReq1HPValid
+	 */
+	REG_INIT(AB9540_REGUVAUX4REQVALID,	0x03, 0x11, 0x0f),
+	/*
+	 * 0x02, VTVoutEna
+	 * 0x04, Vintcore12Ena
+	 * 0x38, Vintcore12Sel
+	 * 0x40, Vintcore12LP
+	 * 0x80, VTVoutLP
+	 */
+	REG_INIT(AB9540_REGUMISC1,		0x03, 0x80, 0xfe),
+	/*
+	 * 0x02, VaudioEna
+	 * 0x04, VdmicEna
+	 * 0x08, Vamic1Ena
+	 * 0x10, Vamic2Ena
+	 */
+	REG_INIT(AB9540_VAUDIOSUPPLY,		0x03, 0x83, 0x1e),
+	/*
+	 * 0x01, Vamic1_dzout
+	 * 0x02, Vamic2_dzout
+	 */
+	REG_INIT(AB9540_REGUCTRL1VAMIC,		0x03, 0x84, 0x03),
+	/*
+	 * 0x03, Vsmps1Regu
+	 * 0x0c, Vsmps1SelCtrl
+	 * 0x10, Vsmps1AutoMode
+	 * 0x20, Vsmps1PWMMode
+	 */
+	REG_INIT(AB9540_VSMPS1REGU,		0x04, 0x03, 0x3f),
+	/*
+	 * 0x03, Vsmps2Regu
+	 * 0x0c, Vsmps2SelCtrl
+	 * 0x10, Vsmps2AutoMode
+	 * 0x20, Vsmps2PWMMode
+	 */
+	REG_INIT(AB9540_VSMPS2REGU,		0x04, 0x04, 0x3f),
+	/*
+	 * 0x03, Vsmps3Regu
+	 * 0x0c, Vsmps3SelCtrl
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB9540_VSMPS3REGU,		0x04, 0x05, 0x0f),
+	/*
+	 * 0x03, VpllRegu
+	 * 0x0c, VanaRegu
+	 */
+	REG_INIT(AB9540_VPLLVANAREGU,		0x04, 0x06, 0x0f),
+	/*
+	 * 0x03, VextSupply1Regu
+	 * 0x0c, VextSupply2Regu
+	 * 0x30, VextSupply3Regu
+	 * 0x40, ExtSupply2Bypass
+	 * 0x80, ExtSupply3Bypass
+	 */
+	REG_INIT(AB9540_EXTSUPPLYREGU,		0x04, 0x08, 0xff),
+	/*
+	 * 0x03, Vaux1Regu
+	 * 0x0c, Vaux2Regu
+	 */
+	REG_INIT(AB9540_VAUX12REGU,		0x04, 0x09, 0x0f),
+	/*
+	 * 0x0c, Vrf1Regu
+	 * 0x03, Vaux3Regu
+	 */
+	REG_INIT(AB9540_VRF1VAUX3REGU,		0x04, 0x0a, 0x0f),
+	/*
+	 * 0x3f, Vsmps1Sel1
+	 */
+	REG_INIT(AB9540_VSMPS1SEL1,		0x04, 0x13, 0x3f),
+	/*
+	 * 0x3f, Vsmps1Sel2
+	 */
+	REG_INIT(AB9540_VSMPS1SEL2,		0x04, 0x14, 0x3f),
+	/*
+	 * 0x3f, Vsmps1Sel3
+	 */
+	REG_INIT(AB9540_VSMPS1SEL3,		0x04, 0x15, 0x3f),
+	/*
+	 * 0x3f, Vsmps2Sel1
+	 */
+	REG_INIT(AB9540_VSMPS2SEL1,		0x04, 0x17, 0x3f),
+	/*
+	 * 0x3f, Vsmps2Sel2
+	 */
+	REG_INIT(AB9540_VSMPS2SEL2,		0x04, 0x18, 0x3f),
+	/*
+	 * 0x3f, Vsmps2Sel3
+	 */
+	REG_INIT(AB9540_VSMPS2SEL3,		0x04, 0x19, 0x3f),
+	/*
+	 * 0x7f, Vsmps3Sel1
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB9540_VSMPS3SEL1,             0x04, 0x1b, 0x7f),
+	/*
+	 * 0x7f, Vsmps3Sel2
+	 * NOTE! PRCMU register
+	 */
+	REG_INIT(AB9540_VSMPS3SEL2,             0x04, 0x1c, 0x7f),
+	/*
+	 * 0x0f, Vaux1Sel
+	 */
+	REG_INIT(AB9540_VAUX1SEL,		0x04, 0x1f, 0x0f),
+	/*
+	 * 0x0f, Vaux2Sel
+	 */
+	REG_INIT(AB9540_VAUX2SEL,		0x04, 0x20, 0x0f),
+	/*
+	 * 0x07, Vaux3Sel
+	 * 0x30, Vrf1Sel
+	 */
+	REG_INIT(AB9540_VRF1VAUX3SEL,		0x04, 0x21, 0x37),
+	/*
+	 * 0x01, VextSupply12LP
+	 */
+	REG_INIT(AB9540_REGUCTRL2SPARE,		0x04, 0x22, 0x01),
+	/*
+	 * 0x03, Vaux4RequestCtrl
+	 */
+	REG_INIT(AB9540_VAUX4REQCTRL,		0x04, 0x2d, 0x03),
+	/*
+	 * 0x03, Vaux4Regu
+	 */
+	REG_INIT(AB9540_VAUX4REGU,		0x04, 0x2e, 0x03),
+	/*
+	 * 0x08, Vaux4Sel
+	 */
+	REG_INIT(AB9540_VAUX4SEL,		0x04, 0x2f, 0x0f),
+	/*
+	 * 0x01, VpllDisch
+	 * 0x02, Vrf1Disch
+	 * 0x04, Vaux1Disch
+	 * 0x08, Vaux2Disch
+	 * 0x10, Vaux3Disch
+	 * 0x20, Vintcore12Disch
+	 * 0x40, VTVoutDisch
+	 * 0x80, VaudioDisch
+	 */
+	REG_INIT(AB9540_REGUCTRLDISCH,		0x04, 0x43, 0xff),
+	/*
+	 * 0x01, VsimDisch
+	 * 0x02, VanaDisch
+	 * 0x04, VdmicPullDownEna
+	 * 0x08, VpllPullDownEna
+	 * 0x10, VdmicDisch
+	 */
+	REG_INIT(AB9540_REGUCTRLDISCH2,		0x04, 0x44, 0x1f),
+	/*
+	 * 0x01, Vaux4Disch
+	 */
+	REG_INIT(AB9540_REGUCTRLDISCH3,		0x04, 0x48, 0x01),
+};
+
 static __devinit int ab8500_regulator_probe(struct platform_device *pdev)
 {
 	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
-	struct ab8500_platform_data *pdata;
+	struct ab8500_platform_data *ppdata;
+	struct ab8500_regulator_platform_data *pdata;
 	int i, err;
+	struct ab8500_regulator_info *regulator_info;
+	int regulator_info_size;
+	struct ab8500_reg_init *reg_init;
+	int reg_init_size;
+	/* cache values needed repeatedly inside for-loops */
 
 	if (!ab8500) {
 		dev_err(&pdev->dev, "null mfd parent\n");
 		return -EINVAL;
 	}
-	pdata = dev_get_platdata(ab8500->dev);
+
+	ppdata = dev_get_platdata(ab8500->dev);
+	if (!ppdata) {
+		dev_err(&pdev->dev, "null parent pdata\n");
+		return -EINVAL;
+	}
+
+	pdata = ppdata->regulator;
 	if (!pdata) {
 		dev_err(&pdev->dev, "null pdata\n");
 		return -EINVAL;
 	}
 
+	if (is_ab9540(ab8500)) {
+		regulator_info = ab9540_regulator_info;
+		regulator_info_size = ARRAY_SIZE(ab9540_regulator_info);
+		reg_init = ab9540_reg_init;
+		reg_init_size = AB9540_NUM_REGULATOR_REGISTERS;
+	} else {
+		regulator_info = ab8500_regulator_info;
+		regulator_info_size = ARRAY_SIZE(ab8500_regulator_info);
+		reg_init = ab8500_reg_init;
+		reg_init_size = AB8500_NUM_REGULATOR_REGISTERS;
+	}
+
 	/* make sure the platform data has the correct size */
-	if (pdata->num_regulator != ARRAY_SIZE(ab8500_regulator_info)) {
+	if (pdata->num_regulator != regulator_info_size) {
 		dev_err(&pdev->dev, "Configuration error: size mismatch.\n");
 		return -EINVAL;
 	}
 
+	/* initialize debug (initial state is recorded with this call) */
+	err = ab8500_regulator_debug_init(pdev);
+	if (err)
+		return err;
+
 	/* initialize registers */
-	for (i = 0; i < pdata->num_regulator_reg_init; i++) {
+	for (i = 0; i < pdata->num_reg_init; i++) {
 		int id;
-		u8 value;
+		u8 mask, value;
 
-		id = pdata->regulator_reg_init[i].id;
-		value = pdata->regulator_reg_init[i].value;
+		id = pdata->reg_init[i].id;
+		mask = pdata->reg_init[i].mask;
+		value = pdata->reg_init[i].value;
 
 		/* check for configuration errors */
-		if (id >= AB8500_NUM_REGULATOR_REGISTERS) {
-			dev_err(&pdev->dev,
-				"Configuration error: id outside range.\n");
-			return -EINVAL;
-		}
-		if (value & ~ab8500_reg_init[id].mask) {
-			dev_err(&pdev->dev,
-				"Configuration error: value outside mask.\n");
-			return -EINVAL;
-		}
+		BUG_ON(id >= reg_init_size);
+		BUG_ON(value & ~mask);
+		BUG_ON(mask & ~reg_init[id].mask);
 
 		/* initialize register */
 		err = abx500_mask_and_set_register_interruptible(&pdev->dev,
-			ab8500_reg_init[id].bank,
-			ab8500_reg_init[id].addr,
-			ab8500_reg_init[id].mask,
-			value);
+			reg_init[id].bank,
+			reg_init[id].addr,
+			mask, value);
 		if (err < 0) {
 			dev_err(&pdev->dev,
 				"Failed to initialize 0x%02x, 0x%02x.\n",
-				ab8500_reg_init[id].bank,
-				ab8500_reg_init[id].addr);
+				reg_init[id].bank,
+				reg_init[id].addr);
 			return err;
 		}
 		dev_vdbg(&pdev->dev,
 			"  init: 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",
-			ab8500_reg_init[id].bank,
-			ab8500_reg_init[id].addr,
-			ab8500_reg_init[id].mask,
-			value);
+			reg_init[id].bank,
+			reg_init[id].addr,
+			mask, value);
+	}
+
+	/*
+	 * This changes the default setting for VextSupply3Regu to low power.
+	 * Active high or low is depending on OTP which is changed from ab8500v3.0.
+	 * Remove this when ab8500v2.0 is no longer important.
+	 * This only affects power consumption and it depends on the
+	 * HREF OTP configurations.
+	 */
+	if (is_ab8500_2p0_or_earlier(ab8500)) {
+		err = abx500_mask_and_set_register_interruptible(&pdev->dev,
+			AB8500_REGU_CTRL2, 0x08, 0x30, 0x30);
+		if (err < 0) {
+			dev_err(&pdev->dev,
+				"Failed to override 0x%02x, 0x%02x.\n",
+				AB8500_REGU_CTRL2, 0x08);
+			return err;
+		}
 	}
 
+	/* register external regulators (before Vaux1, 2 and 3) */
+	err = ab8500_ext_regulator_init(pdev);
+	if (err)
+		return err;
+
 	/* register all regulators */
-	for (i = 0; i < ARRAY_SIZE(ab8500_regulator_info); i++) {
+	for (i = 0; i < regulator_info_size; i++) {
 		struct ab8500_regulator_info *info = NULL;
 
 		/* assign per-regulator data */
-		info = &ab8500_regulator_info[i];
+		info = &regulator_info[i];
 		info->dev = &pdev->dev;
 
 		/* fix for hardware before ab8500v2.0 */
-		if (abx500_get_chip_id(info->dev) < 0x20) {
+		if (is_ab8500_1p1_or_earlier(ab8500)) {
 			if (info->desc.id == AB8500_LDO_AUX3) {
 				info->desc.n_voltages =
 					ARRAY_SIZE(ldo_vauxn_voltages);
@@ -827,7 +1690,7 @@ static __devinit int ab8500_regulator_probe(struct platform_device *pdev)
 					info->desc.name);
 			/* when we fail, un-register all earlier regulators */
 			while (--i >= 0) {
-				info = &ab8500_regulator_info[i];
+				info = &regulator_info[i];
 				regulator_unregister(info->regulator);
 			}
 			return err;
@@ -842,11 +1705,23 @@ static __devinit int ab8500_regulator_probe(struct platform_device *pdev)
 
 static __devexit int ab8500_regulator_remove(struct platform_device *pdev)
 {
-	int i;
+	int i, err;
+	struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
+	struct ab8500_regulator_info *regulator_info;
+	int regulator_info_size;
+
 
-	for (i = 0; i < ARRAY_SIZE(ab8500_regulator_info); i++) {
+	if (is_ab9540(ab8500)) {
+		regulator_info = ab9540_regulator_info;
+		regulator_info_size = ARRAY_SIZE(ab9540_regulator_info);
+	} else {
+		regulator_info = ab8500_regulator_info;
+		regulator_info_size = ARRAY_SIZE(ab8500_regulator_info);
+	}
+
+	for (i = 0; i < regulator_info_size; i++) {
 		struct ab8500_regulator_info *info = NULL;
-		info = &ab8500_regulator_info[i];
+		info = &regulator_info[i];
 
 		dev_vdbg(rdev_get_dev(info->regulator),
 			"%s-remove\n", info->desc.name);
@@ -854,6 +1729,16 @@ static __devexit int ab8500_regulator_remove(struct platform_device *pdev)
 		regulator_unregister(info->regulator);
 	}
 
+	/* remove external regulators (after Vaux1, 2 and 3) */
+	err = ab8500_ext_regulator_exit(pdev);
+	if (err)
+		return err;
+
+	/* remove regulator debug */
+	err = ab8500_regulator_debug_exit(pdev);
+	if (err)
+		return err;
+
 	return 0;
 }
 
@@ -886,5 +1771,6 @@ module_exit(ab8500_regulator_exit);
 
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Sundar Iyer <sundar.iyer@stericsson.com>");
+MODULE_AUTHOR("Bengt Jonsson <bengt.g.jonsson@stericsson.com>");
 MODULE_DESCRIPTION("Regulator Driver for ST-Ericsson AB8500 Mixed-Sig PMIC");
 MODULE_ALIAS("platform:ab8500-regulator");
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 046fb1bd861..bc6c12c8a76 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -81,6 +81,7 @@ struct regulator {
 	struct device_attribute dev_attr;
 	struct regulator_dev *rdev;
 	struct dentry *debugfs;
+	int use;
 };
 
 static int _regulator_is_enabled(struct regulator_dev *rdev);
@@ -199,11 +200,13 @@ static int regulator_check_consumers(struct regulator_dev *rdev,
 		 */
 		if (!regulator->min_uV && !regulator->max_uV)
 			continue;
-
-		if (*max_uV > regulator->max_uV)
-			*max_uV = regulator->max_uV;
-		if (*min_uV < regulator->min_uV)
-			*min_uV = regulator->min_uV;
+		
+		if (regulator->use) {
+			if (*max_uV > regulator->max_uV)
+				*max_uV = regulator->max_uV;
+			if (*min_uV < regulator->min_uV)
+				*min_uV = regulator->min_uV;
+		}
 	}
 
 	if (*min_uV > *max_uV)
@@ -602,6 +605,32 @@ static ssize_t regulator_suspend_standby_state_show(struct device *dev,
 static DEVICE_ATTR(suspend_standby_state, 0444,
 		regulator_suspend_standby_state_show, NULL);
 
+static ssize_t regulator_use_show(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct regulator_dev *rdev = dev_get_drvdata(dev);
+	struct regulator *reg;
+	size_t size = 0;
+
+	if (rdev->use_count == 0)
+		return sprintf(buf, "no users\n");
+
+	list_for_each_entry(reg, &rdev->consumer_list, list) {
+		if (!reg->use)
+			continue;
+
+		if (reg->dev != NULL)
+			size += sprintf((buf + size), "%s (%d) ",
+					dev_name(reg->dev), reg->use);
+		else
+			size += sprintf((buf + size), "unknown (%d) ",
+					reg->use);
+	}
+	size += sprintf((buf + size), "\n");
+
+	return size;
+}
+static DEVICE_ATTR(use, 0444, regulator_use_show, NULL);
 
 /*
  * These are the only attributes are present for all regulators.
@@ -1491,12 +1520,8 @@ static int _regulator_enable(struct regulator_dev *rdev)
 
 			trace_regulator_enable_delay(rdev_get_name(rdev));
 
-			if (delay >= 1000) {
-				mdelay(delay / 1000);
-				udelay(delay % 1000);
-			} else if (delay) {
-				udelay(delay);
-			}
+			if (delay)
+				usleep_range(delay, delay);
 
 			trace_regulator_enable_complete(rdev_get_name(rdev));
 
@@ -1540,6 +1565,8 @@ int regulator_enable(struct regulator *regulator)
 
 	if (ret != 0 && rdev->supply)
 		regulator_disable(rdev->supply);
+	else
+		regulator->use++;
 
 	return ret;
 }
@@ -1613,6 +1640,9 @@ int regulator_disable(struct regulator *regulator)
 	if (ret == 0 && rdev->supply)
 		regulator_disable(rdev->supply);
 
+	if (ret == 0)
+		regulator->use--;
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(regulator_disable);
@@ -2699,6 +2729,10 @@ static int add_regulator_attributes(struct regulator_dev *rdev)
 	struct regulator_ops	*ops = rdev->desc->ops;
 	int			status = 0;
 
+	status = device_create_file(dev, &dev_attr_use);
+	if (status < 0)
+		dev_warn(dev, "Create sysfs file \"use\" failed");
+
 	/* some attributes need specific methods to be displayed */
 	if ((ops->get_voltage && ops->get_voltage(rdev) >= 0) ||
 	    (ops->get_voltage_sel && ops->get_voltage_sel(rdev) >= 0)) {
diff --git a/drivers/regulator/db5500-prcmu.c b/drivers/regulator/db5500-prcmu.c
new file mode 100644
index 00000000000..189362ab8e0
--- /dev/null
+++ b/drivers/regulator/db5500-prcmu.c
@@ -0,0 +1,334 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ * Authors: Sundar Iyer <sundar.iyer@stericsson.com> for ST-Ericsson
+ *          Bengt Jonsson <bengt.g.jonsson@stericsson.com> for ST-Ericsson
+ *
+ * Power domain regulators on DB5500
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/db5500-prcmu.h>
+
+#include <linux/mfd/dbx500-prcmu.h>
+
+#include "dbx500-prcmu.h"
+static int db5500_regulator_enable(struct regulator_dev *rdev)
+{
+	struct dbx500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-%s-enable\n",
+		info->desc.name);
+
+	if (!info->is_enabled) {
+		info->is_enabled = true;
+		if (!info->exclude_from_power_state)
+			power_state_active_enable();
+	}
+
+	return 0;
+}
+
+static int db5500_regulator_disable(struct regulator_dev *rdev)
+{
+	struct dbx500_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret = 0;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-%s-disable\n",
+		info->desc.name);
+
+	if (info->is_enabled) {
+		info->is_enabled = false;
+		if (!info->exclude_from_power_state)
+			ret = power_state_active_disable();
+	}
+
+	return ret;
+}
+
+static int db5500_regulator_is_enabled(struct regulator_dev *rdev)
+{
+	struct dbx500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-%s-is_enabled (is_enabled):"
+		" %i\n", info->desc.name, info->is_enabled);
+
+	return info->is_enabled;
+}
+
+/* db5500 regulator operations */
+static struct regulator_ops db5500_regulator_ops = {
+	.enable			= db5500_regulator_enable,
+	.disable		= db5500_regulator_disable,
+	.is_enabled		= db5500_regulator_is_enabled,
+};
+
+/*
+ * EPOD control
+ */
+static bool epod_on[NUM_EPOD_ID];
+static bool epod_ramret[NUM_EPOD_ID];
+
+static inline int epod_id_to_index(u16 epod_id)
+{
+	return epod_id - DB5500_EPOD_ID_BASE;
+}
+
+static int enable_epod(u16 epod_id, bool ramret)
+{
+	int idx = epod_id_to_index(epod_id);
+	int ret;
+
+	if (ramret) {
+		if (!epod_on[idx]) {
+			ret = prcmu_set_epod(epod_id, EPOD_STATE_RAMRET);
+			if (ret < 0)
+				return ret;
+		}
+		epod_ramret[idx] = true;
+	} else {
+		ret = prcmu_set_epod(epod_id, EPOD_STATE_ON);
+		if (ret < 0)
+			return ret;
+		epod_on[idx] = true;
+	}
+
+	return 0;
+}
+
+static int disable_epod(u16 epod_id, bool ramret)
+{
+	int idx = epod_id_to_index(epod_id);
+	int ret;
+
+	if (ramret) {
+		if (!epod_on[idx]) {
+			ret = prcmu_set_epod(epod_id, EPOD_STATE_OFF);
+			if (ret < 0)
+				return ret;
+		}
+		epod_ramret[idx] = false;
+	} else {
+		if (epod_ramret[idx]) {
+			ret = prcmu_set_epod(epod_id, EPOD_STATE_RAMRET);
+			if (ret < 0)
+				return ret;
+		} else {
+			ret = prcmu_set_epod(epod_id, EPOD_STATE_OFF);
+			if (ret < 0)
+				return ret;
+		}
+		epod_on[idx] = false;
+	}
+
+	return 0;
+}
+
+/*
+ * Regulator switch
+ */
+static int db5500_regulator_switch_enable(struct regulator_dev *rdev)
+{
+	struct dbx500_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-switch-%s-enable\n",
+		info->desc.name);
+
+	ret = enable_epod(info->epod_id, info->is_ramret);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"regulator-switch-%s-enable: prcmu call failed\n",
+			info->desc.name);
+		goto out;
+	}
+
+	info->is_enabled = true;
+out:
+	return ret;
+}
+
+static int db5500_regulator_switch_disable(struct regulator_dev *rdev)
+{
+	struct dbx500_regulator_info *info = rdev_get_drvdata(rdev);
+	int ret;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev), "regulator-switch-%s-disable\n",
+		info->desc.name);
+
+	ret = disable_epod(info->epod_id, info->is_ramret);
+	if (ret < 0) {
+		dev_err(rdev_get_dev(rdev),
+			"regulator_switch-%s-disable: prcmu call failed\n",
+			info->desc.name);
+		goto out;
+	}
+
+	info->is_enabled = 0;
+out:
+	return ret;
+}
+
+static int db5500_regulator_switch_is_enabled(struct regulator_dev *rdev)
+{
+	struct dbx500_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (info == NULL)
+		return -EINVAL;
+
+	dev_vdbg(rdev_get_dev(rdev),
+		"regulator-switch-%s-is_enabled (is_enabled): %i\n",
+		info->desc.name, info->is_enabled);
+
+	return info->is_enabled;
+}
+
+static struct regulator_ops db5500_regulator_switch_ops = {
+	.enable			= db5500_regulator_switch_enable,
+	.disable		= db5500_regulator_switch_disable,
+	.is_enabled		= db5500_regulator_switch_is_enabled,
+};
+
+/*
+ * Regulator information
+ */
+#define DB5500_REGULATOR_SWITCH(_name, reg)                               \
+	[DB5500_REGULATOR_SWITCH_##reg] = {                               \
+		.desc = {                                               \
+			.name   = _name,                                \
+			.id     = DB5500_REGULATOR_SWITCH_##reg,          \
+			.ops    = &db5500_regulator_switch_ops,          \
+			.type   = REGULATOR_VOLTAGE,                    \
+			.owner  = THIS_MODULE,                          \
+		},                                                      \
+		.epod_id = DB5500_EPOD_ID_##reg,                         \
+}
+
+static struct dbx500_regulator_info
+		dbx500_regulator_info[DB5500_NUM_REGULATORS] = {
+	[DB5500_REGULATOR_VAPE] = {
+		.desc = {
+			.name	= "db5500-vape",
+			.id	= DB5500_REGULATOR_VAPE,
+			.ops	= &db5500_regulator_ops,
+			.type	= REGULATOR_VOLTAGE,
+			.owner	= THIS_MODULE,
+		},
+	},
+	DB5500_REGULATOR_SWITCH("db5500-sga", SGA),
+	DB5500_REGULATOR_SWITCH("db5500-hva", HVA),
+	DB5500_REGULATOR_SWITCH("db5500-sia", SIA),
+	DB5500_REGULATOR_SWITCH("db5500-disp", DISP),
+	DB5500_REGULATOR_SWITCH("db5500-esram12", ESRAM12),
+};
+
+static int __devinit db5500_regulator_probe(struct platform_device *pdev)
+{
+	struct regulator_init_data *db5500_init_data =
+					dev_get_platdata(&pdev->dev);
+	int i, err;
+
+	/* register all regulators */
+	for (i = 0; i < ARRAY_SIZE(dbx500_regulator_info); i++) {
+		struct dbx500_regulator_info *info;
+		struct regulator_init_data *init_data = &db5500_init_data[i];
+
+		/* assign per-regulator data */
+		info = &dbx500_regulator_info[i];
+		info->dev = &pdev->dev;
+
+		/* register with the regulator framework */
+		info->rdev = regulator_register(&info->desc, &pdev->dev,
+				init_data, info);
+		if (IS_ERR(info->rdev)) {
+			err = PTR_ERR(info->rdev);
+			dev_err(&pdev->dev, "failed to register %s: err %i\n",
+				info->desc.name, err);
+
+			/* if failing, unregister all earlier regulators */
+			i--;
+			while (i >= 0) {
+				info = &dbx500_regulator_info[i];
+				regulator_unregister(info->rdev);
+				i--;
+			}
+			return err;
+		}
+
+		dev_dbg(rdev_get_dev(info->rdev),
+			"regulator-%s-probed\n", info->desc.name);
+	}
+
+	return 0;
+}
+
+static int __exit db5500_regulator_remove(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dbx500_regulator_info); i++) {
+		struct dbx500_regulator_info *info;
+		info = &dbx500_regulator_info[i];
+
+		dev_vdbg(rdev_get_dev(info->rdev),
+			"regulator-%s-remove\n", info->desc.name);
+
+		regulator_unregister(info->rdev);
+	}
+
+	return 0;
+}
+
+static struct platform_driver db5500_regulator_driver = {
+	.driver = {
+		.name = "db5500-prcmu-regulators",
+		.owner = THIS_MODULE,
+	},
+	.probe = db5500_regulator_probe,
+	.remove = __exit_p(db5500_regulator_remove),
+};
+
+static int __init db5500_regulator_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&db5500_regulator_driver);
+	if (ret < 0)
+		return -ENODEV;
+
+	return 0;
+}
+
+static void __exit db5500_regulator_exit(void)
+{
+	platform_driver_unregister(&db5500_regulator_driver);
+}
+
+arch_initcall(db5500_regulator_init);
+module_exit(db5500_regulator_exit);
+
+MODULE_AUTHOR("STMicroelectronics/ST-Ericsson");
+MODULE_DESCRIPTION("DB5500 regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/dbx500-prcmu.c b/drivers/regulator/dbx500-prcmu.c
index f2e5ecdc586..bee4f7be93b 100644
--- a/drivers/regulator/dbx500-prcmu.c
+++ b/drivers/regulator/dbx500-prcmu.c
@@ -9,6 +9,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/err.h>
 #include <linux/regulator/driver.h>
 #include <linux/debugfs.h>
@@ -62,12 +63,105 @@ out:
 	return ret;
 }
 
+struct ux500_regulator {
+	char *name;
+	void (*enable)(void);
+	int (*disable)(void);
+	int count;
+};
+
+static struct ux500_regulator ux500_atomic_regulators[] = {
+	{
+		.name    = "dma40.0",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "ssp0",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "ssp1",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "spi0",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "spi1",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "spi2",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "spi3",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "cryp1",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+	{
+		.name = "hash1",
+		.enable  = power_state_active_enable,
+		.disable = power_state_active_disable,
+	},
+};
+
+struct ux500_regulator *__must_check ux500_regulator_get(struct device *dev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ux500_atomic_regulators); i++) {
+		if (!strcmp(dev_name(dev), ux500_atomic_regulators[i].name))
+			return &ux500_atomic_regulators[i];
+	}
+
+	return  ERR_PTR(-EINVAL);
+}
+EXPORT_SYMBOL_GPL(ux500_regulator_get);
+
+int ux500_regulator_atomic_enable(struct ux500_regulator *regulator)
+{
+	if (regulator) {
+		regulator->count++;
+		regulator->enable();
+		return 0;
+	}
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(ux500_regulator_atomic_enable);
+
+int ux500_regulator_atomic_disable(struct ux500_regulator *regulator)
+{
+	if (regulator) {
+		regulator->count--;
+		return regulator->disable();
+	}
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(ux500_regulator_atomic_disable);
+
+void ux500_regulator_put(struct ux500_regulator *regulator)
+{
+	/* Here for symetric reasons and for possible future use */
+}
+EXPORT_SYMBOL_GPL(ux500_regulator_put);
+
 #ifdef CONFIG_REGULATOR_DEBUG
 
 static struct ux500_regulator_debug {
 	struct dentry *dir;
-	struct dentry *status_file;
-	struct dentry *power_state_cnt_file;
 	struct dbx500_regulator_info *regulator_array;
 	int num_regulators;
 	u8 *state_before_suspend;
@@ -119,6 +213,35 @@ static const struct file_operations ux500_regulator_power_state_cnt_fops = {
 	.owner = THIS_MODULE,
 };
 
+static int ux500_regulator_power_state_use_print(struct seq_file *s, void *p)
+{
+	int i;
+
+	seq_printf(s, "\nPower state usage:\n\n");
+
+	for (i = 0; i < ARRAY_SIZE(ux500_atomic_regulators); i++) {
+		seq_printf(s, "%s\t : %d\n",
+			   ux500_atomic_regulators[i].name,
+			   ux500_atomic_regulators[i].count);
+	}
+	return 0;
+}
+
+static int ux500_regulator_power_state_use_open(struct inode *inode,
+	struct file *file)
+{
+	return single_open(file, ux500_regulator_power_state_use_print,
+		inode->i_private);
+}
+
+static const struct file_operations ux500_regulator_power_state_use_fops = {
+	.open = ux500_regulator_power_state_use_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
 static int ux500_regulator_status_print(struct seq_file *s, void *p)
 {
 	struct device *dev = s->private;
@@ -180,22 +303,27 @@ ux500_regulator_debug_init(struct platform_device *pdev,
 {
 	/* create directory */
 	rdebug.dir = debugfs_create_dir("ux500-regulator", NULL);
-	if (!rdebug.dir)
+	if (IS_ERR_OR_NULL(rdebug.dir))
 		goto exit_no_debugfs;
 
 	/* create "status" file */
-	rdebug.status_file = debugfs_create_file("status",
-		S_IRUGO, rdebug.dir, &pdev->dev,
-		&ux500_regulator_status_fops);
-	if (!rdebug.status_file)
-		goto exit_destroy_dir;
+	if (IS_ERR_OR_NULL(debugfs_create_file("status",
+					       S_IRUGO, rdebug.dir, &pdev->dev,
+					       &ux500_regulator_status_fops)))
+		goto exit_fail;
+
+	/* create "power-state-count" file */
+	if (IS_ERR_OR_NULL(debugfs_create_file("power-state-count",
+					       S_IRUGO, rdebug.dir, &pdev->dev,
+					       &ux500_regulator_power_state_cnt_fops)))
+		goto exit_fail;
 
 	/* create "power-state-count" file */
-	rdebug.power_state_cnt_file = debugfs_create_file("power-state-count",
-		S_IRUGO, rdebug.dir, &pdev->dev,
-		&ux500_regulator_power_state_cnt_fops);
-	if (!rdebug.power_state_cnt_file)
-		goto exit_destroy_status;
+	if (IS_ERR_OR_NULL(debugfs_create_file("power-state-usage",
+					       S_IRUGO, rdebug.dir, &pdev->dev,
+					       &ux500_regulator_power_state_use_fops)))
+		goto exit_fail;
+
 
 	rdebug.regulator_array = regulator_info;
 	rdebug.num_regulators = num_regulators;
@@ -204,27 +332,22 @@ ux500_regulator_debug_init(struct platform_device *pdev,
 	if (!rdebug.state_before_suspend) {
 		dev_err(&pdev->dev,
 			"could not allocate memory for saving state\n");
-		goto exit_destroy_power_state;
+		goto exit_fail;
 	}
 
 	rdebug.state_after_suspend = kzalloc(num_regulators, GFP_KERNEL);
 	if (!rdebug.state_after_suspend) {
 		dev_err(&pdev->dev,
 			"could not allocate memory for saving state\n");
-		goto exit_free;
+		goto exit_fail;
 	}
 
 	dbx500_regulator_testcase(regulator_info, num_regulators);
 	return 0;
 
-exit_free:
+exit_fail:
 	kfree(rdebug.state_before_suspend);
-exit_destroy_power_state:
-	debugfs_remove(rdebug.power_state_cnt_file);
-exit_destroy_status:
-	debugfs_remove(rdebug.status_file);
-exit_destroy_dir:
-	debugfs_remove(rdebug.dir);
+	debugfs_remove_recursive(rdebug.dir);
 exit_no_debugfs:
 	dev_err(&pdev->dev, "failed to create debugfs entries.\n");
 	return -ENOMEM;
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 8c8377d50c4..3e47885660e 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -704,6 +704,13 @@ config RTC_DRV_PCF50633
 	  If you say yes here you get support for the RTC subsystem of the
 	  NXP PCF50633 used in embedded systems.
 
+config RTC_DRV_AB
+	tristate "ST-Ericsson AB5500 RTC"
+	depends on AB5500_CORE
+	help
+	  Select this to enable the ST-Ericsson AB5500 Mixed Signal IC RTC
+	  support. This chip contains a battery- and capacitor-backed RTC.
+
 config RTC_DRV_AB3100
 	tristate "ST-Ericsson AB3100 RTC"
 	depends on AB3100_CORE
@@ -715,6 +722,7 @@ config RTC_DRV_AB3100
 config RTC_DRV_AB8500
 	tristate "ST-Ericsson AB8500 RTC"
 	depends on AB8500_CORE
+	select RTC_INTF_DEV_UIE_EMUL
 	help
 	  Select this to enable the ST-Ericsson AB8500 power management IC RTC
 	  support. This chip contains a battery- and capacitor-backed RTC.
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 727ae7786e6..56766bbc519 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -16,6 +16,7 @@ rtc-core-$(CONFIG_RTC_INTF_SYSFS) += rtc-sysfs.o
 # Keep the list ordered.
 
 obj-$(CONFIG_RTC_DRV_88PM860X)  += rtc-88pm860x.o
+obj-$(CONFIG_RTC_DRV_AB)	+= rtc-ab.o
 obj-$(CONFIG_RTC_DRV_AB3100)	+= rtc-ab3100.o
 obj-$(CONFIG_RTC_DRV_AB8500)	+= rtc-ab8500.o
 obj-$(CONFIG_RTC_DRV_AT32AP700X)+= rtc-at32ap700x.o
diff --git a/drivers/rtc/rtc-ab.c b/drivers/rtc/rtc-ab.c
new file mode 100644
index 00000000000..009409f39d7
--- /dev/null
+++ b/drivers/rtc/rtc-ab.c
@@ -0,0 +1,485 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com>
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+
+#define AB5500_RTC_CLOCK_RATE	32768
+#define AB5500_RTC		0x00
+#define AB5500_RTC_ALARM	(1 << 1)
+#define AB5500_READREQ		0x01
+#define AB5500_READREQ_REQ	0x01
+#define AB5500_AL0		0x02
+#define AB5500_TI0		0x06
+
+/**
+ * struct ab_rtc - variant specific data
+ * @irqname: optional name for the alarm interrupt resource
+ * @epoch: epoch to adjust year to
+ * @bank: AB bank where this block is present
+ * @rtc: address of the "RTC" (control) register
+ * @rtc_alarmon: mask of the alarm enable bit in the above register
+ * @ti0: address of the TI0 register.  The rest of the TI
+ * registers are assumed to contiguously follow this one.
+ * @nr_ti: number of TI* registers
+ * @al0: address of the AL0 register.  The rest of the
+ * AL registers are assumed to contiguously follow this one.
+ * @nr_al: number of AL* registers
+ * @startup: optional function to initialize the RTC
+ * @alarm_to_regs: function to convert alarm time in seconds
+ * to a list of AL register values
+ * @time_to_regs: function to convert alarm time in seconds
+ * to a list of TI register values
+ * @regs_to_alarm: function to convert a list of AL register
+ * values to the alarm time in seconds
+ * @regs_to_time: function to convert a list of TI register
+ * values to the alarm time in seconds
+ * @request_read: optional function to request a read from the TI* registers
+ * @request_write: optional function to request a write to the TI* registers
+ */
+struct ab_rtc {
+	const char *irqname;
+	unsigned int epoch;
+
+	u8 bank;
+	u8 rtc;
+	u8 rtc_alarmon;
+	u8 ti0;
+	int nr_ti;
+	u8 al0;
+	int nr_al;
+
+	int (*startup)(struct device *dev);
+	void (*alarm_to_regs)(struct device *dev, unsigned long secs, u8 *regs);
+	void (*time_to_regs)(struct device *dev, unsigned long secs, u8 *regs);
+	unsigned long (*regs_to_alarm)(struct device *dev, u8 *regs);
+	unsigned long (*regs_to_time)(struct device *dev, u8 *regs);
+	int (*request_read)(struct device *dev);
+	int (*request_write)(struct device *dev);
+};
+
+static const struct ab_rtc *to_ab_rtc(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	return (struct ab_rtc *)pdev->id_entry->driver_data;
+}
+
+/* Calculate the number of seconds since year, for epoch adjustment */
+static unsigned long ab_rtc_get_elapsed_seconds(unsigned int year)
+{
+	unsigned long secs;
+	struct rtc_time tm = {
+		.tm_year = year - 1900,
+		.tm_mday = 1,
+	};
+
+	rtc_tm_to_time(&tm, &secs);
+
+	return secs;
+}
+
+static int ab5500_rtc_request_read(struct device *dev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned long timeout;
+	int err;
+
+	err = abx500_set_register_interruptible(dev, variant->bank,
+						AB5500_READREQ,
+						AB5500_READREQ_REQ);
+	if (err < 0)
+		return err;
+
+	timeout = jiffies + HZ;
+	while (time_before(jiffies, timeout)) {
+		u8 value;
+
+		err = abx500_get_register_interruptible(dev, variant->bank,
+				AB5500_READREQ, &value);
+		if (err < 0)
+			return err;
+
+		if (!(value & AB5500_READREQ_REQ))
+			return 0;
+
+		msleep(1);
+	}
+
+	return -EIO;
+}
+
+static void
+ab5500_rtc_time_to_regs(struct device *dev, unsigned long secs, u8 *regs)
+{
+	unsigned long mins = secs / 60;
+	u64 fat_time;
+
+	secs %= 60;
+
+	fat_time = secs * AB5500_RTC_CLOCK_RATE;
+	fat_time |= (u64)mins << 21;
+
+	regs[0] = (fat_time) & 0xFF;
+	regs[1] = (fat_time >> 8) & 0xFF;
+	regs[2] = (fat_time >> 16) & 0xFF;
+	regs[3] = (fat_time >> 24) & 0xFF;
+	regs[4] = (fat_time >> 32) & 0xFF;
+	regs[5] = (fat_time >> 40) & 0xFF;
+}
+
+static unsigned long
+ab5500_rtc_regs_to_time(struct device *dev, u8 *regs)
+{
+	u64 fat_time = ((u64)regs[5] << 40) | ((u64)regs[4] << 32) |
+		       ((u64)regs[3] << 24) | ((u64)regs[2] << 16) |
+		       ((u64)regs[1] << 8)  | regs[0];
+	unsigned long secs = (fat_time & 0x1fffff) / AB5500_RTC_CLOCK_RATE;
+	unsigned long mins = fat_time >> 21;
+
+	return mins * 60 + secs;
+}
+
+static void
+ab5500_rtc_alarm_to_regs(struct device *dev, unsigned long secs, u8 *regs)
+{
+	unsigned long mins = secs / 60;
+
+#ifdef CONFIG_ANDROID
+	/*
+	 * Needed because Android believes all hw have a wake-up resolution in
+	 * seconds.
+	 */
+	mins++;
+#endif
+
+	regs[0] = mins & 0xFF;
+	regs[1] = (mins >> 8) & 0xFF;
+	regs[2] = (mins >> 16) & 0xFF;
+}
+
+static unsigned long
+ab5500_rtc_regs_to_alarm(struct device *dev, u8 *regs)
+{
+	unsigned long mins = ((unsigned long)regs[2] << 16) |
+			     ((unsigned long)regs[1] << 8) |
+			     regs[0];
+	unsigned long secs = mins * 60;
+
+	return secs;
+}
+
+static const struct ab_rtc ab5500_rtc = {
+	.irqname	= "RTC_Alarm",
+	.bank		= AB5500_BANK_RTC,
+	.rtc		= AB5500_RTC,
+	.rtc_alarmon	= AB5500_RTC_ALARM,
+	.ti0		= AB5500_TI0,
+	.nr_ti		= 6,
+	.al0		= AB5500_AL0,
+	.nr_al		= 3,
+	.epoch		= 2000,
+	.time_to_regs	= ab5500_rtc_time_to_regs,
+	.regs_to_time	= ab5500_rtc_regs_to_time,
+	.alarm_to_regs	= ab5500_rtc_alarm_to_regs,
+	.regs_to_alarm	= ab5500_rtc_regs_to_alarm,
+	.request_read	= ab5500_rtc_request_read,
+};
+
+static int ab_rtc_request_read(struct device *dev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	if (!variant->request_read)
+		return 0;
+
+	return variant->request_read(dev);
+}
+
+static int ab_rtc_request_write(struct device *dev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	if (!variant->request_write)
+		return 0;
+
+	return variant->request_write(dev);
+}
+
+static bool ab_rtc_valid_time(struct device *dev, struct rtc_time *time)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	if (!variant->epoch)
+		return true;
+
+	return time->tm_year >= variant->epoch - 1900;
+}
+
+static int
+ab_rtc_tm_to_time(struct device *dev, struct rtc_time *tm, unsigned long *secs)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	rtc_tm_to_time(tm, secs);
+
+	if (variant->epoch)
+		*secs -= ab_rtc_get_elapsed_seconds(variant->epoch);
+
+	return 0;
+}
+
+static int
+ab_rtc_time_to_tm(struct device *dev, unsigned long secs, struct rtc_time *tm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+
+	if (variant->epoch)
+		secs += ab_rtc_get_elapsed_seconds(variant->epoch);
+
+	rtc_time_to_tm(secs, tm);
+
+	return 0;
+}
+
+static int ab_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned char buf[variant->nr_ti];
+	unsigned long secs;
+	int err;
+
+	err = ab_rtc_request_read(dev);
+	if (err)
+		return err;
+
+	err = abx500_get_register_page_interruptible(dev, variant->bank,
+						     variant->ti0,
+						     buf, variant->nr_ti);
+	if (err)
+		return err;
+
+	secs = variant->regs_to_time(dev, buf);
+	ab_rtc_time_to_tm(dev, secs, tm);
+
+	return rtc_valid_tm(tm);
+}
+
+static int ab_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned char buf[variant->nr_ti];
+	unsigned long secs;
+	u8 reg = variant->ti0;
+	int err;
+	int i;
+
+	if (!ab_rtc_valid_time(dev, tm))
+		return -EINVAL;
+
+	ab_rtc_tm_to_time(dev, tm, &secs);
+	variant->time_to_regs(dev, secs, buf);
+
+	for (i = 0; i < variant->nr_ti; i++, reg++) {
+		err = abx500_set_register_interruptible(dev, variant->bank,
+							reg, buf[i]);
+		if (err)
+			return err;
+	}
+
+	return ab_rtc_request_write(dev);
+}
+
+static int ab_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned long secs;
+	u8 buf[variant->nr_al];
+	u8 rtcval;
+	int err;
+
+	err = abx500_get_register_interruptible(dev, variant->bank,
+						variant->rtc, &rtcval);
+	if (err)
+		return err;
+
+	alarm->enabled = !!(rtcval & variant->rtc_alarmon);
+	alarm->pending = 0;
+
+	err = abx500_get_register_page_interruptible(dev, variant->bank,
+						     variant->al0, buf,
+						     variant->nr_al);
+	if (err)
+		return err;
+
+	secs = variant->regs_to_alarm(dev, buf);
+	ab_rtc_time_to_tm(dev, secs, &alarm->time);
+
+	return rtc_valid_tm(&alarm->time);
+}
+
+static int ab_rtc_alarm_enable(struct device *dev, unsigned int enabled)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	u8 mask = variant->rtc_alarmon;
+	u8 value = enabled ? mask : 0;
+
+	return abx500_mask_and_set_register_interruptible(dev, variant->bank,
+							  variant->rtc, mask,
+							  value);
+}
+
+static int ab_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+	const struct ab_rtc *variant = to_ab_rtc(dev);
+	unsigned char buf[variant->nr_al];
+	unsigned long secs;
+	u8 reg = variant->al0;
+	int err;
+	int i;
+
+	if (!ab_rtc_valid_time(dev, &alarm->time))
+		return -EINVAL;
+
+	ab_rtc_tm_to_time(dev, &alarm->time, &secs);
+	variant->alarm_to_regs(dev, secs, buf);
+
+	/*
+	 * Disable alarm first.  Otherwise the RTC may not detect an alarm
+	 * reprogrammed for the same time without disabling the alarm in
+	 * between the programmings.
+	 */
+	err = ab_rtc_alarm_enable(dev, false);
+	if (err)
+		return err;
+
+	for (i = 0; i < variant->nr_al; i++, reg++) {
+		err = abx500_set_register_interruptible(dev, variant->bank,
+							reg, buf[i]);
+		if (err)
+			return err;
+	}
+
+	return alarm->enabled ? ab_rtc_alarm_enable(dev, true) : 0;
+}
+
+static const struct rtc_class_ops ab_rtc_ops = {
+	.read_time		= ab_rtc_read_time,
+	.set_time		= ab_rtc_set_time,
+	.read_alarm		= ab_rtc_read_alarm,
+	.set_alarm		= ab_rtc_set_alarm,
+	.alarm_irq_enable	= ab_rtc_alarm_enable,
+};
+
+static irqreturn_t ab_rtc_irq(int irq, void *dev_id)
+{
+	unsigned long events = RTC_IRQF | RTC_AF;
+	struct rtc_device *rtc = dev_id;
+
+	rtc_update_irq(rtc, 1, events);
+
+	return IRQ_HANDLED;
+}
+
+static int __devinit ab_rtc_probe(struct platform_device *pdev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(&pdev->dev);
+	int err;
+	struct rtc_device *rtc;
+	int irq = -ENXIO;
+
+	if (variant->irqname) {
+		irq = platform_get_irq_byname(pdev, variant->irqname);
+		if (irq < 0)
+			return irq;
+	}
+
+	if (variant->startup) {
+		err = variant->startup(&pdev->dev);
+		if (err)
+			return err;
+	}
+
+	device_init_wakeup(&pdev->dev, true);
+
+	rtc = rtc_device_register("ab8500-rtc", &pdev->dev, &ab_rtc_ops,
+			THIS_MODULE);
+	if (IS_ERR(rtc)) {
+		dev_err(&pdev->dev, "Registration failed\n");
+		err = PTR_ERR(rtc);
+		return err;
+	}
+
+	if (irq >= 0) {
+		err = request_any_context_irq(irq, ab_rtc_irq,
+					      IRQF_NO_SUSPEND,
+					      pdev->id_entry->name,
+					      rtc);
+		if (err < 0) {
+			dev_err(&pdev->dev, "could not get irq: %d\n", err);
+			goto out_unregister;
+		}
+	}
+
+	platform_set_drvdata(pdev, rtc);
+
+	return 0;
+
+out_unregister:
+	rtc_device_unregister(rtc);
+	return err;
+}
+
+static int __devexit ab_rtc_remove(struct platform_device *pdev)
+{
+	const struct ab_rtc *variant = to_ab_rtc(&pdev->dev);
+	struct rtc_device *rtc = platform_get_drvdata(pdev);
+	int irq = platform_get_irq_byname(pdev, variant->irqname);
+
+	if (irq >= 0)
+		free_irq(irq, rtc);
+	rtc_device_unregister(rtc);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_device_id ab_rtc_id_table[] = {
+	{ "ab5500-rtc", (kernel_ulong_t)&ab5500_rtc, },
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, ab_rtc_id_table);
+
+static struct platform_driver ab_rtc_driver = {
+	.driver.name	= "ab-rtc",
+	.driver.owner	= THIS_MODULE,
+	.id_table	= ab_rtc_id_table,
+	.probe		= ab_rtc_probe,
+	.remove		= __devexit_p(ab_rtc_remove),
+};
+
+static int __init ab_rtc_init(void)
+{
+	return platform_driver_register(&ab_rtc_driver);
+}
+module_init(ab_rtc_init);
+
+static void __exit ab_rtc_exit(void)
+{
+	platform_driver_unregister(&ab_rtc_driver);
+}
+module_exit(ab_rtc_exit);
+
+MODULE_AUTHOR("Rabin Vincent <rabin.vincent@stericsson.com>");
+MODULE_DESCRIPTION("AB5500 RTC Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/rtc/rtc-ab8500.c b/drivers/rtc/rtc-ab8500.c
index 4bcf9ca2818..63b3a672a17 100644
--- a/drivers/rtc/rtc-ab8500.c
+++ b/drivers/rtc/rtc-ab8500.c
@@ -88,22 +88,17 @@ static int ab8500_rtc_read_time(struct device *dev, struct rtc_time *tm)
 	if (retval < 0)
 		return retval;
 
-	/* Early AB8500 chips will not clear the rtc read request bit */
-	if (abx500_get_chip_id(dev) == 0) {
-		usleep_range(1000, 1000);
-	} else {
-		/* Wait for some cycles after enabling the rtc read in ab8500 */
-		while (time_before(jiffies, timeout)) {
-			retval = abx500_get_register_interruptible(dev,
-				AB8500_RTC, AB8500_RTC_READ_REQ_REG, &value);
-			if (retval < 0)
-				return retval;
-
-			if (!(value & RTC_READ_REQUEST))
-				break;
-
-			usleep_range(1000, 5000);
-		}
+	/* Wait for some cycles after enabling the rtc read in ab8500 */
+	while (time_before(jiffies, timeout)) {
+		retval = abx500_get_register_interruptible(dev,
+			AB8500_RTC, AB8500_RTC_READ_REQ_REG, &value);
+		if (retval < 0)
+			return retval;
+
+		if (!(value & RTC_READ_REQUEST))
+			break;
+
+		usleep_range(1000, 5000);
 	}
 
 	/* Read the Watchtime registers */
@@ -224,8 +219,8 @@ static int ab8500_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 {
 	int retval, i;
 	unsigned char buf[ARRAY_SIZE(ab8500_rtc_alarm_regs)];
-	unsigned long mins, secs = 0;
-
+	unsigned long mins, secs = 0, cursec=0;
+	struct rtc_time curtm;
 	if (alarm->time.tm_year < (AB8500_RTC_EPOCH - 1900)) {
 		dev_dbg(dev, "year should be equal to or greater than %d\n",
 				AB8500_RTC_EPOCH);
@@ -235,14 +230,36 @@ static int ab8500_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 	/* Get the number of seconds since 1970 */
 	rtc_tm_to_time(&alarm->time, &secs);
 
+	/* Check whether alarm is set less than 1min.
+	 * Since our RTC doesn't support alarm resolution less than 1min,
+	 * return -EINVAL, so UIE EMUL can take it up, incase of UIE_ON
+	 */
+	ab8500_rtc_read_time(dev, &curtm); /* Read current time */
+	rtc_tm_to_time(&curtm, &cursec);
+	if ((secs - cursec) < 59) {
+		dev_dbg(dev, "Alarm less than 1 minute not supported\n");
+		return -EINVAL;
+	}
+
 	/*
 	 * Convert it to the number of seconds since 01-01-2000 00:00:00, since
 	 * we only have a small counter in the RTC.
 	 */
 	secs -= get_elapsed_seconds(AB8500_RTC_EPOCH);
 
+#ifndef CONFIG_ANDROID
+	secs += 30; /* Round to nearest minute */
+#endif
+
 	mins = secs / 60;
 
+#ifdef CONFIG_ANDROID
+	/*
+	 * Needed due to Android believes all hw have a wake-up resolution
+	 * in seconds.
+	 */
+	mins++;
+#endif
 	buf[2] = mins & 0xFF;
 	buf[1] = (mins >> 8) & 0xFF;
 	buf[0] = (mins >> 16) & 0xFF;
diff --git a/drivers/rtc/rtc-pl031.c b/drivers/rtc/rtc-pl031.c
index f027c063fb2..cc0533994f6 100644
--- a/drivers/rtc/rtc-pl031.c
+++ b/drivers/rtc/rtc-pl031.c
@@ -220,17 +220,9 @@ static irqreturn_t pl031_interrupt(int irq, void *dev_id)
 	unsigned long events = 0;
 
 	rtcmis = readl(ldata->base + RTC_MIS);
-	if (rtcmis) {
-		writel(rtcmis, ldata->base + RTC_ICR);
-
-		if (rtcmis & RTC_BIT_AI)
-			events |= (RTC_AF | RTC_IRQF);
-
-		/* Timer interrupt is only available in ST variants */
-		if ((rtcmis & RTC_BIT_PI) &&
-			(ldata->hw_designer == AMBA_VENDOR_ST))
-			events |= (RTC_PF | RTC_IRQF);
-
+	if (rtcmis & RTC_BIT_AI) {
+		writel(RTC_BIT_AI, ldata->base + RTC_ICR);
+		events |= (RTC_AF | RTC_IRQF);
 		rtc_update_irq(ldata->rtc, 1, events);
 
 		return IRQ_HANDLED;
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 00c024039c9..b8aa82fa2a6 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -113,6 +113,20 @@ config SPI_BITBANG
 	  need it.  You only need to select this explicitly to support driver
 	  modules that aren't part of this kernel tree.
 
+config STM_MSP_SPI
+	tristate "STM MSP CONTROLLER (SPI master)"
+	default y
+	help
+	  This enables using the STM MSP controller in master
+	  mode.
+
+config SPI_WORKQUEUE
+	bool "SPI_WORKQUEUE"
+	depends on STM_MSP_SPI
+	default n
+	help
+	  This feature allow SPI works to be deferred in MSP driver.
+
 config SPI_BUTTERFLY
 	tristate "Parallel port adapter for AVR Butterfly (DEVELOPMENT)"
 	depends on PARPORT
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index 9d75d2198ff..6c10ed18023 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -62,4 +62,5 @@ obj-$(CONFIG_SPI_TLE62X0)		+= spi-tle62x0.o
 obj-$(CONFIG_SPI_TOPCLIFF_PCH)		+= spi-topcliff-pch.o
 obj-$(CONFIG_SPI_TXX9)			+= spi-txx9.o
 obj-$(CONFIG_SPI_XILINX)		+= spi-xilinx.o
+obj-$(CONFIG_STM_MSP_SPI)               += stm_msp.o
 
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c
index 400ae2121a2..8bf53a76a33 100644
--- a/drivers/spi/spi-pl022.c
+++ b/drivers/spi/spi-pl022.c
@@ -489,6 +489,13 @@ static void giveback(struct pl022 *pl022)
 	pl022->cur_transfer = NULL;
 	pl022->cur_chip = NULL;
 	spi_finalize_current_message(pl022->master);
+
+	/* disable the SPI/SSP operation */
+	writew((readw(SSP_CR1(pl022->virtbase)) &
+		(~SSP_CR1_MASK_SSE)), SSP_CR1(pl022->virtbase));
+
+	/* This message is completed, so let's turn off the clocks & power */
+	pm_runtime_put(&pl022->adev->dev);
 }
 
 /**
@@ -895,6 +902,12 @@ static int configure_dma(struct pl022 *pl022)
 	struct dma_async_tx_descriptor *rxdesc;
 	struct dma_async_tx_descriptor *txdesc;
 
+	/* DMA burstsize should be same as the FIFO trigger level */
+	rx_conf.src_maxburst = pl022->rx_lev_trig ? 1 <<
+		(pl022->rx_lev_trig + 1) : pl022->rx_lev_trig;
+	tx_conf.dst_maxburst = pl022->tx_lev_trig ? 1 <<
+		(pl022->tx_lev_trig + 1) : pl022->tx_lev_trig;
+
 	/* Check that the channels are available */
 	if (!rxchan || !txchan)
 		return -ENODEV;
@@ -2048,6 +2061,9 @@ pl022_probe(struct amba_device *adev, const struct amba_id *id)
 	printk(KERN_INFO "pl022: mapped registers from 0x%08x to %p\n",
 	       adev->res.start, pl022->virtbase);
 
+	pm_runtime_enable(dev);
+	pm_runtime_resume(dev);
+
 	pl022->clk = clk_get(&adev->dev, NULL);
 	if (IS_ERR(pl022->clk)) {
 		status = PTR_ERR(pl022->clk);
@@ -2158,6 +2174,7 @@ pl022_remove(struct amba_device *adev)
 	clk_disable(pl022->clk);
 	clk_unprepare(pl022->clk);
 	clk_put(pl022->clk);
+	pm_runtime_disable(&adev->dev);
 	iounmap(pl022->virtbase);
 	amba_release_regions(adev);
 	tasklet_disable(&pl022->pump_transfers);
diff --git a/drivers/spi/stm_msp.c b/drivers/spi/stm_msp.c
new file mode 100644
index 00000000000..65dc316b37b
--- /dev/null
+++ b/drivers/spi/stm_msp.c
@@ -0,0 +1,1929 @@
+/*
+ * drivers/spi/stm_msp.c
+ *
+ * Copyright (C) 2010 STMicroelectronics Pvt. Ltd.
+ *
+ * Author: Sachin Verma <sachin.verma@st.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/ioport.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/spi/spi.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/amba/bus.h>
+#include <linux/spi/stm_msp.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/interrupt.h>
+
+/**
+ * MSP Controller Register Offsets
+ */
+#define MSP_DR(r)			(r + 0x000)
+#define MSP_GCR(r)			(r + 0x004)
+#define MSP_TCF(r)			(r + 0x008)
+#define MSP_RCF(r)			(r + 0x00C)
+#define MSP_SRG(r)			(r + 0x010)
+#define MSP_FLR(r)			(r + 0x014)
+#define MSP_DMACR(r)			(r + 0x018)
+#define MSP_IMSC(r)			(r + 0x020)
+#define MSP_RIS(r)			(r + 0x024)
+#define MSP_MIS(r)			(r + 0x028)
+#define MSP_ICR(r)			(r + 0x02C)
+#define MSP_MCR(r)			(r + 0x030)
+#define MSP_RCV(r)			(r + 0x034)
+#define MSP_RCM(r)			(r + 0x038)
+#define MSP_TCE0(r)			(r + 0x040)
+#define MSP_TCE1(r)			(r + 0x044)
+#define MSP_TCE2(r)			(r + 0x048)
+#define MSP_TCE3(r)			(r + 0x04C)
+#define MSP_RCE0(r)			(r + 0x060)
+#define MSP_RCE1(r)			(r + 0x064)
+#define MSP_RCE2(r)			(r + 0x068)
+#define MSP_RCE3(r)			(r + 0x06C)
+#define MSP_PID0(r)			(r + 0xFE0)
+#define MSP_PID1(r)			(r + 0xFE4)
+#define MSP_PID2(r)			(r + 0xFE8)
+#define MSP_PID3(r)			(r + 0xFEC)
+
+/**
+ * MSP Global Configuration Register - MSP_GCR
+ */
+#define MSP_GCR_MASK_RXEN		((u32)(0x1UL << 0))
+#define MSP_GCR_MASK_RFFEN		((u32)(0x1UL << 1))
+#define MSP_GCR_MASK_RFSPOL		((u32)(0x1UL << 2))
+#define MSP_GCR_MASK_DCM		((u32)(0x1UL << 3))
+#define MSP_GCR_MASK_RFSSEL		((u32)(0x1UL << 4))
+#define MSP_GCR_MASK_RCKPOL		((u32)(0x1UL << 5))
+#define MSP_GCR_MASK_RCKSEL		((u32)(0x1UL << 6))
+#define MSP_GCR_MASK_LBM		((u32)(0x1UL << 7))
+#define MSP_GCR_MASK_TXEN		((u32)(0x1UL << 8))
+#define MSP_GCR_MASK_TFFEN		((u32)(0x1UL << 9))
+#define MSP_GCR_MASK_TFSPOL		((u32)(0x1UL << 10))
+#define MSP_GCR_MASK_TFSSEL		((u32)(0x3UL << 11))
+#define MSP_GCR_MASK_TCKPOL		((u32)(0x1UL << 13))
+#define MSP_GCR_MASK_TCKSEL		((u32)(0x1UL << 14))
+#define MSP_GCR_MASK_TXDDL		((u32)(0x1UL << 15))
+#define MSP_GCR_MASK_SGEN		((u32)(0x1UL << 16))
+#define MSP_GCR_MASK_SCKPOL		((u32)(0x1UL << 17))
+#define MSP_GCR_MASK_SCKSEL		((u32)(0x3UL << 18))
+#define MSP_GCR_MASK_FGEN		((u32)(0x1UL << 20))
+#define MSP_GCR_MASK_SPICKM		((u32)(0x3UL << 21))
+#define MSP_GCR_MASK_SPIBME		((u32)(0x1UL << 23))
+
+/**
+ * MSP Transmit Configuration Register - MSP_TCF
+ */
+#define MSP_TCF_MASK_TP1ELEN		((u32)(0x7UL << 0))
+#define MSP_TCF_MASK_TP1FLEN		((u32)(0x7FUL << 3))
+#define MSP_TCF_MASK_TDTYP		((u32)(0x3UL << 10))
+#define MSP_TCF_MASK_TENDN		((u32)(0x1UL << 12))
+#define MSP_TCF_MASK_TDDLY		((u32)(0x3UL << 13))
+#define MSP_TCF_MASK_TFSIG		((u32)(0x1UL << 15))
+#define MSP_TCF_MASK_TP2ELEN		((u32)(0x7UL << 16))
+#define MSP_TCF_MASK_TP2FLEN		((u32)(0x7FUL << 19))
+#define MSP_TCF_MASK_TP2SM		((u32)(0x1UL << 26))
+#define MSP_TCF_MASK_TP2EN		((u32)(0x1UL << 27))
+#define MSP_TCF_MASK_TBSWAP		((u32)(0x3UL << 28))
+
+/**
+ * MSP Receive Configuration Register - MSP_RCF
+ */
+#define MSP_RCF_MASK_RP1ELEN		((u32)(0x7UL << 0))
+#define MSP_RCF_MASK_RP1FLEN		((u32)(0x7FUL << 3))
+#define MSP_RCF_MASK_RDTYP		((u32)(0x3UL << 10))
+#define MSP_RCF_MASK_RENDN		((u32)(0x1UL << 12))
+#define MSP_RCF_MASK_RDDLY		((u32)(0x3UL << 13))
+#define MSP_RCF_MASK_RFSIG		((u32)(0x1UL << 15))
+#define MSP_RCF_MASK_RP2ELEN		((u32)(0x7UL << 16))
+#define MSP_RCF_MASK_RP2FLEN		((u32)(0x7FUL << 19))
+#define MSP_RCF_MASK_RP2SM		((u32)(0x1UL << 26))
+#define MSP_RCF_MASK_RP2EN		((u32)(0x1UL << 27))
+#define MSP_RCF_MASK_RBSWAP		((u32)(0x3UL << 28))
+
+/**
+ * MSP Sample Rate Generator Register - MSP_SRG
+ */
+#define MSP_SRG_MASK_SCKDIV		((u32)(0x3FFUL << 0))
+#define MSP_SRG_MASK_FRWID		((u32)(0x3FUL << 10))
+#define MSP_SRG_MASK_FRPER		((u32)(0x1FFFUL << 16))
+
+/**
+ * MSP Flag Register - MSP_FLR
+ */
+#define MSP_FLR_MASK_RBUSY		((u32)(0x1UL << 0))
+#define MSP_FLR_MASK_RFE		((u32)(0x1UL << 1))
+#define MSP_FLR_MASK_RFU		((u32)(0x1UL << 2))
+#define MSP_FLR_MASK_TBUSY		((u32)(0x1UL << 3))
+#define MSP_FLR_MASK_TFE		((u32)(0x1UL << 4))
+#define MSP_FLR_MASK_TFU		((u32)(0x1UL << 5))
+
+/**
+ * MSP DMA Control Register - MSP_DMACR
+ */
+#define MSP_DMACR_MASK_RDMAE		((u32)(0x1UL << 0))
+#define MSP_DMACR_MASK_TDMAE		((u32)(0x1UL << 1))
+
+/**
+ * MSP Interrupt Mask Set/Clear Register - MSP_IMSC
+ */
+#define MSP_IMSC_MASK_RXIM		((u32)(0x1UL << 0))
+#define MSP_IMSC_MASK_ROEIM		((u32)(0x1UL << 1))
+#define MSP_IMSC_MASK_RSEIM		((u32)(0x1UL << 2))
+#define MSP_IMSC_MASK_RFSIM		((u32)(0x1UL << 3))
+#define MSP_IMSC_MASK_TXIM		((u32)(0x1UL << 4))
+#define MSP_IMSC_MASK_TUEIM		((u32)(0x1UL << 5))
+#define MSP_IMSC_MASK_TSEIM		((u32)(0x1UL << 6))
+#define MSP_IMSC_MASK_TFSIM		((u32)(0x1UL << 7))
+#define MSP_IMSC_MASK_RFOIM		((u32)(0x1UL << 8))
+#define MSP_IMSC_MASK_TFOIM		((u32)(0x1UL << 9))
+
+/**
+ * MSP Raw Interrupt status Register - MSP_RIS
+ */
+#define MSP_RIS_MASK_RXRIS		((u32)(0x1UL << 0))
+#define MSP_RIS_MASK_ROERIS		((u32)(0x1UL << 1))
+#define MSP_RIS_MASK_RSERIS		((u32)(0x1UL << 2))
+#define MSP_RIS_MASK_RFSRIS		((u32)(0x1UL << 3))
+#define MSP_RIS_MASK_TXRIS		((u32)(0x1UL << 4))
+#define MSP_RIS_MASK_TUERIS		((u32)(0x1UL << 5))
+#define MSP_RIS_MASK_TSERIS		((u32)(0x1UL << 6))
+#define MSP_RIS_MASK_TFSRIS		((u32)(0x1UL << 7))
+#define MSP_RIS_MASK_RFORIS		((u32)(0x1UL << 8))
+#define MSP_RIS_MASK_TFORIS		((u32)(0x1UL << 9))
+
+/**
+ * MSP Masked Interrupt status Register - MSP_MIS
+ */
+#define MSP_MIS_MASK_RXMIS		((u32)(0x1UL << 0))
+#define MSP_MIS_MASK_ROEMIS		((u32)(0x1UL << 1))
+#define MSP_MIS_MASK_RSEMIS		((u32)(0x1UL << 2))
+#define MSP_MIS_MASK_RFSMIS		((u32)(0x1UL << 3))
+#define MSP_MIS_MASK_TXMIS		((u32)(0x1UL << 4))
+#define MSP_MIS_MASK_TUEMIS		((u32)(0x1UL << 5))
+#define MSP_MIS_MASK_TSEMIS		((u32)(0x1UL << 6))
+#define MSP_MIS_MASK_TFSMIS		((u32)(0x1UL << 7))
+#define MSP_MIS_MASK_RFOMIS		((u32)(0x1UL << 8))
+#define MSP_MIS_MASK_TFOMIS		((u32)(0x1UL << 9))
+
+/**
+ * MSP Interrupt Clear Register - MSP_ICR
+ */
+#define MSP_ICR_MASK_ROEIC		((u32)(0x1UL << 1))
+#define MSP_ICR_MASK_RSEIC		((u32)(0x1UL << 2))
+#define MSP_ICR_MASK_RFSIC		((u32)(0x1UL << 3))
+#define MSP_ICR_MASK_TUEIC		((u32)(0x1UL << 5))
+#define MSP_ICR_MASK_TSEIC		((u32)(0x1UL << 6))
+#define MSP_ICR_MASK_TFSIC		((u32)(0x1UL << 7))
+
+#define GEN_MASK_BITS(val, mask, sb)	((u32)((((u32)val) << (sb)) & (mask)))
+#define MSP_WBITS(reg, val, mask, sb)	((reg) = (((reg) & ~(mask)) |\
+					(((val) << (sb)) & (mask))))
+#define DEFAULT_MSP_REG_DMACR		0x00000000
+#define DEFAULT_MSP_REG_SRG		0x1FFF0000
+
+#define DEFAULT_MSP_REG_GCR	( \
+	GEN_MASK_BITS(MSP_RECEIVER_DISABLED, MSP_GCR_MASK_RXEN, 0)	|\
+	GEN_MASK_BITS(MSP_RX_FIFO_ENABLED, MSP_GCR_MASK_RFFEN, 1)	|\
+	GEN_MASK_BITS(MSP_LOOPBACK_DISABLED, MSP_GCR_MASK_LBM, 7)	|\
+	GEN_MASK_BITS(MSP_TRANSMITTER_DISABLED, MSP_GCR_MASK_TXEN, 8)	|\
+	GEN_MASK_BITS(MSP_TX_FIFO_ENABLED, MSP_GCR_MASK_TFFEN, 9)	|\
+	GEN_MASK_BITS(MSP_TX_FRAME_SYNC_POL_LOW, MSP_GCR_MASK_TFSPOL, 10)|\
+	GEN_MASK_BITS(MSP_TX_FRAME_SYNC_INT, MSP_GCR_MASK_TFSSEL, 11)	|\
+	GEN_MASK_BITS(MSP_TX_CLOCK_POL_HIGH, MSP_GCR_MASK_TCKPOL, 13)	|\
+	GEN_MASK_BITS(MSP_IS_SPI_MASTER, MSP_GCR_MASK_TCKSEL, 14)	|\
+	GEN_MASK_BITS(MSP_TRANSMIT_DATA_WITHOUT_DELAY, MSP_GCR_MASK_TXDDL, 15)|\
+	GEN_MASK_BITS(MSP_SAMPLE_RATE_GEN_ENABLE, MSP_GCR_MASK_SGEN, 16)|\
+	GEN_MASK_BITS(MSP_CLOCK_INTERNAL, MSP_GCR_MASK_SCKSEL, 18)	|\
+	GEN_MASK_BITS(MSP_FRAME_GEN_ENABLE, MSP_GCR_MASK_FGEN, 20)	|\
+	GEN_MASK_BITS(MSP_SPI_PHASE_ZERO_CYCLE_DELAY, MSP_GCR_MASK_SPICKM, 21)|\
+	GEN_MASK_BITS(SPI_BURST_MODE_DISABLE, MSP_GCR_MASK_SPIBME, 23)\
+	)
+#define DEFAULT_MSP_REG_RCF	( \
+	GEN_MASK_BITS(MSP_DATA_BITS_32, MSP_RCF_MASK_RP1ELEN, 0) | \
+	GEN_MASK_BITS(MSP_IGNORE_RX_FRAME_SYNC_PULSE, MSP_RCF_MASK_RFSIG, 15) |\
+	GEN_MASK_BITS(MSP_RX_1BIT_DATA_DELAY, MSP_RCF_MASK_RDDLY, 13) | \
+	GEN_MASK_BITS(MSP_RX_ENDIANESS_LSB, MSP_RCF_MASK_RENDN, 12)  \
+	)
+
+#define DEFAULT_MSP_REG_TCF	( \
+	GEN_MASK_BITS(MSP_DATA_BITS_32, MSP_TCF_MASK_TP1ELEN, 0) | \
+	GEN_MASK_BITS(MSP_IGNORE_TX_FRAME_SYNC_PULSE, MSP_TCF_MASK_TFSIG, 15) |\
+	GEN_MASK_BITS(MSP_TX_1BIT_DATA_DELAY, MSP_TCF_MASK_TDDLY, 13) | \
+	GEN_MASK_BITS(MSP_TX_ENDIANESS_LSB, MSP_TCF_MASK_TENDN, 12)  \
+	)
+
+/**
+ * MSP Receiver/Transmitter states (enabled or disabled)
+ */
+#define MSP_RECEIVER_DISABLED		0
+#define MSP_RECEIVER_ENABLED		1
+#define MSP_TRANSMITTER_DISABLED	0
+#define MSP_TRANSMITTER_ENABLED		1
+
+/**
+ * MSP Receiver/Transmitter FIFO constants
+ */
+#define MSP_LOOPBACK_DISABLED		0
+#define MSP_LOOPBACK_ENABLED		1
+
+#define MSP_TX_FIFO_DISABLED		0
+#define MSP_TX_FIFO_ENABLED		1
+#define MSP_TX_ENDIANESS_MSB		0
+#define MSP_TX_ENDIANESS_LSB		1
+
+#define MSP_RX_FIFO_DISABLED		0
+#define MSP_RX_FIFO_ENABLED		1
+#define MSP_RX_ENDIANESS_MSB		0
+#define MSP_RX_ENDIANESS_LSB		1
+
+#define MSP_TX_FRAME_SYNC_EXT		0x0
+#define MSP_TX_FRAME_SYNC_INT		0x2
+#define MSP_TX_FRAME_SYNC_INT_CFG	0x3
+
+#define MSP_TX_FRAME_SYNC_POL_HIGH	0
+#define MSP_TX_FRAME_SYNC_POL_LOW	1
+
+#define MSP_HANDLE_RX_FRAME_SYNC_PULSE	0
+#define MSP_IGNORE_RX_FRAME_SYNC_PULSE	1
+
+#define MSP_RX_NO_DATA_DELAY		0x0
+#define MSP_RX_1BIT_DATA_DELAY		0x1
+#define MSP_RX_2BIT_DATA_DELAY		0x2
+#define MSP_RX_3BIT_DATA_DELAY		0x3
+
+#define MSP_HANDLE_TX_FRAME_SYNC_PULSE	0
+#define MSP_IGNORE_TX_FRAME_SYNC_PULSE	1
+
+#define MSP_TX_NO_DATA_DELAY		0x0
+#define MSP_TX_1BIT_DATA_DELAY		0x1
+#define MSP_TX_2BIT_DATA_DELAY		0x2
+#define MSP_TX_3BIT_DATA_DELAY		0x3
+
+#define MSP_TX_CLOCK_POL_LOW		0
+#define MSP_TX_CLOCK_POL_HIGH		1
+
+#define MSP_SPI_PHASE_ZERO_CYCLE_DELAY	0x2
+#define MSP_SPI_PHASE_HALF_CYCLE_DELAY	0x3
+
+#define MSP_IS_SPI_SLAVE		0
+#define MSP_IS_SPI_MASTER		1
+
+#define MSP_FRAME_GEN_DISABLE		0
+#define MSP_FRAME_GEN_ENABLE		1
+
+#define MSP_SAMPLE_RATE_GEN_DISABLE	0
+#define MSP_SAMPLE_RATE_GEN_ENABLE	1
+
+#define SPI_BURST_MODE_DISABLE		0
+#define SPI_BURST_MODE_ENABLE		1
+
+#define MSP_TRANSMIT_DATA_WITHOUT_DELAY	0
+#define MSP_TRANSMIT_DATA_WITH_DELAY	1
+
+#define MSP_CLOCK_INTERNAL		0x0 /* 48 MHz */
+
+/* SRG is derived from MSPSCK pin but is resynchronized on MSPRFS
+ * (Receive Frame Sync signal) */
+#define MSP_CLOCK_EXTERNAL		0x2
+#define MSP_CLOCK_EXTERNAL_RESYNC	0x3
+
+#define DISABLE_ALL_MSP_INTERRUPTS	(0x0)
+#define ENABLE_ALL_MSP_INTERRUPTS	(0x333)
+#define CLEAR_ALL_MSP_INTERRUPTS	(0xEE)
+#define DEFAULT_MSP_CLK			(48000000)
+#define MAX_SCKDIV			(1023)
+
+#define MSP_FIFO_DEPTH			8
+
+/**
+ * Queue State
+ */
+#define QUEUE_RUNNING			(0)
+#define QUEUE_STOPPED			(1)
+
+#define START_STATE			((void *)0)
+#define RUNNING_STATE			((void *)1)
+#define DONE_STATE			((void *)2)
+#define ERROR_STATE			((void *)-1)
+
+/* Default values */
+#define SPI_DEFAULT_MAX_SPEED_HZ	48000
+#define SPI_TRANSFER_TIMEOUT_MS		5000
+
+/* CONTROLLER COMMANDS */
+enum cntlr_commands {
+	DISABLE_CONTROLLER = 0,
+	ENABLE_CONTROLLER ,
+	DISABLE_ALL_INTERRUPT ,
+	ENABLE_ALL_INTERRUPT ,
+	FLUSH_FIFO ,
+	RESTORE_STATE ,
+	LOAD_DEFAULT_CONFIG ,
+	CLEAR_ALL_INTERRUPT,
+};
+
+struct stm_msp {
+	struct amba_device		*adev;
+	struct spi_master		*master;
+	struct stm_msp_controller	*master_info;
+	void __iomem			*regs;
+	struct clk			*clk;
+#ifdef CONFIG_SPI_WORKQUEUE
+	struct workqueue_struct		*workqueue;
+#endif
+	struct work_struct		spi_work;
+	spinlock_t			lock;
+	struct list_head		queue;
+	int				busy;
+	int				run;
+	struct tasklet_struct		pump_transfers;
+	struct timer_list		spi_notify_timer;
+	int				spi_io_error;
+	struct spi_message		*cur_msg;
+	struct spi_transfer		*cur_transfer;
+	struct chip_data		*cur_chip;
+	void				*tx;
+	void				*tx_end;
+	void				*rx;
+	void				*rx_end;
+	void				(*write)(struct stm_msp *stm_msp);
+	void				(*read)(struct stm_msp *stm_msp);
+	void				(*delay)(struct stm_msp *stm_msp);
+};
+
+/**
+ * struct chip_data - To maintain runtime state of SPICntlr for each client chip
+ * @ctr_regs: void pointer which is assigned a struct having regs of the cntlr.
+ * @chip_id: Chip Id assigned to this client to identify it.
+ * @n_bytes: how many bytes(power of 2) reqd for a given data width of client
+ * @write: function to be used to write when doing xfer for this chip
+ * @null_write: function to be used for dummy write for receiving data.
+ * @read: function to be used to read when doing xfer for this chip
+ * @null_read: function to be used to for dummy read while writting data.
+ * @cs_control: chip select callback provided by chip
+ * @xfer_type: polling/interrupt
+ *
+ * Runtime state of the SPI controller, maintained per chip,
+ * This would be set according to the current message that would be served
+ */
+struct chip_data {
+	void *ctr_regs;
+	u32 chip_id;
+	u8 n_bytes;
+	void (*write) (struct stm_msp *stm_msp);
+	void (*null_write) (struct stm_msp *stm_msp);
+	void (*read) (struct stm_msp *stm_msp);
+	void (*null_read) (struct stm_msp *stm_msp);
+	void (*delay) (struct stm_msp *stm_msp);
+	void (*cs_control) (u32 command);
+	int xfer_type;
+};
+
+/**
+ * struct msp_regs - Used to store MSP controller registry values
+ *		used by the driver.
+ * @gcr: global configuration register
+ * @tcf: transmit configuration register
+ * @rcf: receive configuration register
+ * @srg: sample rate generator register
+ * @dmacr: DMA configuration register
+ */
+struct msp_regs {
+	u32 gcr;
+	u32 tcf;
+	u32 rcf;
+	u32 srg;
+	u32 dmacr;
+};
+
+/**
+ * stm_msp_controller_cmd - To execute controller commands for MSP
+ * @stm_msp: SPI driver private data structure
+ * @cmd: Command which is to be executed on the controller
+ */
+static int stm_msp_controller_cmd(struct stm_msp *stm_msp, int cmd)
+{
+	int retval = 0;
+	struct msp_regs *msp_regs = NULL;
+
+	switch (cmd) {
+	case DISABLE_CONTROLLER: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Disabling MSP controller...\n");
+			writel((readl(MSP_GCR(stm_msp->regs)) &
+				(~(MSP_GCR_MASK_TXEN | MSP_GCR_MASK_RXEN))),
+				MSP_GCR(stm_msp->regs));
+			break;
+		}
+	case ENABLE_CONTROLLER: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Enabling MSP controller...\n");
+			writel((readl(MSP_GCR(stm_msp->regs)) |
+				(MSP_GCR_MASK_TXEN | MSP_GCR_MASK_RXEN)),
+				MSP_GCR(stm_msp->regs));
+			break;
+		}
+	case DISABLE_ALL_INTERRUPT: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Disabling all MSP interrupts...\n");
+			writel(DISABLE_ALL_MSP_INTERRUPTS,
+				MSP_IMSC(stm_msp->regs));
+			break;
+		}
+	case ENABLE_ALL_INTERRUPT: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Enabling all MSP interrupts...\n");
+			writel(ENABLE_ALL_MSP_INTERRUPTS,
+				MSP_IMSC(stm_msp->regs));
+			break;
+		}
+	case CLEAR_ALL_INTERRUPT: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Clearing all MSP interrupts...\n");
+			writel(CLEAR_ALL_MSP_INTERRUPTS,
+				MSP_ICR(stm_msp->regs));
+			break;
+		}
+	case FLUSH_FIFO: {
+			unsigned long limit = loops_per_jiffy << 1;
+
+			dev_dbg(&stm_msp->adev->dev, "MSP FIFO flushed\n");
+
+			do {
+				while (!(readl(MSP_FLR(stm_msp->regs)) &
+					MSP_FLR_MASK_RFE)) {
+					readl(MSP_DR(stm_msp->regs));
+				}
+			} while ((readl(MSP_FLR(stm_msp->regs)) &
+				(MSP_FLR_MASK_TBUSY | MSP_FLR_MASK_RBUSY)) &&
+				limit--);
+
+			retval = limit;
+			break;
+		}
+	case RESTORE_STATE: {
+			msp_regs =
+				(struct msp_regs *)stm_msp->cur_chip->ctr_regs;
+
+			dev_dbg(&stm_msp->adev->dev,
+				"Restoring MSP state...\n");
+
+			writel(msp_regs->gcr, MSP_GCR(stm_msp->regs));
+			writel(msp_regs->tcf, MSP_TCF(stm_msp->regs));
+			writel(msp_regs->rcf, MSP_RCF(stm_msp->regs));
+			writel(msp_regs->srg, MSP_SRG(stm_msp->regs));
+			writel(msp_regs->dmacr, MSP_DMACR(stm_msp->regs));
+			writel(DISABLE_ALL_MSP_INTERRUPTS,
+			       MSP_IMSC(stm_msp->regs));
+			writel(CLEAR_ALL_MSP_INTERRUPTS,
+			       MSP_ICR(stm_msp->regs));
+			break;
+		}
+	case LOAD_DEFAULT_CONFIG: {
+			dev_dbg(&stm_msp->adev->dev,
+				"Loading default MSP config...\n");
+
+			writel(DEFAULT_MSP_REG_GCR, MSP_GCR(stm_msp->regs));
+			writel(DEFAULT_MSP_REG_TCF, MSP_TCF(stm_msp->regs));
+			writel(DEFAULT_MSP_REG_RCF, MSP_RCF(stm_msp->regs));
+			writel(DEFAULT_MSP_REG_SRG, MSP_SRG(stm_msp->regs));
+			writel(DEFAULT_MSP_REG_DMACR, MSP_DMACR(stm_msp->regs));
+			writel(DISABLE_ALL_MSP_INTERRUPTS,
+				MSP_IMSC(stm_msp->regs));
+			writel(CLEAR_ALL_MSP_INTERRUPTS,
+			       MSP_ICR(stm_msp->regs));
+			break;
+		}
+	default:
+		dev_dbg(&stm_msp->adev->dev, "Unknown command\n");
+		retval = -1;
+		break;
+	}
+
+	return retval;
+}
+
+/**
+ * giveback - current spi_message is over, schedule next spi_message
+ * @message: current SPI message
+ * @stm_msp: spi driver private data structure
+ *
+ * current spi_message is over, schedule next spi_message and call
+ * callback of this msg.
+ */
+static void giveback(struct spi_message *message, struct stm_msp *stm_msp)
+{
+	struct spi_transfer *last_transfer;
+	unsigned long flags;
+	struct spi_message *msg;
+	void (*curr_cs_control)(u32 command);
+
+	spin_lock_irqsave(&stm_msp->lock, flags);
+	msg = stm_msp->cur_msg;
+
+	curr_cs_control = stm_msp->cur_chip->cs_control;
+
+	stm_msp->cur_msg = NULL;
+	stm_msp->cur_transfer = NULL;
+	stm_msp->cur_chip = NULL;
+#ifdef CONFIG_SPI_WORKQUEUE
+	queue_work(stm_msp->workqueue, &stm_msp->spi_work);
+#else
+	schedule_work(&stm_msp->spi_work);
+#endif
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+
+	last_transfer = list_entry(msg->transfers.prev,
+			struct spi_transfer, transfer_list);
+
+	if (!last_transfer->cs_change)
+		curr_cs_control(SPI_CHIP_DESELECT);
+
+	msg->state = NULL;
+
+	if (msg->complete)
+		msg->complete(msg->context);
+
+	stm_msp_controller_cmd(stm_msp, DISABLE_CONTROLLER);
+	clk_disable(stm_msp->clk);
+}
+
+/**
+ * spi_notify - Handles Polling hang issue over spi bus.
+ * @data: main driver data
+ * Context: Process.
+ *
+ * This is  used to handle error condition in transfer and receive function used
+ * in polling mode.
+ * Sometimes due to passing wrong protocol desc , polling transfer may hang.
+ * To prevent this, timer is added.
+ *
+ * Returns void.
+ */
+static void spi_notify(unsigned long data)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)data;
+	stm_msp->spi_io_error = 1;
+
+	dev_err(&stm_msp->adev->dev,
+		"Polling is taking time, maybe device not responding\n");
+
+	del_timer(&stm_msp->spi_notify_timer);
+}
+
+/**
+ * stm_msp_transfer - transfer function registered to SPI master framework
+ * @spi: spi device which is requesting transfer
+ * @msg: spi message which is to handled is queued to driver queue
+ *
+ * This function is registered to the SPI framework for this SPI master
+ * controller. It will queue the spi_message in the queue of driver if
+ * the queue is not stopped and return.
+ */
+static int stm_msp_transfer(struct spi_device *spi, struct spi_message *msg)
+{
+	struct stm_msp *stm_msp = spi_master_get_devdata(spi->master);
+	unsigned long flags;
+
+	spin_lock_irqsave(&stm_msp->lock, flags);
+
+	if (stm_msp->run == QUEUE_STOPPED) {
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		return -ESHUTDOWN;
+	}
+	dev_err(&spi->dev, "Regular request (No infinite DMA ongoing)\n");
+
+	msg->actual_length = 0;
+	msg->status = -EINPROGRESS;
+	msg->state = START_STATE;
+
+	list_add_tail(&msg->queue, &stm_msp->queue);
+
+	if ((stm_msp->run == QUEUE_RUNNING) && (!stm_msp->busy))
+#ifdef CONFIG_SPI_WORKQUEUE
+		queue_work(stm_msp->workqueue, &stm_msp->spi_work);
+#else
+		schedule_work(&stm_msp->spi_work);
+#endif
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+	return 0;
+}
+
+/**
+ * next_transfer - Move to the Next transfer in the current spi message
+ * @stm_msp: spi driver private data structure
+ *
+ * This function moves though the linked list of spi transfers in the
+ * current spi message and returns with the state of current spi
+ * message i.e whether its last transfer is done(DONE_STATE) or
+ * Next transfer is ready(RUNNING_STATE)
+ */
+static void *next_transfer(struct stm_msp *stm_msp)
+{
+	struct spi_message *msg = stm_msp->cur_msg;
+	struct spi_transfer *trans = stm_msp->cur_transfer;
+
+	/* Move to next transfer */
+	if (trans->transfer_list.next != &msg->transfers) {
+		stm_msp->cur_transfer = list_entry(trans->transfer_list.next,
+					struct spi_transfer,
+					transfer_list);
+		return RUNNING_STATE;
+	}
+	return DONE_STATE;
+}
+
+static void do_interrupt_transfer(void *data)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)data;
+
+	stm_msp->tx = (void *)stm_msp->cur_transfer->tx_buf;
+	stm_msp->tx_end = stm_msp->tx + stm_msp->cur_transfer->len;
+
+	stm_msp->rx = (void *)stm_msp->cur_transfer->rx_buf;
+	stm_msp->rx_end = stm_msp->rx + stm_msp->cur_transfer->len;
+
+	stm_msp->write = stm_msp->tx ?
+		stm_msp->cur_chip->write : stm_msp->cur_chip->null_write;
+	stm_msp->read = stm_msp->rx ?
+		stm_msp->cur_chip->read : stm_msp->cur_chip->null_read;
+
+	stm_msp->cur_chip->cs_control(SPI_CHIP_SELECT);
+
+	stm_msp_controller_cmd(stm_msp, ENABLE_ALL_INTERRUPT);
+	stm_msp_controller_cmd(stm_msp, ENABLE_CONTROLLER);
+}
+
+static void do_polling_transfer(void *data)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)data;
+	struct spi_message *message = NULL;
+	struct spi_transfer *transfer = NULL;
+	struct spi_transfer *previous = NULL;
+	struct chip_data *chip;
+	unsigned long limit = 0;
+	u32 timer_expire = 0;
+
+	chip = stm_msp->cur_chip;
+	message = stm_msp->cur_msg;
+
+	while (message->state != DONE_STATE) {
+		/* Handle for abort */
+		if (message->state == ERROR_STATE)
+			break;
+
+		transfer = stm_msp->cur_transfer;
+
+		/* Delay if requested at end of transfer */
+		if (message->state == RUNNING_STATE) {
+			previous = list_entry(transfer->transfer_list.prev,
+					struct spi_transfer,
+					transfer_list);
+
+			if (previous->delay_usecs)
+				udelay(previous->delay_usecs);
+
+			if (previous->cs_change)
+				stm_msp->cur_chip->cs_control(SPI_CHIP_SELECT);
+		} else {
+			/* START_STATE */
+			message->state = RUNNING_STATE;
+			stm_msp->cur_chip->cs_control(SPI_CHIP_SELECT);
+		}
+
+		/* Configuration Changing Per Transfer */
+		stm_msp->tx = (void *)transfer->tx_buf;
+		stm_msp->tx_end = stm_msp->tx + stm_msp->cur_transfer->len;
+		stm_msp->rx = (void *)transfer->rx_buf;
+		stm_msp->rx_end = stm_msp->rx + stm_msp->cur_transfer->len;
+
+		stm_msp->write = stm_msp->tx ?
+			stm_msp->cur_chip->write :
+				stm_msp->cur_chip->null_write;
+		stm_msp->read = stm_msp->rx ?
+			stm_msp->cur_chip->read :
+				stm_msp->cur_chip->null_read;
+		stm_msp->delay = stm_msp->cur_chip->delay;
+
+		stm_msp_controller_cmd(stm_msp, FLUSH_FIFO);
+		stm_msp_controller_cmd(stm_msp, ENABLE_CONTROLLER);
+
+		timer_expire = stm_msp->cur_transfer->len / 1024;
+
+		if (!timer_expire)
+			timer_expire = SPI_TRANSFER_TIMEOUT_MS;
+		else
+			timer_expire =
+				(stm_msp->cur_transfer->len / 1024) *
+				SPI_TRANSFER_TIMEOUT_MS;
+
+		stm_msp->spi_notify_timer.expires =
+			jiffies + msecs_to_jiffies(timer_expire);
+
+		add_timer(&stm_msp->spi_notify_timer);
+
+		dev_dbg(&stm_msp->adev->dev, "Polling transfer ongoing...\n");
+
+		while (stm_msp->tx < stm_msp->tx_end) {
+
+			stm_msp_controller_cmd(stm_msp, DISABLE_CONTROLLER);
+			stm_msp->read(stm_msp);
+			stm_msp->write(stm_msp);
+
+			stm_msp_controller_cmd(stm_msp, ENABLE_CONTROLLER);
+
+			if (stm_msp->delay)
+				stm_msp->delay(stm_msp);
+
+			if (stm_msp->spi_io_error == 1)
+				break;
+		}
+
+		del_timer(&stm_msp->spi_notify_timer);
+
+		if (stm_msp->spi_io_error == 1)
+			goto out;
+
+		limit = loops_per_jiffy << 1;
+
+		while ((stm_msp->rx < stm_msp->rx_end) && (limit--))
+			stm_msp->read(stm_msp);
+
+		/* Update total byte transfered */
+		message->actual_length += stm_msp->cur_transfer->len;
+
+		if (stm_msp->cur_transfer->cs_change)
+			stm_msp->cur_chip->cs_control(SPI_CHIP_DESELECT);
+
+		stm_msp_controller_cmd(stm_msp, DISABLE_CONTROLLER);
+
+		/* Move to next transfer */
+		message->state = next_transfer(stm_msp);
+	}
+out:
+	/* Handle end of message */
+	if (message->state == DONE_STATE)
+		message->status = 0;
+	else
+		message->status = -EIO;
+
+	giveback(message, stm_msp);
+
+	stm_msp->spi_io_error = 0; /* Reset state for further transfers */
+
+	return;
+}
+
+/**
+ * pump_messages - Workqueue function which processes spi message queue
+ * @work: pointer to work
+ *
+ * This function checks if there is any spi message in the queue that
+ * needs processing and delegate control to appropriate function
+ * do_polling_transfer()/do_interrupt_transfer()/do_dma_transfer()
+ * based on the kind of the transfer
+ *
+ */
+static void pump_messages(struct work_struct *work)
+{
+	struct stm_msp *stm_msp = container_of(work, struct stm_msp, spi_work);
+	unsigned long flags;
+
+	/* Lock queue and check for queue work */
+	spin_lock_irqsave(&stm_msp->lock, flags);
+
+	if (list_empty(&stm_msp->queue) || stm_msp->run == QUEUE_STOPPED) {
+		dev_dbg(&stm_msp->adev->dev, "work_queue: Queue Empty\n");
+		stm_msp->busy = 0;
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		return;
+	}
+	/* Make sure we are not already running a message */
+	if (stm_msp->cur_msg) {
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		return;
+	}
+
+	clk_enable(stm_msp->clk);
+
+	/* Extract head of queue */
+	stm_msp->cur_msg = list_entry(stm_msp->queue.next,
+				struct spi_message,
+				queue);
+
+	list_del_init(&stm_msp->cur_msg->queue);
+	stm_msp->busy = 1;
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+
+	/* Initial message state */
+	stm_msp->cur_msg->state = START_STATE;
+	stm_msp->cur_transfer = list_entry(stm_msp->cur_msg->transfers.next,
+				struct spi_transfer,
+				transfer_list);
+
+	/* Setup the SPI using the per chip configuration */
+	stm_msp->cur_chip = spi_get_ctldata(stm_msp->cur_msg->spi);
+	stm_msp_controller_cmd(stm_msp, RESTORE_STATE);
+	stm_msp_controller_cmd(stm_msp, FLUSH_FIFO);
+
+	if (stm_msp->cur_chip->xfer_type == SPI_POLLING_TRANSFER)
+		do_polling_transfer(stm_msp);
+	else if (stm_msp->cur_chip->xfer_type == SPI_INTERRUPT_TRANSFER)
+		do_interrupt_transfer(stm_msp);
+}
+
+/**
+ * pump_transfers - Tasklet function which schedules next interrupt xfer
+ * @data: spi driver private data structure
+ */
+static void pump_transfers(unsigned long data)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)data;
+	struct spi_message *message = NULL;
+	struct spi_transfer *transfer = NULL;
+	struct spi_transfer *previous = NULL;
+
+	message = stm_msp->cur_msg;
+
+	/* Handle for abort */
+	if (message->state == ERROR_STATE) {
+		message->status = -EIO;
+		giveback(message, stm_msp);
+		return;
+	}
+
+	/* Handle end of message */
+	if (message->state == DONE_STATE) {
+		message->status = 0;
+		giveback(message, stm_msp);
+		return;
+	}
+	transfer = stm_msp->cur_transfer;
+
+	/* Delay if requested at end of transfer */
+	if (message->state == RUNNING_STATE) {
+		previous = list_entry(transfer->transfer_list.prev,
+					struct spi_transfer, transfer_list);
+		if (previous->delay_usecs)
+			udelay(previous->delay_usecs);
+		if (previous->cs_change)
+			stm_msp->cur_chip->cs_control(SPI_CHIP_SELECT);
+	} else {
+		/* START_STATE */
+		message->state = RUNNING_STATE;
+	}
+	stm_msp->tx = (void *)transfer->tx_buf;
+	stm_msp->tx_end = stm_msp->tx + stm_msp->cur_transfer->len;
+	stm_msp->rx = (void *)transfer->rx_buf;
+	stm_msp->rx_end = stm_msp->rx + stm_msp->cur_transfer->len;
+
+	stm_msp->write = stm_msp->tx ?
+		stm_msp->cur_chip->write : stm_msp->cur_chip->null_write;
+	stm_msp->read = stm_msp->rx ?
+		stm_msp->cur_chip->read : stm_msp->cur_chip->null_read;
+
+	stm_msp_controller_cmd(stm_msp, FLUSH_FIFO);
+	stm_msp_controller_cmd(stm_msp, ENABLE_ALL_INTERRUPT);
+}
+
+static int init_queue(struct stm_msp *stm_msp)
+{
+	INIT_LIST_HEAD(&stm_msp->queue);
+	spin_lock_init(&stm_msp->lock);
+
+	stm_msp->run = QUEUE_STOPPED;
+	stm_msp->busy = 0;
+
+	tasklet_init(&stm_msp->pump_transfers, pump_transfers,
+			(unsigned long)stm_msp);
+	INIT_WORK(&stm_msp->spi_work, pump_messages);
+
+#ifdef CONFIG_SPI_WORKQUEUE
+	stm_msp->workqueue = create_singlethread_workqueue(
+		dev_name(&stm_msp->master->dev));
+
+	if (stm_msp->workqueue == NULL)
+		return -EBUSY;
+#endif /* CONFIG_SPI_WORKQUEUE */
+
+	init_timer(&stm_msp->spi_notify_timer);
+
+	stm_msp->spi_notify_timer.expires = jiffies + msecs_to_jiffies(1000);
+	stm_msp->spi_notify_timer.function = spi_notify;
+	stm_msp->spi_notify_timer.data = (unsigned long)stm_msp;
+
+	return 0;
+}
+
+static int start_queue(struct stm_msp *stm_msp)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&stm_msp->lock, flags);
+
+	if (stm_msp->run == QUEUE_RUNNING || stm_msp->busy) {
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		return -EBUSY;
+	}
+
+	stm_msp->run = QUEUE_RUNNING;
+	stm_msp->cur_msg = NULL;
+	stm_msp->cur_transfer = NULL;
+	stm_msp->cur_chip = NULL;
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+	return 0;
+}
+
+static int stop_queue(struct stm_msp *stm_msp)
+{
+	unsigned long flags;
+	unsigned limit = 500;
+	int status = 0;
+
+	spin_lock_irqsave(&stm_msp->lock, flags);
+
+	/* This is a bit lame, but is optimized for the common execution path.
+	 * A wait_queue on the stm_msp->busy could be used, but then the common
+	 * execution path (pump_messages) would be required to call wake_up or
+	 * friends on every SPI message. Do this instead */
+
+	stm_msp->run = QUEUE_STOPPED;
+
+	while (!list_empty(&stm_msp->queue) && stm_msp->busy && limit--) {
+		spin_unlock_irqrestore(&stm_msp->lock, flags);
+		msleep(10);
+		spin_lock_irqsave(&stm_msp->lock, flags);
+	}
+
+	if (!list_empty(&stm_msp->queue) || stm_msp->busy)
+		status = -EBUSY;
+
+	spin_unlock_irqrestore(&stm_msp->lock, flags);
+
+	return status;
+}
+
+static int destroy_queue(struct stm_msp *stm_msp)
+{
+	int status;
+
+	status = stop_queue(stm_msp);
+
+	if (status != 0)
+		return status;
+#ifdef CONFIG_SPI_WORKQUEUE
+	destroy_workqueue(stm_msp->workqueue);
+#endif
+	del_timer_sync(&stm_msp->spi_notify_timer);
+
+	return 0;
+}
+
+/**
+ * stm_msp_null_writer - To Write Dummy Data in Data register
+ * @stm_msp: spi driver private data structure
+ *
+ * This function is set as a write function for transfer which have
+ * Tx transfer buffer as NULL. It simply writes '0' in the Data
+ * register
+ */
+static void stm_msp_null_writer(struct stm_msp *stm_msp)
+{
+	u32 cur_write = 0;
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_TFU) ||
+			(stm_msp->tx >= stm_msp->tx_end))
+				return;
+
+		writel(0x0, MSP_DR(stm_msp->regs));
+		stm_msp->tx += (stm_msp->cur_chip->n_bytes);
+		cur_write++;
+
+		if (cur_write == 8)
+			return;
+	}
+}
+
+/**
+ * stm_msp_null_reader - To read data from Data register and discard it
+ * @stm_msp: spi driver private data structure
+ *
+ * This function is set as a reader function for transfer which have
+ * Rx Transfer buffer as null. Read Data is rejected
+ */
+static void stm_msp_null_reader(struct stm_msp *stm_msp)
+{
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_RFE) ||
+			(stm_msp->rx >= stm_msp->rx_end))
+			return;
+
+		readl(MSP_DR(stm_msp->regs));
+		stm_msp->rx += (stm_msp->cur_chip->n_bytes);
+	}
+}
+
+/**
+ * stm_msp_u8_writer - Write FIFO data in Data register as a 8 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function writes data in Tx FIFO till it is not full
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary write ptr tx in stm_msp which maintains
+ * current write position in transfer buffer. we do not write data more than
+ * FIFO depth
+ */
+void stm_msp_u8_writer(struct stm_msp *stm_msp)
+{
+	u32 cur_write = 0;
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_TFU) ||
+			(stm_msp->tx >= stm_msp->tx_end))
+			return;
+
+		writel((u32)(*(u8 *)(stm_msp->tx)), MSP_DR(stm_msp->regs));
+		stm_msp->tx += (stm_msp->cur_chip->n_bytes);
+		cur_write++;
+
+		if (cur_write == MSP_FIFO_DEPTH)
+			return;
+	}
+}
+
+/**
+ * stm_msp_u8_reader - Read FIFO data in Data register as a 8 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function reads data in Rx FIFO till it is not empty
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary Read ptr rx in stm_msp which maintains
+ * current read position in transfer buffer
+ */
+void stm_msp_u8_reader(struct stm_msp *stm_msp)
+{
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_RFE) ||
+			(stm_msp->rx >= stm_msp->rx_end))
+			return;
+
+		*(u8 *)(stm_msp->rx) = (u8)readl(MSP_DR(stm_msp->regs));
+		stm_msp->rx += (stm_msp->cur_chip->n_bytes);
+	}
+}
+
+/**
+ * stm_msp_u16_writer - Write FIFO data in Data register as a 16 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function writes data in Tx FIFO till it is not full
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary write ptr tx in stm_msp which maintains
+ * current write position in transfer buffer. we do not write data more than
+ * FIFO depth
+ */
+void stm_msp_u16_writer(struct stm_msp *stm_msp)
+{
+	u32 cur_write = 0;
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_TFU) ||
+			(stm_msp->tx >= stm_msp->tx_end))
+			return;
+
+		writel((u32)(*(u16 *)(stm_msp->tx)), MSP_DR(stm_msp->regs));
+		stm_msp->tx += (stm_msp->cur_chip->n_bytes);
+		cur_write++;
+
+		if (cur_write == MSP_FIFO_DEPTH)
+			return;
+	}
+}
+
+/**
+ * stm_msp_u16_reader - Read FIFO data in Data register as a 16 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function reads data in Rx FIFO till it is not empty
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary Read ptr rx in stm_msp which maintains
+ * current read position in transfer buffer
+ */
+void stm_msp_u16_reader(struct stm_msp *stm_msp)
+{
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_RFE) ||
+			(stm_msp->rx >= stm_msp->rx_end))
+			return;
+
+		*(u16 *)(stm_msp->rx) = (u16)readl(MSP_DR(stm_msp->regs));
+		stm_msp->rx += (stm_msp->cur_chip->n_bytes);
+	}
+}
+
+/**
+ * stm_msp_u32_writer - Write FIFO data in Data register as a 32 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function writes data in Tx FIFO till it is not full
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary write ptr tx in stm_msp which maintains
+ * current write position in transfer buffer. we do not write data more than
+ * FIFO depth
+ */
+void stm_msp_u32_writer(struct stm_msp *stm_msp)
+{
+	u32 cur_write = 0;
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_TFU) ||
+			(stm_msp->tx >= stm_msp->tx_end))
+			return;
+
+		/* Write Data to Data Register */
+		writel(*(u32 *)(stm_msp->tx), MSP_DR(stm_msp->regs));
+		stm_msp->tx += (stm_msp->cur_chip->n_bytes);
+		cur_write++;
+
+		if (cur_write == MSP_FIFO_DEPTH)
+			return;
+	}
+}
+
+/**
+ * stm_msp_u32_reader - Read FIFO data in Data register as a 32 Bit Data
+ * @stm_msp: spi driver private data structure
+ *
+ * This function reads data in Rx FIFO till it is not empty
+ * which is indicated by the status register or our transfer is complete.
+ * It also updates the temporary Read ptr rx in stm_msp which maintains
+ * current read position in transfer buffer
+ */
+void stm_msp_u32_reader(struct stm_msp *stm_msp)
+{
+	u32 status;
+
+	while (1) {
+		status = readl(MSP_FLR(stm_msp->regs));
+
+		if ((status & MSP_FLR_MASK_RFE) ||
+			(stm_msp->rx >= stm_msp->rx_end))
+			return;
+
+		*(u32 *)(stm_msp->rx) = readl(MSP_DR(stm_msp->regs));
+		stm_msp->rx += (stm_msp->cur_chip->n_bytes);
+	}
+}
+
+/**
+ * stm_msp_interrupt_handler - Interrupt hanlder function
+ */
+static irqreturn_t stm_msp_interrupt_handler(int irq, void *dev_id)
+{
+	struct stm_msp *stm_msp = (struct stm_msp *)dev_id;
+	struct spi_message *msg = stm_msp->cur_msg;
+	u32 irq_status = 0;
+	u32 flag = 0;
+
+	if (!msg) {
+		dev_err(&stm_msp->adev->dev,
+			"Bad message state in interrupt handler");
+		/* Never fail */
+		return IRQ_HANDLED;
+	}
+
+	/* Read the Interrupt Status Register */
+	irq_status = readl(MSP_MIS(stm_msp->regs));
+
+	if (irq_status) {
+		if (irq_status & MSP_MIS_MASK_ROEMIS) {	/* Overrun interrupt */
+			/* Bail out our Data has been corrupted */
+			dev_dbg(&stm_msp->adev->dev,
+				"Received ROR interrupt\n");
+
+			stm_msp_controller_cmd(stm_msp, DISABLE_ALL_INTERRUPT);
+			stm_msp_controller_cmd(stm_msp, CLEAR_ALL_INTERRUPT);
+			stm_msp_controller_cmd(stm_msp, DISABLE_CONTROLLER);
+			msg->state = ERROR_STATE;
+			tasklet_schedule(&stm_msp->pump_transfers);
+			return IRQ_HANDLED;
+		}
+
+		stm_msp->read(stm_msp);
+		stm_msp->write(stm_msp);
+
+		if ((stm_msp->tx == stm_msp->tx_end) && (flag == 0)) {
+			flag = 1;
+			/* Disable Transmit interrupt */
+			writel(readl(MSP_IMSC(stm_msp->regs)) &
+				(~MSP_IMSC_MASK_TXIM) & (~MSP_IMSC_MASK_TFOIM),
+			       (stm_msp->regs + 0x14));
+		}
+
+		/* Clearing any Xmit underrun error. Overrun already handled */
+		stm_msp_controller_cmd(stm_msp, CLEAR_ALL_INTERRUPT);
+
+		if (stm_msp->rx == stm_msp->rx_end) {
+			stm_msp_controller_cmd(stm_msp, DISABLE_ALL_INTERRUPT);
+			stm_msp_controller_cmd(stm_msp, CLEAR_ALL_INTERRUPT);
+
+			dev_dbg(&stm_msp->adev->dev,
+				"Interrupt transfer completed.\n");
+
+			/* Update total bytes transfered */
+			msg->actual_length += stm_msp->cur_transfer->len;
+
+			if (stm_msp->cur_transfer->cs_change)
+				stm_msp->cur_chip->cs_control(
+					SPI_CHIP_DESELECT);
+
+			/* Move to next transfer */
+			msg->state = next_transfer(stm_msp);
+			tasklet_schedule(&stm_msp->pump_transfers);
+			return IRQ_HANDLED;
+		}
+	}
+	return IRQ_HANDLED;
+}
+
+/**
+ * stm_msp_cleanup - cleanup function registered to SPI master framework
+ * @spi: spi device which is requesting cleanup
+ *
+ * This function is registered to the SPI framework for this SPI master
+ * controller. It will free the runtime state of chip.
+ */
+static void stm_msp_cleanup(struct spi_device *spi)
+{
+	struct chip_data *chip = spi_get_ctldata((struct spi_device *)spi);
+	struct stm_msp *stm_msp = spi_master_get_devdata(spi->master);
+	struct spi_master *master;
+	master = stm_msp->master;
+
+	if (chip) {
+		kfree(chip->ctr_regs);
+		kfree(chip);
+		spi_set_ctldata(spi, NULL);
+	}
+}
+
+/**
+ * null_cs_control - Dummy chip select function
+ * @command: select/delect the chip
+ *
+ * If no chip select function is provided by client this is used as dummy
+ * chip select
+ */
+static void null_cs_control(u32 command)
+{
+	/* Nothing to do */
+	(void)command;
+}
+
+static int verify_msp_controller_parameters(struct stm_msp_config_chip
+		*chip_info)
+{
+
+	/* FIXME: check clock params */
+	if ((chip_info->lbm != SPI_LOOPBACK_ENABLED) &&
+		(chip_info->lbm != SPI_LOOPBACK_DISABLED)) {
+		dev_dbg(chip_info->dev,
+			"Loopback Mode is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->hierarchy != SPI_MASTER) &&
+		(chip_info->hierarchy != SPI_SLAVE)) {
+		dev_dbg(chip_info->dev,
+			"hierarchy is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->endian_rx != SPI_FIFO_MSB) &&
+		(chip_info->endian_rx != SPI_FIFO_LSB)) {
+		dev_dbg(chip_info->dev,
+			"Rx FIFO endianess is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->endian_tx != SPI_FIFO_MSB) &&
+		(chip_info->endian_tx != SPI_FIFO_LSB)) {
+		dev_dbg(chip_info->dev,
+			"Tx FIFO endianess is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->data_size < MSP_DATA_BITS_8) ||
+		(chip_info->data_size > MSP_DATA_BITS_32)) {
+		dev_dbg(chip_info->dev,
+			"MSP DATA Size is configured incorrectly\n");
+		return -1;
+	}
+	if ((chip_info->com_mode != SPI_INTERRUPT_TRANSFER) &&
+		(chip_info->com_mode != SPI_POLLING_TRANSFER)) {
+		dev_dbg(chip_info->dev,
+			"Communication mode is configured incorrectly\n");
+		return -1;
+	}
+	if (((chip_info->proto_params).clk_phase !=
+	     SPI_CLK_ZERO_CYCLE_DELAY) &&
+		((chip_info->proto_params).clk_phase !=
+		SPI_CLK_HALF_CYCLE_DELAY)) {
+		dev_dbg(chip_info->dev,
+			"Clock Phase is configured incorrectly\n");
+		return -1;
+	}
+	if (((chip_info->proto_params).clk_pol !=
+	     SPI_CLK_POL_IDLE_LOW) &&
+		((chip_info->proto_params).clk_pol !=
+		SPI_CLK_POL_IDLE_HIGH)) {
+		dev_dbg(chip_info->dev,
+			"Clk Polarity configured incorrectly\n");
+		return -1;
+	}
+	if (chip_info->cs_control == NULL) {
+		dev_dbg(chip_info->dev,
+			"Chip Select Function is NULL for this chip\n");
+		chip_info->cs_control = null_cs_control;
+	}
+	return 0;
+}
+
+static struct stm_msp_config_chip *allocate_default_msp_chip_cfg(
+		struct spi_device *spi)
+{
+	struct stm_msp_config_chip *chip_info;
+
+	chip_info = kzalloc(sizeof(struct stm_msp_config_chip), GFP_KERNEL);
+
+	if (!chip_info) {
+		dev_err(&spi->dev, "setup - cannot allocate controller data");
+		return NULL;
+	}
+	dev_dbg(&spi->dev, "Allocated Memory for controller data\n");
+
+	chip_info->lbm = SPI_LOOPBACK_DISABLED;
+	chip_info->com_mode = SPI_POLLING_TRANSFER;
+	chip_info->hierarchy = SPI_MASTER;
+	chip_info->endian_tx = SPI_FIFO_LSB;
+	chip_info->endian_rx = SPI_FIFO_LSB;
+	chip_info->data_size = MSP_DATA_BITS_32;
+
+	if (spi->max_speed_hz != 0)
+		chip_info->freq = spi->max_speed_hz;
+	else
+		chip_info->freq = SPI_DEFAULT_MAX_SPEED_HZ;
+
+	chip_info->proto_params.clk_phase = SPI_CLK_HALF_CYCLE_DELAY;
+	chip_info->proto_params.clk_pol = SPI_CLK_POL_IDLE_LOW;
+	chip_info->cs_control = null_cs_control;
+
+	return chip_info;
+}
+
+static void stm_msp_delay(struct stm_msp *stm_msp)
+{
+	udelay(15);
+
+	while (readl(MSP_FLR(stm_msp->regs)) &
+		(MSP_FLR_MASK_RBUSY | MSP_FLR_MASK_TBUSY))
+			udelay(1);
+}
+
+/**
+ * stm_msp_setup - setup function registered to SPI master framework
+ * @spi: spi device which is requesting setup
+ *
+ * This function is registered to the SPI framework for this SPI master
+ * controller. If it is the first time when setup is called by this device,
+ * this function will initialize the runtime state for this chip and save
+ * the same in the device structure. Else it will update the runtime info
+ * with the updated chip info.
+ */
+static int stm_msp_setup(struct spi_device *spi)
+{
+	struct stm_msp_config_chip *chip_info;
+	struct chip_data *curr_cfg;
+	struct spi_master *master;
+	int status = 0;
+	u16 sckdiv = 0;
+	s16 bus_num = 0;
+	struct stm_msp *stm_msp = spi_master_get_devdata(spi->master);
+	struct msp_regs *msp_regs;
+	master = stm_msp->master;
+	bus_num = master->bus_num - 1;
+
+	/* Get controller data */
+	chip_info = spi->controller_data;
+	/* Get controller_state */
+	curr_cfg = spi_get_ctldata(spi);
+
+	if (curr_cfg == NULL) {
+		curr_cfg = kzalloc(sizeof(struct chip_data), GFP_KERNEL);
+
+		if (!curr_cfg) {
+			dev_err(&stm_msp->adev->dev,
+				"setup - cannot allocate controller state");
+			return -ENOMEM;
+		}
+
+		curr_cfg->chip_id = spi->chip_select;
+		curr_cfg->ctr_regs = kzalloc(sizeof(struct msp_regs),
+						GFP_KERNEL);
+
+		if (curr_cfg->ctr_regs == NULL) {
+			dev_err(&stm_msp->adev->dev,
+				"setup - cannot allocate mem for regs");
+			goto err_first_setup;
+		}
+
+		dev_err(&stm_msp->adev->dev,
+			"chip Id = %d\n", curr_cfg->chip_id);
+
+		if (chip_info == NULL) {
+			chip_info = allocate_default_msp_chip_cfg(spi);
+
+			if (!chip_info) {
+				dev_err(&stm_msp->adev->dev,
+					"setup - cannot allocate cntlr data");
+				status = -ENOMEM;
+				goto err_first_setup;
+			}
+
+			spi->controller_data = chip_info;
+		}
+	}
+
+	/* Pointer back to the SPI device */
+	chip_info->dev = &spi->dev;
+
+	if (chip_info->freq == 0) {
+		/* Calculate Specific Freq. */
+		if ((MSP_INTERNAL_CLK == chip_info->clk_freq.clk_src) ||
+			(MSP_EXTERNAL_CLK == chip_info->clk_freq.clk_src)) {
+			sckdiv = chip_info->clk_freq.sckdiv;
+		} else {
+			status = -1;
+			dev_err(&stm_msp->adev->dev,
+				"setup - controller clock data is incorrect");
+			goto err_config_params;
+		}
+	} else {
+		/* Calculate Effective Freq. */
+		sckdiv = (DEFAULT_MSP_CLK / (chip_info->freq)) - 1;
+
+		if (sckdiv > MAX_SCKDIV) {
+			dev_dbg(&stm_msp->adev->dev,
+				"SPI: Cannot set frequency less than 48Khz,"
+				"setting lowest(48 Khz)\n");
+			sckdiv = MAX_SCKDIV;
+		}
+	}
+
+	status = verify_msp_controller_parameters(chip_info);
+
+	if (status) {
+		dev_err(&stm_msp->adev->dev,
+			"setup - controller data is incorrect");
+		goto err_config_params;
+	}
+
+	/* Now set controller state based on controller data */
+	curr_cfg->xfer_type = chip_info->com_mode;
+	curr_cfg->cs_control = chip_info->cs_control;
+	curr_cfg->delay = stm_msp_delay;
+
+	curr_cfg->null_write = stm_msp_null_writer;
+	curr_cfg->null_read = stm_msp_null_reader;
+
+	if (chip_info->data_size <= MSP_DATA_BITS_8) {
+		dev_dbg(&stm_msp->adev->dev, "Less than 8 bits per word...\n");
+
+		curr_cfg->n_bytes = 1;
+		curr_cfg->read = stm_msp_u8_reader;
+		curr_cfg->write = stm_msp_u8_writer;
+	} else if (chip_info->data_size <= MSP_DATA_BITS_16) {
+		dev_dbg(&stm_msp->adev->dev, "Less than 16 bits per word...\n");
+
+		curr_cfg->n_bytes = 2;
+		curr_cfg->read = stm_msp_u16_reader;
+		curr_cfg->write = stm_msp_u16_writer;
+	} else {
+		dev_dbg(&stm_msp->adev->dev, "Less than 32 bits per word...\n");
+
+		curr_cfg->n_bytes = 4;
+		curr_cfg->read = stm_msp_u32_reader;
+		curr_cfg->write = stm_msp_u32_writer;
+	}
+
+	/* Now initialize all register settings reqd. for this chip */
+
+	msp_regs = (struct msp_regs *)(curr_cfg->ctr_regs);
+	msp_regs->gcr = 0x0;
+	msp_regs->tcf = 0x0;
+	msp_regs->rcf = 0x0;
+	msp_regs->srg = 0x0;
+	msp_regs->dmacr = 0x0;
+
+	MSP_WBITS(msp_regs->dmacr, 0x0, MSP_DMACR_MASK_RDMAE, 0);
+	MSP_WBITS(msp_regs->dmacr, 0x0, MSP_DMACR_MASK_TDMAE, 1);
+
+	/* GCR Reg Config */
+
+	MSP_WBITS(msp_regs->gcr,
+			MSP_RECEIVER_DISABLED, MSP_GCR_MASK_RXEN, 0);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_RX_FIFO_ENABLED, MSP_GCR_MASK_RFFEN, 1);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TRANSMITTER_DISABLED, MSP_GCR_MASK_TXEN, 8);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TX_FIFO_ENABLED, MSP_GCR_MASK_TFFEN, 9);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TX_FRAME_SYNC_POL_LOW, MSP_GCR_MASK_TFSPOL, 10);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TX_FRAME_SYNC_INT, MSP_GCR_MASK_TFSSEL, 11);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_TRANSMIT_DATA_WITH_DELAY, MSP_GCR_MASK_TXDDL, 15);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_SAMPLE_RATE_GEN_ENABLE, MSP_GCR_MASK_SGEN, 16);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_CLOCK_INTERNAL, MSP_GCR_MASK_SCKSEL, 18);
+	MSP_WBITS(msp_regs->gcr,
+			MSP_FRAME_GEN_ENABLE, MSP_GCR_MASK_FGEN, 20);
+	MSP_WBITS(msp_regs->gcr,
+			SPI_BURST_MODE_DISABLE, MSP_GCR_MASK_SPIBME, 23);
+
+	if (chip_info->lbm == SPI_LOOPBACK_ENABLED)
+		MSP_WBITS(msp_regs->gcr,
+				MSP_LOOPBACK_ENABLED, MSP_GCR_MASK_LBM, 7);
+	else
+		MSP_WBITS(msp_regs->gcr,
+				MSP_LOOPBACK_DISABLED, MSP_GCR_MASK_LBM, 7);
+
+	if (chip_info->hierarchy == SPI_MASTER)
+		MSP_WBITS(msp_regs->gcr,
+				MSP_IS_SPI_MASTER, MSP_GCR_MASK_TCKSEL, 14);
+	else
+		MSP_WBITS(msp_regs->gcr,
+				MSP_IS_SPI_SLAVE, MSP_GCR_MASK_TCKSEL, 14);
+
+	if (chip_info->proto_params.clk_phase == SPI_CLK_ZERO_CYCLE_DELAY)
+		MSP_WBITS(msp_regs->gcr,
+				MSP_SPI_PHASE_ZERO_CYCLE_DELAY,
+				MSP_GCR_MASK_SPICKM, 21);
+	else
+		MSP_WBITS(msp_regs->gcr,
+				MSP_SPI_PHASE_HALF_CYCLE_DELAY,
+				MSP_GCR_MASK_SPICKM, 21);
+
+	if (chip_info->proto_params.clk_pol == SPI_CLK_POL_IDLE_HIGH)
+		MSP_WBITS(msp_regs->gcr,
+				MSP_TX_CLOCK_POL_HIGH, MSP_GCR_MASK_TCKPOL, 13);
+	else
+		MSP_WBITS(msp_regs->gcr,
+				MSP_TX_CLOCK_POL_LOW, MSP_GCR_MASK_TCKPOL, 13);
+
+	/* RCF Reg Config */
+	MSP_WBITS(msp_regs->rcf,
+			MSP_IGNORE_RX_FRAME_SYNC_PULSE, MSP_RCF_MASK_RFSIG, 15);
+	MSP_WBITS(msp_regs->rcf,
+			MSP_RX_1BIT_DATA_DELAY, MSP_RCF_MASK_RDDLY, 13);
+
+	if (chip_info->endian_rx == SPI_FIFO_LSB)
+		MSP_WBITS(msp_regs->rcf,
+				MSP_RX_ENDIANESS_LSB, MSP_RCF_MASK_RENDN, 12);
+	else
+		MSP_WBITS(msp_regs->rcf,
+				MSP_RX_ENDIANESS_MSB, MSP_RCF_MASK_RENDN, 12);
+
+	MSP_WBITS(msp_regs->rcf, chip_info->data_size, MSP_RCF_MASK_RP1ELEN, 0);
+
+	/* TCF Reg Config */
+
+	MSP_WBITS(msp_regs->tcf,
+			MSP_IGNORE_TX_FRAME_SYNC_PULSE, MSP_TCF_MASK_TFSIG, 15);
+	MSP_WBITS(msp_regs->tcf,
+			MSP_TX_1BIT_DATA_DELAY, MSP_TCF_MASK_TDDLY, 13);
+
+	if (chip_info->endian_rx == SPI_FIFO_LSB)
+		MSP_WBITS(msp_regs->tcf,
+				MSP_TX_ENDIANESS_LSB, MSP_TCF_MASK_TENDN, 12);
+	else
+		MSP_WBITS(msp_regs->tcf,
+				MSP_TX_ENDIANESS_MSB, MSP_TCF_MASK_TENDN, 12);
+	MSP_WBITS(msp_regs->tcf, chip_info->data_size, MSP_TCF_MASK_TP1ELEN, 0);
+
+	/* SRG Reg Config */
+
+	MSP_WBITS(msp_regs->srg, sckdiv, MSP_SRG_MASK_SCKDIV, 0);
+
+	/* Save controller_state */
+	spi_set_ctldata(spi, curr_cfg);
+
+	return status;
+
+err_config_params:
+err_first_setup:
+
+	kfree(curr_cfg);
+	return status;
+}
+
+static int __init stm_msp_probe(struct amba_device *adev, const struct amba_id *id)
+{
+	struct device *dev = &adev->dev;
+	struct stm_msp_controller *platform_info = adev->dev.platform_data;
+	struct spi_master *master;
+	struct stm_msp *stm_msp = NULL;	/* Data for this driver */
+	int irq, status = 0;
+
+	dev_info(dev, "STM MSP driver, device ID: 0x%08x\n", adev->periphid);
+
+	if (platform_info == NULL) {
+		dev_err(dev, "probe - no platform data supplied\n");
+		status = -ENODEV;
+		goto err_no_pdata;
+	}
+
+	/* Allocate master with space for data */
+	master = spi_alloc_master(dev, sizeof(struct stm_msp));
+
+	if (master == NULL) {
+		dev_err(dev, "probe - cannot alloc spi_master\n");
+		status = -ENOMEM;
+		goto err_no_mem;
+	}
+
+	stm_msp = spi_master_get_devdata(master);
+	stm_msp->master = master;
+	stm_msp->master_info = platform_info;
+	stm_msp->adev = adev;
+
+	stm_msp->clk = clk_get(&adev->dev, NULL);
+
+	if (IS_ERR(stm_msp->clk)) {
+		dev_err(dev, "probe - cannot find clock\n");
+		status = PTR_ERR(stm_msp->clk);
+		goto free_master;
+	}
+
+	/* Fetch the Resources, using platform data */
+	status = amba_request_regions(adev, NULL);
+
+	if (status) {
+		status = -ENODEV;
+		goto disable_clk;
+	}
+
+	/* Get Hold of Device Register Area... */
+	stm_msp->regs = ioremap(adev->res.start, resource_size(&adev->res));
+
+	if (stm_msp->regs == NULL) {
+		status = -ENODEV;
+		goto disable_clk;
+	}
+
+	irq = adev->irq[0];
+
+	if (irq <= 0) {
+		status = -ENODEV;
+		goto err_no_iores;
+	}
+
+	stm_msp_controller_cmd(stm_msp, LOAD_DEFAULT_CONFIG);
+
+	/* Required Info for an SPI controller */
+	/* Bus Number Which Assigned to this SPI controller on this board */
+	master->bus_num = (u16) platform_info->id;
+	master->num_chipselect = platform_info->num_chipselect;
+	master->setup = stm_msp_setup;
+	master->cleanup = (void *)stm_msp_cleanup;
+	master->transfer = stm_msp_transfer;
+
+	dev_dbg(dev, "BUSNO: %d\n", master->bus_num);
+
+	/* Initialize and start queue */
+	status = init_queue(stm_msp);
+
+	if (status != 0) {
+		dev_err(dev, "probe - problem initializing queue\n");
+		goto err_init_queue;
+	}
+
+	status = start_queue(stm_msp);
+
+	if (status != 0) {
+		dev_err(dev, "probe - problem starting queue\n");
+		goto err_start_queue;
+	}
+
+	amba_set_drvdata(adev, stm_msp);
+
+	dev_dbg(dev, "probe succeded\n");
+	dev_dbg(dev, "Bus No = %d, IRQ Line = %d, Virtual Addr: %x\n",
+		master->bus_num, irq, (u32)(stm_msp->regs));
+
+	status = request_irq(stm_msp->adev->irq[0],
+			stm_msp_interrupt_handler,
+			0, stm_msp->master_info->device_name,
+			stm_msp);
+
+	if (status < 0) {
+		dev_err(dev, "probe - cannot get IRQ (%d)\n", status);
+		goto err_irq;
+	}
+
+	/* Register with the SPI framework */
+	status = spi_register_master(master);
+
+	if (status != 0) {
+		dev_err(dev, "probe - problem registering spi master\n");
+		goto err_spi_register;
+	}
+
+	return 0;
+
+err_spi_register:
+	free_irq(stm_msp->adev->irq[0], stm_msp);
+err_irq:
+err_init_queue:
+err_start_queue:
+	destroy_queue(stm_msp);
+err_no_iores:
+	iounmap(stm_msp->regs);
+disable_clk:
+	clk_put(stm_msp->clk);
+free_master:
+	spi_master_put(master);
+err_no_mem:
+err_no_pdata:
+	return status;
+}
+
+static int __exit stm_msp_remove(struct amba_device *adev)
+{
+	struct stm_msp *stm_msp = amba_get_drvdata(adev);
+	int status = 0;
+
+	if (!stm_msp)
+		return 0;
+
+	/* Remove the queue */
+	status = destroy_queue(stm_msp);
+
+	if (status != 0) {
+		dev_err(&adev->dev, "queue remove failed (%d)\n", status);
+		return status;
+	}
+
+	stm_msp_controller_cmd(stm_msp, LOAD_DEFAULT_CONFIG);
+
+	/* Release map resources */
+	iounmap(stm_msp->regs);
+	amba_release_regions(adev);
+	tasklet_disable(&stm_msp->pump_transfers);
+	free_irq(stm_msp->adev->irq[0], stm_msp);
+
+	/* Disconnect from the SPI framework */
+	spi_unregister_master(stm_msp->master);
+
+	clk_put(stm_msp->clk);
+
+	/* Prevent double remove */
+	amba_set_drvdata(adev, NULL);
+	dev_dbg(&adev->dev, "remove succeded\n");
+	return status;
+}
+
+#ifdef CONFIG_PM
+
+/**
+ * stm_msp_suspend - MSP suspend function registered with PM framework.
+ * @dev: Reference to amba device structure of the device
+ * @state: power mgmt state.
+ *
+ * This function is invoked when the system is going into sleep, called
+ * by the power management framework of the linux kernel.
+ */
+static int stm_msp_suspend(struct amba_device *adev, pm_message_t state)
+{
+	struct stm_msp *stm_msp = amba_get_drvdata(adev);
+	int status = 0;
+
+	status = stop_queue(stm_msp);
+
+	if (status != 0) {
+		dev_warn(&adev->dev, "suspend cannot stop queue\n");
+		return status;
+	}
+
+	dev_dbg(&adev->dev, "suspended\n");
+	return 0;
+}
+
+/**
+ * stm_msp_resume - MSP Resume function registered with PM framework.
+ * @dev: Reference to amba device structure of the device
+ *
+ * This function is invoked when the system is coming out of sleep, called
+ * by the power management framework of the linux kernel.
+ */
+static int stm_msp_resume(struct amba_device *adev)
+{
+	struct stm_msp *stm_msp = amba_get_drvdata(adev);
+	int status = 0;
+
+	/* Start the queue running */
+	status = start_queue(stm_msp);
+
+	if (status)
+		dev_err(&adev->dev, "problem starting queue (%d)\n", status);
+	else
+		dev_dbg(&adev->dev, "resumed\n");
+
+	return status;
+}
+
+#else
+#define stm_msp_suspend	NULL
+#define stm_msp_resume	NULL
+#endif /* CONFIG_PM */
+
+static struct amba_id stm_msp_ids[] = {
+	{
+		.id = 0x00280021,
+		.mask = 0x00ffffff,
+	},
+	{
+		0,
+		0,
+	},
+};
+
+static struct amba_driver __refdata stm_msp_driver = {
+	.drv = {
+		.name	= "MSP",
+	},
+	.id_table	= stm_msp_ids,
+	.probe		= stm_msp_probe,
+	.remove		= __exit_p(stm_msp_remove),
+	.resume		= stm_msp_resume,
+	.suspend	= stm_msp_suspend,
+};
+
+static int __init stm_msp_init(void)
+{
+	return amba_driver_register(&stm_msp_driver);
+}
+
+static void __exit stm_msp_exit(void)
+{
+	amba_driver_unregister(&stm_msp_driver);
+}
+
+module_init(stm_msp_init);
+module_exit(stm_msp_exit);
+
+MODULE_AUTHOR("Sachin Verma <sachin.verma@st.com>");
+MODULE_DESCRIPTION("STM MSP (SPI protocol) Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 97d412d9145..f0f47306244 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -24,6 +24,16 @@ menuconfig STAGING
 
 if STAGING
 
+config AB5500_SIM
+        bool "ST-Ericsson AB5500 SIM Interface driver"
+        depends on AB5500_CORE
+        help
+          SIM Interface driver provides interface to configure
+          various parameters of AB5550 SIM Level Shifter.Support provided are:
+          Configure Pull up on sim lines
+          Configure Operation Mode
+          Notify Sim Insert/Extract Interrupt
+
 source "drivers/staging/serial/Kconfig"
 
 source "drivers/staging/et131x/Kconfig"
@@ -38,6 +48,8 @@ source "drivers/staging/wlan-ng/Kconfig"
 
 source "drivers/staging/echo/Kconfig"
 
+source "drivers/staging/cg2900/Kconfig"
+
 source "drivers/staging/comedi/Kconfig"
 
 source "drivers/staging/olpc_dcon/Kconfig"
@@ -132,4 +144,12 @@ source "drivers/staging/ramster/Kconfig"
 
 source "drivers/staging/ozwpan/Kconfig"
 
+source "drivers/staging/cw1200/Kconfig"
+
+source "drivers/staging/mmio/Kconfig"
+
+source "drivers/staging/nmf-cm/Kconfig"
+
+source "drivers/staging/camera_flash/Kconfig"
+
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index ffe7d44374e..1b023288d9b 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -11,6 +11,7 @@ obj-$(CONFIG_USBIP_CORE)	+= usbip/
 obj-$(CONFIG_W35UND)		+= winbond/
 obj-$(CONFIG_PRISM2_USB)	+= wlan-ng/
 obj-$(CONFIG_ECHO)		+= echo/
+obj-$(CONFIG_CG2900)		+= cg2900/
 obj-$(CONFIG_COMEDI)		+= comedi/
 obj-$(CONFIG_FB_OLPC_DCON)	+= olpc_dcon/
 obj-$(CONFIG_ASUS_OLED)		+= asus_oled/
@@ -57,3 +58,8 @@ obj-$(CONFIG_ANDROID)		+= android/
 obj-$(CONFIG_PHONE)		+= telephony/
 obj-$(CONFIG_RAMSTER)		+= ramster/
 obj-$(CONFIG_USB_WPAN_HCD)	+= ozwpan/
+obj-$(CONFIG_CW1200)   		+= cw1200/
+obj-$(CONFIG_U8500_MMIO)	+= mmio/
+obj-$(CONFIG_U8500_FLASH)	+= camera_flash/
+obj-$(CONFIG_U8500_CM)		+= nmf-cm/
+obj-$(CONFIG_AB5500_SIM)	+= ab5500_sim/
diff --git a/drivers/staging/ab5500_sim/Makefile b/drivers/staging/ab5500_sim/Makefile
new file mode 100644
index 00000000000..520717e4dd7
--- /dev/null
+++ b/drivers/staging/ab5500_sim/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_AB5500_SIM)	+= ab5500-sim.o
diff --git a/drivers/staging/ab5500_sim/ab5500-sim.c b/drivers/staging/ab5500_sim/ab5500-sim.c
new file mode 100644
index 00000000000..d222a22ed24
--- /dev/null
+++ b/drivers/staging/ab5500_sim/ab5500-sim.c
@@ -0,0 +1,306 @@
+/*
+ * Copyright (C) ST Ericsson SA 2010
+ *
+ * Sim Interface driver for AB5500
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Bibek Basu <bibek.basu@stericsson.com>
+ */
+#include <linux/init.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/platform_device.h>
+#include <linux/kobject.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/abx500/ab5500.h>
+#include <linux/io.h>
+#include <linux/err.h>
+
+#define USIM_SUP2_REG		0x13
+#define USIM_SUP_REG		0x14
+#define USIM_SIMCTRL_REG	0x17
+#define USIM_SIMCTRL2_REG	0x18
+#define USIM_USBUICC_REG	0x19
+#define USIM_USBUICC2_REG	0x20
+#define SIM_DAT_PULLUP_10K	0x0F
+#define SIM_LDO_1_8V		1875000
+#define SIM_LDO_2_8V		2800000
+#define SIM_LDO_2_9V		2900000
+
+enum shift {
+	SHIFT0,
+	SHIFT1,
+	SHIFT2,
+	SHIFT3,
+	SHIFT4,
+	SHIFT5,
+	SHIFT6,
+	SHIFT7,
+};
+
+enum mask {
+	MASK1 = 1,
+	MASK3 = 3,
+	MASK7 = 7,
+};
+
+enum sim_mode {
+	OFF_MODE,
+	LOW_PWR,
+	PWRCTRL,
+	FULL_PWR,
+};
+/**
+ * struct ab5500_sim - ab5500 Sim Interface device information
+ * @dev:		pointer to the structure device
+ * @lock:		mutex lock
+ * @sim_int_status:	Sim presence status
+ * @irq_base:	Base of the two irqs
+ */
+struct ab5500_sim {
+	struct device *dev;
+	struct mutex lock;
+	bool sim_int_status;
+	u8 irq_base;
+};
+
+/* Exposure to the sysfs interface */
+int ab5500_sim_weak_pulldforce(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+	bool enable;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	enable = user_val ? true : false;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_USBUICC2_REG, MASK1 << SHIFT5, user_val << SHIFT5);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+int ab5500_sim_load_sel(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+	bool enable;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	enable = user_val ? true : false;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_USBUICC_REG, MASK1 << SHIFT1, user_val << SHIFT1);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+int ab5500_sim_mode_sel(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_SIMCTRL2_REG, MASK3 << SHIFT4, user_val << SHIFT4);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+int ab5500_sim_dat_pullup(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_SIMCTRL_REG, MASK7, user_val);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+int ab5500_sim_enable_pullup(struct device *dev,
+		struct device_attribute *attr,
+		const char *user_buf, size_t count)
+{
+	unsigned long user_val;
+	int err;
+	bool enable;
+
+	err = strict_strtoul(user_buf, 0, &user_val);
+	if (err)
+		return -EINVAL;
+	enable = user_val ? true : false;
+	err = abx500_mask_and_set(dev, AB5500_BANK_SIM_USBSIM,
+		USIM_SIMCTRL_REG, MASK1 << SHIFT3, enable << SHIFT3);
+	if (err)
+		return -EINVAL;
+	return count;
+}
+
+static ssize_t ab5500_simoff_int(struct device *dev,
+			       struct device_attribute *devattr, char *user_buf)
+{
+	struct ab5500_sim *di = dev_get_drvdata(dev);
+	int len;
+
+	mutex_lock(&di->lock);
+	len = sprintf(user_buf, "%d\n", di->sim_int_status);
+	mutex_unlock(&di->lock);
+	return len;
+}
+
+static DEVICE_ATTR(enable_pullup, S_IWUSR, NULL, ab5500_sim_enable_pullup);
+static DEVICE_ATTR(dat_pullup, S_IWUSR, NULL, ab5500_sim_dat_pullup);
+static DEVICE_ATTR(mode_sel, S_IWUSR, NULL, ab5500_sim_mode_sel);
+static DEVICE_ATTR(load_sel, S_IWUSR, NULL, ab5500_sim_load_sel);
+static DEVICE_ATTR(weak_pulldforce, S_IWUSR, NULL, ab5500_sim_weak_pulldforce);
+static DEVICE_ATTR(simoff_int, S_IRUGO, ab5500_simoff_int, NULL);
+
+static struct attribute *ab5500_sim_attributes[] = {
+	&dev_attr_enable_pullup.attr,
+	&dev_attr_dat_pullup.attr,
+	&dev_attr_mode_sel.attr,
+	&dev_attr_load_sel.attr,
+	&dev_attr_weak_pulldforce.attr,
+	&dev_attr_simoff_int.attr,
+	NULL
+};
+
+static const struct attribute_group ab5500sim_attr_grp = {
+	.attrs = ab5500_sim_attributes,
+};
+
+static irqreturn_t ab5500_sim_irq_handler(int irq, void *irq_data)
+{
+	struct platform_device *pdev = irq_data;
+	struct ab5500_sim *data = platform_get_drvdata(pdev);
+
+	if (irq == data->irq_base)
+		data->sim_int_status = true;
+	else
+		data->sim_int_status = false;
+	sysfs_notify(&pdev->dev.kobj, NULL, "simoff_int");
+
+	return IRQ_HANDLED;
+}
+
+static int __devexit ab5500_sim_remove(struct platform_device *pdev)
+{
+	struct ab5500_sim *di = platform_get_drvdata(pdev);
+	int irq = platform_get_irq_byname(pdev, "SIMOFF");
+
+	if (irq >= 0) {
+		free_irq(irq, di);
+		irq++;
+		free_irq(irq, di);
+	}
+	sysfs_remove_group(&pdev->dev.kobj, &ab5500sim_attr_grp);
+	platform_set_drvdata(pdev, NULL);
+	kfree(di);
+
+	return 0;
+}
+
+static int __devinit ab5500_sim_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	int irq;
+	struct ab5500_sim *di =
+		kzalloc(sizeof(struct ab5500_sim), GFP_KERNEL);
+	if (!di) {
+		ret = -ENOMEM;
+		goto error_alloc;
+	}
+	dev_info(&pdev->dev, "ab5500_sim_driver PROBE\n");
+	irq = platform_get_irq_byname(pdev, "SIMOFF");
+	if (irq < 0) {
+		dev_err(&pdev->dev, "Get irq by name failed\n");
+		ret = irq;
+		goto exit;
+	}
+	di->irq_base = irq;
+	di->dev = &pdev->dev;
+	mutex_init(&di->lock);
+	platform_set_drvdata(pdev, di);
+	/* sysfs interface to configure sim reg from user space */
+	if (sysfs_create_group(&pdev->dev.kobj, &ab5500sim_attr_grp) < 0) {
+		dev_err(&pdev->dev, " Failed creating sysfs group\n");
+		ret = -ENOMEM;
+		goto error_sysfs;
+	}
+	ret = request_threaded_irq(irq, NULL, ab5500_sim_irq_handler,
+			IRQF_NO_SUSPEND , "ab5500-sim", pdev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Request threaded irq failed (%d)\n", ret);
+		goto error_irq;
+	}
+	/* this is the contiguous irq for sim removal,falling edge */
+	irq = irq + 1;
+	ret = request_threaded_irq(irq, NULL, ab5500_sim_irq_handler,
+			IRQF_NO_SUSPEND , "ab5500-sim", pdev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Request threaded irq failed (%d)\n", ret);
+		free_irq(--irq, di);
+		goto error_irq;
+	}
+	return ret;
+error_irq:
+	sysfs_remove_group(&pdev->dev.kobj, &ab5500sim_attr_grp);
+error_sysfs:
+	platform_set_drvdata(pdev, NULL);
+exit:
+	kfree(di);
+error_alloc:
+	return ret;
+}
+
+static struct platform_driver ab5500_sim_driver = {
+	.probe = ab5500_sim_probe,
+	.remove = __devexit_p(ab5500_sim_remove),
+	.driver = {
+		.name = "ab5500-sim",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init ab5500_sim_init(void)
+{
+	return platform_driver_register(&ab5500_sim_driver);
+}
+
+static void __exit ab5500_sim_exit(void)
+{
+	platform_driver_unregister(&ab5500_sim_driver);
+}
+
+module_init(ab5500_sim_init);
+module_exit(ab5500_sim_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Bibek Basu");
+MODULE_ALIAS("platform:ab5500-sim");
+MODULE_DESCRIPTION("AB5500 sim interface driver");
diff --git a/drivers/staging/ab5500_sim/sysfs-sim b/drivers/staging/ab5500_sim/sysfs-sim
new file mode 100644
index 00000000000..b809b21e39e
--- /dev/null
+++ b/drivers/staging/ab5500_sim/sysfs-sim
@@ -0,0 +1,83 @@
+What:		/sys/devices/platform/ab5500-core.0/ab5500-sim.4/
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4 directory contains attributes
+		allowing the user space to check and configure ab5500 sim level
+		shifter interface caracteristics for communication to SIM card
+
+What:		/sys/devices/.../enable_pullup
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/enable_pullup attribute allows
+		the user space to configure if internal pull up in SIMIO lines
+		has to be enabled or disabled. For enabling write 1 to the file
+		and 0 for disabling
+
+
+What:		/sys/devices/.../dat_pullup
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/dat_pullup attribute allows
+		the user space to configure the resistance value for internal
+		pull up in SIMIO lines. Following value can be written on the file
+		0	SIM_DAT pull-up disabled
+		1	SIM_DAT pull-up 4kOhm
+		2	SIM_DAT pull-up 5kOhm
+		3	SIM_DAT pull-up 6kOhm
+		4	SIM_DAT pull-up 7kOhm
+		5	SIM_DAT pull-up 8kOhm
+		6	SIM_DAT pull-up 9kOhm
+		7	SIM_DAT pull-up 10kOhm
+
+What:		/sys/devices/.../mode_sel
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/mode_sel attribute allows
+		the user space to configure the mode at which the level shifter
+		will work. Following value can be written on the file
+		0	TG mode and LI mode off
+		1	TG mode on
+		2	LI mode on
+		3	TG mode and LI mode off
+
+What:		/sys/devices/.../load_sel
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/load_sel attribute allows
+		the user space to configure the load on the USBUICC lines.
+		Following value can be written on the file.
+		0	Data line load < 21pF
+		1	Data line load 21-30pF
+
+What:		/sys/devices/.../weak_pulldforce
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/weak_pulldforce attribute allows
+		the user space to configure the weak pull down on the USBUICC lines.
+		Following value can be written on the file.
+		0	USB-UICC data lines weak pull down active
+		1	USB-UICC data lines weak pull down not active
+
+What:		/sys/devices/.../simoff_int
+Date:		June 2011
+KernelVersion:	2.6.35
+Contact:	Bibek Basu <bibek.basu@stericsson.com>
+Description:
+		The /sys/devices/.../ab5500-sim.4/simoff_int attribute allows
+		the user space to poll this file and get notified in case a sim
+		hot swap has happened. a zero means sim extracetd and a one means
+		inserted.
+
+
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
index eb1dee26bda..a4495997d1b 100644
--- a/drivers/staging/android/Kconfig
+++ b/drivers/staging/android/Kconfig
@@ -8,6 +8,16 @@ config ANDROID
 
 if ANDROID
 
+config ANDROID_AB5500_TIMED_VIBRA
+	bool "AB5500 Timed Output Vibrator"
+	depends on AB5500_CORE
+	depends on ANDROID_TIMED_OUTPUT
+	default y
+	help
+	  Say Y here to enable linear/rotary vibrator driver using timed
+	  output class device for ST-Ericsson's based on ST-Ericsson's
+	  AB5500 Mix-Sig PMIC
+
 config ANDROID_BINDER_IPC
 	bool "Android Binder IPC Driver"
 	default n
@@ -53,6 +63,14 @@ config ANDROID_LOW_MEMORY_KILLER
 	---help---
 	  Register processes to be killed when memory is low
 
+config ANDROID_STE_TIMED_VIBRA
+	bool "ST-Ericsson Timed Output Vibrator"
+	depends on SND_SOC_AB8500
+	depends on ANDROID_TIMED_OUTPUT
+	default y
+	help
+	  ST-Ericsson's vibrator driver using timed output class device
+
 source "drivers/staging/android/switch/Kconfig"
 
 config ANDROID_INTF_ALARM
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
index 9b6c9ed91f6..0d642936a0b 100644
--- a/drivers/staging/android/Makefile
+++ b/drivers/staging/android/Makefile
@@ -1,3 +1,4 @@
+obj-$(CONFIG_ANDROID_AB5500_TIMED_VIBRA)	+= ab5500-timed-vibra.o
 obj-$(CONFIG_ANDROID_BINDER_IPC)	+= binder.o
 obj-$(CONFIG_ASHMEM)			+= ashmem.o
 obj-$(CONFIG_ANDROID_LOGGER)		+= logger.o
@@ -6,6 +7,7 @@ obj-$(CONFIG_ANDROID_RAM_CONSOLE)	+= ram_console.o
 obj-$(CONFIG_ANDROID_TIMED_OUTPUT)	+= timed_output.o
 obj-$(CONFIG_ANDROID_TIMED_GPIO)	+= timed_gpio.o
 obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
+obj-$(CONFIG_ANDROID_STE_TIMED_VIBRA)	+= ste_timed_vibra.o
 obj-$(CONFIG_ANDROID_SWITCH)		+= switch/
 obj-$(CONFIG_ANDROID_INTF_ALARM)	+= alarm.o
 obj-$(CONFIG_ANDROID_INTF_ALARM_DEV)	+= alarm-dev.o
diff --git a/drivers/staging/android/ab5500-timed-vibra.c b/drivers/staging/android/ab5500-timed-vibra.c
new file mode 100644
index 00000000000..35c627a6285
--- /dev/null
+++ b/drivers/staging/android/ab5500-timed-vibra.c
@@ -0,0 +1,490 @@
+/*
+ * ab5500-vibra.c - driver for vibrator in ST-Ericsson AB5500 chip
+ *
+ * Copyright (C) 2011 ST-Ericsson SA.
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Shreshtha Kumar SAHU <shreshthakumar.sahu@stericsson.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/hrtimer.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/wait.h>
+#include <linux/err.h>
+#include "timed_output.h"
+
+#include <linux/mfd/abx500.h>		/* abx500_* */
+#include <linux/mfd/abx500/ab5500.h>
+#include <linux/mfd/abx500/ab5500-gpadc.h>
+#include <linux/ab5500-vibra.h>
+
+#define AB5500_VIBRA_DEV_NAME          "ab5500:vibra"
+#define AB5500_VIBRA_DRV_NAME		"ab5500-vibrator"
+
+/* Vibrator Register Address Offsets */
+#define AB5500_VIB_CTRL		0x10
+#define AB5500_VIB_VOLT		0x11
+#define AB5500_VIB_FUND_FREQ	0x12	/* Linear vibra resonance freq. */
+#define AB5500_VIB_FUND_DUTY	0x13
+#define AB5500_KELVIN_ANA	0xB1
+#define AB5500_VIBRA_KELVIN	0xFE
+
+/* Vibrator Control */
+#define AB5500_VIB_DISABLE	(0x80)
+#define AB5500_VIB_PWR_ON	(0x40)
+#define AB5500_VIB_FUND_EN	(0x20)
+#define AB5500_VIB_FREQ_SHIFT	(0)
+#define AB5500_VIB_DUTY_SHIFT	(3)
+#define AB5500_VIB_VOLT_SHIFT	(0)
+#define AB5500_VIB_PULSE_SHIFT	(4)
+#define VIBRA_KELVIN_ENABLE	(0x90)
+#define VIBRA_KELVIN_VOUT	(0x20)
+
+/* Vibrator Freq. (in HZ) and Duty */
+enum ab5500_vibra_freq {
+	AB5500_VIB_FREQ_1HZ = 1,
+	AB5500_VIB_FREQ_2HZ,
+	AB5500_VIB_FREQ_4HZ,
+	AB5500_VIB_FREQ_8HZ,
+};
+
+enum ab5500_vibra_duty {
+	AB5500_VIB_DUTY_100 = 0,
+	AB5500_VIB_DUTY_75 = 8,
+	AB5500_VIB_DUTY_50 = 16,
+	AB5500_VIB_DUTY_25 = 24,
+};
+
+/* Linear vibrator resonance freq. duty */
+#define AB5500_VIB_RDUTY_50	(0x7F)
+
+/* Vibration magnitudes */
+#define AB5500_VIB_FREQ_MAX	(4)
+#define AB5500_VIB_DUTY_MAX	(4)
+
+static u8 vib_freq[AB5500_VIB_FREQ_MAX] = {
+	AB5500_VIB_FREQ_1HZ,
+	AB5500_VIB_FREQ_2HZ,
+	AB5500_VIB_FREQ_4HZ,
+	AB5500_VIB_FREQ_8HZ,
+};
+
+static u8 vib_duty[AB5500_VIB_DUTY_MAX] = {
+	AB5500_VIB_DUTY_100,
+	AB5500_VIB_DUTY_75,
+	AB5500_VIB_DUTY_50,
+	AB5500_VIB_DUTY_25,
+};
+
+/**
+ * struct ab5500_vibra - Vibrator driver interal info.
+ * @tdev:		Pointer to timed output device structure
+ * @dev:		Reference to vibra device structure
+ * @vibra_workqueue:    Pointer to vibrator workqueue structure
+ * @vibra_work:		Vibrator work
+ * @gpadc:		Gpadc instance
+ * @vibra_wait:		Vibrator wait queue head
+ * @vibra_lock:		Vibrator lock
+ * @timeout_ms:		Indicates how long time the vibrator will be enabled
+ * @timeout_start:	Start of vibrator in jiffies
+ * @pdata:		Local pointer to platform data with vibrator parameters
+ * @magnitude:		required vibration strength
+ * @enable:		Vibrator running status
+ * @eol:		Vibrator end of life(eol) status
+ **/
+struct ab5500_vibra {
+	struct timed_output_dev		tdev;
+	struct device			*dev;
+	struct workqueue_struct		*vibra_workqueue;
+	struct work_struct		vibra_work;
+	struct ab5500_gpadc		*gpadc;
+	wait_queue_head_t		vibra_wait;
+	spinlock_t			vibra_lock;
+	unsigned int			timeout_ms;
+	unsigned long			timeout_start;
+	struct ab5500_vibra_platform_data *pdata;
+	u8				magnitude;
+	bool				enable;
+	bool				eol;
+};
+
+static inline u8 vibra_magnitude(u8 mag)
+{
+	mag /= (AB5500_VIB_FREQ_MAX * AB5500_VIB_DUTY_MAX);
+	mag = vib_freq[mag / AB5500_VIB_FREQ_MAX] << AB5500_VIB_FREQ_SHIFT;
+	mag |= vib_duty[mag % AB5500_VIB_DUTY_MAX] << AB5500_VIB_DUTY_SHIFT;
+
+	return mag;
+}
+
+static int ab5500_setup_vibra_kelvin(struct ab5500_vibra* vibra)
+{
+	int ret;
+
+	/* Establish the kelvin IP connection to be measured */
+	ret = abx500_set_register_interruptible(vibra->dev,
+		AB5500_BANK_VIT_IO_I2C_CLK_TST_OTP,
+		AB5500_KELVIN_ANA, VIBRA_KELVIN_ENABLE);
+	if (ret < 0) {
+		dev_err(vibra->dev, "failed to set kelvin network\n");
+		return ret;
+	}
+
+	/* Select vibra parameter to be measured */
+	ret = abx500_set_register_interruptible(vibra->dev, AB5500_BANK_VIBRA,
+		AB5500_VIBRA_KELVIN, VIBRA_KELVIN_VOUT);
+	if (ret < 0)
+		dev_err(vibra->dev, "failed to select the kelvin param\n");
+
+	return ret;
+}
+
+static int ab5500_vibra_start(struct ab5500_vibra* vibra)
+{
+	u8 ctrl = 0;
+
+	ctrl = AB5500_VIB_PWR_ON |
+		vibra_magnitude(vibra->magnitude);
+
+	if (vibra->pdata->type == AB5500_VIB_LINEAR)
+		ctrl |= AB5500_VIB_FUND_EN;
+
+	return abx500_set_register_interruptible(vibra->dev,
+			AB5500_BANK_VIBRA, AB5500_VIB_CTRL, ctrl);
+}
+
+static int ab5500_vibra_stop(struct ab5500_vibra* vibra)
+{
+	return abx500_mask_and_set_register_interruptible(vibra->dev,
+			AB5500_BANK_VIBRA, AB5500_VIB_CTRL,
+			AB5500_VIB_PWR_ON, 0);
+}
+
+static int ab5500_vibra_eol_check(struct ab5500_vibra* vibra)
+{
+	int ret, vout;
+
+	ret = ab5500_setup_vibra_kelvin(vibra);
+	if (ret < 0) {
+		dev_err(vibra->dev, "failed to setup kelvin network\n");
+		return ret;
+	}
+
+	/* Start vibra to measure voltage */
+	ret = ab5500_vibra_start(vibra);
+	if (ret < 0) {
+		dev_err(vibra->dev, "failed to start vibra\n");
+		return ret;
+	}
+	/* 20ms delay required for voltage rampup */
+	wait_event_interruptible_timeout(vibra->vibra_wait,
+			0, msecs_to_jiffies(20));
+
+	vout  = ab5500_gpadc_convert(vibra->gpadc, VIBRA_KELVIN);
+	if (vout < 0) {
+		dev_err(vibra->dev, "failed to read gpadc vibra\n");
+		return vout;
+	}
+
+	/* Stop vibra after measuring voltage */
+	ret  = ab5500_vibra_stop(vibra);
+	if (ret < 0) {
+		dev_err(vibra->dev, "failed to stop vibra\n");
+		return ret;
+	}
+	/* Check for vibra eol condition */
+	if (vout < vibra->pdata->eol_voltage) {
+		vibra->eol = true;
+		dev_err(vibra->dev, "Vibra eol detected. Disabling vibra!\n");
+	}
+
+	return ret;
+}
+
+/**
+ * ab5500_vibra_work() - Vibrator work, turns on/off vibrator
+ * @work: Pointer to work structure
+ *
+ * This function is called from workqueue, turns on/off vibrator
+ **/
+static void ab5500_vibra_work(struct work_struct *work)
+{
+	struct ab5500_vibra *vibra = container_of(work,
+			struct ab5500_vibra, vibra_work);
+	unsigned long flags;
+	int ret;
+
+	ret = ab5500_vibra_start(vibra);
+	if (ret < 0)
+		dev_err(vibra->dev, "reg[%d] w failed: %d\n",
+				AB5500_VIB_CTRL, ret);
+
+	wait_event_interruptible_timeout(vibra->vibra_wait,
+			0, msecs_to_jiffies(vibra->timeout_ms));
+
+	ret = ab5500_vibra_stop(vibra);
+	if (ret < 0)
+		dev_err(vibra->dev, "reg[%d] w failed: %d\n",
+				AB5500_VIB_CTRL, ret);
+
+	spin_lock_irqsave(&vibra->vibra_lock, flags);
+
+	vibra->timeout_start = 0;
+	vibra->enable = false;
+
+	spin_unlock_irqrestore(&vibra->vibra_lock, flags);
+}
+
+/**
+ * vibra_enable() - Enables vibrator
+ * @tdev:      Pointer to timed output device structure
+ * @timeout:	Time indicating how long vibrator will be enabled
+ *
+ * This function enables vibrator
+ **/
+static void vibra_enable(struct timed_output_dev *tdev, int timeout)
+{
+	struct ab5500_vibra *vibra = dev_get_drvdata(tdev->dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vibra->vibra_lock, flags);
+
+	if ((!vibra->enable || timeout) && !vibra->eol) {
+		vibra->enable = true;
+
+		vibra->timeout_ms = timeout;
+		vibra->timeout_start = jiffies;
+		queue_work(vibra->vibra_workqueue, &vibra->vibra_work);
+	}
+
+	spin_unlock_irqrestore(&vibra->vibra_lock, flags);
+}
+
+/**
+ * vibra_get_time() - Returns remaining time to disabling vibration
+ * @tdev:      Pointer to timed output device structure
+ *
+ * This function returns time remaining to disabling vibration
+ *
+ * Returns:
+ *	Returns remaining time to disabling vibration
+ **/
+static int vibra_get_time(struct timed_output_dev *tdev)
+{
+	struct ab5500_vibra *vibra = dev_get_drvdata(tdev->dev);
+	unsigned int ms;
+	unsigned long flags;
+
+	spin_lock_irqsave(&vibra->vibra_lock, flags);
+
+	if (vibra->enable)
+		ms = jiffies_to_msecs(vibra->timeout_start +
+				msecs_to_jiffies(vibra->timeout_ms) - jiffies);
+	else
+		ms = 0;
+
+	spin_unlock_irqrestore(&vibra->vibra_lock, flags);
+
+	return ms;
+}
+
+static int ab5500_vibra_reg_init(struct ab5500_vibra *vibra)
+{
+	int ret = 0;
+	u8 ctrl = 0;
+	u8 pulse = 0;
+
+	ctrl = (AB5500_VIB_DUTY_50 << AB5500_VIB_DUTY_SHIFT) |
+		(AB5500_VIB_FREQ_8HZ << AB5500_VIB_FREQ_SHIFT);
+
+	if (vibra->pdata->type == AB5500_VIB_LINEAR) {
+		ctrl |= AB5500_VIB_FUND_EN;
+
+		if (vibra->pdata->voltage > AB5500_VIB_VOLT_MAX)
+			vibra->pdata->voltage = AB5500_VIB_VOLT_MAX;
+
+		pulse = (vibra->pdata->pulse << AB5500_VIB_PULSE_SHIFT) |
+			(vibra->pdata->voltage << AB5500_VIB_VOLT_SHIFT);
+		ret = abx500_set_register_interruptible(vibra->dev,
+				AB5500_BANK_VIBRA, AB5500_VIB_VOLT,
+				pulse);
+		if (ret < 0) {
+			dev_err(vibra->dev,
+				"reg[%#x] w %#x failed: %d\n",
+				AB5500_VIB_VOLT, vibra->pdata->voltage, ret);
+			return ret;
+		}
+
+		ret = abx500_set_register_interruptible(vibra->dev,
+				AB5500_BANK_VIBRA, AB5500_VIB_FUND_FREQ,
+				vibra->pdata->res_freq);
+		if (ret < 0) {
+			dev_err(vibra->dev, "reg[%#x] w %#x failed: %d\n",
+					AB5500_VIB_FUND_FREQ,
+					vibra->pdata->res_freq, ret);
+			return ret;
+		}
+
+		ret = abx500_set_register_interruptible(vibra->dev,
+				AB5500_BANK_VIBRA, AB5500_VIB_FUND_DUTY,
+				AB5500_VIB_RDUTY_50);
+		if (ret < 0) {
+			dev_err(vibra->dev, "reg[%#x] w %#x failed: %d\n",
+					AB5500_VIB_FUND_DUTY,
+					AB5500_VIB_RDUTY_50, ret);
+			return ret;
+		}
+	}
+
+	ret = abx500_set_register_interruptible(vibra->dev,
+			AB5500_BANK_VIBRA, AB5500_VIB_CTRL, ctrl);
+	if (ret < 0) {
+		dev_err(vibra->dev, "reg[%#x] w %#x failed: %d\n",
+				AB5500_VIB_CTRL, ctrl, ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int ab5500_vibra_register_dev(struct ab5500_vibra *vibra,
+		struct platform_device *pdev)
+{
+	int ret = 0;
+
+	ret = timed_output_dev_register(&vibra->tdev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register timed output device\n");
+		goto err_out;
+	}
+
+	dev_set_drvdata(vibra->tdev.dev, vibra);
+
+
+	/* Create workqueue just for timed output vibrator */
+	vibra->vibra_workqueue =
+		create_singlethread_workqueue("ste-timed-output-vibra");
+	if (!vibra->vibra_workqueue) {
+		dev_err(&pdev->dev, "failed to allocate workqueue\n");
+		ret = -ENOMEM;
+		goto exit_output_unregister;
+	}
+
+	init_waitqueue_head(&vibra->vibra_wait);
+	INIT_WORK(&vibra->vibra_work, ab5500_vibra_work);
+	spin_lock_init(&vibra->vibra_lock);
+
+	platform_set_drvdata(pdev, vibra);
+
+	return ret;
+
+exit_output_unregister:
+	timed_output_dev_unregister(&vibra->tdev);
+err_out:
+	return ret;
+}
+
+static int __devinit ab5500_vibra_probe(struct platform_device *pdev)
+{
+	struct ab5500_vibra_platform_data *pdata = pdev->dev.platform_data;
+	struct ab5500_vibra *vibra = NULL;
+	int ret = 0;
+
+	if (pdata == NULL) {
+		dev_err(&pdev->dev, "platform data required. Quitting...\n");
+		return -ENODEV;
+	}
+
+	vibra = kzalloc(sizeof(struct ab5500_vibra), GFP_KERNEL);
+	if (vibra == NULL)
+		return -ENOMEM;
+
+	vibra->tdev.name = "vibrator";
+	vibra->tdev.enable = vibra_enable;
+	vibra->tdev.get_time = vibra_get_time;
+	vibra->timeout_start = 0;
+	vibra->enable = false;
+	vibra->magnitude = pdata->magnitude;
+	vibra->pdata = pdata;
+	vibra->dev = &pdev->dev;
+
+	if (vibra->pdata->eol_voltage) {
+		vibra->gpadc = ab5500_gpadc_get("ab5500-adc.0");
+		if (IS_ERR(vibra->gpadc))
+			goto err_alloc;
+	}
+
+	if (vibra->pdata->type == AB5500_VIB_LINEAR)
+		dev_info(&pdev->dev, "Linear Type Vibrators\n");
+	else
+		dev_info(&pdev->dev, "Rotary Type Vibrators\n");
+
+	ret = ab5500_vibra_reg_init(vibra);
+	if (ret < 0)
+		goto err_alloc;
+
+	ret = ab5500_vibra_register_dev(vibra, pdev);
+	if (ret < 0)
+		goto err_alloc;
+
+	/* Perform vibra eol diagnostics if eol_voltage is set */
+	if (vibra->pdata->eol_voltage) {
+		ret = ab5500_vibra_eol_check(vibra);
+		if (ret < 0)
+			dev_warn(&pdev->dev, "EOL check failed\n");
+	}
+
+	dev_info(&pdev->dev, "initialization success\n");
+
+	return ret;
+
+err_alloc:
+	kfree(vibra);
+
+	return ret;
+}
+
+static int __devexit ab5500_vibra_remove(struct platform_device *pdev)
+{
+	struct ab5500_vibra *vibra = platform_get_drvdata(pdev);
+
+	timed_output_dev_unregister(&vibra->tdev);
+	destroy_workqueue(vibra->vibra_workqueue);
+	kfree(vibra);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver ab5500_vibra_driver = {
+	.probe		= ab5500_vibra_probe,
+	.remove		= __devexit_p(ab5500_vibra_remove),
+	.driver		= {
+		.name	= AB5500_VIBRA_DRV_NAME,
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init ab5500_vibra_module_init(void)
+{
+	return platform_driver_register(&ab5500_vibra_driver);
+}
+
+static void __exit ab5500_vibra_module_exit(void)
+{
+	platform_driver_unregister(&ab5500_vibra_driver);
+}
+
+module_init(ab5500_vibra_module_init);
+module_exit(ab5500_vibra_module_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Shreshtha Kumar SAHU <shreshthakumar.sahu@stericsson.com>");
+MODULE_DESCRIPTION("Timed Output Driver for AB5500 Vibrator");
+
diff --git a/drivers/staging/android/ste_timed_vibra.c b/drivers/staging/android/ste_timed_vibra.c
new file mode 100644
index 00000000000..4621b2fb441
--- /dev/null
+++ b/drivers/staging/android/ste_timed_vibra.c
@@ -0,0 +1,431 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com>
+ *	   for ST-Ericsson
+ * License Terms: GNU General Public License v2
+ */
+
+#include <linux/kernel.h>
+#include <linux/hrtimer.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/ste_timed_vibra.h>
+#include <linux/delay.h>
+#include "timed_output.h"
+
+/**
+ * struct vibra_info - Vibrator information structure
+ * @tdev:		Pointer to timed output device structure
+ * @linear_workqueue:   Pointer to linear vibrator workqueue structure
+ * @linear_work:	Linear Vibrator work
+ * @linear_tick:	Linear Vibrator high resolution timer
+ * @vibra_workqueue:    Pointer to vibrator workqueue structure
+ * @vibra_work:		Vibrator work
+ * @vibra_timer:	Vibrator high resolution timer
+ * @vibra_lock:		Vibrator lock
+ * @vibra_state:	Actual vibrator state
+ * @state_force:	Indicates if oppositive state is requested
+ * @timeout:		Indicates how long time the vibrator will be enabled
+ * @time_passed:	Total time passed in states
+ * @pdata:		Local pointer to platform data with vibrator parameters
+ *
+ * Structure vibra_info holds vibrator information
+ **/
+struct vibra_info {
+	struct timed_output_dev			tdev;
+	struct workqueue_struct			*linear_workqueue;
+	struct work_struct			linear_work;
+	struct hrtimer				linear_tick;
+	struct workqueue_struct			*vibra_workqueue;
+	struct work_struct			vibra_work;
+	struct hrtimer				vibra_timer;
+	spinlock_t				vibra_lock;
+	enum ste_timed_vibra_states		vibra_state;
+	bool					state_force;
+	unsigned int				timeout;
+	unsigned int				time_passed;
+	struct ste_timed_vibra_platform_data	*pdata;
+};
+
+/*
+ * Linear vibrator hardware operates on a particular resonance
+ * frequency. The resonance frequency (f) may also vary with h/w.
+ * This define is half time period (t) in micro seconds (us).
+ * For resonance frequency f = 150 Hz
+ * t = T/2 = ((1/150) / 2) = 3333 usec.
+ */
+#define LINEAR_RESONANCE	3333
+
+/**
+ * linear_vibra_work() - Linear Vibrator work, turns on/off vibrator
+ * @work:	Pointer to work structure
+ *
+ * This function is called from workqueue, turns on/off vibrator
+ **/
+static void linear_vibra_work(struct work_struct *work)
+{
+	struct vibra_info *vinfo =
+			container_of(work, struct vibra_info, linear_work);
+	unsigned char speed_pos = 0, speed_neg = 0;
+	ktime_t ktime;
+	static unsigned char toggle;
+
+	if (toggle) {
+		speed_pos = vinfo->pdata->boost_level;
+		speed_neg = 0;
+	} else {
+		speed_neg = vinfo->pdata->boost_level;
+		speed_pos = 0;
+	}
+
+	toggle = !toggle;
+	vinfo->pdata->timed_vibra_control(speed_pos, speed_neg,
+			speed_pos, speed_neg);
+
+	if ((vinfo->vibra_state != STE_VIBRA_IDLE) &&
+		(vinfo->vibra_state != STE_VIBRA_OFF)) {
+		ktime = ktime_set((LINEAR_RESONANCE / USEC_PER_SEC),
+			(LINEAR_RESONANCE % USEC_PER_SEC) * NSEC_PER_USEC);
+		hrtimer_start(&vinfo->linear_tick, ktime, HRTIMER_MODE_REL);
+	}
+}
+
+/**
+ * vibra_control_work() - Vibrator work, turns on/off vibrator
+ * @work:	Pointer to work structure
+ *
+ * This function is called from workqueue, turns on/off vibrator
+ **/
+static void vibra_control_work(struct work_struct *work)
+{
+	struct vibra_info *vinfo =
+			container_of(work, struct vibra_info, vibra_work);
+	unsigned val = 0;
+	unsigned char speed_pos = 0, speed_neg = 0;
+	unsigned long flags;
+
+	/*
+	 * Cancel scheduled timer if it has not started
+	 * else it will wait for timer callback to complete.
+	 * It should be done before taking vibra_lock to
+	 * prevent race condition, as timer callback also
+	 * takes same lock.
+	 */
+	hrtimer_cancel(&vinfo->vibra_timer);
+
+	spin_lock_irqsave(&vinfo->vibra_lock, flags);
+
+	switch (vinfo->vibra_state) {
+	case STE_VIBRA_BOOST:
+		/* Turn on both vibrators with boost speed */
+		speed_pos = vinfo->pdata->boost_level;
+		val = vinfo->pdata->boost_time;
+		break;
+	case STE_VIBRA_ON:
+		/* Turn on both vibrators with speed */
+		speed_pos = vinfo->pdata->on_level;
+		val = vinfo->timeout - vinfo->pdata->boost_time;
+		break;
+	case STE_VIBRA_OFF:
+		/* Turn on both vibrators with reversed speed */
+		speed_neg = vinfo->pdata->off_level;
+		val = vinfo->pdata->off_time;
+		break;
+	case STE_VIBRA_IDLE:
+		vinfo->time_passed = 0;
+		break;
+	default:
+		break;
+	}
+	spin_unlock_irqrestore(&vinfo->vibra_lock, flags);
+
+	/* Send new settings (only for rotary vibrators) */
+	if (!vinfo->pdata->is_linear_vibra)
+		vinfo->pdata->timed_vibra_control(speed_pos, speed_neg,
+				speed_pos, speed_neg);
+
+	if (vinfo->vibra_state != STE_VIBRA_IDLE) {
+		/* Start timer if it's not in IDLE state */
+		ktime_t ktime;
+		ktime = ktime_set((val / MSEC_PER_SEC),
+				(val % MSEC_PER_SEC) * NSEC_PER_MSEC),
+		hrtimer_start(&vinfo->vibra_timer, ktime, HRTIMER_MODE_REL);
+	} else if (vinfo->pdata->is_linear_vibra) {
+		/* Cancel work and timers of linear vibrator in IDLE state */
+		hrtimer_cancel(&vinfo->linear_tick);
+		flush_workqueue(vinfo->linear_workqueue);
+		vinfo->pdata->timed_vibra_control(0, 0, 0, 0);
+	}
+}
+
+/**
+ * vibra_enable() - Enables vibrator
+ * @tdev:      Pointer to timed output device structure
+ * @timeout:	Time indicating how long vibrator will be enabled
+ *
+ * This function enables vibrator
+ **/
+static void vibra_enable(struct timed_output_dev *tdev, int timeout)
+{
+	struct vibra_info *vinfo = dev_get_drvdata(tdev->dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vinfo->vibra_lock, flags);
+	switch (vinfo->vibra_state) {
+	case STE_VIBRA_IDLE:
+		if (timeout)
+			vinfo->vibra_state = STE_VIBRA_BOOST;
+		else    /* Already disabled */
+			break;
+
+		vinfo->state_force = false;
+		/* Trim timeout */
+		vinfo->timeout = timeout < vinfo->pdata->boost_time ?
+				vinfo->pdata->boost_time : timeout;
+
+		if (vinfo->pdata->is_linear_vibra)
+			queue_work(vinfo->linear_workqueue,
+					&vinfo->linear_work);
+		queue_work(vinfo->vibra_workqueue, &vinfo->vibra_work);
+		break;
+	case STE_VIBRA_BOOST:
+		/* Force only when user requested OFF while BOOST */
+		if (!timeout)
+			vinfo->state_force = true;
+		break;
+	case STE_VIBRA_ON:
+		/* If user requested OFF */
+		if (!timeout) {
+			if (vinfo->pdata->is_linear_vibra)
+				hrtimer_cancel(&vinfo->linear_tick);
+			/* Cancel timer if it has not expired yet.
+			 * Else setting the vibra_state to STE_VIBRA_OFF
+			 * will make take care that vibrator will move to
+			 * STE_VIBRA_IDLE in timer callback just after
+			 * this function call.
+			 */
+			hrtimer_try_to_cancel(&vinfo->vibra_timer);
+			vinfo->vibra_state = STE_VIBRA_OFF;
+			queue_work(vinfo->vibra_workqueue, &vinfo->vibra_work);
+		}
+		break;
+	case STE_VIBRA_OFF:
+		/* Force only when user requested ON while OFF */
+		if (timeout)
+			vinfo->state_force = true;
+		break;
+	default:
+		break;
+	}
+	spin_unlock_irqrestore(&vinfo->vibra_lock, flags);
+}
+
+/**
+ * linear_vibra_tick() - Generate resonance frequency waveform
+ * @hrtimer: Pointer to high resolution timer structure
+ *
+ * This function helps in generating the resonance frequency
+ * waveform required for linear vibrators
+ *
+ * Returns:
+ *	Returns value which indicates whether hrtimer should be restarted
+ **/
+static enum hrtimer_restart linear_vibra_tick(struct hrtimer *hrtimer)
+{
+	struct vibra_info *vinfo =
+			container_of(hrtimer, struct vibra_info, linear_tick);
+
+	if ((vinfo->vibra_state != STE_VIBRA_IDLE) &&
+		(vinfo->vibra_state != STE_VIBRA_OFF)) {
+		queue_work(vinfo->linear_workqueue, &vinfo->linear_work);
+	}
+
+	return HRTIMER_NORESTART;
+}
+
+/**
+ * vibra_timer_expired() - Handles vibrator machine state
+ * @hrtimer:      Pointer to high resolution timer structure
+ *
+ * This function handles vibrator machine state
+ *
+ * Returns:
+ *	Returns value which indicates wether hrtimer should be restarted
+ **/
+static enum hrtimer_restart vibra_timer_expired(struct hrtimer *hrtimer)
+{
+	struct vibra_info *vinfo =
+			container_of(hrtimer, struct vibra_info, vibra_timer);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vinfo->vibra_lock, flags);
+	switch (vinfo->vibra_state) {
+	case STE_VIBRA_BOOST:
+		/* If BOOST finished and force, go to OFF */
+		if (vinfo->state_force)
+			vinfo->vibra_state = STE_VIBRA_OFF;
+		else
+			vinfo->vibra_state = STE_VIBRA_ON;
+		vinfo->time_passed = vinfo->pdata->boost_time;
+		break;
+	case STE_VIBRA_ON:
+		vinfo->vibra_state = STE_VIBRA_OFF;
+		vinfo->time_passed = vinfo->timeout;
+		break;
+	case STE_VIBRA_OFF:
+		/* If OFF finished and force, go to ON */
+		if (vinfo->state_force)
+			vinfo->vibra_state = STE_VIBRA_ON;
+		else
+			vinfo->vibra_state = STE_VIBRA_IDLE;
+		vinfo->time_passed += vinfo->pdata->off_time;
+		break;
+	case STE_VIBRA_IDLE:
+		break;
+	default:
+		break;
+	}
+	vinfo->state_force = false;
+	spin_unlock_irqrestore(&vinfo->vibra_lock, flags);
+
+	queue_work(vinfo->vibra_workqueue, &vinfo->vibra_work);
+
+	return HRTIMER_NORESTART;
+}
+
+/**
+ * vibra_get_time() - Returns remaining time to disabling vibration
+ * @tdev:      Pointer to timed output device structure
+ *
+ * This function returns time remaining to disabling vibration
+ *
+ * Returns:
+ *	Returns remaining time to disabling vibration
+ **/
+static int vibra_get_time(struct timed_output_dev *tdev)
+{
+	struct vibra_info *vinfo = dev_get_drvdata(tdev->dev);
+	u32 ms;
+
+	if (hrtimer_active(&vinfo->vibra_timer)) {
+		ktime_t remain = hrtimer_get_remaining(&vinfo->vibra_timer);
+		ms = (u32) ktime_to_ms(remain);
+		return ms + vinfo->time_passed;
+	} else
+		return 0;
+}
+
+static int __devinit ste_timed_vibra_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct vibra_info *vinfo;
+
+	if (!pdev->dev.platform_data) {
+		dev_err(&pdev->dev, "No platform data supplied\n");
+		return -ENODEV;
+	}
+
+	vinfo = kmalloc(sizeof *vinfo, GFP_KERNEL);
+	if (!vinfo) {
+		dev_err(&pdev->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	vinfo->tdev.name = "vibrator";
+	vinfo->tdev.enable = vibra_enable;
+	vinfo->tdev.get_time = vibra_get_time;
+	vinfo->time_passed = 0;
+	vinfo->vibra_state = STE_VIBRA_IDLE;
+	vinfo->state_force = false;
+	vinfo->pdata = pdev->dev.platform_data;
+
+	if (vinfo->pdata->is_linear_vibra)
+		dev_info(&pdev->dev, "Linear Type Vibrators\n");
+	else
+		dev_info(&pdev->dev, "Rotary Type Vibrators\n");
+
+	ret = timed_output_dev_register(&vinfo->tdev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register timed output device\n");
+		goto exit_free_vinfo;
+	}
+
+	dev_set_drvdata(vinfo->tdev.dev, vinfo);
+
+	vinfo->linear_workqueue =
+		create_singlethread_workqueue("ste-timed-linear-vibra");
+	if (!vinfo->linear_workqueue) {
+		dev_err(&pdev->dev, "failed to allocate workqueue\n");
+		ret = -ENOMEM;
+		goto exit_timed_output_unregister;
+	}
+
+	/* Create workqueue just for timed output vibrator */
+	vinfo->vibra_workqueue =
+		create_singlethread_workqueue("ste-timed-output-vibra");
+	if (!vinfo->vibra_workqueue) {
+		dev_err(&pdev->dev, "failed to allocate workqueue\n");
+		ret = -ENOMEM;
+		goto exit_destroy_workqueue;
+	}
+
+	INIT_WORK(&vinfo->linear_work, linear_vibra_work);
+	INIT_WORK(&vinfo->vibra_work, vibra_control_work);
+	spin_lock_init(&vinfo->vibra_lock);
+	hrtimer_init(&vinfo->linear_tick, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	hrtimer_init(&vinfo->vibra_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	vinfo->linear_tick.function = linear_vibra_tick;
+	vinfo->vibra_timer.function = vibra_timer_expired;
+
+	platform_set_drvdata(pdev, vinfo);
+	return 0;
+
+exit_destroy_workqueue:
+	destroy_workqueue(vinfo->linear_workqueue);
+exit_timed_output_unregister:
+	timed_output_dev_unregister(&vinfo->tdev);
+exit_free_vinfo:
+	kfree(vinfo);
+	return ret;
+}
+
+static int __devexit ste_timed_vibra_remove(struct platform_device *pdev)
+{
+	struct vibra_info *vinfo = platform_get_drvdata(pdev);
+
+	timed_output_dev_unregister(&vinfo->tdev);
+	destroy_workqueue(vinfo->linear_workqueue);
+	destroy_workqueue(vinfo->vibra_workqueue);
+	kfree(vinfo);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver ste_timed_vibra_driver = {
+	.driver = {
+		.name = "ste_timed_output_vibra",
+		.owner = THIS_MODULE,
+	},
+	.probe  = ste_timed_vibra_probe,
+	.remove = __devexit_p(ste_timed_vibra_remove)
+};
+
+static int __init ste_timed_vibra_init(void)
+{
+	return platform_driver_register(&ste_timed_vibra_driver);
+}
+module_init(ste_timed_vibra_init);
+
+static void __exit ste_timed_vibra_exit(void)
+{
+	platform_driver_unregister(&ste_timed_vibra_driver);
+}
+module_exit(ste_timed_vibra_exit);
+
+MODULE_AUTHOR("Marcin Mielczarczyk <marcin.mielczarczyk@tieto.com>");
+MODULE_DESCRIPTION("STE Timed Output Vibrator");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/camera_flash/Kconfig b/drivers/staging/camera_flash/Kconfig
new file mode 100644
index 00000000000..187217d763f
--- /dev/null
+++ b/drivers/staging/camera_flash/Kconfig
@@ -0,0 +1,7 @@
+
+config U8500_FLASH
+	bool "ST-Ericsson Flash (Camera) Driver"
+	depends on ARCH_U8500
+	help
+	  Adds ST-Ericsson Flash (Camera) Driver
+
diff --git a/drivers/staging/camera_flash/Makefile b/drivers/staging/camera_flash/Makefile
new file mode 100644
index 00000000000..bf2f5aa2dd3
--- /dev/null
+++ b/drivers/staging/camera_flash/Makefile
@@ -0,0 +1,5 @@
+export ADP1653_SUPPORT
+EXTRA_CFLAGS += -DADP1653_SUPPORT
+obj-$(CONFIG_U8500_FLASH) := camera_flash.o
+camera_flash-y := flash_common.o
+camera_flash-y += adp1653.o
diff --git a/drivers/staging/camera_flash/adp1653.c b/drivers/staging/camera_flash/adp1653.c
new file mode 100644
index 00000000000..f7483eac11f
--- /dev/null
+++ b/drivers/staging/camera_flash/adp1653.c
@@ -0,0 +1,537 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * adp1653: Driver Adp1653 HPLED flash driver chip. This driver
+ *          currently support I2C interface, 2bit interface is not supported.
+ * Author: Pankaj Chauhan/pankaj.chauhan@stericsson.com for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/version.h>
+#include <asm/mach-types.h>
+#include "flash_common.h"
+#include "adp1653.h"
+#include "camera_flash.h"
+#include "adp1653_plat.h"
+
+/* This data is platform specific for 8500 href-v1 platform,
+ * Ideally this should be supplied from platform code
+ */
+
+static int adapter_i2c2 = 2;
+static int flash_position = 0;
+module_param(adapter_i2c2, int, S_IRUGO);
+MODULE_PARM_DESC(adapter_i2c2, "use the given I2C adaptater to communicate with the chip");
+module_param(flash_position, int, S_IRUGO);
+MODULE_PARM_DESC(flash_position, "the position of the flash chip (0=PRIMARY, 1=SECONDARY)");
+
+
+int __flash_gpio_to_irq(int gpio)
+{
+
+	return NOMADIK_GPIO_TO_IRQ(gpio);
+}
+
+#define DEBUG_LOG(...) printk(KERN_DEBUG "Adp1653 flash driver: " __VA_ARGS__)
+
+#define ADP1653_SUPPORTED_MODES (FLASH_MODE_VIDEO_LED | FLASH_MODE_STILL_LED | \
+	FLASH_MODE_STILL_LED_EXTERNAL_STROBE |                                 \
+	FLASH_MODE_AF_ASSISTANT | FLASH_MODE_INDICATOR)
+
+#define ADP1653_SELFTEST_SUPPORTED_MODES (FLASH_SELFTEST_CONNECTION | FLASH_SELFTEST_FLASH_WITH_STROBE | \
+	FLASH_SELFTEST_VIDEO_LIGHT | FLASH_SELFTEST_AF_LIGHT | FLASH_SELFTEST_INDICATOR | FLASH_SELFTEST_TORCH_LIGHT)
+
+static int adp1653_trigger_strobe(void *priv_data, int enable);
+
+static int adp1653_get_modes(void *priv_data,unsigned long *modes)
+{
+	int err;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv_data;
+	err = i2c_smbus_read_byte_data(priv_p->i2c_client, FAULT_STATUS_REG);
+	if (err)
+		*modes = 0x0;
+	else
+		*modes = ADP1653_SUPPORTED_MODES;
+	return 0;
+}
+
+static int adp1653_get_mode_details(void *priv_data, unsigned long mode,
+struct flash_mode_details *details_p)
+{
+	int err = 0;
+	memset(details_p,0,sizeof(struct flash_mode_details));
+
+	details_p->led_type = 2;
+
+	/* Still LED settings*/
+	details_p->nbFaultRegisters = 1;
+	if(mode & (FLASH_MODE_STILL_LED | FLASH_MODE_STILL_LED_EXTERNAL_STROBE)){
+		details_p->max_intensity_uAmp = FLASH_MAX_INTENSITY;
+		details_p->min_intensity_uAmp = FLASH_MIN_INTENSITY;
+		details_p->max_strobe_duration_uSecs = FLASH_MAX_STROBE_DURATION;
+		details_p->feature_bitmap = INTENSITY_PROGRAMMABLE | DURATION_PROGRAMMABLE;
+		goto out;
+	}
+	/*Video LED settings*/
+	if(mode & FLASH_MODE_VIDEO_LED){
+		details_p->max_intensity_uAmp = TORCH_MAX_INTENSITY;
+		details_p->min_intensity_uAmp = TORCH_MIN_INTENSITY;
+		details_p->max_strobe_duration_uSecs = 0;
+		details_p->feature_bitmap = INTENSITY_PROGRAMMABLE;
+		goto out;
+	}
+	/*Privacy Indicator settings */
+	if(mode & FLASH_MODE_INDICATOR){
+		details_p->max_intensity_uAmp = ILED_MAX_INTENSITY;
+		details_p->min_intensity_uAmp = ILED_MIN_INTENSITY;
+		details_p->max_strobe_duration_uSecs = 0;
+		details_p->feature_bitmap = INTENSITY_PROGRAMMABLE;
+		goto out;
+	}
+	DEBUG_LOG("Mode %lx, not supported\n",mode);
+	err = EINVAL;
+out:
+	return err;
+}
+
+static int adp1653_enable_flash_mode(void *priv_data,
+				     unsigned long mode, int enable)
+{
+	int err = 0;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv_data;
+
+	if(enable){
+
+		if((!(mode & ADP1653_SUPPORTED_MODES)) &&
+		  (mode != FLASH_MODE_NONE)) {
+			DEBUG_LOG("Unsupported mode %lx\n",mode);
+			err = -EINVAL;
+			goto out;
+		}
+		/*Nothing to be done in enabling, just set current mode and return*/
+		/*May be enable disable can be done here but why not enable in
+		*probe and keep it on always
+		*/
+		adp1653_trigger_strobe(priv_p,0);
+		priv_p->curr_mode = mode;
+	}else{
+		adp1653_trigger_strobe(priv_p,0);
+		priv_p->curr_mode =0;
+	}
+out:
+	return err;
+}
+
+static int adp1653_configure_flash_mode(void *priv,unsigned long mode,
+struct flash_mode_params *params_p)
+{
+	int err = 0;
+	unsigned char intensity_code;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv;
+
+	if(!(mode & ADP1653_SUPPORTED_MODES)){
+		DEBUG_LOG("Mode %lx not supported\n",mode);
+		err = -EINVAL;
+		goto out;
+	}
+	switch(mode){
+	case FLASH_MODE_STILL_LED:
+	case FLASH_MODE_STILL_LED_EXTERNAL_STROBE:
+		{
+			FLASH_UAMP_TO_CODE(intensity_code,params_p->intensity_uAmp);
+			if(params_p->duration_uSecs){
+				DURATION_USEC_TO_CODE(priv_p->flash_duration,
+					params_p->duration_uSecs);
+				DEBUG_LOG("Duration %lu, code 0x%x\n",params_p->duration_uSecs,
+					priv_p->flash_duration);
+				priv_p->flash_duration |= TIMER_ENABLE;
+			}else{
+				priv_p->flash_duration = 0;
+			}
+			priv_p->flash_intensity = intensity_code << 3;
+		}
+		break;
+	case FLASH_MODE_VIDEO_LED:
+		{
+			TORCH_UAMP_TO_CODE(intensity_code,params_p->intensity_uAmp);
+			DEBUG_LOG("Torch mode setting intensity 0x%x, current(uA) %lu\n",
+				intensity_code,params_p->intensity_uAmp);
+			priv_p->torch_intensity = intensity_code << 3;
+		}
+		break;
+	case FLASH_MODE_INDICATOR:
+		{
+			ILED_UAMP_TO_CODE(intensity_code,params_p->intensity_uAmp);
+			DEBUG_LOG("ILED setting intensity 0x%x, current(uA) %lu\n",
+				intensity_code,params_p->intensity_uAmp);
+			priv_p->indicator_intensity = intensity_code;
+		}
+		break;
+	default:
+		err = -EINVAL;
+		DEBUG_LOG("Unsupported mode %lx\n",mode);
+		break;
+	}
+
+	if((mode == FLASH_MODE_STILL_LED_EXTERNAL_STROBE) || (mode == FLASH_MODE_STILL_LED))
+	{
+		adp1653_trigger_strobe(priv_p,0);
+		DEBUG_LOG("CONFIG_TIMER_REG : 0x%x\n",priv_p->flash_duration);
+		DEBUG_LOG("OUTPUT_SEL_REG : 0x%x\n",priv_p->flash_intensity);
+
+		/*TimeOut Must be programmed before Intensity*/
+		err = i2c_smbus_write_byte_data(priv_p->i2c_client,CONFIG_TIMER_REG,
+			priv_p->flash_duration);
+		if(err){
+			DEBUG_LOG("I2C: Unsable to write timer config, err %d\n",err);
+			goto out;
+		}
+		err = i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,
+			priv_p->flash_intensity);
+		if(err){
+			DEBUG_LOG("I2C: Unable to write   OUTPUT_SEL_REG , err %d\n",err);
+			goto out;
+		}
+	}
+out:
+	return err;
+}
+
+static int adp1653_set_intensity(struct adp1653_priv_data *priv_p, uint8_t intensity)
+{
+	return i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,intensity);
+}
+
+static int adp1653_strobe_still_led(struct adp1653_priv_data *priv_p,int enable)
+{
+	int err=0,gpio_val;
+	uint8_t intensity,duration;
+
+	if(enable){
+		intensity = priv_p->flash_intensity;
+		duration = priv_p->flash_duration;
+		gpio_val = 1;
+	}else{
+		intensity = 0;
+		duration = 0;
+		gpio_val = 0;
+	}
+
+	err = adp1653_set_intensity(priv_p,intensity);
+	if(err){
+		DEBUG_LOG("I2C: Unable to write OUTPUT_SEL_REG reg, err %d\n",err);
+		goto out;
+	}
+
+	/*TimeOut Must be programmed before Intensity*/
+	err = i2c_smbus_write_byte_data(priv_p->i2c_client,CONFIG_TIMER_REG,
+		priv_p->flash_duration);
+	if(err){
+		DEBUG_LOG("I2C: Unsable to write timer config, err %d\n",err);
+		goto out;
+	}
+	err = i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,intensity);
+	if(err){
+		DEBUG_LOG("I2C: Unable to write OUTPUT_SEL_REG, err %d\n",err);
+		goto out;
+	}
+
+out:
+	return err;
+}
+
+static int adp1653_trigger_strobe(void *priv, int enable)
+{
+	int err = 0;
+	uint8_t intensity;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv;
+
+	switch(priv_p->curr_mode){
+	case FLASH_MODE_STILL_LED:
+	case FLASH_MODE_STILL_LED_EXTERNAL_STROBE:
+		err = adp1653_strobe_still_led(priv_p,enable);
+		break;
+	case FLASH_MODE_VIDEO_LED:
+		{
+			if(enable)
+				intensity = priv_p->torch_intensity;
+			else
+				intensity = 0;
+			err = adp1653_set_intensity(priv_p,intensity);
+		}
+		break;
+	case FLASH_MODE_INDICATOR:
+		{
+			if(enable)
+				intensity = priv_p->indicator_intensity;
+			else
+				intensity =0;
+			err = adp1653_set_intensity(priv_p,intensity);
+		}
+		break;
+	default:
+		DEBUG_LOG("Unsupported mode %lx\n",priv_p->curr_mode);
+		goto out;
+	}
+	if(err){
+		DEBUG_LOG("Unable to enable/disable %d, strobe. Mode %lx, err %d\n",enable,
+			priv_p->curr_mode,err);
+		goto out;
+	}
+	disable_irq(priv_p->i2c_client->irq);
+	if(enable)
+		SET_FLASH_STATUS(priv_p->status,FLASH_STATUS_LIT);
+	else
+		CLR_FLASH_STATUS(priv_p->status,FLASH_STATUS_LIT);
+
+	enable_irq(priv_p->i2c_client->irq);
+
+out:
+	return err;
+}
+#define FLASH_ERR_ALL	(FLASH_ERR_OVER_CHARGE |FLASH_ERR_OVER_HEAT |	\
+	FLASH_ERR_SHORT_CIRCUIT | FLASH_ERR_TIMEOUT |			\
+	FLASH_ERR_OVER_VOLTAGE)
+int  adp1653_get_status(void *priv_data,unsigned long *status)
+{
+	struct adp1653_priv_data *priv_p= (struct adp1653_priv_data *)priv_data;
+	disable_irq(priv_p->i2c_client->irq);
+	if(priv_p->fault){
+		if(priv_p->fault & OVER_VOLTAGE_FAULT)
+			SET_FLASH_ERROR(priv_p->status,FLASH_ERR_OVER_VOLTAGE);
+		if(priv_p->fault & TIMEOUT_FAULT)
+			SET_FLASH_ERROR(priv_p->status,FLASH_ERR_TIMEOUT);
+		if(priv_p->fault & OVER_TEMPERATURE_FAULT)
+			SET_FLASH_ERROR(priv_p->status,FLASH_ERR_OVER_HEAT);
+		if(priv_p->fault & SHORT_CIRCUIT_FAULT){
+			CLR_FLASH_STATUS(priv_p->status,FLASH_STATUS_READY);
+			SET_FLASH_STATUS(priv_p->status,FLASH_STATUS_BROKEN);
+			SET_FLASH_ERROR(priv_p->status,FLASH_ERR_SHORT_CIRCUIT);
+		}
+		priv_p->fault =0;
+	}else{
+		CLR_FLASH_ERROR(priv_p->status,FLASH_ERR_ALL);
+	}
+	enable_irq(priv_p->i2c_client->irq);
+	*status = priv_p->status;
+	return 0;
+}
+
+int adp1653_get_selftest_modes(void *priv_data, unsigned long *modes)
+{
+	int err;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv_data;
+	err = i2c_smbus_read_byte_data(priv_p->i2c_client, FAULT_STATUS_REG);
+	if (err) *modes = 0x0;
+	else     *modes = ADP1653_SELFTEST_SUPPORTED_MODES;
+	return 0;
+}
+
+int adp1653_get_fault_registers(void *priv_data, unsigned long mode, unsigned long *status)
+{
+	int err = 0;
+	struct adp1653_priv_data *priv_p = (struct adp1653_priv_data *)priv_data;
+
+	*status = i2c_smbus_read_byte_data(priv_p->i2c_client, FAULT_STATUS_REG);
+
+	/* clear fault register */
+	err = i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,0);
+	if(0 != err)
+	{
+		DEBUG_LOG("Unable to write OUTPUT_SEL_REG, err %d\n",err);
+	}
+	return err;
+}
+
+struct flash_chip_ops adp1653_ops = {
+	.get_modes = adp1653_get_modes,
+	.get_mode_details = adp1653_get_mode_details,
+	.get_status = adp1653_get_status,
+	.enable_flash_mode = adp1653_enable_flash_mode,
+	.configure_flash_mode = adp1653_configure_flash_mode,
+	.trigger_strobe = adp1653_trigger_strobe,
+	.get_selftest_modes = adp1653_get_selftest_modes,
+	.get_fault_registers = adp1653_get_fault_registers
+};
+
+static irqreturn_t adp1653_irq_hdlr(int irq_no,void *data)
+{
+	int err;
+	struct adp1653_priv_data *priv_p= (struct adp1653_priv_data *)data;
+
+	priv_p->fault = i2c_smbus_read_byte_data(priv_p->i2c_client,
+		FAULT_STATUS_REG);
+	DEBUG_LOG("Got Fault, status 0x%x\n",priv_p->fault);
+	/*Writing 0 to OUTPUT_SEL_REG clears the interrtup
+	*and FAULT_STATUS_REG register
+	*/
+	err = i2c_smbus_write_byte_data(priv_p->i2c_client,OUTPUT_SEL_REG,0);
+	if(err)
+		DEBUG_LOG("Unable to write OUTPUT_SEL_REG to clr intr, err %d\n",err);
+	/*TBD: send even to user process*/
+	return IRQ_HANDLED;
+}
+static int __devinit adp1653_probe(struct i2c_client *client,
+				   const struct i2c_device_id *id)
+{
+	int err = 0;
+	struct flash_chip *flash_chip_p=NULL;
+	struct adp1653_priv_data *priv_p=NULL;
+	struct adp1653_platform_data *pdata = client->dev.platform_data;
+
+	DEBUG_LOG("> adp1653_probe\n");
+
+	priv_p = kzalloc(sizeof(struct adp1653_priv_data),GFP_KERNEL);
+	if(!priv_p){
+		DEBUG_LOG("Kmalloc failed for priv data\n");
+		err = ENOMEM;
+		goto err_priv;
+	}
+	priv_p->i2c_client = client;
+	flash_chip_p = kzalloc(sizeof(struct flash_chip),GFP_KERNEL);
+	if(!flash_chip_p){
+		DEBUG_LOG("Kmalloc failed for flash_chip_p");
+		err = ENOMEM;
+		goto err_flash_chip_alloc;
+	}
+
+	if (!pdata) {
+		dev_err(&client->dev,
+			"%s: No platform data supplied.\n", __func__);
+		err = -EINVAL;
+		goto err_pdata;
+	}
+
+	flash_chip_p->priv_data = priv_p;
+	flash_chip_p->ops = &adp1653_ops;
+	SET_FLASHCHIP_TYPE(flash_chip_p,FLASH_TYPE_HPLED);
+	SET_FLASHCHIP_ID(flash_chip_p,ADP1653_ID);
+
+	strncpy(flash_chip_p->name,"Adp1653",FLASH_NAME_SIZE);
+
+	i2c_set_clientdata(client,priv_p);
+	/*Request GPIO and Register IRQ if supported by platform and flash chip*/
+
+	err = gpio_request(pdata->enable_gpio,"Camera LED flash Enable");
+	if(err){
+		DEBUG_LOG("Unable to get GPIO %d, for enable\n",pdata->enable_gpio);
+		goto err_pdata;
+	}
+
+	err = gpio_direction_output(pdata->enable_gpio, 1);
+	if(err){
+		DEBUG_LOG("Unable to set GPIO %u in output mode, err %d\n",pdata->enable_gpio,err);
+		gpio_free(pdata->enable_gpio);
+		goto err_gpio_set;
+	}
+	gpio_set_value_cansleep(pdata->enable_gpio, 1);
+
+	err = request_threaded_irq(gpio_to_irq(pdata->irq_no),NULL,adp1653_irq_hdlr,
+		IRQF_ONESHOT|IRQF_TRIGGER_FALLING,
+		"Adp1653 flash",priv_p);
+	if(err){
+		DEBUG_LOG("Unable to register flash IRQ handler, irq %d, err %d\n",
+			pdata->irq_no,err);
+		goto err_irq;
+	}
+
+	err = register_flash_chip(flash_position,flash_chip_p);
+	if(err){
+		DEBUG_LOG("Failed to register Adp1653 as flash for %s camera\n",
+			(flash_position?"Primary":"Secondary"));
+		goto err_register;
+	}
+	SET_FLASH_STATUS(priv_p->status,FLASH_STATUS_READY);
+	DEBUG_LOG("< adp1653_probe ok\n");
+	return err;
+err_register:
+	if(pdata->irq_no)
+		free_irq(pdata->irq_no,NULL);
+err_irq:
+	gpio_set_value_cansleep(pdata->enable_gpio, 0);
+err_gpio_set:
+	if(pdata->enable_gpio)
+		gpio_free(pdata->enable_gpio);
+err_pdata:
+	if(flash_chip_p)
+		kfree(flash_chip_p);
+err_flash_chip_alloc:
+        if(priv_p)
+                kfree(priv_p);
+err_priv:
+	DEBUG_LOG("< adp1653_probe (%d)\n", err);
+	return err;
+}
+
+static int __devexit adp1653_remove(struct i2c_client *client)
+{
+	int err=0;
+	/*Nothing here yet, implement it later.*/
+	return err;
+}
+static const struct i2c_device_id adp1653_id[] = {
+	{ "adp1653", 0},
+	{}
+};
+static struct i2c_driver adp1653_i2c_driver = {
+	.driver = {
+		.name = "adp1653",
+		.owner = THIS_MODULE,
+	},
+	.probe = adp1653_probe,
+	.remove = __devexit_p(adp1653_remove),
+	.id_table = adp1653_id,
+};
+
+int adp1653_init(void){
+	int err = 0;
+	struct i2c_adapter *adap_p;
+	struct i2c_board_info info;
+
+	/* Registration of I2C flash device is platform specific code
+	 * Ideally it should be done from kernel (arch/arm/mach-XXX).
+	 * Do it locally till the time it gets into platform code
+	 * OR This portion (registration of device) and flash chip init
+	 * Routine can be moved to Flash chip module init. */
+	DEBUG_LOG("getting I2C adaptor %d\n",adapter_i2c2);
+	adap_p = i2c_get_adapter(adapter_i2c2);
+	if(!adap_p){
+		DEBUG_LOG("Unable to get I2C adaptor\n");
+		goto out;
+	}
+	memset(&info,0,sizeof( struct i2c_board_info));
+
+	strcpy(&info.type[0],"adp1653");
+	DEBUG_LOG("trying to register %s at position %d\n",
+		info.type,
+		flash_position);
+
+	/* I2C framework expects least significant 7 bits as address, not complete
+	* 8 bits with bit 0 (read/write bit)
+	*/
+	info.addr = 0x60 >> 1;
+
+	err = i2c_add_driver(&adp1653_i2c_driver);
+	if(err)
+	{
+		DEBUG_LOG("Failed to register i2c driver\n");
+		goto out;
+	}
+
+	DEBUG_LOG("Initialized adp1653\n");
+	if(!i2c_new_device(adap_p,&info)){
+		DEBUG_LOG("Unable to add i2c dev: %s (err=%d)\n",info.type, err);
+		goto out;
+	}
+out:
+	return err;
+}
+
+/*
+MODULE_DEPEND
+*/
diff --git a/drivers/staging/camera_flash/adp1653.h b/drivers/staging/camera_flash/adp1653.h
new file mode 100755
index 00000000000..3035ab56d99
--- /dev/null
+++ b/drivers/staging/camera_flash/adp1653.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __ADP1653_H__
+#define __ADP1653_H__
+
+#include <linux/types.h>
+#define ADP1653_ID	(0)	/*chip does not give any id :) so be it zero!*/
+
+#define OUTPUT_SEL_REG		(0x00)
+#define CONFIG_TIMER_REG	(0x01)
+#define SW_STROBE_REG		(0x02)
+#define FAULT_STATUS_REG	(0x03)
+
+/* Fault codes, FALUT_STATUS_REG bits */
+#define OVER_VOLTAGE_FAULT		(0x01)
+#define TIMEOUT_FAULT			(0x02)
+#define OVER_TEMPERATURE_FAULT	(0x04)
+#define SHORT_CIRCUIT_FAULT		(0x08)
+
+/*CONFIG_TIMER_REG bits*/
+#define TIMER_ENABLE	(0x10)
+
+struct adp1653_priv_data{
+	struct i2c_client *i2c_client;
+	unsigned long curr_mode;
+	unsigned long enable_gpio;
+	unsigned long strobe_gpio;
+	unsigned long irq_no;
+	unsigned long status;
+	uint8_t fault;
+	uint8_t flash_intensity;
+	uint8_t flash_duration;
+	uint8_t torch_intensity;
+	uint8_t indicator_intensity;
+};
+
+/*Intensity current limits in Micro Amps*/
+/* over 250mA flash current is reduced */
+/* do not know why, neither really care about */
+//#define FLASH_MAX_INTENSITY	(500000)	/*code - 31*/
+#define FLASH_MAX_INTENSITY	(250000)
+#define FLASH_MIN_INTENSITY	(215000)	/*code - 12*/
+#define TORCH_MAX_INTENSITY	(200000)	/*code - 11*/
+#define TORCH_MIN_INTENSITY	(50000)		/*code - 1*/
+#define ILED_MAX_INTENSITY  (17500)		/*Code - 7*/
+#define ILED_MIN_INTENSITY	(2500)		/*code - 1*/
+
+#define FLASH_MAX_STROBE_DURATION (820000)	 /*820 uSec*/
+
+#define DURATION_USEC_TO_CODE(_code,_duration) do{			\
+	 if(_duration > FLASH_MAX_STROBE_DURATION)				\
+	 	_duration = FLASH_MAX_STROBE_DURATION;				\
+	_code = (FLASH_MAX_STROBE_DURATION - _duration) / 54600;\
+}while(0);
+
+#define HPLED_UAMP_TO_CODE(_current) 	((_current - 35000) / 15000)
+
+#define FLASH_UAMP_TO_CODE(_code,_current){		\
+	if(_current > FLASH_MAX_INTENSITY)			\
+		_current = FLASH_MAX_INTENSITY;			\
+	if(_current < FLASH_MIN_INTENSITY)			\
+		_current = FLASH_MIN_INTENSITY;			\
+	_code = HPLED_UAMP_TO_CODE(_current);		\
+}while(0)
+
+#define TORCH_UAMP_TO_CODE(_code,_current){		\
+	if(_current > TORCH_MAX_INTENSITY)			\
+		_current = TORCH_MAX_INTENSITY;			\
+	if(_current < TORCH_MIN_INTENSITY)			\
+		_current = TORCH_MIN_INTENSITY;			\
+	_code = HPLED_UAMP_TO_CODE(_current);		\
+}while(0)
+
+#define ILED_UAMP_TO_CODE(_code,_current) do {								\
+	 if(_current > ILED_MAX_INTENSITY)										\
+	 	_current = ILED_MAX_INTENSITY;										\
+	_code = _current / ILED_MIN_INTENSITY; /* Min current: 2.5mA/2500uA*/	\
+}while(0)
+
+#endif
diff --git a/drivers/staging/camera_flash/adp1653_plat.h b/drivers/staging/camera_flash/adp1653_plat.h
new file mode 100755
index 00000000000..325097aa2a8
--- /dev/null
+++ b/drivers/staging/camera_flash/adp1653_plat.h
@@ -0,0 +1,24 @@
+/*
+ * adp1653_plat.h
+ * ADP1653 Led Flash Driver platform specific structures
+ *
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Rajat Verma <rajat.verma@stericsson.com>
+ *
+ * License Terms: GNU General Public License v2
+ */
+
+#ifndef __LINUX_I2C_ADP1653_PLAT_H__
+#define __LINUX_I2C_ADP1653_PLAT_H__
+
+/**
+ * struct adp1653_platform_data - platform data structure for adp1653
+ * @enable_gpio: gpio for chip enable/disable
+ * @irq_no: interrupt line for flash ic
+ */
+struct adp1653_platform_data {
+	u32 enable_gpio;
+	u32 irq_no;
+};
+
+#endif //__LINUX_I2C_ADP1653_PLAT_H__
diff --git a/drivers/staging/camera_flash/camera_flash.h b/drivers/staging/camera_flash/camera_flash.h
new file mode 100644
index 00000000000..15faf706dc9
--- /dev/null
+++ b/drivers/staging/camera_flash/camera_flash.h
@@ -0,0 +1,74 @@
+#ifndef __CAMERA_FLASH_H__
+#define __CAMERA_FLASH_H__
+
+#define FLASH_NAME_SIZE	(20)
+
+struct flash_mode_details {
+	unsigned long led_type;
+	unsigned long max_intensity_uAmp;
+	unsigned long min_intensity_uAmp;
+	unsigned long max_strobe_duration_uSecs;
+	unsigned long feature_bitmap;
+	unsigned char nbFaultRegisters;
+};
+
+/*feature_bitmap (in struct flash_mode_details) bit values*/
+#define INTENSITY_PROGRAMMABLE (0x01)
+#define DURATION_PROGRAMMABLE  (0x02)
+#define TIMEOUT_PROGRAMMABLE   (0x04)
+
+/*Status word returned by driver has status in lower 16 bits
+ *and Error in higher 16 bits. definition of status and error
+ *bits are there in flash_bitfields.h
+ */
+#define SET_FLASH_STATUS(_bitmap, _status) (_bitmap |= (_status & 0xffff))
+#define CLR_FLASH_STATUS(_bitmap, _status) (_bitmap &= ~(_status & 0xffff))
+#define SET_FLASH_ERROR(_bitmap, _status) (_bitmap |= (_status << 16))
+#define CLR_FLASH_ERROR(_bitmap, _status) (_bitmap &= ~(_status << 16))
+#define GET_FLASH_STATUS(_bitmap) (_bitmap & 0xffff)
+#define GET_FLASH_ERROR(_bitmap) (_bitmap >> 16)
+
+struct flash_mode_params {
+	unsigned long duration_uSecs;
+	unsigned long intensity_uAmp;
+	unsigned long timeout_uSecs;
+};
+
+struct flash_ioctl_args_t {
+	unsigned long flash_mode;
+	unsigned long cam;
+	unsigned long status;
+	union mode_arg{
+		struct flash_mode_details details;
+		struct flash_mode_params  params;
+		unsigned long strobe_enable;
+	} mode_arg;
+};
+
+#define FLASH_MAGIC_NUMBER 0x17
+#define FLASH_GET_MODES			_IOR(FLASH_MAGIC_NUMBER, 1,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_MODE_DETAILS		_IOWR(FLASH_MAGIC_NUMBER, 2,\
+struct flash_ioctl_args_t *)
+#define FLASH_ENABLE_MODE		_IOW(FLASH_MAGIC_NUMBER, 3,\
+struct flash_ioctl_args_t *)
+#define FLASH_DISABLE_MODE		_IOW(FLASH_MAGIC_NUMBER, 4,\
+struct flash_ioctl_args_t *)
+#define FLASH_CONFIGURE_MODE		_IOW(FLASH_MAGIC_NUMBER, 5,\
+struct flash_ioctl_args_t *)
+#define FLASH_TRIGGER_STROBE		_IOW(FLASH_MAGIC_NUMBER, 6,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_STATUS		_IOW(FLASH_MAGIC_NUMBER, 7,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_LIFE_COUNTER		_IOW(FLASH_MAGIC_NUMBER, 8,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_SELF_TEST_MODES	_IOR(FLASH_MAGIC_NUMBER, 9,\
+struct flash_ioctl_args_t *)
+#define FLASH_SELF_TEST			_IOW(FLASH_MAGIC_NUMBER, 10,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_FAULT_REGISTERS	_IOR(FLASH_MAGIC_NUMBER, 11,\
+struct flash_ioctl_args_t *)
+#define FLASH_GET_SELF_TEST_RESULT	_IOR(FLASH_MAGIC_NUMBER, 12,\
+struct flash_ioctl_args_t *)
+
+#endif
diff --git a/drivers/staging/camera_flash/camera_flash_bitfields.h b/drivers/staging/camera_flash/camera_flash_bitfields.h
new file mode 100644
index 00000000000..05da9c5ef58
--- /dev/null
+++ b/drivers/staging/camera_flash/camera_flash_bitfields.h
@@ -0,0 +1,83 @@
+/*
+ * 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. 
+ */
+/**
+* \file    camera_flash_bitfields.h
+* \brief   Define some constants for the flash drivers API.
+* \author  ST-Ericsson
+*/
+#ifndef __CAMERA_FLASH_BITFIELDS_H__
+#define __CAMERA_FLASH_BITFIELDS_H__
+
+/* Flash Mode definitions */
+/* All Operating Modes are off (shutdown low power state)*/
+#define FLASH_MODE_NONE				(0x000)
+/* Enables the xenon driver. Strobe is managed by the flash driver itself.
+Charges the xenon. Automatic periodic recharge is abstracted by the driver */
+#define FLASH_MODE_XENON			(0x001)
+/* Enables the xenon driver. Strobe is managed externally to the driver */
+#define FLASH_MODE_XENON_EXTERNAL_STROBE	(0x002)
+/* Enables the video led driver. Strobing is managed by the driver */
+#define FLASH_MODE_VIDEO_LED			(0x004)
+/* Enables the video led driver. Strobing is managed externally to driver */
+#define FLASH_MODE_VIDEO_LED_EXTERNAL_STROBE	(0x008)
+/* Enables the still LED driver. Strobing is managed by the driver itself */
+#define FLASH_MODE_STILL_LED			(0x010)
+/* Enables the still LED driver. Strobe is managed externally to the driver */
+#define FLASH_MODE_STILL_LED_EXTERNAL_STROBE	(0x020)
+/* Enables the AF assistant driver. Strobe is managed by the driver */
+#define FLASH_MODE_AF_ASSISTANT			(0x040)
+/* Enable the driver. Strobe is managed by the driver */
+#define FLASH_MODE_INDICATOR			(0x080)
+/* Enables the still HP LED driver. Strobing is managed by the driver itself */
+#define FLASH_MODE_STILL_HPLED			(0x100)
+/* Enables the still HP LED driver. Strobe is managed externally to the
+driver */
+#define FLASH_MODE_STILL_HPLED_EXTERNAL_STROBE	(0x200)
+
+
+/* The flash is not usable anymore */
+#define FLASH_STATUS_BROKEN     (0x00)
+/* The flash is ready to be fired and unlit */
+#define FLASH_STATUS_READY      (0x01)
+/* The flash is discharged and by construction, charging; usually an
+application shall not try to fire it in that state (although possible
+typically in sport mode flash) */
+#define FLASH_STATUS_NOT_READY  (0x02)
+/* The flash is in shutdown state */
+#define FLASH_STATUS_SHUTDOWN   (0x04)
+/* Intermediate state that may exist where I2C registers can be programmed */
+#define FLASH_STATUS_STANDBY    (0x08)
+/* The flash is already strobing */
+#define FLASH_STATUS_LIT        (0x10)
+
+#define FLASH_SELFTEST_NONE              0x000
+/* tests connections to flash driver ICs */
+#define FLASH_SELFTEST_CONNECTION        0x001
+/* tests capture flash without using strobe signal from camera */
+#define FLASH_SELFTEST_FLASH             0x002
+/* tests capture flash using strobe signal from camera: ONLY this one needs to
+be done in idle state from flash tests cases */
+#define FLASH_SELFTEST_FLASH_WITH_STROBE 0x004
+/* tests video light */
+#define FLASH_SELFTEST_VIDEO_LIGHT       0x008
+/* tests AF assistance light */
+#define FLASH_SELFTEST_AF_LIGHT          0x010
+/* tests capture indicator light */
+#define FLASH_SELFTEST_INDICATOR         0x020
+/* tests flash in torch mode */
+#define FLASH_SELFTEST_TORCH_LIGHT       0x040
+
+/** \brief Flash Error */
+enum TFlashError {
+	FLASH_ERR_NONE		, /* None */
+	FLASH_ERR_OVER_CHARGE	, /* Error happened during the charge */
+	FLASH_ERR_OVER_HEAT	, /* Over temperature */
+	FLASH_ERR_SHORT_CIRCUIT	, /* Short circuit */
+	FLASH_ERR_TIMEOUT	, /* Timeout */
+	FLASH_ERR_OVER_VOLTAGE	/* Over voltage */
+} ;
+
+#endif
diff --git a/drivers/staging/camera_flash/flash_common.c b/drivers/staging/camera_flash/flash_common.c
new file mode 100644
index 00000000000..fc59879a170
--- /dev/null
+++ b/drivers/staging/camera_flash/flash_common.c
@@ -0,0 +1,460 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * camera flash: Flash driver to export camera flash to user space application.
+ *               It supports two flashes, one for primary and one for secondary camera
+ * Author: Pankaj Chauhan/pankaj.chauhan@stericsson.com for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <linux/fs.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/kthread.h>
+#include <linux/jiffies.h>
+#include <linux/miscdevice.h>
+#include "camera_flash.h"
+#include "flash_common.h"
+
+#define DEBUG_LOG(...) printk(KERN_DEBUG "Camera Flash driver: " __VA_ARGS__)
+
+#define PRIMARY_CAMERA		(0)
+#define SECONDARY_CAMERA	(1)
+static struct miscdevice misc_dev;
+struct flash_chip *flash_chips[2];
+struct fasync_struct * async_queue;
+struct task_struct* ptaskStruct;
+wait_queue_head_t waitQueue;
+int waitCondition = 0;
+struct flash_ioctl_args_t flashArg;
+
+#define COPY_ARG_FROM_USER(_to,_from_usr)	do{												\
+	memset((_to),0,sizeof(struct flash_ioctl_args_t));													\
+	if (copy_from_user((_to), (struct flash_ioctl_args_t*) (_from_usr), sizeof(struct flash_ioctl_args_t))) {	\
+		DEBUG_LOG("Could not copy data from userspace successfully\n");							\
+		break;																					\
+	}																							\
+}while(0)
+
+#define COPY_ARG_TO_USER(_to_usr,_from) do{											\
+	if (copy_to_user((struct flash_ioctl_args_t *)(_to_usr), (_from), sizeof(struct flash_ioctl_args_t))) {	\
+		DEBUG_LOG("Could not copy data from userspace successfully\n");						\
+		break;																				\
+	}																						\
+}while(0)
+
+
+static long flash_ioctl(struct file *file_p, unsigned int cmd, unsigned long arg)
+{
+	int err=0;
+	struct flash_chip *flash_p = NULL;
+	struct flash_chip_ops *ops = NULL;
+	char *my_name=NULL;
+	struct flash_ioctl_args_t flash_arg;
+
+	if (_IOC_TYPE(cmd) != FLASH_MAGIC_NUMBER) {
+                printk(KERN_ALERT "Flash driver: Not an ioctl for this module\n");
+		err =  -EINVAL;
+	}
+
+	COPY_ARG_FROM_USER(&flash_arg,arg);
+
+	if(flash_arg.cam == SECONDARY_CAMERA || flash_arg.cam == PRIMARY_CAMERA)
+		flash_p = flash_chips[flash_arg.cam];
+	else{
+		DEBUG_LOG("unsupported cam %lu\n",flash_arg.cam);
+		err = -ENODEV;
+		goto out;
+	}
+	my_name = flash_arg.cam ?"Secondary":"Primary";
+
+	if (flash_arg.cam == PRIMARY_CAMERA)
+	{
+		ops = flash_p->ops;
+	}
+
+	switch(cmd){
+	case FLASH_GET_MODES:
+	{
+		if (flash_arg.cam == PRIMARY_CAMERA)
+		{
+			err  = ops->get_modes(flash_p->priv_data,&flash_arg.flash_mode);
+			if(!err){
+				DEBUG_LOG("Supported flash modes for %s camera: %lx\n",
+					my_name,flash_arg.flash_mode);
+				COPY_ARG_TO_USER(arg,&flash_arg);
+			}else{
+				DEBUG_LOG("unable to get supported modes for %s camera\n",my_name);
+			}
+		}
+		else
+		{
+			flash_arg.flash_mode = FLASH_MODE_NONE;
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}
+	}
+		break;
+	case FLASH_GET_MODE_DETAILS:
+	{
+		err = ops->get_mode_details(flash_p->priv_data,flash_arg.flash_mode,
+				&flash_arg.mode_arg.details);
+		if(!err){
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}else{
+			DEBUG_LOG("Unable to get mode details for %s camera, flash mode %lx\n",
+				my_name,flash_arg.flash_mode);
+		}
+	}
+		break;
+	case FLASH_ENABLE_MODE:
+	case FLASH_DISABLE_MODE:
+	{
+		int enable=0;
+		if(cmd == FLASH_ENABLE_MODE){
+			enable = 1;
+		}
+		err = ops->enable_flash_mode(flash_p->priv_data,flash_arg.flash_mode,enable);
+		if(err){
+			DEBUG_LOG("Unable to %s:  %s camera, flash mode %lx\n",
+				(enable ?"Enable":"Disable"), my_name,flash_arg.flash_mode);
+		}
+	}
+		break;
+	case FLASH_CONFIGURE_MODE:
+		err = ops->configure_flash_mode(flash_p->priv_data,flash_arg.flash_mode,
+				&flash_arg.mode_arg.params);
+		if(err){
+			DEBUG_LOG("Unable to configure  %s camera, flash mode %lx\n",
+				my_name,flash_arg.flash_mode);
+		}
+		break;
+	case FLASH_TRIGGER_STROBE:
+		err = ops->trigger_strobe(flash_p->priv_data,flash_arg.mode_arg.strobe_enable);
+		if(err){
+			DEBUG_LOG("Unable to %s:  %s camera strobe trigger, mode %lx\n",
+				(arg ?"Enable":"Disable"), my_name,flash_arg.flash_mode);
+		}
+		break;
+	case FLASH_GET_STATUS:
+		err = ops->get_status(flash_p->priv_data,&flash_arg.status);
+		if(!err){
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}else{
+			DEBUG_LOG("Unable to get status details for %s camera, flash mode %lx\n",
+				my_name,flash_arg.flash_mode);
+		}
+		 break;
+	case FLASH_GET_LIFE_COUNTER:
+		 DEBUG_LOG("Not Implemented\n");
+		 break;
+        case FLASH_SELF_TEST:
+		flashArg = flash_arg;
+		if (0 != (flashArg.cam & (FLASH_SELFTEST_FLASH | FLASH_SELFTEST_FLASH_WITH_STROBE)))
+		{
+			err = ENODEV;
+		}
+		else
+		{
+			/* wake up worker thread */
+			waitCondition = 1;
+			wake_up_interruptible(&waitQueue);
+		}
+		break;
+        case FLASH_GET_SELF_TEST_MODES:
+	{
+		if (flash_arg.cam == PRIMARY_CAMERA)
+		{
+			err  = ops->get_selftest_modes(flash_p->priv_data,&flash_arg.flash_mode);
+			if(!err){
+				COPY_ARG_TO_USER(arg,&flash_arg);
+			}else{
+				DEBUG_LOG("unable to get supported modes for %s camera\n",my_name);
+			}
+		}
+		else
+		{
+			flash_arg.flash_mode = FLASH_SELFTEST_NONE;
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}
+		break;
+	}
+        case FLASH_GET_FAULT_REGISTERS:
+	{
+		err  = ops->get_fault_registers(flash_p->priv_data,flash_arg.flash_mode,&flash_arg.status);
+		if(!err){
+			COPY_ARG_TO_USER(arg,&flash_arg);
+		}else{
+			DEBUG_LOG("unable to get supported modes for %s camera\n",my_name);
+		}
+
+		break;
+	}
+        case FLASH_GET_SELF_TEST_RESULT:
+	{
+		COPY_ARG_TO_USER(arg,&flashArg);
+		DEBUG_LOG("FLASH_GET_SELF_TEST_RESULT arg : 0x%lx\n", flashArg.status);
+		break;
+	}
+	default:
+		DEBUG_LOG("Unknown command %x\n",cmd);
+
+	}
+out:
+	return err;
+}
+
+int worker_thread (void* data)
+{
+	int err = 0;
+	struct flash_chip *flash_p=NULL;
+	struct flash_chip_ops *ops=NULL;
+	struct flash_mode_params params;
+	struct flash_mode_details details;
+
+	while (1)
+	{
+		/* waiting for some job to do */
+		wait_event_interruptible(waitQueue, (waitCondition != 0));
+		waitCondition = 0;
+
+		DEBUG_LOG("worker_thread wakes up\n");
+		/* do we need to stop ? */
+		err = kthread_should_stop();
+		if (0 != err)
+		{
+			DEBUG_LOG("worker_thread stops\n");
+			break;
+		}
+
+		/* do the job */
+		flash_p = flash_chips[flashArg.cam];
+		ops = flash_p->ops;
+
+		/* clear fault registers */
+		err  = ops->get_fault_registers(flash_p->priv_data, FLASH_MODE_INDICATOR, &flashArg.status);
+		if (0 != err)
+		{
+			flashArg.status = flashArg.flash_mode;
+			flashArg.flash_mode = 0;
+		}
+		flashArg.status = 0;
+
+		/* do all selftests */
+		while (flashArg.flash_mode != FLASH_SELFTEST_NONE)
+		{
+			if (0 != (flashArg.flash_mode & FLASH_SELFTEST_CONNECTION))
+			{
+				err = ops->get_mode_details(flash_p->priv_data, FLASH_MODE_INDICATOR, &details);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to get mode FLASH_MODE_INDICATOR details\n");
+					flashArg.status |= FLASH_SELFTEST_CONNECTION;
+				}
+				flashArg.flash_mode &= ~FLASH_SELFTEST_CONNECTION;
+			}
+			else if (0 != (flashArg.flash_mode & (FLASH_SELFTEST_FLASH | FLASH_SELFTEST_FLASH_WITH_STROBE)))
+			{
+				if (0 != (flashArg.flash_mode & FLASH_SELFTEST_FLASH))
+				{
+					flashArg.status |= FLASH_SELFTEST_FLASH;
+					flashArg.flash_mode &= ~FLASH_SELFTEST_FLASH;
+				}
+				else
+				{
+					flashArg.status |= FLASH_SELFTEST_FLASH_WITH_STROBE;
+					flashArg.flash_mode &= ~FLASH_SELFTEST_FLASH_WITH_STROBE;
+				}
+			}
+			/* FLASH_SELFTEST_VIDEO_LIGHT | FLASH_SELFTEST_AF_LIGHT | FLASH_SELFTEST_INDICATOR | FLASH_SELFTEST_TORCH_LIGHT */
+			else
+			{
+				unsigned long currentSelftest = FLASH_SELFTEST_NONE;
+				unsigned long currentFlashMode = FLASH_MODE_NONE;
+
+				if (0 != (flashArg.flash_mode & FLASH_SELFTEST_VIDEO_LIGHT))
+				{
+					currentSelftest = FLASH_SELFTEST_VIDEO_LIGHT;
+					currentFlashMode = FLASH_MODE_VIDEO_LED;
+				}
+				else if (0 != (flashArg.flash_mode & FLASH_SELFTEST_AF_LIGHT))
+				{
+					currentSelftest = FLASH_SELFTEST_AF_LIGHT;
+					currentFlashMode = FLASH_MODE_AF_ASSISTANT;
+				}
+				else if (0 != (flashArg.flash_mode & FLASH_SELFTEST_INDICATOR))
+				{
+					currentSelftest = FLASH_SELFTEST_INDICATOR;
+					currentFlashMode = FLASH_MODE_INDICATOR;
+				}
+				else
+				{
+					currentSelftest = FLASH_SELFTEST_TORCH_LIGHT;
+					currentFlashMode = FLASH_MODE_VIDEO_LED;
+				}
+
+				err = ops->get_mode_details(flash_p->priv_data, currentFlashMode, &details);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to get mode 0x%lx details\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+
+				err = ops->enable_flash_mode(flash_p->priv_data, currentFlashMode, 1);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to enable flash mode 0x%lx\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+
+				params.duration_uSecs = 0;
+				params.intensity_uAmp = details.max_intensity_uAmp;
+				params.timeout_uSecs = 0;
+				err = ops->configure_flash_mode(flash_p->priv_data, currentFlashMode, &params);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to configure flash mode 0x%lx\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+
+				err = ops->trigger_strobe(flash_p->priv_data,1);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to strobe, mode : 0x%lx\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+
+				wait_event_timeout(waitQueue, 0, msecs_to_jiffies(1000));
+
+				err = ops->trigger_strobe(flash_p->priv_data,0);
+				if (0 != err)
+				{
+					DEBUG_LOG("not able to strobe, mode : 0x%lx\n",currentFlashMode);
+					flashArg.status |= currentSelftest;
+					flashArg.flash_mode &= ~currentSelftest;
+					continue;
+				}
+				flashArg.flash_mode &= ~currentSelftest;
+			}
+		}
+
+		/* job's done ! */
+		flash_async_notify();
+	}
+	return 0;
+}
+
+int flash_open(struct inode *node, struct file *file_p)
+{
+	// init sleep queue
+	init_waitqueue_head(&waitQueue);
+
+	// start worker thread
+	ptaskStruct = kthread_run (&worker_thread, NULL, "flashDriverWorker");
+
+	return 0;
+}
+
+int register_flash_chip(unsigned int cam, struct flash_chip *flash_p)
+{
+	int err =0;
+	DEBUG_LOG("Registering cam %d\n", cam);
+	DEBUG_LOG("flash_p: name=%s\n", flash_p->name);
+	if(cam > 1 || !flash_p){
+		DEBUG_LOG("Registration: something is wrong! cam %d, flash_p %x \n",cam,(int)flash_p);
+		err = EINVAL;
+		goto out;
+	}
+	if(!flash_chips[cam]){
+		flash_chips[cam] = flash_p;
+		DEBUG_LOG("Registered flash: id %lx, %s for camera %d\n",
+			flash_p->id,flash_p->name,cam);
+	}else{
+		DEBUG_LOG("%s flash already registered for camera %d, ignore flash %s\n",
+		flash_chips[cam]->name,cam, flash_p->name);
+	}
+out:
+	return err;
+}
+
+int flash_async_notify ()
+{
+	kill_fasync(&async_queue, SIGIO, POLL_IN);
+	return 0;
+}
+
+static int flash_fasync(int fd, struct file *filp, int mode)
+{
+        DEBUG_LOG("registered async notification on %d fd\n",fd);
+        return fasync_helper(fd, filp, mode, &async_queue);
+}
+
+static int flash_release(struct inode *node, struct file *file_p)
+{
+	int err = 0;
+
+	fasync_helper(-1, file_p, 0, &async_queue);
+
+	// stop worker thread
+	waitCondition = 1;
+	err = kthread_stop(ptaskStruct);
+	return err;
+}
+
+static struct file_operations flash_fops = {
+	owner:THIS_MODULE,
+	unlocked_ioctl:flash_ioctl,
+	open:flash_open,
+	release:flash_release,
+        fasync:flash_fasync,
+};
+
+int  major_device_number;
+
+/*Temporary here (adp_init)*/
+extern int adp1653_init(void);
+static int __init flash_init(void)
+{
+	int err = 0;
+	err = adp1653_init();
+	if(err){
+		DEBUG_LOG("Unable to initialize adp1653, err %d\n",err);
+		goto out;
+	}
+	/* Register misc device */
+    	misc_dev.minor = MISC_DYNAMIC_MINOR;
+    	misc_dev.name = "camera_flash";
+    	misc_dev.fops = &flash_fops;
+    	err = misc_register(&misc_dev);
+	if (err < 0) {
+                printk(KERN_INFO "camera_flash driver misc_register failed (%d)\n", err);
+                return err;
+	} else {
+                major_device_number = err;
+		printk(KERN_INFO "camera_flash driver initialized with minor=%d\n", misc_dev.minor);
+	}
+out:
+	return err;
+}
+
+static void __exit flash_exit(void)
+{
+	misc_deregister(&misc_dev);
+	printk(KERN_INFO"Camera flash driver unregistered\n");
+}
+
+module_init(flash_init);
+module_exit(flash_exit);
+MODULE_LICENSE("GPL");
+EXPORT_SYMBOL(register_flash_chip);
+EXPORT_SYMBOL(flash_async_notify);
diff --git a/drivers/staging/camera_flash/flash_common.h b/drivers/staging/camera_flash/flash_common.h
new file mode 100755
index 00000000000..d1f63631e82
--- /dev/null
+++ b/drivers/staging/camera_flash/flash_common.h
@@ -0,0 +1,57 @@
+#ifndef __FLASH_COMMON_H__
+#define __FLASH_COMMON_H__
+
+#include "camera_flash_bitfields.h"
+#include "camera_flash.h"
+
+struct flash_chip_ops{
+	int (*get_modes)( void *priv_data, unsigned long *modes);
+	int (*get_mode_details)(void *priv_data,unsigned long mode,
+				struct flash_mode_details *details_p);
+	int (*enable_flash_mode) (void *priv_data,unsigned long mode,
+		int enable);
+	int (*configure_flash_mode) (void *priv_data, unsigned long mode,
+				struct flash_mode_params *params_p);
+	int (*trigger_strobe) (void *priv_data, int enable);
+	int (*get_life_counter) (void *priv_data);
+	int (*get_status)	(void *priv_data, unsigned long *status);
+	int (*get_selftest_modes)	(void *priv_data,
+		unsigned long *modes);
+	int (*get_fault_registers)	(void *priv_data, unsigned long mode,
+		unsigned long *status);
+};
+
+#define FLASH_TYPE_XENON	(0x1)
+#define FLASH_TYPE_HPLED	(0x2)
+
+#define SET_FLASHCHIP_TYPE(flash_chip_p,_TYPE) ((flash_chip_p)->id |= _TYPE)
+#define GET_FLASHHIP_TYPE(flash_chip_p) ((flash_chip_p)->id & 0xffff)
+#define GET_FLASHCHIP_ID(flash_chip_p)  ((flash_chip_p)->id >> 16)
+#define SET_FLASHCHIP_ID(flash_chip_p,_ID) ((flash_chip_p)->id |= (_ID << 16))
+
+struct flash_chip {
+	unsigned long id;
+	struct flash_chip_ops *ops;
+	void *priv_data;
+	unsigned char name[FLASH_NAME_SIZE];
+};
+
+/**
+ * struct flash_platform_data:
+ * platform specific data For flash chip driver
+ * @cam : 0 - primary, 1 - secondary
+ * @strobe_gpio: GPIO used as strobe
+ * @enable_gpio: GPIO used for enable/reset input
+ */
+struct flash_platform_data{
+	unsigned long cam;
+	unsigned long strobe_gpio;
+	unsigned long strobe_gpio_alt_func;
+	unsigned long enable_gpio;
+	unsigned long enable_gpio_alt_func;
+};
+
+extern int register_flash_chip(unsigned int cam, struct flash_chip *flash_p);
+extern int flash_async_notify (void );
+
+#endif
diff --git a/drivers/staging/cg2900/Kconfig b/drivers/staging/cg2900/Kconfig
new file mode 100644
index 00000000000..92046b9bf76
--- /dev/null
+++ b/drivers/staging/cg2900/Kconfig
@@ -0,0 +1,73 @@
+#
+# CG2900
+#
+
+config CG2900
+	tristate "Support ST-Ericsson CG2900 main structure"
+	depends on NET && HAS_IOMEM
+	select MFD_CORE
+	help
+	  ST-Ericsson CG2900 Connectivity Combo controller main
+	  structure.
+	  Supports multiple functionalities muxed over a Bluetooth HCI H:4
+	  interface.
+	  CG2900 support Bluetooth, FM radio, and GPS.
+
+config CG2900_CHIP
+	tristate "Support CG2900 Connectivity controller"
+	depends on CG2900
+	help
+	  ST-Ericsson CG2900 Connectivity Controller chip handler.
+	  Contains chip handler performing driver initialization
+	  such as patchdownload and also instantiates the supported
+	  MFD devices.
+
+config STLC2690_CHIP
+	tristate "Support STLC2690 Connectivity controller"
+	depends on CG2900
+	help
+	  ST-Ericsson STLC2690 Connectivity Controller chip handler.
+	  Contains chip handler performing driver initialization
+	  such as patchdownload and also instantiates the supported
+	  MFD devices.
+
+config CG2900_UART
+	tristate "Support CG2900 UART transport"
+	depends on CG2900
+	select BT
+	select BT_HCIUART
+	help
+	  UART driver for ST-Ericsson CG2900 Connectivity Controller.
+	  Contains functions for setting baud rate and to transport
+	  data to and from the CG2900 controller over UART.
+	  Also handles low power handling for the CG2900 when using UART as
+	  transport.
+
+config CG2900_AUDIO
+	tristate "Support CG2900 audio interface"
+	depends on CG2900
+	help
+	  ST-Ericsson CG2900 Connectivity audio interface driver.
+	  Gives a module the ability to setup audio paths
+	  within the CG2900 controller.
+	  Supports both a normal function API and using character device
+	  from user space.
+
+config CG2900_TEST
+	tristate "Support CG2900 Test Char Device"
+	depends on CG2900
+	help
+	  ST-Ericsson CG2900 Test Character Device driver.
+	  Creates a character device which can be used by
+	  a test framework in user space to emulate a connected chip.
+	  Note that this is used to test the chip handler driver,
+	  not to test the connected chip.
+
+config BT_CG2900
+	tristate "ST-Ericsson CG2900 Bluetooth driver"
+	depends on CG2900 && BT
+	help
+	  Select if ST-Ericsson CG2900 Connectivity controller shall be used as
+	  Bluetooth controller for BlueZ.
+	  This driver registers to the Bluetooth stack and when opened,
+	  enables the CG2900 controller in a proper way.
diff --git a/drivers/staging/cg2900/Makefile b/drivers/staging/cg2900/Makefile
new file mode 100644
index 00000000000..14e2847bacf
--- /dev/null
+++ b/drivers/staging/cg2900/Makefile
@@ -0,0 +1,15 @@
+#
+# Makefile for ST-Ericsson CG2900 connectivity combo controller
+#
+
+ccflags-y :=					\
+	-Idrivers/staging/cg2900/include \
+	-Iarch/arm/mach-ux500
+
+obj-$(CONFIG_CG2900)	+= devices-cg2900.o \
+				devices-cg2900-ux500.o \
+				board-ux500-cg2900.o \
+				clock-cg2900.o
+
+obj-y				+= mfd/
+obj-y				+= bluetooth/
diff --git a/drivers/staging/cg2900/TODO b/drivers/staging/cg2900/TODO
new file mode 100644
index 00000000000..e122eba6d53
--- /dev/null
+++ b/drivers/staging/cg2900/TODO
@@ -0,0 +1,23 @@
+TODO
+----
+
+ - Decide upon main driver architecture.
+
+ - Decide if the CG2900 driver should be a separate driver as today or if it
+   should be a sub-driver using the TI-ST (Shared Transport) driver that is also
+   written for a combo connectivity controller.
+
+ - Decide if cg2900_uart should register on top of hci_ldisc.c (as now) or if it
+   should instead register on top of hci_h4.c thereby reusing hci_h4
+   implementation.
+
+ - Update the hci_ldisc.c so that it will allow drivers to be registered without
+   registering them directly to the Bluetooth stack. Also extend the hci_ldisc.c
+   with more functions to abstract the tty API in a conformative way (currently
+   sometimes the tty API used, sometimes the hci_ldisc interface).
+
+ - Decide if the CG2900 driver should use imported structs and defines to create
+   Bluetooth packets as today or if the Bluetooth stack in the Kernel should be
+   extended so it is possible to use generic functions to send and receive
+   commands and events both from the Bluetooth stack itself and from external
+   drivers such as the CG2900 driver.
diff --git a/drivers/staging/cg2900/bluetooth/Makefile b/drivers/staging/cg2900/bluetooth/Makefile
new file mode 100644
index 00000000000..936a4a257da
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for ST-Ericsson CG2900 connectivity combo controller
+#
+
+ccflags-y :=					\
+	-Idrivers/staging/cg2900/include
+
+obj-$(CONFIG_BT_CG2900)		+= btcg2900.o
+obj-$(CONFIG_CG2900_UART)	+= cg2900_uart.o hci_ldisc.o
diff --git a/drivers/staging/cg2900/bluetooth/btcg2900.c b/drivers/staging/cg2900/bluetooth/btcg2900.c
new file mode 100644
index 00000000000..cebd5872222
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/btcg2900.c
@@ -0,0 +1,1195 @@
+/*
+ * Bluetooth driver for ST-Ericsson CG2900 connectivity controller.
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com)
+ * Henrik Possung (henrik.possung@stericsson.com)
+ * Josef Kindberg (josef.kindberg@stericsson.com)
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com)
+ * Kjell Andersson (kjell.k.andersson@stericsson.com)
+ *
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <asm/byteorder.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/sched.h>
+#include <linux/platform_device.h>
+#include <linux/skbuff.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/core.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "cg2900.h"
+
+#define BT_HEADER_LENGTH		0x03
+
+#define NAME				"BTCG2900 "
+
+/* Wait for 5 seconds for a response to our requests */
+#define RESP_TIMEOUT			5000
+
+/* Bluetooth error codes */
+#define HCI_ERR_NO_ERROR		0x00
+#define HCI_ERR_CMD_DISALLOWED		0x0C
+
+/**
+ * enum reset_state - RESET-states of the HCI driver.
+ *
+ * @RESET_IDLE:		No reset in progress.
+ * @RESET_ACTIVATED:	Reset in progress.
+ * @RESET_UNREGISTERED:	hdev is unregistered.
+ */
+
+enum reset_state {
+	RESET_IDLE,
+	RESET_ACTIVATED,
+	RESET_UNREGISTERED
+};
+
+/**
+ * enum enable_state - ENABLE-states of the HCI driver.
+ *
+ * @ENABLE_IDLE:			The HCI driver is loaded but not opened.
+ * @ENABLE_WAITING_BT_ENABLED_CC:	The HCI driver is waiting for a command
+ *					complete event from the BT chip as a
+ *					response to a BT Enable (true) command.
+ * @ENABLE_BT_ENABLED:			The BT chip is enabled.
+ * @ENABLE_WAITING_BT_DISABLED_CC:	The HCI driver is waiting for a command
+ *					complete event from the BT chip as a
+ *					response to a BT Enable (false) command.
+ * @ENABLE_BT_DISABLED:			The BT chip is disabled.
+ * @ENABLE_BT_ERROR:			The HCI driver is in a bad state, some
+ *					thing has failed and is not expected to
+ *					work properly.
+ */
+enum enable_state {
+	ENABLE_IDLE,
+	ENABLE_WAITING_BT_ENABLED_CC,
+	ENABLE_BT_ENABLED,
+	ENABLE_WAITING_BT_DISABLED_CC,
+	ENABLE_BT_DISABLED,
+	ENABLE_BT_ERROR
+};
+
+/* Defines which state the driver has when BT is active */
+#define BTCG2900_ACTIVE_STATE		ENABLE_BT_ENABLED
+
+/**
+ * struct btcg2900_info - Specifies HCI driver private data.
+ *
+ * This type specifies CG2900 HCI driver private data.
+ *
+ * @list:		list_head struct.
+ * @parent:		Parent to this BT device. All BT channels will have
+ *			common parent.
+ * @cmd:		Device structure for BT command channel.
+ * @evt:		Device structure for BT event channel.
+ * @acl:		Device structure for BT ACL channel.
+ * @pdev:		Device structure for platform device.
+ * @hdev:		Device structure for HCI device.
+ * @reset_state:	Device enum for HCI driver reset state.
+ * @enable_state:	Device enum for HCI driver BT enable state.
+ */
+struct btcg2900_info {
+	struct list_head	list;
+	struct device		*parent;
+	struct device		*cmd;
+	struct device		*evt;
+	struct device		*acl;
+	struct hci_dev		*hdev;
+	enum reset_state	reset_state;
+	enum enable_state	enable_state;
+};
+
+/**
+ * struct enable_info - Specifies data for sending enable commands.
+ *
+ * @enable:		True if command should enable the functionality.
+ * @name:		Name of the command, only informative.
+ * @get_cmd:		Function for retrieving command.
+ * @success:		State to set upon success.
+ * @awaiting_cc:	State to set while waiting for response.
+ * @failed:		State to set upon failure.
+ */
+struct enable_info {
+	bool	enable;
+	char	*name;
+	struct sk_buff* (*get_cmd)(struct btcg2900_info *info, bool enable);
+	enum enable_state success;
+	enum enable_state awaiting_cc;
+	enum enable_state failed;
+};
+
+/**
+ * struct dev_info - Specifies private data used when receiving callbacks from CG2900 driver.
+ *
+ * @hci_data_type:	Type of data according to BlueZ.
+ */
+struct dev_info {
+	u8	hci_data_type;
+};
+
+/* Defines for vs_bt_enable_cmd */
+#define BT_VS_BT_ENABLE			0xFF10
+#define VS_BT_DISABLE			0x00
+#define VS_BT_ENABLE			0x01
+
+/**
+ * struct vs_bt_enable_cmd - Specifies HCI VS Bluetooth_Enable command.
+ *
+ * @op_code:	HCI command op code.
+ * @len:	Parameter length of command.
+ * @enable:	0 for disable BT, 1 for enable BT.
+ */
+struct vs_bt_enable_cmd {
+	__le16	op_code;
+	u8	len;
+	u8	enable;
+} __packed;
+
+/*
+ * hci_wait_queue - Main Wait Queue in HCI driver.
+ */
+static DECLARE_WAIT_QUEUE_HEAD(hci_wait_queue);
+
+/*
+ * btcg2900_devices - List of active CG2900 BT devices.
+ */
+static LIST_HEAD(btcg2900_devices);
+
+/* Internal function declarations */
+static int register_bluetooth(struct btcg2900_info *info);
+
+/* Internal functions */
+
+/**
+ * get_bt_enable_cmd() - Get HCI BT enable command.
+ * @info:	Device info structure.
+ * @bt_enable:	true if Bluetooth IP shall be enabled, false otherwise.
+ *
+ * Returns:
+ *   NULL if no command shall be sent,
+ *   sk_buffer with command otherwise.
+ */
+static struct sk_buff *get_bt_enable_cmd(struct btcg2900_info *info,
+					 bool bt_enable)
+{
+	struct sk_buff *skb;
+	struct vs_bt_enable_cmd *cmd;
+	struct cg2900_rev_data rev_data;
+	struct cg2900_user_data *pf_data;
+
+	pf_data = dev_get_platdata(info->cmd);
+
+	if (!pf_data->get_local_revision(pf_data, &rev_data)) {
+		BT_ERR(NAME "Couldn't get revision");
+		return NULL;
+	}
+
+	/* If connected chip does not support the command return NULL */
+	if (!check_chip_revision_support(rev_data.revision)) {
+		BT_ERR(NAME "Unsupported Chip revision:0x%x\n",
+				rev_data.revision);
+		return NULL;
+	}
+
+	/* CG2900 used */
+	skb = pf_data->alloc_skb(sizeof(*cmd), GFP_KERNEL);
+	if (!skb) {
+		BT_ERR(NAME "Could not allocate skb");
+		return NULL;
+	}
+
+	cmd = (struct vs_bt_enable_cmd *)skb_put(skb, sizeof(*cmd));
+	cmd->op_code = cpu_to_le16(BT_VS_BT_ENABLE);
+	cmd->len = sizeof(*cmd) - BT_HEADER_LENGTH;
+	if (bt_enable)
+		cmd->enable = VS_BT_ENABLE;
+	else
+		cmd->enable = VS_BT_DISABLE;
+
+	return skb;
+}
+
+/**
+ * close_bt_users() - Close all BT channels.
+ * @info:	HCI driver info structure.
+ */
+static void close_bt_users(struct btcg2900_info *info)
+{
+	struct cg2900_user_data *pf_data;
+
+	pf_data = dev_get_platdata(info->cmd);
+	if (pf_data->opened)
+		pf_data->close(pf_data);
+
+	pf_data = dev_get_platdata(info->acl);
+	if (pf_data->opened)
+		pf_data->close(pf_data);
+
+	pf_data = dev_get_platdata(info->evt);
+	if (pf_data->opened)
+		pf_data->close(pf_data);
+}
+
+/**
+ * handle_bt_enable_comp() - Handle received BtEnable Complete event.
+ * @info:	Info structure.
+ * @skb:	Buffer with data coming from device.
+ *
+ * Returns:
+ *   true if data has been handled internally,
+ *   false otherwise.
+ */
+static bool handle_bt_enable_comp(struct btcg2900_info *info, u8 status)
+{
+	if (info->enable_state != ENABLE_WAITING_BT_ENABLED_CC &&
+	    info->enable_state != ENABLE_WAITING_BT_DISABLED_CC)
+		return false;
+	/*
+	 * This is the command complete event for
+	 * the HCI_Cmd_VS_Bluetooth_Enable.
+	 * Check result and update state.
+	 *
+	 * The BT chip is enabled/disabled. Either it was enabled/
+	 * disabled now (status NO_ERROR) or it was already enabled/
+	 * disabled (assuming status CMD_DISALLOWED is already enabled/
+	 * disabled).
+	 */
+	if (status != HCI_ERR_NO_ERROR && status != HCI_ERR_CMD_DISALLOWED) {
+		BT_ERR(NAME "Could not enable/disable BT core (0x%X)",
+			   status);
+		BT_DBG("New enable_state: ENABLE_BT_ERROR");
+		info->enable_state = ENABLE_BT_ERROR;
+		goto finished;
+	}
+
+	if (info->enable_state == ENABLE_WAITING_BT_ENABLED_CC) {
+		BT_DBG("New enable_state: ENABLE_BT_ENABLED");
+		info->enable_state = ENABLE_BT_ENABLED;
+		BT_INFO("CG2900 BT core is enabled");
+	} else {
+		BT_DBG("New enable_state: ENABLE_BT_DISABLED");
+		info->enable_state = ENABLE_BT_DISABLED;
+		BT_INFO("CG2900 BT core is disabled");
+	}
+
+finished:
+	/* Wake up whoever is waiting for this result. */
+	wake_up_all(&hci_wait_queue);
+	return true;
+}
+
+/**
+ * handle_bt_enable_stat() - Handle received BtEnable Status event.
+ * @info:	Info structure.
+ * @skb:	Buffer with data coming from device.
+ *
+ * Returns:
+ *   true if data has been handled internally,
+ *   false otherwise.
+ */
+static bool handle_bt_enable_stat(struct btcg2900_info *info, u8 status)
+{
+	if (info->enable_state != ENABLE_WAITING_BT_DISABLED_CC &&
+	    info->enable_state != ENABLE_WAITING_BT_ENABLED_CC)
+		return false;
+
+	BT_DBG("HCI Driver received Command Status (BT enable): 0x%X", status);
+	/*
+	 * This is the command status event for the HCI_Cmd_VS_Bluetooth_Enable.
+	 * Just free the packet.
+	 */
+	return true;
+}
+
+/**
+ * handle_rx_evt() - Check if received data is response to internal command.
+ * @info:	Info structure.
+ * @skb:	Buffer with data coming from device.
+ *
+ * Returns:
+ *   true if data has been handled internally,
+ *   false otherwise.
+ */
+static bool handle_rx_evt(struct btcg2900_info *info, struct sk_buff *skb)
+{
+	struct hci_event_hdr *evt = (struct hci_event_hdr *)skb->data;
+	struct hci_ev_cmd_complete *cmd_complete;
+	struct hci_ev_cmd_status *cmd_status;
+	u16 op_code;
+	u8 status;
+	bool pkt_handled = false;
+
+	/* If BT is active no internal packets shall be generated */
+	if (info->enable_state == BTCG2900_ACTIVE_STATE)
+		return false;
+
+	if (evt->evt == HCI_EV_CMD_COMPLETE) {
+		cmd_complete = (struct hci_ev_cmd_complete *)(evt + 1);
+		status = *((u8 *)(cmd_complete + 1));
+		op_code = le16_to_cpu(cmd_complete->opcode);
+
+		if (op_code == BT_VS_BT_ENABLE)
+			pkt_handled = handle_bt_enable_comp(info, status);
+	} else if (evt->evt == HCI_EV_CMD_STATUS) {
+		cmd_status = (struct hci_ev_cmd_status *)(evt + 1);
+		op_code = le16_to_cpu(cmd_status->opcode);
+		status = cmd_status->status;
+
+		if (op_code == BT_VS_BT_ENABLE)
+			pkt_handled = handle_bt_enable_stat(info, status);
+	}
+
+	if (pkt_handled)
+		kfree_skb(skb);
+
+	return pkt_handled;
+}
+
+/**
+ * hci_read_cb() - Callback for handling data received from CG2900 driver.
+ * @dev:	Device receiving data.
+ * @skb:	Buffer with data coming from device.
+ */
+static void hci_read_cb(struct cg2900_user_data *user, struct sk_buff *skb)
+{
+	int err = 0;
+	struct dev_info *dev_info;
+	struct btcg2900_info *info;
+
+	dev_info = cg2900_get_usr(user);
+	info = dev_get_drvdata(user->dev);
+
+	if (user->dev != info->evt || !handle_rx_evt(info, skb)) {
+		bt_cb(skb)->pkt_type = dev_info->hci_data_type;
+		skb->dev = (struct net_device *)info->hdev;
+		/* Update BlueZ stats */
+		info->hdev->stat.byte_rx += skb->len;
+		if (bt_cb(skb)->pkt_type == HCI_ACLDATA_PKT)
+			info->hdev->stat.acl_rx++;
+		else
+			info->hdev->stat.evt_rx++;
+
+		BT_DBG("Data receive %d bytes", skb->len);
+
+		/* Provide BlueZ with received frame*/
+		err = hci_recv_frame(skb);
+		/* If err, skb have been freed in hci_recv_frame() */
+		if (err)
+			BT_ERR(NAME "Failed in supplying packet to Bluetooth"
+			       " stack (%d)", err);
+	}
+}
+
+/**
+ * hci_reset_cb() - Callback for handling reset from CG2900 driver.
+ * @dev:	CPD device resetting.
+ */
+static void hci_reset_cb(struct cg2900_user_data *dev)
+{
+	int err;
+	struct btcg2900_info *info;
+	struct cg2900_user_data *pf_data;
+
+	BT_INFO(NAME "hci_reset_cb");
+
+	info = dev_get_drvdata(dev->dev);
+
+	BT_DBG("New reset_state: RESET_ACTIVATED");
+	info->reset_state = RESET_ACTIVATED;
+
+	/*
+	 * Continue to deregister hdev if all channels has been reset else
+	 * return.
+	 */
+	pf_data = dev_get_platdata(info->acl);
+	if (pf_data->opened)
+		return;
+	pf_data = dev_get_platdata(info->cmd);
+	if (pf_data->opened)
+		return;
+	pf_data = dev_get_platdata(info->evt);
+	if (pf_data->opened)
+		return;
+
+	/*
+	 * Deregister HCI device. Close and Destruct functions should
+	 * in turn be called by BlueZ.
+	 */
+	BT_DBG("Deregister HCI device");
+	hci_unregister_dev(info->hdev);
+
+	wait_event_timeout(hci_wait_queue,
+			(RESET_UNREGISTERED == info->reset_state),
+			msecs_to_jiffies(RESP_TIMEOUT));
+	if (RESET_UNREGISTERED != info->reset_state)
+		/*
+		 * Now we are in trouble. Try to register a new hdev
+		 * anyway even though this will cost some memory.
+		 */
+		BT_ERR(NAME "Timeout expired. Could not deregister HCI device");
+
+	/* Init and register hdev */
+	BT_DBG("Register HCI device");
+	err = register_bluetooth(info);
+	if (err)
+		BT_ERR(NAME "HCI Device registration error (%d)", err);
+}
+
+/**
+ * send_enable_cmd() - Send a command with only enable/disable functionality.
+ * @info:	Info structure.
+ * @en_info:	Enable info structure.
+ *
+ * Returns:
+ *   0 if successful,
+ *   -EACCES if correct response to command is not received,
+ *   Error codes from CG2900 write.
+ */
+static int send_enable_cmd(struct btcg2900_info *info,
+			   struct enable_info *en_info)
+{
+	struct sk_buff *enable_cmd;
+	int err;
+	struct cg2900_user_data *pf_data;
+
+	/*
+	 * Call function that returns the chip dependent enable HCI command.
+	 * If NULL is returned, then no bt_enable command should be sent to the
+	 * chip.
+	 */
+	enable_cmd = en_info->get_cmd(info, en_info->enable);
+	if (!enable_cmd) {
+		BT_DBG("%s New enable_state: %d", en_info->name,
+		       en_info->success);
+		info->enable_state = en_info->success;
+		return 0;
+	}
+
+	/* Set the HCI state before sending command to chip. */
+	BT_DBG("%s New enable_state: %d", en_info->name, en_info->awaiting_cc);
+	info->enable_state = en_info->awaiting_cc;
+
+	/* Send command to chip */
+	pf_data = dev_get_platdata(info->cmd);
+	err = pf_data->write(pf_data, enable_cmd);
+	if (err) {
+		BT_ERR("Couldn't send %s command (%d)", en_info->name, err);
+		kfree_skb(enable_cmd);
+		info->enable_state = en_info->failed;
+		return err;
+	}
+
+	/*
+	 * Wait for callback to receive command complete and then wake us up
+	 * again.
+	 */
+	wait_event_timeout(hci_wait_queue,
+				info->enable_state == en_info->success,
+				msecs_to_jiffies(RESP_TIMEOUT));
+	/* Check the current state to see if it worked */
+	if (info->enable_state != en_info->success) {
+		BT_ERR("Could not change %s state (%d)",
+		       en_info->name, info->enable_state);
+		BT_DBG("%s New enable_state: %d", en_info->name,
+		       en_info->failed);
+		info->enable_state = en_info->failed;
+		return -EACCES;
+	}
+
+	return 0;
+}
+
+/**
+ * btcg2900_open() - Open HCI interface.
+ * @hdev:	HCI device being opened.
+ *
+ * BlueZ callback function for opening HCI interface to device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL pointer is supplied.
+ *   -EOPNOTSUPP if supplied packet type is not supported.
+ *   -EBUSY if device is already opened.
+ *   -EACCES if opening of channels failed.
+ */
+static int btcg2900_open(struct hci_dev *hdev)
+{
+	struct btcg2900_info *info;
+	struct cg2900_user_data *pf_data;
+	int err;
+	struct enable_info en_info;
+
+	BT_INFO("Open ST-Ericsson CG2900 driver");
+
+	if (!hdev) {
+		BT_ERR(NAME "NULL supplied for hdev");
+		return -EINVAL;
+	}
+
+	info = hci_get_drvdata(hdev);
+	if (!info) {
+		BT_ERR(NAME "NULL supplied for driver_data");
+		return -EINVAL;
+	}
+
+	if (test_and_set_bit(HCI_RUNNING, &(hdev->flags))) {
+		BT_ERR(NAME "Device already opened!");
+		return -EBUSY;
+	}
+
+	pf_data = dev_get_platdata(info->acl);
+	err = pf_data->open(pf_data);
+	if (err) {
+		BT_ERR("Couldn't open BT ACL channel (%d)", err);
+		goto handle_error;
+	}
+
+	pf_data = dev_get_platdata(info->cmd);
+	err = pf_data->open(pf_data);
+	if (err) {
+		BT_ERR("Couldn't open BT CMD channel (%d)", err);
+		goto handle_error;
+	}
+
+	pf_data = dev_get_platdata(info->evt);
+	err = pf_data->open(pf_data);
+	if (err) {
+		BT_ERR("Couldn't open BT EVT channel (%d)", err);
+		goto handle_error;
+	}
+
+	if (info->reset_state == RESET_ACTIVATED) {
+		BT_DBG("New reset_state: RESET_IDLE");
+		info->reset_state = RESET_IDLE;
+	}
+
+	/* First enable the BT core */
+	en_info.enable = true;
+	en_info.get_cmd = get_bt_enable_cmd;
+	en_info.name = "VS BT Enable (true)";
+	en_info.success = ENABLE_BT_ENABLED;
+	en_info.awaiting_cc = ENABLE_WAITING_BT_ENABLED_CC;
+	en_info.failed = ENABLE_BT_DISABLED;
+
+	err = send_enable_cmd(info, &en_info);
+	if (err) {
+		BT_ERR("Couldn't enable BT core (%d)", err);
+		goto handle_error;
+	}
+
+	return 0;
+
+handle_error:
+	close_bt_users(info);
+	clear_bit(HCI_RUNNING, &(hdev->flags));
+	return err;
+
+}
+
+/**
+ * btcg2900_close() - Close HCI interface.
+ * @hdev:	HCI device being closed.
+ *
+ * BlueZ callback function for closing HCI interface.
+ * It flushes the interface first.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL pointer is supplied.
+ *   -EOPNOTSUPP if supplied packet type is not supported.
+ *   -EBUSY if device is not opened.
+ */
+static int btcg2900_close(struct hci_dev *hdev)
+{
+	struct btcg2900_info *info = NULL;
+	int err;
+	struct enable_info en_info;
+
+	BT_DBG("btcg2900_close");
+
+	if (!hdev) {
+		BT_ERR(NAME "NULL supplied for hdev");
+		return -EINVAL;
+	}
+
+	info = hci_get_drvdata(hdev);
+	if (!info) {
+		BT_ERR(NAME "NULL supplied for driver_data");
+		return -EINVAL;
+	}
+
+	if (!test_and_clear_bit(HCI_RUNNING, &(hdev->flags))) {
+		BT_ERR(NAME "Device already closed!");
+		return -EBUSY;
+	}
+
+	/* Do not do this if there is an reset ongoing */
+	if (info->reset_state == RESET_ACTIVATED)
+		goto remove_users;
+
+	/* Now disable the BT core */
+	en_info.enable = false;
+	en_info.get_cmd = get_bt_enable_cmd;
+	en_info.name = "VS BT Enable (false)";
+	en_info.success = ENABLE_BT_DISABLED;
+	en_info.awaiting_cc = ENABLE_WAITING_BT_DISABLED_CC;
+	en_info.failed = ENABLE_BT_ENABLED;
+
+	err = send_enable_cmd(info, &en_info);
+	if (err)
+		BT_ERR("Couldn't disable BT core (%d)", err);
+
+remove_users:
+	/* Finally deregister all users and free allocated data */
+	close_bt_users(info);
+	return 0;
+}
+
+/**
+ * btcg2900_send() - Send packet to device.
+ * @skb:	sk buffer to be sent.
+ *
+ * BlueZ callback function for sending sk buffer.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL pointer is supplied.
+ *   -EOPNOTSUPP if supplied packet type is not supported.
+ *   Error codes from cg2900_write.
+ */
+static int btcg2900_send(struct sk_buff *skb)
+{
+	struct hci_dev *hdev;
+	struct btcg2900_info *info;
+	struct cg2900_user_data *pf_data;
+	int err = 0;
+
+	if (!skb) {
+		BT_ERR(NAME "NULL supplied for skb");
+		return -EINVAL;
+	}
+
+	hdev = (struct hci_dev *)(skb->dev);
+	if (!hdev) {
+		BT_ERR(NAME "NULL supplied for hdev");
+		return -EINVAL;
+	}
+
+	info = hci_get_drvdata(hdev);
+	if (!info) {
+		BT_ERR(NAME "NULL supplied for info");
+		return -EINVAL;
+	}
+
+	/* Update BlueZ stats */
+	hdev->stat.byte_tx += skb->len;
+
+	BT_DBG("Data transmit %d bytes", skb->len);
+
+	switch (bt_cb(skb)->pkt_type) {
+	case HCI_COMMAND_PKT:
+		BT_DBG("Sending HCI_COMMAND_PKT");
+		pf_data = dev_get_platdata(info->cmd);
+		err = pf_data->write(pf_data, skb);
+		hdev->stat.cmd_tx++;
+		break;
+	case HCI_ACLDATA_PKT:
+		BT_DBG("Sending HCI_ACLDATA_PKT");
+		pf_data = dev_get_platdata(info->acl);
+		err = pf_data->write(pf_data, skb);
+		hdev->stat.acl_tx++;
+		break;
+	default:
+		BT_ERR(NAME "Trying to transmit unsupported packet type"
+		       " (0x%.2X)", bt_cb(skb)->pkt_type);
+		err = -EOPNOTSUPP;
+		break;
+	};
+
+	return err;
+}
+
+/**
+ * btcg2900_destruct() - Destruct HCI interface.
+ * @hdev:	HCI device being destructed.
+ */
+static void btcg2900_destruct(struct hci_dev *hdev)
+{
+	struct btcg2900_info *info;
+
+	BT_DBG("btcg2900_destruct");
+
+	info = hci_get_drvdata(hdev);
+	if (!info) {
+		BT_ERR(NAME "NULL supplied for info");
+		return;
+	}
+
+	/*
+	 * When destruct is called it means that the Bluetooth stack is done
+	 * with the HCI device and we can now free it.
+	 * Normally we do this only when removing the whole module through
+	 * btcg2900_remove(), but when being reset we free the device here and
+	 * we then set the reset state so that the reset handler can allocate a
+	 * new HCI device and then register it to the Bluetooth stack.
+	 */
+	if (info->reset_state == RESET_ACTIVATED) {
+		if (info->hdev) {
+			hci_free_dev(info->hdev);
+			info->hdev = NULL;
+		}
+		BT_DBG("New reset_state: RESET_UNREGISTERED");
+		info->reset_state = RESET_UNREGISTERED;
+		wake_up_all(&hci_wait_queue);
+	}
+}
+
+/**
+ * get_info() - Return info structure for this device.
+ * @dev:	Current device.
+ *
+ * Returns:
+ *   Pointer to info struct if there is no error.
+ *   ERR_PTR(-ENOMEM) if allocation fails.
+ */
+static struct btcg2900_info *get_info(struct device *dev)
+{
+	struct list_head *cursor;
+	struct btcg2900_info *tmp;
+	struct btcg2900_info *info = NULL;
+
+	/* Find the info structure */
+	list_for_each(cursor, &btcg2900_devices) {
+		tmp = list_entry(cursor, struct btcg2900_info, list);
+		if (tmp->parent == dev->parent) {
+			info = tmp;
+			break;
+		}
+	}
+
+	if (info)
+		return info;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		BT_ERR("Could not allocate info struct");
+		return ERR_PTR(-ENOMEM);
+	}
+	info->parent = dev->parent;
+	list_add_tail(&info->list, &btcg2900_devices);
+	BT_DBG("CG2900 device added");
+	return info;
+}
+
+/**
+ * device_removed() - Remove device from list if there are no channels left.
+ * @info:	BTCG2900 info structure.
+ */
+static void device_removed(struct btcg2900_info *info)
+{
+	struct list_head *cursor;
+	struct btcg2900_info *tmp;
+
+	if (info->acl || info->cmd || info->evt)
+		/* There are still devices active */
+		return;
+
+	/* Find the info structure and delete it */
+	list_for_each(cursor, &btcg2900_devices) {
+		tmp = list_entry(cursor, struct btcg2900_info, list);
+		if (tmp == info) {
+			list_del(cursor);
+			break;
+		}
+	}
+	kfree(info);
+	BT_DBG("CG2900 device removed");
+}
+
+/**
+ * register_bluetooth() - Initialize module.
+ *
+ * Alloc, init, and register HCI device to BlueZ.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   Error codes from hci_register_dev.
+ */
+static int register_bluetooth(struct btcg2900_info *info)
+{
+	int err;
+	struct cg2900_user_data *pf_data;
+
+	/* Check if all channels have been probed */
+	if (!info->acl || !info->cmd || !info->evt)
+		return 0;
+
+	pf_data = dev_get_platdata(info->cmd);
+
+	info->hdev = hci_alloc_dev();
+	if (!info->hdev) {
+		BT_ERR("Could not allocate mem for CG2900 BT driver");
+		return -ENOMEM;
+	}
+
+	SET_HCIDEV_DEV(info->hdev, info->parent);
+	info->hdev->bus = pf_data->channel_data.bt_bus;
+	hci_set_drvdata(info->hdev, info);
+	info->hdev->open = btcg2900_open;
+	info->hdev->close = btcg2900_close;
+	info->hdev->send = btcg2900_send;
+	/*  FIXME no more destruct move this management somewhere
+	* info->hdev->destruct = btcg2900_destruct;
+	*/
+
+	err = hci_register_dev(info->hdev);
+	if (err) {
+		BT_ERR("Can not register BTCG2900 HCI device (%d)", err);
+		hci_free_dev(info->hdev);
+		info->hdev = NULL;
+	}
+
+	BT_INFO("CG2900 registered");
+
+	BT_DBG("New enable_state: ENABLE_IDLE");
+	info->enable_state = ENABLE_IDLE;
+	BT_DBG("New reset_state: RESET_IDLE");
+	info->reset_state = RESET_IDLE;
+
+	return err;
+}
+
+/**
+ * probe_common() - Initialize channel and register to BT stack.
+ * @dev:		Current device.
+ * @info:		BTCG2900 info structure.
+ * @hci_data_type:	Data type of this channel, e.g. ACL.
+ *
+ * Allocate and initialize private data. Register to Bluetooth stack.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   Error codes from register_bluetooth.
+ */
+static int probe_common(struct platform_device *pdev,
+			struct btcg2900_info *info,
+			u8 hci_data_type)
+{
+	int err;
+	struct cg2900_user_data *pf_data;
+	struct dev_info *dev_info;
+	struct device *dev = &pdev->dev;
+
+	dev_info = kzalloc(sizeof(*dev_info), GFP_KERNEL);
+	if (!dev_info) {
+		BT_ERR("Could not allocate dev_info");
+		return -ENOMEM;
+	}
+
+	dev_set_drvdata(dev, info);
+
+	pf_data = dev_get_platdata(dev);
+	pf_data->dev = dev;
+	pf_data->read_cb = hci_read_cb;
+	pf_data->reset_cb = hci_reset_cb;
+
+	/* Init and register hdev */
+	err = register_bluetooth(info);
+	if (err) {
+		BT_ERR("HCI Device registration error (%d)", err);
+		kfree(dev_info);
+		return err;
+	}
+	dev_info->hci_data_type = hci_data_type;
+	cg2900_set_usr(pf_data, dev_info);
+
+	return 0;
+}
+
+/**
+ * btcg2900_cmd_probe() - Initialize command channel.
+ * @pdev:	Platform device.
+ *
+ * Allocate and initialize private data.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from get_info and probe_common.
+ */
+static int __devinit btcg2900_cmd_probe(struct platform_device *pdev)
+{
+	int err;
+	struct btcg2900_info *info;
+
+	BT_DBG("Starting CG2900 Command channel");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->cmd = &pdev->dev;
+
+	err = probe_common(pdev, info, HCI_COMMAND_PKT);
+	if (err) {
+		BT_ERR("Failed to initialize channel");
+		info->cmd = NULL;
+		device_removed(info);
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * btcg2900_acl_probe() - Initialize command channel.
+ * @pdev:	Platform device.
+ *
+ * Allocate and initialize private data.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from get_info and probe_common.
+ */
+static int __devinit btcg2900_acl_probe(struct platform_device *pdev)
+{
+	int err;
+	struct btcg2900_info *info;
+
+	BT_DBG("Starting CG2900 ACL channel");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->acl = &pdev->dev;
+
+	err = probe_common(pdev, info, HCI_ACLDATA_PKT);
+	if (err) {
+		BT_ERR("Failed to initialize channel");
+		info->acl = NULL;
+		device_removed(info);
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * btcg2900_evt_probe() - Initialize event channel.
+ * @pdev:	Platform device.
+ *
+ * Allocate and initialize private data.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from get_info and probe_common.
+ */
+static int __devinit btcg2900_evt_probe(struct platform_device *pdev)
+{
+	int err;
+	struct btcg2900_info *info;
+
+	BT_DBG("Starting CG2900 Event channel");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->evt = &pdev->dev;
+
+	err = probe_common(pdev, info, HCI_EVENT_PKT);
+	if (err) {
+		BT_ERR("Failed to initialize channel");
+		info->evt = NULL;
+		device_removed(info);
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * remove_common() - Remove channel.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from hci_unregister_dev.
+ */
+static int remove_common(struct platform_device *pdev,
+			 struct btcg2900_info *info)
+{
+	int err = 0;
+	struct cg2900_user_data *pf_data;
+	struct dev_info *dev_info;
+
+	pf_data = dev_get_platdata(&pdev->dev);
+	dev_info = cg2900_get_usr(pf_data);
+
+	kfree(dev_info);
+	cg2900_set_usr(pf_data, NULL);
+
+	if (!info->hdev)
+		goto finished;
+
+	BT_INFO("Unregistering CG2900");
+	hci_set_drvdata(info->hdev, NULL);
+	hci_unregister_dev(info->hdev);
+	hci_free_dev(info->hdev);
+	info->hdev = NULL;
+
+finished:
+	device_removed(info);
+	return err;
+}
+
+/**
+ * btcg2900_cmd_remove() - Remove command channel.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from remove_common.
+ */
+static int __devexit btcg2900_cmd_remove(struct platform_device *pdev)
+{
+	struct btcg2900_info *info;
+
+	BT_DBG("Removing CG2900 Command channel");
+
+	info = dev_get_drvdata(&pdev->dev);
+	info->cmd = NULL;
+	return remove_common(pdev, info);
+}
+
+/**
+ * btcg2900_acl_remove() - Remove ACL channel.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from remove_common.
+ */
+static int __devexit btcg2900_acl_remove(struct platform_device *pdev)
+{
+	struct btcg2900_info *info;
+
+	BT_DBG("Removing CG2900 ACL channel");
+
+	info = dev_get_drvdata(&pdev->dev);
+	info->acl = NULL;
+	return remove_common(pdev, info);
+}
+
+/**
+ * btcg2900_evt_remove() - Remove event channel.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from remove_common.
+ */
+static int __devexit btcg2900_evt_remove(struct platform_device *pdev)
+{
+	struct btcg2900_info *info;
+
+	BT_DBG("Removing CG2900 Event channel");
+
+	info = dev_get_drvdata(&pdev->dev);
+	info->evt = NULL;
+	return remove_common(pdev, info);
+}
+
+static struct platform_driver btcg2900_cmd_driver = {
+	.driver = {
+		.name	= "cg2900-btcmd",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= btcg2900_cmd_probe,
+	.remove	= __devexit_p(btcg2900_cmd_remove),
+};
+
+static struct platform_driver btcg2900_acl_driver = {
+	.driver = {
+		.name	= "cg2900-btacl",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= btcg2900_acl_probe,
+	.remove	= __devexit_p(btcg2900_acl_remove),
+};
+
+static struct platform_driver btcg2900_evt_driver = {
+	.driver = {
+		.name	= "cg2900-btevt",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= btcg2900_evt_probe,
+	.remove	= __devexit_p(btcg2900_evt_remove),
+};
+
+/**
+ * btcg2900_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init btcg2900_init(void)
+{
+	int err;
+
+	BT_DBG("btcg2900_init");
+
+	err = platform_driver_register(&btcg2900_cmd_driver);
+	if (err) {
+		BT_ERR("Failed to register cmd (%d)", err);
+		return err;
+	}
+	err = platform_driver_register(&btcg2900_acl_driver);
+	if (err) {
+		BT_ERR("Failed to register acl (%d)", err);
+		return err;
+	}
+	err = platform_driver_register(&btcg2900_evt_driver);
+	if (err) {
+		BT_ERR("Failed to register evt (%d)", err);
+		return err;
+	}
+	return err;
+}
+
+/**
+ * btcg2900_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit btcg2900_exit(void)
+{
+	BT_DBG("btcg2900_exit");
+	platform_driver_unregister(&btcg2900_cmd_driver);
+	platform_driver_unregister(&btcg2900_acl_driver);
+	platform_driver_unregister(&btcg2900_evt_driver);
+}
+
+module_init(btcg2900_init);
+module_exit(btcg2900_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_AUTHOR("Henrik Possung ST-Ericsson");
+MODULE_AUTHOR("Josef Kindberg ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux Bluetooth HCI H:4 Driver for ST-Ericsson controller");
diff --git a/drivers/staging/cg2900/bluetooth/cg2900_uart.c b/drivers/staging/cg2900/bluetooth/cg2900_uart.c
new file mode 100644
index 00000000000..adfc1b26e83
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/cg2900_uart.c
@@ -0,0 +1,2297 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * Lukasz Rymanowski (lukasz.rymanowski@tieto.com) for ST-Ericsson.
+ * Hemant Gupta (hemant.gupta@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth UART Driver for ST-Ericsson CG2900 connectivity controller.
+ */
+#define NAME			"cg2900_uart"
+#define pr_fmt(fmt)		NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/atomic.h>
+#include <linux/device.h>
+#include <linux/gpio.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/pm_qos.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/timer.h>
+#include <linux/regulator/consumer.h>
+#include <linux/tty.h>
+#include <linux/tty_ldisc.h>
+#include <linux/types.h>
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+
+#include "hci_uart.h"
+
+#define MAIN_DEV		(uart_info->dev)
+
+/* Workqueues' names */
+#define UART_WQ_NAME		"cg2900_uart_wq"
+#define UART_NAME		"cg2900_uart"
+
+/*
+ * A BT command complete event without any parameters is the defined size plus
+ * 1 byte extra for the status field which is always present in a
+ * command complete event.
+ */
+#define HCI_BT_CMD_COMPLETE_LEN	(sizeof(struct hci_ev_cmd_complete) + 1)
+
+/* Timers used in milliseconds */
+#define UART_TX_TIMEOUT		10
+#define UART_RX_TIMEOUT		10
+#define UART_RESP_TIMEOUT	1000
+#define UART_RESUME_TIMEOUT	20
+/* Minimum time host should maintain the break */
+#define UART_MIN_BREAK_ON_TIME 5
+
+/* Timers used in microseconds */
+/* Minimum time required after sending the last data to apply break */
+#define UART_MIN_TIME_BEFORE_APPLYING_BREAK 900
+/* Minimum time required after exiting break condition */
+#define UART_MIN_BREAK_OFF_TIME 200
+
+/* Max latency in microseconds for PM QoS to achieve max throughput */
+#define CG2900_PM_QOS_LATENCY	30
+
+/* Number of bytes to reserve at start of sk_buffer when receiving packet */
+#define RX_SKB_RESERVE		8
+/* Max size of received packet (not including reserved bytes) */
+#define RX_SKB_MAX_SIZE		1024
+
+/* Size of the header in the different packets */
+#define HCI_BT_EVT_HDR_SIZE	2
+#define HCI_BT_ACL_HDR_SIZE	4
+#define HCI_NFC_HDR_SIZE	3
+#define HCI_FM_RADIO_HDR_SIZE	1
+#define HCI_GNSS_HDR_SIZE	3
+
+/* Position of length field in the different packets */
+#define HCI_EVT_LEN_POS		2
+#define HCI_ACL_LEN_POS		3
+#define FM_RADIO_LEN_POS	1
+#define GNSS_LEN_POS		2
+
+/* Baud rate defines */
+#define ZERO_BAUD_RATE		0
+#define DEFAULT_BAUD_RATE	115200
+#define HIGH_BAUD_RATE		3000000
+
+#define BT_SIZE_OF_HDR				(sizeof(__le16) + sizeof(__u8))
+#define BT_PARAM_LEN(__pkt_len)			(__pkt_len - BT_SIZE_OF_HDR)
+
+/* Standardized Bluetooth H:4 channels */
+#define HCI_BT_CMD_H4_CHANNEL			0x01
+#define HCI_BT_ACL_H4_CHANNEL			0x02
+#define HCI_BT_SCO_H4_CHANNEL			0x03
+#define HCI_BT_EVT_H4_CHANNEL			0x04
+
+#define BT_BDADDR_SIZE				6
+
+/* Reserve 1 byte for the HCI H:4 header */
+#define HCI_H4_SIZE				1
+#define CG2900_SKB_RESERVE			HCI_H4_SIZE
+
+/* Default H4 channels which may change depending on connected controller */
+#define HCI_NFC_H4_CHANNEL			0x05
+#define HCI_FM_RADIO_H4_CHANNEL			0x08
+#define HCI_GNSS_H4_CHANNEL			0x09
+
+/* Bluetooth error codes */
+#define HCI_BT_ERROR_NO_ERROR			0x00
+
+/* Bytes in the command Hci_Cmd_ST_Set_Uart_Baud_Rate */
+#define CG2900_BAUD_RATE_57600				0x03
+#define CG2900_BAUD_RATE_115200				0x02
+#define CG2900_BAUD_RATE_230400				0x01
+#define CG2900_BAUD_RATE_460800				0x00
+#define CG2900_BAUD_RATE_921600				0x20
+#define CG2900_BAUD_RATE_2000000			0x25
+#define CG2900_BAUD_RATE_3000000			0x27
+#define CG2900_BAUD_RATE_3250000			0x28
+#define CG2900_BAUD_RATE_4000000			0x2B
+
+/* NFC: 1byte extra is recieved for checksum */
+#define NFC_CHECKSUM_DATA_LEN			0x01
+
+/* NFC */
+struct nfc_hci_hdr {
+	__u8	op_code;
+	__le16	plen;
+} __packed;
+
+/**
+ * enum sleep_allowed_bits - bits indicating if sleep is allowed.
+ * @SLEEP_TTY_READY:	Bit for checking if break can be applied using tty.
+ * @SLEEP_CORE_READY:	Bit for checking if core is ready for sleep or not.
+ */
+enum sleep_allowed_bits {
+	SLEEP_TTY_READY,
+	SLEEP_CORE_READY
+};
+
+/* GNSS */
+struct gnss_hci_hdr {
+	__u8	op_code;
+	__le16	plen;
+} __packed;
+
+/* FM legacy command packet */
+struct fm_leg_cmd {
+	__u8	length;
+	__u8	opcode;
+	__u8	read_write;
+	__u8	fm_function;
+	union { /* Payload varies with function */
+		__le16	irqmask;
+		struct fm_leg_fm_cmd {
+			__le16	head;
+			__le16	data[];
+		} fm_cmd;
+	};
+} __packed;
+
+/* FM legacy command complete packet */
+struct fm_leg_cmd_cmpl {
+	__u8	param_length;
+	__u8	status;
+	__u8	opcode;
+	__u8	read_write;
+	__u8	cmd_status;
+	__u8	fm_function;
+	__le16	response_head;
+	__le16	data[];
+} __packed;
+
+/* FM legacy interrupt packet, PG2 style */
+struct fm_leg_irq_v2 {
+	__u8	param_length;
+	__u8	status;
+	__u8	opcode;
+	__u8	event_type;
+	__u8	event_id;
+	__le16	irq;
+} __packed;
+
+/* FM legacy interrupt packet, PG1 style */
+struct fm_leg_irq_v1 {
+	__u8	param_length;
+	__u8	opcode;
+	__u8	event_id;
+	__le16	irq;
+} __packed;
+
+union fm_leg_evt_or_irq {
+	__u8			param_length;
+	struct fm_leg_cmd_cmpl	evt;
+	struct fm_leg_irq_v2	irq_v2;
+	struct fm_leg_irq_v1	irq_v1;
+} __packed;
+
+/* BT VS SetBaudRate command */
+#define CG2900_BT_OP_VS_SET_BAUD_RATE			0xFC09
+struct bt_vs_set_baud_rate_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	baud_rate;
+} __packed;
+
+/**
+ * enum uart_rx_state - UART RX-state for UART.
+ * @W4_PACKET_TYPE:	Waiting for packet type.
+ * @W4_EVENT_HDR:	Waiting for BT event header.
+ * @W4_ACL_HDR:		Waiting for BT ACL header.
+ * @W4_NFC_HDR:		Waiting for NFC header.
+ * @W4_FM_RADIO_HDR:	Waiting for FM header.
+ * @W4_GNSS_HDR:	Waiting for GNSS header.
+ * @W4_DATA:		Waiting for data in rest of the packet (after header).
+ */
+enum uart_rx_state {
+	W4_PACKET_TYPE,
+	W4_EVENT_HDR,
+	W4_ACL_HDR,
+	W4_NFC_HDR,
+	W4_FM_RADIO_HDR,
+	W4_GNSS_HDR,
+	W4_DATA
+};
+
+/**
+  * enum sleep_state - Sleep-state for UART.
+  * @CHIP_AWAKE:  Chip is awake.
+  * @CHIP_FALLING_ASLEEP:  Chip is falling asleep.
+  * @CHIP_ASLEEP: Chip is asleep.
+  * @CHIP_SUSPENDED: Chip in suspend state.
+  * @CHIP_RESUMING: Chip is going back from suspend state.
+  * @CHIP_POWERED_DOWN: Chip is off.
+  */
+enum sleep_state {
+	CHIP_AWAKE,
+	CHIP_FALLING_ASLEEP,
+	CHIP_ASLEEP,
+	CHIP_SUSPENDED,
+	CHIP_RESUMING,
+	CHIP_POWERED_DOWN
+};
+
+/**
+ * enum baud_rate_change_state - Baud rate-state for UART.
+ * @BAUD_IDLE:		No baud rate change is ongoing.
+ * @BAUD_SENDING_RESET:	HCI reset has been sent. Waiting for command complete
+ *			event.
+ * @BAUD_START:		Set baud rate cmd scheduled for sending.
+ * @BAUD_SENDING:	Set baud rate cmd sending in progress.
+ * @BAUD_WAITING:	Set baud rate cmd sent, waiting for command complete
+ *			event.
+ * @BAUD_SUCCESS:	Baud rate change has succeeded.
+ * @BAUD_FAIL:		Baud rate change has failed.
+ */
+enum baud_rate_change_state {
+	BAUD_IDLE,
+	BAUD_SENDING_RESET,
+	BAUD_START,
+	BAUD_SENDING,
+	BAUD_WAITING,
+	BAUD_SUCCESS,
+	BAUD_FAIL
+};
+
+/**
+ * struct uart_work_struct - Work structure for UART module.
+ * @work:	Work structure.
+ * @data:	Pointer to private data.
+ *
+ * This structure is used to pack work for work queue.
+ */
+struct uart_work_struct {
+	struct work_struct	work;
+	void			*data;
+};
+
+/**
+ * struct uart_delayed_work_struct - Work structure for UART module.
+ * @delayed_work:	Work structure.
+ * @data:	Pointer to private data.
+ *
+ * This structure is used to pack work for work queue.
+ */
+struct uart_delayed_work_struct {
+	struct delayed_work	work;
+	void			*data;
+};
+
+/**
+ * struct uart_info - Main UART info structure.
+ * @rx_state:		Current RX state.
+ * @rx_count:		Number of bytes left to receive.
+ * @rx_skb:		SK_buffer to store the received data into.
+ * @tx_queue:		TX queue for sending data to chip.
+ * @rx_skb_lock	Spin lock to protect rx_skb.
+ * @hu:			Hci uart structure.
+ * @wq:			UART work queue.
+ * @baud_rate_state:	UART baud rate change state.
+ * @baud_rate:		Current baud rate setting.
+ * @sleep_state:	UART sleep state.
+ * @sleep_work:	Delayed sleep work struct.
+ * @wakeup_work:	Wake-up work struct.
+ * @restart_sleep_work:	Reschedule sleep_work and wake-up work struct.
+ * @sleep_state_lock:	Used to protect chip state.
+ * @sleep_flags:	Indicates whether sleep mode is allowed.
+ * @tx_in_progress:	Indicates data sending in progress.
+ * @rx_in_progress:	Indicates data receiving in progress.
+ * @transmission_lock:	Spin_lock to protect tx/rx_in_progress.
+ * @regulator:		Regulator.
+ * @regulator_enabled:	True if regulator is enabled.
+ * @dev:		Pointer to CG2900 uart device.
+ * @chip_dev:		Chip device for current UART transport.
+ * @cts_irq:		CTS interrupt for this UART.
+ * @cts_gpio:		CTS GPIO for this UART.
+ * @suspend_blocked:	True if suspend operation is blocked in the framework.
+ * @pm_qos_latency:	PM QoS structure.
+ */
+struct uart_info {
+	enum uart_rx_state		rx_state;
+	unsigned long			rx_count;
+	struct sk_buff			*rx_skb;
+	struct sk_buff_head		tx_queue;
+	spinlock_t			rx_skb_lock;
+
+	struct hci_uart			*hu;
+
+	struct workqueue_struct		*wq;
+	enum baud_rate_change_state	baud_rate_state;
+	int				baud_rate;
+	enum sleep_state		sleep_state;
+	struct uart_delayed_work_struct	sleep_work;
+	struct uart_work_struct		wakeup_work;
+	struct uart_work_struct		restart_sleep_work;
+	struct mutex			sleep_state_lock;
+	unsigned long			sleep_flags;
+	bool				tx_in_progress;
+	bool				rx_in_progress;
+	spinlock_t			transmission_lock;
+	struct	regulator		*regulator;
+	bool				regulator_enabled;
+	struct device			*dev;
+	struct cg2900_chip_dev		chip_dev;
+	int				cts_irq;
+	int				cts_gpio;
+	bool				suspend_blocked;
+	struct pm_qos_request		pm_qos_latency;
+};
+
+/* Module parameters */
+static int uart_default_baud = DEFAULT_BAUD_RATE;
+static int uart_high_baud = HIGH_BAUD_RATE;
+static int uart_debug;
+
+static DECLARE_WAIT_QUEUE_HEAD(uart_wait_queue);
+
+static void wake_up_chip(struct uart_info *uart_info);
+
+/**
+ * is_chip_flow_off() - Check if chip has set flow off.
+ * @tty:	Pointer to tty.
+ *
+ * Returns:
+ *   true - chip flows off.
+ *   false - chip flows on.
+ */
+static bool is_chip_flow_off(struct uart_info *uart_info)
+{
+	int lines = 0;
+
+	if (uart_info->hu)
+		lines = hci_uart_tiocmget(uart_info->hu);
+
+	if (lines & TIOCM_CTS)
+		return false;
+	else
+		return true;
+}
+
+/**
+ * create_work_item() - Create work item and add it to the work queue.
+ * @uart_info:	Main Uart structure.
+ * @work_func:	Work function.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EBUSY if not possible to queue work.
+ *   -ENOMEM if allocation fails.
+ */
+static int create_work_item(struct uart_info *uart_info,
+					work_func_t work_func)
+{
+	struct uart_work_struct *new_work;
+	int res;
+
+	new_work = kmalloc(sizeof(*new_work), GFP_ATOMIC);
+	if (!new_work) {
+		dev_err(MAIN_DEV,
+			"Failed to alloc memory for uart_work_struct\n");
+		return -ENOMEM;
+	}
+
+	new_work->data = uart_info;
+	INIT_WORK(&new_work->work, work_func);
+
+	res = queue_work(uart_info->wq, &new_work->work);
+	if (!res) {
+		dev_err(MAIN_DEV,
+			"Failed to queue work_struct because it's already "
+			"in the queue\n");
+		kfree(new_work);
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+/**
+ * handle_cts_irq() - Called to handle CTS interrupt in work context.
+ * @work:	work which needs to be done.
+ *
+ * The handle_cts_irq() function is a work handler called if interrupt on CTS
+ * occurred. It wakes up the transport.
+ */
+static void handle_cts_irq(struct work_struct *work)
+{
+	struct uart_work_struct *current_work =
+		container_of(work, struct uart_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+
+	pm_qos_update_request(&uart_info->pm_qos_latency,
+			      CG2900_PM_QOS_LATENCY);
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	/* Mark that there is an ongoing transfer. */
+	uart_info->rx_in_progress = true;
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	/* Cancel pending sleep work if there is any. */
+	cancel_delayed_work_sync(&uart_info->sleep_work.work);
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	if (uart_info->sleep_state == CHIP_SUSPENDED) {
+		dev_dbg(MAIN_DEV, "New sleep_state: CHIP_RESUMING\n");
+		uart_info->sleep_state = CHIP_RESUMING;
+		mutex_unlock(&(uart_info->sleep_state_lock));
+	} else {
+		mutex_unlock(&(uart_info->sleep_state_lock));
+		wake_up_chip(uart_info);
+	}
+
+	kfree(current_work);
+}
+
+/**
+ * cts_interrupt() - Called to handle CTS interrupt.
+ * @irq:	Interrupt that occurred.
+ * @dev_id:	Device ID where interrupt occurred.
+ *
+ * The cts_interrupt() function is called if interrupt on CTS occurred.
+ * It disables the interrupt and starts a new work thread to handle
+ * the interrupt.
+ */
+static irqreturn_t cts_interrupt(int irq, void *dev_id)
+{
+	struct uart_info *uart_info = dev_get_drvdata(dev_id);
+#ifdef CONFIG_PM
+	disable_irq_wake(irq);
+#endif
+	disable_irq_nosync(irq);
+
+	/* Create work and leave IRQ context. */
+	(void)create_work_item(uart_info, handle_cts_irq);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * set_cts_irq() - Enable interrupt on CTS.
+ * @uart_info: Main Uart structure.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from request_irq and disable_uart.
+ */
+static int set_cts_irq(struct uart_info *uart_info)
+{
+	int err;
+	int cts_val = 0;
+
+	/* Set IRQ on CTS. */
+	err = request_irq(uart_info->cts_irq,
+			  cts_interrupt,
+			  IRQF_TRIGGER_FALLING,
+			  UART_NAME,
+			  uart_info->dev);
+	if (err) {
+		dev_err(MAIN_DEV, "Could not request CTS IRQ (%d)\n", err);
+		return err;
+	}
+
+	/*
+	 * It may happen that there was already an interrupt on CTS just before
+	 * the enable_irq() call above. If the CTS line is low now it means that
+	 * it's happened, so disable the CTS interrupt and return -ECANCELED.
+	 */
+	cts_val = gpio_get_value(uart_info->cts_gpio);
+	if (!cts_val) {
+		dev_dbg(MAIN_DEV, "Missed interrupt, going back to "
+			"awake state\n");
+		free_irq(uart_info->cts_irq, uart_info->dev);
+		return -ECANCELED;
+	}
+
+#ifdef CONFIG_PM
+	enable_irq_wake(uart_info->cts_irq);
+#endif
+	return 0;
+}
+
+/**
+ * disable_uart_pins() - Disable the UART pins.
+ * @uart_info: Main Uart structure.
+ */
+static void disable_uart_pins(struct uart_info *uart_info)
+{
+	struct cg2900_platform_data *pf_data;
+
+	pf_data = dev_get_platdata(uart_info->dev);
+
+	if (pf_data->uart.disable_uart) {
+		int err = pf_data->uart.disable_uart(&uart_info->chip_dev);
+		if (err)
+			dev_err(MAIN_DEV,
+				"Unable to disable UART Hardware (%d)\n", err);
+	}
+}
+
+/**
+ * enable_uart_pins() - Enable the UART pins.
+ * @uart_info: Main Uart structure.
+ */
+static void enable_uart_pins(struct uart_info *uart_info)
+{
+	struct cg2900_platform_data *pf_data;
+
+	pf_data = dev_get_platdata(uart_info->dev);
+
+	if (pf_data->uart.enable_uart) {
+		int err = pf_data->uart.enable_uart(&uart_info->chip_dev);
+		if (err)
+			dev_err(MAIN_DEV,
+				"Unable to enable UART Hardware (%d)\n", err);
+	}
+}
+
+/**
+ * unset_cts_irq() - Disable interrupt on CTS.
+ * @uart_info: Main Uart structure.
+ */
+static void unset_cts_irq(struct uart_info *uart_info)
+{
+	/* Free CTS interrupt */
+	free_irq(uart_info->cts_irq, uart_info->dev);
+}
+
+/**
+ * get_sleep_timeout() - Get sleep timeout.
+ * @uart_info: Main Uart structure.
+ *
+ * Check all conditions for sleep and return sleep timeout.
+ * Return:
+ *	0: sleep not allowed.
+ *	other: Timeout value in jiffies.
+ */
+static unsigned long get_sleep_timeout(struct uart_info *uart_info)
+{
+	unsigned long timeout_jiffies = 0;
+	bool check_sleep = false;
+	if (uart_info->sleep_state == CHIP_FALLING_ASLEEP)
+		check_sleep = true;
+
+	timeout_jiffies = cg2900_get_sleep_timeout(check_sleep);
+
+	if (timeout_jiffies &&
+		uart_info->hu &&
+		uart_info->hu->fd &&
+		test_bit(SLEEP_TTY_READY, &uart_info->sleep_flags) &&
+		test_bit(SLEEP_CORE_READY, &uart_info->sleep_flags))
+		return timeout_jiffies;
+
+	return 0;
+}
+
+/**
+ * work_wake_up_chip() - Called to wake up of the transport in work context.
+ * @work:	work which needs to be done.
+ */
+static void work_wake_up_chip(struct work_struct *work)
+{
+	struct uart_work_struct *current_work =
+		container_of(work, struct uart_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+
+	wake_up_chip(uart_info);
+}
+
+/**
+ * wake_up_chip() - Wakes up the chip and transport.
+ * @work:	pointer to a work struct if the function was called that way.
+ *
+ * Depending on the current sleep state it may wake up the transport.
+ */
+static void wake_up_chip(struct uart_info *uart_info)
+{
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+
+	/* Resuming state is special. Need to get back chip to awake state. */
+	if (!timeout_jiffies && uart_info->sleep_state != CHIP_RESUMING)
+		return;
+
+	if (!uart_info->hu) {
+		dev_err(MAIN_DEV, "wake_up_chip: UART not open\n");
+		return;
+	}
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	/*
+	 * If chip is powered down we cannot wake it up here. It has to be woken
+	 * up through a call to uart_set_chip_power()
+	 */
+	if (CHIP_POWERED_DOWN == uart_info->sleep_state)
+		goto finished;
+
+	if (!uart_info->suspend_blocked) {
+		uart_info->suspend_blocked = true;
+		pm_qos_update_request(&uart_info->pm_qos_latency,
+				      CG2900_PM_QOS_LATENCY);
+	}
+
+	/*
+	 * This function indicates data is transmitted.
+	 * Therefore see to that the chip is awake.
+	 */
+	if (CHIP_AWAKE == uart_info->sleep_state)
+		goto finished;
+
+	if (CHIP_ASLEEP == uart_info->sleep_state ||
+		CHIP_RESUMING == uart_info->sleep_state) {
+		/* Wait before disabling IRQ */
+		schedule_timeout_killable(
+				msecs_to_jiffies(UART_RESUME_TIMEOUT));
+
+		/* Disable IRQ only when it was enabled. */
+		unset_cts_irq(uart_info);
+		(void)hci_uart_set_baudrate(uart_info->hu,
+					    uart_info->baud_rate);
+
+		enable_uart_pins(uart_info);
+
+		/*
+		 * Wait before flowing on. Otherwise UART might not be ready in
+		 * time
+		 */
+		schedule_timeout_killable(
+				msecs_to_jiffies(UART_RESUME_TIMEOUT));
+
+		/* Set FLOW on. */
+		hci_uart_flow_ctrl(uart_info->hu, FLOW_ON);
+	}
+
+	/* Unset BREAK. */
+	dev_dbg(MAIN_DEV, "wake_up_chip: Clear break\n");
+	hci_uart_set_break(uart_info->hu, BREAK_OFF);
+	udelay(UART_MIN_BREAK_OFF_TIME);
+
+	dev_dbg(MAIN_DEV, "New sleep_state: CHIP_AWAKE\n");
+	uart_info->sleep_state = CHIP_AWAKE;
+
+finished:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+}
+
+/**
+ * set_chip_sleep_mode() - Put the chip and transport to sleep mode.
+ * @work:	pointer to work_struct.
+ *
+ * The set_chip_sleep_mode() function is called if there are no ongoing data
+ * transmissions. It tries to put the chip in sleep mode.
+ *
+ */
+static void set_chip_sleep_mode(struct work_struct *work)
+{
+	int err = 0;
+	struct delayed_work *delayed_work =
+			container_of(work, struct delayed_work, work);
+	struct uart_delayed_work_struct *current_work =	container_of(
+			delayed_work, struct uart_delayed_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+	int chars_in_buffer;
+
+	if (!timeout_jiffies)
+		return;
+
+	if (!uart_info->hu) {
+		dev_err(MAIN_DEV, "set_chip_sleep_mode: UART not open\n");
+		return;
+	}
+
+	if (uart_info->tx_in_progress || uart_info->rx_in_progress) {
+		dev_dbg(MAIN_DEV, "Not going to sleep, TX/RX in progress\n");
+		return;
+	}
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	switch (uart_info->sleep_state) {
+	case CHIP_FALLING_ASLEEP:
+		if (!is_chip_flow_off(uart_info)) {
+			dev_dbg(MAIN_DEV, "Chip flow is on, it's not ready to"
+				"sleep yet\n");
+			goto schedule_sleep_work;
+		}
+
+		/* Flow OFF. */
+		hci_uart_flow_ctrl(uart_info->hu, FLOW_OFF);
+
+		disable_uart_pins(uart_info);
+
+		/*
+		 * Set baud zero.
+		 * This cause shut off UART clock as well.
+		 */
+		(void)hci_uart_set_baudrate(uart_info->hu, ZERO_BAUD_RATE);
+		err = set_cts_irq(uart_info);
+		if (err < 0) {
+			enable_uart_pins(uart_info);
+			(void)hci_uart_set_baudrate(uart_info->hu,
+						uart_info->baud_rate);
+			hci_uart_flow_ctrl(uart_info->hu, FLOW_ON);
+			hci_uart_set_break(uart_info->hu, BREAK_OFF);
+			udelay(UART_MIN_BREAK_OFF_TIME);
+
+			dev_dbg(MAIN_DEV, "New sleep_state: CHIP_AWAKE\n");
+			uart_info->sleep_state = CHIP_AWAKE;
+
+			if (err == -ECANCELED)
+				goto finished;
+			else {
+				dev_err(MAIN_DEV, "Can not set interrupt on "
+						"CTS, err:%d\n", err);
+				goto error;
+			}
+		}
+
+		dev_dbg(MAIN_DEV, "New sleep_state: CHIP_ASLEEP\n");
+		uart_info->sleep_state = CHIP_ASLEEP;
+		if (uart_info->suspend_blocked) {
+			uart_info->suspend_blocked = false;
+			pm_qos_update_request(&uart_info->pm_qos_latency,
+					      PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE);
+		}
+		break;
+	case CHIP_AWAKE:
+		chars_in_buffer = hci_uart_chars_in_buffer(uart_info->hu);
+		if (chars_in_buffer) {
+			dev_dbg(MAIN_DEV, "sleep_timer_expired: "
+					"tx not finished, stay awake and "
+					"restart the sleep timer\n");
+			goto schedule_sleep_work;
+		}
+
+		dev_dbg(MAIN_DEV, "sleep_timer_expired: Set break\n");
+		udelay(UART_MIN_TIME_BEFORE_APPLYING_BREAK);
+		hci_uart_set_break(uart_info->hu, BREAK_ON);
+		schedule_timeout_killable(
+				msecs_to_jiffies(UART_MIN_BREAK_ON_TIME));
+
+		dev_dbg(MAIN_DEV, "New sleep_state: CHIP_FALLING_ASLEEP\n");
+		uart_info->sleep_state = CHIP_FALLING_ASLEEP;
+		goto schedule_sleep_work;
+
+	case CHIP_POWERED_DOWN:
+	case CHIP_SUSPENDED:
+	case CHIP_ASLEEP: /* Fallthrough. */
+	default:
+		dev_dbg(MAIN_DEV,
+			"Chip sleeps, is suspended or powered down\n");
+		break;
+	}
+
+	mutex_unlock(&(uart_info->sleep_state_lock));
+
+	return;
+
+finished:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+	return;
+schedule_sleep_work:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+	if (timeout_jiffies)
+		queue_delayed_work(uart_info->wq, &uart_info->sleep_work.work,
+				timeout_jiffies);
+	return;
+error:
+	/* Disable sleep mode.*/
+	dev_err(MAIN_DEV, "Disable sleep mode\n");
+	clear_bit(SLEEP_CORE_READY, &uart_info->sleep_flags);
+	mutex_unlock(&(uart_info->sleep_state_lock));
+}
+
+#ifdef CONFIG_PM
+/**
+ * cg2900_uart_suspend() - Called by Linux PM to put the device in a low power mode.
+ * @pdev:	Pointer to platform device.
+ * @state:	New state.
+ *
+ * In UART case, CG2900 driver does nothing on suspend.
+ *
+ * Returns:
+ *   0 - Success.
+ */
+static int cg2900_uart_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int err = 0;
+	struct uart_info *uart_info = dev_get_drvdata(&pdev->dev);
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	if (uart_info->sleep_state == CHIP_POWERED_DOWN)
+		goto finished;
+
+	if (uart_info->sleep_state != CHIP_ASLEEP) {
+		err = -EBUSY;
+		goto finished;
+	}
+
+	dev_dbg(MAIN_DEV, "New sleep_state: CHIP_SUSPENDED\n");
+	uart_info->sleep_state = CHIP_SUSPENDED;
+
+finished:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+	return err;
+}
+
+/**
+ * cg2900_uart_resume() - Called to bring a device back from a low power state.
+ * @pdev:	Pointer to platform device.
+ *
+ * In UART case, CG2900 driver does nothing on resume.
+ *
+ * Returns:
+ *   0 - Success.
+ */
+static int cg2900_uart_resume(struct platform_device *pdev)
+{
+	struct uart_info *uart_info = dev_get_drvdata(&pdev->dev);
+
+	mutex_lock(&(uart_info->sleep_state_lock));
+
+	if (uart_info->sleep_state == CHIP_RESUMING)
+		/* System resume because of trafic on UART. Lets wakeup.*/
+		(void)queue_work(uart_info->wq, &uart_info->wakeup_work.work);
+	else if (uart_info->sleep_state != CHIP_POWERED_DOWN) {
+		/* No need to wakeup chip. Go back to Asleep state.*/
+		dev_dbg(MAIN_DEV, "New sleep_state: CHIP_ASLEEP\n");
+		uart_info->sleep_state = CHIP_ASLEEP;
+	}
+
+	mutex_unlock(&(uart_info->sleep_state_lock));
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+/**
+ * cg2900_enable_regulator() - Enable regulator.
+ * @uart_info: Main Uart structure.
+ *
+ * Returns:
+ *   0 - Success.
+ *   Error from regulator_get, regulator_enable.
+ */
+static int cg2900_enable_regulator(struct uart_info *uart_info)
+{
+#ifdef CONFIG_REGULATOR
+	int err;
+
+	/* Get and enable regulator. */
+	uart_info->regulator = regulator_get(uart_info->dev, "gbf_1v8");
+	if (IS_ERR(uart_info->regulator)) {
+		dev_err(MAIN_DEV, "Not able to find regulator\n");
+		err = PTR_ERR(uart_info->regulator);
+	} else {
+		err = regulator_enable(uart_info->regulator);
+		if (err)
+			dev_err(MAIN_DEV, "Not able to enable regulator\n");
+		else
+			uart_info->regulator_enabled = true;
+	}
+	return err;
+#else
+	return 0;
+#endif
+}
+
+/**
+ * cg2900_disable_regulator() - Disable regulator.
+ * @uart_info: Main Uart structure.
+ *
+ */
+static void cg2900_disable_regulator(struct uart_info *uart_info)
+{
+#ifdef CONFIG_REGULATOR
+	/* Disable and put regulator. */
+	if (uart_info->regulator && uart_info->regulator_enabled) {
+		regulator_disable(uart_info->regulator);
+		uart_info->regulator_enabled = false;
+	}
+	regulator_put(uart_info->regulator);
+	uart_info->regulator = NULL;
+#endif
+}
+
+/**
+ * is_set_baud_rate_cmd() - Checks if data contains set baud rate hci cmd.
+ * @data:	Pointer to data array to check.
+ *
+ * Returns:
+ *   true - if cmd found;
+ *   false - otherwise.
+ */
+static bool is_set_baud_rate_cmd(const char *data)
+{
+	struct hci_command_hdr *cmd;
+
+	if (data[0] != HCI_BT_CMD_H4_CHANNEL)
+		return false;
+
+	cmd = (struct hci_command_hdr *)&data[1];
+	if (le16_to_cpu(cmd->opcode) == CG2900_BT_OP_VS_SET_BAUD_RATE &&
+	    cmd->plen == BT_PARAM_LEN(sizeof(struct bt_vs_set_baud_rate_cmd)))
+		return true;
+
+	return false;
+}
+
+/**
+ * is_bt_cmd_complete_no_param() - Checks if data contains command complete event for a certain command.
+ * @skb:	sk_buffer containing the data including H:4 header.
+ * @opcode:	Command op code.
+ * @status:	Command status.
+ *
+ * Returns:
+ *   true - If this is the command complete we were looking for;
+ *   false - otherwise.
+ */
+static bool is_bt_cmd_complete_no_param(struct sk_buff *skb, u16 opcode,
+					u8 *status)
+{
+	struct hci_event_hdr *event;
+	struct hci_ev_cmd_complete *complete;
+	u8 *data = &(skb->data[0]);
+
+	if (HCI_BT_EVT_H4_CHANNEL != *data)
+		return false;
+
+	data += HCI_H4_SIZE;
+	event = (struct hci_event_hdr *)data;
+	if (HCI_EV_CMD_COMPLETE != event->evt ||
+	    HCI_BT_CMD_COMPLETE_LEN != event->plen)
+		return false;
+
+	data += sizeof(*event);
+	complete = (struct hci_ev_cmd_complete *)data;
+	if (opcode != le16_to_cpu(complete->opcode))
+		return false;
+
+	if (status) {
+		/*
+		 * All command complete have the status field at first byte of
+		 * packet data.
+		 */
+		data += sizeof(*complete);
+		*status = *data;
+	}
+	return true;
+}
+
+/**
+ * alloc_rx_skb() - Alloc an sk_buff structure for receiving data from controller.
+ * @size:	Size in number of octets.
+ * @priority:	Allocation priority, e.g. GFP_KERNEL.
+ *
+ * Returns:
+ *   Pointer to sk_buff structure.
+ */
+static struct sk_buff *alloc_rx_skb(unsigned int size, gfp_t priority)
+{
+	struct sk_buff *skb;
+
+	/* Allocate the SKB and reserve space for the header */
+	skb = alloc_skb(size + RX_SKB_RESERVE, priority);
+	if (skb)
+		skb_reserve(skb, RX_SKB_RESERVE);
+
+	return skb;
+}
+
+/**
+ * finish_setting_baud_rate() - Handles sending the ste baud rate hci cmd.
+ * @hu:	Pointer to associated Hci uart structure.
+ *
+ * finish_setting_baud_rate() makes sure that the set baud rate cmd has
+ * been really sent out on the wire and then switches the tty driver to new
+ * baud rate.
+ */
+static void finish_setting_baud_rate(struct hci_uart *hu)
+{
+	struct uart_info *uart_info =
+			(struct uart_info *)dev_get_drvdata(hu->proto->dev);
+	/*
+	 * Give the tty driver time to send data and proceed. If it hasn't
+	 * been sent we can't do much about it anyway.
+	 */
+	schedule_timeout_killable(msecs_to_jiffies(UART_TX_TIMEOUT));
+
+	/*
+	 * Now set the termios struct to the new baudrate. Start by storing
+	 * the old termios.
+	 */
+	if (hci_uart_set_baudrate(hu, uart_info->baud_rate) < 0) {
+		/* Something went wrong.*/
+		dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+		uart_info->baud_rate_state = BAUD_IDLE;
+	} else {
+		dev_dbg(MAIN_DEV, "Setting termios to new baud rate\n");
+		dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_WAITING\n");
+		uart_info->baud_rate_state = BAUD_WAITING;
+	}
+
+	hci_uart_flow_ctrl(hu, FLOW_ON);
+}
+
+/**
+ * alloc_set_baud_rate_cmd() - Allocates new sk_buff and fills in the change baud rate hci cmd.
+ * @uart_info:	Main Uart structure.
+ * @baud:	(in/out) Requested new baud rate. Updated to default baud rate
+ *		upon invalid value.
+ *
+ * Returns:
+ *   Pointer to allocated sk_buff if successful;
+ *   NULL otherwise.
+ */
+static struct sk_buff *alloc_set_baud_rate_cmd(struct uart_info *uart_info,
+							int *baud)
+{
+	struct sk_buff *skb;
+	u8 *h4;
+	struct bt_vs_set_baud_rate_cmd *cmd;
+
+	skb = alloc_skb(sizeof(*cmd) + CG2900_SKB_RESERVE, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(MAIN_DEV,
+			"alloc_set_baud_rate_cmd: Failed to alloc skb\n");
+		return NULL;
+	}
+	skb_reserve(skb, CG2900_SKB_RESERVE);
+
+	cmd = (struct bt_vs_set_baud_rate_cmd *)skb_put(skb, sizeof(cmd));
+
+	/* Create the Hci_Cmd_ST_Set_Uart_Baud_Rate packet */
+	cmd->opcode = cpu_to_le16(CG2900_BT_OP_VS_SET_BAUD_RATE);
+	cmd->plen = BT_PARAM_LEN(sizeof(cmd));
+
+	switch (*baud) {
+	case 57600:
+		cmd->baud_rate = CG2900_BAUD_RATE_57600;
+		break;
+	case 115200:
+		cmd->baud_rate = CG2900_BAUD_RATE_115200;
+		break;
+	case 230400:
+		cmd->baud_rate = CG2900_BAUD_RATE_230400;
+		break;
+	case 460800:
+		cmd->baud_rate = CG2900_BAUD_RATE_460800;
+		break;
+	case 921600:
+		cmd->baud_rate = CG2900_BAUD_RATE_921600;
+		break;
+	case 2000000:
+		cmd->baud_rate = CG2900_BAUD_RATE_2000000;
+		break;
+	case 3000000:
+		cmd->baud_rate = CG2900_BAUD_RATE_3000000;
+		break;
+	case 3250000:
+		cmd->baud_rate = CG2900_BAUD_RATE_3250000;
+		break;
+	case 4000000:
+		cmd->baud_rate = CG2900_BAUD_RATE_4000000;
+		break;
+	default:
+		dev_err(MAIN_DEV,
+			"Invalid speed requested (%d), using 115200 bps "
+			"instead\n", *baud);
+		cmd->baud_rate = CG2900_BAUD_RATE_115200;
+		*baud = 115200;
+		break;
+	};
+
+	h4 = skb_push(skb, HCI_H4_SIZE);
+	*h4 = HCI_BT_CMD_H4_CHANNEL;
+
+	return skb;
+}
+
+/**
+ * work_do_transmit() - Transmit data packet to connectivity controller over UART.
+ * @work:	Pointer to work info structure. Contains uart_info structure
+ *		pointer.
+ */
+static void work_do_transmit(struct work_struct *work)
+{
+	struct uart_work_struct *current_work =
+			container_of(work, struct uart_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+
+	kfree(current_work);
+
+	if (!uart_info->hu) {
+		dev_err(MAIN_DEV, "work_do_transmit: UART not open\n");
+		return;
+	}
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	/* Mark that there is an ongoing transfer. */
+	uart_info->tx_in_progress = true;
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	/* Cancel pending sleep work if there is any. */
+	cancel_delayed_work_sync(&uart_info->sleep_work.work);
+
+	/* Wake up the chip and transport. */
+	wake_up_chip(uart_info);
+
+	(void)hci_uart_tx_wakeup(uart_info->hu);
+}
+
+/**
+ * work_hw_deregistered() - Handle HW deregistered.
+ * @work: Reference to work data.
+ */
+static void work_hw_deregistered(struct work_struct *work)
+{
+	struct uart_work_struct *current_work;
+	struct uart_info *uart_info;
+	int err;
+	current_work = container_of(work, struct uart_work_struct, work);
+	uart_info = (struct uart_info *)current_work->data;
+
+	err = cg2900_deregister_trans_driver(&uart_info->chip_dev);
+	if (err)
+		dev_err(MAIN_DEV, "Could not deregister UART from Core (%d)\n",
+			err);
+
+	kfree(current_work);
+}
+
+/**
+ * set_baud_rate() - Sets new baud rate for the UART.
+ * @hu:		Pointer to hci_uart structure.
+ * @baud:	New baud rate.
+ *
+ * This function first sends the HCI command
+ * Hci_Cmd_ST_Set_Uart_Baud_Rate. It then changes the baud rate in HW, and
+ * finally it waits for the Command Complete event for the
+ * Hci_Cmd_ST_Set_Uart_Baud_Rate command.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EALREADY if baud rate change is already in progress.
+ *   -EFAULT if one or more of the UART related structs is not allocated.
+ *   -ENOMEM if skb allocation has failed.
+ *   -EPERM if setting the new baud rate has failed.
+ *   Errors from create_work_item.
+ */
+static int set_baud_rate(struct hci_uart *hu, int baud)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	int old_baud_rate;
+	struct uart_info *uart_info =
+			(struct uart_info *)dev_get_drvdata(hu->proto->dev);
+
+	dev_dbg(MAIN_DEV, "set_baud_rate (%d baud)\n", baud);
+
+	if (uart_info->baud_rate_state != BAUD_IDLE) {
+		dev_err(MAIN_DEV,
+			"Trying to set new baud rate before old setting "
+			   "is finished\n");
+		return -EALREADY;
+	}
+
+	if (!uart_info->hu) {
+		dev_err(MAIN_DEV, "set_baud_rate: UART not open\n");
+		return -EFAULT;
+	}
+
+	/*
+	 * Wait some time to be sure that any RX process has finished (which
+	 * flows on RTS in the end) before flowing off the RTS.
+	 */
+	schedule_timeout_killable(msecs_to_jiffies(UART_RX_TIMEOUT));
+	hci_uart_flow_ctrl(uart_info->hu, FLOW_OFF);
+
+	/*
+	 * Store old baud rate so that we can restore it if something goes
+	 * wrong.
+	 */
+	old_baud_rate = uart_info->baud_rate;
+
+	skb = alloc_set_baud_rate_cmd(uart_info, &baud);
+	if (!skb) {
+		dev_err(MAIN_DEV, "alloc_set_baud_rate_cmd failed\n");
+		return -ENOMEM;
+	}
+
+	dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_START\n");
+	uart_info->baud_rate_state = BAUD_START;
+	uart_info->baud_rate = baud;
+
+	/* Queue the sk_buffer... */
+	skb_queue_tail(&uart_info->tx_queue, skb);
+
+	/* ... and call the common UART TX function */
+	err = create_work_item(uart_info, work_do_transmit);
+	if (err) {
+		dev_err(MAIN_DEV,
+			"Failed to send change baud rate cmd, freeing skb\n");
+		skb = skb_dequeue_tail(&uart_info->tx_queue);
+		dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+		uart_info->baud_rate_state = BAUD_IDLE;
+		uart_info->baud_rate = old_baud_rate;
+		kfree_skb(skb);
+		return err;
+	}
+
+	dev_dbg(MAIN_DEV, "Set baud rate cmd scheduled for sending\n");
+
+	/*
+	 * Now wait for the command complete.
+	 * It will come at the new baudrate.
+	 */
+	wait_event_timeout(uart_wait_queue,
+				((BAUD_SUCCESS == uart_info->baud_rate_state) ||
+				 (BAUD_FAIL    == uart_info->baud_rate_state)),
+				 msecs_to_jiffies(UART_RESP_TIMEOUT));
+	if (BAUD_SUCCESS == uart_info->baud_rate_state)
+		dev_info(MAIN_DEV, "Baud rate changed to %d baud\n", baud);
+	else {
+		dev_err(MAIN_DEV, "Failed to set new baud rate (%d)\n",
+			   uart_info->baud_rate_state);
+		err = -EPERM;
+	}
+
+	/* Finally flush the TTY so we are sure that is no bad data there */
+	hci_uart_flush_buffer(hu);
+	dev_dbg(MAIN_DEV, "Flushing TTY after baud rate change\n");
+	/* Finished. Set state to IDLE */
+	dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+	uart_info->baud_rate_state = BAUD_IDLE;
+
+	return err;
+}
+
+/**
+ * uart_set_baud_rate() - External Interface for setting baud rate
+ * @dev:		Transport device information.
+ * @low_baud:	whether switch to low_baud or high.
+ *
+ * Returns:
+ *   none
+ */
+static void uart_set_baud_rate(struct cg2900_chip_dev *dev, bool low_baud)
+{
+	struct uart_info *uart_info = dev_get_drvdata(dev->dev);
+
+	if (!uart_info->hu) {
+		dev_err(MAIN_DEV, "uart_set_baud_rate: UART not open\n");
+		return;
+	}
+
+	if (low_baud) {
+		if (uart_info->baud_rate != uart_default_baud) {
+			dev_dbg(MAIN_DEV, "Changing BAUD now to : %d",
+					uart_default_baud);
+			set_baud_rate(uart_info->hu, uart_default_baud);
+		} else {
+			dev_dbg(MAIN_DEV, "BAUD is already set to :%d",
+					uart_default_baud);
+		}
+	} else {
+		if (uart_info->baud_rate != uart_high_baud) {
+			dev_dbg(MAIN_DEV, "Changing BAUD now to : %d",
+					uart_high_baud);
+			set_baud_rate(uart_info->hu, uart_high_baud);
+		} else {
+			dev_dbg(MAIN_DEV, "BAUD is already set to :%d",
+					uart_high_baud);
+		}
+	}
+}
+
+/**
+ * uart_write() - Transmit data to CG2900 over UART.
+ * @dev:	Transport device information.
+ * @skb:	SK buffer to transmit.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Errors from create_work_item.
+ */
+static int uart_write(struct cg2900_chip_dev *dev, struct sk_buff *skb)
+{
+	int err;
+	struct uart_info *uart_info = dev_get_drvdata(dev->dev);
+
+	if (uart_debug)
+		dev_dbg(MAIN_DEV, "uart_write: data len = %d\n", skb->len);
+
+	/* Queue the sk_buffer... */
+	skb_queue_tail(&uart_info->tx_queue, skb);
+
+	/* ...and start TX operation */
+
+	err = create_work_item(uart_info, work_do_transmit);
+	if (err)
+		dev_err(MAIN_DEV,
+			"Failed to create work item (%d) uart_tty_wakeup\n",
+			err);
+
+	return err;
+}
+
+/**
+ * uart_open() - Open the CG2900 UART for data transfers.
+ * @dev:	Transport device information.
+ *
+ * Returns:
+ *   0 if there is no error,
+ *   -EACCES if write to transport failed,
+ *   -EIO if chip did not answer to commands.
+ *   Errors from set_baud_rate.
+ */
+static int uart_open(struct cg2900_chip_dev *dev)
+{
+	u8 *h4;
+	struct sk_buff *skb;
+	struct hci_command_hdr *cmd;
+	struct uart_info *uart_info = dev_get_drvdata(dev->dev);
+
+	if (!uart_info->hu) {
+		dev_err(MAIN_DEV, "uart_open: UART not open\n");
+		return -EACCES;
+	}
+
+	uart_info->baud_rate = uart_default_baud;
+	/*
+	 * Chip has just been started up. It has a system to autodetect
+	 * exact baud rate and transport to use. There are only a few commands
+	 * it will recognize and HCI Reset is one of them.
+	 * We therefore start with sending that before actually changing
+	 * baud rate.
+	 *
+	 * Create the Hci_Reset packet
+	 */
+
+	skb = alloc_skb(sizeof(*cmd) + HCI_H4_SIZE, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(MAIN_DEV, "Couldn't allocate sk_buff with length %d\n",
+			     sizeof(*cmd));
+		return -EACCES;
+	}
+	skb_reserve(skb, HCI_H4_SIZE);
+	cmd = (struct hci_command_hdr *)skb_put(skb, sizeof(*cmd));
+	cmd->opcode = cpu_to_le16(HCI_OP_RESET);
+	cmd->plen = 0; /* No parameters for HCI reset */
+
+	h4 = skb_push(skb, HCI_H4_SIZE);
+	*h4 = HCI_BT_CMD_H4_CHANNEL;
+
+	dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_SENDING_RESET\n");
+	uart_info->baud_rate_state = BAUD_SENDING_RESET;
+	dev_dbg(MAIN_DEV, "Sending HCI reset before baud rate change\n");
+
+
+	/* Queue the sk_buffer... */
+	skb_queue_tail(&uart_info->tx_queue, skb);
+
+	(void)hci_uart_tx_wakeup(uart_info->hu);
+
+	/*
+	 * Wait for command complete. If error, exit without changing
+	 * baud rate.
+	 */
+	wait_event_timeout(uart_wait_queue,
+					BAUD_IDLE == uart_info->baud_rate_state,
+					msecs_to_jiffies(UART_RESP_TIMEOUT));
+	if (BAUD_IDLE != uart_info->baud_rate_state) {
+		dev_err(MAIN_DEV, "Failed to send HCI Reset\n");
+		dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+		uart_info->baud_rate_state = BAUD_IDLE;
+		return -EIO;
+	}
+
+	/* Just return if there will be no change of baud rate */
+	if (uart_default_baud != uart_high_baud) {
+
+		/*
+		 * Don't change to high baud yet
+		 * as there is bug CG2905/10 PG1 that firmware d/l
+		 * has to take place at lower baud
+		 * after firmware d/l UART will be switched
+		 * to higher baud.
+		 */
+		if (CG2905_PG1_1_REV != dev->chip.hci_revision &&
+				CG2910_PG1_REV != dev->chip.hci_revision)
+			return set_baud_rate(uart_info->hu, uart_high_baud);
+	}
+
+	return 0;
+}
+
+/**
+ * uart_set_chip_power() - Enable or disable the CG2900.
+ * @chip_on:	true if chip shall be enabled, false otherwise.
+ */
+static void uart_set_chip_power(struct cg2900_chip_dev *dev, bool chip_on)
+{
+	int uart_baudrate = uart_default_baud;
+	struct cg2900_platform_data *pf_data;
+	struct uart_info *uart_info;
+
+	pf_data = dev_get_platdata(dev->dev);
+	uart_info = dev_get_drvdata(dev->dev);
+
+	dev_info(MAIN_DEV, "Set chip power: %s\n",
+		    (chip_on ? "ENABLE" : "DISABLE"));
+
+	/* Cancel any ongoing works.*/
+	cancel_work_sync(&uart_info->wakeup_work.work);
+	cancel_delayed_work_sync(&uart_info->sleep_work.work);
+
+	mutex_lock(&uart_info->sleep_state_lock);
+
+	if (!uart_info->hu) {
+		dev_err(MAIN_DEV, "Hci uart struct is not allocated\n");
+		goto unlock;
+	}
+
+	if (chip_on) {
+		if (!uart_info->suspend_blocked) {
+			uart_info->suspend_blocked = true;
+			pm_qos_update_request(&uart_info->pm_qos_latency,
+					      CG2900_PM_QOS_LATENCY);
+		}
+		if (uart_info->sleep_state != CHIP_POWERED_DOWN) {
+			dev_err(MAIN_DEV, "Chip is already powered up (%d)\n",
+				uart_info->sleep_state);
+			goto unlock;
+		}
+
+		if (cg2900_enable_regulator(uart_info))
+			goto unlock;
+
+		if (pf_data->enable_chip) {
+			pf_data->enable_chip(dev);
+			dev_dbg(MAIN_DEV, "New sleep_state: CHIP_AWAKE\n");
+			uart_info->sleep_state = CHIP_AWAKE;
+		}
+
+		(void)hci_uart_set_baudrate(uart_info->hu, uart_baudrate);
+
+		hci_uart_flow_ctrl(uart_info->hu, FLOW_ON);
+		hci_uart_set_break(uart_info->hu, BREAK_OFF);
+		udelay(UART_MIN_BREAK_OFF_TIME);
+	} else {
+		/* Turn off the chip.*/
+		switch (uart_info->sleep_state) {
+		case CHIP_AWAKE:
+			break;
+		case CHIP_FALLING_ASLEEP:
+			hci_uart_set_break(uart_info->hu, BREAK_OFF);
+			udelay(UART_MIN_BREAK_OFF_TIME);
+			break;
+		case CHIP_SUSPENDED:
+		case CHIP_ASLEEP:
+			unset_cts_irq(uart_info);
+			enable_uart_pins(uart_info);
+			break;
+		default:
+			break;
+		}
+
+		if (uart_info->suspend_blocked) {
+			uart_info->suspend_blocked = false;
+			pm_qos_update_request(&uart_info->pm_qos_latency,
+					      PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE);
+		}
+
+		if (pf_data->disable_chip) {
+			pf_data->disable_chip(dev);
+			dev_dbg(MAIN_DEV,
+				"New sleep_state: CHIP_POWERED_DOWN\n");
+			uart_info->sleep_state = CHIP_POWERED_DOWN;
+			/*
+			 * This is to ensure when chip is switched
+			 * on next time sleep_flags is again set with
+			 * SLEEP_CORE_READY when startup is done properly
+			 */
+			clear_bit(SLEEP_CORE_READY, &uart_info->sleep_flags);
+		}
+
+		cg2900_disable_regulator(uart_info);
+		/*
+		 * Setting baud rate to 0 will tell UART driver to shut off its
+		 * clocks.
+		 */
+		(void)hci_uart_set_baudrate(uart_info->hu, ZERO_BAUD_RATE);
+
+		spin_lock_bh(&uart_info->rx_skb_lock);
+		if (uart_info->rx_skb) {
+			/*
+			 * Reset the uart_info state so that
+			 * next packet can be handled
+			 * correctly by driver.
+			 */
+			dev_dbg(MAIN_DEV, "Power off in the middle of data receiving?"
+							"Reseting state machine.\n");
+			kfree_skb(uart_info->rx_skb);
+			uart_info->rx_skb = NULL;
+			uart_info->rx_state = W4_PACKET_TYPE;
+			uart_info->rx_count = 0;
+		}
+		spin_unlock_bh(&uart_info->rx_skb_lock);
+	}
+
+unlock:
+	mutex_unlock(&(uart_info->sleep_state_lock));
+}
+
+/**
+ * uart_chip_startup_finished() - CG2900 startup finished.
+ * @dev:	Transport device information.
+ */
+static void uart_chip_startup_finished(struct cg2900_chip_dev *dev)
+{
+	struct uart_info *uart_info = dev_get_drvdata(dev->dev);
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+
+	/*
+	 * Chip startup is done, now chip is allowed
+	 * to go to sleep
+	 */
+	set_bit(SLEEP_CORE_READY, &uart_info->sleep_flags);
+	/* Schedule work to put the chip and transport to sleep. */
+	if (timeout_jiffies)
+		queue_delayed_work(uart_info->wq, &uart_info->sleep_work.work,
+				timeout_jiffies);
+}
+/**
+ * uart_close() - Close the CG2900 UART for data transfers.
+ * @dev:	Transport device information.
+ *
+ * Returns:
+ *   0 if there is no error.
+ */
+static int uart_close(struct cg2900_chip_dev *dev)
+{
+	/* The chip is already shut down. Power off the chip. */
+	uart_set_chip_power(dev, false);
+	return 0;
+}
+
+/**
+ * send_skb_to_core() - Sends packet received from UART to CG2900 Core.
+ * @skb:	Received data packet.
+ *
+ * This function checks if UART is waiting for Command complete event,
+ * see set_baud_rate.
+ * If it is waiting it checks if it is the expected packet and the status.
+ * If not is passes the packet to CG2900 Core.
+ */
+static void send_skb_to_core(struct uart_info *uart_info, struct sk_buff *skb)
+{
+	u8 status;
+
+	if (!skb) {
+		dev_err(MAIN_DEV, "send_skb_to_core: Received NULL as skb\n");
+		return;
+	}
+
+	if (BAUD_WAITING == uart_info->baud_rate_state) {
+		/*
+		 * Should only really be one packet received now:
+		 * the CmdComplete for the SetBaudrate command
+		 * Let's see if this is the packet we are waiting for.
+		 */
+		if (!is_bt_cmd_complete_no_param(skb,
+				CG2900_BT_OP_VS_SET_BAUD_RATE, &status)) {
+			/*
+			 * Received other event. Should not really happen,
+			 * but pass the data to CG2900 Core anyway.
+			 */
+			dev_dbg(MAIN_DEV, "Sending packet to CG2900 Core while "
+				"waiting for BaudRate CmdComplete\n");
+			uart_info->chip_dev.c_cb.data_from_chip
+				(&uart_info->chip_dev, skb);
+			return;
+		}
+
+		/*
+		 * We have received complete event for our baud rate
+		 * change command
+		 */
+		if (HCI_BT_ERROR_NO_ERROR == status) {
+			dev_dbg(MAIN_DEV, "Received baud rate change complete "
+				"event OK\n");
+			dev_dbg(MAIN_DEV,
+				"New baud_rate_state: BAUD_SUCCESS\n");
+			uart_info->baud_rate_state = BAUD_SUCCESS;
+		} else {
+			dev_err(MAIN_DEV,
+				"Received baud rate change complete event "
+				"with status 0x%X\n", status);
+			dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_FAIL\n");
+			uart_info->baud_rate_state = BAUD_FAIL;
+		}
+		wake_up_all(&uart_wait_queue);
+		kfree_skb(skb);
+	} else if (BAUD_SENDING_RESET == uart_info->baud_rate_state) {
+		/*
+		 * Should only really be one packet received now:
+		 * the CmdComplete for the Reset command
+		 * Let's see if this is the packet we are waiting for.
+		 */
+		if (!is_bt_cmd_complete_no_param(skb, HCI_OP_RESET, &status)) {
+			/*
+			 * Received other event. Should not really happen,
+			 * but pass the data to CG2900 Core anyway.
+			 */
+			dev_dbg(MAIN_DEV, "Sending packet to CG2900 Core while "
+				"waiting for Reset CmdComplete\n");
+			uart_info->chip_dev.c_cb.data_from_chip
+					(&uart_info->chip_dev, skb);
+			return;
+		}
+
+		/*
+		 * We have received complete event for our baud rate
+		 * change command
+		 */
+		if (HCI_BT_ERROR_NO_ERROR == status) {
+			dev_dbg(MAIN_DEV,
+				"Received HCI reset complete event OK\n");
+			/*
+			 * Go back to BAUD_IDLE since this was not really
+			 * baud rate change but just a preparation of the chip
+			 * to be ready to receive commands.
+			 */
+			dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_IDLE\n");
+			uart_info->baud_rate_state = BAUD_IDLE;
+		} else {
+			dev_err(MAIN_DEV,
+				"Received HCI reset complete event with "
+				"status 0x%X", status);
+			dev_dbg(MAIN_DEV, "New baud_rate_state: BAUD_FAIL\n");
+			uart_info->baud_rate_state = BAUD_FAIL;
+		}
+		wake_up_all(&uart_wait_queue);
+		kfree_skb(skb);
+	} else {
+		/* Just pass data to CG2900 Core */
+		uart_info->chip_dev.c_cb.data_from_chip
+						(&uart_info->chip_dev, skb);
+	}
+}
+
+/**
+ * check_data_len() - Check number of bytes to receive.
+ * @len:	Number of bytes left to receive.
+ */
+static void check_data_len(struct uart_info *uart_info, int len)
+{
+	/* First get number of bytes left in the sk_buffer */
+	register int room = skb_tailroom(uart_info->rx_skb);
+
+	if (!len) {
+		/* No data left to receive. Transmit to CG2900 Core */
+		send_skb_to_core(uart_info, uart_info->rx_skb);
+	} else if (len > room) {
+		dev_err(MAIN_DEV, "Data length is too large (%d > %d)\n",
+			len, room);
+		kfree_skb(uart_info->rx_skb);
+	} else {
+		/*
+		 * "Normal" case. Switch to data receiving state and store
+		 * data length.
+		 */
+		uart_info->rx_state = W4_DATA;
+		uart_info->rx_count = len;
+		return;
+	}
+
+	uart_info->rx_state = W4_PACKET_TYPE;
+	uart_info->rx_skb   = NULL;
+	uart_info->rx_count = 0;
+}
+
+/**
+ * work_restart_sleep() - Cancel pending sleep_work, wake-up driver and
+ * schedule new sleep_work in a work context.
+ * @work:	work which needs to be done.
+ */
+static void work_restart_sleep(struct work_struct *work)
+{
+	struct uart_work_struct *current_work =
+		container_of(work, struct uart_work_struct, work);
+	struct uart_info *uart_info = (struct uart_info *)current_work->data;
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	uart_info->rx_in_progress = false;
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	/* Cancel pending sleep work if there is any. */
+	cancel_delayed_work_sync(&uart_info->sleep_work.work);
+
+	wake_up_chip(uart_info);
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	/*
+	 * If there are no ongoing transfers schedule the sleep work.
+	 */
+	if (!(uart_info->tx_in_progress) && timeout_jiffies)
+		queue_delayed_work(uart_info->wq,
+				&uart_info->sleep_work.work,
+				timeout_jiffies);
+	spin_unlock_bh(&(uart_info->transmission_lock));
+}
+
+/**
+ * cg2900_hu_receive() - Handles received UART data.
+ * @data:	Data received
+ * @count:	Number of bytes received
+ *
+ * The cg2900_hu_receive() function handles received UART data and puts it
+ * together to one complete packet.
+ *
+ * Returns:
+ *   Number of bytes not handled, i.e. 0 = no error.
+ */
+static int cg2900_hu_receive(struct hci_uart *hu,
+					void *data, int count)
+{
+	const u8 *r_ptr;
+	u8 *w_ptr;
+	int len;
+	struct hci_event_hdr	*evt;
+	struct hci_acl_hdr	*acl;
+	struct nfc_hci_hdr	*nfc;
+	union fm_leg_evt_or_irq	*fm;
+	struct gnss_hci_hdr	*gnss;
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+	u8 *tmp;
+
+	r_ptr = (const u8 *)data;
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+	/* Mark that there is an ongoing transfer. */
+	uart_info->rx_in_progress = true;
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	/* Cancel pending sleep work if there is any. */
+	cancel_delayed_work(&uart_info->sleep_work.work);
+
+	if (uart_debug)
+		print_hex_dump_bytes(NAME " RX:\t", DUMP_PREFIX_NONE,
+				     data, count);
+
+	spin_lock_bh(&uart_info->rx_skb_lock);
+
+	/* Continue while there is data left to handle */
+	while (count) {
+		/*
+		 * If we have already received a packet we know how many bytes
+		 * there are left.
+		 */
+		if (!uart_info->rx_count)
+			goto check_h4_header;
+
+		/* First copy received data into the skb_rx */
+		len = min_t(unsigned int, uart_info->rx_count, count);
+		memcpy(skb_put(uart_info->rx_skb, len), r_ptr, len);
+		/* Update counters from the length and step the data pointer */
+		uart_info->rx_count -= len;
+		count -= len;
+		r_ptr += len;
+
+		if (uart_info->rx_count)
+			/*
+			 * More data to receive to current packet. Break and
+			 * wait for next data on the UART.
+			 */
+			break;
+
+		/* Handle the different states */
+		tmp = uart_info->rx_skb->data + CG2900_SKB_RESERVE;
+		switch (uart_info->rx_state) {
+		case W4_DATA:
+			/*
+			 * Whole data packet has been received.
+			 * Transmit it to CG2900 Core.
+			 */
+			send_skb_to_core(uart_info, uart_info->rx_skb);
+
+			uart_info->rx_state = W4_PACKET_TYPE;
+			uart_info->rx_skb = NULL;
+			continue;
+
+		case W4_EVENT_HDR:
+			evt = (struct hci_event_hdr *)tmp;
+			check_data_len(uart_info, evt->plen);
+			/* Header read. Continue with next bytes */
+			continue;
+
+		case W4_ACL_HDR:
+			acl = (struct hci_acl_hdr *)tmp;
+			check_data_len(uart_info, le16_to_cpu(acl->dlen));
+			/* Header read. Continue with next bytes */
+			continue;
+
+		case W4_NFC_HDR:
+			nfc = (struct nfc_hci_hdr *)tmp;
+			/*
+			 * NFC Packet(s) have 1 extra byte for checksum.
+			 * This is not indicated by the byte length determined
+			 * from the received NFC Packet over HCI. So
+			 * length of received NFC packet should be updated
+			 * accordingly.
+			 */
+			check_data_len(uart_info, le16_to_cpu(nfc->plen) +
+					NFC_CHECKSUM_DATA_LEN);
+			/* Header read. Continue with next bytes */
+			continue;
+
+		case W4_FM_RADIO_HDR:
+			fm = (union fm_leg_evt_or_irq *)tmp;
+			check_data_len(uart_info, fm->param_length);
+			/* Header read. Continue with next bytes */
+			continue;
+
+		case W4_GNSS_HDR:
+			gnss = (struct gnss_hci_hdr *)tmp;
+			check_data_len(uart_info, le16_to_cpu(gnss->plen));
+			/* Header read. Continue with next bytes */
+			continue;
+
+		default:
+			dev_err(MAIN_DEV,
+				"Bad state indicating memory overwrite "
+				"(0x%X)\n", (u8)(uart_info->rx_state));
+			break;
+		}
+
+check_h4_header:
+		/* Check which H:4 packet this is and update RX states */
+		if (*r_ptr == HCI_BT_EVT_H4_CHANNEL) {
+			uart_info->rx_state = W4_EVENT_HDR;
+			uart_info->rx_count = HCI_BT_EVT_HDR_SIZE;
+		} else if (*r_ptr == HCI_BT_ACL_H4_CHANNEL) {
+			uart_info->rx_state = W4_ACL_HDR;
+			uart_info->rx_count = HCI_BT_ACL_HDR_SIZE;
+		} else if (*r_ptr == HCI_NFC_H4_CHANNEL) {
+			uart_info->rx_state = W4_NFC_HDR;
+			uart_info->rx_count = HCI_NFC_HDR_SIZE;
+		} else if (*r_ptr == HCI_FM_RADIO_H4_CHANNEL) {
+			uart_info->rx_state = W4_FM_RADIO_HDR;
+			uart_info->rx_count = HCI_FM_RADIO_HDR_SIZE;
+		} else if (*r_ptr == HCI_GNSS_H4_CHANNEL) {
+			uart_info->rx_state = W4_GNSS_HDR;
+			uart_info->rx_count = HCI_GNSS_HDR_SIZE;
+		} else {
+			dev_err(MAIN_DEV, "Unknown HCI packet type 0x%X\n",
+				(u8)*r_ptr);
+			r_ptr++;
+			count--;
+			continue;
+		}
+
+		/*
+		 * Allocate packet. We do not yet know the size and therefore
+		 * allocate max size.
+		 */
+		uart_info->rx_skb = alloc_rx_skb(RX_SKB_MAX_SIZE, GFP_ATOMIC);
+		if (!uart_info->rx_skb) {
+			dev_err(MAIN_DEV,
+				"Can't allocate memory for new packet\n");
+			uart_info->rx_state = W4_PACKET_TYPE;
+			uart_info->rx_count = 0;
+
+			spin_lock_bh(&(uart_info->transmission_lock));
+			uart_info->rx_in_progress = false;
+			spin_unlock_bh(&(uart_info->transmission_lock));
+
+			spin_unlock_bh(&uart_info->rx_skb_lock);
+			return 0;
+		}
+
+		/* Write the H:4 header first in the sk_buffer */
+		w_ptr = skb_put(uart_info->rx_skb, 1);
+		*w_ptr = *r_ptr;
+
+		/* First byte (H4 header) read. Goto next byte */
+		r_ptr++;
+		count--;
+	}
+
+	(void)queue_work(uart_info->wq, &uart_info->restart_sleep_work.work);
+
+	spin_unlock_bh(&uart_info->rx_skb_lock);
+	return count;
+}
+
+/**
+ * cg2900_hu_open() - Called when UART line discipline changed to N_HCI.
+ * @hu:	Pointer to associated Hci uart structure.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Errors from cg2900_register_trans_driver.
+ */
+static int cg2900_hu_open(struct hci_uart *hu)
+{
+	int err;
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+
+	if (!uart_info)
+		return -EACCES;
+
+	dev_info(MAIN_DEV, "UART opened\n");
+
+	skb_queue_head_init(&uart_info->tx_queue);
+
+	uart_info->hu = hu;
+
+	/* Tell CG2900 Core that UART is connected */
+	err = cg2900_register_trans_driver(&uart_info->chip_dev);
+	if (err)
+		dev_err(MAIN_DEV, "Could not register transport driver (%d)\n",
+			err);
+
+	if (hu->tty->ops->tiocmget && hu->tty->ops->break_ctl)
+		set_bit(SLEEP_TTY_READY, &uart_info->sleep_flags);
+	else {
+		dev_err(MAIN_DEV, "Sleep mode not available\n");
+		clear_bit(SLEEP_TTY_READY, &uart_info->sleep_flags);
+	}
+
+	return err;
+
+}
+
+/**
+ * cg2900_hu_close() - Close UART tty.
+ * @hu:	Pointer to associated hci_uart structure.
+ *
+ * The uart_tty_close() function is called when the line discipline is changed
+ * to something else, the TTY is closed, or the TTY detects a hangup.
+ */
+static int cg2900_hu_close(struct hci_uart *hu)
+{
+	int err;
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+
+
+	BUG_ON(!uart_info);
+	BUG_ON(!uart_info->wq);
+
+	clear_bit(SLEEP_TTY_READY, &uart_info->sleep_flags);
+
+	/* Purge any stored sk_buffers */
+	skb_queue_purge(&uart_info->tx_queue);
+
+	spin_lock_bh(&uart_info->rx_skb_lock);
+	if (uart_info->rx_skb) {
+		kfree_skb(uart_info->rx_skb);
+		uart_info->rx_skb = NULL;
+	}
+	spin_unlock_bh(&uart_info->rx_skb_lock);
+
+	dev_info(MAIN_DEV, "UART closed\n");
+	err = create_work_item(uart_info, work_hw_deregistered);
+	if (err)
+		dev_err(MAIN_DEV, "Failed to create work item (%d) "
+			"work_hw_deregistered\n", err);
+
+	uart_info->hu = NULL;
+
+	return 0;
+}
+
+/**
+ * cg2900_hu_dequeue() - Get new skbuff.
+ * @hu: Pointer to associated hci_uart structure.
+ *
+ * The uart_tty_close() function is called when the line discipline is changed
+ * to something else, the TTY is closed, or the TTY detects a hangup.
+ */
+static struct sk_buff *cg2900_hu_dequeue(struct hci_uart *hu)
+{
+	struct sk_buff *skb;
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+	unsigned long timeout_jiffies = get_sleep_timeout(uart_info);
+
+	spin_lock_bh(&(uart_info->transmission_lock));
+
+	skb = skb_dequeue(&uart_info->tx_queue);
+
+	if (!skb)
+		uart_info->tx_in_progress = false;
+
+	/*
+	 * If there are no ongoing transfers schedule the sleep work.
+	 */
+	if (!(uart_info->rx_in_progress) && timeout_jiffies && !skb)
+		queue_delayed_work(uart_info->wq,
+				&uart_info->sleep_work.work,
+				timeout_jiffies);
+
+	spin_unlock_bh(&(uart_info->transmission_lock));
+
+	if (BAUD_SENDING == uart_info->baud_rate_state && !skb)
+		finish_setting_baud_rate(hu);
+	/*
+	 * If it's set baud rate cmd set correct baud state and after
+	 * sending is finished inform the tty driver about the new
+	 * baud rate.
+	 */
+	if ((BAUD_START == uart_info->baud_rate_state) &&
+		skb && (is_set_baud_rate_cmd(skb->data))) {
+		dev_dbg(MAIN_DEV, "UART set baud rate cmd found\n");
+		uart_info->baud_rate_state = BAUD_SENDING;
+	}
+
+	if (uart_debug && skb)
+		print_hex_dump_bytes(NAME " TX:\t", DUMP_PREFIX_NONE,
+				     skb->data, skb->len);
+
+	return skb;
+}
+
+/**
+ * cg2900_hu_flush() - Flush buffers.
+ * @hu: Pointer to associated hci_uart structure.
+ *
+ */
+static int cg2900_hu_flush(struct hci_uart *hu)
+{
+	struct uart_info *uart_info = dev_get_drvdata(hu->proto->dev);
+
+	dev_dbg(MAIN_DEV, "ui %p", uart_info);
+	skb_queue_purge(&uart_info->tx_queue);
+	return 0;
+}
+
+/**
+ * cg2900_uart_probe() - Initialize CG2900 UART resources.
+ * @pdev:	Platform device.
+ *
+ * This function initializes the module and registers to the UART framework.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ *   -ECHILD for failed work queue creation.
+ *   Error codes generated by tty_register_ldisc.
+ */
+static int __devinit cg2900_uart_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct uart_info *uart_info;
+	struct hci_uart_proto *p;
+	struct resource *resource;
+
+	pr_debug("cg2900_uart_probe");
+
+	uart_info = kzalloc(sizeof(*uart_info), GFP_KERNEL);
+	if (!uart_info) {
+		pr_err("Couldn't allocate uart_info");
+		return -ENOMEM;
+	}
+
+	uart_info->sleep_state = CHIP_POWERED_DOWN;
+	mutex_init(&(uart_info->sleep_state_lock));
+
+	spin_lock_init(&(uart_info->transmission_lock));
+	spin_lock_init(&(uart_info->rx_skb_lock));
+
+	uart_info->chip_dev.t_cb.open = uart_open;
+	uart_info->chip_dev.t_cb.close = uart_close;
+	uart_info->chip_dev.t_cb.write = uart_write;
+	uart_info->chip_dev.t_cb.set_chip_power = uart_set_chip_power;
+	uart_info->chip_dev.t_cb.chip_startup_finished =
+			uart_chip_startup_finished;
+	uart_info->chip_dev.t_cb.set_baud_rate =
+			uart_set_baud_rate;
+	uart_info->chip_dev.pdev = pdev;
+	uart_info->chip_dev.dev = &pdev->dev;
+	uart_info->chip_dev.t_data = uart_info;
+
+	pm_qos_add_request(&uart_info->pm_qos_latency, PM_QOS_CPU_DMA_LATENCY,
+			   PM_QOS_CPU_DMA_LAT_DEFAULT_VALUE);
+
+	resource = platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+						"cts_irq");
+	if (!resource) {
+		dev_err(&pdev->dev, "CTS IRQ does not exist\n");
+		err = -EINVAL;
+		goto error_handling_free;
+	}
+	uart_info->cts_irq = resource->start;
+
+	resource = platform_get_resource_byname(pdev, IORESOURCE_IO,
+						"cts_gpio");
+	if (!resource) {
+		dev_err(&pdev->dev, "CTS GPIO does not exist\n");
+		err = -EINVAL;
+		goto error_handling_free;
+	}
+	uart_info->cts_gpio = resource->start;
+
+	/* Init UART TX work queue */
+	uart_info->wq = create_singlethread_workqueue(UART_WQ_NAME);
+	if (!uart_info->wq) {
+		dev_err(MAIN_DEV, "Could not create workqueue\n");
+		err = -ECHILD; /* No child processes */
+		goto error_handling_free;
+	}
+
+	/* Initialize sleep work data */
+	uart_info->sleep_work.data = uart_info;
+	INIT_DELAYED_WORK(&uart_info->sleep_work.work, set_chip_sleep_mode);
+
+	/* Initialize wake-up work data */
+	uart_info->wakeup_work.data = uart_info;
+	INIT_WORK(&uart_info->wakeup_work.work, work_wake_up_chip);
+
+	/* Initialize after_receive work data */
+	uart_info->restart_sleep_work.data = uart_info;
+	INIT_WORK(&uart_info->restart_sleep_work.work, work_restart_sleep);
+
+	uart_info->dev = &pdev->dev;
+
+	p = kzalloc(sizeof(*p), GFP_KERNEL);
+	if (!p) {
+		dev_err(MAIN_DEV, "cg2900_uart_probe: Could not allocate p\n");
+		goto error_handling_wq;
+	}
+
+	p->dev		= uart_info->dev;
+	p->id		= HCI_UART_STE;
+	p->open		= &cg2900_hu_open;
+	p->close	= &cg2900_hu_close;
+	p->recv		= &cg2900_hu_receive;
+	p->dequeue	= &cg2900_hu_dequeue;
+	p->flush	= &cg2900_hu_flush;
+
+	dev_set_drvdata(uart_info->dev, (void *)uart_info);
+
+	err = hci_uart_register_proto(p);
+	if (err) {
+		dev_err(MAIN_DEV, "cg2900_uart_probe: Can not register "
+			"protocol\n");
+		kfree(p);
+		goto error_handling_wq;
+	}
+
+	goto finished;
+
+error_handling_wq:
+	destroy_workqueue(uart_info->wq);
+error_handling_free:
+	kfree(uart_info);
+	uart_info = NULL;
+finished:
+	return err;
+}
+
+/**
+ * cg2900_uart_remove() - Release CG2900 UART resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   Error codes generated by tty_unregister_ldisc.
+ */
+static int __devexit cg2900_uart_remove(struct platform_device *pdev)
+{
+	struct uart_info *uart_info = dev_get_drvdata(&pdev->dev);
+
+	pr_debug("cg2900_uart_remove");
+
+	if (!uart_info)
+		return -ECHILD;
+
+	if (uart_info->hu)
+		hci_uart_unregister_proto(uart_info->hu->proto);
+
+	pm_qos_remove_request(&uart_info->pm_qos_latency);
+	destroy_workqueue(uart_info->wq);
+
+	dev_info(MAIN_DEV, "CG2900 UART removed\n");
+	kfree(uart_info);
+	uart_info = NULL;
+	return 0;
+}
+
+static struct platform_driver cg2900_uart_driver = {
+	.driver = {
+		.name	= "cg2900-uart",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_uart_probe,
+	.remove	= __devexit_p(cg2900_uart_remove),
+#ifdef CONFIG_PM
+	.suspend = cg2900_uart_suspend,
+	.resume = cg2900_uart_resume
+#endif
+};
+
+
+/**
+ * cg2900_uart_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_uart_init(void)
+{
+	pr_debug("cg2900_uart_init");
+	return platform_driver_register(&cg2900_uart_driver);
+}
+
+/**
+ * cg2900_uart_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_uart_exit(void)
+{
+	pr_debug("cg2900_uart_exit");
+	platform_driver_unregister(&cg2900_uart_driver);
+}
+
+module_init(cg2900_uart_init);
+module_exit(cg2900_uart_exit);
+
+module_param(uart_default_baud, int, S_IRUGO);
+MODULE_PARM_DESC(uart_default_baud,
+		 "Default UART baud rate, e.g. 115200. If not set 115200 will "
+		 "be used.");
+
+module_param(uart_high_baud, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(uart_high_baud,
+		 "High speed UART baud rate, e.g. 4000000.  If not set 3000000 "
+		 "will be used.");
+
+module_param(uart_debug, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(uart_debug, "Enable/Disable debug. 0 means Debug disabled.");
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("ST-Ericsson CG2900 UART Driver");
diff --git a/drivers/staging/cg2900/bluetooth/hci_ldisc.c b/drivers/staging/cg2900/bluetooth/hci_ldisc.c
new file mode 100644
index 00000000000..f6518659969
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/hci_ldisc.c
@@ -0,0 +1,645 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * This file is a staging solution and shall be integrated into
+ * /drivers/bluetooth/hci_ldisc.c.
+ *
+ * Original hci_ldisc.c file:
+ *  Copyright (C) 2000-2001  Qualcomm Incorporated
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2004-2005  Marcel Holtmann <marcel@holtmann.org>
+ */
+
+#include <linux/module.h>
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/interrupt.h>
+#include <linux/ptrace.h>
+#include <linux/poll.h>
+
+#include <linux/slab.h>
+#include <linux/tty.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/signal.h>
+#include <linux/ioctl.h>
+#include <linux/skbuff.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "hci_uart.h"
+
+#define VERSION "2.3"
+
+#define TTY_BREAK_ON		(-1)
+#define TTY_BREAK_OFF		(0)
+
+static bool reset;
+
+static struct hci_uart_proto *hup[HCI_UART_MAX_PROTO];
+
+int cg2900_hci_uart_register_proto(struct hci_uart_proto *p)
+{
+	if (p->id >= HCI_UART_MAX_PROTO)
+		return -EINVAL;
+
+	if (hup[p->id])
+		return -EEXIST;
+
+	hup[p->id] = p;
+
+	return 0;
+}
+
+int cg2900_hci_uart_unregister_proto(struct hci_uart_proto *p)
+{
+	if (p->id >= HCI_UART_MAX_PROTO)
+		return -EINVAL;
+
+	if (!hup[p->id])
+		return -EINVAL;
+
+	hup[p->id] = NULL;
+
+	return 0;
+}
+
+static struct hci_uart_proto *hci_uart_get_proto(unsigned int id)
+{
+	if (id >= HCI_UART_MAX_PROTO)
+		return NULL;
+
+	return hup[id];
+}
+
+static inline void hci_uart_tx_complete(struct hci_uart *hu, int pkt_type)
+{
+	struct hci_dev *hdev = hu->hdev;
+
+	if (!hdev)
+		return;
+
+	/* Update HCI stat counters */
+	switch (pkt_type) {
+	case HCI_COMMAND_PKT:
+		hdev->stat.cmd_tx++;
+		break;
+
+	case HCI_ACLDATA_PKT:
+		hdev->stat.acl_tx++;
+		break;
+
+	case HCI_SCODATA_PKT:
+		hdev->stat.sco_tx++;
+		break;
+	}
+}
+
+static inline struct sk_buff *hci_uart_dequeue(struct hci_uart *hu)
+{
+	struct sk_buff *skb = hu->tx_skb;
+
+	if (!skb)
+		skb = hu->proto->dequeue(hu);
+	else
+		hu->tx_skb = NULL;
+
+	return skb;
+}
+
+int cg2900_hci_uart_tx_wakeup(struct hci_uart *hu)
+{
+	struct tty_struct *tty = hu->tty;
+	struct hci_dev *hdev = hu->hdev;
+	struct sk_buff *skb;
+
+	if (test_and_set_bit(HCI_UART_SENDING, &hu->tx_state)) {
+		set_bit(HCI_UART_TX_WAKEUP, &hu->tx_state);
+		return 0;
+	}
+
+	BT_DBG("");
+
+restart:
+	clear_bit(HCI_UART_TX_WAKEUP, &hu->tx_state);
+
+	while ((skb = hci_uart_dequeue(hu))) {
+		int len;
+
+		set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+		len = tty->ops->write(tty, skb->data, skb->len);
+		if (hdev)
+			hdev->stat.byte_tx += len;
+
+		skb_pull(skb, len);
+		if (skb->len) {
+			hu->tx_skb = skb;
+			break;
+		}
+
+		hci_uart_tx_complete(hu, bt_cb(skb)->pkt_type);
+		kfree_skb(skb);
+	}
+
+	if (test_bit(HCI_UART_TX_WAKEUP, &hu->tx_state))
+		goto restart;
+
+	clear_bit(HCI_UART_SENDING, &hu->tx_state);
+	return 0;
+}
+
+int cg2900_hci_uart_set_break(struct hci_uart *hu, bool break_on)
+{
+	struct tty_struct *tty = hu->tty;
+	int state = TTY_BREAK_OFF;
+
+	if (break_on)
+		state = TTY_BREAK_ON;
+
+	if (tty->ops->break_ctl)
+		return tty->ops->break_ctl(tty, state);
+	else
+		return -EOPNOTSUPP;
+}
+
+void cg2900_hci_uart_flow_ctrl(struct hci_uart *hu, bool flow_on)
+{
+	if (flow_on)
+		tty_unthrottle(hu->tty);
+	else
+		tty_throttle(hu->tty);
+}
+
+int cg2900_hci_uart_set_baudrate(struct hci_uart *hu, int baud)
+{
+	struct ktermios old_termios;
+	struct tty_struct *tty = hu->tty;
+
+	if (!tty->ops->set_termios)
+		return -EOPNOTSUPP;
+
+	mutex_lock(&(tty->termios_mutex));
+	/* Start by storing the old termios. */
+	memcpy(&old_termios, tty->termios, sizeof(old_termios));
+
+	tty_encode_baud_rate(tty, baud, baud);
+
+	/* Finally inform the driver */
+	tty->ops->set_termios(tty, &old_termios);
+
+	mutex_unlock(&(tty->termios_mutex));
+
+	return 0;
+}
+
+int cg2900_hci_uart_tiocmget(struct hci_uart *hu)
+{
+	struct tty_struct *tty = hu->tty;
+
+	if (!tty->ops->tiocmget ||  !hu->fd)
+		return -EOPNOTSUPP;
+
+	return tty->ops->tiocmget(tty);
+}
+
+void cg2900_hci_uart_flush_buffer(struct hci_uart *hu)
+{
+	tty_driver_flush_buffer(hu->tty);
+}
+
+int cg2900_hci_uart_chars_in_buffer(struct hci_uart *hu)
+{
+	return tty_chars_in_buffer(hu->tty);
+}
+
+/* ------- Interface to HCI layer ------ */
+/* Initialize device */
+static int hci_uart_open(struct hci_dev *hdev)
+{
+	BT_DBG("%s %p", hdev->name, hdev);
+
+	/* Nothing to do for UART driver */
+
+	set_bit(HCI_RUNNING, &hdev->flags);
+
+	return 0;
+}
+
+/* Reset device */
+static int hci_uart_flush(struct hci_dev *hdev)
+{
+	struct hci_uart *hu  = hci_get_drvdata(hdev);
+	struct tty_struct *tty = hu->tty;
+
+	BT_DBG("hdev %p tty %p", hdev, tty);
+
+	if (hu->tx_skb) {
+		kfree_skb(hu->tx_skb); hu->tx_skb = NULL;
+	}
+
+	/* Flush any pending characters in the driver and discipline. */
+	tty_ldisc_flush(tty);
+	tty_driver_flush_buffer(tty);
+
+	if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
+		hu->proto->flush(hu);
+
+	return 0;
+}
+
+/* Close device */
+static int hci_uart_close(struct hci_dev *hdev)
+{
+	BT_DBG("hdev %p", hdev);
+
+	if (!test_and_clear_bit(HCI_RUNNING, &hdev->flags))
+		return 0;
+
+	hci_uart_flush(hdev);
+	hdev->flush = NULL;
+	return 0;
+}
+
+/* Send frames from HCI layer */
+static int hci_uart_send_frame(struct sk_buff *skb)
+{
+	struct hci_dev* hdev = (struct hci_dev *) skb->dev;
+	struct hci_uart *hu;
+
+	if (!hdev) {
+		BT_ERR("Frame for unknown device (hdev=NULL)");
+		return -ENODEV;
+	}
+
+	if (!test_bit(HCI_RUNNING, &hdev->flags))
+		return -EBUSY;
+
+	hu = hci_get_drvdata(hdev);
+
+	BT_DBG("%s: type %d len %d", hdev->name, bt_cb(skb)->pkt_type,
+	       skb->len);
+
+	hu->proto->enqueue(hu, skb);
+
+	hci_uart_tx_wakeup(hu);
+
+	return 0;
+}
+
+/* ------ LDISC part ------ */
+/* hci_uart_tty_open
+ *
+ *     Called when line discipline changed to HCI_UART.
+ *
+ * Arguments:
+ *     tty    pointer to tty info structure
+ * Return Value:
+ *     0 if success, otherwise error code
+ */
+static int hci_uart_tty_open(struct tty_struct *tty)
+{
+	struct hci_uart *hu = (void *) tty->disc_data;
+
+	BT_DBG("tty %p", tty);
+
+	/* FIXME: This btw is bogus, nothing requires the old ldisc to clear
+	   the pointer */
+	if (hu)
+		return -EEXIST;
+
+	/* Error if the tty has no write op instead of leaving an exploitable
+	   hole */
+	if (tty->ops->write == NULL)
+		return -EOPNOTSUPP;
+
+	hu = kzalloc(sizeof(struct hci_uart), GFP_KERNEL);
+	if (!hu) {
+		BT_ERR("Can't allocate control structure");
+		return -ENFILE;
+	}
+
+	tty->disc_data = hu;
+	hu->tty = tty;
+	tty->receive_room = 65536;
+
+	spin_lock_init(&hu->rx_lock);
+
+	/* Flush any pending characters in the driver and line discipline. */
+
+	/* FIXME: why is this needed. Note don't use ldisc_ref here as the
+	   open path is before the ldisc is referencable */
+
+	if (tty->ldisc->ops->flush_buffer)
+		tty->ldisc->ops->flush_buffer(tty);
+	tty_driver_flush_buffer(tty);
+
+	return 0;
+}
+
+/* hci_uart_tty_close()
+ *
+ *    Called when the line discipline is changed to something
+ *    else, the tty is closed, or the tty detects a hangup.
+ */
+static void hci_uart_tty_close(struct tty_struct *tty)
+{
+	struct hci_uart *hu = (void *)tty->disc_data;
+
+	BT_DBG("tty %p", tty);
+
+	/* Detach from the tty */
+	tty->disc_data = NULL;
+
+	if (hu) {
+		struct hci_dev *hdev = hu->hdev;
+
+		if (hdev)
+			hci_uart_close(hdev);
+
+		if (test_and_clear_bit(HCI_UART_PROTO_SET, &hu->flags)) {
+			hu->proto->close(hu);
+			if (hdev) {
+				hci_unregister_dev(hdev);
+				hci_free_dev(hdev);
+			}
+		}
+	}
+}
+
+/* hci_uart_tty_wakeup()
+ *
+ *    Callback for transmit wakeup. Called when low level
+ *    device driver can accept more send data.
+ *
+ * Arguments:        tty    pointer to associated tty instance data
+ * Return Value:    None
+ */
+static void hci_uart_tty_wakeup(struct tty_struct *tty)
+{
+	struct hci_uart *hu = (void *)tty->disc_data;
+
+	BT_DBG("");
+
+	if (!hu)
+		return;
+
+	clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+
+	if (tty != hu->tty)
+		return;
+
+	if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
+		hci_uart_tx_wakeup(hu);
+}
+
+/* hci_uart_tty_receive()
+ *
+ *     Called by tty low level driver when receive data is
+ *     available.
+ *
+ * Arguments:  tty          pointer to tty isntance data
+ *             data         pointer to received data
+ *             flags        pointer to flags for data
+ *             count        count of received data in bytes
+ *
+ * Return Value:    None
+ */
+static void hci_uart_tty_receive(struct tty_struct *tty, const u8 *data,
+				 char *flags, int count)
+{
+	struct hci_uart *hu = (void *)tty->disc_data;
+
+	if (!hu || tty != hu->tty)
+		return;
+
+	if (!test_bit(HCI_UART_PROTO_SET, &hu->flags))
+		return;
+
+	spin_lock(&hu->rx_lock);
+	hu->proto->recv(hu, (void *) data, count);
+	if (hu->hdev)
+		hu->hdev->stat.byte_rx += count;
+	spin_unlock(&hu->rx_lock);
+
+	tty_unthrottle(tty);
+}
+
+static int hci_uart_register_dev(struct hci_uart *hu)
+{
+	struct hci_dev *hdev;
+
+	BT_DBG("");
+
+	/* Initialize and register HCI device */
+	hdev = hci_alloc_dev();
+	if (!hdev) {
+		BT_ERR("Can't allocate HCI device");
+		return -ENOMEM;
+	}
+
+	hu->hdev = hdev;
+
+	hdev->bus = HCI_UART;
+	hci_set_drvdata(hdev, hu);
+
+	hdev->open  = hci_uart_open;
+	hdev->close = hci_uart_close;
+	hdev->flush = hci_uart_flush;
+	hdev->send  = hci_uart_send_frame;
+
+	if (!reset)
+		set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+
+	if (test_bit(HCI_UART_RAW_DEVICE, &hu->hdev_flags))
+		set_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks);
+
+	if (hci_register_dev(hdev) < 0) {
+		BT_ERR("Can't register HCI device");
+		hci_free_dev(hdev);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int hci_uart_set_proto(struct hci_uart *hu, int id)
+{
+	struct hci_uart_proto *p;
+	int err;
+
+	p = hci_uart_get_proto(id);
+	if (!p)
+		return -EPROTONOSUPPORT;
+
+	hu->proto = p;
+
+	err = p->open(hu);
+	if (err)
+		return err;
+
+	/*
+	 * Protocol might register hdev by itself.
+	 * In that case, there is no need to register it here.
+	 */
+	if (!hu->proto->register_hci_dev)
+		return 0;
+
+	err = hci_uart_register_dev(hu);
+	if (err) {
+		p->close(hu);
+		return err;
+	}
+
+	return 0;
+}
+
+/* hci_uart_tty_ioctl()
+ *
+ *    Process IOCTL system call for the tty device.
+ *
+ * Arguments:
+ *
+ *    tty        pointer to tty instance data
+ *    file       pointer to open file object for device
+ *    cmd        IOCTL command code
+ *    arg        argument for IOCTL call (cmd dependent)
+ *
+ * Return Value:    Command dependent
+ */
+static int hci_uart_tty_ioctl(struct tty_struct *tty, struct file * file,
+					unsigned int cmd, unsigned long arg)
+{
+	struct hci_uart *hu = (void *)tty->disc_data;
+	int err = 0;
+
+	BT_DBG("");
+
+	/* Verify the status of the device */
+	if (!hu)
+		return -EBADF;
+
+	switch (cmd) {
+	case HCIUARTSETPROTO:
+		if (!test_and_set_bit(HCI_UART_PROTO_SET, &hu->flags)) {
+			err = hci_uart_set_proto(hu, arg);
+			if (err) {
+				clear_bit(HCI_UART_PROTO_SET, &hu->flags);
+				return err;
+			}
+			/* Keep file descriptor.*/
+			hu->fd = file;
+		} else
+			return -EBUSY;
+		break;
+
+	case HCIUARTGETPROTO:
+		if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
+			return hu->proto->id;
+		return -EUNATCH;
+
+	case HCIUARTGETDEVICE:
+		if (test_bit(HCI_UART_PROTO_SET, &hu->flags)) {
+			if (hu->hdev)
+				return hu->hdev->id;
+			else
+				return -ENOMSG;
+		}
+		return -EUNATCH;
+
+	case HCIUARTSETFLAGS:
+		if (test_bit(HCI_UART_PROTO_SET, &hu->flags))
+			return -EBUSY;
+		hu->hdev_flags = arg;
+		break;
+
+	case HCIUARTGETFLAGS:
+		return hu->hdev_flags;
+
+	default:
+		err = n_tty_ioctl_helper(tty, file, cmd, arg);
+		break;
+	};
+
+	return err;
+}
+
+/*
+ * We don't provide read/write/poll interface for user space.
+ */
+static ssize_t hci_uart_tty_read(struct tty_struct *tty, struct file *file,
+					unsigned char __user *buf, size_t nr)
+{
+	return 0;
+}
+
+static ssize_t hci_uart_tty_write(struct tty_struct *tty, struct file *file,
+					const unsigned char *data, size_t count)
+{
+	return 0;
+}
+
+static unsigned int hci_uart_tty_poll(struct tty_struct *tty,
+					struct file *filp, poll_table *wait)
+{
+	return 0;
+}
+
+static int __init cg2900_hci_uart_init(void)
+{
+	static struct tty_ldisc_ops hci_uart_ldisc;
+	int err;
+
+	BT_INFO("HCI UART driver ver %s", VERSION);
+
+	/* Register the tty discipline */
+
+	memset(&hci_uart_ldisc, 0, sizeof(hci_uart_ldisc));
+	hci_uart_ldisc.magic		= TTY_LDISC_MAGIC;
+	hci_uart_ldisc.name		= "n_cg2900_hci";
+	hci_uart_ldisc.open		= hci_uart_tty_open;
+	hci_uart_ldisc.close		= hci_uart_tty_close;
+	hci_uart_ldisc.read		= hci_uart_tty_read;
+	hci_uart_ldisc.write		= hci_uart_tty_write;
+	hci_uart_ldisc.ioctl		= hci_uart_tty_ioctl;
+	hci_uart_ldisc.poll		= hci_uart_tty_poll;
+	hci_uart_ldisc.receive_buf	= hci_uart_tty_receive;
+	hci_uart_ldisc.write_wakeup	= hci_uart_tty_wakeup;
+	hci_uart_ldisc.owner		= THIS_MODULE;
+
+	err = tty_register_ldisc(N_CG2900_HCI, &hci_uart_ldisc);
+	if (err) {
+		BT_ERR("HCI line discipline registration failed. (%d)", err);
+		return err;
+	}
+
+	return 0;
+}
+
+static void __exit cg2900_hci_uart_exit(void)
+{
+	int err;
+
+	/* Release tty registration of line discipline */
+	err = tty_unregister_ldisc(N_CG2900_HCI);
+	if (err)
+		BT_ERR("Can't unregister HCI line discipline (%d)", err);
+}
+
+module_init(cg2900_hci_uart_init);
+module_exit(cg2900_hci_uart_exit);
+
+module_param(reset, bool, 0644);
+MODULE_PARM_DESC(reset, "Send HCI reset command on initialization");
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl <par-gunnar.p.hjalmdahl@stericsson.com>");
+MODULE_DESCRIPTION("CG2900 Staging Bluetooth HCI UART driver ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_LDISC(N_CG2900_HCI);
diff --git a/drivers/staging/cg2900/bluetooth/hci_uart.h b/drivers/staging/cg2900/bluetooth/hci_uart.h
new file mode 100644
index 00000000000..23a69519ccd
--- /dev/null
+++ b/drivers/staging/cg2900/bluetooth/hci_uart.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * This file is a staging solution and shall be integrated into
+ * /drivers/bluetooth/hci_uart.h.
+ *
+ * Original hci_uart.h file:
+ *  Copyright (C) 2000-2001  Qualcomm Incorporated
+ *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com>
+ *  Copyright (C) 2004-2005  Marcel Holtmann <marcel@holtmann.org>
+ */
+
+/*
+ * Staging CG2900 Bluetooth HCI UART. Will be replaced by normal N_HCI when
+ * moved to normal driver folder.
+ */
+#ifndef N_CG2900_HCI
+#define N_CG2900_HCI		23
+#endif /* N_CG2900_HCI */
+
+/* Ioctls */
+#define HCIUARTSETPROTO		_IOW('U', 200, int)
+#define HCIUARTGETPROTO		_IOR('U', 201, int)
+#define HCIUARTGETDEVICE	_IOR('U', 202, int)
+#define HCIUARTSETFLAGS		_IOW('U', 203, int)
+#define HCIUARTGETFLAGS		_IOR('U', 204, int)
+
+/* UART protocols */
+#define HCI_UART_MAX_PROTO	7
+
+#define HCI_UART_H4	0
+#define HCI_UART_BCSP	1
+#define HCI_UART_3WIRE	2
+#define HCI_UART_H4DS	3
+#define HCI_UART_LL	4
+#define HCI_UART_ATH3K	5
+#define HCI_UART_STE	6
+
+#define HCI_UART_RAW_DEVICE	0
+
+/* UART break and flow control parameters */
+#define BREAK_ON		true
+#define BREAK_OFF		false
+#define FLOW_ON			true
+#define FLOW_OFF		false
+
+struct hci_uart;
+
+struct hci_uart_proto {
+	unsigned int id;
+	int (*open)(struct hci_uart *hu);
+	int (*close)(struct hci_uart *hu);
+	int (*flush)(struct hci_uart *hu);
+	int (*recv)(struct hci_uart *hu, void *data, int len);
+	int (*enqueue)(struct hci_uart *hu, struct sk_buff *skb);
+	struct sk_buff *(*dequeue)(struct hci_uart *hu);
+	bool register_hci_dev;
+	struct device *dev;
+};
+
+struct hci_uart {
+	struct tty_struct	*tty;
+	struct hci_dev		*hdev;
+	unsigned long		flags;
+	unsigned long		hdev_flags;
+
+	struct hci_uart_proto	*proto;
+	void			*priv;
+
+	struct sk_buff		*tx_skb;
+	unsigned long		tx_state;
+	spinlock_t		rx_lock;
+
+	struct file		*fd;
+};
+
+/* HCI_UART proto flag bits */
+#define HCI_UART_PROTO_SET	0
+
+/* TX states  */
+#define HCI_UART_SENDING	1
+#define HCI_UART_TX_WAKEUP	2
+
+int cg2900_hci_uart_register_proto(struct hci_uart_proto *p);
+int cg2900_hci_uart_unregister_proto(struct hci_uart_proto *p);
+int cg2900_hci_uart_tx_wakeup(struct hci_uart *hu);
+int cg2900_hci_uart_set_baudrate(struct hci_uart *hu, int baud);
+int cg2900_hci_uart_set_break(struct hci_uart *hu, bool break_on);
+int cg2900_hci_uart_tiocmget(struct hci_uart *hu);
+void cg2900_hci_uart_flush_buffer(struct hci_uart *hu);
+void cg2900_hci_uart_flow_ctrl(struct hci_uart *hu, bool flow_on);
+int cg2900_hci_uart_chars_in_buffer(struct hci_uart *hu);
+
+#define hci_uart_register_proto cg2900_hci_uart_register_proto
+#define hci_uart_unregister_proto cg2900_hci_uart_unregister_proto
+#define hci_uart_tx_wakeup cg2900_hci_uart_tx_wakeup
+#define hci_uart_set_baudrate cg2900_hci_uart_set_baudrate
+#define hci_uart_set_break cg2900_hci_uart_set_break
+#define hci_uart_tiocmget cg2900_hci_uart_tiocmget
+#define hci_uart_flush_buffer cg2900_hci_uart_flush_buffer
+#define hci_uart_flow_ctrl cg2900_hci_uart_flow_ctrl
+#define hci_uart_chars_in_buffer cg2900_hci_uart_chars_in_buffer
diff --git a/drivers/staging/cg2900/board-ux500-cg2900.c b/drivers/staging/cg2900/board-ux500-cg2900.c
new file mode 100644
index 00000000000..66d1f075ec4
--- /dev/null
+++ b/drivers/staging/cg2900/board-ux500-cg2900.c
@@ -0,0 +1,368 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Par-Gunnar Hjalmdahl <par-gunnar.p.hjalmdahl@stericsson.com>
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <asm/mach-types.h>
+#include <linux/gpio.h>
+#include <linux/gpio/nomadik.h>
+#include <linux/ioport.h>
+#include <linux/mfd/abx500/ab8500-gpio.h>
+#include <linux/platform_device.h>
+#include <mach/gpio.h>
+#include <mach/id.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <plat/pincfg.h>
+
+#include "board-mop500.h"
+#include "cg2900.h"
+#include "devices-cg2900.h"
+#include "pins-db5500.h"
+#include "pins-db8500.h"
+#include "pins.h"
+
+#define CG2900_BT_ENABLE_GPIO		170
+#define CG2900_GBF_ENA_RESET_GPIO	171
+#define WLAN_PMU_EN_GPIO		226
+#define WLAN_PMU_EN_GPIO_SNOWBALL	161
+#define WLAN_PMU_EN_GPIO_U9500		AB8500_PIN_GPIO11
+#define CG2900_UX500_BT_CTS_GPIO	0
+#define CG2900_U5500_BT_CTS_GPIO	168
+
+enum cg2900_gpio_pull_sleep ux500_cg2900_sleep_gpio[21] = {
+	CG2900_NO_PULL,		/* GPIO 0:  PTA_CONFX */
+	CG2900_PULL_DN,		/* GPIO 1:  PTA_STATUS */
+	CG2900_NO_PULL,		/* GPIO 2:  UART_CTSN */
+	CG2900_PULL_UP,		/* GPIO 3:  UART_RTSN */
+	CG2900_PULL_UP,		/* GPIO 4:  UART_TXD */
+	CG2900_NO_PULL,		/* GPIO 5:  UART_RXD */
+	CG2900_PULL_DN,		/* GPIO 6:  IOM_DOUT */
+	CG2900_NO_PULL,		/* GPIO 7:  IOM_FSC */
+	CG2900_NO_PULL,		/* GPIO 8:  IOM_CLK */
+	CG2900_NO_PULL,		/* GPIO 9:  IOM_DIN */
+	CG2900_PULL_DN,		/* GPIO 10: PWR_REQ */
+	CG2900_PULL_DN,		/* GPIO 11: HOST_WAKEUP */
+	CG2900_PULL_DN,		/* GPIO 12: IIS_DOUT */
+	CG2900_NO_PULL,		/* GPIO 13: IIS_WS */
+	CG2900_NO_PULL,		/* GPIO 14: IIS_CLK */
+	CG2900_NO_PULL,		/* GPIO 15: IIS_DIN */
+	CG2900_PULL_DN,		/* GPIO 16: PTA_FREQ */
+	CG2900_PULL_DN,		/* GPIO 17: PTA_RF_ACTIVE */
+	CG2900_NO_PULL,		/* GPIO 18: NotConnected (J6428) */
+	CG2900_NO_PULL,		/* GPIO 19: EXT_DUTY_CYCLE */
+	CG2900_NO_PULL,		/* GPIO 20: EXT_FRM_SYNCH */
+};
+
+static struct platform_device ux500_cg2900_device = {
+	.name = "cg2900",
+};
+
+static struct platform_device ux500_cg2900_chip_device = {
+	.name = "cg2900-chip",
+	.dev = {
+		.parent = &ux500_cg2900_device.dev,
+	},
+};
+
+static struct platform_device ux500_stlc2690_chip_device = {
+	.name = "stlc2690-chip",
+	.dev = {
+		.parent = &ux500_cg2900_device.dev,
+	},
+};
+
+static struct cg2900_platform_data ux500_cg2900_test_platform_data = {
+	.bus = HCI_VIRTUAL,
+	.gpio_sleep = ux500_cg2900_sleep_gpio,
+};
+
+static struct platform_device ux500_cg2900_test_device = {
+	.name = "cg2900-test",
+	.dev = {
+		.parent = &ux500_cg2900_device.dev,
+		.platform_data = &ux500_cg2900_test_platform_data,
+	},
+};
+
+static struct resource cg2900_uart_resources_pre_v60[] = {
+	{
+		.start = CG2900_GBF_ENA_RESET_GPIO,
+		.end = CG2900_GBF_ENA_RESET_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "gbf_ena_reset",
+	},
+	{
+		.start = CG2900_BT_ENABLE_GPIO,
+		.end = CG2900_BT_ENABLE_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "bt_enable",
+	},
+	{
+		.start = CG2900_UX500_BT_CTS_GPIO,
+		.end = CG2900_UX500_BT_CTS_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "cts_gpio",
+	},
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.end = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.flags = IORESOURCE_IRQ,
+		.name = "cts_irq",
+	},
+};
+
+static struct resource cg2900_uart_resources_u5500[] = {
+	{
+		.start = CG2900_U5500_BT_CTS_GPIO,
+		.end = CG2900_U5500_BT_CTS_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "cts_gpio",
+	},
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(CG2900_U5500_BT_CTS_GPIO),
+		.end = NOMADIK_GPIO_TO_IRQ(CG2900_U5500_BT_CTS_GPIO),
+		.flags = IORESOURCE_IRQ,
+		.name = "cts_irq",
+	},
+};
+
+static struct resource cg2900_uart_resources_u8500[] = {
+	{
+		.start = CG2900_GBF_ENA_RESET_GPIO,
+		.end = CG2900_GBF_ENA_RESET_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "gbf_ena_reset",
+	},
+	{
+		.start = WLAN_PMU_EN_GPIO,
+		.end = WLAN_PMU_EN_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "pmu_en",
+	},
+	{
+		.start = CG2900_UX500_BT_CTS_GPIO,
+		.end = CG2900_UX500_BT_CTS_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "cts_gpio",
+	},
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.end = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.flags = IORESOURCE_IRQ,
+		.name = "cts_irq",
+	},
+};
+
+static struct resource cg2900_uart_resources_snowball[] = {
+	{
+		.start = CG2900_GBF_ENA_RESET_GPIO,
+		.end = CG2900_GBF_ENA_RESET_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "gbf_ena_reset",
+	},
+	{
+		.start = WLAN_PMU_EN_GPIO_SNOWBALL,
+		.end = WLAN_PMU_EN_GPIO_SNOWBALL,
+		.flags = IORESOURCE_IO,
+		.name = "pmu_en",
+	},
+	{
+		.start = CG2900_UX500_BT_CTS_GPIO,
+		.end = CG2900_UX500_BT_CTS_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "cts_gpio",
+	},
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.end = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.flags = IORESOURCE_IRQ,
+		.name = "cts_irq",
+	},
+};
+
+
+static struct resource cg2900_uart_resources_u9500[] = {
+	{
+		.start = CG2900_GBF_ENA_RESET_GPIO,
+		.end = CG2900_GBF_ENA_RESET_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "gbf_ena_reset",
+	},
+	{
+		.start = WLAN_PMU_EN_GPIO_U9500,
+		.end = WLAN_PMU_EN_GPIO_U9500,
+		.flags = IORESOURCE_IO,
+		.name = "pmu_en",
+	},
+	{
+		.start = CG2900_UX500_BT_CTS_GPIO,
+		.end = CG2900_UX500_BT_CTS_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "cts_gpio",
+	},
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.end = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.flags = IORESOURCE_IRQ,
+		.name = "cts_irq",
+	},
+};
+
+static pin_cfg_t u5500_cg2900_uart_enabled[] = {
+	GPIO165_U3_RXD    | PIN_INPUT_PULLUP,
+	GPIO166_U3_TXD    | PIN_OUTPUT_HIGH,
+	GPIO167_U3_RTSn   | PIN_OUTPUT_HIGH,
+	GPIO168_U3_CTSn   | PIN_INPUT_PULLUP,
+};
+
+static pin_cfg_t u5500_cg2900_uart_disabled[] = {
+	GPIO165_GPIO   | PIN_INPUT_PULLUP,	/* RX pull down. */
+	GPIO166_GPIO   | PIN_OUTPUT_LOW,	/* TX low - break on. */
+	GPIO167_GPIO   | PIN_OUTPUT_HIGH,	/* RTS high-flow off. */
+	GPIO168_GPIO   | PIN_INPUT_PULLUP,	/* CTS pull up. */
+};
+
+static pin_cfg_t ux500_cg2900_uart_enabled[] = {
+	GPIO0_U0_CTSn   | PIN_INPUT_PULLUP,
+	GPIO1_U0_RTSn   | PIN_OUTPUT_HIGH,
+	GPIO2_U0_RXD    | PIN_INPUT_PULLUP,
+	GPIO3_U0_TXD    | PIN_OUTPUT_HIGH
+};
+
+static pin_cfg_t ux500_cg2900_uart_disabled[] = {
+	GPIO0_GPIO   | PIN_INPUT_PULLUP,	/* CTS pull up. */
+	GPIO1_GPIO   | PIN_OUTPUT_HIGH,		/* RTS high-flow off. */
+	GPIO2_GPIO   | PIN_INPUT_PULLUP,	/* RX pull down. */
+	GPIO3_GPIO   | PIN_OUTPUT_LOW		/* TX low - break on. */
+};
+
+static struct cg2900_platform_data ux500_cg2900_uart_platform_data = {
+	.bus = HCI_UART,
+	.gpio_sleep = ux500_cg2900_sleep_gpio,
+	.uart = {
+		.n_uart_gpios = 4,
+	},
+};
+
+static struct platform_device ux500_cg2900_uart_device = {
+	.name = "cg2900-uart",
+	.dev = {
+		.platform_data = &ux500_cg2900_uart_platform_data,
+		.parent = &ux500_cg2900_device.dev,
+	},
+};
+
+static bool mach_supported(void)
+{
+	if (machine_is_u8500() ||
+	    machine_is_u5500() ||
+	    machine_is_hrefv60() ||
+	    machine_is_u8520() ||
+	    machine_is_nomadik() ||
+	    machine_is_snowball() ||
+	    machine_is_u9540())
+		return true;
+
+	return false;
+}
+
+static int __init board_cg2900_init(void)
+{
+	int err;
+
+	if (!mach_supported())
+		return 0;
+
+	dcg2900_init_platdata(&ux500_cg2900_test_platform_data);
+	if (machine_is_u5500()) {
+		ux500_cg2900_uart_platform_data.uart.uart_enabled =
+						u5500_cg2900_uart_enabled;
+		ux500_cg2900_uart_platform_data.uart.uart_disabled =
+						u5500_cg2900_uart_disabled;
+	} else {
+		ux500_cg2900_uart_platform_data.uart.uart_enabled =
+						ux500_cg2900_uart_enabled;
+		ux500_cg2900_uart_platform_data.uart.uart_disabled =
+						ux500_cg2900_uart_disabled;
+		ux500_cg2900_uart_platform_data.regulator_id = "vdd";
+	}
+	dcg2900_init_platdata(&ux500_cg2900_uart_platform_data);
+
+	if (pins_for_u9500()) {
+		/* u9500 */
+		ux500_cg2900_uart_device.num_resources =
+				ARRAY_SIZE(cg2900_uart_resources_u9500);
+		ux500_cg2900_uart_device.resource =
+				cg2900_uart_resources_u9500;
+	} else if (cpu_is_u8500()) {
+		if (machine_is_hrefv60() || machine_is_u8520()) {
+			/* u8500 */
+			ux500_cg2900_uart_device.num_resources =
+				ARRAY_SIZE(cg2900_uart_resources_u8500);
+			ux500_cg2900_uart_device.resource =
+				cg2900_uart_resources_u8500;
+		} else if (machine_is_snowball()) {
+			/* snowball have diffrent PMU_EN gpio */
+			ux500_cg2900_uart_device.num_resources =
+				ARRAY_SIZE(cg2900_uart_resources_snowball);
+			ux500_cg2900_uart_device.resource =
+				cg2900_uart_resources_snowball;
+		} else {
+			/* u8500 pre v60*/
+			ux500_cg2900_uart_device.num_resources =
+				ARRAY_SIZE(cg2900_uart_resources_pre_v60);
+			ux500_cg2900_uart_device.resource =
+				cg2900_uart_resources_pre_v60;
+		}
+	} else if (cpu_is_u5500()) {
+		/* u5500 */
+		ux500_cg2900_uart_device.num_resources =
+				ARRAY_SIZE(cg2900_uart_resources_u5500);
+		ux500_cg2900_uart_device.resource =
+				cg2900_uart_resources_u5500;
+	}
+
+	err = platform_device_register(&ux500_cg2900_device);
+	if (err)
+		return err;
+	err = platform_device_register(&ux500_cg2900_uart_device);
+	if (err)
+		return err;
+	err = platform_device_register(&ux500_cg2900_test_device);
+	if (err)
+		return err;
+	err = platform_device_register(&ux500_cg2900_chip_device);
+	if (err)
+		return err;
+	err = platform_device_register(&ux500_stlc2690_chip_device);
+	if (err)
+		return err;
+
+	dev_info(&ux500_cg2900_device.dev, "CG2900 initialized\n");
+	return 0;
+}
+
+static void __exit board_cg2900_exit(void)
+{
+	if (!mach_supported())
+		return;
+
+	platform_device_unregister(&ux500_stlc2690_chip_device);
+	platform_device_unregister(&ux500_cg2900_chip_device);
+	platform_device_unregister(&ux500_cg2900_test_device);
+	platform_device_unregister(&ux500_cg2900_uart_device);
+	platform_device_unregister(&ux500_cg2900_device);
+
+	dev_info(&ux500_cg2900_device.dev, "CG2900 removed\n");
+}
+
+module_init(board_cg2900_init);
+module_exit(board_cg2900_exit);
diff --git a/drivers/staging/cg2900/clock-cg2900.c b/drivers/staging/cg2900/clock-cg2900.c
new file mode 100644
index 00000000000..54900c185fb
--- /dev/null
+++ b/drivers/staging/cg2900/clock-cg2900.c
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Hemant Gupta <hemant.gupta@stericsson.com>
+ * Author: Tomasz Hliwiak <tomasz.hliwiak@tieto.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <asm/mach-types.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/skbuff.h>
+#include "clock.h"
+#include "cg2900.h"
+
+static DEFINE_MUTEX(cg2900_clk_mutex);
+
+static struct cg2900_user_data *pf_data;
+static struct clk_lookup *cg2900_clk_lookup;
+
+/**
+ * cg2900_clk_enable() - Enables CG2900 Clock
+ *
+ * Enables CG2900 Clock by starting CG2900.
+ *
+ * Returns:
+ *  0 if success.
+ *  -EINVAL if stored pf_data is NULL.
+ *  Error codes generated by open.
+ */
+
+static int cg2900_clk_enable(struct clk *clk)
+{
+	int err = -EINVAL;
+	if (pf_data) {
+		pf_data->is_clk_user = true;
+		err = pf_data->open(pf_data);
+	}
+	return err;
+}
+
+/**
+ * cg2900_clk_disable() - Disables CG2900 Clock
+ *
+ * Disables CG2900 Clock by switching off CG2900.
+ */
+static void cg2900_clk_disable(struct clk *clk)
+{
+	if (pf_data)
+		pf_data->close(pf_data);
+}
+
+static struct clkops cg2900_clk_ops = {
+	.enable		= cg2900_clk_enable,
+	.disable	= cg2900_clk_disable,
+};
+
+static struct clk cg2900_clk = {
+	.name	= "cg2900_clk",
+	.ops	= &cg2900_clk_ops,
+	.mutex	= &cg2900_clk_mutex,
+};
+
+/**
+ * cg2900_read_cb() - Dummy callback for cg2900 core read.
+ *
+ * Function is required by cg2900_core->open().
+ */
+static void cg2900_read_cb(struct cg2900_user_data *user, struct sk_buff *skb)
+{
+	kfree_skb(skb);
+}
+
+/**
+ * cg2900_core_probe() - Initialize resources.
+ *
+ * Function initializes pf_data structure and also adds the cg2900
+ * clock source.
+ */
+static int __devinit cg2900_core_probe(struct platform_device *pdev)
+{
+	cg2900_clk_lookup = clkdev_alloc(&cg2900_clk, "sys_clk_out",
+			"cw1200_wlan");
+
+	if (!cg2900_clk_lookup)
+		return -ENOMEM;
+
+	clkdev_add(cg2900_clk_lookup);
+	pf_data = dev_get_platdata(&pdev->dev);
+	pf_data->dev = &pdev->dev;
+	pf_data->read_cb = cg2900_read_cb;
+
+	return 0;
+}
+
+/**
+ * cg2900_core_remove() - Clean resources.
+ *
+ * Function cleans pf_data structure and removes the clock source.
+ */
+static int __devexit cg2900_core_remove(struct platform_device *pdev)
+{
+	clkdev_drop(cg2900_clk_lookup);
+	pf_data = NULL;
+
+	return 0;
+}
+
+static struct platform_driver cg2900_core_ctrl_driver = {
+	.driver = {
+		.name      = "cg2900-core",
+		.owner     = THIS_MODULE,
+	},
+	.probe     = cg2900_core_probe,
+	.remove    = __devexit_p(cg2900_core_remove),
+};
+
+/**
+ * clock_cg2900_init() - Register Platform Data
+ *
+ * Registers the platform data.
+ */
+static int __init clock_cg2900_init(void)
+{
+	return platform_driver_register(&cg2900_core_ctrl_driver);
+}
+
+/**
+ * clock_cg2900_exit() - Unregister Platform Data
+ *
+ * Unregister Platform Data
+ */
+static void __exit clock_cg2900_exit(void)
+{
+	platform_driver_unregister(&cg2900_core_ctrl_driver);
+}
+
+module_init(clock_cg2900_init);
+module_exit(clock_cg2900_exit);
diff --git a/drivers/staging/cg2900/devices-cg2900-ux500.c b/drivers/staging/cg2900/devices-cg2900-ux500.c
new file mode 100644
index 00000000000..00cd4a543b2
--- /dev/null
+++ b/drivers/staging/cg2900/devices-cg2900-ux500.c
@@ -0,0 +1,206 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * Hemant Gupta (hemant.gupta@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Board specific device support for the Linux Bluetooth HCI H:4 Driver
+ * for ST-Ericsson connectivity controller.
+ */
+
+#include <asm/byteorder.h>
+#include <asm-generic/errno-base.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/ioport.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/mfd/abx500/ab5500.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <plat/pincfg.h>
+
+#include "devices-cg2900.h"
+
+void dcg2900_u8500_enable_chip(struct cg2900_chip_dev *dev)
+{
+	struct dcg2900_info *info = dev->b_data;
+
+	if (info->gbf_gpio == -1)
+		return;
+
+	/*
+	 * - SET PMU_EN to high
+	 * - Wait for 300usec
+	 * - Set PDB to high.
+	 */
+
+	if (info->pmuen_gpio != -1) {
+		/*
+		 * We must first set PMU_EN pin high and then wait 300 us before
+		 * setting the GBF_EN high.
+		 */
+		gpio_set_value(info->pmuen_gpio, 1);
+		udelay(CHIP_ENABLE_PMU_EN_TIMEOUT);
+	}
+
+	gpio_set_value(info->gbf_gpio, 1);
+}
+
+void dcg2900_u8500_disable_chip(struct cg2900_chip_dev *dev)
+{
+	struct dcg2900_info *info = dev->b_data;
+
+	if (info->gbf_gpio != -1)
+		gpio_set_value(info->gbf_gpio, 0);
+	if (info->pmuen_gpio != -1)
+		gpio_set_value(info->pmuen_gpio, 0);
+}
+
+int dcg2900_u8500_setup(struct cg2900_chip_dev *dev,
+					struct dcg2900_info *info)
+{
+	int err = 0;
+	struct resource *resource;
+	const char *gbf_name;
+	const char *bt_name = NULL;
+	const char *pmuen_name = NULL;
+
+	resource = platform_get_resource_byname(dev->pdev, IORESOURCE_IO,
+						"gbf_ena_reset");
+	if (!resource) {
+		dev_err(dev->dev, "GBF GPIO does not exist\n");
+		err = -EINVAL;
+		goto err_handling;
+	}
+
+	info->gbf_gpio = resource->start;
+	gbf_name = resource->name;
+
+	resource = platform_get_resource_byname(dev->pdev, IORESOURCE_IO,
+						"bt_enable");
+	/* BT Enable GPIO may not exist */
+	if (resource) {
+		info->bt_gpio = resource->start;
+		bt_name = resource->name;
+	}
+
+	resource = platform_get_resource_byname(dev->pdev, IORESOURCE_IO,
+						"pmu_en");
+	/* PMU_EN GPIO may not exist */
+	if (resource) {
+		info->pmuen_gpio = resource->start;
+		pmuen_name = resource->name;
+	}
+
+	/* Now setup the GPIOs */
+	err = gpio_request(info->gbf_gpio, gbf_name);
+	if (err < 0) {
+		dev_err(dev->dev, "gpio_request %s failed with err: %d\n",
+			gbf_name, err);
+		goto err_handling;
+	}
+
+	err = gpio_direction_output(info->gbf_gpio, 0);
+	if (err < 0) {
+		dev_err(dev->dev,
+			"gpio_direction_output %s failed with err: %d\n",
+			gbf_name, err);
+		goto err_handling_free_gpio_gbf;
+	}
+
+	if (!pmuen_name)
+		goto set_bt_gpio;
+
+	err = gpio_request(info->pmuen_gpio, pmuen_name);
+	if (err < 0) {
+		dev_err(dev->dev, "gpio_request %s failed with err: %d\n",
+			pmuen_name, err);
+		goto err_handling_free_gpio_gbf;
+	}
+
+	err = gpio_direction_output(info->pmuen_gpio, 0);
+	if (err < 0) {
+		dev_err(dev->dev,
+			"gpio_direction_output %s failed with err: %d\n",
+			pmuen_name, err);
+		goto err_handling_free_gpio_pmuen;
+	}
+
+set_bt_gpio:
+	if (!bt_name)
+		goto finished;
+
+	err = gpio_request(info->bt_gpio, bt_name);
+	if (err < 0) {
+		dev_err(dev->dev, "gpio_request %s failed with err: %d\n",
+			bt_name, err);
+		goto err_handling_free_gpio_pmuen;
+	}
+
+	err = gpio_direction_output(info->bt_gpio, 1);
+	if (err < 0) {
+		dev_err(dev->dev,
+			"gpio_direction_output %s failed with err: %d\n",
+			bt_name, err);
+		goto err_handling_free_gpio_bt;
+	}
+
+finished:
+
+	return 0;
+
+err_handling_free_gpio_bt:
+	gpio_free(info->bt_gpio);
+	info->bt_gpio = -1;
+err_handling_free_gpio_pmuen:
+	if (info->pmuen_gpio != -1) {
+		gpio_free(info->pmuen_gpio);
+		info->pmuen_gpio = -1;
+	}
+err_handling_free_gpio_gbf:
+	gpio_free(info->gbf_gpio);
+	info->gbf_gpio = -1;
+err_handling:
+
+	return err;
+}
+
+/* prcmu resout1 pin is used for CG2900 reset*/
+void dcg2900_u5500_enable_chip(struct cg2900_chip_dev *dev)
+{
+	struct dcg2900_info *info = dev->b_data;
+
+	clk_enable(info->lpoclk);
+	/*
+	 * - Set PDB to high.
+	 */
+	prcmu_resetout(1, 1);
+}
+
+void dcg2900_u5500_disable_chip(struct cg2900_chip_dev *dev)
+{
+	struct dcg2900_info *info = dev->b_data;
+
+	prcmu_resetout(1, 0);
+	clk_disable(info->lpoclk);
+}
+
+int dcg2900_u5500_setup(struct cg2900_chip_dev *dev,
+				struct dcg2900_info *info)
+{
+	info->lpoclk = clk_get(dev->dev, "lpoclk");
+	if (IS_ERR(info->lpoclk))
+		return PTR_ERR(info->lpoclk);
+
+	return 0;
+}
+
diff --git a/drivers/staging/cg2900/devices-cg2900.c b/drivers/staging/cg2900/devices-cg2900.c
new file mode 100644
index 00000000000..4c5d4a22a77
--- /dev/null
+++ b/drivers/staging/cg2900/devices-cg2900.c
@@ -0,0 +1,292 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * Hemant Gupta (hemant.gupta@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Board specific device support for the Linux Bluetooth HCI H:4 Driver
+ * for ST-Ericsson connectivity controller.
+ */
+
+#define NAME			"devices-cg2900"
+#define pr_fmt(fmt)		NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <asm-generic/errno-base.h>
+#include <asm/mach-types.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/ioport.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/skbuff.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <linux/regulator/consumer.h>
+#include <mach/id.h>
+#include <plat/pincfg.h>
+#include "cg2900.h"
+#include "devices-cg2900.h"
+
+#define BT_VS_POWER_SWITCH_OFF		0xFD40
+
+#define H4_HEADER_LENGTH		0x01
+#define BT_HEADER_LENGTH		0x03
+
+struct vs_power_sw_off_cmd {
+	__le16	op_code;
+	u8	len;
+	u8	gpio_0_7_pull_up;
+	u8	gpio_8_15_pull_up;
+	u8	gpio_16_20_pull_up;
+	u8	gpio_0_7_pull_down;
+	u8	gpio_8_15_pull_down;
+	u8	gpio_16_20_pull_down;
+} __packed;
+
+static struct sk_buff *dcg2900_get_power_switch_off_cmd
+				(struct cg2900_chip_dev *dev, u16 *op_code)
+{
+	struct sk_buff *skb;
+	struct vs_power_sw_off_cmd *cmd;
+	struct dcg2900_info *info;
+	int i;
+
+	/* If connected chip does not support the command return NULL */
+	if (!check_chip_revision_support(dev->chip.hci_revision))
+		return NULL;
+
+	dev_dbg(dev->dev, "Generating PowerSwitchOff command\n");
+
+	info = dev->b_data;
+
+	skb = alloc_skb(sizeof(*cmd) + H4_HEADER_LENGTH, GFP_KERNEL);
+	if (!skb) {
+		dev_err(dev->dev, "Could not allocate skb\n");
+		return NULL;
+	}
+
+	skb_reserve(skb, H4_HEADER_LENGTH);
+	cmd = (struct vs_power_sw_off_cmd *)skb_put(skb, sizeof(*cmd));
+	cmd->op_code = cpu_to_le16(BT_VS_POWER_SWITCH_OFF);
+	cmd->len = sizeof(*cmd) - BT_HEADER_LENGTH;
+	/*
+	 * Enter system specific GPIO settings here:
+	 * Section data[3-5] is GPIO pull-up selection
+	 * Section data[6-8] is GPIO pull-down selection
+	 * Each section is a bitfield where
+	 * - byte 0 bit 0 is GPIO 0
+	 * - byte 0 bit 1 is GPIO 1
+	 * - up to
+	 * - byte 2 bit 4 which is GPIO 20
+	 * where each bit means:
+	 * - 0: No pull-up / no pull-down
+	 * - 1: Pull-up / pull-down
+	 * All GPIOs are set as input.
+	 */
+	if (!info->sleep_gpio_set) {
+		struct cg2900_platform_data *pf_data;
+
+		pf_data = dev_get_platdata(dev->dev);
+		for (i = 0; i < 8; i++) {
+			if (pf_data->gpio_sleep[i] == CG2900_PULL_UP)
+				info->gpio_0_7_pull_up |= (1 << i);
+			else if (pf_data->gpio_sleep[i] == CG2900_PULL_DN)
+				info->gpio_0_7_pull_down |= (1 << i);
+		}
+		for (i = 8; i < 16; i++) {
+			if (pf_data->gpio_sleep[i] == CG2900_PULL_UP)
+				info->gpio_8_15_pull_up |= (1 << (i - 8));
+			else if (pf_data->gpio_sleep[i] == CG2900_PULL_DN)
+				info->gpio_8_15_pull_down |= (1 << (i - 8));
+		}
+		for (i = 16; i < 21; i++) {
+			if (pf_data->gpio_sleep[i] == CG2900_PULL_UP)
+				info->gpio_16_20_pull_up |= (1 << (i - 16));
+			else if (pf_data->gpio_sleep[i] == CG2900_PULL_DN)
+				info->gpio_16_20_pull_down |= (1 << (i - 16));
+		}
+		info->sleep_gpio_set = true;
+	}
+	cmd->gpio_0_7_pull_up = info->gpio_0_7_pull_up;
+	cmd->gpio_8_15_pull_up = info->gpio_8_15_pull_up;
+	cmd->gpio_16_20_pull_up = info->gpio_16_20_pull_up;
+	cmd->gpio_0_7_pull_down = info->gpio_0_7_pull_down;
+	cmd->gpio_8_15_pull_down = info->gpio_8_15_pull_down;
+	cmd->gpio_16_20_pull_down = info->gpio_16_20_pull_down;
+
+
+	if (op_code)
+		*op_code = BT_VS_POWER_SWITCH_OFF;
+
+	return skb;
+}
+
+static int dcg2900_init(struct cg2900_chip_dev *dev)
+{
+	int err = 0;
+	struct dcg2900_info *info;
+	struct cg2900_platform_data *pdata = dev_get_platdata(dev->dev);
+
+	/* First retrieve and save the resources */
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev->dev, "Could not allocate dcg2900_info\n");
+		return -ENOMEM;
+	}
+
+	info->gbf_gpio = -1;
+	info->pmuen_gpio = -1;
+	info->bt_gpio = -1;
+
+	if (!dev->pdev->num_resources) {
+		dev_dbg(dev->dev, "No resources available\n");
+		goto finished;
+	}
+
+	if (cpu_is_u5500())
+		err = dcg2900_u5500_setup(dev, info);
+	else
+		err = dcg2900_u8500_setup(dev, info);
+
+	if (err)
+		goto err_handling;
+
+	/*
+	 * Enable the power on snowball
+	 */
+	if (machine_is_snowball()) {
+		/* Take the regulator */
+		if (pdata->regulator_id) {
+			info->regulator_wlan = regulator_get(dev->dev,
+					pdata->regulator_id);
+			if (IS_ERR(info->regulator_wlan)) {
+				err = PTR_ERR(info->regulator_wlan);
+				dev_warn(dev->dev,
+					"%s: Failed to get regulator '%s'\n",
+					__func__, pdata->regulator_id);
+				info->regulator_wlan = NULL;
+				goto err_handling_free_gpios;
+			}
+			/* Enable it also */
+			err = regulator_enable(info->regulator_wlan);
+			if (err < 0) {
+				dev_warn(dev->dev, "%s: regulator_enable failed\n",
+						__func__);
+				goto err_handling_put_reg;
+			}
+		} else {
+			dev_warn(dev->dev, "%s: no regulator defined for snowball.\n",
+					__func__);
+		}
+	}
+
+finished:
+	dev->b_data = info;
+	return 0;
+err_handling_put_reg:
+	regulator_put(info->regulator_wlan);
+err_handling_free_gpios:
+	if (info->bt_gpio != -1)
+		gpio_free(info->bt_gpio);
+	if (info->pmuen_gpio != -1)
+		gpio_free(info->pmuen_gpio);
+	if (info->gbf_gpio != -1)
+		gpio_free(info->gbf_gpio);
+ 
+err_handling:
+	kfree(info);
+	return err;
+}
+
+static void dcg2900_exit(struct cg2900_chip_dev *dev)
+{
+	struct dcg2900_info *info = dev->b_data;
+
+	if (machine_is_snowball()) {
+		/* Turn off power if we have any */
+		if (info->regulator_wlan) {
+			regulator_disable(info->regulator_wlan);
+			regulator_put(info->regulator_wlan);
+		}
+	}
+
+	if (cpu_is_u5500())
+		dcg2900_u5500_disable_chip(dev);
+	else
+		dcg2900_u8500_disable_chip(dev);
+
+	if (info->bt_gpio != -1)
+		gpio_free(info->bt_gpio);
+	if (info->pmuen_gpio != -1)
+		gpio_free(info->pmuen_gpio);
+	if (info->gbf_gpio != -1)
+		gpio_free(info->gbf_gpio);
+	kfree(info);
+	dev->b_data = NULL;
+}
+
+static int dcg2900_disable_uart(struct cg2900_chip_dev *dev)
+{
+	int err;
+	struct cg2900_platform_data *pdata = dev_get_platdata(dev->dev);
+
+	/*
+	 * Without this delay we get interrupt on CTS immediately
+	 * due to some turbulences on this line.
+	 */
+	mdelay(4);
+
+	/* Disable UART functions. */
+	err = nmk_config_pins(pdata->uart.uart_disabled,
+			      pdata->uart.n_uart_gpios);
+	if (err)
+		goto error;
+
+	return 0;
+
+error:
+	(void)nmk_config_pins(pdata->uart.uart_enabled,
+			      pdata->uart.n_uart_gpios);
+	dev_err(dev->dev, "Cannot set interrupt (%d)\n", err);
+	return err;
+}
+
+static int dcg2900_enable_uart(struct cg2900_chip_dev *dev)
+{
+	int err;
+	struct cg2900_platform_data *pdata = dev_get_platdata(dev->dev);
+
+	/* Restore UART settings. */
+	err = nmk_config_pins(pdata->uart.uart_enabled,
+			      pdata->uart.n_uart_gpios);
+	if (err)
+		dev_err(dev->dev, "Unable to enable UART (%d)\n", err);
+
+	return err;
+}
+
+void dcg2900_init_platdata(struct cg2900_platform_data *data)
+{
+	data->init = dcg2900_init;
+	data->exit = dcg2900_exit;
+
+	if (cpu_is_u5500()) {
+		data->enable_chip = dcg2900_u5500_enable_chip;
+		data->disable_chip = dcg2900_u5500_disable_chip;
+	} else {
+		data->enable_chip = dcg2900_u8500_enable_chip;
+		data->disable_chip = dcg2900_u8500_disable_chip;
+	}
+	data->get_power_switch_off_cmd = dcg2900_get_power_switch_off_cmd;
+
+	data->uart.enable_uart = dcg2900_enable_uart;
+	data->uart.disable_uart = dcg2900_disable_uart;
+}
diff --git a/drivers/staging/cg2900/devices-cg2900.h b/drivers/staging/cg2900/devices-cg2900.h
new file mode 100644
index 00000000000..0a1db162279
--- /dev/null
+++ b/drivers/staging/cg2900/devices-cg2900.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Par-Gunnar Hjalmdahl <par-gunnar.p.hjalmdahl@stericsson.com>
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __DEVICES_CG2900_H
+#define __DEVICES_CG2900_H
+
+#include "cg2900.h"
+#include "mfd/cg2900_chip.h"
+#include <linux/clk.h>
+
+#define CHIP_ENABLE_PDB_LOW_TIMEOUT     100 /* ms */
+#define CHIP_ENABLE_PMU_EN_TIMEOUT	300 /* us */
+
+struct dcg2900_info {
+	int	gbf_gpio;
+	int	pmuen_gpio;
+	int	bt_gpio;
+	bool	sleep_gpio_set;
+	u8	gpio_0_7_pull_up;
+	u8	gpio_8_15_pull_up;
+	u8	gpio_16_20_pull_up;
+	u8	gpio_0_7_pull_down;
+	u8	gpio_8_15_pull_down;
+	u8	gpio_16_20_pull_down;
+	struct clk	*lpoclk;
+	struct regulator        *regulator_wlan;
+};
+
+extern void dcg2900_u8500_enable_chip(struct cg2900_chip_dev *dev);
+extern void dcg2900_u8500_disable_chip(struct cg2900_chip_dev *dev);
+extern int dcg2900_u8500_setup(struct cg2900_chip_dev *dev,
+				struct dcg2900_info *info);
+extern void dcg2900_u5500_enable_chip(struct cg2900_chip_dev *dev);
+extern void dcg2900_u5500_disable_chip(struct cg2900_chip_dev *dev);
+extern int dcg2900_u5500_setup(struct cg2900_chip_dev *dev,
+				struct dcg2900_info *info);
+
+/**
+ * enum cg2900_gpio_pull_sleep - GPIO pull setting in sleep.
+ * @CG2900_NO_PULL:	Normal input in sleep (no pull up or down).
+ * @CG2900_PULL_UP:	Pull up in sleep.
+ * @CG2900_PULL_DN:	Pull down in sleep.
+ */
+enum cg2900_gpio_pull_sleep {
+	CG2900_NO_PULL,
+	CG2900_PULL_UP,
+	CG2900_PULL_DN
+};
+
+/**
+ * dcg2900_init_platdata() - Initializes platform data with callback functions.
+ * @data:	Platform data.
+ */
+extern void dcg2900_init_platdata(struct cg2900_platform_data *data);
+
+#endif /* __DEVICES_CG2900_H */
diff --git a/drivers/staging/cg2900/include/cg2900.h b/drivers/staging/cg2900/include/cg2900.h
new file mode 100644
index 00000000000..e1c1860765a
--- /dev/null
+++ b/drivers/staging/cg2900/include/cg2900.h
@@ -0,0 +1,306 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 connectivity
+ * controller.
+ */
+
+#ifndef _CG2900_H_
+#define _CG2900_H_
+
+#include <linux/types.h>
+
+/* Perform reset. No parameters used */
+#define CG2900_CHAR_DEV_IOCTL_RESET		_IOW('U', 210, int)
+/* Check for reset */
+#define CG2900_CHAR_DEV_IOCTL_CHECK4RESET	_IOR('U', 212, int)
+/* Retrieve revision info */
+#define CG2900_CHAR_DEV_IOCTL_GET_REVISION	_IOR('U', 213, \
+						     struct cg2900_rev_data)
+
+#define CG2900_CHAR_DEV_IOCTL_EVENT_IDLE	0
+#define CG2900_CHAR_DEV_IOCTL_EVENT_RESET	1
+
+/* Specific chip version data */
+#define STLC2690_REV			0x0600
+#define CG2900_PG1_REV			0x0101
+#define CG2900_PG2_REV			0x0200
+#define CG2900_PG1_SPECIAL_REV	0x0700
+#define CG2905_PG1_1_REV		0x1805
+#define CG2910_PG1_REV			0x1004
+#define CG2910_PG2_REV			0x1008
+
+/**
+ * struct cg2900_rev_data - Contains revision data for the local controller.
+ * @revision:		Revision of the controller, e.g. to indicate that it is
+ *			a CG2900 controller.
+ * @sub_version:	Subversion of the controller, e.g. to indicate a certain
+ *			tape-out of the controller.
+ *
+ * The values to match retrieved values to each controller may be retrieved from
+ * the manufacturer.
+ */
+struct cg2900_rev_data {
+	u16 revision;
+	u16 sub_version;
+};
+
+#ifdef __KERNEL__
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/skbuff.h>
+
+/* Temporary solution while in staging directory */
+#include "cg2900_hci.h"
+
+/**
+ * struct cg2900_chip_rev_info - Chip info structure.
+ * @manufacturer:	Chip manufacturer.
+ * @hci_version:	Bluetooth version supported over HCI.
+ * @hci_revision:	Chip revision, i.e. which chip is this.
+ * @lmp_pal_version:	Bluetooth version supported over air.
+ * @hci_sub_version:	Chip sub-version, i.e. which tape-out is this.
+ *
+ * Note that these values match the Bluetooth Assigned Numbers,
+ * see http://www.bluetooth.org/
+ */
+struct cg2900_chip_rev_info {
+	u16	manufacturer;
+	u8	hci_version;
+	u16	hci_revision;
+	u8	lmp_pal_version;
+	u16	hci_sub_version;
+};
+
+struct cg2900_chip_dev;
+
+/**
+ * struct cg2900_id_callbacks - Chip handler identification callbacks.
+ * @check_chip_support:	Called when chip is connected. If chip is supported by
+ *			driver, return true and fill in @callbacks in @dev.
+ *
+ * Note that the callback may be NULL. It must always be NULL checked before
+ * calling.
+ */
+struct cg2900_id_callbacks {
+	bool (*check_chip_support)(struct cg2900_chip_dev *dev);
+};
+
+/**
+ * struct cg2900_chip_callbacks - Callback functions registered by chip handler.
+ * @data_from_chip:	Called when data shall be transmitted to user.
+ * @chip_removed:	Called when chip is removed.
+ *
+ * Note that some callbacks may be NULL. They must always be NULL checked before
+ * calling.
+ */
+struct cg2900_chip_callbacks {
+	void (*data_from_chip)(struct cg2900_chip_dev *dev,
+			       struct sk_buff *skb);
+	void (*chip_removed)(struct cg2900_chip_dev *dev);
+};
+
+/**
+ * struct cg2900_trans_callbacks - Callback functions registered by transport.
+ * @open:		CG2900 Core needs a transport.
+ * @close:		CG2900 Core does not need a transport.
+ * @write:		CG2900 Core transmits to the chip.
+ * @set_chip_power:	CG2900 Core enables or disables the chip.
+ * @chip_startup_finished:	CG2900 Chip startup finished notification.
+ *
+ * Note that some callbacks may be NULL. They must always be NULL checked before
+ * calling.
+ */
+struct cg2900_trans_callbacks {
+	int (*open)(struct cg2900_chip_dev *dev);
+	int (*close)(struct cg2900_chip_dev *dev);
+	int (*write)(struct cg2900_chip_dev *dev, struct sk_buff *skb);
+	void (*set_chip_power)(struct cg2900_chip_dev *dev, bool chip_on);
+	void (*chip_startup_finished)(struct cg2900_chip_dev *dev);
+	void (*set_baud_rate)(struct cg2900_chip_dev *dev, bool low_baud);
+};
+
+/**
+ * struct cg2900_chip_dev - Chip handler info structure.
+ * @dev:	Device associated with this chip.
+ * @pdev:	Platform device associated with this chip.
+ * @chip:	Chip info such as manufacturer.
+ * @c_cb:	Callback structure for the chip handler.
+ * @t_cb:	Callback structure for the transport.
+ * @c_data:	Arbitrary data set by chip handler.
+ * @t_data:	Arbitrary data set by transport.
+ * @b_data:	Arbitrary data set by board handler.
+ * @prv_data:	Arbitrary data set by CG2900 Core.
+ */
+struct cg2900_chip_dev {
+	struct device			*dev;
+	struct platform_device		*pdev;
+	struct cg2900_chip_rev_info	chip;
+	struct cg2900_chip_callbacks	c_cb;
+	struct cg2900_trans_callbacks	t_cb;
+	void				*c_data;
+	void				*t_data;
+	void				*b_data;
+	void				*prv_data;
+};
+
+/**
+ * struct cg2900_platform_data - Contains platform data for CG2900.
+ * @init:		Callback called upon system start.
+ * @exit:		Callback called upon system shutdown.
+ * @enable_chip:	Callback called for enabling CG2900 chip.
+ * @disable_chip:	Callback called for disabling CG2900 chip.
+ * @get_power_switch_off_cmd:	Callback called to retrieve
+ *				HCI VS_Power_Switch_Off command (command
+ *				HCI requires platform specific GPIO data).
+ * @regulator_id:	Id of the regulator that powers on the chip
+ * @bus:		Transport used, see @include/net/bluetooth/hci.h.
+ * @gpio_sleep:		Array of GPIO sleep settings.
+ * @enable_uart:	Callback called when switching from UART GPIO to
+ *			UART HW.
+ * @disable_uart:	Callback called when switching from UART HW to
+ *			UART GPIO.
+ * @n_uart_gpios:	Number of UART GPIOs.
+ * @uart_enabled:	Array of size @n_uart_gpios with GPIO setting for
+ *			enabling UART HW (switching from GPIO mode).
+ * @uart_disabled:	Array of size @n_uart_gpios with GPIO setting for
+ *			disabling UART HW (switching to GPIO mode).
+ * @uart:		Platform data structure for UART transport.
+ *
+ * Any callback may be NULL if not needed.
+ */
+struct cg2900_platform_data {
+	int (*init)(struct cg2900_chip_dev *dev);
+	void (*exit)(struct cg2900_chip_dev *dev);
+	void (*enable_chip)(struct cg2900_chip_dev *dev);
+	void (*disable_chip)(struct cg2900_chip_dev *dev);
+	struct sk_buff* (*get_power_switch_off_cmd)(struct cg2900_chip_dev *dev,
+						    u16 *op_code);
+
+	char *regulator_id;
+	__u8 bus;
+	enum cg2900_gpio_pull_sleep *gpio_sleep;
+
+	struct {
+		int (*enable_uart)(struct cg2900_chip_dev *dev);
+		int (*disable_uart)(struct cg2900_chip_dev *dev);
+		int n_uart_gpios;
+		unsigned long *uart_enabled;
+		unsigned long *uart_disabled;
+	} uart;
+};
+
+/**
+ * struct cg2900_user_data - Contains platform data for CG2900 user.
+ * @dev:		Current device. Set by CG2900 user upon probe.
+ * @opened:		True if channel is opened.
+ * @user_data:		Data set and used by CG2900 user.
+ * @private_data:	Data set and used by CG2900 driver.
+ * @h4_channel:		H4 channel. Set by CG2900 driver.
+ * @is_audio:		True if this channel is an audio channel. Set by CG2900
+ *			driver.
+ * @is_clk_user:	whether enabling CG29XX was started external entity
+ *			for eg. WLAN.
+ * @chip_independent:	True if this channel does not require chip to be
+ *			powered. Set by CG2900 driver.
+ * @bt_bus:		Transport used, see @include/net/bluetooth/hci.h.
+ * @char_dev_name:	Name to be used for character device.
+ * @channel_data:	Input data specific to current device.
+ * @open:		Open device channel. Set by CG2900 driver.
+ * @close:		Close device channel. Set by CG2900 driver.
+ * @reset:		Reset connectivity controller. Set by CG2900 driver.
+ * @alloc_skb:		Alloc sk_buffer. Set by CG2900 driver.
+ * @write:		Write to device channel. Set by CG2900 driver.
+ * @get_local_revision:	Get revision data of conncected chip. Set by CG2900
+ *			driver.
+ * @read_cb:		Callback function called when data is received on the
+ *			device channel. Set by CG2900 user. Mandatory.
+ * @reset_cb:		Callback function called when the connectivity
+ *			controller has been reset. Set by CG2900 user.
+ *
+ * Any callback may be NULL if not needed.
+ */
+struct cg2900_user_data {
+	struct device *dev;
+	bool opened;
+
+	void *user_data;
+	void *private_data;
+
+	int	h4_channel;
+	bool	is_audio;
+	bool	is_clk_user;
+	bool	chip_independent;
+
+	union {
+		__u8 bt_bus;
+		char *char_dev_name;
+	} channel_data;
+
+	int (*open)(struct cg2900_user_data *user_data);
+	void (*close)(struct cg2900_user_data *user_data);
+	int (*reset)(struct cg2900_user_data *user_data);
+	struct sk_buff * (*alloc_skb)(unsigned int size, gfp_t priority);
+	int (*write)(struct cg2900_user_data *user_data, struct sk_buff *skb);
+	bool (*get_local_revision)(struct cg2900_user_data *user_data,
+				   struct cg2900_rev_data *rev_data);
+
+	void (*read_cb)(struct cg2900_user_data *user_data,
+			struct sk_buff *skb);
+	void (*reset_cb)(struct cg2900_user_data *user_data);
+};
+
+static inline void *cg2900_get_usr(struct cg2900_user_data *dev)
+{
+	if (dev)
+		return dev->user_data;
+	return NULL;
+}
+
+static inline void cg2900_set_usr(struct cg2900_user_data *dev, void *data)
+{
+	if (dev)
+		dev->user_data = data;
+}
+
+static inline void *cg2900_get_prv(struct cg2900_user_data *dev)
+{
+	if (dev)
+		return dev->private_data;
+	return NULL;
+}
+
+static inline void cg2900_set_prv(struct cg2900_user_data *dev, void *data)
+{
+	if (dev)
+		dev->private_data = data;
+}
+
+static inline bool check_chip_revision_support(u16 hci_revision)
+{
+	if (hci_revision != CG2900_PG1_SPECIAL_REV &&
+			hci_revision != CG2900_PG1_REV &&
+			hci_revision != CG2900_PG2_REV &&
+			hci_revision != CG2905_PG1_1_REV &&
+			hci_revision != CG2910_PG1_REV &&
+			hci_revision != CG2910_PG2_REV)
+		return false;
+
+	return true;
+}
+
+extern int cg2900_register_chip_driver(struct cg2900_id_callbacks *cb);
+extern void cg2900_deregister_chip_driver(struct cg2900_id_callbacks *cb);
+extern int cg2900_register_trans_driver(struct cg2900_chip_dev *dev);
+extern int cg2900_deregister_trans_driver(struct cg2900_chip_dev *dev);
+extern unsigned long cg2900_get_sleep_timeout(bool check_sleep);
+
+#endif /* __KERNEL__ */
+#endif /* _CG2900_H_ */
diff --git a/drivers/staging/cg2900/include/cg2900_audio.h b/drivers/staging/cg2900/include/cg2900_audio.h
new file mode 100644
index 00000000000..ff0f053fa53
--- /dev/null
+++ b/drivers/staging/cg2900/include/cg2900_audio.h
@@ -0,0 +1,473 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth Audio Driver for ST-Ericsson controller.
+ */
+
+#ifndef _CG2900_AUDIO_H_
+#define _CG2900_AUDIO_H_
+
+#include <linux/types.h>
+
+/*
+ * Digital Audio Interface configuration types
+ */
+
+/** CG2900_A2DP_MAX_AVDTP_HDR_LEN - Max length of a AVDTP header.
+ * Max length of a AVDTP header for an A2DP packet.
+ */
+#define CG2900_A2DP_MAX_AVDTP_HDR_LEN		25
+
+/*
+ * Op codes used when writing commands to the audio interface from user space
+ * using the char device.
+ */
+#define CG2900_OPCODE_SET_DAI_CONF		0x01
+#define CG2900_OPCODE_GET_DAI_CONF		0x02
+#define CG2900_OPCODE_CONFIGURE_ENDPOINT	0x03
+#define CG2900_OPCODE_START_STREAM		0x04
+#define CG2900_OPCODE_STOP_STREAM		0x05
+
+/**
+ * enum cg2900_dai_dir - Contains the DAI port directions alternatives.
+ * @DAI_DIR_B_RX_A_TX: Port B as Rx and port A as Tx.
+ * @DAI_DIR_B_TX_A_RX: Port B as Tx and port A as Rx.
+ */
+enum cg2900_dai_dir {
+	DAI_DIR_B_RX_A_TX = 0x00,
+	DAI_DIR_B_TX_A_RX = 0x01
+};
+
+/**
+ * enum cg2900_dai_mode - DAI mode alternatives.
+ * @DAI_MODE_SLAVE: Slave.
+ * @DAI_MODE_MASTER: Master.
+ */
+enum cg2900_dai_mode {
+	DAI_MODE_SLAVE = 0x00,
+	DAI_MODE_MASTER = 0x01
+};
+
+/**
+ * enum cg2900_dai_stream_ratio - Voice stream ratio alternatives.
+ * @STREAM_RATIO_FM16_VOICE16:	FM 16kHz, Voice 16kHz.
+ * @STREAM_RATIO_FM16_VOICE8:	FM 16kHz, Voice 8kHz.
+ * @STREAM_RATIO_FM48_VOICE16:	FM 48kHz, Voice 16Khz.
+ * @STREAM_RATIO_FM48_VOICE8:	FM 48kHz, Voice 8kHz.
+ *
+ * Contains the alternatives for the voice stream ratio between the Audio stream
+ * sample rate and the Voice stream sample rate.
+ */
+enum cg2900_dai_stream_ratio {
+	STREAM_RATIO_FM16_VOICE16 = 0x01,
+	STREAM_RATIO_FM16_VOICE8 = 0x02,
+	STREAM_RATIO_FM48_VOICE16 = 0x03,
+	STREAM_RATIO_FM48_VOICE8 = 0x06
+};
+
+/**
+ * enum cg2900_dai_fs_duration - Frame sync duration alternatives.
+ * @SYNC_DURATION_8: 8 frames sync duration.
+ * @SYNC_DURATION_16: 16 frames sync duration.
+ * @SYNC_DURATION_24: 24 frames sync duration.
+ * @SYNC_DURATION_32: 32 frames sync duration.
+ * @SYNC_DURATION_48: 48 frames sync duration.
+ * @SYNC_DURATION_50: 50 frames sync duration.
+ * @SYNC_DURATION_64: 64 frames sync duration.
+ * @SYNC_DURATION_75: 75 frames sync duration.
+ * @SYNC_DURATION_96: 96 frames sync duration.
+ * @SYNC_DURATION_125: 125 frames sync duration.
+ * @SYNC_DURATION_128: 128 frames sync duration.
+ * @SYNC_DURATION_150: 150 frames sync duration.
+ * @SYNC_DURATION_192: 192 frames sync duration.
+ * @SYNC_DURATION_250: 250 frames sync duration.
+ * @SYNC_DURATION_256: 256 frames sync duration.
+ * @SYNC_DURATION_300: 300 frames sync duration.
+ * @SYNC_DURATION_384: 384 frames sync duration.
+ * @SYNC_DURATION_500: 500 frames sync duration.
+ * @SYNC_DURATION_512: 512 frames sync duration.
+ * @SYNC_DURATION_600: 600 frames sync duration.
+ * @SYNC_DURATION_768: 768 frames sync duration.
+ *
+ * This parameter sets the PCM frame sync duration. It is calculated as the
+ * ratio between the bit clock and the frame rate. For example, if the bit
+ * clock is 512 kHz and the stream sample rate is 8 kHz, the PCM frame sync
+ * duration is 512 / 8 = 64.
+ */
+enum cg2900_dai_fs_duration {
+	SYNC_DURATION_8   = 0,
+	SYNC_DURATION_16  = 1,
+	SYNC_DURATION_24  = 2,
+	SYNC_DURATION_32  = 3,
+	SYNC_DURATION_48  = 4,
+	SYNC_DURATION_50  = 5,
+	SYNC_DURATION_64  = 6,
+	SYNC_DURATION_75  = 7,
+	SYNC_DURATION_96  = 8,
+	SYNC_DURATION_125 = 9,
+	SYNC_DURATION_128 = 10,
+	SYNC_DURATION_150 = 11,
+	SYNC_DURATION_192 = 12,
+	SYNC_DURATION_250 = 13,
+	SYNC_DURATION_256 = 14,
+	SYNC_DURATION_300 = 15,
+	SYNC_DURATION_384 = 16,
+	SYNC_DURATION_500 = 17,
+	SYNC_DURATION_512 = 18,
+	SYNC_DURATION_600 = 19,
+	SYNC_DURATION_768 = 20
+};
+
+/**
+ * enum cg2900_dai_bit_clk - Bit Clock alternatives.
+ * @BIT_CLK_128:	128 Kbits clock.
+ * @BIT_CLK_256:	256 Kbits clock.
+ * @BIT_CLK_512:	512 Kbits clock.
+ * @BIT_CLK_768:	768 Kbits clock.
+ * @BIT_CLK_1024:	1024 Kbits clock.
+ * @BIT_CLK_1411_76:	1411.76 Kbits clock.
+ * @BIT_CLK_1536:	1536 Kbits clock.
+ * @BIT_CLK_2000:	2000 Kbits clock.
+ * @BIT_CLK_2048:	2048 Kbits clock.
+ * @BIT_CLK_2400:	2400 Kbits clock.
+ * @BIT_CLK_2823_52:	2823.52 Kbits clock.
+ * @BIT_CLK_3072:	3072 Kbits clock.
+ *
+ *  This parameter sets the bit clock speed. This is the clocking of the actual
+ *  data. A usual parameter for eSCO voice is 512 kHz.
+ */
+enum cg2900_dai_bit_clk {
+	BIT_CLK_128 = 0x00,
+	BIT_CLK_256 = 0x01,
+	BIT_CLK_512 = 0x02,
+	BIT_CLK_768 = 0x03,
+	BIT_CLK_1024 = 0x04,
+	BIT_CLK_1411_76 = 0x05,
+	BIT_CLK_1536 = 0x06,
+	BIT_CLK_2000 = 0x07,
+	BIT_CLK_2048 = 0x08,
+	BIT_CLK_2400 = 0x09,
+	BIT_CLK_2823_52 = 0x0A,
+	BIT_CLK_3072 = 0x0B
+};
+
+/**
+ * enum cg2900_dai_sample_rate - Sample rates alternatives.
+ * @SAMPLE_RATE_8:	8 kHz sample rate.
+ * @SAMPLE_RATE_16:	16 kHz sample rate.
+ * @SAMPLE_RATE_44_1:	44.1 kHz sample rate.
+ * @SAMPLE_RATE_48:	48 kHz sample rate.
+ */
+enum cg2900_dai_sample_rate {
+	SAMPLE_RATE_8    = 0,
+	SAMPLE_RATE_16   = 1,
+	SAMPLE_RATE_44_1 = 2,
+	SAMPLE_RATE_48   = 3
+};
+
+/**
+ * enum cg2900_dai_port_protocol - Port protocol alternatives.
+ * @PORT_PROTOCOL_PCM: Protocol PCM.
+ * @PORT_PROTOCOL_I2S: Protocol I2S.
+ */
+enum cg2900_dai_port_protocol {
+	PORT_PROTOCOL_PCM = 0x00,
+	PORT_PROTOCOL_I2S = 0x01
+};
+
+/**
+ * enum cg2900_dai_channel_sel - The channel selection alternatives.
+ * @CHANNEL_SELECTION_RIGHT: Right channel used.
+ * @CHANNEL_SELECTION_LEFT: Left channel used.
+ * @CHANNEL_SELECTION_BOTH: Both channels used.
+ */
+enum cg2900_dai_channel_sel {
+	CHANNEL_SELECTION_RIGHT = 0x00,
+	CHANNEL_SELECTION_LEFT = 0x01,
+	CHANNEL_SELECTION_BOTH = 0x02
+};
+
+/**
+ * struct cg2900_dai_conf_i2s_pcm - Port configuration structure.
+ * @mode:		Operational mode of the port configured.
+ * @i2s_channel_sel:	I2S channels used. Only valid if used in I2S mode.
+ * @slot_0_used:	True if SCO slot 0 is used.
+ * @slot_1_used:	True if SCO slot 1 is used.
+ * @slot_2_used:	True if SCO slot 2 is used.
+ * @slot_3_used:	True if SCO slot 3 is used.
+ * @slot_0_dir:		Direction of slot 0.
+ * @slot_1_dir:		Direction of slot 1.
+ * @slot_2_dir:		Direction of slot 2.
+ * @slot_3_dir:		Direction of slot 3.
+ * @slot_0_start:	Slot 0 start (relative to the PCM frame sync).
+ * @slot_1_start:	Slot 1 start (relative to the PCM frame sync)
+ * @slot_2_start:	Slot 2 start (relative to the PCM frame sync)
+ * @slot_3_start:	Slot 3 start (relative to the PCM frame sync)
+ * @ratio:		Voice stream ratio between the Audio stream sample rate
+ *			and the Voice stream sample rate.
+ * @protocol:		Protocol used on port.
+ * @duration:		Frame sync duration.
+ * @clk:		Bit clock.
+ * @sample_rate:	Sample rate.
+ */
+struct cg2900_dai_conf_i2s_pcm {
+	enum cg2900_dai_mode mode;
+	enum cg2900_dai_channel_sel i2s_channel_sel;
+	bool slot_0_used;
+	bool slot_1_used;
+	bool slot_2_used;
+	bool slot_3_used;
+	enum cg2900_dai_dir slot_0_dir;
+	enum cg2900_dai_dir slot_1_dir;
+	enum cg2900_dai_dir slot_2_dir;
+	enum cg2900_dai_dir slot_3_dir;
+	__u8 slot_0_start;
+	__u8 slot_1_start;
+	__u8 slot_2_start;
+	__u8 slot_3_start;
+	enum cg2900_dai_stream_ratio ratio;
+	enum cg2900_dai_port_protocol protocol;
+	enum cg2900_dai_fs_duration duration;
+	enum cg2900_dai_bit_clk clk;
+	enum cg2900_dai_sample_rate sample_rate;
+};
+
+/**
+ * enum cg2900_dai_half_period - Half period duration alternatives.
+ * @HALF_PER_DUR_8:	8 Bits.
+ * @HALF_PER_DUR_16:	16 Bits.
+ * @HALF_PER_DUR_24:	24 Bits.
+ * @HALF_PER_DUR_25:	25 Bits.
+ * @HALF_PER_DUR_32:	32 Bits.
+ * @HALF_PER_DUR_48:	48 Bits.
+ * @HALF_PER_DUR_64:	64 Bits.
+ * @HALF_PER_DUR_75:	75 Bits.
+ * @HALF_PER_DUR_96:	96 Bits.
+ * @HALF_PER_DUR_128:	128 Bits.
+ * @HALF_PER_DUR_150:	150 Bits.
+ * @HALF_PER_DUR_192:	192 Bits.
+ *
+ * This parameter sets the number of bits contained in each I2S half period,
+ * i.e. each channel slot. A usual value is 16 bits.
+ */
+enum cg2900_dai_half_period {
+	HALF_PER_DUR_8 = 0x00,
+	HALF_PER_DUR_16 = 0x01,
+	HALF_PER_DUR_24 = 0x02,
+	HALF_PER_DUR_25 = 0x03,
+	HALF_PER_DUR_32 = 0x04,
+	HALF_PER_DUR_48 = 0x05,
+	HALF_PER_DUR_64 = 0x06,
+	HALF_PER_DUR_75 = 0x07,
+	HALF_PER_DUR_96 = 0x08,
+	HALF_PER_DUR_128 = 0x09,
+	HALF_PER_DUR_150 = 0x0A,
+	HALF_PER_DUR_192 = 0x0B
+};
+
+/**
+ * enum cg2900_dai_word_width - Word width alternatives.
+ * @WORD_WIDTH_16: 16 bits words.
+ * @WORD_WIDTH_32: 32 bits words.
+ */
+enum cg2900_dai_word_width {
+	WORD_WIDTH_16 = 0x00,
+	WORD_WIDTH_32 = 0x01
+};
+
+/**
+ * struct cg2900_dai_conf_i2s - Port configuration struct for I2S.
+ * @mode:		Operational mode of the port.
+ * @half_period:	Half period duration.
+ * @channel_sel:	Channel selection.
+ * @sample_rate:	Sample rate.
+ * @word_width:		Word width.
+ */
+struct cg2900_dai_conf_i2s {
+	enum cg2900_dai_mode			mode;
+	enum cg2900_dai_half_period		half_period;
+	enum cg2900_dai_channel_sel		channel_sel;
+	enum cg2900_dai_sample_rate		sample_rate;
+	enum cg2900_dai_word_width		word_width;
+};
+
+/**
+ * union cg2900_dai_port_conf - DAI port configuration union.
+ * @i2s: The configuration struct for a port supporting only I2S.
+ * @i2s_pcm: The configuration struct for a port supporting both PCM and I2S.
+ */
+union cg2900_dai_port_conf {
+	struct cg2900_dai_conf_i2s i2s;
+	struct cg2900_dai_conf_i2s_pcm i2s_pcm;
+};
+
+/**
+ * enum cg2900_dai_ext_port_id - DAI external port id alternatives.
+ * @PORT_0_I2S: Port id is 0 and it supports only I2S.
+ * @PORT_1_I2S_PCM: Port id is 1 and it supports both I2S and PCM.
+ */
+enum cg2900_dai_ext_port_id {
+	PORT_0_I2S,
+	PORT_1_I2S_PCM
+};
+
+/**
+ * enum cg2900_audio_endpoint_id - Audio endpoint id alternatives.
+ * @ENDPOINT_PORT_0_I2S:	Internal audio endpoint of the external I2S
+ *				interface.
+ * @ENDPOINT_PORT_1_I2S_PCM:	Internal audio endpoint of the external I2S/PCM
+ *				interface.
+ * @ENDPOINT_SLIMBUS_VOICE:	Internal audio endpoint of the external Slimbus
+ *				voice interface. (Currently not supported)
+ * @ENDPOINT_SLIMBUS_AUDIO:	Internal audio endpoint of the external Slimbus
+ *				audio interface. (Currently not supported)
+ * @ENDPOINT_BT_SCO_INOUT:	Bluetooth SCO bidirectional.
+ * @ENDPOINT_BT_A2DP_SRC:	Bluetooth A2DP source.
+ * @ENDPOINT_BT_A2DP_SNK:	Bluetooth A2DP sink.
+ * @ENDPOINT_FM_RX:		FM receive.
+ * @ENDPOINT_FM_TX:		FM transmit.
+ * @ENDPOINT_ANALOG_OUT:	Analog out.
+ * @ENDPOINT_DSP_AUDIO_IN:	DSP audio in.
+ * @ENDPOINT_DSP_AUDIO_OUT:	DSP audio out.
+ * @ENDPOINT_DSP_VOICE_IN:	DSP voice in.
+ * @ENDPOINT_DSP_VOICE_OUT:	DSP voice out.
+ * @ENDPOINT_DSP_TONE_IN:	DSP tone in.
+ * @ENDPOINT_BURST_BUFFER_IN:	Burst buffer in.
+ * @ENDPOINT_BURST_BUFFER_OUT:	Burst buffer out.
+ * @ENDPOINT_MUSIC_DECODER:	Music decoder.
+ * @ENDPOINT_HCI_AUDIO_IN:	HCI audio in.
+ */
+enum cg2900_audio_endpoint_id {
+	ENDPOINT_PORT_0_I2S,
+	ENDPOINT_PORT_1_I2S_PCM,
+	ENDPOINT_SLIMBUS_VOICE,
+	ENDPOINT_SLIMBUS_AUDIO,
+	ENDPOINT_BT_SCO_INOUT,
+	ENDPOINT_BT_A2DP_SRC,
+	ENDPOINT_BT_A2DP_SNK,
+	ENDPOINT_FM_RX,
+	ENDPOINT_FM_TX,
+	ENDPOINT_ANALOG_OUT,
+	ENDPOINT_DSP_AUDIO_IN,
+	ENDPOINT_DSP_AUDIO_OUT,
+	ENDPOINT_DSP_VOICE_IN,
+	ENDPOINT_DSP_VOICE_OUT,
+	ENDPOINT_DSP_TONE_IN,
+	ENDPOINT_BURST_BUFFER_IN,
+	ENDPOINT_BURST_BUFFER_OUT,
+	ENDPOINT_MUSIC_DECODER,
+	ENDPOINT_HCI_AUDIO_IN
+};
+
+/**
+ * struct cg2900_dai_config - Configuration struct for Digital Audio Interface.
+ * @port: The port id to configure. Acts as a discriminator for @conf parameter
+ *	  which is a union.
+ * @conf: The configuration union that contains the parameters for the port.
+ */
+struct cg2900_dai_config {
+	enum cg2900_dai_ext_port_id	port;
+	union cg2900_dai_port_conf	conf;
+};
+
+/*
+ * Endpoint configuration types
+ */
+
+/**
+ * enum cg2900_endpoint_sample_rate - Audio endpoint configuration sample rate alternatives.
+ *
+ * This enum defines the same values as @cg2900_dai_sample_rate, but
+ * is kept to preserve the API.
+ *
+ * @ENDPOINT_SAMPLE_RATE_8_KHZ: 8 kHz sample rate.
+ * @ENDPOINT_SAMPLE_RATE_16_KHZ: 16 kHz sample rate.
+ * @ENDPOINT_SAMPLE_RATE_44_1_KHZ: 44.1 kHz sample rate.
+ * @ENDPOINT_SAMPLE_RATE_48_KHZ: 48 kHz sample rate.
+ */
+enum cg2900_endpoint_sample_rate {
+	ENDPOINT_SAMPLE_RATE_8_KHZ	= SAMPLE_RATE_8,
+	ENDPOINT_SAMPLE_RATE_16_KHZ	= SAMPLE_RATE_16,
+	ENDPOINT_SAMPLE_RATE_44_1_KHZ	= SAMPLE_RATE_44_1,
+	ENDPOINT_SAMPLE_RATE_48_KHZ	= SAMPLE_RATE_48
+};
+
+
+/**
+ * struct cg2900_endpoint_config_a2dp_src - A2DP source audio endpoint configurations.
+ * @sample_rate: Sample rate.
+ * @channel_count: Number of channels.
+ */
+struct cg2900_endpoint_config_a2dp_src {
+	enum cg2900_endpoint_sample_rate	sample_rate;
+	unsigned int				channel_count;
+};
+
+/**
+ * struct cg2900_endpoint_config_fm - Configuration parameters for an FM endpoint.
+ * @sample_rate: The sample rate alternatives for the FM audio endpoints.
+ */
+struct cg2900_endpoint_config_fm {
+	enum cg2900_endpoint_sample_rate	sample_rate;
+};
+
+
+/**
+ * struct cg2900_endpoint_config_sco_in_out - SCO audio endpoint configuration structure.
+ * @sample_rate: Sample rate, valid values are
+ *		 * ENDPOINT_SAMPLE_RATE_8_KHZ
+ *		 * ENDPOINT_SAMPLE_RATE_16_KHZ.
+ */
+struct cg2900_endpoint_config_sco_in_out {
+	enum cg2900_endpoint_sample_rate	sample_rate;
+};
+
+/**
+ * union cg2900_endpoint_config - Different audio endpoint configurations.
+ * @sco:	SCO audio endpoint configuration structure.
+ * @a2dp_src:	A2DP source audio endpoint configuration structure.
+ * @fm:		FM audio endpoint configuration structure.
+ */
+union cg2900_endpoint_config_union {
+	struct cg2900_endpoint_config_sco_in_out	sco;
+	struct cg2900_endpoint_config_a2dp_src		a2dp_src;
+	struct cg2900_endpoint_config_fm		fm;
+};
+
+/**
+ * struct cg2900_endpoint_config - Audio endpoint configuration.
+ * @endpoint_id:	Identifies the audio endpoint. Works as a discriminator
+ *			for the config union.
+ * @config:		Union holding the configuration parameters for
+ *			the endpoint.
+ */
+struct cg2900_endpoint_config {
+	enum cg2900_audio_endpoint_id		endpoint_id;
+	union cg2900_endpoint_config_union	config;
+};
+
+#ifdef __KERNEL__
+#include <linux/device.h>
+
+int cg2900_audio_get_devices(struct device *devices[], __u8 size);
+int cg2900_audio_open(unsigned int *session, struct device *parent);
+int cg2900_audio_close(unsigned int *session);
+int cg2900_audio_set_dai_config(unsigned int session,
+				struct cg2900_dai_config *config);
+int cg2900_audio_get_dai_config(unsigned int session,
+				struct cg2900_dai_config *config);
+int cg2900_audio_config_endpoint(unsigned int session,
+				 struct cg2900_endpoint_config *config);
+int cg2900_audio_start_stream(unsigned int session,
+			      enum cg2900_audio_endpoint_id ep_1,
+			      enum cg2900_audio_endpoint_id ep_2,
+			      unsigned int *stream_handle);
+int cg2900_audio_stop_stream(unsigned int session,
+			     unsigned int stream_handle);
+
+#endif /* __KERNEL__ */
+#endif /* _CG2900_AUDIO_H_ */
diff --git a/drivers/staging/cg2900/include/cg2900_hci.h b/drivers/staging/cg2900/include/cg2900_hci.h
new file mode 100644
index 00000000000..e094a9dddbd
--- /dev/null
+++ b/drivers/staging/cg2900/include/cg2900_hci.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * This file is a staging solution and shall be integrated into
+ * /include/net/bluetooth/hci.h.
+ */
+
+#ifndef __CG2900_HCI_H
+#define __CG2900_HCI_H
+
+#define HCI_EV_HW_ERROR			0x10
+struct hci_ev_hw_error {
+	__u8	hw_code;
+} __packed;
+
+#endif /* __CG2900_HCI_H */
diff --git a/drivers/staging/cg2900/mfd/Makefile b/drivers/staging/cg2900/mfd/Makefile
new file mode 100644
index 00000000000..bdbd8de90ee
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/Makefile
@@ -0,0 +1,18 @@
+#
+# Makefile for ST-Ericsson CG2900 connectivity combo controller
+#
+
+ccflags-y :=					\
+	-Idrivers/staging/cg2900/include
+
+obj-$(CONFIG_CG2900)	+= cg2900_core.o cg2900_lib.o
+export-objs			:= cg2900_core.o cg2900_lib.o
+
+obj-$(CONFIG_CG2900)	+= cg2900_char_devices.o
+
+obj-$(CONFIG_CG2900_TEST)	+= cg2900_test.o
+
+obj-$(CONFIG_CG2900_CHIP)	+= cg2900_chip.o
+obj-$(CONFIG_STLC2690_CHIP)	+= stlc2690_chip.o
+
+obj-$(CONFIG_CG2900_AUDIO)	+= cg2900_audio.o
diff --git a/drivers/staging/cg2900/mfd/cg2900_audio.c b/drivers/staging/cg2900/mfd/cg2900_audio.c
new file mode 100644
index 00000000000..2b21cc30962
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_audio.c
@@ -0,0 +1,3530 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * Hemant Gupta (hemant.gupta@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth Audio Driver for ST-Ericsson CG2900 controller.
+ */
+#define NAME					"cg2900_audio"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/types.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+#include "cg2900_audio.h"
+#include "cg2900_chip.h"
+
+#define MAX_NBR_OF_USERS			10
+#define FIRST_USER				1
+
+/*
+ * This is a default ACL handle. It is necessary to provide to the chip, but
+ * does not actually do anything.
+ */
+#define DEFAULT_ACL_HANDLE			0x0001
+
+/* Use a timeout of 5 seconds when waiting for a command response */
+#define RESP_TIMEOUT				5000
+
+#define BT_DEV					(info->dev_bt)
+#define FM_DEV					(info->dev_fm)
+
+/* FM Set control conversion macros for CG2905/10 */
+#define CG2910_FM_CMD_SET_CTRL_48000_HEX	0x12C0
+#define CG2910_FM_CMD_SET_CTRL_44100_HEX	0x113A
+
+/* Bluetooth error codes */
+#define HCI_BT_ERROR_NO_ERROR			0x00
+
+/* Used to select proper API, ignoring subrevisions etc */
+enum chip_revision {
+	CG2900_CHIP_REV_PG1,
+	CG2900_CHIP_REV_PG2,
+	CG2905_CHIP_REV_PG1_1,
+	CG2910_CHIP_REV_PG1,
+	CG2910_CHIP_REV_PG2
+};
+
+/**
+ * enum chip_resp_state - State when communicating with the CG2900 controller.
+ * @IDLE:		No outstanding packets to the controller.
+ * @WAITING:		Packet has been sent to the controller. Waiting for
+ *			response.
+ * @RESP_RECEIVED:	Response from controller has been received but not yet
+ *			handled.
+ */
+enum chip_resp_state {
+	IDLE,
+	WAITING,
+	RESP_RECEIVED
+};
+
+/**
+ * enum main_state - Main state for the CG2900 Audio driver.
+ * @OPENED:	Audio driver has registered to CG2900 Core.
+ * @CLOSED:	Audio driver is not registered to CG2900 Core.
+ * @RESET:	A reset of CG2900 Core has occurred and no user has re-opened
+ *		the audio driver.
+ */
+enum main_state {
+	OPENED,
+	CLOSED,
+	RESET
+};
+
+/**
+ * struct endpoint_list - List for storing endpoint configuration nodes.
+ * @ep_list:		Pointer to first node in list.
+ * @management_mutex:	Mutex for handling access to list.
+ */
+struct endpoint_list {
+	struct list_head	ep_list;
+	struct mutex		management_mutex;
+};
+
+/**
+ * struct endpoint_config_node - Node for storing endpoint configuration.
+ * @list:		list_head struct.
+ * @endpoint_id:	Endpoint ID.
+ * @config:		Stored configuration for this endpoint.
+ */
+struct endpoint_config_node {
+	struct list_head			list;
+	enum cg2900_audio_endpoint_id		endpoint_id;
+	union cg2900_endpoint_config_union	config;
+};
+
+/**
+ * struct audio_info - Main CG2900 Audio driver info structure.
+ * @list:			list_head struct.
+ * @state:			Current state of the CG2900 Audio driver.
+ * @revision:			Chip revision, used to select API.
+ * @misc_dev:			The misc device created by this driver.
+ * @misc_registered:		True if misc device is registered.
+ * @parent:			Parent device.
+ * @dev_bt:			Device registered by this driver for the BT
+ *				audio channel.
+ * @dev_fm:			Device registered by this driver for the FM
+ *				audio channel.
+ * @filp:			Current char device file pointer.
+ * @management_mutex:		Mutex for handling access to CG2900 Audio driver
+ *				management.
+ * @bt_mutex:			Mutex for handling access to BT audio channel.
+ * @fm_mutex:			Mutex for handling access to FM audio channel.
+ * @nbr_of_users_active:	Number of sessions open in the CG2900 Audio
+ *				driver.
+ * @i2s_config:			DAI I2S configuration.
+ * @i2s_pcm_config:		DAI PCM_I2S configuration.
+ * @i2s_config_known:		@true if @i2s_config has been set,
+ *				@false otherwise.
+ * @i2s_pcm_config_known:	@true if @i2s_pcm_config has been set,
+ *				@false otherwise.
+ * @endpoints:			List containing the endpoint configurations.
+ * @stream_ids:			Bitmask for in-use stream ids (only used with
+ *				CG2900 PG2 onwards chip API).
+ */
+struct audio_info {
+	struct list_head		list;
+	enum main_state			state;
+	enum chip_revision		revision;
+	struct miscdevice		misc_dev;
+	bool				misc_registered;
+	struct device			*parent;
+	struct device			*dev_bt;
+	struct device			*dev_fm;
+	struct file			*filp;
+	struct mutex			management_mutex;
+	struct mutex			bt_mutex;
+	struct mutex			fm_mutex;
+	int				nbr_of_users_active;
+	struct cg2900_dai_conf_i2s	i2s_config;
+	struct cg2900_dai_conf_i2s_pcm	i2s_pcm_config;
+	bool				i2s_config_known;
+	bool				i2s_pcm_config_known;
+	struct endpoint_list		endpoints;
+	u8				stream_ids[16];
+};
+
+/**
+ * struct audio_user - CG2900 audio user info structure.
+ * @session:	Stored session for the char device.
+ * @resp_state:	State for controller communications.
+ * @info:	CG2900 audio info structure.
+ */
+struct audio_user {
+	int			session;
+	enum chip_resp_state	resp_state;
+	struct audio_info	*info;
+};
+
+/**
+ * struct audio_cb_info - Callback info structure registered in @user_data.
+ * @user:	Audio user currently awaiting data on the channel.
+ * @wq:		Wait queue for this channel.
+ * @skb_queue:	Sk buffer queue.
+ */
+struct audio_cb_info {
+	struct audio_user	*user;
+	wait_queue_head_t	wq;
+	struct sk_buff_head	skb_queue;
+};
+
+/**
+ * struct char_dev_info - CG2900 character device info structure.
+ * @session:		Stored session for the char device.
+ * @stored_data:	Data returned when executing last command, if any.
+ * @stored_data_len:	Length of @stored_data in bytes.
+ * @management_mutex:	Mutex for handling access to char dev management.
+ * @rw_mutex:		Mutex for handling access to char dev writes and reads.
+ * @info:		CG2900 audio info struct.
+ * @rx_queue:		Data queue.
+ */
+struct char_dev_info {
+	int			session;
+	u8			*stored_data;
+	int			stored_data_len;
+	struct mutex		management_mutex;
+	struct mutex		rw_mutex;
+	struct audio_info	*info;
+	struct sk_buff_head	rx_queue;
+};
+
+/*
+ * cg2900_audio_devices - List of active CG2900 audio devices.
+ */
+LIST_HEAD(cg2900_audio_devices);
+
+/*
+ * cg2900_audio_sessions - Pointers to currently opened sessions (maps
+ *			   session ID to user info).
+ */
+static struct audio_user *cg2900_audio_sessions[MAX_NBR_OF_USERS];
+
+/*
+ *	Internal conversion functions
+ *
+ *	Since the CG2900 APIs uses several different ways to encode the
+ *	same parameter in different cases, we have to use translator
+ *	functions.
+ */
+
+/**
+ * session_config_sample_rate() - Convert sample rate to format used in VS_Set_SessionConfiguration.
+ * @rate: Sample rate in API encoding.
+ */
+static u8 session_config_sample_rate(enum cg2900_endpoint_sample_rate rate)
+{
+	static const u8 codes[] = {
+		[ENDPOINT_SAMPLE_RATE_8_KHZ]    = CG2900_BT_SESSION_RATE_8K,
+		[ENDPOINT_SAMPLE_RATE_16_KHZ]   = CG2900_BT_SESSION_RATE_16K,
+		[ENDPOINT_SAMPLE_RATE_44_1_KHZ] = CG2900_BT_SESSION_RATE_44_1K,
+		[ENDPOINT_SAMPLE_RATE_48_KHZ]   = CG2900_BT_SESSION_RATE_48K
+	};
+
+	return codes[rate];
+}
+
+/**
+ * mc_i2s_sample_rate() - Convert sample rate to format used in VS_Port_Config for I2S.
+ * @rate: Sample rate in API encoding.
+ */
+static u8 mc_i2s_sample_rate(enum cg2900_dai_sample_rate rate)
+{
+	static const u8 codes[] = {
+		[SAMPLE_RATE_8]    = CG2900_MC_I2S_SAMPLE_RATE_8,
+		[SAMPLE_RATE_16]   = CG2900_MC_I2S_SAMPLE_RATE_16,
+		[SAMPLE_RATE_44_1] = CG2900_MC_I2S_SAMPLE_RATE_44_1,
+		[SAMPLE_RATE_48]   = CG2900_MC_I2S_SAMPLE_RATE_48
+	};
+
+	return codes[rate];
+}
+
+/**
+ * mc_pcm_sample_rate() - Convert sample rate to format used in VS_Port_Config for PCM/I2S.
+ * @rate: Sample rate in API encoding.
+ */
+static u8 mc_pcm_sample_rate(enum cg2900_dai_sample_rate rate)
+{
+	static const u8 codes[] = {
+		[SAMPLE_RATE_8]    = CG2900_MC_PCM_SAMPLE_RATE_8,
+		[SAMPLE_RATE_16]   = CG2900_MC_PCM_SAMPLE_RATE_16,
+		[SAMPLE_RATE_44_1] = CG2900_MC_PCM_SAMPLE_RATE_44_1,
+		[SAMPLE_RATE_48]   = CG2900_MC_PCM_SAMPLE_RATE_48
+	};
+
+	return codes[rate];
+}
+
+/**
+ * mc_i2s_channel_select() - Convert channel selection to format used in VS_Port_Config.
+ * @sel: Channel selection in API encoding.
+ */
+static u8 mc_i2s_channel_select(enum cg2900_dai_channel_sel sel)
+{
+	static const u8 codes[] = {
+		[CHANNEL_SELECTION_RIGHT] = CG2900_MC_I2S_RIGHT_CHANNEL,
+		[CHANNEL_SELECTION_LEFT]  = CG2900_MC_I2S_LEFT_CHANNEL,
+		[CHANNEL_SELECTION_BOTH]  = CG2900_MC_I2S_BOTH_CHANNELS
+	};
+	return codes[sel];
+}
+
+/**
+ * get_fs_duration() - Convert framesync-enumeration to real value.
+ * @duration: Framsync duration (API encoding).
+ *
+ * Returns:
+ * Duration in bits.
+ */
+static u16 get_fs_duration(enum cg2900_dai_fs_duration duration)
+{
+	static const u16 values[] = {
+		[SYNC_DURATION_8] = 8,
+		[SYNC_DURATION_16] = 16,
+		[SYNC_DURATION_24] = 24,
+		[SYNC_DURATION_32] = 32,
+		[SYNC_DURATION_48] = 48,
+		[SYNC_DURATION_50] = 50,
+		[SYNC_DURATION_64] = 64,
+		[SYNC_DURATION_75] = 75,
+		[SYNC_DURATION_96] = 96,
+		[SYNC_DURATION_125] = 125,
+		[SYNC_DURATION_128] = 128,
+		[SYNC_DURATION_150] = 150,
+		[SYNC_DURATION_192] = 192,
+		[SYNC_DURATION_250] = 250,
+		[SYNC_DURATION_256] = 256,
+		[SYNC_DURATION_300] = 300,
+		[SYNC_DURATION_384] = 384,
+		[SYNC_DURATION_500] = 500,
+		[SYNC_DURATION_512] = 512,
+		[SYNC_DURATION_600] = 600,
+		[SYNC_DURATION_768] = 768
+	};
+	return values[duration];
+}
+
+/**
+ * mc_i2s_role() - Convert master/slave encoding to format for I2S-ports.
+ * @mode: Master/slave in API encoding.
+ */
+static u8 mc_i2s_role(enum cg2900_dai_mode mode)
+{
+	if (mode == DAI_MODE_SLAVE)
+		return CG2900_I2S_MODE_SLAVE;
+	else
+		return CG2900_I2S_MODE_MASTER;
+}
+
+/**
+ * mc_pcm_role() - Convert master/slave encoding to format for PCM/I2S-port.
+ * @mode: Master/slave in API encoding.
+ */
+static u8 mc_pcm_role(enum cg2900_dai_mode mode)
+{
+	if (mode == DAI_MODE_SLAVE)
+		return CG2900_PCM_MODE_SLAVE;
+	else
+		return CG2900_PCM_MODE_MASTER;
+}
+
+/**
+ * fm_get_conversion() - Convert sample rate to convert up/down used in X_Set_Control FM commands.
+ * @srate: Sample rate.
+ */
+static u16 fm_get_conversion(struct audio_info *info,
+		enum cg2900_endpoint_sample_rate srate)
+{
+	/*
+	 * For CG2910, Set the external sample rate (host side)
+	 * of the digital output in units of [10Hz]
+	 */
+	if (info->revision == CG2910_CHIP_REV_PG1 ||
+			info->revision == CG2910_CHIP_REV_PG2 ||
+			info->revision == CG2905_CHIP_REV_PG1_1) {
+		if (srate > ENDPOINT_SAMPLE_RATE_44_1_KHZ)
+			return CG2910_FM_CMD_SET_CTRL_48000_HEX;
+		else
+			return CG2910_FM_CMD_SET_CTRL_44100_HEX;
+
+	} else {
+		if (srate >= ENDPOINT_SAMPLE_RATE_44_1_KHZ)
+			return CG2900_FM_CMD_SET_CTRL_CONV_UP;
+		else
+			return CG2900_FM_CMD_SET_CTRL_CONV_DOWN;
+	}
+}
+
+/**
+ * get_info() - Return info structure for this device.
+ * @dev:	Current device.
+ *
+ * This function returns the info structure on the following basis:
+ *	* If dev is NULL return first info struct found. If none is found return
+ *	  NULL.
+ *	* If dev is valid we will return corresponding info struct if dev is the
+ *	  parent of the info struct or if dev's parent is the parent of the info
+ *	  struct.
+ *	* If dev is valid and no info structure is found, a new info struct is
+ *	  allocated, initialized, and returned.
+ *
+ * Returns:
+ *   Pointer to info struct if there is no error.
+ *   NULL if NULL was supplied and no info structure exist.
+ *   ERR_PTR(-ENOMEM) if allocation fails.
+ */
+static struct audio_info *get_info(struct device *dev)
+{
+	struct list_head *cursor;
+	struct audio_info *tmp;
+	struct audio_info *info = NULL;
+
+	/*
+	 * Find the info structure for dev. If NULL is supplied for dev
+	 * just return first device found.
+	 */
+	list_for_each(cursor, &cg2900_audio_devices) {
+		tmp = list_entry(cursor, struct audio_info, list);
+		if (!dev || tmp->parent == dev->parent || tmp->parent == dev) {
+			info = tmp;
+			break;
+		}
+	}
+
+	if (!dev || info)
+		return info;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev, "Could not allocate info struct\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	info->parent = dev->parent;
+
+	/* Initiate the mutexes */
+	mutex_init(&(info->management_mutex));
+	mutex_init(&(info->bt_mutex));
+	mutex_init(&(info->fm_mutex));
+	mutex_init(&(info->endpoints.management_mutex));
+
+	/* Initiate the endpoint list */
+	INIT_LIST_HEAD(&info->endpoints.ep_list);
+
+	list_add_tail(&info->list, &cg2900_audio_devices);
+
+	dev_info(dev, "CG2900 device added\n");
+	return info;
+}
+
+/**
+ * flush_endpoint_list() - Deletes all stored endpoints in @list.
+ * @list:	List of endpoints.
+ */
+static void flush_endpoint_list(struct endpoint_list *list)
+{
+	struct list_head *cursor, *next;
+	struct endpoint_config_node *tmp;
+
+	mutex_lock(&list->management_mutex);
+	list_for_each_safe(cursor, next, &(list->ep_list)) {
+		tmp = list_entry(cursor, struct endpoint_config_node, list);
+		list_del(cursor);
+		kfree(tmp);
+	}
+	mutex_unlock(&list->management_mutex);
+}
+
+/**
+ * device_removed() - Remove device from list if there are no channels left.
+ * @info:	CG2900 audio info structure.
+ */
+static void device_removed(struct audio_info *info)
+{
+	struct list_head *cursor;
+	struct audio_info *tmp;
+
+	if (info->dev_bt || info->dev_fm)
+		/* There are still devices active */
+		return;
+
+	/* Find the stored info structure */
+	list_for_each(cursor, &cg2900_audio_devices) {
+		tmp = list_entry(cursor, struct audio_info, list);
+		if (tmp == info) {
+			list_del(cursor);
+			break;
+		}
+	}
+
+	flush_endpoint_list(&info->endpoints);
+
+	mutex_destroy(&info->management_mutex);
+	mutex_destroy(&info->bt_mutex);
+	mutex_destroy(&info->fm_mutex);
+	mutex_destroy(&info->endpoints.management_mutex);
+
+	kfree(info);
+	pr_info("CG2900 Audio device removed");
+}
+
+/**
+ * read_cb() - Handle data received from STE connectivity driver.
+ * @dev:	Device receiving data.
+ * @skb:	Buffer with data coming form device.
+ */
+static void read_cb(struct cg2900_user_data *dev, struct sk_buff *skb)
+{
+	struct audio_cb_info *cb_info;
+
+	cb_info = cg2900_get_usr(dev);
+
+	if (!(cb_info->user)) {
+		dev_err(dev->dev, "NULL supplied as cb_info->user\n");
+		return;
+	}
+
+	/* Mark that packet has been received */
+	dev_dbg(dev->dev, "New resp_state: RESP_RECEIVED");
+	cb_info->user->resp_state = RESP_RECEIVED;
+	skb_queue_tail(&cb_info->skb_queue, skb);
+	wake_up_all(&cb_info->wq);
+}
+
+/**
+ * reset_cb() - Reset callback function.
+ * @dev:	CG2900_Core device resetting.
+ */
+static void reset_cb(struct cg2900_user_data *dev)
+{
+	struct audio_info *info;
+
+	dev_dbg(dev->dev, "reset_cb\n");
+
+	info = dev_get_drvdata(dev->dev);
+	mutex_lock(&info->management_mutex);
+	info->nbr_of_users_active = 0;
+	info->state = RESET;
+	mutex_unlock(&info->management_mutex);
+}
+
+/**
+ * get_session_user() - Check that supplied session is within valid range.
+ * @session:	Session ID.
+ *
+ * Returns:
+ *   Audio_user if there is no error.
+ *   NULL for bad session ID.
+ */
+static struct audio_user *get_session_user(int session)
+{
+	struct audio_user *audio_user;
+
+	if (session < FIRST_USER || session >= MAX_NBR_OF_USERS) {
+		pr_err("Calling with invalid session %d", session);
+		return NULL;
+	}
+
+	audio_user = cg2900_audio_sessions[session];
+	if (!audio_user)
+		pr_err("Calling with non-opened session %d", session);
+	return audio_user;
+}
+
+/**
+ * del_endpoint_private() - Deletes an endpoint from @list.
+ * @endpoint_id:	Endpoint ID.
+ * @list:		List of endpoints.
+ *
+ * Deletes an endpoint from the supplied endpoint list.
+ * This function is not protected by any semaphore.
+ */
+static void del_endpoint_private(enum cg2900_audio_endpoint_id endpoint_id,
+				 struct endpoint_list *list)
+{
+	struct list_head *cursor, *next;
+	struct endpoint_config_node *tmp;
+
+	list_for_each_safe(cursor, next, &(list->ep_list)) {
+		tmp = list_entry(cursor, struct endpoint_config_node, list);
+		if (tmp->endpoint_id == endpoint_id) {
+			list_del(cursor);
+			kfree(tmp);
+		}
+	}
+}
+
+/**
+ * add_endpoint() - Add endpoint node to @list.
+ * @ep_config:	Endpoint configuration.
+ * @list:	List of endpoints.
+ *
+ * Add endpoint node to the supplied list and copies supplied config to node.
+ * If a node already exists for the supplied endpoint, the old node is removed
+ * and replaced by the new node.
+ */
+static void add_endpoint(struct cg2900_endpoint_config *ep_config,
+			 struct endpoint_list *list)
+{
+	struct endpoint_config_node *item;
+
+	item = kzalloc(sizeof(*item), GFP_KERNEL);
+	if (!item) {
+		pr_err("add_endpoint: Failed to alloc memory");
+		return;
+	}
+
+	/* Store values */
+	item->endpoint_id = ep_config->endpoint_id;
+	memcpy(&(item->config), &(ep_config->config), sizeof(item->config));
+
+	mutex_lock(&(list->management_mutex));
+
+	/*
+	 * Check if endpoint ID already exist in list.
+	 * If that is the case, remove it.
+	 */
+	if (!list_empty(&(list->ep_list)))
+		del_endpoint_private(ep_config->endpoint_id, list);
+
+	list_add_tail(&(item->list), &(list->ep_list));
+
+	mutex_unlock(&(list->management_mutex));
+}
+
+/**
+ * find_endpoint() - Finds endpoint identified by @endpoint_id in @list.
+ * @endpoint_id:	Endpoint ID.
+ * @list:		List of endpoints.
+ *
+ * Returns:
+ *   Endpoint configuration if there is no error.
+ *   NULL if no configuration can be found for @endpoint_id.
+ */
+static union cg2900_endpoint_config_union *
+find_endpoint(enum cg2900_audio_endpoint_id endpoint_id,
+	      struct endpoint_list *list)
+{
+	struct list_head *cursor, *next;
+	struct endpoint_config_node *tmp;
+	struct endpoint_config_node *ret_ep = NULL;
+
+	mutex_lock(&list->management_mutex);
+	list_for_each_safe(cursor, next, &(list->ep_list)) {
+		tmp = list_entry(cursor, struct endpoint_config_node, list);
+		if (tmp->endpoint_id == endpoint_id) {
+			ret_ep = tmp;
+			break;
+		}
+	}
+	mutex_unlock(&list->management_mutex);
+
+	if (ret_ep)
+		return &(ret_ep->config);
+	else
+		return NULL;
+}
+
+/**
+ * new_stream_id() - Allocate a new stream id.
+ * @info:	Current audio info struct.
+ *
+ * Returns:
+ *  0-127 new valid id.
+ *  -ENOMEM if no id is available.
+ */
+static s8 new_stream_id(struct audio_info *info)
+{
+	int r;
+
+	mutex_lock(&info->management_mutex);
+
+	r = find_first_zero_bit(info->stream_ids,
+				8 * sizeof(info->stream_ids));
+
+	if (r >= 8 * sizeof(info->stream_ids)) {
+		r = -ENOMEM;
+		goto out;
+	}
+
+	set_bit(r, (unsigned long int *)info->stream_ids);
+
+out:
+	mutex_unlock(&info->management_mutex);
+	return r;
+}
+
+/**
+ * release_stream_id() - Release a stream id.
+ * @info:	Current audio info struct.
+ * @id:		Stream to release.
+ */
+static void release_stream_id(struct audio_info *info, u8 id)
+{
+	if (id >= 8 * sizeof(info->stream_ids))
+		return;
+
+	mutex_lock(&info->management_mutex);
+	clear_bit(id, (unsigned long int *)info->stream_ids);
+	mutex_unlock(&info->management_mutex);
+}
+
+/**
+ * receive_fm_write_response() - Wait for and handle the response to an FM Legacy WriteCommand request.
+ * @audio_user:	Audio user to check for.
+ * @command:	FM command to wait for.
+ *
+ * This function first waits (up to 5 seconds) for a response to an FM
+ * write command and when one arrives, it checks that it is the one we
+ * are waiting for and also that no error has occurred.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -EIO for other errors.
+ */
+static int receive_fm_write_response(struct audio_user *audio_user,
+				     u16 command)
+{
+	int err = 0;
+	int res;
+	struct sk_buff *skb;
+	struct fm_leg_cmd_cmpl *pkt;
+	u16 rsp_cmd;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = audio_user->info;
+	pf_data = dev_get_platdata(info->dev_fm);
+	cb_info = cg2900_get_usr(pf_data);
+
+	/*
+	 * Wait for callback to receive command complete and then wake us up
+	 * again.
+	 */
+	res = wait_event_timeout(cb_info->wq,
+				 audio_user->resp_state == RESP_RECEIVED,
+				 msecs_to_jiffies(RESP_TIMEOUT));
+	if (!res) {
+		dev_err(FM_DEV, "Timeout while waiting for return packet\n");
+		return -ECOMM;
+	} else if (res < 0) {
+		dev_err(FM_DEV,
+			"Error %d occurred while waiting for return packet\n",
+			res);
+		return -ECOMM;
+	}
+
+	/* OK, now we should have received answer. Let's check it. */
+	skb = skb_dequeue_tail(&cb_info->skb_queue);
+	if (!skb) {
+		dev_err(FM_DEV, "No skb in queue when it should be there\n");
+		return -EIO;
+	}
+
+	pkt = (struct fm_leg_cmd_cmpl *)skb->data;
+
+	/* Check if we received the correct event */
+	if (pkt->opcode != CG2900_FM_GEN_ID_LEGACY) {
+		dev_err(FM_DEV,
+			"Received unknown FM packet. 0x%X %X %X %X %X\n",
+			skb->data[0], skb->data[1], skb->data[2],
+			skb->data[3], skb->data[4]);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+
+	/* FM Legacy Command complete event */
+	rsp_cmd = cg2900_get_fm_cmd_id(le16_to_cpu(pkt->response_head));
+
+	if (pkt->fm_function != CG2900_FM_CMD_PARAM_WRITECOMMAND ||
+	    rsp_cmd != command) {
+		dev_err(FM_DEV,
+			"Received unexpected packet func 0x%X cmd 0x%04X\n",
+			pkt->fm_function, rsp_cmd);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+
+	if (pkt->cmd_status != CG2900_FM_CMD_STATUS_COMMAND_SUCCEEDED) {
+		dev_err(FM_DEV, "FM Command failed (%d)\n", pkt->cmd_status);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+	/* Operation succeeded. We are now done */
+
+error_handling_free_skb:
+	kfree_skb(skb);
+	return err;
+}
+
+/**
+ * receive_bt_cmd_complete() - Wait for and handle an BT Command Complete event.
+ * @audio_user:	Audio user to check for.
+ * @rsp:	Opcode of BT command to wait for.
+ * @data:	Pointer to buffer if any received data should be stored (except
+ *		status).
+ * @data_len:	Length of @data in bytes.
+ *
+ * This function first waits for BT Command Complete event (up to 5 seconds)
+ * and when one arrives, it checks that it is the one we are waiting for and
+ * also that no error has occurred.
+ * If @data is supplied it also copies received data into @data.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -EIO for other errors.
+ */
+static int receive_bt_cmd_complete(struct audio_user *audio_user, u16 rsp,
+				   void *data, int data_len)
+{
+	int err = 0;
+	int res;
+	struct sk_buff *skb;
+	struct bt_cmd_cmpl_event *evt;
+	u16 opcode;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = audio_user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	/*
+	 * Wait for callback to receive command complete and then wake us up
+	 * again.
+	 */
+	res = wait_event_timeout(cb_info->wq,
+				 audio_user->resp_state == RESP_RECEIVED,
+				 msecs_to_jiffies(RESP_TIMEOUT));
+	if (!res) {
+		dev_err(BT_DEV, "Timeout while waiting for return packet\n");
+		return -ECOMM;
+	} else if (res < 0) {
+		/* We timed out or an error occurred */
+		dev_err(BT_DEV,
+			"Error %d occurred while waiting for return packet\n",
+			res);
+		return -ECOMM;
+	}
+
+	/* OK, now we should have received answer. Let's check it. */
+	skb = skb_dequeue_tail(&cb_info->skb_queue);
+	if (!skb) {
+		dev_err(BT_DEV, "No skb in queue when it should be there\n");
+		return -EIO;
+	}
+
+	evt = (struct bt_cmd_cmpl_event *)skb->data;
+	if (evt->eventcode != HCI_EV_CMD_COMPLETE) {
+		dev_err(BT_DEV,
+			"We did not receive the event we expected (0x%X)\n",
+			evt->eventcode);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+
+	opcode = le16_to_cpu(evt->opcode);
+	if (opcode != rsp) {
+		dev_err(BT_DEV,
+			"Received cmd complete for unexpected command: "
+			"0x%04X\n", opcode);
+		err = -EIO;
+		goto error_handling_free_skb;
+	}
+
+	if (evt->status != HCI_BT_ERROR_NO_ERROR) {
+		dev_err(BT_DEV, "Received command complete with err %d\n",
+			evt->status);
+		err = -EIO;
+		/*
+		* In data there might be more detailed error code.
+		* Let's copy it.
+		*/
+	}
+
+	/*
+	 * Copy the rest of the parameters if a buffer has been supplied.
+	 * The caller must have set the length correctly.
+	 */
+	if (data)
+		memcpy(data, evt->data, data_len);
+
+	/* Operation succeeded. We are now done */
+
+error_handling_free_skb:
+	kfree_skb(skb);
+	return err;
+}
+
+/**
+ * send_vs_delete_stream() - Delete an audio stream defined by @stream_handle.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	Handle of the audio stream.
+ *
+ * This function is used to delete an audio stream defined by a stream
+ * handle.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   Errors from @cg2900_write.
+ *   -EIO for other errors.
+ */
+static int send_vs_delete_stream(struct audio_user *audio_user,
+			    unsigned int stream_handle)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	u16 opcode;
+	struct audio_info *info = audio_user->info;
+	struct cg2900_user_data *pf_data = dev_get_platdata(info->dev_bt);
+	struct audio_cb_info *cb_info = cg2900_get_usr(pf_data);
+
+	/* Now delete the stream - format command... */
+	if (info->revision == CG2900_CHIP_REV_PG1) {
+		struct bt_vs_reset_session_cfg_cmd *cmd;
+
+		dev_dbg(BT_DEV, "BT: HCI_VS_Reset_Session_Configuration\n");
+
+		skb = pf_data->alloc_skb(sizeof(*cmd), GFP_KERNEL);
+		if (!skb) {
+			dev_err(BT_DEV, "Could not allocate skb\n");
+			err = -ENOMEM;
+			return err;
+		}
+
+		cmd = (struct bt_vs_reset_session_cfg_cmd *)
+			skb_put(skb, sizeof(*cmd));
+
+		opcode = CG2900_BT_VS_RESET_SESSION_CONFIG;
+		cmd->opcode = cpu_to_le16(opcode);
+		cmd->plen   = BT_PARAM_LEN(sizeof(*cmd));
+		cmd->id     = (u8)stream_handle;
+	} else {
+		struct mc_vs_delete_stream_cmd *cmd;
+
+		dev_dbg(BT_DEV, "BT: HCI_VS_Delete_Stream\n");
+
+		skb = pf_data->alloc_skb(sizeof(*cmd), GFP_KERNEL);
+		if (!skb) {
+			dev_err(BT_DEV, "Could not allocate skb\n");
+			err = -ENOMEM;
+			return err;
+		}
+
+		cmd = (struct mc_vs_delete_stream_cmd *)
+			skb_put(skb, sizeof(*cmd));
+
+		opcode = CG2900_MC_VS_DELETE_STREAM;
+		cmd->opcode = cpu_to_le16(opcode);
+		cmd->plen   = BT_PARAM_LEN(sizeof(*cmd));
+		cmd->stream = (u8)stream_handle;
+	}
+
+	/* ...and send it */
+	cb_info->user = audio_user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	audio_user->resp_state = WAITING;
+
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		goto error_handling_free_skb;
+	}
+
+	/* wait for response */
+	if (info->revision == CG2900_CHIP_REV_PG1) {
+		err = receive_bt_cmd_complete(audio_user, opcode, NULL, 0);
+	} else {
+		u8 vs_err;
+
+		/* All commands on CG2900 PG2 onwards
+		 * API returns one byte extra status */
+		err = receive_bt_cmd_complete(audio_user, opcode,
+					      &vs_err, sizeof(vs_err));
+
+	if (err)
+		dev_err(BT_DEV,
+			"VS_DELETE_STREAM - failed with error 0x%02X\n",
+			vs_err);
+		else
+			release_stream_id(info, stream_handle);
+	}
+
+	return err;
+
+error_handling_free_skb:
+	kfree_skb(skb);
+	return err;
+}
+
+/**
+ * send_vs_session_ctrl() - Formats an sends a CG2900_BT_VS_SESSION_CTRL command.
+ * @user:		Audio user this command belongs to.
+ * @stream_handle:	Handle to stream.
+ * @command:		Command to execute on stream, should be one of
+ *			CG2900_BT_SESSION_START, CG2900_BT_SESSION_STOP,
+ *			CG2900_BT_SESSION_PAUSE, CG2900_BT_SESSION_RESUME.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_session_ctrl(struct audio_user *user,
+				u8 stream_handle, u8 command)
+{
+	int err = 0;
+	struct bt_vs_session_ctrl_cmd *pkt;
+	struct sk_buff *skb;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "BT: HCI_VS_Session_Control handle: %d cmd: %d\n",
+		stream_handle, command);
+
+	skb = pf_data->alloc_skb(sizeof(*pkt), GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV,
+			"send_vs_session_ctrl: Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	/* Enter data into the skb */
+	pkt = (struct bt_vs_session_ctrl_cmd *) skb_put(skb, sizeof(*pkt));
+
+	pkt->opcode  = cpu_to_le16(CG2900_BT_VS_SESSION_CTRL);
+	pkt->plen    = BT_PARAM_LEN(sizeof(*pkt));
+	pkt->id      = stream_handle;
+	pkt->control = command; /* Start/stop etc */
+
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	err = receive_bt_cmd_complete(user, CG2900_BT_VS_SESSION_CTRL,
+				      NULL, 0);
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_vs_session_config() - Formats an sends a CG2900_BT_VS_SESSION_CONFIG command.
+ * @user:		Audio user this command belongs to.
+ * @config_stream:	Custom function for configuring the stream.
+ * @priv_data:		Private data passed to @config_stream untouched.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * Space is allocated for one stream and a custom function is used to
+ * fill in the stream configuration.
+ *
+ * Returns:
+ *  0-255 stream handle if no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_session_config(struct audio_user *user,
+	void(*config_stream)(struct audio_info *, void *,
+			     struct session_config_stream *),
+	void *priv_data)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct bt_vs_session_config_cmd *pkt;
+	u8 session_id;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "BT: HCI_VS_Set_Session_Configuration\n");
+
+	skb = pf_data->alloc_skb(sizeof(*pkt), GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV,
+			"send_vs_session_config: Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	pkt = (struct bt_vs_session_config_cmd *)skb_put(skb, sizeof(*pkt));
+	/* zero the packet so we don't have to set all reserved fields */
+	memset(pkt, 0, sizeof(*pkt));
+
+	/* Common parameters */
+	pkt->opcode    = cpu_to_le16(CG2900_BT_VS_SET_SESSION_CONFIG);
+	pkt->plen      = BT_PARAM_LEN(sizeof(*pkt));
+	pkt->n_streams = 1; /* 1 stream configuration supplied */
+
+	/* Let the custom-function fill in the rest */
+	config_stream(info, priv_data, &pkt->stream);
+
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	err = receive_bt_cmd_complete(user,
+				      CG2900_BT_VS_SET_SESSION_CONFIG,
+				      &session_id, sizeof(session_id));
+	/* Return session id/stream handle if success */
+	if (!err)
+		err = session_id;
+
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_fm_write_1_param() - Formats and sends an FM legacy write command with one parameter.
+ * @user:	Audio user this command belongs to.
+ * @command:	Command.
+ * @param:	Parameter for command.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the fm_mutex held.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_fm_write_1_param(struct audio_user *user,
+				 u16 command, u16 param)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct fm_leg_cmd *cmd;
+	size_t len;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_fm);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(FM_DEV, "send_fm_write_1_param cmd 0x%X param 0x%X\n",
+		command, param);
+
+	/* base package + one parameter */
+	len = sizeof(*cmd) + sizeof(cmd->fm_cmd.data[0]);
+
+	skb = pf_data->alloc_skb(len, GFP_KERNEL);
+	if (!skb) {
+		dev_err(FM_DEV,
+			"send_fm_write_1_param: Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	cmd = (struct fm_leg_cmd *)skb_put(skb, len);
+
+	cmd->length      = CG2900_FM_CMD_PARAM_LEN(len);
+	cmd->opcode      = CG2900_FM_GEN_ID_LEGACY;
+	cmd->read_write  = CG2900_FM_CMD_LEG_PARAM_WRITE;
+	cmd->fm_function = CG2900_FM_CMD_PARAM_WRITECOMMAND;
+	/* one parameter - builtin assumption for this function */
+	cmd->fm_cmd.head    = cpu_to_le16(cg2900_make_fm_cmd_id(command, 1));
+	cmd->fm_cmd.data[0] = cpu_to_le16(param);
+
+	cb_info->user = user;
+	dev_dbg(FM_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(FM_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	err = receive_fm_write_response(user, command);
+finished:
+	dev_dbg(FM_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_vs_stream_ctrl() - Formats an sends a CG2900_MC_VS_STREAM_CONTROL command.
+ * @user:	Audio user this command belongs to.
+ * @stream:	Stream id.
+ * @command:	Start/stop etc.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * While the HCI command allows for multiple streams in one command,
+ * this function only handles one.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_stream_ctrl(struct audio_user *user, u8 stream, u8 command)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct mc_vs_stream_ctrl_cmd *cmd;
+	size_t len;
+	u8 vs_err;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "send_vs_stream_ctrl stream %d command %d\n", stream,
+		command);
+
+	/* basic length + one stream */
+	len = sizeof(*cmd) + sizeof(cmd->stream[0]);
+
+	skb = pf_data->alloc_skb(len, GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV, "send_vs_stream_ctrl:Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	cmd = (struct mc_vs_stream_ctrl_cmd *)skb_put(skb, len);
+
+	cmd->opcode  = cpu_to_le16(CG2900_MC_VS_STREAM_CONTROL);
+	cmd->plen    = BT_PARAM_LEN(len);
+	cmd->command = command;
+
+	/* one stream */
+	cmd->n_streams  = 1;
+	cmd->stream[0] = stream;
+
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	/* All commands on CG2900 PG2 onwards
+	 * API returns one byte extra status */
+	err = receive_bt_cmd_complete(user,
+				      CG2900_MC_VS_STREAM_CONTROL,
+				      &vs_err, sizeof(vs_err));
+	if (err)
+		dev_err(BT_DEV,
+			"VS_STREAM_CONTROL - failed with error 0x%02x\n",
+			vs_err);
+
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_vs_create_stream() - Formats an sends a CG2900_MC_VS_CREATE_STREAM command.
+ * @user:	Audio user this command belongs to.
+ * @inport:	Stream id.
+ * @outport:	Start/stop etc.
+ * @order:	Activation order.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_create_stream(struct audio_user *user, u8 inport,
+				 u8 outport, u8 order)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct mc_vs_create_stream_cmd *cmd;
+	s8 id;
+	u8 vs_err;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV,
+		"send_vs_create_stream inport %d outport %d order %d\n",
+		inport, outport, order);
+
+	id = new_stream_id(info);
+	if (id < 0) {
+		dev_err(BT_DEV, "No free stream id\n");
+		err = -EIO;
+		goto finished;
+	}
+
+	skb = pf_data->alloc_skb(sizeof(*cmd), GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV,
+			"send_vs_create_stream: Could not allocate skb\n");
+		err = -ENOMEM;
+		goto finished_release_id;
+	}
+
+	cmd = (struct mc_vs_create_stream_cmd *)skb_put(skb, sizeof(*cmd));
+
+	cmd->opcode  = cpu_to_le16(CG2900_MC_VS_CREATE_STREAM);
+	cmd->plen    = BT_PARAM_LEN(sizeof(*cmd));
+	cmd->id      = (u8)id;
+	cmd->inport  = inport;
+	cmd->outport = outport;
+	cmd->order   = order;
+
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished_release_id;
+	}
+
+	/* All commands on CG2900 PG2 onwards
+	 * API returns one byte extra status */
+	err = receive_bt_cmd_complete(user,
+				      CG2900_MC_VS_CREATE_STREAM,
+				      &vs_err, sizeof(vs_err));
+	if (err) {
+		dev_err(BT_DEV,
+			"VS_CREATE_STREAM - failed with error 0x%02x\n",
+			vs_err);
+		goto finished_release_id;
+	}
+
+	err = id;
+	goto finished;
+
+finished_release_id:
+	release_stream_id(info, id);
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * send_vs_port_cfg() - Formats an sends a CG2900_MC_VS_PORT_CONFIG command.
+ * @user:	Audio user this command belongs to.
+ * @port:	Port id to configure.
+ * @cfg:	Pointer to specific configuration.
+ * @cfglen:	Length of configuration.
+ *
+ * Packs and sends a command packet and waits for the response. Must
+ * be called with the bt_mutex held.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int send_vs_port_cfg(struct audio_user *user, u8 port,
+			    const void *cfg, size_t cfglen)
+{
+	int err = 0;
+	struct sk_buff *skb;
+	struct mc_vs_port_cfg_cmd *cmd;
+	void *ptr;
+	u8 vs_err;
+	struct audio_cb_info *cb_info;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data;
+
+	info = user->info;
+	pf_data = dev_get_platdata(info->dev_bt);
+	cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "send_vs_port_cfg len %d\n", cfglen);
+
+	skb = pf_data->alloc_skb(sizeof(*cmd) + cfglen, GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV, "send_vs_port_cfg: Could not allocate skb\n");
+		return -ENOMEM;
+	}
+
+	/* Fill in common part */
+	cmd = (struct mc_vs_port_cfg_cmd *) skb_put(skb, sizeof(*cmd));
+	cmd->opcode = cpu_to_le16(CG2900_MC_VS_PORT_CONFIG);
+	cmd->plen = BT_PARAM_LEN(sizeof(*cmd) + cfglen);
+	cmd->type = port;
+
+	/* Copy specific configuration */
+	ptr = skb_put(skb, cfglen);
+	memcpy(ptr, cfg, cfglen);
+
+	/* Send */
+	cb_info->user = user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	user->resp_state = WAITING;
+
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		kfree_skb(skb);
+		goto finished;
+	}
+
+	/* All commands on CG2900 PG2 onwards
+	 * API returns one byte extra status */
+	err = receive_bt_cmd_complete(user, CG2900_MC_VS_PORT_CONFIG,
+				      &vs_err, sizeof(vs_err));
+	if (err)
+		dev_err(BT_DEV, "VS_PORT_CONFIG - failed with error 0x%02x\n",
+			vs_err);
+
+finished:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	user->resp_state = IDLE;
+	return err;
+}
+
+/**
+ * set_dai_config_pg1() - Internal implementation of @cg2900_audio_set_dai_config for PG1 hardware.
+ * @audio_user:	Pointer to audio user struct.
+ * @config:	Pointer to the configuration to set.
+ *
+ * Sets the Digital Audio Interface (DAI) configuration for PG1
+ * hardware. This is and internal function and basic
+ * argument-verification should have been done by the caller.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -EACCESS if port is not supported.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int set_dai_config_pg1(struct audio_user *audio_user,
+			      struct cg2900_dai_config *config)
+{
+	int err = 0;
+	struct cg2900_dai_conf_i2s_pcm *i2s_pcm;
+	struct sk_buff *skb = NULL;
+	struct bt_vs_set_hw_cfg_cmd_i2s *i2s_cmd;
+	struct bt_vs_set_hw_cfg_cmd_pcm *pcm_cmd;
+	struct audio_info *info = audio_user->info;
+	struct cg2900_user_data *pf_data = dev_get_platdata(info->dev_bt);
+	struct audio_cb_info *cb_info = cg2900_get_usr(pf_data);
+
+	dev_dbg(BT_DEV, "set_dai_config_pg1 port %d\n", config->port);
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any time on
+	 * each channel.
+	 */
+	mutex_lock(&info->bt_mutex);
+
+	/* Allocate the sk_buffer. The length is actually a max length since
+	 * length varies depending on logical transport.
+	 */
+	skb = pf_data->alloc_skb(CG2900_BT_LEN_VS_SET_HARDWARE_CONFIG,
+				 GFP_KERNEL);
+	if (!skb) {
+		dev_err(BT_DEV, "set_dai_config_pg1: Could not allocate skb\n");
+		err = -ENOMEM;
+		goto finished_unlock_mutex;
+	}
+
+	/* Fill in hci-command according to received configuration */
+	switch (config->port) {
+	case PORT_0_I2S:
+		i2s_cmd = (struct bt_vs_set_hw_cfg_cmd_i2s *)
+			skb_put(skb, sizeof(*i2s_cmd));
+
+		i2s_cmd->opcode = cpu_to_le16(CG2900_BT_VS_SET_HARDWARE_CONFIG);
+		i2s_cmd->plen   = BT_PARAM_LEN(sizeof(*i2s_cmd));
+
+		i2s_cmd->vp_type = PORT_PROTOCOL_I2S;
+		i2s_cmd->port_id = 0x00; /* First/only I2S port */
+		i2s_cmd->half_period = config->conf.i2s.half_period;
+
+		i2s_cmd->master_slave = mc_i2s_role(config->conf.i2s.mode);
+
+		/* Store the new configuration */
+		mutex_lock(&info->management_mutex);
+		memcpy(&info->i2s_config, &config->conf.i2s,
+		       sizeof(config->conf.i2s));
+		info->i2s_config_known = true;
+		mutex_unlock(&info->management_mutex);
+		break;
+
+	case PORT_1_I2S_PCM:
+		pcm_cmd = (struct bt_vs_set_hw_cfg_cmd_pcm *)
+			skb_put(skb, sizeof(*pcm_cmd));
+
+		pcm_cmd->opcode = cpu_to_le16(CG2900_BT_VS_SET_HARDWARE_CONFIG);
+		pcm_cmd->plen   = BT_PARAM_LEN(sizeof(*pcm_cmd));
+
+		i2s_pcm = &config->conf.i2s_pcm;
+
+		/*
+		 * CG2900 PG1 chips don't support I2S over the PCM/I2S bus,
+		 * and CG2900 PG2 onwards chips don't use this command
+		 */
+		if (i2s_pcm->protocol != PORT_PROTOCOL_PCM) {
+			dev_err(BT_DEV,
+				"I2S not supported over the PCM/I2S bus\n");
+			err = -EACCES;
+			goto error_handling_free_skb;
+		}
+
+		pcm_cmd->vp_type = PORT_PROTOCOL_PCM;
+		pcm_cmd->port_id = 0x00; /* First/only PCM port */
+
+		HWCONFIG_PCM_SET_MODE(pcm_cmd, mc_pcm_role(i2s_pcm->mode));
+
+		HWCONFIG_PCM_SET_DIR(pcm_cmd, 0, i2s_pcm->slot_0_dir);
+		HWCONFIG_PCM_SET_DIR(pcm_cmd, 1, i2s_pcm->slot_1_dir);
+		HWCONFIG_PCM_SET_DIR(pcm_cmd, 2, i2s_pcm->slot_2_dir);
+		HWCONFIG_PCM_SET_DIR(pcm_cmd, 3, i2s_pcm->slot_3_dir);
+
+		pcm_cmd->bit_clock = i2s_pcm->clk;
+		pcm_cmd->frame_len =
+			cpu_to_le16(get_fs_duration(i2s_pcm->duration));
+
+		/* Store the new configuration */
+		mutex_lock(&info->management_mutex);
+		memcpy(&info->i2s_pcm_config, &config->conf.i2s_pcm,
+		       sizeof(config->conf.i2s_pcm));
+		info->i2s_pcm_config_known = true;
+		mutex_unlock(&info->management_mutex);
+		break;
+
+	default:
+		dev_err(BT_DEV, "Unknown port configuration %d\n",
+			config->port);
+		err = -EACCES;
+		goto error_handling_free_skb;
+	};
+
+	cb_info->user = audio_user;
+	dev_dbg(BT_DEV, "New resp_state: WAITING\n");
+	audio_user->resp_state = WAITING;
+
+	/* Send packet to controller */
+	err = pf_data->write(pf_data, skb);
+	if (err) {
+		dev_err(BT_DEV, "Error %d occurred while transmitting skb\n",
+			err);
+		goto error_handling_free_skb;
+	}
+
+	err = receive_bt_cmd_complete(audio_user,
+				      CG2900_BT_VS_SET_HARDWARE_CONFIG,
+				      NULL, 0);
+
+	goto finished_unlock_mutex;
+
+error_handling_free_skb:
+	kfree_skb(skb);
+finished_unlock_mutex:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	return err;
+}
+
+/**
+ * set_dai_config_pg2() - Internal implementation of
+ * cg2900_audio_set_dai_config for CG2900 PG2 onwards hardware.
+ * @audio_user:	Pointer to audio user struct.
+ * @config:	Pointer to the configuration to set.
+ *
+ * Sets the Digital Audio Interface (DAI) configuration for
+ * CG2900 PG2 onwards hardware. This is an internal function
+ * and basic argument-verification should have been
+ * done by the caller.
+ *
+ * Returns:
+ *  0 if there is no error.
+ *  -EACCESS if port is not supported.
+ *  -ENOMEM if not possible to allocate packet.
+ *  -ECOMM if no response was received.
+ *  -EIO for other errors.
+ */
+static int set_dai_config_pg2(struct audio_user *audio_user,
+			      struct cg2900_dai_config *config)
+{
+	int err = 0;
+	struct cg2900_dai_conf_i2s *i2s;
+	struct cg2900_dai_conf_i2s_pcm *i2s_pcm;
+
+	struct mc_vs_port_cfg_i2s i2s_cfg;
+	struct mc_vs_port_cfg_pcm_i2s pcm_cfg;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(BT_DEV, "set_dai_config_pg2 port %d\n", config->port);
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any time on
+	 * each channel.
+	 */
+	mutex_lock(&info->bt_mutex);
+
+	switch (config->port) {
+	case PORT_0_I2S:
+		i2s = &config->conf.i2s;
+
+		memset(&i2s_cfg, 0, sizeof(i2s_cfg)); /* just to be safe  */
+
+		/* master/slave */
+		PORTCFG_I2S_SET_ROLE(i2s_cfg, mc_i2s_role(i2s->mode));
+
+		PORTCFG_I2S_SET_HALFPERIOD(i2s_cfg, i2s->half_period);
+		PORTCFG_I2S_SET_CHANNELS(i2s_cfg,
+			mc_i2s_channel_select(i2s->channel_sel));
+		PORTCFG_I2S_SET_SRATE(i2s_cfg,
+			mc_i2s_sample_rate(i2s->sample_rate));
+		switch (i2s->word_width) {
+		case WORD_WIDTH_16:
+			PORTCFG_I2S_SET_WORDLEN(i2s_cfg, CG2900_MC_I2S_WORD_16);
+			break;
+		case WORD_WIDTH_32:
+			PORTCFG_I2S_SET_WORDLEN(i2s_cfg, CG2900_MC_I2S_WORD_32);
+			break;
+		}
+
+		/* Store the new configuration */
+		mutex_lock(&info->management_mutex);
+		memcpy(&(info->i2s_config), &(config->conf.i2s),
+		       sizeof(config->conf.i2s));
+		info->i2s_config_known = true;
+		mutex_unlock(&info->management_mutex);
+
+		/* Send */
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_I2S,
+				       &i2s_cfg, sizeof(i2s_cfg));
+		break;
+
+	case PORT_1_I2S_PCM:
+		i2s_pcm = &config->conf.i2s_pcm;
+
+		memset(&pcm_cfg, 0, sizeof(pcm_cfg)); /* just to be safe  */
+
+		/* master/slave */
+		PORTCFG_PCM_SET_ROLE(pcm_cfg, mc_pcm_role(i2s_pcm->mode));
+
+		/* set direction for all 4 slots */
+		PORTCFG_PCM_SET_DIR(pcm_cfg, 0, i2s_pcm->slot_0_dir);
+		PORTCFG_PCM_SET_DIR(pcm_cfg, 1, i2s_pcm->slot_1_dir);
+		PORTCFG_PCM_SET_DIR(pcm_cfg, 2, i2s_pcm->slot_2_dir);
+		PORTCFG_PCM_SET_DIR(pcm_cfg, 3, i2s_pcm->slot_3_dir);
+
+		/* set used SCO slots, other use cases not supported atm */
+		PORTCFG_PCM_SET_SCO_USED(pcm_cfg, 0, i2s_pcm->slot_0_used);
+		PORTCFG_PCM_SET_SCO_USED(pcm_cfg, 1, i2s_pcm->slot_1_used);
+		PORTCFG_PCM_SET_SCO_USED(pcm_cfg, 2, i2s_pcm->slot_2_used);
+		PORTCFG_PCM_SET_SCO_USED(pcm_cfg, 3, i2s_pcm->slot_3_used);
+
+		/* slot starts */
+		pcm_cfg.slot_start[0] = i2s_pcm->slot_0_start;
+		pcm_cfg.slot_start[1] = i2s_pcm->slot_1_start;
+		pcm_cfg.slot_start[2] = i2s_pcm->slot_2_start;
+		pcm_cfg.slot_start[3] = i2s_pcm->slot_3_start;
+
+		/* audio/voice sample-rate ratio */
+		PORTCFG_PCM_SET_RATIO(pcm_cfg, i2s_pcm->ratio);
+
+		/* PCM or I2S mode */
+		PORTCFG_PCM_SET_MODE(pcm_cfg, i2s_pcm->protocol);
+
+		pcm_cfg.frame_len = i2s_pcm->duration;
+
+		PORTCFG_PCM_SET_BITCLK(pcm_cfg, i2s_pcm->clk);
+		PORTCFG_PCM_SET_SRATE(pcm_cfg,
+			mc_pcm_sample_rate(i2s_pcm->sample_rate));
+
+		/* Store the new configuration */
+		mutex_lock(&info->management_mutex);
+		memcpy(&(info->i2s_pcm_config), &(config->conf.i2s_pcm),
+		       sizeof(config->conf.i2s_pcm));
+		info->i2s_pcm_config_known = true;
+		mutex_unlock(&info->management_mutex);
+
+		/* Send */
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_PCM_I2S,
+				       &pcm_cfg, sizeof(pcm_cfg));
+		break;
+
+	default:
+		dev_err(BT_DEV, "Unknown port configuration %d\n",
+			config->port);
+		err = -EACCES;
+	};
+
+	mutex_unlock(&info->bt_mutex);
+	return err;
+}
+
+/**
+ * struct i2s_fm_stream_config_priv - Helper struct for stream i2s-fm streams.
+ * @fm_config:	FM endpoint configuration.
+ * @rx:		true for FM-RX, false for FM-TX.
+ */
+struct i2s_fm_stream_config_priv {
+	struct cg2900_endpoint_config_fm	*fm_config;
+	bool					rx;
+
+};
+
+/**
+ * config_i2s_fm_stream() - Callback for @send_vs_session_config.
+ * @info:	Audio info structure.
+ * @_priv:	Pointer to a @i2s_fm_stream_config_priv struct.
+ * @cfg:	Pointer to stream config block in command packet.
+ *
+ * Fills in stream configuration for I2S-FM RX/TX.
+ */
+
+static void config_i2s_fm_stream(struct audio_info *info, void *_priv,
+				 struct session_config_stream *cfg)
+{
+	struct i2s_fm_stream_config_priv *priv = _priv;
+	struct session_config_vport *fm;
+	struct session_config_vport *i2s;
+
+	cfg->media_type = CG2900_BT_SESSION_MEDIA_TYPE_AUDIO;
+
+	if (info->i2s_config.channel_sel == CHANNEL_SELECTION_BOTH)
+		SESSIONCFG_SET_CHANNELS(cfg, CG2900_BT_MEDIA_CONFIG_STEREO);
+	else
+		SESSIONCFG_SET_CHANNELS(cfg, CG2900_BT_MEDIA_CONFIG_MONO);
+
+	SESSIONCFG_I2S_SET_SRATE(cfg,
+		session_config_sample_rate(priv->fm_config->sample_rate));
+
+	cfg->codec_type = CG2900_CODEC_TYPE_NONE;
+	/* codec mode and parameters not used  */
+
+	if (priv->rx) {
+		fm  = &cfg->inport;  /* FM is input */
+		i2s = &cfg->outport; /* I2S is output */
+	} else {
+		i2s = &cfg->inport;  /* I2S is input */
+		fm  = &cfg->outport; /* FM is output */
+	}
+
+	fm->type = CG2900_BT_VP_TYPE_FM;
+
+	i2s->type = CG2900_BT_VP_TYPE_I2S;
+	i2s->i2s.index   = CG2900_BT_SESSION_I2S_INDEX_I2S;
+	i2s->i2s.channel = info->i2s_config.channel_sel;
+}
+
+/**
+ * conn_start_i2s_to_fm_rx() - Start an audio stream connecting FM RX to I2S.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	[out] Pointer where to store the stream handle.
+ *
+ * This function sets up an FM RX to I2S stream.
+ * It does this by first setting the output mode and then the configuration of
+ * the External Sample Rate Converter.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   -EIO for other errors.
+ */
+static int conn_start_i2s_to_fm_rx(struct audio_user *audio_user,
+				   unsigned int *stream_handle)
+{
+	int err = 0;
+	union cg2900_endpoint_config_union *fm_config;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(FM_DEV, "conn_start_i2s_to_fm_rx\n");
+
+	fm_config = find_endpoint(ENDPOINT_FM_RX, &info->endpoints);
+	if (!fm_config) {
+		dev_err(FM_DEV, "FM RX not configured before stream start\n");
+		return -EIO;
+	}
+
+	if (!(info->i2s_config_known)) {
+		dev_err(FM_DEV,
+			"I2S DAI not configured before stream start\n");
+		return -EIO;
+	}
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any
+	 * time on each channel.
+	 */
+	mutex_lock(&info->fm_mutex);
+	mutex_lock(&info->bt_mutex);
+
+	/*
+	 * Now set the output mode of the External Sample Rate Converter by
+	 * sending HCI_Write command with AUP_EXT_SetMode.
+	 */
+	err = send_fm_write_1_param(audio_user,
+				    CG2900_FM_CMD_ID_AUP_EXT_SET_MODE,
+				    CG2900_FM_CMD_AUP_EXT_SET_MODE_PARALLEL);
+	if (err)
+		goto finished_unlock_mutex;
+
+	/*
+	 * Now configure the External Sample Rate Converter by sending
+	 * HCI_Write command with AUP_EXT_SetControl.
+	 */
+	err = send_fm_write_1_param(
+		audio_user, CG2900_FM_CMD_ID_AUP_EXT_SET_CTRL,
+		fm_get_conversion(info, fm_config->fm.sample_rate));
+	if (err)
+		goto finished_unlock_mutex;
+
+	/* Set up the stream */
+	if (info->revision == CG2900_CHIP_REV_PG1) {
+		struct i2s_fm_stream_config_priv stream_priv;
+
+		/* Now send HCI_VS_Set_Session_Configuration command */
+		stream_priv.fm_config = &fm_config->fm;
+		stream_priv.rx = true;
+		err = send_vs_session_config(audio_user, config_i2s_fm_stream,
+					     &stream_priv);
+	} else {
+		struct mc_vs_port_cfg_fm fm_cfg;
+
+		memset(&fm_cfg, 0, sizeof(fm_cfg));
+
+		/* Configure port FM RX */
+		/* Expects 0-3 - same as user API - so no conversion needed */
+		PORTCFG_FM_SET_SRATE(fm_cfg, (u8)fm_config->fm.sample_rate);
+
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_FM_RX_1,
+				       &fm_cfg, sizeof(fm_cfg));
+		if (err)
+			goto finished_unlock_mutex;
+
+		/* CreateStream */
+		err = send_vs_create_stream(audio_user,
+					    CG2900_MC_PORT_FM_RX_1,
+					    CG2900_MC_PORT_I2S,
+					    0); /* chip doesn't care */
+	}
+
+	if (err < 0)
+		goto finished_unlock_mutex;
+
+	/* Store the stream handle (used for start and stop stream) */
+	*stream_handle = (u8)err;
+	dev_dbg(FM_DEV, "stream_handle set to %d\n", *stream_handle);
+
+	/* Now start the stream */
+	if (info->revision == CG2900_CHIP_REV_PG1)
+		err = send_vs_session_ctrl(audio_user, *stream_handle,
+					   CG2900_BT_SESSION_START);
+	else
+		err = send_vs_stream_ctrl(audio_user, *stream_handle,
+					  CG2900_MC_STREAM_START);
+	/*Let's delete a stream.*/
+	if (err < 0) {
+		dev_dbg(BT_DEV, "Could not start a stream.");
+		(void)send_vs_delete_stream(audio_user, *stream_handle);
+	}
+
+finished_unlock_mutex:
+	dev_dbg(FM_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	mutex_unlock(&info->fm_mutex);
+	return err;
+}
+
+/**
+ * conn_start_i2s_to_fm_tx() - Start an audio stream connecting FM TX to I2S.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	[out] Pointer where to store the stream handle.
+ *
+ * This function sets up an I2S to FM TX stream.
+ * It does this by first setting the Audio Input source and then setting the
+ * configuration and input source of BT sample rate converter.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   -EIO for other errors.
+ */
+static int conn_start_i2s_to_fm_tx(struct audio_user *audio_user,
+				   unsigned int *stream_handle)
+{
+	int err = 0;
+	union cg2900_endpoint_config_union *fm_config;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(FM_DEV, "conn_start_i2s_to_fm_tx\n");
+
+	fm_config = find_endpoint(ENDPOINT_FM_TX, &info->endpoints);
+	if (!fm_config) {
+		dev_err(FM_DEV, "FM TX not configured before stream start\n");
+		return -EIO;
+	}
+
+	if (!(info->i2s_config_known)) {
+		dev_err(FM_DEV,
+			"I2S DAI not configured before stream start\n");
+		return -EIO;
+	}
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any time
+	 * on each channel.
+	 */
+	mutex_lock(&info->fm_mutex);
+	mutex_lock(&info->bt_mutex);
+
+	/*
+	 * Select Audio Input Source by sending HCI_Write command with
+	 * AIP_SetMode.
+	 */
+	if (info->revision == CG2900_CHIP_REV_PG1 ||
+			info->revision == CG2900_CHIP_REV_PG2) {
+		dev_dbg(FM_DEV, "FM: AIP_SetMode\n");
+		err = send_fm_write_1_param(audio_user,
+				CG2900_FM_CMD_ID_AIP_SET_MODE,
+				CG2900_FM_CMD_AIP_SET_MODE_INPUT_DIG);
+		if (err)
+			goto finished_unlock_mutex;
+	}
+	/*
+	 * Now configure the BT sample rate converter by sending HCI_Write
+	 * command with AIP_BT_SetControl.
+	 */
+	dev_dbg(FM_DEV, "FM: AIP_BT_SetControl\n");
+	err = send_fm_write_1_param(
+		audio_user, CG2900_FM_CMD_ID_AIP_BT_SET_CTRL,
+		fm_get_conversion(info, fm_config->fm.sample_rate));
+	if (err)
+		goto finished_unlock_mutex;
+
+	/*
+	 * Now set input of the BT sample rate converter by sending HCI_Write
+	 * command with AIP_BT_SetMode.
+	 */
+	dev_dbg(FM_DEV, "FM: AIP_BT_SetMode\n");
+	err = send_fm_write_1_param(audio_user,
+				    CG2900_FM_CMD_ID_AIP_BT_SET_MODE,
+				    CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_PAR);
+	if (err)
+		goto finished_unlock_mutex;
+
+	/* Set up the stream */
+	if (info->revision == CG2900_CHIP_REV_PG1) {
+		struct i2s_fm_stream_config_priv stream_priv;
+
+		/* Now send HCI_VS_Set_Session_Configuration command */
+		stream_priv.fm_config = &fm_config->fm;
+		stream_priv.rx = false;
+		err = send_vs_session_config(audio_user, config_i2s_fm_stream,
+					     &stream_priv);
+	} else {
+		struct mc_vs_port_cfg_fm fm_cfg;
+
+		memset(&fm_cfg, 0, sizeof(fm_cfg));
+
+		/* Configure port FM TX */
+		/* Expects 0-3 - same as user API - so no conversion needed */
+		PORTCFG_FM_SET_SRATE(fm_cfg, (u8)fm_config->fm.sample_rate);
+
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_FM_TX,
+				       &fm_cfg, sizeof(fm_cfg));
+		if (err)
+			goto finished_unlock_mutex;
+
+		/* CreateStream */
+		err = send_vs_create_stream(audio_user,
+					    CG2900_MC_PORT_I2S,
+					    CG2900_MC_PORT_FM_TX,
+					    0); /* chip doesn't care */
+	}
+
+	if (err < 0)
+		goto finished_unlock_mutex;
+
+	/* Store the stream handle (used for start and stop stream) */
+	*stream_handle = (u8)err;
+	dev_dbg(FM_DEV, "stream_handle set to %d\n", *stream_handle);
+
+	/* Now start the stream */
+	if (info->revision == CG2900_CHIP_REV_PG1)
+		err = send_vs_session_ctrl(audio_user, *stream_handle,
+					   CG2900_BT_SESSION_START);
+	else
+		err = send_vs_stream_ctrl(audio_user, *stream_handle,
+					  CG2900_MC_STREAM_START);
+	/* Let's delete and release stream.*/
+	if (err < 0) {
+		dev_dbg(BT_DEV, "Could not start a stream.");
+		(void)send_vs_delete_stream(audio_user, *stream_handle);
+	}
+
+finished_unlock_mutex:
+	dev_dbg(FM_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	mutex_unlock(&info->fm_mutex);
+	return err;
+}
+
+/**
+ * config_pcm_sco_stream() - Callback for @send_vs_session_config.
+ * @info:	Audio info structure.
+ * @_priv:	Pointer to a @cg2900_endpoint_config_sco_in_out struct.
+ * @cfg:	Pointer to stream config block in command packet.
+ *
+ * Fills in stream configuration for PCM-SCO.
+ */
+static void config_pcm_sco_stream(struct audio_info *info, void *_priv,
+				  struct session_config_stream *cfg)
+{
+	struct cg2900_endpoint_config_sco_in_out *sco_ep = _priv;
+
+	cfg->media_type = CG2900_BT_SESSION_MEDIA_TYPE_AUDIO;
+
+	SESSIONCFG_SET_CHANNELS(cfg, CG2900_BT_MEDIA_CONFIG_MONO);
+	SESSIONCFG_I2S_SET_SRATE(cfg,
+		session_config_sample_rate(sco_ep->sample_rate));
+
+	cfg->codec_type = CG2900_CODEC_TYPE_NONE;
+	/* codec mode and parameters not used  */
+
+	cfg->inport.type = CG2900_BT_VP_TYPE_BT_SCO;
+	cfg->inport.sco.acl_handle = cpu_to_le16(DEFAULT_ACL_HANDLE);
+
+	cfg->outport.type = CG2900_BT_VP_TYPE_PCM;
+	cfg->outport.pcm.index = CG2900_BT_SESSION_PCM_INDEX_PCM_I2S;
+
+	SESSIONCFG_PCM_SET_USED(cfg->outport, 0,
+				info->i2s_pcm_config.slot_0_used);
+	SESSIONCFG_PCM_SET_USED(cfg->outport, 1,
+				info->i2s_pcm_config.slot_1_used);
+	SESSIONCFG_PCM_SET_USED(cfg->outport, 2,
+				info->i2s_pcm_config.slot_2_used);
+	SESSIONCFG_PCM_SET_USED(cfg->outport, 3,
+				info->i2s_pcm_config.slot_3_used);
+
+	cfg->outport.pcm.slot_start[0] =
+		info->i2s_pcm_config.slot_0_start;
+	cfg->outport.pcm.slot_start[1] =
+		info->i2s_pcm_config.slot_1_start;
+	cfg->outport.pcm.slot_start[2] =
+		info->i2s_pcm_config.slot_2_start;
+	cfg->outport.pcm.slot_start[3] =
+		info->i2s_pcm_config.slot_3_start;
+}
+
+/**
+ * conn_start_pcm_to_sco() - Start an audio stream connecting Bluetooth (e)SCO to PCM_I2S.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	[out] Pointer where to store the stream handle.
+ *
+ * This function sets up a BT to_from PCM_I2S stream. It does this by
+ * first setting the Session configuration and then starting the Audio
+ * Stream.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   Errors from @cg2900_write
+ *   -EIO for other errors.
+ */
+static int conn_start_pcm_to_sco(struct audio_user *audio_user,
+				 unsigned int *stream_handle)
+{
+	int err = 0;
+	union cg2900_endpoint_config_union *bt_config;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(BT_DEV, "conn_start_pcm_to_sco\n");
+
+	bt_config = find_endpoint(ENDPOINT_BT_SCO_INOUT, &info->endpoints);
+	if (!bt_config) {
+		dev_err(BT_DEV, "BT not configured before stream start\n");
+		return -EIO;
+	}
+
+	if (!(info->i2s_pcm_config_known)) {
+		dev_err(BT_DEV,
+			"I2S_PCM DAI not configured before stream start\n");
+		return -EIO;
+	}
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any time on each
+	 * channel.
+	 */
+	mutex_lock(&info->bt_mutex);
+
+	/* Set up the stream */
+	if (info->revision == CG2900_CHIP_REV_PG1) {
+		err = send_vs_session_config(audio_user, config_pcm_sco_stream,
+					     &bt_config->sco);
+	} else {
+		struct mc_vs_port_cfg_sco sco_cfg;
+
+		/* zero codec params etc */
+		memset(&sco_cfg, 0, sizeof(sco_cfg));
+		sco_cfg.acl_id = DEFAULT_ACL_HANDLE;
+		PORTCFG_SCO_SET_CODEC(sco_cfg, CG2900_CODEC_TYPE_NONE);
+
+		err = send_vs_port_cfg(audio_user, CG2900_MC_PORT_BT_SCO,
+				       &sco_cfg, sizeof(sco_cfg));
+		if (err)
+			goto finished_unlock_mutex;
+
+		/* CreateStream */
+		err = send_vs_create_stream(audio_user,
+					    CG2900_MC_PORT_PCM_I2S,
+					    CG2900_MC_PORT_BT_SCO,
+					    0); /* chip doesn't care */
+	}
+
+	if (err < 0)
+		goto finished_unlock_mutex;
+
+	/* Store the stream handle (used for start and stop stream) */
+	*stream_handle = (u8)err;
+	dev_dbg(BT_DEV, "stream_handle set to %d\n", *stream_handle);
+
+	/* Now start the stream */
+	if (info->revision == CG2900_CHIP_REV_PG1)
+		err = send_vs_session_ctrl(audio_user, *stream_handle,
+					   CG2900_BT_SESSION_START);
+	else
+		err = send_vs_stream_ctrl(audio_user, *stream_handle,
+					  CG2900_MC_STREAM_START);
+	/* Let's delete and release stream.*/
+	if (err < 0) {
+		dev_dbg(BT_DEV, "Could not start a stream.");
+		(void)send_vs_delete_stream(audio_user, *stream_handle);
+	}
+
+finished_unlock_mutex:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	return err;
+}
+
+/**
+ * conn_stop_stream() - Stops an audio stream defined by @stream_handle.
+ * @audio_user:		Audio user to check for.
+ * @stream_handle:	Handle of the audio stream.
+ *
+ * This function is used to stop an audio stream defined by a stream
+ * handle. It does this by first stopping the stream and then
+ * resetting the session/stream.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ECOMM if no response was received.
+ *   -ENOMEM upon allocation errors.
+ *   Errors from @cg2900_write.
+ *   -EIO for other errors.
+ */
+static int conn_stop_stream(struct audio_user *audio_user,
+			    unsigned int stream_handle)
+{
+	int err = 0;
+	struct audio_info *info = audio_user->info;
+
+	dev_dbg(BT_DEV, "conn_stop_stream handle %d\n", stream_handle);
+
+	/*
+	 * Use mutex to assure that only ONE command is sent at any
+	 * time on each channel.
+	 */
+	mutex_lock(&info->bt_mutex);
+
+	/* Now stop the stream */
+	if (info->revision == CG2900_CHIP_REV_PG1)
+		err = send_vs_session_ctrl(audio_user, stream_handle,
+					   CG2900_BT_SESSION_STOP);
+	else
+		err = send_vs_stream_ctrl(audio_user, stream_handle,
+					  CG2900_MC_STREAM_STOP);
+	if (err)
+		goto finished_unlock_mutex;
+
+	err = send_vs_delete_stream(audio_user, stream_handle);
+
+finished_unlock_mutex:
+	dev_dbg(BT_DEV, "New resp_state: IDLE\n");
+	audio_user->resp_state = IDLE;
+	mutex_unlock(&info->bt_mutex);
+	return err;
+}
+
+/**
+ * cg2900_audio_get_devices() - Returns connected CG2900 Audio devices.
+ * @devices:	Array of CG2900 Audio devices.
+ * @size:	Max number of devices in array.
+ *
+ * Returns:
+ *   0 if no devices exist.
+ *   > 0 is the number of devices inserted in the list.
+ *   -EINVAL upon bad input parameter.
+ */
+int cg2900_audio_get_devices(struct device *devices[], __u8 size)
+{
+	struct list_head *cursor;
+	struct audio_info *tmp;
+	int i = 0;
+
+	if (!size) {
+		pr_err("No space to insert devices into list\n");
+		return 0;
+	}
+
+	if (!devices) {
+		pr_err("NULL submitted as devices array\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Go through and store the devices. If NULL is supplied for dev
+	 * just return first device found.
+	 */
+	list_for_each(cursor, &cg2900_audio_devices) {
+		tmp = list_entry(cursor, struct audio_info, list);
+		devices[i] = tmp->parent;
+		i++;
+		if (i == size)
+			break;
+	}
+	return i;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_get_devices);
+
+/**
+ * cg2900_audio_open() - Opens a session to the ST-Ericsson CG2900 Audio control interface.
+ * @session:	[out] Address where to store the session identifier.
+ *		Allocated by caller, must not be NULL.
+ * @parent:	Parent device representing the CG2900 controller connected.
+ *		If NULL is supplied the first available device is used.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EACCES if no info structure can be found.
+ *   -EINVAL upon bad input parameter.
+ *   -ENOMEM upon allocation failure.
+ *   -EMFILE if no more user session could be opened.
+ *   -EIO upon failure to register to CG2900.
+ *   Error codes from get_info.
+ */
+int cg2900_audio_open(unsigned int *session, struct device *parent)
+{
+	int err = 0;
+	int i;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data_bt;
+	struct cg2900_user_data *pf_data_fm;
+
+	pr_debug("cg2900_audio_open");
+
+	info = get_info(parent);
+	if (!info) {
+		pr_err("No audio info exist");
+		return -EACCES;
+	} else if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	if (!session) {
+		pr_err("NULL supplied as session");
+		return -EINVAL;
+	}
+
+	mutex_lock(&info->management_mutex);
+
+	*session = 0;
+
+	/*
+	 * First find a free session to use and allocate the session structure.
+	 */
+	for (i = FIRST_USER;
+	     i < MAX_NBR_OF_USERS && cg2900_audio_sessions[i];
+	     i++)
+		; /* Just loop until found or end reached */
+
+	if (i >= MAX_NBR_OF_USERS) {
+		pr_err("Couldn't find free user");
+		err = -EMFILE;
+		goto finished;
+	}
+
+	cg2900_audio_sessions[i] =
+		kzalloc(sizeof(*(cg2900_audio_sessions[0])), GFP_KERNEL);
+	if (!cg2900_audio_sessions[i]) {
+		pr_err("Could not allocate user");
+		err = -ENOMEM;
+		goto finished;
+	}
+	pr_debug("Found free session %d", i);
+	*session = i;
+	info->nbr_of_users_active++;
+
+	cg2900_audio_sessions[*session]->resp_state = IDLE;
+	cg2900_audio_sessions[*session]->session = *session;
+	cg2900_audio_sessions[*session]->info = info;
+
+	pf_data_bt = dev_get_platdata(info->dev_bt);
+	pf_data_fm = dev_get_platdata(info->dev_fm);
+
+	if (info->nbr_of_users_active == 1) {
+		struct cg2900_rev_data rev_data;
+
+		/*
+		 * First user so register to CG2900 Core.
+		 * First the BT audio device.
+		 */
+		err = pf_data_bt->open(pf_data_bt);
+		if (err) {
+			dev_err(BT_DEV, "Failed to open BT audio channel\n");
+			goto error_handling;
+		}
+
+		/* Then the FM audio device */
+		err = pf_data_fm->open(pf_data_fm);
+		if (err) {
+			dev_err(FM_DEV, "Failed to open FM audio channel\n");
+			goto error_handling;
+		}
+
+		/* Read chip revision data */
+		if (!pf_data_bt->get_local_revision(pf_data_bt, &rev_data)) {
+			pr_err("Couldn't retrieve revision data");
+			err = -EIO;
+			goto error_handling;
+		}
+
+		/* Decode revision data */
+		switch (rev_data.revision) {
+		case CG2900_PG1_REV:
+		case CG2900_PG1_SPECIAL_REV:
+			info->revision = CG2900_CHIP_REV_PG1;
+			break;
+
+		case CG2900_PG2_REV:
+			info->revision = CG2900_CHIP_REV_PG2;
+			break;
+
+		case CG2905_PG1_1_REV:
+			info->revision = CG2905_CHIP_REV_PG1_1;
+			break;
+
+		case CG2910_PG1_REV:
+			info->revision = CG2910_CHIP_REV_PG1;
+			break;
+
+		case CG2910_PG2_REV:
+			info->revision = CG2910_CHIP_REV_PG2;
+			break;
+
+		default:
+			pr_err("Chip rev 0x%04X sub 0x%04X not supported",
+			       rev_data.revision, rev_data.sub_version);
+			err = -EIO;
+			goto error_handling;
+		}
+
+		info->state = OPENED;
+	}
+
+	pr_info("Session %d opened", *session);
+
+	goto finished;
+
+error_handling:
+	if (pf_data_fm->opened)
+		pf_data_fm->close(pf_data_fm);
+	if (pf_data_bt->opened)
+		pf_data_bt->close(pf_data_bt);
+	info->nbr_of_users_active--;
+	kfree(cg2900_audio_sessions[*session]);
+	cg2900_audio_sessions[*session] = NULL;
+finished:
+	mutex_unlock(&info->management_mutex);
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_open);
+
+/**
+ * cg2900_audio_close() - Closes an opened session to the ST-Ericsson CG2900 audio control interface.
+ * @session:	[in_out] Pointer to session identifier to close.
+ *		Will be 0 after this call.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened.
+ *   -EACCES if session has not opened.
+ */
+int cg2900_audio_close(unsigned int *session)
+{
+	int err = 0;
+	struct audio_user *audio_user;
+	struct audio_info *info;
+	struct cg2900_user_data *pf_data_bt;
+	struct cg2900_user_data *pf_data_fm;
+
+	pr_debug("cg2900_audio_close");
+
+	if (!session) {
+		pr_err("NULL pointer supplied");
+		return -EINVAL;
+	}
+
+	audio_user = get_session_user(*session);
+	if (!audio_user) {
+		pr_err("Invalid session ID");
+		return -EINVAL;
+	}
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	mutex_lock(&info->management_mutex);
+
+	pf_data_bt = dev_get_platdata(info->dev_bt);
+	pf_data_fm = dev_get_platdata(info->dev_fm);
+
+	if (!cg2900_audio_sessions[*session]) {
+		dev_err(BT_DEV, "Session %d not opened\n", *session);
+		err = -EACCES;
+		goto err_unlock_mutex;
+	}
+
+	kfree(cg2900_audio_sessions[*session]);
+	cg2900_audio_sessions[*session] = NULL;
+
+	info->nbr_of_users_active--;
+	if (info->nbr_of_users_active == 0) {
+		/* No more sessions open. Close channels */
+		pf_data_fm->close(pf_data_fm);
+		pf_data_bt->close(pf_data_bt);
+		info->state = CLOSED;
+	}
+
+	dev_info(BT_DEV, "Session %d closed\n", *session);
+
+	*session = 0;
+
+err_unlock_mutex:
+	mutex_unlock(&info->management_mutex);
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_close);
+
+/**
+ * cg2900_audio_set_dai_config() -  Sets the Digital Audio Interface configuration.
+ * @session:	Session identifier this call is related to.
+ * @config:	Pointer to the configuration to set.
+ *		Allocated by caller, must not be NULL.
+ *
+ * Sets the Digital Audio Interface (DAI) configuration. The DAI is the external
+ * interface between the combo chip and the platform.
+ * For example the PCM or I2S interface.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened.
+ *   -ENOMEM upon allocation failure.
+ *   -EACCES if trying to set unsupported configuration.
+ *   Errors from @receive_bt_cmd_complete.
+ */
+int cg2900_audio_set_dai_config(unsigned int session,
+				struct cg2900_dai_config *config)
+{
+	int err = 0;
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_set_dai_config session %d", session);
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	/* Different command is used for CG2900 PG1 */
+	if (info->revision == CG2900_CHIP_REV_PG1)
+		err = set_dai_config_pg1(audio_user, config);
+	else
+		err = set_dai_config_pg2(audio_user, config);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_set_dai_config);
+
+/**
+ * cg2900_audio_get_dai_config() - Gets the current Digital Audio Interface configuration.
+ * @session:	Session identifier this call is related to.
+ * @config:	[out] Pointer to the configuration to get.
+ *		Allocated by caller, must not be NULL.
+ *
+ * Gets the current Digital Audio Interface configuration. Currently this method
+ * can only be called after some one has called
+ * cg2900_audio_set_dai_config(), there is today no way of getting
+ * the static settings file parameters from this method.
+ * Note that the @port parameter within @config must be set when calling this
+ * function so that the ST-Ericsson CG2900 Audio driver will know which
+ * configuration to return.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened or configuration has not been set.
+ */
+int cg2900_audio_get_dai_config(unsigned int session,
+				struct cg2900_dai_config *config)
+{
+	int err = 0;
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_get_dai_config session %d", session);
+
+	if (!config) {
+		pr_err("NULL supplied as config structure");
+		return -EINVAL;
+	}
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	/*
+	 * Return DAI configuration based on the received port.
+	 * If port has not been configured return error.
+	 */
+	switch (config->port) {
+	case PORT_0_I2S:
+		mutex_lock(&info->management_mutex);
+		if (info->i2s_config_known)
+			memcpy(&config->conf.i2s,
+			       &info->i2s_config,
+			       sizeof(config->conf.i2s));
+		else
+			err = -EIO;
+		mutex_unlock(&info->management_mutex);
+		break;
+
+	case PORT_1_I2S_PCM:
+		mutex_lock(&info->management_mutex);
+		if (info->i2s_pcm_config_known)
+			memcpy(&config->conf.i2s_pcm,
+			       &info->i2s_pcm_config,
+			       sizeof(config->conf.i2s_pcm));
+		else
+			err = -EIO;
+		mutex_unlock(&info->management_mutex);
+		break;
+
+	default:
+		dev_err(BT_DEV, "Unknown port configuration %d\n",
+			config->port);
+		err = -EIO;
+		break;
+	};
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_get_dai_config);
+
+/**
+ * cg2900_audio_config_endpoint() - Configures one endpoint in the combo chip's audio system.
+ * @session:	Session identifier this call is related to.
+ * @config:	Pointer to the endpoint's configuration structure.
+ *
+ * Configures one endpoint in the combo chip's audio system.
+ * Supported @endpoint_id values are:
+ *  * ENDPOINT_BT_SCO_INOUT
+ *  * ENDPOINT_BT_A2DP_SRC
+ *  * ENDPOINT_FM_RX
+ *  * ENDPOINT_FM_TX
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened.
+ *   -EACCES if supplied cg2900_dai_config struct contains not supported
+ *   endpoint_id.
+ */
+int cg2900_audio_config_endpoint(unsigned int session,
+				 struct cg2900_endpoint_config *config)
+{
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_config_endpoint\n");
+
+	if (!config) {
+		pr_err("NULL supplied as configuration structure");
+		return -EINVAL;
+	}
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	switch (config->endpoint_id) {
+	case ENDPOINT_BT_SCO_INOUT:
+	case ENDPOINT_BT_A2DP_SRC:
+	case ENDPOINT_FM_RX:
+	case ENDPOINT_FM_TX:
+		add_endpoint(config, &info->endpoints);
+		break;
+
+	case ENDPOINT_PORT_0_I2S:
+	case ENDPOINT_PORT_1_I2S_PCM:
+	case ENDPOINT_SLIMBUS_VOICE:
+	case ENDPOINT_SLIMBUS_AUDIO:
+	case ENDPOINT_BT_A2DP_SNK:
+	case ENDPOINT_ANALOG_OUT:
+	case ENDPOINT_DSP_AUDIO_IN:
+	case ENDPOINT_DSP_AUDIO_OUT:
+	case ENDPOINT_DSP_VOICE_IN:
+	case ENDPOINT_DSP_VOICE_OUT:
+	case ENDPOINT_DSP_TONE_IN:
+	case ENDPOINT_BURST_BUFFER_IN:
+	case ENDPOINT_BURST_BUFFER_OUT:
+	case ENDPOINT_MUSIC_DECODER:
+	case ENDPOINT_HCI_AUDIO_IN:
+	default:
+		dev_err(BT_DEV, "Unsupported endpoint_id %d\n",
+			config->endpoint_id);
+		return -EACCES;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_config_endpoint);
+
+static bool is_dai_port(enum cg2900_audio_endpoint_id ep)
+{
+	/* These are the only supported ones */
+	return (ep == ENDPOINT_PORT_0_I2S) || (ep == ENDPOINT_PORT_1_I2S_PCM);
+}
+
+/**
+ * cg2900_audio_start_stream() - Connects two endpoints and starts the audio stream.
+ * @session:		Session identifier this call is related to.
+ * @ep_1:		One of the endpoints, no relation to direction or role.
+ * @ep_2:		The other endpoint, no relation to direction or role.
+ * @stream_handle:	Pointer where to store the stream handle.
+ *			Allocated by caller, must not be NULL.
+ *
+ * Connects two endpoints and starts the audio stream.
+ * Note that the endpoints need to be configured before the stream is started;
+ * DAI endpoints, such as ENDPOINT_PORT_0_I2S, are
+ * configured through @cg2900_audio_set_dai_config() while other
+ * endpoints are configured through @cg2900_audio_config_endpoint().
+ *
+ * Supported @endpoint_id values are:
+ *  * ENDPOINT_PORT_0_I2S
+ *  * ENDPOINT_PORT_1_I2S_PCM
+ *  * ENDPOINT_BT_SCO_INOUT
+ *  * ENDPOINT_FM_RX
+ *  * ENDPOINT_FM_TX
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter or unsupported configuration.
+ *   -EIO if driver has not been opened.
+ *   Errors from @conn_start_i2s_to_fm_rx, @conn_start_i2s_to_fm_tx, and
+ *   @conn_start_pcm_to_sco.
+ */
+int cg2900_audio_start_stream(unsigned int session,
+			      enum cg2900_audio_endpoint_id ep_1,
+			      enum cg2900_audio_endpoint_id ep_2,
+			      unsigned int *stream_handle)
+{
+	int err;
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_start_stream session %d ep_1 %d ep_2 %d",
+		 session, ep_1, ep_2);
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	/* Put digital interface in ep_1 to simplify comparison below */
+	if (!is_dai_port(ep_1)) {
+		/* Swap endpoints */
+		enum cg2900_audio_endpoint_id t = ep_1;
+		ep_1 = ep_2;
+		ep_2 = t;
+	}
+
+	if (ep_1 == ENDPOINT_PORT_1_I2S_PCM && ep_2 == ENDPOINT_BT_SCO_INOUT) {
+		err = conn_start_pcm_to_sco(audio_user, stream_handle);
+	} else if (ep_1 == ENDPOINT_PORT_0_I2S && ep_2 == ENDPOINT_FM_RX) {
+		err = conn_start_i2s_to_fm_rx(audio_user, stream_handle);
+	} else if (ep_1 == ENDPOINT_PORT_0_I2S && ep_2 == ENDPOINT_FM_TX) {
+		err = conn_start_i2s_to_fm_tx(audio_user, stream_handle);
+	} else {
+		dev_err(BT_DEV, "Endpoint config not handled: ep1: %d, "
+			"ep2: %d\n", ep_1, ep_2);
+		err = -EINVAL;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_start_stream);
+
+/**
+ * cg2900_audio_stop_stream() - Stops a stream and disconnects the endpoints.
+ * @session:		Session identifier this call is related to.
+ * @stream_handle:	Handle to the stream to stop.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL upon bad input parameter.
+ *   -EIO if driver has not been opened.
+ */
+int cg2900_audio_stop_stream(unsigned int session, unsigned int stream_handle)
+{
+	struct audio_user *audio_user;
+	struct audio_info *info;
+
+	pr_debug("cg2900_audio_stop_stream handle %d", stream_handle);
+
+	audio_user = get_session_user(session);
+	if (!audio_user)
+		return -EINVAL;
+
+	info = audio_user->info;
+
+	if (info->state != OPENED) {
+		dev_err(BT_DEV, "Audio driver not open\n");
+		return -EIO;
+	}
+
+	return conn_stop_stream(audio_user, stream_handle);
+}
+EXPORT_SYMBOL_GPL(cg2900_audio_stop_stream);
+
+/**
+ * audio_dev_open() - Open char device.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation failed.
+ *   Errors from @cg2900_audio_open.
+ */
+static int audio_dev_open(struct inode *inode, struct file *filp)
+{
+	int err;
+	struct char_dev_info *char_dev_info;
+	int minor;
+	struct audio_info *info = NULL;
+	struct audio_info *tmp;
+	struct list_head *cursor;
+
+	pr_debug("audio_dev_open");
+
+	minor = iminor(inode);
+
+	/* Find the info struct for this file */
+	list_for_each(cursor, &cg2900_audio_devices) {
+		tmp = list_entry(cursor, struct audio_info, list);
+		if (tmp->misc_dev.minor == minor) {
+			info = tmp;
+			break;
+		}
+	}
+	if (!info) {
+		pr_err("Could not identify device in inode");
+		return -EINVAL;
+	}
+
+	/*
+	 * Allocate the char dev info structure. It will be stored inside
+	 * the file pointer and supplied when file_ops are called.
+	 * It's free'd in audio_dev_release.
+	 */
+	char_dev_info = kzalloc(sizeof(*char_dev_info), GFP_KERNEL);
+	if (!char_dev_info) {
+		dev_err(BT_DEV, "Couldn't allocate char_dev_info\n");
+		return -ENOMEM;
+	}
+	filp->private_data = char_dev_info;
+	char_dev_info->info = info;
+	info->filp = filp;
+
+	mutex_init(&char_dev_info->management_mutex);
+	mutex_init(&char_dev_info->rw_mutex);
+	skb_queue_head_init(&char_dev_info->rx_queue);
+
+	mutex_lock(&char_dev_info->management_mutex);
+	err = cg2900_audio_open(&char_dev_info->session, info->dev_bt->parent);
+	mutex_unlock(&char_dev_info->management_mutex);
+	if (err) {
+		dev_err(BT_DEV, "Failed to open CG2900 Audio driver (%d)\n",
+			err);
+		goto error_handling_free_mem;
+	}
+
+	return 0;
+
+error_handling_free_mem:
+	kfree(char_dev_info);
+	filp->private_data = NULL;
+	return err;
+}
+
+/**
+ * audio_dev_release() - Release char device.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   Errors from @cg2900_audio_close.
+ */
+static int audio_dev_release(struct inode *inode, struct file *filp)
+{
+	int err = 0;
+	struct char_dev_info *dev = filp->private_data;
+	struct audio_info *info;
+
+	if (!dev) {
+		pr_err("audio_dev_release: Transport closed");
+		return -EBADF;
+	}
+
+	info = dev->info;
+
+	dev_dbg(BT_DEV, "audio_dev_release\n");
+
+	mutex_lock(&dev->management_mutex);
+	err = cg2900_audio_close(&dev->session);
+	if (err)
+		/*
+		 * Just print the error. Still free the char_dev_info since we
+		 * don't know the filp structure is valid after this call
+		 */
+		dev_err(BT_DEV, "Error %d when closing CG2900 audio driver\n",
+			err);
+
+	mutex_unlock(&dev->management_mutex);
+
+	kfree(dev);
+	filp->private_data = NULL;
+	info->filp = NULL;
+
+	return err;
+}
+
+/**
+ * audio_dev_read() - Return information to the user from last @write call.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Received buffer.
+ * @count:	Size of buffer.
+ * @f_pos:	Position in buffer.
+ *
+ * The audio_dev_read() function returns information from
+ * the last @write call to same char device.
+ * The data is in the following format:
+ *   * OpCode of command for this data
+ *   * Data content (Length of data is determined by the command OpCode, i.e.
+ *     fixed for each command)
+ *
+ * Returns:
+ *   Bytes successfully read (could be 0).
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   -EFAULT if copy_to_user fails.
+ *   -ENOMEM upon allocation failure.
+ */
+static ssize_t audio_dev_read(struct file *filp, char __user *buf, size_t count,
+			      loff_t *f_pos)
+{
+	struct char_dev_info *dev = filp->private_data;
+	struct audio_info *info;
+	unsigned int bytes_to_copy;
+	int err = 0;
+	struct sk_buff *skb;
+
+	if (!dev) {
+		pr_err("audio_dev_read: Transport closed");
+		return -EBADF;
+	}
+
+	info = dev->info;
+
+	dev_dbg(BT_DEV, "audio_dev_read count %d\n", count);
+
+	mutex_lock(&dev->rw_mutex);
+
+	skb = skb_dequeue(&dev->rx_queue);
+	if (!skb) {
+		/* No data to read */
+		bytes_to_copy = 0;
+		goto finished;
+	}
+
+	bytes_to_copy = min(count, (unsigned int)(skb->len));
+
+	err = copy_to_user(buf, skb->data, bytes_to_copy);
+	if (err) {
+		dev_err(BT_DEV, "copy_to_user error %d\n", err);
+		skb_queue_head(&dev->rx_queue, skb);
+		err = -EFAULT;
+		goto error_handling;
+	}
+
+	skb_pull(skb, bytes_to_copy);
+
+	if (skb->len > 0)
+		skb_queue_head(&dev->rx_queue, skb);
+	else
+		kfree_skb(skb);
+
+	goto finished;
+
+error_handling:
+	mutex_unlock(&dev->rw_mutex);
+	return (ssize_t)err;
+finished:
+	mutex_unlock(&dev->rw_mutex);
+	return bytes_to_copy;
+}
+
+/**
+ * audio_dev_write() - Call CG2900 Audio API function.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Write buffer.
+ * @count:	Size of the buffer write.
+ * @f_pos:	Position of buffer.
+ *
+ * audio_dev_write() function executes supplied data and
+ * interprets it as if it was a function call to the CG2900 Audio API.
+ * The data is according to:
+ *   * OpCode (4 bytes, see API).
+ *   * Data according to OpCode (see API). No padding between parameters.
+ *
+ * Returns:
+ *   Bytes successfully written (could be 0). Equals input @count if successful.
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   -EFAULT if copy_from_user fails.
+ *   Error codes from all CG2900 Audio API functions.
+ */
+static ssize_t audio_dev_write(struct file *filp, const char __user *buf,
+			       size_t count, loff_t *f_pos)
+{
+	u8 *rec_data;
+	struct char_dev_info *dev = filp->private_data;
+	struct audio_info *info;
+	int err = 0;
+	int op_code = 0;
+	u8 *curr_data;
+	unsigned int stream_handle;
+	struct cg2900_dai_config dai_config;
+	struct cg2900_endpoint_config ep_config;
+	enum cg2900_audio_endpoint_id ep_1;
+	enum cg2900_audio_endpoint_id ep_2;
+	int bytes_left = count;
+
+	pr_debug("audio_dev_write count %d", count);
+
+	if (!dev) {
+		pr_err("audio_dev_write: Transport closed");
+		return -EBADF;
+	}
+	info = dev->info;
+
+	rec_data = kmalloc(count, GFP_KERNEL);
+	if (!rec_data) {
+		dev_err(BT_DEV, "kmalloc failed (%d bytes)\n", count);
+		return -ENOMEM;
+	}
+
+	mutex_lock(&dev->rw_mutex);
+
+	err = copy_from_user(rec_data, buf, count);
+	if (err) {
+		dev_err(BT_DEV, "copy_from_user failed (%d)\n", err);
+		err = -EFAULT;
+		goto finished_mutex_unlock;
+	}
+
+	/* Initialize temporary data pointer used to traverse the packet */
+	curr_data = rec_data;
+
+	op_code = curr_data[0];
+	/* OpCode is int size to keep data int aligned */
+	curr_data += sizeof(unsigned int);
+	bytes_left -= sizeof(unsigned int);
+
+	switch (op_code) {
+	case CG2900_OPCODE_SET_DAI_CONF:
+		if (bytes_left < sizeof(dai_config)) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_SET_DAI_CONF\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		memcpy(&dai_config, curr_data, sizeof(dai_config));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_SET_DAI_CONF port %d\n",
+			dai_config.port);
+		err = cg2900_audio_set_dai_config(dev->session, &dai_config);
+		break;
+
+	case CG2900_OPCODE_GET_DAI_CONF:
+		if (bytes_left < sizeof(dai_config)) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_GET_DAI_CONF\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		/*
+		 * Only need to copy the port really, but let's copy
+		 * like this for simplicity. It's only test functionality
+		 * after all.
+		 */
+		memcpy(&dai_config, curr_data, sizeof(dai_config));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_GET_DAI_CONF port %d\n",
+			dai_config.port);
+		err = cg2900_audio_get_dai_config(dev->session, &dai_config);
+		if (!err) {
+			int len;
+			struct sk_buff *skb;
+
+			/*
+			 * Command succeeded. Store data so it can be returned
+			 * when calling read.
+			 */
+			len = sizeof(op_code) + sizeof(dai_config);
+			skb = alloc_skb(len, GFP_KERNEL);
+			if (!skb) {
+				dev_err(BT_DEV, "CG2900_OPCODE_GET_DAI_CONF: "
+						"Could not allocate skb\n");
+				err = -ENOMEM;
+				goto finished_mutex_unlock;
+			}
+			memcpy(skb_put(skb, sizeof(op_code)), &op_code,
+			       sizeof(op_code));
+			memcpy(skb_put(skb, sizeof(dai_config)),
+			       &dai_config, sizeof(dai_config));
+			skb_queue_tail(&dev->rx_queue, skb);
+		}
+		break;
+
+	case CG2900_OPCODE_CONFIGURE_ENDPOINT:
+		if (bytes_left < sizeof(ep_config)) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_CONFIGURE_ENDPOINT\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		memcpy(&ep_config, curr_data, sizeof(ep_config));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_CONFIGURE_ENDPOINT ep_id %d\n",
+			ep_config.endpoint_id);
+		err = cg2900_audio_config_endpoint(dev->session, &ep_config);
+		break;
+
+	case CG2900_OPCODE_START_STREAM:
+		if (bytes_left < (sizeof(ep_1) + sizeof(ep_2))) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_START_STREAM\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		memcpy(&ep_1, curr_data, sizeof(ep_1));
+		curr_data += sizeof(ep_1);
+		memcpy(&ep_2, curr_data, sizeof(ep_2));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_START_STREAM ep_1 %d ep_2 %d\n",
+			ep_1, ep_2);
+
+		err = cg2900_audio_start_stream(dev->session,
+			ep_1, ep_2, &stream_handle);
+		if (!err) {
+			int len;
+			struct sk_buff *skb;
+
+			/*
+			 * Command succeeded. Store data so it can be returned
+			 * when calling read.
+			 */
+			len = sizeof(op_code) + sizeof(stream_handle);
+			skb = alloc_skb(len, GFP_KERNEL);
+			if (!skb) {
+				dev_err(BT_DEV, "CG2900_OPCODE_START_STREAM: "
+						"Could not allocate skb\n");
+				err = -ENOMEM;
+				goto finished_mutex_unlock;
+			}
+			memcpy(skb_put(skb, sizeof(op_code)), &op_code,
+			       sizeof(op_code));
+			memcpy(skb_put(skb, sizeof(stream_handle)),
+			       &stream_handle, sizeof(stream_handle));
+			skb_queue_tail(&dev->rx_queue, skb);
+
+			dev_dbg(BT_DEV, "stream_handle %d\n", stream_handle);
+		}
+		break;
+
+	case CG2900_OPCODE_STOP_STREAM:
+		if (bytes_left < sizeof(stream_handle)) {
+			dev_err(BT_DEV, "Not enough data supplied for "
+				"CG2900_OPCODE_STOP_STREAM\n");
+			err = -EINVAL;
+			goto finished_mutex_unlock;
+		}
+		memcpy(&stream_handle, curr_data, sizeof(stream_handle));
+		dev_dbg(BT_DEV, "CG2900_OPCODE_STOP_STREAM stream_handle %d\n",
+			stream_handle);
+		err = cg2900_audio_stop_stream(dev->session, stream_handle);
+		break;
+
+	default:
+		dev_err(BT_DEV, "Received bad op_code %d\n", op_code);
+		break;
+	};
+
+finished_mutex_unlock:
+	kfree(rec_data);
+	mutex_unlock(&dev->rw_mutex);
+
+	if (err)
+		return err;
+	else
+		return count;
+}
+
+/**
+ * audio_dev_poll() - Handle POLL call to the interface.
+ * @filp:	Pointer to the file struct.
+ * @wait:	Poll table supplied to caller.
+ *
+ * This function is used by the User Space application to see if the device is
+ * still open and if there is any data available for reading.
+ *
+ * Returns:
+ *   Mask of current set POLL values.
+ */
+static unsigned int audio_dev_poll(struct file *filp, poll_table *wait)
+{
+	struct char_dev_info *dev = filp->private_data;
+	struct audio_info *info;
+	unsigned int mask = 0;
+
+	if (!dev) {
+		pr_err("audio_dev_poll: Transport closed");
+		return POLLERR | POLLRDHUP;
+	}
+	info = dev->info;
+
+	if (RESET == info->state)
+		mask |= POLLERR | POLLRDHUP | POLLPRI;
+	else
+		/* Unless RESET we can transmit */
+		mask |= POLLOUT;
+
+	if (!skb_queue_empty(&dev->rx_queue))
+		mask |= POLLIN | POLLRDNORM;
+
+	return mask;
+}
+
+static const struct file_operations char_dev_fops = {
+	.open = audio_dev_open,
+	.release = audio_dev_release,
+	.read = audio_dev_read,
+	.write = audio_dev_write,
+	.poll = audio_dev_poll
+};
+
+/**
+ * probe_common() - Register misc device.
+ * @info:	Audio info structure.
+ * @dev:	Current device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   Error codes from misc_register.
+ */
+static int probe_common(struct audio_info *info, struct device *dev)
+{
+	struct audio_cb_info *cb_info;
+	struct cg2900_user_data *pf_data;
+	int err;
+
+	cb_info = kzalloc(sizeof(*cb_info), GFP_KERNEL);
+	if (!cb_info) {
+		dev_err(dev, "Failed to allocate cb_info\n");
+		return -ENOMEM;
+	}
+	init_waitqueue_head(&cb_info->wq);
+	skb_queue_head_init(&cb_info->skb_queue);
+
+	pf_data = dev_get_platdata(dev);
+	cg2900_set_usr(pf_data, cb_info);
+	pf_data->dev = dev;
+	pf_data->read_cb = read_cb;
+	pf_data->reset_cb = reset_cb;
+
+	/* Only register misc device when both devices (BT and FM) are probed */
+	if (!info->dev_bt || !info->dev_fm)
+		return 0;
+
+	/* Prepare and register MISC device */
+	info->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	info->misc_dev.name = NAME;
+	info->misc_dev.fops = &char_dev_fops;
+	info->misc_dev.parent = dev;
+	info->misc_dev.mode = S_IRUGO | S_IWUGO;
+
+	err = misc_register(&info->misc_dev);
+	if (err) {
+		dev_err(dev, "Error %d registering misc dev\n", err);
+		return err;
+	}
+	info->misc_registered = true;
+
+	dev_info(dev, "CG2900 Audio driver started\n");
+	return 0;
+}
+
+/**
+ * cg2900_audio_bt_probe() - Initialize CG2900 BT audio resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   -EEXIST if device has already been started.
+ *   Error codes from probe_common.
+ */
+static int __devinit cg2900_audio_bt_probe(struct platform_device *pdev)
+{
+	int err;
+	struct audio_info *info;
+
+	dev_dbg(&pdev->dev, "cg2900_audio_bt_probe\n");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->dev_bt = &pdev->dev;
+	dev_set_drvdata(&pdev->dev, info);
+
+	err = probe_common(info, &pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "Could not probe audio BT (%d)\n", err);
+		dev_set_drvdata(&pdev->dev, NULL);
+		device_removed(info);
+	}
+
+	return err;
+}
+
+/**
+ * cg2900_audio_bt_probe() - Initialize CG2900 FM audio resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -ENOMEM if allocation fails.
+ *   -EEXIST if device has already been started.
+ *   Error codes from probe_common.
+ */
+static int __devinit cg2900_audio_fm_probe(struct platform_device *pdev)
+{
+	int err;
+	struct audio_info *info;
+
+	dev_dbg(&pdev->dev, "cg2900_audio_fm_probe\n");
+
+	info = get_info(&pdev->dev);
+	if (IS_ERR(info))
+		return PTR_ERR(info);
+
+	info->dev_fm = &pdev->dev;
+	dev_set_drvdata(&pdev->dev, info);
+
+	err = probe_common(info, &pdev->dev);
+	if (err) {
+		dev_err(&pdev->dev, "Could not probe audio FM (%d)\n", err);
+		dev_set_drvdata(&pdev->dev, NULL);
+		device_removed(info);
+	}
+
+	return err;
+}
+
+/**
+ * common_remove() - Dergister misc device.
+ * @info:	Audio info structure.
+ * @dev:	Current device.
+ *
+ * Returns:
+ *   0 if success.
+ *   Error codes from misc_deregister.
+ */
+static int common_remove(struct audio_info *info, struct device *dev)
+{
+	int err;
+	struct audio_cb_info *cb_info;
+	struct cg2900_user_data *pf_data;
+
+	pf_data = dev_get_platdata(dev);
+	cb_info = cg2900_get_usr(pf_data);
+	skb_queue_purge(&cb_info->skb_queue);
+	wake_up_all(&cb_info->wq);
+	kfree(cb_info);
+
+	if (!info->misc_registered)
+		return 0;
+
+	err = misc_deregister(&info->misc_dev);
+	if (err)
+		dev_err(dev, "Error %d deregistering misc dev\n", err);
+	info->misc_registered = false;
+
+	if (info->filp)
+		info->filp->private_data = NULL;
+
+	dev_info(dev, "CG2900 Audio driver removed\n");
+	return err;
+}
+
+/**
+ * cg2900_audio_bt_remove() - Release CG2900 audio resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   Error codes from common_remove.
+ */
+static int __devexit cg2900_audio_bt_remove(struct platform_device *pdev)
+{
+	int err;
+	struct audio_info *info;
+
+	dev_dbg(&pdev->dev, "cg2900_audio_bt_remove\n");
+
+	info = dev_get_drvdata(&pdev->dev);
+
+	info->dev_bt = NULL;
+
+	err = common_remove(info, &pdev->dev);
+	if (err)
+		dev_err(&pdev->dev,
+			"cg2900_audio_bt_remove:common_remove failed\n");
+
+	device_removed(info);
+
+	return 0;
+}
+
+/**
+ * cg2900_audio_fm_remove() - Release CG2900 audio resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   Error codes from common_remove.
+ */
+static int __devexit cg2900_audio_fm_remove(struct platform_device *pdev)
+{
+	int err;
+	struct audio_info *info;
+
+	dev_dbg(&pdev->dev, "cg2900_audio_fm_remove\n");
+
+	info = dev_get_drvdata(&pdev->dev);
+
+	info->dev_fm = NULL;
+
+	err = common_remove(info, &pdev->dev);
+	if (err)
+		dev_err(&pdev->dev,
+			"cg2900_audio_fm_remove:common_remove failed\n");
+
+	device_removed(info);
+
+	return 0;
+}
+
+static struct platform_driver cg2900_audio_bt_driver = {
+	.driver = {
+		.name	= "cg2900-audiobt",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_audio_bt_probe,
+	.remove	= __devexit_p(cg2900_audio_bt_remove),
+};
+
+static struct platform_driver cg2900_audio_fm_driver = {
+	.driver = {
+		.name	= "cg2900-audiofm",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_audio_fm_probe,
+	.remove	= __devexit_p(cg2900_audio_fm_remove),
+};
+
+/**
+ * cg2900_audio_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_audio_init(void)
+{
+	int err;
+
+	pr_debug("cg2900_audio_init");
+
+	err = platform_driver_register(&cg2900_audio_bt_driver);
+	if (err)
+		return err;
+	return platform_driver_register(&cg2900_audio_fm_driver);
+}
+
+/**
+ * cg2900_audio_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_audio_exit(void)
+{
+	pr_debug("cg2900_audio_exit");
+	platform_driver_unregister(&cg2900_audio_fm_driver);
+	platform_driver_unregister(&cg2900_audio_bt_driver);
+}
+
+module_init(cg2900_audio_init);
+module_exit(cg2900_audio_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_AUTHOR("Kjell Andersson ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux Bluetooth Audio ST-Ericsson controller");
diff --git a/drivers/staging/cg2900/mfd/cg2900_char_devices.c b/drivers/staging/cg2900/mfd/cg2900_char_devices.c
new file mode 100644
index 00000000000..81d230227db
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_char_devices.c
@@ -0,0 +1,719 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson connectivity controller.
+ */
+#define NAME					"cg2900_char_dev"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <linux/compiler.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/mfd/core.h>
+
+#include "cg2900.h"
+#include "cg2900_core.h"
+
+#define MAIN_DEV				(dev->dev)
+
+/**
+ * struct char_dev_user - Stores device information.
+ * @dev:		Current device.
+ * @miscdev:		Registered device struct.
+ * @filp:		Current file pointer.
+ * @name:		Name of device.
+ * @rx_queue:		Data queue.
+ * @rx_wait_queue:	Wait queue.
+ * @reset_wait_queue:	Reset Wait queue.
+ * @read_mutex:		Read mutex.
+ * @write_mutex:	Write mutex.
+ * @list:		List header for inserting into device list.
+ */
+struct char_dev_user {
+	struct device		*dev;
+	struct miscdevice	miscdev;
+	struct file		*filp;
+	char			*name;
+	struct sk_buff_head	rx_queue;
+	wait_queue_head_t	rx_wait_queue;
+	wait_queue_head_t	reset_wait_queue;
+	struct mutex		read_mutex;
+	struct mutex		write_mutex;
+	struct list_head	list;
+};
+
+/**
+ * struct char_info - Stores all current users.
+ * @open_mutex:	Open mutex (used for both open and release).
+ * @man_mutex:	Management mutex.
+ * @dev_users:	List of char dev users.
+ */
+struct char_info {
+	struct mutex		open_mutex;
+	struct mutex		man_mutex;
+	struct list_head	dev_users;
+};
+
+static struct char_info *char_info;
+
+/**
+ * char_dev_read_cb() - Handle data received from controller.
+ * @dev:	Device receiving data.
+ * @skb:	Buffer with data coming from controller.
+ *
+ * The char_dev_read_cb() function handles data received from the CG2900 driver.
+ */
+static void char_dev_read_cb(struct cg2900_user_data *dev, struct sk_buff *skb)
+{
+	struct char_dev_user *char_dev = dev_get_drvdata(dev->dev);
+
+	dev_dbg(dev->dev, "char_dev_read_cb len %d\n", skb->len);
+
+	skb_queue_tail(&char_dev->rx_queue, skb);
+
+	wake_up_interruptible(&char_dev->rx_wait_queue);
+}
+
+/**
+ * char_dev_reset_cb() - Handle reset from controller.
+ * @dev:	Device resetting.
+ *
+ * The char_dev_reset_cb() function handles reset from the CG2900 driver.
+ */
+static void char_dev_reset_cb(struct cg2900_user_data *dev)
+{
+	struct char_dev_user *char_dev = dev_get_drvdata(dev->dev);
+
+	dev_dbg(dev->dev, "char_dev_reset_cb\n");
+
+	wake_up_interruptible(&char_dev->rx_wait_queue);
+	wake_up_interruptible(&char_dev->reset_wait_queue);
+}
+
+/**
+ * char_dev_open() - Open char device.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * The char_dev_open() function opens the char device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if device cannot be found in device list.
+ *   Error codes from cg2900->open.
+ */
+static int char_dev_open(struct inode *inode, struct file *filp)
+{
+	int err;
+	int minor;
+	struct char_dev_user *dev = NULL;
+	struct char_dev_user *tmp;
+	struct list_head *cursor;
+	struct cg2900_user_data *user;
+
+	mutex_lock(&char_info->open_mutex);
+
+	minor = iminor(inode);
+
+	/* Find the device for this file */
+	mutex_lock(&char_info->man_mutex);
+	list_for_each(cursor, &char_info->dev_users) {
+		tmp = list_entry(cursor, struct char_dev_user, list);
+		if (tmp->miscdev.minor == minor) {
+			dev = tmp;
+			break;
+		}
+	}
+	mutex_unlock(&char_info->man_mutex);
+	if (!dev) {
+		pr_err("Could not identify device in inode");
+		err = -EINVAL;
+		goto error_handling;
+	}
+
+	filp->private_data = dev;
+	dev->filp = filp;
+	user = dev_get_platdata(dev->dev);
+
+	/* First initiate wait queues for this device. */
+	init_waitqueue_head(&dev->rx_wait_queue);
+	init_waitqueue_head(&dev->reset_wait_queue);
+
+	/* Register to CG2900 Driver */
+	err = user->open(user);
+	if (err) {
+		dev_err(MAIN_DEV,
+			"Couldn't register to CG2900 for H:4 channel %s\n",
+			dev->name);
+		goto error_handling;
+	}
+	dev_info(MAIN_DEV, "char_dev %s opened\n", dev->name);
+
+error_handling:
+	mutex_unlock(&char_info->open_mutex);
+	return err;
+}
+
+/**
+ * char_dev_release() - Release char device.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * The char_dev_release() function release the char device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EBADF if NULL pointer was supplied in private data.
+ */
+static int char_dev_release(struct inode *inode, struct file *filp)
+{
+	int err = 0;
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+
+	pr_debug("char_dev_release");
+
+	if (!dev) {
+		pr_err("char_dev_release: Calling with NULL pointer");
+		return -EBADF;
+	}
+
+	mutex_lock(&char_info->open_mutex);
+	mutex_lock(&dev->read_mutex);
+	mutex_lock(&dev->write_mutex);
+
+	user = dev_get_platdata(dev->dev);
+	if (user->opened)
+		user->close(user);
+
+	dev_info(MAIN_DEV, "char_dev %s closed\n", dev->name);
+
+	filp->private_data = NULL;
+	dev->filp = NULL;
+	wake_up_interruptible(&dev->rx_wait_queue);
+	wake_up_interruptible(&dev->reset_wait_queue);
+
+	/* Purge the queue since the device is closed now */
+	skb_queue_purge(&dev->rx_queue);
+
+	mutex_unlock(&dev->write_mutex);
+	mutex_unlock(&dev->read_mutex);
+	mutex_unlock(&char_info->open_mutex);
+
+	return err;
+}
+
+/**
+ * char_dev_read() - Queue and copy buffer to user.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Received buffer.
+ * @count:	Size of buffer.
+ * @f_pos:	Position in buffer.
+ *
+ * The char_dev_read() function queues and copy the received buffer to
+ * the user space char device. If no data is available this function will block.
+ *
+ * Returns:
+ *   Bytes successfully read (could be 0).
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   -EFAULT if copy_to_user fails.
+ *   Error codes from wait_event_interruptible.
+ */
+static ssize_t char_dev_read(struct file *filp, char __user *buf, size_t count,
+			     loff_t *f_pos)
+{
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+	struct sk_buff *skb;
+	int bytes_to_copy;
+	int err = 0;
+
+	pr_debug("char_dev_read");
+
+	if (!dev) {
+		pr_err("char_dev_read: Calling with NULL pointer");
+		return -EBADF;
+	}
+	mutex_lock(&dev->read_mutex);
+
+	user = dev_get_platdata(dev->dev);
+
+	if (user->opened && skb_queue_empty(&dev->rx_queue)) {
+		err = wait_event_interruptible(dev->rx_wait_queue,
+				(!(skb_queue_empty(&dev->rx_queue))) ||
+				!user->opened);
+		if (err) {
+			dev_err(MAIN_DEV, "Failed to wait for event\n");
+			goto error_handling;
+		}
+	}
+
+	if (!user->opened) {
+		dev_err(MAIN_DEV, "Channel has been closed\n");
+		err = -EBADF;
+		goto error_handling;
+	}
+
+	skb = skb_dequeue(&dev->rx_queue);
+	if (!skb) {
+		dev_dbg(MAIN_DEV,
+			"skb queue is empty - return with zero bytes\n");
+		bytes_to_copy = 0;
+		goto finished;
+	}
+
+	bytes_to_copy = min(count, skb->len);
+
+	err = copy_to_user(buf, skb->data, bytes_to_copy);
+	if (err) {
+		dev_err(MAIN_DEV, "Error %d from copy_to_user\n", err);
+		skb_queue_head(&dev->rx_queue, skb);
+		err = -EFAULT;
+		goto error_handling;
+	}
+
+	skb_pull(skb, bytes_to_copy);
+
+	if (skb->len > 0)
+		skb_queue_head(&dev->rx_queue, skb);
+	else
+		kfree_skb(skb);
+
+	goto finished;
+
+error_handling:
+	mutex_unlock(&dev->read_mutex);
+	return (ssize_t)err;
+finished:
+	mutex_unlock(&dev->read_mutex);
+	return bytes_to_copy;
+}
+
+/**
+ * char_dev_write() - Copy buffer from user and write to CG2900 driver.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Write buffer.
+ * @count:	Size of the buffer write.
+ * @f_pos:	Position of buffer.
+ *
+ * Returns:
+ *   Bytes successfully written (could be 0).
+ *   -EBADF if NULL pointer was supplied in private data.
+ *   -EFAULT if copy_from_user fails.
+ */
+static ssize_t char_dev_write(struct file *filp, const char __user *buf,
+			      size_t count, loff_t *f_pos)
+{
+	struct sk_buff *skb;
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+	int err = 0;
+
+	pr_debug("char_dev_write");
+
+	if (!dev) {
+		pr_err("char_dev_write: Calling with NULL pointer");
+		return -EBADF;
+	}
+
+	user = dev_get_platdata(dev->dev);
+	if (!user->opened) {
+		dev_err(MAIN_DEV, "char_dev_write: Channel not opened\n");
+		return -EACCES;
+	}
+
+	mutex_lock(&dev->write_mutex);
+
+	skb = user->alloc_skb(count, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(MAIN_DEV, "Couldn't allocate sk_buff with length %d\n",
+			count);
+		goto error_handling;
+	}
+
+	err = copy_from_user(skb_put(skb, count), buf, count);
+	if (err) {
+		dev_err(MAIN_DEV, "Error %d from copy_from_user\n", err);
+		kfree_skb(skb);
+		err = -EFAULT;
+		goto error_handling;
+	}
+
+	err = user->write(user, skb);
+	if (err) {
+		dev_err(MAIN_DEV, "cg2900_write failed (%d)\n", err);
+		kfree_skb(skb);
+		goto error_handling;
+	}
+
+	mutex_unlock(&dev->write_mutex);
+	return count;
+
+error_handling:
+	mutex_unlock(&dev->write_mutex);
+	return err;
+}
+
+/**
+ * char_dev_unlocked_ioctl() - Handle IOCTL call to the interface.
+ * @filp:	Pointer to the file struct.
+ * @cmd:	IOCTL command.
+ * @arg:	IOCTL argument.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if supplied cmd is not supported.
+ *   For cmd CG2900_CHAR_DEV_IOCTL_CHECK4RESET 0x01 is returned if device is
+ *   reset and 0x02 is returned if device is closed.
+ */
+static long char_dev_unlocked_ioctl(struct file *filp, unsigned int cmd,
+				    unsigned long arg)
+{
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+	struct cg2900_rev_data rev_data;
+	int err = 0;
+	int ret_val;
+	void __user *user_arg = (void __user *)arg;
+
+	if (!dev) {
+		pr_err("char_dev_unlocked_ioctl: Calling with NULL pointer");
+		return -EBADF;
+	}
+
+	dev_dbg(dev->dev, "char_dev_unlocked_ioctl for %s\n"
+		"\tDIR: %d\n"
+		"\tTYPE: %d\n"
+		"\tNR: %d\n"
+		"\tSIZE: %d",
+		dev->name, _IOC_DIR(cmd), _IOC_TYPE(cmd), _IOC_NR(cmd),
+		_IOC_SIZE(cmd));
+
+	user = dev_get_platdata(dev->dev);
+
+	switch (cmd) {
+	case CG2900_CHAR_DEV_IOCTL_RESET:
+		if (!user->opened)
+			return -EACCES;
+		dev_dbg(MAIN_DEV, "ioctl reset command for device %s\n",
+			dev->name);
+		err = user->reset(user);
+		break;
+
+	case CG2900_CHAR_DEV_IOCTL_CHECK4RESET:
+		if (user->opened)
+			ret_val = CG2900_CHAR_DEV_IOCTL_EVENT_IDLE;
+		else
+			ret_val = CG2900_CHAR_DEV_IOCTL_EVENT_RESET;
+
+		dev_dbg(MAIN_DEV, "ioctl check for reset command for device %s",
+			dev->name);
+
+		err = copy_to_user(user_arg, &ret_val, sizeof(ret_val));
+		if (err) {
+			dev_err(MAIN_DEV,
+				"Error %d from copy_to_user for reset\n", err);
+			return -EFAULT;
+		}
+		break;
+
+	case CG2900_CHAR_DEV_IOCTL_GET_REVISION:
+		if (!user->get_local_revision(user, &rev_data)) {
+			dev_err(MAIN_DEV, "No revision data available\n");
+			return -EIO;
+		}
+		dev_dbg(MAIN_DEV, "ioctl check for local revision info\n"
+			"\trevision 0x%04X\n"
+			"\tsub_version 0x%04X\n",
+			rev_data.revision, rev_data.sub_version);
+		err = copy_to_user(user_arg, &rev_data, sizeof(rev_data));
+		if (err) {
+			dev_err(MAIN_DEV,
+				"Error %d from copy_to_user for "
+				"revision\n", err);
+			return -EFAULT;
+		}
+		break;
+
+	default:
+		dev_err(MAIN_DEV, "Unknown ioctl command %08X\n", cmd);
+		err = -EINVAL;
+		break;
+	};
+
+	return err;
+}
+
+/**
+ * char_dev_poll() - Handle POLL call to the interface.
+ * @filp:	Pointer to the file struct.
+ * @wait:	Poll table supplied to caller.
+ *
+ * Returns:
+ *   Mask of current set POLL values
+ */
+static unsigned int char_dev_poll(struct file *filp, poll_table *wait)
+{
+	struct char_dev_user *dev = filp->private_data;
+	struct cg2900_user_data *user;
+	unsigned int mask = 0;
+
+	if (!dev) {
+		pr_debug("char_dev_poll: Device not open");
+		return POLLERR | POLLRDHUP;
+	}
+
+	user = dev_get_platdata(dev->dev);
+
+	poll_wait(filp, &dev->reset_wait_queue, wait);
+	poll_wait(filp, &dev->rx_wait_queue, wait);
+
+	if (!user->opened)
+		mask |= POLLERR | POLLRDHUP | POLLPRI;
+	else
+		mask |= POLLOUT; /* We can TX unless there is an error */
+
+	if (!(skb_queue_empty(&dev->rx_queue)))
+		mask |= POLLIN | POLLRDNORM;
+
+	return mask;
+}
+
+/*
+ * struct char_dev_fops - Char devices file operations.
+ * @read:		Function that reads from the char device.
+ * @write:		Function that writes to the char device.
+ * @unlocked_ioctl:	Function that performs IO operations with
+ *			the char device.
+ * @poll:		Function that checks if there are possible operations
+ *			with the char device.
+ * @open:		Function that opens the char device.
+ * @release:		Function that release the char device.
+ */
+static const struct file_operations char_dev_fops = {
+	.read		= char_dev_read,
+	.write		= char_dev_write,
+	.unlocked_ioctl	= char_dev_unlocked_ioctl,
+	.poll		= char_dev_poll,
+	.open		= char_dev_open,
+	.release	= char_dev_release
+};
+
+/**
+ * remove_dev() - Remove char device structure for device.
+ * @dev_usr:	Char device user.
+ *
+ * The remove_dev() function releases the char_dev structure for this device.
+ */
+static void remove_dev(struct char_dev_user *dev_usr)
+{
+	if (!dev_usr)
+		return;
+
+	dev_dbg(dev_usr->dev,
+		"Removing char device %s with major %d and minor %d\n",
+		dev_usr->name,
+		MAJOR(dev_usr->miscdev.this_device->devt),
+		MINOR(dev_usr->miscdev.this_device->devt));
+
+	skb_queue_purge(&dev_usr->rx_queue);
+
+	mutex_destroy(&dev_usr->read_mutex);
+	mutex_destroy(&dev_usr->write_mutex);
+
+	dev_usr->dev = NULL;
+	if (dev_usr->filp)
+		dev_usr->filp->private_data = NULL;
+
+	/* Remove device node in file system. */
+	misc_deregister(&dev_usr->miscdev);
+	kfree(dev_usr);
+}
+
+/**
+ * cg2900_char_probe() - Initialize char device module.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM if allocation fails.
+ *   -EACCES if device already have been initiated.
+ */
+static int __devinit cg2900_char_probe(struct platform_device *pdev)
+{
+	int err = 0;
+	struct char_dev_user *dev_usr;
+	struct cg2900_user_data *user;
+	struct device *dev = &pdev->dev;
+
+	dev_dbg(&pdev->dev, "cg2900_char_probe\n");
+
+	user = dev_get_platdata(dev);
+	user->dev = dev;
+	user->read_cb = char_dev_read_cb;
+	user->reset_cb = char_dev_reset_cb;
+
+	dev_usr = kzalloc(sizeof(*dev_usr), GFP_KERNEL);
+	if (!dev_usr) {
+		dev_err(&pdev->dev, "Couldn't allocate dev_usr\n");
+		return -ENOMEM;
+	}
+
+	dev_set_drvdata(&pdev->dev, dev_usr);
+	dev_usr->dev = &pdev->dev;
+
+	/* Store device name */
+	dev_usr->name = user->channel_data.char_dev_name;
+
+	/* Prepare miscdevice struct before registering the device */
+	dev_usr->miscdev.minor = MISC_DYNAMIC_MINOR;
+	dev_usr->miscdev.name = dev_usr->name;
+	dev_usr->miscdev.nodename = dev_usr->name;
+	dev_usr->miscdev.fops = &char_dev_fops;
+	dev_usr->miscdev.parent = &pdev->dev;
+	dev_usr->miscdev.mode = S_IRUGO | S_IWUGO;
+
+	err = misc_register(&dev_usr->miscdev);
+	if (err) {
+		dev_err(&pdev->dev, "Error %d registering misc dev\n", err);
+		goto err_free_usr;
+	}
+
+	dev_dbg(&pdev->dev, "Added char device %s with major %d and minor %d\n",
+		dev_usr->name, MAJOR(dev_usr->miscdev.this_device->devt),
+		MINOR(dev_usr->miscdev.this_device->devt));
+
+	mutex_init(&dev_usr->read_mutex);
+	mutex_init(&dev_usr->write_mutex);
+
+	skb_queue_head_init(&dev_usr->rx_queue);
+
+	mutex_lock(&char_info->man_mutex);
+	list_add_tail(&dev_usr->list, &char_info->dev_users);
+	mutex_unlock(&char_info->man_mutex);
+
+	return 0;
+
+err_free_usr:
+	kfree(dev_usr);
+	dev_set_drvdata(&pdev->dev, NULL);
+	return err;
+}
+
+/**
+ * cg2900_char_remove() - Release the char device module.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success (always success).
+ */
+static int __devexit cg2900_char_remove(struct platform_device *pdev)
+{
+	struct list_head *cursor, *next;
+	struct char_dev_user *tmp;
+	struct char_dev_user *user;
+
+	dev_dbg(&pdev->dev, "cg2900_char_remove\n");
+
+	user = dev_get_drvdata(&pdev->dev);
+
+	mutex_lock(&char_info->man_mutex);
+	list_for_each_safe(cursor, next, &char_info->dev_users) {
+		tmp = list_entry(cursor, struct char_dev_user, list);
+		if (tmp == user) {
+			list_del(cursor);
+			remove_dev(tmp);
+			dev_set_drvdata(&pdev->dev, NULL);
+			break;
+		}
+	}
+	mutex_unlock(&char_info->man_mutex);
+	return 0;
+}
+
+static struct platform_driver cg2900_char_driver = {
+	.driver = {
+		.name	= "cg2900-chardev",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_char_probe,
+	.remove	= __devexit_p(cg2900_char_remove),
+};
+
+/**
+ * cg2900_char_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_char_init(void)
+{
+	pr_debug("cg2900_char_init");
+
+	/* Initialize private data. */
+	char_info = kzalloc(sizeof(*char_info), GFP_ATOMIC);
+	if (!char_info) {
+		pr_err("Could not alloc char_info struct");
+		return -ENOMEM;
+	}
+
+	mutex_init(&char_info->open_mutex);
+	mutex_init(&char_info->man_mutex);
+	INIT_LIST_HEAD(&char_info->dev_users);
+
+	return platform_driver_register(&cg2900_char_driver);
+}
+
+/**
+ * cg2900_char_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_char_exit(void)
+{
+	struct list_head *cursor, *next;
+	struct char_dev_user *tmp;
+
+	pr_debug("cg2900_char_exit");
+
+	platform_driver_unregister(&cg2900_char_driver);
+
+	if (!char_info)
+		return;
+
+	list_for_each_safe(cursor, next, &char_info->dev_users) {
+		tmp = list_entry(cursor, struct char_dev_user, list);
+		list_del(cursor);
+		remove_dev(tmp);
+	}
+
+	mutex_destroy(&char_info->open_mutex);
+	mutex_destroy(&char_info->man_mutex);
+
+	kfree(char_info);
+	char_info = NULL;
+}
+
+module_init(cg2900_char_init);
+module_exit(cg2900_char_exit);
+
+MODULE_AUTHOR("Henrik Possung ST-Ericsson");
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("ST-Ericsson CG2900 Char Devices Driver");
diff --git a/drivers/staging/cg2900/mfd/cg2900_chip.c b/drivers/staging/cg2900/mfd/cg2900_chip.c
new file mode 100644
index 00000000000..2ae418c9f03
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_chip.c
@@ -0,0 +1,3791 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * Hemant Gupta (hemant.gupta@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+#define NAME					"cg2900_chip"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/gfp.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/limits.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/stat.h>
+#include <linux/string.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/core.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+#include "cg2900_chip.h"
+#include "cg2900_core.h"
+#include "cg2900_lib.h"
+
+#ifndef MAX
+#define MAX(x, y) (((x) > (y)) ? (x) : (y))
+#endif
+
+#define MAIN_DEV				(main_info->dev)
+#define BOOT_DEV				(info->user_in_charge->dev)
+
+#define WQ_NAME					"cg2900_chip_wq"
+
+/*
+ * After waiting the first 500 ms we should just try to get the selftest results
+ * for another number of poll attempts
+ */
+#define MAX_NBR_OF_POLLS			50
+
+#define LINE_TOGGLE_DETECT_TIMEOUT		50	/* ms */
+#define CHIP_READY_TIMEOUT			100	/* ms */
+#define CHIP_STARTUP_TIMEOUT			15000	/* ms */
+#define CHIP_SHUTDOWN_TIMEOUT			15000	/* ms */
+#define POWER_SW_OFF_WAIT			500	/* ms */
+#define SELFTEST_INITIAL			500	/* ms */
+#define SELFTEST_POLLING			20	/* ms */
+
+/** CHANNEL_BT_CMD - Bluetooth HCI H:4 channel
+ * for Bluetooth commands in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_CMD				0x01
+
+/** CHANNEL_BT_ACL - Bluetooth HCI H:4 channel
+ * for Bluetooth ACL data in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_ACL				0x02
+
+/** CHANNEL_BT_EVT - Bluetooth HCI H:4 channel
+ * for Bluetooth events in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_EVT				0x04
+
+/** CHANNEL_NFC - Bluetooth HCI H:4 channel
+ * for NFC in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_NFC			0x05
+
+/** CHANNEL_FM_RADIO - Bluetooth HCI H:4 channel
+ * for FM radio in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_FM_RADIO			0x08
+
+/** CHANNEL_GNSS - Bluetooth HCI H:4 channel
+ * for GNSS in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_GNSS				0x09
+
+/** CHANNEL_DEBUG - Bluetooth HCI H:4 channel
+ * for internal debug data in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_DEBUG				0x0B
+
+/** CHANNEL_STE_TOOLS - Bluetooth HCI H:4 channel
+ * for development tools data in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_STE_TOOLS			0x0D
+
+/** CHANNEL_HCI_LOGGER - Bluetooth HCI H:4 channel
+ * for logging all transmitted H4 packets (on all channels).
+ */
+#define CHANNEL_HCI_LOGGER			0xFA
+
+/** CHANNEL_CORE - Bluetooth HCI H:4 channel
+ * for user space control of the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_CORE				0xFD
+
+/** CHANNEL_HCI_RAW - Bluetooth HCI H:4 channel
+ * for user space read/write on the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_HCI_RAW				0xFE
+
+/** CG2900_BT_CMD - Bluetooth HCI H4 channel for Bluetooth commands.
+ */
+#define CG2900_BT_CMD				"cg2900_bt_cmd"
+
+/** CG2900_BT_ACL - Bluetooth HCI H4 channel for Bluetooth ACL data.
+ */
+#define CG2900_BT_ACL				"cg2900_bt_acl"
+
+/** CG2900_BT_EVT - Bluetooth HCI H4 channel for Bluetooth events.
+ */
+#define CG2900_BT_EVT				"cg2900_bt_evt"
+
+/** CG2900_NFC - Bluetooth HCI H4 channel for NFC.
+ */
+#define CG2900_NFC				"cg2900_nfc"
+
+/** CG2900_FM_RADIO - Bluetooth HCI H4 channel for FM radio.
+ */
+#define CG2900_FM_RADIO				"cg2900_fm_radio"
+
+/** CG2900_GNSS - Bluetooth HCI H4 channel for GNSS.
+ */
+#define CG2900_GNSS				"cg2900_gnss"
+
+/** CG2900_DEBUG - Bluetooth HCI H4 channel for internal debug data.
+ */
+#define CG2900_DEBUG				"cg2900_debug"
+
+/** CG2900_STE_TOOLS - Bluetooth HCI H4 channel for development tools data.
+ */
+#define CG2900_STE_TOOLS			"cg2900_ste_tools"
+
+/** CG2900_HCI_LOGGER - BT channel for logging all transmitted H4 packets.
+ * Data read is copy of all data transferred on the other channels.
+ * Only write allowed is configuration of the HCI Logger.
+ */
+#define CG2900_HCI_LOGGER			"cg2900_hci_logger"
+
+/** CG2900_BT_AUDIO - HCI Channel for BT audio configuration commands.
+ * Maps to Bluetooth command and event channels.
+ */
+#define CG2900_BT_AUDIO				"cg2900_bt_audio"
+
+/** CG2900_FM_AUDIO - HCI channel for FM audio configuration commands.
+ * Maps to FM Radio channel.
+ */
+#define CG2900_FM_AUDIO				"cg2900_fm_audio"
+
+/** CG2900_CORE- Channel for keeping ST-Ericsson CG2900 enabled.
+ * Opening this channel forces the chip to stay powered.
+ * No data can be written to or read from this channel.
+ */
+#define CG2900_CORE				"cg2900_core"
+
+/** CG2900_HCI_RAW - Channel for HCI RAW data exchange.
+ * Opening this channel will not allow any others HCI Channels
+ * to be opened except Logger Channel.
+ */
+#define CG2900_HCI_RAW				"cg2900_hci_raw"
+
+/**
+ * enum main_state - Main-state for CG2900 driver.
+ * @CG2900_INIT:	CG2900 initializing.
+ * @CG2900_IDLE:	No user registered to CG2900 driver.
+ * @CG2900_BOOTING:	CG2900 booting after first user is registered.
+ * @CG2900_CLOSING:	CG2900 closing after last user has deregistered.
+ * @CG2900_RESETING:	CG2900 reset requested.
+ * @CG2900_ACTIVE:	CG2900 up and running with at least one user.
+ * @CG2900_ACTIVE_BEFORE_SELFTEST: CG2900 up and running before
+ *		BT self test is run.
+ */
+enum main_state {
+	CG2900_INIT,
+	CG2900_IDLE,
+	CG2900_BOOTING,
+	CG2900_CLOSING,
+	CG2900_RESETING,
+	CG2900_ACTIVE,
+	CG2900_ACTIVE_BEFORE_SELFTEST
+};
+
+/**
+ * enum boot_state - BOOT-state for CG2900 chip driver.
+ * @BOOT_NOT_STARTED:			Boot has not yet started.
+ * @BOOT_SEND_BD_ADDRESS:		VS Store In FS command with BD address
+ *					has been sent.
+ * @BOOT_GET_FILES_TO_LOAD:		CG2900 chip driver is retrieving file to
+ *					load.
+ * @BOOT_DOWNLOAD_PATCH:		CG2900 chip driver is downloading
+ *					patches.
+ * @BOOT_ACTIVATE_PATCHES_AND_SETTINGS:	CG2900 chip driver is activating patches
+ *					and settings.
+ * @BOOT_READ_SELFTEST_RESULT:		CG2900 is performing selftests that
+ *					shall be read out.
+ * @BOOT_DISABLE_BT:			Disable BT Core.
+ * @BOOT_READY:				CG2900 chip driver boot is ready.
+ * @BOOT_FAILED:			CG2900 chip driver boot failed.
+ */
+enum boot_state {
+	BOOT_NOT_STARTED,
+	BOOT_SEND_BD_ADDRESS,
+	BOOT_GET_FILES_TO_LOAD,
+	BOOT_DOWNLOAD_PATCH,
+	BOOT_ACTIVATE_PATCHES_AND_SETTINGS,
+	BOOT_READ_SELFTEST_RESULT,
+	BOOT_DISABLE_BT,
+	BOOT_READY,
+	BOOT_FAILED
+};
+
+/**
+ * enum closing_state - CLOSING-state for CG2900 chip driver.
+ * @CLOSING_RESET:		HCI RESET_CMD has been sent.
+ * @CLOSING_POWER_SWITCH_OFF:	HCI VS_POWER_SWITCH_OFF command has been sent.
+ * @CLOSING_SHUT_DOWN:		We have now shut down the chip.
+ */
+enum closing_state {
+	CLOSING_RESET,
+	CLOSING_POWER_SWITCH_OFF,
+	CLOSING_SHUT_DOWN
+};
+
+/**
+ * enum file_load_state - BOOT_FILE_LOAD-state for CG2900 chip driver.
+ * @FILE_LOAD_GET_PATCH:		Loading patches.
+ * @FILE_LOAD_GET_STATIC_SETTINGS:	Loading static settings.
+ * @FILE_LOAD_NO_MORE_FILES:		No more files to load.
+ * @FILE_LOAD_FAILED:			File loading failed.
+ */
+enum file_load_state {
+	FILE_LOAD_GET_PATCH,
+	FILE_LOAD_GET_STATIC_SETTINGS,
+	FILE_LOAD_NO_MORE_FILES,
+	FILE_LOAD_FAILED
+};
+
+/**
+ * enum download_state - BOOT_DOWNLOAD state.
+ * @DOWNLOAD_PENDING:	Download in progress.
+ * @DOWNLOAD_SUCCESS:	Download successfully finished.
+ * @DOWNLOAD_FAILED:	Downloading failed.
+ */
+enum download_state {
+	DOWNLOAD_PENDING,
+	DOWNLOAD_SUCCESS,
+	DOWNLOAD_FAILED
+};
+
+/**
+ * enum fm_radio_mode - FM Radio mode.
+ * It's needed because some FM do-commands generate interrupts only when
+ * the FM driver is in specific mode and we need to know if we should expect
+ * the interrupt.
+ * @FM_RADIO_MODE_IDLE:	Radio mode is Idle (default).
+ * @FM_RADIO_MODE_FMT:	Radio mode is set to FMT (transmitter).
+ * @FM_RADIO_MODE_FMR:	Radio mode is set to FMR (receiver).
+ */
+enum fm_radio_mode {
+	FM_RADIO_MODE_IDLE = 0,
+	FM_RADIO_MODE_FMT = 1,
+	FM_RADIO_MODE_FMR = 2
+};
+
+
+/**
+ * struct cg2900_channel_item - List object for channel.
+ * @list:	list_head struct.
+ * @user:	User for this channel.
+ */
+struct cg2900_channel_item {
+	struct list_head	list;
+	struct cg2900_user_data	*user;
+};
+
+/**
+ * struct cg2900_delayed_work_struct - Work structure for CG2900 chip.
+ * @delayed_work:	Work structure.
+ * @data:		Pointer to private data.
+ */
+struct cg2900_delayed_work_struct {
+	struct delayed_work	work;
+	void			*data;
+};
+
+/**
+ * struct cg2900_skb_data - Structure for storing private data in an sk_buffer.
+ * @dev:	CG2900 device for this sk_buffer.
+ */
+struct cg2900_skb_data {
+	struct cg2900_user_data *user;
+};
+#define cg2900_skb_data(__skb) ((struct cg2900_skb_data *)((__skb)->cb))
+
+/**
+ * struct cg2900_chip_info - Main info structure for CG2900 chip driver.
+ * @dev:			Current device. Same as @chip_dev->dev.
+ * @patch_file_name:		Stores patch file name.
+ * @settings_file_name:		Stores settings file name.
+ * @file_info:			Firmware file info (patch or settings).
+ * @boot_state:			Current BOOT-state of CG2900 chip driver.
+ * @closing_state:		Current CLOSING-state of CG2900 chip driver.
+ * @file_load_state:		Current BOOT_FILE_LOAD-state of CG2900 chip
+ *				driver.
+ * @download_state:		Current BOOT_DOWNLOAD-state of CG2900 chip
+ *				driver.
+ * @wq:				CG2900 chip driver workqueue.
+ * @chip_dev:			Chip handler info.
+ * @tx_bt_lock:			Spinlock used to protect some global structures
+ *				related to internal BT command flow control.
+ * @tx_fm_lock:			Spinlock used to protect some global structures
+ *				related to internal FM command flow control.
+ * @tx_fm_audio_awaiting_irpt:	Indicates if an FM interrupt event related to
+ *				audio driver command is expected.
+ * @fm_radio_mode:		Current FM radio mode.
+ * @tx_nr_pkts_allowed_bt:	Number of packets allowed to send on BT HCI CMD
+ *				H4 channel.
+ * @audio_bt_cmd_op:		Stores the OpCode of the last sent audio driver
+ *				HCI BT CMD.
+ * @audio_fm_cmd_id:		Stores the command id of the last sent
+ *				HCI FM RADIO command by the fm audio user.
+ * @hci_fm_cmd_func:		Stores the command function of the last sent
+ *				HCI FM RADIO command by the fm radio user.
+ * @tx_queue_bt:		TX queue for HCI BT commands when nr of commands
+ *				allowed is 0 (CG2900 internal flow control).
+ * @tx_queue_fm:		TX queue for HCI FM commands when nr of commands
+ *				allowed is 0 (CG2900 internal flow control).
+ * @user_in_charge:		User currently operating. Normally used at
+ *				channel open and close.
+ * @last_user:			Last user of this chip. To avoid complications
+ *				this will never be set for bt_audio and
+ *				fm_audio.
+ * @logger:			Logger user of this chip.
+ * @hci_raw:			HCI Raw user of this chip.
+ * @selftest_work:		Delayed work for reading selftest results.
+ * @nbr_of_polls:		Number of times we should poll for selftest
+ *				results.
+ * @startup:			True if system is starting up.
+ * @mfd_size:			Number of MFD cells.
+ * @mfd_char_size:		Number of MFD char device cells.
+ */
+struct cg2900_chip_info {
+	struct device			*dev;
+	char				*patch_file_name;
+	char				*settings_file_name;
+	struct cg2900_file_info		file_info;
+	enum main_state			main_state;
+	enum boot_state			boot_state;
+	enum closing_state		closing_state;
+	enum file_load_state		file_load_state;
+	enum download_state		download_state;
+	struct workqueue_struct		*wq;
+	struct cg2900_chip_dev		*chip_dev;
+	spinlock_t			tx_bt_lock;
+	spinlock_t			tx_fm_lock;
+	spinlock_t			rw_lock;
+	bool				tx_fm_audio_awaiting_irpt;
+	enum fm_radio_mode		fm_radio_mode;
+	int				tx_nr_pkts_allowed_bt;
+	u16				audio_bt_cmd_op;
+	u16				audio_fm_cmd_id;
+	u16				hci_fm_cmd_func;
+	struct sk_buff_head		tx_queue_bt;
+	struct sk_buff_head		tx_queue_fm;
+	struct list_head		open_channels;
+	struct cg2900_user_data		*user_in_charge;
+	struct cg2900_user_data		*last_user;
+	struct cg2900_user_data		*logger;
+	struct cg2900_user_data		*hci_raw;
+	struct cg2900_user_data		*bt_audio;
+	struct cg2900_user_data		*fm_audio;
+	struct cg2900_delayed_work_struct	selftest_work;
+	int				nbr_of_polls;
+	bool				startup;
+	int				mfd_size;
+	int				mfd_extra_size;
+	int				mfd_char_size;
+	int				mfd_extra_char_size;
+};
+
+/**
+ * struct main_info - Main info structure for CG2900 chip driver.
+ * @dev:			Device structure.
+ * @cell_base_id:		Base ID for MFD cells.
+ * @man_mutex:			Management mutex.
+ */
+struct main_info {
+	struct device			*dev;
+	int				cell_base_id;
+	struct mutex			man_mutex;
+};
+
+static struct main_info *main_info;
+
+/*
+ * main_wait_queue - Main Wait Queue in CG2900 driver.
+ */
+static DECLARE_WAIT_QUEUE_HEAD(main_wait_queue);
+/*
+ * Clock enabler should be released earlier
+ * before running read self test
+ * as that is not required for clock enabler
+ * as is the case for WLAN
+ */
+static DECLARE_WAIT_QUEUE_HEAD(clk_user_wait_queue);
+
+static struct mfd_cell cg2900_devs[];
+static struct mfd_cell cg2900_char_devs[];
+static struct mfd_cell cg2910_extra_devs[];
+static struct mfd_cell cg2910_extra_char_devs[];
+
+static void chip_startup_finished(struct cg2900_chip_info *info, int err);
+static void chip_shutdown(struct cg2900_user_data *user);
+
+/**
+ * bt_is_open() - Checks if any BT user is in open state.
+ * @info:	CG2900 info.
+ *
+ * Returns:
+ *   true if a BT channel is open.
+ *   false if no BT channel is open.
+ */
+static bool bt_is_open(struct cg2900_chip_info *info)
+{
+	struct list_head *cursor;
+	struct cg2900_channel_item *tmp;
+
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user->h4_channel == CHANNEL_BT_CMD)
+			return true;
+	}
+	return false;
+}
+
+/**
+ * fm_is_open() - Checks if any FM user is in open state.
+ * @info:	CG2900 info.
+ *
+ * Returns:
+ *   true if a FM channel is open.
+ *   false if no FM channel is open.
+ */
+static bool fm_is_open(struct cg2900_chip_info *info)
+{
+	struct list_head *cursor;
+	struct cg2900_channel_item *tmp;
+
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user->h4_channel == CHANNEL_FM_RADIO)
+			return true;
+	}
+	return false;
+}
+
+/**
+ * fm_irpt_expected() - check if this FM command will generate an interrupt.
+ * @cmd_id:	command identifier.
+ *
+ * Returns:
+ *   true if the command will generate an interrupt.
+ *   false if it won't.
+ */
+static bool fm_irpt_expected(struct cg2900_chip_info *info, u16 cmd_id)
+{
+	bool retval = false;
+
+	switch (cmd_id) {
+	case CG2900_FM_DO_AIP_FADE_START:
+		if (info->fm_radio_mode == FM_RADIO_MODE_FMT)
+			retval = true;
+		break;
+
+	case CG2900_FM_DO_AUP_BT_FADE_START:
+	case CG2900_FM_DO_AUP_EXT_FADE_START:
+	case CG2900_FM_DO_AUP_FADE_START:
+		if (info->fm_radio_mode == FM_RADIO_MODE_FMR)
+			retval = true;
+		break;
+
+	case CG2900_FM_DO_FMR_SETANTENNA:
+	case CG2900_FM_DO_FMR_SP_AFSWITCH_START:
+	case CG2900_FM_DO_FMR_SP_AFUPDATE_START:
+	case CG2900_FM_DO_FMR_SP_BLOCKSCAN_START:
+	case CG2900_FM_DO_FMR_SP_PRESETPI_START:
+	case CG2900_FM_DO_FMR_SP_SCAN_START:
+	case CG2900_FM_DO_FMR_SP_SEARCH_START:
+	case CG2900_FM_DO_FMR_SP_SEARCHPI_START:
+	case CG2900_FM_DO_FMR_SP_TUNE_SETCHANNEL:
+	case CG2900_FM_DO_FMR_SP_TUNE_STEPCHANNEL:
+	case CG2900_FM_DO_FMT_PA_SETCTRL:
+	case CG2900_FM_DO_FMT_PA_SETMODE:
+	case CG2900_FM_DO_FMT_SP_TUNE_SETCHANNEL:
+	case CG2900_FM_DO_GEN_ANTENNACHECK_START:
+	case CG2900_FM_DO_GEN_GOTOMODE:
+	case CG2900_FM_DO_GEN_POWERSUPPLY_SETMODE:
+	case CG2900_FM_DO_GEN_SELECTREFERENCECLOCK:
+	case CG2900_FM_DO_GEN_SETPROCESSINGCLOCK:
+	case CG2900_FM_DO_GEN_SETREFERENCECLOCKPLL:
+	case CG2900_FM_DO_TST_TX_RAMP_START:
+		retval = true;
+		break;
+
+	default:
+		break;
+	}
+
+	if (retval)
+		dev_dbg(info->dev, "Following interrupt event expected for this"
+			" Cmd complete evt: cmd_id = 0x%X\n",
+			cmd_id);
+
+	return retval;
+}
+
+/**
+ * fm_is_do_cmd_irpt() - Check if irpt_val is one of the FM DO command related interrupts.
+ * @irpt_val:	interrupt value.
+ *
+ * Returns:
+ *   true if it's do-command related interrupt value.
+ *   false if it's not.
+ */
+static bool fm_is_do_cmd_irpt(u16 irpt_val)
+{
+	if ((irpt_val & CG2900_FM_IRPT_OPERATION_SUCCEEDED) ||
+	    (irpt_val & CG2900_FM_IRPT_OPERATION_FAILED)) {
+		dev_dbg(MAIN_DEV, "Irpt evt for FM do-command found, "
+			"irpt_val = 0x%X\n", irpt_val);
+		return true;
+	}
+
+	return false;
+}
+
+/**
+ * fm_reset_flow_ctrl - Clears up internal FM flow control.
+ *
+ * Resets outstanding commands and clear FM TX list and set CG2900 FM mode to
+ * idle.
+ */
+static void fm_reset_flow_ctrl(struct cg2900_chip_info *info)
+{
+	dev_dbg(info->dev, "fm_reset_flow_ctrl\n");
+
+	skb_queue_purge(&info->tx_queue_fm);
+
+	/* Reset the fm_cmd_id. */
+	info->audio_fm_cmd_id = CG2900_FM_CMD_NONE;
+	info->hci_fm_cmd_func = CG2900_FM_CMD_PARAM_NONE;
+
+	info->fm_radio_mode = FM_RADIO_MODE_IDLE;
+}
+
+
+/**
+ * fm_parse_cmd - Parses a FM command packet.
+ * @data:	FM command packet.
+ * @cmd_func:	Out: FM legacy command function.
+ * @cmd_id:	Out: FM legacy command ID.
+ */
+static void fm_parse_cmd(u8 *data, u8 *cmd_func, u16 *cmd_id)
+{
+	/* Move past H4-header to start of actual package */
+	struct fm_leg_cmd *pkt = (struct fm_leg_cmd *)(data + HCI_H4_SIZE);
+
+	*cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	*cmd_id   = CG2900_FM_CMD_NONE;
+
+	if (pkt->opcode != CG2900_FM_GEN_ID_LEGACY) {
+		dev_err(MAIN_DEV, "fm_parse_cmd: Not an FM legacy command "
+			"0x%02X\n", pkt->opcode);
+		return;
+	}
+
+	*cmd_func = pkt->fm_function;
+	if (*cmd_func == CG2900_FM_CMD_PARAM_WRITECOMMAND)
+		*cmd_id = cg2900_get_fm_cmd_id(le16_to_cpu(pkt->fm_cmd.head));
+}
+
+
+/**
+ * fm_parse_event - Parses a FM event packet
+ * @data:	FM event packet.
+ * @event:	Out: FM event.
+ * @cmd_func:	Out: FM legacy command function.
+ * @cmd_id:	Out: FM legacy command ID.
+ * @intr_val:	Out: FM interrupt value.
+ */
+static void fm_parse_event(u8 *data, u8 *event, u8 *cmd_func, u16 *cmd_id,
+			   u16 *intr_val)
+{
+	/* Move past H4-header to start of actual package */
+	union fm_leg_evt_or_irq *pkt = (union fm_leg_evt_or_irq *)data;
+
+	*cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	*cmd_id = CG2900_FM_CMD_NONE;
+	*intr_val = 0;
+	*event = CG2900_FM_EVENT_UNKNOWN;
+
+	if (pkt->evt.opcode == CG2900_FM_GEN_ID_LEGACY &&
+	    pkt->evt.read_write == CG2900_FM_CMD_LEG_PARAM_WRITE) {
+		/* Command complete */
+		*event = CG2900_FM_EVENT_CMD_COMPLETE;
+		*cmd_func = pkt->evt.fm_function;
+		if (*cmd_func == CG2900_FM_CMD_PARAM_WRITECOMMAND)
+			*cmd_id = cg2900_get_fm_cmd_id(
+				le16_to_cpu(pkt->evt.response_head));
+	} else if (pkt->irq_v2.opcode == CG2900_FM_GEN_ID_LEGACY &&
+		   pkt->irq_v2.event_type == CG2900_FM_CMD_LEG_PARAM_IRQ) {
+		/* Interrupt, PG2 style */
+		*event = CG2900_FM_EVENT_INTERRUPT;
+		*intr_val = le16_to_cpu(pkt->irq_v2.irq);
+	} else if (pkt->irq_v1.opcode == CG2900_FM_GEN_ID_LEGACY) {
+		/* Interrupt, PG1 style */
+		*event = CG2900_FM_EVENT_INTERRUPT;
+		*intr_val = le16_to_cpu(pkt->irq_v1.irq);
+	} else
+		dev_err(MAIN_DEV, "fm_parse_event: Not an FM legacy command "
+			"0x%X %X %X %X\n", data[0], data[1], data[2], data[3]);
+}
+
+/**
+ * fm_update_mode - Updates the FM mode state machine.
+ * @data:	FM command packet.
+ *
+ * Parses a FM command packet and updates the FM mode state machine.
+ */
+static void fm_update_mode(struct cg2900_chip_info *info, u8 *data)
+{
+	u8 cmd_func;
+	u16 cmd_id;
+
+	fm_parse_cmd(data, &cmd_func, &cmd_id);
+
+	if (cmd_func == CG2900_FM_CMD_PARAM_WRITECOMMAND &&
+	    cmd_id == CG2900_FM_DO_GEN_GOTOMODE) {
+		/* Move past H4-header to start of actual package */
+		struct fm_leg_cmd *pkt =
+			(struct fm_leg_cmd *)(data + HCI_H4_SIZE);
+
+		info->fm_radio_mode = le16_to_cpu(pkt->fm_cmd.data[0]);
+		dev_dbg(info->dev, "FM Radio mode changed to %d\n",
+			info->fm_radio_mode);
+	}
+}
+
+
+/**
+ * transmit_skb_from_tx_queue_bt() - Check flow control info and transmit skb.
+ *
+ * The transmit_skb_from_tx_queue_bt() function checks if there are tickets
+ * available and commands waiting in the TX queue and if so transmits them
+ * to the controller.
+ * It shall always be called within spinlock_bh.
+ */
+static void transmit_skb_from_tx_queue_bt(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_user_data *user;
+	struct cg2900_chip_info *info = dev->c_data;
+	struct sk_buff *skb;
+
+	dev_dbg(dev->dev, "transmit_skb_from_tx_queue_bt\n");
+
+	/* Dequeue an skb from the head of the list */
+	skb = skb_dequeue(&info->tx_queue_bt);
+	while (skb) {
+		if (info->tx_nr_pkts_allowed_bt <= 0) {
+			/*
+			 * If no more packets allowed just return, we'll get
+			 * back here after next Command Complete/Status event.
+			 * Put skb back at head of queue.
+			 */
+			skb_queue_head(&info->tx_queue_bt, skb);
+			return;
+		}
+
+		(info->tx_nr_pkts_allowed_bt)--;
+		dev_dbg(dev->dev, "tx_nr_pkts_allowed_bt = %d\n",
+			info->tx_nr_pkts_allowed_bt);
+
+		user = cg2900_skb_data(skb)->user; /* user is never NULL */
+
+		/*
+		 * If it's a command from audio application, store the OpCode,
+		 * it'll be used later to decide where to dispatch
+		 * the Command Complete event.
+		 */
+		if (info->bt_audio == user) {
+			struct hci_command_hdr *hdr = (struct hci_command_hdr *)
+				(skb->data + HCI_H4_SIZE);
+
+			info->audio_bt_cmd_op = le16_to_cpu(hdr->opcode);
+			dev_dbg(user->dev,
+				"Sending cmd from audio driver, saving "
+				"OpCode = 0x%04X\n", info->audio_bt_cmd_op);
+		}
+
+		cg2900_tx_to_chip(user, info->logger, skb);
+
+		/* Dequeue an skb from the head of the list */
+		skb = skb_dequeue(&info->tx_queue_bt);
+	}
+}
+
+/**
+ * transmit_skb_from_tx_queue_fm() - Check flow control info and transmit skb.
+ *
+ * The transmit_skb_from_tx_queue_fm() function checks if it possible to
+ * transmit and commands waiting in the TX queue and if so transmits them
+ * to the controller.
+ * It shall always be called within spinlock_bh.
+ */
+static void transmit_skb_from_tx_queue_fm(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_user_data *user;
+	struct cg2900_chip_info *info = dev->c_data;
+	struct sk_buff *skb;
+
+	dev_dbg(dev->dev, "transmit_skb_from_tx_queue_fm\n");
+
+	/* Dequeue an skb from the head of the list */
+	skb = skb_dequeue(&info->tx_queue_fm);
+	while (skb) {
+		u16 cmd_id;
+		u8 cmd_func;
+
+		if (info->audio_fm_cmd_id != CG2900_FM_CMD_NONE ||
+		    info->hci_fm_cmd_func != CG2900_FM_CMD_PARAM_NONE) {
+			/*
+			 * There are currently outstanding FM commands.
+			 * Wait for them to finish. We will get back here later.
+			 * Queue back the skb at head of list.
+			 */
+			skb_queue_head(&info->tx_queue_bt, skb);
+			return;
+		}
+
+		user = cg2900_skb_data(skb)->user; /* user is never NULL */
+
+		if (!user->opened) {
+			/*
+			 * Channel is not open. That means that the user that
+			 * originally sent it has deregistered.
+			 * Just throw it away and check the next skb in the
+			 * queue.
+			 */
+			kfree_skb(skb);
+			/* Dequeue an skb from the head of the list */
+			skb = skb_dequeue(&info->tx_queue_fm);
+			continue;
+		}
+
+		fm_parse_cmd(&(skb->data[0]), &cmd_func, &cmd_id);
+
+		/*
+		 * Store the FM command function , it'll be used later to decide
+		 * where to dispatch the Command Complete event.
+		 */
+		if (info->fm_audio == user) {
+			info->audio_fm_cmd_id = cmd_id;
+			dev_dbg(user->dev, "Sending FM audio cmd 0x%04X\n",
+				info->audio_fm_cmd_id);
+		} else {
+			/* FM radio command */
+			info->hci_fm_cmd_func = cmd_func;
+			fm_update_mode(info, &skb->data[0]);
+			dev_dbg(user->dev, "Sending FM radio cmd 0x%04X\n",
+				info->hci_fm_cmd_func);
+		}
+
+		/*
+		 * We have only one ticket on FM. Just return after
+		 * sending the skb.
+		 */
+		cg2900_tx_to_chip(user, info->logger, skb);
+		return;
+	}
+}
+
+/**
+ * update_flow_ctrl_bt() - Update number of outstanding commands for BT CMD.
+ * @dev:	Current chip device.
+ * @skb:	skb with received packet.
+ *
+ * The update_flow_ctrl_bt() checks if incoming data packet is
+ * BT Command Complete/Command Status Event and if so updates number of tickets
+ * and number of outstanding commands. It also calls function to send queued
+ * commands (if the list of queued commands is not empty).
+ */
+static void update_flow_ctrl_bt(struct cg2900_chip_dev *dev,
+				const struct sk_buff * const skb)
+{
+	u8 *data = skb->data;
+	struct hci_event_hdr *event;
+	struct cg2900_chip_info *info = dev->c_data;
+
+	event = (struct hci_event_hdr *)data;
+	data += sizeof(*event);
+
+	if (HCI_EV_CMD_COMPLETE == event->evt) {
+		struct hci_ev_cmd_complete *complete;
+		complete = (struct hci_ev_cmd_complete *)data;
+
+		/*
+		 * If it's HCI Command Complete Event then we might get some
+		 * HCI tickets back. Also we can decrease the number outstanding
+		 * HCI commands (if it's not NOP command or one of the commands
+		 * that generate both Command Status Event and Command Complete
+		 * Event).
+		 * Check if we have any HCI commands waiting in the TX list and
+		 * send them if there are tickets available.
+		 */
+		spin_lock_bh(&info->tx_bt_lock);
+		info->tx_nr_pkts_allowed_bt = complete->ncmd;
+		dev_dbg(dev->dev, "New tx_nr_pkts_allowed_bt = %d\n",
+			info->tx_nr_pkts_allowed_bt);
+
+		if (!skb_queue_empty(&info->tx_queue_bt))
+			transmit_skb_from_tx_queue_bt(dev);
+		spin_unlock_bh(&info->tx_bt_lock);
+	} else if (HCI_EV_CMD_STATUS == event->evt) {
+		struct hci_ev_cmd_status *status;
+		status = (struct hci_ev_cmd_status *)data;
+
+		/*
+		 * If it's HCI Command Status Event then we might get some
+		 * HCI tickets back. Also we can decrease the number outstanding
+		 * HCI commands (if it's not NOP command).
+		 * Check if we have any HCI commands waiting in the TX queue and
+		 * send them if there are tickets available.
+		 */
+		spin_lock_bh(&info->tx_bt_lock);
+		info->tx_nr_pkts_allowed_bt = status->ncmd;
+		dev_dbg(dev->dev, "New tx_nr_pkts_allowed_bt = %d\n",
+			info->tx_nr_pkts_allowed_bt);
+
+		if (!skb_queue_empty(&info->tx_queue_bt))
+			transmit_skb_from_tx_queue_bt(dev);
+		spin_unlock_bh(&info->tx_bt_lock);
+	}
+}
+
+/**
+ * update_flow_ctrl_fm() - Update packets allowed for FM channel.
+ * @dev:	Current chip device.
+ * @skb:	skb with received packet.
+ *
+ * The update_flow_ctrl_fm() checks if incoming data packet is FM packet
+ * indicating that the previous command has been handled and if so update
+ * packets. It also calls function to send queued commands (if the list of
+ * queued commands is not empty).
+ */
+static void update_flow_ctrl_fm(struct cg2900_chip_dev *dev,
+				const struct sk_buff * const skb)
+{
+	u8 cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	u16 cmd_id = CG2900_FM_CMD_NONE;
+	u16 irpt_val = 0;
+	u8 event = CG2900_FM_EVENT_UNKNOWN;
+	struct cg2900_chip_info *info = dev->c_data;
+
+	fm_parse_event(&(skb->data[0]), &event, &cmd_func, &cmd_id, &irpt_val);
+
+	if (event == CG2900_FM_EVENT_CMD_COMPLETE) {
+		/* FM legacy command complete event */
+		spin_lock_bh(&info->tx_fm_lock);
+		/*
+		 * Check if it's not an write command complete event, because
+		 * then it cannot be a DO command.
+		 * If it's a write command complete event check that is not a
+		 * DO command complete event before setting the outstanding
+		 * FM packets to none.
+		 */
+		if (cmd_func != CG2900_FM_CMD_PARAM_WRITECOMMAND ||
+		    !fm_irpt_expected(info, cmd_id)) {
+			info->hci_fm_cmd_func = CG2900_FM_CMD_PARAM_NONE;
+			info->audio_fm_cmd_id = CG2900_FM_CMD_NONE;
+			dev_dbg(dev->dev,
+				"FM_Write: Outstanding FM commands:\n"
+				"\tRadio: 0x%04X\n"
+				"\tAudio: 0x%04X\n",
+				info->hci_fm_cmd_func,
+				info->audio_fm_cmd_id);
+			transmit_skb_from_tx_queue_fm(dev);
+
+		/*
+		 * If there was a write do command complete event check if it is
+		 * DO command previously sent by the FM audio user. If that's
+		 * the case we need remember that in order to be able to
+		 * dispatch the interrupt to the correct user.
+		 */
+		} else if (cmd_id == info->audio_fm_cmd_id) {
+			info->tx_fm_audio_awaiting_irpt = true;
+			dev_dbg(dev->dev,
+				"FM Audio waiting for interrupt = true\n");
+		}
+		spin_unlock_bh(&info->tx_fm_lock);
+	} else if (event == CG2900_FM_EVENT_INTERRUPT) {
+		/* FM legacy interrupt */
+		if (fm_is_do_cmd_irpt(irpt_val)) {
+			/*
+			 * If it is an interrupt related to a DO command update
+			 * the outstanding flow control and transmit blocked
+			 * FM commands.
+			 */
+			spin_lock_bh(&info->tx_fm_lock);
+			info->hci_fm_cmd_func = CG2900_FM_CMD_PARAM_NONE;
+			info->audio_fm_cmd_id = CG2900_FM_CMD_NONE;
+			dev_dbg(dev->dev,
+				"FM_INT: Outstanding FM commands:\n"
+				"\tRadio: 0x%04X\n"
+				"\tAudio: 0x%04X\n",
+				info->hci_fm_cmd_func,
+				info->audio_fm_cmd_id);
+			info->tx_fm_audio_awaiting_irpt = false;
+			dev_dbg(dev->dev,
+				"FM Audio waiting for interrupt = false\n");
+			transmit_skb_from_tx_queue_fm(dev);
+			spin_unlock_bh(&info->tx_fm_lock);
+		}
+	}
+}
+
+/**
+ * send_bt_enable() - Send HCI VS BT Enable command to the chip.
+ * @info:	Chip info structure.
+ * @bt_enable:	Value for BT Enable parameter (e.g. CG2900_BT_DISABLE).
+ */
+static void send_bt_enable(struct cg2900_chip_info *info, u8 bt_enable)
+{
+	struct bt_vs_bt_enable_cmd cmd;
+
+	cmd.op_code = cpu_to_le16(CG2900_BT_OP_VS_BT_ENABLE);
+	cmd.plen = BT_PARAM_LEN(sizeof(cmd));
+	cmd.enable = bt_enable;
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger,
+			   &cmd, sizeof(cmd));
+}
+
+/**
+ * send_bd_address() - Send HCI VS command with BD address to the chip.
+ */
+static void send_bd_address(struct cg2900_chip_info *info)
+{
+	struct bt_vs_store_in_fs_cmd *cmd;
+	u8 plen = sizeof(*cmd) + BT_BDADDR_SIZE;
+
+	cmd = kmalloc(plen, GFP_KERNEL);
+	if (!cmd) {
+		dev_err(info->dev, "send_bd_address could not allocate cmd\n");
+		return;
+	}
+
+	cmd->opcode = cpu_to_le16(CG2900_BT_OP_VS_STORE_IN_FS);
+	cmd->plen = BT_PARAM_LEN(plen);
+	cmd->user_id = CG2900_VS_STORE_IN_FS_USR_ID_BD_ADDR;
+	cmd->len = BT_BDADDR_SIZE;
+	/* Now copy the BD address received from user space control app. */
+	memcpy(cmd->data, bd_address, BT_BDADDR_SIZE);
+
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_SEND_BD_ADDRESS\n");
+	info->boot_state = BOOT_SEND_BD_ADDRESS;
+
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger, cmd, plen);
+
+	kfree(cmd);
+}
+
+/**
+ * send_settings_file() - Transmit settings file.
+ *
+ * The send_settings_file() function transmit settings file.
+ * The file is read in parts to fit in HCI packets. When finished,
+ * close the settings file and send HCI reset to activate settings and patches.
+ */
+static void send_settings_file(struct cg2900_chip_info *info)
+{
+	int bytes_sent;
+
+	bytes_sent = cg2900_read_and_send_file_part(info->user_in_charge,
+						    info->logger,
+						    &info->file_info);
+	if (bytes_sent > 0) {
+		/* Data sent. Wait for CmdComplete */
+		return;
+	} else if (bytes_sent < 0) {
+		dev_err(BOOT_DEV, "send_settings_file: Error %d occurred\n",
+			bytes_sent);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, bytes_sent);
+		return;
+	}
+
+	/* No data was sent. This file is finished */
+	info->download_state = DOWNLOAD_SUCCESS;
+
+	/* Settings file finished. Release used resources */
+	dev_dbg(BOOT_DEV, "Settings file finished, release used resources\n");
+	release_firmware(info->file_info.fw_file);
+	info->file_info.fw_file = NULL;
+
+	dev_dbg(BOOT_DEV, "New file_load_state: FILE_LOAD_NO_MORE_FILES\n");
+	info->file_load_state = FILE_LOAD_NO_MORE_FILES;
+
+	/* Create and send HCI VS Store In FS command with bd address. */
+	send_bd_address(info);
+}
+
+/**
+ * send_patch_file - Transmit patch file.
+ *
+ * The send_patch_file() function transmit patch file.
+ * The file is read in parts to fit in HCI packets. When the complete file is
+ * transmitted, the file is closed.
+ * When finished, continue with settings file.
+ */
+static void send_patch_file(struct cg2900_chip_dev *dev)
+{
+	int err;
+	int bytes_sent;
+	struct cg2900_chip_info *info = dev->c_data;
+	int file_name_size = strlen("CG29XX_XXXX_XXXX_settings.fw");
+
+	bytes_sent = cg2900_read_and_send_file_part(info->user_in_charge,
+						    info->logger,
+						    &info->file_info);
+	if (bytes_sent > 0) {
+		/* Data sent. Wait for CmdComplete */
+		return;
+	} else if (bytes_sent < 0) {
+		dev_err(BOOT_DEV, "send_patch_file: Error %d occurred\n",
+			bytes_sent);
+		err = bytes_sent;
+		goto error_handling;
+	}
+
+	/* No data was sent. This file is finished */
+	info->download_state = DOWNLOAD_SUCCESS;
+
+	dev_dbg(BOOT_DEV, "Patch file finished, release used resources\n");
+	release_firmware(info->file_info.fw_file);
+	info->file_info.fw_file = NULL;
+
+	/*
+	 * Create the settings file name from HCI revision and sub_version.
+	 * file_name_size does not include terminating NULL character
+	 * so add 1.
+	 */
+	err = snprintf(info->settings_file_name, file_name_size + 1,
+			"CG29XX_%04X_%04X_settings.fw", dev->chip.hci_revision,
+			dev->chip.hci_sub_version);
+	if (err == file_name_size) {
+		dev_dbg(BOOT_DEV, "Downloading settings file %s\n",
+				info->settings_file_name);
+	} else {
+		dev_err(BOOT_DEV, "Settings file name failed! err=%d\n", err);
+		goto error_handling;
+	}
+
+	/* Retrieve the settings file */
+	err = request_firmware(&info->file_info.fw_file,
+			       info->settings_file_name,
+			       info->dev);
+	if (err) {
+		dev_err(BOOT_DEV, "Couldn't get settings file (%d)\n", err);
+		goto error_handling;
+	}
+	/* Now send the settings file */
+	dev_dbg(BOOT_DEV,
+		"New file_load_state: FILE_LOAD_GET_STATIC_SETTINGS\n");
+	info->file_load_state = FILE_LOAD_GET_STATIC_SETTINGS;
+	dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_PENDING\n");
+	info->download_state = DOWNLOAD_PENDING;
+	send_settings_file(info);
+	return;
+
+error_handling:
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, err);
+}
+
+/**
+ * work_power_off_chip() - Work item to power off the chip.
+ * @work:	Reference to work data.
+ *
+ * The work_power_off_chip() function handles transmission of the HCI command
+ * vs_power_switch_off and then informs the CG2900 Core that this chip driver is
+ * finished and the Core driver can now shut off the chip.
+ */
+static void work_power_off_chip(struct work_struct *work)
+{
+	struct sk_buff *skb = NULL;
+	u8 *h4_header;
+	struct cg2900_platform_data *pf_data;
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_power_off_chip: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/*
+	 * Get the VS Power Switch Off command to use based on connected
+	 * connectivity controller
+	 */
+	pf_data = dev_get_platdata(dev->dev);
+	if (pf_data->get_power_switch_off_cmd)
+		skb = pf_data->get_power_switch_off_cmd(dev, NULL);
+
+	/*
+	 * Transmit the received command.
+	 * If no command found for the device, just continue
+	 */
+	if (!skb) {
+		dev_err(dev->dev,
+			"Could not retrieve PowerSwitchOff command\n");
+		goto shut_down_chip;
+	}
+
+	dev_dbg(dev->dev,
+		"Got power_switch_off command. Add H4 header and transmit\n");
+
+	/*
+	 * Move the data pointer to the H:4 header position and store
+	 * the H4 header
+	 */
+	h4_header = skb_push(skb, CG2900_SKB_RESERVE);
+	*h4_header = CHANNEL_BT_CMD;
+
+	dev_dbg(dev->dev, "New closing_state: CLOSING_POWER_SWITCH_OFF\n");
+	info->closing_state = CLOSING_POWER_SWITCH_OFF;
+
+	if (info->user_in_charge)
+		cg2900_tx_to_chip(info->user_in_charge, info->logger, skb);
+	else
+		cg2900_tx_no_user(dev, skb);
+
+	/*
+	 * Mandatory to wait 500ms after the power_switch_off command has been
+	 * transmitted, in order to make sure that the controller is ready.
+	 */
+	schedule_timeout_killable(msecs_to_jiffies(POWER_SW_OFF_WAIT));
+
+shut_down_chip:
+	dev_dbg(dev->dev, "New closing_state: CLOSING_SHUT_DOWN\n");
+	info->closing_state = CLOSING_SHUT_DOWN;
+
+	/* Close the transport, which will power off the chip */
+	if (dev->t_cb.close)
+		dev->t_cb.close(dev);
+
+	/* Chip shut-down finished, set correct state and wake up the chip. */
+	dev_dbg(dev->dev, "New main_state: CG2900_IDLE\n");
+	info->main_state = CG2900_IDLE;
+	wake_up_all(&main_wait_queue);
+
+	/* If this is called during system startup, register the devices. */
+	if (info->startup) {
+		int err;
+
+		err = mfd_add_devices(dev->dev, main_info->cell_base_id,
+				cg2900_devs, info->mfd_size, NULL, 0);
+		if (err) {
+			dev_err(dev->dev, "Failed to add cg2900_devs (%d)\n",
+					err);
+			goto finished;
+		}
+
+		err = mfd_add_devices(dev->dev, main_info->cell_base_id,
+				cg2900_char_devs, info->mfd_char_size,
+				NULL, 0);
+		if (err) {
+			dev_err(dev->dev, "Failed to add cg2900_char_devs (%d)"
+					"\n", err);
+			mfd_remove_devices(dev->dev);
+			goto finished;
+		}
+
+		/*
+		 * Increase base ID so next connected transport will not get the
+		 * same device IDs.
+		 */
+		main_info->cell_base_id +=
+				MAX(info->mfd_size, info->mfd_char_size);
+
+		if (dev->chip.hci_revision == CG2910_PG1_REV ||
+				dev->chip.hci_revision == CG2910_PG2_REV) {
+			err = mfd_add_devices(dev->dev, main_info->cell_base_id,
+					cg2910_extra_devs,
+					info->mfd_extra_size, NULL, 0);
+			if (err) {
+				dev_err(dev->dev, "Failed to add cg2910_extra_devs "
+						"(%d)\n", err);
+				goto finished;
+			}
+
+			err = mfd_add_devices(dev->dev, main_info->cell_base_id,
+					cg2910_extra_char_devs,
+					info->mfd_extra_char_size, NULL, 0);
+			if (err) {
+				dev_err(dev->dev, "Failed to add cg2910_extra_char_devs "
+						"(%d)\n", err);
+				mfd_remove_devices(dev->dev);
+				goto finished;
+			}
+
+			/*
+			 * Increase base ID so next connected transport
+			 * will not get the same device IDs.
+			 */
+			main_info->cell_base_id +=
+					MAX(info->mfd_extra_size,
+						info->mfd_extra_char_size);
+		}
+
+		info->startup = false;
+	}
+
+finished:
+	kfree(my_work);
+}
+
+/**
+ * work_chip_shutdown() - Shut down the chip.
+ * @work:	Reference to work data.
+ */
+static void work_chip_shutdown(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_user_data *user;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_chip_shutdown: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	user = my_work->user_data;
+
+	chip_shutdown(user);
+
+	kfree(my_work);
+}
+
+/**
+ * work_reset_after_error() - Handle reset.
+ * @work:	Reference to work data.
+ *
+ * Handle a reset after received Command Complete event.
+ */
+static void work_reset_after_error(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_reset_after_error: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	chip_startup_finished(info, -EIO);
+
+	kfree(my_work);
+}
+
+/**
+ * work_load_patch_and_settings() - Start loading patches and settings.
+ * @work:	Reference to work data.
+ */
+static void work_load_patch_and_settings(struct work_struct *work)
+{
+	int err = 0;
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	int file_name_size = strlen("CG29XX_XXXX_XXXX_patch.fw");
+
+	if (!work) {
+		dev_err(MAIN_DEV,
+			"work_load_patch_and_settings: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/* Check that we are in the right state */
+	if (info->boot_state != BOOT_GET_FILES_TO_LOAD)
+		goto finished;
+
+	/*
+	 * Create the patch file name from HCI revision and sub_version.
+	 * file_name_size does not include terminating NULL character
+	 * so add 1.
+	 */
+	err = snprintf(info->patch_file_name, file_name_size + 1,
+			"CG29XX_%04X_%04X_patch.fw", dev->chip.hci_revision,
+			dev->chip.hci_sub_version);
+	if (err == file_name_size) {
+		dev_dbg(BOOT_DEV, "Downloading patch file %s\n",
+				info->patch_file_name);
+	} else {
+		dev_err(BOOT_DEV, "Patch file name failed! err=%d\n", err);
+		goto error_handling;
+	}
+
+	/* We now all info needed */
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_DOWNLOAD_PATCH\n");
+	info->boot_state = BOOT_DOWNLOAD_PATCH;
+	dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_PENDING\n");
+	info->download_state = DOWNLOAD_PENDING;
+	dev_dbg(BOOT_DEV, "New file_load_state: FILE_LOAD_GET_PATCH\n");
+	info->file_load_state = FILE_LOAD_GET_PATCH;
+	info->file_info.chunk_id = 0;
+	info->file_info.file_offset = 0;
+	info->file_info.fw_file = NULL;
+
+	/* OK. Now it is time to download the patches */
+	err = request_firmware(&(info->file_info.fw_file),
+			       info->patch_file_name,
+			       dev->dev);
+	if (err < 0) {
+		dev_err(BOOT_DEV, "Couldn't get patch file (%d)\n", err);
+		goto error_handling;
+	}
+	send_patch_file(dev);
+
+	goto finished;
+
+error_handling:
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, -EIO);
+finished:
+	kfree(my_work);
+}
+
+/**
+ * work_cont_file_download() - A file block has been written.
+ * @work:	Reference to work data.
+ *
+ * Handle a received HCI VS Write File Block Complete event.
+ * Normally this means continue to send files to the controller.
+ */
+static void work_cont_file_download(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_cont_file_download: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/* Continue to send patches or settings to the controller */
+	if (info->file_load_state == FILE_LOAD_GET_PATCH)
+		send_patch_file(dev);
+	else if (info->file_load_state == FILE_LOAD_GET_STATIC_SETTINGS)
+		send_settings_file(info);
+	else
+		dev_dbg(BOOT_DEV, "No more files to load\n");
+
+	kfree(my_work);
+}
+
+/**
+ * work_send_read_selftest_cmd() - HCI VS Read_SelfTests_Result command shall be sent.
+ * @work:	Reference to work data.
+ */
+static void work_send_read_selftest_cmd(struct work_struct *work)
+{
+	struct delayed_work *del_work;
+	struct cg2900_delayed_work_struct *current_work;
+	struct cg2900_chip_info *info;
+	struct hci_command_hdr cmd;
+
+	if (!work) {
+		dev_err(MAIN_DEV,
+			"work_send_read_selftest_cmd: work == NULL\n");
+		return;
+	}
+
+	del_work = to_delayed_work(work);
+	current_work = container_of(del_work,
+				    struct cg2900_delayed_work_struct, work);
+	info = current_work->data;
+
+	if (info->boot_state != BOOT_READ_SELFTEST_RESULT)
+		return;
+
+	cmd.opcode = cpu_to_le16(CG2900_BT_OP_VS_READ_SELTESTS_RESULT);
+	cmd.plen = 0; /* No parameters for Read Selftests Result */
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger, &cmd,
+			   sizeof(cmd));
+}
+
+/**
+ * handle_reset_cmd_complete() - Handles HCI Reset Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_reset_cmd_complete(struct cg2900_chip_dev *dev, u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	dev_dbg(BOOT_DEV, "Received Reset complete event with status 0x%X\n",
+		status);
+
+	if (CG2900_CLOSING != info->main_state &&
+	    CLOSING_RESET != info->closing_state)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		/*
+		 * Continue in case of error, the chip is going to be shut down
+		 * anyway.
+		 */
+		dev_err(BOOT_DEV, "Command complete for HciReset received with "
+			"error 0x%X\n", status);
+	}
+
+	cg2900_create_work_item(info->wq, work_power_off_chip, dev);
+
+	return true;
+}
+
+/**
+ * handle_vs_store_in_fs_cmd_complete() - Handles HCI VS StoreInFS Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_store_in_fs_cmd_complete(struct cg2900_chip_dev *dev,
+					       u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	dev_dbg(BOOT_DEV,
+		"Received Store_in_FS complete event with status 0x%X\n",
+		status);
+
+	if (info->boot_state != BOOT_SEND_BD_ADDRESS)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR == status) {
+		struct hci_command_hdr cmd;
+
+		/* Send HCI SystemReset command to activate patches */
+		dev_dbg(BOOT_DEV,
+			"New boot_state: BOOT_ACTIVATE_PATCHES_AND_SETTINGS\n");
+		info->boot_state = BOOT_ACTIVATE_PATCHES_AND_SETTINGS;
+
+		cmd.opcode = cpu_to_le16(CG2900_BT_OP_VS_SYSTEM_RESET);
+		cmd.plen = 0; /* No parameters for System Reset */
+		cg2900_send_bt_cmd(info->user_in_charge, info->logger, &cmd,
+				   sizeof(cmd));
+	} else {
+		dev_err(BOOT_DEV,
+			"Command complete for StoreInFS received with error "
+			"0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_write_file_block_cmd_complete() - Handles HCI VS WriteFileBlock Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_write_file_block_cmd_complete(struct cg2900_chip_dev *dev,
+						    u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DOWNLOAD_PATCH ||
+	    info->download_state != DOWNLOAD_PENDING)
+		return false;
+
+	if (HCI_BT_WRONG_SEQ_ERROR == status && info->file_info.chunk_id == 1 &&
+			(CG2905_PG1_1_REV == dev->chip.hci_revision ||
+			CG2910_PG1_REV == dev->chip.hci_revision)) {
+		/*
+		 * Because of bug in CG2905/CG2910 PG1 H/W, the first chunk
+		 * will return an error of wrong sequence number. As a
+		 * workaround the first chunk needs to be sent again.
+		 */
+		info->file_info.chunk_id = 0;
+		info->file_info.file_offset = 0;
+		/*
+		 * Set the status back to success so that it continues on the
+		 * success path rather than failure.
+		 */
+		status = HCI_BT_ERROR_NO_ERROR;
+	}
+
+	if (HCI_BT_ERROR_NO_ERROR == status)
+		cg2900_create_work_item(info->wq, work_cont_file_download, dev);
+	else {
+		dev_err(BOOT_DEV,
+			"Command complete for WriteFileBlock received with"
+			" error 0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_FAILED\n");
+		info->download_state = DOWNLOAD_FAILED;
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		if (info->file_info.fw_file) {
+			release_firmware(info->file_info.fw_file);
+			info->file_info.fw_file = NULL;
+		}
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_write_file_block_cmd_status() - Handles HCI VS WriteFileBlock Command Status event.
+ * @status:	Returned status of WriteFileBlock command.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_write_file_block_cmd_status(struct cg2900_chip_dev *dev,
+						  u8 status)
+{
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DOWNLOAD_PATCH ||
+	    info->download_state != DOWNLOAD_PENDING)
+		return false;
+
+	/*
+	 * Only do something if there is an error. Otherwise we will wait for
+	 * CmdComplete.
+	 */
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		dev_err(BOOT_DEV,
+			"Command status for WriteFileBlock received with"
+			" error 0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_FAILED\n");
+		info->download_state = DOWNLOAD_FAILED;
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		if (info->file_info.fw_file) {
+			release_firmware(info->file_info.fw_file);
+			info->file_info.fw_file = NULL;
+		}
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_power_switch_off_cmd_complete() - Handles HCI VS PowerSwitchOff Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_power_switch_off_cmd_complete(struct cg2900_chip_dev *dev,
+						    u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (CLOSING_POWER_SWITCH_OFF != info->closing_state)
+		return false;
+
+	dev_dbg(BOOT_DEV,
+		"handle_vs_power_switch_off_cmd_complete status %d\n", status);
+
+	/*
+	 * We were waiting for this but we don't need to do anything upon
+	 * reception except warn for error status
+	 */
+	if (HCI_BT_ERROR_NO_ERROR != status)
+		dev_err(BOOT_DEV,
+			"Command Complete for PowerSwitchOff received with "
+			"error 0x%X", status);
+
+	return true;
+}
+
+/**
+ * handle_vs_system_reset_cmd_complete() - Handle HCI VS SystemReset Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_system_reset_cmd_complete(struct cg2900_chip_dev *dev,
+						u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_ACTIVATE_PATCHES_AND_SETTINGS)
+		return false;
+
+	dev_dbg(BOOT_DEV, "handle_vs_system_reset_cmd_complete status %d\n",
+		status);
+
+	if (HCI_BT_ERROR_NO_ERROR == status) {
+		if (dev->chip.hci_revision == CG2900_PG2_REV &&
+				dev->chip.hci_revision == CG2905_PG1_1_REV &&
+				dev->chip.hci_revision == CG2910_PG1_REV &&
+				dev->chip.hci_revision == CG2910_PG2_REV) {
+			/*
+			 * We must now wait for the selftest results. They will
+			 * take a certain amount of time to finish so start a
+			 * delayed work that will then send the command.
+			 */
+			dev_dbg(BOOT_DEV,
+				"New boot_state: BOOT_READ_SELFTEST_RESULT\n");
+			info->boot_state = BOOT_READ_SELFTEST_RESULT;
+			queue_delayed_work(info->wq, &info->selftest_work.work,
+					   msecs_to_jiffies(SELFTEST_INITIAL));
+			info->nbr_of_polls = 0;
+		} else {
+			/*
+			 * We are now almost finished. Shut off BT Core. It will
+			 * be re-enabled by the Bluetooth driver when needed.
+			 */
+			dev_dbg(BOOT_DEV, "New boot_state: BOOT_DISABLE_BT\n");
+			info->boot_state = BOOT_DISABLE_BT;
+			send_bt_enable(info, CG2900_BT_DISABLE);
+		}
+	} else {
+		dev_err(BOOT_DEV,
+			"Received Reset complete event with status 0x%X\n",
+			status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, -EIO);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_read_selftests_cmd_complete() - Handle HCI VS ReadSelfTestsResult Command Complete event.
+ * @dev:	Current chip.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_read_selftests_cmd_complete(struct cg2900_chip_dev *dev,
+						  u8 *data)
+{
+	struct bt_vs_read_selftests_result_evt *evt =
+			(struct bt_vs_read_selftests_result_evt *)data;
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_READ_SELFTEST_RESULT)
+		return false;
+
+	dev_dbg(BOOT_DEV,
+		"handle_vs_read_selftests_cmd_complete status %d result %d\n",
+		evt->status, evt->result);
+
+	if (HCI_BT_ERROR_NO_ERROR != evt->status)
+		goto err_handling;
+
+	if (CG2900_BT_SELFTEST_SUCCESSFUL == evt->result ||
+	    CG2900_BT_SELFTEST_FAILED == evt->result) {
+		if (CG2900_BT_SELFTEST_FAILED == evt->result)
+			dev_err(BOOT_DEV, "CG2900 self test failed\n");
+
+		/*
+		 * We are now almost finished. Shut off BT Core. It will
+		 * be re-enabled by the Bluetooth driver when needed.
+		 */
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_DISABLE_BT\n");
+		info->boot_state = BOOT_DISABLE_BT;
+		send_bt_enable(info, CG2900_BT_DISABLE);
+		return true;
+	} else if (CG2900_BT_SELFTEST_NOT_COMPLETED == evt->result) {
+		/*
+		 * Self tests are not yet finished. Wait some more time
+		 * before resending the command
+		 */
+		if (info->nbr_of_polls > MAX_NBR_OF_POLLS) {
+			dev_err(BOOT_DEV, "Selftest results reached max"
+				" number of polls\n");
+			goto err_handling;
+		}
+		queue_delayed_work(info->wq, &info->selftest_work.work,
+				   msecs_to_jiffies(SELFTEST_POLLING));
+		info->nbr_of_polls++;
+		return true;
+	}
+
+err_handling:
+	dev_err(BOOT_DEV,
+		"Received Read SelfTests Result complete event with "
+		"status 0x%X and result 0x%X\n",
+		evt->status, evt->result);
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, -EIO);
+	return true;
+}
+
+/**
+ * handle_vs_bt_enable_cmd_status() - Handles HCI VS BtEnable Command Status event.
+ * @status:	Returned status of BtEnable command.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_bt_enable_cmd_status(struct cg2900_chip_dev *dev,
+					   u8 status)
+{
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DISABLE_BT)
+		return false;
+
+	dev_dbg(BOOT_DEV, "handle_vs_bt_enable_cmd_status status %d\n", status);
+
+	/*
+	 * Only do something if there is an error. Otherwise we will wait for
+	 * CmdComplete.
+	 */
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		dev_err(BOOT_DEV,
+			"Received BtEnable status event with status 0x%X\n",
+			status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, -EIO);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_bt_enable_cmd_complete() - Handle HCI VS BtEnable Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_bt_enable_cmd_complete(struct cg2900_chip_dev *dev,
+					     u8 *data)
+{
+	u8 status = data[0];
+	struct cg2900_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DISABLE_BT)
+		return false;
+
+	dev_dbg(BOOT_DEV, "handle_vs_bt_enable_cmd_complete status %d\n",
+		status);
+
+	if (HCI_BT_ERROR_NO_ERROR == status) {
+		/*
+		 * The boot sequence is now finished successfully.
+		 * Set states and signal to waiting thread.
+		 */
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_READY\n");
+		info->boot_state = BOOT_READY;
+		chip_startup_finished(info, 0);
+	} else {
+		dev_err(BOOT_DEV,
+			"Received BtEnable complete event with status 0x%X\n",
+			status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, -EIO);
+	}
+
+	return true;
+}
+
+/**
+ * handle_rx_data_bt_evt() - Check if received data should be handled in CG2900 chip driver.
+ * @skb:	Data packet
+ *
+ * The handle_rx_data_bt_evt() function checks if received data should be
+ * handled in CG2900 chip driver. If so handle it correctly.
+ * Received data is always HCI BT Event.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool handle_rx_data_bt_evt(struct cg2900_chip_dev *dev,
+				  struct sk_buff *skb)
+{
+	bool pkt_handled = false;
+	/* skb cannot be NULL here so it is safe to de-reference */
+	u8 *data = skb->data;
+	struct hci_event_hdr *evt;
+	u16 op_code;
+
+	evt = (struct hci_event_hdr *)data;
+	data += sizeof(*evt);
+
+	/* First check the event code. */
+	if (HCI_EV_CMD_COMPLETE == evt->evt) {
+		struct hci_ev_cmd_complete *cmd_complete;
+
+		cmd_complete = (struct hci_ev_cmd_complete *)data;
+		op_code = le16_to_cpu(cmd_complete->opcode);
+		dev_dbg(dev->dev,
+			"Received Command Complete: op_code = 0x%04X\n",
+			op_code);
+		/* Move to first byte after OCF */
+		data += sizeof(*cmd_complete);
+
+		if (op_code == HCI_OP_RESET)
+			pkt_handled = handle_reset_cmd_complete(dev, data);
+		else if (op_code == CG2900_BT_OP_VS_STORE_IN_FS)
+			pkt_handled = handle_vs_store_in_fs_cmd_complete(dev,
+									 data);
+		else if (op_code == CG2900_BT_OP_VS_WRITE_FILE_BLOCK)
+			pkt_handled =
+				handle_vs_write_file_block_cmd_complete(dev,
+									data);
+		else if (op_code == CG2900_BT_OP_VS_POWER_SWITCH_OFF)
+			pkt_handled =
+				handle_vs_power_switch_off_cmd_complete(dev,
+									data);
+		else if (op_code == CG2900_BT_OP_VS_SYSTEM_RESET) {
+			struct cg2900_chip_info *info = dev->c_data;
+			/*
+			 * Don't wait till READ_SELFTESTS_RESULT is complete
+			 * for clock users
+			 */
+			dev_dbg(dev->dev,
+				"New main_state: CG2900_ACTIVE_BEFORE_SELFTEST\n");
+			info->main_state = CG2900_ACTIVE_BEFORE_SELFTEST;
+			wake_up_all(&clk_user_wait_queue);
+
+			pkt_handled =
+				handle_vs_system_reset_cmd_complete(dev, data);
+		} else if (op_code == CG2900_BT_OP_VS_BT_ENABLE)
+			pkt_handled = handle_vs_bt_enable_cmd_complete(dev,
+								       data);
+		else if (op_code == CG2900_BT_OP_VS_READ_SELTESTS_RESULT)
+			pkt_handled = handle_vs_read_selftests_cmd_complete(dev,
+									data);
+	} else if (HCI_EV_CMD_STATUS == evt->evt) {
+		struct hci_ev_cmd_status *cmd_status;
+
+		cmd_status = (struct hci_ev_cmd_status *)data;
+
+		op_code = le16_to_cpu(cmd_status->opcode);
+
+		dev_dbg(dev->dev, "Received Command Status: op_code = 0x%04X\n",
+			op_code);
+
+		if (op_code == CG2900_BT_OP_VS_WRITE_FILE_BLOCK)
+			pkt_handled = handle_vs_write_file_block_cmd_status
+				(dev, cmd_status->status);
+		else if (op_code == CG2900_BT_OP_VS_BT_ENABLE)
+			pkt_handled = handle_vs_bt_enable_cmd_status
+				(dev, cmd_status->status);
+	} else if (HCI_EV_VENDOR_SPECIFIC == evt->evt) {
+		/*
+		 * In the new versions of CG29xx i.e. after CG2900 we
+		 * will recieve HCI_Event_VS_Write_File_Block_Complete
+		 * instead of Command Complete while downloading
+		 * the patches/settings file
+		 */
+		struct bt_vs_evt *cmd_evt;
+		cmd_evt = (struct bt_vs_evt *)data;
+
+		if (cmd_evt->evt_id == CG2900_EV_VS_WRITE_FILE_BLOCK_COMPLETE) {
+			struct bt_vs_write_file_block_evt *write_block_evt;
+			write_block_evt =
+				(struct bt_vs_write_file_block_evt *)
+				cmd_evt->data;
+			dev_dbg(dev->dev, "Received VS write file block complete\n");
+			pkt_handled = handle_vs_write_file_block_cmd_complete(
+				dev, &write_block_evt->status);
+		} else {
+			dev_err(dev->dev, "Vendor Specific Event 0x%x"
+					"Not Supported\n", cmd_evt->evt_id);
+			return false;
+		}
+	} else if (HCI_EV_HW_ERROR == evt->evt) {
+		struct hci_ev_hw_error *hw_error;
+
+		hw_error = (struct hci_ev_hw_error *)data;
+		/*
+		 * Only do a printout. There might be a receiving stack that can
+		 * handle this event
+		 */
+		dev_err(dev->dev, "HW Error event received with error 0x%02X\n",
+			hw_error->hw_code);
+		return false;
+	} else
+		return false;
+
+	if (pkt_handled)
+		kfree_skb(skb);
+
+	return pkt_handled;
+}
+
+/**
+ * transmit_skb_with_flow_ctrl_bt() - Send the BT skb to the controller if it is allowed or queue it.
+ * @user:	Current user.
+ * @skb:	Data packet.
+ *
+ * The transmit_skb_with_flow_ctrl_bt() function checks if there are
+ * tickets available and if so transmits buffer to controller. Otherwise the skb
+ * and user name is stored in a list for later sending.
+ * If enabled, copy the transmitted data to the HCI logger as well.
+ */
+static void transmit_skb_with_flow_ctrl_bt(struct cg2900_user_data *user,
+					   struct sk_buff *skb)
+{
+	struct cg2900_chip_dev *dev = cg2900_get_prv(user);
+	struct cg2900_chip_info *info = dev->c_data;
+
+	/*
+	 * Because there are more users of some H4 channels (currently audio
+	 * application for BT command and FM channel) we need to have an
+	 * internal HCI command flow control in CG2900 driver.
+	 * So check here how many tickets we have and store skb in a queue if
+	 * there are no tickets left. The skb will be sent later when we get
+	 * more ticket(s).
+	 */
+	spin_lock_bh(&info->tx_bt_lock);
+
+	if (info->tx_nr_pkts_allowed_bt > 0) {
+		info->tx_nr_pkts_allowed_bt--;
+		dev_dbg(user->dev, "New tx_nr_pkts_allowed_bt = %d\n",
+			info->tx_nr_pkts_allowed_bt);
+
+		/*
+		 * If it's command from audio app store the OpCode,
+		 * it'll be used later to decide where to dispatch Command
+		 * Complete event.
+		 */
+		if (info->bt_audio == user) {
+			struct hci_command_hdr *hdr = (struct hci_command_hdr *)
+				(skb->data + HCI_H4_SIZE);
+
+			info->audio_bt_cmd_op = le16_to_cpu(hdr->opcode);
+			dev_dbg(user->dev,
+				"Sending cmd from audio driver, saving "
+				"OpCode = 0x%X\n",
+				info->audio_bt_cmd_op);
+		}
+
+		cg2900_tx_to_chip(user, info->logger, skb);
+	} else {
+		dev_dbg(user->dev, "Not allowed to send cmd to controller, "
+			"storing in TX queue\n");
+
+		cg2900_skb_data(skb)->user = user;
+		skb_queue_tail(&info->tx_queue_bt, skb);
+	}
+	spin_unlock_bh(&info->tx_bt_lock);
+}
+
+/**
+ * transmit_skb_with_flow_ctrl_fm() - Send the FM skb to the controller if it is allowed or queue it.
+ * @user:	Current user.
+ * @skb:	Data packet.
+ *
+ * The transmit_skb_with_flow_ctrl_fm() function checks if chip is available and
+ * if so transmits buffer to controller. Otherwise the skb and user name is
+ * stored in a list for later sending.
+ * Also it updates the FM radio mode if it's FM GOTOMODE command, this is needed
+ * to know how to handle some FM DO commands complete events.
+ * If enabled, copy the transmitted data to the HCI logger as well.
+ */
+static void transmit_skb_with_flow_ctrl_fm(struct cg2900_user_data *user,
+					   struct sk_buff *skb)
+{
+	u8 cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	u16 cmd_id = CG2900_FM_CMD_NONE;
+	struct cg2900_chip_dev *dev = cg2900_get_prv(user);
+	struct cg2900_chip_info *info = dev->c_data;
+
+	fm_parse_cmd(&(skb->data[0]), &cmd_func, &cmd_id);
+
+	/*
+	 * If this is an FM IP disable or reset send command and also reset
+	 * the flow control and audio user.
+	 */
+	if (cmd_func == CG2900_FM_CMD_PARAM_DISABLE ||
+	    cmd_func == CG2900_FM_CMD_PARAM_RESET) {
+		spin_lock_bh(&info->tx_fm_lock);
+		fm_reset_flow_ctrl(info);
+		spin_unlock_bh(&info->tx_fm_lock);
+		cg2900_tx_to_chip(user, info->logger, skb);
+		return;
+	}
+
+	/*
+	 * If this is a FM user and no FM audio user command pending just send
+	 * FM command. It is up to the user of the FM channel to handle its own
+	 * flow control.
+	 */
+	spin_lock_bh(&info->tx_fm_lock);
+	if (info->fm_audio != user &&
+	    info->audio_fm_cmd_id == CG2900_FM_CMD_NONE) {
+		info->hci_fm_cmd_func = cmd_func;
+		dev_dbg(user->dev, "Sending FM radio command 0x%04X\n",
+			info->hci_fm_cmd_func);
+		/* If a GotoMode command update FM mode */
+		fm_update_mode(info, &skb->data[0]);
+		cg2900_tx_to_chip(user, info->logger, skb);
+	} else if (info->fm_audio == user &&
+		   info->hci_fm_cmd_func == CG2900_FM_CMD_PARAM_NONE &&
+		   info->audio_fm_cmd_id == CG2900_FM_CMD_NONE) {
+		/*
+		 * If it's command from fm audio user store the command id.
+		 * It'll be used later to decide where to dispatch
+		 * command complete event.
+		 */
+		info->audio_fm_cmd_id = cmd_id;
+		dev_dbg(user->dev, "Sending FM audio command 0x%04X\n",
+			info->audio_fm_cmd_id);
+		cg2900_tx_to_chip(user, info->logger, skb);
+	} else {
+		dev_dbg(user->dev,
+			"Not allowed to send FM cmd to controller, storing in "
+			"TX queue\n");
+
+		cg2900_skb_data(skb)->user = user;
+		skb_queue_tail(&info->tx_queue_fm, skb);
+	}
+	spin_unlock_bh(&info->tx_fm_lock);
+}
+
+/**
+ * is_bt_audio_user() - Checks if this packet is for the BT audio user.
+ * @info:	CG2900 info.
+ * @h4_channel:	H:4 channel for this packet.
+ * @skb:	Packet to check.
+ *
+ * Returns:
+ *   true if packet is for BT audio user.
+ *   false otherwise.
+ */
+static bool is_bt_audio_user(struct cg2900_chip_info *info, int h4_channel,
+			     const struct sk_buff * const skb)
+{
+	struct hci_event_hdr *hdr;
+	u8 *payload;
+	u16 opcode;
+
+	if (h4_channel != CHANNEL_BT_EVT)
+		return false;
+
+	hdr = (struct hci_event_hdr *)skb->data;
+	payload = (u8 *)(hdr + 1); /* follows header */
+
+	if (HCI_EV_CMD_COMPLETE == hdr->evt)
+		opcode = le16_to_cpu(
+			((struct hci_ev_cmd_complete *)payload)->opcode);
+	else if (HCI_EV_CMD_STATUS == hdr->evt)
+		opcode = le16_to_cpu(
+			((struct hci_ev_cmd_status *)payload)->opcode);
+	else
+		return false;
+
+	if (opcode != info->audio_bt_cmd_op)
+		return false;
+
+	dev_dbg(info->bt_audio->dev, "Audio BT OpCode match = 0x%04X\n",
+		opcode);
+	info->audio_bt_cmd_op = CG2900_BT_OPCODE_NONE;
+	return true;
+}
+
+/**
+ * is_fm_audio_user() - Checks if this packet is for the FM audio user.
+ * @info:	CG2900 info.
+ * @h4_channel:	H:4 channel for this packet.
+ * @skb:	Packet to check.
+ *
+ * Returns:
+ *   true if packet is for BT audio user.
+ *   false otherwise.
+ */
+static bool is_fm_audio_user(struct cg2900_chip_info *info, int h4_channel,
+			     const struct sk_buff * const skb)
+{
+	u8 cmd_func;
+	u16 cmd_id;
+	u16 irpt_val;
+	u8 event;
+
+	if (h4_channel != CHANNEL_FM_RADIO)
+		return false;
+
+	cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	cmd_id = CG2900_FM_CMD_NONE;
+	irpt_val = 0;
+	event = CG2900_FM_EVENT_UNKNOWN;
+
+	fm_parse_event(&skb->data[0], &event, &cmd_func, &cmd_id,
+		       &irpt_val);
+	/* Check if command complete event FM legacy interface. */
+	if ((event == CG2900_FM_EVENT_CMD_COMPLETE) &&
+	    (cmd_func == CG2900_FM_CMD_PARAM_WRITECOMMAND) &&
+	    (cmd_id == info->audio_fm_cmd_id)) {
+		dev_dbg(info->fm_audio->dev,
+			"FM Audio Function Code match = 0x%04X\n",
+			cmd_id);
+		return true;
+	}
+
+	/* Check if Interrupt legacy interface. */
+	if ((event == CG2900_FM_EVENT_INTERRUPT) &&
+	    (fm_is_do_cmd_irpt(irpt_val)) &&
+	    (info->tx_fm_audio_awaiting_irpt))
+		return true;
+
+	return false;
+}
+
+/**
+ * data_from_chip() - Called when data is received from the chip.
+ * @dev:	Chip info.
+ * @cg2900_dev:	CG2900 user for this packet.
+ * @skb:	Packet received.
+ *
+ * The data_from_chip() function updates flow control and checks
+ * if packet is a response for a packet it itself has transmitted. If not it
+ * finds the correct user and sends the packet* to the user.
+ */
+static void data_from_chip(struct cg2900_chip_dev *dev,
+			   struct sk_buff *skb)
+{
+	int h4_channel;
+	struct list_head *cursor;
+	struct cg2900_channel_item *tmp;
+	struct cg2900_chip_info *info = dev->c_data;
+	struct cg2900_user_data *user = NULL;
+
+	spin_lock_bh(&info->rw_lock);
+	/* Copy RX Data into logger.*/
+	if (info->logger)
+		cg2900_send_to_hci_logger(info->logger, skb,
+						LOGGER_DIRECTION_RX);
+
+	/*
+	 * HCI Raw user can only have exclusive access to chip, there won't be
+	 * other users once it's opened.
+	 */
+	if (info->hci_raw && info->hci_raw->opened) {
+		info->hci_raw->read_cb(info->hci_raw, skb);
+		spin_unlock_bh(&info->rw_lock);
+		return;
+	}
+
+	h4_channel = skb->data[0];
+	skb_pull(skb, HCI_H4_SIZE);
+
+	/* First check if it is a BT or FM audio event */
+	if (is_bt_audio_user(info, h4_channel, skb))
+		user = info->bt_audio;
+	else if (is_fm_audio_user(info, h4_channel, skb))
+		user = info->fm_audio;
+	spin_unlock_bh(&info->rw_lock);
+
+	/* Now check if we should update flow control */
+	if (h4_channel == CHANNEL_BT_EVT)
+		update_flow_ctrl_bt(dev, skb);
+	else if (h4_channel == CHANNEL_FM_RADIO)
+		update_flow_ctrl_fm(dev, skb);
+
+	/* Then check if this is a response to data we have sent */
+	if (h4_channel == CHANNEL_BT_EVT && handle_rx_data_bt_evt(dev, skb))
+		return;
+
+	spin_lock_bh(&info->rw_lock);
+
+	if (user)
+		goto user_found;
+
+	/* Let's see if it is the last user */
+	if (info->last_user && info->last_user->h4_channel == h4_channel) {
+		user = info->last_user;
+		goto user_found;
+	}
+
+	/*
+	 * Search through the list of all open channels to find the user.
+	 * We skip the audio channels since they have already been checked
+	 * earlier in this function.
+	 */
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user->h4_channel == h4_channel &&
+		    !tmp->user->is_audio) {
+			user = tmp->user;
+			goto user_found;
+		}
+	}
+
+user_found:
+	if (user != info->bt_audio && user != info->fm_audio)
+		info->last_user = user;
+
+	spin_unlock_bh(&info->rw_lock);
+
+	if (user)
+		user->read_cb(user, skb);
+	else {
+		dev_err(dev->dev,
+			"Could not find corresponding user to h4_channel %d\n",
+			h4_channel);
+		kfree_skb(skb);
+	}
+}
+
+/**
+ * chip_removed() - Called when transport has been removed.
+ * @dev:	Chip device.
+ *
+ * Removes registered MFD devices and frees internal resources.
+ */
+static void chip_removed(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_chip_info *info = dev->c_data;
+
+	cancel_delayed_work(&info->selftest_work.work);
+	mfd_remove_devices(dev->dev);
+	kfree(info->settings_file_name);
+	kfree(info->patch_file_name);
+	destroy_workqueue(info->wq);
+	kfree(info);
+	dev->c_data = NULL;
+	dev->c_cb.chip_removed = NULL;
+	dev->c_cb.data_from_chip = NULL;
+}
+
+/**
+ * last_bt_user_removed() - Called when last BT user is removed.
+ * @info:	Chip handler info.
+ *
+ * Clears out TX queue for BT.
+ */
+static void last_bt_user_removed(struct cg2900_chip_info *info)
+{
+	spin_lock_bh(&info->tx_bt_lock);
+	skb_queue_purge(&info->tx_queue_bt);
+
+	/*
+	 * Reset number of packets allowed and number of outstanding
+	 * BT commands.
+	 */
+	info->tx_nr_pkts_allowed_bt = 1;
+	/* Reset the audio_bt_cmd_op. */
+	info->audio_bt_cmd_op = CG2900_BT_OPCODE_NONE;
+	spin_unlock_bh(&info->tx_bt_lock);
+}
+
+/**
+ * last_fm_user_removed() - Called when last FM user is removed.
+ * @info:	Chip handler info.
+ *
+ * Clears out TX queue for BT.
+ */
+static void last_fm_user_removed(struct cg2900_chip_info *info)
+{
+	spin_lock_bh(&info->tx_fm_lock);
+	fm_reset_flow_ctrl(info);
+	spin_unlock_bh(&info->tx_fm_lock);
+}
+
+/**
+ * chip_shutdown() - Reset and power the chip off.
+ * @user:	MFD device.
+ */
+static void chip_shutdown(struct cg2900_user_data *user)
+{
+	struct hci_command_hdr cmd;
+	struct cg2900_chip_dev *dev = cg2900_get_prv(user);
+	struct cg2900_chip_info *info = dev->c_data;
+
+	dev_dbg(user->dev, "chip_shutdown\n");
+
+	/* First do a quick power switch of the chip to assure a good state */
+	if (dev->t_cb.set_chip_power)
+		dev->t_cb.set_chip_power(dev, false);
+
+	/*
+	 * Wait 50ms before continuing to be sure that the chip detects
+	 * chip power off.
+	 */
+	schedule_timeout_killable(
+			msecs_to_jiffies(LINE_TOGGLE_DETECT_TIMEOUT));
+
+	if (dev->t_cb.set_chip_power)
+		dev->t_cb.set_chip_power(dev, true);
+
+	/* Wait 100ms before continuing to be sure that the chip is ready */
+	schedule_timeout_killable(msecs_to_jiffies(CHIP_READY_TIMEOUT));
+
+	if (user != info->bt_audio && user != info->fm_audio)
+		info->last_user = user;
+	info->user_in_charge = user;
+
+	/*
+	 * Transmit HCI reset command to ensure the chip is using
+	 * the correct transport and to put BT part in reset.
+	 */
+	dev_dbg(user->dev, "New closing_state: CLOSING_RESET\n");
+	info->closing_state = CLOSING_RESET;
+	cmd.opcode = cpu_to_le16(HCI_OP_RESET);
+	cmd.plen = 0; /* No parameters for HCI reset */
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger, &cmd,
+			   sizeof(cmd));
+}
+
+/**
+ * chip_startup_finished() - Called when chip startup has finished.
+ * @info:	Chip handler info.
+ * @err:	Result of chip startup, 0 for no error.
+ *
+ * Shuts down the chip upon error, sets state to active, wakes waiting threads,
+ * and informs transport that startup has finished.
+ */
+static void chip_startup_finished(struct cg2900_chip_info *info, int err)
+{
+	dev_dbg(BOOT_DEV, "chip_startup_finished (%d)\n", err);
+
+	if (err)
+		/* Shutdown the chip */
+		cg2900_create_work_item(info->wq, work_chip_shutdown,
+					info->user_in_charge);
+	else {
+		dev_dbg(BOOT_DEV, "New main_state: CG2900_ACTIVE\n");
+		info->main_state = CG2900_ACTIVE;
+	}
+
+	wake_up_all(&main_wait_queue);
+
+	if (err) {
+		/*
+		 * This will wakeup clock user too
+		 * if it started the initialization process
+		 */
+		wake_up_all(&clk_user_wait_queue);
+		return;
+	}
+
+	if (!info->chip_dev->t_cb.chip_startup_finished)
+		dev_dbg(BOOT_DEV, "chip_startup_finished callback not found\n");
+	else
+		info->chip_dev->t_cb.chip_startup_finished(info->chip_dev);
+}
+
+/**
+ * cg2900_open() - Called when user wants to open an H4 channel.
+ * @user:	MFD device to open.
+ *
+ * Checks that H4 channel is not already opened. If chip is not started, starts
+ * up the chip. Sets channel as opened and adds user to active users.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL or read_cb is NULL.
+ *   -EBUSY if chip is in transit state (being started or shutdown).
+ *   -EACCES if H4 channel is already opened.
+ *   -ENOMEM if allocation fails.
+ *   -EIO if chip startup fails.
+ *   Error codes generated by t_cb.open.
+ */
+static int cg2900_open(struct cg2900_user_data *user)
+{
+	int err;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	struct list_head *cursor;
+	struct cg2900_channel_item *tmp;
+	enum main_state state_to_check = CG2900_ACTIVE;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	if (!user->read_cb) {
+		dev_err(user->dev, "cg2900_open: read_cb missing\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "cg2900_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	/* HCI Raw channel shall have exclusive access to chip. */
+	if (info->hci_raw && user->h4_channel != CHANNEL_HCI_RAW &&
+		user->h4_channel != CHANNEL_HCI_LOGGER) {
+		dev_err(user->dev, "cg2900_open: Cannot open %s "
+			"channel while HCI Raw channel is opened\n",
+			user->channel_data.char_dev_name);
+		return -EACCES;
+	}
+
+	mutex_lock(&main_info->man_mutex);
+
+	/*
+	 * Add a minor wait in order to avoid CPU blocking, looping openings.
+	 * Note there will of course be no wait if we are already in the right
+	 * state.
+	 */
+	err = wait_event_timeout(main_wait_queue,
+			(CG2900_IDLE == info->main_state ||
+			 CG2900_ACTIVE == info->main_state),
+			msecs_to_jiffies(LINE_TOGGLE_DETECT_TIMEOUT));
+	if (err <= 0) {
+		if (CG2900_INIT == info->main_state)
+			dev_err(user->dev, "Transport not opened\n");
+		else
+			dev_err(user->dev, "cg2900_open currently busy (0x%X). "
+				"Try again\n", info->main_state);
+		err = -EBUSY;
+		goto err_free_mutex;
+	}
+
+	err = 0;
+
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user->h4_channel == user->h4_channel &&
+		    tmp->user->is_audio == user->is_audio) {
+			dev_err(user->dev, "Channel %d is already opened\n",
+				user->h4_channel);
+			err = -EACCES;
+			goto err_free_mutex;
+		}
+	}
+
+	tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
+	if (!tmp) {
+		dev_err(user->dev, "Could not allocate tmp\n");
+		err = -ENOMEM;
+		goto err_free_mutex;
+	}
+	tmp->user = user;
+
+	if (CG2900_ACTIVE != info->main_state &&
+	    !user->chip_independent) {
+		/* Open transport and start-up the chip */
+		if (dev->t_cb.set_chip_power)
+			dev->t_cb.set_chip_power(dev, true);
+
+		/* Wait to be sure that the chip is ready */
+		schedule_timeout_killable(
+				msecs_to_jiffies(CHIP_READY_TIMEOUT));
+
+		if (dev->t_cb.open) {
+			err = dev->t_cb.open(dev);
+			if (err) {
+				if (dev->t_cb.set_chip_power)
+					dev->t_cb.set_chip_power(dev, false);
+				goto err_free_list_item;
+			}
+		}
+
+		/* Start the boot sequence */
+		info->user_in_charge = user;
+		if (user != info->bt_audio && user != info->fm_audio)
+			info->last_user = user;
+		dev_dbg(user->dev, "New boot_state: BOOT_GET_FILES_TO_LOAD\n");
+		info->boot_state = BOOT_GET_FILES_TO_LOAD;
+		dev_dbg(user->dev, "New main_state: CG2900_BOOTING\n");
+		info->main_state = CG2900_BOOTING;
+		cg2900_create_work_item(info->wq, work_load_patch_and_settings,
+					dev);
+
+		dev_dbg(user->dev, "Wait 15sec for chip to start\n");
+		if (user->is_clk_user) {
+			dev_dbg(user->dev, "Clock user is Waiting here\n");
+			wait_event_timeout(clk_user_wait_queue,
+				CG2900_ACTIVE_BEFORE_SELFTEST
+				== info->main_state,
+				msecs_to_jiffies(CHIP_STARTUP_TIMEOUT));
+
+			state_to_check = CG2900_ACTIVE_BEFORE_SELFTEST;
+		} else {
+			dev_dbg(user->dev, "Not a Clock user\n");
+			wait_event_timeout(main_wait_queue,
+				(CG2900_ACTIVE == info->main_state ||
+				 CG2900_IDLE   == info->main_state),
+				msecs_to_jiffies(CHIP_STARTUP_TIMEOUT));
+		}
+
+		if (state_to_check != info->main_state) {
+			dev_err(user->dev, "CG2900 init failed\n");
+
+			if (dev->t_cb.close)
+				dev->t_cb.close(dev);
+
+			dev_dbg(user->dev, "main_state: CG2900_IDLE\n");
+			info->main_state = CG2900_IDLE;
+			err = -EIO;
+			goto err_free_list_item;
+		}
+
+		if (CG2905_PG1_1_REV == dev->chip.hci_revision ||
+				CG2910_PG1_REV == dev->chip.hci_revision) {
+			/*
+			 * Switch to higher baud rate
+			 * Because of bug in CG2905/CG2910 PG1 H/W,
+			 * We have to download the ptc/ssf files
+			 * at lower baud and then switch to Higher Baud
+			 */
+			if (info->chip_dev->t_cb.set_baud_rate)
+				info->chip_dev->t_cb.set_baud_rate(
+						info->chip_dev, false);
+		}
+
+	}
+
+	list_add_tail(&tmp->list, &info->open_channels);
+
+	user->opened = true;
+
+	dev_dbg(user->dev, "H:4 channel opened\n");
+
+	mutex_unlock(&main_info->man_mutex);
+	return 0;
+err_free_list_item:
+	kfree(tmp);
+err_free_mutex:
+	mutex_unlock(&main_info->man_mutex);
+	return err;
+}
+
+/**
+ * cg2900_hci_log_open() - Called when user wants to open HCI logger channel.
+ * @user:	MFD device to open.
+ *
+ * Registers user as hci_logger and calls @cg2900_open to open the channel.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL.
+ *   -EEXIST if H4 channel is already opened.
+ *   Error codes generated by cg2900_open.
+ */
+static int cg2900_hci_log_open(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	int err;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_hci_log_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "cg2900_hci_log_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (info->logger) {
+		dev_err(user->dev, "HCI Logger already stored\n");
+		return -EEXIST;
+	}
+
+	info->logger = user;
+	err = cg2900_open(user);
+	if (err)
+		info->logger = NULL;
+	return err;
+}
+
+/**
+ * cg2900_hci_raw_open() - Called when user wants to open HCI Raw channel.
+ * @user:	MFD device to open.
+ *
+ * Registers user as hci_raw and calls @cg2900_open to open the channel.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL.
+ *   -EEXIST if H4 channel is already opened.
+ *   -EACCES if H4 channel iother than HCI RAW is already opened.
+ *   Error codes generated by cg2900_open.
+ */
+static int cg2900_hci_raw_open(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	struct list_head *cursor;
+	struct cg2900_channel_item *tmp;
+	int err;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_hci_raw_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "cg2900_hci_raw_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (info->hci_raw) {
+		dev_err(user->dev, "HCI Raw Channel already stored\n");
+		return -EEXIST;
+	}
+
+	if (!list_empty(&info->open_channels)) {
+		/*
+		 * Go through each open channel to check if it is logger
+		 * channel or some other channel.
+		 */
+		list_for_each(cursor, &info->open_channels) {
+			tmp = list_entry(cursor,
+					struct cg2900_channel_item, list);
+			if (tmp->user->h4_channel != CHANNEL_HCI_LOGGER) {
+				dev_err(user->dev, "Other channels other than "
+					"Logger is already opened. Cannot open "
+					"HCI Raw Channel\n");
+				return -EACCES;
+			}
+		}
+	}
+
+	info->hci_raw = user;
+	err = cg2900_open(user);
+	if (err)
+		info->hci_raw = NULL;
+	return err;
+}
+
+/**
+ * cg2900_bt_audio_open() - Called when user wants to open BT audio channel.
+ * @user:	MFD device to open.
+ *
+ * Registers user as bt_audio and calls @cg2900_open to open the channel.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL.
+ *   -EEXIST if H4 channel is already opened.
+ *   Error codes generated by cg2900_open.
+ */
+static int cg2900_bt_audio_open(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	int err;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_bt_audio_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "cg2900_bt_audio_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (info->bt_audio) {
+		dev_err(user->dev, "BT Audio already stored\n");
+		return -EEXIST;
+	}
+
+	info->bt_audio = user;
+	err = cg2900_open(user);
+	if (err)
+		info->bt_audio = NULL;
+	return err;
+}
+
+/**
+ * cg2900_fm_audio_open() - Called when user wants to open FM audio channel.
+ * @user:	MFD device to open.
+ *
+ * Registers user as fm_audio and calls @cg2900_open to open the channel.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL.
+ *   -EEXIST if H4 channel is already opened.
+ *   Error codes generated by cg2900_open.
+ */
+static int cg2900_fm_audio_open(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+	int err;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_fm_audio_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "cg2900_fm_audio_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (info->fm_audio) {
+		dev_err(user->dev, "FM Audio already stored\n");
+		return -EEXIST;
+	}
+
+	info->fm_audio = user;
+	err = cg2900_open(user);
+	if (err)
+		info->fm_audio = NULL;
+	return err;
+}
+
+/**
+ * cg2900_close() - Called when user wants to close an H4 channel.
+ * @user:	MFD device to close.
+ *
+ * Clears up internal resources, sets channel as closed, and shuts down chip if
+ * this was the last user.
+ */
+static void cg2900_close(struct cg2900_user_data *user)
+{
+	bool keep_powered = false;
+	struct list_head *cursor, *next;
+	struct cg2900_channel_item *tmp;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "cg2900_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	mutex_lock(&main_info->man_mutex);
+
+	/*
+	 * Go through each open channel. Remove our channel and check if there
+	 * is any other channel that want to keep the chip running
+	 */
+	list_for_each_safe(cursor, next, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		if (tmp->user == user) {
+			list_del(cursor);
+			kfree(tmp);
+		} else if (!tmp->user->chip_independent)
+			keep_powered = true;
+	}
+
+	if (user->h4_channel == CHANNEL_BT_CMD && !bt_is_open(info))
+		last_bt_user_removed(info);
+	else if (user->h4_channel == CHANNEL_FM_RADIO && !fm_is_open(info))
+		last_fm_user_removed(info);
+
+	if (keep_powered)
+		/* This was not the last user, we're done. */
+		goto finished;
+
+	if (CG2900_IDLE == info->main_state)
+		/* Chip has already been shut down. */
+		goto finished;
+
+	dev_dbg(user->dev, "New main_state: CG2900_CLOSING\n");
+	info->main_state = CG2900_CLOSING;
+	chip_shutdown(user);
+
+	dev_dbg(user->dev, "Wait up to 15 seconds for chip to shut-down\n");
+	wait_event_timeout(main_wait_queue,
+				(CG2900_IDLE == info->main_state),
+				msecs_to_jiffies(CHIP_SHUTDOWN_TIMEOUT));
+
+	/* Force shutdown if we timed out */
+	if (CG2900_IDLE != info->main_state) {
+		dev_err(user->dev,
+			"ST-Ericsson CG2900 Core Driver was shut-down with "
+			"problems\n");
+
+		if (dev->t_cb.close)
+			dev->t_cb.close(dev);
+
+		dev_dbg(user->dev, "New main_state: CG2900_IDLE\n");
+		info->main_state = CG2900_IDLE;
+	}
+
+finished:
+	mutex_unlock(&main_info->man_mutex);
+	user->opened = false;
+	dev_dbg(user->dev, "H:4 channel closed\n");
+}
+
+/**
+ * cg2900_hci_log_close() - Called when user wants to close HCI logger channel.
+ * @user:	MFD device to close.
+ *
+ * Clears hci_logger user and calls @cg2900_close to close the channel.
+ */
+static void cg2900_hci_log_close(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_hci_log_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "cg2900_hci_log_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (user != info->logger) {
+		dev_err(user->dev, "cg2900_hci_log_close: Trying to remove "
+			"another user\n");
+		return;
+	}
+
+	info->logger = NULL;
+	cg2900_close(user);
+}
+
+/**
+ * cg2900_hci_raw_close() - Called when user wants to close HCI Raw channel.
+ * @user:	MFD device to close.
+ *
+ * Clears hci_raw user and calls @cg2900_close to close the channel.
+ */
+static void cg2900_hci_raw_close(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_hci_raw_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "cg2900_hci_raw_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (user != info->hci_raw) {
+		dev_err(user->dev, "cg2900_hci_raw_close: Trying to remove "
+			"another user\n");
+		return;
+	}
+
+	info->hci_raw = NULL;
+	cg2900_close(user);
+}
+
+/**
+ * cg2900_bt_audio_close() - Called when user wants to close BT audio channel.
+ * @user:	MFD device to close.
+ *
+ * Clears bt_audio user and calls @cg2900_close to close the channel.
+ */
+static void cg2900_bt_audio_close(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_bt_audio_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "cg2900_bt_audio_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (user != info->bt_audio) {
+		dev_err(user->dev, "cg2900_bt_audio_close: Trying to remove "
+			"another user\n");
+		return;
+	}
+
+	info->bt_audio = NULL;
+	cg2900_close(user);
+}
+
+/**
+ * cg2900_fm_audio_close() - Called when user wants to close FM audio channel.
+ * @user:	MFD device to close.
+ *
+ * Clears fm_audio user and calls @cg2900_close to close the channel.
+ */
+static void cg2900_fm_audio_close(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"cg2900_fm_audio_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "cg2900_fm_audio_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (user != info->fm_audio) {
+		dev_err(user->dev, "cg2900_fm_audio_close: Trying to remove "
+			"another user\n");
+		return;
+	}
+
+	info->fm_audio = NULL;
+	cg2900_close(user);
+}
+
+/**
+ * cg2900_reset() - Called when user wants to reset the chip.
+ * @user:	MFD device to reset.
+ *
+ * Closes down the chip and calls reset_cb for all open users.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user is NULL.
+ */
+static int cg2900_reset(struct cg2900_user_data *user)
+{
+	struct list_head *cursor, *next;
+	struct cg2900_channel_item *tmp;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_reset: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	dev_info(user->dev, "cg2900_reset\n");
+
+	BUG_ON(!main_info);
+
+	mutex_lock(&main_info->man_mutex);
+
+	dev_dbg(user->dev, "New main_state: CG2900_RESETING\n");
+	info->main_state = CG2900_RESETING;
+
+	chip_shutdown(user);
+
+	/*
+	 * Inform all opened channels about the reset and free the user devices
+	 */
+	list_for_each_safe(cursor, next, &info->open_channels) {
+		tmp = list_entry(cursor, struct cg2900_channel_item, list);
+		list_del(cursor);
+		tmp->user->opened  = false;
+		tmp->user->reset_cb(tmp->user);
+		kfree(tmp);
+	}
+
+	/* Reset finished. We are now idle until first channel is opened */
+	dev_dbg(user->dev, "New main_state: CG2900_IDLE\n");
+	info->main_state = CG2900_IDLE;
+
+	mutex_unlock(&main_info->man_mutex);
+
+	/*
+	 * Send wake-up since this might have been called from a failed boot.
+	 * No harm done if it is a CG2900 chip user who called.
+	 */
+	wake_up_all(&main_wait_queue);
+
+	return 0;
+}
+
+/**
+ * cg2900_alloc_skb() - Allocates socket buffer.
+ * @size:	Sk_buffer size in bytes.
+ * @priority:	GFP priorit for allocation.
+ *
+ * Allocates a sk_buffer and reserves space for H4 header.
+ *
+ * Returns:
+ *   sk_buffer if success.
+ *   NULL if allocation fails.
+ */
+static struct sk_buff *cg2900_alloc_skb(unsigned int size, gfp_t priority)
+{
+	struct sk_buff *skb;
+
+	dev_dbg(MAIN_DEV, "cg2900_alloc_skb size %d bytes\n", size);
+
+	/* Allocate the SKB and reserve space for the header */
+	skb = alloc_skb(size + CG2900_SKB_RESERVE, priority);
+	if (skb)
+		skb_reserve(skb, CG2900_SKB_RESERVE);
+
+	return skb;
+}
+
+/**
+ * cg2900_write() - Called when user wants to write to the chip.
+ * @user:	MFD device representing H4 channel to write to.
+ * @skb:	Sk_buffer to transmit.
+ *
+ * Transmits the sk_buffer to the chip. If it is a BT cmd or FM audio packet it
+ * is checked that it is allowed to transmit the chip.
+ * Note that if error is returned it is up to the user to free the skb.
+ *
+ * Returns:
+ *   0 if success.
+ *   -EINVAL if user or skb is NULL.
+ *   -EACCES if channel is closed.
+ */
+static int cg2900_write(struct cg2900_user_data *user, struct sk_buff *skb)
+{
+	u8 *h4_header;
+	struct cg2900_chip_dev *dev;
+	struct cg2900_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_write: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	if (!skb) {
+		dev_err(user->dev, "cg2900_write with no sk_buffer\n");
+		return -EINVAL;
+	}
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	dev_dbg(user->dev, "cg2900_write length %d bytes\n", skb->len);
+
+	if (!user->opened) {
+		dev_err(user->dev,
+			"Trying to transmit data on a closed channel\n");
+		return -EACCES;
+	}
+
+	if (user->h4_channel == CHANNEL_HCI_RAW) {
+		/*
+		 * Since the data transmitted on HCI Raw channel
+		 * can be byte by byte, flow control cannot be used.
+		 * This should be handled by user space application
+		 * of the HCI Raw channel, so just transmit the
+		 * received data to chip.
+		 */
+		cg2900_tx_to_chip(user, info->logger, skb);
+		return 0;
+	}
+
+	/*
+	 * Move the data pointer to the H:4 header position and
+	 * store the H4 header.
+	 */
+	h4_header = skb_push(skb, CG2900_SKB_RESERVE);
+	*h4_header = (u8)user->h4_channel;
+
+	if (user->h4_channel == CHANNEL_BT_CMD)
+		transmit_skb_with_flow_ctrl_bt(user, skb);
+	else if (user->h4_channel == CHANNEL_FM_RADIO)
+		transmit_skb_with_flow_ctrl_fm(user, skb);
+	else
+		cg2900_tx_to_chip(user, info->logger, skb);
+
+	return 0;
+}
+
+/**
+ * cg2900_no_write() - Used for channels where it is not allowed to write.
+ * @user:	MFD device representing H4 channel to write to.
+ * @skb:	Sk_buffer to transmit.
+ *
+ * Returns:
+ *   -EPERM.
+ */
+static int cg2900_no_write(struct cg2900_user_data *user,
+			   __attribute__((unused)) struct sk_buff *skb)
+{
+	dev_err(user->dev, "Not allowed to send on this channel\n");
+	return -EPERM;
+}
+
+/**
+ * cg2900_get_local_revision() - Called to retrieve revision data for the chip.
+ * @user:	MFD device to check.
+ * @rev_data:	Revision data to fill in.
+ *
+ * Returns:
+ *   true if success.
+ *   false upon failure.
+ */
+static bool cg2900_get_local_revision(struct cg2900_user_data *user,
+				      struct cg2900_rev_data *rev_data)
+{
+	struct cg2900_chip_dev *dev;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "cg2900_get_local_revision: Calling with "
+			"NULL pointer\n");
+		return false;
+	}
+
+	if (!rev_data) {
+		dev_err(user->dev, "Calling with rev_data NULL\n");
+		return false;
+	}
+
+	dev = cg2900_get_prv(user);
+
+	rev_data->revision = dev->chip.hci_revision;
+	rev_data->sub_version = dev->chip.hci_sub_version;
+
+	return true;
+}
+
+static struct cg2900_user_data btcmd_data = {
+	.h4_channel = CHANNEL_BT_CMD,
+};
+static struct cg2900_user_data btacl_data = {
+	.h4_channel = CHANNEL_BT_ACL,
+};
+static struct cg2900_user_data btevt_data = {
+	.h4_channel = CHANNEL_BT_EVT,
+};
+static struct cg2900_user_data nfc_data = {
+	.h4_channel = CHANNEL_NFC,
+};
+static struct cg2900_user_data fm_data = {
+	.h4_channel = CHANNEL_FM_RADIO,
+};
+static struct cg2900_user_data gnss_data = {
+	.h4_channel = CHANNEL_GNSS,
+};
+static struct cg2900_user_data debug_data = {
+	.h4_channel = CHANNEL_DEBUG,
+};
+static struct cg2900_user_data ste_tools_data = {
+	.h4_channel = CHANNEL_STE_TOOLS,
+};
+static struct cg2900_user_data hci_logger_data = {
+	.h4_channel = CHANNEL_HCI_LOGGER,
+	.chip_independent = true,
+	.write = cg2900_no_write,
+	.open = cg2900_hci_log_open,
+	.close = cg2900_hci_log_close,
+};
+static struct cg2900_user_data core_data = {
+	.h4_channel = CHANNEL_CORE,
+	.write = cg2900_no_write,
+};
+static struct cg2900_user_data audio_bt_data = {
+	.h4_channel = CHANNEL_BT_CMD,
+	.is_audio = true,
+	.open = cg2900_bt_audio_open,
+	.close = cg2900_bt_audio_close,
+};
+static struct cg2900_user_data audio_fm_data = {
+	.h4_channel = CHANNEL_FM_RADIO,
+	.is_audio = true,
+	.open = cg2900_fm_audio_open,
+	.close = cg2900_fm_audio_close,
+};
+static struct cg2900_user_data hci_raw_data = {
+	.h4_channel = CHANNEL_HCI_RAW,
+	.open = cg2900_hci_raw_open,
+	.close = cg2900_hci_raw_close,
+};
+
+static struct mfd_cell cg2900_devs[] = {
+	{
+		.name = "cg2900-btcmd",
+		.platform_data = &btcmd_data,
+		.pdata_size = sizeof(btcmd_data),
+	},
+	{
+		.name = "cg2900-btacl",
+		.platform_data = &btacl_data,
+		.pdata_size = sizeof(btacl_data),
+	},
+	{
+		.name = "cg2900-btevt",
+		.platform_data = &btevt_data,
+		.pdata_size = sizeof(btevt_data),
+	},
+	{
+		.name = "cg2900-fm",
+		.platform_data = &fm_data,
+		.pdata_size = sizeof(fm_data),
+	},
+	{
+		.name = "cg2900-gnss",
+		.platform_data = &gnss_data,
+		.pdata_size = sizeof(gnss_data),
+	},
+	{
+		.name = "cg2900-debug",
+		.platform_data = &debug_data,
+		.pdata_size = sizeof(debug_data),
+	},
+	{
+		.name = "cg2900-stetools",
+		.platform_data = &ste_tools_data,
+		.pdata_size = sizeof(ste_tools_data),
+	},
+	{
+		.name = "cg2900-hcilogger",
+		.platform_data = &hci_logger_data,
+		.pdata_size = sizeof(hci_logger_data),
+	},
+	{
+		.name = "cg2900-core",
+		.platform_data = &core_data,
+		.pdata_size = sizeof(core_data),
+	},
+	{
+		.name = "cg2900-audiobt",
+		.platform_data = &audio_bt_data,
+		.pdata_size = sizeof(audio_bt_data),
+	},
+	{
+		.name = "cg2900-audiofm",
+		.platform_data = &audio_fm_data,
+		.pdata_size = sizeof(audio_fm_data),
+	},
+	{
+		.name = "cg2900-hciraw",
+		.platform_data = &hci_raw_data,
+		.pdata_size = sizeof(hci_raw_data),
+	},
+};
+
+static struct mfd_cell cg2910_extra_devs[] = {
+	{
+		.name = "cg2900-nfc",
+		.platform_data = &nfc_data,
+		.pdata_size = sizeof(nfc_data)
+	}
+};
+
+static struct cg2900_user_data char_btcmd_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_BT_CMD,
+	},
+	.h4_channel = CHANNEL_BT_CMD,
+};
+static struct cg2900_user_data char_btacl_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_BT_ACL,
+	},
+	.h4_channel = CHANNEL_BT_ACL,
+};
+static struct cg2900_user_data char_btevt_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_BT_EVT,
+	},
+	.h4_channel = CHANNEL_BT_EVT,
+};
+static struct cg2900_user_data char_nfc_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_NFC,
+	},
+	.h4_channel = CHANNEL_NFC,
+};
+static struct cg2900_user_data char_fm_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_FM_RADIO,
+	},
+	.h4_channel = CHANNEL_FM_RADIO,
+};
+static struct cg2900_user_data char_gnss_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_GNSS,
+	},
+	.h4_channel = CHANNEL_GNSS,
+};
+static struct cg2900_user_data char_debug_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_DEBUG,
+	},
+	.h4_channel = CHANNEL_DEBUG,
+};
+static struct cg2900_user_data char_ste_tools_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_STE_TOOLS,
+	},
+	.h4_channel = CHANNEL_STE_TOOLS,
+};
+static struct cg2900_user_data char_hci_logger_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_HCI_LOGGER,
+	},
+	.h4_channel = CHANNEL_HCI_LOGGER,
+	.chip_independent = true,
+	.write = cg2900_no_write,
+	.open = cg2900_hci_log_open,
+	.close = cg2900_hci_log_close,
+};
+static struct cg2900_user_data char_core_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_CORE,
+	},
+	.h4_channel = CHANNEL_CORE,
+	.write = cg2900_no_write,
+};
+static struct cg2900_user_data char_audio_bt_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_BT_AUDIO,
+	},
+	.h4_channel = CHANNEL_BT_CMD,
+	.is_audio = true,
+};
+static struct cg2900_user_data char_audio_fm_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_FM_AUDIO,
+	},
+	.h4_channel = CHANNEL_FM_RADIO,
+	.is_audio = true,
+};
+static struct cg2900_user_data char_hci_raw_data = {
+	.channel_data = {
+		.char_dev_name = CG2900_HCI_RAW,
+	},
+	.h4_channel = CHANNEL_HCI_RAW,
+	.open = cg2900_hci_raw_open,
+	.close = cg2900_hci_raw_close,
+};
+
+
+static struct mfd_cell cg2900_char_devs[] = {
+	{
+		.name = "cg2900-chardev",
+		.id = 0,
+		.platform_data = &char_btcmd_data,
+		.pdata_size = sizeof(char_btcmd_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 1,
+		.platform_data = &char_btacl_data,
+		.pdata_size = sizeof(char_btacl_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 2,
+		.platform_data = &char_btevt_data,
+		.pdata_size = sizeof(char_btevt_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 3,
+		.platform_data = &char_fm_data,
+		.pdata_size = sizeof(char_fm_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 4,
+		.platform_data = &char_gnss_data,
+		.pdata_size = sizeof(char_gnss_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 5,
+		.platform_data = &char_debug_data,
+		.pdata_size = sizeof(char_debug_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 6,
+		.platform_data = &char_ste_tools_data,
+		.pdata_size = sizeof(char_ste_tools_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 7,
+		.platform_data = &char_hci_logger_data,
+		.pdata_size = sizeof(char_hci_logger_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 8,
+		.platform_data = &char_core_data,
+		.pdata_size = sizeof(char_core_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 9,
+		.platform_data = &char_audio_bt_data,
+		.pdata_size = sizeof(char_audio_bt_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 10,
+		.platform_data = &char_audio_fm_data,
+		.pdata_size = sizeof(char_audio_fm_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 11,
+		.platform_data = &char_hci_raw_data,
+		.pdata_size = sizeof(char_hci_raw_data),
+	},
+};
+
+static struct mfd_cell cg2910_extra_char_devs[] = {
+	{
+		.name = "cg2900-chardev",
+		.id = 12,
+		.platform_data = &char_nfc_data,
+		.pdata_size = sizeof(char_nfc_data)
+	}
+};
+
+/**
+ * set_plat_data() - Initializes data for an MFD cell.
+ * @cell:	MFD cell.
+ * @dev:	Current chip.
+ *
+ * Sets each callback to default function unless already set.
+ */
+static void set_plat_data(struct mfd_cell *cell, struct cg2900_chip_dev *dev)
+{
+	struct cg2900_user_data *pf_data = cell->platform_data;
+
+	if (!pf_data->open)
+		pf_data->open = cg2900_open;
+	if (!pf_data->close)
+		pf_data->close = cg2900_close;
+	if (!pf_data->reset)
+		pf_data->reset = cg2900_reset;
+	if (!pf_data->alloc_skb)
+		pf_data->alloc_skb = cg2900_alloc_skb;
+	if (!pf_data->write)
+		pf_data->write = cg2900_write;
+	if (!pf_data->get_local_revision)
+		pf_data->get_local_revision = cg2900_get_local_revision;
+
+	cg2900_set_prv(pf_data, dev);
+}
+
+/**
+ * check_chip_support() - Checks if connected chip is handled by this driver.
+ * @dev:	Chip info structure.
+ *
+ * First check if chip is supported by this driver. If that is the case fill in
+ * the callbacks in @dev and initiate internal variables. Finally create MFD
+ * devices for all supported H4 channels. When finished power off the chip.
+ *
+ * Returns:
+ *   true if chip is handled by this driver.
+ *   false otherwise.
+ */
+static bool check_chip_support(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_platform_data *pf_data;
+	struct cg2900_chip_info *info;
+	int i;
+
+	dev_dbg(dev->dev, "check_chip_support\n");
+
+	/*
+	 * Check if this is a CG29XX revision.
+	 * We do not care about the sub-version at the moment. Change this if
+	 * necessary.
+	 */
+	if (dev->chip.manufacturer != CG2900_SUPP_MANUFACTURER
+		|| !(check_chip_revision_support(dev->chip.hci_revision))) {
+		dev_err(dev->dev, "Unsupported Chip revision:0x%x\n",
+				dev->chip.hci_revision);
+		return false;
+	}
+
+	/* Store needed data */
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev->dev, "Couldn't allocate info struct\n");
+		return false;
+	}
+
+	/* Initialize all variables */
+	skb_queue_head_init(&info->tx_queue_bt);
+	skb_queue_head_init(&info->tx_queue_fm);
+
+	INIT_LIST_HEAD(&info->open_channels);
+
+	spin_lock_init(&info->tx_bt_lock);
+	spin_lock_init(&info->tx_fm_lock);
+	spin_lock_init(&info->rw_lock);
+
+	info->tx_nr_pkts_allowed_bt = 1;
+	info->audio_bt_cmd_op = CG2900_BT_OPCODE_NONE;
+	info->audio_fm_cmd_id = CG2900_FM_CMD_NONE;
+	info->hci_fm_cmd_func = CG2900_FM_CMD_PARAM_NONE;
+	info->fm_radio_mode = FM_RADIO_MODE_IDLE;
+	info->chip_dev = dev;
+	info->dev = dev->dev;
+
+	info->wq = create_singlethread_workqueue(WQ_NAME);
+	if (!info->wq) {
+		dev_err(dev->dev, "Could not create workqueue\n");
+		goto err_handling_free_info;
+	}
+
+	info->patch_file_name = kzalloc(NAME_MAX + 1, GFP_ATOMIC);
+	if (!info->patch_file_name) {
+		dev_err(dev->dev,
+			"Couldn't allocate name buffer for patch file\n");
+		goto err_handling_destroy_wq;
+	}
+
+	info->settings_file_name = kzalloc(NAME_MAX + 1, GFP_ATOMIC);
+	if (!info->settings_file_name) {
+		dev_err(dev->dev,
+			"Couldn't allocate name buffers settings file\n");
+		goto err_handling_free_patch_name;
+	}
+
+	info->selftest_work.data = info;
+	INIT_DELAYED_WORK(&info->selftest_work.work,
+			  work_send_read_selftest_cmd);
+
+	dev->c_data = info;
+	/* Set the callbacks */
+	dev->c_cb.data_from_chip = data_from_chip;
+	dev->c_cb.chip_removed = chip_removed;
+
+	mutex_lock(&main_info->man_mutex);
+
+	pf_data = dev_get_platdata(dev->dev);
+	btcmd_data.channel_data.bt_bus = pf_data->bus;
+	btacl_data.channel_data.bt_bus = pf_data->bus;
+	btevt_data.channel_data.bt_bus = pf_data->bus;
+
+	/* Set the Platform data based on supported chip */
+	for (i = 0; i < ARRAY_SIZE(cg2900_devs); i++)
+		set_plat_data(&cg2900_devs[i], dev);
+	for (i = 0; i < ARRAY_SIZE(cg2900_char_devs); i++)
+		set_plat_data(&cg2900_char_devs[i], dev);
+	info->mfd_size = ARRAY_SIZE(cg2900_devs);
+	info->mfd_char_size = ARRAY_SIZE(cg2900_char_devs);
+
+	if (dev->chip.hci_revision == CG2910_PG1_REV ||
+			dev->chip.hci_revision == CG2910_PG2_REV) {
+		for (i = 0; i < ARRAY_SIZE(cg2910_extra_devs); i++)
+			set_plat_data(&cg2910_extra_devs[i], dev);
+		for (i = 0; i < ARRAY_SIZE(cg2910_extra_char_devs); i++)
+			set_plat_data(&cg2910_extra_char_devs[i], dev);
+		info->mfd_extra_size = ARRAY_SIZE(cg2910_extra_devs);
+		info->mfd_extra_char_size = ARRAY_SIZE(cg2910_extra_char_devs);
+	}
+
+
+	info->startup = true;
+
+	/*
+	 * The devices will be registered when chip has been powered down, i.e.
+	 * when the system startup is ready.
+	 */
+
+	mutex_unlock(&main_info->man_mutex);
+
+	dev_info(dev->dev, "Chip supported by the CG2900 driver\n");
+
+	/* Finish by turning off the chip */
+	cg2900_create_work_item(info->wq, work_power_off_chip, dev);
+
+	return true;
+
+err_handling_free_patch_name:
+	kfree(info->patch_file_name);
+err_handling_destroy_wq:
+	destroy_workqueue(info->wq);
+err_handling_free_info:
+	kfree(info);
+	return false;
+}
+
+static struct cg2900_id_callbacks chip_support_callbacks = {
+	.check_chip_support = check_chip_support,
+};
+
+/**
+ * cg2900_chip_probe() - Initialize CG2900 chip handler resources.
+ * @pdev:	Platform device.
+ *
+ * This function initializes the CG2900 driver, then registers to
+ * the CG2900 Core.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ *   Error codes generated by cg2900_register_chip_driver.
+ */
+static int __devinit cg2900_chip_probe(struct platform_device *pdev)
+{
+	int err;
+
+	dev_dbg(&pdev->dev, "cg2900_chip_probe\n");
+
+	main_info = kzalloc(sizeof(*main_info), GFP_ATOMIC);
+	if (!main_info) {
+		dev_err(&pdev->dev, "Couldn't allocate main_info\n");
+		return -ENOMEM;
+	}
+
+	main_info->dev = &pdev->dev;
+	mutex_init(&main_info->man_mutex);
+
+	err = cg2900_register_chip_driver(&chip_support_callbacks);
+	if (err) {
+		dev_err(&pdev->dev,
+			"Couldn't register chip driver (%d)\n", err);
+		goto error_handling;
+	}
+
+	dev_info(&pdev->dev, "CG2900 chip driver started\n");
+
+	return 0;
+
+error_handling:
+	mutex_destroy(&main_info->man_mutex);
+	kfree(main_info);
+	main_info = NULL;
+	return err;
+}
+
+/**
+ * cg2900_chip_remove() - Release CG2900 chip handler resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success (always success).
+ */
+static int __devexit cg2900_chip_remove(struct platform_device *pdev)
+{
+	dev_info(&pdev->dev, "CG2900 chip driver removed\n");
+
+	cg2900_deregister_chip_driver(&chip_support_callbacks);
+
+	if (!main_info)
+		return 0;
+	mutex_destroy(&main_info->man_mutex);
+	kfree(main_info);
+	main_info = NULL;
+	return 0;
+}
+
+static struct platform_driver cg2900_chip_driver = {
+	.driver = {
+		.name	= "cg2900-chip",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_chip_probe,
+	.remove	= __devexit_p(cg2900_chip_remove),
+};
+
+/**
+ * cg2900_chip_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_chip_init(void)
+{
+	pr_debug("cg2900_chip_init");
+	return platform_driver_register(&cg2900_chip_driver);
+}
+
+/**
+ * cg2900_chip_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_chip_exit(void)
+{
+	pr_debug("cg2900_chip_exit");
+	platform_driver_unregister(&cg2900_chip_driver);
+}
+
+module_init(cg2900_chip_init);
+module_exit(cg2900_chip_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux CG2900 Connectivity Device Driver");
diff --git a/drivers/staging/cg2900/mfd/cg2900_chip.h b/drivers/staging/cg2900/mfd/cg2900_chip.h
new file mode 100644
index 00000000000..5db655903a8
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_chip.h
@@ -0,0 +1,619 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * Hemant Gupta (hemant.gupta@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+
+#ifndef _CG2900_CHIP_H_
+#define _CG2900_CHIP_H_
+
+/*
+ *	Utility
+ */
+
+static inline void set_low_nibble(__u8 *var, __u8 value)
+{
+	*var = (*var & 0xf0) | (value & 0x0f);
+}
+
+static inline void set_high_nibble(__u8 *var, __u8 value)
+{
+	*var = (*var & 0x0f) | (value << 4);
+}
+
+static inline void store_bit(__u8 *var, size_t bit, __u8 value)
+{
+	*var = (*var & ~(1u << bit)) | (value << bit);
+}
+
+/*
+ *	General chip defines
+ */
+
+/* Supported chips */
+#define CG2900_SUPP_MANUFACTURER			0x30
+
+/*
+ *	Bluetooth
+ */
+
+#define BT_SIZE_OF_HDR				(sizeof(__le16) + sizeof(__u8))
+#define BT_PARAM_LEN(__pkt_len)			(__pkt_len - BT_SIZE_OF_HDR)
+
+struct bt_cmd_cmpl_event {
+	__u8	eventcode;
+	__u8	plen;
+	__u8	n_commands;
+	__le16	opcode;
+	/*
+	 * According to BT-specification what follows is "parameters"
+	 * and unique to every command, but all commands start the
+	 * parameters with the status field so include it here for
+	 * convenience
+	 */
+	__u8	status;
+	__u8	data[];
+} __packed;
+
+/* BT VS Store In FS command */
+#define CG2900_BT_OP_VS_STORE_IN_FS			0xFC22
+struct bt_vs_store_in_fs_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	user_id;
+	__u8	len;
+	__u8	data[];
+} __packed;
+
+#define CG2900_VS_STORE_IN_FS_USR_ID_BD_ADDR		0xFE
+
+#define HCI_EV_VENDOR_SPECIFIC					0xFF
+#define CG2900_EV_VS_WRITE_FILE_BLOCK_COMPLETE	0x60
+/* BT VS Event */
+struct bt_vs_evt {
+	__u8	evt_id;
+	__u8	data[];
+} __packed;
+
+/* BT VS Write File Block Event */
+struct bt_vs_write_file_block_evt {
+	__u8	status;
+	__u8	file_blk_id;
+} __packed;
+
+/* BT VS Write File Block command */
+#define CG2900_BT_OP_VS_WRITE_FILE_BLOCK		0xFC2E
+struct bt_vs_write_file_block_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+	__u8	data[];
+} __packed;
+
+#define CG2900_BT_DISABLE				0x00
+#define CG2900_BT_ENABLE				0x01
+
+/* BT VS BT Enable command */
+#define CG2900_BT_OP_VS_BT_ENABLE			0xFF10
+struct bt_vs_bt_enable_cmd {
+	__le16	op_code;
+	u8	plen;
+	u8	enable;
+} __packed;
+
+/* Bytes in the command Hci_Cmd_ST_Set_Uart_Baud_Rate */
+#define CG2900_BAUD_RATE_57600				0x03
+#define CG2900_BAUD_RATE_115200				0x02
+#define CG2900_BAUD_RATE_230400				0x01
+#define CG2900_BAUD_RATE_460800				0x00
+#define CG2900_BAUD_RATE_921600				0x20
+#define CG2900_BAUD_RATE_2000000			0x25
+#define CG2900_BAUD_RATE_3000000			0x27
+#define CG2900_BAUD_RATE_3250000			0x28
+#define CG2900_BAUD_RATE_4000000			0x2B
+
+/* BT VS SetBaudRate command */
+#define CG2900_BT_OP_VS_SET_BAUD_RATE			0xFC09
+struct bt_vs_set_baud_rate_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	baud_rate;
+} __packed;
+
+#define CG2900_BT_SELFTEST_SUCCESSFUL			0x00
+#define CG2900_BT_SELFTEST_FAILED			0x01
+#define CG2900_BT_SELFTEST_NOT_COMPLETED		0x02
+
+/* BT VS ReadSelfTestsResult command & event */
+#define CG2900_BT_OP_VS_READ_SELTESTS_RESULT		0xFC10
+struct bt_vs_read_selftests_result_evt {
+	__u8	status;
+	__u8	result;
+} __packed;
+
+/* Bluetooth Vendor Specific Opcodes */
+#define CG2900_BT_OP_VS_POWER_SWITCH_OFF		0xFD40
+#define CG2900_BT_OP_VS_SYSTEM_RESET			0xFF12
+
+#define CG2900_BT_OPCODE_NONE				0xFFFF
+
+/*
+ *	Common multimedia
+ */
+
+#define CG2900_CODEC_TYPE_NONE				0x00
+#define CG2900_CODEC_TYPE_SBC				0x01
+
+#define CG2900_PCM_MODE_SLAVE				0x00
+#define CG2900_PCM_MODE_MASTER				0x01
+
+#define CG2900_I2S_MODE_MASTER				0x00
+#define CG2900_I2S_MODE_SLAVE				0x01
+
+/*
+ *	CG2900 PG1 multimedia API
+ */
+
+#define CG2900_BT_VP_TYPE_PCM				0x00
+#define CG2900_BT_VP_TYPE_I2S				0x01
+#define CG2900_BT_VP_TYPE_SLIMBUS			0x02
+#define CG2900_BT_VP_TYPE_FM				0x03
+#define CG2900_BT_VP_TYPE_BT_SCO			0x04
+#define CG2900_BT_VP_TYPE_BT_A2DP			0x05
+#define CG2900_BT_VP_TYPE_ANALOG			0x07
+
+#define CG2900_BT_VS_SET_HARDWARE_CONFIG		0xFD54
+/* These don't have the same length, so a union won't work */
+struct bt_vs_set_hw_cfg_cmd_pcm {
+	__le16	opcode;
+	__u8	plen;
+	__u8	vp_type;
+	__u8	port_id;
+	__u8	mode_dir; /* NB: mode is in bit 1 (not 0) */
+	__u8	bit_clock;
+	__le16	frame_len;
+} __packed;
+#define HWCONFIG_PCM_SET_MODE(pcfg, mode)		\
+	set_low_nibble(&(pcfg)->mode_dir, (mode) << 1)
+#define HWCONFIG_PCM_SET_DIR(pcfg, idx, dir)		\
+	store_bit(&(pcfg)->mode_dir, (idx) + 4, (dir))
+
+struct bt_vs_set_hw_cfg_cmd_i2s {
+	__le16	opcode;
+	__u8	plen;
+	__u8	vp_type;
+	__u8	port_id;
+	__u8	half_period;
+	__u8	master_slave;
+} __packed;
+
+/* Max length for allocating */
+#define CG2900_BT_LEN_VS_SET_HARDWARE_CONFIG	\
+	(sizeof(struct bt_vs_set_hw_cfg_cmd_pcm))
+
+#define CG2900_BT_VS_SET_SESSION_CONFIG			0xFD55
+struct session_config_vport {
+	__u8	type;
+	union {
+		struct {
+			__le16	acl_handle;
+			__u8	reserved[10];
+		} sco;
+		struct {
+			__u8	reserved[12];
+		} fm;
+		struct {
+			__u8	index;
+			__u8	slots_used;
+			__u8	slot_start[4];
+			__u8	reserved[6];
+		} pcm;
+		struct {
+			__u8	index;
+			__u8	channel;
+			__u8	reserved[10];
+		} i2s;
+	};
+} __packed;
+#define SESSIONCFG_PCM_SET_USED(port, idx, use)		\
+	store_bit(&(port).pcm.slots_used, (idx), (use))
+
+struct session_config_stream {
+	__u8	media_type;
+	__u8	csel_srate;
+	__u8	codec_type;
+	__u8	codec_mode;
+	__u8	codec_params[3];
+	struct session_config_vport inport;
+	struct session_config_vport outport;
+} __packed;
+#define SESSIONCFG_SET_CHANNELS(pcfg, chnl)		\
+	set_low_nibble(&(pcfg)->csel_srate, (chnl))
+#define SESSIONCFG_I2S_SET_SRATE(pcfg, rate)		\
+	set_high_nibble(&(pcfg)->csel_srate, (rate))
+
+struct bt_vs_session_config_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	n_streams; /* we only support one here */
+	struct session_config_stream stream;
+} __packed;
+
+#define CG2900_BT_SESSION_MEDIA_TYPE_AUDIO		0x00
+
+#define CG2900_BT_SESSION_RATE_8K			0x01
+#define CG2900_BT_SESSION_RATE_16K			0x02
+#define CG2900_BT_SESSION_RATE_44_1K			0x04
+#define CG2900_BT_SESSION_RATE_48K			0x05
+
+#define CG2900_BT_MEDIA_CONFIG_MONO			0x00
+#define CG2900_BT_MEDIA_CONFIG_STEREO			0x01
+#define CG2900_BT_MEDIA_CONFIG_JOINT_STEREO		0x02
+#define CG2900_BT_MEDIA_CONFIG_DUAL_CHANNEL		0x03
+
+#define CG2900_BT_SESSION_I2S_INDEX_I2S			0x00
+#define CG2900_BT_SESSION_PCM_INDEX_PCM_I2S		0x00
+
+
+#define CG2900_BT_VS_SESSION_CTRL			0xFD57
+struct bt_vs_session_ctrl_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+	__u8	control;
+} __packed;
+
+#define CG2900_BT_SESSION_START				0x00
+#define CG2900_BT_SESSION_STOP				0x01
+#define CG2900_BT_SESSION_PAUSE				0x02
+#define CG2900_BT_SESSION_RESUME			0x03
+
+#define CG2900_BT_VS_RESET_SESSION_CONFIG		0xFD56
+struct bt_vs_reset_session_cfg_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+} __packed;
+
+/*
+ *	CG2900 PG2 multimedia API
+ */
+
+#define CG2900_MC_PORT_PCM_I2S				0x00
+#define CG2900_MC_PORT_I2S				0x01
+#define CG2900_MC_PORT_BT_SCO				0x04
+#define CG2900_MC_PORT_FM_RX_0				0x07
+#define CG2900_MC_PORT_FM_RX_1				0x08
+#define CG2900_MC_PORT_FM_TX				0x09
+
+#define CG2900_MC_VS_PORT_CONFIG			0xFD64
+struct mc_vs_port_cfg_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	type;
+	/*
+	 * one of the following configuration structs should follow, but they
+	 * have different lengths so a union will not work
+	 */
+} __packed;
+
+struct mc_vs_port_cfg_pcm_i2s {
+	__u8 role_dir;
+	__u8 sco_a2dp_slots_used;
+	__u8 fm_slots_used;
+	__u8 ring_slots_used;
+	__u8 slot_start[4];
+	__u8 ratio_mode;
+	__u8 frame_len;
+	__u8 bitclk_srate;
+} __packed;
+#define PORTCFG_PCM_SET_ROLE(cfg, role)			\
+	set_low_nibble(&(cfg).role_dir, (role))
+#define PORTCFG_PCM_SET_DIR(cfg, idx, dir)		\
+	store_bit(&(cfg).role_dir, (idx) + 4, (dir))
+static inline void portcfg_pcm_set_sco_used(struct mc_vs_port_cfg_pcm_i2s *cfg,
+					    size_t index, __u8 use)
+{
+	if (use) {
+		/* clear corresponding slot in all cases */
+		cfg->sco_a2dp_slots_used &= ~(0x11 << index);
+		cfg->fm_slots_used &= ~(0x11 << index);
+		cfg->ring_slots_used &= ~(0x11 << index);
+		/* set for sco */
+		cfg->sco_a2dp_slots_used |= (1u << index);
+	} else {
+		/* only clear for sco */
+		cfg->sco_a2dp_slots_used &= ~(1u << index);
+	}
+}
+#define PORTCFG_PCM_SET_SCO_USED(cfg, idx, use)		\
+	portcfg_pcm_set_sco_used(&cfg, idx, use)
+#define PORTCFG_PCM_SET_RATIO(cfg, r)			\
+	set_low_nibble(&(cfg).ratio_mode, (r))
+#define PORTCFG_PCM_SET_MODE(cfg, mode)			\
+	set_high_nibble(&(cfg).ratio_mode, (mode))
+#define PORTCFG_PCM_SET_BITCLK(cfg, clk)		\
+	set_low_nibble(&(cfg).bitclk_srate, (clk))
+#define PORTCFG_PCM_SET_SRATE(cfg, rate)		\
+	set_high_nibble(&(cfg).bitclk_srate, (rate))
+
+#define CG2900_MC_PCM_SAMPLE_RATE_8			1
+#define CG2900_MC_PCM_SAMPLE_RATE_16			2
+#define CG2900_MC_PCM_SAMPLE_RATE_44_1			4
+#define CG2900_MC_PCM_SAMPLE_RATE_48			6
+
+struct mc_vs_port_cfg_i2s {
+	__u8 role_hper;
+	__u8 csel_srate;
+	__u8 wordlen;
+};
+#define PORTCFG_I2S_SET_ROLE(cfg, role)			\
+	set_low_nibble(&(cfg).role_hper, (role))
+#define PORTCFG_I2S_SET_HALFPERIOD(cfg, hper)		\
+	set_high_nibble(&(cfg).role_hper, (hper))
+#define PORTCFG_I2S_SET_CHANNELS(cfg, chnl)		\
+	set_low_nibble(&(cfg).csel_srate, (chnl))
+#define PORTCFG_I2S_SET_SRATE(cfg, rate)		\
+	set_high_nibble(&(cfg).csel_srate, (rate))
+#define PORTCFG_I2S_SET_WORDLEN(cfg, len)		\
+	set_low_nibble(&(cfg).wordlen, len)
+
+#define CG2900_MC_I2S_RIGHT_CHANNEL			1
+#define CG2900_MC_I2S_LEFT_CHANNEL			2
+#define CG2900_MC_I2S_BOTH_CHANNELS			3
+
+#define CG2900_MC_I2S_SAMPLE_RATE_8			0
+#define CG2900_MC_I2S_SAMPLE_RATE_16			1
+#define CG2900_MC_I2S_SAMPLE_RATE_44_1			2
+#define CG2900_MC_I2S_SAMPLE_RATE_48			4
+
+#define CG2900_MC_I2S_WORD_16				1
+#define CG2900_MC_I2S_WORD_32				3
+
+struct mc_vs_port_cfg_fm {
+	__u8 srate; /* NB: value goes in _upper_ nibble! */
+};
+#define PORTCFG_FM_SET_SRATE(cfg, rate)		\
+	set_high_nibble(&(cfg).srate, (rate))
+
+struct mc_vs_port_cfg_sco {
+	__le16	acl_id;
+	__u8	wbs_codec;
+} __packed;
+#define PORTCFG_SCO_SET_CODEC(cfg, codec)	\
+	set_high_nibble(&(cfg).wbs_codec, (codec))
+
+#define CG2900_MC_VS_CREATE_STREAM			0xFD66
+struct mc_vs_create_stream_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+	__u8	inport;
+	__u8	outport;
+	__u8	order; /* NB: not used by chip */
+} __packed;
+
+#define CG2900_MC_VS_DELETE_STREAM			0xFD67
+struct mc_vs_delete_stream_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	stream;
+} __packed;
+
+#define CG2900_MC_VS_STREAM_CONTROL			0xFD68
+struct mc_vs_stream_ctrl_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	command;
+	__u8	n_streams;
+	__u8	stream[];
+} __packed;
+
+#define CG2900_MC_STREAM_START				0x00
+#define CG2900_MC_STREAM_STOP				0x01
+#define CG2900_MC_STREAM_STOP_FLUSH			0x02
+
+#define CG2900_MC_VS_SET_FM_START_MODE			0xFD69
+
+/*
+ *	FM
+ */
+
+/* FM legacy command packet */
+struct fm_leg_cmd {
+	__u8	length;
+	__u8	opcode;
+	__u8	read_write;
+	__u8	fm_function;
+	union { /* Payload varies with function */
+		__le16	irqmask;
+		struct fm_leg_fm_cmd {
+			__le16	head;
+			__le16	data[];
+		} fm_cmd;
+	};
+} __packed;
+
+/* FM legacy command complete packet */
+struct fm_leg_cmd_cmpl {
+	__u8	param_length;
+	__u8	status;
+	__u8	opcode;
+	__u8	read_write;
+	__u8	cmd_status;
+	__u8	fm_function;
+	__le16	response_head;
+	__le16	data[];
+} __packed;
+
+/* FM legacy interrupt packet, PG2 style */
+struct fm_leg_irq_v2 {
+	__u8	param_length;
+	__u8	status;
+	__u8	opcode;
+	__u8	event_type;
+	__u8	event_id;
+	__le16	irq;
+} __packed;
+
+/* FM legacy interrupt packet, PG1 style */
+struct fm_leg_irq_v1 {
+	__u8	param_length;
+	__u8	opcode;
+	__u8	event_id;
+	__le16	irq;
+} __packed;
+
+union fm_leg_evt_or_irq {
+	__u8			param_length;
+	struct fm_leg_cmd_cmpl	evt;
+	struct fm_leg_irq_v2	irq_v2;
+	struct fm_leg_irq_v1	irq_v1;
+} __packed;
+
+/* FM Opcode generic*/
+#define CG2900_FM_GEN_ID_LEGACY				0xFE
+
+/* FM event*/
+#define CG2900_FM_EVENT_UNKNOWN				0
+#define CG2900_FM_EVENT_CMD_COMPLETE			1
+#define CG2900_FM_EVENT_INTERRUPT			2
+
+/* FM do-command identifiers. */
+#define CG2900_FM_DO_AIP_FADE_START			0x0046
+#define CG2900_FM_DO_AUP_BT_FADE_START			0x01C2
+#define CG2900_FM_DO_AUP_EXT_FADE_START			0x0102
+#define CG2900_FM_DO_AUP_FADE_START			0x00A2
+#define CG2900_FM_DO_FMR_SETANTENNA			0x0663
+#define CG2900_FM_DO_FMR_SP_AFSWITCH_START		0x04A3
+#define CG2900_FM_DO_FMR_SP_AFUPDATE_START		0x0463
+#define CG2900_FM_DO_FMR_SP_BLOCKSCAN_START		0x0683
+#define CG2900_FM_DO_FMR_SP_PRESETPI_START		0x0443
+#define CG2900_FM_DO_FMR_SP_SCAN_START			0x0403
+#define CG2900_FM_DO_FMR_SP_SEARCH_START		0x03E3
+#define CG2900_FM_DO_FMR_SP_SEARCHPI_START		0x0703
+#define CG2900_FM_DO_FMR_SP_TUNE_SETCHANNEL		0x03C3
+#define CG2900_FM_DO_FMR_SP_TUNE_STEPCHANNEL		0x04C3
+#define CG2900_FM_DO_FMT_PA_SETCTRL			0x01A4
+#define CG2900_FM_DO_FMT_PA_SETMODE			0x01E4
+#define CG2900_FM_DO_FMT_SP_TUNE_SETCHANNEL		0x0064
+#define CG2900_FM_DO_GEN_ANTENNACHECK_START		0x02A1
+#define CG2900_FM_DO_GEN_GOTOMODE			0x0041
+#define CG2900_FM_DO_GEN_POWERSUPPLY_SETMODE		0x0221
+#define CG2900_FM_DO_GEN_SELECTREFERENCECLOCK		0x0201
+#define CG2900_FM_DO_GEN_SETPROCESSINGCLOCK		0x0241
+#define CG2900_FM_DO_GEN_SETREFERENCECLOCKPLL		0x01A1
+#define CG2900_FM_DO_TST_TX_RAMP_START			0x0147
+#define CG2900_FM_CMD_NONE				0xFFFF
+#define CG2900_FM_CMD_ID_GEN_GOTO_POWER_DOWN		0x0081
+#define CG2900_FM_CMD_ID_GEN_GOTO_STANDBY		0x0061
+
+/* FM Command IDs */
+#define CG2900_FM_CMD_ID_AUP_EXT_SET_MODE		0x0162
+#define CG2900_FM_CMD_ID_AUP_EXT_SET_CTRL		0x0182
+#define CG2900_FM_CMD_ID_AIP_SET_MODE			0x01C6
+#define CG2900_FM_CMD_ID_AIP_BT_SET_CTRL		0x01A6
+#define CG2900_FM_CMD_ID_AIP_BT_SET_MODE		0x01E6
+
+/* FM Command Parameters. */
+#define CG2900_FM_CMD_PARAM_ENABLE			0x00
+#define CG2900_FM_CMD_PARAM_DISABLE			0x01
+#define CG2900_FM_CMD_PARAM_RESET			0x02
+#define CG2900_FM_CMD_PARAM_WRITECOMMAND		0x10
+#define CG2900_FM_CMD_PARAM_SET_INT_MASK_ALL		0x20
+#define CG2900_FM_CMD_PARAM_GET_INT_MASK_ALL		0x21
+#define CG2900_FM_CMD_PARAM_SET_INT_MASK		0x22
+#define CG2900_FM_CMD_PARAM_GET_INT_MASK		0x23
+#define CG2900_FM_CMD_PARAM_FM_FW_DOWNLOAD		0x30
+#define CG2900_FM_CMD_PARAM_NONE			0xFF
+
+/* FM Legacy Command Parameters */
+#define CG2900_FM_CMD_LEG_PARAM_WRITE			0x00
+#define CG2900_FM_CMD_LEG_PARAM_IRQ			0x01
+
+/* FM Command Status. */
+#define CG2900_FM_CMD_STATUS_COMMAND_SUCCEEDED		0x00
+#define CG2900_FM_CMD_STATUS_HW_FAILURE			0x03
+#define CG2900_FM_CMD_STATUS_INVALID_PARAMS		0x12
+#define CG2900_FM_CMD_STATUS_UNINITILIZED		0x15
+#define CG2900_FM_CMD_STATUS_UNSPECIFIED_ERROR		0x1F
+#define CG2900_FM_CMD_STATUS_COMMAND_DISALLOWED		0x0C
+#define CG2900_FM_CMD_STATUS_FW_WRONG_SEQUENCE_NR	0xF1
+#define CG2900_FM_CMD_STATUS_FW_UNKNOWN_FILE		0xF2
+#define CG2900_FM_CMD_STATUS_FW_FILE_VER_MISMATCH	0xF3
+
+/* FM Interrupts. */
+#define CG2900_FM_IRPT_FIQ				0x0000
+#define CG2900_FM_IRPT_OPERATION_SUCCEEDED		0x0001
+#define CG2900_FM_IRPT_OPERATION_FAILED			0x0002
+#define CG2900_FM_IRPT_BUFFER_FULL			0x0008
+#define CG2900_FM_IRPT_BUFFER_EMPTY			0x0008
+#define CG2900_FM_IRPT_SIGNAL_QUALITY_LOW		0x0010
+#define CG2900_FM_IRPT_MUTE_STATUS_CHANGED		0x0010
+#define CG2900_FM_IRPT_MONO_STEREO_TRANSITION		0x0020
+#define CG2900_FM_IRPT_OVER_MODULATION			0x0020
+#define CG2900_FM_IRPT_RDS_SYNC_FOUND			0x0040
+#define CG2900_FM_IRPT_INPUT_OVERDRIVE			0x0040
+#define CG2900_FM_IRPT_RDS_SYNC_LOST			0x0080
+#define CG2900_FM_IRPT_PI_CODE_CHANGED			0x0100
+#define CG2900_FM_IRPT_REQUEST_BLOCK_AVALIBLE		0x0200
+#define CG2900_FM_IRPT_BUFFER_CLEARED			0x2000
+#define CG2900_FM_IRPT_WARM_BOOT_READY			0x4000
+#define CG2900_FM_IRPT_COLD_BOOT_READY			0x8000
+
+/* FM Legacy Function Command Parameters */
+
+/* AUP_EXT_SetMode Output enum */
+#define CG2900_FM_CMD_AUP_EXT_SET_MODE_DISABLED		0x0000
+#define CG2900_FM_CMD_AUP_EXT_SET_MODE_I2S		0x0001
+#define CG2900_FM_CMD_AUP_EXT_SET_MODE_PARALLEL		0x0002
+
+/* SetControl Conversion enum */
+#define CG2900_FM_CMD_SET_CTRL_CONV_UP			0x0000
+#define CG2900_FM_CMD_SET_CTRL_CONV_DOWN		0x0001
+
+/* AIP_SetMode Input enum */
+#define CG2900_FM_CMD_AIP_SET_MODE_INPUT_ANA		0x0000
+#define CG2900_FM_CMD_AIP_SET_MODE_INPUT_DIG		0x0001
+
+/* AIP_BT_SetMode Input enum */
+#define CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_RESERVED	0x0000
+#define CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_I2S		0x0001
+#define CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_PAR		0x0002
+#define CG2900_FM_CMD_AIP_BT_SET_MODE_INPUT_FIFO	0x0003
+
+/* FM Parameter Lengths = FM command length - length field (1 byte) */
+#define CG2900_FM_CMD_PARAM_LEN(len) (len - 1)
+
+/*
+ * FM Command ID mapped per byte and shifted 3 bits left
+ * Also adds number of parameters at first 3 bits of LSB.
+ */
+static inline __u16 cg2900_get_fm_cmd_id(__u16 opcode)
+{
+	return opcode >> 3;
+}
+
+static inline __u16 cg2900_make_fm_cmd_id(__u16 id, __u8 num_params)
+{
+	return (id << 3) | num_params;
+}
+
+/*
+ *	GNSS
+ */
+
+struct gnss_hci_hdr {
+	__u8	op_code;
+	__le16	plen;
+} __packed;
+
+#endif /* _CG2900_CHIP_H_ */
diff --git a/drivers/staging/cg2900/mfd/cg2900_core.c b/drivers/staging/cg2900/mfd/cg2900_core.c
new file mode 100644
index 00000000000..fe28ab6b7ae
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_core.c
@@ -0,0 +1,719 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+#define NAME					"cg2900_core"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/firmware.h>
+#include <linux/fs.h>
+#include <linux/gfp.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/stat.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/core.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+#include "cg2900_core.h"
+
+/* Device names */
+#define CG2900_CDEV_NAME		"cg2900_core_test"
+#define CG2900_CLASS_NAME		"cg2900_class"
+#define CG2900_DEVICE_NAME		"cg2900_driver"
+#define CORE_WQ_NAME			"cg2900_core_wq"
+
+#define LOGGER_DIRECTION_TX		0
+#define LOGGER_DIRECTION_RX		1
+
+/*
+ * Timeout values
+ */
+#define CHIP_READY_TIMEOUT		(100)	/* ms */
+#define REVISION_READOUT_TIMEOUT	(500)	/* ms */
+#define SLEEP_TIMEOUT_MS		(150)	/* ms */
+/* Timeout value to check CTS line for low power */
+#define READY_FOR_SLEEP_TIMEOUT_MS	(50)	/* ms */
+
+/**
+ * enum boot_state - BOOT-state for CG2900 Core.
+ * @BOOT_RESET:					HCI Reset has been sent.
+ * @BOOT_READ_LOCAL_VERSION_INFORMATION:	ReadLocalVersionInformation
+ *						command has been sent.
+ * @BOOT_READY:					CG2900 Core boot is ready.
+ * @BOOT_FAILED:				CG2900 Core boot failed.
+ */
+enum boot_state {
+	BOOT_RESET,
+	BOOT_READ_LOCAL_VERSION_INFORMATION,
+	BOOT_READY,
+	BOOT_FAILED
+};
+
+/**
+ * struct chip_handler_item - Structure to store chip handler cb.
+ * @list:	list_head struct.
+ * @cb:		Chip handler callback struct.
+ */
+struct chip_handler_item {
+	struct list_head		list;
+	struct cg2900_id_callbacks	cb;
+};
+
+/**
+ * struct core_info - Main info structure for CG2900 Core.
+ * @boot_state:		Current BOOT-state of CG2900 Core.
+ * @wq:			CG2900 Core workqueue.
+ * @chip_dev:		Device structure for chip driver.
+ * @work:		Work structure.
+ */
+struct core_info {
+	enum boot_state			boot_state;
+	struct workqueue_struct		*wq;
+	struct cg2900_chip_dev		*chip_dev;
+	struct work_struct		work;
+};
+
+/**
+ * struct main_info - Main info structure for CG2900 Core.
+ * @dev:		Device structure for STE Connectivity driver.
+ * @man_mutex:		Management mutex.
+ * @chip_handlers:	List of the register handlers for different chips.
+ * @wq:			Wait queue.
+ */
+struct main_info {
+	struct device		*dev;
+	struct mutex		man_mutex;
+	struct list_head	chip_handlers;
+	wait_queue_head_t	wq;
+};
+
+/* core_info - Main information object for CG2900 Core. */
+static struct main_info *main_info;
+
+/* Module parameters */
+u8 bd_address[] = {0x00, 0xBE, 0xAD, 0xDE, 0x80, 0x00};
+EXPORT_SYMBOL_GPL(bd_address);
+int bd_addr_count = BT_BDADDR_SIZE;
+
+static int sleep_timeout_ms = SLEEP_TIMEOUT_MS;
+
+/**
+ * send_bt_cmd() - Copy and send sk_buffer with no assigned user.
+ * @dev:	Current chip to transmit to.
+ * @data:	Data to send.
+ * @length:	Length in bytes of data.
+ *
+ * The send_bt_cmd() function allocate sk_buffer, copy supplied
+ * data to it, and send the sk_buffer to controller.
+ */
+void send_bt_cmd(struct cg2900_chip_dev *dev, void *data, int length)
+{
+	struct sk_buff *skb;
+	int err;
+
+	skb = alloc_skb(length + HCI_H4_SIZE, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(dev->dev, "send_bt_cmd: Couldn't alloc sk_buff with "
+			"length %d\n", length);
+		return;
+	}
+
+	skb_reserve(skb, HCI_H4_SIZE);
+	memcpy(skb_put(skb, length), data, length);
+	skb_push(skb, HCI_H4_SIZE);
+	skb->data[0] = HCI_BT_CMD_H4_CHANNEL;
+
+	err = dev->t_cb.write(dev, skb);
+	if (err) {
+		dev_err(dev->dev, "send_bt_cmd: Transport write failed (%d)\n",
+			err);
+		kfree_skb(skb);
+	}
+}
+
+/**
+ * handle_reset_cmd_complete_evt() - Handle a received HCI Command Complete event for a Reset command.
+ * @dev:	Current device.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool handle_reset_cmd_complete_evt(struct cg2900_chip_dev *dev, u8 *data)
+{
+	bool pkt_handled = false;
+	u8 status = data[0];
+	struct hci_command_hdr cmd;
+	struct core_info *info = dev->prv_data;
+
+	dev_dbg(dev->dev, "Received Reset complete event with status 0x%X\n",
+		status);
+
+	if (info->boot_state == BOOT_RESET) {
+		/* Transmit HCI Read Local Version Information command */
+		dev_dbg(dev->dev, "New boot_state: "
+			"BOOT_READ_LOCAL_VERSION_INFORMATION\n");
+		info->boot_state = BOOT_READ_LOCAL_VERSION_INFORMATION;
+		cmd.opcode = cpu_to_le16(HCI_OP_READ_LOCAL_VERSION);
+		cmd.plen = 0; /* No parameters for HCI reset */
+		send_bt_cmd(dev, &cmd, sizeof(cmd));
+
+		pkt_handled = true;
+	}
+
+	return pkt_handled;
+}
+
+/**
+ * handle_read_local_version_info_cmd_complete_evt() - Handle a received HCI Command Complete event for a ReadLocalVersionInformation command.
+ * @dev:	Current device.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool
+handle_read_local_version_info_cmd_complete_evt(struct cg2900_chip_dev *dev,
+						u8 *data)
+{
+	struct hci_rp_read_local_version *evt;
+	struct core_info *info = dev->prv_data;
+
+	/* Check we're in the right state */
+	if (info->boot_state != BOOT_READ_LOCAL_VERSION_INFORMATION)
+		return false;
+
+	/* We got an answer for our HCI command. Extract data */
+	evt = (struct hci_rp_read_local_version *)data;
+
+	/* We will handle the packet */
+	if (HCI_BT_ERROR_NO_ERROR != evt->status) {
+		dev_err(dev->dev, "Received Read Local Version Information "
+			"with status 0x%X\n", evt->status);
+		dev_dbg(dev->dev, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		wake_up_all(&main_info->wq);
+		return true;
+	}
+
+	/* The command worked. Store the data */
+	dev->chip.hci_version = evt->hci_ver;
+	dev->chip.hci_revision = le16_to_cpu(evt->hci_rev);
+	dev->chip.lmp_pal_version = evt->lmp_ver;
+	dev->chip.manufacturer = le16_to_cpu(evt->manufacturer);
+	dev->chip.hci_sub_version = le16_to_cpu(evt->lmp_subver);
+	dev_info(dev->dev, "Received Read Local Version Information with:\n"
+		 "\thci_version:  0x%02X\n"
+		 "\thci_revision: 0x%04X\n"
+		 "\tlmp_pal_version: 0x%02X\n"
+		 "\tmanufacturer: 0x%04X\n"
+		 "\thci_sub_version: 0x%04X\n",
+		 dev->chip.hci_version, dev->chip.hci_revision,
+		 dev->chip.lmp_pal_version, dev->chip.manufacturer,
+		 dev->chip.hci_sub_version);
+
+	dev_dbg(dev->dev, "New boot_state: BOOT_READY\n");
+	info->boot_state = BOOT_READY;
+	wake_up_all(&main_info->wq);
+
+	return true;
+}
+
+/**
+ * handle_rx_data_bt_evt() - Check if data should be handled in CG2900 Core.
+ * @dev:	Current chip
+ * @skb:	Data packet
+ *
+ * The handle_rx_data_bt_evt() function checks if received data should be
+ * handled in CG2900 Core. If so handle it correctly.
+ * Received data is always HCI BT Event.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool handle_rx_data_bt_evt(struct cg2900_chip_dev *dev,
+				  struct sk_buff *skb)
+{
+	bool pkt_handled = false;
+	u8 *data = &skb->data[CG2900_SKB_RESERVE];
+	struct hci_event_hdr *evt;
+	struct hci_ev_cmd_complete *cmd_complete;
+	u16 op_code;
+
+	evt = (struct hci_event_hdr *)data;
+
+	/* First check the event code */
+	if (HCI_EV_CMD_COMPLETE != evt->evt)
+		return false;
+
+	data += sizeof(*evt);
+	cmd_complete = (struct hci_ev_cmd_complete *)data;
+
+	op_code = le16_to_cpu(cmd_complete->opcode);
+
+	dev_dbg(dev->dev, "Received Command Complete: op_code = 0x%04X\n",
+		op_code);
+	data += sizeof(*cmd_complete); /* Move to first byte after OCF */
+
+	if (op_code == HCI_OP_RESET)
+		pkt_handled = handle_reset_cmd_complete_evt(dev, data);
+	else if (op_code == HCI_OP_READ_LOCAL_VERSION)
+		pkt_handled = handle_read_local_version_info_cmd_complete_evt
+					(dev, data);
+
+	if (pkt_handled)
+		kfree_skb(skb);
+
+	return pkt_handled;
+}
+
+static void cg2900_data_from_chip(struct cg2900_chip_dev *dev,
+				  struct sk_buff *skb)
+{
+	u8 h4_channel;
+
+	dev_dbg(dev->dev, "cg2900_data_from_chip\n");
+
+	if (!skb) {
+		dev_err(dev->dev, "No data supplied\n");
+		return;
+	}
+
+	h4_channel = skb->data[0];
+
+	/*
+	 * First check if this is the response for something
+	 * we have sent internally.
+	 */
+	if (HCI_BT_EVT_H4_CHANNEL == h4_channel &&
+	    handle_rx_data_bt_evt(dev, skb)) {
+		dev_dbg(dev->dev, "Received packet handled internally\n");
+	} else {
+		dev_err(dev->dev,
+			"cg2900_data_from_chip: Received unexpected packet\n");
+		kfree_skb(skb);
+	}
+}
+
+/**
+ * work_hw_registered() - Called when the interface to HW has been established.
+ * @work:	Reference to work data.
+ *
+ * Since there now is a transport identify the connected chip and decide which
+ * chip handler to use.
+ */
+static void work_hw_registered(struct work_struct *work)
+{
+	struct hci_command_hdr cmd;
+	struct cg2900_chip_dev *dev;
+	struct core_info *info;
+	bool chip_handled = false;
+	struct list_head *cursor;
+	struct chip_handler_item *tmp;
+
+	dev_dbg(main_info->dev, "work_hw_registered\n");
+
+	if (!work) {
+		dev_err(main_info->dev, "work_hw_registered: work == NULL\n");
+		return;
+	}
+
+	info = container_of(work, struct core_info, work);
+	dev = info->chip_dev;
+
+	/*
+	 * This might look strange, but we need to read out
+	 * the revision info in order to be able to shutdown the chip properly.
+	 */
+	if (dev->t_cb.set_chip_power)
+		dev->t_cb.set_chip_power(dev, true);
+
+	/* Set our function to receive data from chip */
+	dev->c_cb.data_from_chip = cg2900_data_from_chip;
+
+	/*
+	 * Transmit HCI reset command to ensure the chip is using
+	 * the correct transport
+	 */
+	dev_dbg(dev->dev, "New boot_state: BOOT_RESET\n");
+	info->boot_state = BOOT_RESET;
+	cmd.opcode = cpu_to_le16(HCI_OP_RESET);
+	cmd.plen = 0; /* No parameters for HCI reset */
+	send_bt_cmd(dev, &cmd, sizeof(cmd));
+
+	dev_dbg(dev->dev,
+		"Wait up to 500 milliseconds for revision to be read\n");
+	wait_event_timeout(main_info->wq,
+			   (BOOT_READY == info->boot_state ||
+			    BOOT_FAILED == info->boot_state),
+			    msecs_to_jiffies(REVISION_READOUT_TIMEOUT));
+
+	if (BOOT_READY != info->boot_state) {
+		dev_err(dev->dev,
+			"Could not read out revision from the chip\n");
+		info->boot_state = BOOT_FAILED;
+		dev->t_cb.set_chip_power(dev, false);
+		return;
+	}
+
+	dev->c_cb.data_from_chip = NULL;
+
+	mutex_lock(&main_info->man_mutex);
+	list_for_each(cursor, &main_info->chip_handlers) {
+		tmp = list_entry(cursor, struct chip_handler_item, list);
+		chip_handled = tmp->cb.check_chip_support(dev);
+		if (chip_handled) {
+			dev_info(dev->dev, "Chip handler found\n");
+			break;
+		}
+	}
+	mutex_unlock(&main_info->man_mutex);
+
+	if (!chip_handled)
+		dev_info(dev->dev, "No chip handler found\n");
+}
+
+/**
+ * cg2900_register_chip_driver() - Register a chip handler.
+ * @cb:	Callbacks to call when chip is connected.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL is supplied as @cb.
+ *   -ENOMEM if allocation fails or work queue can't be created.
+ */
+int cg2900_register_chip_driver(struct cg2900_id_callbacks *cb)
+{
+	struct chip_handler_item *item;
+
+	dev_dbg(main_info->dev, "cg2900_register_chip_driver\n");
+
+	if (!cb) {
+		dev_err(main_info->dev, "NULL supplied as cb\n");
+		return -EINVAL;
+	}
+
+	item = kzalloc(sizeof(*item), GFP_KERNEL);
+	if (!item) {
+		dev_err(main_info->dev,
+			"cg2900_register_chip_driver: "
+			"Failed to alloc memory\n");
+		return -ENOMEM;
+	}
+
+	memcpy(&item->cb, cb, sizeof(cb));
+	mutex_lock(&main_info->man_mutex);
+	list_add_tail(&item->list, &main_info->chip_handlers);
+	mutex_unlock(&main_info->man_mutex);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cg2900_register_chip_driver);
+
+/**
+ * cg2900_deregister_chip_driver() - Deregister a chip handler.
+ * @cb:	Callbacks to call when chip is connected.
+ */
+void cg2900_deregister_chip_driver(struct cg2900_id_callbacks *cb)
+{
+	struct chip_handler_item *tmp;
+	struct list_head *cursor, *next;
+
+	dev_dbg(main_info->dev, "cg2900_deregister_chip_driver\n");
+
+	if (!cb) {
+		dev_err(main_info->dev, "NULL supplied as cb\n");
+		return;
+	}
+	mutex_lock(&main_info->man_mutex);
+	list_for_each_safe(cursor, next, &main_info->chip_handlers) {
+		tmp = list_entry(cursor, struct chip_handler_item, list);
+		if (tmp->cb.check_chip_support == cb->check_chip_support) {
+			list_del(cursor);
+			kfree(tmp);
+			break;
+		}
+	}
+	mutex_unlock(&main_info->man_mutex);
+}
+EXPORT_SYMBOL_GPL(cg2900_deregister_chip_driver);
+
+/**
+ * cg2900_register_trans_driver() - Register a transport driver.
+ * @dev:	Transport device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL is supplied as @cb.
+ *   -ENOMEM if allocation fails or work queue can't be created.
+ *   -EACCES if work can't be queued.
+ */
+int cg2900_register_trans_driver(struct cg2900_chip_dev *dev)
+{
+	int err;
+	struct cg2900_platform_data *pf_data;
+	struct core_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!dev || !dev->dev) {
+		dev_err(main_info->dev, "cg2900_register_trans_driver: "
+			"Received NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(dev->dev, "cg2900_register_trans_driver\n");
+
+	if (!dev->t_cb.write) {
+		dev_err(dev->dev, "cg2900_register_trans_driver: Write function"
+			" missing\n");
+		return -EINVAL;
+	}
+
+	pf_data = dev_get_platdata(dev->dev);
+	if (!pf_data) {
+		dev_err(dev->dev, "cg2900_register_trans_driver: Missing "
+			"platform data\n");
+		return -EINVAL;
+	}
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev->dev, "Couldn't allocate info\n");
+		return -ENOMEM;
+	}
+
+	if (pf_data->init) {
+		err = pf_data->init(dev);
+		if (err) {
+			dev_err(dev->dev, "Platform init failed (%d)\n", err);
+			goto error_handling;
+		}
+	}
+
+	info->chip_dev = dev;
+	dev->prv_data = info;
+
+	info->wq = create_singlethread_workqueue(CORE_WQ_NAME);
+	if (!info->wq) {
+		dev_err(dev->dev, "Could not create workqueue\n");
+		err = -ENOMEM;
+		goto error_handling_exit;
+	}
+
+	dev_info(dev->dev, "Transport connected\n");
+
+	INIT_WORK(&info->work, work_hw_registered);
+	if (!queue_work(info->wq, &info->work)) {
+		dev_err(dev->dev, "Failed to queue work_hw_registered because "
+			"it's already in the queue\n");
+		err = -EACCES;
+		goto error_handling_wq;
+	}
+
+	return 0;
+
+error_handling_wq:
+	destroy_workqueue(info->wq);
+error_handling_exit:
+	if (pf_data->exit)
+		pf_data->exit(dev);
+error_handling:
+	kfree(info);
+	return err;
+}
+EXPORT_SYMBOL_GPL(cg2900_register_trans_driver);
+
+/**
+ * cg2900_deregister_trans_driver() - Deregister a transport driver.
+ * @dev:	Transport device.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EINVAL if NULL is supplied as @cb.
+ *   -ENOMEM if allocation fails or work queue can't be created.
+ */
+int cg2900_deregister_trans_driver(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_platform_data *pf_data;
+	struct core_info *info = dev->prv_data;
+
+	BUG_ON(!main_info);
+
+	dev_dbg(dev->dev, "cg2900_deregister_trans_driver\n");
+
+	if (dev->c_cb.chip_removed)
+		dev->c_cb.chip_removed(dev);
+
+	destroy_workqueue(info->wq);
+
+	dev->prv_data = NULL;
+	kfree(info);
+
+	dev_info(dev->dev, "Transport disconnected\n");
+
+	pf_data = dev_get_platdata(dev->dev);
+	if (!pf_data) {
+		dev_err(dev->dev, "Missing platform data\n");
+		return -EINVAL;
+	}
+
+	if (pf_data->exit)
+		pf_data->exit(dev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cg2900_deregister_trans_driver);
+
+/**
+ * cg2900_get_sleep_timeout() - Return sleep timeout in jiffies.
+ * @check_sleep:	If we want to check whether chip has gone
+ * to sleep then use lesser timeout value
+ *
+ * Returns:
+ *   Sleep timeout in jiffies. 0 means that sleep timeout shall not be used.
+ */
+unsigned long cg2900_get_sleep_timeout(bool check_sleep)
+{
+	if (!sleep_timeout_ms)
+		return 0;
+
+	if (check_sleep)
+		return msecs_to_jiffies(READY_FOR_SLEEP_TIMEOUT_MS);
+	else
+		return msecs_to_jiffies(sleep_timeout_ms);
+}
+EXPORT_SYMBOL_GPL(cg2900_get_sleep_timeout);
+
+/**
+ * cg2900_probe() - Initialize module.
+ *
+ * @pdev:	Platform device.
+ *
+ * This function initialize the transport and CG2900 Core, then
+ * register to the transport framework.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ */
+static int __devinit cg2900_probe(struct platform_device *pdev)
+{
+	dev_dbg(&pdev->dev, "cg2900_probe\n");
+
+	main_info = kzalloc(sizeof(*main_info), GFP_KERNEL);
+	if (!main_info) {
+		dev_err(&pdev->dev, "Couldn't allocate main_info\n");
+		return -ENOMEM;
+	}
+
+	main_info->dev = &pdev->dev;
+	mutex_init(&main_info->man_mutex);
+	INIT_LIST_HEAD(&main_info->chip_handlers);
+	init_waitqueue_head(&main_info->wq);
+
+	dev_info(&pdev->dev, "CG2900 Core driver started\n");
+
+	return 0;
+}
+
+/**
+ * cg2900_remove() - Remove module.
+ *
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM if core_info does not exist.
+ *   -EINVAL if platform data does not exist in the device.
+ */
+static int __devexit cg2900_remove(struct platform_device *pdev)
+{
+	dev_dbg(&pdev->dev, "cg2900_remove\n");
+
+	kfree(main_info);
+	main_info = NULL;
+
+	dev_info(&pdev->dev, "CG2900 Core driver removed\n");
+
+	return 0;
+}
+
+static struct platform_driver cg2900_driver = {
+	.driver = {
+		.name	= "cg2900",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_probe,
+	.remove	= __devexit_p(cg2900_remove),
+};
+
+/**
+ * cg2900_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_init(void)
+{
+	pr_debug("cg2900_init");
+	return platform_driver_register(&cg2900_driver);
+}
+
+/**
+ * cg2900_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_exit(void)
+{
+	pr_debug("cg2900_exit");
+	platform_driver_unregister(&cg2900_driver);
+}
+
+module_init(cg2900_init);
+module_exit(cg2900_exit);
+
+module_param(sleep_timeout_ms, int, S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(sleep_timeout_ms,
+		 "Sleep timeout for data transmissions:\n"
+		 "\tDefault 10000 ms\n"
+		 "\t0 = disable\n"
+		 "\t>0 = sleep timeout in milliseconds");
+
+module_param_array(bd_address, byte, &bd_addr_count,
+		   S_IRUGO | S_IWUSR | S_IWGRP);
+MODULE_PARM_DESC(bd_address,
+		 "Bluetooth Device address. "
+		 "Default 0x00 0x80 0xDE 0xAD 0xBE 0xEF. "
+		 "Enter as comma separated value.");
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux Bluetooth HCI H:4 CG2900 Connectivity Device Driver");
diff --git a/drivers/staging/cg2900/mfd/cg2900_core.h b/drivers/staging/cg2900/mfd/cg2900_core.h
new file mode 100644
index 00000000000..a0615fa0e46
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_core.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+
+#ifndef _CG2900_CORE_H_
+#define _CG2900_CORE_H_
+
+#include <linux/device.h>
+#include <linux/skbuff.h>
+
+/* Reserve 1 byte for the HCI H:4 header */
+#define HCI_H4_SIZE				1
+#define CG2900_SKB_RESERVE			HCI_H4_SIZE
+
+/* Number of bytes to reserve at start of sk_buffer when receiving packet */
+#define RX_SKB_RESERVE				8
+
+#define BT_BDADDR_SIZE				6
+
+/* Standardized Bluetooth H:4 channels */
+#define HCI_BT_CMD_H4_CHANNEL			0x01
+#define HCI_BT_ACL_H4_CHANNEL			0x02
+#define HCI_BT_SCO_H4_CHANNEL			0x03
+#define HCI_BT_EVT_H4_CHANNEL			0x04
+
+/* Default H4 channels which may change depending on connected controller */
+#define HCI_FM_RADIO_H4_CHANNEL			0x08
+#define HCI_GNSS_H4_CHANNEL			0x09
+
+/* Bluetooth error codes */
+#define HCI_BT_ERROR_NO_ERROR			0x00
+#define HCI_BT_WRONG_SEQ_ERROR			0xF1
+
+/* Bluetooth lengths */
+#define HCI_BT_SEND_FILE_MAX_CHUNK_SIZE		254
+
+#define LOGGER_DIRECTION_TX			0
+#define LOGGER_DIRECTION_RX			1
+
+/* module_param declared in cg2900_core.c */
+extern u8 bd_address[BT_BDADDR_SIZE];
+
+#endif /* _CG2900_CORE_H_ */
diff --git a/drivers/staging/cg2900/mfd/cg2900_lib.c b/drivers/staging/cg2900/mfd/cg2900_lib.c
new file mode 100644
index 00000000000..84f7eb5eb62
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_lib.c
@@ -0,0 +1,284 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+#define NAME					"cg2900_lib"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <linux/kernel.h>
+#include <linux/limits.h>
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <linux/types.h>
+
+#include "cg2900.h"
+#include "cg2900_chip.h"
+#include "cg2900_core.h"
+#include "cg2900_lib.h"
+
+/*
+ * Max length in bytes for line buffer used to parse settings and patch file.
+ * Must be max length of name plus characters used to define chip version.
+ */
+#define LINE_BUFFER_LENGTH			(NAME_MAX + 30)
+#define LOGGER_HEADER_SIZE			1
+/**
+ * cg2900_tx_to_chip() - Transmit buffer to the transport.
+ * @user:	User data for BT command channel.
+ * @logger:	User data for logger channel.
+ * @skb:	Data packet.
+ *
+ * The transmit_skb_to_chip() function transmit buffer to the transport.
+ * If enabled, copy the transmitted data to the HCI logger as well.
+ */
+void cg2900_tx_to_chip(struct cg2900_user_data *user,
+		       struct cg2900_user_data *logger, struct sk_buff *skb)
+{
+	int err;
+	struct cg2900_chip_dev *chip_dev;
+
+	dev_dbg(user->dev, "cg2900_tx_to_chip %d bytes.\n", skb->len);
+
+	if (logger)
+		cg2900_send_to_hci_logger(logger, skb, LOGGER_DIRECTION_TX);
+
+	chip_dev = cg2900_get_prv(user);
+	err = chip_dev->t_cb.write(chip_dev, skb);
+	if (err) {
+		dev_err(user->dev, "cg2900_tx_to_chip: Transport write failed "
+			"(%d)\n", err);
+		kfree_skb(skb);
+	}
+}
+EXPORT_SYMBOL_GPL(cg2900_tx_to_chip);
+
+/**
+ * cg2900_tx_no_user() - Transmit buffer to the transport.
+ * @dev:	Current chip to transmit to.
+ * @skb:	Data packet.
+ *
+ * This function transmits buffer to the transport when no user exist (system
+ * startup for example).
+ */
+void cg2900_tx_no_user(struct cg2900_chip_dev *dev, struct sk_buff *skb)
+{
+	int err;
+
+	dev_dbg(dev->dev, "cg2900_tx_no_user %d bytes.\n", skb->len);
+
+	err = dev->t_cb.write(dev, skb);
+	if (err) {
+		dev_err(dev->dev, "cg2900_tx_no_user: Transport write failed "
+			"(%d)\n", err);
+		kfree_skb(skb);
+	}
+}
+EXPORT_SYMBOL_GPL(cg2900_tx_no_user);
+
+/**
+ * create_and_send_bt_cmd() - Copy and send sk_buffer.
+ * @user:	User data for current channel.
+ * @logger:	User data for logger channel.
+ * @data:	Data to send.
+ * @length:	Length in bytes of data.
+ *
+ * The create_and_send_bt_cmd() function allocate sk_buffer, copy supplied data
+ * to it, and send the sk_buffer to controller.
+ */
+void cg2900_send_bt_cmd(struct cg2900_user_data *user,
+			struct cg2900_user_data *logger,
+			void *data, int length)
+{
+	struct sk_buff *skb;
+
+	skb = user->alloc_skb(length, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(user->dev, "cg2900_send_bt_cmd: Couldn't alloc "
+			"sk_buff with length %d\n", length);
+		return;
+	}
+
+	memcpy(skb_put(skb, length), data, length);
+	skb_push(skb, HCI_H4_SIZE);
+	skb->data[0] = HCI_BT_CMD_H4_CHANNEL;
+
+	cg2900_tx_to_chip(user, logger, skb);
+}
+EXPORT_SYMBOL_GPL(cg2900_send_bt_cmd);
+
+/**
+ * cg2900_send_bt_cmd_no_user() - Copy and send sk_buffer with no assigned user.
+ * @dev:	Current chip to transmit to.
+ * @data:	Data to send.
+ * @length:	Length in bytes of data.
+ *
+ * The cg2900_send_bt_cmd_no_user() function allocate sk_buffer, copy supplied
+ * data to it, and send the sk_buffer to controller.
+ */
+void cg2900_send_bt_cmd_no_user(struct cg2900_chip_dev *dev, void *data,
+				int length)
+{
+	struct sk_buff *skb;
+
+	skb = alloc_skb(length + HCI_H4_SIZE, GFP_KERNEL);
+	if (!skb) {
+		dev_err(dev->dev, "cg2900_send_bt_cmd_no_user: Couldn't alloc "
+			"sk_buff with length %d\n", length);
+		return;
+	}
+
+	skb_reserve(skb, HCI_H4_SIZE);
+	memcpy(skb_put(skb, length), data, length);
+	skb_push(skb, HCI_H4_SIZE);
+	skb->data[0] = HCI_BT_CMD_H4_CHANNEL;
+
+	cg2900_tx_no_user(dev, skb);
+}
+EXPORT_SYMBOL_GPL(cg2900_send_bt_cmd_no_user);
+
+/**
+ * create_work_item() - Create work item and add it to the work queue.
+ * @wq:		Work queue.
+ * @work_func:	Work function.
+ * @user_data:	Arbitrary data set by user.
+ *
+ * The create_work_item() function creates work item and add it to
+ * the work queue.
+ * Note that work is allocated by kmalloc and work must be freed when work
+ * function is started.
+ */
+void cg2900_create_work_item(struct workqueue_struct *wq, work_func_t work_func,
+			     void *user_data)
+{
+	struct cg2900_work *new_work;
+	int err;
+
+	new_work = kmalloc(sizeof(*new_work), GFP_ATOMIC);
+	if (!new_work) {
+		pr_err("Failed to alloc memory for new_work");
+		return;
+	}
+
+	INIT_WORK(&new_work->work, work_func);
+	new_work->user_data = user_data;
+
+	err = queue_work(wq, &new_work->work);
+	if (!err) {
+		pr_err("Failed to queue work_struct because it's already "
+		       "in the queue");
+		kfree(new_work);
+	}
+}
+EXPORT_SYMBOL_GPL(cg2900_create_work_item);
+
+/**
+ * read_and_send_file_part() - Transmit a part of the supplied file.
+ * @user:	User data for current channel.
+ * @logger:	User data for logger channel.
+ * @info:	File information.
+ *
+ * The cg2900_read_and_send_file_part() function transmit a part of the supplied
+ * file to the controller.
+ *
+ * Returns:
+ *   0 if there is no more data in the file.
+ *   >0 for number of bytes sent.
+ *   -ENOMEM if skb allocation failed.
+ */
+int cg2900_read_and_send_file_part(struct cg2900_user_data *user,
+				   struct cg2900_user_data *logger,
+				   struct cg2900_file_info *info)
+{
+	int bytes_to_copy;
+	struct sk_buff *skb;
+	struct bt_vs_write_file_block_cmd *cmd;
+	int plen;
+
+	/*
+	 * Calculate number of bytes to copy;
+	 * either max bytes for HCI packet or number of bytes left in file
+	 */
+	bytes_to_copy = min((int)HCI_BT_SEND_FILE_MAX_CHUNK_SIZE,
+			    (int)(info->fw_file->size - info->file_offset));
+
+	if (bytes_to_copy <= 0) {
+		/* Nothing more to read in file. */
+		dev_dbg(user->dev, "File download finished\n");
+		info->chunk_id = 0;
+		info->file_offset = 0;
+		return 0;
+	}
+
+	/* There is more data to send */
+	plen = sizeof(*cmd) + bytes_to_copy;
+	skb = user->alloc_skb(plen, GFP_KERNEL);
+	if (!skb) {
+		dev_err(user->dev, "Couldn't allocate sk_buffer\n");
+		return -ENOMEM;
+	}
+
+	skb_put(skb, plen);
+
+	cmd = (struct bt_vs_write_file_block_cmd *)skb->data;
+	cmd->opcode = cpu_to_le16(CG2900_BT_OP_VS_WRITE_FILE_BLOCK);
+	cmd->plen = BT_PARAM_LEN(plen);
+	cmd->id = info->chunk_id;
+	info->chunk_id++;
+
+	/* Copy the data from offset position */
+	memcpy(cmd->data,
+	       &(info->fw_file->data[info->file_offset]),
+	       bytes_to_copy);
+
+	/* Increase offset with number of bytes copied */
+	info->file_offset += bytes_to_copy;
+
+	skb_push(skb, CG2900_SKB_RESERVE);
+	skb->data[0] = HCI_BT_CMD_H4_CHANNEL;
+
+	cg2900_tx_to_chip(user, logger, skb);
+
+	return bytes_to_copy;
+}
+EXPORT_SYMBOL_GPL(cg2900_read_and_send_file_part);
+
+void cg2900_send_to_hci_logger(struct cg2900_user_data *logger,
+							struct sk_buff *skb,
+							u8 direction)
+{
+	struct sk_buff *skb_log;
+	u8 *p;
+
+	/*
+	 * Alloc a new sk_buff and copy the data into it. Then send it to
+	 * the HCI logger.
+	 */
+	skb_log = alloc_skb(skb->len + LOGGER_HEADER_SIZE, GFP_NOWAIT);
+	if (!skb_log) {
+		pr_err("cg2900_send_to_hci_logger:\
+				 Couldn't allocate skb_log\n");
+		return;
+	}
+	/* Reserve 1 byte for direction.*/
+	skb_reserve(skb_log, LOGGER_HEADER_SIZE);
+
+	memcpy(skb_put(skb_log, skb->len), skb->data, skb->len);
+	p = skb_push(skb_log, LOGGER_HEADER_SIZE);
+	*p = (u8) direction;
+
+	if (logger->read_cb)
+		logger->read_cb(logger, skb_log);
+	else
+		kfree_skb(skb_log);
+
+	return;
+}
+EXPORT_SYMBOL_GPL(cg2900_send_to_hci_logger);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux CG2900 Library functions");
diff --git a/drivers/staging/cg2900/mfd/cg2900_lib.h b/drivers/staging/cg2900/mfd/cg2900_lib.h
new file mode 100644
index 00000000000..99d5ce6cfdb
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_lib.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson CG2900 GPS/BT/FM controller.
+ */
+
+#ifndef _CG2900_LIB_H_
+#define _CG2900_LIB_H_
+
+#include <linux/firmware.h>
+#include <linux/skbuff.h>
+#include <linux/workqueue.h>
+
+#include "cg2900.h"
+
+/**
+ * struct cg2900_work - Generic work structure.
+ * @work:	Work structure.
+ * @user_data:	Arbitrary data set by user.
+ */
+struct cg2900_work {
+	struct work_struct work;
+	void *user_data;
+};
+
+/**
+ * struct cg2900_file_info - Info structure for file to download.
+ * @fw_file:		Stores firmware file.
+ * @file_offset:	Current read offset in firmware file.
+ * @chunk_id:		Stores current chunk ID of write file
+ *			operations.
+ */
+struct cg2900_file_info {
+	const struct firmware	*fw_file;
+	int			file_offset;
+	u8			chunk_id;
+};
+
+extern void cg2900_tx_to_chip(struct cg2900_user_data *user,
+			      struct cg2900_user_data *logger,
+			      struct sk_buff *skb);
+extern void cg2900_tx_no_user(struct cg2900_chip_dev *dev, struct sk_buff *skb);
+extern void cg2900_send_bt_cmd(struct cg2900_user_data *user,
+			       struct cg2900_user_data *logger,
+			       void *data, int length);
+extern void cg2900_send_bt_cmd_no_user(struct cg2900_chip_dev *dev, void *data,
+				       int length);
+extern void cg2900_create_work_item(struct workqueue_struct *wq,
+				    work_func_t work_func,
+				    void *user_data);
+extern int cg2900_read_and_send_file_part(struct cg2900_user_data *user,
+					  struct cg2900_user_data *logger,
+					  struct cg2900_file_info *info);
+extern void cg2900_send_to_hci_logger(struct cg2900_user_data *logger,
+							struct sk_buff *skb,
+							u8 direction);
+
+#endif /* _CG2900_LIB_H_ */
diff --git a/drivers/staging/cg2900/mfd/cg2900_test.c b/drivers/staging/cg2900/mfd/cg2900_test.c
new file mode 100644
index 00000000000..58ac6166af6
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/cg2900_test.c
@@ -0,0 +1,402 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Driver for ST-Ericsson CG2900 test character device.
+ */
+#define NAME					"cg2900_test"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/firmware.h>
+#include <linux/fs.h>
+#include <linux/gfp.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/wait.h>
+
+#include "cg2900.h"
+#include "cg2900_core.h"
+
+#define MISC_DEV			(info->misc_dev.this_device)
+
+/* Device names */
+#define CG2900_CDEV_NAME		"cg2900_core_test"
+
+/**
+ * struct test_info - Main info structure for CG2900 test char device.
+ * @misc_dev:	Registered Misc Device.
+ * @rx_queue:	RX data queue.
+ * @dev:	Device structure for STE Connectivity driver.
+ * @pdev:	Platform device structure for STE Connectivity driver.
+ */
+struct test_info {
+	struct miscdevice	misc_dev;
+	struct sk_buff_head	rx_queue;
+	struct device		*dev;
+	struct platform_device	*pdev;
+};
+
+static struct test_info *test_info;
+
+/*
+ * main_wait_queue - Char device Wait Queue in CG2900 Core.
+ */
+static DECLARE_WAIT_QUEUE_HEAD(char_wait_queue);
+
+/**
+ * tx_to_char_dev() - Handle data received from CG2900 Core.
+ * @dev:	Current chip device information.
+ * @skb:	Buffer with data coming form device.
+ */
+static int tx_to_char_dev(struct cg2900_chip_dev *dev, struct sk_buff *skb)
+{
+	struct test_info *info = dev->t_data;
+	skb_queue_tail(&info->rx_queue, skb);
+	wake_up_interruptible_all(&char_wait_queue);
+	return 0;
+}
+
+/**
+ * cg2900_test_open() - User space char device has been opened.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   -EACCES if transport already exists.
+ *   -ENOMEM if allocation fails.
+ *   Errors from create_work_item.
+ */
+static int cg2900_test_open(struct inode *inode, struct file *filp)
+{
+	struct test_info *info = test_info;
+	struct cg2900_chip_dev *dev;
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(MISC_DEV, "Cannot allocate test_dev\n");
+		return -ENOMEM;
+	}
+	dev->dev = info->dev;
+	dev->pdev = info->pdev;
+	dev->t_data = info;
+	dev->t_cb.write = tx_to_char_dev;
+	filp->private_data = dev;
+
+	dev_info(MISC_DEV, "CG2900 test char dev opened\n");
+	return cg2900_register_trans_driver(dev);
+}
+
+/**
+ * cg2900_test_release() - User space char device has been closed.
+ * @inode:	Device driver information.
+ * @filp:	Pointer to the file struct.
+ *
+ * Returns:
+ *   0 if there is no error.
+ */
+static int cg2900_test_release(struct inode *inode, struct file *filp)
+{
+	struct cg2900_chip_dev *dev = filp->private_data;
+	struct test_info *info = dev->t_data;
+
+	dev_info(MISC_DEV, "CG2900 test char dev closed\n");
+	skb_queue_purge(&info->rx_queue);
+	cg2900_deregister_trans_driver(dev);
+	kfree(dev);
+
+	return 0;
+}
+
+/**
+ * cg2900_test_read() - Queue and copy buffer to user space char device.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Received buffer.
+ * @count:	Count of received data in bytes.
+ * @f_pos:	Position in buffer.
+ *
+ * Returns:
+ *   >= 0 is number of bytes read.
+ *   -EFAULT if copy_to_user fails.
+ */
+static ssize_t cg2900_test_read(struct file *filp, char __user *buf,
+				size_t count, loff_t *f_pos)
+{
+	struct sk_buff *skb;
+	int bytes_to_copy;
+	int err;
+	struct cg2900_chip_dev *dev = filp->private_data;
+	struct test_info *info = dev->t_data;
+	struct sk_buff_head *rx_queue = &info->rx_queue;
+
+	dev_dbg(MISC_DEV, "cg2900_test_read count %d\n", count);
+
+	if (skb_queue_empty(rx_queue))
+		wait_event_interruptible(char_wait_queue,
+					 !(skb_queue_empty(rx_queue)));
+
+	skb = skb_dequeue(rx_queue);
+	if (!skb) {
+		dev_dbg(MISC_DEV,
+			"skb queue is empty - return with zero bytes\n");
+		bytes_to_copy = 0;
+		goto finished;
+	}
+
+	bytes_to_copy = min(count, skb->len);
+	err = copy_to_user(buf, skb->data, bytes_to_copy);
+	if (err) {
+		skb_queue_head(rx_queue, skb);
+		return -EFAULT;
+	}
+
+	skb_pull(skb, bytes_to_copy);
+
+	if (skb->len > 0)
+		skb_queue_head(rx_queue, skb);
+	else
+		kfree_skb(skb);
+
+finished:
+	return bytes_to_copy;
+}
+
+/**
+ * cg2900_test_write() - Copy buffer from user and write to CG2900 Core.
+ * @filp:	Pointer to the file struct.
+ * @buf:	Read buffer.
+ * @count:	Size of the buffer write.
+ * @f_pos:	Position in buffer.
+ *
+ * Returns:
+ *   >= 0 is number of bytes written.
+ *   -EFAULT if copy_from_user fails.
+ */
+static ssize_t cg2900_test_write(struct file *filp, const char __user *buf,
+				 size_t count, loff_t *f_pos)
+{
+	struct sk_buff *skb;
+	struct cg2900_chip_dev *dev = filp->private_data;
+	struct test_info *info = dev->t_data;
+
+	dev_dbg(MISC_DEV, "cg2900_test_write count %d\n", count);
+
+	/* Allocate the SKB and reserve space for the header */
+	skb = alloc_skb(count + RX_SKB_RESERVE, GFP_KERNEL);
+	if (!skb) {
+		dev_err(MISC_DEV, "cg2900_test_write: Failed to alloc skb\n");
+		return -ENOMEM;
+	}
+	skb_reserve(skb, RX_SKB_RESERVE);
+
+	if (copy_from_user(skb_put(skb, count), buf, count)) {
+		kfree_skb(skb);
+		return -EFAULT;
+	}
+
+	dev->c_cb.data_from_chip(dev, skb);
+
+	return count;
+}
+
+/**
+ * cg2900_test_poll() - Handle POLL call to the interface.
+ * @filp:	Pointer to the file struct.
+ * @wait:	Poll table supplied to caller.
+ *
+ * Returns:
+ *   Mask of current set POLL values (0 or (POLLIN | POLLRDNORM))
+ */
+static unsigned int cg2900_test_poll(struct file *filp, poll_table *wait)
+{
+	struct cg2900_chip_dev *dev = filp->private_data;
+	struct test_info *info = dev->t_data;
+	unsigned int mask = 0;
+
+	poll_wait(filp, &char_wait_queue, wait);
+
+	if (!(skb_queue_empty(&info->rx_queue)))
+		mask |= POLLIN | POLLRDNORM;
+
+	return mask;
+}
+
+static const struct file_operations test_char_dev_fops = {
+	.open = cg2900_test_open,
+	.release = cg2900_test_release,
+	.read = cg2900_test_read,
+	.write = cg2900_test_write,
+	.poll = cg2900_test_poll
+};
+
+/**
+ * test_char_dev_create() - Create a char device for testing.
+ * @info:	Test device info.
+ *
+ * Creates a separate char device that will interact directly with userspace
+ * test application.
+ *
+ * Returns:
+ *   0 if there is no error.
+ *   Error codes from misc_register.
+ */
+static int test_char_dev_create(struct test_info *info)
+{
+	int err;
+
+	/* Initialize the RX queue */
+	skb_queue_head_init(&info->rx_queue);
+
+	/* Prepare miscdevice struct before registering the device */
+	info->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	info->misc_dev.name = CG2900_CDEV_NAME;
+	info->misc_dev.fops = &test_char_dev_fops;
+	info->misc_dev.parent = info->dev;
+	info->misc_dev.mode = S_IRUGO | S_IWUGO;
+
+	err = misc_register(&info->misc_dev);
+	if (err) {
+		dev_err(info->dev, "Error %d registering misc dev", err);
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * test_char_dev_destroy() - Clean up after test_char_dev_create().
+ * @info:	Test device info.
+ */
+static void test_char_dev_destroy(struct test_info *info)
+{
+	int err;
+
+	err = misc_deregister(&info->misc_dev);
+	if (err)
+		dev_err(info->dev, "Error %d deregistering misc dev\n", err);
+
+	/* Clean the message queue */
+	skb_queue_purge(&info->rx_queue);
+}
+
+/**
+ * cg2900_test_probe() - Initialize module.
+ *
+ * @pdev:	Platform device.
+ *
+ * This function initializes and registers the test misc char device.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ *   -EEXIST if device already exists.
+ *   Error codes generated by test_char_dev_create.
+ */
+static int __devinit cg2900_test_probe(struct platform_device *pdev)
+{
+	int err;
+
+	dev_dbg(&pdev->dev, "cg2900_test_probe\n");
+
+	if (test_info) {
+		dev_err(&pdev->dev, "test_info exists\n");
+		return -EEXIST;
+	}
+
+	test_info = kzalloc(sizeof(*test_info), GFP_KERNEL);
+	if (!test_info) {
+		dev_err(&pdev->dev, "Couldn't allocate test_info\n");
+		return -ENOMEM;
+	}
+
+	test_info->dev = &pdev->dev;
+	test_info->pdev = pdev;
+
+	/* Create and add test char device. */
+	err = test_char_dev_create(test_info);
+	if (err) {
+		kfree(test_info);
+		test_info = NULL;
+		return err;
+	}
+
+	dev_set_drvdata(&pdev->dev, test_info);
+
+	dev_info(&pdev->dev, "CG2900 test char device driver started\n");
+
+	return 0;
+}
+
+/**
+ * cg2900_test_remove() - Remove module.
+ *
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM if core_info does not exist.
+ *   -EINVAL if platform data does not exist in the device.
+ */
+static int __devexit cg2900_test_remove(struct platform_device *pdev)
+{
+	struct test_info *test_info;
+
+	dev_dbg(&pdev->dev, "cg2900_test_remove\n");
+	test_info = dev_get_drvdata(&pdev->dev);
+	test_char_dev_destroy(test_info);
+	dev_set_drvdata(&pdev->dev, NULL);
+	kfree(test_info);
+	test_info = NULL;
+	dev_info(&pdev->dev, "CG2900 Test char device driver removed\n");
+	return 0;
+}
+
+static struct platform_driver cg2900_test_driver = {
+	.driver = {
+		.name	= "cg2900-test",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= cg2900_test_probe,
+	.remove	= __devexit_p(cg2900_test_remove),
+};
+
+/**
+ * cg2900_test_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init cg2900_test_init(void)
+{
+	pr_debug("cg2900_test_init");
+	return platform_driver_register(&cg2900_test_driver);
+}
+
+/**
+ * cg2900_test_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit cg2900_test_exit(void)
+{
+	pr_debug("cg2900_test_exit");
+	platform_driver_unregister(&cg2900_test_driver);
+}
+
+module_init(cg2900_test_init);
+module_exit(cg2900_test_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux CG2900 Test Char Device Driver");
diff --git a/drivers/staging/cg2900/mfd/stlc2690_chip.c b/drivers/staging/cg2900/mfd/stlc2690_chip.c
new file mode 100644
index 00000000000..84de0d7a976
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/stlc2690_chip.c
@@ -0,0 +1,1671 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson STLC2690 BT/FM controller.
+ */
+#define NAME					"stlc2690_chip"
+#define pr_fmt(fmt)				NAME ": " fmt "\n"
+
+#include <asm/byteorder.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/gfp.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/limits.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/stat.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/core.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+
+#include "cg2900.h"
+#include "cg2900_core.h"
+#include "cg2900_lib.h"
+#include "stlc2690_chip.h"
+
+#ifndef MAX
+#define MAX(x, y) (((x) > (y)) ? (x) : (y))
+#endif
+
+#define MAIN_DEV				(main_info->dev)
+#define BOOT_DEV				(info->user_in_charge->dev)
+
+#define WQ_NAME					"stlc2690_chip_wq"
+
+#define LINE_TOGGLE_DETECT_TIMEOUT		50	/* ms */
+#define CHIP_READY_TIMEOUT			100	/* ms */
+#define CHIP_STARTUP_TIMEOUT			15000	/* ms */
+#define CHIP_SHUTDOWN_TIMEOUT			15000	/* ms */
+
+/** CHANNEL_BT_CMD - Bluetooth HCI H:4 channel
+ * for Bluetooth commands in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_CMD				0x01
+
+/** CHANNEL_BT_ACL - Bluetooth HCI H:4 channel
+ * for Bluetooth ACL data in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_ACL				0x02
+
+/** CHANNEL_BT_EVT - Bluetooth HCI H:4 channel
+ * for Bluetooth events in the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_BT_EVT				0x04
+
+/** CHANNEL_HCI_LOGGER - Bluetooth HCI H:4 channel
+ * for logging all transmitted H4 packets (on all channels).
+ */
+#define CHANNEL_HCI_LOGGER			0xFA
+
+/** CHANNEL_CORE - Bluetooth HCI H:4 channel
+ * for user space control of the ST-Ericsson connectivity controller.
+ */
+#define CHANNEL_CORE				0xFD
+
+/*
+ * For the char dev names we keep the same names in order to be able to reuse
+ * the users and to keep a consistent interface.
+ */
+
+/** STLC2690_BT_CMD - Bluetooth HCI H4 channel for Bluetooth commands.
+ */
+#define STLC2690_BT_CMD				"cg2900_bt_cmd"
+
+/** STLC2690_BT_ACL - Bluetooth HCI H4 channel for Bluetooth ACL data.
+ */
+#define STLC2690_BT_ACL				"cg2900_bt_acl"
+
+/** STLC2690_BT_EVT - Bluetooth HCI H4 channel for Bluetooth events.
+ */
+#define STLC2690_BT_EVT				"cg2900_bt_evt"
+
+/** STLC2690_HCI_LOGGER - BT channel for logging all transmitted H4 packets.
+ * Data read is copy of all data transferred on the other channels.
+ * Only write allowed is configuration of the HCI Logger.
+ */
+#define STLC2690_HCI_LOGGER			"cg2900_hci_logger"
+
+/** STLC2690_CORE- Channel for keeping ST-Ericsson STLC2690 enabled.
+ * Opening this channel forces the chip to stay powered.
+ * No data can be written to or read from this channel.
+ */
+#define STLC2690_CORE				"cg2900_core"
+
+/**
+ * enum main_state - Main-state for STLC2690 driver.
+ * @STLC2690_INIT:	STLC2690 initializing.
+ * @STLC2690_IDLE:	No user registered to STLC2690 driver.
+ * @STLC2690_BOOTING:	STLC2690 booting after first user is registered.
+ * @STLC2690_CLOSING:	STLC2690 closing after last user has deregistered.
+ * @STLC2690_RESETING:	STLC2690 reset requested.
+ * @STLC2690_ACTIVE:	STLC2690 up and running with at least one user.
+ */
+enum main_state {
+	STLC2690_INIT,
+	STLC2690_IDLE,
+	STLC2690_BOOTING,
+	STLC2690_CLOSING,
+	STLC2690_RESETING,
+	STLC2690_ACTIVE
+};
+
+/**
+ * enum boot_state - BOOT-state for STLC2690 chip driver.
+ * @BOOT_RESET:				HCI Reset has been sent.
+ * @BOOT_SEND_BD_ADDRESS:		VS Store In FS command with BD address
+ *					has been sent.
+ * @BOOT_GET_FILES_TO_LOAD:		STLC2690 chip driver is retrieving file
+ *					to load.
+ * @BOOT_DOWNLOAD_PATCH:		STLC2690 chip driver is downloading
+ *					patches.
+ * @BOOT_ACTIVATE_PATCHES_AND_SETTINGS:	STLC2690 chip driver is activating
+ *					patches and settings.
+ * @BOOT_READY:				STLC2690 chip driver boot is ready.
+ * @BOOT_FAILED:			STLC2690 chip driver boot failed.
+ */
+enum boot_state {
+	BOOT_RESET,
+	BOOT_SEND_BD_ADDRESS,
+	BOOT_GET_FILES_TO_LOAD,
+	BOOT_DOWNLOAD_PATCH,
+	BOOT_ACTIVATE_PATCHES_AND_SETTINGS,
+	BOOT_READY,
+	BOOT_FAILED
+};
+
+/**
+ * enum file_load_state - BOOT_FILE_LOAD-state for STLC2690 chip driver.
+ * @FILE_LOAD_GET_PATCH:		Loading patches.
+ * @FILE_LOAD_GET_STATIC_SETTINGS:	Loading static settings.
+ * @FILE_LOAD_NO_MORE_FILES:		No more files to load.
+ * @FILE_LOAD_FAILED:			File loading failed.
+ */
+enum file_load_state {
+	FILE_LOAD_GET_PATCH,
+	FILE_LOAD_GET_STATIC_SETTINGS,
+	FILE_LOAD_NO_MORE_FILES,
+	FILE_LOAD_FAILED
+};
+
+/**
+ * enum download_state - BOOT_DOWNLOAD state.
+ * @DOWNLOAD_PENDING:	Download in progress.
+ * @DOWNLOAD_SUCCESS:	Download successfully finished.
+ * @DOWNLOAD_FAILED:	Downloading failed.
+ */
+enum download_state {
+	DOWNLOAD_PENDING,
+	DOWNLOAD_SUCCESS,
+	DOWNLOAD_FAILED
+};
+
+
+/**
+ * struct stlc2690_channel_item - List object for channel.
+ * @list:	list_head struct.
+ * @user:	User for this channel.
+ */
+struct stlc2690_channel_item {
+	struct list_head	list;
+	struct cg2900_user_data	*user;
+};
+
+/**
+ * struct stlc2690_skb_data - Structure for storing private data in an sk_buffer.
+ * @dev:	STLC2690 device for this sk_buffer.
+ */
+struct stlc2690_skb_data {
+	struct cg2900_user_data *user;
+};
+#define stlc2690_skb_data(__skb) ((struct stlc2690_skb_data *)((__skb)->cb))
+
+/**
+ * struct stlc2690_chip_info - Main info structure for STLC2690 chip driver.
+ * @patch_file_name:		Stores patch file name.
+ * @settings_file_name:		Stores settings file name.
+ * @file_info:			Firmware file info (patch or settings).
+ * @main_state:			Current MAIN-state of STLC2690 chip driver.
+ * @boot_state:			Current BOOT-state of STLC2690 chip driver.
+ * @file_load_state:		Current BOOT_FILE_LOAD-state of STLC2690 chip
+ *				driver.
+ * @download_state:		Current BOOT_DOWNLOAD-state of STLC2690 chip
+ *				driver.
+ * @wq:				STLC2690 chip driver workqueue.
+ * @chip_dev:			Chip handler info.
+ * @user_in_charge:		User currently operating. Normally used at
+ *				channel open and close.
+ * @last_user:			Last user of this chip.
+ * @logger:			Logger user of this chip.
+ * @startup:			True if system is starting up.
+ * @mfd_size:			Number of MFD cells.
+ * @mfd_char_size:		Number of MFD char device cells.
+ */
+struct stlc2690_chip_info {
+	char				*patch_file_name;
+	char				*settings_file_name;
+	struct cg2900_file_info		file_info;
+	enum main_state			main_state;
+	enum boot_state			boot_state;
+	enum file_load_state		file_load_state;
+	enum download_state		download_state;
+	struct workqueue_struct		*wq;
+	struct cg2900_chip_dev		*chip_dev;
+	spinlock_t			rw_lock;
+	struct list_head		open_channels;
+	struct cg2900_user_data		*user_in_charge;
+	struct cg2900_user_data		*last_user;
+	struct cg2900_user_data		*logger;
+	bool				startup;
+	int				mfd_size;
+	int				mfd_char_size;
+};
+
+/**
+ * struct main_info - Main info structure for STLC2690 chip driver.
+ * @dev:			Device structure.
+ * @cell_base_id:		Base ID for MFD cells.
+ * @man_mutex:			Management mutex.
+ */
+struct main_info {
+	struct device			*dev;
+	int				cell_base_id;
+	struct mutex			man_mutex;
+};
+
+static struct main_info *main_info;
+
+/*
+ * main_wait_queue - Main Wait Queue in STLC2690 driver.
+ */
+static DECLARE_WAIT_QUEUE_HEAD(main_wait_queue);
+
+static struct mfd_cell stlc2690_devs[];
+static struct mfd_cell stlc2690_char_devs[];
+
+static void chip_startup_finished(struct stlc2690_chip_info *info, int err);
+
+/**
+ * send_bd_address() - Send HCI VS command with BD address to the chip.
+ */
+static void send_bd_address(struct stlc2690_chip_info *info)
+{
+	struct bt_vs_store_in_fs_cmd *cmd;
+	u8 plen = sizeof(*cmd) + BT_BDADDR_SIZE;
+
+	cmd = kmalloc(plen, GFP_KERNEL);
+	if (!cmd)
+		return;
+
+	cmd->opcode = cpu_to_le16(STLC2690_BT_OP_VS_STORE_IN_FS);
+	cmd->plen = BT_PARAM_LEN(plen);
+	cmd->user_id = STLC2690_VS_STORE_IN_FS_USR_ID_BD_ADDR;
+	cmd->len = BT_BDADDR_SIZE;
+	/* Now copy the BD address received from user space control app. */
+	memcpy(cmd->data, bd_address, BT_BDADDR_SIZE);
+
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_SEND_BD_ADDRESS\n");
+	info->boot_state = BOOT_SEND_BD_ADDRESS;
+
+	cg2900_send_bt_cmd(info->user_in_charge, info->logger, cmd, plen);
+
+	kfree(cmd);
+}
+
+/**
+ * send_settings_file() - Transmit settings file.
+ *
+ * The send_settings_file() function transmit settings file.
+ * The file is read in parts to fit in HCI packets. When finished,
+ * close the settings file and send HCI reset to activate settings and patches.
+ */
+static void send_settings_file(struct stlc2690_chip_info *info)
+{
+	int bytes_sent;
+
+	bytes_sent = cg2900_read_and_send_file_part(info->user_in_charge,
+						    info->logger,
+						    &info->file_info);
+	if (bytes_sent > 0) {
+		/* Data sent. Wait for CmdComplete */
+		return;
+	} else if (bytes_sent < 0) {
+		dev_err(BOOT_DEV, "send_settings_file: Error %d occurred\n",
+			bytes_sent);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		chip_startup_finished(info, bytes_sent);
+		return;
+	}
+
+	/* No data was sent. This file is finished */
+	info->download_state = DOWNLOAD_SUCCESS;
+
+	/* Settings file finished. Release used resources */
+	dev_dbg(BOOT_DEV, "Settings file finished, release used resources\n");
+	release_firmware(info->file_info.fw_file);
+	info->file_info.fw_file = NULL;
+
+	dev_dbg(BOOT_DEV, "New file_load_state: FILE_LOAD_NO_MORE_FILES\n");
+	info->file_load_state = FILE_LOAD_NO_MORE_FILES;
+
+	/* Create and send HCI VS Store In FS command with bd address. */
+	send_bd_address(info);
+}
+
+/**
+ * send_patch_file - Transmit patch file.
+ *
+ * The send_patch_file() function transmit patch file.
+ * The file is read in parts to fit in HCI packets. When the complete file is
+ * transmitted, the file is closed.
+ * When finished, continue with settings file.
+ */
+static void send_patch_file(struct cg2900_chip_dev *dev)
+{
+	int err;
+	int bytes_sent;
+	struct stlc2690_chip_info *info = dev->c_data;
+	int file_name_size = strlen("STLC2690_XXXX_XXXX_settings.fw");
+
+	bytes_sent = cg2900_read_and_send_file_part(info->user_in_charge,
+						    info->logger,
+						    &info->file_info);
+	if (bytes_sent > 0) {
+		/* Data sent. Wait for CmdComplete */
+		return;
+	} else if (bytes_sent < 0) {
+		dev_err(BOOT_DEV, "send_patch_file: Error %d occurred\n",
+			bytes_sent);
+		err = bytes_sent;
+		goto error_handling;
+	}
+
+	/* No data was sent. This file is finished */
+	info->download_state = DOWNLOAD_SUCCESS;
+
+	dev_dbg(BOOT_DEV, "Patch file finished, release used resources\n");
+	release_firmware(info->file_info.fw_file);
+	info->file_info.fw_file = NULL;
+
+	/*
+	 * Create the settings file name from HCI revision and sub_version.
+	 * file_name_size does not include terminating NULL character
+	 * so add 1.
+	 */
+	err = snprintf(info->settings_file_name, file_name_size + 1,
+			"STLC2690_%04X_%04X_settings.fw",
+			dev->chip.hci_revision, dev->chip.hci_sub_version);
+	if (err == file_name_size) {
+		dev_dbg(BOOT_DEV, "Downloading settings file %s\n",
+				info->settings_file_name);
+	} else {
+		dev_err(BOOT_DEV, "Settings file name failed! err=%d\n", err);
+		goto error_handling;
+	}
+
+	/* Retrieve the settings file */
+	err = request_firmware(&info->file_info.fw_file,
+			       info->settings_file_name,
+			       info->chip_dev->dev);
+	if (err) {
+		dev_err(BOOT_DEV, "Couldn't get settings file (%d)\n", err);
+		goto error_handling;
+	}
+	/* Now send the settings file */
+	dev_dbg(BOOT_DEV,
+		"New file_load_state: FILE_LOAD_GET_STATIC_SETTINGS\n");
+	info->file_load_state = FILE_LOAD_GET_STATIC_SETTINGS;
+	dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_PENDING\n");
+	info->download_state = DOWNLOAD_PENDING;
+	send_settings_file(info);
+	return;
+
+error_handling:
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, err);
+}
+
+/**
+ * work_reset_after_error() - Handle reset.
+ * @work:	Reference to work data.
+ *
+ * Handle a reset after received Command Complete event.
+ */
+static void work_reset_after_error(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_reset_after_error: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	chip_startup_finished(info, -EIO);
+
+	kfree(my_work);
+}
+
+/**
+ * work_load_patch_and_settings() - Start loading patches and settings.
+ * @work:	Reference to work data.
+ */
+static void work_load_patch_and_settings(struct work_struct *work)
+{
+	int err = 0;
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+	int file_name_size = strlen("STLC2690_XXXX_XXXX_patch.fw");
+
+	if (!work) {
+		dev_err(MAIN_DEV,
+			"work_load_patch_and_settings: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/* Check that we are in the right state */
+	if (info->boot_state != BOOT_GET_FILES_TO_LOAD)
+		goto finished;
+
+	/*
+	 * Create the patch file name from HCI revision and sub_version.
+	 * file_name_size does not include terminating NULL character
+	 * so add 1.
+	 */
+	err = snprintf(info->patch_file_name, file_name_size + 1,
+			"STLC2690_%04X_%04X_patch.fw", dev->chip.hci_revision,
+			dev->chip.hci_sub_version);
+	if (err == file_name_size) {
+		dev_dbg(BOOT_DEV, "Downloading patch file %s\n",
+				info->patch_file_name);
+	} else {
+		dev_err(BOOT_DEV, "Patch file name failed! err=%d\n", err);
+		goto error_handling;
+	}
+
+	/* We now all info needed */
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_DOWNLOAD_PATCH\n");
+	info->boot_state = BOOT_DOWNLOAD_PATCH;
+	dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_PENDING\n");
+	info->download_state = DOWNLOAD_PENDING;
+	dev_dbg(BOOT_DEV, "New file_load_state: FILE_LOAD_GET_PATCH\n");
+	info->file_load_state = FILE_LOAD_GET_PATCH;
+	info->file_info.chunk_id = 0;
+	info->file_info.file_offset = 0;
+	info->file_info.fw_file = NULL;
+
+	/* OK. Now it is time to download the patches */
+	err = request_firmware(&(info->file_info.fw_file),
+			       info->patch_file_name,
+			       dev->dev);
+	if (err < 0) {
+		dev_err(BOOT_DEV, "Couldn't get patch file (%d)\n", err);
+		goto error_handling;
+	}
+	send_patch_file(dev);
+
+	goto finished;
+
+error_handling:
+	dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+	info->boot_state = BOOT_FAILED;
+	chip_startup_finished(info, -EIO);
+finished:
+	kfree(my_work);
+}
+
+/**
+ * work_cont_file_download() - A file block has been written.
+ * @work:	Reference to work data.
+ *
+ * Handle a received HCI VS Write File Block Complete event.
+ * Normally this means continue to send files to the controller.
+ */
+static void work_cont_file_download(struct work_struct *work)
+{
+	struct cg2900_work *my_work;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	if (!work) {
+		dev_err(MAIN_DEV, "work_cont_file_download: work == NULL\n");
+		return;
+	}
+
+	my_work = container_of(work, struct cg2900_work, work);
+	dev = my_work->user_data;
+	info = dev->c_data;
+
+	/* Continue to send patches or settings to the controller */
+	if (info->file_load_state == FILE_LOAD_GET_PATCH)
+		send_patch_file(dev);
+	else if (info->file_load_state == FILE_LOAD_GET_STATIC_SETTINGS)
+		send_settings_file(info);
+	else
+		dev_dbg(BOOT_DEV, "No more files to load\n");
+
+	kfree(my_work);
+}
+
+/**
+ * handle_reset_cmd_complete() - Handles HCI Reset Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_reset_cmd_complete(struct cg2900_chip_dev *dev, u8 *data)
+{
+	u8 status = data[0];
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	dev_dbg(BOOT_DEV, "Received Reset complete event with status 0x%X\n",
+		status);
+
+	if (BOOT_RESET != info->boot_state &&
+	    BOOT_ACTIVATE_PATCHES_AND_SETTINGS != info->boot_state)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		dev_err(BOOT_DEV, "Command complete for HciReset received with "
+			"error 0x%X\n", status);
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+		return true;
+	}
+
+	if (BOOT_RESET == info->boot_state) {
+		info->boot_state = BOOT_GET_FILES_TO_LOAD;
+		cg2900_create_work_item(info->wq, work_load_patch_and_settings,
+					dev);
+	} else {
+		/*
+		 * The boot sequence is now finished successfully.
+		 * Set states and signal to waiting thread.
+		 */
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_READY\n");
+		info->boot_state = BOOT_READY;
+		chip_startup_finished(info, 0);
+	}
+
+	return true;
+}
+
+
+/**
+ * handle_vs_store_in_fs_cmd_complete() - Handles HCI VS StoreInFS Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_store_in_fs_cmd_complete(struct cg2900_chip_dev *dev,
+					       u8 *data)
+{
+	u8 status = data[0];
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	dev_dbg(BOOT_DEV,
+		"Received Store_in_FS complete event with status 0x%X\n",
+		status);
+
+	if (info->boot_state != BOOT_SEND_BD_ADDRESS)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR == status) {
+		struct hci_command_hdr cmd;
+
+		/* Send HCI Reset command to activate patches */
+		dev_dbg(BOOT_DEV,
+			"New boot_state: BOOT_ACTIVATE_PATCHES_AND_SETTINGS\n");
+		info->boot_state = BOOT_ACTIVATE_PATCHES_AND_SETTINGS;
+
+		cmd.opcode = cpu_to_le16(HCI_OP_RESET);
+		cmd.plen = 0; /* No parameters for Reset */
+		cg2900_send_bt_cmd(info->user_in_charge, info->logger, &cmd,
+				   sizeof(cmd));
+	} else {
+		dev_err(BOOT_DEV,
+			"Command complete for StoreInFS received with error "
+			"0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_write_file_block_cmd_complete() - Handles HCI VS WriteFileBlock Command Complete event.
+ * @data:	Pointer to received HCI data packet.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_write_file_block_cmd_complete(struct cg2900_chip_dev *dev,
+						    u8 *data)
+{
+	u8 status = data[0];
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DOWNLOAD_PATCH ||
+	    info->download_state != DOWNLOAD_PENDING)
+		return false;
+
+	if (HCI_BT_ERROR_NO_ERROR == status)
+		cg2900_create_work_item(info->wq, work_cont_file_download, dev);
+	else {
+		dev_err(BOOT_DEV,
+			"Command complete for WriteFileBlock received with"
+			" error 0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_FAILED\n");
+		info->download_state = DOWNLOAD_FAILED;
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		if (info->file_info.fw_file) {
+			release_firmware(info->file_info.fw_file);
+			info->file_info.fw_file = NULL;
+		}
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_vs_write_file_block_cmd_status() - Handles HCI VS WriteFileBlock Command Status event.
+ * @status:	Returned status of WriteFileBlock command.
+ *
+ * Returns:
+ *   true,  if packet was handled internally,
+ *   false, otherwise.
+ */
+static bool handle_vs_write_file_block_cmd_status(struct cg2900_chip_dev *dev,
+						  u8 status)
+{
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	if (info->boot_state != BOOT_DOWNLOAD_PATCH ||
+	    info->download_state != DOWNLOAD_PENDING)
+		return false;
+
+	/*
+	 * Only do something if there is an error. Otherwise we will wait for
+	 * CmdComplete.
+	 */
+	if (HCI_BT_ERROR_NO_ERROR != status) {
+		dev_err(BOOT_DEV,
+			"Command status for WriteFileBlock received with"
+			" error 0x%X\n", status);
+		dev_dbg(BOOT_DEV, "New download_state: DOWNLOAD_FAILED\n");
+		info->download_state = DOWNLOAD_FAILED;
+		dev_dbg(BOOT_DEV, "New boot_state: BOOT_FAILED\n");
+		info->boot_state = BOOT_FAILED;
+		if (info->file_info.fw_file) {
+			release_firmware(info->file_info.fw_file);
+			info->file_info.fw_file = NULL;
+		}
+		cg2900_create_work_item(info->wq, work_reset_after_error, dev);
+	}
+
+	return true;
+}
+
+/**
+ * handle_rx_data_bt_evt() - Check if received data should be handled in STLC2690 chip driver.
+ * @skb:	Data packet
+ *
+ * The handle_rx_data_bt_evt() function checks if received data should be
+ * handled in STLC2690 chip driver. If so handle it correctly.
+ * Received data is always HCI BT Event.
+ *
+ * Returns:
+ *   True,  if packet was handled internally,
+ *   False, otherwise.
+ */
+static bool handle_rx_data_bt_evt(struct cg2900_chip_dev *dev,
+				  struct sk_buff *skb)
+{
+	bool pkt_handled = false;
+	/* skb cannot be NULL here so it is safe to de-reference */
+	u8 *data = skb->data;
+	struct hci_event_hdr *evt;
+	u16 op_code;
+
+	evt = (struct hci_event_hdr *)data;
+	data += sizeof(*evt);
+
+	/* First check the event code. */
+	if (HCI_EV_CMD_COMPLETE == evt->evt) {
+		struct hci_ev_cmd_complete *cmd_complete;
+
+		cmd_complete = (struct hci_ev_cmd_complete *)data;
+		op_code = le16_to_cpu(cmd_complete->opcode);
+		dev_dbg(dev->dev,
+			"Received Command Complete: op_code = 0x%04X\n",
+			op_code);
+		/* Move to first byte after OCF */
+		data += sizeof(*cmd_complete);
+
+		if (op_code == HCI_OP_RESET)
+			pkt_handled = handle_reset_cmd_complete(dev, data);
+		else if (op_code == STLC2690_BT_OP_VS_STORE_IN_FS)
+			pkt_handled = handle_vs_store_in_fs_cmd_complete(dev,
+									 data);
+		else if (op_code == STLC2690_BT_OP_VS_WRITE_FILE_BLOCK)
+			pkt_handled =
+				handle_vs_write_file_block_cmd_complete(dev,
+									data);
+	} else if (HCI_EV_CMD_STATUS == evt->evt) {
+		struct hci_ev_cmd_status *cmd_status;
+
+		cmd_status = (struct hci_ev_cmd_status *)data;
+
+		op_code = le16_to_cpu(cmd_status->opcode);
+
+		dev_dbg(dev->dev, "Received Command Status: op_code = 0x%04X\n",
+			op_code);
+
+		if (op_code == STLC2690_BT_OP_VS_WRITE_FILE_BLOCK)
+			pkt_handled = handle_vs_write_file_block_cmd_status
+				(dev, cmd_status->status);
+	} else if (HCI_EV_HW_ERROR == evt->evt) {
+		struct hci_ev_hw_error *hw_error;
+
+		hw_error = (struct hci_ev_hw_error *)data;
+		/*
+		 * Only do a printout. There might be a receiving stack that can
+		 * handle this event
+		 */
+		dev_err(dev->dev, "HW Error event received with error 0x%02X\n",
+			hw_error->hw_code);
+		return false;
+	} else
+		return false;
+
+	if (pkt_handled)
+		kfree_skb(skb);
+
+	return pkt_handled;
+}
+
+/**
+ * data_from_chip() - Called when data is received from the chip.
+ * @dev:	Chip info.
+ * @skb:	Packet received.
+ *
+ * The data_from_chip() function checks if packet is a response for a packet it
+ * itself has transmitted. If not it finds the correct user and sends the packet
+ * to the user.
+ */
+static void data_from_chip(struct cg2900_chip_dev *dev,
+			   struct sk_buff *skb)
+{
+	int h4_channel;
+	struct list_head *cursor;
+	struct stlc2690_channel_item *tmp;
+	struct stlc2690_chip_info *info = dev->c_data;
+	struct cg2900_user_data *user = NULL;
+
+	h4_channel = skb->data[0];
+	skb_pull(skb, HCI_H4_SIZE);
+
+	/* Then check if this is a response to data we have sent */
+	if (h4_channel == CHANNEL_BT_EVT && handle_rx_data_bt_evt(dev, skb))
+		return;
+
+	spin_lock_bh(&info->rw_lock);
+
+	/* Let's see if this packet has the same user as the last one */
+	if (info->last_user && info->last_user->h4_channel == h4_channel) {
+		user = info->last_user;
+		goto user_found;
+	}
+
+	/* Search through the list of all open channels to find the user */
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct stlc2690_channel_item, list);
+		if (tmp->user->h4_channel == h4_channel) {
+			user = tmp->user;
+			goto user_found;
+		}
+	}
+
+user_found:
+	info->last_user = user;
+	spin_unlock_bh(&info->rw_lock);
+
+	if (user)
+		user->read_cb(user, skb);
+	else {
+		dev_err(dev->dev,
+			"Could not find corresponding user to h4_channel %d\n",
+			h4_channel);
+		kfree_skb(skb);
+	}
+}
+
+static void chip_removed(struct cg2900_chip_dev *dev)
+{
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	mfd_remove_devices(dev->dev);
+	kfree(info->settings_file_name);
+	kfree(info->patch_file_name);
+	destroy_workqueue(info->wq);
+	kfree(info);
+	dev->c_data = NULL;
+	dev->c_cb.chip_removed = NULL;
+	dev->c_cb.data_from_chip = NULL;
+}
+
+/**
+ * chip_shutdown() - Reset and power the chip off.
+ */
+static void chip_shutdown(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev = cg2900_get_prv(user);
+	struct stlc2690_chip_info *info = dev->c_data;
+
+	dev_dbg(user->dev, "chip_shutdown\n");
+
+	/* Close the transport, which will power off the chip */
+	if (dev->t_cb.close)
+		dev->t_cb.close(dev);
+
+	/* Chip shut-down finished, set correct state and wake up the chip. */
+	dev_dbg(dev->dev, "New main_state: STLC2690_IDLE\n");
+	info->main_state = STLC2690_IDLE;
+	wake_up_all(&main_wait_queue);
+}
+
+static void chip_startup_finished(struct stlc2690_chip_info *info, int err)
+{
+	dev_dbg(BOOT_DEV, "chip_startup_finished (%d)\n", err);
+
+	if (err)
+		/* Shutdown the chip */
+		chip_shutdown(info->user_in_charge);
+	else {
+		dev_dbg(BOOT_DEV, "New main_state: CORE_ACTIVE\n");
+		info->main_state = STLC2690_ACTIVE;
+	}
+
+	wake_up_all(&main_wait_queue);
+
+	if (err)
+		return;
+
+	if (!info->chip_dev->t_cb.chip_startup_finished)
+		dev_err(BOOT_DEV, "chip_startup_finished callback not found\n");
+	else
+		info->chip_dev->t_cb.chip_startup_finished(info->chip_dev);
+}
+
+static int stlc2690_open(struct cg2900_user_data *user)
+{
+	int err;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+	struct list_head *cursor;
+	struct stlc2690_channel_item *tmp;
+	struct hci_command_hdr cmd;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "stlc2690_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "stlc2690_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	mutex_lock(&main_info->man_mutex);
+
+	/* Add a minor wait in order to avoid CPU blocking, looping openings */
+	err = wait_event_timeout(main_wait_queue,
+				 (STLC2690_IDLE == info->main_state ||
+				  STLC2690_ACTIVE == info->main_state),
+				 msecs_to_jiffies(LINE_TOGGLE_DETECT_TIMEOUT));
+	if (err <= 0) {
+		if (STLC2690_INIT == info->main_state)
+			dev_err(user->dev, "Transport not opened\n");
+		else
+			dev_err(user->dev, "stlc2690_open currently busy "
+				"(0x%X). Try again\n", info->main_state);
+		err = -EBUSY;
+		goto err_free_mutex;
+	}
+
+	err = 0;
+
+	list_for_each(cursor, &info->open_channels) {
+		tmp = list_entry(cursor, struct stlc2690_channel_item, list);
+		if (tmp->user->h4_channel == user->h4_channel) {
+			dev_err(user->dev, "Channel %d is already opened\n",
+				user->h4_channel);
+			err = -EACCES;
+			goto err_free_mutex;
+		}
+	}
+
+	tmp = kzalloc(sizeof(*tmp), GFP_KERNEL);
+	if (!tmp) {
+		dev_err(user->dev, "Could not allocate tmp\n");
+		err = -ENOMEM;
+		goto err_free_mutex;
+	}
+	tmp->user = user;
+
+	if (STLC2690_ACTIVE != info->main_state &&
+	    !user->chip_independent) {
+		/* Open transport and start-up the chip */
+		if (dev->t_cb.set_chip_power)
+			dev->t_cb.set_chip_power(dev, true);
+
+		/* Wait to be sure that the chip is ready */
+		schedule_timeout_killable(
+				msecs_to_jiffies(CHIP_READY_TIMEOUT));
+
+		if (dev->t_cb.open)
+			err = dev->t_cb.open(dev);
+		if (err) {
+			if (dev->t_cb.set_chip_power)
+				dev->t_cb.set_chip_power(dev, false);
+			goto err_free_list_item;
+		}
+
+		/* Start the boot sequence */
+		info->user_in_charge = user;
+		info->last_user = user;
+		dev_dbg(user->dev, "New boot_state: BOOT_RESET\n");
+		info->boot_state = BOOT_RESET;
+		dev_dbg(user->dev, "New main_state: STLC2690_BOOTING\n");
+		info->main_state = STLC2690_BOOTING;
+		cmd.opcode = cpu_to_le16(HCI_OP_RESET);
+		cmd.plen = 0; /* No parameters for HCI reset */
+		cg2900_send_bt_cmd(user, info->logger, &cmd, sizeof(cmd));
+
+		dev_dbg(user->dev, "Wait up to 15 seconds for chip to start\n");
+		wait_event_timeout(main_wait_queue,
+				   (STLC2690_ACTIVE == info->main_state ||
+				    STLC2690_IDLE   == info->main_state),
+				   msecs_to_jiffies(CHIP_STARTUP_TIMEOUT));
+		if (STLC2690_ACTIVE != info->main_state) {
+			dev_err(user->dev, "STLC2690 driver failed to start\n");
+
+			if (dev->t_cb.close)
+				dev->t_cb.close(dev);
+
+			dev_dbg(user->dev, "New main_state: CORE_IDLE\n");
+			info->main_state = STLC2690_IDLE;
+			err = -EIO;
+			goto err_free_list_item;
+		}
+	}
+
+	list_add_tail(&tmp->list, &info->open_channels);
+
+	user->opened = true;
+
+	dev_dbg(user->dev, "H:4 channel opened\n");
+
+	mutex_unlock(&main_info->man_mutex);
+	return 0;
+err_free_list_item:
+	kfree(tmp);
+err_free_mutex:
+	mutex_unlock(&main_info->man_mutex);
+	return err;
+}
+
+static int stlc2690_hci_log_open(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+	int err;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_hci_log_open: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(user->dev, "stlc2690_hci_log_open\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	if (info->logger) {
+		dev_err(user->dev, "HCI Logger already stored\n");
+		return -EACCES;
+	}
+
+	info->logger = user;
+	err = stlc2690_open(user);
+	if (err)
+		info->logger = NULL;
+	return err;
+}
+
+static void stlc2690_close(struct cg2900_user_data *user)
+{
+	bool keep_powered = false;
+	struct list_head *cursor, *next;
+	struct stlc2690_channel_item *tmp;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "stlc2690_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	mutex_lock(&main_info->man_mutex);
+
+	/*
+	 * Go through each open channel. Remove our channel and check if there
+	 * is any other channel that want to keep the chip running
+	 */
+	list_for_each_safe(cursor, next, &info->open_channels) {
+		tmp = list_entry(cursor, struct stlc2690_channel_item, list);
+		if (tmp->user == user) {
+			list_del(cursor);
+			kfree(tmp);
+		} else if (!tmp->user->chip_independent)
+			keep_powered = true;
+	}
+
+	if (keep_powered)
+		/* This was not the last user, we're done. */
+		goto finished;
+
+	if (STLC2690_IDLE == info->main_state)
+		/* Chip has already been shut down. */
+		goto finished;
+
+	dev_dbg(user->dev, "New main_state: CORE_CLOSING\n");
+	info->main_state = STLC2690_CLOSING;
+	chip_shutdown(user);
+
+	dev_dbg(user->dev, "Wait up to 15 seconds for chip to shut-down\n");
+	wait_event_timeout(main_wait_queue,
+			   STLC2690_IDLE == info->main_state,
+			   msecs_to_jiffies(CHIP_SHUTDOWN_TIMEOUT));
+
+	/* Force shutdown if we timed out */
+	if (STLC2690_IDLE != info->main_state) {
+		dev_err(user->dev,
+			"ST-Ericsson STLC2690 Core Driver was shut-down with "
+			"problems\n");
+
+		if (dev->t_cb.close)
+			dev->t_cb.close(dev);
+
+		dev_dbg(user->dev, "New main_state: CORE_IDLE\n");
+		info->main_state = STLC2690_IDLE;
+	}
+
+finished:
+	mutex_unlock(&main_info->man_mutex);
+	user->opened = false;
+	dev_dbg(user->dev, "H:4 channel closed\n");
+}
+
+static void stlc2690_hci_log_close(struct cg2900_user_data *user)
+{
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_hci_log_close: Calling with NULL pointer\n");
+		return;
+	}
+
+	dev_dbg(user->dev, "stlc2690_hci_log_close\n");
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	info->logger = NULL;
+	stlc2690_close(user);
+}
+
+static int stlc2690_reset(struct cg2900_user_data *user)
+{
+	struct list_head *cursor, *next;
+	struct stlc2690_channel_item *tmp;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_reset: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	dev_info(user->dev, "stlc2690_reset\n");
+
+	BUG_ON(!main_info);
+
+	mutex_lock(&main_info->man_mutex);
+
+	dev_dbg(user->dev, "New main_state: CORE_RESETING\n");
+	info->main_state = STLC2690_RESETING;
+
+	chip_shutdown(user);
+
+	/*
+	 * Inform all opened channels about the reset and free the user devices
+	 */
+	list_for_each_safe(cursor, next, &info->open_channels) {
+		tmp = list_entry(cursor, struct stlc2690_channel_item, list);
+		list_del(cursor);
+		tmp->user->opened = false;
+		tmp->user->reset_cb(tmp->user);
+		kfree(tmp);
+	}
+
+	/* Reset finished. We are now idle until first channel is opened */
+	dev_dbg(user->dev, "New main_state: STLC2690_IDLE\n");
+	info->main_state = STLC2690_IDLE;
+
+	mutex_unlock(&main_info->man_mutex);
+
+	/*
+	 * Send wake-up since this might have been called from a failed boot.
+	 * No harm done if it is a STLC2690 chip user who called.
+	 */
+	wake_up_all(&main_wait_queue);
+
+	return 0;
+}
+
+static struct sk_buff *stlc2690_alloc_skb(unsigned int size, gfp_t priority)
+{
+	struct sk_buff *skb;
+
+	dev_dbg(MAIN_DEV, "stlc2690_alloc_skb size %d bytes\n", size);
+
+	/* Allocate the SKB and reserve space for the header */
+	skb = alloc_skb(size + CG2900_SKB_RESERVE, priority);
+	if (skb)
+		skb_reserve(skb, CG2900_SKB_RESERVE);
+
+	return skb;
+}
+
+static int stlc2690_write(struct cg2900_user_data *user, struct sk_buff *skb)
+{
+	int err = 0;
+	u8 *h4_header;
+	struct cg2900_chip_dev *dev;
+	struct stlc2690_chip_info *info;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV,
+			"stlc2690_write: Calling with NULL pointer\n");
+		return -EINVAL;
+	}
+
+	if (!skb) {
+		dev_err(user->dev, "stlc2690_write with no sk_buffer\n");
+		return -EINVAL;
+	}
+
+	dev = cg2900_get_prv(user);
+	info = dev->c_data;
+
+	dev_dbg(user->dev, "stlc2690_write length %d bytes\n", skb->len);
+
+	if (!user->opened) {
+		dev_err(user->dev,
+			"Trying to transmit data on a closed channel\n");
+		return -EACCES;
+	}
+
+	/*
+	 * Move the data pointer to the H:4 header position and
+	 * store the H4 header.
+	 */
+	h4_header = skb_push(skb, CG2900_SKB_RESERVE);
+	*h4_header = (u8)user->h4_channel;
+	cg2900_tx_to_chip(user, info->logger, skb);
+
+	return err;
+}
+
+static int stlc2690_no_write(struct cg2900_user_data *user,
+			     struct sk_buff *skb)
+{
+	dev_err(user->dev, "Not allowed to send on this channel\n");
+	return -EPERM;
+}
+
+static bool stlc2690_get_local_revision(struct cg2900_user_data *user,
+					struct cg2900_rev_data *rev_data)
+{
+	struct cg2900_chip_dev *dev;
+
+	BUG_ON(!main_info);
+
+	if (!user) {
+		dev_err(MAIN_DEV, "stlc2690_get_local_revision: Calling with "
+			"NULL pointer\n");
+		return false;
+	}
+
+	if (!rev_data) {
+		dev_err(user->dev, "Calling with rev_data NULL\n");
+		return false;
+	}
+
+	dev = cg2900_get_prv(user);
+
+	rev_data->revision = dev->chip.hci_revision;
+	rev_data->sub_version = dev->chip.hci_sub_version;
+
+	return true;
+}
+
+static struct cg2900_user_data btcmd_data = {
+	.h4_channel = CHANNEL_BT_CMD,
+};
+static struct cg2900_user_data btacl_data = {
+	.h4_channel = CHANNEL_BT_ACL,
+};
+static struct cg2900_user_data btevt_data = {
+	.h4_channel = CHANNEL_BT_EVT,
+};
+static struct cg2900_user_data hci_logger_data = {
+	.h4_channel = CHANNEL_HCI_LOGGER,
+	.chip_independent = true,
+	.write = stlc2690_no_write,
+	.open = stlc2690_hci_log_open,
+	.close = stlc2690_hci_log_close,
+};
+static struct cg2900_user_data core_data = {
+	.h4_channel = CHANNEL_CORE,
+	.write = stlc2690_no_write,
+};
+
+static struct mfd_cell stlc2690_devs[] = {
+	{
+		.name = "cg2900-btcmd",
+		.platform_data = &btcmd_data,
+		.pdata_size = sizeof(btcmd_data),
+	},
+	{
+		.name = "cg2900-btacl",
+		.platform_data = &btacl_data,
+		.pdata_size = sizeof(btacl_data),
+	},
+	{
+		.name = "cg2900-btevt",
+		.platform_data = &btevt_data,
+		.pdata_size = sizeof(btevt_data),
+	},
+	{
+		.name = "cg2900-hcilogger",
+		.platform_data = &hci_logger_data,
+		.pdata_size = sizeof(hci_logger_data),
+	},
+	{
+		.name = "cg2900-core",
+		.platform_data = &core_data,
+		.pdata_size = sizeof(core_data),
+	},
+};
+
+static struct cg2900_user_data char_btcmd_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_BT_CMD,
+	},
+	.h4_channel = CHANNEL_BT_CMD,
+};
+static struct cg2900_user_data char_btacl_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_BT_ACL,
+	},
+	.h4_channel = CHANNEL_BT_ACL,
+};
+static struct cg2900_user_data char_btevt_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_BT_EVT,
+	},
+	.h4_channel = CHANNEL_BT_EVT,
+};
+static struct cg2900_user_data char_hci_logger_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_HCI_LOGGER,
+	},
+	.h4_channel = CHANNEL_HCI_LOGGER,
+	.chip_independent = true,
+	.write = stlc2690_no_write,
+	.open = stlc2690_hci_log_open,
+	.close = stlc2690_hci_log_close,
+};
+static struct cg2900_user_data char_core_data = {
+	.channel_data = {
+		.char_dev_name = STLC2690_CORE,
+	},
+	.h4_channel = CHANNEL_CORE,
+	.write = stlc2690_no_write,
+};
+
+static struct mfd_cell stlc2690_char_devs[] = {
+	{
+		.name = "cg2900-chardev",
+		.id = 0,
+		.platform_data = &char_btcmd_data,
+		.pdata_size = sizeof(char_btcmd_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 1,
+		.platform_data = &char_btacl_data,
+		.pdata_size = sizeof(char_btacl_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 2,
+		.platform_data = &char_btevt_data,
+		.pdata_size = sizeof(char_btevt_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 7,
+		.platform_data = &char_hci_logger_data,
+		.pdata_size = sizeof(char_hci_logger_data),
+	},
+	{
+		.name = "cg2900-chardev",
+		.id = 8,
+		.platform_data = &char_core_data,
+		.pdata_size = sizeof(char_core_data),
+	},
+};
+
+/**
+ * set_plat_data() - Initializes data for an MFD cell.
+ * @cell:	MFD cell.
+ * @dev:	Current chip.
+ *
+ * Sets each callback to default function unless already set.
+ */
+static void set_plat_data(struct mfd_cell *cell, struct cg2900_chip_dev *dev)
+{
+	struct cg2900_user_data *user = cell->platform_data;
+
+	if (!user->open)
+		user->open = stlc2690_open;
+	if (!user->close)
+		user->close = stlc2690_close;
+	if (!user->reset)
+		user->reset = stlc2690_reset;
+	if (!user->alloc_skb)
+		user->alloc_skb = stlc2690_alloc_skb;
+	if (!user->write)
+		user->write = stlc2690_write;
+	if (!user->get_local_revision)
+		user->get_local_revision = stlc2690_get_local_revision;
+
+	cg2900_set_prv(user, dev);
+}
+
+/**
+ * check_chip_support() - Checks if connected chip is handled by this driver.
+ * @dev:	Chip info structure.
+ *
+ * If supported return true and fill in @callbacks.
+ *
+ * Returns:
+ *   true if chip is handled by this driver.
+ *   false otherwise.
+ */
+static bool check_chip_support(struct cg2900_chip_dev *dev)
+{
+	struct cg2900_platform_data *pf_data;
+	struct stlc2690_chip_info *info;
+	int i;
+	int err;
+
+	dev_dbg(dev->dev, "check_chip_support\n");
+
+	/*
+	 * Check if this is a STLC2690 revision.
+	 * We do not care about the sub-version at the moment. Change this if
+	 * necessary.
+	 */
+	if (dev->chip.manufacturer != STLC2690_SUPP_MANUFACTURER ||
+	    dev->chip.hci_revision < STLC2690_SUPP_REVISION_MIN ||
+	    dev->chip.hci_revision > STLC2690_SUPP_REVISION_MAX) {
+		dev_dbg(dev->dev, "Chip not supported by STLC2690 driver\n"
+			"\tMan: 0x%02X\n"
+			"\tRev: 0x%04X\n"
+			"\tSub: 0x%04X\n",
+			dev->chip.manufacturer, dev->chip.hci_revision,
+			dev->chip.hci_sub_version);
+		return false;
+	}
+
+	/* Store needed data */
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(dev->dev, "Couldn't allocate info struct\n");
+		return false;
+	}
+
+	/* Initialize all variables */
+	INIT_LIST_HEAD(&info->open_channels);
+	spin_lock_init(&info->rw_lock);
+	info->chip_dev = dev;
+
+	info->wq = create_singlethread_workqueue(WQ_NAME);
+	if (!info->wq) {
+		dev_err(dev->dev, "Could not create workqueue\n");
+		goto err_handling_free_info;
+	}
+
+	info->patch_file_name = kzalloc(NAME_MAX + 1, GFP_ATOMIC);
+	if (!info->patch_file_name) {
+		dev_err(dev->dev,
+			"Couldn't allocate name buffer for patch file\n");
+		goto err_handling_destroy_wq;
+	}
+
+	info->settings_file_name = kzalloc(NAME_MAX + 1, GFP_ATOMIC);
+	if (!info->settings_file_name) {
+		dev_err(dev->dev,
+			"Couldn't allocate name buffers settings file\n");
+		goto err_handling_free_patch_name;
+	}
+
+	dev->c_data = info;
+	/* Set the callbacks */
+	dev->c_cb.data_from_chip = data_from_chip;
+	dev->c_cb.chip_removed = chip_removed,
+	info->chip_dev = dev;
+
+	mutex_lock(&main_info->man_mutex);
+
+	pf_data = dev_get_platdata(dev->dev);
+	btcmd_data.channel_data.bt_bus = pf_data->bus;
+	btacl_data.channel_data.bt_bus = pf_data->bus;
+	btevt_data.channel_data.bt_bus = pf_data->bus;
+
+	for (i = 0; i < ARRAY_SIZE(stlc2690_devs); i++)
+		set_plat_data(&stlc2690_devs[i], dev);
+	for (i = 0; i < ARRAY_SIZE(stlc2690_char_devs); i++)
+		set_plat_data(&stlc2690_char_devs[i], dev);
+	mutex_unlock(&main_info->man_mutex);
+
+	dev_info(dev->dev, "Chip supported by the STLC2690 chip driver\n");
+
+	/* Close the transport, which will power off the chip */
+	if (dev->t_cb.close)
+		dev->t_cb.close(dev);
+
+	dev_dbg(dev->dev, "New main_state: STLC2690_IDLE\n");
+	info->main_state = STLC2690_IDLE;
+
+	err = mfd_add_devices(dev->dev, main_info->cell_base_id, stlc2690_devs,
+			      ARRAY_SIZE(stlc2690_devs), NULL, 0);
+	if (err) {
+		dev_err(dev->dev, "Failed to add stlc2690_devs (%d)\n", err);
+		goto err_handling_free_settings_name;
+	}
+
+	err = mfd_add_devices(dev->dev, main_info->cell_base_id,
+			      stlc2690_char_devs,
+			      ARRAY_SIZE(stlc2690_char_devs), NULL, 0);
+	if (err) {
+		dev_err(dev->dev, "Failed to add stlc2690_char_devs (%d)\n",
+			err);
+		goto err_handling_remove_devs;
+	}
+
+	/*
+	 * Increase base ID so next connected transport will not get the
+	 * same device IDs.
+	 */
+	main_info->cell_base_id += MAX(ARRAY_SIZE(stlc2690_devs),
+				       ARRAY_SIZE(stlc2690_char_devs));
+
+	return true;
+
+err_handling_remove_devs:
+	mfd_remove_devices(dev->dev);
+err_handling_free_settings_name:
+	kfree(info->settings_file_name);
+err_handling_free_patch_name:
+	kfree(info->patch_file_name);
+err_handling_destroy_wq:
+	destroy_workqueue(info->wq);
+err_handling_free_info:
+	kfree(info);
+	return false;
+}
+
+static struct cg2900_id_callbacks chip_support_callbacks = {
+	.check_chip_support = check_chip_support,
+};
+
+/**
+ * stlc2690_chip_probe() - Initialize STLC2690 chip handler resources.
+ * @pdev:	Platform device.
+ *
+ * This function initializes the STLC2690 driver, then registers to
+ * the CG2900 Core.
+ *
+ * Returns:
+ *   0 if success.
+ *   -ENOMEM for failed alloc or structure creation.
+ *   Error codes generated by cg2900_register_chip_driver.
+ */
+static int __devinit stlc2690_chip_probe(struct platform_device *pdev)
+{
+	int err;
+
+	dev_dbg(&pdev->dev, "stlc2690_chip_probe\n");
+
+	main_info = kzalloc(sizeof(*main_info), GFP_ATOMIC);
+	if (!main_info) {
+		dev_err(&pdev->dev, "Couldn't allocate main_info\n");
+		return -ENOMEM;
+	}
+
+	main_info->dev = &pdev->dev;
+	mutex_init(&main_info->man_mutex);
+
+	err = cg2900_register_chip_driver(&chip_support_callbacks);
+	if (err) {
+		dev_err(&pdev->dev,
+			"Couldn't register chip driver (%d)\n", err);
+		goto error_handling;
+	}
+
+	dev_info(&pdev->dev, "STLC2690 chip driver started\n");
+
+	return 0;
+
+error_handling:
+	mutex_destroy(&main_info->man_mutex);
+	kfree(main_info);
+	main_info = NULL;
+	return err;
+}
+
+/**
+ * stlc2690_chip_remove() - Release STLC2690 chip handler resources.
+ * @pdev:	Platform device.
+ *
+ * Returns:
+ *   0 if success (always success).
+ */
+static int __devexit stlc2690_chip_remove(struct platform_device *pdev)
+{
+	dev_info(&pdev->dev, "STLC2690 chip driver removed\n");
+
+	cg2900_deregister_chip_driver(&chip_support_callbacks);
+
+	if (!main_info)
+		return 0;
+
+	mutex_destroy(&main_info->man_mutex);
+	kfree(main_info);
+	main_info = NULL;
+	return 0;
+}
+
+static struct platform_driver stlc2690_chip_driver = {
+	.driver = {
+		.name	= "stlc2690-chip",
+		.owner	= THIS_MODULE,
+	},
+	.probe	= stlc2690_chip_probe,
+	.remove	= __devexit_p(stlc2690_chip_remove),
+};
+
+/**
+ * stlc2690_chip_init() - Initialize module.
+ *
+ * Registers platform driver.
+ */
+static int __init stlc2690_chip_init(void)
+{
+	pr_debug("stlc2690_chip_init");
+	return platform_driver_register(&stlc2690_chip_driver);
+}
+
+/**
+ * stlc2690_chip_exit() - Remove module.
+ *
+ * Unregisters platform driver.
+ */
+static void __exit stlc2690_chip_exit(void)
+{
+	pr_debug("stlc2690_chip_exit");
+	platform_driver_unregister(&stlc2690_chip_driver);
+}
+
+module_init(stlc2690_chip_init);
+module_exit(stlc2690_chip_exit);
+
+MODULE_AUTHOR("Par-Gunnar Hjalmdahl ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Linux STLC2690 Connectivity Device Driver");
diff --git a/drivers/staging/cg2900/mfd/stlc2690_chip.h b/drivers/staging/cg2900/mfd/stlc2690_chip.h
new file mode 100644
index 00000000000..d14e7737636
--- /dev/null
+++ b/drivers/staging/cg2900/mfd/stlc2690_chip.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Authors:
+ * Par-Gunnar Hjalmdahl (par-gunnar.p.hjalmdahl@stericsson.com) for ST-Ericsson.
+ * Henrik Possung (henrik.possung@stericsson.com) for ST-Ericsson.
+ * Josef Kindberg (josef.kindberg@stericsson.com) for ST-Ericsson.
+ * Dariusz Szymszak (dariusz.xd.szymczak@stericsson.com) for ST-Ericsson.
+ * Kjell Andersson (kjell.k.andersson@stericsson.com) for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2
+ *
+ * Linux Bluetooth HCI H:4 Driver for ST-Ericsson STLC2690 BT/FM controller.
+ */
+
+#ifndef _STLC2690_CHIP_H_
+#define _STLC2690_CHIP_H_
+
+/* Supported chips */
+#define STLC2690_SUPP_MANUFACTURER		0x30
+#define STLC2690_SUPP_REVISION_MIN		0x0500
+#define STLC2690_SUPP_REVISION_MAX		0x06FF
+
+#define BT_SIZE_OF_HDR				(sizeof(__le16) + sizeof(__u8))
+#define BT_PARAM_LEN(__pkt_len)			(__pkt_len - BT_SIZE_OF_HDR)
+
+/* BT VS Store In FS command */
+#define STLC2690_BT_OP_VS_STORE_IN_FS		0xFC22
+struct bt_vs_store_in_fs_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	user_id;
+	__u8	len;
+	__u8	data[];
+} __packed;
+
+/* BT VS Write File Block command */
+#define STLC2690_BT_OP_VS_WRITE_FILE_BLOCK	0xFC2E
+struct bt_vs_write_file_block_cmd {
+	__le16	opcode;
+	__u8	plen;
+	__u8	id;
+	__u8	data[];
+} __packed;
+
+/* User ID for storing BD address in chip using Store_In_FS command */
+#define STLC2690_VS_STORE_IN_FS_USR_ID_BD_ADDR	0xFE
+
+#endif /* _STLC2690_CHIP_H_ */
diff --git a/drivers/staging/cw1200/.gitignore b/drivers/staging/cw1200/.gitignore
new file mode 100644
index 00000000000..6ad0d1ec58e
--- /dev/null
+++ b/drivers/staging/cw1200/.gitignore
@@ -0,0 +1,10 @@
+*.o
+*.ko
+*.ko.cmd
+.tmp_versions
+modules.order
+Module.symvers
+Module.markers
+*.o.cmd
+*.mod.c
+*.swp
diff --git a/drivers/staging/cw1200/Kconfig b/drivers/staging/cw1200/Kconfig
new file mode 100644
index 00000000000..4be840c1576
--- /dev/null
+++ b/drivers/staging/cw1200/Kconfig
@@ -0,0 +1,105 @@
+config CW1200
+       tristate "CW1200 WLAN support"
+       select MAC80211
+       select CFG80211
+       help
+
+         This is an experimental driver for the cw1200 chip-set.
+         Enabling this option enables the generic driver without
+         any platform support.
+
+         Please select the appropriate platform below.
+
+if CW1200
+
+config CW1200_NON_POWER_OF_TWO_BLOCKSIZES
+      bool "Platform supports non-power-of-two SDIO transfer"
+      depends on CW1200
+      help
+        Say N here only if you are running the driver on a platform
+        which does not have support for non-power-of-two SDIO transfer.
+        If unsure, say Y.
+
+config CW1200_USE_GPIO_IRQ
+      bool "Use GPIO interrupt"
+      depends on CW1200
+      help
+        Say Y here if you want to include GPIO IRQ support instead of SDIO IRQ.
+        If unsure, say N.
+
+config CW1200_5GHZ_SUPPORT
+      bool "5GHz band support"
+      depends on CW1200
+      help
+        Say Y if your device supports 5GHz band. Should be disabled for
+        CW1100 silicon.
+        If unsure, say N.
+
+config CW1200_WAPI_SUPPORT
+      bool "WAPI support"
+      depends on CW1200
+      help
+        Say Y if your compat-wireless support WAPI.
+        If unsure, say N.
+
+config CW1200_USE_STE_EXTENSIONS
+      bool "STE extensions"
+      depends on CW1200
+      help
+        Say Y if you want to include STE extensions.
+        If unsure, say N.
+
+config CW1200_DISABLE_BEACON_HINTS
+      bool "Disable 11d beacon hints"
+      depends on CW1200
+      help
+        Say Y if you want to disable 11d beacon hints.
+        If unsure, say N.
+
+config CW1200_U5500_SUPPORT
+      bool "Enable U5500 support"
+      depends on CW1200
+      help
+        Say Y if you want to enable wlan on u5500 platform support.
+        If unsure, say N.
+
+menu "Driver debug features"
+      depends on CW1200
+
+config CW1200_DEBUGFS
+      bool "Expose driver internals to DebugFS (DEVELOPMENT)"
+
+config CW1200_BH_DEBUG
+      bool "Enable low-level device communication logs (DEVELOPMENT)"
+
+config CW1200_WSM_DEBUG
+      bool "Enable WSM API debug messages (DEVELOPMENT)"
+
+config CW1200_WSM_DUMPS
+      bool "Verbose WSM API logging (DEVELOPMENT)"
+
+config CW1200_WSM_DUMPS_SHORT
+      bool "Dump only first x bytes (default 20) (DEVELOPMENT)"
+
+config CW1200_TXRX_DEBUG
+      bool "Enable TX/RX debug messages (DEVELOPMENT)"
+
+config CW1200_TX_POLICY_DEBUG
+      bool "Enable TX policy debug (DEVELOPMENT)"
+
+config CW1200_STA_DEBUG
+      bool "Enable STA/AP debug (DEVELOPMENT)"
+
+config CW1200_DUMP_ON_ERROR
+      bool "Dump kernel in case of critical error (DEVELOPMENT)"
+
+endmenu
+
+config CW1200_ITP
+      bool "Enable ITP DebugFS"
+      depends on CW1200
+      help
+        Say Y if you want to include ITP code.
+        If unsure, say N.
+
+endif
diff --git a/drivers/staging/cw1200/Makefile b/drivers/staging/cw1200/Makefile
new file mode 100644
index 00000000000..67d7867c1b5
--- /dev/null
+++ b/drivers/staging/cw1200/Makefile
@@ -0,0 +1,20 @@
+cw1200_core-y := \
+		fwio.o \
+		txrx.o \
+		main.o \
+		queue.o \
+		hwio.o \
+		bh.o \
+		wsm.o \
+		sta.o \
+		ap.o \
+		scan.o
+cw1200_core-$(CONFIG_CW1200_DEBUGFS)	+= debug.o
+cw1200_core-$(CONFIG_CW1200_ITP)	+= itp.o
+cw1200_core-$(CONFIG_PM)		+= pm.o
+
+cw1200_wlan-y := cw1200_sdio.o
+
+obj-$(CONFIG_CW1200) += cw1200_core.o
+obj-$(CONFIG_CW1200) += cw1200_wlan.o
+
diff --git a/drivers/staging/cw1200/TODO b/drivers/staging/cw1200/TODO
new file mode 100644
index 00000000000..0d2be40e1f4
--- /dev/null
+++ b/drivers/staging/cw1200/TODO
@@ -0,0 +1,10 @@
+TODO:
+        - IBSS: Not implemented (3-10 m*d).
+        - 11n: Almost done. WSM API upgrade is required fo finish implementation. (2-3 m*d).
+        - 11n: verification (??? m*d Resources? WLAN RF lab? 11n sniffers
+          availability? Bring up of the test equipment?).
+        - memory leakage verification and proper cleanup: not done (1-3 m*d).
+        - AP (hot-spot) mode: Implemented, some problems with WEP104/WPA/WPA2 security.
+          FW bug? To be investigated.
+        - U-APSD configuration (0.5-1 m*d).
+        - Cleanup of debug printouts (1 m*d).
diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
new file mode 100644
index 00000000000..55096cf0e2c
--- /dev/null
+++ b/drivers/staging/cw1200/ap.c
@@ -0,0 +1,1149 @@
+/*
+ * mac80211 STA and AP API for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include "cw1200.h"
+#include "sta.h"
+#include "ap.h"
+#include "bh.h"
+
+#if defined(CONFIG_CW1200_STA_DEBUG)
+#define ap_printk(...) printk(__VA_ARGS__)
+#else
+#define ap_printk(...)
+#endif
+
+#define CW1200_LINK_ID_GC_TIMEOUT ((unsigned long)(10 * HZ))
+
+#ifndef ERP_INFO_BYTE_OFFSET
+#define ERP_INFO_BYTE_OFFSET 2
+#endif
+
+static int cw1200_upload_beacon(struct cw1200_common *priv);
+static int cw1200_upload_pspoll(struct cw1200_common *priv);
+static int cw1200_upload_null(struct cw1200_common *priv);
+static int cw1200_start_ap(struct cw1200_common *priv);
+static int cw1200_update_beaconing(struct cw1200_common *priv);
+static int cw1200_enable_beaconing(struct cw1200_common *priv,
+				   bool enable);
+static void __cw1200_sta_notify(struct ieee80211_hw *dev,
+				struct ieee80211_vif *vif,
+				enum sta_notify_cmd notify_cmd,
+				int link_id);
+
+/* ******************************************************************** */
+/* AP API								*/
+
+int cw1200_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		   struct ieee80211_sta *sta)
+{
+	struct cw1200_common *priv = hw->priv;
+	struct cw1200_sta_priv *sta_priv =
+			(struct cw1200_sta_priv *)&sta->drv_priv;
+	struct cw1200_link_entry *entry;
+	struct sk_buff *skb;
+
+	if (priv->mode != NL80211_IFTYPE_AP)
+		return 0;
+
+	sta_priv->link_id = cw1200_find_link_id(priv, sta->addr);
+	if (WARN_ON(!sta_priv->link_id)) {
+		/* Impossible error */
+		wiphy_info(priv->hw->wiphy,
+			"[AP] No more link IDs available.\n");
+		return -ENOENT;
+	}
+
+	entry = &priv->link_id_db[sta_priv->link_id - 1];
+	spin_lock_bh(&priv->ps_state_lock);
+	if ((sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK) ==
+					IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK)
+		priv->sta_asleep_mask |= BIT(sta_priv->link_id);
+	entry->status = CW1200_LINK_HARD;
+	while ((skb = skb_dequeue(&entry->rx_queue)))
+		ieee80211_rx_irqsafe(priv->hw, skb);
+	spin_unlock_bh(&priv->ps_state_lock);
+	return 0;
+}
+
+int cw1200_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		      struct ieee80211_sta *sta)
+{
+	struct cw1200_common *priv = hw->priv;
+	struct cw1200_sta_priv *sta_priv =
+			(struct cw1200_sta_priv *)&sta->drv_priv;
+	struct cw1200_link_entry *entry;
+
+	if (priv->mode != NL80211_IFTYPE_AP || !sta_priv->link_id)
+		return 0;
+
+	entry = &priv->link_id_db[sta_priv->link_id - 1];
+	spin_lock_bh(&priv->ps_state_lock);
+	entry->status = CW1200_LINK_RESERVE;
+	entry->timestamp = jiffies;
+	wsm_lock_tx_async(priv);
+	if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
+		wsm_unlock_tx(priv);
+	spin_unlock_bh(&priv->ps_state_lock);
+	flush_workqueue(priv->workqueue);
+	return 0;
+}
+
+static void __cw1200_sta_notify(struct ieee80211_hw *dev,
+				struct ieee80211_vif *vif,
+				enum sta_notify_cmd notify_cmd,
+				int link_id)
+{
+	struct cw1200_common *priv = dev->priv;
+	u32 bit, prev;
+
+	/* Zero link id means "for all link IDs" */
+	if (link_id)
+		bit = BIT(link_id);
+	else if (WARN_ON_ONCE(notify_cmd != STA_NOTIFY_AWAKE))
+		bit = 0;
+	else
+		bit = priv->link_id_map;
+	prev = priv->sta_asleep_mask & bit;
+
+	switch (notify_cmd) {
+	case STA_NOTIFY_SLEEP:
+		if (!prev) {
+			if (priv->buffered_multicasts &&
+					!priv->sta_asleep_mask)
+				queue_work(priv->workqueue,
+					&priv->multicast_start_work);
+			priv->sta_asleep_mask |= bit;
+		}
+		break;
+	case STA_NOTIFY_AWAKE:
+		if (prev) {
+			priv->sta_asleep_mask &= ~bit;
+			priv->pspoll_mask &= ~bit;
+			if (priv->tx_multicast && link_id &&
+					!priv->sta_asleep_mask)
+				queue_work(priv->workqueue,
+					&priv->multicast_stop_work);
+			cw1200_bh_wakeup(priv);
+		}
+		break;
+	}
+}
+
+void cw1200_sta_notify(struct ieee80211_hw *dev,
+		       struct ieee80211_vif *vif,
+		       enum sta_notify_cmd notify_cmd,
+		       struct ieee80211_sta *sta)
+{
+	struct cw1200_common *priv = dev->priv;
+	struct cw1200_sta_priv *sta_priv =
+		(struct cw1200_sta_priv *)&sta->drv_priv;
+
+	spin_lock_bh(&priv->ps_state_lock);
+	__cw1200_sta_notify(dev, vif, notify_cmd, sta_priv->link_id);
+	spin_unlock_bh(&priv->ps_state_lock);
+}
+
+static void cw1200_ps_notify(struct cw1200_common *priv,
+		      int link_id, bool ps)
+{
+	if (link_id > CW1200_MAX_STA_IN_AP_MODE)
+		return;
+
+	txrx_printk(KERN_DEBUG "%s for LinkId: %d. STAs asleep: %.8X\n",
+			ps ? "Stop" : "Start",
+			link_id, priv->sta_asleep_mask);
+
+	__cw1200_sta_notify(priv->hw, priv->vif,
+		ps ? STA_NOTIFY_SLEEP : STA_NOTIFY_AWAKE, link_id);
+}
+
+static int cw1200_set_tim_impl(struct cw1200_common *priv, bool aid0_bit_set)
+{
+	struct sk_buff *skb;
+	struct wsm_update_ie update_ie = {
+		.what = WSM_UPDATE_IE_BEACON,
+		.count = 1,
+	};
+	u16 tim_offset, tim_length;
+
+	ap_printk(KERN_DEBUG "[AP] %s mcast: %s.\n",
+		__func__, aid0_bit_set ? "ena" : "dis");
+
+	skb = ieee80211_beacon_get_tim(priv->hw, priv->vif,
+			&tim_offset, &tim_length);
+	if (!skb) {
+		if (!__cw1200_flush(priv, true))
+			wsm_unlock_tx(priv);
+		return -ENOENT;
+	}
+
+	if (tim_offset && tim_length >= 6) {
+		/* Ignore DTIM count from mac80211:
+		 * firmware handles DTIM internally. */
+		skb->data[tim_offset + 2] = 0;
+
+		/* Set/reset aid0 bit */
+		if (aid0_bit_set)
+			skb->data[tim_offset + 4] |= 1;
+		else
+			skb->data[tim_offset + 4] &= ~1;
+	}
+
+	update_ie.ies = &skb->data[tim_offset];
+	update_ie.length = tim_length;
+	WARN_ON(wsm_update_ie(priv, &update_ie));
+
+	dev_kfree_skb(skb);
+
+	return 0;
+}
+
+void cw1200_set_tim_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, set_tim_work);
+	(void)cw1200_set_tim_impl(priv, priv->aid0_bit_set);
+}
+
+int cw1200_set_tim(struct ieee80211_hw *dev, struct ieee80211_sta *sta,
+		   bool set)
+{
+	struct cw1200_common *priv = dev->priv;
+	queue_work(priv->workqueue, &priv->set_tim_work);
+	return 0;
+}
+
+void cw1200_set_cts_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, set_cts_work.work);
+
+	u8 erp_ie[3] = {WLAN_EID_ERP_INFO, 0x1, 0};
+	struct wsm_update_ie update_ie = {
+		.what = WSM_UPDATE_IE_BEACON,
+		.count = 1,
+		.ies = erp_ie,
+		.length = 3,
+	};
+	u32 erp_info;
+	__le32 use_cts_prot;
+	mutex_lock(&priv->conf_mutex);
+	erp_info = priv->erp_info;
+	mutex_unlock(&priv->conf_mutex);
+	use_cts_prot =
+		erp_info & WLAN_ERP_USE_PROTECTION ?
+		__cpu_to_le32(1) : 0;
+
+	erp_ie[ERP_INFO_BYTE_OFFSET] = erp_info;
+
+	ap_printk(KERN_DEBUG "[STA] ERP information 0x%x\n", erp_info);
+
+	WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_NON_ERP_PROTECTION,
+				&use_cts_prot, sizeof(use_cts_prot)));
+	WARN_ON(wsm_update_ie(priv, &update_ie));
+
+	return;
+}
+
+static int cw1200_set_btcoexinfo(struct cw1200_common *priv)
+{
+	struct wsm_override_internal_txrate arg;
+	int ret = 0;
+
+	if (priv->mode == NL80211_IFTYPE_STATION) {
+		/* Plumb PSPOLL and NULL template */
+		WARN_ON(cw1200_upload_pspoll(priv));
+		WARN_ON(cw1200_upload_null(priv));
+	} else {
+		return 0;
+	}
+
+	memset(&arg, 0, sizeof(struct wsm_override_internal_txrate));
+
+	if (!priv->vif->p2p) {
+		/* STATION mode */
+		if (priv->bss_params.operationalRateSet & ~0xF) {
+			ap_printk(KERN_DEBUG "[STA] STA has ERP rates\n");
+			/* G or BG mode */
+			arg.internalTxRate = (__ffs(
+			priv->bss_params.operationalRateSet & ~0xF));
+		} else {
+			ap_printk(KERN_DEBUG "[STA] STA has non ERP rates\n");
+			/* B only mode */
+			arg.internalTxRate = (__ffs(
+			priv->association_mode.basicRateSet));
+		}
+		arg.nonErpInternalTxRate = (__ffs(
+			priv->association_mode.basicRateSet));
+	} else {
+		/* P2P mode */
+		arg.internalTxRate = (__ffs(
+			priv->bss_params.operationalRateSet & ~0xF));
+		arg.nonErpInternalTxRate = (__ffs(
+			priv->bss_params.operationalRateSet & ~0xF));
+	}
+
+	ap_printk(KERN_DEBUG "[STA] BTCOEX_INFO"
+		"MODE %d, internalTxRate : %x, nonErpInternalTxRate: %x\n",
+		priv->mode,
+		arg.internalTxRate,
+		arg.nonErpInternalTxRate);
+
+	ret = WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE,
+		&arg, sizeof(arg)));
+
+	return ret;
+}
+
+void cw1200_bss_info_changed(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif,
+			     struct ieee80211_bss_conf *info,
+			     u32 changed)
+{
+	struct cw1200_common *priv = dev->priv;
+
+	mutex_lock(&priv->conf_mutex);
+	if (changed & BSS_CHANGED_BSSID) {
+		memcpy(priv->bssid, info->bssid, ETH_ALEN);
+		cw1200_setup_mac(priv);
+	}
+
+	/* TODO: BSS_CHANGED_IBSS */
+
+	if (changed & BSS_CHANGED_ARP_FILTER) {
+		struct wsm_arp_ipv4_filter filter = {0};
+		int i;
+
+		ap_printk(KERN_DEBUG "[STA] BSS_CHANGED_ARP_FILTER "
+				     "enabled: %d, cnt: %d\n",
+				     info->arp_filter_enabled,
+				     info->arp_addr_cnt);
+
+		if (info->arp_filter_enabled)
+			filter.enable = __cpu_to_le32(1);
+
+		/* Currently only one IP address is supported by firmware.
+		 * In case of more IPs arp filtering will be disabled. */
+		if (info->arp_addr_cnt > 0 &&
+		    info->arp_addr_cnt <= WSM_MAX_ARP_IP_ADDRTABLE_ENTRIES) {
+			for (i = 0; i < info->arp_addr_cnt; i++) {
+				filter.ipv4Address[i] = info->arp_addr_list[i];
+				ap_printk(KERN_DEBUG "[STA] addr[%d]: 0x%X\n",
+					  i, filter.ipv4Address[i]);
+			}
+		} else
+			filter.enable = 0;
+
+		ap_printk(KERN_DEBUG "[STA] arp ip filter enable: %d\n",
+			  __le32_to_cpu(filter.enable));
+
+		if (wsm_set_arp_ipv4_filter(priv, &filter))
+			WARN_ON(1);
+	}
+
+
+	if (changed & BSS_CHANGED_BEACON) {
+		ap_printk(KERN_DEBUG "BSS_CHANGED_BEACON\n");
+		WARN_ON(cw1200_update_beaconing(priv));
+		WARN_ON(cw1200_upload_beacon(priv));
+	}
+
+	if (changed & BSS_CHANGED_BEACON_ENABLED) {
+		ap_printk(KERN_DEBUG "BSS_CHANGED_BEACON_ENABLED\n");
+
+		if (priv->enable_beacon != info->enable_beacon) {
+			WARN_ON(cw1200_enable_beaconing(priv,
+							info->enable_beacon));
+			priv->enable_beacon = info->enable_beacon;
+		}
+	}
+
+	if (changed & BSS_CHANGED_BEACON_INT) {
+		ap_printk(KERN_DEBUG "CHANGED_BEACON_INT\n");
+		/* Restart AP only when connected */
+		if (priv->join_status == CW1200_JOIN_STATUS_AP)
+			WARN_ON(cw1200_update_beaconing(priv));
+	}
+
+
+	if (changed & BSS_CHANGED_ASSOC) {
+		wsm_lock_tx(priv);
+		priv->wep_default_key_id = -1;
+		wsm_unlock_tx(priv);
+
+		if (!info->assoc /* && !info->ibss_joined */) {
+			priv->cqm_link_loss_count = 60;
+			priv->cqm_beacon_loss_count = 20;
+			priv->cqm_tx_failure_thold = 0;
+		}
+		priv->cqm_tx_failure_count = 0;
+	}
+
+	if (changed &
+	    (BSS_CHANGED_ASSOC |
+	     BSS_CHANGED_BASIC_RATES |
+	     BSS_CHANGED_ERP_PREAMBLE |
+	     BSS_CHANGED_HT |
+	     BSS_CHANGED_ERP_SLOT)) {
+		ap_printk(KERN_DEBUG "BSS_CHANGED_ASSOC.\n");
+		if (info->assoc) { /* TODO: ibss_joined */
+			struct ieee80211_sta *sta = NULL;
+			priv->join_dtim_period = info->dtim_period;
+			priv->beacon_int = info->beacon_int;
+
+			/* Associated: kill join timeout */
+			cancel_delayed_work_sync(&priv->join_timeout);
+
+			rcu_read_lock();
+			if (info->bssid)
+				sta = ieee80211_find_sta(vif, info->bssid);
+			if (sta) {
+				BUG_ON(!priv->channel);
+				priv->ht_info.ht_cap = sta->ht_cap;
+				priv->bss_params.operationalRateSet =
+					__cpu_to_le32(
+					cw1200_rate_mask_to_wsm(priv,
+					sta->supp_rates[priv->channel->band]));
+				priv->ht_info.channel_type =
+						info->channel_type;
+				priv->ht_info.operation_mode =
+						info->ht_operation_mode;
+			} else {
+				memset(&priv->ht_info, 0,
+						sizeof(priv->ht_info));
+				priv->bss_params.operationalRateSet = -1;
+			}
+			rcu_read_unlock();
+
+			if (sta) {
+				__le32 val = 0;
+				if (priv->ht_info.operation_mode &
+				IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT) {
+					ap_printk(KERN_DEBUG"[STA]"
+						" Non-GF STA present\n");
+					/* Non Green field capable STA */
+					val = __cpu_to_le32(BIT(1));
+				}
+				WARN_ON(wsm_write_mib(priv,
+					WSM_MID_ID_SET_HT_PROTECTION,
+					&val, sizeof(val)));
+			}
+
+			priv->association_mode.greenfieldMode =
+				cw1200_ht_greenfield(&priv->ht_info);
+			priv->association_mode.flags =
+				WSM_ASSOCIATION_MODE_SNOOP_ASSOC_FRAMES |
+				WSM_ASSOCIATION_MODE_USE_PREAMBLE_TYPE |
+				WSM_ASSOCIATION_MODE_USE_HT_MODE |
+				WSM_ASSOCIATION_MODE_USE_BASIC_RATE_SET |
+				WSM_ASSOCIATION_MODE_USE_MPDU_START_SPACING;
+			priv->association_mode.preambleType =
+				info->use_short_preamble ?
+				WSM_JOIN_PREAMBLE_SHORT :
+				WSM_JOIN_PREAMBLE_LONG;
+			priv->association_mode.basicRateSet = __cpu_to_le32(
+				cw1200_rate_mask_to_wsm(priv,
+				info->basic_rates));
+			priv->association_mode.mpduStartSpacing =
+				cw1200_ht_ampdu_density(&priv->ht_info);
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+			priv->cqm_beacon_loss_count =
+					info->cqm_beacon_miss_thold;
+			priv->cqm_tx_failure_thold =
+					info->cqm_tx_fail_thold;
+			priv->cqm_tx_failure_count = 0;
+			cancel_delayed_work_sync(&priv->bss_loss_work);
+			cancel_delayed_work_sync(&priv->connection_loss_work);
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+
+			priv->bss_params.beaconLostCount =
+					priv->cqm_beacon_loss_count ?
+					priv->cqm_beacon_loss_count :
+					priv->cqm_link_loss_count;
+
+			priv->bss_params.aid = info->aid;
+
+			if (priv->join_dtim_period < 1)
+				priv->join_dtim_period = 1;
+
+			ap_printk(KERN_DEBUG "[STA] DTIM %d, interval: %d\n",
+				priv->join_dtim_period, priv->beacon_int);
+			ap_printk(KERN_DEBUG "[STA] Preamble: %d, " \
+				"Greenfield: %d, Aid: %d, " \
+				"Rates: 0x%.8X, Basic: 0x%.8X\n",
+				priv->association_mode.preambleType,
+				priv->association_mode.greenfieldMode,
+				priv->bss_params.aid,
+				priv->bss_params.operationalRateSet,
+				priv->association_mode.basicRateSet);
+			WARN_ON(wsm_set_association_mode(priv,
+				&priv->association_mode));
+			WARN_ON(wsm_keep_alive_period(priv, 30 /* sec */));
+			WARN_ON(wsm_set_bss_params(priv, &priv->bss_params));
+			priv->setbssparams_done = true;
+			WARN_ON(wsm_set_beacon_wakeup_period(priv,
+				    priv->beacon_int * priv->join_dtim_period >
+				    MAX_BEACON_SKIP_TIME_MS ? 1 :
+				    priv->join_dtim_period, 0));
+
+			cw1200_set_pm(priv, &priv->powersave_mode);
+			if (priv->vif->p2p) {
+				ap_printk(KERN_DEBUG
+					"[STA] Setting p2p powersave "
+					"configuration.\n");
+				WARN_ON(wsm_set_p2p_ps_modeinfo(priv,
+					&priv->p2p_ps_modeinfo));
+			}
+
+			if (priv->is_BT_Present)
+				WARN_ON(cw1200_set_btcoexinfo(priv));
+#if 0
+			/* It's better to override internal TX rete; otherwise
+			 * device sends RTS at too high rate. However device
+			 * can't receive CTS at 1 and 2 Mbps. Well, 5.5 is a
+			 * good choice for RTS/CTS, but that means PS poll
+			 * will be sent at the same rate - impact on link
+			 * budget. Not sure what is better.. */
+
+			/* Update: internal rate selection algorythm is not
+			 * bad: if device is not receiving CTS at high rate,
+			 * it drops RTS rate.
+			 * So, conclusion: if-0 the code. Keep code just for
+			 * information:
+			 * Do not touch WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE! */
+
+			/* ~3 is a bug in device: RTS/CTS is not working at
+			 * low rates */
+
+			__le32 internal_tx_rate = __cpu_to_le32(__ffs(
+				priv->association_mode.basicRateSet & ~3));
+			WARN_ON(wsm_write_mib(priv,
+				WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE,
+				&internal_tx_rate,
+				sizeof(internal_tx_rate)));
+#endif
+		} else {
+			memset(&priv->association_mode, 0,
+				sizeof(priv->association_mode));
+			memset(&priv->bss_params, 0, sizeof(priv->bss_params));
+		}
+	}
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_ERP_CTS_PROT)) {
+		u32 prev_erp_info = priv->erp_info;
+
+		if (info->use_cts_prot)
+			priv->erp_info |= WLAN_ERP_USE_PROTECTION;
+		else if (!(prev_erp_info & WLAN_ERP_NON_ERP_PRESENT))
+			priv->erp_info &= ~WLAN_ERP_USE_PROTECTION;
+
+		if (prev_erp_info != priv->erp_info)
+			queue_delayed_work(priv->workqueue,
+					&priv->set_cts_work, 0*HZ);
+	}
+
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_ERP_SLOT)) {
+		__le32 slot_time = info->use_short_slot ?
+			__cpu_to_le32(9) : __cpu_to_le32(20);
+		ap_printk(KERN_DEBUG "[STA] Slot time :%d us.\n",
+			__le32_to_cpu(slot_time));
+		WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_DOT11_SLOT_TIME,
+			&slot_time, sizeof(slot_time)));
+	}
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_CQM)) {
+		struct wsm_rcpi_rssi_threshold threshold = {
+			.rollingAverageCount = 8,
+		};
+
+#if 0
+		/* For verification purposes */
+		info->cqm_rssi_thold = -50;
+		info->cqm_rssi_hyst = 4;
+#endif /* 0 */
+
+		ap_printk(KERN_DEBUG "[CQM] RSSI threshold "
+			"subscribe: %d +- %d\n",
+			info->cqm_rssi_thold, info->cqm_rssi_hyst);
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		ap_printk(KERN_DEBUG "[CQM] Beacon loss subscribe: %d\n",
+			info->cqm_beacon_miss_thold);
+		ap_printk(KERN_DEBUG "[CQM] TX failure subscribe: %d\n",
+			info->cqm_tx_fail_thold);
+		priv->cqm_rssi_thold = info->cqm_rssi_thold;
+		priv->cqm_rssi_hyst = info->cqm_rssi_hyst;
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+		if (info->cqm_rssi_thold || info->cqm_rssi_hyst) {
+			/* RSSI subscription enabled */
+			/* TODO: It's not a correct way of setting threshold.
+			 * Upper and lower must be set equal here and adjusted
+			 * in callback. However current implementation is much
+			 * more relaible and stable. */
+			threshold.upperThreshold =
+				info->cqm_rssi_thold + info->cqm_rssi_hyst;
+			threshold.lowerThreshold =
+				info->cqm_rssi_thold;
+			threshold.rssiRcpiMode |=
+				WSM_RCPI_RSSI_THRESHOLD_ENABLE;
+		} else {
+			/* There is a bug in FW, see sta.c. We have to enable
+			 * dummy subscription to get correct RSSI values. */
+			threshold.rssiRcpiMode |=
+				WSM_RCPI_RSSI_THRESHOLD_ENABLE |
+				WSM_RCPI_RSSI_DONT_USE_UPPER |
+				WSM_RCPI_RSSI_DONT_USE_LOWER;
+		}
+		WARN_ON(wsm_set_rcpi_rssi_threshold(priv, &threshold));
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		priv->cqm_tx_failure_thold = info->cqm_tx_fail_thold;
+		priv->cqm_tx_failure_count = 0;
+
+		if (priv->cqm_beacon_loss_count !=
+				info->cqm_beacon_miss_thold) {
+			priv->cqm_beacon_loss_count =
+				info->cqm_beacon_miss_thold;
+			priv->bss_params.beaconLostCount =
+				priv->cqm_beacon_loss_count ?
+				priv->cqm_beacon_loss_count :
+				priv->cqm_link_loss_count;
+			/* Make sure we are associated before sending
+			 * set_bss_params to firmware */
+			if (priv->bss_params.aid) {
+				WARN_ON(wsm_set_bss_params(priv,
+					&priv->bss_params));
+				priv->setbssparams_done = true;
+			}
+		}
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+	}
+	mutex_unlock(&priv->conf_mutex);
+}
+
+void cw1200_multicast_start_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, multicast_start_work);
+	long tmo = priv->join_dtim_period *
+			(priv->beacon_int + 20) * HZ / 1024;
+
+	cancel_work_sync(&priv->multicast_stop_work);
+
+	if (!priv->aid0_bit_set) {
+		wsm_lock_tx(priv);
+		cw1200_set_tim_impl(priv, true);
+		priv->aid0_bit_set = true;
+		mod_timer(&priv->mcast_timeout, jiffies + tmo);
+		wsm_unlock_tx(priv);
+	}
+}
+
+void cw1200_multicast_stop_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, multicast_stop_work);
+
+	if (priv->aid0_bit_set) {
+		del_timer_sync(&priv->mcast_timeout);
+		wsm_lock_tx(priv);
+		priv->aid0_bit_set = false;
+		cw1200_set_tim_impl(priv, false);
+		wsm_unlock_tx(priv);
+	}
+}
+
+void cw1200_mcast_timeout(unsigned long arg)
+{
+	struct cw1200_common *priv =
+		(struct cw1200_common *)arg;
+
+	wiphy_warn(priv->hw->wiphy,
+		"Multicast delivery timeout.\n");
+	spin_lock_bh(&priv->ps_state_lock);
+	priv->tx_multicast = priv->aid0_bit_set &&
+			priv->buffered_multicasts;
+	if (priv->tx_multicast)
+		cw1200_bh_wakeup(priv);
+	spin_unlock_bh(&priv->ps_state_lock);
+}
+
+int cw1200_ampdu_action(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			enum ieee80211_ampdu_mlme_action action,
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			u8 buf_size)
+{
+	/* Aggregation is implemented fully in firmware,
+	 * including block ack negotiation. Do not allow
+	 * mac80211 stack to do anything: it interferes with
+	 * the firmware. */
+	return -ENOTSUPP;
+}
+
+/* ******************************************************************** */
+/* WSM callback								*/
+void cw1200_suspend_resume(struct cw1200_common *priv,
+			  struct wsm_suspend_resume *arg)
+{
+	ap_printk(KERN_DEBUG "[AP] %s: %s\n",
+			arg->stop ? "stop" : "start",
+			arg->multicast ? "broadcast" : "unicast");
+
+	if (arg->multicast) {
+		bool cancel_tmo = false;
+		spin_lock_bh(&priv->ps_state_lock);
+		if (arg->stop) {
+			priv->tx_multicast = false;
+		} else {
+			/* Firmware sends this indication every DTIM if there
+			 * is a STA in powersave connected. There is no reason
+			 * to suspend, following wakeup will consume much more
+			 * power than it could be saved. */
+			cw1200_pm_stay_awake(&priv->pm_state,
+					priv->join_dtim_period *
+					(priv->beacon_int + 20) * HZ / 1024);
+			priv->tx_multicast = priv->aid0_bit_set &&
+					priv->buffered_multicasts;
+			if (priv->tx_multicast) {
+				cancel_tmo = true;
+				cw1200_bh_wakeup(priv);
+			}
+		}
+		spin_unlock_bh(&priv->ps_state_lock);
+		if (cancel_tmo)
+			del_timer_sync(&priv->mcast_timeout);
+	} else {
+		spin_lock_bh(&priv->ps_state_lock);
+		cw1200_ps_notify(priv, arg->link_id, arg->stop);
+		spin_unlock_bh(&priv->ps_state_lock);
+		if (!arg->stop)
+			cw1200_bh_wakeup(priv);
+	}
+	return;
+}
+
+/* ******************************************************************** */
+/* AP privates								*/
+
+static int cw1200_upload_beacon(struct cw1200_common *priv)
+{
+	int ret = 0;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_BEACON,
+	};
+	struct ieee80211_mgmt *mgmt;
+	u8 *erp_inf, *ies;
+	u32 ies_len;
+
+	if (priv->vif->p2p)
+		frame.rate = WSM_TRANSMIT_RATE_6;
+
+	frame.skb = ieee80211_beacon_get(priv->hw, priv->vif);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+	mgmt = (void *)frame.skb->data;
+	ies = mgmt->u.beacon.variable;
+	ies_len = frame.skb->len - (u32)(ies - (u8 *)mgmt);
+	erp_inf = (u8 *)cfg80211_find_ie(WLAN_EID_ERP_INFO, ies, ies_len);
+	if (erp_inf) {
+		if (erp_inf[ERP_INFO_BYTE_OFFSET]
+				& WLAN_ERP_BARKER_PREAMBLE)
+			priv->erp_info |= WLAN_ERP_BARKER_PREAMBLE;
+		else
+			priv->erp_info &= ~WLAN_ERP_BARKER_PREAMBLE;
+
+		if (erp_inf[ERP_INFO_BYTE_OFFSET]
+				& WLAN_ERP_NON_ERP_PRESENT) {
+			priv->erp_info |= WLAN_ERP_USE_PROTECTION;
+			priv->erp_info |= WLAN_ERP_NON_ERP_PRESENT;
+		} else {
+			priv->erp_info &= ~WLAN_ERP_USE_PROTECTION;
+			priv->erp_info &= ~WLAN_ERP_NON_ERP_PRESENT;
+		}
+	}
+
+	ret = wsm_set_template_frame(priv, &frame);
+	if (!ret) {
+		/* TODO: Distille probe resp; remove TIM
+		 * and other beacon-specific IEs */
+		*(__le16 *)frame.skb->data =
+			__cpu_to_le16(IEEE80211_FTYPE_MGMT |
+				      IEEE80211_STYPE_PROBE_RESP);
+		frame.frame_type = WSM_FRAME_TYPE_PROBE_RESPONSE;
+		if (priv->vif->p2p)
+			frame.disable = true;
+		ret = wsm_set_template_frame(priv, &frame);
+	}
+	dev_kfree_skb(frame.skb);
+
+	return ret;
+}
+
+static int cw1200_upload_pspoll(struct cw1200_common *priv)
+{
+	int ret = 0;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PS_POLL,
+		.rate = 0xFF,
+	};
+
+
+	frame.skb = ieee80211_pspoll_get(priv->hw, priv->vif);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+	ret = wsm_set_template_frame(priv, &frame);
+
+	dev_kfree_skb(frame.skb);
+
+	return ret;
+}
+
+static int cw1200_upload_null(struct cw1200_common *priv)
+{
+	int ret = 0;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_NULL,
+		.rate = 0xFF,
+	};
+
+
+	frame.skb = ieee80211_nullfunc_get(priv->hw, priv->vif);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+	ret = wsm_set_template_frame(priv, &frame);
+
+	dev_kfree_skb(frame.skb);
+
+	return ret;
+}
+
+static int cw1200_enable_beaconing(struct cw1200_common *priv,
+				   bool enable)
+{
+	struct wsm_beacon_transmit transmit = {
+		.enableBeaconing = enable,
+	};
+
+	return wsm_beacon_transmit(priv, &transmit);
+}
+
+static int cw1200_start_ap(struct cw1200_common *priv)
+{
+	int ret;
+	const u8 *ssidie;
+	struct sk_buff *skb;
+	int offset;
+	struct ieee80211_bss_conf *conf = &priv->vif->bss_conf;
+	struct wsm_start start = {
+		.mode = priv->vif->p2p ?
+				WSM_START_MODE_P2P_GO : WSM_START_MODE_AP,
+		.band = (priv->channel->band == IEEE80211_BAND_5GHZ) ?
+				WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G,
+		.channelNumber = priv->channel->hw_value,
+		.beaconInterval = conf->beacon_int,
+		.DTIMPeriod = conf->dtim_period,
+		.preambleType = conf->use_short_preamble ?
+				WSM_JOIN_PREAMBLE_SHORT :
+				WSM_JOIN_PREAMBLE_LONG,
+		.probeDelay = 100,
+		.basicRateSet = cw1200_rate_mask_to_wsm(priv,
+				conf->basic_rates),
+	};
+	struct wsm_operational_mode mode = {
+		.power_mode = wsm_power_mode_quiescent,
+		.disableMoreFlagUsage = true,
+	};
+
+	/* Get SSID */
+	skb = ieee80211_beacon_get(priv->hw, priv->vif);
+	if (WARN_ON(!skb))
+		return -ENOMEM;
+
+	offset = offsetof(struct ieee80211_mgmt, u.beacon.variable);
+	ssidie = cfg80211_find_ie(WLAN_EID_SSID, skb->data + offset,
+				  skb->len - offset);
+
+	memset(priv->ssid, 0, sizeof(priv->ssid));
+	if (ssidie) {
+		priv->ssid_length = ssidie[1];
+		if (WARN_ON(priv->ssid_length > sizeof(priv->ssid)))
+			priv->ssid_length = sizeof(priv->ssid);
+		memcpy(priv->ssid, &ssidie[2], priv->ssid_length);
+	} else {
+		priv->ssid_length = 0;
+	}
+	dev_kfree_skb(skb);
+
+	priv->beacon_int = conf->beacon_int;
+	priv->join_dtim_period = conf->dtim_period;
+
+	start.ssidLength = priv->ssid_length;
+	memcpy(&start.ssid[0], priv->ssid, start.ssidLength);
+
+	memset(&priv->link_id_db, 0, sizeof(priv->link_id_db));
+
+	ap_printk(KERN_DEBUG "[AP] ch: %d(%d), bcn: %d(%d), "
+		"brt: 0x%.8X, ssid: %.*s.\n",
+		start.channelNumber, start.band,
+		start.beaconInterval, start.DTIMPeriod,
+		start.basicRateSet,
+		start.ssidLength, start.ssid);
+	ret = WARN_ON(wsm_start(priv, &start));
+	if (!ret)
+		ret = WARN_ON(cw1200_upload_keys(priv));
+	if (!ret && priv->vif->p2p) {
+		ap_printk(KERN_DEBUG
+			"[AP] Setting p2p powersave "
+			"configuration.\n");
+		WARN_ON(wsm_set_p2p_ps_modeinfo(priv,
+			&priv->p2p_ps_modeinfo));
+	}
+	if (!ret) {
+		WARN_ON(wsm_set_block_ack_policy(priv,
+			0, 0));
+		priv->join_status = CW1200_JOIN_STATUS_AP;
+		cw1200_update_filtering(priv);
+	}
+	WARN_ON(wsm_set_operational_mode(priv, &mode));
+	return ret;
+}
+
+static int cw1200_update_beaconing(struct cw1200_common *priv)
+{
+	struct ieee80211_bss_conf *conf = &priv->vif->bss_conf;
+	struct wsm_reset reset = {
+		.link_id = 0,
+		.reset_statistics = true,
+	};
+
+	if (priv->mode == NL80211_IFTYPE_AP) {
+		/* TODO: check if changed channel, band */
+		if (priv->join_status != CW1200_JOIN_STATUS_AP ||
+		    priv->beacon_int != conf->beacon_int) {
+			ap_printk(KERN_DEBUG "ap restarting\n");
+			wsm_lock_tx(priv);
+			if (priv->join_status != CW1200_JOIN_STATUS_PASSIVE)
+				WARN_ON(wsm_reset(priv, &reset));
+			priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+			WARN_ON(cw1200_start_ap(priv));
+			wsm_unlock_tx(priv);
+		} else
+			ap_printk(KERN_DEBUG "ap started join_status: %d\n",
+				  priv->join_status);
+	}
+	return 0;
+}
+
+int cw1200_find_link_id(struct cw1200_common *priv, const u8 *mac)
+{
+	int i, ret = 0;
+	spin_lock_bh(&priv->ps_state_lock);
+	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
+		if (!memcmp(mac, priv->link_id_db[i].mac, ETH_ALEN) &&
+				priv->link_id_db[i].status) {
+			priv->link_id_db[i].timestamp = jiffies;
+			ret = i + 1;
+			break;
+		}
+	}
+	spin_unlock_bh(&priv->ps_state_lock);
+	return ret;
+}
+
+int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac)
+{
+	int i, ret = 0;
+	unsigned long max_inactivity = 0;
+	unsigned long now = jiffies;
+
+	spin_lock_bh(&priv->ps_state_lock);
+	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
+		if (!priv->link_id_db[i].status) {
+			ret = i + 1;
+			break;
+		} else if (priv->link_id_db[i].status != CW1200_LINK_HARD &&
+			!priv->tx_queue_stats.link_map_cache[i + 1]) {
+
+			unsigned long inactivity =
+					now - priv->link_id_db[i].timestamp;
+			if (inactivity < max_inactivity)
+				continue;
+			max_inactivity = inactivity;
+			ret = i + 1;
+		}
+	}
+	if (ret) {
+		struct cw1200_link_entry *entry = &priv->link_id_db[ret - 1];
+		ap_printk(KERN_DEBUG "[AP] STA added, link_id: %d\n",
+			ret);
+		entry->status = CW1200_LINK_RESERVE;
+		memcpy(&entry->mac, mac, ETH_ALEN);
+		memset(&entry->buffered, 0, CW1200_MAX_TID);
+		skb_queue_head_init(&entry->rx_queue);
+		wsm_lock_tx_async(priv);
+		if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
+			wsm_unlock_tx(priv);
+	} else {
+		wiphy_info(priv->hw->wiphy,
+			"[AP] Early: no more link IDs available.\n");
+	}
+
+	spin_unlock_bh(&priv->ps_state_lock);
+	return ret;
+}
+
+void cw1200_link_id_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, link_id_work);
+	wsm_flush_tx(priv);
+	cw1200_link_id_gc_work(&priv->link_id_gc_work.work);
+	wsm_unlock_tx(priv);
+}
+
+void cw1200_link_id_gc_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, link_id_gc_work.work);
+	struct wsm_reset reset = {
+		.reset_statistics = false,
+	};
+	struct wsm_map_link map_link = {
+		.link_id = 0,
+	};
+	unsigned long now = jiffies;
+	unsigned long next_gc = -1;
+	long ttl;
+	bool need_reset;
+	u32 mask;
+	int i;
+
+	if (priv->join_status != CW1200_JOIN_STATUS_AP)
+		return;
+
+	wsm_lock_tx(priv);
+	spin_lock_bh(&priv->ps_state_lock);
+	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
+		need_reset = false;
+		mask = BIT(i + 1);
+		if (priv->link_id_db[i].status == CW1200_LINK_RESERVE ||
+			(priv->link_id_db[i].status == CW1200_LINK_HARD &&
+			 !(priv->link_id_map & mask))) {
+			if (priv->link_id_map & mask) {
+				priv->sta_asleep_mask &= ~mask;
+				priv->pspoll_mask &= ~mask;
+				need_reset = true;
+			}
+			priv->link_id_map |= mask;
+			if (priv->link_id_db[i].status != CW1200_LINK_HARD)
+				priv->link_id_db[i].status = CW1200_LINK_SOFT;
+			memcpy(map_link.mac_addr, priv->link_id_db[i].mac,
+					ETH_ALEN);
+			spin_unlock_bh(&priv->ps_state_lock);
+			if (need_reset) {
+				reset.link_id = i + 1;
+				WARN_ON(wsm_reset(priv, &reset));
+			}
+			map_link.link_id = i + 1;
+			WARN_ON(wsm_map_link(priv, &map_link));
+			next_gc = min(next_gc, CW1200_LINK_ID_GC_TIMEOUT);
+			spin_lock_bh(&priv->ps_state_lock);
+		} else if (priv->link_id_db[i].status == CW1200_LINK_SOFT) {
+			ttl = priv->link_id_db[i].timestamp - now +
+					CW1200_LINK_ID_GC_TIMEOUT;
+			if (ttl <= 0) {
+				need_reset = true;
+				priv->link_id_db[i].status = CW1200_LINK_OFF;
+				priv->link_id_map &= ~mask;
+				priv->sta_asleep_mask &= ~mask;
+				priv->pspoll_mask &= ~mask;
+				memset(map_link.mac_addr, 0, ETH_ALEN);
+				spin_unlock_bh(&priv->ps_state_lock);
+				reset.link_id = i + 1;
+				WARN_ON(wsm_reset(priv, &reset));
+				spin_lock_bh(&priv->ps_state_lock);
+			} else {
+				next_gc = min_t(unsigned long, next_gc, ttl);
+			}
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		} else if (priv->link_id_db[i].status == CW1200_LINK_RESET ||
+				priv->link_id_db[i].status ==
+				CW1200_LINK_RESET_REMAP) {
+			int status = priv->link_id_db[i].status;
+			priv->link_id_db[i].status =
+					priv->link_id_db[i].prev_status;
+			priv->link_id_db[i].timestamp = now;
+			reset.link_id = i + 1;
+			spin_unlock_bh(&priv->ps_state_lock);
+			WARN_ON(wsm_reset(priv, &reset));
+			if (status == CW1200_LINK_RESET_REMAP) {
+				memcpy(map_link.mac_addr,
+						priv->link_id_db[i].mac,
+						ETH_ALEN);
+				map_link.link_id = i + 1;
+				WARN_ON(wsm_map_link(priv, &map_link));
+				next_gc = min(next_gc,
+						CW1200_LINK_ID_GC_TIMEOUT);
+			}
+			spin_lock_bh(&priv->ps_state_lock);
+#endif
+		}
+		if (need_reset) {
+			skb_queue_purge(&priv->link_id_db[i].rx_queue);
+			ap_printk(KERN_DEBUG "[AP] STA removed, link_id: %d\n",
+					reset.link_id);
+		}
+	}
+	spin_unlock_bh(&priv->ps_state_lock);
+	if (next_gc != -1)
+		queue_delayed_work(priv->workqueue,
+				&priv->link_id_gc_work, next_gc);
+	wsm_unlock_tx(priv);
+}
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+void cw1200_notify_noa(struct cw1200_common *priv, int delay)
+{
+	struct cfg80211_p2p_ps p2p_ps = {0};
+	struct wsm_p2p_ps_modeinfo *modeinfo;
+	modeinfo = &priv->p2p_ps_modeinfo;
+
+	ap_printk(KERN_DEBUG "[AP]: %s called\n", __func__);
+
+	if (priv->join_status != CW1200_JOIN_STATUS_AP)
+		return;
+
+	if (delay)
+		msleep(delay);
+
+	if (!WARN_ON(wsm_get_p2p_ps_modeinfo(priv, modeinfo))) {
+#if defined(CONFIG_CW1200_STA_DEBUG)
+		print_hex_dump_bytes("[AP] p2p_get_ps_modeinfo: ",
+				     DUMP_PREFIX_NONE,
+				     (u8 *)modeinfo,
+				     sizeof(*modeinfo));
+#endif /* CONFIG_CW1200_STA_DEBUG */
+		p2p_ps.opp_ps = !!(modeinfo->oppPsCTWindow & BIT(7));
+		p2p_ps.ctwindow = modeinfo->oppPsCTWindow & (~BIT(7));
+		p2p_ps.count = modeinfo->count;
+		p2p_ps.start = __le32_to_cpu(modeinfo->startTime);
+		p2p_ps.duration = __le32_to_cpu(modeinfo->duration);
+		p2p_ps.interval = __le32_to_cpu(modeinfo->interval);
+		p2p_ps.index = modeinfo->reserved;
+
+		ieee80211_p2p_noa_notify(priv->vif,
+					 &p2p_ps,
+					 GFP_KERNEL);
+	}
+}
+#endif
diff --git a/drivers/staging/cw1200/ap.h b/drivers/staging/cw1200/ap.h
new file mode 100644
index 00000000000..c10e4ef16d2
--- /dev/null
+++ b/drivers/staging/cw1200/ap.h
@@ -0,0 +1,49 @@
+/*
+ * mac80211 STA and AP API for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef AP_H_INCLUDED
+#define AP_H_INCLUDED
+
+int cw1200_set_tim(struct ieee80211_hw *dev, struct ieee80211_sta *sta,
+		   bool set);
+int cw1200_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		   struct ieee80211_sta *sta);
+int cw1200_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		      struct ieee80211_sta *sta);
+void cw1200_sta_notify(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
+		       enum sta_notify_cmd notify_cmd,
+		       struct ieee80211_sta *sta);
+void cw1200_bss_info_changed(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif,
+			     struct ieee80211_bss_conf *info,
+			     u32 changed);
+int cw1200_ampdu_action(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			enum ieee80211_ampdu_mlme_action action,
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			u8 buf_size);
+
+void cw1200_suspend_resume(struct cw1200_common *priv,
+			  struct wsm_suspend_resume *arg);
+void cw1200_set_tim_work(struct work_struct *work);
+void cw1200_set_cts_work(struct work_struct *work);
+void cw1200_multicast_start_work(struct work_struct *work);
+void cw1200_multicast_stop_work(struct work_struct *work);
+void cw1200_mcast_timeout(unsigned long arg);
+int cw1200_find_link_id(struct cw1200_common *priv, const u8 *mac);
+int cw1200_alloc_link_id(struct cw1200_common *priv, const u8 *mac);
+void cw1200_link_id_work(struct work_struct *work);
+void cw1200_link_id_gc_work(struct work_struct *work);
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+void cw1200_notify_noa(struct cw1200_common *priv, int delay);
+#endif
+
+#endif
diff --git a/drivers/staging/cw1200/bh.c b/drivers/staging/cw1200/bh.c
new file mode 100644
index 00000000000..427f3e2ba52
--- /dev/null
+++ b/drivers/staging/cw1200/bh.c
@@ -0,0 +1,622 @@
+/*
+ * Device handling thread implementation for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver, which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <net/mac80211.h>
+#include <linux/kthread.h>
+
+#include "cw1200.h"
+#include "bh.h"
+#include "hwio.h"
+#include "wsm.h"
+#include "sbus.h"
+#include "debug.h"
+
+#if defined(CONFIG_CW1200_BH_DEBUG)
+#define bh_printk(...) printk(__VA_ARGS__)
+#else
+#define bh_printk(...)
+#endif
+
+static int cw1200_bh(void *arg);
+
+/* TODO: Verify these numbers with WSM specification. */
+#define DOWNLOAD_BLOCK_SIZE_WR	(0x1000 - 4)
+/* an SPI message cannot be bigger than (2"12-1)*2 bytes
+ * "*2" to cvt to bytes */
+#define MAX_SZ_RD_WR_BUFFERS	(DOWNLOAD_BLOCK_SIZE_WR*2)
+#define PIGGYBACK_CTRL_REG	(2)
+#define EFFECTIVE_BUF_SIZE	(MAX_SZ_RD_WR_BUFFERS - PIGGYBACK_CTRL_REG)
+
+/* Suspend state privates */
+enum cw1200_bh_pm_state {
+	CW1200_BH_RESUMED = 0,
+	CW1200_BH_SUSPEND,
+	CW1200_BH_SUSPENDED,
+	CW1200_BH_RESUME,
+};
+
+typedef int (*cw1200_wsm_handler)(struct cw1200_common *priv,
+	u8 *data, size_t size);
+
+
+int cw1200_register_bh(struct cw1200_common *priv)
+{
+	int err = 0;
+	struct sched_param param = { .sched_priority = 1 };
+	bh_printk(KERN_DEBUG "[BH] register.\n");
+	BUG_ON(priv->bh_thread);
+	atomic_set(&priv->bh_rx, 0);
+	atomic_set(&priv->bh_tx, 0);
+	atomic_set(&priv->bh_term, 0);
+	atomic_set(&priv->bh_suspend, CW1200_BH_RESUMED);
+	priv->buf_id_tx = 0;
+	priv->buf_id_rx = 0;
+	init_waitqueue_head(&priv->bh_wq);
+	init_waitqueue_head(&priv->bh_evt_wq);
+	priv->bh_thread = kthread_create(&cw1200_bh, priv, "cw1200_bh");
+	if (IS_ERR(priv->bh_thread)) {
+		err = PTR_ERR(priv->bh_thread);
+		priv->bh_thread = NULL;
+	} else {
+		WARN_ON(sched_setscheduler(priv->bh_thread,
+			SCHED_FIFO, &param));
+#ifdef HAS_PUT_TASK_STRUCT
+		get_task_struct(priv->bh_thread);
+#endif
+		wake_up_process(priv->bh_thread);
+	}
+	return err;
+}
+
+void cw1200_unregister_bh(struct cw1200_common *priv)
+{
+	struct task_struct *thread = priv->bh_thread;
+	if (WARN_ON(!thread))
+		return;
+
+	priv->bh_thread = NULL;
+	bh_printk(KERN_DEBUG "[BH] unregister.\n");
+	atomic_add(1, &priv->bh_term);
+	wake_up(&priv->bh_wq);
+	kthread_stop(thread);
+#ifdef HAS_PUT_TASK_STRUCT
+	put_task_struct(thread);
+#endif
+}
+
+void cw1200_irq_handler(struct cw1200_common *priv)
+{
+	bh_printk(KERN_DEBUG "[BH] irq.\n");
+	if (/* WARN_ON */(priv->bh_error))
+		return;
+
+	if (atomic_add_return(1, &priv->bh_rx) == 1)
+		wake_up(&priv->bh_wq);
+}
+
+void cw1200_bh_wakeup(struct cw1200_common *priv)
+{
+	bh_printk(KERN_DEBUG "[BH] wakeup.\n");
+	if (WARN_ON(priv->bh_error))
+		return;
+
+	if (atomic_add_return(1, &priv->bh_tx) == 1)
+		wake_up(&priv->bh_wq);
+}
+
+int cw1200_bh_suspend(struct cw1200_common *priv)
+{
+	bh_printk(KERN_DEBUG "[BH] suspend.\n");
+	if (WARN_ON(priv->bh_error))
+		return 0;
+
+	atomic_set(&priv->bh_suspend, CW1200_BH_SUSPEND);
+	wake_up(&priv->bh_wq);
+	return wait_event_timeout(priv->bh_evt_wq, priv->bh_error ||
+		(CW1200_BH_SUSPENDED == atomic_read(&priv->bh_suspend)),
+		 1 * HZ) ? 0 : -ETIMEDOUT;
+}
+
+int cw1200_bh_resume(struct cw1200_common *priv)
+{
+	bh_printk(KERN_DEBUG "[BH] resume.\n");
+	if (WARN_ON(priv->bh_error))
+		return 0;
+
+	atomic_set(&priv->bh_suspend, CW1200_BH_RESUME);
+	wake_up(&priv->bh_wq);
+	return wait_event_timeout(priv->bh_evt_wq, priv->bh_error ||
+		(CW1200_BH_RESUMED == atomic_read(&priv->bh_suspend)),
+		1 * HZ) ? 0 : -ETIMEDOUT;
+}
+
+static inline void wsm_alloc_tx_buffer(struct cw1200_common *priv)
+{
+	++priv->hw_bufs_used;
+}
+
+int wsm_release_tx_buffer(struct cw1200_common *priv, int count)
+{
+	int ret = 0;
+	int hw_bufs_used = priv->hw_bufs_used;
+
+	priv->hw_bufs_used -= count;
+	if (WARN_ON(priv->hw_bufs_used < 0))
+		ret = -1;
+	else if (hw_bufs_used >= priv->wsm_caps.numInpChBufs)
+		ret = 1;
+	if (!priv->hw_bufs_used)
+		wake_up(&priv->bh_evt_wq);
+	return ret;
+}
+
+static struct sk_buff *cw1200_get_skb(struct cw1200_common *priv, size_t len)
+{
+	struct sk_buff *skb;
+	size_t alloc_len = (len > SDIO_BLOCK_SIZE) ? len : SDIO_BLOCK_SIZE;
+
+	if (len > SDIO_BLOCK_SIZE || !priv->skb_cache) {
+		skb = dev_alloc_skb(alloc_len
+				+ WSM_TX_EXTRA_HEADROOM
+				+ 8  /* TKIP IV */
+				+ 12 /* TKIP ICV + MIC */
+				- 2  /* Piggyback */);
+		/* In AP mode RXed SKB can be looped back as a broadcast.
+		 * Here we reserve enough space for headers. */
+		skb_reserve(skb, WSM_TX_EXTRA_HEADROOM
+				+ 8 /* TKIP IV */
+				- WSM_RX_EXTRA_HEADROOM);
+	} else {
+		skb = priv->skb_cache;
+		priv->skb_cache = NULL;
+	}
+	return skb;
+}
+
+static void cw1200_put_skb(struct cw1200_common *priv, struct sk_buff *skb)
+{
+	if (priv->skb_cache)
+		dev_kfree_skb(skb);
+	else
+		priv->skb_cache = skb;
+}
+
+static int cw1200_bh_read_ctrl_reg(struct cw1200_common *priv,
+					  u16 *ctrl_reg)
+{
+	int ret;
+
+	ret = cw1200_reg_read_16(priv,
+			ST90TDS_CONTROL_REG_ID, ctrl_reg);
+	if (ret) {
+		ret = cw1200_reg_read_16(priv,
+				ST90TDS_CONTROL_REG_ID, ctrl_reg);
+		if (ret)
+			printk(KERN_ERR
+				"[BH] Failed to read control register.\n");
+	}
+
+	return ret;
+}
+
+static int cw1200_device_wakeup(struct cw1200_common *priv)
+{
+	u16 ctrl_reg;
+	int ret;
+
+	bh_printk(KERN_DEBUG "[BH] Device wakeup.\n");
+
+	/* To force the device to be always-on, the host sets WLAN_UP to 1 */
+	ret = cw1200_reg_write_16(priv, ST90TDS_CONTROL_REG_ID,
+			ST90TDS_CONT_WUP_BIT);
+	if (WARN_ON(ret))
+		return ret;
+
+	ret = cw1200_bh_read_ctrl_reg(priv, &ctrl_reg);
+	if (WARN_ON(ret))
+		return ret;
+
+	/* If the device returns WLAN_RDY as 1, the device is active and will
+	 * remain active. */
+	if (ctrl_reg & ST90TDS_CONT_RDY_BIT) {
+		bh_printk(KERN_DEBUG "[BH] Device awake.\n");
+		return 1;
+	}
+
+	return 0;
+}
+
+/* Must be called from BH thraed. */
+void cw1200_enable_powersave(struct cw1200_common *priv,
+			     bool enable)
+{
+	bh_printk(KERN_DEBUG "[BH] Powerave is %s.\n",
+			enable ? "enabled" : "disabled");
+	priv->powersave_enabled = enable;
+}
+
+static int cw1200_bh(void *arg)
+{
+	struct cw1200_common *priv = arg;
+	struct sk_buff *skb_rx = NULL;
+	size_t read_len = 0;
+	int rx, tx, term, suspend;
+	struct wsm_hdr *wsm;
+	size_t wsm_len;
+	int wsm_id;
+	u8 wsm_seq;
+	int rx_resync = 1;
+	u16 ctrl_reg = 0;
+	int tx_allowed;
+	int pending_tx = 0;
+	int tx_burst;
+	int rx_burst = 0;
+	long status;
+#if defined(CONFIG_CW1200_WSM_DUMPS)
+	size_t wsm_dump_max = -1;
+#endif
+	u32 dummy;
+
+	for (;;) {
+		if (!priv->hw_bufs_used
+				&& priv->powersave_enabled
+				&& !priv->device_can_sleep)
+			status = 1 * HZ;
+		else if (priv->hw_bufs_used)
+			/* Interrupt loss detection */
+			status = 1 * HZ;
+		else
+			status = MAX_SCHEDULE_TIMEOUT;
+
+		/* Dummy Read for SDIO retry mechanism*/
+		if (((atomic_read(&priv->bh_rx) == 0) &&
+				(atomic_read(&priv->bh_tx) == 0)))
+			cw1200_reg_read(priv, ST90TDS_CONFIG_REG_ID,
+					&dummy, sizeof(dummy));
+#if defined(CONFIG_CW1200_WSM_DUMPS_SHORT)
+		wsm_dump_max = priv->wsm_dump_max_size;
+#endif /* CONFIG_CW1200_WSM_DUMPS_SHORT */
+
+		status = wait_event_interruptible_timeout(priv->bh_wq, ({
+				rx = atomic_xchg(&priv->bh_rx, 0);
+				tx = atomic_xchg(&priv->bh_tx, 0);
+				term = atomic_xchg(&priv->bh_term, 0);
+				suspend = pending_tx ?
+					0 : atomic_read(&priv->bh_suspend);
+				(rx || tx || term || suspend || priv->bh_error);
+			}), status);
+
+		if (status < 0 || term || priv->bh_error)
+			break;
+
+		if (!status && priv->hw_bufs_used) {
+			unsigned long timestamp = jiffies;
+			long timeout;
+			bool pending = false;
+			int i;
+
+			wiphy_warn(priv->hw->wiphy, "Missed interrupt?\n");
+			rx = 1;
+
+			/* Get a timestamp of "oldest" frame */
+			for (i = 0; i < 4; ++i)
+				pending |= cw1200_queue_get_xmit_timestamp(
+						&priv->tx_queue[i],
+						&timestamp);
+
+			/* Check if frame transmission is timed out.
+			 * Add an extra second with respect to possible
+			 * interrupt loss. */
+			timeout = timestamp +
+					WSM_CMD_LAST_CHANCE_TIMEOUT +
+					1 * HZ  -
+					jiffies;
+
+			/* And terminate BH tread if the frame is "stuck" */
+			if (pending && timeout < 0) {
+				wiphy_warn(priv->hw->wiphy,
+					"Timeout waiting for TX confirm.\n");
+				break;
+			}
+
+#if defined(CONFIG_CW1200_DUMP_ON_ERROR)
+			BUG_ON(1);
+#endif /* CONFIG_CW1200_DUMP_ON_ERROR */
+		} else if (!status) {
+			bh_printk(KERN_DEBUG "[BH] Device wakedown.\n");
+			WARN_ON(cw1200_reg_write_16(priv,
+					ST90TDS_CONTROL_REG_ID, 0));
+			priv->device_can_sleep = true;
+			continue;
+		} else if (suspend) {
+			bh_printk(KERN_DEBUG "[BH] Device suspend.\n");
+			if (priv->powersave_enabled) {
+				WARN_ON(cw1200_reg_write_16(priv,
+						ST90TDS_CONTROL_REG_ID, 0));
+				priv->device_can_sleep = true;
+			}
+
+			atomic_set(&priv->bh_suspend, CW1200_BH_SUSPENDED);
+			wake_up(&priv->bh_evt_wq);
+			status = wait_event_interruptible(priv->bh_wq,
+					CW1200_BH_RESUME == atomic_read(
+						&priv->bh_suspend));
+			if (status < 0) {
+				wiphy_err(priv->hw->wiphy,
+					"%s: Failed to wait for resume: %ld.\n",
+					__func__, status);
+				break;
+			}
+			bh_printk(KERN_DEBUG "[BH] Device resume.\n");
+			atomic_set(&priv->bh_suspend, CW1200_BH_RESUMED);
+			wake_up(&priv->bh_evt_wq);
+			atomic_add(1, &priv->bh_rx);
+			continue;
+		}
+
+		tx += pending_tx;
+		pending_tx = 0;
+
+		if (rx) {
+			size_t alloc_len;
+			u8 *data;
+
+			if (WARN_ON(cw1200_bh_read_ctrl_reg(
+					priv, &ctrl_reg)))
+				break;
+rx:
+			read_len = (ctrl_reg & ST90TDS_CONT_NEXT_LEN_MASK) * 2;
+			if (!read_len) {
+				rx_burst = 0;
+				goto tx;
+			}
+
+			if (WARN_ON((read_len < sizeof(struct wsm_hdr)) ||
+					(read_len > EFFECTIVE_BUF_SIZE))) {
+				printk(KERN_DEBUG "Invalid read len: %d",
+					read_len);
+				break;
+			}
+
+			/* Add SIZE of PIGGYBACK reg (CONTROL Reg)
+			 * to the NEXT Message length + 2 Bytes for SKB */
+			read_len = read_len + 2;
+
+#if defined(CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES)
+			alloc_len = priv->sbus_ops->align_size(
+					priv->sbus_priv, read_len);
+#else /* CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES */
+			/* Platform's SDIO workaround */
+			alloc_len = read_len & ~(SDIO_BLOCK_SIZE - 1);
+			if (read_len & (SDIO_BLOCK_SIZE - 1))
+				alloc_len += SDIO_BLOCK_SIZE;
+#endif /* CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES */
+
+			/* Check if not exceeding CW1200 capabilities */
+			if (WARN_ON_ONCE(alloc_len > EFFECTIVE_BUF_SIZE)) {
+				printk(KERN_DEBUG "Read aligned len: %d\n",
+					alloc_len);
+			}
+
+			skb_rx = cw1200_get_skb(priv, alloc_len);
+			if (WARN_ON(!skb_rx))
+				break;
+
+			skb_trim(skb_rx, 0);
+			skb_put(skb_rx, read_len);
+			data = skb_rx->data;
+			if (WARN_ON(!data))
+				break;
+
+			if (WARN_ON(cw1200_data_read(priv, data, alloc_len)))
+				break;
+
+			/* Piggyback */
+			ctrl_reg = __le16_to_cpu(
+				((__le16 *)data)[alloc_len / 2 - 1]);
+
+			wsm = (struct wsm_hdr *)data;
+			wsm_len = __le32_to_cpu(wsm->len);
+			if (WARN_ON(wsm_len > read_len))
+				break;
+
+#if defined(CONFIG_CW1200_WSM_DUMPS)
+			if (unlikely(priv->wsm_enable_wsm_dumps))
+				print_hex_dump_bytes("<-- ",
+					DUMP_PREFIX_NONE,
+					data, min(wsm_len, wsm_dump_max));
+#endif /* CONFIG_CW1200_WSM_DUMPS */
+
+			wsm_id  = __le32_to_cpu(wsm->id) & 0xFFF;
+			wsm_seq = (__le32_to_cpu(wsm->id) >> 13) & 7;
+
+			skb_trim(skb_rx, wsm_len);
+
+			if (unlikely(wsm_id == 0x0800)) {
+				wsm_handle_exception(priv,
+					 &data[sizeof(*wsm)],
+					wsm_len - sizeof(*wsm));
+				break;
+			} else if (unlikely(!rx_resync)) {
+				if (WARN_ON(wsm_seq != priv->wsm_rx_seq)) {
+#if defined(CONFIG_CW1200_DUMP_ON_ERROR)
+					BUG_ON(1);
+#endif /* CONFIG_CW1200_DUMP_ON_ERROR */
+					break;
+				}
+			}
+			priv->wsm_rx_seq = (wsm_seq + 1) & 7;
+			rx_resync = 0;
+
+			if (wsm_id & 0x0400) {
+				int rc = wsm_release_tx_buffer(priv, 1);
+				if (WARN_ON(rc < 0))
+					break;
+				else if (rc > 0)
+					tx = 1;
+			}
+
+			/* cw1200_wsm_rx takes care on SKB livetime */
+			if (WARN_ON(wsm_handle_rx(priv, wsm_id, wsm, &skb_rx)))
+				break;
+
+			if (skb_rx) {
+				cw1200_put_skb(priv, skb_rx);
+				skb_rx = NULL;
+			}
+
+			read_len = 0;
+
+			if (rx_burst) {
+				cw1200_debug_rx_burst(priv);
+				--rx_burst;
+				goto rx;
+			}
+		}
+
+tx:
+		BUG_ON(priv->hw_bufs_used > priv->wsm_caps.numInpChBufs);
+		tx_burst = priv->wsm_caps.numInpChBufs - priv->hw_bufs_used;
+		tx_allowed = tx_burst > 0;
+
+		if (tx && tx_allowed) {
+			size_t tx_len;
+			u8 *data;
+			int ret;
+
+			if (priv->device_can_sleep) {
+				ret = cw1200_device_wakeup(priv);
+				if (WARN_ON(ret < 0))
+					break;
+				else if (ret)
+					priv->device_can_sleep = false;
+				else {
+					/* Wait for "awake" interrupt */
+					pending_tx = tx;
+					continue;
+				}
+			}
+
+			wsm_alloc_tx_buffer(priv);
+			ret = wsm_get_tx(priv, &data, &tx_len, &tx_burst);
+			if (ret <= 0) {
+				wsm_release_tx_buffer(priv, 1);
+				if (WARN_ON(ret < 0))
+					break;
+			} else {
+				wsm = (struct wsm_hdr *)data;
+				BUG_ON(tx_len < sizeof(*wsm));
+				BUG_ON(__le32_to_cpu(wsm->len) != tx_len);
+
+#if 0 /* count is not implemented */
+				if (ret > 1)
+					atomic_add(1, &priv->bh_tx);
+#else
+				atomic_add(1, &priv->bh_tx);
+#endif
+
+
+#if defined(CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES)
+				tx_len = priv->sbus_ops->align_size(
+						priv->sbus_priv, tx_len);
+#else /* CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES */
+				/* HACK!!! Platform limitation.
+				* It is also supported by upper layer:
+				* there is always enough space at the
+				* end of the buffer. */
+				if (tx_len & (SDIO_BLOCK_SIZE - 1)) {
+					tx_len &= ~(SDIO_BLOCK_SIZE - 1);
+					tx_len += SDIO_BLOCK_SIZE;
+				}
+#endif /* CONFIG_CW1200_NON_POWER_OF_TWO_BLOCKSIZES */
+
+				/* Check if not exceeding CW1200
+				    capabilities */
+				if (WARN_ON_ONCE(
+				    tx_len > EFFECTIVE_BUF_SIZE)) {
+					printk(KERN_DEBUG "Write aligned len:"
+					" %d\n", tx_len);
+				}
+
+				wsm->id &= __cpu_to_le32(
+						~WSM_TX_SEQ(WSM_TX_SEQ_MAX));
+				wsm->id |= cpu_to_le32(
+						WSM_TX_SEQ(priv->wsm_tx_seq));
+
+				if (WARN_ON(cw1200_data_write(priv,
+				    data, tx_len))) {
+					wsm_release_tx_buffer(priv, 1);
+					break;
+				}
+
+#if defined(CONFIG_CW1200_WSM_DUMPS)
+				if (unlikely(priv->wsm_enable_wsm_dumps))
+					print_hex_dump_bytes("--> ",
+						DUMP_PREFIX_NONE,
+						data,
+						min(__le32_to_cpu(wsm->len),
+						 wsm_dump_max));
+#endif /* CONFIG_CW1200_WSM_DUMPS */
+
+				wsm_txed(priv, data);
+				priv->wsm_tx_seq = (priv->wsm_tx_seq + 1) &
+						WSM_TX_SEQ_MAX;
+
+				if (tx_burst > 1) {
+					cw1200_debug_tx_burst(priv);
+					++rx_burst;
+					goto tx;
+				}
+			}
+		}
+
+		if (ctrl_reg & ST90TDS_CONT_NEXT_LEN_MASK)
+			goto rx;
+	}
+
+	if (skb_rx) {
+		cw1200_put_skb(priv, skb_rx);
+		skb_rx = NULL;
+	}
+
+
+	if (!term) {
+		cw1200_dbg(CW1200_DBG_ERROR, "[BH] Fatal error, exitting.\n");
+#if defined(CONFIG_CW1200_DUMP_ON_ERROR)
+		BUG_ON(1);
+#endif /* CONFIG_CW1200_DUMP_ON_ERROR */
+		priv->bh_error = 1;
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		ieee80211_driver_hang_notify(priv->vif, GFP_KERNEL);
+		cw1200_pm_stay_awake(&priv->pm_state, 3*HZ);
+#endif
+		/* TODO: schedule_work(recovery) */
+#ifndef HAS_PUT_TASK_STRUCT
+		/* The only reason of having this stupid code here is
+		 * that __put_task_struct is not exported by kernel. */
+		for (;;) {
+			int status = wait_event_interruptible(priv->bh_wq, ({
+				term = atomic_xchg(&priv->bh_term, 0);
+				(term);
+				}));
+
+			if (status || term)
+				break;
+		}
+#endif
+	}
+	return 0;
+}
diff --git a/drivers/staging/cw1200/bh.h b/drivers/staging/cw1200/bh.h
new file mode 100644
index 00000000000..ea4598afc43
--- /dev/null
+++ b/drivers/staging/cw1200/bh.h
@@ -0,0 +1,30 @@
+/*
+ * Device handling thread interface for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_BH_H
+#define CW1200_BH_H
+
+/* extern */ struct cw1200_common;
+
+#define SDIO_BLOCK_SIZE (512)
+
+int cw1200_register_bh(struct cw1200_common *priv);
+void cw1200_unregister_bh(struct cw1200_common *priv);
+void cw1200_irq_handler(struct cw1200_common *priv);
+void cw1200_bh_wakeup(struct cw1200_common *priv);
+int cw1200_bh_suspend(struct cw1200_common *priv);
+int cw1200_bh_resume(struct cw1200_common *priv);
+/* Must be called from BH thread. */
+void cw1200_enable_powersave(struct cw1200_common *priv,
+			     bool enable);
+int wsm_release_tx_buffer(struct cw1200_common *priv, int count);
+
+#endif /* CW1200_BH_H */
diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
new file mode 100644
index 00000000000..5869c917fe6
--- /dev/null
+++ b/drivers/staging/cw1200/cw1200.h
@@ -0,0 +1,313 @@
+/*
+ * Common private data for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on the mac80211 Prism54 code, which is
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ *
+ * Based on the islsm (softmac prism54) driver, which is:
+ * Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ *
+ * 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 CW1200_H
+#define CW1200_H
+
+#include <linux/wait.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/atomic.h>
+#include <net/mac80211.h>
+
+#include "queue.h"
+#include "wsm.h"
+#include "scan.h"
+#include "txrx.h"
+#include "ht.h"
+#include "pm.h"
+
+/* extern */ struct sbus_ops;
+/* extern */ struct task_struct;
+/* extern */ struct cw1200_debug_priv;
+/* extern */ struct firmware;
+
+#if defined(CONFIG_CW1200_TXRX_DEBUG)
+#define txrx_printk(...) printk(__VA_ARGS__)
+#else
+#define txrx_printk(...)
+#endif
+
+#define CW1200_MAX_CTRL_FRAME_LEN	(0x1000)
+
+#define CW1200_MAX_STA_IN_AP_MODE	(5)
+#define CW1200_LINK_ID_AFTER_DTIM	(CW1200_MAX_STA_IN_AP_MODE + 1)
+#define CW1200_LINK_ID_UAPSD		(CW1200_MAX_STA_IN_AP_MODE + 2)
+#define CW1200_LINK_ID_MAX		(CW1200_MAX_STA_IN_AP_MODE + 3)
+#define CW1200_MAX_REQUEUE_ATTEMPTS	(5)
+
+#define CW1200_MAX_TID			(8)
+
+#define CW1200_BLOCK_ACK_CNT		(30)
+#define CW1200_BLOCK_ACK_THLD		(800)
+#define CW1200_BLOCK_ACK_HIST		(3)
+#define CW1200_BLOCK_ACK_INTERVAL	(1 * HZ / CW1200_BLOCK_ACK_HIST)
+
+/* Please keep order */
+enum cw1200_join_status {
+	CW1200_JOIN_STATUS_PASSIVE = 0,
+	CW1200_JOIN_STATUS_MONITOR,
+	CW1200_JOIN_STATUS_STA,
+	CW1200_JOIN_STATUS_AP,
+};
+
+enum cw1200_link_status {
+	CW1200_LINK_OFF,
+	CW1200_LINK_RESERVE,
+	CW1200_LINK_SOFT,
+	CW1200_LINK_HARD,
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	CW1200_LINK_RESET,
+	CW1200_LINK_RESET_REMAP,
+#endif
+};
+
+enum cw1200_bss_loss_status {
+	CW1200_BSS_LOSS_NONE,
+	CW1200_BSS_LOSS_CHECKING,
+	CW1200_BSS_LOSS_CONFIRMING,
+	CW1200_BSS_LOSS_CONFIRMED,
+};
+
+struct cw1200_link_entry {
+	unsigned long			timestamp;
+	enum cw1200_link_status		status;
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	enum cw1200_link_status		prev_status;
+#endif
+	u8				mac[ETH_ALEN];
+	u8				buffered[CW1200_MAX_TID];
+	struct sk_buff_head		rx_queue;
+};
+
+struct cw1200_common {
+	struct cw1200_queue		tx_queue[4];
+	struct cw1200_queue_stats	tx_queue_stats;
+	int				tx_burst_idx;
+	struct cw1200_debug_priv	*debug;
+
+	struct ieee80211_hw		*hw;
+	struct ieee80211_vif		*vif;
+	struct device			*pdev;
+	struct workqueue_struct		*workqueue;
+
+	struct mutex			conf_mutex;
+
+	const struct sbus_ops		*sbus_ops;
+	struct sbus_priv		*sbus_priv;
+
+	/* HW type (HIF_...) */
+	int				hw_type;
+	int				hw_revision;
+
+	/* firmware/hardware info */
+	unsigned int tx_hdr_len;
+
+	/* Radio data */
+	int output_power;
+	int noise;
+
+	/* calibration, output power limit and rssi<->dBm conversation data */
+
+	/* BBP/MAC state */
+	const struct firmware		*sdd;
+	struct ieee80211_rate		*rates;
+	struct ieee80211_rate		*mcs_rates;
+	u8 mac_addr[ETH_ALEN];
+	struct ieee80211_channel	*channel;
+	u8 bssid[ETH_ALEN];
+	struct wsm_edca_params		edca;
+	struct wsm_tx_queue_params	tx_queue_params;
+	struct wsm_association_mode	association_mode;
+	struct wsm_set_bss_params	bss_params;
+	struct cw1200_ht_info		ht_info;
+	struct wsm_set_pm		powersave_mode;
+	struct wsm_set_pm		firmware_ps_mode;
+	int				cqm_rssi_thold;
+	unsigned			cqm_rssi_hyst;
+	unsigned			cqm_tx_failure_thold;
+	unsigned			cqm_tx_failure_count;
+	bool				cqm_use_rssi;
+	int				cqm_link_loss_count;
+	int				cqm_beacon_loss_count;
+	int				channel_switch_in_progress;
+	wait_queue_head_t		channel_switch_done;
+	u8				long_frame_max_tx_count;
+	u8				short_frame_max_tx_count;
+	int				mode;
+	bool				enable_beacon;
+	int				beacon_int;
+	size_t				ssid_length;
+	u8				ssid[IEEE80211_MAX_SSID_LEN];
+	bool				listening;
+	struct wsm_rx_filter		rx_filter;
+	struct wsm_beacon_filter_table	bf_table;
+	struct wsm_beacon_filter_control bf_control;
+	struct wsm_multicast_filter	multicast_filter;
+	bool				has_multicast_subscription;
+	bool				disable_beacon_filter;
+	struct work_struct		update_filtering_work;
+	u8				ba_tid_mask;
+	int				ba_acc;
+	int				ba_cnt;
+	int				ba_hist;
+	struct timer_list		ba_timer;
+	spinlock_t			ba_lock;
+	bool				ba_ena;
+	struct work_struct              ba_work;
+	struct cw1200_pm_state		pm_state;
+	struct wsm_p2p_ps_modeinfo	p2p_ps_modeinfo;
+	struct wsm_uapsd_info		uapsd_info;
+	bool				setbssparams_done;
+	bool				is_BT_Present;
+	u8				conf_listen_interval;
+	u32				listen_interval;
+	u32				erp_info;
+
+	/* BH */
+	atomic_t			bh_rx;
+	atomic_t			bh_tx;
+	atomic_t			bh_term;
+	atomic_t			bh_suspend;
+	struct task_struct		*bh_thread;
+	int				bh_error;
+	wait_queue_head_t		bh_wq;
+	wait_queue_head_t		bh_evt_wq;
+	int				buf_id_tx;	/* byte */
+	int				buf_id_rx;	/* byte */
+	int				wsm_rx_seq;	/* byte */
+	int				wsm_tx_seq;	/* byte */
+	int				hw_bufs_used;
+	struct sk_buff			*skb_cache;
+	bool				powersave_enabled;
+	bool				device_can_sleep;
+
+	/* WSM */
+	struct wsm_caps			wsm_caps;
+	struct mutex			wsm_cmd_mux;
+	struct wsm_buf			wsm_cmd_buf;
+	struct wsm_cmd			wsm_cmd;
+	wait_queue_head_t		wsm_cmd_wq;
+	wait_queue_head_t		wsm_startup_done;
+	struct wsm_cbc			wsm_cbc;
+	atomic_t			tx_lock;
+
+	/* WSM debug */
+	int				wsm_enable_wsm_dumps;
+	u32				wsm_dump_max_size;
+
+	/* Scan status */
+	struct cw1200_scan scan;
+
+	/* WSM Join */
+	enum cw1200_join_status	join_status;
+	u8			join_bssid[ETH_ALEN];
+	u32			pending_frame_id;
+	struct work_struct	join_work;
+	struct delayed_work	join_timeout;
+	struct work_struct	unjoin_work;
+	struct work_struct	offchannel_work;
+	int			join_dtim_period;
+	bool			delayed_unjoin;
+
+	/* TX/RX and security */
+	s8			wep_default_key_id;
+	struct work_struct	wep_key_work;
+	u32			key_map;
+	struct wsm_add_key	keys[WSM_KEY_MAX_INDEX + 1];
+	unsigned long		rx_timestamp;
+
+	/* AP powersave */
+	u32			link_id_map;
+	struct cw1200_link_entry link_id_db[CW1200_MAX_STA_IN_AP_MODE];
+	struct work_struct	link_id_work;
+	struct delayed_work	link_id_gc_work;
+	u32			sta_asleep_mask;
+	u32			pspoll_mask;
+	bool			aid0_bit_set;
+	spinlock_t		ps_state_lock;
+	bool			buffered_multicasts;
+	bool			tx_multicast;
+	struct work_struct	set_tim_work;
+	struct delayed_work	set_cts_work;
+	struct work_struct	multicast_start_work;
+	struct work_struct	multicast_stop_work;
+	struct timer_list	mcast_timeout;
+
+
+	/* WSM events and CQM implementation */
+	spinlock_t		event_queue_lock;
+	struct list_head	event_queue;
+	struct work_struct	event_handler;
+	struct delayed_work	bss_loss_work;
+	struct delayed_work	connection_loss_work;
+	struct work_struct	tx_failure_work;
+	int			delayed_link_loss;
+	spinlock_t		bss_loss_lock;
+	int			bss_loss_status;
+	int			bss_loss_confirm_id;
+
+	/* TX rate policy cache */
+	struct tx_policy_cache tx_policy_cache;
+	struct work_struct tx_policy_upload_work;
+
+	/* cryptographic engine information */
+
+	/* bit field of glowing LEDs */
+	u16 softled_state;
+
+	/* statistics */
+	struct ieee80211_low_level_stats stats;
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	/* Workaround for WFD testcase 6.1.10*/
+	struct work_struct	linkid_reset_work;
+	u8			action_frame_sa[ETH_ALEN];
+	u8			action_linkid;
+#endif
+};
+
+struct cw1200_sta_priv {
+	int link_id;
+};
+
+/* interfaces for the drivers */
+int cw1200_core_probe(const struct sbus_ops *sbus_ops,
+		      struct sbus_priv *sbus,
+		      struct device *pdev,
+		      struct cw1200_common **pself);
+void cw1200_core_release(struct cw1200_common *self);
+
+#define CW1200_DBG_MSG		0x00000001
+#define CW1200_DBG_NIY		0x00000002
+#define CW1200_DBG_SBUS		0x00000004
+#define CW1200_DBG_INIT		0x00000008
+#define CW1200_DBG_ERROR	0x00000010
+#define CW1200_DBG_LEVEL	0xFFFFFFFF
+
+#define cw1200_dbg(level, ...)				\
+	do {						\
+		if ((level) & CW1200_DBG_LEVEL)		\
+			printk(KERN_DEBUG __VA_ARGS__);	\
+	} while (0)
+
+#define STUB()						\
+	do {						\
+		cw1200_dbg(CW1200_DBG_NIY, "%s: STUB at line %d.\n", \
+		__func__, __LINE__);			\
+	} while (0)
+
+#endif /* CW1200_H */
diff --git a/drivers/staging/cw1200/cw1200_plat.h b/drivers/staging/cw1200/cw1200_plat.h
new file mode 100644
index 00000000000..3a73183c9f8
--- /dev/null
+++ b/drivers/staging/cw1200/cw1200_plat.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#ifndef CW1200_PLAT_H_INCLUDED
+#define CW1200_PLAT_H_INCLUDED
+
+#include <linux/ioport.h>
+
+struct cw1200_platform_data {
+	const char *mmc_id;
+	const struct resource *irq;
+	const struct resource *reset;
+	int (*power_ctrl)(const struct cw1200_platform_data *pdata,
+			  bool enable);
+	int (*clk_ctrl)(const struct cw1200_platform_data *pdata,
+			  bool enable);
+	int (*prcmu_ctrl)(const struct cw1200_platform_data *pdata,
+			  bool enable);
+};
+
+/* Declaration only. Should be implemented in arch/xxx/mach-yyy */
+const struct cw1200_platform_data *cw1200_get_platform_data(void);
+
+#endif /* CW1200_PLAT_H_INCLUDED */
diff --git a/drivers/staging/cw1200/cw1200_sdio.c b/drivers/staging/cw1200/cw1200_sdio.c
new file mode 100644
index 00000000000..4b9d622689f
--- /dev/null
+++ b/drivers/staging/cw1200/cw1200_sdio.c
@@ -0,0 +1,469 @@
+/*
+ * Mac80211 SDIO driver for ST-Ericsson CW1200 device
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio.h>
+#include <linux/spinlock.h>
+#include <asm/mach-types.h>
+#include <net/mac80211.h>
+
+#include "cw1200.h"
+#include "sbus.h"
+#include "cw1200_plat.h"
+
+MODULE_AUTHOR("Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>");
+MODULE_DESCRIPTION("mac80211 ST-Ericsson CW1200 SDIO driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("cw1200_wlan");
+
+struct sbus_priv {
+	struct sdio_func	*func;
+	struct cw1200_common	*core;
+	const struct cw1200_platform_data *pdata;
+	spinlock_t		lock;
+	sbus_irq_handler	irq_handler;
+	void			*irq_priv;
+};
+
+static const struct sdio_device_id cw1200_sdio_ids[] = {
+	{ SDIO_DEVICE(SDIO_ANY_ID, SDIO_ANY_ID) },
+	{ /* end: all zeroes */			},
+};
+
+/* sbus_ops implemetation */
+
+static int cw1200_sdio_memcpy_fromio(struct sbus_priv *self,
+				     unsigned int addr,
+				     void *dst, int count)
+{
+	return sdio_memcpy_fromio(self->func, dst, addr, count);
+}
+
+static int cw1200_sdio_memcpy_toio(struct sbus_priv *self,
+				   unsigned int addr,
+				   const void *src, int count)
+{
+	return sdio_memcpy_toio(self->func, addr, (void *)src, count);
+}
+
+static void cw1200_sdio_lock(struct sbus_priv *self)
+{
+	sdio_claim_host(self->func);
+}
+
+static void cw1200_sdio_unlock(struct sbus_priv *self)
+{
+	sdio_release_host(self->func);
+}
+
+#ifndef CONFIG_CW1200_USE_GPIO_IRQ
+static void cw1200_sdio_irq_handler(struct sdio_func *func)
+{
+	struct sbus_priv *self = sdio_get_drvdata(func);
+	unsigned long flags;
+
+	BUG_ON(!self);
+	spin_lock_irqsave(&self->lock, flags);
+	if (self->irq_handler)
+		self->irq_handler(self->irq_priv);
+	spin_unlock_irqrestore(&self->lock, flags);
+}
+#else /* CONFIG_CW1200_USE_GPIO_IRQ */
+static irqreturn_t cw1200_gpio_irq_handler(int irq, void *dev_id)
+{
+	struct sbus_priv *self = dev_id;
+
+	BUG_ON(!self);
+	if (self->irq_handler)
+		self->irq_handler(self->irq_priv);
+	return IRQ_HANDLED;
+}
+
+static int cw1200_request_irq(struct sbus_priv *self,
+			      irq_handler_t handler)
+{
+	int ret;
+	int func_num;
+	const struct resource *irq = self->pdata->irq;
+	u8 cccr;
+
+	ret = request_any_context_irq(irq->start, handler,
+			IRQF_TRIGGER_RISING, irq->name, self);
+	if (WARN_ON(ret < 0))
+		goto exit;
+
+	/* Hack to access Fuction-0 */
+	func_num = self->func->num;
+	self->func->num = 0;
+
+	cccr = sdio_readb(self->func, SDIO_CCCR_IENx, &ret);
+	if (WARN_ON(ret))
+		goto set_func;
+
+	/* Master interrupt enable ... */
+	cccr |= BIT(0);
+
+	/* ... for our function */
+	cccr |= BIT(func_num);
+
+	sdio_writeb(self->func, cccr, SDIO_CCCR_IENx, &ret);
+	if (WARN_ON(ret))
+		goto set_func;
+
+	/* Restore the WLAN function number */
+	self->func->num = func_num;
+	return 0;
+
+set_func:
+	self->func->num = func_num;
+	free_irq(irq->start, self);
+exit:
+	return ret;
+}
+#endif /* CONFIG_CW1200_USE_GPIO_IRQ */
+
+static int cw1200_sdio_irq_subscribe(struct sbus_priv *self,
+				     sbus_irq_handler handler,
+				     void *priv)
+{
+	int ret;
+	unsigned long flags;
+
+	if (!handler)
+		return -EINVAL;
+
+	spin_lock_irqsave(&self->lock, flags);
+	self->irq_priv = priv;
+	self->irq_handler = handler;
+	spin_unlock_irqrestore(&self->lock, flags);
+
+	printk(KERN_DEBUG "SW IRQ subscribe\n");
+	sdio_claim_host(self->func);
+#ifndef CONFIG_CW1200_USE_GPIO_IRQ
+	ret = sdio_claim_irq(self->func, cw1200_sdio_irq_handler);
+#else
+	ret = cw1200_request_irq(self, cw1200_gpio_irq_handler);
+#endif
+	sdio_release_host(self->func);
+	return ret;
+}
+
+static int cw1200_sdio_irq_unsubscribe(struct sbus_priv *self)
+{
+	int ret = 0;
+	unsigned long flags;
+#ifdef CONFIG_CW1200_USE_GPIO_IRQ
+	const struct resource *irq = self->pdata->irq;
+#endif
+
+	WARN_ON(!self->irq_handler);
+	if (!self->irq_handler)
+		return 0;
+
+	printk(KERN_DEBUG "SW IRQ unsubscribe\n");
+#ifndef CONFIG_CW1200_USE_GPIO_IRQ
+	sdio_claim_host(self->func);
+	ret = sdio_release_irq(self->func);
+	sdio_release_host(self->func);
+#else
+	free_irq(irq->start, self);
+#endif
+
+	spin_lock_irqsave(&self->lock, flags);
+	self->irq_priv = NULL;
+	self->irq_handler = NULL;
+	spin_unlock_irqrestore(&self->lock, flags);
+
+	return ret;
+}
+
+static int cw1200_detect_card(const struct cw1200_platform_data *pdata)
+{
+	/* HACK!!!
+	 * Rely on mmc->class_dev.class set in mmc_alloc_host
+	 * Tricky part: a new mmc hook is being (temporary) created
+	 * to discover mmc_host class.
+	 * Do you know more elegant way how to enumerate mmc_hosts?
+	 */
+
+	struct mmc_host *mmc = NULL;
+	struct class_dev_iter iter;
+	struct device *dev;
+
+	mmc = mmc_alloc_host(0, NULL);
+	if (!mmc)
+		return -ENOMEM;
+
+	BUG_ON(!mmc->class_dev.class);
+	class_dev_iter_init(&iter, mmc->class_dev.class, NULL, NULL);
+	for (;;) {
+		dev = class_dev_iter_next(&iter);
+		if (!dev) {
+			printk(KERN_ERR "cw1200: %s is not found.\n",
+				pdata->mmc_id);
+			break;
+		} else {
+			struct mmc_host *host = container_of(dev,
+				struct mmc_host, class_dev);
+
+			if (dev_name(&host->class_dev) &&
+				strcmp(dev_name(&host->class_dev),
+					pdata->mmc_id))
+				continue;
+
+			mmc_detect_change(host, 10);
+			break;
+		}
+	}
+	mmc_free_host(mmc);
+	return 0;
+}
+
+static int cw1200_sdio_off(const struct cw1200_platform_data *pdata)
+{
+	int ret = 0;
+#ifndef CONFIG_CW1200_U5500_SUPPORT
+	const struct resource *reset = pdata->reset;
+	gpio_set_value(reset->start, 0);
+	gpio_free(reset->start);
+#else
+	if (pdata->prcmu_ctrl)
+		ret = pdata->prcmu_ctrl(pdata, false);
+	msleep(50);
+#endif
+	cw1200_detect_card(pdata);
+	return ret;
+}
+
+static int cw1200_sdio_on(const struct cw1200_platform_data *pdata)
+{
+	int ret = 0;
+#ifndef CONFIG_CW1200_U5500_SUPPORT
+	const struct resource *reset = pdata->reset;
+	gpio_request(reset->start, reset->name);
+	gpio_direction_output(reset->start, 1);
+	/* It is not stated in the datasheet, but at least some of devices
+	 * have problems with reset if this stage is omited. */
+	msleep(50);
+	gpio_direction_output(reset->start, 0);
+	/* A valid reset shall be obtained by maintaining WRESETN
+	 * active (low) for at least two cycles of LP_CLK after VDDIO
+	 * is stable within it operating range. */
+	usleep_range(1000, 20000);
+	gpio_set_value(reset->start, 1);
+	/* The host should wait 32 ms after the WRESETN release
+	 * for the on-chip LDO to stabilize */
+	msleep(32);
+#else
+	if (pdata->prcmu_ctrl)
+		ret = pdata->prcmu_ctrl(pdata, true);
+	msleep(50);
+#endif
+	cw1200_detect_card(pdata);
+	return ret;
+}
+
+static int cw1200_sdio_reset(struct sbus_priv *self)
+{
+	cw1200_sdio_off(self->pdata);
+	msleep(1000);
+	cw1200_sdio_on(self->pdata);
+	return 0;
+}
+
+static size_t cw1200_sdio_align_size(struct sbus_priv *self, size_t size)
+{
+	size_t aligned = sdio_align_size(self->func, size);
+	return aligned;
+}
+
+int cw1200_sdio_set_block_size(struct sbus_priv *self, size_t size)
+{
+	return sdio_set_block_size(self->func, size);
+}
+
+static int cw1200_sdio_pm(struct sbus_priv *self, bool  suspend)
+{
+	int ret = 0;
+	const struct resource *irq = self->pdata->irq;
+
+	if (irq)
+		ret = irq_set_irq_wake(irq->start, suspend);
+
+	return ret;
+}
+
+static struct sbus_ops cw1200_sdio_sbus_ops = {
+	.sbus_memcpy_fromio	= cw1200_sdio_memcpy_fromio,
+	.sbus_memcpy_toio	= cw1200_sdio_memcpy_toio,
+	.lock			= cw1200_sdio_lock,
+	.unlock			= cw1200_sdio_unlock,
+	.irq_subscribe		= cw1200_sdio_irq_subscribe,
+	.irq_unsubscribe	= cw1200_sdio_irq_unsubscribe,
+	.reset			= cw1200_sdio_reset,
+	.align_size		= cw1200_sdio_align_size,
+	.power_mgmt		= cw1200_sdio_pm,
+	.set_block_size		= cw1200_sdio_set_block_size,
+};
+
+/* Probe Function to be called by SDIO stack when device is discovered */
+static int cw1200_sdio_probe(struct sdio_func *func,
+			      const struct sdio_device_id *id)
+{
+	struct sbus_priv *self;
+	int status;
+
+	cw1200_dbg(CW1200_DBG_INIT, "Probe called\n");
+
+	self = kzalloc(sizeof(*self), GFP_KERNEL);
+	if (!self) {
+		cw1200_dbg(CW1200_DBG_ERROR, "Can't allocate SDIO sbus_priv.");
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&self->lock);
+	self->pdata = cw1200_get_platform_data();
+	self->func = func;
+	sdio_set_drvdata(func, self);
+	sdio_claim_host(func);
+	sdio_enable_func(func);
+	sdio_release_host(func);
+
+	status = cw1200_core_probe(&cw1200_sdio_sbus_ops,
+			      self, &func->dev, &self->core);
+	if (status) {
+		sdio_claim_host(func);
+		sdio_disable_func(func);
+		sdio_release_host(func);
+		sdio_set_drvdata(func, NULL);
+		kfree(self);
+	}
+
+	return status;
+}
+
+/* Disconnect Function to be called by SDIO stack when
+ * device is disconnected */
+static void cw1200_sdio_disconnect(struct sdio_func *func)
+{
+	struct sbus_priv *self = sdio_get_drvdata(func);
+
+	if (self) {
+		if (self->core) {
+			cw1200_core_release(self->core);
+			self->core = NULL;
+		}
+		sdio_claim_host(func);
+		sdio_disable_func(func);
+		sdio_release_host(func);
+		sdio_set_drvdata(func, NULL);
+		kfree(self);
+	}
+}
+
+static int cw1200_suspend(struct device *dev)
+{
+	int ret;
+	struct sdio_func *func = dev_to_sdio_func(dev);
+
+	/* Notify SDIO that CW1200 will remain powered during suspend */
+	ret = sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER);
+	if (ret)
+		cw1200_dbg(CW1200_DBG_ERROR,
+			   "Error setting SDIO pm flags: %i\n", ret);
+
+	return ret;
+}
+
+static int cw1200_resume(struct device *dev)
+{
+	return 0;
+}
+
+static const struct dev_pm_ops cw1200_pm_ops = {
+	.suspend = cw1200_suspend,
+	.resume = cw1200_resume,
+};
+
+static struct sdio_driver sdio_driver = {
+	.name		= "cw1200_wlan",
+	.id_table	= cw1200_sdio_ids,
+	.probe		= cw1200_sdio_probe,
+	.remove		= cw1200_sdio_disconnect,
+	.drv = {
+		.pm = &cw1200_pm_ops,
+	}
+};
+
+/* Init Module function -> Called by insmod */
+static int __init cw1200_sdio_init(void)
+{
+	const struct cw1200_platform_data *pdata;
+	int ret;
+
+	pdata = cw1200_get_platform_data();
+
+	ret = sdio_register_driver(&sdio_driver);
+	if (ret)
+		goto err_reg;
+
+	if (pdata->clk_ctrl) {
+		ret = pdata->clk_ctrl(pdata, true);
+		if (ret)
+			goto err_clk;
+	}
+
+	if (pdata->power_ctrl) {
+		ret = pdata->power_ctrl(pdata, true);
+		if (ret)
+			goto err_power;
+	}
+
+	ret = cw1200_sdio_on(pdata);
+	if (ret)
+		goto err_on;
+
+	return 0;
+
+err_on:
+	if (pdata->power_ctrl)
+		pdata->power_ctrl(pdata, false);
+err_power:
+	if (pdata->clk_ctrl)
+		pdata->clk_ctrl(pdata, false);
+err_clk:
+	sdio_unregister_driver(&sdio_driver);
+err_reg:
+	return ret;
+}
+
+/* Called at Driver Unloading */
+static void __exit cw1200_sdio_exit(void)
+{
+	const struct cw1200_platform_data *pdata;
+	pdata = cw1200_get_platform_data();
+	sdio_unregister_driver(&sdio_driver);
+	cw1200_sdio_off(pdata);
+	if (pdata->power_ctrl)
+		pdata->power_ctrl(pdata, false);
+	if (pdata->clk_ctrl)
+		pdata->clk_ctrl(pdata, false);
+}
+
+
+module_init(cw1200_sdio_init);
+module_exit(cw1200_sdio_exit);
diff --git a/drivers/staging/cw1200/debug.c b/drivers/staging/cw1200/debug.c
new file mode 100644
index 00000000000..3da603ac87d
--- /dev/null
+++ b/drivers/staging/cw1200/debug.c
@@ -0,0 +1,611 @@
+/*
+ * mac80211 glue code for mac80211 ST-Ericsson CW1200 drivers
+ * DebugFS code
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include "cw1200.h"
+#include "debug.h"
+
+/* join_status */
+static const char * const cw1200_debug_join_status[] = {
+	"passive",
+	"monitor",
+	"station",
+	"access point",
+};
+
+/* WSM_JOIN_PREAMBLE_... */
+static const char * const cw1200_debug_preamble[] = {
+	"long",
+	"short",
+	"long on 1 and 2 Mbps",
+};
+
+static const char * const cw1200_debug_fw_types[] = {
+	"ETF",
+	"WFM",
+	"WSM",
+	"HI test",
+	"Platform test",
+};
+
+static const char * const cw1200_debug_link_id[] = {
+	"OFF",
+	"REQ",
+	"SOFT",
+	"HARD",
+};
+
+static const char *cw1200_debug_mode(int mode)
+{
+	switch (mode) {
+	case NL80211_IFTYPE_UNSPECIFIED:
+		return "unspecified";
+	case NL80211_IFTYPE_MONITOR:
+		return "monitor";
+	case NL80211_IFTYPE_STATION:
+		return "station";
+	case NL80211_IFTYPE_ADHOC:
+		return "ad-hok";
+	case NL80211_IFTYPE_MESH_POINT:
+		return "mesh point";
+	case NL80211_IFTYPE_AP:
+		return "access point";
+	case NL80211_IFTYPE_P2P_CLIENT:
+		return "p2p client";
+	case NL80211_IFTYPE_P2P_GO:
+		return "p2p go";
+	default:
+		return "unsupported";
+	}
+}
+
+static void cw1200_queue_status_show(struct seq_file *seq,
+				     struct cw1200_queue *q)
+{
+	int i;
+	seq_printf(seq, "Queue       %d:\n", q->queue_id);
+	seq_printf(seq, "  capacity: %d\n", q->capacity);
+	seq_printf(seq, "  queued:   %d\n", q->num_queued);
+	seq_printf(seq, "  pending:  %d\n", q->num_pending);
+	seq_printf(seq, "  sent:     %d\n", q->num_sent);
+	seq_printf(seq, "  locked:   %s\n", q->tx_locked_cnt ? "yes" : "no");
+	seq_printf(seq, "  overfull: %s\n", q->overfull ? "yes" : "no");
+	seq_puts(seq,   "  link map: 0-> ");
+	for (i = 0; i < q->stats->map_capacity; ++i)
+		seq_printf(seq, "%.2d ", q->link_map_cache[i]);
+	seq_printf(seq, "<-%d\n", q->stats->map_capacity);
+}
+
+static void cw1200_debug_print_map(struct seq_file *seq,
+				   struct cw1200_common *priv,
+				   const char *label,
+				   u32 map)
+{
+	int i;
+	seq_printf(seq, "%s0-> ", label);
+	for (i = 0; i < priv->tx_queue_stats.map_capacity; ++i)
+		seq_printf(seq, "%s ", (map & BIT(i)) ? "**" : "..");
+	seq_printf(seq, "<-%d\n", priv->tx_queue_stats.map_capacity - 1);
+}
+
+static int cw1200_status_show(struct seq_file *seq, void *v)
+{
+	int i;
+	struct list_head *item;
+	struct cw1200_common *priv = seq->private;
+	struct cw1200_debug_priv *d = priv->debug;
+	int ba_cnt, ba_acc, ba_avg = 0;
+	bool ba_ena;
+
+	spin_lock_bh(&priv->ba_lock);
+	ba_cnt = priv->debug->ba_cnt;
+	ba_acc = priv->debug->ba_acc;
+	ba_ena = priv->ba_ena;
+	if (ba_cnt)
+		ba_avg = ba_acc / ba_cnt;
+	spin_unlock_bh(&priv->ba_lock);
+
+	seq_puts(seq,   "CW1200 Wireless LAN driver status\n");
+	seq_printf(seq, "Hardware:   %d.%d\n",
+		priv->wsm_caps.hardwareId,
+		priv->wsm_caps.hardwareSubId);
+	seq_printf(seq, "Firmware:   %s %d.%d\n",
+		cw1200_debug_fw_types[priv->wsm_caps.firmwareType],
+		priv->wsm_caps.firmwareVersion,
+		priv->wsm_caps.firmwareBuildNumber);
+	seq_printf(seq, "FW API:     %d\n",
+		priv->wsm_caps.firmwareApiVer);
+	seq_printf(seq, "FW caps:    0x%.4X\n",
+		priv->wsm_caps.firmwareCap);
+	seq_printf(seq, "Mode:       %s%s\n",
+		cw1200_debug_mode(priv->mode),
+		priv->listening ? " (listening)" : "");
+	seq_printf(seq, "Assoc:      %s\n",
+		cw1200_debug_join_status[priv->join_status]);
+	if (priv->channel)
+		seq_printf(seq, "Channel:    %d%s\n",
+			priv->channel->hw_value,
+			priv->channel_switch_in_progress ?
+			" (switching)" : "");
+	if (priv->rx_filter.promiscuous)
+		seq_puts(seq,   "Filter:     promisc\n");
+	else if (priv->rx_filter.fcs)
+		seq_puts(seq,   "Filter:     fcs\n");
+	if (priv->rx_filter.bssid)
+		seq_puts(seq,   "Filter:     bssid\n");
+	if (priv->bf_control.bcn_count)
+		seq_puts(seq,   "Filter:     beacons\n");
+
+	if (priv->enable_beacon ||
+			priv->mode == NL80211_IFTYPE_AP ||
+			priv->mode == NL80211_IFTYPE_ADHOC ||
+			priv->mode == NL80211_IFTYPE_MESH_POINT ||
+			priv->mode == NL80211_IFTYPE_P2P_GO)
+		seq_printf(seq, "Beaconing:  %s\n",
+			priv->enable_beacon ?
+			"enabled" : "disabled");
+	if (priv->ssid_length ||
+			priv->mode == NL80211_IFTYPE_AP ||
+			priv->mode == NL80211_IFTYPE_ADHOC ||
+			priv->mode == NL80211_IFTYPE_MESH_POINT ||
+			priv->mode == NL80211_IFTYPE_P2P_GO)
+		seq_printf(seq, "SSID:       %.*s\n",
+			priv->ssid_length, priv->ssid);
+
+	for (i = 0; i < 4; ++i) {
+		seq_printf(seq, "EDCA(%d):    %d, %d, %d, %d, %d\n", i,
+			priv->edca.params[i].cwMin,
+			priv->edca.params[i].cwMax,
+			priv->edca.params[i].aifns,
+			priv->edca.params[i].txOpLimit,
+			priv->edca.params[i].maxReceiveLifetime);
+	}
+	if (priv->join_status == CW1200_JOIN_STATUS_STA) {
+		static const char *pmMode = "unknown";
+		switch (priv->powersave_mode.pmMode) {
+		case WSM_PSM_ACTIVE:
+			pmMode = "off";
+			break;
+		case WSM_PSM_PS:
+			pmMode = "on";
+			break;
+		case WSM_PSM_FAST_PS:
+			pmMode = "dynamic";
+			break;
+		}
+		seq_printf(seq, "Preamble:   %s\n",
+			cw1200_debug_preamble[
+			priv->association_mode.preambleType]);
+		seq_printf(seq, "AMPDU spcn: %d\n",
+			priv->association_mode.mpduStartSpacing);
+		seq_printf(seq, "Basic rate: 0x%.8X\n",
+			le32_to_cpu(priv->association_mode.basicRateSet));
+		seq_printf(seq, "Bss lost:   %d beacons\n",
+			priv->bss_params.beaconLostCount);
+		seq_printf(seq, "AID:        %d\n",
+			priv->bss_params.aid);
+		seq_printf(seq, "Rates:      0x%.8X\n",
+			priv->bss_params.operationalRateSet);
+		seq_printf(seq, "Powersave:  %s\n", pmMode);
+	}
+	seq_printf(seq, "HT:         %s\n",
+		cw1200_is_ht(&priv->ht_info) ? "on" : "off");
+	if (cw1200_is_ht(&priv->ht_info)) {
+		seq_printf(seq, "Greenfield: %s\n",
+			cw1200_ht_greenfield(&priv->ht_info) ? "yes" : "no");
+		seq_printf(seq, "AMPDU dens: %d\n",
+			cw1200_ht_ampdu_density(&priv->ht_info));
+	}
+	seq_printf(seq, "RSSI thold: %d\n",
+		priv->cqm_rssi_thold);
+	seq_printf(seq, "RSSI hyst:  %d\n",
+		priv->cqm_rssi_hyst);
+	seq_printf(seq, "TXFL thold: %d\n",
+		priv->cqm_tx_failure_thold);
+	seq_printf(seq, "Linkloss:   %d\n",
+		priv->cqm_link_loss_count);
+	seq_printf(seq, "Bcnloss:    %d\n",
+		priv->cqm_beacon_loss_count);
+	seq_printf(seq, "Long retr:  %d\n",
+		priv->long_frame_max_tx_count);
+	seq_printf(seq, "Short retr: %d\n",
+		priv->short_frame_max_tx_count);
+	spin_lock_bh(&priv->tx_policy_cache.lock);
+	i = 0;
+	list_for_each(item, &priv->tx_policy_cache.used)
+		++i;
+	spin_unlock_bh(&priv->tx_policy_cache.lock);
+	seq_printf(seq, "RC in use:  %d\n", i);
+	seq_printf(seq, "BA stat:    %d, %d (%d)\n",
+		ba_cnt, ba_acc, ba_avg);
+	seq_printf(seq, "Block ACK:  %s\n", ba_ena ? "on" : "off");
+
+	seq_puts(seq, "\n");
+	for (i = 0; i < 4; ++i) {
+		cw1200_queue_status_show(seq, &priv->tx_queue[i]);
+		seq_puts(seq, "\n");
+	}
+
+	cw1200_debug_print_map(seq, priv, "Link map:   ",
+		priv->link_id_map);
+	cw1200_debug_print_map(seq, priv, "Asleep map: ",
+		priv->sta_asleep_mask);
+	cw1200_debug_print_map(seq, priv, "PSPOLL map: ",
+		priv->pspoll_mask);
+
+	seq_puts(seq, "\n");
+
+	for (i = 0; i < CW1200_MAX_STA_IN_AP_MODE; ++i) {
+		if (priv->link_id_db[i].status) {
+			seq_printf(seq, "Link %d:     %s, %pM\n",
+				i + 1, cw1200_debug_link_id[
+				priv->link_id_db[i].status],
+				priv->link_id_db[i].mac);
+		}
+	}
+
+	seq_puts(seq, "\n");
+
+	seq_printf(seq, "BH status:  %s\n",
+		atomic_read(&priv->bh_term) ? "terminated" : "alive");
+	seq_printf(seq, "Pending RX: %d\n",
+		atomic_read(&priv->bh_rx));
+	seq_printf(seq, "Pending TX: %d\n",
+		atomic_read(&priv->bh_tx));
+	if (priv->bh_error)
+		seq_printf(seq, "BH errcode: %d\n",
+			priv->bh_error);
+	seq_printf(seq, "TX bufs:    %d x %d bytes\n",
+		priv->wsm_caps.numInpChBufs,
+		priv->wsm_caps.sizeInpChBuf);
+	seq_printf(seq, "Used bufs:  %d\n",
+		priv->hw_bufs_used);
+	seq_printf(seq, "Powermgmt:  %s\n",
+		priv->powersave_enabled ? "on" : "off");
+	seq_printf(seq, "Device:     %s\n",
+		priv->device_can_sleep ? "alseep" : "awake");
+
+	spin_lock(&priv->wsm_cmd.lock);
+	seq_printf(seq, "WSM status: %s\n",
+		priv->wsm_cmd.done ? "idle" : "active");
+	seq_printf(seq, "WSM cmd:    0x%.4X (%d bytes)\n",
+		priv->wsm_cmd.cmd, priv->wsm_cmd.len);
+	seq_printf(seq, "WSM retval: %d\n",
+		priv->wsm_cmd.ret);
+	spin_unlock(&priv->wsm_cmd.lock);
+
+	seq_printf(seq, "Datapath:   %s\n",
+		atomic_read(&priv->tx_lock) ? "locked" : "unlocked");
+	if (atomic_read(&priv->tx_lock))
+		seq_printf(seq, "TXlock cnt: %d\n",
+			atomic_read(&priv->tx_lock));
+
+	seq_printf(seq, "TXed:       %d\n",
+		d->tx);
+	seq_printf(seq, "AGG TXed:   %d\n",
+		d->tx_agg);
+	seq_printf(seq, "MULTI TXed: %d (%d)\n",
+		d->tx_multi, d->tx_multi_frames);
+	seq_printf(seq, "RXed:       %d\n",
+		d->rx);
+	seq_printf(seq, "AGG RXed:   %d\n",
+		d->rx_agg);
+	seq_printf(seq, "TX miss:    %d\n",
+		d->tx_cache_miss);
+	seq_printf(seq, "TX align:   %d\n",
+		d->tx_align);
+	seq_printf(seq, "TX burst:   %d\n",
+		d->tx_burst);
+	seq_printf(seq, "RX burst:   %d\n",
+		d->rx_burst);
+	seq_printf(seq, "TX TTL:     %d\n",
+		d->tx_ttl);
+	seq_printf(seq, "Scan:       %s\n",
+		atomic_read(&priv->scan.in_progress) ? "active" : "idle");
+	seq_printf(seq, "Led state:  0x%.2X\n",
+		priv->softled_state);
+
+	return 0;
+}
+
+static int cw1200_status_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, &cw1200_status_show,
+		inode->i_private);
+}
+
+static const struct file_operations fops_status = {
+	.open = cw1200_status_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int cw1200_counters_show(struct seq_file *seq, void *v)
+{
+	int ret;
+	struct cw1200_common *priv = seq->private;
+	struct wsm_counters_table counters;
+
+	ret = wsm_get_counters_table(priv, &counters);
+	if (ret)
+		return ret;
+
+#define CAT_STR(x, y) x ## y
+#define PUT_COUNTER(tab, name) \
+	seq_printf(seq, "%s:" tab "%d\n", #name, \
+		__le32_to_cpu(counters.CAT_STR(count, name)))
+
+	PUT_COUNTER("\t\t", PlcpErrors);
+	PUT_COUNTER("\t\t", FcsErrors);
+	PUT_COUNTER("\t\t", TxPackets);
+	PUT_COUNTER("\t\t", RxPackets);
+	PUT_COUNTER("\t\t", RxPacketErrors);
+	PUT_COUNTER("\t",   RxDecryptionFailures);
+	PUT_COUNTER("\t\t", RxMicFailures);
+	PUT_COUNTER("\t",   RxNoKeyFailures);
+	PUT_COUNTER("\t",   TxMulticastFrames);
+	PUT_COUNTER("\t",   TxFramesSuccess);
+	PUT_COUNTER("\t",   TxFrameFailures);
+	PUT_COUNTER("\t",   TxFramesRetried);
+	PUT_COUNTER("\t",   TxFramesMultiRetried);
+	PUT_COUNTER("\t",   RxFrameDuplicates);
+	PUT_COUNTER("\t\t", RtsSuccess);
+	PUT_COUNTER("\t\t", RtsFailures);
+	PUT_COUNTER("\t\t", AckFailures);
+	PUT_COUNTER("\t",   RxMulticastFrames);
+	PUT_COUNTER("\t",   RxFramesSuccess);
+	PUT_COUNTER("\t",   RxCMACICVErrors);
+	PUT_COUNTER("\t\t", RxCMACReplays);
+	PUT_COUNTER("\t",   RxMgmtCCMPReplays);
+
+#undef PUT_COUNTER
+#undef CAT_STR
+
+	return 0;
+}
+
+static int cw1200_counters_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, &cw1200_counters_show,
+		inode->i_private);
+}
+
+static const struct file_operations fops_counters = {
+	.open = cw1200_counters_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+	.owner = THIS_MODULE,
+};
+
+static int cw1200_generic_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static ssize_t cw1200_11n_read(struct file *file,
+	char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct cw1200_common *priv = file->private_data;
+	struct ieee80211_supported_band *band =
+		priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ];
+	return simple_read_from_buffer(user_buf, count, ppos,
+		band->ht_cap.ht_supported ? "1\n" : "0\n", 2);
+}
+
+static ssize_t cw1200_11n_write(struct file *file,
+	const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct cw1200_common *priv = file->private_data;
+	struct ieee80211_supported_band *band[2] = {
+		priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ],
+		priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ],
+	};
+	char buf[1];
+	int ena = 0;
+
+	if (!count)
+		return -EINVAL;
+	if (copy_from_user(buf, user_buf, 1))
+		return -EFAULT;
+	if (buf[0] == 1)
+		ena = 1;
+
+	band[0]->ht_cap.ht_supported = ena;
+#ifdef CONFIG_CW1200_5GHZ_SUPPORT
+	band[1]->ht_cap.ht_supported = ena;
+#endif /* CONFIG_CW1200_5GHZ_SUPPORT */
+
+	return count;
+}
+
+static const struct file_operations fops_11n = {
+	.open = cw1200_generic_open,
+	.read = cw1200_11n_read,
+	.write = cw1200_11n_write,
+	.llseek = default_llseek,
+};
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+static ssize_t cw1200_hang_write(struct file *file,
+	const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct cw1200_common *priv = file->private_data;
+	char buf[1];
+
+	if (!count)
+		return -EINVAL;
+	if (copy_from_user(buf, user_buf, 1))
+		return -EFAULT;
+
+	if (priv->vif) {
+		cw1200_pm_stay_awake(&priv->pm_state, 3*HZ);
+		ieee80211_driver_hang_notify(priv->vif, GFP_KERNEL);
+	} else
+		return -ENODEV;
+
+	return count;
+}
+
+static const struct file_operations fops_hang = {
+	.open = cw1200_generic_open,
+	.write = cw1200_hang_write,
+	.llseek = default_llseek,
+};
+#endif
+
+static ssize_t cw1200_wsm_dumps(struct file *file,
+	const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct cw1200_common *priv = file->private_data;
+	char buf[1];
+
+	if (!count)
+		return -EINVAL;
+	if (copy_from_user(buf, user_buf, 1))
+		return -EFAULT;
+
+	if (buf[0] == '1')
+		priv->wsm_enable_wsm_dumps = 1;
+	else
+		priv->wsm_enable_wsm_dumps = 0;
+
+	return count;
+}
+
+static const struct file_operations fops_wsm_dumps = {
+	.open = cw1200_generic_open,
+	.write = cw1200_wsm_dumps,
+	.llseek = default_llseek,
+};
+
+#if defined(CONFIG_CW1200_WSM_DUMPS_SHORT)
+static ssize_t cw1200_short_dump_read(struct file *file,
+	char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct cw1200_common *priv = file->private_data;
+	char buf[20];
+	size_t size = 0;
+
+	sprintf(buf, "Size: %u\n", priv->wsm_dump_max_size);
+	size = strlen(buf);
+
+	return simple_read_from_buffer(user_buf, count, ppos,
+					buf, size);
+}
+
+static ssize_t cw1200_short_dump_write(struct file *file,
+	const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct cw1200_common *priv = file->private_data;
+	char buf[20];
+	unsigned long dump_size = 0;
+
+	if (!count || count > 20)
+		return -EINVAL;
+	if (copy_from_user(buf, user_buf, count))
+		return -EFAULT;
+
+	if (kstrtoul(buf, 10, &dump_size))
+		return -EINVAL;
+	printk(KERN_ERR "%s get %lu\n", __func__, dump_size);
+
+	priv->wsm_dump_max_size = dump_size;
+
+	return count;
+}
+
+static const struct file_operations fops_short_dump = {
+	.open = cw1200_generic_open,
+	.write = cw1200_short_dump_write,
+	.read = cw1200_short_dump_read,
+	.llseek = default_llseek,
+};
+#endif /* CONFIG_CW1200_WSM_DUMPS_SHORT */
+
+int cw1200_debug_init(struct cw1200_common *priv)
+{
+	int ret = -ENOMEM;
+	struct cw1200_debug_priv *d = kzalloc(sizeof(struct cw1200_debug_priv),
+			GFP_KERNEL);
+	priv->debug = d;
+	if (!d)
+		return ret;
+
+	d->debugfs_phy = debugfs_create_dir("cw1200",
+			priv->hw->wiphy->debugfsdir);
+	if (!d->debugfs_phy)
+		goto err;
+
+	if (!debugfs_create_file("status", S_IRUSR, d->debugfs_phy,
+			priv, &fops_status))
+		goto err;
+
+	if (!debugfs_create_file("counters", S_IRUSR, d->debugfs_phy,
+			priv, &fops_counters))
+		goto err;
+
+	if (!debugfs_create_file("11n", S_IRUSR | S_IWUSR,
+			d->debugfs_phy, priv, &fops_11n))
+		goto err;
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	if (!debugfs_create_file("hang", S_IWUSR, d->debugfs_phy,
+			priv, &fops_hang))
+		goto err;
+#endif
+
+	if (!debugfs_create_file("wsm_dumps", S_IWUSR, d->debugfs_phy,
+			priv, &fops_wsm_dumps))
+		goto err;
+
+#if defined(CONFIG_CW1200_WSM_DUMPS_SHORT)
+	if (!debugfs_create_file("wsm_dump_size", S_IRUSR | S_IWUSR,
+			d->debugfs_phy, priv, &fops_short_dump))
+		goto err;
+#endif /* CONFIG_CW1200_WSM_DUMPS_SHORT */
+
+	ret = cw1200_itp_init(priv);
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	priv->debug = NULL;
+	debugfs_remove_recursive(d->debugfs_phy);
+	kfree(d);
+	return ret;
+}
+
+void cw1200_debug_release(struct cw1200_common *priv)
+{
+	struct cw1200_debug_priv *d = priv->debug;
+	if (d) {
+		cw1200_itp_release(priv);
+		priv->debug = NULL;
+		kfree(d);
+	}
+}
+
+int cw1200_print_fw_version(struct cw1200_common *priv, u8 *buf, size_t len)
+{
+	return snprintf(buf, len, "%s %d.%d",
+			cw1200_debug_fw_types[priv->wsm_caps.firmwareType],
+			priv->wsm_caps.firmwareVersion,
+			priv->wsm_caps.firmwareBuildNumber);
+}
diff --git a/drivers/staging/cw1200/debug.h b/drivers/staging/cw1200/debug.h
new file mode 100644
index 00000000000..72b827f296b
--- /dev/null
+++ b/drivers/staging/cw1200/debug.h
@@ -0,0 +1,168 @@
+/*
+ * DebugFS code for ST-Ericsson CW1200 mac80211 driver
+ *
+ * Copyright (c) 2011, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_DEBUG_H_INCLUDED
+#define CW1200_DEBUG_H_INCLUDED
+
+#include "itp.h"
+
+struct cw200_common;
+
+#ifdef CONFIG_CW1200_DEBUGFS
+
+struct cw1200_debug_priv {
+	struct dentry *debugfs_phy;
+	int tx;
+	int tx_agg;
+	int rx;
+	int rx_agg;
+	int tx_multi;
+	int tx_multi_frames;
+	int tx_cache_miss;
+	int tx_align;
+	int tx_ttl;
+	int tx_burst;
+	int rx_burst;
+	int ba_cnt;
+	int ba_acc;
+#ifdef CONFIG_CW1200_ITP
+	struct cw1200_itp itp;
+#endif /* CONFIG_CW1200_ITP */
+};
+
+int cw1200_debug_init(struct cw1200_common *priv);
+void cw1200_debug_release(struct cw1200_common *priv);
+
+static inline void cw1200_debug_txed(struct cw1200_common *priv)
+{
+	++priv->debug->tx;
+}
+
+static inline void cw1200_debug_txed_agg(struct cw1200_common *priv)
+{
+	++priv->debug->tx_agg;
+}
+
+static inline void cw1200_debug_txed_multi(struct cw1200_common *priv,
+					   int count)
+{
+	++priv->debug->tx_multi;
+	priv->debug->tx_multi_frames += count;
+}
+
+static inline void cw1200_debug_rxed(struct cw1200_common *priv)
+{
+	++priv->debug->rx;
+}
+
+static inline void cw1200_debug_rxed_agg(struct cw1200_common *priv)
+{
+	++priv->debug->rx_agg;
+}
+
+static inline void cw1200_debug_tx_cache_miss(struct cw1200_common *priv)
+{
+	++priv->debug->tx_cache_miss;
+}
+
+static inline void cw1200_debug_tx_align(struct cw1200_common *priv)
+{
+	++priv->debug->tx_align;
+}
+
+static inline void cw1200_debug_tx_ttl(struct cw1200_common *priv)
+{
+	++priv->debug->tx_ttl;
+}
+
+static inline void cw1200_debug_tx_burst(struct cw1200_common *priv)
+{
+	++priv->debug->tx_burst;
+}
+
+static inline void cw1200_debug_rx_burst(struct cw1200_common *priv)
+{
+	++priv->debug->rx_burst;
+}
+
+static inline void cw1200_debug_ba(struct cw1200_common *priv,
+				   int ba_cnt, int ba_acc)
+{
+	priv->debug->ba_cnt = ba_cnt;
+	priv->debug->ba_acc = ba_acc;
+}
+
+int cw1200_print_fw_version(struct cw1200_common *priv, u8 *buf, size_t len);
+
+#else /* CONFIG_CW1200_DEBUGFS */
+
+static inline int cw1200_debug_init(struct cw1200_common *priv)
+{
+	return 0;
+}
+
+static inline void cw1200_debug_release(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_txed(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_txed_agg(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_txed_multi(struct cw1200_common *priv,
+					   int count)
+{
+}
+
+static inline void cw1200_debug_rxed(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_rxed_agg(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_tx_cache_miss(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_tx_align(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_tx_ttl(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_tx_burst(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_rx_burst(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_debug_ba(struct cw1200_common *priv,
+				   int ba_cnt, int ba_acc)
+{
+}
+
+int cw1200_print_fw_version(struct cw1200_common *priv, u8 *buf, size_t len)
+{
+}
+
+#endif /* CONFIG_CW1200_DEBUGFS */
+
+#endif /* CW1200_DEBUG_H_INCLUDED */
diff --git a/drivers/staging/cw1200/fwio.c b/drivers/staging/cw1200/fwio.c
new file mode 100644
index 00000000000..72b77bc8bb0
--- /dev/null
+++ b/drivers/staging/cw1200/fwio.c
@@ -0,0 +1,594 @@
+/*
+ * Firmware I/O code for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/vmalloc.h>
+#include <linux/sched.h>
+#include <linux/firmware.h>
+
+#include "cw1200.h"
+#include "fwio.h"
+#include "hwio.h"
+#include "sbus.h"
+#include "bh.h"
+
+static int cw1200_get_hw_type(u32 config_reg_val, int *major_revision)
+{
+	int hw_type = -1;
+	u32 silicon_type = (config_reg_val >> 24) & 0x3;
+	u32 silicon_vers = (config_reg_val >> 31) & 0x1;
+
+	/* Check if we have CW1200 or STLC9000 */
+	if ((silicon_type == 0x1) || (silicon_type == 0x2)) {
+		*major_revision = silicon_type;
+		if (silicon_vers)
+			hw_type = HIF_8601_VERSATILE;
+		else
+			hw_type = HIF_8601_SILICON;
+	} else {
+		*major_revision = 1;
+		hw_type = HIF_9000_SILICON_VERSTAILE;
+	}
+
+	return hw_type;
+}
+
+static int config_reg_read_stlc9000(struct cw1200_common *priv,
+				    u16 reg, u32 *val)
+{
+	u16 val16;
+	int ret = cw1200_reg_read_16(priv, reg, &val16);
+	if (ret < 0)
+		return ret;
+	*val = val16;
+	return 0;
+}
+
+static int config_reg_write_stlc9000(struct cw1200_common *priv,
+				     u16 reg, u32 val)
+{
+	return cw1200_reg_write_16(priv, reg, (u16)val);
+}
+
+static int cw1200_load_firmware_cw1200(struct cw1200_common *priv)
+{
+	int ret, block, num_blocks;
+	unsigned i;
+	u32 val32;
+	u32 put = 0, get = 0;
+	u8 *buf = NULL;
+	const char *fw_path;
+	const struct firmware *firmware = NULL;
+
+	/* Macroses are local. */
+#define APB_WRITE(reg, val) \
+	do { \
+		ret = cw1200_apb_write_32(priv, CW12000_APB(reg), (val)); \
+		if (ret < 0) { \
+			cw1200_dbg(CW1200_DBG_ERROR, \
+				"%s: can't write %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+#define APB_READ(reg, val) \
+	do { \
+		ret = cw1200_apb_read_32(priv, CW12000_APB(reg), &(val)); \
+		if (ret < 0) { \
+			cw1200_dbg(CW1200_DBG_ERROR, \
+				"%s: can't read %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+#define REG_WRITE(reg, val) \
+	do { \
+		ret = cw1200_reg_write_32(priv, (reg), (val)); \
+		if (ret < 0) { \
+			cw1200_dbg(CW1200_DBG_ERROR, \
+				"%s: can't write %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+#define REG_READ(reg, val) \
+	do { \
+		ret = cw1200_reg_read_32(priv, (reg), &(val)); \
+		if (ret < 0) { \
+			cw1200_dbg(CW1200_DBG_ERROR, \
+				"%s: can't read %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+
+	switch (priv->hw_revision) {
+	case CW1200_HW_REV_CUT10:
+		fw_path = FIRMWARE_CUT10;
+		break;
+	case CW1200_HW_REV_CUT11:
+		fw_path = FIRMWARE_CUT11;
+		break;
+	case CW1200_HW_REV_CUT20:
+		fw_path = FIRMWARE_CUT20;
+		break;
+	case CW1200_HW_REV_CUT22:
+		fw_path = FIRMWARE_CUT22;
+		break;
+	default:
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: invalid silicon revision %d.\n",
+			__func__, priv->hw_revision);
+		return -EINVAL;
+	}
+
+	/* Initialize common registers */
+	APB_WRITE(DOWNLOAD_IMAGE_SIZE_REG, DOWNLOAD_ARE_YOU_HERE);
+	APB_WRITE(DOWNLOAD_PUT_REG, 0);
+	APB_WRITE(DOWNLOAD_GET_REG, 0);
+	APB_WRITE(DOWNLOAD_STATUS_REG, DOWNLOAD_PENDING);
+	APB_WRITE(DOWNLOAD_FLAGS_REG, 0);
+
+	/* Write the NOP Instruction */
+	REG_WRITE(ST90TDS_SRAM_BASE_ADDR_REG_ID, 0xFFF20000);
+	REG_WRITE(ST90TDS_AHB_DPORT_REG_ID, 0xEAFFFFFE);
+
+	/* Release CPU from RESET */
+	REG_READ(ST90TDS_CONFIG_REG_ID, val32);
+	val32 &= ~ST90TDS_CONFIG_CPU_RESET_BIT;
+	REG_WRITE(ST90TDS_CONFIG_REG_ID, val32);
+
+	/* Enable Clock */
+	val32 &= ~ST90TDS_CONFIG_CPU_CLK_DIS_BIT;
+	REG_WRITE(ST90TDS_CONFIG_REG_ID, val32);
+
+	/* Load a firmware file */
+	ret = request_firmware(&firmware, fw_path, priv->pdev);
+	if (ret) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't load firmware file %s.\n",
+			__func__, fw_path);
+		goto error;
+	}
+	BUG_ON(!firmware->data);
+
+	buf = kmalloc(DOWNLOAD_BLOCK_SIZE, GFP_KERNEL | GFP_DMA);
+	if (!buf) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't allocate firmware buffer.\n", __func__);
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	/* Check if the bootloader is ready */
+	for (i = 0; i < 100; i += 1 + i / 2) {
+		APB_READ(DOWNLOAD_IMAGE_SIZE_REG, val32);
+		if (val32 == DOWNLOAD_I_AM_HERE)
+			break;
+		mdelay(i);
+	} /* End of for loop */
+
+	if (val32 != DOWNLOAD_I_AM_HERE) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: bootloader is not ready.\n", __func__);
+		ret = -ETIMEDOUT;
+		goto error;
+	}
+
+	/* Calculcate number of download blocks */
+	num_blocks = (firmware->size - 1) / DOWNLOAD_BLOCK_SIZE + 1;
+
+	/* Updating the length in Download Ctrl Area */
+	val32 = firmware->size; /* Explicit cast from size_t to u32 */
+	APB_WRITE(DOWNLOAD_IMAGE_SIZE_REG, val32);
+
+	/* Firmware downloading loop */
+	for (block = 0; block < num_blocks ; block++) {
+		size_t tx_size;
+		size_t block_size;
+
+		/* check the download status */
+		APB_READ(DOWNLOAD_STATUS_REG, val32);
+		if (val32 != DOWNLOAD_PENDING) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: bootloader reported error %d.\n",
+				__func__, val32);
+			ret = -EIO;
+			goto error;
+		}
+
+		/* loop until put - get <= 24K */
+		for (i = 0; i < 100; i++) {
+			APB_READ(DOWNLOAD_GET_REG, get);
+			if ((put - get) <=
+			    (DOWNLOAD_FIFO_SIZE - DOWNLOAD_BLOCK_SIZE))
+				break;
+			mdelay(i);
+		}
+
+		if ((put - get) > (DOWNLOAD_FIFO_SIZE - DOWNLOAD_BLOCK_SIZE)) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Timeout waiting for FIFO.\n",
+				__func__);
+			return -ETIMEDOUT;
+		}
+
+		/* calculate the block size */
+		tx_size = block_size = min((size_t)(firmware->size - put),
+			(size_t)DOWNLOAD_BLOCK_SIZE);
+
+		memcpy(buf, &firmware->data[put], block_size);
+		if (block_size < DOWNLOAD_BLOCK_SIZE) {
+			memset(&buf[block_size],
+				0, DOWNLOAD_BLOCK_SIZE - block_size);
+			tx_size = DOWNLOAD_BLOCK_SIZE;
+		}
+
+		/* send the block to sram */
+		ret = cw1200_apb_write(priv,
+			CW12000_APB(DOWNLOAD_FIFO_OFFSET +
+				(put & (DOWNLOAD_FIFO_SIZE - 1))),
+			buf, tx_size);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: can't write block at line %d.\n",
+				__func__, __LINE__);
+			goto error;
+		}
+
+		/* update the put register */
+		put += block_size;
+		APB_WRITE(DOWNLOAD_PUT_REG, put);
+	} /* End of firmware download loop */
+
+	/* Wait for the download completion */
+	for (i = 0; i < 300; i += 1 + i / 2) {
+		APB_READ(DOWNLOAD_STATUS_REG, val32);
+		if (val32 != DOWNLOAD_PENDING)
+			break;
+		mdelay(i);
+	}
+	if (val32 != DOWNLOAD_SUCCESS) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: wait for download completion failed. " \
+			"Read: 0x%.8X\n", __func__, val32);
+		ret = -ETIMEDOUT;
+		goto error;
+	} else {
+		cw1200_dbg(CW1200_DBG_MSG,
+			"Firmware download completed.\n");
+		ret = 0;
+	}
+
+error:
+	kfree(buf);
+	if (firmware)
+		release_firmware(firmware);
+	return ret;
+
+#undef APB_WRITE
+#undef APB_READ
+#undef REG_WRITE
+#undef REG_READ
+}
+
+int cw1200_load_firmware(struct cw1200_common *priv)
+{
+	int ret;
+	int i;
+	u32 val32;
+	u16 val16;
+	u32 dpll = 0;
+	int major_revision;
+	int (*config_reg_read)(struct cw1200_common *priv, u16 reg, u32 *val);
+	int (*config_reg_write)(struct cw1200_common *priv, u16 reg, u32 val);
+
+	BUG_ON(!priv);
+
+	/* Read CONFIG Register Value - We will read 32 bits */
+	ret = cw1200_reg_read_32(priv, ST90TDS_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't read config register.\n", __func__);
+		goto out;
+	}
+
+	priv->hw_type = cw1200_get_hw_type(val32, &major_revision);
+	if (priv->hw_type < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't deduct hardware type.\n", __func__);
+		ret = -ENOTSUPP;
+		goto out;
+	}
+
+	switch (priv->hw_type) {
+	case HIF_8601_VERSATILE:
+	case HIF_8601_SILICON:
+		dpll = DPLL_INIT_VAL_CW1200;
+		config_reg_read = cw1200_reg_read_32;
+		config_reg_write = cw1200_reg_write_32;
+		break;
+	case HIF_9000_SILICON_VERSTAILE:
+		dpll = DPLL_INIT_VAL_9000;
+		config_reg_read = config_reg_read_stlc9000;
+		config_reg_write = config_reg_write_stlc9000;
+		break;
+	default:
+		BUG_ON(1);
+	}
+
+	ret = cw1200_reg_write_32(priv, ST90TDS_TSET_GEN_R_W_REG_ID, dpll);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't write DPLL register.\n", __func__);
+		goto out;
+	}
+
+	msleep(20);
+
+	/* Read DPLL Reg value and compare with value written */
+	ret = cw1200_reg_read_32(priv,
+		ST90TDS_TSET_GEN_R_W_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't read DPLL register.\n", __func__);
+		goto out;
+	}
+
+	if (val32 != dpll) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: unable to initialise " \
+			"DPLL register. Wrote 0x%.8X, read 0x%.8X.\n",
+			__func__, dpll, val32);
+		ret = -EIO;
+		goto out;
+	}
+
+	/* Set wakeup bit in device */
+	ret = cw1200_reg_read_16(priv, ST90TDS_CONTROL_REG_ID, &val16);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: set_wakeup: can't read " \
+			"control register.\n", __func__);
+		goto out;
+	}
+
+	ret = cw1200_reg_write_16(priv, ST90TDS_CONTROL_REG_ID,
+		val16 | ST90TDS_CONT_WUP_BIT);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: set_wakeup: can't write " \
+			"control register.\n", __func__);
+		goto out;
+	}
+
+	/* Wait for wakeup */
+	for (i = 0 ; i < 300 ; i += 1 + i / 2) {
+		ret = cw1200_reg_read_16(priv,
+			ST90TDS_CONTROL_REG_ID, &val16);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: wait_for_wakeup: can't read " \
+				"control register.\n", __func__);
+			goto out;
+		}
+
+		if (val16 & ST90TDS_CONT_RDY_BIT) {
+			cw1200_dbg(CW1200_DBG_MSG,
+				"WLAN device is ready.\n");
+			break;
+		}
+		msleep(i);
+	}
+
+	if ((val16 & ST90TDS_CONT_RDY_BIT) == 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: wait_for_wakeup: device is not responding.\n",
+			__func__);
+		ret = -ETIMEDOUT;
+		goto out;
+	}
+
+	if (major_revision == 1) {
+		/* CW1200 Hardware detection logic : Check for CUT1.1 */
+		ret = cw1200_ahb_read_32(priv, CW1200_CUT_ID_ADDR, &val32);
+		if (ret) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: HW detection: can't read CUT ID.\n",
+				__func__);
+			goto out;
+		}
+
+		switch (val32) {
+		case CW1200_CUT_11_ID_STR:
+			cw1200_dbg(CW1200_DBG_MSG,
+				   "Cut 1.1 silicon is detected.\n");
+			priv->hw_revision = CW1200_HW_REV_CUT11;
+			break;
+		default:
+			cw1200_dbg(CW1200_DBG_MSG,
+				   "Cut 1.0 silicon is detected.\n");
+			priv->hw_revision = CW1200_HW_REV_CUT10;
+			break;
+		}
+	} else if (major_revision == 2) {
+		u32 ar1, ar2, ar3;
+		cw1200_dbg(CW1200_DBG_MSG, "Cut 2.x silicon is detected.\n");
+
+		ret = cw1200_ahb_read_32(priv, CW1200_CUT2_ID_ADDR, &ar1);
+		if (ret) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: (1) HW detection: can't read CUT ID.\n",
+				__func__);
+			goto out;
+		}
+		ret = cw1200_ahb_read_32(priv, CW1200_CUT2_ID_ADDR + 4, &ar2);
+		if (ret) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: (2) HW detection: can't read CUT ID.\n",
+				__func__);
+			goto out;
+		}
+
+		ret = cw1200_ahb_read_32(priv, CW1200_CUT2_ID_ADDR + 8, &ar3);
+		if (ret) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: (3) HW detection: can't read CUT ID.\n",
+				__func__);
+			goto out;
+		}
+
+		if (ar1 == CW1200_CUT_22_ID_STR1 &&
+		    ar2 == CW1200_CUT_22_ID_STR2 &&
+		    ar3 == CW1200_CUT_22_ID_STR3) {
+			cw1200_dbg(CW1200_DBG_MSG, "Cut 2.2 detected.\n");
+			priv->hw_revision = CW1200_HW_REV_CUT22;
+		} else {
+			cw1200_dbg(CW1200_DBG_MSG, "Cut 2.0 detected.\n");
+			priv->hw_revision = CW1200_HW_REV_CUT20;
+		}
+	} else {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: unsupported silicon major revision %d.\n",
+			__func__, major_revision);
+		ret = -ENOTSUPP;
+		goto out;
+	}
+
+	/* Checking for access mode */
+	ret = config_reg_read(priv, ST90TDS_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: check_access_mode: can't read " \
+			"config register.\n", __func__);
+		goto out;
+	}
+
+	if (val32 & ST90TDS_CONFIG_ACCESS_MODE_BIT) {
+		switch (priv->hw_type)  {
+		case HIF_8601_SILICON:
+			cw1200_dbg(CW1200_DBG_MSG,
+				"%s: CW1200 detected.\n", __func__);
+			ret = cw1200_load_firmware_cw1200(priv);
+			break;
+		case HIF_8601_VERSATILE:
+			/* TODO: Not implemented yet!
+			ret = cw1200_load_firmware_cw1100(priv);
+			*/
+			ret = -ENOTSUPP;
+			goto out;
+		case HIF_9000_SILICON_VERSTAILE:
+			/* TODO: Not implemented yet!
+			ret = cw1200_load_firmware_stlc9000(priv);
+			*/
+			ret = -ENOTSUPP;
+			goto out;
+		default:
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Unknown hardware: %d.\n",
+				__func__, priv->hw_type);
+		}
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: can't download firmware.\n", __func__);
+			goto out;
+		}
+	} else {
+		cw1200_dbg(CW1200_DBG_MSG,
+			"%s: check_access_mode: device is already " \
+			"in QUEUE mode.\n", __func__);
+		/* TODO: verify this branch. Do we need something to do? */
+	}
+
+	/* Register Interrupt Handler */
+	ret = priv->sbus_ops->irq_subscribe(priv->sbus_priv,
+		(sbus_irq_handler)cw1200_irq_handler, priv);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: can't register IRQ handler.\n", __func__);
+		goto out;
+	}
+
+	if (HIF_8601_SILICON  == priv->hw_type) {
+		/* If device is CW1200 the IRQ enable/disable bits
+		 * are in CONFIG register */
+		ret = config_reg_read(priv, ST90TDS_CONFIG_REG_ID, &val32);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: enable_irq: can't read " \
+				"config register.\n", __func__);
+			goto unsubscribe;
+		}
+		ret = config_reg_write(priv, ST90TDS_CONFIG_REG_ID,
+			val32 | ST90TDS_CONF_IRQ_RDY_ENABLE);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: enable_irq: can't write " \
+				"config register.\n", __func__);
+			goto unsubscribe;
+		}
+	} else {
+		/* If device is STLC9000 the IRQ enable/disable bits
+		 * are in CONTROL register */
+		/* Enable device interrupts - Both DATA_RDY and WLAN_RDY */
+		ret = cw1200_reg_read_16(priv, ST90TDS_CONFIG_REG_ID, &val16);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: enable_irq: can't read " \
+				"control register.\n", __func__);
+			goto unsubscribe;
+		}
+		ret = cw1200_reg_write_16(priv, ST90TDS_CONFIG_REG_ID,
+			val16 | ST90TDS_CONT_IRQ_RDY_ENABLE);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: enable_irq: can't write " \
+				"control register.\n", __func__);
+			goto unsubscribe;
+		}
+
+	}
+
+	/* Configure device for MESSSAGE MODE */
+	ret = config_reg_read(priv, ST90TDS_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: set_mode: can't read config register.\n",
+			__func__);
+		goto unsubscribe;
+	}
+	ret = config_reg_write(priv, ST90TDS_CONFIG_REG_ID,
+		val32 & ~ST90TDS_CONFIG_ACCESS_MODE_BIT);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			"%s: set_mode: can't write config register.\n",
+			__func__);
+		goto unsubscribe;
+	}
+
+	/* Unless we read the CONFIG Register we are
+	 * not able to get an interrupt */
+	mdelay(10);
+	config_reg_read(priv, ST90TDS_CONFIG_REG_ID, &val32);
+
+out:
+	return ret;
+
+unsubscribe:
+	priv->sbus_ops->irq_unsubscribe(priv->sbus_priv);
+	return ret;
+}
+
diff --git a/drivers/staging/cw1200/fwio.h b/drivers/staging/cw1200/fwio.h
new file mode 100644
index 00000000000..cb91b8dc481
--- /dev/null
+++ b/drivers/staging/cw1200/fwio.h
@@ -0,0 +1,36 @@
+/*
+ * Firmware API for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FWIO_H_INCLUDED
+#define FWIO_H_INCLUDED
+
+#define FIRMWARE_CUT22		("cw1200/wsm_22.bin")
+#define FIRMWARE_CUT20		("cw1200/wsm_20.bin")
+#define FIRMWARE_CUT11		("cw1200/wsm_11.bin")
+#define FIRMWARE_CUT10		("cw1200/wsm_10.bin")
+#define SDD_FILE_22		("cw1200/sdd_22.bin")
+#define SDD_FILE_20		("cw1200/sdd_20.bin")
+#define SDD_FILE_11		("cw1200/sdd_11.bin")
+#define SDD_FILE_10		("cw1200/sdd_10.bin")
+
+#define CW1200_HW_REV_CUT10	(10)
+#define CW1200_HW_REV_CUT11	(11)
+#define CW1200_HW_REV_CUT20	(20)
+#define CW1200_HW_REV_CUT22	(22)
+
+int cw1200_load_firmware(struct cw1200_common *priv);
+
+#endif
diff --git a/drivers/staging/cw1200/ht.h b/drivers/staging/cw1200/ht.h
new file mode 100644
index 00000000000..5c486a634c7
--- /dev/null
+++ b/drivers/staging/cw1200/ht.h
@@ -0,0 +1,43 @@
+/*
+ *  HT-related code for ST-Ericsson CW1200 driver
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_HT_H_INCLUDED
+#define CW1200_HT_H_INCLUDED
+
+#include <net/mac80211.h>
+
+struct cw1200_ht_info {
+	struct ieee80211_sta_ht_cap	ht_cap;
+	enum nl80211_channel_type	channel_type;
+	u16				operation_mode;
+};
+
+static inline int cw1200_is_ht(const struct cw1200_ht_info *ht_info)
+{
+	return ht_info->channel_type != NL80211_CHAN_NO_HT;
+}
+
+static inline int cw1200_ht_greenfield(const struct cw1200_ht_info *ht_info)
+{
+	return cw1200_is_ht(ht_info) &&
+		(ht_info->ht_cap.cap & IEEE80211_HT_CAP_GRN_FLD) &&
+		!(ht_info->operation_mode &
+			IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT);
+}
+
+static inline int cw1200_ht_ampdu_density(const struct cw1200_ht_info *ht_info)
+{
+	if (!cw1200_is_ht(ht_info))
+		return 0;
+	return ht_info->ht_cap.ampdu_density;
+}
+
+#endif /* CW1200_HT_H_INCLUDED */
diff --git a/drivers/staging/cw1200/hwio.c b/drivers/staging/cw1200/hwio.c
new file mode 100644
index 00000000000..b544a0a4a80
--- /dev/null
+++ b/drivers/staging/cw1200/hwio.c
@@ -0,0 +1,287 @@
+/*
+ * Low-level device IO routines for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver, which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/types.h>
+
+#include "cw1200.h"
+#include "hwio.h"
+#include "sbus.h"
+
+ /* Sdio addr is 4*spi_addr */
+#define SPI_REG_ADDR_TO_SDIO(spi_reg_addr) ((spi_reg_addr) << 2)
+#define SDIO_ADDR17BIT(buf_id, mpf, rfu, reg_id_ofs) \
+				((((buf_id)    & 0x1F) << 7) \
+				| (((mpf)        & 1) << 6) \
+				| (((rfu)        & 1) << 5) \
+				| (((reg_id_ofs) & 0x1F) << 0))
+#define MAX_RETRY		3
+
+
+static int __cw1200_reg_read(struct cw1200_common *priv, u16 addr,
+				void *buf, size_t buf_len, int buf_id)
+{
+	u16 addr_sdio;
+	u32 sdio_reg_addr_17bit ;
+
+	/* Check if buffer is aligned to 4 byte boundary */
+	if (WARN_ON(((unsigned long)buf & 3) && (buf_len > 4))) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+			   "%s: buffer is not aligned.\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Convert to SDIO Register Address */
+	addr_sdio = SPI_REG_ADDR_TO_SDIO(addr);
+	sdio_reg_addr_17bit = SDIO_ADDR17BIT(buf_id, 0, 0, addr_sdio);
+
+	BUG_ON(!priv->sbus_ops);
+	return priv->sbus_ops->sbus_memcpy_fromio(priv->sbus_priv,
+						  sdio_reg_addr_17bit,
+						  buf, buf_len);
+}
+
+static int __cw1200_reg_write(struct cw1200_common *priv, u16 addr,
+				const void *buf, size_t buf_len, int buf_id)
+{
+	u16 addr_sdio;
+	u32 sdio_reg_addr_17bit ;
+
+#if 0
+	/* Check if buffer is aligned to 4 byte boundary */
+	if (WARN_ON(((unsigned long)buf & 3) && (buf_len > 4))) {
+		cw1200_dbg(CW1200_DBG_ERROR, "%s: buffer is not aligned.\n",
+				__func__);
+		return -EINVAL;
+	}
+#endif
+
+	/* Convert to SDIO Register Address */
+	addr_sdio = SPI_REG_ADDR_TO_SDIO(addr);
+	sdio_reg_addr_17bit = SDIO_ADDR17BIT(buf_id, 0, 0, addr_sdio);
+
+	BUG_ON(!priv->sbus_ops);
+	return priv->sbus_ops->sbus_memcpy_toio(priv->sbus_priv,
+						sdio_reg_addr_17bit,
+						buf, buf_len);
+}
+
+static inline int __cw1200_reg_read_32(struct cw1200_common *priv,
+					u16 addr, u32 *val)
+{
+	return __cw1200_reg_read(priv, addr, val, sizeof(val), 0);
+}
+
+static inline int __cw1200_reg_write_32(struct cw1200_common *priv,
+					u16 addr, u32 val)
+{
+	return __cw1200_reg_write(priv, addr, &val, sizeof(val), 0);
+}
+
+int cw1200_reg_read(struct cw1200_common *priv, u16 addr, void *buf,
+			size_t buf_len)
+{
+	int ret;
+	BUG_ON(!priv->sbus_ops);
+	priv->sbus_ops->lock(priv->sbus_priv);
+	ret = __cw1200_reg_read(priv, addr, buf, buf_len, 0);
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_reg_write(struct cw1200_common *priv, u16 addr, const void *buf,
+			size_t buf_len)
+{
+	int ret;
+	BUG_ON(!priv->sbus_ops);
+	priv->sbus_ops->lock(priv->sbus_priv);
+	ret = __cw1200_reg_write(priv, addr, buf, buf_len, 0);
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_data_read(struct cw1200_common *priv, void *buf, size_t buf_len)
+{
+	int ret, retry = 1;
+	BUG_ON(!priv->sbus_ops);
+	priv->sbus_ops->lock(priv->sbus_priv);
+	{
+		int buf_id_rx = priv->buf_id_rx;
+		while (retry <= MAX_RETRY) {
+			ret = __cw1200_reg_read(priv,
+					ST90TDS_IN_OUT_QUEUE_REG_ID, buf,
+					buf_len, buf_id_rx + 1);
+			if (!ret) {
+				buf_id_rx = (buf_id_rx + 1) & 3;
+				priv->buf_id_rx = buf_id_rx;
+				break;
+			} else {
+				retry++;
+				mdelay(1);
+				cw1200_dbg(CW1200_DBG_ERROR, "%s,error :[%d]\n",
+						__func__, ret);
+			}
+		}
+	}
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_data_write(struct cw1200_common *priv, const void *buf,
+			size_t buf_len)
+{
+	int ret, retry = 1;
+	BUG_ON(!priv->sbus_ops);
+	priv->sbus_ops->lock(priv->sbus_priv);
+	{
+		int buf_id_tx = priv->buf_id_tx;
+		while (retry <= MAX_RETRY) {
+			ret = __cw1200_reg_write(priv,
+					ST90TDS_IN_OUT_QUEUE_REG_ID, buf,
+					buf_len, buf_id_tx);
+			if (!ret) {
+				buf_id_tx = (buf_id_tx + 1) & 31;
+				priv->buf_id_tx = buf_id_tx;
+				break;
+			} else {
+				retry++;
+				mdelay(1);
+				cw1200_dbg(CW1200_DBG_ERROR, "%s,error :[%d]\n",
+						__func__, ret);
+			}
+		}
+	}
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_indirect_read(struct cw1200_common *priv, u32 addr, void *buf,
+			 size_t buf_len, u32 prefetch, u16 port_addr)
+{
+	u32 val32 = 0;
+	int i, ret;
+
+	if ((buf_len / 2) >= 0x1000) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't read more than 0xfff words.\n",
+				__func__);
+		WARN_ON(1);
+		return -EINVAL;
+		goto out;
+	}
+
+	priv->sbus_ops->lock(priv->sbus_priv);
+	/* Write address */
+	ret = __cw1200_reg_write_32(priv, ST90TDS_SRAM_BASE_ADDR_REG_ID, addr);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't write address register.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Read CONFIG Register Value - We will read 32 bits */
+	ret = __cw1200_reg_read_32(priv, ST90TDS_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't read config register.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Set PREFETCH bit */
+	ret = __cw1200_reg_write_32(priv, ST90TDS_CONFIG_REG_ID,
+					val32 | prefetch);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't write prefetch bit.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Check for PRE-FETCH bit to be cleared */
+	for (i = 0; i < 20; i++) {
+		ret = __cw1200_reg_read_32(priv, ST90TDS_CONFIG_REG_ID, &val32);
+		if (ret < 0) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+					"%s: Can't check prefetch bit.\n",
+					__func__);
+			goto out;
+		}
+		if (!(val32 & prefetch))
+			break;
+
+		mdelay(i);
+	}
+
+	if (val32 & prefetch) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Prefetch bit is not cleared.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Read data port */
+	ret = __cw1200_reg_read(priv, port_addr, buf, buf_len, 0);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't read data port.\n",
+				__func__);
+		goto out;
+	}
+
+out:
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
+int cw1200_apb_write(struct cw1200_common *priv, u32 addr, const void *buf,
+			size_t buf_len)
+{
+	int ret;
+
+	if ((buf_len / 2) >= 0x1000) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't wrire more than 0xfff words.\n",
+				__func__);
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	priv->sbus_ops->lock(priv->sbus_priv);
+
+	/* Write address */
+	ret = __cw1200_reg_write_32(priv, ST90TDS_SRAM_BASE_ADDR_REG_ID, addr);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: Can't write address register.\n",
+				__func__);
+		goto out;
+	}
+
+	/* Write data port */
+	ret = __cw1200_reg_write(priv, ST90TDS_SRAM_DPORT_REG_ID,
+					buf, buf_len, 0);
+	if (ret < 0) {
+		cw1200_dbg(CW1200_DBG_ERROR, "%s: Can't write data port.\n",
+				__func__);
+		goto out;
+	}
+
+out:
+	priv->sbus_ops->unlock(priv->sbus_priv);
+	return ret;
+}
+
diff --git a/drivers/staging/cw1200/hwio.h b/drivers/staging/cw1200/hwio.h
new file mode 100644
index 00000000000..25c8f6b4c28
--- /dev/null
+++ b/drivers/staging/cw1200/hwio.h
@@ -0,0 +1,243 @@
+/*
+ * Low-level API for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * ST-Ericsson UMAC CW1200 driver which is
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Ajitpal Singh <ajitpal.singh@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_HWIO_H_INCLUDED
+#define CW1200_HWIO_H_INCLUDED
+
+/* extern */ struct cw1200_common;
+
+/* DPLL initial values */
+#define DPLL_INIT_VAL_9000		(0x00000191)
+#define DPLL_INIT_VAL_CW1200		(0x0EC4F121)
+
+/* Hardware Type Definitions */
+#define HIF_8601_VERSATILE		(0)
+#define HIF_8601_SILICON		(1)
+#define HIF_9000_SILICON_VERSTAILE	(2)
+
+#define CW1200_CUT_11_ID_STR		(0x302E3830)
+#define CW1200_CUT_22_ID_STR1		(0x302e3132)
+#define CW1200_CUT_22_ID_STR2		(0x32302e30)
+#define CW1200_CUT_22_ID_STR3		(0x3335)
+#define CW1200_CUT_ID_ADDR		(0xFFF17F90)
+#define CW1200_CUT2_ID_ADDR		(0xFFF1FF90)
+
+/* Download control area */
+/* boot loader start address in SRAM */
+#define DOWNLOAD_BOOT_LOADER_OFFSET	(0x00000000)
+/* 32K, 0x4000 to 0xDFFF */
+#define DOWNLOAD_FIFO_OFFSET		(0x00004000)
+/* 32K */
+#define DOWNLOAD_FIFO_SIZE		(0x00008000)
+/* 128 bytes, 0xFF80 to 0xFFFF */
+#define DOWNLOAD_CTRL_OFFSET		(0x0000FF80)
+#define DOWNLOAD_CTRL_DATA_DWORDS	(32-6)
+
+struct download_cntl_t {
+	/* size of whole firmware file (including Cheksum), host init */
+	u32 ImageSize;
+	/* downloading flags */
+	u32 Flags;
+	/* No. of bytes put into the download, init & updated by host */
+	u32 Put;
+	/* last traced program counter, last ARM reg_pc */
+	u32 TracePc;
+	/* No. of bytes read from the download, host init, device updates */
+	u32 Get;
+	/* r0, boot losader status, host init to pending, device updates */
+	u32 Status;
+	/* Extra debug info, r1 to r14 if status=r0=DOWNLOAD_EXCEPTION */
+	u32 DebugData[DOWNLOAD_CTRL_DATA_DWORDS];
+};
+
+#define	DOWNLOAD_IMAGE_SIZE_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, ImageSize))
+#define	DOWNLOAD_FLAGS_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Flags))
+#define DOWNLOAD_PUT_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Put))
+#define DOWNLOAD_TRACE_PC_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, TracePc))
+#define	DOWNLOAD_GET_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Get))
+#define	DOWNLOAD_STATUS_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Status))
+#define DOWNLOAD_DEBUG_DATA_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, DebugData))
+#define DOWNLOAD_DEBUG_DATA_LEN		(108)
+
+#define DOWNLOAD_BLOCK_SIZE		(1024)
+
+/* For boot loader detection */
+#define DOWNLOAD_ARE_YOU_HERE		(0x87654321)
+#define DOWNLOAD_I_AM_HERE		(0x12345678)
+
+/* Download error code */
+#define DOWNLOAD_PENDING		(0xFFFFFFFF)
+#define DOWNLOAD_SUCCESS		(0)
+#define DOWNLOAD_EXCEPTION		(1)
+#define DOWNLOAD_ERR_MEM_1		(2)
+#define DOWNLOAD_ERR_MEM_2		(3)
+#define DOWNLOAD_ERR_SOFTWARE		(4)
+#define DOWNLOAD_ERR_FILE_SIZE		(5)
+#define DOWNLOAD_ERR_CHECKSUM		(6)
+#define DOWNLOAD_ERR_OVERFLOW		(7)
+#define DOWNLOAD_ERR_IMAGE		(8)
+#define DOWNLOAD_ERR_HOST		(9)
+#define DOWNLOAD_ERR_ABORT		(10)
+
+
+#define SYS_BASE_ADDR_SILICON		(0)
+#define PAC_BASE_ADDRESS_SILICON	(SYS_BASE_ADDR_SILICON + 0x09000000)
+#define PAC_SHARED_MEMORY_SILICON	(PAC_BASE_ADDRESS_SILICON)
+
+#define CW12000_APB(addr)		(PAC_SHARED_MEMORY_SILICON + (addr))
+
+/* ***************************************************************
+*Device register definitions
+*************************************************************** */
+/* WBF - SPI Register Addresses */
+#define ST90TDS_ADDR_ID_BASE		(0x0000)
+/* 16/32 bits */
+#define ST90TDS_CONFIG_REG_ID		(0x0000)
+/* 16/32 bits */
+#define ST90TDS_CONTROL_REG_ID		(0x0001)
+/* 16 bits, Q mode W/R */
+#define ST90TDS_IN_OUT_QUEUE_REG_ID	(0x0002)
+/* 32 bits, AHB bus R/W */
+#define ST90TDS_AHB_DPORT_REG_ID	(0x0003)
+/* 16/32 bits */
+#define ST90TDS_SRAM_BASE_ADDR_REG_ID   (0x0004)
+/* 32 bits, APB bus R/W */
+#define ST90TDS_SRAM_DPORT_REG_ID	(0x0005)
+/* 32 bits, t_settle/general */
+#define ST90TDS_TSET_GEN_R_W_REG_ID	(0x0006)
+/* 16 bits, Q mode read, no length */
+#define ST90TDS_FRAME_OUT_REG_ID	(0x0007)
+#define ST90TDS_ADDR_ID_MAX		(ST90TDS_FRAME_OUT_REG_ID)
+
+/* WBF - Control register bit set */
+/* next o/p length, bit 11 to 0 */
+#define ST90TDS_CONT_NEXT_LEN_MASK	(0x0FFF)
+#define ST90TDS_CONT_WUP_BIT		(BIT(12))
+#define ST90TDS_CONT_RDY_BIT		(BIT(13))
+#define ST90TDS_CONT_IRQ_ENABLE		(BIT(14))
+#define ST90TDS_CONT_RDY_ENABLE		(BIT(15))
+#define ST90TDS_CONT_IRQ_RDY_ENABLE	(BIT(14)|BIT(15))
+
+/* SPI Config register bit set */
+#define ST90TDS_CONFIG_FRAME_BIT	(BIT(2))
+#define ST90TDS_CONFIG_WORD_MODE_BITS	(BIT(3)|BIT(4))
+#define ST90TDS_CONFIG_WORD_MODE_1	(BIT(3))
+#define ST90TDS_CONFIG_WORD_MODE_2	(BIT(4))
+#define ST90TDS_CONFIG_ERROR_0_BIT	(BIT(5))
+#define ST90TDS_CONFIG_ERROR_1_BIT	(BIT(6))
+#define ST90TDS_CONFIG_ERROR_2_BIT	(BIT(7))
+/* TBD: Sure??? */
+#define ST90TDS_CONFIG_CSN_FRAME_BIT	(BIT(7))
+#define ST90TDS_CONFIG_ERROR_3_BIT	(BIT(8))
+#define ST90TDS_CONFIG_ERROR_4_BIT	(BIT(9))
+/* QueueM */
+#define ST90TDS_CONFIG_ACCESS_MODE_BIT	(BIT(10))
+/* AHB bus */
+#define ST90TDS_CONFIG_AHB_PFETCH_BIT	(BIT(11))
+#define ST90TDS_CONFIG_CPU_CLK_DIS_BIT	(BIT(12))
+/* APB bus */
+#define ST90TDS_CONFIG_PFETCH_BIT	(BIT(13))
+/* cpu reset */
+#define ST90TDS_CONFIG_CPU_RESET_BIT	(BIT(14))
+#define ST90TDS_CONFIG_CLEAR_INT_BIT	(BIT(15))
+
+/* For CW1200 the IRQ Enable and Ready Bits are in CONFIG register */
+#define ST90TDS_CONF_IRQ_RDY_ENABLE	(BIT(16)|BIT(17))
+
+int cw1200_data_read(struct cw1200_common *priv,
+		     void *buf, size_t buf_len);
+int cw1200_data_write(struct cw1200_common *priv,
+		      const void *buf, size_t buf_len);
+
+int cw1200_reg_read(struct cw1200_common *priv, u16 addr,
+		    void *buf, size_t buf_len);
+int cw1200_reg_write(struct cw1200_common *priv, u16 addr,
+		     const void *buf, size_t buf_len);
+
+static inline int cw1200_reg_read_16(struct cw1200_common *priv,
+				     u16 addr, u16 *val)
+{
+	u32 bigVal;
+	int ret;
+	ret = cw1200_reg_read(priv, addr, &bigVal, sizeof(bigVal));
+	*val = (u16)bigVal;
+	return ret;
+}
+
+static inline int cw1200_reg_write_16(struct cw1200_common *priv,
+				      u16 addr, u16 val)
+{
+	u32 bigVal = (u32)val;
+	return cw1200_reg_write(priv, addr, &bigVal, sizeof(bigVal));
+}
+
+static inline int cw1200_reg_read_32(struct cw1200_common *priv,
+				     u16 addr, u32 *val)
+{
+	return cw1200_reg_read(priv, addr, val, sizeof(val));
+}
+
+static inline int cw1200_reg_write_32(struct cw1200_common *priv,
+				      u16 addr, u32 val)
+{
+	return cw1200_reg_write(priv, addr, &val, sizeof(val));
+}
+
+int cw1200_indirect_read(struct cw1200_common *priv, u32 addr, void *buf,
+			 size_t buf_len, u32 prefetch, u16 port_addr);
+int cw1200_apb_write(struct cw1200_common *priv, u32 addr, const void *buf,
+		     size_t buf_len);
+
+static inline int cw1200_apb_read(struct cw1200_common *priv, u32 addr,
+				  void *buf, size_t buf_len)
+{
+	return cw1200_indirect_read(priv, addr, buf, buf_len,
+		ST90TDS_CONFIG_PFETCH_BIT, ST90TDS_SRAM_DPORT_REG_ID);
+}
+
+static inline int cw1200_ahb_read(struct cw1200_common *priv, u32 addr,
+				  void *buf, size_t buf_len)
+{
+	return cw1200_indirect_read(priv, addr, buf, buf_len,
+		ST90TDS_CONFIG_AHB_PFETCH_BIT, ST90TDS_AHB_DPORT_REG_ID);
+}
+
+static inline int cw1200_apb_read_32(struct cw1200_common *priv,
+				     u32 addr, u32 *val)
+{
+	return cw1200_apb_read(priv, addr, val, sizeof(val));
+}
+
+static inline int cw1200_apb_write_32(struct cw1200_common *priv,
+				      u32 addr, u32 val)
+{
+	return cw1200_apb_write(priv, addr, &val, sizeof(val));
+}
+
+static inline int cw1200_ahb_read_32(struct cw1200_common *priv,
+				     u32 addr, u32 *val)
+{
+	return cw1200_ahb_read(priv, addr, val, sizeof(val));
+}
+
+#endif /* CW1200_HWIO_H_INCLUDED */
diff --git a/drivers/staging/cw1200/itp.c b/drivers/staging/cw1200/itp.c
new file mode 100644
index 00000000000..eb7e53bf096
--- /dev/null
+++ b/drivers/staging/cw1200/itp.c
@@ -0,0 +1,739 @@
+/*
+ * mac80211 glue code for mac80211 ST-Ericsson CW1200 drivers
+ * ITP code
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/debugfs.h>
+#include <linux/poll.h>
+#include <linux/time.h>
+#include <linux/kallsyms.h>
+#include <net/mac80211.h>
+#include "cw1200.h"
+#include "debug.h"
+#include "itp.h"
+#include "sta.h"
+
+static int __cw1200_itp_open(struct cw1200_common *priv);
+static int __cw1200_itp_close(struct cw1200_common *priv);
+static void cw1200_itp_rx_start(struct cw1200_common *priv);
+static void cw1200_itp_rx_stop(struct cw1200_common *priv);
+static void cw1200_itp_rx_stats(struct cw1200_common *priv);
+static void cw1200_itp_rx_reset(struct cw1200_common *priv);
+static void cw1200_itp_tx_stop(struct cw1200_common *priv);
+static void cw1200_itp_handle(struct cw1200_common *priv,
+			      struct sk_buff *skb);
+static void cw1200_itp_err(struct cw1200_common *priv,
+			   int err,
+			   int arg);
+static void __cw1200_itp_tx_stop(struct cw1200_common *priv);
+
+static ssize_t cw1200_itp_read(struct file *file,
+	char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct cw1200_common *priv = file->private_data;
+	struct cw1200_itp *itp = &priv->debug->itp;
+	struct sk_buff *skb;
+	int ret;
+
+	if (skb_queue_empty(&itp->log_queue))
+		return 0;
+
+	skb = skb_dequeue(&itp->log_queue);
+	ret = copy_to_user(user_buf, skb->data, skb->len);
+	*ppos += skb->len;
+	skb->data[skb->len] = 0;
+	itp_printk(KERN_DEBUG "[ITP] >>> %s", skb->data);
+	consume_skb(skb);
+
+	return skb->len - ret;
+}
+
+static ssize_t cw1200_itp_write(struct file *file,
+	const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct cw1200_common *priv = file->private_data;
+	struct sk_buff *skb;
+
+	if (!count || count > 1024)
+		return -EINVAL;
+	skb = dev_alloc_skb(count + 1);
+	if (!skb)
+		return -ENOMEM;
+	skb_trim(skb, 0);
+	skb_put(skb, count + 1);
+	if (copy_from_user(skb->data, user_buf, count)) {
+		kfree_skb(skb);
+		return -EFAULT;
+	}
+	skb->data[count] = 0;
+
+	cw1200_itp_handle(priv, skb);
+	consume_skb(skb);
+	return count;
+}
+
+static unsigned int cw1200_itp_poll(struct file *file, poll_table *wait)
+{
+	struct cw1200_common *priv = file->private_data;
+	struct cw1200_itp *itp = &priv->debug->itp;
+	unsigned int mask = 0;
+
+	poll_wait(file, &itp->read_wait, wait);
+
+	if (!skb_queue_empty(&itp->log_queue))
+		mask |= POLLIN | POLLRDNORM;
+
+	mask |= POLLOUT | POLLWRNORM;
+
+	return mask;
+}
+
+static int cw1200_itp_open(struct inode *inode, struct file *file)
+{
+	struct cw1200_common *priv = inode->i_private;
+	struct cw1200_itp *itp = &priv->debug->itp;
+	int ret = 0;
+
+	file->private_data = priv;
+	if (atomic_inc_return(&itp->open_count) == 1) {
+		ret = __cw1200_itp_open(priv);
+		if (ret && !atomic_dec_return(&itp->open_count))
+			__cw1200_itp_close(priv);
+	} else {
+		atomic_dec(&itp->open_count);
+		ret = -EBUSY;
+	}
+
+	return ret;
+}
+
+static int cw1200_itp_close(struct inode *inode, struct file *file)
+{
+	struct cw1200_common *priv = file->private_data;
+	struct cw1200_itp *itp = &priv->debug->itp;
+	if (!atomic_dec_return(&itp->open_count)) {
+		__cw1200_itp_close(priv);
+		wake_up(&itp->close_wait);
+	}
+	return 0;
+}
+
+static const struct file_operations fops_itp = {
+	.open = cw1200_itp_open,
+	.read = cw1200_itp_read,
+	.write = cw1200_itp_write,
+	.poll = cw1200_itp_poll,
+	.release = cw1200_itp_close,
+	.llseek = default_llseek,
+	.owner = THIS_MODULE,
+};
+
+static void cw1200_itp_fill_pattern(u8 *data, int size,
+		enum cw1200_itp_data_modes mode)
+{
+	u8 *p = data;
+
+	if (size <= 0)
+		return;
+
+	switch (mode) {
+	default:
+	case ITP_DATA_ZEROS:
+		memset(data, 0x0, size);
+		break;
+	case ITP_DATA_ONES:
+		memset(data, 0xff, size);
+		break;
+	case ITP_DATA_ZERONES:
+		memset(data, 0x55, size);
+		break;
+	case ITP_DATA_RANDOM:
+		while (p < data+size-sizeof(u32)) {
+			(*(u32 *)p) = random32();
+			p += sizeof(u32);
+		}
+		while (p < data+size) {
+			(*p) = random32() & 0xFF;
+			p++;
+		}
+		break;
+	}
+	return;
+}
+
+static void cw1200_itp_tx_work(struct work_struct *work)
+{
+	struct cw1200_itp *itp = container_of(work, struct cw1200_itp,
+		    tx_work.work);
+	struct cw1200_common *priv = itp->priv;
+	atomic_set(&priv->bh_tx, 1);
+	wake_up(&priv->bh_wq);
+}
+
+static void cw1200_itp_tx_finish(struct work_struct *work)
+{
+	struct cw1200_itp *itp = container_of(work, struct cw1200_itp,
+		    tx_finish.work);
+	__cw1200_itp_tx_stop(itp->priv);
+}
+
+int cw1200_itp_init(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+
+	itp->priv = priv;
+	atomic_set(&itp->open_count, 0);
+	atomic_set(&itp->stop_tx, 0);
+	atomic_set(&itp->awaiting_confirm, 0);
+	skb_queue_head_init(&itp->log_queue);
+	spin_lock_init(&itp->tx_lock);
+	init_waitqueue_head(&itp->read_wait);
+	init_waitqueue_head(&itp->write_wait);
+	init_waitqueue_head(&itp->close_wait);
+	INIT_DELAYED_WORK(&itp->tx_work, cw1200_itp_tx_work);
+	INIT_DELAYED_WORK(&itp->tx_finish, cw1200_itp_tx_finish);
+	itp->data = NULL;
+	itp->hdr_len = WSM_TX_EXTRA_HEADROOM +
+			sizeof(struct ieee80211_hdr_3addr);
+
+	if (!debugfs_create_file("itp", S_IRUSR | S_IWUSR,
+			priv->debug->debugfs_phy, priv, &fops_itp))
+		return -ENOMEM;
+
+	return 0;
+}
+
+void cw1200_itp_release(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+
+	wait_event_interruptible(itp->close_wait,
+			!atomic_read(&itp->open_count));
+
+	WARN_ON(atomic_read(&itp->open_count));
+
+	skb_queue_purge(&itp->log_queue);
+	cw1200_itp_tx_stop(priv);
+}
+
+static int __cw1200_itp_open(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+
+	if (!priv->vif)
+		return -EINVAL;
+	if (priv->join_status)
+		return -EINVAL;
+	itp->saved_channel = priv->channel;
+	if (!priv->channel)
+		priv->channel = &priv->hw->
+			wiphy->bands[IEEE80211_BAND_2GHZ]->channels[0];
+	wsm_set_bssid_filtering(priv, false);
+	cw1200_itp_rx_reset(priv);
+	return 0;
+}
+
+static int __cw1200_itp_close(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	if (atomic_read(&itp->test_mode) == TEST_MODE_RX_TEST)
+		cw1200_itp_rx_stop(priv);
+	cw1200_itp_tx_stop(priv);
+	cw1200_disable_listening(priv);
+	cw1200_update_filtering(priv);
+	priv->channel = itp->saved_channel;
+	return 0;
+}
+
+bool cw1200_is_itp(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	return atomic_read(&itp->open_count) != 0;
+}
+
+static void cw1200_itp_rx_reset(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	itp->rx_cnt = 0;
+	itp->rx_rssi = 0;
+	itp->rx_rssi_max = -1000;
+	itp->rx_rssi_min = 1000;
+}
+
+static void cw1200_itp_rx_start(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+
+	itp_printk(KERN_DEBUG "[ITP] RX start, band = %d, ch = %d\n",
+			itp->band, itp->ch);
+	atomic_set(&itp->test_mode, TEST_MODE_RX_TEST);
+	cw1200_update_listening(priv, false);
+	priv->channel = &priv->hw->
+		wiphy->bands[itp->band]->channels[itp->ch];
+	cw1200_update_listening(priv, true);
+	wsm_set_bssid_filtering(priv, false);
+}
+
+static void cw1200_itp_rx_stop(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	itp_printk(KERN_DEBUG "[ITP] RX stop\n");
+	atomic_set(&itp->test_mode, TEST_MODE_NO_TEST);
+	cw1200_itp_rx_reset(priv);
+}
+
+static void cw1200_itp_rx_stats(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	struct sk_buff *skb;
+	char buf[128];
+	int len, ret;
+	struct wsm_counters_table counters;
+
+	ret = wsm_get_counters_table(priv, &counters);
+
+	if (ret)
+		cw1200_itp_err(priv, -EBUSY, 20);
+
+	if (!itp->rx_cnt)
+		len = snprintf(buf, sizeof(buf), "1,0,0,0,0,%d\n",
+				counters.countRxPacketErrors);
+	else
+		len = snprintf(buf, sizeof(buf), "1,%d,%ld,%d,%d,%d\n",
+			itp->rx_cnt,
+			itp->rx_cnt ? itp->rx_rssi / itp->rx_cnt : 0,
+			itp->rx_rssi_min, itp->rx_rssi_max,
+			counters.countRxPacketErrors);
+
+	if (len <= 0) {
+		cw1200_itp_err(priv, -EBUSY, 21);
+		return;
+	}
+
+	skb = dev_alloc_skb(len);
+	if (!skb) {
+		cw1200_itp_err(priv, -ENOMEM, 22);
+		return;
+	}
+
+	itp->rx_cnt = 0;
+	itp->rx_rssi = 0;
+	itp->rx_rssi_max = -1000;
+	itp->rx_rssi_min = 1000;
+
+	skb_trim(skb, 0);
+	skb_put(skb, len);
+
+	memcpy(skb->data, buf, len);
+	skb_queue_tail(&itp->log_queue, skb);
+	wake_up(&itp->read_wait);
+}
+
+static void cw1200_itp_tx_start(struct cw1200_common *priv)
+{
+	struct wsm_tx *tx;
+	struct ieee80211_hdr_3addr *hdr;
+	struct cw1200_itp *itp = &priv->debug->itp;
+	struct wsm_association_mode assoc_mode = {
+		.flags = WSM_ASSOCIATION_MODE_USE_PREAMBLE_TYPE,
+		.preambleType = itp->preamble,
+	};
+	int len;
+	u8 da_addr[6] = ITP_DEFAULT_DA_ADDR;
+
+	/* Rates index 4 and 5 are not supported */
+	if (itp->rate > 3)
+		itp->rate += 2;
+
+	itp_printk(KERN_DEBUG "[ITP] TX start: band = %d, ch = %d, rate = %d,"
+			" preamble = %d, number = %d, data_mode = %d,"
+			" interval = %d, power = %d, data_len = %d\n",
+			itp->band, itp->ch, itp->rate, itp->preamble,
+			itp->number, itp->data_mode, itp->interval_us,
+			itp->power, itp->data_len);
+
+	len = itp->hdr_len + itp->data_len;
+
+	itp->data = kmalloc(len, GFP_KERNEL);
+	tx = (struct wsm_tx *)itp->data;
+	tx->hdr.len = itp->data_len + itp->hdr_len;
+	tx->hdr.id = __cpu_to_le16(0x0004 | 1 << 6);
+	tx->maxTxRate = itp->rate;
+	tx->queueId = 3;
+	tx->more = 0;
+	tx->flags = 0xc;
+	tx->packetID = 0x55ff55;
+	tx->reserved = 0;
+	tx->expireTime = 1;
+
+	if (itp->preamble == ITP_PREAMBLE_GREENFIELD)
+		tx->htTxParameters = WSM_HT_TX_GREENFIELD;
+	else if (itp->preamble == ITP_PREAMBLE_MIXED)
+		tx->htTxParameters = WSM_HT_TX_MIXED;
+
+	hdr = (struct ieee80211_hdr_3addr *)&itp->data[sizeof(struct wsm_tx)];
+	memset(hdr, 0, sizeof(*hdr));
+	hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
+			IEEE80211_FCTL_TODS);
+	memcpy(hdr->addr1, da_addr, ETH_ALEN);
+	memcpy(hdr->addr2, priv->vif->addr, ETH_ALEN);
+	memcpy(hdr->addr3, da_addr, ETH_ALEN);
+
+	cw1200_itp_fill_pattern(&itp->data[itp->hdr_len],
+			itp->data_len, itp->data_mode);
+
+	cw1200_update_listening(priv, false);
+	priv->channel = &priv->hw->
+		wiphy->bands[itp->band]->channels[itp->ch];
+	WARN_ON(wsm_set_output_power(priv, itp->power));
+	if (itp->preamble == ITP_PREAMBLE_SHORT ||
+			itp->preamble == ITP_PREAMBLE_LONG)
+		WARN_ON(wsm_set_association_mode(priv,
+					&assoc_mode));
+	wsm_set_bssid_filtering(priv, false);
+	cw1200_update_listening(priv, true);
+
+	spin_lock_bh(&itp->tx_lock);
+	atomic_set(&itp->test_mode, TEST_MODE_TX_TEST);
+	atomic_set(&itp->awaiting_confirm, 0);
+	atomic_set(&itp->stop_tx, 0);
+	atomic_set(&priv->bh_tx, 1);
+	ktime_get_ts(&itp->last_sent);
+	wake_up(&priv->bh_wq);
+	spin_unlock_bh(&itp->tx_lock);
+}
+
+void __cw1200_itp_tx_stop(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	spin_lock_bh(&itp->tx_lock);
+	kfree(itp->data);
+	itp->data = NULL;
+	atomic_set(&itp->test_mode, TEST_MODE_NO_TEST);
+	spin_unlock_bh(&itp->tx_lock);
+}
+
+static void cw1200_itp_tx_stop(struct cw1200_common *priv)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	itp_printk(KERN_DEBUG "[ITP] TX stop\n");
+	atomic_set(&itp->stop_tx, 1);
+	flush_workqueue(priv->workqueue);
+
+	/* time for FW to confirm all tx requests */
+	msleep(500);
+
+	__cw1200_itp_tx_stop(priv);
+}
+
+static void cw1200_itp_get_version(struct cw1200_common *priv,
+		enum cw1200_itp_version_type type)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	struct sk_buff *skb;
+	char buf[ITP_BUF_SIZE];
+	size_t size = 0;
+	int len;
+	itp_printk(KERN_DEBUG "[ITP] print %s version\n", type == ITP_CHIP_ID ?
+			"chip" : "firmware");
+
+	len = snprintf(buf, ITP_BUF_SIZE, "2,");
+	if (len <= 0) {
+		cw1200_itp_err(priv, -EINVAL, 40);
+		return;
+	}
+	size += len;
+
+	switch (type) {
+	case ITP_CHIP_ID:
+		len = cw1200_print_fw_version(priv, buf+size,
+				ITP_BUF_SIZE - size);
+
+		if (len <= 0) {
+			cw1200_itp_err(priv, -EINVAL, 41);
+			return;
+		}
+		size += len;
+		break;
+	case ITP_FW_VER:
+		len = snprintf(buf+size, ITP_BUF_SIZE - size,
+				"%d.%d", priv->wsm_caps.hardwareId,
+				priv->wsm_caps.hardwareSubId);
+		if (len <= 0) {
+			cw1200_itp_err(priv, -EINVAL, 42);
+			return;
+		}
+		size += len;
+		break;
+	default:
+		cw1200_itp_err(priv, -EINVAL, 43);
+		break;
+	}
+
+	len = snprintf(buf+size, ITP_BUF_SIZE-size, "\n");
+	if (len <= 0) {
+		cw1200_itp_err(priv, -EINVAL, 44);
+		return;
+	}
+	size += len;
+
+	skb = dev_alloc_skb(size);
+	if (!skb) {
+		cw1200_itp_err(priv, -ENOMEM, 45);
+		return;
+	}
+
+	skb_trim(skb, 0);
+	skb_put(skb, size);
+
+	memcpy(skb->data, buf, size);
+	skb_queue_tail(&itp->log_queue, skb);
+	wake_up(&itp->read_wait);
+}
+
+int cw1200_itp_get_tx(struct cw1200_common *priv, u8 **data,
+		size_t *tx_len, int *burst)
+{
+	struct cw1200_itp *itp;
+	struct timespec now;
+	int time_left_us;
+
+	if (!priv->debug)
+		return 0;
+
+	itp	= &priv->debug->itp;
+
+	if (!itp)
+		return 0;
+
+	spin_lock_bh(&itp->tx_lock);
+	if (atomic_read(&itp->test_mode) != TEST_MODE_TX_TEST)
+		goto out;
+
+	if (atomic_read(&itp->stop_tx))
+		goto out;
+
+	if (itp->number == 0) {
+		atomic_set(&itp->stop_tx, 1);
+		queue_delayed_work(priv->workqueue, &itp->tx_finish,
+				HZ/10);
+		goto out;
+	}
+
+	if (!itp->data)
+		goto out;
+
+	if (priv->hw_bufs_used >= 2) {
+		if (!atomic_read(&priv->bh_rx))
+				atomic_set(&priv->bh_rx, 1);
+		atomic_set(&priv->bh_tx, 1);
+		goto out;
+	}
+
+	ktime_get_ts(&now);
+	time_left_us = (itp->last_sent.tv_sec -
+			now.tv_sec)*1000000 +
+		(itp->last_sent.tv_nsec - now.tv_nsec)/1000
+		+ itp->interval_us;
+
+	if (time_left_us > ITP_TIME_THRES_US) {
+		queue_delayed_work(priv->workqueue, &itp->tx_work,
+				ITP_US_TO_MS(time_left_us)*HZ/1000);
+		goto out;
+	}
+
+	if (time_left_us > 50)
+		udelay(time_left_us);
+
+	if (itp->number > 0)
+		itp->number--;
+
+	*data = itp->data;
+	*tx_len = itp->data_len + itp->hdr_len;
+
+	if (itp->data_mode == ITP_DATA_RANDOM)
+		cw1200_itp_fill_pattern(&itp->data[itp->hdr_len],
+				itp->data_len, itp->data_mode);
+	*burst = 2;
+	atomic_set(&priv->bh_tx, 1);
+	ktime_get_ts(&itp->last_sent);
+	atomic_add(1, &itp->awaiting_confirm);
+	spin_unlock_bh(&itp->tx_lock);
+	return 1;
+
+out:
+	spin_unlock_bh(&itp->tx_lock);
+	return 0;
+}
+
+bool cw1200_itp_rxed(struct cw1200_common *priv, struct sk_buff *skb)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	struct ieee80211_rx_status *rx = IEEE80211_SKB_RXCB(skb);
+	int signal;
+
+	if (atomic_read(&itp->test_mode) != TEST_MODE_RX_TEST)
+		return cw1200_is_itp(priv);
+	if (rx->freq != priv->channel->center_freq)
+		return true;
+
+	signal = rx->signal;
+	itp->rx_cnt++;
+	itp->rx_rssi += signal;
+	if (itp->rx_rssi_min > rx->signal)
+		itp->rx_rssi_min = rx->signal;
+	if (itp->rx_rssi_max < rx->signal)
+		itp->rx_rssi_max = rx->signal;
+
+	return true;
+}
+
+void cw1200_itp_wake_up_tx(struct cw1200_common *priv)
+{
+	wake_up(&priv->debug->itp.write_wait);
+}
+
+bool cw1200_itp_tx_running(struct cw1200_common *priv)
+{
+	if (atomic_read(&priv->debug->itp.awaiting_confirm) ||
+			atomic_read(&priv->debug->itp.test_mode) ==
+			TEST_MODE_TX_TEST) {
+		atomic_sub(1, &priv->debug->itp.awaiting_confirm);
+		return true;
+	}
+	return false;
+}
+
+static void cw1200_itp_handle(struct cw1200_common *priv,
+			      struct sk_buff *skb)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	const struct wiphy *wiphy = priv->hw->wiphy;
+	int cmd;
+	int ret;
+
+	itp_printk(KERN_DEBUG "[ITP] <<< %s", skb->data);
+	if (sscanf(skb->data, "%d", &cmd) != 1) {
+		cw1200_itp_err(priv, -EINVAL, 1);
+		return;
+	}
+
+	switch (cmd) {
+	case 1: /* RX test */
+		if (atomic_read(&itp->test_mode)) {
+			cw1200_itp_err(priv, -EBUSY, 0);
+			return;
+		}
+		ret = sscanf(skb->data, "%d,%d,%d",
+				&cmd, &itp->band, &itp->ch);
+		if (ret != 3) {
+			cw1200_itp_err(priv, -EINVAL, ret + 1);
+			return;
+		}
+		if (itp->band >= 2)
+			cw1200_itp_err(priv, -EINVAL, 2);
+		else if (!wiphy->bands[itp->band])
+			cw1200_itp_err(priv, -EINVAL, 2);
+		else if (itp->ch >=
+				wiphy->bands[itp->band]->n_channels)
+			cw1200_itp_err(priv, -EINVAL, 3);
+		else {
+			cw1200_itp_rx_stats(priv);
+			cw1200_itp_rx_start(priv);
+		}
+		break;
+	case 2: /* RX stat */
+		cw1200_itp_rx_stats(priv);
+		break;
+	case 3: /* RX/TX stop */
+		if (atomic_read(&itp->test_mode) == TEST_MODE_RX_TEST) {
+			cw1200_itp_rx_stats(priv);
+			cw1200_itp_rx_stop(priv);
+		} else if (atomic_read(&itp->test_mode) == TEST_MODE_TX_TEST) {
+			cw1200_itp_tx_stop(priv);
+		} else
+			cw1200_itp_err(priv, -EBUSY, 0);
+		break;
+	case 4: /* TX start */
+		if (atomic_read(&itp->test_mode) != TEST_MODE_NO_TEST) {
+			cw1200_itp_err(priv, -EBUSY, 0);
+			return;
+		}
+		ret = sscanf(skb->data, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
+				&cmd, &itp->band, &itp->ch, &itp->rate,
+				&itp->preamble, &itp->number, &itp->data_mode,
+				&itp->interval_us, &itp->power, &itp->data_len);
+		if (ret != 10) {
+			cw1200_itp_err(priv, -EINVAL, ret + 1);
+			return;
+		}
+		if (itp->band >= 2)
+			cw1200_itp_err(priv, -EINVAL, 2);
+		else if (!wiphy->bands[itp->band])
+			cw1200_itp_err(priv, -EINVAL, 2);
+		else if (itp->ch >=
+				wiphy->bands[itp->band]->n_channels)
+			cw1200_itp_err(priv, -EINVAL, 3);
+		else if (itp->rate >= 20)
+			cw1200_itp_err(priv, -EINVAL, 4);
+		else if (itp->preamble >= ITP_PREAMBLE_MAX)
+			cw1200_itp_err(priv, -EINVAL, 5);
+		else if (itp->data_mode >= ITP_DATA_MAX_MODE)
+			cw1200_itp_err(priv, -EINVAL, 7);
+		else if (itp->data_len < ITP_MIN_DATA_SIZE ||
+				itp->data_len > priv->wsm_caps.sizeInpChBuf -
+				itp->hdr_len)
+			cw1200_itp_err(priv, -EINVAL, 8);
+		else {
+		    cw1200_itp_tx_start(priv);
+		}
+		break;
+	case 5:
+		cw1200_itp_get_version(priv, ITP_CHIP_ID);
+		break;
+	case 6:
+		cw1200_itp_get_version(priv, ITP_FW_VER);
+		break;
+
+	}
+}
+
+static void cw1200_itp_err(struct cw1200_common *priv,
+			   int err, int arg)
+{
+	struct cw1200_itp *itp = &priv->debug->itp;
+	struct sk_buff *skb;
+	static char buf[255];
+	int len;
+
+	len = snprintf(buf, sizeof(buf), "%d,%d\n",
+		err, arg);
+	if (len <= 0)
+		return;
+
+	skb = dev_alloc_skb(len);
+	if (!skb)
+		return;
+
+	skb_trim(skb, 0);
+	skb_put(skb, len);
+
+	memcpy(skb->data, buf, len);
+	skb_queue_tail(&itp->log_queue, skb);
+	wake_up(&itp->read_wait);
+
+	len = sprint_symbol(buf,
+			(unsigned long)__builtin_return_address(0));
+	if (len <= 0)
+		return;
+	itp_printk(KERN_DEBUG "[ITP] error %d,%d from %s\n",
+			err, arg, buf);
+}
diff --git a/drivers/staging/cw1200/itp.h b/drivers/staging/cw1200/itp.h
new file mode 100644
index 00000000000..635e7f85ff9
--- /dev/null
+++ b/drivers/staging/cw1200/itp.h
@@ -0,0 +1,151 @@
+/*
+ * ITP code for ST-Ericsson CW1200 mac80211 driver
+ *
+ * Copyright (c) 2011, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_ITP_H_INCLUDED
+#define CW1200_ITP_H_INCLUDED
+
+struct cw200_common;
+struct wsm_tx_confirm;
+struct dentry;
+
+#ifdef CONFIG_CW1200_ITP
+
+/*extern*/ struct ieee80211_channel;
+
+#define TEST_MODE_NO_TEST	(0)
+#define TEST_MODE_RX_TEST	(1)
+#define TEST_MODE_TX_TEST	(2)
+
+#define itp_printk(...) printk(__VA_ARGS__)
+#define ITP_DEFAULT_DA_ADDR {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}
+#define ITP_MIN_DATA_SIZE 6
+#define ITP_MAX_DATA_SIZE 1600
+#define ITP_TIME_THRES_US 10000
+#define ITP_US_TO_MS(x) ((x)/1000)
+#define ITP_MS_TO_US(x) ((x)*1000)
+#if ((ITP_US_TO_MS(ITP_TIME_THRES_US))*HZ/1000) < 1
+#warning not able to achieve non-busywaiting ITP_TIME_THRES_US\
+precision with current HZ value !
+#endif
+#define ITP_BUF_SIZE 255
+
+
+enum cw1200_itp_data_modes {
+	ITP_DATA_ZEROS,
+	ITP_DATA_ONES,
+	ITP_DATA_ZERONES,
+	ITP_DATA_RANDOM,
+	ITP_DATA_MAX_MODE,
+};
+
+enum cw1200_itp_version_type {
+	ITP_CHIP_ID,
+	ITP_FW_VER,
+};
+
+enum cw1200_itp_preamble_type {
+	ITP_PREAMBLE_LONG,
+	ITP_PREAMBLE_SHORT,
+	ITP_PREAMBLE_OFDM,
+	ITP_PREAMBLE_MIXED,
+	ITP_PREAMBLE_GREENFIELD,
+	ITP_PREAMBLE_MAX,
+};
+
+
+struct cw1200_itp {
+	struct cw1200_common	*priv;
+	atomic_t		open_count;
+	atomic_t		awaiting_confirm;
+	struct sk_buff_head	log_queue;
+	wait_queue_head_t	read_wait;
+	wait_queue_head_t	write_wait;
+	wait_queue_head_t	close_wait;
+	struct ieee80211_channel *saved_channel;
+	atomic_t		stop_tx;
+	struct delayed_work	tx_work;
+	struct delayed_work	tx_finish;
+	spinlock_t		tx_lock;
+	struct timespec		last_sent;
+	atomic_t		test_mode;
+	int			rx_cnt;
+	long			rx_rssi;
+	int			rx_rssi_max;
+	int			rx_rssi_min;
+	unsigned		band;
+	unsigned		ch;
+	unsigned		rate;
+	unsigned		preamble;
+	unsigned int		number;
+	unsigned		data_mode;
+	int			interval_us;
+	int			power;
+	u8			*data;
+	int			hdr_len;
+	int			data_len;
+};
+
+int cw1200_itp_init(struct cw1200_common *priv);
+void cw1200_itp_release(struct cw1200_common *priv);
+
+bool cw1200_is_itp(struct cw1200_common *priv);
+bool cw1200_itp_rxed(struct cw1200_common *priv, struct sk_buff *skb);
+void cw1200_itp_wake_up_tx(struct cw1200_common *priv);
+int cw1200_itp_get_tx(struct cw1200_common *priv, u8 **data,
+		size_t *tx_len, int *burst);
+bool cw1200_itp_tx_running(struct cw1200_common *priv);
+
+#else /* CONFIG_CW1200_ITP */
+
+static inline int
+cw1200_itp_init(struct cw1200_common *priv)
+{
+	return 0;
+}
+
+static inline void cw1200_itp_release(struct cw1200_common *priv)
+{
+}
+
+static inline bool cw1200_is_itp(struct cw1200_common *priv)
+{
+	return false;
+}
+
+static inline bool cw1200_itp_rxed(struct cw1200_common *priv,
+		struct sk_buff *skb)
+{
+	return false;
+}
+
+
+static inline void cw1200_itp_consume_txed(struct cw1200_common *priv)
+{
+}
+
+static inline void cw1200_itp_wake_up_tx(struct cw1200_common *priv)
+{
+}
+
+static inline int cw1200_itp_get_tx(struct cw1200_common *priv, u8 **data,
+		size_t *tx_len, int *burst)
+{
+	return 0;
+}
+
+static inline bool cw1200_itp_tx_running(struct cw1200_common *priv)
+{
+	return false;
+}
+
+#endif /* CONFIG_CW1200_ITP */
+
+#endif /* CW1200_ITP_H_INCLUDED */
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
new file mode 100644
index 00000000000..c31a4d45929
--- /dev/null
+++ b/drivers/staging/cw1200/main.c
@@ -0,0 +1,565 @@
+/*
+ * mac80211 glue code for mac80211 ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on:
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ * Copyright (c) 2007-2009, Christian Lamparter <chunkeey@web.de>
+ * Copyright 2008, Johannes Berg <johannes@sipsolutions.net>
+ *
+ * Based on:
+ * - the islsm (softmac prism54) driver, which is:
+ *   Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ * - stlc45xx driver
+ *   Copyright (C) 2008 Nokia Corporation and/or its subsidiary(-ies).
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/firmware.h>
+#include <linux/etherdevice.h>
+#include <linux/vmalloc.h>
+#include <linux/random.h>
+#include <linux/sched.h>
+#include <net/mac80211.h>
+
+#include "cw1200.h"
+#include "txrx.h"
+#include "sbus.h"
+#include "fwio.h"
+#include "hwio.h"
+#include "bh.h"
+#include "sta.h"
+#include "ap.h"
+#include "scan.h"
+#include "debug.h"
+#include "pm.h"
+
+MODULE_AUTHOR("Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>");
+MODULE_DESCRIPTION("Softmac ST-Ericsson CW1200 common code");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("cw1200_core");
+
+/* Accept MAC address of the form macaddr=0x00,0x80,0xE1,0x30,0x40,0x50 */
+static u8 cw1200_mac_template[ETH_ALEN] = {0x00, 0x80, 0xe1, 0x00, 0x00, 0x00};
+module_param_array_named(macaddr, cw1200_mac_template, byte, NULL, S_IRUGO);
+MODULE_PARM_DESC(macaddr, "MAC address");
+
+/* TODO: use rates and channels from the device */
+#define RATETAB_ENT(_rate, _rateid, _flags)		\
+	{						\
+		.bitrate	= (_rate),		\
+		.hw_value	= (_rateid),		\
+		.flags		= (_flags),		\
+	}
+
+static struct ieee80211_rate cw1200_rates[] = {
+	RATETAB_ENT(10,  0,   0),
+	RATETAB_ENT(20,  1,   0),
+	RATETAB_ENT(55,  2,   0),
+	RATETAB_ENT(110, 3,   0),
+	RATETAB_ENT(60,  6,  0),
+	RATETAB_ENT(90,  7,  0),
+	RATETAB_ENT(120, 8,  0),
+	RATETAB_ENT(180, 9,  0),
+	RATETAB_ENT(240, 10, 0),
+	RATETAB_ENT(360, 11, 0),
+	RATETAB_ENT(480, 12, 0),
+	RATETAB_ENT(540, 13, 0),
+};
+
+static struct ieee80211_rate cw1200_mcs_rates[] = {
+	RATETAB_ENT(65,  14, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(130, 15, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(195, 16, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(260, 17, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(390, 18, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(520, 19, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(585, 20, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(650, 21, IEEE80211_TX_RC_MCS),
+};
+
+#define cw1200_a_rates		(cw1200_rates + 4)
+#define cw1200_a_rates_size	(ARRAY_SIZE(cw1200_rates) - 4)
+#define cw1200_g_rates		(cw1200_rates + 0)
+#define cw1200_g_rates_size	(ARRAY_SIZE(cw1200_rates))
+#define cw1200_n_rates		(cw1200_mcs_rates)
+#define cw1200_n_rates_size	(ARRAY_SIZE(cw1200_mcs_rates))
+
+
+#define CHAN2G(_channel, _freq, _flags) {			\
+	.band			= IEEE80211_BAND_2GHZ,		\
+	.center_freq		= (_freq),			\
+	.hw_value		= (_channel),			\
+	.flags			= (_flags),			\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 30,				\
+}
+
+#define CHAN5G(_channel, _flags) {				\
+	.band			= IEEE80211_BAND_5GHZ,		\
+	.center_freq	= 5000 + (5 * (_channel)),		\
+	.hw_value		= (_channel),			\
+	.flags			= (_flags),			\
+	.max_antenna_gain	= 0,				\
+	.max_power		= 30,				\
+}
+
+static struct ieee80211_channel cw1200_2ghz_chantable[] = {
+	CHAN2G(1, 2412, 0),
+	CHAN2G(2, 2417, 0),
+	CHAN2G(3, 2422, 0),
+	CHAN2G(4, 2427, 0),
+	CHAN2G(5, 2432, 0),
+	CHAN2G(6, 2437, 0),
+	CHAN2G(7, 2442, 0),
+	CHAN2G(8, 2447, 0),
+	CHAN2G(9, 2452, 0),
+	CHAN2G(10, 2457, 0),
+	CHAN2G(11, 2462, 0),
+	CHAN2G(12, 2467, 0),
+	CHAN2G(13, 2472, 0),
+	CHAN2G(14, 2484, 0),
+};
+
+#ifdef CONFIG_CW1200_5GHZ_SUPPORT
+static struct ieee80211_channel cw1200_5ghz_chantable[] = {
+	CHAN5G(34, 0),		CHAN5G(36, 0),
+	CHAN5G(38, 0),		CHAN5G(40, 0),
+	CHAN5G(42, 0),		CHAN5G(44, 0),
+	CHAN5G(46, 0),		CHAN5G(48, 0),
+	CHAN5G(52, 0),		CHAN5G(56, 0),
+	CHAN5G(60, 0),		CHAN5G(64, 0),
+	CHAN5G(100, 0),		CHAN5G(104, 0),
+	CHAN5G(108, 0),		CHAN5G(112, 0),
+	CHAN5G(116, 0),		CHAN5G(120, 0),
+	CHAN5G(124, 0),		CHAN5G(128, 0),
+	CHAN5G(132, 0),		CHAN5G(136, 0),
+	CHAN5G(140, 0),		CHAN5G(149, 0),
+	CHAN5G(153, 0),		CHAN5G(157, 0),
+	CHAN5G(161, 0),		CHAN5G(165, 0),
+	CHAN5G(184, 0),		CHAN5G(188, 0),
+	CHAN5G(192, 0),		CHAN5G(196, 0),
+	CHAN5G(200, 0),		CHAN5G(204, 0),
+	CHAN5G(208, 0),		CHAN5G(212, 0),
+	CHAN5G(216, 0),
+};
+#endif /* CONFIG_CW1200_5GHZ_SUPPORT */
+
+static struct ieee80211_supported_band cw1200_band_2ghz = {
+	.channels = cw1200_2ghz_chantable,
+	.n_channels = ARRAY_SIZE(cw1200_2ghz_chantable),
+	.bitrates = cw1200_g_rates,
+	.n_bitrates = cw1200_g_rates_size,
+	.ht_cap = {
+		.cap = IEEE80211_HT_CAP_GRN_FLD |
+			(1 << IEEE80211_HT_CAP_RX_STBC_SHIFT) |
+			IEEE80211_HT_CAP_MAX_AMSDU,
+		.ht_supported = 1,
+		.ampdu_factor = IEEE80211_HT_MAX_AMPDU_8K,
+		.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE,
+		.mcs = {
+			.rx_mask[0] = 0xFF,
+			.rx_highest = __cpu_to_le16(0x41),
+			.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+	},
+};
+
+#ifdef CONFIG_CW1200_5GHZ_SUPPORT
+static struct ieee80211_supported_band cw1200_band_5ghz = {
+	.channels = cw1200_5ghz_chantable,
+	.n_channels = ARRAY_SIZE(cw1200_5ghz_chantable),
+	.bitrates = cw1200_a_rates,
+	.n_bitrates = cw1200_a_rates_size,
+	.ht_cap = {
+		.cap = IEEE80211_HT_CAP_GRN_FLD |
+			(1 << IEEE80211_HT_CAP_RX_STBC_SHIFT) |
+			IEEE80211_HT_CAP_MAX_AMSDU,
+		.ht_supported = 1,
+		.ampdu_factor = IEEE80211_HT_MAX_AMPDU_8K,
+		.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE,
+		.mcs = {
+			.rx_mask[0] = 0xFF,
+			.rx_highest = __cpu_to_le16(0x41),
+			.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+	},
+};
+#endif /* CONFIG_CW1200_5GHZ_SUPPORT */
+
+static const unsigned long cw1200_ttl[] = {
+	1 * HZ,	/* VO */
+	2 * HZ,	/* VI */
+	5 * HZ, /* BE */
+	10 * HZ	/* BK */
+};
+
+static const struct ieee80211_ops cw1200_ops = {
+	.start			= cw1200_start,
+	.stop			= cw1200_stop,
+	.add_interface		= cw1200_add_interface,
+	.remove_interface	= cw1200_remove_interface,
+	.tx			= cw1200_tx,
+	.hw_scan		= cw1200_hw_scan,
+	.set_tim		= cw1200_set_tim,
+	.sta_notify		= cw1200_sta_notify,
+	.sta_add		= cw1200_sta_add,
+	.sta_remove		= cw1200_sta_remove,
+	.set_key		= cw1200_set_key,
+	.set_rts_threshold	= cw1200_set_rts_threshold,
+	.config			= cw1200_config,
+	.bss_info_changed	= cw1200_bss_info_changed,
+	.prepare_multicast	= cw1200_prepare_multicast,
+	.configure_filter	= cw1200_configure_filter,
+	.conf_tx		= cw1200_conf_tx,
+	.get_stats		= cw1200_get_stats,
+	.ampdu_action		= cw1200_ampdu_action,
+	.flush			= cw1200_flush,
+#ifdef CONFIG_PM
+	.suspend		= cw1200_wow_suspend,
+	.resume			= cw1200_wow_resume,
+#endif /* CONFIG_PM */
+	/* Intentionally not offloaded:					*/
+	/*.channel_switch	= cw1200_channel_switch,		*/
+	/*.remain_on_channel	= cw1200_remain_on_channel,		*/
+	/*.cancel_remain_on_channel = cw1200_cancel_remain_on_channel,	*/
+};
+
+struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
+{
+	int i;
+	struct ieee80211_hw *hw;
+	struct cw1200_common *priv;
+
+	hw = ieee80211_alloc_hw(priv_data_len, &cw1200_ops);
+	if (!hw)
+		return NULL;
+
+	priv = hw->priv;
+	priv->hw = hw;
+	priv->mode = NL80211_IFTYPE_UNSPECIFIED;
+	priv->rates = cw1200_rates; /* TODO: fetch from FW */
+	priv->mcs_rates = cw1200_n_rates;
+	/* Enable block ACK for every TID but voice. */
+	priv->ba_tid_mask = 0x3F;
+
+	hw->flags = IEEE80211_HW_SIGNAL_DBM |
+		    IEEE80211_HW_SUPPORTS_PS |
+		    IEEE80211_HW_SUPPORTS_DYNAMIC_PS |
+		    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
+		    IEEE80211_HW_SUPPORTS_UAPSD |
+		    IEEE80211_HW_CONNECTION_MONITOR |
+		    /* Aggregation is fully controlled by firmware.
+		     * Do not need any support from the mac80211 stack */
+		    /* IEEE80211_HW_AMPDU_AGGREGATION | */
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		    IEEE80211_HW_SUPPORTS_P2P_PS |
+		    IEEE80211_HW_SUPPORTS_CQM_BEACON_MISS |
+		    IEEE80211_HW_SUPPORTS_CQM_TX_FAIL |
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+		    IEEE80211_HW_NEED_DTIM_PERIOD;
+
+	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+					  BIT(NL80211_IFTYPE_ADHOC) |
+					  BIT(NL80211_IFTYPE_AP) |
+					  BIT(NL80211_IFTYPE_MESH_POINT) |
+					  BIT(NL80211_IFTYPE_P2P_CLIENT) |
+					  BIT(NL80211_IFTYPE_P2P_GO);
+
+	/* Support only for limited wowlan functionalities */
+	hw->wiphy->wowlan.flags = WIPHY_WOWLAN_ANY |
+					  WIPHY_WOWLAN_DISCONNECT;
+	hw->wiphy->wowlan.n_patterns = 0;
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+
+#if defined(CONFIG_CW1200_DISABLE_BEACON_HINTS)
+	hw->wiphy->flags |= WIPHY_FLAG_DISABLE_BEACON_HINTS;
+#endif
+
+	hw->channel_change_time = 1000;	/* TODO: find actual value */
+	/* priv->beacon_req_id = cpu_to_le32(0); */
+	hw->queues = 4;
+	priv->noise = -94;
+
+	hw->max_rates = 8;
+	hw->max_rate_tries = 15;
+	hw->extra_tx_headroom = WSM_TX_EXTRA_HEADROOM +
+		8  /* TKIP IV */ +
+		12 /* TKIP ICV and MIC */;
+
+	hw->sta_data_size = sizeof(struct cw1200_sta_priv);
+
+	hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &cw1200_band_2ghz;
+#ifdef CONFIG_CW1200_5GHZ_SUPPORT
+	hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &cw1200_band_5ghz;
+#endif /* CONFIG_CW1200_5GHZ_SUPPORT */
+
+	hw->wiphy->max_scan_ssids = 2;
+	hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
+
+	SET_IEEE80211_PERM_ADDR(hw, cw1200_mac_template);
+
+	if (hw->wiphy->perm_addr[3] == 0 &&
+	    hw->wiphy->perm_addr[4] == 0 &&
+	    hw->wiphy->perm_addr[5] == 0) {
+		get_random_bytes(&hw->wiphy->perm_addr[3], 3);
+	}
+
+	mutex_init(&priv->wsm_cmd_mux);
+	mutex_init(&priv->conf_mutex);
+	priv->workqueue = create_singlethread_workqueue("cw1200_wq");
+	sema_init(&priv->scan.lock, 1);
+	INIT_WORK(&priv->scan.work, cw1200_scan_work);
+	INIT_DELAYED_WORK(&priv->scan.probe_work, cw1200_probe_work);
+	INIT_DELAYED_WORK(&priv->scan.timeout, cw1200_scan_timeout);
+	INIT_WORK(&priv->join_work, cw1200_join_work);
+	INIT_DELAYED_WORK(&priv->join_timeout, cw1200_join_timeout);
+	INIT_WORK(&priv->unjoin_work, cw1200_unjoin_work);
+	INIT_WORK(&priv->offchannel_work, cw1200_offchannel_work);
+	INIT_WORK(&priv->wep_key_work, cw1200_wep_key_work);
+	INIT_WORK(&priv->tx_policy_upload_work, tx_policy_upload_work);
+	spin_lock_init(&priv->event_queue_lock);
+	INIT_LIST_HEAD(&priv->event_queue);
+	INIT_WORK(&priv->event_handler, cw1200_event_handler);
+	INIT_DELAYED_WORK(&priv->bss_loss_work, cw1200_bss_loss_work);
+	INIT_DELAYED_WORK(&priv->connection_loss_work,
+			  cw1200_connection_loss_work);
+	spin_lock_init(&priv->bss_loss_lock);
+	INIT_WORK(&priv->tx_failure_work, cw1200_tx_failure_work);
+	spin_lock_init(&priv->ps_state_lock);
+	INIT_DELAYED_WORK(&priv->set_cts_work, cw1200_set_cts_work);
+	INIT_WORK(&priv->set_tim_work, cw1200_set_tim_work);
+	INIT_WORK(&priv->multicast_start_work, cw1200_multicast_start_work);
+	INIT_WORK(&priv->multicast_stop_work, cw1200_multicast_stop_work);
+	INIT_WORK(&priv->link_id_work, cw1200_link_id_work);
+	INIT_DELAYED_WORK(&priv->link_id_gc_work, cw1200_link_id_gc_work);
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	INIT_WORK(&priv->linkid_reset_work, cw1200_link_id_reset);
+#endif
+	INIT_WORK(&priv->update_filtering_work, cw1200_update_filtering_work);
+	INIT_WORK(&priv->ba_work, cw1200_ba_work);
+	init_timer(&priv->mcast_timeout);
+	priv->mcast_timeout.data = (unsigned long)priv;
+	priv->mcast_timeout.function = cw1200_mcast_timeout;
+	spin_lock_init(&priv->ba_lock);
+	init_timer(&priv->ba_timer);
+	priv->ba_timer.data = (unsigned long)priv;
+	priv->ba_timer.function = cw1200_ba_timer;
+
+	if (unlikely(cw1200_queue_stats_init(&priv->tx_queue_stats,
+			CW1200_LINK_ID_MAX,
+			cw1200_skb_dtor,
+			priv))) {
+		ieee80211_free_hw(hw);
+		return NULL;
+	}
+
+	for (i = 0; i < 4; ++i) {
+		if (unlikely(cw1200_queue_init(&priv->tx_queue[i],
+				&priv->tx_queue_stats, i, 16,
+				cw1200_ttl[i]))) {
+			for (; i > 0; i--)
+				cw1200_queue_deinit(&priv->tx_queue[i - 1]);
+			cw1200_queue_stats_deinit(&priv->tx_queue_stats);
+			ieee80211_free_hw(hw);
+			return NULL;
+		}
+	}
+
+	init_waitqueue_head(&priv->channel_switch_done);
+	init_waitqueue_head(&priv->wsm_cmd_wq);
+	init_waitqueue_head(&priv->wsm_startup_done);
+	wsm_buf_init(&priv->wsm_cmd_buf);
+	spin_lock_init(&priv->wsm_cmd.lock);
+	tx_policy_init(priv);
+#if defined(CONFIG_CW1200_WSM_DUMPS_SHORT)
+	priv->wsm_dump_max_size = 20;
+#endif /* CONFIG_CW1200_WSM_DUMPS_SHORT */
+
+	return hw;
+}
+EXPORT_SYMBOL_GPL(cw1200_init_common);
+
+int cw1200_register_common(struct ieee80211_hw *dev)
+{
+	struct cw1200_common *priv = dev->priv;
+	int err;
+
+	err = cw1200_pm_init(&priv->pm_state, priv);
+	if (err) {
+		cw1200_dbg(CW1200_DBG_ERROR, "Cannot init PM. (%d).\n",
+				err);
+		return err;
+	}
+
+	err = ieee80211_register_hw(dev);
+	if (err) {
+		cw1200_dbg(CW1200_DBG_ERROR, "Cannot register device (%d).\n",
+				err);
+		cw1200_pm_deinit(&priv->pm_state);
+		return err;
+	}
+
+#ifdef CONFIG_CW1200_LEDS
+	err = cw1200_init_leds(priv);
+	if (err) {
+		cw1200_pm_deinit(&priv->pm_state);
+		ieee80211_unregister_hw(dev);
+		return err;
+	}
+#endif /* CONFIG_CW1200_LEDS */
+
+	cw1200_debug_init(priv);
+
+	cw1200_dbg(CW1200_DBG_MSG, "is registered as '%s'\n",
+			wiphy_name(dev->wiphy));
+	return 0;
+}
+EXPORT_SYMBOL_GPL(cw1200_register_common);
+
+void cw1200_free_common(struct ieee80211_hw *dev)
+{
+	/* struct cw1200_common *priv = dev->priv; */
+	/* unsigned int i; */
+
+	ieee80211_free_hw(dev);
+}
+EXPORT_SYMBOL_GPL(cw1200_free_common);
+
+void cw1200_unregister_common(struct ieee80211_hw *dev)
+{
+	struct cw1200_common *priv = dev->priv;
+	int i;
+
+	ieee80211_unregister_hw(dev);
+
+	del_timer_sync(&priv->mcast_timeout);
+	del_timer_sync(&priv->ba_timer);
+
+	priv->sbus_ops->irq_unsubscribe(priv->sbus_priv);
+	cw1200_unregister_bh(priv);
+
+	cw1200_debug_release(priv);
+
+#ifdef CONFIG_CW1200_LEDS
+	cw1200_unregister_leds(priv);
+#endif /* CONFIG_CW1200_LEDS */
+
+	mutex_destroy(&priv->conf_mutex);
+
+	wsm_buf_deinit(&priv->wsm_cmd_buf);
+
+	destroy_workqueue(priv->workqueue);
+	priv->workqueue = NULL;
+
+	if (priv->skb_cache) {
+		dev_kfree_skb(priv->skb_cache);
+		priv->skb_cache = NULL;
+	}
+
+	if (priv->sdd) {
+		release_firmware(priv->sdd);
+		priv->sdd = NULL;
+	}
+
+	for (i = 0; i < 4; ++i)
+		cw1200_queue_deinit(&priv->tx_queue[i]);
+	cw1200_queue_stats_deinit(&priv->tx_queue_stats);
+	cw1200_pm_deinit(&priv->pm_state);
+}
+EXPORT_SYMBOL_GPL(cw1200_unregister_common);
+
+int cw1200_core_probe(const struct sbus_ops *sbus_ops,
+		      struct sbus_priv *sbus,
+		      struct device *pdev,
+		      struct cw1200_common **pself)
+{
+	int err = -ENOMEM;
+	struct ieee80211_hw *dev;
+	struct cw1200_common *priv;
+	struct wsm_operational_mode mode = {
+		.power_mode = wsm_power_mode_quiescent,
+		.disableMoreFlagUsage = true,
+	};
+
+	dev = cw1200_init_common(sizeof(struct cw1200_common));
+	if (!dev)
+		goto err;
+
+	priv = dev->priv;
+
+	priv->sbus_ops = sbus_ops;
+	priv->sbus_priv = sbus;
+	priv->pdev = pdev;
+	SET_IEEE80211_DEV(priv->hw, pdev);
+
+	/* WSM callbacks. */
+	priv->wsm_cbc.scan_complete = cw1200_scan_complete_cb;
+	priv->wsm_cbc.tx_confirm = cw1200_tx_confirm_cb;
+	priv->wsm_cbc.rx = cw1200_rx_cb;
+	priv->wsm_cbc.suspend_resume = cw1200_suspend_resume;
+	/* priv->wsm_cbc.set_pm_complete = cw1200_set_pm_complete_cb; */
+	priv->wsm_cbc.channel_switch = cw1200_channel_switch_cb;
+
+	err = cw1200_register_bh(priv);
+	if (err)
+		goto err1;
+
+	err = cw1200_load_firmware(priv);
+	if (err)
+		goto err2;
+	priv->sbus_ops->lock(priv->sbus_priv);
+	WARN_ON(priv->sbus_ops->set_block_size(priv->sbus_priv,
+			SDIO_BLOCK_SIZE));
+	priv->sbus_ops->unlock(priv->sbus_priv);
+
+	if (wait_event_interruptible_timeout(priv->wsm_startup_done,
+				priv->wsm_caps.firmwareReady, 3*HZ) <= 0) {
+		/* TODO: Needs to find how to reset device */
+		/*       in QUEUE mode properly.           */
+		goto err3;
+	}
+
+	/* Set low-power mode. */
+	WARN_ON(wsm_set_operational_mode(priv, &mode));
+
+	/* Enable multi-TX confirmation */
+	WARN_ON(wsm_use_multi_tx_conf(priv, true));
+
+	err = cw1200_register_common(dev);
+	if (err) {
+		priv->sbus_ops->irq_unsubscribe(priv->sbus_priv);
+		goto err3;
+	}
+
+	*pself = dev->priv;
+	return err;
+
+err3:
+	sbus_ops->reset(sbus);
+err2:
+	cw1200_unregister_bh(priv);
+err1:
+	cw1200_free_common(dev);
+err:
+	return err;
+}
+EXPORT_SYMBOL_GPL(cw1200_core_probe);
+
+void cw1200_core_release(struct cw1200_common *self)
+{
+	cw1200_unregister_common(self->hw);
+	cw1200_free_common(self->hw);
+	return;
+}
+EXPORT_SYMBOL_GPL(cw1200_core_release);
diff --git a/drivers/staging/cw1200/pm.c b/drivers/staging/cw1200/pm.c
new file mode 100644
index 00000000000..98796cb2b0b
--- /dev/null
+++ b/drivers/staging/cw1200/pm.c
@@ -0,0 +1,459 @@
+/*
+ * Mac80211 power management API for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2011, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/if_ether.h>
+#include "cw1200.h"
+#include "pm.h"
+#include "sta.h"
+#include "bh.h"
+#include "sbus.h"
+
+#define CW1200_BEACON_SKIPPING_MULTIPLIER 3
+
+struct cw1200_udp_port_filter {
+	struct wsm_udp_port_filter_hdr hdr;
+	struct wsm_udp_port_filter dhcp;
+	struct wsm_udp_port_filter upnp;
+} __packed;
+
+struct cw1200_ether_type_filter {
+	struct wsm_ether_type_filter_hdr hdr;
+	struct wsm_ether_type_filter ip;
+	struct wsm_ether_type_filter pae;
+	struct wsm_ether_type_filter wapi;
+} __packed;
+
+static struct cw1200_udp_port_filter cw1200_udp_port_filter_on = {
+	.hdr.nrFilters = 2,
+	.dhcp = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_OUT,
+		.portType = WSM_FILTER_PORT_TYPE_DST,
+		.udpPort = __cpu_to_le16(67),
+	},
+	.upnp = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_OUT,
+		.portType = WSM_FILTER_PORT_TYPE_DST,
+		.udpPort = __cpu_to_le16(1900),
+	},
+	/* Please add other known ports to be filtered out here and
+	 * update nrFilters field in the header.
+	 * Up to 4 filters are allowed. */
+};
+
+static struct wsm_udp_port_filter_hdr cw1200_udp_port_filter_off = {
+	.nrFilters = 0,
+};
+
+#ifndef ETH_P_WAPI
+#define ETH_P_WAPI     0x88B4
+#endif
+
+static struct cw1200_ether_type_filter cw1200_ether_type_filter_on = {
+	.hdr.nrFilters = 3,
+	.ip = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_IP),
+	},
+	.pae = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_PAE),
+	},
+	.wapi = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_WAPI),
+	},
+	/* Please add other known ether types to be filtered out here and
+	 * update nrFilters field in the header.
+	 * Up to 4 filters are allowed. */
+};
+
+static struct wsm_ether_type_filter_hdr cw1200_ether_type_filter_off = {
+	.nrFilters = 0,
+};
+
+static int cw1200_suspend_late(struct device *dev);
+static void cw1200_pm_release(struct device *dev);
+static int cw1200_pm_probe(struct platform_device *pdev);
+
+/* private */
+struct cw1200_suspend_state {
+	unsigned long bss_loss_tmo;
+	unsigned long connection_loss_tmo;
+	unsigned long join_tmo;
+	unsigned long direct_probe;
+	unsigned long link_id_gc;
+	bool beacon_skipping;
+};
+
+static const struct dev_pm_ops cw1200_pm_ops = {
+	.suspend_noirq = cw1200_suspend_late,
+};
+
+static struct platform_driver cw1200_power_driver = {
+	.probe = cw1200_pm_probe,
+	.driver = {
+		.name = "cw1200_power",
+		.pm = &cw1200_pm_ops,
+	},
+};
+
+static int cw1200_pm_init_common(struct cw1200_pm_state *pm,
+				  struct cw1200_common *priv)
+{
+	int ret;
+
+	spin_lock_init(&pm->lock);
+	ret = platform_driver_register(&cw1200_power_driver);
+	if (ret)
+		return ret;
+	pm->pm_dev = platform_device_alloc("cw1200_power", 0);
+	if (!pm->pm_dev) {
+		platform_driver_unregister(&cw1200_power_driver);
+		return -ENOMEM;
+	}
+
+	pm->pm_dev->dev.platform_data = priv;
+	ret = platform_device_add(pm->pm_dev);
+	if (ret) {
+		kfree(pm->pm_dev);
+		pm->pm_dev = NULL;
+	}
+
+	return ret;
+}
+
+static void cw1200_pm_deinit_common(struct cw1200_pm_state *pm)
+{
+	platform_driver_unregister(&cw1200_power_driver);
+	if (pm->pm_dev) {
+		pm->pm_dev->dev.platform_data = NULL;
+		platform_device_unregister(pm->pm_dev);
+		pm->pm_dev = NULL;
+	}
+}
+
+#ifdef CONFIG_WAKELOCK
+
+int cw1200_pm_init(struct cw1200_pm_state *pm,
+		   struct cw1200_common *priv)
+{
+	int ret = cw1200_pm_init_common(pm, priv);
+	if (!ret)
+		wake_lock_init(&pm->wakelock,
+			WAKE_LOCK_SUSPEND, "cw1200_wlan");
+	return ret;
+}
+
+void cw1200_pm_deinit(struct cw1200_pm_state *pm)
+{
+	if (wake_lock_active(&pm->wakelock))
+		wake_unlock(&pm->wakelock);
+	wake_lock_destroy(&pm->wakelock);
+	cw1200_pm_deinit_common(pm);
+}
+
+void cw1200_pm_stay_awake(struct cw1200_pm_state *pm,
+			  unsigned long tmo)
+{
+	long cur_tmo;
+	spin_lock_bh(&pm->lock);
+	cur_tmo = pm->wakelock.ws.timer_expires - jiffies;
+	if (!wake_lock_active(&pm->wakelock) ||
+			cur_tmo < (long)tmo)
+		wake_lock_timeout(&pm->wakelock, tmo);
+	spin_unlock_bh(&pm->lock);
+}
+
+#else /* CONFIG_WAKELOCK */
+
+static void cw1200_pm_stay_awake_tmo(unsigned long arg)
+{
+}
+
+int cw1200_pm_init(struct cw1200_pm_state *pm,
+		   struct cw1200_common *priv)
+{
+	int ret = cw1200_pm_init_common(pm, priv);
+	if (!ret) {
+		init_timer(&pm->stay_awake);
+		pm->stay_awake.data = (unsigned long)pm;
+		pm->stay_awake.function = cw1200_pm_stay_awake_tmo;
+	}
+	return ret;
+}
+
+void cw1200_pm_deinit(struct cw1200_pm_state *pm)
+{
+	del_timer_sync(&pm->stay_awake);
+	cw1200_pm_deinit_common(pm);
+}
+
+void cw1200_pm_stay_awake(struct cw1200_pm_state *pm,
+			  unsigned long tmo)
+{
+	long cur_tmo;
+	spin_lock_bh(&pm->lock);
+	cur_tmo = pm->stay_awake.expires - jiffies;
+	if (!timer_pending(&pm->stay_awake) ||
+			cur_tmo < (long)tmo)
+		mod_timer(&pm->stay_awake, jiffies + tmo);
+	spin_unlock_bh(&pm->lock);
+}
+
+#endif /* CONFIG_WAKELOCK */
+
+static long cw1200_suspend_work(struct delayed_work *work)
+{
+	int ret = cancel_delayed_work(work);
+	long tmo;
+	if (ret > 0) {
+		/* Timer is pending */
+		tmo = work->timer.expires - jiffies;
+		if (tmo < 0)
+			tmo = 0;
+	} else {
+		tmo = -1;
+	}
+	return tmo;
+}
+
+static int cw1200_resume_work(struct cw1200_common *priv,
+			       struct delayed_work *work,
+			       unsigned long tmo)
+{
+	if ((long)tmo < 0)
+		return 1;
+
+	return queue_delayed_work(priv->workqueue, work, tmo);
+}
+
+static int cw1200_suspend_late(struct device *dev)
+{
+	struct cw1200_common *priv = dev->platform_data;
+	if (atomic_read(&priv->bh_rx)) {
+		wiphy_dbg(priv->hw->wiphy,
+			"%s: Suspend interrupted.\n",
+			__func__);
+		return -EAGAIN;
+	}
+	return 0;
+}
+
+static void cw1200_pm_release(struct device *dev)
+{
+}
+
+static int cw1200_pm_probe(struct platform_device *pdev)
+{
+	pdev->dev.release = cw1200_pm_release;
+	return 0;
+}
+
+int cw1200_wow_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
+{
+	struct cw1200_common *priv = hw->priv;
+	struct cw1200_pm_state *pm_state = &priv->pm_state;
+	struct cw1200_suspend_state *state;
+	int ret;
+
+#ifndef CONFIG_WAKELOCK
+	spin_lock_bh(&pm_state->lock);
+	ret = timer_pending(&pm_state->stay_awake);
+	spin_unlock_bh(&pm_state->lock);
+	if (ret)
+		return -EAGAIN;
+#endif
+
+	/* Do not suspend when datapath is not idle */
+	if (priv->tx_queue_stats.num_queued)
+		return -EBUSY;
+
+	/* Make sure there is no configuration requests in progress. */
+	if (!mutex_trylock(&priv->conf_mutex))
+		return -EBUSY;
+
+	/* Ensure pending operations are done.
+	 * Note also that wow_suspend must return in ~2.5sec, before
+	 * watchdog is triggered. */
+	if (priv->channel_switch_in_progress)
+		goto revert1;
+
+	/* Do not suspend when join work is scheduled */
+	if (work_pending(&priv->join_work))
+		goto revert1;
+
+	/* Do not suspend when scanning */
+	if (down_trylock(&priv->scan.lock))
+		goto revert1;
+
+	/* Lock TX. */
+	wsm_lock_tx_async(priv);
+
+	/* Wait to avoid possible race with bh code.
+	 * But do not wait too long... */
+	if (wait_event_timeout(priv->bh_evt_wq,
+			!priv->hw_bufs_used, HZ / 10) <= 0)
+		goto revert2;
+
+	/* Set UDP filter */
+	wsm_set_udp_port_filter(priv, &cw1200_udp_port_filter_on.hdr);
+
+	/* Set ethernet frame type filter */
+	wsm_set_ether_type_filter(priv, &cw1200_ether_type_filter_on.hdr);
+
+	/* Allocate state */
+	state = kzalloc(sizeof(struct cw1200_suspend_state), GFP_KERNEL);
+	if (!state)
+		goto revert3;
+
+	/* Store delayed work states. */
+	state->bss_loss_tmo =
+		cw1200_suspend_work(&priv->bss_loss_work);
+	state->connection_loss_tmo =
+		cw1200_suspend_work(&priv->connection_loss_work);
+	state->join_tmo =
+		cw1200_suspend_work(&priv->join_timeout);
+	state->direct_probe =
+		cw1200_suspend_work(&priv->scan.probe_work);
+	state->link_id_gc =
+		cw1200_suspend_work(&priv->link_id_gc_work);
+
+	/* Enable beacon skipping */
+	if (priv->join_status == CW1200_JOIN_STATUS_STA
+			&& priv->join_dtim_period
+			&& !priv->has_multicast_subscription) {
+		state->beacon_skipping = true;
+		wsm_set_beacon_wakeup_period(priv,
+				priv->join_dtim_period,
+				CW1200_BEACON_SKIPPING_MULTIPLIER *
+				 priv->join_dtim_period);
+	}
+
+	/* Stop serving thread */
+	if (cw1200_bh_suspend(priv))
+		goto revert4;
+
+	ret = timer_pending(&priv->mcast_timeout);
+	if (ret)
+		goto revert5;
+
+	/* Cancel block ack stat timer */
+	del_timer_sync(&priv->ba_timer);
+
+	/* Store suspend state */
+	pm_state->suspend_state = state;
+
+	/* Enable IRQ wake */
+	ret = priv->sbus_ops->power_mgmt(priv->sbus_priv, true);
+	if (ret) {
+		wiphy_err(priv->hw->wiphy,
+			"%s: PM request failed: %d. WoW is disabled.\n",
+			__func__, ret);
+		cw1200_wow_resume(hw);
+		return -EBUSY;
+	}
+
+	/* Force resume if event is coming from the device. */
+	if (atomic_read(&priv->bh_rx)) {
+		cw1200_wow_resume(hw);
+		return -EAGAIN;
+	}
+
+	return 0;
+
+revert5:
+	WARN_ON(cw1200_bh_resume(priv));
+revert4:
+	cw1200_resume_work(priv, &priv->bss_loss_work,
+			state->bss_loss_tmo);
+	cw1200_resume_work(priv, &priv->connection_loss_work,
+			state->connection_loss_tmo);
+	cw1200_resume_work(priv, &priv->join_timeout,
+			state->join_tmo);
+	cw1200_resume_work(priv, &priv->scan.probe_work,
+			state->direct_probe);
+	cw1200_resume_work(priv, &priv->link_id_gc_work,
+			state->link_id_gc);
+	kfree(state);
+revert3:
+	wsm_set_udp_port_filter(priv, &cw1200_udp_port_filter_off);
+	wsm_set_ether_type_filter(priv, &cw1200_ether_type_filter_off);
+revert2:
+	wsm_unlock_tx(priv);
+	up(&priv->scan.lock);
+revert1:
+	mutex_unlock(&priv->conf_mutex);
+	return -EBUSY;
+}
+
+int cw1200_wow_resume(struct ieee80211_hw *hw)
+{
+	struct cw1200_common *priv = hw->priv;
+	struct cw1200_pm_state *pm_state = &priv->pm_state;
+	struct cw1200_suspend_state *state;
+
+	state = pm_state->suspend_state;
+	pm_state->suspend_state = NULL;
+
+	/* Disable IRQ wake */
+	priv->sbus_ops->power_mgmt(priv->sbus_priv, false);
+
+	/* Resume BH thread */
+	WARN_ON(cw1200_bh_resume(priv));
+
+	if (state->beacon_skipping) {
+		wsm_set_beacon_wakeup_period(priv, priv->beacon_int *
+				priv->join_dtim_period >
+				MAX_BEACON_SKIP_TIME_MS ? 1 :
+				priv->join_dtim_period, 0);
+		state->beacon_skipping = false;
+	}
+
+	/* Resume delayed work */
+	cw1200_resume_work(priv, &priv->bss_loss_work,
+			state->bss_loss_tmo);
+	cw1200_resume_work(priv, &priv->connection_loss_work,
+			state->connection_loss_tmo);
+	cw1200_resume_work(priv, &priv->join_timeout,
+			state->join_tmo);
+	cw1200_resume_work(priv, &priv->scan.probe_work,
+			state->direct_probe);
+	cw1200_resume_work(priv, &priv->link_id_gc_work,
+			state->link_id_gc);
+
+	/* Restart block ack stat */
+	spin_lock_bh(&priv->ba_lock);
+	if (priv->ba_cnt)
+		mod_timer(&priv->ba_timer,
+			jiffies + CW1200_BLOCK_ACK_INTERVAL);
+	spin_unlock_bh(&priv->ba_lock);
+
+	/* Remove UDP port filter */
+	wsm_set_udp_port_filter(priv, &cw1200_udp_port_filter_off);
+
+	/* Remove ethernet frame type filter */
+	wsm_set_ether_type_filter(priv, &cw1200_ether_type_filter_off);
+
+	/* Unlock datapath */
+	wsm_unlock_tx(priv);
+
+	/* Unlock scan */
+	up(&priv->scan.lock);
+
+	/* Unlock configuration mutex */
+	mutex_unlock(&priv->conf_mutex);
+
+	/* Free memory */
+	kfree(state);
+
+	return 0;
+}
diff --git a/drivers/staging/cw1200/pm.h b/drivers/staging/cw1200/pm.h
new file mode 100644
index 00000000000..0515f6cfb92
--- /dev/null
+++ b/drivers/staging/cw1200/pm.h
@@ -0,0 +1,49 @@
+/*
+ * Mac80211 power management interface for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2011, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef PM_H_INCLUDED
+#define PM_H_INCLUDED
+
+#ifdef CONFIG_WAKELOCK
+#include <linux/wakelock.h>
+#endif
+
+/* ******************************************************************** */
+/* mac80211 API								*/
+
+#ifdef CONFIG_PM
+
+/* extern */  struct cw1200_common;
+/* private */ struct cw1200_suspend_state;
+
+struct cw1200_pm_state {
+	struct cw1200_suspend_state *suspend_state;
+#ifdef CONFIG_WAKELOCK
+	struct wake_lock wakelock;
+#else
+	struct timer_list stay_awake;
+#endif
+	struct platform_device *pm_dev;
+	spinlock_t lock;
+};
+
+int cw1200_pm_init(struct cw1200_pm_state *pm,
+		    struct cw1200_common *priv);
+void cw1200_pm_deinit(struct cw1200_pm_state *pm);
+void cw1200_pm_stay_awake(struct cw1200_pm_state *pm,
+			  unsigned long tmo);
+int cw1200_wow_suspend(struct ieee80211_hw *hw,
+		       struct cfg80211_wowlan *wowlan);
+int cw1200_wow_resume(struct ieee80211_hw *hw);
+
+#endif /* CONFIG_PM */
+
+#endif
diff --git a/drivers/staging/cw1200/queue.c b/drivers/staging/cw1200/queue.c
new file mode 100644
index 00000000000..014e6b2a8a4
--- /dev/null
+++ b/drivers/staging/cw1200/queue.c
@@ -0,0 +1,584 @@
+/*
+ * O(1) TX queue with built-in allocator for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <net/mac80211.h>
+#include <linux/sched.h>
+#include "queue.h"
+#include "cw1200.h"
+#include "debug.h"
+
+/* private */ struct cw1200_queue_item
+{
+	struct list_head	head;
+	struct sk_buff		*skb;
+	u32			packetID;
+	unsigned long		queue_timestamp;
+	unsigned long		xmit_timestamp;
+	struct cw1200_txpriv	txpriv;
+	u8			generation;
+};
+
+static inline void __cw1200_queue_lock(struct cw1200_queue *queue)
+{
+	struct cw1200_queue_stats *stats = queue->stats;
+	if (queue->tx_locked_cnt++ == 0) {
+		txrx_printk(KERN_DEBUG "[TX] Queue %d is locked.\n",
+				queue->queue_id);
+		ieee80211_stop_queue(stats->priv->hw, queue->queue_id);
+	}
+}
+
+static inline void __cw1200_queue_unlock(struct cw1200_queue *queue)
+{
+	struct cw1200_queue_stats *stats = queue->stats;
+	BUG_ON(!queue->tx_locked_cnt);
+	if (--queue->tx_locked_cnt == 0) {
+		txrx_printk(KERN_DEBUG "[TX] Queue %d is unlocked.\n",
+				queue->queue_id);
+		ieee80211_wake_queue(stats->priv->hw, queue->queue_id);
+	}
+}
+
+static inline void cw1200_queue_parse_id(u32 packetID, u8 *queue_generation,
+						u8 *queue_id,
+						u8 *item_generation,
+						u8 *item_id)
+{
+	*item_id		= (packetID >>  0) & 0xFF;
+	*item_generation	= (packetID >>  8) & 0xFF;
+	*queue_id		= (packetID >> 16) & 0xFF;
+	*queue_generation	= (packetID >> 24) & 0xFF;
+}
+
+static inline u32 cw1200_queue_make_packet_id(u8 queue_generation, u8 queue_id,
+						u8 item_generation, u8 item_id)
+{
+	return ((u32)item_id << 0) |
+		((u32)item_generation << 8) |
+		((u32)queue_id << 16) |
+		((u32)queue_generation << 24);
+}
+
+static void cw1200_queue_post_gc(struct cw1200_queue_stats *stats,
+				 struct list_head *gc_list)
+{
+	struct cw1200_queue_item *item;
+
+	while (!list_empty(gc_list)) {
+		item = list_first_entry(
+			gc_list, struct cw1200_queue_item, head);
+		list_del(&item->head);
+		stats->skb_dtor(stats->priv, item->skb, &item->txpriv);
+		kfree(item);
+	}
+}
+
+static void cw1200_queue_register_post_gc(struct list_head *gc_list,
+				     struct cw1200_queue_item *item)
+{
+	struct cw1200_queue_item *gc_item;
+	gc_item = kmalloc(sizeof(struct cw1200_queue_item),
+			GFP_ATOMIC);
+	BUG_ON(!gc_item);
+	memcpy(gc_item, item, sizeof(struct cw1200_queue_item));
+	list_add_tail(&gc_item->head, gc_list);
+}
+
+static void __cw1200_queue_gc(struct cw1200_queue *queue,
+			      struct list_head *head,
+			      bool unlock)
+{
+	struct cw1200_queue_stats *stats = queue->stats;
+	struct cw1200_queue_item *item = NULL;
+	bool wakeup_stats = false;
+
+	while (!list_empty(&queue->queue)) {
+		item = list_first_entry(
+			&queue->queue, struct cw1200_queue_item, head);
+		if (jiffies - item->queue_timestamp < queue->ttl)
+			break;
+		--queue->num_queued;
+		--queue->link_map_cache[item->txpriv.link_id];
+		spin_lock_bh(&stats->lock);
+		--stats->num_queued;
+		if (!--stats->link_map_cache[item->txpriv.link_id])
+			wakeup_stats = true;
+		spin_unlock_bh(&stats->lock);
+		cw1200_debug_tx_ttl(stats->priv);
+		cw1200_queue_register_post_gc(head, item);
+		item->skb = NULL;
+		list_move_tail(&item->head, &queue->free_pool);
+	}
+
+	if (wakeup_stats)
+		wake_up(&stats->wait_link_id_empty);
+
+	if (queue->overfull) {
+		if (queue->num_queued <= (queue->capacity >> 1)) {
+			queue->overfull = false;
+			if (unlock)
+				__cw1200_queue_unlock(queue);
+		} else {
+			unsigned long tmo = item->queue_timestamp + queue->ttl;
+			mod_timer(&queue->gc, tmo);
+			cw1200_pm_stay_awake(&stats->priv->pm_state,
+					tmo - jiffies);
+		}
+	}
+}
+
+static void cw1200_queue_gc(unsigned long arg)
+{
+	LIST_HEAD(list);
+	struct cw1200_queue *queue =
+		(struct cw1200_queue *)arg;
+
+	spin_lock_bh(&queue->lock);
+	__cw1200_queue_gc(queue, &list, true);
+	spin_unlock_bh(&queue->lock);
+	cw1200_queue_post_gc(queue->stats, &list);
+}
+
+int cw1200_queue_stats_init(struct cw1200_queue_stats *stats,
+			    size_t map_capacity,
+			    cw1200_queue_skb_dtor_t skb_dtor,
+			    struct cw1200_common *priv)
+{
+	memset(stats, 0, sizeof(*stats));
+	stats->map_capacity = map_capacity;
+	stats->skb_dtor = skb_dtor;
+	stats->priv = priv;
+	spin_lock_init(&stats->lock);
+	init_waitqueue_head(&stats->wait_link_id_empty);
+
+	stats->link_map_cache = kzalloc(sizeof(int[map_capacity]),
+			GFP_KERNEL);
+	if (!stats->link_map_cache)
+		return -ENOMEM;
+
+	return 0;
+}
+
+int cw1200_queue_init(struct cw1200_queue *queue,
+		      struct cw1200_queue_stats *stats,
+		      u8 queue_id,
+		      size_t capacity,
+		      unsigned long ttl)
+{
+	size_t i;
+
+	memset(queue, 0, sizeof(*queue));
+	queue->stats = stats;
+	queue->capacity = capacity;
+	queue->queue_id = queue_id;
+	queue->ttl = ttl;
+	INIT_LIST_HEAD(&queue->queue);
+	INIT_LIST_HEAD(&queue->pending);
+	INIT_LIST_HEAD(&queue->free_pool);
+	spin_lock_init(&queue->lock);
+	init_timer(&queue->gc);
+	queue->gc.data = (unsigned long)queue;
+	queue->gc.function = cw1200_queue_gc;
+
+	queue->pool = kzalloc(sizeof(struct cw1200_queue_item) * capacity,
+			GFP_KERNEL);
+	if (!queue->pool)
+		return -ENOMEM;
+
+	queue->link_map_cache = kzalloc(sizeof(int[stats->map_capacity]),
+			GFP_KERNEL);
+	if (!queue->link_map_cache) {
+		kfree(queue->pool);
+		queue->pool = NULL;
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < capacity; ++i)
+		list_add_tail(&queue->pool[i].head, &queue->free_pool);
+
+	return 0;
+}
+
+int cw1200_queue_clear(struct cw1200_queue *queue)
+{
+	int i;
+	LIST_HEAD(gc_list);
+	struct cw1200_queue_stats *stats = queue->stats;
+
+	spin_lock_bh(&queue->lock);
+	queue->generation++;
+	list_splice_tail_init(&queue->queue, &queue->pending);
+	while (!list_empty(&queue->pending)) {
+		struct cw1200_queue_item *item = list_first_entry(
+			&queue->pending, struct cw1200_queue_item, head);
+		WARN_ON(!item->skb);
+		cw1200_queue_register_post_gc(&gc_list, item);
+		item->skb = NULL;
+		list_move_tail(&item->head, &queue->free_pool);
+	}
+	queue->num_queued = 0;
+	queue->num_pending = 0;
+
+	spin_lock_bh(&stats->lock);
+	for (i = 0; i < stats->map_capacity; ++i) {
+		stats->num_queued -= queue->link_map_cache[i];
+		stats->link_map_cache[i] -= queue->link_map_cache[i];
+		queue->link_map_cache[i] = 0;
+	}
+	spin_unlock_bh(&stats->lock);
+	if (unlikely(queue->overfull)) {
+		queue->overfull = false;
+		__cw1200_queue_unlock(queue);
+	}
+	spin_unlock_bh(&queue->lock);
+	wake_up(&stats->wait_link_id_empty);
+	cw1200_queue_post_gc(stats, &gc_list);
+	return 0;
+}
+
+void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats)
+{
+	kfree(stats->link_map_cache);
+	stats->link_map_cache = NULL;
+}
+
+void cw1200_queue_deinit(struct cw1200_queue *queue)
+{
+	cw1200_queue_clear(queue);
+	del_timer_sync(&queue->gc);
+	INIT_LIST_HEAD(&queue->free_pool);
+	kfree(queue->pool);
+	kfree(queue->link_map_cache);
+	queue->pool = NULL;
+	queue->link_map_cache = NULL;
+	queue->capacity = 0;
+}
+
+size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
+				   u32 link_id_map)
+{
+	size_t ret;
+	int i, bit;
+	size_t map_capacity = queue->stats->map_capacity;
+
+	if (!link_id_map)
+		return 0;
+
+	spin_lock_bh(&queue->lock);
+	if (likely(link_id_map == (u32) -1))
+		ret = queue->num_queued - queue->num_pending;
+	else {
+		ret = 0;
+		for (i = 0, bit = 1; i < map_capacity; ++i, bit <<= 1) {
+			if (link_id_map & bit)
+				ret += queue->link_map_cache[i];
+		}
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+int cw1200_queue_put(struct cw1200_queue *queue,
+		     struct sk_buff *skb,
+		     struct cw1200_txpriv *txpriv)
+{
+	int ret = 0;
+	LIST_HEAD(gc_list);
+	struct cw1200_queue_stats *stats = queue->stats;
+
+	if (txpriv->link_id >= queue->stats->map_capacity)
+		return -EINVAL;
+
+	spin_lock_bh(&queue->lock);
+	if (!WARN_ON(list_empty(&queue->free_pool))) {
+		struct cw1200_queue_item *item = list_first_entry(
+			&queue->free_pool, struct cw1200_queue_item, head);
+		BUG_ON(item->skb);
+
+		list_move_tail(&item->head, &queue->queue);
+		item->skb = skb;
+		item->txpriv = *txpriv;
+		item->generation = 0;
+		item->packetID = cw1200_queue_make_packet_id(
+			queue->generation, queue->queue_id,
+			item->generation, item - queue->pool);
+		item->queue_timestamp = jiffies;
+
+		++queue->num_queued;
+		++queue->link_map_cache[txpriv->link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued;
+		++stats->link_map_cache[txpriv->link_id];
+		spin_unlock_bh(&stats->lock);
+
+		/*
+		 * TX may happen in parallel sometimes.
+		 * Leave extra queue slots so we don't overflow.
+		 */
+		if (queue->overfull == false &&
+				queue->num_queued >=
+				(queue->capacity - (num_present_cpus() - 1))) {
+			queue->overfull = true;
+			__cw1200_queue_lock(queue);
+			mod_timer(&queue->gc, jiffies);
+		}
+	} else {
+		ret = -ENOENT;
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+int cw1200_queue_get(struct cw1200_queue *queue,
+		     u32 link_id_map,
+		     struct wsm_tx **tx,
+		     struct ieee80211_tx_info **tx_info,
+		     const struct cw1200_txpriv **txpriv)
+{
+	int ret = -ENOENT;
+	struct cw1200_queue_item *item;
+	struct cw1200_queue_stats *stats = queue->stats;
+	bool wakeup_stats = false;
+
+	spin_lock_bh(&queue->lock);
+	list_for_each_entry(item, &queue->queue, head) {
+		if (link_id_map & BIT(item->txpriv.link_id)) {
+			ret = 0;
+			break;
+		}
+	}
+
+	if (!WARN_ON(ret)) {
+		*tx = (struct wsm_tx *)item->skb->data;
+		*tx_info = IEEE80211_SKB_CB(item->skb);
+		*txpriv = &item->txpriv;
+		(*tx)->packetID = __cpu_to_le32(item->packetID);
+		list_move_tail(&item->head, &queue->pending);
+		++queue->num_pending;
+		--queue->link_map_cache[item->txpriv.link_id];
+		item->xmit_timestamp = jiffies;
+
+		spin_lock_bh(&stats->lock);
+		--stats->num_queued;
+		if (!--stats->link_map_cache[item->txpriv.link_id])
+			wakeup_stats = true;
+		spin_unlock_bh(&stats->lock);
+	}
+	spin_unlock_bh(&queue->lock);
+	if (wakeup_stats)
+		wake_up(&stats->wait_link_id_empty);
+	return ret;
+}
+
+int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packetID)
+{
+	int ret = 0;
+	u8 queue_generation, queue_id, item_generation, item_id;
+	struct cw1200_queue_item *item;
+	struct cw1200_queue_stats *stats = queue->stats;
+
+	cw1200_queue_parse_id(packetID, &queue_generation, &queue_id,
+				&item_generation, &item_id);
+
+	item = &queue->pool[item_id];
+
+	spin_lock_bh(&queue->lock);
+	BUG_ON(queue_id != queue->queue_id);
+	if (unlikely(queue_generation != queue->generation)) {
+		ret = -ENOENT;
+	} else if (unlikely(item_id >= (unsigned) queue->capacity)) {
+		WARN_ON(1);
+		ret = -EINVAL;
+	} else if (unlikely(item->generation != item_generation)) {
+		WARN_ON(1);
+		ret = -ENOENT;
+	} else {
+		--queue->num_pending;
+		++queue->link_map_cache[item->txpriv.link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued;
+		++stats->link_map_cache[item->txpriv.link_id];
+		spin_unlock_bh(&stats->lock);
+
+		item->generation = ++item_generation;
+		item->packetID = cw1200_queue_make_packet_id(
+			queue_generation, queue_id, item_generation, item_id);
+		list_move(&item->head, &queue->queue);
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+int cw1200_queue_requeue_all(struct cw1200_queue *queue)
+{
+	struct cw1200_queue_stats *stats = queue->stats;
+	spin_lock_bh(&queue->lock);
+	while (!list_empty(&queue->pending)) {
+		struct cw1200_queue_item *item = list_entry(
+			queue->pending.prev, struct cw1200_queue_item, head);
+
+		--queue->num_pending;
+		++queue->link_map_cache[item->txpriv.link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued;
+		++stats->link_map_cache[item->txpriv.link_id];
+		spin_unlock_bh(&stats->lock);
+
+		++item->generation;
+		item->packetID = cw1200_queue_make_packet_id(
+			queue->generation, queue->queue_id,
+			item->generation, item - queue->pool);
+		list_move(&item->head, &queue->queue);
+	}
+	spin_unlock_bh(&queue->lock);
+
+	return 0;
+}
+
+int cw1200_queue_remove(struct cw1200_queue *queue, u32 packetID)
+{
+	int ret = 0;
+	u8 queue_generation, queue_id, item_generation, item_id;
+	struct cw1200_queue_item *item;
+	struct cw1200_queue_stats *stats = queue->stats;
+	struct sk_buff *gc_skb = NULL;
+	struct cw1200_txpriv gc_txpriv;
+
+	cw1200_queue_parse_id(packetID, &queue_generation, &queue_id,
+				&item_generation, &item_id);
+
+	item = &queue->pool[item_id];
+
+	spin_lock_bh(&queue->lock);
+	BUG_ON(queue_id != queue->queue_id);
+	if (unlikely(queue_generation != queue->generation)) {
+		ret = -ENOENT;
+	} else if (unlikely(item_id >= (unsigned) queue->capacity)) {
+		WARN_ON(1);
+		ret = -EINVAL;
+	} else if (unlikely(item->generation != item_generation)) {
+		WARN_ON(1);
+		ret = -ENOENT;
+	} else {
+		gc_txpriv = item->txpriv;
+		gc_skb = item->skb;
+		item->skb = NULL;
+		--queue->num_pending;
+		--queue->num_queued;
+		++queue->num_sent;
+		++item->generation;
+		/* Do not use list_move_tail here, but list_move:
+		 * try to utilize cache row.
+		 */
+		list_move(&item->head, &queue->free_pool);
+
+		if (unlikely(queue->overfull) &&
+		    (queue->num_queued <= (queue->capacity >> 1))) {
+			queue->overfull = false;
+			__cw1200_queue_unlock(queue);
+		}
+	}
+	spin_unlock_bh(&queue->lock);
+
+	if (gc_skb)
+		stats->skb_dtor(stats->priv, gc_skb, &gc_txpriv);
+
+	return ret;
+}
+
+int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packetID,
+			 struct sk_buff **skb,
+			 const struct cw1200_txpriv **txpriv)
+{
+	int ret = 0;
+	u8 queue_generation, queue_id, item_generation, item_id;
+	struct cw1200_queue_item *item;
+	cw1200_queue_parse_id(packetID, &queue_generation, &queue_id,
+				&item_generation, &item_id);
+
+	item = &queue->pool[item_id];
+
+	spin_lock_bh(&queue->lock);
+	BUG_ON(queue_id != queue->queue_id);
+	if (unlikely(queue_generation != queue->generation)) {
+		ret = -ENOENT;
+	} else if (unlikely(item_id >= (unsigned) queue->capacity)) {
+		WARN_ON(1);
+		ret = -EINVAL;
+	} else if (unlikely(item->generation != item_generation)) {
+		WARN_ON(1);
+		ret = -ENOENT;
+	} else {
+		*skb = item->skb;
+		*txpriv = &item->txpriv;
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+void cw1200_queue_lock(struct cw1200_queue *queue)
+{
+	spin_lock_bh(&queue->lock);
+	__cw1200_queue_lock(queue);
+	spin_unlock_bh(&queue->lock);
+}
+
+void cw1200_queue_unlock(struct cw1200_queue *queue)
+{
+	spin_lock_bh(&queue->lock);
+	__cw1200_queue_unlock(queue);
+	spin_unlock_bh(&queue->lock);
+}
+
+bool cw1200_queue_get_xmit_timestamp(struct cw1200_queue *queue,
+				     unsigned long *timestamp)
+{
+	struct cw1200_queue_item *item;
+	bool ret;
+
+	spin_lock_bh(&queue->lock);
+	ret = !list_empty(&queue->pending);
+	if (ret) {
+		list_for_each_entry(item, &queue->pending, head) {
+			if (time_before(item->xmit_timestamp, *timestamp))
+				*timestamp = item->xmit_timestamp;
+		}
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
+				 u32 link_id_map)
+{
+	bool empty = true;
+
+	spin_lock_bh(&stats->lock);
+	if (link_id_map == (u32)-1)
+		empty = stats->num_queued == 0;
+	else {
+		int i;
+		for (i = 0; i < stats->map_capacity; ++i) {
+			if (link_id_map & BIT(i)) {
+				if (stats->link_map_cache[i]) {
+					empty = false;
+					break;
+				}
+			}
+		}
+	}
+	spin_unlock_bh(&stats->lock);
+
+	return empty;
+}
diff --git a/drivers/staging/cw1200/queue.h b/drivers/staging/cw1200/queue.h
new file mode 100644
index 00000000000..aa9a7f9444c
--- /dev/null
+++ b/drivers/staging/cw1200/queue.h
@@ -0,0 +1,116 @@
+/*
+ * O(1) TX queue with built-in allocator for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_QUEUE_H_INCLUDED
+#define CW1200_QUEUE_H_INCLUDED
+
+/* private */ struct cw1200_queue_item;
+
+/* extern */ struct sk_buff;
+/* extern */ struct wsm_tx;
+/* extern */ struct cw1200_common;
+/* extern */ struct ieee80211_tx_queue_stats;
+/* extern */ struct cw1200_txpriv;
+
+/* forward */ struct cw1200_queue_stats;
+
+typedef void (*cw1200_queue_skb_dtor_t)(struct cw1200_common *priv,
+					struct sk_buff *skb,
+					const struct cw1200_txpriv *txpriv);
+
+struct cw1200_queue {
+	struct cw1200_queue_stats *stats;
+	size_t			capacity;
+	size_t			num_queued;
+	size_t			num_pending;
+	size_t			num_sent;
+	struct cw1200_queue_item *pool;
+	struct list_head	queue;
+	struct list_head	free_pool;
+	struct list_head	pending;
+	int			tx_locked_cnt;
+	int			*link_map_cache;
+	bool			overfull;
+	spinlock_t		lock;
+	u8			queue_id;
+	u8			generation;
+	struct timer_list	gc;
+	unsigned long		ttl;
+};
+
+struct cw1200_queue_stats {
+	spinlock_t		lock;
+	int			*link_map_cache;
+	int			num_queued;
+	size_t			map_capacity;
+	wait_queue_head_t	wait_link_id_empty;
+	cw1200_queue_skb_dtor_t	skb_dtor;
+	struct cw1200_common	*priv;
+};
+
+struct cw1200_txpriv {
+	u8 link_id;
+	u8 raw_link_id;
+	u8 tid;
+	u8 rate_id;
+	u8 offset;
+};
+
+int cw1200_queue_stats_init(struct cw1200_queue_stats *stats,
+			    size_t map_capacity,
+			    cw1200_queue_skb_dtor_t skb_dtor,
+			    struct cw1200_common *priv);
+int cw1200_queue_init(struct cw1200_queue *queue,
+		      struct cw1200_queue_stats *stats,
+		      u8 queue_id,
+		      size_t capacity,
+		      unsigned long ttl);
+int cw1200_queue_clear(struct cw1200_queue *queue);
+void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats);
+void cw1200_queue_deinit(struct cw1200_queue *queue);
+
+size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
+				   u32 link_id_map);
+int cw1200_queue_put(struct cw1200_queue *queue,
+		     struct sk_buff *skb,
+		     struct cw1200_txpriv *txpriv);
+int cw1200_queue_get(struct cw1200_queue *queue,
+		     u32 link_id_map,
+		     struct wsm_tx **tx,
+		     struct ieee80211_tx_info **tx_info,
+		     const struct cw1200_txpriv **txpriv);
+int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packetID);
+int cw1200_queue_requeue_all(struct cw1200_queue *queue);
+int cw1200_queue_remove(struct cw1200_queue *queue,
+			u32 packetID);
+int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packetID,
+			 struct sk_buff **skb,
+			 const struct cw1200_txpriv **txpriv);
+void cw1200_queue_lock(struct cw1200_queue *queue);
+void cw1200_queue_unlock(struct cw1200_queue *queue);
+bool cw1200_queue_get_xmit_timestamp(struct cw1200_queue *queue,
+				     unsigned long *timestamp);
+
+
+bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
+				 u32 link_id_map);
+
+static inline u8 cw1200_queue_get_queue_id(u32 packetID)
+{
+	return (packetID >> 16) & 0xFF;
+}
+
+static inline u8 cw1200_queue_get_generation(u32 packetID)
+{
+	return (packetID >>  8) & 0xFF;
+}
+
+#endif /* CW1200_QUEUE_H_INCLUDED */
diff --git a/drivers/staging/cw1200/sbus.h b/drivers/staging/cw1200/sbus.h
new file mode 100644
index 00000000000..49bd06d20e5
--- /dev/null
+++ b/drivers/staging/cw1200/sbus.h
@@ -0,0 +1,39 @@
+/*
+ * Common sbus abstraction layer interface for cw1200 wireless driver
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_SBUS_H
+#define CW1200_SBUS_H
+
+/*
+ * sbus priv forward definition.
+ * Implemented and instantiated in particular modules.
+ */
+struct sbus_priv;
+
+typedef void (*sbus_irq_handler)(void *priv);
+
+struct sbus_ops {
+	int (*sbus_memcpy_fromio)(struct sbus_priv *self, unsigned int addr,
+					void *dst, int count);
+	int (*sbus_memcpy_toio)(struct sbus_priv *self, unsigned int addr,
+					const void *src, int count);
+	void (*lock)(struct sbus_priv *self);
+	void (*unlock)(struct sbus_priv *self);
+	int (*irq_subscribe)(struct sbus_priv *self, sbus_irq_handler handler,
+				void *priv);
+	int (*irq_unsubscribe)(struct sbus_priv *self);
+	int (*reset)(struct sbus_priv *self);
+	size_t (*align_size)(struct sbus_priv *self, size_t size);
+	int (*power_mgmt)(struct sbus_priv *self, bool suspend);
+	int (*set_block_size)(struct sbus_priv *self, size_t size);
+};
+
+#endif /* CW1200_SBUS_H */
diff --git a/drivers/staging/cw1200/scan.c b/drivers/staging/cw1200/scan.c
new file mode 100644
index 00000000000..b12af9ded62
--- /dev/null
+++ b/drivers/staging/cw1200/scan.c
@@ -0,0 +1,446 @@
+/*
+ * Scan implementation for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/sched.h>
+#include "cw1200.h"
+#include "scan.h"
+#include "sta.h"
+#include "pm.h"
+
+static void cw1200_scan_restart_delayed(struct cw1200_common *priv);
+
+static int cw1200_scan_start(struct cw1200_common *priv, struct wsm_scan *scan)
+{
+	int ret, i;
+	int tmo = 2000;
+
+	for (i = 0; i < scan->numOfChannels; ++i)
+		tmo += scan->ch[i].maxChannelTime + 10;
+
+	atomic_set(&priv->scan.in_progress, 1);
+	cw1200_pm_stay_awake(&priv->pm_state, tmo * HZ / 1000);
+	queue_delayed_work(priv->workqueue, &priv->scan.timeout,
+		tmo * HZ / 1000);
+	ret = wsm_scan(priv, scan);
+	if (unlikely(ret)) {
+		atomic_set(&priv->scan.in_progress, 0);
+		cancel_delayed_work_sync(&priv->scan.timeout);
+		cw1200_scan_restart_delayed(priv);
+	}
+	return ret;
+}
+
+int cw1200_hw_scan(struct ieee80211_hw *hw,
+		   struct ieee80211_vif *vif,
+		   struct cfg80211_scan_request *req)
+{
+	struct cw1200_common *priv = hw->priv;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PROBE_REQUEST,
+	};
+	int i;
+
+	if (!priv->vif)
+		return -EINVAL;
+
+	/* Scan when P2P_GO corrupt firmware MiniAP mode */
+	if (priv->join_status == CW1200_JOIN_STATUS_AP)
+		return -EOPNOTSUPP;
+
+	if (req->n_ssids == 1 && !req->ssids[0].ssid_len)
+		req->n_ssids = 0;
+
+	wiphy_dbg(hw->wiphy, "[SCAN] Scan request for %d SSIDs.\n",
+		req->n_ssids);
+
+	if (req->n_ssids > WSM_SCAN_MAX_NUM_OF_SSIDS)
+		return -EINVAL;
+
+	frame.skb = ieee80211_probereq_get(hw, priv->vif, NULL, 0,
+		req->ie, req->ie_len);
+	if (!frame.skb)
+		return -ENOMEM;
+
+	/* will be unlocked in cw1200_scan_work() */
+	down(&priv->scan.lock);
+	mutex_lock(&priv->conf_mutex);
+	if (frame.skb) {
+		int ret = wsm_set_template_frame(priv, &frame);
+		if (0 == ret) {
+			/*
+			* set empty probe response template in order
+			* to receive probe requests from firmware
+			*/
+			frame.frame_type = WSM_FRAME_TYPE_PROBE_RESPONSE;
+			frame.disable = true;
+			ret = wsm_set_template_frame(priv, &frame);
+		}
+		if (ret) {
+			mutex_unlock(&priv->conf_mutex);
+			up(&priv->scan.lock);
+			dev_kfree_skb(frame.skb);
+			return ret;
+		}
+	}
+
+	wsm_lock_tx(priv);
+
+	BUG_ON(priv->scan.req);
+	priv->scan.req = req;
+	priv->scan.n_ssids = 0;
+	priv->scan.status = 0;
+	priv->scan.begin = &req->channels[0];
+	priv->scan.curr = priv->scan.begin;
+	priv->scan.end = &req->channels[req->n_channels];
+	priv->scan.output_power = priv->output_power;
+
+	for (i = 0; i < req->n_ssids; ++i) {
+		struct wsm_ssid *dst =
+			&priv->scan.ssids[priv->scan.n_ssids];
+		BUG_ON(req->ssids[i].ssid_len > sizeof(dst->ssid));
+		memcpy(&dst->ssid[0], req->ssids[i].ssid,
+			sizeof(dst->ssid));
+		dst->length = req->ssids[i].ssid_len;
+		++priv->scan.n_ssids;
+	}
+
+	mutex_unlock(&priv->conf_mutex);
+
+	if (frame.skb)
+		dev_kfree_skb(frame.skb);
+	queue_work(priv->workqueue, &priv->scan.work);
+	return 0;
+}
+
+void cw1200_scan_work(struct work_struct *work)
+{
+	struct cw1200_common *priv = container_of(work, struct cw1200_common,
+							scan.work);
+	struct ieee80211_channel **it;
+	struct wsm_scan scan = {
+		.scanType = WSM_SCAN_TYPE_FOREGROUND,
+		.scanFlags = WSM_SCAN_FLAG_SPLIT_METHOD,
+	};
+	bool first_run = priv->scan.begin == priv->scan.curr &&
+			priv->scan.begin != priv->scan.end;
+	int i;
+
+	if (first_run) {
+		/* Firmware gets crazy if scan request is sent
+		 * when STA is joined but not yet associated.
+		 * Force unjoin in this case. */
+		if (cancel_delayed_work_sync(&priv->join_timeout) > 0)
+			cw1200_join_timeout(&priv->join_timeout.work);
+	}
+
+	mutex_lock(&priv->conf_mutex);
+
+	if (first_run) {
+		if (priv->join_status == CW1200_JOIN_STATUS_STA &&
+				!(priv->powersave_mode.pmMode & WSM_PSM_PS)) {
+			struct wsm_set_pm pm = priv->powersave_mode;
+			pm.pmMode = WSM_PSM_PS;
+			cw1200_set_pm(priv, &pm);
+		} else if (priv->join_status == CW1200_JOIN_STATUS_MONITOR) {
+			/* FW bug: driver has to restart p2p-dev mode
+			 * after scan */
+			cw1200_disable_listening(priv);
+		}
+	}
+
+	if (!priv->scan.req || (priv->scan.curr == priv->scan.end)) {
+		if (priv->scan.output_power != priv->output_power)
+			WARN_ON(wsm_set_output_power(priv,
+						priv->output_power * 10));
+		if (priv->join_status == CW1200_JOIN_STATUS_STA &&
+				!(priv->powersave_mode.pmMode & WSM_PSM_PS))
+			cw1200_set_pm(priv, &priv->powersave_mode);
+
+		if (priv->scan.status < 0)
+			wiphy_dbg(priv->hw->wiphy,
+					"[SCAN] Scan failed (%d).\n",
+					priv->scan.status);
+		else if (priv->scan.req)
+			wiphy_dbg(priv->hw->wiphy,
+					"[SCAN] Scan completed.\n");
+		else
+			wiphy_dbg(priv->hw->wiphy,
+					"[SCAN] Scan canceled.\n");
+
+		priv->scan.req = NULL;
+		cw1200_scan_restart_delayed(priv);
+		wsm_unlock_tx(priv);
+		mutex_unlock(&priv->conf_mutex);
+		ieee80211_scan_completed(priv->hw, priv->scan.status ? 1 : 0);
+		up(&priv->scan.lock);
+		return;
+	} else {
+		struct ieee80211_channel *first = *priv->scan.curr;
+		for (it = priv->scan.curr + 1, i = 1;
+		     it != priv->scan.end && i < WSM_SCAN_MAX_NUM_OF_CHANNELS;
+		     ++it, ++i) {
+			if ((*it)->band != first->band)
+				break;
+			if (((*it)->flags ^ first->flags) &
+					IEEE80211_CHAN_PASSIVE_SCAN)
+				break;
+			if (!(first->flags & IEEE80211_CHAN_PASSIVE_SCAN) &&
+			    (*it)->max_power != first->max_power)
+				break;
+		}
+		scan.band = first->band;
+
+		if (priv->scan.req->no_cck)
+			scan.maxTransmitRate = WSM_TRANSMIT_RATE_6;
+		else
+			scan.maxTransmitRate = WSM_TRANSMIT_RATE_1;
+		/* TODO: Is it optimal? */
+		scan.numOfProbeRequests =
+			(first->flags & IEEE80211_CHAN_PASSIVE_SCAN) ? 0 : 2;
+		scan.numOfSSIDs = priv->scan.n_ssids;
+		scan.ssids = &priv->scan.ssids[0];
+		scan.numOfChannels = it - priv->scan.curr;
+		/* TODO: Is it optimal? */
+		scan.probeDelay = 100;
+		/* It is not stated in WSM specification, however
+		 * FW team says that driver may not use FG scan
+		 * when joined. */
+		if (priv->join_status == CW1200_JOIN_STATUS_STA) {
+			scan.scanType = WSM_SCAN_TYPE_BACKGROUND;
+			scan.scanFlags = WSM_SCAN_FLAG_FORCE_BACKGROUND;
+		}
+		scan.ch = kzalloc(
+			sizeof(struct wsm_scan_ch[it - priv->scan.curr]),
+			GFP_KERNEL);
+		if (!scan.ch) {
+			priv->scan.status = -ENOMEM;
+			goto fail;
+		}
+		for (i = 0; i < scan.numOfChannels; ++i) {
+			scan.ch[i].number = priv->scan.curr[i]->hw_value;
+			scan.ch[i].minChannelTime = 50;
+			scan.ch[i].maxChannelTime = 110;
+		}
+		if (!(first->flags & IEEE80211_CHAN_PASSIVE_SCAN) &&
+		    priv->scan.output_power != first->max_power) {
+			priv->scan.output_power = first->max_power;
+			WARN_ON(wsm_set_output_power(priv,
+						priv->scan.output_power * 10));
+		}
+		priv->scan.status = cw1200_scan_start(priv, &scan);
+		kfree(scan.ch);
+		if (priv->scan.status)
+			goto fail;
+		priv->scan.curr = it;
+	}
+	mutex_unlock(&priv->conf_mutex);
+	return;
+
+fail:
+	priv->scan.curr = priv->scan.end;
+	mutex_unlock(&priv->conf_mutex);
+	queue_work(priv->workqueue, &priv->scan.work);
+	return;
+}
+
+static void cw1200_scan_restart_delayed(struct cw1200_common *priv)
+{
+	if (priv->delayed_link_loss) {
+		int tmo = priv->cqm_beacon_loss_count;
+
+		if (priv->scan.direct_probe)
+			tmo = 0;
+
+		priv->delayed_link_loss = 0;
+		/* Restart beacon loss timer and requeue
+		   BSS loss work. */
+		wiphy_dbg(priv->hw->wiphy,
+				"[CQM] Requeue BSS loss in %d "
+				"beacons.\n", tmo);
+		spin_lock(&priv->bss_loss_lock);
+		priv->bss_loss_status = CW1200_BSS_LOSS_NONE;
+		spin_unlock(&priv->bss_loss_lock);
+		cancel_delayed_work_sync(&priv->bss_loss_work);
+		queue_delayed_work(priv->workqueue,
+				&priv->bss_loss_work,
+				tmo * HZ / 10);
+	}
+
+	/* FW bug: driver has to restart p2p-dev mode after scan. */
+	if (priv->join_status == CW1200_JOIN_STATUS_MONITOR) {
+		cw1200_enable_listening(priv);
+		cw1200_update_filtering(priv);
+	}
+
+	if (priv->delayed_unjoin) {
+		priv->delayed_unjoin = false;
+		if (queue_work(priv->workqueue, &priv->unjoin_work) <= 0)
+			wsm_unlock_tx(priv);
+	}
+}
+
+static void cw1200_scan_complete(struct cw1200_common *priv)
+{
+	if (priv->scan.direct_probe) {
+		wiphy_dbg(priv->hw->wiphy, "[SCAN] Direct probe complete.\n");
+		cw1200_scan_restart_delayed(priv);
+		priv->scan.direct_probe = 0;
+		up(&priv->scan.lock);
+		wsm_unlock_tx(priv);
+	} else {
+		cw1200_scan_work(&priv->scan.work);
+	}
+}
+
+void cw1200_scan_complete_cb(struct cw1200_common *priv,
+				struct wsm_scan_complete *arg)
+{
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED))
+		/* STA is stopped. */
+		return;
+
+	if (cancel_delayed_work_sync(&priv->scan.timeout) > 0) {
+		priv->scan.status = 1;
+		queue_delayed_work(priv->workqueue,
+				&priv->scan.timeout, 0);
+	}
+}
+
+void cw1200_scan_timeout(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, scan.timeout.work);
+	if (likely(atomic_xchg(&priv->scan.in_progress, 0))) {
+		if (priv->scan.status > 0)
+			priv->scan.status = 0;
+		else if (!priv->scan.status) {
+			wiphy_warn(priv->hw->wiphy,
+				"Timeout waiting for scan "
+				"complete notification.\n");
+			priv->scan.status = -ETIMEDOUT;
+			priv->scan.curr = priv->scan.end;
+			WARN_ON(wsm_stop_scan(priv));
+		}
+		cw1200_scan_complete(priv);
+	}
+}
+
+void cw1200_probe_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, scan.probe_work.work);
+	u8 queueId = cw1200_queue_get_queue_id(priv->pending_frame_id);
+	struct cw1200_queue *queue = &priv->tx_queue[queueId];
+	const struct cw1200_txpriv *txpriv;
+	struct wsm_tx *wsm;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PROBE_REQUEST,
+	};
+	struct wsm_ssid ssids[1] = {{
+		.length = 0,
+	} };
+	struct wsm_scan_ch ch[1] = {{
+		.minChannelTime = 0,
+		.maxChannelTime = 10,
+	} };
+	struct wsm_scan scan = {
+		.scanType = WSM_SCAN_TYPE_FOREGROUND,
+		.numOfProbeRequests = 1,
+		.probeDelay = 0,
+		.numOfChannels = 1,
+		.ssids = ssids,
+		.ch = ch,
+	};
+	u8 *ies;
+	size_t ies_len;
+	int ret;
+
+	wiphy_dbg(priv->hw->wiphy, "[SCAN] Direct probe work.\n");
+
+	BUG_ON(queueId >= 4);
+	BUG_ON(!priv->channel);
+
+	mutex_lock(&priv->conf_mutex);
+	if (unlikely(down_trylock(&priv->scan.lock))) {
+		/* Scan is already in progress. Requeue self. */
+		schedule();
+		queue_delayed_work(priv->workqueue,
+					&priv->scan.probe_work, HZ / 10);
+		mutex_unlock(&priv->conf_mutex);
+		return;
+	}
+
+	if (cw1200_queue_get_skb(queue,	priv->pending_frame_id,
+			&frame.skb, &txpriv)) {
+		up(&priv->scan.lock);
+		mutex_unlock(&priv->conf_mutex);
+		wsm_unlock_tx(priv);
+		return;
+	}
+	wsm = (struct wsm_tx *)frame.skb->data;
+	scan.maxTransmitRate = wsm->maxTxRate;
+	scan.band = (priv->channel->band == IEEE80211_BAND_5GHZ) ?
+		WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G;
+	if (priv->join_status == CW1200_JOIN_STATUS_STA) {
+		scan.scanType = WSM_SCAN_TYPE_BACKGROUND;
+		scan.scanFlags = WSM_SCAN_FLAG_FORCE_BACKGROUND;
+	}
+	ch[0].number = priv->channel->hw_value;
+
+	skb_pull(frame.skb, txpriv->offset);
+
+	ies = &frame.skb->data[sizeof(struct ieee80211_hdr_3addr)];
+	ies_len = frame.skb->len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (ies_len) {
+		u8 *ssidie =
+			(u8 *)cfg80211_find_ie(WLAN_EID_SSID, ies, ies_len);
+		if (ssidie && ssidie[1] && ssidie[1] <= sizeof(ssids[0].ssid)) {
+			u8 *nextie = &ssidie[2 + ssidie[1]];
+			/* Remove SSID from the IE list. It has to be provided
+			 * as a separate argument in cw1200_scan_start call */
+
+			/* Store SSID localy */
+			ssids[0].length = ssidie[1];
+			memcpy(ssids[0].ssid, &ssidie[2], ssids[0].length);
+			scan.numOfSSIDs = 1;
+
+			/* Remove SSID from IE list */
+			ssidie[1] = 0;
+			memmove(&ssidie[2], nextie, &ies[ies_len] - nextie);
+			skb_trim(frame.skb, frame.skb->len - ssids[0].length);
+		}
+	}
+
+	/* FW bug: driver has to restart p2p-dev mode after scan */
+	if (priv->join_status == CW1200_JOIN_STATUS_MONITOR)
+		cw1200_disable_listening(priv);
+	ret = WARN_ON(wsm_set_template_frame(priv, &frame));
+	priv->scan.direct_probe = 1;
+	if (!ret) {
+		wsm_flush_tx(priv);
+		ret = WARN_ON(cw1200_scan_start(priv, &scan));
+	}
+	mutex_unlock(&priv->conf_mutex);
+
+	skb_push(frame.skb, txpriv->offset);
+	if (!ret)
+		IEEE80211_SKB_CB(frame.skb)->flags |= IEEE80211_TX_STAT_ACK;
+	BUG_ON(cw1200_queue_remove(queue, priv->pending_frame_id));
+
+	if (ret) {
+		priv->scan.direct_probe = 0;
+		up(&priv->scan.lock);
+		wsm_unlock_tx(priv);
+	}
+
+	return;
+}
diff --git a/drivers/staging/cw1200/scan.h b/drivers/staging/cw1200/scan.h
new file mode 100644
index 00000000000..abffd1d796f
--- /dev/null
+++ b/drivers/staging/cw1200/scan.h
@@ -0,0 +1,54 @@
+/*
+ * Scan interface for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SCAN_H_INCLUDED
+#define SCAN_H_INCLUDED
+
+#include <linux/semaphore.h>
+#include "wsm.h"
+
+/* external */ struct sk_buff;
+/* external */ struct cfg80211_scan_request;
+/* external */ struct ieee80211_channel;
+/* external */ struct ieee80211_hw;
+/* external */ struct work_struct;
+
+struct cw1200_scan {
+	struct semaphore lock;
+	struct work_struct work;
+	struct delayed_work timeout;
+	struct cfg80211_scan_request *req;
+	struct ieee80211_channel **begin;
+	struct ieee80211_channel **curr;
+	struct ieee80211_channel **end;
+	struct wsm_ssid ssids[WSM_SCAN_MAX_NUM_OF_SSIDS];
+	int output_power;
+	int n_ssids;
+	int status;
+	atomic_t in_progress;
+	/* Direct probe requests workaround */
+	struct delayed_work probe_work;
+	int direct_probe;
+};
+
+int cw1200_hw_scan(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			struct cfg80211_scan_request *req);
+void cw1200_scan_work(struct work_struct *work);
+void cw1200_scan_timeout(struct work_struct *work);
+void cw1200_scan_complete_cb(struct cw1200_common *priv,
+				struct wsm_scan_complete *arg);
+
+/* ******************************************************************** */
+/* Raw probe requests TX workaround					*/
+void cw1200_probe_work(struct work_struct *work);
+
+#endif
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
new file mode 100644
index 00000000000..cc9ed7b1507
--- /dev/null
+++ b/drivers/staging/cw1200/sta.c
@@ -0,0 +1,1640 @@
+/*
+ * Mac80211 STA API for ST-Ericsson CW1200 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/vmalloc.h>
+#include <linux/sched.h>
+#include <linux/firmware.h>
+
+#include "cw1200.h"
+#include "sta.h"
+#include "ap.h"
+#include "fwio.h"
+#include "bh.h"
+#include "debug.h"
+
+#if defined(CONFIG_CW1200_STA_DEBUG)
+#define sta_printk(...) printk(__VA_ARGS__)
+#else
+#define sta_printk(...)
+#endif
+
+static inline void __cw1200_free_event_queue(struct list_head *list)
+{
+	while (!list_empty(list)) {
+		struct cw1200_wsm_event *event =
+			list_first_entry(list, struct cw1200_wsm_event,
+			link);
+		list_del(&event->link);
+		kfree(event);
+	}
+}
+
+/* ******************************************************************** */
+/* STA API								*/
+
+int cw1200_start(struct ieee80211_hw *dev)
+{
+	struct cw1200_common *priv = dev->priv;
+	int ret = 0;
+
+	mutex_lock(&priv->conf_mutex);
+
+	/* default EDCA */
+	WSM_EDCA_SET(&priv->edca, 0, 0x0002, 0x0003, 0x0007, 47, 0xc8, false);
+	WSM_EDCA_SET(&priv->edca, 1, 0x0002, 0x0007, 0x000f, 94, 0xc8, false);
+	WSM_EDCA_SET(&priv->edca, 2, 0x0003, 0x000f, 0x03ff, 0, 0xc8, false);
+	WSM_EDCA_SET(&priv->edca, 3, 0x0007, 0x000f, 0x03ff, 0, 0xc8, false);
+	ret = wsm_set_edca_params(priv, &priv->edca);
+	if (WARN_ON(ret))
+		goto out;
+
+	ret = cw1200_set_uapsd_param(priv, &priv->edca);
+	if (WARN_ON(ret))
+		goto out;
+
+	priv->setbssparams_done = false;
+
+	memset(priv->bssid, ~0, ETH_ALEN);
+	memcpy(priv->mac_addr, dev->wiphy->perm_addr, ETH_ALEN);
+	priv->mode = NL80211_IFTYPE_MONITOR;
+	priv->softled_state = 0;
+	priv->wep_default_key_id = -1;
+
+	priv->cqm_link_loss_count = 60;
+	priv->cqm_beacon_loss_count = 20;
+
+	/* Temporary configuration - beacon filter table */
+	priv->bf_table.numOfIEs = __cpu_to_le32(2);
+	priv->bf_table.entry[0].ieId = WLAN_EID_VENDOR_SPECIFIC;
+	priv->bf_table.entry[0].actionFlags = WSM_BEACON_FILTER_IE_HAS_CHANGED |
+					WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT |
+					WSM_BEACON_FILTER_IE_HAS_APPEARED;
+	priv->bf_table.entry[0].oui[0] = 0x50;
+	priv->bf_table.entry[0].oui[1] = 0x6F;
+	priv->bf_table.entry[0].oui[2] = 0x9A;
+	priv->bf_table.entry[1].ieId = WLAN_EID_ERP_INFO;
+	priv->bf_table.entry[1].actionFlags = WSM_BEACON_FILTER_IE_HAS_CHANGED |
+					WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT |
+					WSM_BEACON_FILTER_IE_HAS_APPEARED;
+
+	priv->bf_control.enabled = 1;
+	ret = cw1200_setup_mac(priv);
+	if (WARN_ON(ret))
+		goto out;
+
+	/* err = cw1200_set_leds(priv); */
+
+out:
+	mutex_unlock(&priv->conf_mutex);
+	return ret;
+}
+
+void cw1200_stop(struct ieee80211_hw *dev)
+{
+	struct cw1200_common *priv = dev->priv;
+	LIST_HEAD(list);
+	int i;
+
+	wsm_lock_tx(priv);
+
+	while (down_trylock(&priv->scan.lock)) {
+		/* Scan is in progress. Force it to stop. */
+		priv->scan.req = NULL;
+		schedule();
+	}
+	up(&priv->scan.lock);
+
+	cancel_delayed_work_sync(&priv->scan.probe_work);
+	cancel_delayed_work_sync(&priv->scan.timeout);
+	cancel_delayed_work_sync(&priv->join_timeout);
+	cancel_delayed_work_sync(&priv->bss_loss_work);
+	cancel_delayed_work_sync(&priv->connection_loss_work);
+	cancel_delayed_work_sync(&priv->link_id_gc_work);
+	flush_workqueue(priv->workqueue);
+	del_timer_sync(&priv->mcast_timeout);
+	del_timer_sync(&priv->ba_timer);
+
+	mutex_lock(&priv->conf_mutex);
+	priv->mode = NL80211_IFTYPE_UNSPECIFIED;
+	priv->listening = false;
+
+	priv->softled_state = 0;
+	/* cw1200_set_leds(priv); */
+
+	spin_lock(&priv->event_queue_lock);
+	list_splice_init(&priv->event_queue, &list);
+	spin_unlock(&priv->event_queue_lock);
+	__cw1200_free_event_queue(&list);
+
+	priv->delayed_link_loss = 0;
+
+	priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+
+	for (i = 0; i < 4; i++)
+		cw1200_queue_clear(&priv->tx_queue[i]);
+
+	/* HACK! */
+	if (atomic_xchg(&priv->tx_lock, 1) != 1)
+		sta_printk(KERN_DEBUG "[STA] TX is force-unlocked "
+			"due to stop request.\n");
+
+	wsm_unlock_tx(priv);
+
+	mutex_unlock(&priv->conf_mutex);
+}
+
+int cw1200_add_interface(struct ieee80211_hw *dev,
+			 struct ieee80211_vif *vif)
+{
+	int ret;
+	struct cw1200_common *priv = dev->priv;
+	/* __le32 auto_calibration_mode = __cpu_to_le32(1); */
+
+	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER |
+			     IEEE80211_VIF_SUPPORTS_CQM_RSSI;
+
+	mutex_lock(&priv->conf_mutex);
+
+	if (priv->mode != NL80211_IFTYPE_MONITOR) {
+		mutex_unlock(&priv->conf_mutex);
+		return -EOPNOTSUPP;
+	}
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_MESH_POINT:
+	case NL80211_IFTYPE_AP:
+		priv->mode = vif->type;
+		break;
+	default:
+		mutex_unlock(&priv->conf_mutex);
+		return -EOPNOTSUPP;
+	}
+
+	priv->vif = vif;
+	memcpy(priv->mac_addr, vif->addr, ETH_ALEN);
+
+	ret = WARN_ON(cw1200_setup_mac(priv));
+	/* Enable auto-calibration */
+	/* Exception in subsequent channel switch; disabled.
+	WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_SET_AUTO_CALIBRATION_MODE,
+		&auto_calibration_mode, sizeof(auto_calibration_mode)));
+	*/
+
+	mutex_unlock(&priv->conf_mutex);
+	return ret;
+}
+
+void cw1200_remove_interface(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif)
+{
+	struct cw1200_common *priv = dev->priv;
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+	int i;
+
+	mutex_lock(&priv->conf_mutex);
+	wsm_lock_tx(priv);
+	switch (priv->join_status) {
+	case CW1200_JOIN_STATUS_STA:
+		wsm_lock_tx(priv);
+		if (queue_work(priv->workqueue, &priv->unjoin_work) <= 0)
+			wsm_unlock_tx(priv);
+		break;
+	case CW1200_JOIN_STATUS_AP:
+		for (i = 0; priv->link_id_map; ++i) {
+			if (priv->link_id_map & BIT(i)) {
+				reset.link_id = i;
+				wsm_reset(priv, &reset);
+				priv->link_id_map &= ~BIT(i);
+			}
+		}
+		memset(priv->link_id_db, 0,
+				sizeof(priv->link_id_db));
+		priv->sta_asleep_mask = 0;
+		priv->enable_beacon = false;
+		priv->tx_multicast = false;
+		priv->aid0_bit_set = false;
+		priv->buffered_multicasts = false;
+		priv->pspoll_mask = 0;
+		reset.link_id = 0;
+		wsm_reset(priv, &reset);
+		break;
+	case CW1200_JOIN_STATUS_MONITOR:
+		cw1200_update_listening(priv, false);
+		break;
+	default:
+		break;
+	}
+	priv->vif = NULL;
+	priv->mode = NL80211_IFTYPE_MONITOR;
+	memset(priv->mac_addr, 0, ETH_ALEN);
+	memset(priv->bssid, 0, ETH_ALEN);
+	cw1200_free_keys(priv);
+	cw1200_setup_mac(priv);
+	priv->listening = false;
+	priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+	if (!__cw1200_flush(priv, true))
+		wsm_unlock_tx(priv);
+	wsm_unlock_tx(priv);
+
+	mutex_unlock(&priv->conf_mutex);
+}
+
+int cw1200_config(struct ieee80211_hw *dev, u32 changed)
+{
+	int ret = 0;
+	struct cw1200_common *priv = dev->priv;
+	struct ieee80211_conf *conf = &dev->conf;
+
+	down(&priv->scan.lock);
+	mutex_lock(&priv->conf_mutex);
+	/* TODO: IEEE80211_CONF_CHANGE_QOS */
+	if (changed & IEEE80211_CONF_CHANGE_POWER) {
+		priv->output_power = conf->power_level;
+		sta_printk(KERN_DEBUG "[STA] TX power: %d\n",
+				priv->output_power);
+		WARN_ON(wsm_set_output_power(priv, priv->output_power * 10));
+	}
+
+	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) &&
+			(priv->channel != conf->channel)) {
+		struct ieee80211_channel *ch = conf->channel;
+		struct wsm_switch_channel channel = {
+			.newChannelNumber = ch->hw_value,
+		};
+		sta_printk(KERN_DEBUG "[STA] Freq %d (wsm ch: %d).\n",
+			ch->center_freq, ch->hw_value);
+
+		ret = WARN_ON(__cw1200_flush(priv, false));
+		if (!ret) {
+			ret = WARN_ON(wsm_switch_channel(priv, &channel));
+			if (!ret) {
+				ret = wait_event_timeout(
+					priv->channel_switch_done,
+					!priv->channel_switch_in_progress,
+					3 * HZ);
+				/* TODO: We should check also switch channel
+				 * complete indication
+				 */
+				if (ret) {
+					priv->channel = ch;
+					ret = 0;
+				} else
+					ret = -ETIMEDOUT;
+			} else
+				wsm_unlock_tx(priv);
+		}
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_PS) {
+		if (!(conf->flags & IEEE80211_CONF_PS))
+			priv->powersave_mode.pmMode = WSM_PSM_ACTIVE;
+		else if (conf->dynamic_ps_timeout <= 0)
+			priv->powersave_mode.pmMode = WSM_PSM_PS;
+		else
+			priv->powersave_mode.pmMode = WSM_PSM_FAST_PS;
+
+		/* Firmware requires that value for this 1-byte field must
+		 * be specified in units of 500us. Values above the 128ms
+		 * threshold are not supported. */
+		if (conf->dynamic_ps_timeout >= 0x80)
+			priv->powersave_mode.fastPsmIdlePeriod = 0xFF;
+		else
+			priv->powersave_mode.fastPsmIdlePeriod =
+					conf->dynamic_ps_timeout << 1;
+
+		if (priv->join_status == CW1200_JOIN_STATUS_STA &&
+				priv->bss_params.aid)
+			cw1200_set_pm(priv, &priv->powersave_mode);
+	}
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	if (changed & IEEE80211_CONF_CHANGE_P2P_PS) {
+		struct wsm_p2p_ps_modeinfo *modeinfo;
+		modeinfo = &priv->p2p_ps_modeinfo;
+		sta_printk(KERN_DEBUG "[STA] IEEE80211_CONF_CHANGE_P2P_PS\n");
+		sta_printk(KERN_DEBUG "[STA] Legacy PS: %d for AID %d "
+			"in %d mode.\n", conf->p2p_ps.legacy_ps,
+			priv->bss_params.aid, priv->join_status);
+
+		if (conf->p2p_ps.legacy_ps >= 0) {
+			if (conf->p2p_ps.legacy_ps > 0)
+				priv->powersave_mode.pmMode = WSM_PSM_PS;
+			else
+				priv->powersave_mode.pmMode = WSM_PSM_ACTIVE;
+
+			if (priv->join_status == CW1200_JOIN_STATUS_STA)
+				cw1200_set_pm(priv, &priv->powersave_mode);
+		}
+
+		sta_printk(KERN_DEBUG "[STA] CTWindow: %d\n",
+			conf->p2p_ps.ctwindow);
+		if (conf->p2p_ps.ctwindow >= 128)
+			modeinfo->oppPsCTWindow = 127;
+		else if (conf->p2p_ps.ctwindow >= 0)
+			modeinfo->oppPsCTWindow = conf->p2p_ps.ctwindow;
+
+		sta_printk(KERN_DEBUG "[STA] Opportunistic: %d\n",
+			conf->p2p_ps.opp_ps);
+		switch (conf->p2p_ps.opp_ps) {
+		case 0:
+			modeinfo->oppPsCTWindow &= ~(BIT(7));
+			break;
+		case 1:
+			modeinfo->oppPsCTWindow |= BIT(7);
+			break;
+		default:
+			break;
+		}
+
+		sta_printk(KERN_DEBUG "[STA] NOA: %d, %d, %d, %d\n",
+			conf->p2p_ps.count,
+			conf->p2p_ps.start,
+			conf->p2p_ps.duration,
+			conf->p2p_ps.interval);
+		/* Notice of Absence */
+		modeinfo->count = conf->p2p_ps.count;
+
+		if (conf->p2p_ps.count) {
+			/* In case P2P_GO we need some extra time to be sure
+			 * we will update beacon/probe_resp IEs correctly */
+#define NOA_DELAY_START_MS	300
+			if (priv->join_status == CW1200_JOIN_STATUS_AP)
+				modeinfo->startTime =
+					__cpu_to_le32(conf->p2p_ps.start +
+						      NOA_DELAY_START_MS);
+			else
+				modeinfo->startTime =
+					__cpu_to_le32(conf->p2p_ps.start);
+			modeinfo->duration =
+				__cpu_to_le32(conf->p2p_ps.duration);
+			modeinfo->interval =
+				 __cpu_to_le32(conf->p2p_ps.interval);
+			modeinfo->dtimCount = 1;
+			modeinfo->reserved = 0;
+		} else {
+			modeinfo->dtimCount = 0;
+			modeinfo->startTime = 0;
+			modeinfo->reserved = 0;
+			modeinfo->duration = 0;
+			modeinfo->interval = 0;
+		}
+
+#if defined(CONFIG_CW1200_STA_DEBUG)
+		print_hex_dump_bytes("p2p_set_ps_modeinfo: ",
+				     DUMP_PREFIX_NONE,
+				     (u8 *)modeinfo,
+				     sizeof(*modeinfo));
+#endif /* CONFIG_CW1200_STA_DEBUG */
+		if (priv->join_status == CW1200_JOIN_STATUS_STA ||
+		    priv->join_status == CW1200_JOIN_STATUS_AP) {
+			WARN_ON(wsm_set_p2p_ps_modeinfo(priv, modeinfo));
+		}
+
+		/* Temporary solution while firmware don't support NOA change
+		 * notification yet */
+		cw1200_notify_noa(priv, 10);
+	}
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+
+	if (changed & IEEE80211_CONF_CHANGE_MONITOR) {
+		/* TBD: It looks like it's transparent
+		 * there's a monitor interface present -- use this
+		 * to determine for example whether to calculate
+		 * timestamps for packets or not, do not use instead
+		 * of filter flags! */
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
+		struct wsm_operational_mode mode = {
+			.power_mode = wsm_power_mode_quiescent,
+			.disableMoreFlagUsage = true,
+		};
+
+		wsm_lock_tx(priv);
+		/* Disable p2p-dev mode forced by TX request */
+		if ((priv->join_status == CW1200_JOIN_STATUS_MONITOR) &&
+				(conf->flags & IEEE80211_CONF_IDLE) &&
+				!priv->listening) {
+			cw1200_disable_listening(priv);
+			priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+		}
+		WARN_ON(wsm_set_operational_mode(priv, &mode));
+		wsm_unlock_tx(priv);
+	}
+
+	if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS) {
+		sta_printk(KERN_DEBUG "[STA] Retry limits: %d (long), " \
+			"%d (short).\n",
+			conf->long_frame_max_tx_count,
+			conf->short_frame_max_tx_count);
+		spin_lock_bh(&priv->tx_policy_cache.lock);
+		priv->long_frame_max_tx_count = conf->long_frame_max_tx_count;
+		priv->short_frame_max_tx_count =
+			(conf->short_frame_max_tx_count < 0x0F) ?
+			conf->short_frame_max_tx_count : 0x0F;
+		priv->hw->max_rate_tries = priv->short_frame_max_tx_count;
+		spin_unlock_bh(&priv->tx_policy_cache.lock);
+		/* TBD: I think we don't need tx_policy_force_upload().
+		 * Outdated policies will leave cache in a normal way. */
+		/* WARN_ON(tx_policy_force_upload(priv)); */
+	}
+	mutex_unlock(&priv->conf_mutex);
+	up(&priv->scan.lock);
+	return ret;
+}
+
+void cw1200_update_filtering(struct cw1200_common *priv)
+{
+	int ret;
+	bool bssid_filtering = !priv->rx_filter.bssid;
+	static struct wsm_beacon_filter_control bf_disabled = {
+		.enabled = 0,
+		.bcn_count = 1,
+	};
+
+	if (priv->join_status == CW1200_JOIN_STATUS_PASSIVE)
+		return;
+	else if (priv->join_status == CW1200_JOIN_STATUS_MONITOR)
+		bssid_filtering = false;
+
+	/*
+	* When acting as p2p client being connected to p2p GO, in order to
+	* receive frames from a different p2p device, turn off bssid filter.
+	*
+	* WARNING: FW dependency!
+	* This can only be used with FW WSM371 and its successors.
+	* In that FW version even with bssid filter turned off,
+	* device will block most of the unwanted frames.
+	*/
+	if (priv->vif->p2p)
+		bssid_filtering = false;
+
+	ret = wsm_set_rx_filter(priv, &priv->rx_filter);
+	if (!ret)
+		ret = wsm_set_beacon_filter_table(priv, &priv->bf_table);
+	if (!ret) {
+		if (priv->disable_beacon_filter)
+			ret = wsm_beacon_filter_control(priv,
+					&bf_disabled);
+		else
+			ret = wsm_beacon_filter_control(priv,
+					&priv->bf_control);
+	}
+	if (!ret)
+		ret = wsm_set_bssid_filtering(priv, bssid_filtering);
+	if (!ret)
+		ret = wsm_set_multicast_filter(priv, &priv->multicast_filter);
+	if (ret)
+		wiphy_err(priv->hw->wiphy,
+				"%s: Update filtering failed: %d.\n",
+				__func__, ret);
+	return;
+}
+
+void cw1200_update_filtering_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common,
+		update_filtering_work);
+
+	cw1200_update_filtering(priv);
+}
+
+u64 cw1200_prepare_multicast(struct ieee80211_hw *hw,
+			     struct netdev_hw_addr_list *mc_list)
+{
+	static u8 broadcast_ipv6[ETH_ALEN] = {
+		0x33, 0x33, 0x00, 0x00, 0x00, 0x01
+	};
+	static u8 broadcast_ipv4[ETH_ALEN] = {
+		0x01, 0x00, 0x5e, 0x00, 0x00, 0x01
+	};
+	struct cw1200_common *priv = hw->priv;
+	struct netdev_hw_addr *ha;
+	int count = 0;
+
+	/* Disable multicast filtering */
+	priv->has_multicast_subscription = false;
+	memset(&priv->multicast_filter, 0x00, sizeof(priv->multicast_filter));
+
+	if (netdev_hw_addr_list_count(mc_list) > WSM_MAX_GRP_ADDRTABLE_ENTRIES)
+		return 0;
+
+	/* Enable if requested */
+	netdev_hw_addr_list_for_each(ha, mc_list) {
+		sta_printk(KERN_DEBUG "[STA] multicast: %pM\n", ha->addr);
+		memcpy(&priv->multicast_filter.macAddress[count],
+		       ha->addr, ETH_ALEN);
+		if (memcmp(ha->addr, broadcast_ipv4, ETH_ALEN) &&
+				memcmp(ha->addr, broadcast_ipv6, ETH_ALEN))
+			priv->has_multicast_subscription = true;
+		count++;
+	}
+
+	if (count) {
+		priv->multicast_filter.enable = __cpu_to_le32(1);
+		priv->multicast_filter.numOfAddresses = __cpu_to_le32(count);
+	}
+
+	return netdev_hw_addr_list_count(mc_list);
+}
+
+void cw1200_configure_filter(struct ieee80211_hw *dev,
+			     unsigned int changed_flags,
+			     unsigned int *total_flags,
+			     u64 multicast)
+{
+	struct cw1200_common *priv = dev->priv;
+	bool listening = !!(*total_flags &
+			(FIF_PROMISC_IN_BSS |
+			 FIF_OTHER_BSS |
+			 FIF_BCN_PRBRESP_PROMISC |
+			 FIF_PROBE_REQ));
+
+	*total_flags &= FIF_PROMISC_IN_BSS |
+			FIF_OTHER_BSS |
+			FIF_FCSFAIL |
+			FIF_BCN_PRBRESP_PROMISC |
+			FIF_PROBE_REQ;
+
+	down(&priv->scan.lock);
+	mutex_lock(&priv->conf_mutex);
+
+	priv->rx_filter.promiscuous = (*total_flags & FIF_PROMISC_IN_BSS)
+			? 1 : 0;
+	priv->rx_filter.bssid = (*total_flags & (FIF_OTHER_BSS |
+			FIF_PROBE_REQ)) ? 1 : 0;
+	priv->rx_filter.fcs = (*total_flags & FIF_FCSFAIL) ? 1 : 0;
+	priv->bf_control.bcn_count = (*total_flags &
+			(FIF_BCN_PRBRESP_PROMISC |
+			 FIF_PROMISC_IN_BSS |
+			 FIF_PROBE_REQ)) ? 1 : 0;
+	if (priv->listening ^ listening) {
+		priv->listening = listening;
+		wsm_lock_tx(priv);
+		cw1200_update_listening(priv, listening);
+		wsm_unlock_tx(priv);
+	}
+	cw1200_update_filtering(priv);
+	mutex_unlock(&priv->conf_mutex);
+	up(&priv->scan.lock);
+}
+
+int cw1200_conf_tx(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
+		   u16 queue, const struct ieee80211_tx_queue_params *params)
+{
+	struct cw1200_common *priv = dev->priv;
+	int ret = 0;
+	/* To prevent re-applying PM request OID again and again*/
+	bool old_uapsdFlags;
+
+	mutex_lock(&priv->conf_mutex);
+
+	if (queue < dev->queues) {
+		old_uapsdFlags = priv->uapsd_info.uapsdFlags;
+
+		WSM_TX_QUEUE_SET(&priv->tx_queue_params, queue, 0, 0, 0);
+		ret = wsm_set_tx_queue_params(priv,
+				&priv->tx_queue_params.params[queue], queue);
+		if (ret) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		WSM_EDCA_SET(&priv->edca, queue, params->aifs,
+			params->cw_min, params->cw_max, params->txop, 0xc8,
+			params->uapsd);
+		ret = wsm_set_edca_params(priv, &priv->edca);
+		if (ret) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		if (priv->mode == NL80211_IFTYPE_STATION) {
+			ret = cw1200_set_uapsd_param(priv, &priv->edca);
+			if (!ret && priv->setbssparams_done &&
+				(priv->join_status == CW1200_JOIN_STATUS_STA) &&
+				(old_uapsdFlags != priv->uapsd_info.uapsdFlags))
+				cw1200_set_pm(priv, &priv->powersave_mode);
+		}
+	} else
+		ret = -EINVAL;
+
+out:
+	mutex_unlock(&priv->conf_mutex);
+	return ret;
+}
+
+int cw1200_get_stats(struct ieee80211_hw *dev,
+		     struct ieee80211_low_level_stats *stats)
+{
+	struct cw1200_common *priv = dev->priv;
+
+	memcpy(stats, &priv->stats, sizeof(*stats));
+	return 0;
+}
+
+/*
+int cw1200_get_tx_stats(struct ieee80211_hw *dev,
+			struct ieee80211_tx_queue_stats *stats)
+{
+	int i;
+	struct cw1200_common *priv = dev->priv;
+
+	for (i = 0; i < dev->queues; ++i)
+		cw1200_queue_get_stats(&priv->tx_queue[i], &stats[i]);
+
+	return 0;
+}
+*/
+
+int cw1200_set_pm(struct cw1200_common *priv, const struct wsm_set_pm *arg)
+{
+	struct wsm_set_pm pm = *arg;
+
+	if (priv->uapsd_info.uapsdFlags != 0)
+		pm.pmMode &= ~WSM_PSM_FAST_PS_FLAG;
+
+	if (memcmp(&pm, &priv->firmware_ps_mode,
+			sizeof(struct wsm_set_pm))) {
+		priv->firmware_ps_mode = pm;
+		return wsm_set_pm(priv, &pm);
+	} else {
+		return 0;
+	}
+}
+
+int cw1200_set_key(struct ieee80211_hw *dev, enum set_key_cmd cmd,
+		   struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+		   struct ieee80211_key_conf *key)
+{
+	int ret = -EOPNOTSUPP;
+	struct cw1200_common *priv = dev->priv;
+
+	mutex_lock(&priv->conf_mutex);
+
+	if (cmd == SET_KEY) {
+		u8 *peer_addr = NULL;
+		int pairwise = (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) ?
+			1 : 0;
+		int idx = cw1200_alloc_key(priv);
+		struct wsm_add_key *wsm_key = &priv->keys[idx];
+
+		if (idx < 0) {
+			ret = -EINVAL;
+			goto finally;
+		}
+
+		BUG_ON(pairwise && !sta);
+		if (sta)
+			peer_addr = sta->addr;
+
+		switch (key->cipher) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			if (key->keylen > 16) {
+				cw1200_free_key(priv, idx);
+				ret = -EINVAL;
+				goto finally;
+			}
+
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_WEP_PAIRWISE;
+				memcpy(wsm_key->wepPairwiseKey.peerAddress,
+					 peer_addr, ETH_ALEN);
+				memcpy(wsm_key->wepPairwiseKey.keyData,
+					&key->key[0], key->keylen);
+				wsm_key->wepPairwiseKey.keyLength = key->keylen;
+			} else {
+				wsm_key->type = WSM_KEY_TYPE_WEP_DEFAULT;
+				memcpy(wsm_key->wepGroupKey.keyData,
+					&key->key[0], key->keylen);
+				wsm_key->wepGroupKey.keyLength = key->keylen;
+				wsm_key->wepGroupKey.keyId = key->keyidx;
+			}
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_TKIP_PAIRWISE;
+				memcpy(wsm_key->tkipPairwiseKey.peerAddress,
+					peer_addr, ETH_ALEN);
+				memcpy(wsm_key->tkipPairwiseKey.tkipKeyData,
+					&key->key[0],  16);
+				memcpy(wsm_key->tkipPairwiseKey.txMicKey,
+					&key->key[16],  8);
+				memcpy(wsm_key->tkipPairwiseKey.rxMicKey,
+					&key->key[24],  8);
+			} else {
+				size_t mic_offset =
+					(priv->mode == NL80211_IFTYPE_AP) ?
+					16 : 24;
+				wsm_key->type = WSM_KEY_TYPE_TKIP_GROUP;
+				memcpy(wsm_key->tkipGroupKey.tkipKeyData,
+					&key->key[0],  16);
+				memcpy(wsm_key->tkipGroupKey.rxMicKey,
+					&key->key[mic_offset],  8);
+
+				/* TODO: Where can I find TKIP SEQ? */
+				memset(wsm_key->tkipGroupKey.rxSeqCounter,
+					0,		8);
+				wsm_key->tkipGroupKey.keyId = key->keyidx;
+
+				print_hex_dump_bytes("TKIP: ", DUMP_PREFIX_NONE,
+					key->key, key->keylen);
+			}
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_AES_PAIRWISE;
+				memcpy(wsm_key->aesPairwiseKey.peerAddress,
+					peer_addr, ETH_ALEN);
+				memcpy(wsm_key->aesPairwiseKey.aesKeyData,
+					&key->key[0],  16);
+			} else {
+				wsm_key->type = WSM_KEY_TYPE_AES_GROUP;
+				memcpy(wsm_key->aesGroupKey.aesKeyData,
+					&key->key[0],  16);
+				/* TODO: Where can I find AES SEQ? */
+				memset(wsm_key->aesGroupKey.rxSeqCounter,
+					0,              8);
+				wsm_key->aesGroupKey.keyId = key->keyidx;
+			}
+			break;
+#ifdef CONFIG_CW1200_WAPI_SUPPORT
+		case WLAN_CIPHER_SUITE_SMS4:
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_WAPI_PAIRWISE;
+				memcpy(wsm_key->wapiPairwiseKey.peerAddress,
+					peer_addr, ETH_ALEN);
+				memcpy(wsm_key->wapiPairwiseKey.wapiKeyData,
+					&key->key[0],  16);
+				memcpy(wsm_key->wapiPairwiseKey.micKeyData,
+					&key->key[16], 16);
+				wsm_key->wapiPairwiseKey.keyId = key->keyidx;
+			} else {
+				wsm_key->type = WSM_KEY_TYPE_WAPI_GROUP;
+				memcpy(wsm_key->wapiGroupKey.wapiKeyData,
+					&key->key[0],  16);
+				memcpy(wsm_key->wapiGroupKey.micKeyData,
+					&key->key[16], 16);
+				wsm_key->wapiGroupKey.keyId = key->keyidx;
+			}
+			break;
+#endif /* CONFIG_CW1200_WAPI_SUPPORT */
+		default:
+			WARN_ON(1);
+			cw1200_free_key(priv, idx);
+			ret = -EOPNOTSUPP;
+			goto finally;
+		}
+		ret = WARN_ON(wsm_add_key(priv, wsm_key));
+		if (!ret)
+			key->hw_key_idx = idx;
+		else
+			cw1200_free_key(priv, idx);
+	} else if (cmd == DISABLE_KEY) {
+		struct wsm_remove_key wsm_key = {
+			.entryIndex = key->hw_key_idx,
+		};
+
+		if (wsm_key.entryIndex > WSM_KEY_MAX_INDEX) {
+			ret = -EINVAL;
+			goto finally;
+		}
+
+		cw1200_free_key(priv, wsm_key.entryIndex);
+		ret = wsm_remove_key(priv, &wsm_key);
+	} else {
+		BUG_ON("Unsupported command");
+	}
+
+finally:
+	mutex_unlock(&priv->conf_mutex);
+	return ret;
+}
+
+void cw1200_wep_key_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, wep_key_work);
+	u8 queueId = cw1200_queue_get_queue_id(priv->pending_frame_id);
+	struct cw1200_queue *queue = &priv->tx_queue[queueId];
+	__le32 wep_default_key_id = __cpu_to_le32(
+		priv->wep_default_key_id);
+
+	BUG_ON(queueId >= 4);
+
+	sta_printk(KERN_DEBUG "[STA] Setting default WEP key: %d\n",
+		priv->wep_default_key_id);
+	wsm_flush_tx(priv);
+	WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID,
+		&wep_default_key_id, sizeof(wep_default_key_id)));
+	cw1200_queue_requeue(queue, priv->pending_frame_id);
+	wsm_unlock_tx(priv);
+}
+
+int cw1200_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
+{
+	int ret;
+	__le32 val32;
+
+	if (value != (u32) -1)
+		val32 = __cpu_to_le32(value);
+	else
+		val32 = 0; /* disabled */
+
+	/* mutex_lock(&priv->conf_mutex); */
+	ret = WARN_ON(wsm_write_mib(hw->priv, WSM_MIB_ID_DOT11_RTS_THRESHOLD,
+		&val32, sizeof(val32)));
+	/* mutex_unlock(&priv->conf_mutex); */
+	return ret;
+}
+
+int __cw1200_flush(struct cw1200_common *priv, bool drop)
+{
+	int i, ret;
+
+	for (;;) {
+		/* TODO: correct flush handling is required when dev_stop.
+		 * Temporary workaround: 2s
+		 */
+		if (drop) {
+			for (i = 0; i < 4; ++i)
+				cw1200_queue_clear(&priv->tx_queue[i]);
+		} else {
+			ret = wait_event_timeout(
+				priv->tx_queue_stats.wait_link_id_empty,
+				cw1200_queue_stats_is_empty(
+					&priv->tx_queue_stats, -1),
+				2 * HZ);
+		}
+
+		if (!drop && unlikely(ret <= 0)) {
+			ret = -ETIMEDOUT;
+			break;
+		} else {
+			ret = 0;
+		}
+
+		wsm_lock_tx(priv);
+		if (unlikely(!cw1200_queue_stats_is_empty(
+				&priv->tx_queue_stats, -1))) {
+			/* Highly unlekely: WSM requeued frames. */
+			wsm_unlock_tx(priv);
+			continue;
+		}
+		break;
+	}
+	return ret;
+}
+
+void cw1200_flush(struct ieee80211_hw *hw, bool drop)
+{
+	struct cw1200_common *priv = hw->priv;
+
+	switch (priv->mode) {
+	case NL80211_IFTYPE_MONITOR:
+		drop = true;
+		break;
+	case NL80211_IFTYPE_AP:
+		if (!priv->enable_beacon)
+			drop = true;
+		break;
+	}
+
+	if (!WARN_ON(__cw1200_flush(priv, drop)))
+		wsm_unlock_tx(priv);
+
+	return;
+}
+
+/* ******************************************************************** */
+/* WSM callbacks							*/
+
+void cw1200_channel_switch_cb(struct cw1200_common *priv)
+{
+	wsm_unlock_tx(priv);
+}
+
+void cw1200_free_event_queue(struct cw1200_common *priv)
+{
+	LIST_HEAD(list);
+
+	spin_lock(&priv->event_queue_lock);
+	list_splice_init(&priv->event_queue, &list);
+	spin_unlock(&priv->event_queue_lock);
+
+	__cw1200_free_event_queue(&list);
+}
+
+void cw1200_event_handler(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, event_handler);
+	struct cw1200_wsm_event *event;
+	LIST_HEAD(list);
+
+	spin_lock(&priv->event_queue_lock);
+	list_splice_init(&priv->event_queue, &list);
+	spin_unlock(&priv->event_queue_lock);
+
+	list_for_each_entry(event, &list, link) {
+		switch (event->evt.eventId) {
+		case WSM_EVENT_ERROR:
+			/* I even don't know what is it about.. */
+			STUB();
+			break;
+		case WSM_EVENT_BSS_LOST:
+		{
+			spin_lock(&priv->bss_loss_lock);
+			if (priv->bss_loss_status > CW1200_BSS_LOSS_NONE) {
+				spin_unlock(&priv->bss_loss_lock);
+				break;
+			}
+			priv->bss_loss_status = CW1200_BSS_LOSS_CHECKING;
+			spin_unlock(&priv->bss_loss_lock);
+
+			sta_printk(KERN_DEBUG "[CQM] BSS lost.\n");
+			cancel_delayed_work_sync(&priv->bss_loss_work);
+			cancel_delayed_work_sync(&priv->connection_loss_work);
+			if (!down_trylock(&priv->scan.lock)) {
+				up(&priv->scan.lock);
+				priv->delayed_link_loss = 0;
+				queue_delayed_work(priv->workqueue,
+						&priv->bss_loss_work, 0);
+			} else {
+				/* Scan is in progress. Delay reporting. */
+				/* Scan complete will trigger bss_loss_work */
+				priv->delayed_link_loss = 1;
+				/* Also we're starting watchdog. */
+				queue_delayed_work(priv->workqueue,
+						&priv->bss_loss_work, 10 * HZ);
+			}
+			break;
+		}
+		case WSM_EVENT_BSS_REGAINED:
+		{
+			sta_printk(KERN_DEBUG "[CQM] BSS regained.\n");
+			priv->delayed_link_loss = 0;
+			spin_lock(&priv->bss_loss_lock);
+			priv->bss_loss_status = CW1200_BSS_LOSS_NONE;
+			spin_unlock(&priv->bss_loss_lock);
+			cancel_delayed_work_sync(&priv->bss_loss_work);
+			cancel_delayed_work_sync(&priv->connection_loss_work);
+			break;
+		}
+		case WSM_EVENT_RADAR_DETECTED:
+			STUB();
+			break;
+		case WSM_EVENT_RCPI_RSSI:
+		{
+			/* RSSI: signed Q8.0, RCPI: unsigned Q7.1
+			 * RSSI = RCPI / 2 - 110 */
+			int rcpiRssi = (int)(event->evt.eventData & 0xFF);
+			int cqm_evt;
+			if (priv->cqm_use_rssi)
+				rcpiRssi = (s8)rcpiRssi;
+			else
+				rcpiRssi =  rcpiRssi / 2 - 110;
+
+			cqm_evt = (rcpiRssi <= priv->cqm_rssi_thold) ?
+				NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW :
+				NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH;
+			sta_printk(KERN_DEBUG "[CQM] RSSI event: %d", rcpiRssi);
+			ieee80211_cqm_rssi_notify(priv->vif, cqm_evt,
+								GFP_KERNEL);
+			break;
+		}
+		case WSM_EVENT_BT_INACTIVE:
+			STUB();
+			break;
+		case WSM_EVENT_BT_ACTIVE:
+			STUB();
+			break;
+		}
+	}
+	__cw1200_free_event_queue(&list);
+}
+
+void cw1200_bss_loss_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, bss_loss_work.work);
+	int timeout; /* in beacons */
+	struct sk_buff *skb;
+
+	timeout = priv->cqm_link_loss_count -
+		priv->cqm_beacon_loss_count;
+
+	/* Skip the confimration procedure in P2P case */
+	if (priv->vif->p2p)
+		goto report;
+
+	spin_lock(&priv->bss_loss_lock);
+	if (priv->bss_loss_status == CW1200_BSS_LOSS_CHECKING) {
+		spin_unlock(&priv->bss_loss_lock);
+		skb = ieee80211_nullfunc_get(priv->hw, priv->vif);
+		if (!(WARN_ON(!skb))) {
+			cw1200_tx(priv->hw, skb);
+			/* Start watchdog -- if nullfunc TX doesn't fail
+			 * in 1 sec, forward event to upper layers */
+			queue_delayed_work(priv->workqueue,
+					&priv->bss_loss_work, 1 * HZ);
+		}
+		return;
+	} else if (priv->bss_loss_status == CW1200_BSS_LOSS_CONFIRMING) {
+		priv->bss_loss_status = CW1200_BSS_LOSS_NONE;
+		spin_unlock(&priv->bss_loss_lock);
+		return;
+	}
+	spin_unlock(&priv->bss_loss_lock);
+
+report:
+	if (priv->cqm_beacon_loss_count) {
+		sta_printk(KERN_DEBUG "[CQM] Beacon loss.\n");
+		if (timeout <= 0)
+			timeout = 0;
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+		ieee80211_cqm_beacon_miss_notify(priv->vif, GFP_KERNEL);
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+	} else {
+		timeout = 0;
+	}
+
+	cancel_delayed_work_sync(&priv->connection_loss_work);
+	queue_delayed_work(priv->workqueue,
+		&priv->connection_loss_work,
+		timeout * HZ / 10);
+
+	spin_lock(&priv->bss_loss_lock);
+	priv->bss_loss_status = CW1200_BSS_LOSS_NONE;
+	spin_unlock(&priv->bss_loss_lock);
+}
+
+void cw1200_connection_loss_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common,
+				connection_loss_work.work);
+	sta_printk(KERN_DEBUG "[CQM] Reporting connection loss.\n");
+	ieee80211_connection_loss(priv->vif);
+}
+
+void cw1200_tx_failure_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, tx_failure_work);
+	sta_printk(KERN_DEBUG "[CQM] Reporting TX failure.\n");
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	ieee80211_cqm_tx_fail_notify(priv->vif, GFP_KERNEL);
+#else /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+	(void)priv;
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+}
+
+/* ******************************************************************** */
+/* Internal API								*/
+
+
+
+/*
+* This function is called to Parse the SDD file
+ *to extract listen_interval and PTA related information
+*/
+static int cw1200_parse_SDD_file(struct cw1200_common *priv)
+{
+	u8 *sdd_data = (u8 *)priv->sdd->data;
+	struct cw1200_sdd {
+		u8 id ;
+		u8 length ;
+		u8 data[] ;
+	} *pElement;
+	int parsedLength = 0;
+	#define SDD_PTA_CFG_ELT_ID 0xEB
+	#define FIELD_OFFSET(type, field) ((u8 *)&((type *)0)->field - (u8 *)0)
+
+	priv->is_BT_Present = false;
+
+	pElement = (struct cw1200_sdd *)sdd_data;
+
+	pElement = (struct cw1200_sdd *)((u8 *)pElement +
+		FIELD_OFFSET(struct cw1200_sdd, data) + pElement->length);
+
+	parsedLength += (FIELD_OFFSET(struct cw1200_sdd, data) +
+			pElement->length);
+
+	while (parsedLength <= priv->sdd->size) {
+		switch (pElement->id) {
+		case SDD_PTA_CFG_ELT_ID:
+		{
+			priv->conf_listen_interval =
+				(*((u16 *)pElement->data+1) >> 7) & 0x1F;
+			priv->is_BT_Present = true;
+			sta_printk(KERN_DEBUG "PTA element found.\n");
+			sta_printk(KERN_DEBUG "Listen Interval %d\n",
+						priv->conf_listen_interval);
+		}
+		break;
+
+		default:
+		break;
+		}
+
+		pElement = (struct cw1200_sdd *)
+			((u8 *)pElement + FIELD_OFFSET(struct cw1200_sdd, data)
+					+ pElement->length);
+		parsedLength +=
+		(FIELD_OFFSET(struct cw1200_sdd, data) + pElement->length);
+	}
+
+	if (priv->is_BT_Present == false) {
+		sta_printk(KERN_DEBUG "PTA element NOT found.\n");
+		priv->conf_listen_interval = 0;
+	}
+	return 0;
+
+	#undef SDD_PTA_CFG_ELT_ID
+	#undef FIELD_OFFSET
+}
+
+
+int cw1200_setup_mac(struct cw1200_common *priv)
+{
+	int ret = 0;
+
+	/* NOTE: There is a bug in FW: it reports signal
+	* as RSSI if RSSI subscription is enabled.
+	* It's not enough to set WSM_RCPI_RSSI_USE_RSSI. */
+	/* NOTE2: RSSI based reports have been switched to RCPI, since
+	* FW has a bug and RSSI reported values are not stable,
+	* what can leads to signal level oscilations in user-end applications */
+	struct wsm_rcpi_rssi_threshold threshold = {
+		.rssiRcpiMode = WSM_RCPI_RSSI_THRESHOLD_ENABLE |
+		WSM_RCPI_RSSI_DONT_USE_UPPER |
+		WSM_RCPI_RSSI_DONT_USE_LOWER,
+		.rollingAverageCount = 16,
+	};
+
+	/* Remember the decission here to make sure, we will handle
+	 * the RCPI/RSSI value correctly on WSM_EVENT_RCPI_RSS */
+	if (threshold.rssiRcpiMode & WSM_RCPI_RSSI_USE_RSSI)
+		priv->cqm_use_rssi = true;
+
+	if (!priv->sdd) {
+		const char *sdd_path = NULL;
+		struct wsm_configuration cfg = {
+			.dot11StationId = &priv->mac_addr[0],
+		};
+
+		switch (priv->hw_revision) {
+		case CW1200_HW_REV_CUT10:
+			sdd_path = SDD_FILE_10;
+			break;
+		case CW1200_HW_REV_CUT11:
+			sdd_path = SDD_FILE_11;
+			break;
+		case CW1200_HW_REV_CUT20:
+			sdd_path = SDD_FILE_20;
+			break;
+		case CW1200_HW_REV_CUT22:
+			sdd_path = SDD_FILE_22;
+			break;
+		default:
+			BUG_ON(1);
+		}
+
+		ret = request_firmware(&priv->sdd,
+			sdd_path, priv->pdev);
+
+		if (unlikely(ret)) {
+			cw1200_dbg(CW1200_DBG_ERROR,
+				"%s: can't load sdd file %s.\n",
+				__func__, sdd_path);
+			return ret;
+		}
+
+		cfg.dpdData = priv->sdd->data;
+		cfg.dpdData_size = priv->sdd->size;
+		ret = WARN_ON(wsm_configuration(priv, &cfg));
+		/* Parse SDD file for PTA element */
+		cw1200_parse_SDD_file(priv);
+	}
+	if (ret)
+		return ret;
+
+	/* Configure RSSI/SCPI reporting as RSSI. */
+	WARN_ON(wsm_set_rcpi_rssi_threshold(priv, &threshold));
+
+	/* TODO: */
+	switch (priv->mode) {
+	case NL80211_IFTYPE_STATION:
+	case NL80211_IFTYPE_MONITOR:
+	case NL80211_IFTYPE_AP:
+		break;
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_MESH_POINT:
+		/* TODO: Not verified yet. */
+		STUB();
+		break;
+	}
+
+	return 0;
+}
+
+void cw1200_offchannel_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, offchannel_work);
+	u8 queueId = cw1200_queue_get_queue_id(priv->pending_frame_id);
+	struct cw1200_queue *queue = &priv->tx_queue[queueId];
+
+	BUG_ON(queueId >= 4);
+	BUG_ON(!priv->channel);
+
+	mutex_lock(&priv->conf_mutex);
+	if (likely(!priv->join_status)) {
+		wsm_flush_tx(priv);
+		cw1200_update_listening(priv, true);
+		cw1200_update_filtering(priv);
+	}
+	if (unlikely(!priv->join_status))
+		cw1200_queue_remove(queue, priv->pending_frame_id);
+	else
+		cw1200_queue_requeue(queue, priv->pending_frame_id);
+	mutex_unlock(&priv->conf_mutex);
+	wsm_unlock_tx(priv);
+}
+
+void cw1200_join_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, join_work);
+	u8 queueId = cw1200_queue_get_queue_id(priv->pending_frame_id);
+	struct cw1200_queue *queue = &priv->tx_queue[queueId];
+	const struct cw1200_txpriv *txpriv = NULL;
+	struct sk_buff *skb = NULL;
+	const struct wsm_tx *wsm;
+	const struct ieee80211_hdr *frame;
+	const u8 *bssid;
+	struct cfg80211_bss *bss;
+	const u8 *ssidie;
+	const u8 *dtimie;
+	const struct ieee80211_tim_ie *tim = NULL;
+	struct wsm_protected_mgmt_policy mgmt_policy;
+
+	BUG_ON(queueId >= 4);
+	if (cw1200_queue_get_skb(queue,	priv->pending_frame_id,
+			&skb, &txpriv)) {
+		wsm_unlock_tx(priv);
+		return;
+	}
+	wsm = (struct wsm_tx *)&skb->data[0];
+	frame = (struct ieee80211_hdr *)&skb->data[txpriv->offset];
+	bssid = &frame->addr1[0]; /* AP SSID in a 802.11 frame */
+
+	BUG_ON(!wsm);
+	BUG_ON(!priv->channel);
+
+	if (unlikely(priv->join_status)) {
+		wsm_lock_tx(priv);
+		cw1200_unjoin_work(&priv->unjoin_work);
+	}
+
+	cancel_delayed_work_sync(&priv->join_timeout);
+
+	bss = cfg80211_get_bss(priv->hw->wiphy, NULL, bssid, NULL, 0, 0, 0);
+	if (!bss) {
+		cw1200_queue_remove(queue, priv->pending_frame_id);
+		wsm_unlock_tx(priv);
+		return;
+	}
+	ssidie = cfg80211_find_ie(WLAN_EID_SSID,
+		bss->information_elements,
+		bss->len_information_elements);
+	dtimie = cfg80211_find_ie(WLAN_EID_TIM,
+		bss->information_elements,
+		bss->len_information_elements);
+	if (dtimie)
+		tim = (struct ieee80211_tim_ie *)&dtimie[2];
+
+	mutex_lock(&priv->conf_mutex);
+	{
+		struct wsm_join join = {
+			.mode = (bss->capability & WLAN_CAPABILITY_IBSS) ?
+				WSM_JOIN_MODE_IBSS : WSM_JOIN_MODE_BSS,
+			.preambleType = WSM_JOIN_PREAMBLE_SHORT,
+			.probeForJoin = 1,
+			/* dtimPeriod will be updated after association */
+			.dtimPeriod = 1,
+			.beaconInterval = bss->beacon_interval,
+			/* basicRateSet will be updated after association */
+			.basicRateSet = 7,
+		};
+
+		/* BT Coex related changes */
+		if (priv->is_BT_Present) {
+			if (((priv->conf_listen_interval * 100) %
+					bss->beacon_interval) == 0)
+				priv->listen_interval =
+					((priv->conf_listen_interval * 100) /
+					bss->beacon_interval);
+			else
+				priv->listen_interval =
+					((priv->conf_listen_interval * 100) /
+					bss->beacon_interval + 1);
+		}
+
+		if (tim && tim->dtim_period > 1) {
+			join.dtimPeriod = tim->dtim_period;
+			priv->join_dtim_period = tim->dtim_period;
+		}
+		priv->beacon_int = bss->beacon_interval;
+		sta_printk(KERN_DEBUG "[STA] Join DTIM: %d, interval: %d\n",
+				join.dtimPeriod, priv->beacon_int);
+
+		join.channelNumber = priv->channel->hw_value;
+		join.band = (priv->channel->band == IEEE80211_BAND_5GHZ) ?
+			WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G;
+
+		memcpy(&join.bssid[0], bssid, sizeof(join.bssid));
+		memcpy(&priv->join_bssid[0], bssid, sizeof(priv->join_bssid));
+
+		if (ssidie) {
+			join.ssidLength = ssidie[1];
+			if (WARN_ON(join.ssidLength > sizeof(join.ssid)))
+				join.ssidLength = sizeof(join.ssid);
+			memcpy(&join.ssid[0], &ssidie[2], join.ssidLength);
+		}
+
+		if (priv->vif->p2p) {
+			join.flags |= WSM_JOIN_FLAGS_P2P_GO;
+			join.basicRateSet =
+				cw1200_rate_mask_to_wsm(priv, 0xFF0);
+		}
+
+		wsm_flush_tx(priv);
+
+		/* Queue unjoin if not associated in 3 sec. */
+		queue_delayed_work(priv->workqueue,
+			&priv->join_timeout, 3 * HZ);
+		/*Stay Awake for Join Timeout*/
+		cw1200_pm_stay_awake(&priv->pm_state, 3 * HZ);
+
+		cw1200_update_listening(priv, false);
+		/* BlockACK policy will be updated when assoc is done */
+		WARN_ON(wsm_set_block_ack_policy(priv,
+			0, priv->ba_tid_mask));
+
+		spin_lock_bh(&priv->ba_lock);
+		priv->ba_ena = false;
+		priv->ba_cnt = 0;
+		priv->ba_acc = 0;
+		priv->ba_hist = 0;
+		spin_unlock_bh(&priv->ba_lock);
+
+		mgmt_policy.protectedMgmtEnable = 0;
+		mgmt_policy.unprotectedMgmtFramesAllowed = 1;
+		mgmt_policy.encryptionForAuthFrame = 1;
+		wsm_set_protected_mgmt_policy(priv, &mgmt_policy);
+
+		if (wsm_join(priv, &join)) {
+			memset(&priv->join_bssid[0],
+				0, sizeof(priv->join_bssid));
+			cw1200_queue_remove(queue, priv->pending_frame_id);
+			cancel_delayed_work_sync(&priv->join_timeout);
+			cw1200_update_listening(priv, priv->listening);
+		} else {
+			/* Upload keys */
+			WARN_ON(cw1200_upload_keys(priv));
+			cw1200_queue_requeue(queue, priv->pending_frame_id);
+			priv->join_status = CW1200_JOIN_STATUS_STA;
+
+			/* Due to beacon filtering it is possible that the
+			 * AP's beacon is not known for the mac80211 stack.
+			 * Disable filtering temporary to make sure the stack
+			 * receives at least one */
+			priv->disable_beacon_filter = true;
+
+		}
+		cw1200_update_filtering(priv);
+	}
+	mutex_unlock(&priv->conf_mutex);
+	cfg80211_put_bss(bss);
+	wsm_unlock_tx(priv);
+}
+
+void cw1200_join_timeout(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, join_timeout.work);
+	sta_printk(KERN_DEBUG "[WSM] Issue unjoin command (TMO).\n");
+	wsm_lock_tx(priv);
+	cw1200_unjoin_work(&priv->unjoin_work);
+}
+
+void cw1200_unjoin_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, unjoin_work);
+
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+
+	del_timer_sync(&priv->ba_timer);
+	mutex_lock(&priv->conf_mutex);
+	if (unlikely(atomic_read(&priv->scan.in_progress))) {
+		if (priv->delayed_unjoin) {
+			wiphy_dbg(priv->hw->wiphy,
+				"%s: Delayed unjoin "
+				"is already scheduled.\n",
+				__func__);
+			wsm_unlock_tx(priv);
+		} else {
+			priv->delayed_unjoin = true;
+		}
+		mutex_unlock(&priv->conf_mutex);
+		return;
+	}
+
+	if (priv->join_status &&
+			priv->join_status > CW1200_JOIN_STATUS_STA) {
+		wiphy_err(priv->hw->wiphy,
+				"%s: Unexpected: join status: %d\n",
+				__func__, priv->join_status);
+		BUG_ON(1);
+	}
+	if (priv->join_status) {
+		cancel_work_sync(&priv->update_filtering_work);
+		memset(&priv->join_bssid[0], 0, sizeof(priv->join_bssid));
+		priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+
+		/* Unjoin is a reset. */
+		wsm_flush_tx(priv);
+		WARN_ON(wsm_keep_alive_period(priv, 0));
+		WARN_ON(wsm_reset(priv, &reset));
+		priv->join_dtim_period = 0;
+		WARN_ON(cw1200_setup_mac(priv));
+		cw1200_free_event_queue(priv);
+		cancel_work_sync(&priv->event_handler);
+		cancel_delayed_work_sync(&priv->connection_loss_work);
+		cw1200_update_listening(priv, priv->listening);
+		WARN_ON(wsm_set_block_ack_policy(priv,
+			0, priv->ba_tid_mask));
+		priv->disable_beacon_filter = false;
+		cw1200_update_filtering(priv);
+		priv->setbssparams_done = false;
+		memset(&priv->association_mode, 0,
+			sizeof(priv->association_mode));
+		memset(&priv->bss_params, 0, sizeof(priv->bss_params));
+		memset(&priv->firmware_ps_mode, 0,
+			sizeof(priv->firmware_ps_mode));
+		sta_printk(KERN_DEBUG "[STA] Unjoin.\n");
+	}
+	mutex_unlock(&priv->conf_mutex);
+	wsm_unlock_tx(priv);
+}
+
+int cw1200_enable_listening(struct cw1200_common *priv)
+{
+	struct wsm_start start = {
+		.mode = WSM_START_MODE_P2P_DEV,
+		.band = (priv->channel->band == IEEE80211_BAND_5GHZ) ?
+				WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G,
+		.channelNumber = priv->channel->hw_value,
+		.beaconInterval = 100,
+		.DTIMPeriod = 1,
+		.probeDelay = 0,
+		.basicRateSet = 0x0F,
+	};
+	return wsm_start(priv, &start);
+}
+
+int cw1200_disable_listening(struct cw1200_common *priv)
+{
+	int ret;
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+	ret = wsm_reset(priv, &reset);
+	return ret;
+}
+
+void cw1200_update_listening(struct cw1200_common *priv, bool enabled)
+{
+	if (enabled) {
+		switch (priv->join_status) {
+		case CW1200_JOIN_STATUS_PASSIVE:
+			if (!WARN_ON(cw1200_enable_listening(priv)))
+				priv->join_status = CW1200_JOIN_STATUS_MONITOR;
+			break;
+		default:
+			break;
+		}
+	} else {
+		switch (priv->join_status) {
+		case CW1200_JOIN_STATUS_MONITOR:
+			if (!WARN_ON(cw1200_disable_listening(priv)))
+				priv->join_status = CW1200_JOIN_STATUS_PASSIVE;
+		default:
+			break;
+		}
+	}
+}
+
+int cw1200_set_uapsd_param(struct cw1200_common *priv,
+				const struct wsm_edca_params *arg)
+{
+	int ret;
+	u16 uapsdFlags = 0;
+
+	/* Here's the mapping AC [queue, bit]
+	VO [0,3], VI [1, 2], BE [2, 1], BK [3, 0]*/
+
+	if (arg->params[0].uapsdEnable)
+		uapsdFlags |= 1 << 3;
+
+	if (arg->params[1].uapsdEnable)
+		uapsdFlags |= 1 << 2;
+
+	if (arg->params[2].uapsdEnable)
+		uapsdFlags |= 1 << 1;
+
+	if (arg->params[3].uapsdEnable)
+		uapsdFlags |= 1;
+
+	/* Currently pseudo U-APSD operation is not supported, so setting
+	* MinAutoTriggerInterval, MaxAutoTriggerInterval and
+	* AutoTriggerStep to 0 */
+
+	priv->uapsd_info.uapsdFlags = cpu_to_le16(uapsdFlags);
+	priv->uapsd_info.minAutoTriggerInterval = 0;
+	priv->uapsd_info.maxAutoTriggerInterval = 0;
+	priv->uapsd_info.autoTriggerStep = 0;
+
+	ret = wsm_set_uapsd_info(priv, &priv->uapsd_info);
+	return ret;
+}
+
+void cw1200_ba_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, ba_work);
+	u8 tx_ba_tid_mask;
+
+	if (priv->join_status != CW1200_JOIN_STATUS_STA)
+		return;
+	if (!priv->setbssparams_done)
+		return;
+
+	spin_lock_bh(&priv->ba_lock);
+	tx_ba_tid_mask = priv->ba_ena ? priv->ba_tid_mask : 0;
+	spin_unlock_bh(&priv->ba_lock);
+
+	WARN_ON(wsm_set_block_ack_policy(priv,
+		tx_ba_tid_mask, priv->ba_tid_mask));
+}
+
+void cw1200_ba_timer(unsigned long arg)
+{
+	bool ba_ena;
+	struct cw1200_common *priv =
+		(struct cw1200_common *)arg;
+
+	spin_lock_bh(&priv->ba_lock);
+	cw1200_debug_ba(priv, priv->ba_cnt, priv->ba_acc);
+
+	if (atomic_read(&priv->scan.in_progress))
+		goto skip_statistic_update;
+
+	ba_ena = (priv->ba_cnt >= CW1200_BLOCK_ACK_CNT &&
+			priv->ba_acc / priv->ba_cnt >= CW1200_BLOCK_ACK_THLD);
+	priv->ba_cnt = 0;
+	priv->ba_acc = 0;
+
+	if (ba_ena != priv->ba_ena) {
+		if (ba_ena || ++priv->ba_hist >= CW1200_BLOCK_ACK_HIST) {
+			priv->ba_ena = ba_ena;
+			priv->ba_hist = 0;
+			sta_printk(KERN_DEBUG "[STA] %s block ACK:\n",
+				ba_ena ? "enable" : "disable");
+			queue_work(priv->workqueue, &priv->ba_work);
+		}
+	} else if (priv->ba_hist)
+		--priv->ba_hist;
+
+skip_statistic_update:
+	spin_unlock_bh(&priv->ba_lock);
+}
diff --git a/drivers/staging/cw1200/sta.h b/drivers/staging/cw1200/sta.h
new file mode 100644
index 00000000000..4e4833afcf7
--- /dev/null
+++ b/drivers/staging/cw1200/sta.h
@@ -0,0 +1,87 @@
+/*
+ * Mac80211 STA interface for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef STA_H_INCLUDED
+#define STA_H_INCLUDED
+
+/* ******************************************************************** */
+/* mac80211 API								*/
+
+int cw1200_start(struct ieee80211_hw *dev);
+void cw1200_stop(struct ieee80211_hw *dev);
+int cw1200_add_interface(struct ieee80211_hw *dev,
+			 struct ieee80211_vif *vif);
+void cw1200_remove_interface(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif);
+int cw1200_config(struct ieee80211_hw *dev, u32 changed);
+void cw1200_configure_filter(struct ieee80211_hw *dev,
+			     unsigned int changed_flags,
+			     unsigned int *total_flags,
+			     u64 multicast);
+int cw1200_conf_tx(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
+		   u16 queue, const struct ieee80211_tx_queue_params *params);
+int cw1200_get_stats(struct ieee80211_hw *dev,
+		     struct ieee80211_low_level_stats *stats);
+/* Not more a part of interface?
+int cw1200_get_tx_stats(struct ieee80211_hw *dev,
+			struct ieee80211_tx_queue_stats *stats);
+*/
+int cw1200_set_key(struct ieee80211_hw *dev, enum set_key_cmd cmd,
+		   struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+		   struct ieee80211_key_conf *key);
+
+int cw1200_set_rts_threshold(struct ieee80211_hw *hw, u32 value);
+
+void cw1200_flush(struct ieee80211_hw *hw, bool drop);
+
+u64 cw1200_prepare_multicast(struct ieee80211_hw *hw,
+			     struct netdev_hw_addr_list *mc_list);
+
+int cw1200_set_pm(struct cw1200_common *priv, const struct wsm_set_pm *arg);
+
+/* ******************************************************************** */
+/* WSM callbacks							*/
+
+/* void cw1200_set_pm_complete_cb(struct cw1200_common *priv,
+	struct wsm_set_pm_complete *arg); */
+void cw1200_channel_switch_cb(struct cw1200_common *priv);
+
+/* ******************************************************************** */
+/* WSM events								*/
+
+void cw1200_free_event_queue(struct cw1200_common *priv);
+void cw1200_event_handler(struct work_struct *work);
+void cw1200_bss_loss_work(struct work_struct *work);
+void cw1200_connection_loss_work(struct work_struct *work);
+void cw1200_keep_alive_work(struct work_struct *work);
+void cw1200_tx_failure_work(struct work_struct *work);
+
+/* ******************************************************************** */
+/* Internal API								*/
+
+int cw1200_setup_mac(struct cw1200_common *priv);
+void cw1200_join_work(struct work_struct *work);
+void cw1200_join_timeout(struct work_struct *work);
+void cw1200_unjoin_work(struct work_struct *work);
+void cw1200_offchannel_work(struct work_struct *work);
+void cw1200_wep_key_work(struct work_struct *work);
+void cw1200_update_listening(struct cw1200_common *priv, bool enabled);
+void cw1200_update_filtering(struct cw1200_common *priv);
+void cw1200_update_filtering_work(struct work_struct *work);
+int __cw1200_flush(struct cw1200_common *priv, bool drop);
+int cw1200_enable_listening(struct cw1200_common *priv);
+int cw1200_disable_listening(struct cw1200_common *priv);
+int cw1200_set_uapsd_param(struct cw1200_common *priv,
+				const struct wsm_edca_params *arg);
+void cw1200_ba_work(struct work_struct *work);
+void cw1200_ba_timer(unsigned long arg);
+
+#endif
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
new file mode 100644
index 00000000000..7c0fa0b0f2a
--- /dev/null
+++ b/drivers/staging/cw1200/txrx.c
@@ -0,0 +1,1372 @@
+/*
+ * Datapath implementation for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <net/mac80211.h>
+#include <linux/etherdevice.h>
+
+#include "cw1200.h"
+#include "wsm.h"
+#include "bh.h"
+#include "ap.h"
+#include "debug.h"
+
+#if defined(CONFIG_CW1200_TX_POLICY_DEBUG)
+#define tx_policy_printk(...) printk(__VA_ARGS__)
+#else
+#define tx_policy_printk(...)
+#endif
+
+#define CW1200_INVALID_RATE_ID (0xFF)
+
+static int cw1200_handle_action_rx(struct cw1200_common *priv,
+				   struct sk_buff *skb);
+static const struct ieee80211_rate *
+cw1200_get_tx_rate(const struct cw1200_common *priv,
+		   const struct ieee80211_tx_rate *rate);
+
+/* ******************************************************************** */
+/* TX queue lock / unlock						*/
+
+static inline void cw1200_tx_queues_lock(struct cw1200_common *priv)
+{
+	int i;
+	for (i = 0; i < 4; ++i)
+		cw1200_queue_lock(&priv->tx_queue[i]);
+}
+
+static inline void cw1200_tx_queues_unlock(struct cw1200_common *priv)
+{
+	int i;
+	for (i = 0; i < 4; ++i)
+		cw1200_queue_unlock(&priv->tx_queue[i]);
+}
+
+/* ******************************************************************** */
+/* TX policy cache implementation					*/
+
+static void tx_policy_dump(struct tx_policy *policy)
+{
+	tx_policy_printk(KERN_DEBUG "[TX policy] "
+		"%.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X"
+		"%.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X"
+		"%.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X: %d\n",
+		policy->raw[0] & 0x0F,  policy->raw[0] >> 4,
+		policy->raw[1] & 0x0F,  policy->raw[1] >> 4,
+		policy->raw[2] & 0x0F,  policy->raw[2] >> 4,
+		policy->raw[3] & 0x0F,  policy->raw[3] >> 4,
+		policy->raw[4] & 0x0F,  policy->raw[4] >> 4,
+		policy->raw[5] & 0x0F,  policy->raw[5] >> 4,
+		policy->raw[6] & 0x0F,  policy->raw[6] >> 4,
+		policy->raw[7] & 0x0F,  policy->raw[7] >> 4,
+		policy->raw[8] & 0x0F,  policy->raw[8] >> 4,
+		policy->raw[9] & 0x0F,  policy->raw[9] >> 4,
+		policy->raw[10] & 0x0F,  policy->raw[10] >> 4,
+		policy->raw[11] & 0x0F,  policy->raw[11] >> 4,
+		policy->defined);
+}
+
+static void tx_policy_build(const struct cw1200_common *priv,
+	/* [out] */ struct tx_policy *policy,
+	struct ieee80211_tx_rate *rates, size_t count)
+{
+	int i, j;
+	unsigned limit = priv->short_frame_max_tx_count;
+	unsigned total = 0;
+	BUG_ON(rates[0].idx < 0);
+	memset(policy, 0, sizeof(*policy));
+
+	/* minstrel is buggy a little bit, so distille
+	 * incoming rates first. */
+
+	/* Sort rates in descending order. */
+	for (i = 1; i < count; ++i) {
+		if (rates[i].idx < 0) {
+			count = i;
+			break;
+		}
+		if (rates[i].idx > rates[i - 1].idx) {
+			struct ieee80211_tx_rate tmp = rates[i - 1];
+			rates[i - 1] = rates[i];
+			rates[i] = tmp;
+		}
+	}
+
+	/* Eliminate duplicates. */
+	total = rates[0].count;
+	for (i = 0, j = 1; j < count; ++j) {
+		if (rates[j].idx == rates[i].idx) {
+			rates[i].count += rates[j].count;
+		} else if (rates[j].idx > rates[i].idx) {
+			break;
+		} else {
+			++i;
+			if (i != j)
+				rates[i] = rates[j];
+		}
+		total += rates[j].count;
+	}
+	count = i + 1;
+
+	/* Re-fill policy trying to keep every requested rate and with
+	 * respect to the global max tx retransmission count. */
+	if (limit < count)
+		limit = count;
+	if (total > limit) {
+		for (i = 0; i < count; ++i) {
+			int left = count - i - 1;
+			if (rates[i].count > limit - left)
+				rates[i].count = limit - left;
+			limit -= rates[i].count;
+		}
+	}
+
+	/* HACK!!! Device has problems (at least) switching from
+	 * 54Mbps CTS to 1Mbps. This switch takes enormous amount
+	 * of time (100-200 ms), leading to valuable throughput drop.
+	 * As a workaround, additional g-rates are injected to the
+	 * policy.
+	 */
+	if (count == 2 && !(rates[0].flags & IEEE80211_TX_RC_MCS) &&
+			rates[0].idx > 4 && rates[0].count > 2 &&
+			rates[1].idx < 2) {
+		/* ">> 1" is an equivalent of "/ 2", but faster */
+		int mid_rate = (rates[0].idx + 4) >> 1;
+
+		/* Decrease number of retries for the initial rate */
+		rates[0].count -= 2;
+
+		if (mid_rate != 4) {
+			/* Keep fallback rate at 1Mbps. */
+			rates[3] = rates[1];
+
+			/* Inject 1 transmission on lowest g-rate */
+			rates[2].idx = 4;
+			rates[2].count = 1;
+			rates[2].flags = rates[1].flags;
+
+			/* Inject 1 transmission on mid-rate */
+			rates[1].idx = mid_rate;
+			rates[1].count = 1;
+
+			/* Fallback to 1 Mbps is a really bad thing,
+			 * so let's try to increase probability of
+			 * successful transmission on the lowest g rate
+			 * even more */
+			if (rates[0].count >= 3) {
+				--rates[0].count;
+				++rates[2].count;
+			}
+
+			/* Adjust amount of rates defined */
+			count += 2;
+		} else {
+			/* Keep fallback rate at 1Mbps. */
+			rates[2] = rates[1];
+
+			/* Inject 2 transmissions on lowest g-rate */
+			rates[1].idx = 4;
+			rates[1].count = 2;
+
+			/* Adjust amount of rates defined */
+			count += 1;
+		}
+	}
+
+	policy->defined = cw1200_get_tx_rate(priv, &rates[0])->hw_value + 1;
+
+	for (i = 0; i < count; ++i) {
+		register unsigned rateid, off, shift, retries;
+
+		rateid = cw1200_get_tx_rate(priv, &rates[i])->hw_value;
+		off = rateid >> 3;		/* eq. rateid / 8 */
+		shift = (rateid & 0x07) << 2;	/* eq. (rateid % 8) * 4 */
+
+		retries = rates[i].count;
+		if (unlikely(retries > 0x0F))
+			rates[i].count = retries = 0x0F;
+		policy->tbl[off] |= __cpu_to_le32(retries << shift);
+		policy->retry_count += retries;
+	}
+
+	tx_policy_printk(KERN_DEBUG "[TX policy] Policy (%d): " \
+		"%d:%d, %d:%d, %d:%d, %d:%d, %d:%d\n",
+		count,
+		rates[0].idx, rates[0].count,
+		rates[1].idx, rates[1].count,
+		rates[2].idx, rates[2].count,
+		rates[3].idx, rates[3].count,
+		rates[4].idx, rates[4].count);
+}
+
+static inline bool tx_policy_is_equal(const struct tx_policy *wanted,
+					const struct tx_policy *cached)
+{
+	size_t count = wanted->defined >> 1;
+	if (wanted->defined > cached->defined)
+		return false;
+	if (count) {
+		if (memcmp(wanted->raw, cached->raw, count))
+			return false;
+	}
+	if (wanted->defined & 1) {
+		if ((wanted->raw[count] & 0x0F) != (cached->raw[count] & 0x0F))
+			return false;
+	}
+	return true;
+}
+
+static int tx_policy_find(struct tx_policy_cache *cache,
+				const struct tx_policy *wanted)
+{
+	/* O(n) complexity. Not so good, but there's only 8 entries in
+	 * the cache.
+	 * Also lru helps to reduce search time. */
+	struct tx_policy_cache_entry *it;
+	/* First search for policy in "used" list */
+	list_for_each_entry(it, &cache->used, link) {
+		if (tx_policy_is_equal(wanted, &it->policy))
+			return it - cache->cache;
+	}
+	/* Then - in "free list" */
+	list_for_each_entry(it, &cache->free, link) {
+		if (tx_policy_is_equal(wanted, &it->policy))
+			return it - cache->cache;
+	}
+	return -1;
+}
+
+static inline void tx_policy_use(struct tx_policy_cache *cache,
+				 struct tx_policy_cache_entry *entry)
+{
+	++entry->policy.usage_count;
+	list_move(&entry->link, &cache->used);
+}
+
+static inline int tx_policy_release(struct tx_policy_cache *cache,
+				    struct tx_policy_cache_entry *entry)
+{
+	int ret = --entry->policy.usage_count;
+	if (!ret)
+		list_move(&entry->link, &cache->free);
+	return ret;
+}
+
+/* ******************************************************************** */
+/* External TX policy cache API						*/
+
+void tx_policy_init(struct cw1200_common *priv)
+{
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+	int i;
+
+	memset(cache, 0, sizeof(*cache));
+
+	spin_lock_init(&cache->lock);
+	INIT_LIST_HEAD(&cache->used);
+	INIT_LIST_HEAD(&cache->free);
+
+	for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i)
+		list_add(&cache->cache[i].link, &cache->free);
+}
+
+static int tx_policy_get(struct cw1200_common *priv,
+		  struct ieee80211_tx_rate *rates,
+		  size_t count, bool *renew)
+{
+	int idx;
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+	struct tx_policy wanted;
+
+	tx_policy_build(priv, &wanted, rates, count);
+
+	spin_lock_bh(&cache->lock);
+	if (WARN_ON_ONCE(list_empty(&cache->free))) {
+		spin_unlock_bh(&cache->lock);
+		return CW1200_INVALID_RATE_ID;
+	}
+	idx = tx_policy_find(cache, &wanted);
+	if (idx >= 0) {
+		tx_policy_printk(KERN_DEBUG "[TX policy] Used TX policy: %d\n",
+					idx);
+		*renew = false;
+	} else {
+		struct tx_policy_cache_entry *entry;
+		*renew = true;
+		/* If policy is not found create a new one
+		 * using the oldest entry in "free" list */
+		entry = list_entry(cache->free.prev,
+			struct tx_policy_cache_entry, link);
+		entry->policy = wanted;
+		idx = entry - cache->cache;
+		tx_policy_printk(KERN_DEBUG "[TX policy] New TX policy: %d\n",
+					idx);
+		tx_policy_dump(&entry->policy);
+	}
+	tx_policy_use(cache, &cache->cache[idx]);
+	if (unlikely(list_empty(&cache->free))) {
+		/* Lock TX queues. */
+		cw1200_tx_queues_lock(priv);
+	}
+	spin_unlock_bh(&cache->lock);
+	return idx;
+}
+
+static void tx_policy_put(struct cw1200_common *priv, int idx)
+{
+	int usage, locked;
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+
+	spin_lock_bh(&cache->lock);
+	locked = list_empty(&cache->free);
+	usage = tx_policy_release(cache, &cache->cache[idx]);
+	if (unlikely(locked) && !usage) {
+		/* Unlock TX queues. */
+		cw1200_tx_queues_unlock(priv);
+	}
+	spin_unlock_bh(&cache->lock);
+}
+
+/*
+bool tx_policy_cache_full(struct cw1200_common *priv)
+{
+	bool ret;
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+	spin_lock_bh(&cache->lock);
+	ret = list_empty(&cache->free);
+	spin_unlock_bh(&cache->lock);
+	return ret;
+}
+*/
+
+static int tx_policy_upload(struct cw1200_common *priv)
+{
+	struct tx_policy_cache *cache = &priv->tx_policy_cache;
+	int i;
+	struct wsm_set_tx_rate_retry_policy arg = {
+		.hdr = {
+			.numTxRatePolicies = 0,
+		}
+	};
+	spin_lock_bh(&cache->lock);
+
+	/* Upload only modified entries. */
+	for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i) {
+		struct tx_policy *src = &cache->cache[i].policy;
+		if (src->retry_count && !src->uploaded) {
+			struct wsm_set_tx_rate_retry_policy_policy *dst =
+				&arg.tbl[arg.hdr.numTxRatePolicies];
+			dst->policyIndex = i;
+			dst->shortRetryCount = priv->short_frame_max_tx_count;
+			dst->longRetryCount = priv->long_frame_max_tx_count;
+
+			/* BIT(2) - Terminate retries when Tx rate retry policy
+			 *          finishes.
+			 * BIT(3) - Count initial frame transmission as part of
+			 *          rate retry counting but not as a retry
+			 *          attempt */
+			dst->policyFlags = BIT(2) | BIT(3);
+
+			memcpy(dst->rateCountIndices, src->tbl,
+					sizeof(dst->rateCountIndices));
+			src->uploaded = 1;
+			++arg.hdr.numTxRatePolicies;
+		}
+	}
+	spin_unlock_bh(&cache->lock);
+	cw1200_debug_tx_cache_miss(priv);
+	tx_policy_printk(KERN_DEBUG "[TX policy] Upload %d policies\n",
+				arg.hdr.numTxRatePolicies);
+	return wsm_set_tx_rate_retry_policy(priv, &arg);
+}
+
+void tx_policy_upload_work(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, tx_policy_upload_work);
+
+	tx_policy_printk(KERN_DEBUG "[TX] TX policy upload.\n");
+	WARN_ON(tx_policy_upload(priv));
+
+	wsm_unlock_tx(priv);
+	cw1200_tx_queues_unlock(priv);
+}
+
+/* ******************************************************************** */
+/* cw1200 TX implementation						*/
+
+struct cw1200_txinfo {
+	struct sk_buff *skb;
+	unsigned queue;
+	struct ieee80211_tx_info *tx_info;
+	const struct ieee80211_rate *rate;
+	struct ieee80211_hdr *hdr;
+	size_t hdrlen;
+	const u8 *da;
+	struct cw1200_sta_priv *sta_priv;
+	struct cw1200_txpriv txpriv;
+};
+
+u32 cw1200_rate_mask_to_wsm(struct cw1200_common *priv, u32 rates)
+{
+	u32 ret = 0;
+	int i;
+	for (i = 0; i < 32; ++i) {
+		if (rates & BIT(i))
+			ret |= BIT(priv->rates[i].hw_value);
+	}
+	return ret;
+}
+
+static const struct ieee80211_rate *
+cw1200_get_tx_rate(const struct cw1200_common *priv,
+		   const struct ieee80211_tx_rate *rate)
+{
+	if (rate->idx < 0)
+		return NULL;
+	if (rate->flags & IEEE80211_TX_RC_MCS)
+		return &priv->mcs_rates[rate->idx];
+	return &priv->hw->wiphy->bands[priv->channel->band]->
+		bitrates[rate->idx];
+}
+
+static int
+cw1200_tx_h_calc_link_ids(struct cw1200_common *priv,
+			  struct cw1200_txinfo *t)
+{
+
+	if (likely(t->tx_info->control.sta && t->sta_priv->link_id))
+		t->txpriv.raw_link_id =
+				t->txpriv.link_id =
+				t->sta_priv->link_id;
+	else if (priv->mode != NL80211_IFTYPE_AP)
+		t->txpriv.raw_link_id =
+				t->txpriv.link_id = 0;
+	else if (is_multicast_ether_addr(t->da)) {
+		if (priv->enable_beacon) {
+			t->txpriv.raw_link_id = 0;
+			t->txpriv.link_id = CW1200_LINK_ID_AFTER_DTIM;
+		} else {
+			t->txpriv.raw_link_id = 0;
+			t->txpriv.link_id = 0;
+		}
+	} else {
+		t->txpriv.link_id = cw1200_find_link_id(priv, t->da);
+		if (!t->txpriv.link_id)
+			t->txpriv.link_id = cw1200_alloc_link_id(priv, t->da);
+		if (!t->txpriv.link_id) {
+			wiphy_err(priv->hw->wiphy,
+				"%s: No more link IDs available.\n",
+				__func__);
+			return -ENOENT;
+		}
+		t->txpriv.raw_link_id = t->txpriv.link_id;
+	}
+	if (t->txpriv.raw_link_id)
+		priv->link_id_db[t->txpriv.raw_link_id - 1].timestamp =
+				jiffies;
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	if (t->tx_info->control.sta &&
+			(t->tx_info->control.sta->uapsd_queues & BIT(t->queue)))
+		t->txpriv.link_id = CW1200_LINK_ID_UAPSD;
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+	return 0;
+}
+
+static void
+cw1200_tx_h_pm(struct cw1200_common *priv,
+	       struct cw1200_txinfo *t)
+{
+	if (unlikely(ieee80211_is_auth(t->hdr->frame_control))) {
+		u32 mask = ~BIT(t->txpriv.raw_link_id);
+		spin_lock_bh(&priv->ps_state_lock);
+		priv->sta_asleep_mask &= mask;
+		priv->pspoll_mask &= mask;
+		spin_unlock_bh(&priv->ps_state_lock);
+	}
+}
+
+static void
+cw1200_tx_h_calc_tid(struct cw1200_common *priv,
+		     struct cw1200_txinfo *t)
+{
+	if (ieee80211_is_data_qos(t->hdr->frame_control)) {
+		u8 *qos = ieee80211_get_qos_ctl(t->hdr);
+		t->txpriv.tid = qos[0] & IEEE80211_QOS_CTL_TID_MASK;
+	} else if (ieee80211_is_data(t->hdr->frame_control)) {
+		t->txpriv.tid = 0;
+	}
+}
+
+/* IV/ICV injection. */
+/* TODO: Quite unoptimal. It's better co modify mac80211
+ * to reserve space for IV */
+static int
+cw1200_tx_h_crypt(struct cw1200_common *priv,
+		  struct cw1200_txinfo *t)
+{
+	size_t iv_len;
+	size_t icv_len;
+	u8 *icv;
+	u8 *newhdr;
+
+	if (!t->tx_info->control.hw_key ||
+	    !(t->hdr->frame_control &
+	     __cpu_to_le32(IEEE80211_FCTL_PROTECTED)))
+		return 0;
+
+	iv_len = t->tx_info->control.hw_key->iv_len;
+	icv_len = t->tx_info->control.hw_key->icv_len;
+
+	if (t->tx_info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
+		icv_len += 8; /* MIC */
+
+	if ((skb_headroom(t->skb) + skb_tailroom(t->skb) <
+			 iv_len + icv_len + WSM_TX_EXTRA_HEADROOM) ||
+			(skb_headroom(t->skb) <
+			 iv_len + WSM_TX_EXTRA_HEADROOM)) {
+		wiphy_err(priv->hw->wiphy,
+			"Bug: no space allocated for crypto headers.\n"
+			"headroom: %d, tailroom: %d, "
+			"req_headroom: %d, req_tailroom: %d\n"
+			"Please fix it in cw1200_get_skb().\n",
+			skb_headroom(t->skb), skb_tailroom(t->skb),
+			iv_len + WSM_TX_EXTRA_HEADROOM, icv_len);
+		return -ENOMEM;
+	} else if (skb_tailroom(t->skb) < icv_len) {
+		size_t offset = icv_len - skb_tailroom(t->skb);
+		u8 *p;
+		wiphy_warn(priv->hw->wiphy,
+			"Slowpath: tailroom is not big enough. "
+			"Req: %d, got: %d.\n",
+			icv_len, skb_tailroom(t->skb));
+
+		p = skb_push(t->skb, offset);
+		memmove(p, &p[offset], t->skb->len - offset);
+		skb_trim(t->skb, t->skb->len - offset);
+	}
+
+	newhdr = skb_push(t->skb, iv_len);
+	memmove(newhdr, newhdr + iv_len, t->hdrlen);
+	t->hdr = (struct ieee80211_hdr *) newhdr;
+	t->hdrlen += iv_len;
+	icv = skb_put(t->skb, icv_len);
+
+	return 0;
+}
+
+static int
+cw1200_tx_h_align(struct cw1200_common *priv,
+		  struct cw1200_txinfo *t,
+		  u8 *flags)
+{
+	size_t offset = (size_t)t->skb->data & 3;
+
+	if (!offset)
+		return 0;
+
+	if (offset & 1) {
+		wiphy_err(priv->hw->wiphy,
+			"Bug: attempt to transmit a frame "
+			"with wrong alignment: %d\n",
+			offset);
+		return -EINVAL;
+	}
+
+	if (skb_headroom(t->skb) < offset) {
+		wiphy_err(priv->hw->wiphy,
+			"Bug: no space allocated "
+			"for DMA alignment.\n"
+			"headroom: %d\n",
+			skb_headroom(t->skb));
+		return -ENOMEM;
+	}
+	skb_push(t->skb, offset);
+	t->hdrlen += offset;
+	t->txpriv.offset += offset;
+	*flags |= WSM_TX_2BYTES_SHIFT;
+	cw1200_debug_tx_align(priv);
+	return 0;
+}
+
+static int
+cw1200_tx_h_action(struct cw1200_common *priv,
+		   struct cw1200_txinfo *t)
+{
+	struct ieee80211_mgmt *mgmt =
+		(struct ieee80211_mgmt *)t->hdr;
+	if (ieee80211_is_action(t->hdr->frame_control) &&
+			mgmt->u.action.category == WLAN_CATEGORY_BACK)
+		return 1;
+	else
+		return 0;
+}
+
+/* Add WSM header */
+static struct wsm_tx *
+cw1200_tx_h_wsm(struct cw1200_common *priv,
+		struct cw1200_txinfo *t)
+{
+	struct wsm_tx *wsm;
+
+	if (skb_headroom(t->skb) < sizeof(struct wsm_tx)) {
+		wiphy_err(priv->hw->wiphy,
+			"Bug: no space allocated "
+			"for WSM header.\n"
+			"headroom: %d\n",
+			skb_headroom(t->skb));
+		return NULL;
+	}
+
+	wsm = (struct wsm_tx *)skb_push(t->skb, sizeof(struct wsm_tx));
+	t->txpriv.offset += sizeof(struct wsm_tx);
+	memset(wsm, 0, sizeof(*wsm));
+	wsm->hdr.len = __cpu_to_le16(t->skb->len);
+	wsm->hdr.id = __cpu_to_le16(0x0004);
+	wsm->queueId = wsm_queue_id_to_wsm(t->queue);
+	return wsm;
+}
+
+/* BT Coex specific handling */
+static void
+cw1200_tx_h_bt(struct cw1200_common *priv,
+	       struct cw1200_txinfo *t,
+	       struct wsm_tx *wsm)
+{
+	u8 priority = 0;
+
+	if (!priv->is_BT_Present)
+		return;
+
+	if (unlikely(ieee80211_is_nullfunc(t->hdr->frame_control)))
+		priority = WSM_EPTA_PRIORITY_MGT;
+	else if (ieee80211_is_data(t->hdr->frame_control)) {
+		/* Skip LLC SNAP header (+6) */
+		u8 *payload = &t->skb->data[t->hdrlen];
+		u16 *ethertype = (u16 *) &payload[6];
+		if (unlikely(*ethertype == __be16_to_cpu(ETH_P_PAE)))
+			priority = WSM_EPTA_PRIORITY_EAPOL;
+	} else if (unlikely(ieee80211_is_assoc_req(t->hdr->frame_control) ||
+		ieee80211_is_reassoc_req(t->hdr->frame_control))) {
+		struct ieee80211_mgmt *mgt_frame =
+				(struct ieee80211_mgmt *)t->hdr;
+
+		if (mgt_frame->u.assoc_req.listen_interval <
+						priv->listen_interval) {
+			txrx_printk(KERN_DEBUG
+				"Modified Listen Interval to %d from %d\n",
+				priv->listen_interval,
+				mgt_frame->u.assoc_req.listen_interval);
+			/* Replace listen interval derieved from
+			 * the one read from SDD */
+			mgt_frame->u.assoc_req.listen_interval =
+				priv->listen_interval;
+		}
+	}
+
+	if (likely(!priority)) {
+		if (ieee80211_is_action(t->hdr->frame_control))
+			priority = WSM_EPTA_PRIORITY_ACTION;
+		else if (ieee80211_is_mgmt(t->hdr->frame_control))
+			priority = WSM_EPTA_PRIORITY_MGT;
+		else if ((wsm->queueId == WSM_QUEUE_VOICE))
+			priority = WSM_EPTA_PRIORITY_VOICE;
+		else if ((wsm->queueId == WSM_QUEUE_VIDEO))
+			priority = WSM_EPTA_PRIORITY_VIDEO;
+		else
+			priority = WSM_EPTA_PRIORITY_DATA;
+	}
+
+	txrx_printk(KERN_DEBUG "[TX] EPTA priority %d.\n",
+		priority);
+
+	wsm->flags |= priority << 1;
+}
+
+static int
+cw1200_tx_h_rate_policy(struct cw1200_common *priv,
+			struct cw1200_txinfo *t,
+			struct wsm_tx *wsm)
+{
+	bool tx_policy_renew = false;
+
+	t->txpriv.rate_id = tx_policy_get(priv,
+		t->tx_info->control.rates, IEEE80211_TX_MAX_RATES,
+		&tx_policy_renew);
+	if (t->txpriv.rate_id == CW1200_INVALID_RATE_ID)
+		return -EFAULT;
+
+	wsm->flags |= t->txpriv.rate_id << 4;
+
+	t->rate = cw1200_get_tx_rate(priv,
+		&t->tx_info->control.rates[0]),
+	wsm->maxTxRate = t->rate->hw_value;
+	if (t->rate->flags & IEEE80211_TX_RC_MCS) {
+		if (cw1200_ht_greenfield(&priv->ht_info))
+			wsm->htTxParameters |=
+				__cpu_to_le32(WSM_HT_TX_GREENFIELD);
+		else
+			wsm->htTxParameters |=
+				__cpu_to_le32(WSM_HT_TX_MIXED);
+	}
+
+	if (tx_policy_renew) {
+		tx_policy_printk(KERN_DEBUG "[TX] TX policy renew.\n");
+		/* It's not so optimal to stop TX queues every now and then.
+		 * Maybe it's better to reimplement task scheduling with
+		 * a counter. */
+		/* cw1200_tx_queues_lock(priv); */
+		/* Definetly better. TODO. */
+		wsm_lock_tx_async(priv);
+		cw1200_tx_queues_lock(priv);
+		if (queue_work(priv->workqueue,
+				&priv->tx_policy_upload_work) <= 0) {
+			cw1200_tx_queues_unlock(priv);
+			wsm_unlock_tx(priv);
+		}
+	}
+	return 0;
+}
+
+static bool
+cw1200_tx_h_pm_state(struct cw1200_common *priv,
+		     struct cw1200_txinfo *t)
+{
+	int was_buffered = 1;
+
+	if (t->txpriv.link_id == CW1200_LINK_ID_AFTER_DTIM &&
+			!priv->buffered_multicasts) {
+		priv->buffered_multicasts = true;
+		if (priv->sta_asleep_mask)
+			queue_work(priv->workqueue,
+				&priv->multicast_start_work);
+	}
+
+	if (t->txpriv.raw_link_id && t->txpriv.tid < CW1200_MAX_TID)
+		was_buffered = priv->link_id_db[t->txpriv.raw_link_id - 1]
+				.buffered[t->txpriv.tid]++;
+
+	return !was_buffered;
+}
+
+static void
+cw1200_tx_h_ba_stat(struct cw1200_common *priv,
+		    struct cw1200_txinfo *t)
+{
+	if (priv->join_status != CW1200_JOIN_STATUS_STA)
+		return;
+	if (!cw1200_is_ht(&priv->ht_info))
+		return;
+	if (!priv->setbssparams_done)
+		return;
+	if (!ieee80211_is_data(t->hdr->frame_control))
+		return;
+
+	spin_lock_bh(&priv->ba_lock);
+	priv->ba_acc += t->skb->len - t->hdrlen;
+	if (!priv->ba_cnt++) {
+		mod_timer(&priv->ba_timer,
+			jiffies + CW1200_BLOCK_ACK_INTERVAL);
+	}
+	spin_unlock_bh(&priv->ba_lock);
+}
+
+/* ******************************************************************** */
+
+void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	struct cw1200_common *priv = dev->priv;
+	struct cw1200_txinfo t = {
+		.skb = skb,
+		.queue = skb_get_queue_mapping(skb),
+		.tx_info = IEEE80211_SKB_CB(skb),
+		.hdr = (struct ieee80211_hdr *)skb->data,
+		.txpriv.tid = CW1200_MAX_TID,
+		.txpriv.rate_id = CW1200_INVALID_RATE_ID,
+	};
+	struct ieee80211_sta *sta;
+	struct wsm_tx *wsm;
+	bool tid_update = 0;
+	u8 flags = 0;
+	int ret;
+
+	t.hdrlen = ieee80211_hdrlen(t.hdr->frame_control);
+	t.da = ieee80211_get_DA(t.hdr);
+	t.sta_priv =
+		(struct cw1200_sta_priv *)&t.tx_info->control.sta->drv_priv;
+
+	if (WARN_ON(t.queue >= 4))
+		goto drop;
+
+	ret = cw1200_tx_h_calc_link_ids(priv, &t);
+	if (ret)
+		goto drop;
+
+	txrx_printk(KERN_DEBUG "[TX] TX %d bytes "
+			"(queue: %d, link_id: %d (%d)).\n",
+			skb->len, t.queue, t.txpriv.link_id,
+			t.txpriv.raw_link_id);
+
+	cw1200_tx_h_pm(priv, &t);
+	cw1200_tx_h_calc_tid(priv, &t);
+	ret = cw1200_tx_h_crypt(priv, &t);
+	if (ret)
+		goto drop;
+	ret = cw1200_tx_h_align(priv, &t, &flags);
+	if (ret)
+		goto drop;
+	ret = cw1200_tx_h_action(priv, &t);
+	if (ret)
+		goto drop;
+	wsm = cw1200_tx_h_wsm(priv, &t);
+	if (!wsm) {
+		ret = -ENOMEM;
+		goto drop;
+	}
+	wsm->flags |= flags;
+	cw1200_tx_h_bt(priv, &t, wsm);
+	ret = cw1200_tx_h_rate_policy(priv, &t, wsm);
+	if (ret)
+		goto drop;
+
+	rcu_read_lock();
+	sta = rcu_dereference(t.tx_info->control.sta);
+
+	cw1200_tx_h_ba_stat(priv, &t);
+	spin_lock_bh(&priv->ps_state_lock);
+	{
+		tid_update = cw1200_tx_h_pm_state(priv, &t);
+		BUG_ON(cw1200_queue_put(&priv->tx_queue[t.queue],
+				t.skb, &t.txpriv));
+	}
+	spin_unlock_bh(&priv->ps_state_lock);
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	if (tid_update && sta)
+		ieee80211_sta_set_buffered(sta,
+				t.txpriv.tid, true);
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+
+	rcu_read_unlock();
+
+	cw1200_bh_wakeup(priv);
+
+	return;
+
+drop:
+	cw1200_skb_dtor(priv, skb, &t.txpriv);
+	return;
+}
+
+/* ******************************************************************** */
+
+static int cw1200_handle_action_rx(struct cw1200_common *priv,
+				   struct sk_buff *skb)
+{
+	struct ieee80211_mgmt *mgmt = (void *)skb->data;
+
+	/* Filter block ACK negotiation: fully controlled by firmware */
+	if (mgmt->u.action.category == WLAN_CATEGORY_BACK)
+		return 1;
+
+	return 0;
+}
+
+static int cw1200_handle_pspoll(struct cw1200_common *priv,
+				struct sk_buff *skb)
+{
+	struct ieee80211_sta *sta;
+	struct ieee80211_pspoll *pspoll =
+		(struct ieee80211_pspoll *) skb->data;
+	int link_id = 0;
+	u32 pspoll_mask = 0;
+	int drop = 1;
+	int i;
+
+	if (priv->join_status != CW1200_JOIN_STATUS_AP)
+		goto done;
+	if (memcmp(priv->vif->addr, pspoll->bssid, ETH_ALEN))
+		goto done;
+
+	rcu_read_lock();
+	sta = ieee80211_find_sta(priv->vif, pspoll->ta);
+	if (sta) {
+		struct cw1200_sta_priv *sta_priv;
+		sta_priv = (struct cw1200_sta_priv *)&sta->drv_priv;
+		link_id = sta_priv->link_id;
+		pspoll_mask = BIT(sta_priv->link_id);
+	}
+	rcu_read_unlock();
+	if (!link_id)
+		goto done;
+
+	priv->pspoll_mask |= pspoll_mask;
+	drop = 0;
+
+	/* Do not report pspols if data for given link id is
+	 * queued already. */
+	for (i = 0; i < 4; ++i) {
+		if (cw1200_queue_get_num_queued(
+				&priv->tx_queue[i],
+				pspoll_mask)) {
+			cw1200_bh_wakeup(priv);
+			drop = 1;
+			break;
+		}
+	}
+	txrx_printk(KERN_DEBUG "[RX] PSPOLL: %s\n", drop ? "local" : "fwd");
+done:
+	return drop;
+}
+
+/* ******************************************************************** */
+
+void cw1200_tx_confirm_cb(struct cw1200_common *priv,
+			  struct wsm_tx_confirm *arg)
+{
+	u8 queue_id = cw1200_queue_get_queue_id(arg->packetID);
+	struct cw1200_queue *queue = &priv->tx_queue[queue_id];
+	struct sk_buff *skb;
+	const struct cw1200_txpriv *txpriv;
+
+	txrx_printk(KERN_DEBUG "[TX] TX confirm: %d, %d.\n",
+		arg->status, arg->ackFailures);
+
+	if (unlikely(cw1200_itp_tx_running(priv)))
+		return;
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		return;
+	}
+
+	if (WARN_ON(queue_id >= 4))
+		return;
+
+	if (arg->status)
+		txrx_printk(KERN_DEBUG "TX failed: %d.\n",
+				arg->status);
+
+	if ((arg->status == WSM_REQUEUE) &&
+	    (arg->flags & WSM_TX_STATUS_REQUEUE)) {
+		/* "Requeue" means "implicit suspend" */
+		struct wsm_suspend_resume suspend = {
+			.link_id = arg->link_id,
+			.stop = 1,
+			.multicast = !arg->link_id,
+		};
+		cw1200_suspend_resume(priv, &suspend);
+		wiphy_warn(priv->hw->wiphy, "Requeue for link_id %d (try %d)."
+			" STAs asleep: 0x%.8X\n",
+			arg->link_id,
+			cw1200_queue_get_generation(arg->packetID) + 1,
+			priv->sta_asleep_mask);
+		WARN_ON(cw1200_queue_requeue(queue,
+				arg->packetID));
+		spin_lock_bh(&priv->ps_state_lock);
+		if (!arg->link_id) {
+			priv->buffered_multicasts = true;
+			if (priv->sta_asleep_mask) {
+				queue_work(priv->workqueue,
+					&priv->multicast_start_work);
+			}
+		}
+		spin_unlock_bh(&priv->ps_state_lock);
+	} else if (!WARN_ON(cw1200_queue_get_skb(
+			queue, arg->packetID, &skb, &txpriv))) {
+		struct ieee80211_tx_info *tx = IEEE80211_SKB_CB(skb);
+		int tx_count = arg->ackFailures;
+		u8 ht_flags = 0;
+		int i;
+
+		if (cw1200_ht_greenfield(&priv->ht_info))
+			ht_flags |= IEEE80211_TX_RC_GREEN_FIELD;
+
+		if (likely(!arg->status)) {
+			tx->flags |= IEEE80211_TX_STAT_ACK;
+			priv->cqm_tx_failure_count = 0;
+			++tx_count;
+			cw1200_debug_txed(priv);
+			if (arg->flags & WSM_TX_STATUS_AGGREGATION) {
+				/* Do not report aggregation to mac80211:
+				 * it confuses minstrel a lot. */
+				/* tx->flags |= IEEE80211_TX_STAT_AMPDU; */
+				cw1200_debug_txed_agg(priv);
+			}
+		} else {
+			spin_lock(&priv->bss_loss_lock);
+			if (priv->bss_loss_status ==
+					CW1200_BSS_LOSS_CONFIRMING &&
+					priv->bss_loss_confirm_id ==
+					arg->packetID) {
+				priv->bss_loss_status =
+					CW1200_BSS_LOSS_CONFIRMED;
+				spin_unlock(&priv->bss_loss_lock);
+				cancel_delayed_work(&priv->bss_loss_work);
+				queue_delayed_work(priv->workqueue,
+						&priv->bss_loss_work, 0);
+			} else
+				spin_unlock(&priv->bss_loss_lock);
+
+			/* TODO: Update TX failure counters */
+			if (unlikely(priv->cqm_tx_failure_thold &&
+			     (++priv->cqm_tx_failure_count >
+			      priv->cqm_tx_failure_thold))) {
+				priv->cqm_tx_failure_thold = 0;
+				queue_work(priv->workqueue,
+						&priv->tx_failure_work);
+			}
+			if (tx_count)
+				++tx_count;
+		}
+
+		for (i = 0; i < IEEE80211_TX_MAX_RATES; ++i) {
+			if (tx->status.rates[i].count >= tx_count) {
+				tx->status.rates[i].count = tx_count;
+				break;
+			}
+			tx_count -= tx->status.rates[i].count;
+			if (tx->status.rates[i].flags & IEEE80211_TX_RC_MCS)
+				tx->status.rates[i].flags |= ht_flags;
+		}
+
+		for (++i; i < IEEE80211_TX_MAX_RATES; ++i) {
+			tx->status.rates[i].count = 0;
+			tx->status.rates[i].idx = -1;
+		}
+
+
+		cw1200_queue_remove(queue, arg->packetID);
+	}
+}
+
+static void cw1200_notify_buffered_tx(struct cw1200_common *priv,
+			       struct sk_buff *skb, int link_id, int tid)
+{
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	struct ieee80211_sta *sta;
+	struct ieee80211_hdr *hdr;
+	u8 *buffered;
+	u8 still_buffered = 0;
+
+	if (link_id && tid < CW1200_MAX_TID) {
+		buffered = priv->link_id_db
+				[link_id - 1].buffered;
+
+		spin_lock_bh(&priv->ps_state_lock);
+		if (!WARN_ON(!buffered[tid]))
+			still_buffered = --buffered[tid];
+		spin_unlock_bh(&priv->ps_state_lock);
+
+		if (!still_buffered && tid < CW1200_MAX_TID) {
+			hdr = (struct ieee80211_hdr *) skb->data;
+			rcu_read_lock();
+			sta = ieee80211_find_sta(priv->vif, hdr->addr1);
+			if (sta)
+				ieee80211_sta_set_buffered(sta, tid, false);
+			rcu_read_unlock();
+		}
+	}
+#endif /* CONFIG_CW1200_USE_STE_EXTENSIONS */
+}
+
+void cw1200_skb_dtor(struct cw1200_common *priv,
+		     struct sk_buff *skb,
+		     const struct cw1200_txpriv *txpriv)
+{
+	skb_pull(skb, txpriv->offset);
+	if (txpriv->rate_id != CW1200_INVALID_RATE_ID) {
+		cw1200_notify_buffered_tx(priv, skb,
+				txpriv->raw_link_id, txpriv->tid);
+		tx_policy_put(priv, txpriv->rate_id);
+	}
+	if (likely(!cw1200_is_itp(priv)))
+		ieee80211_tx_status(priv->hw, skb);
+}
+
+void cw1200_rx_cb(struct cw1200_common *priv,
+		  struct wsm_rx *arg,
+		  struct sk_buff **skb_p)
+{
+	struct sk_buff *skb = *skb_p;
+	struct ieee80211_rx_status *hdr = IEEE80211_SKB_RXCB(skb);
+	struct ieee80211_hdr *frame = (struct ieee80211_hdr *)skb->data;
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
+#endif
+	struct cw1200_link_entry *entry = NULL;
+	unsigned long grace_period;
+	bool early_data = false;
+	hdr->flag = 0;
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		goto drop;
+	}
+
+	if (arg->link_id && arg->link_id <= CW1200_MAX_STA_IN_AP_MODE) {
+		entry =	&priv->link_id_db[arg->link_id - 1];
+		if (entry->status == CW1200_LINK_SOFT &&
+				ieee80211_is_data(frame->frame_control))
+			early_data = true;
+		entry->timestamp = jiffies;
+	}
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	else if ((priv->vif->p2p == WSM_START_MODE_P2P_GO)
+			&& ieee80211_is_action(frame->frame_control)
+			&& (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
+		txrx_printk(KERN_DEBUG "[RX] Going to MAP&RESET link ID\n");
+
+		if (work_pending(&priv->linkid_reset_work))
+			WARN_ON(1);
+
+		memcpy(&priv->action_frame_sa[0],
+				ieee80211_get_SA(frame), ETH_ALEN);
+		priv->action_linkid = 0;
+		schedule_work(&priv->linkid_reset_work);
+	}
+
+	if (arg->link_id && (priv->vif->p2p == WSM_START_MODE_P2P_GO)
+			&& ieee80211_is_action(frame->frame_control)
+			&& (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
+		/* Link ID already exists for the ACTION frame.
+		 * Reset and Remap */
+		if (work_pending(&priv->linkid_reset_work))
+			WARN_ON(1);
+		memcpy(&priv->action_frame_sa[0],
+				ieee80211_get_SA(frame), ETH_ALEN);
+		priv->action_linkid = arg->link_id;
+		schedule_work(&priv->linkid_reset_work);
+	}
+#endif
+	if (unlikely(arg->status)) {
+		if (arg->status == WSM_STATUS_MICFAILURE) {
+			txrx_printk(KERN_DEBUG "[RX] MIC failure.\n");
+			hdr->flag |= RX_FLAG_MMIC_ERROR;
+		} else if (arg->status == WSM_STATUS_NO_KEY_FOUND) {
+			txrx_printk(KERN_DEBUG "[RX] No key found.\n");
+			goto drop;
+		} else {
+			txrx_printk(KERN_DEBUG "[RX] Receive failure: %d.\n",
+				arg->status);
+			goto drop;
+		}
+	}
+
+	if (skb->len < sizeof(struct ieee80211_pspoll)) {
+		wiphy_warn(priv->hw->wiphy, "Mailformed SDU rx'ed. "
+				"Size is lesser than IEEE header.\n");
+		goto drop;
+	}
+
+	if (unlikely(ieee80211_is_pspoll(frame->frame_control)))
+		if (cw1200_handle_pspoll(priv, skb))
+			goto drop;
+
+	hdr->mactime = 0; /* Not supported by WSM */
+	hdr->band = (arg->channelNumber > 14) ?
+			IEEE80211_BAND_5GHZ : IEEE80211_BAND_2GHZ;
+	hdr->freq = ieee80211_channel_to_frequency(
+			arg->channelNumber,
+			hdr->band);
+
+	if (arg->rxedRate >= 14) {
+		hdr->flag |= RX_FLAG_HT;
+		hdr->rate_idx = arg->rxedRate - 14;
+	} else if (arg->rxedRate >= 4) {
+		hdr->rate_idx = arg->rxedRate - 2;
+	} else {
+		hdr->rate_idx = arg->rxedRate;
+	}
+
+	hdr->signal = (s8)arg->rcpiRssi;
+	hdr->antenna = 0;
+
+	if (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
+		size_t iv_len = 0, icv_len = 0;
+		size_t hdrlen = ieee80211_hdrlen(frame->frame_control);
+
+		hdr->flag |= RX_FLAG_DECRYPTED | RX_FLAG_IV_STRIPPED;
+
+		/* Oops... There is no fast way to ask mac80211 about
+		 * IV/ICV lengths. Even defineas are not exposed.*/
+		switch (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
+		case WSM_RX_STATUS_WEP:
+			iv_len = 4 /* WEP_IV_LEN */;
+			icv_len = 4 /* WEP_ICV_LEN */;
+			break;
+		case WSM_RX_STATUS_TKIP:
+			iv_len = 8 /* TKIP_IV_LEN */;
+			icv_len = 4 /* TKIP_ICV_LEN */
+				+ 8 /*MICHAEL_MIC_LEN*/;
+			hdr->flag |= RX_FLAG_MMIC_STRIPPED;
+			break;
+		case WSM_RX_STATUS_AES:
+			iv_len = 8 /* CCMP_HDR_LEN */;
+			icv_len = 8 /* CCMP_MIC_LEN */;
+			break;
+		case WSM_RX_STATUS_WAPI:
+			iv_len = 18 /* WAPI_HDR_LEN */;
+			icv_len = 16 /* WAPI_MIC_LEN */;
+			break;
+		default:
+			WARN_ON("Unknown encryption type");
+			goto drop;
+		}
+
+		/* Firmware strips ICV in case of MIC failure. */
+		if (arg->status == WSM_STATUS_MICFAILURE)
+			icv_len = 0;
+
+		if (skb->len < hdrlen + iv_len + icv_len) {
+			wiphy_warn(priv->hw->wiphy, "Mailformed SDU rx'ed. "
+				"Size is lesser than crypto headers.\n");
+			goto drop;
+		}
+
+		/* Remove IV, ICV and MIC */
+		skb_trim(skb, skb->len - icv_len);
+		memmove(skb->data + iv_len, skb->data, hdrlen);
+		skb_pull(skb, iv_len);
+	}
+
+	cw1200_debug_rxed(priv);
+	if (arg->flags & WSM_RX_STATUS_AGGREGATE)
+		cw1200_debug_rxed_agg(priv);
+
+	if (ieee80211_is_action(frame->frame_control) &&
+			(arg->flags & WSM_RX_STATUS_ADDRESS1)) {
+		if (cw1200_handle_action_rx(priv, skb))
+			return;
+	} else if (unlikely(priv->disable_beacon_filter) &&
+			!arg->status &&
+			ieee80211_is_beacon(frame->frame_control) &&
+			!memcmp(ieee80211_get_SA(frame), priv->join_bssid,
+				ETH_ALEN)) {
+		priv->disable_beacon_filter = false;
+		queue_work(priv->workqueue, &priv->update_filtering_work);
+	}
+
+	/* Stay awake for 1sec. after frame is received to give
+	 * userspace chance to react and acquire appropriate
+	 * wakelock. */
+	if (ieee80211_is_auth(frame->frame_control))
+		grace_period = 5 * HZ;
+	else if (ieee80211_is_deauth(frame->frame_control))
+		grace_period = 5 * HZ;
+	else
+		grace_period = 1 * HZ;
+	cw1200_pm_stay_awake(&priv->pm_state, grace_period);
+
+	if (unlikely(cw1200_itp_rxed(priv, skb)))
+		consume_skb(skb);
+	else if (unlikely(early_data)) {
+		spin_lock_bh(&priv->ps_state_lock);
+		/* Double-check status with lock held */
+		if (entry->status == CW1200_LINK_SOFT)
+			skb_queue_tail(&entry->rx_queue, skb);
+		else
+			ieee80211_rx_irqsafe(priv->hw, skb);
+		spin_unlock_bh(&priv->ps_state_lock);
+	} else {
+		ieee80211_rx_irqsafe(priv->hw, skb);
+	}
+	*skb_p = NULL;
+
+	return;
+
+drop:
+	/* TODO: update failure counters */
+	return;
+}
+
+/* ******************************************************************** */
+/* Security								*/
+
+int cw1200_alloc_key(struct cw1200_common *priv)
+{
+	int idx;
+
+	idx = ffs(~priv->key_map) - 1;
+	if (idx < 0 || idx > WSM_KEY_MAX_INDEX)
+		return -1;
+
+	priv->key_map |= BIT(idx);
+	priv->keys[idx].entryIndex = idx;
+	return idx;
+}
+
+void cw1200_free_key(struct cw1200_common *priv, int idx)
+{
+	BUG_ON(!(priv->key_map & BIT(idx)));
+	memset(&priv->keys[idx], 0, sizeof(priv->keys[idx]));
+	priv->key_map &= ~BIT(idx);
+}
+
+void cw1200_free_keys(struct cw1200_common *priv)
+{
+	memset(&priv->keys, 0, sizeof(priv->keys));
+	priv->key_map = 0;
+}
+
+int cw1200_upload_keys(struct cw1200_common *priv)
+{
+	int idx, ret = 0;
+	for (idx = 0; idx <= WSM_KEY_MAX_INDEX; ++idx)
+		if (priv->key_map & BIT(idx)) {
+			ret = wsm_add_key(priv, &priv->keys[idx]);
+			if (ret < 0)
+				break;
+		}
+	return ret;
+}
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+/* Workaround for WFD test case 6.1.10 */
+void cw1200_link_id_reset(struct work_struct *work)
+{
+	struct cw1200_common *priv =
+		container_of(work, struct cw1200_common, linkid_reset_work);
+	int temp_linkid;
+
+	if (!priv->action_linkid) {
+		/* In GO mode we can receive ACTION frames without a linkID */
+		temp_linkid = cw1200_alloc_link_id(priv,
+				&priv->action_frame_sa[0]);
+		WARN_ON(!temp_linkid);
+		if (temp_linkid) {
+			/* Make sure we execute the WQ */
+			flush_workqueue(priv->workqueue);
+			/* Release the link ID */
+			spin_lock_bh(&priv->ps_state_lock);
+			priv->link_id_db[temp_linkid - 1].prev_status =
+				priv->link_id_db[temp_linkid - 1].status;
+			priv->link_id_db[temp_linkid - 1].status =
+				CW1200_LINK_RESET;
+			spin_unlock_bh(&priv->ps_state_lock);
+			wsm_lock_tx_async(priv);
+			if (queue_work(priv->workqueue,
+					&priv->link_id_work) <= 0)
+				wsm_unlock_tx(priv);
+		}
+	} else {
+		spin_lock_bh(&priv->ps_state_lock);
+		priv->link_id_db[priv->action_linkid - 1].prev_status =
+			priv->link_id_db[priv->action_linkid - 1].status;
+		priv->link_id_db[priv->action_linkid - 1].status =
+			CW1200_LINK_RESET_REMAP;
+		spin_unlock_bh(&priv->ps_state_lock);
+		wsm_lock_tx_async(priv);
+		if (queue_work(priv->workqueue, &priv->link_id_work) <= 0)
+			wsm_unlock_tx(priv);
+		flush_workqueue(priv->workqueue);
+	}
+}
+#endif
diff --git a/drivers/staging/cw1200/txrx.h b/drivers/staging/cw1200/txrx.h
new file mode 100644
index 00000000000..f3b2023ff2b
--- /dev/null
+++ b/drivers/staging/cw1200/txrx.h
@@ -0,0 +1,95 @@
+/*
+ * Datapath interface for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_TXRX_H
+#define CW1200_TXRX_H
+
+#include <linux/list.h>
+
+/* extern */ struct ieee80211_hw;
+/* extern */ struct sk_buff;
+/* extern */ struct wsm_tx;
+/* extern */ struct wsm_rx;
+/* extern */ struct wsm_tx_confirm;
+/* extern */ struct cw1200_txpriv;
+
+struct tx_policy {
+	union {
+		__le32 tbl[3];
+		u8 raw[12];
+	};
+	u8  defined;		/* TODO: u32 or u8, profile and select best */
+	u8  usage_count;	/* --// -- */
+	u8  retry_count;	/* --// -- */
+	u8  uploaded;
+};
+
+struct tx_policy_cache_entry {
+	struct tx_policy policy;
+	struct list_head link;
+};
+
+#define TX_POLICY_CACHE_SIZE	(8)
+struct tx_policy_cache {
+	struct tx_policy_cache_entry cache[TX_POLICY_CACHE_SIZE];
+	struct list_head used;
+	struct list_head free;
+	spinlock_t lock;
+};
+
+/* ******************************************************************** */
+/* TX policy cache							*/
+/* Intention of TX policy cache is an overcomplicated WSM API.
+ * Device does not accept per-PDU tx retry sequence.
+ * It uses "tx retry policy id" instead, so driver code has to sync
+ * linux tx retry sequences with a retry policy table in the device.
+ */
+void tx_policy_init(struct cw1200_common *priv);
+void tx_policy_upload_work(struct work_struct *work);
+
+/* ******************************************************************** */
+/* TX implementation							*/
+
+u32 cw1200_rate_mask_to_wsm(struct cw1200_common *priv,
+			       u32 rates);
+void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb);
+void cw1200_skb_dtor(struct cw1200_common *priv,
+		     struct sk_buff *skb,
+		     const struct cw1200_txpriv *txpriv);
+
+/* ******************************************************************** */
+/* WSM callbacks							*/
+
+void cw1200_tx_confirm_cb(struct cw1200_common *priv,
+			  struct wsm_tx_confirm *arg);
+void cw1200_rx_cb(struct cw1200_common *priv,
+		  struct wsm_rx *arg,
+		  struct sk_buff **skb_p);
+
+/* ******************************************************************** */
+/* Timeout								*/
+
+void cw1200_tx_timeout(struct work_struct *work);
+
+/* ******************************************************************** */
+/* Security								*/
+int cw1200_alloc_key(struct cw1200_common *priv);
+void cw1200_free_key(struct cw1200_common *priv, int idx);
+void cw1200_free_keys(struct cw1200_common *priv);
+int cw1200_upload_keys(struct cw1200_common *priv);
+
+/* ******************************************************************** */
+/* Workaround for WFD test case 6.1.10					*/
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+void cw1200_link_id_reset(struct work_struct *work);
+#endif
+
+#endif /* CW1200_TXRX_H */
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
new file mode 100644
index 00000000000..3cf53704f07
--- /dev/null
+++ b/drivers/staging/cw1200/wsm.c
@@ -0,0 +1,1836 @@
+/*
+ * WSM host interface (HI) implementation for
+ * ST-Ericsson CW1200 mac80211 drivers.
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/skbuff.h>
+#include <linux/wait.h>
+#include <linux/skbuff.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/random.h>
+
+#include "cw1200.h"
+#include "wsm.h"
+#include "bh.h"
+#include "debug.h"
+#include "itp.h"
+
+#if defined(CONFIG_CW1200_WSM_DEBUG)
+#define wsm_printk(...) printk(__VA_ARGS__)
+#else
+#define wsm_printk(...)
+#endif
+
+#define WSM_CMD_TIMEOUT		(2 * HZ) /* With respect to interrupt loss */
+#define WSM_CMD_JOIN_TIMEOUT	(7 * HZ) /* Join timeout is 5 sec. in FW   */
+#define WSM_CMD_START_TIMEOUT	(7 * HZ)
+#define WSM_CMD_RESET_TIMEOUT	(3 * HZ) /* 2 sec. timeout was observed.   */
+
+#define WSM_SKIP(buf, size)						\
+	do {								\
+		if (unlikely((buf)->data + size > (buf)->end))		\
+			goto underflow;					\
+		(buf)->data += size;					\
+	} while (0)
+
+#define WSM_GET(buf, ptr, size)						\
+	do {								\
+		if (unlikely((buf)->data + size > (buf)->end))		\
+			goto underflow;					\
+		memcpy(ptr, (buf)->data, size);				\
+		(buf)->data += size;					\
+	} while (0)
+
+#define __WSM_GET(buf, type, cvt)					\
+	({								\
+		type val;						\
+		if (unlikely((buf)->data + sizeof(type) > (buf)->end))	\
+			goto underflow;					\
+		val = cvt(*(type *)(buf)->data);			\
+		(buf)->data += sizeof(type);				\
+		val;							\
+	})
+
+#define WSM_GET8(buf)  __WSM_GET(buf, u8, (u8))
+#define WSM_GET16(buf) __WSM_GET(buf, u16, __le16_to_cpu)
+#define WSM_GET32(buf) __WSM_GET(buf, u32, __le32_to_cpu)
+
+#define WSM_PUT(buf, ptr, size)						\
+	do {								\
+		if (unlikely((buf)->data + size > (buf)->end))		\
+			if (unlikely(wsm_buf_reserve((buf), size)))	\
+				goto nomem;				\
+		memcpy((buf)->data, ptr, size);				\
+		(buf)->data += size;					\
+	} while (0)
+
+#define __WSM_PUT(buf, val, type, cvt)					\
+	do {								\
+		if (unlikely((buf)->data + sizeof(type) > (buf)->end))	\
+			if (unlikely(wsm_buf_reserve((buf), sizeof(type)))) \
+				goto nomem;				\
+		*(type *)(buf)->data = cvt(val);			\
+		(buf)->data += sizeof(type);				\
+	} while (0)
+
+#define WSM_PUT8(buf, val)  __WSM_PUT(buf, val, u8, (u8))
+#define WSM_PUT16(buf, val) __WSM_PUT(buf, val, u16, __cpu_to_le16)
+#define WSM_PUT32(buf, val) __WSM_PUT(buf, val, u32, __cpu_to_le32)
+
+static void wsm_buf_reset(struct wsm_buf *buf);
+static int wsm_buf_reserve(struct wsm_buf *buf, size_t extra_size);
+
+static int wsm_cmd_send(struct cw1200_common *priv,
+			struct wsm_buf *buf,
+			void *arg, u16 cmd, long tmo);
+
+static inline void wsm_cmd_lock(struct cw1200_common *priv)
+{
+	mutex_lock(&priv->wsm_cmd_mux);
+}
+
+static inline void wsm_cmd_unlock(struct cw1200_common *priv)
+{
+	mutex_unlock(&priv->wsm_cmd_mux);
+}
+
+/* ******************************************************************** */
+/* WSM API implementation						*/
+
+static int wsm_generic_confirm(struct cw1200_common *priv,
+			     void *arg,
+			     struct wsm_buf *buf)
+{
+	u32 status = WSM_GET32(buf);
+	if (status != WSM_STATUS_SUCCESS)
+		return -EINVAL;
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+int wsm_configuration(struct cw1200_common *priv, struct wsm_configuration *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT32(buf, arg->dot11MaxTransmitMsduLifeTime);
+	WSM_PUT32(buf, arg->dot11MaxReceiveLifeTime);
+	WSM_PUT32(buf, arg->dot11RtsThreshold);
+
+	/* DPD block. */
+	WSM_PUT16(buf, arg->dpdData_size + 12);
+	WSM_PUT16(buf, 1); /* DPD version */
+	WSM_PUT(buf, arg->dot11StationId, ETH_ALEN);
+	WSM_PUT16(buf, 5); /* DPD flags */
+	WSM_PUT(buf, arg->dpdData, arg->dpdData_size);
+
+	ret = wsm_cmd_send(priv, buf, arg, 0x0009, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+static int wsm_configuration_confirm(struct cw1200_common *priv,
+				     struct wsm_configuration *arg,
+				     struct wsm_buf *buf)
+{
+	int i;
+	int status;
+
+	status = WSM_GET32(buf);
+	if (WARN_ON(status != WSM_STATUS_SUCCESS))
+		return -EINVAL;
+
+	WSM_GET(buf, arg->dot11StationId, ETH_ALEN);
+	arg->dot11FrequencyBandsSupported = WSM_GET8(buf);
+	WSM_SKIP(buf, 1);
+	arg->supportedRateMask = WSM_GET32(buf);
+	for (i = 0; i < 2; ++i) {
+		arg->txPowerRange[i].min_power_level = WSM_GET32(buf);
+		arg->txPowerRange[i].max_power_level = WSM_GET32(buf);
+		arg->txPowerRange[i].stepping = WSM_GET32(buf);
+	}
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+/* ******************************************************************** */
+
+int wsm_reset(struct cw1200_common *priv, const struct wsm_reset *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	u16 cmd = 0x000A | WSM_TX_LINK_ID(arg->link_id);
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT32(buf, arg->reset_statistics ? 0 : 1);
+	ret = wsm_cmd_send(priv, buf, NULL, cmd, WSM_CMD_RESET_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+struct wsm_mib {
+	u16 mibId;
+	void *buf;
+	size_t buf_size;
+};
+
+int wsm_read_mib(struct cw1200_common *priv, u16 mibId, void *_buf,
+			size_t buf_size)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	struct wsm_mib mib_buf = {
+		.mibId = mibId,
+		.buf = _buf,
+		.buf_size = buf_size,
+	};
+	wsm_cmd_lock(priv);
+
+	WSM_PUT16(buf, mibId);
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(priv, buf, &mib_buf, 0x0005, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+static int wsm_read_mib_confirm(struct cw1200_common *priv,
+				struct wsm_mib *arg,
+				struct wsm_buf *buf)
+{
+	u16 size;
+	if (WARN_ON(WSM_GET32(buf) != WSM_STATUS_SUCCESS))
+		return -EINVAL;
+
+	if (WARN_ON(WSM_GET16(buf) != arg->mibId))
+		return -EINVAL;
+
+	size = WSM_GET16(buf);
+	if (size > arg->buf_size)
+		size = arg->buf_size;
+
+	WSM_GET(buf, arg->buf, size);
+	arg->buf_size = size;
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+/* ******************************************************************** */
+
+int wsm_write_mib(struct cw1200_common *priv, u16 mibId, void *_buf,
+			size_t buf_size)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	struct wsm_mib mib_buf = {
+		.mibId = mibId,
+		.buf = _buf,
+		.buf_size = buf_size,
+	};
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT16(buf, mibId);
+	WSM_PUT16(buf, buf_size);
+	WSM_PUT(buf, _buf, buf_size);
+
+	ret = wsm_cmd_send(priv, buf, &mib_buf, 0x0006, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+static int wsm_write_mib_confirm(struct cw1200_common *priv,
+				struct wsm_mib *arg,
+				struct wsm_buf *buf)
+{
+	int ret;
+
+	ret = wsm_generic_confirm(priv, arg, buf);
+	if (ret)
+		return ret;
+
+	if (arg->mibId == 0x1006) {
+		/* OperationalMode: update PM status. */
+		const char *p = arg->buf;
+		cw1200_enable_powersave(priv,
+				(p[0] & 0x0F) ? true : false);
+	}
+	return 0;
+}
+
+/* ******************************************************************** */
+
+int wsm_scan(struct cw1200_common *priv, const struct wsm_scan *arg)
+{
+	int i;
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	if (unlikely(arg->numOfChannels > 48))
+		return -EINVAL;
+
+	if (unlikely(arg->numOfSSIDs > 2))
+		return -EINVAL;
+
+	if (unlikely(arg->band > 1))
+		return -EINVAL;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->band);
+	WSM_PUT8(buf, arg->scanType);
+	WSM_PUT8(buf, arg->scanFlags);
+	WSM_PUT8(buf, arg->maxTransmitRate);
+	WSM_PUT32(buf, arg->autoScanInterval);
+	WSM_PUT8(buf, arg->numOfProbeRequests);
+	WSM_PUT8(buf, arg->numOfChannels);
+	WSM_PUT8(buf, arg->numOfSSIDs);
+	WSM_PUT8(buf, arg->probeDelay);
+
+	for (i = 0; i < arg->numOfChannels; ++i) {
+		WSM_PUT16(buf, arg->ch[i].number);
+		WSM_PUT16(buf, 0);
+		WSM_PUT32(buf, arg->ch[i].minChannelTime);
+		WSM_PUT32(buf, arg->ch[i].maxChannelTime);
+		WSM_PUT32(buf, 0);
+	}
+
+	for (i = 0; i < arg->numOfSSIDs; ++i) {
+		WSM_PUT32(buf, arg->ssids[i].length);
+		WSM_PUT(buf, &arg->ssids[i].ssid[0],
+				sizeof(arg->ssids[i].ssid));
+	}
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0007, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_stop_scan(struct cw1200_common *priv)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	wsm_cmd_lock(priv);
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0008, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+}
+
+
+static int wsm_tx_confirm(struct cw1200_common *priv,
+			  struct wsm_buf *buf,
+			  int link_id)
+{
+	struct wsm_tx_confirm tx_confirm;
+
+	tx_confirm.packetID = WSM_GET32(buf);
+	tx_confirm.status = WSM_GET32(buf);
+	tx_confirm.txedRate = WSM_GET8(buf);
+	tx_confirm.ackFailures = WSM_GET8(buf);
+	tx_confirm.flags = WSM_GET16(buf);
+	tx_confirm.mediaDelay = WSM_GET32(buf);
+	tx_confirm.txQueueDelay = WSM_GET32(buf);
+	tx_confirm.link_id = link_id;
+
+	if (priv->wsm_cbc.tx_confirm)
+		priv->wsm_cbc.tx_confirm(priv, &tx_confirm);
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+static int wsm_multi_tx_confirm(struct cw1200_common *priv,
+				struct wsm_buf *buf, int link_id)
+{
+	int ret;
+	int count;
+	int i;
+
+	count = WSM_GET32(buf);
+	if (WARN_ON(count <= 0))
+		return -EINVAL;
+	else if (count > 1) {
+		ret = wsm_release_tx_buffer(priv, count - 1);
+		if (ret < 0)
+			return ret;
+		else if (ret > 0)
+			cw1200_bh_wakeup(priv);
+	}
+
+	cw1200_debug_txed_multi(priv, count);
+	for (i = 0; i < count; ++i) {
+		ret = wsm_tx_confirm(priv, buf, link_id);
+		if (ret)
+			return ret;
+	}
+	return ret;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+/* ******************************************************************** */
+
+static int wsm_join_confirm(struct cw1200_common *priv,
+			    struct wsm_join *arg,
+			    struct wsm_buf *buf)
+{
+	if (WARN_ON(WSM_GET32(buf) != WSM_STATUS_SUCCESS))
+		return -EINVAL;
+
+	arg->minPowerLevel = WSM_GET32(buf);
+	arg->maxPowerLevel = WSM_GET32(buf);
+
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+int wsm_join(struct cw1200_common *priv, struct wsm_join *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->mode);
+	WSM_PUT8(buf, arg->band);
+	WSM_PUT16(buf, arg->channelNumber);
+	WSM_PUT(buf, &arg->bssid[0], sizeof(arg->bssid));
+	WSM_PUT16(buf, arg->atimWindow);
+	WSM_PUT8(buf, arg->preambleType);
+	WSM_PUT8(buf, arg->probeForJoin);
+	WSM_PUT8(buf, arg->dtimPeriod);
+	WSM_PUT8(buf, arg->flags);
+	WSM_PUT32(buf, arg->ssidLength);
+	WSM_PUT(buf, &arg->ssid[0], sizeof(arg->ssid));
+	WSM_PUT32(buf, arg->beaconInterval);
+	WSM_PUT32(buf, arg->basicRateSet);
+
+	priv->tx_burst_idx = -1;
+	ret = wsm_cmd_send(priv, buf, arg, 0x000B, WSM_CMD_JOIN_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_bss_params(struct cw1200_common *priv,
+			const struct wsm_set_bss_params *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, 0);
+	WSM_PUT8(buf, arg->beaconLostCount);
+	WSM_PUT16(buf, arg->aid);
+	WSM_PUT32(buf, arg->operationalRateSet);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0011, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_add_key(struct cw1200_common *priv, const struct wsm_add_key *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT(buf, arg, sizeof(*arg));
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x000C, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_remove_key(struct cw1200_common *priv, const struct wsm_remove_key *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->entryIndex);
+	WSM_PUT8(buf, 0);
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x000D, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_tx_queue_params(struct cw1200_common *priv,
+		const struct wsm_set_tx_queue_params *arg, u8 id)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	u8 queue_id_to_wmm_aci[] = {3, 2, 0, 1};
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, queue_id_to_wmm_aci[id]);
+	WSM_PUT8(buf, 0);
+	WSM_PUT8(buf, arg->ackPolicy);
+	WSM_PUT8(buf, 0);
+	WSM_PUT32(buf, arg->maxTransmitLifetime);
+	WSM_PUT16(buf, arg->allowedMediumTime);
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0012, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_edca_params(struct cw1200_common *priv,
+				const struct wsm_edca_params *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	/* Implemented according to specification. */
+
+	WSM_PUT16(buf, arg->params[3].cwMin);
+	WSM_PUT16(buf, arg->params[2].cwMin);
+	WSM_PUT16(buf, arg->params[1].cwMin);
+	WSM_PUT16(buf, arg->params[0].cwMin);
+
+	WSM_PUT16(buf, arg->params[3].cwMax);
+	WSM_PUT16(buf, arg->params[2].cwMax);
+	WSM_PUT16(buf, arg->params[1].cwMax);
+	WSM_PUT16(buf, arg->params[0].cwMax);
+
+	WSM_PUT8(buf, arg->params[3].aifns);
+	WSM_PUT8(buf, arg->params[2].aifns);
+	WSM_PUT8(buf, arg->params[1].aifns);
+	WSM_PUT8(buf, arg->params[0].aifns);
+
+	WSM_PUT16(buf, arg->params[3].txOpLimit);
+	WSM_PUT16(buf, arg->params[2].txOpLimit);
+	WSM_PUT16(buf, arg->params[1].txOpLimit);
+	WSM_PUT16(buf, arg->params[0].txOpLimit);
+
+	WSM_PUT32(buf, arg->params[3].maxReceiveLifetime);
+	WSM_PUT32(buf, arg->params[2].maxReceiveLifetime);
+	WSM_PUT32(buf, arg->params[1].maxReceiveLifetime);
+	WSM_PUT32(buf, arg->params[0].maxReceiveLifetime);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0013, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_switch_channel(struct cw1200_common *priv,
+			const struct wsm_switch_channel *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_lock_tx(priv);
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->channelMode);
+	WSM_PUT8(buf, arg->channelSwitchCount);
+	WSM_PUT16(buf, arg->newChannelNumber);
+
+	priv->channel_switch_in_progress = 1;
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0016, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	if (ret) {
+		wsm_unlock_tx(priv);
+		priv->channel_switch_in_progress = 0;
+	}
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	wsm_unlock_tx(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_pm(struct cw1200_common *priv, const struct wsm_set_pm *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->pmMode);
+	WSM_PUT8(buf, arg->fastPsmIdlePeriod);
+	WSM_PUT8(buf, arg->apPsmChangePeriod);
+	WSM_PUT8(buf, arg->minAutoPsPollPeriod);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0010, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_start(struct cw1200_common *priv, const struct wsm_start *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT8(buf, arg->mode);
+	WSM_PUT8(buf, arg->band);
+	WSM_PUT16(buf, arg->channelNumber);
+	WSM_PUT32(buf, arg->CTWindow);
+	WSM_PUT32(buf, arg->beaconInterval);
+	WSM_PUT8(buf, arg->DTIMPeriod);
+	WSM_PUT8(buf, arg->preambleType);
+	WSM_PUT8(buf, arg->probeDelay);
+	WSM_PUT8(buf, arg->ssidLength);
+	WSM_PUT(buf, arg->ssid, sizeof(arg->ssid));
+	WSM_PUT32(buf, arg->basicRateSet);
+
+	priv->tx_burst_idx = -1;
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0017, WSM_CMD_START_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_beacon_transmit(struct cw1200_common *priv,
+			const struct wsm_beacon_transmit *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT32(buf, arg->enableBeaconing ? 1 : 0);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0018, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_start_find(struct cw1200_common *priv)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+	ret = wsm_cmd_send(priv, buf, NULL, 0x0019, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+}
+
+/* ******************************************************************** */
+
+int wsm_stop_find(struct cw1200_common *priv)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+	ret = wsm_cmd_send(priv, buf, NULL, 0x001A, WSM_CMD_TIMEOUT);
+	wsm_cmd_unlock(priv);
+	return ret;
+}
+
+/* ******************************************************************** */
+
+int wsm_map_link(struct cw1200_common *priv, const struct wsm_map_link *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+	u16 cmd = 0x001C | WSM_TX_LINK_ID(arg->link_id);
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT(buf, &arg->mac_addr[0], sizeof(arg->mac_addr));
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(priv, buf, NULL, cmd, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_update_ie(struct cw1200_common *priv,
+		  const struct wsm_update_ie *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv);
+
+	WSM_PUT16(buf, arg->what);
+	WSM_PUT16(buf, arg->count);
+	WSM_PUT(buf, arg->ies, arg->length);
+
+	ret = wsm_cmd_send(priv, buf, NULL, 0x001B, WSM_CMD_TIMEOUT);
+
+	wsm_cmd_unlock(priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv);
+	return -ENOMEM;
+
+}
+
+/* ******************************************************************** */
+/* WSM indication events implementation					*/
+
+static int wsm_startup_indication(struct cw1200_common *priv,
+					struct wsm_buf *buf)
+{
+	u16 status;
+	char fw_label[129];
+	static const char * const fw_types[] = {
+		"ETF",
+		"WFM",
+		"WSM",
+		"HI test",
+		"Platform test"
+	};
+
+	priv->wsm_caps.numInpChBufs	= WSM_GET16(buf);
+	priv->wsm_caps.sizeInpChBuf	= WSM_GET16(buf);
+	priv->wsm_caps.hardwareId	= WSM_GET16(buf);
+	priv->wsm_caps.hardwareSubId	= WSM_GET16(buf);
+	status				= WSM_GET16(buf);
+	priv->wsm_caps.firmwareCap	= WSM_GET16(buf);
+	priv->wsm_caps.firmwareType	= WSM_GET16(buf);
+	priv->wsm_caps.firmwareApiVer	= WSM_GET16(buf);
+	priv->wsm_caps.firmwareBuildNumber = WSM_GET16(buf);
+	priv->wsm_caps.firmwareVersion	= WSM_GET16(buf);
+	WSM_GET(buf, &fw_label[0], sizeof(fw_label) - 1);
+	fw_label[sizeof(fw_label) - 1] = 0; /* Do not trust FW too much. */
+
+	if (WARN_ON(status))
+		return -EINVAL;
+
+	if (WARN_ON(priv->wsm_caps.firmwareType > 4))
+		return -EINVAL;
+
+	printk(KERN_INFO "CW1200 WSM init done.\n"
+		"   Input buffers: %d x %d bytes\n"
+		"   Hardware: %d.%d\n"
+		"   %s firmware [%s], ver: %d, build: %d,"
+		    " api: %d, cap: 0x%.4X\n",
+		priv->wsm_caps.numInpChBufs, priv->wsm_caps.sizeInpChBuf,
+		priv->wsm_caps.hardwareId, priv->wsm_caps.hardwareSubId,
+		fw_types[priv->wsm_caps.firmwareType],
+		&fw_label[0], priv->wsm_caps.firmwareVersion,
+		priv->wsm_caps.firmwareBuildNumber,
+		priv->wsm_caps.firmwareApiVer, priv->wsm_caps.firmwareCap);
+
+	priv->wsm_caps.firmwareReady = 1;
+
+	wake_up(&priv->wsm_startup_done);
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+static int wsm_receive_indication(struct cw1200_common *priv,
+					int link_id,
+					struct wsm_buf *buf,
+					struct sk_buff **skb_p)
+{
+	priv->rx_timestamp = jiffies;
+	if (priv->wsm_cbc.rx) {
+		struct wsm_rx rx;
+		struct ieee80211_hdr *hdr;
+		size_t hdr_len;
+		__le16 fctl;
+
+		rx.status = WSM_GET32(buf);
+		rx.channelNumber = WSM_GET16(buf);
+		rx.rxedRate = WSM_GET8(buf);
+		rx.rcpiRssi = WSM_GET8(buf);
+		rx.flags = WSM_GET32(buf);
+
+		/* FW Workaround: Drop probe resp or
+		beacon when RSSI is 0 */
+		hdr = (struct ieee80211_hdr *) (*skb_p)->data;
+
+		if (!rx.rcpiRssi &&
+		    (ieee80211_is_probe_resp(hdr->frame_control) ||
+		    ieee80211_is_beacon(hdr->frame_control)))
+			return 0;
+
+		/* If no RSSI subscription has been made,
+		* convert RCPI to RSSI here */
+		if (!priv->cqm_use_rssi)
+			rx.rcpiRssi = rx.rcpiRssi / 2 - 110;
+
+		rx.link_id = link_id;
+		fctl = *(__le16 *)buf->data;
+		hdr_len = buf->data - buf->begin;
+		skb_pull(*skb_p, hdr_len);
+		if (!rx.status && unlikely(ieee80211_is_deauth(fctl))) {
+			if (priv->join_status == CW1200_JOIN_STATUS_STA) {
+				/* Shedule unjoin work */
+				wsm_printk(KERN_DEBUG \
+					"[WSM] Issue unjoin command (RX).\n");
+				wsm_lock_tx_async(priv);
+				if (queue_work(priv->workqueue,
+						&priv->unjoin_work) <= 0)
+					wsm_unlock_tx(priv);
+			}
+		}
+		priv->wsm_cbc.rx(priv, &rx, skb_p);
+		if (*skb_p)
+			skb_push(*skb_p, hdr_len);
+	}
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+static int wsm_event_indication(struct cw1200_common *priv, struct wsm_buf *buf)
+{
+	int first;
+	struct cw1200_wsm_event *event;
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		return 0;
+	}
+
+	event = kzalloc(sizeof(struct cw1200_wsm_event), GFP_KERNEL);
+
+	event->evt.eventId = __le32_to_cpu(WSM_GET32(buf));
+	event->evt.eventData = __le32_to_cpu(WSM_GET32(buf));
+
+	wsm_printk(KERN_DEBUG "[WSM] Event: %d(%d)\n",
+		event->evt.eventId, event->evt.eventData);
+
+	spin_lock(&priv->event_queue_lock);
+	first = list_empty(&priv->event_queue);
+	list_add_tail(&event->link, &priv->event_queue);
+	spin_unlock(&priv->event_queue_lock);
+
+	if (first)
+		queue_work(priv->workqueue, &priv->event_handler);
+
+	return 0;
+
+underflow:
+	kfree(event);
+	return -EINVAL;
+}
+
+static int wsm_channel_switch_indication(struct cw1200_common *priv,
+						struct wsm_buf *buf)
+{
+	wsm_unlock_tx(priv); /* Re-enable datapath */
+	WARN_ON(WSM_GET32(buf));
+
+	priv->channel_switch_in_progress = 0;
+	wake_up(&priv->channel_switch_done);
+
+	if (priv->wsm_cbc.channel_switch)
+		priv->wsm_cbc.channel_switch(priv);
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+static int wsm_set_pm_indication(struct cw1200_common *priv,
+					struct wsm_buf *buf)
+{
+	return 0;
+}
+
+static int wsm_scan_complete_indication(struct cw1200_common *priv,
+					struct wsm_buf *buf)
+{
+	if (priv->wsm_cbc.scan_complete) {
+		struct wsm_scan_complete arg;
+		arg.status = WSM_GET32(buf);
+		arg.psm = WSM_GET8(buf);
+		arg.numChannels = WSM_GET8(buf);
+		priv->wsm_cbc.scan_complete(priv, &arg);
+	}
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+static int wsm_find_complete_indication(struct cw1200_common *priv,
+					struct wsm_buf *buf)
+{
+	/* TODO: Implement me. */
+	STUB();
+	return 0;
+}
+
+static int wsm_suspend_resume_indication(struct cw1200_common *priv,
+					 int link_id, struct wsm_buf *buf)
+{
+	if (priv->wsm_cbc.suspend_resume) {
+		u32 flags;
+		struct wsm_suspend_resume arg;
+
+		flags = WSM_GET32(buf);
+		arg.link_id = link_id;
+		arg.stop = !(flags & 1);
+		arg.multicast = !!(flags & 8);
+		arg.queue = (flags >> 1) & 3;
+
+		priv->wsm_cbc.suspend_resume(priv, &arg);
+	}
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+
+/* ******************************************************************** */
+/* WSM TX								*/
+
+int wsm_cmd_send(struct cw1200_common *priv,
+		 struct wsm_buf *buf,
+		 void *arg, u16 cmd, long tmo)
+{
+	size_t buf_len = buf->data - buf->begin;
+	int ret;
+
+	if (cmd == 0x0006) /* Write MIB */
+		wsm_printk(KERN_DEBUG "[WSM] >>> 0x%.4X [MIB: 0x%.4X] (%d)\n",
+			cmd, __le16_to_cpu(((__le16 *)buf->begin)[2]),
+			buf_len);
+	else
+		wsm_printk(KERN_DEBUG "[WSM] >>> 0x%.4X (%d)\n", cmd, buf_len);
+
+	/* Fill HI message header */
+	/* BH will add sequence number */
+	((__le16 *)buf->begin)[0] = __cpu_to_le16(buf_len);
+	((__le16 *)buf->begin)[1] = __cpu_to_le16(cmd);
+
+	spin_lock(&priv->wsm_cmd.lock);
+	BUG_ON(priv->wsm_cmd.ptr);
+	priv->wsm_cmd.done = 0;
+	priv->wsm_cmd.ptr = buf->begin;
+	priv->wsm_cmd.len = buf_len;
+	priv->wsm_cmd.arg = arg;
+	priv->wsm_cmd.cmd = cmd;
+	spin_unlock(&priv->wsm_cmd.lock);
+
+	cw1200_bh_wakeup(priv);
+
+	if (unlikely(priv->bh_error)) {
+		/* Do not wait for timeout if BH is dead. Exit immediately. */
+		ret = 0;
+	} else {
+		long rx_timestamp;
+		/* Firmware prioritizes data traffic over control confirm.
+		 * Loop below checks if data was RXed and increases timeout
+		 * accordingly. */
+		do {
+			/* It's safe to use unprotected access to
+			 * wsm_cmd.done here */
+			ret = wait_event_timeout(
+					priv->wsm_cmd_wq,
+					priv->wsm_cmd.done, tmo);
+			rx_timestamp = jiffies - priv->rx_timestamp;
+			if (unlikely(rx_timestamp < 0))
+				rx_timestamp = tmo + 1;
+		} while (!ret && rx_timestamp <= tmo);
+	}
+
+	if (unlikely(ret == 0)) {
+		u16 raceCheck;
+
+		spin_lock(&priv->wsm_cmd.lock);
+		raceCheck = priv->wsm_cmd.cmd;
+		priv->wsm_cmd.arg = NULL;
+		priv->wsm_cmd.ptr = NULL;
+		spin_unlock(&priv->wsm_cmd.lock);
+
+		/* Race condition check to make sure _confirm is not called
+		 * after exit of _send */
+		if (raceCheck == 0xFFFF) {
+			/* If wsm_handle_rx got stuck in _confirm we will hang
+			 * system there. It's better than silently currupt
+			 * stack or heap, isn't it? */
+			BUG_ON(wait_event_timeout(
+					priv->wsm_cmd_wq, priv->wsm_cmd.done,
+					WSM_CMD_LAST_CHANCE_TIMEOUT) <= 0);
+		}
+
+		/* Kill BH thread to report the error to the top layer. */
+		priv->bh_error = 1;
+		wake_up(&priv->bh_wq);
+		ret = -ETIMEDOUT;
+	} else {
+		spin_lock(&priv->wsm_cmd.lock);
+		BUG_ON(!priv->wsm_cmd.done);
+		ret = priv->wsm_cmd.ret;
+		spin_unlock(&priv->wsm_cmd.lock);
+	}
+	wsm_buf_reset(buf);
+	return ret;
+}
+
+/* ******************************************************************** */
+/* WSM TX port control							*/
+
+void wsm_lock_tx(struct cw1200_common *priv)
+{
+	wsm_cmd_lock(priv);
+	if (atomic_add_return(1, &priv->tx_lock) == 1) {
+		if (wsm_flush_tx(priv))
+			wsm_printk(KERN_DEBUG "[WSM] TX is locked.\n");
+	}
+	wsm_cmd_unlock(priv);
+}
+
+void wsm_lock_tx_async(struct cw1200_common *priv)
+{
+	if (atomic_add_return(1, &priv->tx_lock) == 1)
+		wsm_printk(KERN_DEBUG "[WSM] TX is locked (async).\n");
+}
+
+bool wsm_flush_tx(struct cw1200_common *priv)
+{
+	unsigned long timestamp = jiffies;
+	bool pending = false;
+	long timeout;
+	int i;
+
+	/* Flush must be called with TX lock held. */
+	BUG_ON(!atomic_read(&priv->tx_lock));
+
+	/* First check if we really need to do something.
+	 * It is safe to use unprotected access, as hw_bufs_used
+	 * can only decrements. */
+	if (!priv->hw_bufs_used)
+		return true;
+
+	if (priv->bh_error) {
+		/* In case of failure do not wait for magic. */
+		wsm_printk(KERN_ERR "[WSM] Fatal error occured, "
+				"will not flush TX.\n");
+		return false;
+	} else {
+		/* Get a timestamp of "oldest" frame */
+		for (i = 0; i < 4; ++i)
+			pending |= cw1200_queue_get_xmit_timestamp(
+					&priv->tx_queue[i],
+					&timestamp);
+		/* It is allowed to lock TX with only a command in the pipe. */
+		if (!pending)
+			return true;
+
+		timeout = timestamp + WSM_CMD_LAST_CHANCE_TIMEOUT - jiffies;
+		if (timeout < 0 || wait_event_timeout(priv->bh_evt_wq,
+				!priv->hw_bufs_used,
+				timeout) <= 0) {
+			/* Hmmm... Not good. Frame had stuck in firmware. */
+			priv->bh_error = 1;
+			wake_up(&priv->bh_wq);
+			return false;
+		}
+
+		/* Ok, everything is flushed. */
+		return true;
+	}
+}
+
+void wsm_unlock_tx(struct cw1200_common *priv)
+{
+	int tx_lock;
+	if (priv->bh_error)
+		wsm_printk(KERN_ERR "fatal error occured, unlock is unsafe\n");
+	else {
+		tx_lock = atomic_sub_return(1, &priv->tx_lock);
+		if (tx_lock < 0) {
+			BUG_ON(1);
+		} else if (tx_lock == 0) {
+			cw1200_bh_wakeup(priv);
+			wsm_printk(KERN_DEBUG "[WSM] TX is unlocked.\n");
+		}
+	}
+}
+
+/* ******************************************************************** */
+/* WSM RX								*/
+
+int wsm_handle_exception(struct cw1200_common *priv, u8 *data, size_t len)
+{
+	struct wsm_buf buf;
+	u32 reason;
+	u32 reg[18];
+	char fname[48];
+	size_t i;
+
+	static const char * const reason_str[] = {
+		"undefined instruction",
+		"prefetch abort",
+		"data abort",
+		"unknown error",
+	};
+
+#if defined(CONFIG_CW1200_USE_STE_EXTENSIONS)
+	/* Send the event upwards on the FW exception */
+	cw1200_pm_stay_awake(&priv->pm_state, 3*HZ);
+	ieee80211_driver_hang_notify(priv->vif, GFP_KERNEL);
+#endif
+
+	buf.begin = buf.data = data;
+	buf.end = &buf.begin[len];
+
+	reason = WSM_GET32(&buf);
+	for (i = 0; i < ARRAY_SIZE(reg); ++i)
+		reg[i] = WSM_GET32(&buf);
+	WSM_GET(&buf, fname, sizeof(fname));
+
+	if (reason < 4)
+		wiphy_err(priv->hw->wiphy,
+			"Firmware exception: %s.\n",
+			reason_str[reason]);
+	else
+		wiphy_err(priv->hw->wiphy,
+			"Firmware assert at %.*s, line %d\n",
+			sizeof(fname), fname, reg[1]);
+
+	for (i = 0; i < 12; i += 4)
+		wiphy_err(priv->hw->wiphy,
+			"R%d: 0x%.8X, R%d: 0x%.8X, R%d: 0x%.8X, R%d: 0x%.8X,\n",
+			i + 0, reg[i + 0], i + 1, reg[i + 1],
+			i + 2, reg[i + 2], i + 3, reg[i + 3]);
+	wiphy_err(priv->hw->wiphy,
+		"R12: 0x%.8X, SP: 0x%.8X, LR: 0x%.8X, PC: 0x%.8X,\n",
+		reg[i + 0], reg[i + 1], reg[i + 2], reg[i + 3]);
+	i += 4;
+	wiphy_err(priv->hw->wiphy,
+		"CPSR: 0x%.8X, SPSR: 0x%.8X\n",
+		reg[i + 0], reg[i + 1]);
+
+	print_hex_dump_bytes("R1: ", DUMP_PREFIX_NONE,
+		fname, sizeof(fname));
+	return 0;
+
+underflow:
+	wiphy_err(priv->hw->wiphy,
+		"Firmware exception.\n");
+	print_hex_dump_bytes("Exception: ", DUMP_PREFIX_NONE,
+		data, len);
+	return -EINVAL;
+}
+
+int wsm_handle_rx(struct cw1200_common *priv, int id,
+		  struct wsm_hdr *wsm, struct sk_buff **skb_p)
+{
+	int ret = 0;
+	struct wsm_buf wsm_buf;
+	int link_id = (id >> 6) & 0x0F;
+
+	/* Strip link id. */
+	id &= ~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX);
+
+	wsm_buf.begin = (u8 *)&wsm[0];
+	wsm_buf.data = (u8 *)&wsm[1];
+	wsm_buf.end = &wsm_buf.begin[__le32_to_cpu(wsm->len)];
+
+	wsm_printk(KERN_DEBUG "[WSM] <<< 0x%.4X (%d)\n", id,
+			wsm_buf.end - wsm_buf.begin);
+
+	if (id == 0x404) {
+		ret = wsm_tx_confirm(priv, &wsm_buf, link_id);
+	} else if (id == 0x41E) {
+		ret = wsm_multi_tx_confirm(priv, &wsm_buf, link_id);
+	} else if (id & 0x0400) {
+		void *wsm_arg;
+		u16 wsm_cmd;
+
+		/* Do not trust FW too much. Protection against repeated
+		 * response and race condition removal (see above). */
+		spin_lock(&priv->wsm_cmd.lock);
+		wsm_arg = priv->wsm_cmd.arg;
+		wsm_cmd = priv->wsm_cmd.cmd &
+				~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX);
+		priv->wsm_cmd.cmd = 0xFFFF;
+		spin_unlock(&priv->wsm_cmd.lock);
+
+		if (WARN_ON((id & ~0x0400) != wsm_cmd)) {
+			/* Note that any non-zero is a fatal retcode. */
+			ret = -EINVAL;
+			goto out;
+		}
+
+		switch (id) {
+		case 0x0409:
+			/* Note that wsm_arg can be NULL in case of timeout in
+			 * wsm_cmd_send(). */
+			if (likely(wsm_arg))
+				ret = wsm_configuration_confirm(priv, wsm_arg,
+								&wsm_buf);
+			break;
+		case 0x0405:
+			if (likely(wsm_arg))
+				ret = wsm_read_mib_confirm(priv, wsm_arg,
+								&wsm_buf);
+			break;
+		case 0x0406:
+			if (likely(wsm_arg))
+				ret = wsm_write_mib_confirm(priv, wsm_arg,
+								&wsm_buf);
+			break;
+		case 0x040B:
+			if (likely(wsm_arg))
+				ret = wsm_join_confirm(priv, wsm_arg, &wsm_buf);
+			break;
+		case 0x0407: /* start-scan */
+		case 0x0408: /* stop-scan */
+		case 0x040A: /* wsm_reset */
+		case 0x040C: /* add_key */
+		case 0x040D: /* remove_key */
+		case 0x0410: /* wsm_set_pm */
+		case 0x0411: /* set_bss_params */
+		case 0x0412: /* set_tx_queue_params */
+		case 0x0413: /* set_edca_params */
+		case 0x0416: /* switch_channel */
+		case 0x0417: /* start */
+		case 0x0418: /* beacon_transmit */
+		case 0x0419: /* start_find */
+		case 0x041A: /* stop_find */
+		case 0x041B: /* update_ie */
+		case 0x041C: /* map_link */
+			WARN_ON(wsm_arg != NULL);
+			ret = wsm_generic_confirm(priv, wsm_arg, &wsm_buf);
+			if (ret)
+				wiphy_warn(priv->hw->wiphy,
+					"wsm_generic_confirm "
+					"failed for request 0x%.4X.\n",
+					id & ~0x0400);
+			break;
+		default:
+			BUG_ON(1);
+		}
+
+		spin_lock(&priv->wsm_cmd.lock);
+		priv->wsm_cmd.ret = ret;
+		priv->wsm_cmd.done = 1;
+		spin_unlock(&priv->wsm_cmd.lock);
+		ret = 0; /* Error response from device should ne stop BH. */
+
+		wake_up(&priv->wsm_cmd_wq);
+	} else if (id & 0x0800) {
+		switch (id) {
+		case 0x0801:
+			ret = wsm_startup_indication(priv, &wsm_buf);
+			break;
+		case 0x0804:
+			ret = wsm_receive_indication(priv, link_id,
+					&wsm_buf, skb_p);
+			break;
+		case 0x0805:
+			ret = wsm_event_indication(priv, &wsm_buf);
+			break;
+		case 0x080A:
+			ret = wsm_channel_switch_indication(priv, &wsm_buf);
+			break;
+		case 0x0809:
+			ret = wsm_set_pm_indication(priv, &wsm_buf);
+			break;
+		case 0x0806:
+			ret = wsm_scan_complete_indication(priv, &wsm_buf);
+			break;
+		case 0x080B:
+			ret = wsm_find_complete_indication(priv, &wsm_buf);
+			break;
+		case 0x080C:
+			ret = wsm_suspend_resume_indication(priv,
+					link_id, &wsm_buf);
+			break;
+		default:
+			STUB();
+		}
+	} else {
+		WARN_ON(1);
+		ret = -EINVAL;
+	}
+out:
+	return ret;
+}
+
+static bool wsm_handle_tx_data(struct cw1200_common *priv,
+			       const struct wsm_tx *wsm,
+			       const struct ieee80211_tx_info *tx_info,
+			       const struct cw1200_txpriv *txpriv,
+			       struct cw1200_queue *queue)
+{
+	bool handled = false;
+	const struct ieee80211_hdr *frame =
+		(struct ieee80211_hdr *) &((u8 *)wsm)[txpriv->offset];
+	__le16 fctl = frame->frame_control;
+	enum {
+		doProbe,
+		doDrop,
+		doJoin,
+		doOffchannel,
+		doWep,
+		doTx,
+	} action = doTx;
+
+	switch (priv->mode) {
+	case NL80211_IFTYPE_STATION:
+		if (unlikely((priv->join_status == CW1200_JOIN_STATUS_STA) &&
+			ieee80211_is_nullfunc(fctl))) {
+			spin_lock(&priv->bss_loss_lock);
+			if (priv->bss_loss_status == CW1200_BSS_LOSS_CHECKING) {
+				priv->bss_loss_status =
+						CW1200_BSS_LOSS_CONFIRMING;
+				priv->bss_loss_confirm_id = wsm->packetID;
+			}
+			spin_unlock(&priv->bss_loss_lock);
+		} else if (unlikely(
+			(priv->join_status <= CW1200_JOIN_STATUS_MONITOR) ||
+			memcmp(frame->addr1, priv->join_bssid,
+				sizeof(priv->join_bssid)))) {
+			if (ieee80211_is_auth(fctl))
+				action = doJoin;
+			else if (ieee80211_is_probe_req(fctl))
+				action = doTx;
+			else if (priv->join_status >=
+					CW1200_JOIN_STATUS_MONITOR)
+				action = doTx;
+			else
+				action = doOffchannel;
+		}
+		break;
+	case NL80211_IFTYPE_AP:
+		if (unlikely(!priv->join_status))
+			action = doDrop;
+		else if (unlikely(!(BIT(txpriv->raw_link_id) &
+				(BIT(0) | priv->link_id_map)))) {
+			wiphy_warn(priv->hw->wiphy,
+					"A frame with expired link id "
+					"is dropped.\n");
+			action = doDrop;
+		}
+		if (cw1200_queue_get_generation(wsm->packetID) >
+				CW1200_MAX_REQUEUE_ATTEMPTS) {
+			/* HACK!!! WSM324 firmware has tendency to requeue
+			 * multicast frames in a loop, causing performance
+			 * drop and high power consumption of the driver.
+			 * In this situation it is better just to drop
+			 * the problematic frame. */
+			wiphy_warn(priv->hw->wiphy,
+					"Too many attempts "
+					"to requeue a frame. "
+					"Frame is dropped.\n");
+			action = doDrop;
+		}
+		break;
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_MESH_POINT:
+		STUB();
+	case NL80211_IFTYPE_MONITOR:
+	default:
+		action = doDrop;
+		break;
+	}
+
+	if (action == doTx) {
+		if (unlikely(ieee80211_is_probe_req(fctl)))
+			action = doProbe;
+		else if ((fctl & __cpu_to_le32(IEEE80211_FCTL_PROTECTED)) &&
+			tx_info->control.hw_key &&
+			unlikely(tx_info->control.hw_key->keyidx !=
+					priv->wep_default_key_id) &&
+			(tx_info->control.hw_key->cipher ==
+					WLAN_CIPHER_SUITE_WEP40 ||
+			 tx_info->control.hw_key->cipher ==
+					WLAN_CIPHER_SUITE_WEP104))
+			action = doWep;
+	}
+
+	switch (action) {
+	case doProbe:
+	{
+		/* An interesting FW "feature". Device filters
+		 * probe responses.
+		 * The easiest way to get it back is to convert
+		 * probe request into WSM start_scan command. */
+		wsm_printk(KERN_DEBUG \
+			"[WSM] Convert probe request to scan.\n");
+		wsm_lock_tx_async(priv);
+		priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
+		queue_delayed_work(priv->workqueue,
+				&priv->scan.probe_work, 0);
+		handled = true;
+	}
+	break;
+	case doDrop:
+	{
+		/* See detailed description of "join" below.
+		 * We are dropping everything except AUTH in non-joined mode. */
+		wsm_printk(KERN_DEBUG "[WSM] Drop frame (0x%.4X).\n", fctl);
+		BUG_ON(cw1200_queue_remove(queue,
+			__le32_to_cpu(wsm->packetID)));
+		handled = true;
+	}
+	break;
+	case doJoin:
+	{
+		/* There is one more interesting "feature"
+		 * in FW: it can't do RX/TX before "join".
+		 * "Join" here is not an association,
+		 * but just a syncronization between AP and STA.
+		 * priv->join_status is used only in bh thread and does
+		 * not require protection */
+		wsm_printk(KERN_DEBUG "[WSM] Issue join command.\n");
+		wsm_lock_tx_async(priv);
+		priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
+		if (queue_work(priv->workqueue, &priv->join_work) <= 0)
+			wsm_unlock_tx(priv);
+		handled = true;
+	}
+	break;
+	case doOffchannel:
+	{
+		wsm_printk(KERN_DEBUG "[WSM] Offchannel TX request.\n");
+		wsm_lock_tx_async(priv);
+		priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
+		if (queue_work(priv->workqueue, &priv->offchannel_work) <= 0)
+			wsm_unlock_tx(priv);
+		handled = true;
+	}
+	break;
+	case doWep:
+	{
+		wsm_printk(KERN_DEBUG "[WSM] Issue set_default_wep_key.\n");
+		wsm_lock_tx_async(priv);
+		priv->wep_default_key_id = tx_info->control.hw_key->keyidx;
+		priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
+		if (queue_work(priv->workqueue, &priv->wep_key_work) <= 0)
+			wsm_unlock_tx(priv);
+		handled = true;
+	}
+	break;
+	case doTx:
+	{
+#if 0
+		/* Kept for history. If you want to implement wsm->more,
+		 * make sure you are able to send a frame after that. */
+		wsm->more = (count > 1) ? 1 : 0;
+		if (wsm->more) {
+			/* HACK!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+			 * It's undocumented in WSM spec, but CW1200 hangs
+			 * if 'more' is set and no TX is performed due to TX
+			 * buffers limitation. */
+			if (priv->hw_bufs_used + 1 ==
+					priv->wsm_caps.numInpChBufs)
+				wsm->more = 0;
+		}
+
+		/* BUG!!! FIXME: we can't use 'more' at all: we don't know
+		 * future. It could be a request from upper layer with TX lock
+		 * requirements (scan, for example). If "more" is set device
+		 * will not send data and wsm_tx_lock() will fail...
+		 * It's not obvious how to fix this deadlock. Any ideas?
+		 * As a workaround more is set to 0. */
+		wsm->more = 0;
+#endif /* 0 */
+
+		if (ieee80211_is_deauth(fctl) &&
+				priv->mode != NL80211_IFTYPE_AP) {
+			/* Shedule unjoin work */
+			wsm_printk(KERN_DEBUG "[WSM] Issue unjoin command"
+				" (TX).\n");
+#if 0
+			wsm->more = 0;
+#endif /* 0 */
+			wsm_lock_tx_async(priv);
+			if (queue_work(priv->workqueue,
+					&priv->unjoin_work) <= 0)
+				wsm_unlock_tx(priv);
+		}
+	}
+	break;
+	}
+	return handled;
+}
+
+static int cw1200_get_prio_queue(struct cw1200_common *priv,
+				 u32 link_id_map, int *total)
+{
+	static const int urgent = BIT(CW1200_LINK_ID_AFTER_DTIM) |
+		BIT(CW1200_LINK_ID_UAPSD);
+	struct wsm_edca_queue_params *edca;
+	unsigned score, best = -1;
+	int winner = -1;
+	int queued;
+	int i;
+
+	/* search for a winner using edca params */
+	for (i = 0; i < 4; ++i) {
+		queued = cw1200_queue_get_num_queued(&priv->tx_queue[i],
+				link_id_map);
+		if (!queued)
+			continue;
+		*total += queued;
+		edca = &priv->edca.params[i];
+		score = ((edca->aifns + edca->cwMin) << 16) +
+				(edca->cwMax - edca->cwMin) *
+				(random32() & 0xFFFF);
+		if (score < best && (winner < 0 || i != 3)) {
+			best = score;
+			winner = i;
+		}
+	}
+
+	/* override winner if bursting */
+	if (winner >= 0 && priv->tx_burst_idx >= 0 &&
+			winner != priv->tx_burst_idx &&
+			!cw1200_queue_get_num_queued(
+				&priv->tx_queue[winner],
+				link_id_map & urgent) &&
+			cw1200_queue_get_num_queued(
+				&priv->tx_queue[priv->tx_burst_idx],
+				link_id_map))
+		winner = priv->tx_burst_idx;
+
+	return winner;
+}
+
+static int wsm_get_tx_queue_and_mask(struct cw1200_common *priv,
+				     struct cw1200_queue **queue_p,
+				     u32 *tx_allowed_mask_p,
+				     bool *more)
+{
+	int idx;
+	u32 tx_allowed_mask;
+	int total = 0;
+
+	/* Search for a queue with multicast frames buffered */
+	if (priv->tx_multicast) {
+		tx_allowed_mask = BIT(CW1200_LINK_ID_AFTER_DTIM);
+		idx = cw1200_get_prio_queue(priv,
+				tx_allowed_mask, &total);
+		if (idx >= 0) {
+			*more = total > 1;
+			goto found;
+		}
+	}
+
+	/* Search for unicast traffic */
+	tx_allowed_mask = ~priv->sta_asleep_mask;
+	tx_allowed_mask |= BIT(CW1200_LINK_ID_UAPSD);
+	if (priv->sta_asleep_mask) {
+		tx_allowed_mask |= priv->pspoll_mask;
+		tx_allowed_mask &= ~BIT(CW1200_LINK_ID_AFTER_DTIM);
+	} else {
+		tx_allowed_mask |= BIT(CW1200_LINK_ID_AFTER_DTIM);
+	}
+	idx = cw1200_get_prio_queue(priv,
+			tx_allowed_mask, &total);
+	if (idx < 0)
+		return -ENOENT;
+
+found:
+	*queue_p = &priv->tx_queue[idx];
+	*tx_allowed_mask_p = tx_allowed_mask;
+	return 0;
+}
+
+int wsm_get_tx(struct cw1200_common *priv, u8 **data,
+	       size_t *tx_len, int *burst)
+{
+	struct wsm_tx *wsm = NULL;
+	struct ieee80211_tx_info *tx_info;
+	struct cw1200_queue *queue = NULL;
+	int queue_num;
+	u32 tx_allowed_mask = 0;
+	const struct cw1200_txpriv *txpriv = NULL;
+	/*
+	 * Count was intended as an input for wsm->more flag.
+	 * During implementation it was found that wsm->more
+	 * is not usable, see details above. It is kept just
+	 * in case you would like to try to implement it again.
+	 */
+	int count = 0;
+
+	/* More is used only for broadcasts. */
+	bool more = false;
+
+	count = cw1200_itp_get_tx(priv, data, tx_len, burst);
+	if (count)
+		return count;
+
+	if (priv->wsm_cmd.ptr) {
+		++count;
+		spin_lock(&priv->wsm_cmd.lock);
+		BUG_ON(!priv->wsm_cmd.ptr);
+		*data = priv->wsm_cmd.ptr;
+		*tx_len = priv->wsm_cmd.len;
+		*burst = 1;
+		spin_unlock(&priv->wsm_cmd.lock);
+	} else {
+		for (;;) {
+			int ret;
+
+			if (atomic_add_return(0, &priv->tx_lock))
+				break;
+
+			spin_lock_bh(&priv->ps_state_lock);
+
+			ret = wsm_get_tx_queue_and_mask(priv, &queue,
+					&tx_allowed_mask, &more);
+			queue_num = queue - priv->tx_queue;
+
+			if (priv->buffered_multicasts &&
+					(ret || !more) &&
+					(priv->tx_multicast ||
+					 !priv->sta_asleep_mask)) {
+				priv->buffered_multicasts = false;
+				if (priv->tx_multicast) {
+					priv->tx_multicast = false;
+					queue_work(priv->workqueue,
+						&priv->multicast_stop_work);
+				}
+			}
+
+			spin_unlock_bh(&priv->ps_state_lock);
+
+			if (ret)
+				break;
+
+			if (cw1200_queue_get(queue,
+					tx_allowed_mask,
+					&wsm, &tx_info, &txpriv))
+				continue;
+
+			if (wsm_handle_tx_data(priv, wsm,
+					tx_info, txpriv, queue))
+				continue;  /* Handled by WSM */
+
+			wsm->hdr.id &= __cpu_to_le16(
+				~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX));
+			wsm->hdr.id |= cpu_to_le16(
+				WSM_TX_LINK_ID(txpriv->raw_link_id));
+			priv->pspoll_mask &= ~BIT(txpriv->raw_link_id);
+
+			*data = (u8 *)wsm;
+			*tx_len = __le16_to_cpu(wsm->hdr.len);
+
+			/* allow bursting if txop is set */
+			if (priv->edca.params[queue_num].txOpLimit)
+				*burst = min(*burst,
+					(int)cw1200_queue_get_num_queued(
+						queue, tx_allowed_mask) + 1);
+			else
+				*burst = 1;
+
+			/* store index of bursting queue */
+			if (*burst > 1)
+				priv->tx_burst_idx = queue_num;
+			else
+				priv->tx_burst_idx = -1;
+
+			if (more) {
+				struct ieee80211_hdr *hdr =
+					(struct ieee80211_hdr *)
+					&((u8 *)wsm)[txpriv->offset];
+				/* more buffered multicast/broadcast frames
+				 *  ==> set MoreData flag in IEEE 802.11 header
+				 *  to inform PS STAs */
+				hdr->frame_control |=
+					cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+			}
+
+			wsm_printk(KERN_DEBUG "[WSM] >>> 0x%.4X (%d) %p %c\n",
+				0x0004, *tx_len, *data,
+				wsm->more ? 'M' : ' ');
+			++count;
+			break;
+		}
+	}
+
+	return count;
+}
+
+void wsm_txed(struct cw1200_common *priv, u8 *data)
+{
+	if (data == priv->wsm_cmd.ptr) {
+		spin_lock(&priv->wsm_cmd.lock);
+		priv->wsm_cmd.ptr = NULL;
+		spin_unlock(&priv->wsm_cmd.lock);
+	}
+}
+
+/* ******************************************************************** */
+/* WSM buffer								*/
+
+void wsm_buf_init(struct wsm_buf *buf)
+{
+	BUG_ON(buf->begin);
+	buf->begin = kmalloc(SDIO_BLOCK_SIZE, GFP_KERNEL | GFP_DMA);
+	buf->end = buf->begin ? &buf->begin[SDIO_BLOCK_SIZE] : buf->begin;
+	wsm_buf_reset(buf);
+}
+
+void wsm_buf_deinit(struct wsm_buf *buf)
+{
+	kfree(buf->begin);
+	buf->begin = buf->data = buf->end = NULL;
+}
+
+static void wsm_buf_reset(struct wsm_buf *buf)
+{
+	if (buf->begin) {
+		buf->data = &buf->begin[4];
+		*(u32 *)buf->begin = 0;
+	} else
+		buf->data = buf->begin;
+}
+
+static int wsm_buf_reserve(struct wsm_buf *buf, size_t extra_size)
+{
+	size_t pos = buf->data - buf->begin;
+	size_t size = pos + extra_size;
+
+
+	if (size & (SDIO_BLOCK_SIZE - 1)) {
+		size &= SDIO_BLOCK_SIZE;
+		size += SDIO_BLOCK_SIZE;
+	}
+
+	buf->begin = krealloc(buf->begin, size, GFP_KERNEL | GFP_DMA);
+	if (buf->begin) {
+		buf->data = &buf->begin[pos];
+		buf->end = &buf->begin[size];
+		return 0;
+	} else {
+		buf->end = buf->data = buf->begin;
+		return -ENOMEM;
+	}
+}
+
+
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
new file mode 100644
index 00000000000..c3bd002d432
--- /dev/null
+++ b/drivers/staging/cw1200/wsm.h
@@ -0,0 +1,1833 @@
+/*
+ * WSM host interface (HI) interface for ST-Ericsson CW1200 mac80211 drivers
+ *
+ * Copyright (c) 2010, ST-Ericsson
+ * Author: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
+ *
+ * Based on CW1200 UMAC WSM API, which is
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Stewart Mathers <stewart.mathers@stericsson.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef CW1200_WSM_H_INCLUDED
+#define CW1200_WSM_H_INCLUDED
+
+#include <linux/spinlock.h>
+
+struct cw1200_common;
+
+/* Bands */
+/* Radio band 2.412 -2.484 GHz. */
+#define WSM_PHY_BAND_2_4G		(0)
+
+/* Radio band 4.9375-5.8250 GHz. */
+#define WSM_PHY_BAND_5G			(1)
+
+/* Transmit rates */
+/* 1   Mbps            ERP-DSSS */
+#define WSM_TRANSMIT_RATE_1		(0)
+
+/* 2   Mbps            ERP-DSSS */
+#define WSM_TRANSMIT_RATE_2		(1)
+
+/* 5.5 Mbps            ERP-CCK, ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_5		(2) */
+
+/* 11  Mbps            ERP-CCK, ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_11		(3) */
+
+/* 22  Mbps            ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_22		(4) */
+
+/* 33  Mbps            ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_33		(5) */
+
+/* 6   Mbps   (3 Mbps) ERP-OFDM, BPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_6		(6)
+
+/* 9   Mbps (4.5 Mbps) ERP-OFDM, BPSK coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_9		(7)
+
+/* 12  Mbps  (6 Mbps)  ERP-OFDM, QPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_12		(8)
+
+/* 18  Mbps  (9 Mbps)  ERP-OFDM, QPSK coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_18		(9)
+
+/* 24  Mbps (12 Mbps)  ERP-OFDM, 16QAM coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_24		(10)
+
+/* 36  Mbps (18 Mbps)  ERP-OFDM, 16QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_36		(11)
+
+/* 48  Mbps (24 Mbps)  ERP-OFDM, 64QAM coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_48		(12)
+
+/* 54  Mbps (27 Mbps)  ERP-OFDM, 64QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_54		(13)
+
+/* 6.5 Mbps            HT-OFDM, BPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_HT_6		(14)
+
+/* 13  Mbps            HT-OFDM, QPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_HT_13		(15)
+
+/* 19.5 Mbps           HT-OFDM, QPSK coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_HT_19		(16)
+
+/* 26  Mbps            HT-OFDM, 16QAM coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_HT_26		(17)
+
+/* 39  Mbps            HT-OFDM, 16QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_HT_39		(18)
+
+/* 52  Mbps            HT-OFDM, 64QAM coding rate 2/3 */
+#define WSM_TRANSMIT_RATE_HT_52		(19)
+
+/* 58.5 Mbps           HT-OFDM, 64QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_HT_58		(20)
+
+/* 65  Mbps            HT-OFDM, 64QAM coding rate 5/6 */
+#define WSM_TRANSMIT_RATE_HT_65		(21)
+
+/* Scan types */
+/* Foreground scan */
+#define WSM_SCAN_TYPE_FOREGROUND	(0)
+
+/* Background scan */
+#define WSM_SCAN_TYPE_BACKGROUND	(1)
+
+/* Auto scan */
+#define WSM_SCAN_TYPE_AUTO		(2)
+
+/* Scan flags */
+/* Forced background scan means if the station cannot */
+/* enter the power-save mode, it shall force to perform a */
+/* background scan. Only valid when ScanType is */
+/* background scan. */
+#define WSM_SCAN_FLAG_FORCE_BACKGROUND	(BIT(0))
+
+/* The WLAN device scans one channel at a time so */
+/* that disturbance to the data traffic is minimized. */
+#define WSM_SCAN_FLAG_SPLIT_METHOD	(BIT(1))
+
+/* Preamble Type. Long if not set. */
+#define WSM_SCAN_FLAG_SHORT_PREAMBLE	(BIT(2))
+
+/* 11n Tx Mode. Mixed if not set. */
+#define WSM_SCAN_FLAG_11N_GREENFIELD	(BIT(3))
+
+/* Scan constraints */
+/* Maximum number of channels to be scanned. */
+#define WSM_SCAN_MAX_NUM_OF_CHANNELS	(48)
+
+/* The maximum number of SSIDs that the device can scan for. */
+#define WSM_SCAN_MAX_NUM_OF_SSIDS	(2)
+
+/* Power management modes */
+/* 802.11 Active mode */
+#define WSM_PSM_ACTIVE			(0)
+
+/* 802.11 PS mode */
+#define WSM_PSM_PS			BIT(0)
+
+/* Fast Power Save bit */
+#define WSM_PSM_FAST_PS_FLAG		BIT(7)
+
+/* Dynamic aka Fast power save */
+#define WSM_PSM_FAST_PS			(BIT(0) | BIT(7))
+
+/* Undetermined */
+/* Note : Undetermined status is reported when the */
+/* NULL data frame used to advertise the PM mode to */
+/* the AP at Pre or Post Background Scan is not Acknowledged */
+#define WSM_PSM_UNKNOWN			BIT(1)
+
+/* Queue IDs */
+/* best effort/legacy */
+#define WSM_QUEUE_BEST_EFFORT		(0)
+
+/* background */
+#define WSM_QUEUE_BACKGROUND		(1)
+
+/* video */
+#define WSM_QUEUE_VIDEO			(2)
+
+/* voice */
+#define WSM_QUEUE_VOICE			(3)
+
+/* HT TX parameters */
+/* Non-HT */
+#define WSM_HT_TX_NON_HT		(0)
+
+/* Mixed format */
+#define WSM_HT_TX_MIXED			(1)
+
+/* Greenfield format */
+#define WSM_HT_TX_GREENFIELD		(2)
+
+/* STBC allowed */
+#define WSM_HT_TX_STBC			(BIT(7))
+
+/* EPTA prioirty flags for BT Coex */
+/* default epta priority */
+#define WSM_EPTA_PRIORITY_DEFAULT	4
+/* use for normal data */
+#define WSM_EPTA_PRIORITY_DATA		4
+/* use for connect/disconnect/roaming*/
+#define WSM_EPTA_PRIORITY_MGT		5
+/* use for action frames */
+#define WSM_EPTA_PRIORITY_ACTION	5
+/* use for AC_VI data */
+#define WSM_EPTA_PRIORITY_VIDEO		5
+/* use for AC_VO data */
+#define WSM_EPTA_PRIORITY_VOICE		6
+/* use for EAPOL exchange */
+#define WSM_EPTA_PRIORITY_EAPOL		7
+
+/* TX status */
+/* Frame was sent aggregated */
+/* Only valid for WSM_SUCCESS status. */
+#define WSM_TX_STATUS_AGGREGATION	(BIT(0))
+
+/* Host should requeue this frame later. */
+/* Valid only when status is WSM_REQUEUE. */
+#define WSM_TX_STATUS_REQUEUE		(BIT(1))
+
+/* Normal Ack */
+#define WSM_TX_STATUS_NORMAL_ACK	(0<<2)
+
+/* No Ack */
+#define WSM_TX_STATUS_NO_ACK		(1<<2)
+
+/* No explicit acknowledgement */
+#define WSM_TX_STATUS_NO_EXPLICIT_ACK	(2<<2)
+
+/* Block Ack */
+/* Only valid for WSM_SUCCESS status. */
+#define WSM_TX_STATUS_BLOCK_ACK		(3<<2)
+
+/* RX status */
+/* Unencrypted */
+#define WSM_RX_STATUS_UNENCRYPTED	(0<<0)
+
+/* WEP */
+#define WSM_RX_STATUS_WEP		(1<<0)
+
+/* TKIP */
+#define WSM_RX_STATUS_TKIP		(2<<0)
+
+/* AES */
+#define WSM_RX_STATUS_AES		(3<<0)
+
+/* WAPI */
+#define WSM_RX_STATUS_WAPI		(4<<0)
+
+/* Macro to fetch encryption subfield. */
+#define WSM_RX_STATUS_ENCRYPTION(status) ((status) & 0x07)
+
+/* Frame was part of an aggregation */
+#define WSM_RX_STATUS_AGGREGATE		(BIT(3))
+
+/* Frame was first in the aggregation */
+#define WSM_RX_STATUS_AGGREGATE_FIRST	(BIT(4))
+
+/* Frame was last in the aggregation */
+#define WSM_RX_STATUS_AGGREGATE_LAST	(BIT(5))
+
+/* Indicates a defragmented frame */
+#define WSM_RX_STATUS_DEFRAGMENTED	(BIT(6))
+
+/* Indicates a Beacon frame */
+#define WSM_RX_STATUS_BEACON		(BIT(7))
+
+/* Indicates STA bit beacon TIM field */
+#define WSM_RX_STATUS_TIM		(BIT(8))
+
+/* Indicates Beacon frame's virtual bitmap contains multicast bit */
+#define WSM_RX_STATUS_MULTICAST		(BIT(9))
+
+/* Indicates frame contains a matching SSID */
+#define WSM_RX_STATUS_MATCHING_SSID	(BIT(10))
+
+/* Indicates frame contains a matching BSSI */
+#define WSM_RX_STATUS_MATCHING_BSSI	(BIT(11))
+
+/* Indicates More bit set in Framectl field */
+#define WSM_RX_STATUS_MORE_DATA		(BIT(12))
+
+/* Indicates frame received during a measurement process */
+#define WSM_RX_STATUS_MEASUREMENT	(BIT(13))
+
+/* Indicates frame received as an HT packet */
+#define WSM_RX_STATUS_HT		(BIT(14))
+
+/* Indicates frame received with STBC */
+#define WSM_RX_STATUS_STBC		(BIT(15))
+
+/* Indicates Address 1 field matches dot11StationId */
+#define WSM_RX_STATUS_ADDRESS1		(BIT(16))
+
+/* Indicates Group address present in the Address 1 field */
+#define WSM_RX_STATUS_GROUP		(BIT(17))
+
+/* Indicates Broadcast address present in the Address 1 field */
+#define WSM_RX_STATUS_BROADCAST		(BIT(18))
+
+/* Indicates group key used with encrypted frames */
+#define WSM_RX_STATUS_GROUP_KEY		(BIT(19))
+
+/* Macro to fetch encryption key index. */
+#define WSM_RX_STATUS_KEY_IDX(status)	(((status >> 20)) & 0x0F)
+
+/* Frame Control field starts at Frame offset + 2 */
+#define WSM_TX_2BYTES_SHIFT		(BIT(7))
+
+/* Join mode */
+/* IBSS */
+#define WSM_JOIN_MODE_IBSS		(0)
+
+/* BSS */
+#define WSM_JOIN_MODE_BSS		(1)
+
+/* PLCP preamble type */
+/* For long preamble */
+#define WSM_JOIN_PREAMBLE_LONG		(0)
+
+/* For short preamble (Long for 1Mbps) */
+#define WSM_JOIN_PREAMBLE_SHORT		(1)
+
+/* For short preamble (Long for 1 and 2Mbps) */
+#define WSM_JOIN_PREAMBLE_SHORT_2	(2)
+
+/* Join flags */
+/* Unsynchronized */
+#define WSM_JOIN_FLAGS_UNSYNCRONIZED	BIT(0)
+/* The BSS owner is a P2P GO */
+#define WSM_JOIN_FLAGS_P2P_GO		BIT(1)
+/* Force to join BSS with the BSSID and the
+ * SSID specified without waiting for beacons. The
+ * ProbeForJoin parameter is ignored. */
+#define WSM_JOIN_FLAGS_FORCE		BIT(2)
+/* Give probe request/response higher
+ * priority over the BT traffic */
+#define WSM_JOIN_FLAGS_PRIO		BIT(3)
+
+/* Key types */
+#define WSM_KEY_TYPE_WEP_DEFAULT	(0)
+#define WSM_KEY_TYPE_WEP_PAIRWISE	(1)
+#define WSM_KEY_TYPE_TKIP_GROUP		(2)
+#define WSM_KEY_TYPE_TKIP_PAIRWISE	(3)
+#define WSM_KEY_TYPE_AES_GROUP		(4)
+#define WSM_KEY_TYPE_AES_PAIRWISE	(5)
+#define WSM_KEY_TYPE_WAPI_GROUP		(6)
+#define WSM_KEY_TYPE_WAPI_PAIRWISE	(7)
+
+/* Key indexes */
+#define WSM_KEY_MAX_INDEX		(10)
+
+/* ACK policy */
+#define WSM_ACK_POLICY_NORMAL		(0)
+#define WSM_ACK_POLICY_NO_ACK		(1)
+
+/* Start modes */
+#define WSM_START_MODE_AP		(0)	/* Mini AP */
+#define WSM_START_MODE_P2P_GO		(1)	/* P2P GO */
+#define WSM_START_MODE_P2P_DEV		(2)	/* P2P device */
+
+/* SetAssociationMode MIB flags */
+#define WSM_ASSOCIATION_MODE_USE_PREAMBLE_TYPE		(BIT(0))
+#define WSM_ASSOCIATION_MODE_USE_HT_MODE		(BIT(1))
+#define WSM_ASSOCIATION_MODE_USE_BASIC_RATE_SET		(BIT(2))
+#define WSM_ASSOCIATION_MODE_USE_MPDU_START_SPACING	(BIT(3))
+#define WSM_ASSOCIATION_MODE_SNOOP_ASSOC_FRAMES		(BIT(4))
+
+/* RcpiRssiThreshold MIB flags */
+#define WSM_RCPI_RSSI_THRESHOLD_ENABLE	(BIT(0))
+#define WSM_RCPI_RSSI_USE_RSSI		(BIT(1))
+#define WSM_RCPI_RSSI_DONT_USE_UPPER	(BIT(2))
+#define WSM_RCPI_RSSI_DONT_USE_LOWER	(BIT(3))
+
+/* Update-ie constants */
+#define WSM_UPDATE_IE_BEACON		(BIT(0))
+#define WSM_UPDATE_IE_PROBE_RESP	(BIT(1))
+#define WSM_UPDATE_IE_PROBE_REQ		(BIT(2))
+
+/* WSM events */
+/* Error */
+#define WSM_EVENT_ERROR			(0)
+
+/* BSS lost */
+#define WSM_EVENT_BSS_LOST		(1)
+
+/* BSS regained */
+#define WSM_EVENT_BSS_REGAINED		(2)
+
+/* Radar detected */
+#define WSM_EVENT_RADAR_DETECTED	(3)
+
+/* RCPI or RSSI threshold triggered */
+#define WSM_EVENT_RCPI_RSSI		(4)
+
+/* BT inactive */
+#define WSM_EVENT_BT_INACTIVE		(5)
+
+/* BT active */
+#define WSM_EVENT_BT_ACTIVE		(6)
+
+/* MIB IDs */
+/* 4.1  dot11StationId */
+#define WSM_MIB_ID_DOT11_STATION_ID		0x0000
+
+/* 4.2  dot11MaxtransmitMsduLifeTime */
+#define WSM_MIB_ID_DOT11_MAX_TRANSMIT_LIFTIME	0x0001
+
+/* 4.3  dot11MaxReceiveLifeTime */
+#define WSM_MIB_ID_DOT11_MAX_RECEIVE_LIFETIME	0x0002
+
+/* 4.4  dot11SlotTime */
+#define WSM_MIB_ID_DOT11_SLOT_TIME		0x0003
+
+/* 4.5  dot11GroupAddressesTable */
+#define WSM_MIB_ID_DOT11_GROUP_ADDRESSES_TABLE	0x0004
+#define WSM_MAX_GRP_ADDRTABLE_ENTRIES		8
+
+/* 4.6  dot11WepDefaultKeyId */
+#define WSM_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID	0x0005
+
+/* 4.7  dot11CurrentTxPowerLevel */
+#define WSM_MIB_ID_DOT11_CURRENT_TX_POWER_LEVEL	0x0006
+
+/* 4.8  dot11RTSThreshold */
+#define WSM_MIB_ID_DOT11_RTS_THRESHOLD		0x0007
+
+/* 4.9  NonErpProtection */
+#define WSM_MIB_ID_NON_ERP_PROTECTION		0x1000
+
+/* 4.10 ArpIpAddressesTable */
+#define WSM_MIB_ID_ARP_IP_ADDRESSES_TABLE	0x1001
+#define WSM_MAX_ARP_IP_ADDRTABLE_ENTRIES	1
+
+/* 4.11 TemplateFrame */
+#define WSM_MIB_ID_TEMPLATE_FRAME		0x1002
+
+/* 4.12 RxFilter */
+#define WSM_MIB_ID_RX_FILTER			0x1003
+
+/* 4.13 BeaconFilterTable */
+#define WSM_MIB_ID_BEACON_FILTER_TABLE		0x1004
+
+/* 4.14 BeaconFilterEnable */
+#define WSM_MIB_ID_BEACON_FILTER_ENABLE		0x1005
+
+/* 4.15 OperationalPowerMode */
+#define WSM_MIB_ID_OPERATIONAL_POWER_MODE	0x1006
+
+/* 4.16 BeaconWakeUpPeriod */
+#define WSM_MIB_ID_BEACON_WAKEUP_PERIOD		0x1007
+
+/* 4.17 RcpiRssiThreshold */
+#define WSM_MIB_ID_RCPI_RSSI_THRESHOLD		0x1009
+
+/* 4.18 StatisticsTable */
+#define WSM_MIB_ID_STATISTICS_TABLE		0x100A
+
+/* 4.19 IbssPsConfig */
+#define WSM_MIB_ID_IBSS_PS_CONFIG		0x100B
+
+/* 4.20 CountersTable */
+#define WSM_MIB_ID_COUNTERS_TABLE		0x100C
+
+/* 4.21 BlockAckPolicy */
+#define WSM_MIB_ID_BLOCK_ACK_POLICY		0x100E
+
+/* 4.22 OverrideInternalTxRate */
+#define WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE	0x100F
+
+/* 4.23 SetAssociationMode */
+#define WSM_MIB_ID_SET_ASSOCIATION_MODE		0x1010
+
+/* 4.24 UpdateEptaConfigData */
+#define WSM_MIB_ID_UPDATE_EPTA_CONFIG_DATA	0x1011
+
+/* 4.25 SelectCcaMethod */
+#define WSM_MIB_ID_SELECT_CCA_METHOD		0x1012
+
+/* 4.26 SetUpasdInformation */
+#define WSM_MIB_ID_SET_UAPSD_INFORMATION	0x1013
+
+/* 4.27 SetAutoCalibrationMode  WBF00004073 */
+#define WSM_MIB_ID_SET_AUTO_CALIBRATION_MODE	0x1015
+
+/* 4.28 SetTxRateRetryPolicy */
+#define WSM_MIB_ID_SET_TX_RATE_RETRY_POLICY	0x1016
+
+/* 4.29 SetHostMessageTypeFilter */
+#define WSM_MIB_ID_SET_HOST_MSG_TYPE_FILTER	0x1017
+
+/* 4.30 P2PFindInfo */
+#define WSM_MIB_ID_P2P_FIND_INFO		0x1018
+
+/* 4.31 P2PPsModeInfo */
+#define WSM_MIB_ID_P2P_PS_MODE_INFO		0x1019
+
+/* 4.32 SetEtherTypeDataFrameFilter */
+#define WSM_MIB_ID_SET_ETHERTYPE_DATAFRAME_FILTER 0x101A
+
+/* 4.33 SetUDPPortDataFrameFilter */
+#define WSM_MIB_ID_SET_UDPPORT_DATAFRAME_FILTER	0x101B
+
+/* 4.34 SetMagicDataFrameFilter */
+#define WSM_MIB_ID_SET_MAGIC_DATAFRAME_FILTER	0x101C
+
+/* This is the end of specification. */
+
+/* 4.35 P2PDeviceInfo */
+#define WSM_MIB_ID_P2P_DEVICE_INFO		0x101D
+
+/* 4.36 SetWCDMABand */
+#define WSM_MIB_ID_SET_WCDMA_BAND		0x101E
+
+/* 4.37 GroupTxSequenceCounter */
+#define WSM_MIB_ID_GRP_SEQ_COUNTER		0x101F
+
+/* 4.38 ProtectedMgmtPolicy */
+#define WSM_MIB_ID_PROTECTED_MGMT_POLICY	0x1020
+
+/* 4.39 SetHtProtection */
+#define WSM_MID_ID_SET_HT_PROTECTION		0x1021
+
+/* 4.40 GPIO Command */
+#define WSM_MIB_ID_GPIO_COMMAND			0x1022
+
+/* 4.41 TSF Counter Value */
+#define WSM_MIB_ID_TSF_COUNTER			0x1023
+
+/* Test Purposes Only */
+#define WSM_MIB_ID_BLOCK_ACK_INFO		0x100D
+
+/* 4.42 UseMultiTxConfMessage */
+#define WSM_MIB_USE_MULTI_TX_CONF		0x1024
+
+/* 4.43 Keep-alive period */
+#define WSM_MIB_ID_KEEP_ALIVE_PERIOD		0x1025
+
+/* 4.44 Disable BSSID filter */
+#define WSM_MIB_ID_DISABLE_BSSID_FILTER		0x1026
+
+/* Frame template types */
+#define WSM_FRAME_TYPE_PROBE_REQUEST	(0)
+#define WSM_FRAME_TYPE_BEACON		(1)
+#define WSM_FRAME_TYPE_NULL		(2)
+#define WSM_FRAME_TYPE_QOS_NULL		(3)
+#define WSM_FRAME_TYPE_PS_POLL		(4)
+#define WSM_FRAME_TYPE_PROBE_RESPONSE	(5)
+
+#define WSM_FRAME_GREENFIELD		(0x80)	/* See 4.11 */
+
+/* Status */
+/* The WSM firmware has completed a request */
+/* successfully. */
+#define WSM_STATUS_SUCCESS              (0)
+
+/* This is a generic failure code if other error codes do */
+/* not apply. */
+#define WSM_STATUS_FAILURE              (1)
+
+/* A request contains one or more invalid parameters. */
+#define WSM_INVALID_PARAMETER           (2)
+
+/* The request cannot perform because the device is in */
+/* an inappropriate mode. */
+#define WSM_ACCESS_DENIED               (3)
+
+/* The frame received includes a decryption error. */
+#define WSM_STATUS_DECRYPTFAILURE       (4)
+
+/* A MIC failure is detected in the received packets. */
+#define WSM_STATUS_MICFAILURE           (5)
+
+/* The transmit request failed due to retry limit being */
+/* exceeded. */
+#define WSM_STATUS_RETRY_EXCEEDED       (6)
+
+/* The transmit request failed due to MSDU life time */
+/* being exceeded. */
+#define WSM_STATUS_TX_LIFETIME_EXCEEDED (7)
+
+/* The link to the AP is lost. */
+#define WSM_STATUS_LINK_LOST            (8)
+
+/* No key was found for the encrypted frame */
+#define WSM_STATUS_NO_KEY_FOUND         (9)
+
+/* Jammer was detected when transmitting this frame */
+#define WSM_STATUS_JAMMER_DETECTED      (10)
+
+/* The message should be requeued later. */
+/* This is applicable only to Transmit */
+#define WSM_REQUEUE                     (11)
+
+/* Advanced filtering options */
+#define WSM_MAX_FILTER_ELEMENTS		(4)
+
+#define WSM_FILTER_ACTION_IGNORE	(0)
+#define WSM_FILTER_ACTION_FILTER_IN	(1)
+#define WSM_FILTER_ACTION_FILTER_OUT	(2)
+
+#define WSM_FILTER_PORT_TYPE_DST	(0)
+#define WSM_FILTER_PORT_TYPE_SRC	(1)
+
+
+
+struct wsm_hdr {
+	__le16 len;
+	__le16 id;
+};
+
+#define WSM_TX_SEQ_MAX			(7)
+#define WSM_TX_SEQ(seq)			\
+		((seq & WSM_TX_SEQ_MAX) << 13)
+#define WSM_TX_LINK_ID_MAX		(0x0F)
+#define WSM_TX_LINK_ID(link_id)		\
+		((link_id & WSM_TX_LINK_ID_MAX) << 6)
+
+#define MAX_BEACON_SKIP_TIME_MS 1000
+
+#define WSM_CMD_LAST_CHANCE_TIMEOUT (HZ * 3 / 2)
+
+/* ******************************************************************** */
+/* WSM capcbility							*/
+
+struct wsm_caps {
+	u16 numInpChBufs;
+	u16 sizeInpChBuf;
+	u16 hardwareId;
+	u16 hardwareSubId;
+	u16 firmwareCap;
+	u16 firmwareType;
+	u16 firmwareApiVer;
+	u16 firmwareBuildNumber;
+	u16 firmwareVersion;
+	int firmwareReady;
+};
+
+/* ******************************************************************** */
+/* WSM commands								*/
+
+struct wsm_tx_power_range {
+	int min_power_level;
+	int max_power_level;
+	u32 stepping;
+};
+
+/* 3.1 */
+struct wsm_configuration {
+	/* [in] */ u32 dot11MaxTransmitMsduLifeTime;
+	/* [in] */ u32 dot11MaxReceiveLifeTime;
+	/* [in] */ u32 dot11RtsThreshold;
+	/* [in, out] */ u8 *dot11StationId;
+	/* [in] */ const void *dpdData;
+	/* [in] */ size_t dpdData_size;
+	/* [out] */ u8 dot11FrequencyBandsSupported;
+	/* [out] */ u32 supportedRateMask;
+	/* [out] */ struct wsm_tx_power_range txPowerRange[2];
+};
+
+int wsm_configuration(struct cw1200_common *priv,
+		      struct wsm_configuration *arg);
+
+/* 3.3 */
+struct wsm_reset {
+	/* [in] */ int link_id;
+	/* [in] */ bool reset_statistics;
+};
+
+int wsm_reset(struct cw1200_common *priv, const struct wsm_reset *arg);
+
+/* 3.5 */
+int wsm_read_mib(struct cw1200_common *priv, u16 mibId, void *buf,
+		 size_t buf_size);
+
+/* 3.7 */
+int wsm_write_mib(struct cw1200_common *priv, u16 mibId, void *buf,
+		  size_t buf_size);
+
+/* 3.9 */
+struct wsm_ssid {
+	u8 ssid[32];
+	u32 length;
+};
+
+struct wsm_scan_ch {
+	u16 number;
+	u32 minChannelTime;
+	u32 maxChannelTime;
+	u32 txPowerLevel;
+};
+
+/* 3.13 */
+struct wsm_scan_complete {
+	/* WSM_STATUS_... */
+	u32 status;
+
+	/* WSM_PSM_... */
+	u8 psm;
+
+	/* Number of channels that the scan operation completed. */
+	u8 numChannels;
+};
+
+typedef void (*wsm_scan_complete_cb) (struct cw1200_common *priv,
+				      struct wsm_scan_complete *arg);
+
+/* 3.9 */
+struct wsm_scan {
+	/* WSM_PHY_BAND_... */
+	/* [in] */ u8 band;
+
+	/* WSM_SCAN_TYPE_... */
+	/* [in] */ u8 scanType;
+
+	/* WSM_SCAN_FLAG_... */
+	/* [in] */ u8 scanFlags;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [in] */ u8 maxTransmitRate;
+
+	/* Interval period in TUs that the device shall the re- */
+	/* execute the requested scan. Max value supported by the device */
+	/* is 256s. */
+	/* [in] */ u32 autoScanInterval;
+
+	/* Number of probe requests (per SSID) sent to one (1) */
+	/* channel. Zero (0) means that none is send, which */
+	/* means that a passive scan is to be done. Value */
+	/* greater than zero (0) means that an active scan is to */
+	/* be done. */
+	/* [in] */ u32 numOfProbeRequests;
+
+	/* Number of channels to be scanned. */
+	/* Maximum value is WSM_SCAN_MAX_NUM_OF_CHANNELS. */
+	/* [in] */ u8 numOfChannels;
+
+	/* Number of SSID provided in the scan command (this */
+	/* is zero (0) in broadcast scan) */
+	/* The maximum number of SSIDs is WSM_SCAN_MAX_NUM_OF_SSIDS. */
+	/* [in] */ u8 numOfSSIDs;
+
+	/* The delay time (in microseconds) period */
+	/* before sending a probe-request. */
+	/* [in] */ u8 probeDelay;
+
+	/* SSIDs to be scanned [numOfSSIDs]; */
+	/* [in] */ struct wsm_ssid *ssids;
+
+	/* Channels to be scanned [numOfChannels]; */
+	/* [in] */ struct wsm_scan_ch *ch;
+};
+
+int wsm_scan(struct cw1200_common *priv, const struct wsm_scan *arg);
+
+/* 3.11 */
+int wsm_stop_scan(struct cw1200_common *priv);
+
+/* 3.14 */
+struct wsm_tx_confirm {
+	/* Packet identifier used in wsm_tx. */
+	/* [out] */ u32 packetID;
+
+	/* WSM_STATUS_... */
+	/* [out] */ u32 status;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [out] */ u8 txedRate;
+
+	/* The number of times the frame was transmitted */
+	/* without receiving an acknowledgement. */
+	/* [out] */ u8 ackFailures;
+
+	/* WSM_TX_STATUS_... */
+	/* [out] */ u16 flags;
+
+	/* The total time in microseconds that the frame spent in */
+	/* the WLAN device before transmission as completed. */
+	/* [out] */ u32 mediaDelay;
+
+	/* The total time in microseconds that the frame spent in */
+	/* the WLAN device before transmission was started. */
+	/* [out] */ u32 txQueueDelay;
+
+	/* [out]*/ u32 link_id;
+};
+
+/* 3.15 */
+typedef void (*wsm_tx_confirm_cb) (struct cw1200_common *priv,
+				   struct wsm_tx_confirm *arg);
+
+/* Note that ideology of wsm_tx struct is different against the rest of
+ * WSM API. wsm_hdr is /not/ a caller-adapted struct to be used as an input
+ * argument for WSM call, but a prepared bytestream to be sent to firmware.
+ * It is filled partly in cw1200_tx, partly in low-level WSM code.
+ * Please pay attention once again: ideology is different.
+ *
+ * Legend:
+ * - [in]: cw1200_tx must fill this field.
+ * - [wsm]: the field is filled by low-level WSM.
+ */
+struct wsm_tx {
+	/* common WSM header */
+	/* [in/wsm] */ struct wsm_hdr hdr;
+
+	/* Packet identifier that meant to be used in completion. */
+	/* [in] */ __le32 packetID;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [in] */ u8 maxTxRate;
+
+	/* WSM_QUEUE_... */
+	/* [in] */ u8 queueId;
+
+	/* True: another packet is pending on the host for transmission. */
+	/* [wsm] */ u8 more;
+
+	/* Bit 0 = 0 - Start expiry time from first Tx attempt (default) */
+	/* Bit 0 = 1 - Start expiry time from receipt of Tx Request */
+	/* Bits 3:1  - PTA Priority */
+	/* Bits 6:4  - Tx Rate Retry Policy */
+	/* Bit 7 - Reserved */
+	/* [in] */ u8 flags;
+
+	/* Should be 0. */
+	/* [in] */ __le32 reserved;
+
+	/* The elapsed time in TUs, after the initial transmission */
+	/* of an MSDU, after which further attempts to transmit */
+	/* the MSDU shall be terminated. Overrides the global */
+	/* dot11MaxTransmitMsduLifeTime setting [optional] */
+	/* Device will set the default value if this is 0. */
+	/* [wsm] */ __le32 expireTime;
+
+	/* WSM_HT_TX_... */
+	/* [in] */ __le32 htTxParameters;
+};
+
+/* = sizeof(generic hi hdr) + sizeof(wsm hdr) + sizeof(alignment) */
+#define WSM_TX_EXTRA_HEADROOM (28)
+
+/* 3.16 */
+struct wsm_rx {
+	/* WSM_STATUS_... */
+	/* [out] */ u32 status;
+
+	/* Specifies the channel of the received packet. */
+	/* [out] */ u16 channelNumber;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [out] */ u8 rxedRate;
+
+	/* This value is expressed in signed Q8.0 format for */
+	/* RSSI and unsigned Q7.1 format for RCPI. */
+	/* [out] */ u8 rcpiRssi;
+
+	/* WSM_RX_STATUS_... */
+	/* [out] */ u32 flags;
+
+	/* An 802.11 frame. */
+	/* [out] */ void *frame;
+
+	/* Size of the frame */
+	/* [out] */ size_t frame_size;
+
+	/* Link ID */
+	/* [out] */ int link_id;
+};
+
+/* = sizeof(generic hi hdr) + sizeof(wsm hdr) */
+#define WSM_RX_EXTRA_HEADROOM (16)
+
+typedef void (*wsm_rx_cb) (struct cw1200_common *priv, struct wsm_rx *arg,
+			   struct sk_buff **skb_p);
+
+/* 3.17 */
+struct wsm_event {
+	/* WSM_STATUS_... */
+	/* [out] */ u32 eventId;
+
+	/* Indication parameters. */
+	/* For error indication, this shall be a 32-bit WSM status. */
+	/* For RCPI or RSSI indication, this should be an 8-bit */
+	/* RCPI or RSSI value. */
+	/* [out] */ u32 eventData;
+};
+
+struct cw1200_wsm_event {
+	struct list_head link;
+	struct wsm_event evt;
+};
+
+/* 3.18 - 3.22 */
+/* Measurement. Skipped for now. Irrelevent. */
+
+typedef void (*wsm_event_cb) (struct cw1200_common *priv,
+			      struct wsm_event *arg);
+
+/* 3.23 */
+struct wsm_join {
+	/* WSM_JOIN_MODE_... */
+	/* [in] */ u8 mode;
+
+	/* WSM_PHY_BAND_... */
+	/* [in] */ u8 band;
+
+	/* Specifies the channel number to join. The channel */
+	/* number will be mapped to an actual frequency */
+	/* according to the band */
+	/* [in] */ u16 channelNumber;
+
+	/* Specifies the BSSID of the BSS or IBSS to be joined */
+	/* or the IBSS to be started. */
+	/* [in] */ u8 bssid[6];
+
+	/* ATIM window of IBSS */
+	/* When ATIM window is zero the initiated IBSS does */
+	/* not support power saving. */
+	/* [in] */ u16 atimWindow;
+
+	/* WSM_JOIN_PREAMBLE_... */
+	/* [in] */ u8 preambleType;
+
+	/* Specifies if a probe request should be send with the */
+	/* specified SSID when joining to the network. */
+	/* [in] */ u8 probeForJoin;
+
+	/* DTIM Period (In multiples of beacon interval) */
+	/* [in] */ u8 dtimPeriod;
+
+	/* WSM_JOIN_FLAGS_... */
+	/* [in] */ u8 flags;
+
+	/* Length of the SSID */
+	/* [in] */ u32 ssidLength;
+
+	/* Specifies the SSID of the IBSS to join or start */
+	/* [in] */ u8 ssid[32];
+
+	/* Specifies the time between TBTTs in TUs */
+	/* [in] */ u32 beaconInterval;
+
+	/* A bit mask that defines the BSS basic rate set. */
+	/* [in] */ u32 basicRateSet;
+
+	/* Minimum transmission power level in units of 0.1dBm */
+	/* [out] */ int minPowerLevel;
+
+	/* Maximum transmission power level in units of 0.1dBm */
+	/* [out] */ int maxPowerLevel;
+};
+
+int wsm_join(struct cw1200_common *priv, struct wsm_join *arg);
+
+/* 3.25 */
+struct wsm_set_pm {
+	/* WSM_PSM_... */
+	/* [in] */ u8 pmMode;
+
+	/* in unit of 500us; 0 to use default */
+	/* [in] */ u8 fastPsmIdlePeriod;
+
+	/* in unit of 500us; 0 to use default */
+	/* [in] */ u8 apPsmChangePeriod;
+
+	/* in unit of 500us; 0 to disable auto-pspoll */
+	/* [in] */ u8 minAutoPsPollPeriod;
+};
+
+int wsm_set_pm(struct cw1200_common *priv, const struct wsm_set_pm *arg);
+
+/* 3.27 */
+struct wsm_set_pm_complete {
+	u8 psm;			/* WSM_PSM_... */
+};
+
+typedef void (*wsm_set_pm_complete_cb) (struct cw1200_common *priv,
+					struct wsm_set_pm_complete *arg);
+
+/* 3.28 */
+struct wsm_set_bss_params {
+	/* The number of lost consecutive beacons after which */
+	/* the WLAN device should indicate the BSS-Lost event */
+	/* to the WLAN host driver. */
+	u8 beaconLostCount;
+
+	/* The AID received during the association process. */
+	u16 aid;
+
+	/* The operational rate set mask */
+	u32 operationalRateSet;
+};
+
+int wsm_set_bss_params(struct cw1200_common *priv,
+		       const struct wsm_set_bss_params *arg);
+
+/* 3.30 */
+struct wsm_add_key {
+	u8 type;		/* WSM_KEY_TYPE_... */
+	u8 entryIndex;		/* Key entry index: 0 -- WSM_KEY_MAX_INDEX */
+	u16 reserved;
+	union {
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u8 reserved;
+			u8 keyLength;		/* Key length in bytes */
+			u8 keyData[16];		/* Key data */
+		} __packed wepPairwiseKey;
+		struct {
+			u8 keyId;		/* Unique per key identifier
+						 * (0..3) */
+			u8 keyLength;		/* Key length in bytes */
+			u16 reserved;
+			u8 keyData[16];		/* Key data */
+		} __packed wepGroupKey;
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u8 reserved[2];
+			u8 tkipKeyData[16];	/* TKIP key data */
+			u8 rxMicKey[8];		/* Rx MIC key */
+			u8 txMicKey[8];		/* Tx MIC key */
+		} __packed tkipPairwiseKey;
+		struct {
+			u8 tkipKeyData[16];	/* TKIP key data */
+			u8 rxMicKey[8];		/* Rx MIC key */
+			u8 keyId;		/* Key ID */
+			u8 reserved[3];
+			u8 rxSeqCounter[8];	/* Receive Sequence Counter */
+		} __packed tkipGroupKey;
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u16 reserved;
+			u8 aesKeyData[16];	/* AES key data */
+		} __packed aesPairwiseKey;
+		struct {
+			u8 aesKeyData[16];	/* AES key data */
+			u8 keyId;		/* Key ID */
+			u8 reserved[3];
+			u8 rxSeqCounter[8];	/* Receive Sequence Counter */
+		} __packed aesGroupKey;
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u8 keyId;		/* Key ID */
+			u8 reserved;
+			u8 wapiKeyData[16];	/* WAPI key data */
+			u8 micKeyData[16];	/* MIC key data */
+		} __packed wapiPairwiseKey;
+		struct {
+			u8 wapiKeyData[16];	/* WAPI key data */
+			u8 micKeyData[16];	/* MIC key data */
+			u8 keyId;		/* Key ID */
+			u8 reserved[3];
+		} __packed wapiGroupKey;
+	} __packed;
+} __packed;
+
+int wsm_add_key(struct cw1200_common *priv, const struct wsm_add_key *arg);
+
+/* 3.32 */
+struct wsm_remove_key {
+	/* Key entry index : 0-10 */
+	u8 entryIndex;
+};
+
+int wsm_remove_key(struct cw1200_common *priv,
+		   const struct wsm_remove_key *arg);
+
+/* 3.34 */
+struct wsm_set_tx_queue_params {
+	/* WSM_ACK_POLICY_... */
+	u8 ackPolicy;
+
+	/* Medium Time of TSPEC (in 32us units) allowed per */
+	/* One Second Averaging Period for this queue. */
+	u16 allowedMediumTime;
+
+	/* dot11MaxTransmitMsduLifetime to be used for the */
+	/* specified queue. */
+	u32 maxTransmitLifetime;
+};
+
+struct wsm_tx_queue_params {
+	/* NOTE: index is a linux queue id. */
+	struct wsm_set_tx_queue_params params[4];
+};
+
+
+#define WSM_TX_QUEUE_SET(queue_params, queue, ack_policy, allowed_time,\
+		max_life_time)	\
+do {							\
+	struct wsm_set_tx_queue_params *p = &(queue_params)->params[queue]; \
+	p->ackPolicy = (ack_policy);				\
+	p->allowedMediumTime = (allowed_time);				\
+	p->maxTransmitLifetime = (max_life_time);			\
+} while (0)
+
+int wsm_set_tx_queue_params(struct cw1200_common *priv,
+			    const struct wsm_set_tx_queue_params *arg, u8 id);
+
+/* 3.36 */
+struct wsm_edca_queue_params {
+	/* CWmin (in slots) for the access class. */
+	/* [in] */ u16 cwMin;
+
+	/* CWmax (in slots) for the access class. */
+	/* [in] */ u16 cwMax;
+
+	/* AIFS (in slots) for the access class. */
+	/* [in] */ u8 aifns;
+
+	/* TX OP Limit (in microseconds) for the access class. */
+	/* [in] */ u16 txOpLimit;
+
+	/* dot11MaxReceiveLifetime to be used for the specified */
+	/* the access class. Overrides the global */
+	/* dot11MaxReceiveLifetime value */
+	/* [in] */ u32 maxReceiveLifetime;
+
+	/* UAPSD trigger support for the access class. */
+	/* [in] */ bool uapsdEnable;
+};
+
+struct wsm_edca_params {
+	/* NOTE: index is a linux queue id. */
+	struct wsm_edca_queue_params params[4];
+};
+
+#define TXOP_UNIT 32
+#define WSM_EDCA_SET(edca, queue, aifs, cw_min, cw_max, txop, life_time,\
+		uapsd)	\
+	do {							\
+		struct wsm_edca_queue_params *p = &(edca)->params[queue]; \
+		p->cwMin = (cw_min);				\
+		p->cwMax = (cw_max);				\
+		p->aifns = (aifs);				\
+		p->txOpLimit = ((txop) * TXOP_UNIT);		\
+		p->maxReceiveLifetime = (life_time);		\
+		p->uapsdEnable = (uapsd);			\
+	} while (0)
+
+int wsm_set_edca_params(struct cw1200_common *priv,
+			const struct wsm_edca_params *arg);
+
+int wsm_set_uapsd_param(struct cw1200_common *priv,
+			const struct wsm_edca_params *arg);
+
+/* 3.38 */
+/* Set-System info. Skipped for now. Irrelevent. */
+
+/* 3.40 */
+struct wsm_switch_channel {
+	/* 1 - means the STA shall not transmit any further */
+	/* frames until the channel switch has completed */
+	/* [in] */ u8 channelMode;
+
+	/* Number of TBTTs until channel switch occurs. */
+	/* 0 - indicates switch shall occur at any time */
+	/* 1 - occurs immediately before the next TBTT */
+	/* [in] */ u8 channelSwitchCount;
+
+	/* The new channel number to switch to. */
+	/* Note this is defined as per section 2.7. */
+	/* [in] */ u16 newChannelNumber;
+};
+
+int wsm_switch_channel(struct cw1200_common *priv,
+		       const struct wsm_switch_channel *arg);
+
+typedef void (*wsm_channel_switch_cb) (struct cw1200_common *priv);
+
+struct wsm_start {
+	/* WSM_START_MODE_... */
+	/* [in] */ u8 mode;
+
+	/* WSM_PHY_BAND_... */
+	/* [in] */ u8 band;
+
+	/* Channel number */
+	/* [in] */ u16 channelNumber;
+
+	/* Client Traffic window in units of TU */
+	/* Valid only when mode == ..._P2P */
+	/* [in] */ u32 CTWindow;
+
+	/* Interval between two consecutive */
+	/* beacon transmissions in TU. */
+	/* [in] */ u32 beaconInterval;
+
+	/* DTIM period in terms of beacon intervals */
+	/* [in] */ u8 DTIMPeriod;
+
+	/* WSM_JOIN_PREAMBLE_... */
+	/* [in] */ u8 preambleType;
+
+	/* The delay time (in microseconds) period */
+	/* before sending a probe-request. */
+	/* [in] */ u8 probeDelay;
+
+	/* Length of the SSID */
+	/* [in] */ u8 ssidLength;
+
+	/* SSID of the BSS or P2P_GO to be started now. */
+	/* [in] */ u8 ssid[32];
+
+	/* The basic supported rates for the MiniAP. */
+	/* [in] */ u32 basicRateSet;
+};
+
+int wsm_start(struct cw1200_common *priv, const struct wsm_start *arg);
+
+struct wsm_beacon_transmit {
+	/* 1: enable; 0: disable */
+	/* [in] */ u8 enableBeaconing;
+};
+
+int wsm_beacon_transmit(struct cw1200_common *priv,
+			const struct wsm_beacon_transmit *arg);
+
+int wsm_start_find(struct cw1200_common *priv);
+
+int wsm_stop_find(struct cw1200_common *priv);
+
+typedef void (*wsm_find_complete_cb) (struct cw1200_common *priv, u32 status);
+
+struct wsm_suspend_resume {
+	/* See 3.52 */
+	/* Link ID */
+	/* [out] */ int link_id;
+	/* Stop sending further Tx requests down to device for this link */
+	/* [out] */ bool stop;
+	/* Transmit multicast Frames */
+	/* [out] */ bool multicast;
+	/* The AC on which Tx to be suspended /resumed. */
+	/* This is applicable only for U-APSD */
+	/* WSM_QUEUE_... */
+	/* [out] */ int queue;
+};
+
+typedef void (*wsm_suspend_resume_cb) (struct cw1200_common *priv,
+				       struct wsm_suspend_resume *arg);
+
+/* 3.54 Update-IE request. */
+struct wsm_update_ie {
+	/* WSM_UPDATE_IE_... */
+	/* [in] */ u16 what;
+	/* [in] */ u16 count;
+	/* [in] */ u8 *ies;
+	/* [in] */ size_t length;
+};
+
+int wsm_update_ie(struct cw1200_common *priv,
+		  const struct wsm_update_ie *arg);
+
+/* 3.56 */
+struct wsm_map_link {
+	/* MAC address of the remote device */
+	/* [in] */ u8 mac_addr[6];
+	/* [in] */ u8 link_id;
+};
+
+int wsm_map_link(struct cw1200_common *priv, const struct wsm_map_link *arg);
+
+struct wsm_cbc {
+	wsm_scan_complete_cb scan_complete;
+	wsm_tx_confirm_cb tx_confirm;
+	wsm_rx_cb rx;
+	wsm_event_cb event;
+	wsm_set_pm_complete_cb set_pm_complete;
+	wsm_channel_switch_cb channel_switch;
+	wsm_find_complete_cb find_complete;
+	wsm_suspend_resume_cb suspend_resume;
+};
+
+/* ******************************************************************** */
+/* MIB shortcats							*/
+
+static inline int wsm_set_output_power(struct cw1200_common *priv,
+				       int power_level)
+{
+	__le32 val = __cpu_to_le32(power_level);
+	return wsm_write_mib(priv, WSM_MIB_ID_DOT11_CURRENT_TX_POWER_LEVEL,
+			     &val, sizeof(val));
+}
+
+static inline int wsm_set_beacon_wakeup_period(struct cw1200_common *priv,
+					       unsigned dtim_interval,
+					       unsigned listen_interval)
+{
+	struct {
+		u8 numBeaconPeriods;
+		u8 reserved;
+		__le16 listenInterval;
+	} val = {
+	dtim_interval, 0, __cpu_to_le16(listen_interval)};
+	if (dtim_interval > 0xFF || listen_interval > 0xFFFF)
+		return -EINVAL;
+	else
+		return wsm_write_mib(priv, WSM_MIB_ID_BEACON_WAKEUP_PERIOD,
+				     &val, sizeof(val));
+}
+
+struct wsm_rcpi_rssi_threshold {
+	u8 rssiRcpiMode;	/* WSM_RCPI_RSSI_... */
+	u8 lowerThreshold;
+	u8 upperThreshold;
+	u8 rollingAverageCount;
+};
+
+static inline int wsm_set_rcpi_rssi_threshold(struct cw1200_common *priv,
+					struct wsm_rcpi_rssi_threshold *arg)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_RCPI_RSSI_THRESHOLD, arg,
+			     sizeof(*arg));
+}
+
+struct wsm_counters_table {
+	__le32 countPlcpErrors;
+	__le32 countFcsErrors;
+	__le32 countTxPackets;
+	__le32 countRxPackets;
+	__le32 countRxPacketErrors;
+	__le32 countRxDecryptionFailures;
+	__le32 countRxMicFailures;
+	__le32 countRxNoKeyFailures;
+	__le32 countTxMulticastFrames;
+	__le32 countTxFramesSuccess;
+	__le32 countTxFrameFailures;
+	__le32 countTxFramesRetried;
+	__le32 countTxFramesMultiRetried;
+	__le32 countRxFrameDuplicates;
+	__le32 countRtsSuccess;
+	__le32 countRtsFailures;
+	__le32 countAckFailures;
+	__le32 countRxMulticastFrames;
+	__le32 countRxFramesSuccess;
+	__le32 countRxCMACICVErrors;
+	__le32 countRxCMACReplays;
+	__le32 countRxMgmtCCMPReplays;
+};
+
+static inline int wsm_get_counters_table(struct cw1200_common *priv,
+					 struct wsm_counters_table *arg)
+{
+	return wsm_read_mib(priv, WSM_MIB_ID_COUNTERS_TABLE,
+			arg, sizeof(*arg));
+}
+
+static inline int wsm_get_station_id(struct cw1200_common *priv, u8 *mac)
+{
+	return wsm_read_mib(priv, WSM_MIB_ID_DOT11_STATION_ID, mac, ETH_ALEN);
+}
+
+struct wsm_rx_filter {
+	bool promiscuous;
+	bool bssid;
+	bool fcs;
+};
+
+static inline int wsm_set_rx_filter(struct cw1200_common *priv,
+				    const struct wsm_rx_filter *arg)
+{
+	__le32 val = 0;
+	if (arg->promiscuous)
+		val |= __cpu_to_le32(BIT(0));
+	if (arg->bssid)
+		val |= __cpu_to_le32(BIT(1));
+	if (arg->fcs)
+		val |= __cpu_to_le32(BIT(2));
+	return wsm_write_mib(priv, WSM_MIB_ID_RX_FILTER, &val, sizeof(val));
+}
+
+#define WSM_BEACON_FILTER_IE_HAS_CHANGED	BIT(0)
+#define WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT	BIT(1)
+#define WSM_BEACON_FILTER_IE_HAS_APPEARED	BIT(2)
+
+struct wsm_beacon_filter_table_entry {
+	u8	ieId;
+	u8	actionFlags;
+	u8	oui[3];
+	u8	matchData[3];
+} __packed;
+
+struct wsm_beacon_filter_table {
+	__le32 numOfIEs;
+	struct wsm_beacon_filter_table_entry entry[10];
+} __packed;
+
+static inline int wsm_set_beacon_filter_table(struct cw1200_common *priv,
+					struct wsm_beacon_filter_table *ft)
+{
+	size_t size = __le32_to_cpu(ft->numOfIEs) *
+		     sizeof(struct wsm_beacon_filter_table_entry) +
+		     sizeof(__le32);
+
+	return wsm_write_mib(priv, WSM_MIB_ID_BEACON_FILTER_TABLE, ft, size);
+}
+
+struct wsm_beacon_filter_control {
+	int enabled;
+	int bcn_count;
+};
+
+static inline int wsm_beacon_filter_control(struct cw1200_common *priv,
+					struct wsm_beacon_filter_control *arg)
+{
+	struct {
+		__le32 enabled;
+		__le32 bcn_count;
+	} val;
+	val.enabled = __cpu_to_le32(arg->enabled);
+	val.bcn_count = __cpu_to_le32(arg->bcn_count);
+	return wsm_write_mib(priv, WSM_MIB_ID_BEACON_FILTER_ENABLE, &val,
+			     sizeof(val));
+}
+
+enum wsm_power_mode {
+	wsm_power_mode_active = 0,
+	wsm_power_mode_doze = 1,
+	wsm_power_mode_quiescent = 2,
+};
+
+struct wsm_operational_mode {
+	enum wsm_power_mode power_mode;
+	int disableMoreFlagUsage;
+	int performAntDiversity;
+};
+
+static inline int wsm_set_operational_mode(struct cw1200_common *priv,
+					const struct wsm_operational_mode *arg)
+{
+	u8 val = arg->power_mode;
+	if (arg->disableMoreFlagUsage)
+		val |= BIT(4);
+	if (arg->performAntDiversity)
+		val |= BIT(5);
+	return wsm_write_mib(priv, WSM_MIB_ID_OPERATIONAL_POWER_MODE, &val,
+			     sizeof(val));
+}
+
+struct wsm_template_frame {
+	u8 frame_type;
+	u8 rate;
+	bool disable;
+	struct sk_buff *skb;
+};
+
+static inline int wsm_set_template_frame(struct cw1200_common *priv,
+					 struct wsm_template_frame *arg)
+{
+	int ret;
+	u8 *p = skb_push(arg->skb, 4);
+	p[0] = arg->frame_type;
+	p[1] = arg->rate;
+	if (arg->disable)
+		((u16 *) p)[1] = 0;
+	else
+		((u16 *) p)[1] = __cpu_to_le16(arg->skb->len - 4);
+	ret = wsm_write_mib(priv, WSM_MIB_ID_TEMPLATE_FRAME, p, arg->skb->len);
+	skb_pull(arg->skb, 4);
+	return ret;
+}
+
+
+struct wsm_protected_mgmt_policy {
+	bool protectedMgmtEnable;
+	bool unprotectedMgmtFramesAllowed;
+	bool encryptionForAuthFrame;
+};
+
+static inline int wsm_set_protected_mgmt_policy(struct cw1200_common *priv,
+		struct wsm_protected_mgmt_policy *arg)
+{
+	__le32 val = 0;
+	int ret;
+	if (arg->protectedMgmtEnable)
+		val |= __cpu_to_le32(BIT(0));
+	if (arg->unprotectedMgmtFramesAllowed)
+		val |= __cpu_to_le32(BIT(1));
+	if (arg->encryptionForAuthFrame)
+		val |= __cpu_to_le32(BIT(2));
+	ret = wsm_write_mib(priv, WSM_MIB_ID_PROTECTED_MGMT_POLICY,
+			&val, sizeof(val));
+	return ret;
+}
+
+static inline int wsm_set_block_ack_policy(struct cw1200_common *priv,
+					   u8 blockAckTxTidPolicy,
+					   u8 blockAckRxTidPolicy)
+{
+	struct {
+		u8 blockAckTxTidPolicy;
+		u8 reserved1;
+		u8 blockAckRxTidPolicy;
+		u8 reserved2;
+	} val = {
+		.blockAckTxTidPolicy = blockAckTxTidPolicy,
+		.blockAckRxTidPolicy = blockAckRxTidPolicy,
+	};
+	return wsm_write_mib(priv, WSM_MIB_ID_BLOCK_ACK_POLICY, &val,
+			     sizeof(val));
+}
+
+struct wsm_association_mode {
+	u8 flags;		/* WSM_ASSOCIATION_MODE_... */
+	u8 preambleType;	/* WSM_JOIN_PREAMBLE_... */
+	u8 greenfieldMode;	/* 1 for greenfield */
+	u8 mpduStartSpacing;
+	__le32 basicRateSet;
+};
+
+static inline int wsm_set_association_mode(struct cw1200_common *priv,
+					   struct wsm_association_mode *arg)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_SET_ASSOCIATION_MODE, arg,
+			     sizeof(*arg));
+}
+
+struct wsm_set_tx_rate_retry_policy_header {
+	u8 numTxRatePolicies;
+	u8 reserved[3];
+} __packed;
+
+struct wsm_set_tx_rate_retry_policy_policy {
+	u8 policyIndex;
+	u8 shortRetryCount;
+	u8 longRetryCount;
+	u8 policyFlags;
+	u8 rateRecoveryCount;
+	u8 reserved[3];
+	__le32 rateCountIndices[3];
+} __packed;
+
+struct wsm_set_tx_rate_retry_policy {
+	struct wsm_set_tx_rate_retry_policy_header hdr;
+	struct wsm_set_tx_rate_retry_policy_policy tbl[8];
+} __packed;
+
+static inline int wsm_set_tx_rate_retry_policy(struct cw1200_common *priv,
+				struct wsm_set_tx_rate_retry_policy *arg)
+{
+	size_t size = sizeof(struct wsm_set_tx_rate_retry_policy_header) +
+	    arg->hdr.numTxRatePolicies *
+	    sizeof(struct wsm_set_tx_rate_retry_policy_policy);
+	return wsm_write_mib(priv, WSM_MIB_ID_SET_TX_RATE_RETRY_POLICY, arg,
+			     size);
+}
+
+/* 4.32 SetEtherTypeDataFrameFilter */
+struct wsm_ether_type_filter_hdr {
+	u8 nrFilters;		/* Up to WSM_MAX_FILTER_ELEMENTS */
+	u8 reserved[3];
+} __packed;
+
+struct wsm_ether_type_filter {
+	u8 filterAction;	/* WSM_FILTER_ACTION_XXX */
+	u8 reserved;
+	__le16 etherType;	/* Type of ethernet frame */
+} __packed;
+
+static inline int wsm_set_ether_type_filter(struct cw1200_common *priv,
+				struct wsm_ether_type_filter_hdr *arg)
+{
+	size_t size = sizeof(struct wsm_ether_type_filter_hdr) +
+		arg->nrFilters * sizeof(struct wsm_ether_type_filter);
+	return wsm_write_mib(priv, WSM_MIB_ID_SET_ETHERTYPE_DATAFRAME_FILTER,
+		arg, size);
+}
+
+
+/* 4.33 SetUDPPortDataFrameFilter */
+struct wsm_udp_port_filter_hdr {
+	u8 nrFilters;		/* Up to WSM_MAX_FILTER_ELEMENTS */
+	u8 reserved[3];
+} __packed;
+
+struct wsm_udp_port_filter {
+	u8 filterAction;	/* WSM_FILTER_ACTION_XXX */
+	u8 portType;		/* WSM_FILTER_PORT_TYPE_XXX */
+	__le16 udpPort;		/* Port number */
+} __packed;
+
+static inline int wsm_set_udp_port_filter(struct cw1200_common *priv,
+				struct wsm_udp_port_filter_hdr *arg)
+{
+	size_t size = sizeof(struct wsm_udp_port_filter_hdr) +
+		arg->nrFilters * sizeof(struct wsm_udp_port_filter);
+	return wsm_write_mib(priv, WSM_MIB_ID_SET_UDPPORT_DATAFRAME_FILTER,
+		arg, size);
+}
+
+/* Undocumented MIBs: */
+/* 4.35 P2PDeviceInfo */
+#define D11_MAX_SSID_LEN		(32)
+
+struct wsm_p2p_device_type {
+	__le16 categoryId;
+	u8 oui[4];
+	__le16 subCategoryId;
+} __packed;
+
+struct wsm_p2p_device_info {
+	struct wsm_p2p_device_type primaryDevice;
+	u8 reserved1[3];
+	u8 devNameSize;
+	u8 localDevName[D11_MAX_SSID_LEN];
+	u8 reserved2[3];
+	u8 numSecDevSupported;
+	struct wsm_p2p_device_type secondaryDevices[0];
+} __packed;
+
+/* 4.36 SetWCDMABand - WO */
+struct wsm_cdma_band {
+	u8 WCDMA_Band;
+	u8 reserved[3];
+} __packed;
+
+/* 4.37 GroupTxSequenceCounter - RO */
+struct wsm_group_tx_seq {
+	__le32 bits_47_16;
+	__le16 bits_15_00;
+	__le16 reserved;
+} __packed;
+
+/* 4.39 SetHtProtection - WO */
+#define WSM_DUAL_CTS_PROT_ENB		(1 << 0)
+#define WSM_NON_GREENFIELD_STA		PRESENT(1 << 1)
+#define WSM_HT_PROT_MODE__NO_PROT	(0 << 2)
+#define WSM_HT_PROT_MODE__NON_MEMBER	(1 << 2)
+#define WSM_HT_PROT_MODE__20_MHZ	(2 << 2)
+#define WSM_HT_PROT_MODE__NON_HT_MIXED	(3 << 2)
+#define WSM_LSIG_TXOP_PROT_FULL		(1 << 4)
+#define WSM_LARGE_L_LENGTH_PROT		(1 << 5)
+
+struct wsm_ht_protection {
+	__le32 flags;
+} __packed;
+
+/* 4.40 GPIO Command - R/W */
+#define WSM_GPIO_COMMAND_SETUP	0
+#define WSM_GPIO_COMMAND_READ	1
+#define WSM_GPIO_COMMAND_WRITE	2
+#define WSM_GPIO_COMMAND_RESET	3
+#define WSM_GPIO_ALL_PINS	0xFF
+
+struct wsm_gpio_command {
+	u8 GPIO_Command;
+	u8 pin;
+	__le16 config;
+} __packed;
+
+/* 4.41 TSFCounter - RO */
+struct wsm_tsf_counter {
+	__le64 TSF_Counter;
+} __packed;
+
+/* 4.43 Keep alive period */
+struct wsm_keep_alive_period {
+	__le16 keepAlivePeriod;
+	u8 reserved[2];
+} __packed;
+
+static inline int wsm_keep_alive_period(struct cw1200_common *priv,
+					int period)
+{
+	struct wsm_keep_alive_period arg = {
+		.keepAlivePeriod = __cpu_to_le16(period),
+	};
+	return wsm_write_mib(priv, WSM_MIB_ID_KEEP_ALIVE_PERIOD,
+			&arg, sizeof(arg));
+};
+
+/* BSSID filtering */
+struct wsm_set_bssid_filtering {
+	u8 filter;
+	u8 reserved[3];
+} __packed;
+
+static inline int wsm_set_bssid_filtering(struct cw1200_common *priv,
+					  bool enabled)
+{
+	struct wsm_set_bssid_filtering arg = {
+		.filter = !enabled,
+	};
+	return wsm_write_mib(priv, WSM_MIB_ID_DISABLE_BSSID_FILTER,
+			&arg, sizeof(arg));
+}
+
+/* Multicat filtering - 4.5 */
+struct wsm_multicast_filter {
+	__le32 enable;
+	__le32 numOfAddresses;
+	u8 macAddress[WSM_MAX_GRP_ADDRTABLE_ENTRIES][ETH_ALEN];
+} __packed;
+
+static inline int wsm_set_multicast_filter(struct cw1200_common *priv,
+					   struct wsm_multicast_filter *fp)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_DOT11_GROUP_ADDRESSES_TABLE,
+			     fp, sizeof(*fp));
+}
+
+/* ARP IPv4 filtering - 4.10 */
+struct wsm_arp_ipv4_filter {
+	__le32 enable;
+	__be32 ipv4Address[WSM_MAX_ARP_IP_ADDRTABLE_ENTRIES];
+} __packed;
+
+static inline int wsm_set_arp_ipv4_filter(struct cw1200_common *priv,
+					  struct wsm_arp_ipv4_filter *fp)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_ARP_IP_ADDRESSES_TABLE,
+			    fp, sizeof(*fp));
+}
+
+/* P2P Power Save Mode Info - 4.31 */
+struct wsm_p2p_ps_modeinfo {
+	u8	oppPsCTWindow;
+	u8	count;
+	u8	reserved;
+	u8	dtimCount;
+	__le32	duration;
+	__le32	interval;
+	__le32	startTime;
+} __packed;
+
+static inline int wsm_set_p2p_ps_modeinfo(struct cw1200_common *priv,
+					  struct wsm_p2p_ps_modeinfo *mi)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_P2P_PS_MODE_INFO,
+			     mi, sizeof(*mi));
+}
+
+static inline int wsm_get_p2p_ps_modeinfo(struct cw1200_common *priv,
+					  struct wsm_p2p_ps_modeinfo *mi)
+{
+	return wsm_read_mib(priv, WSM_MIB_ID_P2P_PS_MODE_INFO,
+			    mi, sizeof(*mi));
+}
+
+/* UseMultiTxConfMessage */
+
+static inline int wsm_use_multi_tx_conf(struct cw1200_common *priv,
+					bool enabled)
+{
+	__le32 arg = enabled ? __cpu_to_le32(1) : 0;
+
+	return wsm_write_mib(priv, WSM_MIB_USE_MULTI_TX_CONF,
+			&arg, sizeof(arg));
+}
+
+
+/* 4.26 SetUpasdInformation */
+struct wsm_uapsd_info {
+	__le16 uapsdFlags;
+	__le16 minAutoTriggerInterval;
+	__le16 maxAutoTriggerInterval;
+	__le16 autoTriggerStep;
+};
+
+static inline int wsm_set_uapsd_info(struct cw1200_common *priv,
+				     struct wsm_uapsd_info *arg)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_SET_UAPSD_INFORMATION,
+				arg, sizeof(*arg));
+}
+
+/* 4.22 OverrideInternalTxRate */
+struct wsm_override_internal_txrate {
+	u8 internalTxRate;
+	u8 nonErpInternalTxRate;
+	u8 reserved[2];
+} __packed;
+
+static inline int wsm_set_override_internal_txrate(struct cw1200_common *priv,
+				     struct wsm_override_internal_txrate *arg)
+{
+	return wsm_write_mib(priv, WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE,
+				arg, sizeof(*arg));
+}
+
+/* ******************************************************************** */
+/* WSM TX port control							*/
+
+void wsm_lock_tx(struct cw1200_common *priv);
+void wsm_lock_tx_async(struct cw1200_common *priv);
+bool wsm_flush_tx(struct cw1200_common *priv);
+void wsm_unlock_tx(struct cw1200_common *priv);
+
+/* ******************************************************************** */
+/* WSM / BH API								*/
+
+int wsm_handle_exception(struct cw1200_common *priv, u8 *data, size_t len);
+int wsm_handle_rx(struct cw1200_common *priv, int id, struct wsm_hdr *wsm,
+		  struct sk_buff **skb_p);
+
+/* ******************************************************************** */
+/* wsm_buf API								*/
+
+struct wsm_buf {
+	u8 *begin;
+	u8 *data;
+	u8 *end;
+};
+
+void wsm_buf_init(struct wsm_buf *buf);
+void wsm_buf_deinit(struct wsm_buf *buf);
+
+/* ******************************************************************** */
+/* wsm_cmd API								*/
+
+struct wsm_cmd {
+	spinlock_t lock;
+	int done;
+	u8 *ptr;
+	size_t len;
+	void *arg;
+	int ret;
+	u16 cmd;
+};
+
+/* ******************************************************************** */
+/* WSM TX buffer access							*/
+
+int wsm_get_tx(struct cw1200_common *priv, u8 **data,
+	       size_t *tx_len, int *burst);
+void wsm_txed(struct cw1200_common *priv, u8 *data);
+
+/* ******************************************************************** */
+/* Queue mapping: WSM <---> linux					*/
+/* Linux: VO VI BE BK							*/
+/* WSM:   BE BK VI VO							*/
+
+static inline u8 wsm_queue_id_to_linux(u8 queueId)
+{
+	static const u8 queue_mapping[] = {
+		2, 3, 1, 0
+	};
+	return queue_mapping[queueId];
+}
+
+static inline u8 wsm_queue_id_to_wsm(u8 queueId)
+{
+	static const u8 queue_mapping[] = {
+		3, 2, 0, 1
+	};
+	return queue_mapping[queueId];
+}
+
+#endif /* CW1200_HWIO_H_INCLUDED */
diff --git a/drivers/staging/mmio/Kconfig b/drivers/staging/mmio/Kconfig
new file mode 100644
index 00000000000..d6a5a9ad918
--- /dev/null
+++ b/drivers/staging/mmio/Kconfig
@@ -0,0 +1,11 @@
+
+config U8500_MMIO
+	bool "ST-Ericsson MMIO (Camera) Driver"
+	depends on ARCH_U8500
+	help
+	  Enables the ST-Ericsson MMIO (Camera) Driver
+
+config U5500_MMIO
+	bool "ST-Ericsson U5500 MMIO (Camera) Driver"
+	depends on UX500_SOC_DB5500
+
diff --git a/drivers/staging/mmio/Makefile b/drivers/staging/mmio/Makefile
new file mode 100644
index 00000000000..bec2a6efe63
--- /dev/null
+++ b/drivers/staging/mmio/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_U8500_MMIO) := st_mmio.o
diff --git a/drivers/staging/mmio/mmio.h b/drivers/staging/mmio/mmio.h
new file mode 100644
index 00000000000..1b201021982
--- /dev/null
+++ b/drivers/staging/mmio/mmio.h
@@ -0,0 +1,217 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * Author: Joakim Axelsson <joakim.axelsson@stericsson.com> for ST-Ericsson
+ * Author: Rajat Verma <rajat.verma@stericsson.com> for ST-Ericsson
+ * Author: Vincent Abriou <vincent.abriou@stericsson.com> for ST-Ericsson.
+ * License Terms: GNU General Public License v2
+ */
+
+#ifndef MMIO_H
+#define MMIO_H
+
+#include <linux/ioctl.h>
+
+#define MMIO_NAME     "mmio_camera"      /* kept for backward compatibility */
+#define MMIO_RAW_NAME "mmio_camera_raw"
+#define MMIO_YUV_NAME "mmio_camera_yuv"
+
+#define SRA_SUPPORT 1
+
+#ifdef SRA_SUPPORT
+#define SREG_16_BIT (0x1)
+#define SREG_32_BIT (0x2)
+#endif
+/* Kernel side interface for MMIO */
+/* Which camera is currently active */
+enum camera_slot_t {
+	PRIMARY_CAMERA = 0,
+	SECONDARY_CAMERA,
+	CAMERA_SLOT_END
+};
+
+enum camera_type_t {
+	RAW_CAMERA = 0,
+	YUV_CAMERA,
+	CAMERA_TYPE_END
+};
+
+struct mmio_gpio {
+	/* Name of the gpio */
+	const char *name;
+	/* Gpio number */
+	int gpio;
+	/* pin configuration when feature is enabled */
+	unsigned long cfg_ena;
+	/* pin configuration when feature is disabled */
+	unsigned long cfg_disa;
+	/* Set if pin is active high */
+	/* kept for backward compatibility */
+	int active_high;
+	/* Time to wait when activating the pin, in usec */
+	/* kept for backward compatibility */
+	int udelay;
+};
+
+struct mmio_clk {
+	const char *name;          /* Name of the clock */
+	struct clk *clk_ptr;       /* Pointer on the allocated clock */
+};
+
+struct mmio_regulator {
+	const char *name;          /* Name of the clock */
+	struct regulator *reg_ptr; /* Pointer on the allocated regulator */
+};
+
+enum mmio_select_i2c_t {
+	MMIO_ACTIVATE_IPI2C2 = 0, /* kept for backward compatibility */
+	MMIO_ACTIVATE_I2C_HOST,   /* kept for backward compatibility */
+	MMIO_ACTIVATE_I2C,
+	MMIO_DEACTIVATE_I2C
+};
+
+enum mmio_select_xshutdown_t {
+	MMIO_ENABLE_XSHUTDOWN_FW = 0,
+	MMIO_ENABLE_XSHUTDOWN_HOST,
+	MMIO_DISABLE_XSHUTDOWN
+};
+struct mmio_platform_data {
+	struct device *dev;
+	enum camera_slot_t camera_slot; /* Which camera is currently used,
+					 * Primary/Secondary */
+	void *extra;			/* Board's private data structure
+					 * placeholder */
+	int reset_ipgpio[CAMERA_SLOT_END]; /* Contains logical IP GPIO for
+					    * reset pin */
+	int sia_base;
+	int cr_base;
+	int (*platform_init)(struct mmio_platform_data *pdata);
+	void (*platform_exit)(struct mmio_platform_data *pdata);
+	int (*power_enable)(struct mmio_platform_data *pdata);
+	void (*power_disable)(struct mmio_platform_data *pdata);
+	/* kept for backward compatibility */
+	int (*config_xshutdown_pins)(struct mmio_platform_data *pdata,
+		enum mmio_select_xshutdown_t select, int is_active_high);
+	int (*config_i2c_pins)(struct mmio_platform_data *pdata,
+		enum mmio_select_i2c_t select);
+	int (*clock_enable)(struct mmio_platform_data *pdata);
+	void (*clock_disable)(struct mmio_platform_data *pdata);
+	/* kept for backward compatibility */
+	void (*set_xshutdown)(struct mmio_platform_data *pdata);
+};
+
+#define USER_SIDE_INTERFACE 1
+/* User side is only allowed to access code in USER_SIDE_INTERFACE block */
+#ifdef USER_SIDE_INTERFACE
+enum mmio_bool_t {
+	MMIO_FALSE = 0,
+	MMIO_TRUE = !MMIO_FALSE,
+	MMIO_BOOL_MAX = 0x7FFFFFFF
+};
+
+struct xshutdown_info_t {
+	int ip_gpio;
+	int camera_function;
+};
+
+struct xp70_fw_t {
+	void __iomem *addr_sdram_ext;
+	void __iomem *addr_esram_ext;
+	void __iomem *addr_split;
+	void __iomem *addr_data;
+	unsigned int size_sdram_ext;
+	unsigned int size_esram_ext;
+	unsigned int size_split;
+	unsigned int size_data;
+};
+
+struct isp_write_t {
+	unsigned long t1_dest;
+	unsigned long *data;
+	unsigned long count;
+};
+
+struct trace_buf_t {
+	void *address;
+	unsigned int size;
+};
+
+#ifdef SRA_SUPPORT
+struct s_reg {
+	unsigned int addr;
+	unsigned int value;
+	unsigned int mask;
+};
+
+struct s_reg_list {
+	unsigned int access_mode;
+	unsigned int entries;
+	struct s_reg  *s_regs_p;
+};
+#endif
+struct mmio_input_output_t {
+	union {
+		enum mmio_bool_t	power_on;
+		struct xp70_fw_t	xp70_fw;
+		struct isp_write_t	isp_write;
+		unsigned int		addr_to_map;
+		struct xshutdown_info_t	xshutdown_info;
+		enum camera_slot_t	camera_slot;
+		struct trace_buf_t	trace_buf;
+#ifdef SRA_SUPPORT
+		struct s_reg_list s_reg_list;
+#endif
+	} mmio_arg;
+};
+
+#define MMIO_TRUE	(1)
+#define MMIO_FALSE	(0)
+#define MMIO_INVALID	(~0)
+
+/*Xshutdown from host takes two arguments*/
+/* kept for backward compatibility */
+#define MMIO_XSHUTDOWN_ENABLE			(0x1)
+/* kept for backward compatibility */
+#define MMIO_XSHUTDOWN_ACTIVE_HIGH		(0x2)
+#define MMIO_MAGIC_NUMBER 0x15
+
+#define MMIO_CAM_INITBOARD	_IOW(MMIO_MAGIC_NUMBER, 1,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_PWR_SENSOR	_IOW(MMIO_MAGIC_NUMBER, 2,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_SET_EXT_CLK	_IOW(MMIO_MAGIC_NUMBER, 3,\
+struct mmio_input_output_t*)
+/* kept for backward compatibility */
+#define MMIO_CAM_SET_PRI_HWIF	_IO(MMIO_MAGIC_NUMBER, 4)
+/* kept for backward compatibility */
+#define MMIO_CAM_SET_SEC_HWIF	_IO(MMIO_MAGIC_NUMBER, 5)
+#define MMIO_CAM_INITMMDSPTIMER	_IO(MMIO_MAGIC_NUMBER, 6)
+#define MMIO_CAM_LOAD_XP70_FW	_IOW(MMIO_MAGIC_NUMBER, 7,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_MAP_STATS_AREA	_IOWR(MMIO_MAGIC_NUMBER, 8,\
+struct mmio_input_output_t*)
+/* kept for backward compatibility */
+#define MMIO_ACTIVATE_I2C2	_IOW(MMIO_MAGIC_NUMBER, 9, int*)
+/* kept for backward compatibility */
+#define MMIO_ENABLE_XSHUTDOWN_FROM_HOST _IOW(MMIO_MAGIC_NUMBER, 10, int*)
+#define MMIO_CAM_ISP_WRITE	_IOW(MMIO_MAGIC_NUMBER, 11,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_GET_IP_GPIO	_IOWR(MMIO_MAGIC_NUMBER, 12,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_DESINITBOARD	_IO(MMIO_MAGIC_NUMBER, 13)
+#define MMIO_CAM_SET_TRACE_BUFFER _IOW(MMIO_MAGIC_NUMBER, 14,\
+struct mmio_input_output_t*)
+
+#ifdef SRA_SUPPORT
+#define MMIO_CAM_READ_REGS	_IOWR(MMIO_MAGIC_NUMBER, 15,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_MODIFY_REGS	_IOWR(MMIO_MAGIC_NUMBER, 16,\
+struct mmio_input_output_t*)
+#define MMIO_CAM_WRITE_REGS	_IOWR(MMIO_MAGIC_NUMBER, 17,\
+struct mmio_input_output_t*)
+#endif
+
+#endif /* USER_SIDE_INTERFACE */
+
+#endif
+/* MMIO_H */
diff --git a/drivers/staging/mmio/st_mmio.c b/drivers/staging/mmio/st_mmio.c
new file mode 100644
index 00000000000..27371bb3564
--- /dev/null
+++ b/drivers/staging/mmio/st_mmio.c
@@ -0,0 +1,1204 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pankaj Chauhan <pankaj.chauhan@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#include <linux/delay.h>
+#include <linux/init.h>		/* Initiliasation support */
+#include <linux/module.h>	/* Module support */
+#include <linux/kernel.h>	/* Kernel support */
+#include <linux/version.h>	/* Kernel version */
+#include <linux/fs.h>		/* File operations (fops) defines */
+#include <linux/errno.h>	/* Defines standard err codes */
+#include <linux/io.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/dbx500-prcmu.h>
+#include <linux/ratelimit.h>
+#include "mmio.h"
+
+#define ISP_REGION_IO				(0xE0000000)
+#define SIA_ISP_REG_ADDR			(0x521E4)
+#define SIA_BASE_ADDR				(0x54000)
+#define SIA_ISP_MEM				(0x56000)
+#define SIA_TIMER_ITC				(0x5BC00)
+#define SIA_ISP_MCU_SYS_SIZE			(0x100000)
+#define SIA_ISP_MEM_PAGE_REG			(0x54070)
+#define SIA_ISP_MCU_SYS_ADDR0_OFFSET	(SIA_BASE_ADDR + 0x40)
+#define SIA_ISP_MCU_SYS_SIZE0_OFFSET	(SIA_BASE_ADDR + 0x42)
+#define SIA_ISP_MCU_SYS_ADDR1_OFFSET	(SIA_ISP_MCU_SYS_ADDR0_OFFSET + 0x04)
+#define SIA_ISP_MCU_SYS_SIZE1_OFFSET	(SIA_ISP_MCU_SYS_SIZE0_OFFSET + 0x04)
+#define SIA_ISP_MCU_IO_ADDR0_HI		(SIA_BASE_ADDR + 0x60)
+
+/* HTimer enable in CR register */
+#define CR_REG0_HTIMEN				(1 << 26)
+#define PICTOR_IN_XP70_L2_MEM_BASE_ADDR		(0x40000)
+#define PICTOR_IN_XP70_TCDM_MEM_BASE_ADDR	(0x60000)
+#define L2_PSRAM_MEM_SIZE			(0x10000)
+
+#define FW_TO_HOST_ADDR_MASK		(0x00001FFF)
+#define FW_TO_HOST_ADDR_SHIFT		(0xD)
+#define FW_TO_HOST_CLR_MASK		(0x3F)
+#define PHY_TO_ISP_MCU_IO_ADDR0_HI(x)	(((x) >> 24) << 8)
+#define XP70_ADDR_MASK			(0x00FFFFFF)
+
+#define CLOCK_ENABLE_DELAY		(0x2)
+
+#define MAX_PRCMU_QOS_APP		(0x64)
+
+#define ISP_WRITE_DATA_SIZE		(0x4)
+
+#define clrbits32(_addr, _clear) \
+	writel(readl(_addr) & ~(u32)(_clear), _addr)
+#define setbits32(_addr, _set) \
+	writel(readl(_addr) | (u32)(_set), _addr)
+
+#define XP70_BLOCK_SIZE		124
+#define XP70_NB_BLOCK		50
+/*
+ * For 30 fps video, there is 33 msec delay between every two frames
+ * MMIO driver reads traces from trace buffer every XP70_TIMEOUT_MSEC.
+ * If traces are not read in time from trace buffer, camera firmware
+ * will start overwiting the traces as size of trace buffer is limited.
+ */
+#define XP70_TIMEOUT_MSEC	30
+#define XP70_DEFAULT_MSG_ID	(0xCDCDCDCD)
+#define XP70_MAX_BLOCK_ID	(0xFFFFFFFF)
+
+#define upper_16_bits(n) ((u16)((u32)(n) >> 16))
+
+struct trace_block {
+	u32 msg_id;
+	char data[XP70_BLOCK_SIZE];
+};
+
+struct mmio_trace {
+	u32 nb_block;
+	u32 block_size;
+	u32 block_id;
+	u32 overwrite_count;
+	struct trace_block block[XP70_NB_BLOCK];
+};
+
+struct trace_buffer_status {
+	u32 prev_overwrite_count;
+	u32 prev_block_id;
+};
+
+struct mmio_info {
+	struct mmio_platform_data *pdata; /* Config from board */
+	struct device *dev; /* My device */
+	/* Runtime variables */
+	struct miscdevice misc_dev;
+	void __iomem *siabase;
+	void __iomem *crbase;
+	/* States */
+	int xshutdown_enabled;
+	int xshutdown_is_active_high;
+	/* tracing */
+	struct trace_buffer_status trace_status;
+	struct mmio_trace *trace_buffer;
+	struct delayed_work trace_work;
+	int trace_allowed;
+	struct mutex lock;
+};
+
+/*
+ * The one and only private data holder. Default inited to NULL.
+ * Declare it here so no code above can use it directly.
+ */
+static struct mmio_info *info;
+
+/*
+ * static 1K buffer to do I/O write instead of kmalloc,
+ * no locking, caller can not have parallel use of
+ * MMIO_CAM_LOAD_XP70_FW and MMIO_CAM_ISP_WRITE ioctl's
+ */
+static u16 copybuff[512];
+
+/*
+ * This function converts a given logical memory region size
+ * to appropriate ISP_MCU_SYS_SIZEx register value.
+ */
+static int get_mcu_sys_size(u32 size, u32 *val)
+{
+	int ret = 0;
+
+	if (size > 0 && size <= SZ_4K)
+		*val = 4;
+	else if (size > SZ_4K && size <= SZ_8K)
+		*val = 5;
+	else if (size > SZ_8K && size <= SZ_16K)
+		*val = 6;
+	else if (size > SZ_16K && size <= SZ_32K)
+		*val = 7;
+	else if (size > SZ_32K && size <= SZ_64K)
+		*val = 0;
+	else if (size > SZ_64K && size <= SZ_1M)
+		*val = 1;
+	else if (size > SZ_1M  && size <= SZ_16M)
+		*val = 2;
+	else if (size > SZ_16M && size <= SZ_256M)
+		*val = 3;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+static int mmio_cam_pwr_sensor(struct mmio_info *info, int on)
+{
+	int err = 0;
+
+	if (on) {
+		err = info->pdata->power_enable(info->pdata);
+
+		if (err)
+			dev_err(info->dev,
+				"power_enable failed. err = %d\n", err);
+
+		/*
+		 * When switching from secondary YUV camera
+		 * to primary Raw Bayer Camera, a hang is observed without the
+		 * below delay. I2C access failure are observed while
+		 * communicating with primary camera sensor indicating camera
+		 * sensor was not powered up correctly.
+		 */
+		mdelay(CLOCK_ENABLE_DELAY);
+	} else {
+		info->pdata->power_disable(info->pdata);
+	}
+
+	return err;
+}
+
+static int mmio_cam_control_clocks(struct mmio_info *info,
+				   enum mmio_bool_t power_on)
+{
+	int err = 0;
+
+	if (power_on) {
+		err = info->pdata->clock_enable(info->pdata);
+
+		if (err)
+			dev_err(info->dev,
+				"clock_enable failed, err = %d\n",
+				err);
+	} else {
+		info->pdata->clock_disable(info->pdata);
+	}
+
+	return err;
+}
+
+static int mmio_cam_set_pri_hwif(struct mmio_info *info)
+{
+	if (info->xshutdown_enabled)
+		info->pdata->set_xshutdown(info->pdata);
+
+	return 0;
+}
+
+static int mmio_cam_set_sec_hwif(struct mmio_info *info)
+{
+	if (info->xshutdown_enabled)
+		info->pdata->set_xshutdown(info->pdata);
+
+	return 0;
+}
+
+static int mmio_cam_init_mmdsp_timer(struct mmio_info *info)
+{
+	/* Disabling Accelerators timers */
+	clrbits32(info->crbase, CR_REG0_HTIMEN);
+	/* Write MMDSPTimer */
+	writel(0, info->siabase + SIA_TIMER_ITC);
+	/* Enabling Accelerators timers */
+	setbits32(info->crbase, CR_REG0_HTIMEN);
+	return 0;
+}
+
+static u32 t1_to_arm(u32 t1_addr, void __iomem *smia_base_address,
+		     u16 *p_mem_page)
+{
+	u16 mem_page_update = 0;
+	mem_page_update = (t1_addr >> FW_TO_HOST_ADDR_SHIFT) &
+			  FW_TO_HOST_CLR_MASK;
+
+	if (mem_page_update != *p_mem_page) {
+		/* Update sia_mem_page register */
+		dev_dbg(info->dev, "mem_page_update=0x%x, mem_page=0x%x\n",
+			mem_page_update, *p_mem_page);
+		writew(mem_page_update, smia_base_address +
+		       SIA_ISP_MEM_PAGE_REG);
+		*p_mem_page = mem_page_update;
+	}
+
+	return SIA_ISP_MEM + (t1_addr & FW_TO_HOST_ADDR_MASK);
+}
+
+static int write_user_buffer(struct mmio_info *info, u32 ioaddr,
+					void __iomem *src_buf, u32 size)
+{
+	u32 i, count, offset = 0;
+	u32 itval = 0;
+	u16 mem_page = 0;
+	int err = 0;
+
+	if (!src_buf || !ioaddr) {
+		dev_err(info->dev, "invalid parameters: %p, 0x%x",
+				src_buf, ioaddr);
+
+		return -EINVAL;
+	}
+
+	for (offset = 0; offset < size; ) {
+
+		if ((size - offset) > sizeof(copybuff))
+			count = sizeof(copybuff);
+		else
+			count = (size - offset);
+
+		if (copy_from_user(copybuff, src_buf + offset, count)) {
+
+			dev_err(info->dev, "failed to copy user buffer"
+				" %p at offset=%d, count=%d\n",
+				src_buf, offset, count);
+
+			err = -EFAULT;
+			goto cp_failed;
+		}
+
+		for (i = 0; i < count; ) {
+			itval = t1_to_arm(ioaddr + offset,
+					info->siabase, &mem_page);
+			itval = ((u32) info->siabase) + itval;
+
+			writew(copybuff[i/2], itval);
+			offset += 2;
+			i = i + 2;
+		}
+	}
+
+cp_failed:
+	return err;
+}
+
+static int mmio_load_xp70_fw(struct mmio_info *info,
+			     struct xp70_fw_t *xp70_fw)
+{
+	u32 itval = 0;
+	int err = 0;
+
+	if (xp70_fw->size_split != 0) {
+		/* if buff size is not as expected */
+		if (xp70_fw->size_split != L2_PSRAM_MEM_SIZE) {
+			dev_err(info->dev, "xp70_fw_t.size_split must be "
+				"%d bytes!\n", L2_PSRAM_MEM_SIZE);
+			err = -EINVAL;
+			goto err_exit;
+		}
+
+		writel(0x0, info->siabase + SIA_ISP_REG_ADDR);
+
+		/* Put the low 64k IRP firmware in ISP MCU L2 PSRAM */
+		err = write_user_buffer(info, PICTOR_IN_XP70_L2_MEM_BASE_ADDR,
+					xp70_fw->addr_split,
+					L2_PSRAM_MEM_SIZE);
+		if (err)
+			goto err_exit;
+	}
+
+	if (xp70_fw->size_data != 0) {
+
+		writel(0x0, info->siabase + SIA_ISP_REG_ADDR);
+
+		err = write_user_buffer(info, PICTOR_IN_XP70_TCDM_MEM_BASE_ADDR,
+					xp70_fw->addr_data,
+					xp70_fw->size_data);
+
+		if (err)
+			goto err_exit;
+	}
+
+	if (xp70_fw->size_esram_ext != 0) {
+		/*
+		 * ISP_MCU_SYS_ADDRx XP70 register (@ of ESRAM where the
+		 * external code has been loaded
+		 */
+		writew(upper_16_bits(xp70_fw->addr_esram_ext),
+		       info->siabase + SIA_ISP_MCU_SYS_ADDR0_OFFSET);
+		/* ISP_MCU_SYS_SIZEx XP70 register (size of the code =64KB) */
+		writew(0x0, info->siabase + SIA_ISP_MCU_SYS_SIZE0_OFFSET);
+	}
+
+	if (xp70_fw->size_sdram_ext != 0) {
+		/*
+		 * ISP_MCU_SYS_ADDRx XP70 register (@ of SDRAM where the
+		 * external code has been loaded
+		 */
+		writew(upper_16_bits(xp70_fw->addr_sdram_ext),
+		       info->siabase + SIA_ISP_MCU_SYS_ADDR1_OFFSET);
+		/* ISP_MCU_SYS_SIZEx XP70 register */
+		err = get_mcu_sys_size(xp70_fw->size_sdram_ext, &itval);
+
+		if (err)
+			goto err_exit;
+
+		writew(itval, info->siabase + SIA_ISP_MCU_SYS_SIZE1_OFFSET);
+	}
+
+	return 0;
+err_exit:
+	dev_err(info->dev, "Loading XP70 fw failed\n");
+	return -EFAULT;
+}
+
+static int mmio_map_statistics_mem_area(struct mmio_info *info,
+					void __iomem *addr_to_map)
+{
+	u16 value;
+	BUG_ON(addr_to_map == NULL);
+	/* 16 Mbyte aligned page */
+	value = PHY_TO_ISP_MCU_IO_ADDR0_HI(*((u32 *)addr_to_map));
+	writew(value, info->siabase + SIA_ISP_MCU_IO_ADDR0_HI);
+	/* Return the address in the XP70 address space */
+	*((u32 *)addr_to_map) = (*((u32 *)addr_to_map) & XP70_ADDR_MASK) |
+				ISP_REGION_IO;
+	return 0;
+}
+
+static int mmio_activate_i2c2(struct mmio_info *info, unsigned long enable)
+{
+	int err = 0;
+
+	switch (enable) {
+	case MMIO_ACTIVATE_I2C_HOST:
+		/* Select I2C-2 */
+		err = info->pdata->config_i2c_pins(info->pdata,
+						   MMIO_ACTIVATE_I2C_HOST);
+
+		if (err) {
+			dev_err(info->dev, "Failed to Enable I2C-2, err %d\n",
+				err);
+			goto out;
+		}
+
+		break;
+	case MMIO_ACTIVATE_IPI2C2:
+		/* Select IPI2C */
+		err = info->pdata->config_i2c_pins(info->pdata,
+						   MMIO_ACTIVATE_IPI2C2);
+
+		if (err) {
+			dev_err(info->dev, "Failed to Enable IPI2C, err %d\n",
+				err);
+			goto out;
+		}
+
+		break;
+	case MMIO_DEACTIVATE_I2C: {
+		info->pdata->config_i2c_pins(info->pdata, MMIO_DEACTIVATE_I2C);
+	}
+	break;
+	default:
+		dev_warn(info->dev, "Invalid I2C2 config\n");
+		err = -EINVAL;
+		break;
+	}
+
+out:
+	return err;
+}
+
+static int mmio_enable_xshutdown_from_host(struct mmio_info *info,
+		unsigned long enable)
+{
+	int err = 0;
+	info->xshutdown_is_active_high = enable & MMIO_XSHUTDOWN_ACTIVE_HIGH;
+
+	if (enable & MMIO_XSHUTDOWN_ENABLE) {
+		err = info->pdata->config_xshutdown_pins(info->pdata,
+				MMIO_ENABLE_XSHUTDOWN_HOST, enable &
+				MMIO_XSHUTDOWN_ACTIVE_HIGH);
+	} else {
+		info->pdata->config_xshutdown_pins(info->pdata,
+						MMIO_ENABLE_XSHUTDOWN_FW, -1);
+		/*
+		 * XShutdown is controlled by firmware, initial output value is
+		 * provided by firmware
+		 */
+	}
+
+	info->xshutdown_enabled = enable & MMIO_XSHUTDOWN_ENABLE;
+	return 0;
+}
+
+static int mmio_cam_initboard(struct mmio_info *info)
+{
+	int err = 0;
+	err = prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP, MMIO_NAME,
+					MAX_PRCMU_QOS_APP);
+
+	if (err) {
+		dev_err(info->dev, "Error adding PRCMU QoS requirement %d\n",
+			err);
+		goto out;
+	}
+
+	/* Configure xshutdown to be disabled by default */
+	err = mmio_enable_xshutdown_from_host(info, 0);
+
+	if (err)
+		goto out;
+
+	/* Enable IPI2C */
+	err = mmio_activate_i2c2(info, MMIO_ACTIVATE_IPI2C2);
+out:
+	return err;
+}
+
+static int mmio_cam_desinitboard(struct mmio_info *info)
+{
+	prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP, MMIO_NAME);
+	return 0;
+}
+
+static int mmio_isp_write(struct mmio_info *info,
+			  struct isp_write_t *isp_write_p)
+{
+	int err = 0, i;
+	u32 __iomem *data = NULL;
+	void __iomem *addr = NULL;
+	u16 mem_page = 0;
+	u32 size, count, offset;
+
+	if (!isp_write_p->count) {
+		dev_warn(info->dev, "no data to write to isp\n");
+		return -EINVAL;
+	}
+
+	size = isp_write_p->count * ISP_WRITE_DATA_SIZE;
+	data = (u32 *) copybuff;
+
+	for (offset = 0; offset < size; ) {
+
+		/* 'offset' and 'size' and 'count' is in bytes */
+		if ((size - offset) > sizeof(copybuff))
+			count = sizeof(copybuff);
+		else
+			count = (size - offset);
+
+		if (copy_from_user(data, ((u8 *)isp_write_p->data) + offset,
+		    count)) {
+			dev_err(info->dev, "failed to copy user buffer"
+				" %p at offset=%d, count=%d\n",
+				isp_write_p->data, offset, count);
+
+			err = -EFAULT;
+			goto out;
+		}
+
+		/* index 'i' and 'offset' is in bytes */
+		for (i = 0; i < count; ) {
+			addr = (void *)(info->siabase + t1_to_arm(
+					isp_write_p->t1_dest
+					+ offset,
+					info->siabase, &mem_page));
+
+			*((u32 *)addr) = data[i/ISP_WRITE_DATA_SIZE];
+
+			offset += ISP_WRITE_DATA_SIZE;
+			i = i + ISP_WRITE_DATA_SIZE;
+		}
+	}
+
+out:
+	return err;
+}
+
+static int mmio_set_trace_buffer(struct mmio_info *info,
+				 struct trace_buf_t *buf)
+{
+	u32 i;
+	int ret = 0;
+
+	if (info->trace_allowed != 1) {
+		dev_warn(info->dev, "trace disabled in kernel\n");
+		ret = -EPERM;
+		goto out;
+	}
+
+	if (!buf->size || !buf->address
+			|| buf->size < sizeof(struct mmio_trace)) {
+		dev_err(info->dev, "invalid xp70 trace buffer\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	mutex_lock(&info->lock);
+	if (info->trace_buffer) {
+		dev_info(info->dev, "unmap old buffer");
+		iounmap(info->trace_buffer);
+		info->trace_buffer = NULL;
+	}
+
+	info->trace_buffer = ioremap((u32)buf->address, buf->size);
+
+	if (!info->trace_buffer) {
+		dev_err(info->dev, "failed to map trace buffer\n");
+		ret = -ENOMEM;
+		goto out_unlock;
+	}
+
+	dev_info(info->dev, "xp70 overwrite_cnt=%d (0x%x) blk_id=%d (0x%x)",
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->block_id, info->trace_buffer->block_id);
+#ifndef CAM_SHARED_MEM_DEBUG
+
+	/* Reset the allocated buffer contents */
+	for (i = 0; i < XP70_NB_BLOCK; i++)
+		info->trace_buffer->block[i].msg_id = XP70_DEFAULT_MSG_ID;
+
+#endif /* CAM_SHARED_MEMORY_DEBUG */
+	dev_info(info->dev, "xp70 overwrite_cnt=%d (0x%x) blk_id=%d (0x%x)\n",
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->block_id, info->trace_buffer->block_id);
+	info->trace_status.prev_overwrite_count = 0;
+	info->trace_status.prev_block_id = 0;
+
+	/* schedule work */
+	if (!schedule_delayed_work(&info->trace_work,
+				   msecs_to_jiffies(XP70_TIMEOUT_MSEC)))
+		dev_err(info->dev, "failed to schedule work\n");
+
+out_unlock:
+	mutex_unlock(&info->lock);
+out:
+	return ret;
+}
+
+static long mmio_ioctl(struct file *filp, u32 cmd,
+		      unsigned long arg)
+{
+	struct mmio_input_output_t data;
+	int no_of_bytes;
+	int enable;
+	int ret = 0;
+	struct mmio_info *info = (struct mmio_info *)filp->private_data;
+	BUG_ON(info == NULL);
+
+	switch (cmd) {
+	case MMIO_CAM_INITBOARD:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user(&data, (struct mmio_input_output_t *)arg,
+				   no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		info->pdata->camera_slot = data.mmio_arg.camera_slot;
+		ret = mmio_cam_initboard(info);
+		break;
+	case MMIO_CAM_DESINITBOARD:
+		ret = mmio_cam_desinitboard(info);
+		break;
+	case MMIO_CAM_PWR_SENSOR:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_cam_pwr_sensor(info, data.mmio_arg.power_on);
+		break;
+	case MMIO_CAM_SET_EXT_CLK:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_cam_control_clocks(info, data.mmio_arg.power_on);
+		break;
+	case MMIO_CAM_LOAD_XP70_FW:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_load_xp70_fw(info, &data.mmio_arg.xp70_fw);
+		break;
+	case MMIO_CAM_MAP_STATS_AREA:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_map_statistics_mem_area(info,
+						   &data.mmio_arg.addr_to_map);
+
+		if (0 != ret) {
+			dev_err(info->dev,
+				"Unable to map Statistics Mem area\n");
+			break;
+		}
+
+		if (copy_to_user((struct mmio_input_output_t *)arg,
+				 &data, sizeof(no_of_bytes))) {
+			dev_err(info->dev,
+				"Copy to userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		break;
+	case MMIO_CAM_SET_PRI_HWIF:
+		ret = mmio_cam_set_pri_hwif(info);
+		break;
+	case MMIO_CAM_SET_SEC_HWIF:
+		ret = mmio_cam_set_sec_hwif(info);
+		break;
+	case MMIO_CAM_INITMMDSPTIMER:
+		ret = mmio_cam_init_mmdsp_timer(info);
+		break;
+	case MMIO_CAM_ISP_WRITE:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_isp_write(info, &data.mmio_arg.isp_write);
+		break;
+	case MMIO_ACTIVATE_I2C2:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&enable, (int *)arg, sizeof(enable))) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_activate_i2c2(info, enable);
+		break;
+	case MMIO_ENABLE_XSHUTDOWN_FROM_HOST:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&enable, (int *)arg, sizeof(enable))) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_enable_xshutdown_from_host(info, enable);
+		break;
+	case MMIO_CAM_GET_IP_GPIO:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *)arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		data.mmio_arg.xshutdown_info.ip_gpio =
+			info->pdata->reset_ipgpio
+			[data.mmio_arg.xshutdown_info.camera_function];
+
+		if (copy_to_user((struct mmio_input_output_t *)arg,
+				 &data, sizeof(no_of_bytes))) {
+			dev_err(info->dev,
+				"Copy to userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		break;
+	case MMIO_CAM_SET_TRACE_BUFFER:
+		no_of_bytes = sizeof(struct mmio_input_output_t);
+		memset(&data, 0, sizeof(struct mmio_input_output_t));
+
+		if (copy_from_user
+				(&data, (struct mmio_input_output_t *) arg,
+				 no_of_bytes)) {
+			dev_err(info->dev,
+				"Copy from userspace failed\n");
+			ret = -EFAULT;
+			break;
+		}
+
+		ret = mmio_set_trace_buffer(info, &data.mmio_arg.trace_buf);
+		break;
+	default:
+		dev_err(info->dev, "Not an ioctl for this module\n");
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int mmio_release(struct inode *node, struct file *filp)
+{
+	struct mmio_info *info = filp->private_data;
+	BUG_ON(info == NULL);
+	mmio_activate_i2c2(info, MMIO_DEACTIVATE_I2C);
+	info->pdata->config_xshutdown_pins(info->pdata, MMIO_DISABLE_XSHUTDOWN,
+					   -1);
+
+	mutex_lock(&info->lock);
+	if (info->trace_buffer) {
+		flush_delayed_work_sync(&info->trace_work);
+		iounmap(info->trace_buffer);
+		info->trace_buffer = NULL;
+	}
+	mutex_unlock(&info->lock);
+	return 0;
+}
+
+static int mmio_open(struct inode *node, struct file *filp)
+{
+	filp->private_data = info;	/* Hook our mmio info */
+	return 0;
+}
+
+static const struct file_operations mmio_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = mmio_ioctl,
+	.open = mmio_open,
+	.release = mmio_release,
+};
+
+
+static ssize_t xp70_data_show(struct device *device,
+			      struct device_attribute *attr, char *buf)
+{
+	int i;
+	int len;
+	int size = 0;
+	int count = 0;
+	int first_index;
+	mutex_lock(&info->lock);
+	first_index = info->trace_status.prev_block_id + 1;
+
+	if (!info->trace_buffer || info->trace_buffer->block_id ==
+			XP70_MAX_BLOCK_ID)
+		goto out_unlock;
+
+	if (info->trace_allowed != 1) {
+		dev_warn(info->dev, "xp70 trace disabled in kernel\n");
+		size = sprintf(buf, "xp70 trace disabled in kernel, "
+			       "use sysfs to enable\n");
+		goto out_unlock;
+	}
+
+	count = info->trace_buffer->block_id - info->trace_status.prev_block_id;
+
+	if ((info->trace_buffer->overwrite_count -
+			info->trace_status.prev_overwrite_count) * XP70_NB_BLOCK
+			+ (info->trace_buffer->block_id -
+			   info->trace_status.prev_block_id)
+			>= XP70_NB_BLOCK) {
+		/* overflow case */
+		info->trace_status.prev_block_id =
+			info->trace_buffer->block_id - XP70_NB_BLOCK;
+		first_index = info->trace_buffer->block_id + 1;
+		count = XP70_NB_BLOCK;
+		len = sprintf(buf, "XP70 trace overflow\n");
+		size += len;
+		buf += len;
+	}
+
+	for (i = first_index; count; count--) {
+		int msg_len;
+
+		if (i < 0 || i >= XP70_NB_BLOCK || count > XP70_NB_BLOCK) {
+			dev_err(info->dev, "trace index out-of-bounds\n");
+			goto out_unlock;
+		}
+
+		msg_len = strnlen(info->trace_buffer->block[i].data,
+				  XP70_BLOCK_SIZE);
+
+		if (msg_len > 0) {
+			/* zero terminate full length message */
+			if (msg_len == XP70_BLOCK_SIZE)
+				info->trace_buffer->block[i].data[
+					XP70_BLOCK_SIZE - 1] = '\0';
+
+			len = snprintf(buf, PAGE_SIZE - size, "%d %s\n",
+				       info->trace_buffer->block[i].msg_id,
+				       info->trace_buffer->block[i].data);
+
+			if (len > PAGE_SIZE - size) {
+				dev_err(info->dev, "sysfs buffer overflow\n");
+				size = PAGE_SIZE;
+				goto out_unlock;
+			}
+
+			size += len;
+			buf += len;
+		}
+
+		i = (i + 1) % XP70_NB_BLOCK;
+	}
+
+out_unlock:
+	mutex_unlock(&info->lock);
+	return size;
+}
+
+static ssize_t xp70_trace_allowed_show(struct device *device,
+				       struct device_attribute *attr,
+				       char *buf)
+{
+	int len;
+	len = sprintf(buf, "%d\n", info->trace_allowed);
+	return len;
+}
+
+static ssize_t xp70_trace_allowed_store(struct device *device,
+					struct device_attribute *attr,
+					const char *buf, size_t count)
+{
+	if (count <= 0) {
+		dev_err(info->dev, "empty buffer to store\n");
+		return 0;
+	}
+
+	if (buf[0] == '1')
+		info->trace_allowed = 1;
+	else if (buf[0] == '0')
+		info->trace_allowed = 0;
+	else
+		dev_err(info->dev, "illegal trace_allowed val %c\n",
+			buf[0]);
+
+	return count;
+}
+
+static struct device_attribute xp70_device_attrs[] = {
+	__ATTR_RO(xp70_data),
+	__ATTR(trace_allowed, S_IRUGO | S_IWUSR, xp70_trace_allowed_show,
+	xp70_trace_allowed_store),
+	__ATTR_NULL
+};
+
+static void xp70_buffer_wqtask(struct work_struct *data)
+{
+	int i;
+	int first_index = info->trace_status.prev_block_id + 1;
+	int count;
+	mutex_lock(&info->lock);
+
+	if (!info->trace_buffer)
+		goto out_err;
+
+	dev_dbg(info->dev, "xp70 overwrite_cnt=%d (0x%x) blk_id=%d (0x%x)",
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->overwrite_count,
+		 info->trace_buffer->block_id, info->trace_buffer->block_id);
+
+	/* check if trace already started */
+	if (info->trace_buffer->block_id == XP70_MAX_BLOCK_ID ||
+		info->trace_buffer->block_id == XP70_DEFAULT_MSG_ID ||
+		info->trace_buffer->overwrite_count == XP70_DEFAULT_MSG_ID)
+		goto out;
+
+	if ((info->trace_buffer->overwrite_count -
+			info->trace_status.prev_overwrite_count) * XP70_NB_BLOCK
+			+ (info->trace_buffer->block_id -
+			   info->trace_status.prev_block_id)
+			>= XP70_NB_BLOCK) {
+		/* overflow case */
+		info->trace_status.prev_block_id =
+			info->trace_buffer->block_id - XP70_NB_BLOCK;
+		first_index = info->trace_buffer->block_id + 1;
+		count = XP70_NB_BLOCK;
+
+		pr_info_ratelimited("XP70 trace overflow\n");
+	} else if (info->trace_buffer->block_id
+			>= info->trace_status.prev_block_id) {
+		count = info->trace_buffer->block_id -
+			info->trace_status.prev_block_id;
+	} else {
+		u32 block_id, prev_block_id, diff;
+		block_id = (u32)(info->trace_buffer->block_id);
+		prev_block_id = (u32)(info->trace_status.prev_block_id);
+		diff = (block_id + XP70_NB_BLOCK) - prev_block_id;
+		count = (u32)diff;
+	}
+
+	for (i = first_index; count; count--) {
+		if (i < 0 || i >= XP70_NB_BLOCK || count > XP70_NB_BLOCK) {
+				pr_info_ratelimited("trace index out-of-bounds"
+					 "i=%d count=%d XP70_NB_BLOCK=%d\n",
+					 i, count, XP70_NB_BLOCK);
+
+			break;
+		}
+
+		if (info->trace_buffer->block[i].msg_id !=
+				XP70_DEFAULT_MSG_ID) {
+			int msg_len = strnlen(
+					      info->trace_buffer->block[i].data,
+					      XP70_BLOCK_SIZE);
+
+			/* zero terminate full length message */
+			if (msg_len > 0) {
+				if (msg_len == XP70_BLOCK_SIZE)
+					info->trace_buffer->block[i].data[
+						XP70_BLOCK_SIZE - 1] = '\0';
+
+				dev_info(info->dev, "%d %s\n",
+					 info->trace_buffer->block[i].msg_id,
+					 info->trace_buffer->block[i].data);
+			}
+		}
+
+		i = (i + 1) % XP70_NB_BLOCK;
+	}
+
+	info->trace_status.prev_overwrite_count =
+		info->trace_buffer->overwrite_count;
+	info->trace_status.prev_block_id = info->trace_buffer->block_id;
+out:
+	/* Schedule work */
+	if (!schedule_delayed_work(&info->trace_work,
+				   msecs_to_jiffies(XP70_TIMEOUT_MSEC)))
+		dev_info(info->dev, "failed to schedule work\n");
+
+out_err:
+	mutex_unlock(&info->lock);
+	return;
+}
+
+/**
+* mmio_probe() - Initialize MMIO Camera resources.
+* @pdev: Platform device.
+*
+* Initialize the module and register misc device.
+*
+* Returns:
+*	0 if there is no err.
+*	-ENOMEM if allocation fails.
+*	-EEXIST if device has already been started.
+*	Error codes from misc_register.
+*/
+static int __devinit mmio_probe(struct platform_device *pdev)
+{
+	int err;
+	int i;
+	int ret;
+	printk(KERN_INFO "%s\n", __func__);
+	/* Initialize private data. */
+	info = kzalloc(sizeof(struct mmio_info), GFP_KERNEL);
+
+	if (!info) {
+		dev_err(&pdev->dev, "Could not alloc info struct\n");
+		err = -ENOMEM;
+		goto err_alloc;
+	}
+
+	/* Fill in private data */
+	info->pdata = pdev->dev.platform_data;
+	info->dev = &pdev->dev;
+	info->pdata->dev = &pdev->dev;
+	info->misc_dev.minor = MISC_DYNAMIC_MINOR;
+	info->misc_dev.name = MMIO_NAME;
+	info->misc_dev.fops = &mmio_fops;
+	info->misc_dev.parent = pdev->dev.parent;
+	mutex_init(&info->lock);
+	info->xshutdown_enabled = 0;
+	info->xshutdown_is_active_high = 0;
+	info->trace_allowed = 0;
+	/* Register Misc character device */
+	err = misc_register(&(info->misc_dev));
+
+	if (err) {
+		dev_err(&pdev->dev, "Error %d registering misc dev!", err);
+		goto err_miscreg;
+	}
+
+	/* Memory mapping */
+	info->siabase = ioremap(info->pdata->sia_base, SIA_ISP_MCU_SYS_SIZE);
+
+	if (!info->siabase) {
+		dev_err(info->dev, "Could not ioremap SIA_BASE\n");
+		err = -ENOMEM;
+		goto err_ioremap_sia_base;
+	}
+
+	info->crbase = ioremap(info->pdata->cr_base, PAGE_SIZE);
+
+	if (!info->crbase) {
+		dev_err(info->dev, "Could not ioremap CR_BASE\n");
+		err = -ENOMEM;
+		goto err_ioremap_cr_base;
+	}
+
+	/* Initialize platform specific data */
+	err = info->pdata->platform_init(info->pdata);
+
+	if (err)
+		goto err_platform_init;
+
+	/* create sysfs entries */
+	for (i = 0; attr_name(xp70_device_attrs[i]); i++) {
+		ret = device_create_file(info->misc_dev.this_device,
+					 &xp70_device_attrs[i]);
+
+		if (ret) {
+			dev_err(info->dev, "Error creating SYSFS entry"
+				" %s (%d)\n", xp70_device_attrs[i].attr.name,
+				ret);
+		}
+	}
+
+	INIT_DELAYED_WORK(&info->trace_work, xp70_buffer_wqtask);
+	dev_info(&pdev->dev, "MMIO driver initialized with minor=%d\n",
+		 info->misc_dev.minor);
+	return 0;
+err_platform_init:
+	iounmap(info->crbase);
+err_ioremap_cr_base:
+	iounmap(info->siabase);
+err_ioremap_sia_base:
+	misc_deregister(&info->misc_dev);
+err_miscreg:
+	kfree(info);
+	info = NULL;
+err_alloc:
+	return err;
+}
+
+/**
+* mmio_remove() - Release MMIO Camera resources.
+* @pdev:	Platform device.
+*
+* Remove misc device and free resources.
+*
+* Returns:
+*	0 if success.
+*	Error codes from misc_deregister.
+*/
+static int __devexit mmio_remove(struct platform_device *pdev)
+{
+	int err;
+	int i;
+
+	if (!info)
+		return 0;
+
+	flush_scheduled_work();
+
+	/* sysfs parameters */
+	for (i = 0; attr_name(xp70_device_attrs[i]); i++)
+		device_remove_file(info->misc_dev.this_device,
+				   &xp70_device_attrs[i]);
+
+	err = misc_deregister(&info->misc_dev);
+
+	if (err)
+		dev_err(&pdev->dev, "Error %d deregistering misc dev", err);
+
+	info->pdata->platform_exit(info->pdata);
+	iounmap(info->siabase);
+	iounmap(info->crbase);
+	mutex_destroy(&info->lock);
+	kfree(info);
+	info = NULL;
+	return 0;
+}
+static struct platform_driver mmio_driver = {
+	.driver = {
+		.name = MMIO_NAME,
+		.owner = THIS_MODULE,
+	},
+	.probe = mmio_probe,
+	.remove = __devexit_p(mmio_remove)
+};
+
+/**
+* mmio_init() - Initialize module.
+*
+* Registers platform driver.
+*/
+static int __init mmio_init(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	return platform_driver_register(&mmio_driver);
+}
+
+/**
+* mmio_exit() - Remove module.
+*
+* Unregisters platform driver.
+*/
+static void __exit mmio_exit(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	platform_driver_unregister(&mmio_driver);
+}
+
+module_init(mmio_init);
+module_exit(mmio_exit);
+
+MODULE_AUTHOR("Joakim Axelsson ST-Ericsson");
+MODULE_AUTHOR("Pankaj Chauhan ST-Ericsson");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MMIO Camera driver");
diff --git a/drivers/staging/nmf-cm/Kconfig b/drivers/staging/nmf-cm/Kconfig
new file mode 100644
index 00000000000..9545fd5acd1
--- /dev/null
+++ b/drivers/staging/nmf-cm/Kconfig
@@ -0,0 +1,12 @@
+
+config U8500_CM
+	tristate "U8500 Component Manager driver"
+	depends on UX500_SOC_DB8500
+	help
+	  This is the Component Manager driver. It is part of the
+	  Nomadik Multiprocessing Framework.
+
+	  Note: This option allows the kernel developers to build
+	  the driver in kernel to ease there life. By default, this driver
+	  must be built outside this kernel source tree.
+
diff --git a/drivers/staging/nmf-cm/Make.config b/drivers/staging/nmf-cm/Make.config
new file mode 100644
index 00000000000..ccbc150158b
--- /dev/null
+++ b/drivers/staging/nmf-cm/Make.config
@@ -0,0 +1,8 @@
+# Copyright (C) ST-Ericsson SA 2011. All rights reserved.
+# This code is ST-Ericsson proprietary and confidential.
+# Any use of the code for whatever purpose is subject to
+# specific written permission of ST-Ericsson SA.
+
+#CM driver file to copy but not to compile
+CMENGINESRC_COPY_NO_BUILD = cm/engine/elf/src/elfxx.c
+
diff --git a/drivers/staging/nmf-cm/Makefile b/drivers/staging/nmf-cm/Makefile
new file mode 100644
index 00000000000..b1a5b9afe1f
--- /dev/null
+++ b/drivers/staging/nmf-cm/Makefile
@@ -0,0 +1,99 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+# License terms: GNU General Public License (GPL), version 2.
+#
+
+#
+# Rules to build kernel modules
+#
+ifneq ($(findstring KERNELRELEASE,$(.VARIABLES)),)
+
+  # $(src): current relative dir; $(kbuild-dir): cur absolute dir
+  ifdef kbuild-dir
+    SRCDIR = $(realpath $(kbuild-dir))
+  else
+    SRCDIR = $(realpath $(src))
+  endif
+  include $(SRCDIR)/Make.config
+  ifndef FIXED_CPPFLAGS
+    # In Android env, we can not depend on files that are out of kernel tree.
+    # and thus we can't include $(SRCDIR)/../../../../mmenv/SharedARMFlags.mk
+    # where FIXED_CPPFLAGS is defined.
+    # So, define FIXED_CPPFLAGS here
+    FIXED_CPPFLAGS=-D__STN_8500=30 -DLINUX -D__ARM_LINUX
+  endif
+  EXTRA_CFLAGS := -I$(SRCDIR) $(FIXED_CPPFLAGS)
+  EXTRA_CFLAGS += -Wall -Werror
+  #EXTRA_CFLAGS += -DCM_DEBUG_ALLOC
+
+  #
+  # CM object files to compile with
+  #
+  GENERIC_CM_FILES:=$(shell cd $(SRCDIR); find cm -name "*.c")
+  GENERIC_CM_FILES := $(filter-out $(CMENGINESRC_COPY_NO_BUILD), $(GENERIC_CM_FILES))
+
+  CM_OBJS := $(GENERIC_CM_FILES:.c=.o)
+  CM_OBJS += cmld.o cm_syscall.o osal-kernel.o cm_service.o cm_debug.o configuration.o
+  CM_OBJS += cm_dma.o
+
+  obj-$(CONFIG_U8500_CM) := cm.o
+
+  #Note: build system prepends the $(PWD) directory to these objects paths
+  cm-objs := $(CM_OBJS)
+
+else
+
+  # CM module is built in kernel in android env
+  # or as module otherwise (OSI env, ...)
+  export CONFIG_U8500_CM ?= m
+
+  ifeq ($(findstring install,$(MAKECMDGOALS)),)
+    # If not only performing install then include needed files for build
+    include $(MM_MAKEFILES_DIR)/SharedARMFlags.mk
+    export FIXED_CPPFLAGS
+    -include $(MM_MAKEFILES_DIR)/KernelConfig.mk
+
+    ifeq ($(findstring clean,$(MAKECMDGOALS)),)
+      ifndef KERNEL_BUILD_DIR
+        $(error KERNEL_BUILD_DIR not defined)
+      endif
+    endif
+  endif
+
+  include $(MM_MAKEFILES_DIR)/SharedConfig.mk
+
+  module:
+	$(MAKE) ARCH=arm CROSS_COMPILE=$(CROSS_COMPILE) -C $(KERNEL_BUILD_DIR) \
+          M=$(PWD) INSTALL_HEADER_DIR=$(INSTALL_HEADER_DIR) \
+          modules
+
+  all: module
+	$(MAKE) ARCH=arm CROSS_COMPILE=$(CROSS_COMPILE) -C $(KERNEL_BUILD_DIR) \
+          M=$(PWD) INSTALL_HEADER_DIR=$(INSTALL_HEADER_DIR) \
+          INSTALL_MOD_PATH=$(PWD)/lib/$(PLATFORM) \
+          modules_install
+	rm -f $(PWD)/lib/$(PLATFORM)/lib/modules/*/modules.*
+
+  #
+  # Rules to clean and install
+  #
+  clean:
+	@rm -rf $(PLATFORM) $(CM_OBJS) .built-in.o.cmd .cm*o.cmd Module.symvers \
+                .tmp_versions modules.order cm.ko cm.o cm.mod.* lib \
+		$(foreach f,$(CM_OBJS), $(dir $f).$(notdir $f).cmd)
+
+  realclean: clean
+	$(foreach platform, \
+          $(shell grep property ../../component/component.xml | cut -d\" -f 4), \
+          rm -rf $(platform);)
+	@rm -rf *~
+
+  install:
+	$(GEN_LN) -d lib/$(PLATFORM)/lib $(INSTALL_LIB_DIR)/lib
+
+  uninstall:
+	$(GEN_LN) -r -d lib/$(PLATFORM)/lib $(INSTALL_LIB_DIR)/lib
+
+endif #ifdef KERNELRELEASE
+
diff --git a/drivers/staging/nmf-cm/cm/engine/api/channel_engine.h b/drivers/staging/nmf-cm/cm/engine/api/channel_engine.h
new file mode 100644
index 00000000000..19353ee7328
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/channel_engine.h
@@ -0,0 +1,101 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Communication Component Manager internal API type.
+ */
+
+#ifndef CHANNEL_ENGINE_H
+#define CHANNEL_ENGINE_H
+
+#include <nmf/inc/channel_type.h>
+#include <nmf/inc/service_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+
+/*!
+ * \brief Internal channel identification.
+ *
+ * Same as t_nmf_channel meaning but this the channel used internaly by
+ * OS Integration part
+ *
+ * \ingroup CM_OS_API
+ */
+typedef t_uint32 t_os_channel;
+
+/*!
+ * \brief Invalid value for os_channel
+ *
+ * Invalid value for os channel.
+ *
+ * \ingroup CM_OS_API
+ */
+#define NMF_OS_CHANNEL_INVALID_HANDLE       0xffffffff
+
+/*!
+ * \brief Structure used for storing required parameters for Interface Callback
+ * messages.
+ *
+ * This struture is used internally by CM_GetMessage() and CM_ExecuteMessage() as
+ * the message content in the given buffer.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef struct {
+    t_nmf_mpc2host_handle       THIS;               //!< Context of interface implementation
+    t_uint32                    methodIndex;        //!< Method index in interface
+    char                        params[1];          //!< Is of variable length concretely
+} t_interface_data;
+
+/*!
+ * \brief Structure used for storing required parameters for Service Callback
+ * messages.
+ *
+ * This struture is used internally by CM_GetMessage() and CM_ExecuteMessage() as
+ * the message content in the given buffer.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef struct {
+    t_nmf_service_type type;        //!< Type of the service message
+    t_nmf_service_data data;
+} t_service_data;
+
+typedef enum {
+	MSG_INTERFACE,
+	MSG_SERVICE
+} t_message_type;
+
+/*!
+ * \brief Structure used for storing required parameters for the internal NMF
+ * messages.
+ *
+ * This struture is used internally by CM_GetMessage() and CM_ExecuteMessage() as
+ * the message content in the given buffer.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef struct {
+    t_message_type type;           //!< Type of the nmf message
+    union {
+	t_interface_data    itf;
+	t_service_data      srv;
+    } data;
+} t_os_message;
+
+/*!
+ * \brief Structure used for storing required parameters for the internal NMF
+ * messages.
+ *
+ * This struture is used internally by CM_GetMessage() and CM_ExecuteMessage() as
+ * the message content in the given buffer.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef struct {
+    t_nmf_channel               channel;        //!< Channel (required to handle service message)
+    t_os_message                osMsg;
+} t_nmf_message;
+
+#endif /* CHANNEL_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/cm_engine.h b/drivers/staging/nmf-cm/cm/engine/api/cm_engine.h
new file mode 100644
index 00000000000..0f4c1e4219e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/cm_engine.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief CM Engine API.
+ *
+ * This file contains the Component Manager Engine API.
+ */
+
+/*!
+ * \defgroup CM_ENGINE_MODULE CM Engine
+ */
+/*!
+ * \defgroup CM_ENGINE_API CM Engine API
+ *
+ * \note This API is not for user developers, this API is only an internal API.
+ *
+ * \warning All parameters in out from this API means that the parameter is a reference to a data that is complete by the call.
+ *
+ * This API is provided by CM Engine and shall be required by driver kernel part.
+ * \ingroup CM_ENGINE_MODULE
+ */
+
+#ifndef CM_ENGINE_H_
+#define CM_ENGINE_H_
+
+#include <cm/engine/api/configuration_engine.h>
+
+#include <cm/engine/api/component_engine.h>
+
+#include <cm/engine/api/memory_engine.h>
+
+#include <cm/engine/api/communication_engine.h>
+
+#include <cm/engine/api/perfmeter_engine.h>
+
+#include <cm/engine/api/executive_engine_mgt_engine.h>
+
+#include <cm/engine/api/repository_mgt_engine.h>
+
+#include <cm/engine/api/domain_engine.h>
+
+#include <cm/engine/api/migration_engine.h>
+
+#endif /*CM_ENGINE_H_*/
+
diff --git a/drivers/staging/nmf-cm/cm/engine/api/communication_engine.h b/drivers/staging/nmf-cm/cm/engine/api/communication_engine.h
new file mode 100644
index 00000000000..477a66a4002
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/communication_engine.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Communication User Engine API.
+ *
+ * This file contains the Communication Engine API for manipulating components.
+ *
+ */
+#ifndef COMMUNICATION_ENGINE_H_
+#define COMMUNICATION_ENGINE_H_
+
+#include <cm/engine/communication/inc/communication_type.h>
+
+/*!
+ * \brief Allocate Event buffer where parameters will be marshalled.
+ *
+ * In order to optimize call, this method don't need to be exported to user space,
+ * but must be used by CM driver.
+ *
+ * See \ref HOST2MPC "Host->MPC binding" for seeing an integration example.
+ *
+ * \note This method is not called from user space!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_event_params_handle CM_ENGINE_AllocEvent(t_cm_bf_host2mpc_handle host2mpcId);
+
+/*!
+ * \brief Push a event in Fifo.
+ *
+ * In order to optimize call, this method don't need to be exported to user space,
+ * but must be used by CM driver.
+ *
+ * See \ref HOST2MPC "Host->MPC binding" for seeing an integration example.
+ *
+ * \note This method is not called from user space!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_PushEvent(t_cm_bf_host2mpc_handle host2mpcId, t_event_params_handle h, t_uint32 methodIndex);
+
+/*!
+ * \brief Push a event in Fifo.
+ *
+ * In order to optimize call, this method need to be exported to user space
+ * and must be implemented by CM driver.
+ *
+ * See \ref HOST2MPC "Host->MPC binding" for seeing an integration example.
+ *
+ * \note No implementation of this method is provided in kernel CM engine!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC t_cm_error CM_ENGINE_PushEventWithSize(t_cm_bf_host2mpc_handle host2mpcId, t_event_params_handle h, t_uint32 size, t_uint32 methodIndex);
+
+/*!
+ * \brief Aknowledge a Fifo that the received event has been demarshalled.
+ *
+ * In order to optimize call, this method don't need to be exported to user space,
+ * but must be used by CM driver.
+ *
+ * See \ref MPC2HOST "MPC->Host binding" for seeing an integration example.
+ *
+ * \note This method is not called from user space!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED void CM_ENGINE_AcknowledgeEvent(t_cm_bf_mpc2host_handle mpc2hostId);
+
+#endif /*COMMUNICATION_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/component_engine.h b/drivers/staging/nmf-cm/cm/engine/api/component_engine.h
new file mode 100644
index 00000000000..a1e9defda7e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/component_engine.h
@@ -0,0 +1,418 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Components Component Manager User Engine API.
+ *
+ * This file contains the Component Manager Engine API for manipulating components.
+ *
+ */
+
+#ifndef COMPONENT_ENGINE_H_
+#define COMPONENT_ENGINE_H_
+
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/component/inc/component_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+#include <inc/nmf-limits.h>
+
+/*!
+ * \brief Instantiate a new component.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_InstantiateComponent(
+    const char templateName[MAX_TEMPLATE_NAME_LENGTH],          //!< [in] Null terminated string (Max size=\ref MAX_TEMPLATE_NAME_LENGTH)
+    t_cm_domain_id domainId,                                    //!< [in] Domain
+    t_nmf_client_id clientId,                                   //!< [in] Client ID (aka PID)
+    t_nmf_ee_priority priority,                                 //!< [in] Component priority
+    const char localName[MAX_COMPONENT_NAME_LENGTH],            //!< [in] Null terminated string (Max size=\ref MAX_COMPONENT_NAME_LENGTH)
+    const char *dataFile,                                       //!< [in] Optional reference on file where component is stored
+    t_cm_instance_handle *component                             //!< [out] component
+    );
+
+/*!
+ * \brief Start a component.
+ *
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_StartComponent(
+    t_cm_instance_handle component,
+    t_nmf_client_id clientId);
+
+/*!
+ * \brief Stop a component.
+ *
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_StopComponent(
+    t_cm_instance_handle component,
+    t_nmf_client_id clientId);
+
+/*!
+ * \brief Destroy a component.
+ *
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_DestroyComponent(
+    t_cm_instance_handle component,
+    t_nmf_client_id clientId);
+
+/*!
+ * \brief Stop and destroy all components belonging to the given client.
+ *
+ * \param[in] client
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_FlushComponents(
+        t_nmf_client_id client);
+
+/*!
+ * \brief Bind two components together.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponent(
+    const t_cm_instance_handle      client,                                             //!<
+    const char                      requiredItfClientName[MAX_INTERFACE_NAME_LENGTH],   //!< Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+    const t_cm_instance_handle      server,                                             //!<
+    const char                      providedItfServerName[MAX_INTERFACE_NAME_LENGTH],   //!< Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+    t_bool                          traced,                                             //!< FALSE for synchronous binding, TRUE for traced one
+    t_nmf_client_id                 clientId,                                           //!< Client ID
+    const char                      *dataFileTrace                                      //!< Component file data in case on traced (Note: could be null if file already in cache)
+    );
+
+/*!
+ * \brief Unbind a component.
+ *
+ * \param[in] client
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_UnbindComponent(
+    const t_cm_instance_handle      client,
+    const char                      * requiredItfClientName,
+    t_nmf_client_id                 clientId);
+
+/*!
+ * \brief Bind a component to void (silently ignore a call).
+ *
+ * \param[in] client
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponentToVoid(
+    const t_cm_instance_handle      client,
+    const char                      requiredItfClientName[MAX_INTERFACE_NAME_LENGTH],
+    t_nmf_client_id                 clientId);
+
+/*!
+ * \brief Bind two components together in an asynchronous way
+ * (the components can be on the same MPC or on two different MPC)
+ *
+ * \param[in] client
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[in] server
+ * \param[in] providedItfServerName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[in] fifosize
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponentAsynchronous(
+    const t_cm_instance_handle      client,
+    const char                      * requiredItfClientName,
+    const t_cm_instance_handle      server,
+    const char                      * providedItfServerName,
+    t_uint32                        fifosize,
+    t_cm_mpc_memory_type            eventMemType,
+    t_nmf_client_id                 clientId,
+    const char                      *dataFileSkeletonOrEvent,
+    const char                      *dataFileStub);
+
+/*!
+ * \brief Unbind a component previously binded asynchronously
+ *
+ * \param[in] client
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentAsynchronous(
+    const t_cm_instance_handle      client,
+    const char                      * requiredItfClientName,
+    t_nmf_client_id                 clientId);
+
+/*!
+ * \brief Bind the Host to a component.
+ *
+ * \param[in] server
+ * \param[in] providedItfServerName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[in] fifosize
+ * \param[out] host2mpcId
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponentFromCMCore(
+    const t_cm_instance_handle      server,
+    const char                      * providedItfServerName,
+    t_uint32                        fifosize,
+    t_cm_mpc_memory_type            eventMemType,
+    t_cm_bf_host2mpc_handle         *host2mpcId,
+    t_nmf_client_id                 clientId,
+    const char                      *dataFileSkeleton);
+
+/*!
+ * \brief Unbind a component from the Host.
+ *
+ * \param[in] host2mpcId
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentFromCMCore(
+        t_cm_bf_host2mpc_handle         host2mpcId);
+
+/*!
+ * \brief Bind a component to the Host, see \ref CM_ENGINE_BindComponentToCMCore.
+ *
+ * See \ref MPC2HOST "MPC->Host binding" for seeing an integration example.
+ *
+ * \note This method is not called from CM Proxy, its only there for wrapping purpose!!!
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_BindComponentToCMCore(
+        const t_cm_instance_handle  client,
+        const char                  *requiredItfClientName,
+        t_uint32                    fifosize,
+        t_nmf_mpc2host_handle       upLayerThis,
+        const char                  *dataFileStub,
+        t_cm_bf_mpc2host_handle     *mpc2hostId,
+        t_nmf_client_id             clientId);
+
+/*!
+ * \brief Unbind a component to the Host, see \ref CM_ENGINE_UnbindComponentToCMCore.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentToCMCore(
+        const t_cm_instance_handle      client,
+        const char                      *requiredItfClientName,
+        t_nmf_mpc2host_handle           *upLayerThis,
+        t_nmf_client_id                 clientId);
+
+/*!
+ * \brief Read a value on an attribute exported by a component instance.
+ *
+ * \param[in] component
+ * \param[in] attrName  Null terminated string (Max size=\ref MAX_ATTRIBUTE_NAME_LENGTH).
+ * \param[out] value
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_ReadComponentAttribute(
+    const t_cm_instance_handle component,
+    const char* attrName,
+    t_uint24 *value);
+
+/*!
+ * \brief Write a value on an attribute exported by a component instance.
+ *
+ * \param[in] component
+ * \param[in] attrName  Null terminated string (Max size=\ref MAX_ATTRIBUTE_NAME_LENGTH).
+ * \param[out] value
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_WriteComponentAttribute(
+    const t_cm_instance_handle component,
+    const char* attrName,
+    t_uint24 value);
+
+
+/*!
+ * \brief Get the older component.
+ *
+ * \param[in] client
+ * \param[out] headerComponent
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentListHeader(
+        const t_nmf_client_id       client,
+	t_cm_instance_handle        *headerComponent);
+
+/*!
+ * \brief Get the next component.
+ *
+ * \param[in] client
+ * \param[in] prevComponent
+ * \param[out] nextComponent
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentListNext(
+        const t_nmf_client_id       client,
+	const t_cm_instance_handle  prevComponent,
+	t_cm_instance_handle        *nextComponent);
+
+/*!
+ * \brief Get a component description
+ *
+ * \param[in] component
+ * \param[in] templateNameLength
+ * \param[in] localNameLength
+ * \param[out] templateName         Null terminated string (Size=templateNameLength, Max size=\ref MAX_TEMPLATE_NAME_LENGTH).
+ * \param[out] coreId
+ * \param[out] localName            Null terminated string (Size=localNameLength, Max size=\ref MAX_COMPONENT_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentDescription(
+        const t_cm_instance_handle  component,
+        char                        *templateName,
+        t_uint32                    templateNameLength,
+        t_nmf_core_id               *coreId,
+        char                        *localName,
+        t_uint32                    localNameLength,
+	t_nmf_ee_priority           *priority);
+
+/*!
+ * \brief Get number of interface required by a component.
+ *
+ * \param[in] component
+ * \param[out] numberRequiredInterfaces
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterfaceNumber(
+    const t_cm_instance_handle  component,
+    t_uint8                     *numberRequiredInterfaces);
+
+/*!
+ * \brief Return information about required interface.
+ *
+ * \param[in] component
+ * \param[in] index
+ * \param[in] itfNameLength
+ * \param[in] itfTypeLength
+ * \param[out] itfName          Null terminated string (Size=itfNameLength, Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[out] itfType          Null terminated string (Size=itfTypeLength, Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH).
+ * \param[out] collectionSize
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterface(
+        const t_cm_instance_handle  component,
+        const t_uint8               index,
+        char                        *itfName,
+        t_uint32                    itfNameLength,
+        char                        *itfType,
+        t_uint32                    itfTypeLength,
+        t_cm_require_state          *requireState,
+        t_sint16                    *collectionSize);
+
+/*!
+ * \brief Get the component binded to a required interface.
+ *
+ * \param[in] component
+ * \param[in] itfName               Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[in] serverItfNameLength
+ * \param[out] server
+ * \param[out] serverItfName        Null terminated string (Size=serverItfNameLength, Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterfaceBinding(
+        const t_cm_instance_handle  component,
+        const char                  *itfName,
+        t_cm_instance_handle        *server,
+        char                        *serverItfName,
+        t_uint32                    serverItfNameLength);
+
+/*!
+ * \brief Get number of interface provided by a component.
+ *
+ * \param[in] component
+ * \param[out] numberProvidedInterfaces
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentProvidedInterfaceNumber(
+    const t_cm_instance_handle  component,
+    t_uint8                     *numberProvidedInterfaces);
+
+/*!
+ * \brief Return information about provided interface.
+ *
+ * \param[in] component
+ * \param[in] index
+ * \param[in] itfNameLength
+ * \param[in] itfTypeLength
+ * \param[out] itfName          Null terminated string (Size=itfNameLength, Max size=\ref MAX_INTERFACE_NAME_LENGTH).
+ * \param[out] itfType          Null terminated string (Size=itfTypeLength, Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH).
+ * \param[out] collectionSize
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentProvidedInterface(
+        const t_cm_instance_handle  component,
+        const t_uint8               index,
+        char                        *itfName,
+        t_uint32                    itfNameLength,
+        char                        *itfType,
+        t_uint32                    itfTypeLength,
+        t_sint16                    *collectionSize);
+
+/*!
+ * \brief Get number of properties of a component.
+ *
+ * \param[in] component
+ * \param[out] numberProperties
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyNumber(
+    const t_cm_instance_handle  component,
+    t_uint8                     *numberProperties);
+
+/*!
+ * \brief Return the name of a property.
+ *
+ * \param[in] component
+ * \param[in] index
+ * \param[in] propertyNameLength
+ * \param[out] propertyName         Null terminated string (Size=propertyNameLength, Max size=\ref MAX_PROPERTY_NAME_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyName(
+        const t_cm_instance_handle  component,
+        const t_uint8               index,
+        char                        *propertyName,
+        t_uint32                    propertyNameLength);
+
+/*!
+ * \brief Get property value of a component.
+ *
+ * \param[in] component
+ * \param[in] propertyName
+ * \param[in] propertyValueLength
+ * \param[out] propertyValue         Null terminated string (Size=propertyValueLength, Max size=\ref MAX_PROPERTY_VALUE_LENGTH).
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyValue(
+        const t_cm_instance_handle  component,
+        const char                  *propertyName,
+        char                        *propertyValue,
+        t_uint32                    propertyValueLength);
+
+#endif /*COMPONENT_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/configuration_engine.h b/drivers/staging/nmf-cm/cm/engine/api/configuration_engine.h
new file mode 100644
index 00000000000..0336f62265e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/configuration_engine.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Configuration Component Manager User Engine API.
+ *
+ * This file contains the Configuration CM Engine API for manipulating CM.
+ *
+ */
+
+#ifndef CONFIGURATION_ENGINE_H
+#define CONFIGURATION_ENGINE_H
+
+#include <cm/engine/configuration/inc/configuration_type.h>
+
+/*!
+ * \brief Dynamically set some debug parameters of the CM
+ *
+ * \param[in] aCmdID The command for the parameter to update
+ * \param[in] aParam The actual value to set for the given command
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_SetMode(t_cm_cmd_id aCmdID, t_sint32 aParam);
+
+#endif /* CONFIGURATION_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/control/configuration_engine.h b/drivers/staging/nmf-cm/cm/engine/api/control/configuration_engine.h
new file mode 100644
index 00000000000..a9543a2af39
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/control/configuration_engine.h
@@ -0,0 +1,193 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Configuration Component Manager User Engine API.
+ *
+ * This file contains the Configuration CM Engine API for manipulating CM.
+ */
+
+#ifndef CONTROL_CONFIGURATION_ENGINE_H
+#define CONTROL_CONFIGURATION_ENGINE_H
+
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+
+/*****************************************************************************************/
+/*  Component Manager dedicated (for Configuration purpose) structured types definition  */
+/*****************************************************************************************/
+
+/*!
+ * \brief Description of the Nomadik HW mapping configuration
+ *
+ * Describe the Nomadik mapping that is to say:
+ *   - the ESRAM memory managed by the CM (The ESRAM address space SHALL BE declared as non cacheable, non bufferable inside host MMU table)
+ *   - the mapping of the System HW Semaphore IP
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef struct {
+    t_nmf_memory_segment  esramDesc;                   //!< Description of the ESRAM memory mapping into Nomadik SOC
+    t_cm_system_address   hwSemaphoresMappingBaseAddr; //!< Description of the System HW Semaphores IP mapping into Nomadik SOC
+} t_nmf_hw_mapping_desc;
+
+/*!
+ * @defgroup t_nmf_nomadik_version t_nmf_nomadik_version
+ * \brief Description of the various supported Nomadik SOC version
+ * @{
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef t_uint8 t_nmf_nomadik_version;                              //!< Fake enumeration type
+#define NOMADIK_8810                ((t_nmf_nomadik_version)0)      //!< STn8810 chip (any cut)
+#define NOMADIK_8815A0              ((t_nmf_nomadik_version)1)      //!< STn8815 chip (cut A0)
+#define NOMADIK_8815                ((t_nmf_nomadik_version)2)      //!< STn8815 chip (other cuts)
+#define NOMADIK_8820                ((t_nmf_nomadik_version)3)      //!< STn8820 chip
+#define NOMADIK_8500                ((t_nmf_nomadik_version)4)      //!< STn8500 chip
+/* @} */
+
+/*!
+ * \brief Description of the configuration parameters of the Component Manager
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef struct {
+    t_nmf_coms_location     comsLocation;      //!< Configure where CM Communications objects are put (see \ref t_nmf_coms_location)
+} t_nmf_config_desc;
+
+/*!
+ * @defgroup t_nmf_power_ctx t_nmf_power_ctx
+ * \brief Definition of the CM-engine context
+ *
+ * OS integrator uses this value to known the context where the associated OSAL routine is called
+ *
+ * @{
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+
+typedef t_uint32 t_nmf_power_ctx;                            //!< Fake enumeration type
+#define PWR_FLUSH_REQ_INTERRUPT_CTX ((t_nmf_power_ctx)0x00)  //!< Interrupt context - called by \ref CM_ProcessMpcEvent
+#define PWR_FLUSH_REQ_NORMAL_CTX    ((t_nmf_power_ctx)0x01)  //!< Normal context (CM user call)
+
+/* @} */
+
+
+/****************************************************************************************************************/
+/*  Component Manager dedicated (for Media Processors Cores Configuration purpose) structured types definition  */
+/****************************************************************************************************************/
+/*!
+ * @defgroup t_nmf_executive_engine_id t_nmf_executive_engine_id
+ * \brief Identification of the Media Processor Executive Engine to deploy
+ * @{
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef t_uint8 t_nmf_executive_engine_id;                                  //!< Fake enumeration type
+#define SYNCHRONOUS_EXECUTIVE_ENGINE        ((t_nmf_executive_engine_id)0)  //!< MPC Synchronous executive engine
+#define HYBRID_EXECUTIVE_ENGINE             ((t_nmf_executive_engine_id)1)  //!< MPC Hybrid synchronous executive engine
+/* @} */
+
+/*!
+ * @defgroup t_nmf_semaphore_type_id t_nmf_semaphore_type_id
+ * \brief Definition of which type semaphore shall be used for the given Media Processor communication mechanism
+ * @{
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef t_uint8 t_nmf_semaphore_type_id;                                    //!< Fake enumeration type
+#define LOCAL_SEMAPHORES                    ((t_nmf_semaphore_type_id)0)    //!< Embedded MMDSP macrocell semaphore, so CM_ProcessMpcEvent(<coreId>) shall be called under ISR connected to local MMDSP IRQ0
+#define SYSTEM_SEMAPHORES                   ((t_nmf_semaphore_type_id)1)    //!< Shared system HW Semaphores, so CM_ProcessMpcEvent(ARM_CORE_ID) shall be called under ISR connected to shared HW Sem Host IRQ
+/* @} */
+
+
+/*!
+ * \brief Opaque type for allocator, returned at CM configuration.
+ */
+typedef t_uint32 t_cfg_allocator_id;
+
+/********************************************************************************/
+/*       Configuration Component Manager API prototypes                         */
+/********************************************************************************/
+
+/*!
+ * \brief Initialisation part
+ *
+ * This routine initialize and configure the Component Manager.
+ *
+ * \param[in] pNmfHwMappingDesc hardware mapping description
+ * \param[in] pNmfConfigDesc NMF (mainly CM) Configuration description
+ *
+ * \exception TBD
+ * \return exception number.
+ *
+ * \warning The ESRAM address space SHALL BE declared as non cacheable, non bufferable inside host MMU table
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC t_cm_error CM_ENGINE_Init(
+        const t_nmf_hw_mapping_desc *pNmfHwMappingDesc,
+        const t_nmf_config_desc *pNmfConfigDesc
+        );
+
+
+/*!
+ * \brief Media Processor core initialisation part
+ *
+ * This routine configures a given Media Processor core
+ *
+ * \param[in] coreId Media Processor identifier
+ * \param[in] executiveEngineId Media Processor Executive Engine identifier
+ * \param[in] semaphoreTypeId Media Processor semaphores (to be used by communication mechanism) identifier
+ * \param[in] nbYramBanks is the number of tcm ram banks to reserved for y memory
+ * \param[in] mediaProcessorMappingBaseAddr Media Processor mapping into host CPU addressable space
+ * \param[in] commDomain Domain for allocating communication FIFOs
+ * \param[in] eeDomain Domain for EE instantiation
+ * \param[in] sdramCodeAllocId Allocator Id for the SDRAM Code segment
+ * \param[in] sdramDataAllocId Allocator Id for the SDRAM Data segment
+ *
+ * \exception TBD
+ * \return exception number.
+ *
+ * \warning The Media Processor mapping address space SHALL BE declared as non cacheable, non bufferable inside host MMU table
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC t_cm_error CM_ENGINE_ConfigureMediaProcessorCore(
+        t_nmf_core_id coreId,
+        t_nmf_executive_engine_id executiveEngineId,
+        t_nmf_semaphore_type_id semaphoreTypeId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *mediaProcessorMappingBaseAddr,
+        const t_cm_domain_id eeDomain,
+        const t_cfg_allocator_id sdramCodeAllocId,
+        const t_cfg_allocator_id sdramDataAllocId
+        );
+
+/*!
+ * \brief Configure a memory segment for later
+ *
+ * \exception TBD
+ * \return TBD
+ *
+ * \warning
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC t_cm_error CM_ENGINE_AddMpcSdramSegment(
+        const t_nmf_memory_segment          *pDesc,                     //!< [in]  Memory segment description.
+        t_cfg_allocator_id                  *allocId,                   //!< [out] Identifier of the created allocator.
+        const char                          *memoryname                 //!< [in]  Memory purpose name
+        );
+
+/********************************************************************************/
+/*       Destruction Component Manager API prototypes                         */
+/********************************************************************************/
+/*!
+ * \brief Destruction part
+ *
+ * This routine destroyes and releases all resources used by the Component Manager.
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC void CM_ENGINE_Destroy(void);
+
+
+#endif /* CONTROL_CONFIGURATION_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/control/control_engine.h b/drivers/staging/nmf-cm/cm/engine/api/control/control_engine.h
new file mode 100644
index 00000000000..1d823b27fc1
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/control/control_engine.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief CM Engine API.
+ *
+ * This file contains the Component Manager Engine API.
+ */
+/*!
+ * \defgroup CM_ENGINE_CONTROL_API CM Engine Control API
+ * \note This API is not for OS integrator, it's only for low level system integration.
+ * \ingroup CM_ENGINE_MODULE
+ */
+
+#ifndef CM_CONTROL_H_
+#define CM_CONTROL_H_
+
+#include <cm/engine/api/control/configuration_engine.h>
+
+#include <cm/engine/api/control/irq_engine.h>
+
+#include <cm/engine/api/control/power_engine.h>
+
+#endif /*CM_CONTROL_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/control/irq_engine.h b/drivers/staging/nmf-cm/cm/engine/api/control/irq_engine.h
new file mode 100644
index 00000000000..e3974764e91
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/control/irq_engine.h
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief NMF API for interrupt handler.
+ *
+ * This file contains the Component Manager API for interrupt handler.
+ */
+#ifndef CONTROL_IRQ_ENGINE_H
+#define CONTROL_IRQ_ENGINE_H
+
+#include <share/inc/nmf.h>
+#include <cm/inc/cm_type.h>
+#include <nmf/inc/service_type.h>
+#include <ee/api/trace.idt>
+
+/*!
+ * \brief MPCs -> HOST communication handler
+ *
+ * This routine shall be integrated as interrupt handler into the OS
+ *
+ * If the given Media Processor Core has been configured (through CM_ConfigureMediaProcessorCore()) as using \ref LOCAL_SEMAPHORES, then
+ * the NMF communication mechanism will use the embedded MMDSP macrocell semaphore,
+ * so CM_ProcessMpcEvent(<\e coreId>) shall be called under ISR connected to local MMDSP IRQ0, with the related \e coreId as parameter.
+ *
+ * If the given Media Processor Core has been configured (through CM_ConfigureMediaProcessorCore()) as using \ref SYSTEM_SEMAPHORES, then
+ * the NMF communication mechanism will use the shared system HW Semaphores,
+ * so CM_ProcessMpcEvent(\ref ARM_CORE_ID) shall be called under ISR connected to shared HW Sem Host IRQ, with \ref ARM_CORE_ID as parameter.
+ *
+ * NB: A Media Processor Core belonging to the distribution pool shall be configured with \ref SYSTEM_SEMAPHORES
+ *
+ * \see t_nmf_semaphore_type_id description
+ *
+ * \param[in] coreId identification of the source of the interrupt
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC IMPORT_SHARED void CM_ProcessMpcEvent(t_nmf_core_id coreId);
+
+/*!
+ * \brief Service type
+ *
+ * \note We used an enumeration in structure since this description remain inside the kernel
+ *  and we assume that everything in the kernel is compile with same compiler and option.
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef enum { // Allowed since i
+        CM_MPC_SERVICE_NONE = 0,                                //!< No service found
+        CM_MPC_SERVICE_PANIC = 1,                               //!< Panic service found
+        CM_MPC_SERVICE_PRINT = 2,                               //!< Print service found
+        CM_MPC_SERVICE_TRACE = 3                                //!< Trace service found
+} t_cm_service_type;
+                                          //!< Service description type
+/*!
+ * \brief Service description data
+ *
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+typedef struct {
+    union {
+       t_nmf_panic_data panic;                                                 //!< Panic description
+       struct {
+           t_uint32                     dspAddress;
+           t_uint32                     value1;
+           t_uint32                     value2;
+       } print;                                                 //!< Printf like description
+    } u;                                                    //!< Union of service description
+} t_cm_service_description;
+
+/*!
+ * \brief MPC Panic handler
+ *
+ * This routine shall be called as interrupt handler into the OS.
+ *
+ * So CM_getPanicDescription shall be called under ISR connected to local MMDSP IRQ1, with the related \e coreId as parameter.
+ *
+ * \param[in] coreId                identification of the source of the interrupt
+ * \param[out] srcType              Pointer on service type
+ * \param[out] srcDescr             Pointer on service description
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_getServiceDescription(
+        t_nmf_core_id               coreId,
+        t_cm_service_type           *srcType,
+	t_cm_service_description    *srcDescr);
+
+/*!
+ * \brief Read a null terminated string inside an MPC
+ *
+ * This routine could be used to read the MPC string give as parameter during an CM_NMF_SERVICE_PRINT
+ *
+ * \param[in] coreId                Identification of the code where read string
+ * \param[in] dspAddress            Address of the string in the MPC
+ * \param[out] buffer             Buffer pointer where returning null terminated string
+ * \param[in] bufferSize            Buffer size
+ *
+ * \ingroup CM_ENGINE_CONTROL_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ReadMPCString(
+        t_nmf_core_id               coreId,
+        t_uint32                    dspAddress,
+        char *                      buffer,
+        t_uint32                    bufferSize);
+
+typedef enum {
+        CM_MPC_TRACE_NONE = 0,
+        CM_MPC_TRACE_READ = 1,
+        CM_MPC_TRACE_READ_OVERRUN = 2
+} t_cm_trace_type;
+
+PUBLIC IMPORT_SHARED t_cm_trace_type CM_ENGINE_GetNextTrace(
+        t_nmf_core_id               coreId,
+        struct t_nmf_trace          *trace);
+
+#endif /* CONTROL_IRQ_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/domain_engine.h b/drivers/staging/nmf-cm/cm/engine/api/domain_engine.h
new file mode 100644
index 00000000000..7cc6f33ed90
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/domain_engine.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Public Component Manager Memory User SYSCALL API.
+ *
+ * This file contains the Component Manager SYSCALL API for manipulating domains.
+ *
+ */
+
+#ifndef __INC_DOMAIN_ENGINE_H
+#define __INC_DOMAIN_ENGINE_H
+
+#include <cm/engine/memory/inc/domain_type.h>
+
+/*!
+ * \brief Create a domain.
+ *
+ * Create a memory domain for use in the CM for component instantiation and memory allocation.
+ *
+ * \param[in]  client Id of the client.
+ * \param[in]  domain Description of domain memories.
+ * \param[out] handle Idetifier of the created domain
+ *
+ * \exception CM_INVALID_DOMAIN_DEFINITION
+ * \exception CM_INTERNAL_DOMAIN_OVERFLOW
+ * \exception CM_OK
+ *
+ * \return Error code.
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_CreateMemoryDomain(
+        const t_nmf_client_id     client,
+        const t_cm_domain_memory  *domain,
+        t_cm_domain_id            *handle
+        );
+
+/*!
+ * \brief Create a scratch domain.
+ *
+ * Create a scratch memory domain. Scratch domains
+ * are used to perform overlapping allocations.
+ *
+ * \param[in]  client Id of the client.
+ * \param[in]  parentId Identifier of the parent domain.
+ * \param[in]  domain Description of domain memories.
+ * \param[out] handle Idetifier of the created domain
+ *
+ * \exception CM_INVALID_DOMAIN_DEFINITION
+ * \exception CM_INTERNAL_DOMAIN_OVERFLOW
+ * \exception CM_NO_MORE_MEMORY
+ * \exception CM_OK
+ *
+ * \return Error code.
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_CreateMemoryDomainScratch(
+        const t_nmf_client_id      client,
+        const t_cm_domain_id       parentId,
+        const t_cm_domain_memory  *domain,
+        t_cm_domain_id            *handle
+        );
+
+/*!
+ * \brief Destroy a memory domain.
+
+ * \param[in] handle Domain identifier to destroy.
+ *
+ * \exception CM_INVALID_DOMAIN_HANDLE
+ * \exception CM_OK
+ *
+ * \return Error code.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_DestroyMemoryDomain(
+        t_cm_domain_id handle);
+
+/*!
+ * \brief Destroy all domains belonging to a given client.
+ *
+ * \param[in] client
+ *
+ * \return Error code.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_FlushMemoryDomains(
+        t_nmf_client_id client);
+
+/*!
+ * \brief Retrieve the coreId for a given domain. Utility.
+
+ * \param[in]  domainId   Domain identifier.
+ * \param[out] coreId     Core identifier.
+ *
+ * \exception CM_INVALID_DOMAIN_HANDLE  Invalid domain handle
+ * \exception CM_OK
+ *
+ * \return Error code.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetDomainCoreId(const t_cm_domain_id domainId, t_nmf_core_id *coreId);
+
+#endif /* __INC_DOMAIN_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/executive_engine_mgt_engine.h b/drivers/staging/nmf-cm/cm/engine/api/executive_engine_mgt_engine.h
new file mode 100644
index 00000000000..9cb8bc1481b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/executive_engine_mgt_engine.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief CM executive engine management Engine API.
+ *
+ * This file contains the Component Manager executive engine management Engine API.
+ */
+#ifndef CM_EXECUTIVE_ENGINE_MANAGEMENT_ENGINE_H_
+#define CM_EXECUTIVE_ENGINE_MANAGEMENT_ENGINE_H_
+
+#include <cm/inc/cm_type.h>
+
+/*!
+ * \brief Return executive engine handle for given core
+ *
+ * \param[in] coreId The core for which we want executive engine handle.
+ * \param[out] executiveEngineHandle executive engine instance (null if the executive engine is not loaded)
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetExecutiveEngineHandle(
+        t_cm_domain_id domainId,
+        t_cm_instance_handle *executiveEngineHandle);
+
+#endif /*CM_EXECUTIVE_ENGINE_MANAGEMENT_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/memory_engine.h b/drivers/staging/nmf-cm/cm/engine/api/memory_engine.h
new file mode 100644
index 00000000000..9f5e25b3ebf
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/memory_engine.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Public Component Manager Memory User Engine API.
+ *
+ * This file contains the Component Manager Engine API for manipulating memory.
+ *
+ */
+
+#ifndef CM_MEMORY_ENGINE_H_
+#define CM_MEMORY_ENGINE_H_
+
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+
+/*!
+ * \brief Allocate memory in a Media Processor Core memory
+ *
+ * \param[in] domainId
+ * \param[in] memType
+ * \param[in] size
+ * \param[in] memAlignment
+ * \param[out] pHandle
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_AllocMpcMemory(
+        t_cm_domain_id domainId,
+        t_nmf_client_id clientId,                                   //!< [in] Client ID (aka PID)
+        t_cm_mpc_memory_type memType,
+        t_cm_size size,
+        t_cm_mpc_memory_alignment memAlignment,
+        t_cm_memory_handle *pHandle
+        );
+
+
+/*!
+ * \brief Free a MPC memory block.
+ *
+ * \param[in] handle
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_FreeMpcMemory(t_cm_memory_handle handle);
+
+/*!
+ * \brief Get the start address of the MPC memory block seen by the host CPU (physical and logical)
+ *
+ * The logical system address returned by this method is valid only in kernel space and the physical
+ * address is accessible only from kernel space too.
+ *
+ * \see OSMem "OS Memory management" for seeing an integration example.
+ *
+ * \param[in] handle
+ * \param[out] pSystemAddress
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemorySystemAddress(
+        t_cm_memory_handle handle,
+        t_cm_system_address *pSystemAddress);
+
+/*!
+ * \brief Get the start address of the memory block seen by the Media Processor Core
+ *
+ * \param[in] handle
+ * \param[out] pMpcAddress
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemoryMpcAddress(
+        t_cm_memory_handle handle,
+        t_uint32 *pMpcAddress);
+
+/*!
+ * \brief Get the memory status for given memory type of a given Media Processor Core
+ *
+ * \param[in] domainId
+ * \param[in] memType
+ * \param[out] pStatus
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemoryStatus(
+        t_cm_domain_id domainId,
+        t_cm_mpc_memory_type memType,
+        t_cm_allocator_status *pStatus);
+
+#endif /* CM_MEMORY_ENGINE_H_ */
+
diff --git a/drivers/staging/nmf-cm/cm/engine/api/migration_engine.h b/drivers/staging/nmf-cm/cm/engine/api/migration_engine.h
new file mode 100644
index 00000000000..77a266d4459
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/migration_engine.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#ifndef CM_MIGRATION_ENGINE_H
+#define CM_MIGRATION_ENGINE_H
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/domain_type.h>
+
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_Migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst);
+
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_Unmigrate(void);
+
+#endif /* CM_MIGRATION_ENGINE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/api/perfmeter_engine.h b/drivers/staging/nmf-cm/cm/engine/api/perfmeter_engine.h
new file mode 100644
index 00000000000..bead49dc81e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/perfmeter_engine.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief CM Performance Meter Engine API.
+ *
+ * This file contains the Component Manager Performance Meter Engine API.
+ */
+#ifndef CM_ENGINE_PERFMETER_ENGINE_H_
+#define CM_ENGINE_PERFMETER_ENGINE_H_
+
+#include <cm/engine/perfmeter/inc/perfmeter_type.h>
+
+/*!
+ * \brief MPC cpu load
+ *
+ * \param[in] coreId identification of mpc from which we want cpu load
+ * \param[out] mpcLoadCounter will contain mpc cpu load counters value if success
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_getMpcLoadCounter(
+        t_nmf_core_id coreId,
+        t_cm_mpc_load_counter *mpcLoadCounter);
+
+/*!
+ * \brief MPC cpu load
+ * Same as \ref CM_ENGINE_getMpcLoadCounter() without lock
+ *
+ * \param[in] coreId identification of mpc from which we want cpu load
+ * \param[out] mpcLoadCounter will contain mpc cpu load counters value if success
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_GetMpcLoadCounter(
+        t_nmf_core_id coreId,
+        t_cm_mpc_load_counter *mpcLoadCounter);
+
+#endif /*CM_ENGINE_PERFMETER_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/api/repository_mgt_engine.h b/drivers/staging/nmf-cm/cm/engine/api/repository_mgt_engine.h
new file mode 100644
index 00000000000..b63c60d85eb
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/api/repository_mgt_engine.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief Repository Component Manager User Engine API.
+ *
+ * This file contains the Component Manager Engine API for manipulating the components files.
+ */
+
+#ifndef REPOSITORY_MGT_ENGINE_H_
+#define REPOSITORY_MGT_ENGINE_H_
+
+#include <inc/nmf-limits.h>
+#include <cm/engine/repository_mgt/inc/repository_type.h>
+
+/*!
+ * \brief Get the name(s) of the component(s) to load.
+ *
+ * \param[in] client                Handle of the client component (optional)
+ * \param[in] requiredItfClientName Null terminated string (Max size=\ref MAX_INTERFACE_NAME_LENGTH)  (optional).
+ * \param[in] server                Handle of the server component  (optional)
+ * \param[in] providedItfServerName Null terminated string (Max size==\ref MAX_INTERFACE_NAME_LENGTH) (optional).
+ * \param[out] fileList             List of required component(s).
+ * \param[in,out] listSize          Initial size of the list as input. Updated with the number of entries really used.
+ * \param[out] type                 Interface type of the client required or server provided interface. Null terminated string (Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH) (optional) .
+ * \param[out] methodNumber         Number of method in the interface type of the client required interface. (only used when called from CM_BindComponentToUser) (optional)
+ *
+ * \note It returns the component(s) name(s) to load, depending on the first four parameters.
+ *
+ *  - If all 4 are NULL, it returns the name of the Executive Engine components to load
+ *  - If 'client' is NULL, it returns the name of the required components for a Bind From CMCore.
+ *  - If 'server' is NULL, it returns the name of the required components for a Bind To CMCore.
+ *  - If none is NULL, it returns the name of the required components for an asynchronous binding
+ *
+ * The names are returned in fileList, whose initial size is specified in listSize.
+ * (sizeList must be the number of provided entries of \ref MAX_INTERFACE_TYPE_NAME_LENGTH length
+ * If not enough space is provided, CM_NO_MORE_MEMORY is returned
+ *
+ * sizeList is updated with the number entries really filled.
+ *
+ * This method is also used to retrieve the interface type when called from CM_BindComponentToUser and CM_BindComponentFromUser
+ * and the number of methods when called from CM_BindComponentToUser.
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_GetRequiredComponentFiles(
+        // IN
+        t_action_to_do action,
+        const t_cm_instance_handle client,
+        const char *requiredItfClientName,
+        const t_cm_instance_handle server,
+        const char *providedItfServerName,
+        // OUT component to be pushed
+        char fileList[][MAX_INTERFACE_TYPE_NAME_LENGTH],
+        // IN max component allowed to be pushed
+        t_uint32 listSize,
+        // OUT interface information
+        char type[MAX_INTERFACE_TYPE_NAME_LENGTH],
+        t_uint32 *methodNumber);
+
+/*!
+ * \brief Push a component into the CM Component Cache.
+ *
+ * \param[in] name Component name, null terminated string (Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH)
+ * \param[in] data Pointer to _user_ data of the component.
+ * \param[in] size Size of the data.
+ *
+ * \note Push a component in the Component Cache
+ *       The 'data' must be provided such a way that they can be freed by a call to OSAL_Free()
+ *       The caller doesn't need and must NOT free the data, even in case of failure.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_PushComponent(const char *name, const void *data, t_cm_size size);
+
+/*!
+ * \brief Remove a component from the CM Component Cache.
+ *
+ * \param[in] name Component name, null terminated string (Max size=\ref MAX_INTERFACE_TYPE_NAME_LENGTH)
+ *
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_cm_error CM_ENGINE_ReleaseComponent (const char *name);
+
+/*!
+ * \brief Check if the CM Component Cache is empty.
+ *
+ * \return a boolean value TRUE or FALSE.
+ * \ingroup CM_ENGINE_API
+ */
+PUBLIC IMPORT_SHARED t_bool CM_ENGINE_IsComponentCacheEmpty(void);
+#endif /*REPOSITORY_MGT_ENGINE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/fifo/inc/nmf_fifo_arm.h b/drivers/staging/nmf-cm/cm/engine/communication/fifo/inc/nmf_fifo_arm.h
new file mode 100644
index 00000000000..0463d6a71a5
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/fifo/inc/nmf_fifo_arm.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#ifndef __INC_NMF_FIFO_ARM
+#define __INC_NMF_FIFO_ARM
+
+#include <cm/inc/cm_type.h>
+#include <share/communication/inc/nmf_fifo_desc.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/memory/inc/domain.h>
+
+/*
+ * ARM Fifo descriptor (encapsulate the share one)
+ */
+typedef struct
+{
+    t_uint32 magic;
+    t_memory_handle chunkHandle;
+    t_nmf_core_id pusherCoreId;
+    t_nmf_core_id poperCoreId;
+    t_shared_addr dspAdress;
+    t_dsp_address_info dspAddressInfo;
+    t_nmf_fifo_desc *fifoDesc;       //used for all fifo operations and systematically updated by the migrated offset (see cm_AllocEvent)
+    t_nmf_fifo_desc *fifoDescShadow; //shadow desc, is used to restore state after migration and perform the update of the real desc
+
+    // ExtendedField
+    t_memory_handle extendedFieldHandle;
+    t_shared_field  *extendedField;
+} t_nmf_fifo_arm_desc;
+
+PUBLIC t_uint32 fifo_isFifoIdValid(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_nmf_fifo_arm_desc* fifo_alloc(
+        t_nmf_core_id pusherCoreId, t_nmf_core_id poperCoreId,
+        t_uint16 size_in_16bit, t_uint16 nbElem, t_uint16 nbExtendedSharedFields,
+        t_dsp_memory_type_id memType, t_dsp_memory_type_id memExtendedFieldType, t_cm_domain_id domainId);
+PUBLIC void fifo_free(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_uint16 fifo_normalizeDepth(t_uint16 requestedDepth);
+
+PUBLIC t_shared_addr fifo_getAndAckNextElemToWritePointer(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_shared_addr fifo_getAndAckNextElemToReadPointer(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_shared_addr fifo_getNextElemToWritePointer(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_shared_addr fifo_getNextElemToReadPointer(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC void fifo_acknowledgeRead(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC void fifo_acknowledgeWrite(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC void fifo_coms_acknowledgeWriteAndInterruptGeneration(t_nmf_fifo_arm_desc *pArmFifo);
+
+PUBLIC t_cm_error fifo_params_setSharedField(t_nmf_fifo_arm_desc *pArmFifo, t_uint32 sharedFieldIndex, t_shared_field value);
+
+#endif /* __INC_NMF_FIFO_ARM */
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/fifo/src/nmf_fifo_arm.c b/drivers/staging/nmf-cm/cm/engine/communication/fifo/src/nmf_fifo_arm.c
new file mode 100644
index 00000000000..48d7f9e9f03
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/fifo/src/nmf_fifo_arm.c
@@ -0,0 +1,241 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <share/communication/inc/nmf_fifo_desc.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include "../inc/nmf_fifo_arm.h"
+
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/trace/inc/trace.h>
+
+/* define value of fifo magic number */
+#define NMF_FIFO_MAGIC_NB                   0xF1F0BEEF
+
+PRIVATE t_uint16 fifo_getCount(
+    t_uint16 writeIndex,
+    t_uint16 readIndex,
+    t_uint16 fifoSize
+)
+{
+    if (writeIndex >= readIndex) {return writeIndex - readIndex;}
+    else {return fifoSize - readIndex + writeIndex;}
+}
+
+PRIVATE t_uint16 fifo_incrementIndex(
+    t_uint16 index,
+    t_uint16 wrappingValue
+)
+{
+    if (++index == wrappingValue) {index = 0;}
+
+    return index;
+}
+
+PUBLIC t_uint16 fifo_normalizeDepth(t_uint16 requestedDepth)
+{
+    /* with new implementation we don't align on power of two */
+    return requestedDepth;
+}
+
+PUBLIC t_nmf_fifo_arm_desc* fifo_alloc(
+        t_nmf_core_id pusherCoreId, t_nmf_core_id poperCoreId,
+        t_uint16 size_in_16bit, t_uint16 nbElem, t_uint16 nbExtendedSharedFields,
+        t_dsp_memory_type_id memType, t_dsp_memory_type_id memExtendedFieldType, t_cm_domain_id domainId)
+{
+    t_uint16 realNbElem = nbElem + 1;/* we need one more elem in new implementation */
+    t_uint16 sizeToAlloc = sizeof(t_nmf_fifo_desc) + ((size_in_16bit<<1)*realNbElem);
+    t_nmf_fifo_arm_desc *pArmFifoDesc;
+
+    pArmFifoDesc = (t_nmf_fifo_arm_desc*)OSAL_Alloc(sizeof (t_nmf_fifo_arm_desc));
+    if (pArmFifoDesc == NULL)
+        goto errorde;
+
+    pArmFifoDesc->chunkHandle = cm_DM_Alloc(domainId, memType,
+            (sizeToAlloc/2), CM_MM_ALIGN_2WORDS, TRUE); /* size in 16-bit since we use EXT16 memory */
+    if (pArmFifoDesc->chunkHandle == INVALID_MEMORY_HANDLE)
+        goto errorsh;
+
+    pArmFifoDesc->magic = NMF_FIFO_MAGIC_NB;
+    pArmFifoDesc->pusherCoreId = pusherCoreId;
+    pArmFifoDesc->poperCoreId = poperCoreId;
+
+    pArmFifoDesc->fifoDesc = (t_nmf_fifo_desc *)cm_DSP_GetHostLogicalAddress(pArmFifoDesc->chunkHandle);
+    cm_DSP_GetDspAddress(pArmFifoDesc->chunkHandle, &pArmFifoDesc->dspAdress);
+
+    pArmFifoDesc->fifoDescShadow = pArmFifoDesc->fifoDesc;
+    cm_DSP_GetDspDataAddressInfo(cm_DM_GetDomainCoreId(domainId), pArmFifoDesc->dspAdress, &pArmFifoDesc->dspAddressInfo);
+
+    pArmFifoDesc->extendedFieldHandle = INVALID_MEMORY_HANDLE;
+    pArmFifoDesc->extendedField = NULL;
+
+    pArmFifoDesc->fifoDesc->elemSize = size_in_16bit;
+    pArmFifoDesc->fifoDesc->fifoFullValue = nbElem;
+    pArmFifoDesc->fifoDesc->wrappingValue = realNbElem;
+
+    pArmFifoDesc->fifoDesc->semId = cm_SEM_Alloc(pusherCoreId, poperCoreId);
+    pArmFifoDesc->fifoDesc->readIndex = 0;
+    pArmFifoDesc->fifoDesc->writeIndex = 0;
+
+    LOG_INTERNAL(2, "\n##### Fifo alloc 0x%x (0x%x)\n\n", pArmFifoDesc, pArmFifoDesc->fifoDesc, 0, 0, 0, 0);
+
+    if (nbExtendedSharedFields >= 1)
+    {
+        if(poperCoreId == ARM_CORE_ID)
+        {
+            /* Optimization: Don't put extended Field in DSP memory since use only by ARM if popper */
+            pArmFifoDesc->extendedField = (t_shared_field*)OSAL_Alloc(nbExtendedSharedFields * sizeof(t_shared_field));
+            if (pArmFifoDesc->extendedField == NULL)
+                goto errorex;
+
+            pArmFifoDesc->fifoDesc->extendedField = (t_uint32)pArmFifoDesc->extendedField;
+        }
+        else
+        {
+            pArmFifoDesc->extendedFieldHandle = cm_DM_Alloc(domainId, memExtendedFieldType,
+                    nbExtendedSharedFields * sizeof(t_shared_field) / 4, CM_MM_ALIGN_WORD, TRUE);
+            if (pArmFifoDesc->extendedFieldHandle == INVALID_MEMORY_HANDLE)
+                goto errorex;
+
+            pArmFifoDesc->extendedField = (t_shared_field*)cm_DSP_GetHostLogicalAddress(pArmFifoDesc->extendedFieldHandle);
+            cm_DSP_GetDspAddress(pArmFifoDesc->extendedFieldHandle, (t_uint32*)&pArmFifoDesc->fifoDesc->extendedField);
+        }
+
+        pArmFifoDesc->extendedField[EXTENDED_FIELD_BCTHIS_OR_TOP] = (t_shared_field)0;
+    }
+
+    return pArmFifoDesc;
+
+errorex:
+    (void)cm_DM_Free(pArmFifoDesc->chunkHandle, TRUE);
+errorsh:
+    OSAL_Free(pArmFifoDesc);
+errorde:
+    return NULL;
+}
+
+PUBLIC t_uint32 fifo_isFifoIdValid(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    if (((t_uint32)pArmFifo & CM_MM_ALIGN_WORD) != 0) {return FALSE;}
+    if (pArmFifo->magic == NMF_FIFO_MAGIC_NB)  {return TRUE;}
+    else {return FALSE;}
+}
+
+PUBLIC void fifo_free(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    CM_ASSERT(pArmFifo->pusherCoreId != ARM_CORE_ID || pArmFifo->poperCoreId != ARM_CORE_ID);
+
+    pArmFifo->magic = ~NMF_FIFO_MAGIC_NB;
+
+    if(pArmFifo->extendedFieldHandle != INVALID_MEMORY_HANDLE)
+        (void)cm_DM_Free(pArmFifo->extendedFieldHandle, TRUE);
+    else if(pArmFifo->extendedField != NULL)
+        OSAL_Free(pArmFifo->extendedField);
+
+    (void)cm_DM_Free(pArmFifo->chunkHandle, TRUE);
+    OSAL_Free(pArmFifo);
+}
+
+PUBLIC t_shared_addr fifo_getAndAckNextElemToWritePointer(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_shared_addr retValue;
+
+    retValue = fifo_getNextElemToWritePointer(pArmFifo);
+    if (retValue != 0)
+    {
+        fifo_acknowledgeWrite(pArmFifo);
+    }
+
+    return retValue;
+}
+
+PUBLIC t_shared_addr fifo_getAndAckNextElemToReadPointer(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_shared_addr retValue;
+
+    retValue = fifo_getNextElemToReadPointer(pArmFifo);
+    if (retValue != 0)
+    {
+        fifo_acknowledgeRead(pArmFifo);
+    }
+
+    return retValue;
+}
+
+PUBLIC t_shared_addr fifo_getNextElemToWritePointer(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_shared_addr retValue = 0;
+    t_nmf_fifo_desc *pDesc;
+    t_uint16 count;
+
+    if ((NULL == pArmFifo) || (NULL == (pDesc = pArmFifo->fifoDesc)))
+	    return 0;
+
+    count = fifo_getCount(pDesc->writeIndex, pDesc->readIndex,pDesc->wrappingValue);
+    if (count < pDesc->fifoFullValue)
+    {
+        retValue = ((t_shared_addr)pDesc + sizeof(t_nmf_fifo_desc) + (pDesc->writeIndex*(pDesc->elemSize<<1)));
+    }
+
+    return retValue;
+}
+
+PUBLIC t_shared_addr fifo_getNextElemToReadPointer(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_shared_addr retValue = 0;
+    t_nmf_fifo_desc *pDesc;
+    t_uint16 count;
+
+    if ((NULL == pArmFifo) || (NULL == (pDesc = pArmFifo->fifoDesc)))
+	    return 0;
+
+    count = fifo_getCount(pDesc->writeIndex, pDesc->readIndex,pDesc->wrappingValue);
+    if (count != 0)
+    {
+        retValue = ((t_shared_addr)pDesc+ sizeof(t_nmf_fifo_desc) + (pDesc->readIndex*(pDesc->elemSize<<1)));
+    }
+
+    return retValue;
+}
+
+PUBLIC void fifo_acknowledgeRead(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_nmf_fifo_desc *pDesc = pArmFifo->fifoDesc;
+
+    pDesc->readIndex = fifo_incrementIndex(pDesc->readIndex, pDesc->wrappingValue);
+}
+
+PUBLIC void fifo_acknowledgeWrite(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_nmf_fifo_desc *pDesc = pArmFifo->fifoDesc;
+
+    pDesc->writeIndex = fifo_incrementIndex(pDesc->writeIndex, pDesc->wrappingValue);
+}
+
+PUBLIC void fifo_coms_acknowledgeWriteAndInterruptGeneration(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    t_nmf_fifo_desc *pDesc = pArmFifo->fifoDesc;
+
+    fifo_acknowledgeWrite(pArmFifo);
+    //Be sure before generate irq that fifo has been updated
+    OSAL_mb();
+    cm_SEM_GenerateIrq[pArmFifo->poperCoreId](pArmFifo->poperCoreId, pDesc->semId);
+    //cm_SEM_Take[pArmFifo->poperCoreId](pArmFifo->poperCoreId, pDesc->semId);
+    //cm_SEM_GiveWithInterruptGeneration[pArmFifo->poperCoreId](pArmFifo->poperCoreId, pDesc->semId);
+}
+
+PUBLIC t_cm_error fifo_params_setSharedField(t_nmf_fifo_arm_desc *pArmFifo, t_uint32 sharedFieldIndex, t_shared_field value)
+{
+    pArmFifo->extendedField[sharedFieldIndex] = value;
+
+    return CM_OK;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/inc/communication.h b/drivers/staging/nmf-cm/cm/engine/communication/inc/communication.h
new file mode 100644
index 00000000000..53ab87b7096
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/inc/communication.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Components Management internal methods - Communication part.
+ *
+ */
+#ifndef __INC_NMF_COM
+#define __INC_NMF_COM
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/communication/fifo/inc/nmf_fifo_arm.h>
+#include <cm/engine/memory/inc/memory.h>
+
+#include <cm/engine/communication/inc/communication_type.h>
+
+extern t_dsp_memory_type_id comsLocation;
+extern t_dsp_memory_type_id paramsLocation;
+extern t_dsp_memory_type_id extendedFieldLocation;
+
+PUBLIC t_cm_error cm_COM_Init(t_nmf_coms_location comsLocation);
+PUBLIC t_cm_error cm_COM_AllocateMpc(t_nmf_core_id coreId);
+PUBLIC void cm_COM_InitMpc(t_nmf_core_id coreId);
+PUBLIC void cm_COM_FreeMpc(t_nmf_core_id coreId);
+
+PUBLIC t_cm_error cm_PushEventTrace(t_nmf_fifo_arm_desc*, t_event_params_handle h, t_uint32 methodIndex, t_uint32 isTrace);
+PUBLIC t_cm_error cm_PushEvent(t_nmf_fifo_arm_desc *pArmFifo, t_event_params_handle h, t_uint32 methodIndex);
+PUBLIC void cm_AcknowledgeEvent(t_nmf_fifo_arm_desc *pArmFifo);
+PUBLIC t_event_params_handle cm_AllocEvent(t_nmf_fifo_arm_desc *pArmFifo);
+
+/*!
+ * \internal
+ * \brief Definition of custom value for userTHIS parameter of PostDfc OSAL call
+ *
+ * This value is used as 1st parameter of a pPostDfc call to indicate that a given interrupt is linked to an internal Component Manager event
+ */
+#define NMF_INTERNAL_USERTHIS ((void*)MASK_ALL32)
+
+typedef void (*t_callback_method)(t_nmf_core_id coreId, t_event_params_handle pParam);
+
+#endif /* __INC_NMF_COM */
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/inc/communication_type.h b/drivers/staging/nmf-cm/cm/engine/communication/inc/communication_type.h
new file mode 100644
index 00000000000..53a6ff39b07
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/inc/communication_type.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Communication Component Manager API type.
+ */
+#ifndef COMMUNICATION_TYPE_H_
+#define COMMUNICATION_TYPE_H_
+
+#include <cm/inc/cm_type.h>
+
+
+/*!
+ * \brief Buffer type used for (un)marshalling parameters.
+ *
+ * This buffer type is used for (un)marshalling paramaters. It can either be a
+ * shared memory buffer (ESRAM or SDRAM) or a pure host software memory (stack).
+
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint16 *t_event_params_handle;
+
+/*!
+ * \brief Component manager handle to Host -> MPC communication.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint32 t_cm_bf_host2mpc_handle;
+
+/*!
+ * \brief Component manager handle to MPC -> Host communication.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint32 t_cm_bf_mpc2host_handle;
+
+/*!
+ * \brief Component manager proxy handle to MPC -> Host skeleton context.
+ *
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint32 t_nmf_mpc2host_handle;
+
+/*!
+ * @defgroup t_nmf_coms_location t_nmf_coms_location
+ * \brief Definition of the location of the internal CM communication objects
+ *
+ * @{
+ * \ingroup CM_ENGINE_API
+ */
+typedef t_uint8 t_nmf_coms_location;                                //!< Fake enumeration type
+#define COMS_IN_ESRAM               ((t_nmf_coms_location)0)        //!< All coms objects (coms and params fifos) will be in embedded RAM
+#define COMS_IN_SDRAM               ((t_nmf_coms_location)1)        //!< All coms objects (coms and params fifos) will be in external RAM
+/* @} */
+
+#endif /*COMMUNICATION_TYPE_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/communication/src/communication.c b/drivers/staging/nmf-cm/cm/engine/communication/src/communication.c
new file mode 100644
index 00000000000..811e9b8c1e6
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/communication/src/communication.c
@@ -0,0 +1,334 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#include <cm/inc/cm_type.h>
+#include "../inc/communication.h"
+#include <share/communication/inc/communication_fifo.h>
+#include <cm/engine/api/control/irq_engine.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/communication/fifo/inc/nmf_fifo_arm.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+#include <cm/engine/component/inc/initializer.h>
+
+#define ARM_DSP_EVENT_FIFO_SIZE 128
+
+t_dsp_memory_type_id comsLocation;
+t_dsp_memory_type_id paramsLocation;
+t_dsp_memory_type_id extendedFieldLocation;
+
+#define __DEBUG
+
+#ifdef __DEBUG
+PRIVATE volatile t_uint32 armdspCounter = 0;
+PRIVATE volatile t_uint32 armdspIrqCounter = 0;
+PRIVATE volatile t_uint32 dsparmCounter = 0;
+PRIVATE volatile t_uint32 dsparmIrqCounter = 0;
+#endif /* __DEBUG */
+
+t_nmf_fifo_arm_desc* mpc2mpcComsFifoId[NB_CORE_IDS][NB_CORE_IDS];
+
+PRIVATE const t_callback_method internalHostJumptable[] = {
+    processAsyncAcknowledge,
+    processAsyncAcknowledge,
+    processAsyncAcknowledge,
+    processSyncAcknowledge,
+    processAsyncAcknowledge,
+    processAsyncAcknowledge,
+    processAsyncAcknowledge,
+    processSyncAcknowledge,
+    processAsyncAcknowledge,
+    processSyncAcknowledge,
+    processSyncAcknowledge, // Start sync
+    processSyncAcknowledge // Stop sync
+};
+
+PUBLIC t_cm_error cm_COM_Init(t_nmf_coms_location _comsLocation)
+{
+    t_nmf_core_id coreId, localCoreId;
+
+    /*
+     * Configure the default location of coms and params fifo (configuration by user) */
+    switch(_comsLocation)
+    {
+        case COMS_IN_SDRAM:
+            comsLocation = SDRAM_EXT16;
+            paramsLocation = SDRAM_EXT16;
+            extendedFieldLocation = SDRAM_EXT24;
+            break;
+        case COMS_IN_ESRAM:
+            comsLocation = ESRAM_EXT16;
+            paramsLocation = ESRAM_EXT16;
+            extendedFieldLocation = ESRAM_EXT24;
+            break;
+        default: CM_ASSERT(0);
+    }
+
+    for (coreId =  ARM_CORE_ID; coreId < NB_CORE_IDS; coreId++)
+    {
+        for (localCoreId = ARM_CORE_ID; localCoreId < NB_CORE_IDS; localCoreId++)
+        {
+            mpc2mpcComsFifoId[coreId][localCoreId] = NULL;
+        }
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_COM_AllocateMpc(t_nmf_core_id coreId)
+{
+    t_nmf_core_id localCoreId;
+
+    /*
+     * Allocation of the coms fifo with neighbor MPCs
+     * if they are already initialized (known through initializedCoresMask)
+     */
+    for (localCoreId = ARM_CORE_ID; localCoreId < NB_CORE_IDS; localCoreId++)
+    {
+        if (localCoreId == coreId) continue; /* no coms fifo with itself ;) */
+        if(cm_DSP_GetState(localCoreId)->state != MPC_STATE_BOOTED) continue;
+
+        /*
+         * coms fifo from other initialized MPCs to the given one
+         */
+        if (mpc2mpcComsFifoId[coreId][localCoreId] != NULL) continue; /* coms fifo already allocated */
+
+        mpc2mpcComsFifoId[coreId][localCoreId] = fifo_alloc(
+                coreId, localCoreId,
+                EVENT_ELEM_SIZE_IN_BYTE/2, ARM_DSP_EVENT_FIFO_SIZE,
+                0, comsLocation, extendedFieldLocation, cm_DSP_GetState(coreId)->domainEE
+                );
+        if (mpc2mpcComsFifoId[coreId][localCoreId] == NULL)
+            goto oom;
+
+        /*
+         * coms fifo from the given MPC to the other initialized ones
+         */
+        if (mpc2mpcComsFifoId[localCoreId][coreId] != NULL) continue; /* coms fifo already allocated */
+
+        mpc2mpcComsFifoId[localCoreId][coreId] = fifo_alloc(
+                localCoreId, coreId,
+                EVENT_ELEM_SIZE_IN_BYTE/2, ARM_DSP_EVENT_FIFO_SIZE,
+                0, comsLocation, extendedFieldLocation, cm_DSP_GetState(coreId)->domainEE
+                );
+        if (mpc2mpcComsFifoId[localCoreId][coreId] == NULL)
+            goto oom;
+    }
+
+    return CM_OK;
+oom:
+    cm_COM_FreeMpc(coreId);
+    ERROR("CM_NO_MORE_MEMORY: fifo_alloc() failed in cm_COM_AllocateMpc()\n", 0, 0, 0, 0, 0, 0);
+    return CM_NO_MORE_MEMORY;
+}
+
+PUBLIC void cm_COM_InitMpc(t_nmf_core_id coreId)
+{
+    // Here we assume that attribute are in XRAM, thus we don't need memory type
+    t_uint32* toNeighborsComsFifoIdSharedVar[NB_CORE_IDS];
+    t_uint32* fromNeighborsComsFifoIdSharedVar[NB_CORE_IDS];
+
+    t_nmf_core_id localCoreId;
+
+    /*
+     * Initialization of the core identifier of a given Executive Engine
+     * Used into communication scheme so the init is done here, will be moved MAY BE into EE loading module!!!
+     */
+    cm_writeAttribute(cm_EEM_getExecutiveEngine(coreId)->instance, "semaphores/myCoreId", coreId);
+
+    /*
+     * Initialization of the coms fifo with the Host for the given coreId
+     */
+    for (localCoreId = FIRST_MPC_ID/* NOT ARM*/; localCoreId <= LAST_CORE_ID; localCoreId++)
+    {
+        // Note: This loop will also include coreId in order to fill
+        if(cm_DSP_GetState(localCoreId)->state != MPC_STATE_BOOTED) continue;/* no coms fifo initialisation with not booted MPC */
+
+        toNeighborsComsFifoIdSharedVar[localCoreId] = (t_uint32*)cm_getAttributeHostAddr(cm_EEM_getExecutiveEngine(localCoreId)->instance, "comms/toNeighborsComsFifoId");
+
+        fromNeighborsComsFifoIdSharedVar[localCoreId] = (t_uint32*)cm_getAttributeHostAddr(cm_EEM_getExecutiveEngine(localCoreId)->instance, "comms/fromNeighborsComsFifoId");
+    }
+
+    toNeighborsComsFifoIdSharedVar[coreId][ARM_CORE_ID] = mpc2mpcComsFifoId[coreId][ARM_CORE_ID]->dspAdress;
+    fromNeighborsComsFifoIdSharedVar[coreId][ARM_CORE_ID] = mpc2mpcComsFifoId[ARM_CORE_ID][coreId]->dspAdress;
+
+    for (localCoreId = FIRST_MPC_ID/* NOT ARM*/; localCoreId <= LAST_CORE_ID; localCoreId++)
+    {
+        if (localCoreId == coreId) continue; /* no coms fifo with itself ;) */
+        if(cm_DSP_GetState(localCoreId)->state != MPC_STATE_BOOTED) continue;/* no coms fifo initialisation with not booted MPC */
+
+        fromNeighborsComsFifoIdSharedVar[coreId][localCoreId] = mpc2mpcComsFifoId[localCoreId][coreId]->dspAdress;
+        toNeighborsComsFifoIdSharedVar[coreId][localCoreId] = mpc2mpcComsFifoId[coreId][localCoreId]->dspAdress;
+
+        LOG_INTERNAL(1, "ARM: Force Try to wake up on core id : %d\n", localCoreId, 0, 0, 0, 0, 0);
+        cm_EEM_ForceWakeup(localCoreId);
+
+        fromNeighborsComsFifoIdSharedVar[localCoreId][coreId] = mpc2mpcComsFifoId[coreId][localCoreId]->dspAdress;
+        toNeighborsComsFifoIdSharedVar[localCoreId][coreId] = mpc2mpcComsFifoId[localCoreId][coreId]->dspAdress;
+
+        LOG_INTERNAL(1, "ARM: Force Allow sleep on core id : %d\n", localCoreId, 0, 0, 0, 0, 0);
+        cm_EEM_AllowSleep(localCoreId);
+    }
+}
+
+PUBLIC void cm_COM_FreeMpc(t_nmf_core_id coreId)
+{
+    t_nmf_core_id localCoreId;
+
+    for (localCoreId = ARM_CORE_ID; localCoreId < NB_CORE_IDS; localCoreId++)
+    {
+        /*
+         * Free coms fifo from other initialized MPCs to the given one
+         */
+        if ( mpc2mpcComsFifoId[coreId][localCoreId] != NULL)
+        {
+            fifo_free(mpc2mpcComsFifoId[coreId][localCoreId]);
+            mpc2mpcComsFifoId[coreId][localCoreId] = NULL;
+        }
+
+        /*
+         * Free coms fifo from the given MPC to the other initialized ones
+         */
+        if ( mpc2mpcComsFifoId[localCoreId][coreId] != NULL)
+        {
+            fifo_free(mpc2mpcComsFifoId[localCoreId][coreId]);
+            mpc2mpcComsFifoId[localCoreId][coreId] = NULL;
+        }
+    }
+}
+
+PUBLIC t_event_params_handle cm_AllocEvent(t_nmf_fifo_arm_desc *pArmFifo)
+
+{
+    t_uint32 retValue;
+
+    //migration impacts the ARM-side address of the fifoDesc,
+    //thus translate the fifo desc adress systematically.
+    pArmFifo->fifoDesc = (t_nmf_fifo_desc*)cm_migration_translate(pArmFifo->dspAddressInfo.segmentType, (t_shared_addr)pArmFifo->fifoDescShadow);
+
+    retValue = fifo_getAndAckNextElemToWritePointer(pArmFifo);
+
+    return (t_event_params_handle)retValue;
+}
+
+PUBLIC void cm_AcknowledgeEvent(t_nmf_fifo_arm_desc *pArmFifo)
+{
+    fifo_acknowledgeRead(pArmFifo);
+}
+
+PUBLIC t_cm_error cm_PushEventTrace(t_nmf_fifo_arm_desc *pArmFifo, t_event_params_handle h, t_uint32 methodIndex, t_uint32 isTrace)
+{
+    t_uint32 retValue;
+
+    retValue = fifo_getNextElemToWritePointer(mpc2mpcComsFifoId[ARM_CORE_ID][pArmFifo->poperCoreId]);
+
+    if(retValue != 0x0) {
+        t_shared_field *pEvent = (t_shared_field *)retValue;
+
+#ifdef __DEBUG
+        armdspCounter++;
+#endif /* __DEBUG */
+
+        pEvent[EVENT_ELEM_METHOD_IDX]   = (t_shared_addr)methodIndex;
+        pEvent[EVENT_ELEM_PARAM_IDX]    = pArmFifo->dspAdress + (((t_cm_logical_address)h - (t_cm_logical_address)pArmFifo->fifoDesc) >> 1); //note byte to half-word conversion
+        pEvent[EVENT_ELEM_EXTFIELD_IDX] = pArmFifo->fifoDesc->extendedField;
+
+        if (isTrace)
+        {
+            cm_TRC_traceCommunication(
+                    TRACE_COMMUNICATION_COMMAND_SEND,
+                    ARM_CORE_ID,
+                    pArmFifo->poperCoreId);
+        }
+        fifo_coms_acknowledgeWriteAndInterruptGeneration(mpc2mpcComsFifoId[ARM_CORE_ID][pArmFifo->poperCoreId]);
+
+        return CM_OK;
+    }
+
+    ERROR("CM_MPC_NOT_RESPONDING: FIFO COM full '%s'\n", 0, 0, 0, 0, 0, 0);
+    return CM_MPC_NOT_RESPONDING;
+}
+
+PUBLIC t_cm_error cm_PushEvent(t_nmf_fifo_arm_desc *pArmFifo, t_event_params_handle h, t_uint32 methodIndex)
+{
+    return cm_PushEventTrace(pArmFifo,h,methodIndex,1);
+}
+
+static void cmProcessMPCFifo(t_nmf_core_id coreId)
+{
+    t_shared_field *pEvent;
+
+    while((pEvent = (t_shared_field *)fifo_getNextElemToReadPointer(mpc2mpcComsFifoId[coreId][ARM_CORE_ID])) != NULL)
+    {
+        t_event_params_handle pParamsAddr;
+        t_shared_field *pParamsFifoESFDesc;
+
+        pParamsAddr = (t_event_params_handle)cm_DSP_ConvertDspAddressToHostLogicalAddress(
+                coreId,
+                pEvent[EVENT_ELEM_PARAM_IDX]);
+        pParamsFifoESFDesc = (t_shared_field *)pEvent[EVENT_ELEM_EXTFIELD_IDX];
+#ifdef __DEBUG
+        dsparmCounter++;
+#endif /* __DEBUG */
+
+        if(pParamsFifoESFDesc[EXTENDED_FIELD_BCTHIS_OR_TOP] == (t_shared_field)NMF_INTERNAL_USERTHIS)
+        {
+            internalHostJumptable[pEvent[EVENT_ELEM_METHOD_IDX]](coreId, pParamsAddr);
+        }
+        else
+        {
+            cm_TRC_traceCommunication(
+                    TRACE_COMMUNICATION_COMMAND_RECEIVE,
+                    ARM_CORE_ID,
+                    coreId);
+
+            OSAL_PostDfc(
+                    pParamsFifoESFDesc[EXTENDED_FIELD_BCTHIS_OR_TOP],
+                    pEvent[EVENT_ELEM_METHOD_IDX],
+                    pParamsAddr,
+                    pParamsFifoESFDesc[EXTENDED_FIELD_BCDESC]);
+        }
+
+        // [Pwr] mpc2hostComsFifoId value is checked to support the case where
+        //       CM_PostCleanUpAndFlush method is called under interrupt context
+        //       -> mpc2hostComsFifoId can be released.
+        if (mpc2mpcComsFifoId[coreId][ARM_CORE_ID] != NULL)
+            fifo_acknowledgeRead(mpc2mpcComsFifoId[coreId][ARM_CORE_ID]);
+        else
+            break;
+    }
+}
+
+PUBLIC EXPORT_SHARED void CM_ProcessMpcEvent(t_nmf_core_id coreId)
+{
+#ifdef __DEBUG
+    dsparmIrqCounter++;
+#endif /* __DEBUG */
+
+    if (coreId != ARM_CORE_ID)
+    {
+        /* Acknowledge DSP communication interrupt */
+        cm_DSP_AcknowledgeDspIrq(coreId, DSP2ARM_IRQ_0);
+
+        cmProcessMPCFifo(coreId);
+    }
+    else
+    {
+        while((coreId = cm_HSEM_GetCoreIdFromIrqSrc()) <= LAST_MPC_ID)
+            cmProcessMPCFifo(coreId);
+    }
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/bind.h b/drivers/staging/nmf-cm/cm/engine/component/inc/bind.h
new file mode 100644
index 00000000000..325703e3367
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/bind.h
@@ -0,0 +1,443 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ * \brief Binding Factories internal API.
+ *
+ * \defgroup BF_COMMON Binding factories: Common API
+ * \defgroup BF_PRIMITIVE Binding Factories: Primitive API
+ * \defgroup BF_TRACE Binding Factories: Trace API
+ * \defgroup BF_ASYNCHRONOUS Binding Factories: Asynchronous API
+ * \defgroup BF_DISTRIBUTED Binding Factories: Distributed API
+ */
+#ifndef __INC_CM_BIND_H
+#define __INC_CM_BIND_H
+
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/utils/inc/table.h>
+
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * \brief Identification number of prefedined Binding Factories
+ */
+typedef enum {
+    BF_SYNCHRONOUS,     //!< Intra-DSP Synchronous Binding Factory Identifier
+    BF_TRACE,           //!< Intra-DSP trace synchronous Binding Factory Identifier
+    BF_ASYNCHRONOUS, 	//!< Intra-DSP Asynchronous Binding Factory Identifier
+    BF_DSP2HOST,        //!< DSP to Host Binding Factory Identifier
+    BF_HOST2DSP,        //!< Host to DSP Binding Factory Identifier
+    BF_DSP2DSP,         //!< DSP to DSP Binding Factory Identifier
+} t_bf_info_ID;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct _t_interface_reference {
+	const t_component_instance          *instance;      //!< Component instance that provide this interface
+    t_uint8                             provideIndex;   //!< Index of the interface in the provide array
+    t_uint8                             collectionIndex;//!< Index in the collection if provided interface is a collection
+    t_bf_info_ID                        bfInfoID;       //!< Identification of BF used for creating binding
+    void*                               bfInfo;         //!< Storage of the binding factory info
+} t_interface_reference;
+
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * Make some basic sanity check for a client:
+ *  - component stopped
+ *  - Interface really required
+ *
+ * \param[in] client The client component instance handle.
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[out] requiredItf return the required interface (avoid user searching)
+ */
+t_cm_error cm_checkValidClient(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        t_interface_require_description *itfRequire,
+        t_bool *bindable);
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * Make some basic sanity check for a server:
+ *  - Interface really provided
+ *
+ * \param[in] server The server component instance handle.
+ * \param[in] providedItfServerName The server provided interface name
+ * \param[out] itf return the provided interface (avoid user searching)
+ */
+t_cm_error cm_checkValidServer(
+        const t_component_instance* server,
+        const char* providedItfServerName,
+        t_interface_provide_description *itfProvide);
+
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * Make some basic sanity check for a binding:
+ *  - Sanity check for a server
+ *  - Sanity check for a client (and potentially wait initialisation)
+ *  - Provided and required interface matches
+ *
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[in] server The server component instance handle
+ * \param[in] providedItfServerName The server provided interface name
+ * \param[out] requiredItf return the required interface (avoid user searching)
+ * \param[out] itf return the provided interface (avoid user searching)
+ */
+t_cm_error cm_checkValidBinding(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        const t_component_instance* server,
+        const char* requiredItfServerName,
+        t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide,
+        t_bool *bindable);
+
+/**
+ * \internal
+ * \ingroup BF_COMMON
+ *
+ * Make some basic sanity check for each unbinding:
+ *  - Interface really required
+ *  - Component stopped
+ *
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[out] itfRequire return the previously binded required interface (avoid user searching)
+ * \param[out] itfProvide return the previously binded provided interface (avoid user searching)
+ * \param[out] bfInfoID return the binding factory identifiant which done the previously bind
+ * \param[out] bfInfo return the binding factory information which done the previously bind
+ */
+t_cm_error cm_checkValidUnbinding(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide);
+
+/**
+ * \internal
+ * \ingroup BF_PRIMITIVE
+ *
+ * Create a primitive binding between a client to a server interface.
+ *
+ * \param[in] itfRequire The client required interface description
+ * \param[in] itfProvide The server provided interface description
+ */
+t_cm_error cm_bindInterface(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide);
+
+/**
+ * \internal
+ * \ingroup BF_PRIMITIVE
+ *
+ * Unbind a previously binded client.
+ *
+ * \param[in] itfRequire The client required interafce description
+ */
+void cm_unbindInterface(
+        const t_interface_require_description *itfRequire);
+
+/**
+ * \internal
+ * \ingroup BF_PRIMITIVE
+ *
+ * Get a server interface previouly binded to a client
+ *
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[out] itf The server interface
+ */
+t_cm_error cm_lookupInterface(
+        const t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide);
+
+/**
+ * \internal
+ * \ingroup BF_PRIMITIVE
+ *
+ * Create a void binding.
+ *
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ */
+t_cm_error cm_bindInterfaceToVoid(
+        const t_interface_require_description *itfRequire);
+
+/**
+ * \internal
+ * \ingroup BF_TRACE
+ *
+ * Trace synchronous binding factory Information
+ */
+typedef struct {
+    t_component_instance    *traceInstance;   //!< Trace binding component instance
+} t_trace_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_TRACE
+ *
+ * Create a traced binding between a client to a server interface.
+ *
+ * \param[in] itfRequire The client required interface description
+ * \param[in] itfProvide The server provided interface description
+ */
+t_cm_error cm_bindInterfaceTrace(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_elfdescription *elfhandleTrace);
+
+/**
+ * \internal
+ * \ingroup BF_TRACE
+ *
+ * Unbind a previously binded client.
+ *
+ * \param[in] itfRequire The client required interafce description
+ */
+void cm_unbindInterfaceTrace(
+        const t_interface_require_description *itfRequire,
+        t_trace_bf_info                       *bfInfo);
+
+
+/**
+ * \internal
+ * \ingroup BF_ASYNCHRONOUS
+ *
+ * Asynchronous binding factory Information
+ */
+typedef struct {
+    t_component_instance 	*eventInstance;   //!< Event binding component instance
+    t_memory_handle 		dspfifoHandle;    //!< Memory handle of allocated event fifo (pass to the event binding component)
+} t_async_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_ASYNCHRONOUS
+ *
+ * Create a asynchronous binding between a client to a server interface.
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[in] itf The server interface
+ * \param[in] fifosize Number of waited event in the fifo
+ */
+t_cm_error cm_bindInterfaceAsynchronous(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription *elfhandleEvent);
+/**
+ * \internal
+ * \ingroup BF_ASYNCHRONOUS
+ *
+ * Destroy a asynchronous binding between a client to a server interface.
+ * \param[in] itfRequire the required interface
+ */
+void cm_unbindInterfaceAsynchronous(
+        const t_interface_require_description   *itfRequire,
+        t_async_bf_info                         *bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Stub information in distributed binding factory (client side)
+ */
+typedef struct {
+    t_component_instance    *stubInstance;      //!< Stub
+} t_dspstub_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Skeleton information in distributed binding factory (server side)
+ */
+typedef struct {
+    t_component_instance    *skelInstance;      //!< Skeleton binding component instance
+    t_memory_handle         dspfifoHandle;      //!< Memory handle of allocated event fifo (pass to the event binding component)
+} t_dspskel_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Host to DSP distributed binding factory Information
+ */
+typedef struct {
+    t_dspskel_bf_info       dspskeleton;            //!< Information about the DSP skeleton (server side)
+    t_nmf_fifo_arm_desc*    fifo;                   //!< Handle of the fifo params
+    t_nmf_client_id         clientId;               //!< Client ID of the host client
+} t_host2mpc_bf_info;
+
+/*
+ * Table of instantiated of host2mpc bindings
+ */
+extern t_nmf_table Host2MpcBindingTable; /**< list (table) of host2mpc bindings */
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Create a Host to DSP distributed binding between a host client interface to a server interface.
+ * (Not manage in the same way as distributed binding since the Host programming model is not component aware).
+ * \param[in] itfServer The server interface
+ * \param[in] fifosize Number of waited event in the fifo
+ * \param[in] dspEventMemType The type of memory to use
+ * \param[in] bfInfo info structure
+ */
+t_cm_error cm_bindComponentFromCMCore(
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription *elfhandleSkeleton,
+        t_host2mpc_bf_info **bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Destroy a Host to DSP distributed binding between a host client interface to a server interface.
+ * \param[in] bfInfo The Host to DSP distributed binding factory information
+ */
+void cm_unbindComponentFromCMCore(
+        t_host2mpc_bf_info *bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * DSP to Host distributed binding factory Information
+ */
+typedef struct {
+    t_dspstub_bf_info           dspstub;            //!< Information about the DSP stub (client side)
+    t_nmf_fifo_arm_desc*        fifo;               //!< Handle of the fifo params
+    t_uint32                    context;
+} t_mpc2host_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Create a DSP to Host distributed binding between a client interface to a host server interface.
+ * (Not manage in the same way as distributed binding since the Host programming model is not component aware).
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[in] itfref The host server interface to be called
+ * \param[in] fifosize Number of waited event in the fifo
+ */
+t_cm_error cm_bindComponentToCMCore(
+        const t_interface_require_description   *itfRequire,
+        t_uint32                                fifosize,
+        t_uint32                                context,
+        t_elfdescription                        *elfhandleStub,
+        t_mpc2host_bf_info                      ** bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Destroy a DSP to Host distributed binding between a client interface to a server interface.
+ * \param[in] itfRequire The required interface
+ * \param[out] upLayerThis The 'THIS' context of upper layer
+ */
+void cm_unbindComponentToCMCore(
+        const t_interface_require_description   *itfRequire,
+        t_mpc2host_bf_info                      *bfInfo);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Asynchronous distributed binding factory Information
+ */
+typedef struct {
+    t_nmf_fifo_arm_desc*    fifo;                   //!< Handle of the fifo params
+    t_dspstub_bf_info       dspstub;                //!< Information about the DSP stub (client side)
+    t_dspskel_bf_info       dspskeleton;            //!< Information about the DSP skeleton (server side)
+} t_mpc2mpc_bf_info;
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Create a asynchronous distributed binding between a client interface to a server interface.
+ * \param[in] client The client component instance handle
+ * \param[in] requiredItfClientName The client required interface name
+ * \param[in] itf The server interface
+ * \param[in] fifosize Number of waited event in the fifo
+ */
+t_cm_error cm_bindInterfaceDistributed(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription                        *elfhandleSkeleton,
+        t_elfdescription                        *elfhandleStub);
+
+/**
+ * \internal
+ * \ingroup BF_DISTRIBUTED
+ *
+ * Destroy a asynchronous distributed binding between a client interface to a server interface.
+ * \param[in] itfRequire The required interface
+ */
+void cm_unbindInterfaceDistributed(
+        const t_interface_require_description   *itfRequire,
+        t_mpc2mpc_bf_info                       *bfInfo);
+
+/**
+ * \internal
+ *
+ * Bind a static interrupt to server provide interface name.
+ * \param[in] coreId The core to which component is loaded
+ * \param[in] interruptLine Interrupt line number to use
+ * \param[in] server Server instance that provide interrupt service
+ * \param[in] providedItfServerName Interface name hat provide interrupt service
+ */
+t_cm_error cm_bindInterfaceStaticInterrupt(
+        const t_nmf_core_id coreId,
+        const int interruptLine,
+        const t_component_instance *server,
+        const char* providedItfServerName);
+
+/**
+ * \internal
+ *
+ * Unbind a static interrupt.
+ * \param[in] coreId The core to which component is loaded
+ * \param[in] interruptLine Interrupt line number to use
+ */
+t_cm_error cm_unbindInterfaceStaticInterrupt(
+        const t_nmf_core_id coreId,
+        const int interruptLine);
+
+void cm_destroyRequireInterface(t_component_instance* component, t_nmf_client_id clientId);
+void cm_registerSingletonBinding(
+        t_component_instance*                   component,
+        t_interface_require_description*        itfRequire,
+        t_interface_provide_description*        itfProvide,
+        t_nmf_client_id                         clientId);
+t_bool cm_unregisterSingletonBinding(
+        t_component_instance*                   component,
+        t_interface_require_description*        itfRequire,
+        t_interface_provide_description*        itfProvide,
+        t_nmf_client_id                         clientId);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/component_type.h b/drivers/staging/nmf-cm/cm/engine/component/inc/component_type.h
new file mode 100644
index 00000000000..2e769ec8b22
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/component_type.h
@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Components Component Manager API type.
+ *
+ * \defgroup COMPONENT CM Components API
+ * \ingroup CM_USER_API
+ */
+
+#ifndef COMPONENT_TYPE_H_
+#define COMPONENT_TYPE_H_
+
+#include <cm/inc/cm_type.h>
+#include <nmf/inc/component_type.h>
+
+/*!
+ * @defgroup t_nmf_ee_priority t_nmf_ee_priority
+ * \brief Identification of the execution engine priority and sub priority.
+ * @{
+ * \ingroup COMPONENT
+ */
+typedef t_uint32 t_nmf_ee_priority;                                      //!< Fake enumeration type
+
+#define NMF_SCHED_BACKGROUND                    ((t_nmf_ee_priority)0)  //!< Background priority
+#define NMF_SCHED_NORMAL                        ((t_nmf_ee_priority)1)  //!< Normal priority
+#define NMF_SCHED_URGENT                        ((t_nmf_ee_priority)2)  //!< Urgent priority
+/* @} */
+
+
+/*!
+ * \brief Identification of host component returned during introspection
+ *
+ * \ingroup COMPONENT_INTROSPECTION
+ */
+#define NMF_HOST_COMPONENT		((t_cm_instance_handle)0xFFFFFFFF)
+
+/*!
+ * \brief Identification of void component returned during introspection
+ *
+ * \ingroup COMPONENT_INTROSPECTION
+ */
+#define NMF_VOID_COMPONENT      ((t_cm_instance_handle)0xFFFFFFFE)
+
+
+/*!
+ * @defgroup t_nmf_ee_priority t_nmf_ee_priority
+ * \brief Identification of the execution engine priority and sub priority.
+ * @{
+ * \ingroup COMPONENT
+ */
+typedef t_uint8 t_cm_require_state;                                     //!< Fake enumeration type
+
+#define CM_REQUIRE_STATIC                    ((t_cm_require_state)0)  //!< Required interface is static
+#define CM_REQUIRE_OPTIONAL                  ((t_cm_require_state)1)  //!< Required interface is optional
+#define CM_REQUIRE_COLLECTION                ((t_cm_require_state)2)  //!< Required interface is a collection
+
+/* @} */
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/description.h b/drivers/staging/nmf-cm/cm/engine/component/inc/description.h
new file mode 100644
index 00000000000..882dc1ea873
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/description.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#ifndef __INC_CM_COMPONENT_DESCRIPTION_H
+#define __INC_CM_COMPONENT_DESCRIPTION_H
+
+#include <cm/engine/elf/inc/memory.h>
+#include <cm/engine/utils/inc/string.h>
+
+#include <inc/nmf-limits.h>
+
+/*!
+ * \internal
+ * \brief Description of an interface
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct _t_interface_description {
+    t_dup_char              type;                  //!< Type of the interface
+    t_uint16                referenceCounter;       //!< Number of template referencing the interface
+    t_uint8                 methodNumber;           //!< Number of method in the interfaces
+    struct _t_interface_description* next;
+    t_dup_char              methodNames[1];        //!< Array of method names
+} t_interface_description;
+
+/*!
+ * \internal
+ * \brief Description of a variable memory on a collection index
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_uint32            offset;            //!< Offset in the memory
+    const t_elfmemory   *memory;          //!< Memory
+} t_memory_reference;
+
+/*!
+ * \internal
+ * \brief Description of a required interface on a collection index
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_uint32                numberOfClient;                //!< Number of interface descriptor really connected to this interface
+    t_memory_reference      *memories;                   /*!< Memory where each interface reference descriptor resides
+                                                                                \note memories[numberOfClient] */
+} t_interface_require_index;
+
+/*!
+ * \internal
+ * \brief Description of a required interface
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_dup_char                      name;                                //!< Name of the interface
+    t_interface_description         *interface;                           //!< Description of the interface
+    t_uint8                         requireTypes;                         //!< Mask of t_elf_interface_require_type
+    t_uint8                         collectionSize;                       //!< Size of the collection (1 if not a collection)
+    t_interface_require_index       *indexes;         /*!< Require information for each collection index
+                                                                \note indexes[collectionSize] */
+} t_interface_require;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface method on a collection index
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_memory_reference     memory;                      //!< Memory of the method
+} t_interface_provide_index;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_dup_char                     name;                                 //!< Name of the interface
+    t_interface_description        *interface;                            //!< Description of the interface
+    t_uint8                         provideTypes;                         //!< Mask of t_elf_interface_provide_type
+    t_uint8                        interruptLine;                         //!< Interrupt line if interrupt (0 if not)
+    t_uint8                        collectionSize;                        //!< Size of the collection (1 if not a collection)
+    t_interface_provide_index      **indexes;                         //!< Provide information for each collection index
+} t_interface_provide;
+
+/*!
+ * \internal
+ * \brief Description of a attribute
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_dup_char              name;                             //!< Name of the attribute
+    t_memory_reference      memory;                            //!< Memory where the attribute reside
+} t_attribute;
+
+/*!
+ * \internal
+ * \brief Description of a property
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_dup_char          name;           //!< Name of this attribute
+    t_dup_char          value;          //!< String of the value
+} t_property;
+
+
+
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/dspevent.h b/drivers/staging/nmf-cm/cm/engine/component/inc/dspevent.h
new file mode 100644
index 00000000000..bb47363c0ae
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/dspevent.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_DSP_EVENT
+#define __INC_DSP_EVENT
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/memory/inc/memory.h>
+
+/* value should be size of t_remote_event in mmdsp word */
+#define DSP_REMOTE_EVENT_SIZE_IN_DSPWORD 5
+
+t_cm_error dspevent_createDspEventFifo(
+    const t_component_instance *pComp,
+    const char* nameOfTOP,
+    t_uint32 fifoNbElem,
+    t_uint32 fifoElemSizeInWord,
+    t_dsp_memory_type_id dspEventMemType,
+    t_memory_handle *pHandle);
+void dspevent_destroyDspEventFifo(t_memory_handle handle);
+
+#endif /* __INC_DSP_EVENT */
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/initializer.h b/drivers/staging/nmf-cm/cm/engine/component/inc/initializer.h
new file mode 100644
index 00000000000..5ac9ec453b7
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/initializer.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_NMF_INITIALIZER
+#define __INC_NMF_INITIALIZER
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/instance.h>
+#include <share/communication/inc/initializer.h>
+
+PUBLIC t_cm_error cm_COMP_INIT_Init(t_nmf_core_id coreId);
+PUBLIC t_cm_error cm_COMP_CallService(int serviceIndex, t_component_instance *pComp, t_uint32 methodAddress);
+PUBLIC void cm_COMP_Flush(t_nmf_core_id coreId);
+PUBLIC void cm_COMP_INIT_Close(t_nmf_core_id coreId);
+PUBLIC t_cm_error cm_COMP_UpdateStack(t_nmf_core_id coreId, t_uint32 stackSize);
+PUBLIC t_cm_error cm_COMP_ULPForceWakeup(t_nmf_core_id coreId);
+PUBLIC t_cm_error cm_COMP_ULPAllowSleep(t_nmf_core_id coreId);
+PUBLIC t_cm_error cm_COMP_InstructionCacheLock(t_nmf_core_id coreId, t_uint32 mmdspAddr, t_uint32 mmdspSize);
+PUBLIC t_cm_error cm_COMP_InstructionCacheUnlock(t_nmf_core_id coreId, t_uint32 mmdspAddr, t_uint32 mmdspSize);
+
+
+PUBLIC void processAsyncAcknowledge(t_nmf_core_id coreId, t_event_params_handle pParam);
+PUBLIC void processSyncAcknowledge(t_nmf_core_id coreId, t_event_params_handle pParam);
+
+#endif /* __INC_NMF_INITIALIZER */
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/instance.h b/drivers/staging/nmf-cm/cm/engine/component/inc/instance.h
new file mode 100644
index 00000000000..0a7d80e2e02
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/instance.h
@@ -0,0 +1,222 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Components Management internal methods - Instance API.
+ *
+ */
+#ifndef __INC_CM_INSTANCE_H
+#define __INC_CM_INSTANCE_H
+
+#include <cm/engine/component/inc/template.h>
+#include <cm/engine/repository_mgt/inc/repository_mgt.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/utils/inc/table.h>
+#include <cm/engine/utils/inc/string.h>
+
+/*----------------------------------------------------------------------------
+ * Component Instance API.
+ *----------------------------------------------------------------------------*/
+struct _t_interface_reference;
+
+/*!
+ * \internal
+ * \brief Component life cycle state
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef enum {
+    STATE_NONE,
+    STATE_STOPPED,
+    STATE_RUNNABLE,
+    // STATE_DESTROYED identified when component remove from component list
+} t_component_state;
+
+struct t_client_of_singleton
+{
+    struct t_client_of_singleton    *next;
+    t_nmf_client_id                 clientId;
+    t_uint16                        numberOfInstance;
+    t_uint16                        numberOfStart;
+    t_uint16                        numberOfBind;
+};
+
+/*!
+ * \internal
+ * \brief Description of a component instance
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct t_component_instance {
+    t_dup_char              pathname;                         //!< Path Name of this component in the components architecture
+
+    t_component_state       state;                            //!< Component state
+    t_nmf_ee_priority       priority;                         //!< Executive engine component priority
+    t_component_template   *Template;                         //!< Component template
+
+    t_uint32                thisAddress;                      //!< Cached value of cm_DSP_GetDspAddress(component->memories[data], &thisAddress);
+
+    t_memory_handle         memories[NUMBER_OF_MMDSP_MEMORY];  //!<Reference in different memory where datas are (YES, we fix implementation to MMDSP)
+
+    struct _t_interface_reference **interfaceReferences;    /*!< Interface references
+                                      (Share same index as template->u.p.requires)
+                                      type == targets[interface_index][collection_index] */
+
+    t_uint16                providedItfUsedCount;             //!< Use count to reference the number of components binded to this once, ie count the number of provided interfaces in use
+    t_cm_instance_handle    instance;                         //!< index of this component within the ComponentTable
+    t_cm_domain_id          domainId;                         //!< Domain where the component has been installed
+
+    struct t_client_of_singleton    *clientOfSingleton;       //!< Client of singleton list
+    t_memory_handle         loadMapHandle;       // handle of allocated memory for the loadMap structure and name;
+    void *dbgCooky;                                           //!< pointer to OS internal data
+} t_component_instance;
+
+t_component_template* cm_lookupTemplate(t_nmf_core_id dspId, t_dup_char str);
+
+/*!
+ * \internal
+ * \brief Load a component template.
+ *
+ * ...
+ *
+ * \param[in] templateName name of the template to load
+ * \param[in] coreId DSP where template must be loaded
+ * \praem[in] pRepComponent Pointer to the component entry stored in the Component Cache Repository
+ * \param[in, out] template reference to put the loaded template (null if first instance)
+ *
+ * \exception CM_COMPONENT_NOT_FOUND
+ * \exception CM_NO_MORE_MEMORY
+ *
+ * \return exception number.
+ *
+ * \warning For Component manager use only.
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_loadComponent(
+        t_dup_char templateName,
+        t_cm_domain_id domainId,
+        t_elfdescription* elfhandle,
+        t_component_template **reftemplate);
+
+/*!
+ * \internal
+ * \brief Unload a component template.
+ *
+ * ...
+ *
+ * \param[in] template template to be unloaded
+ * \praem[in] Private memories that has been created from component binary file
+ *
+ * \return exception number.
+ *
+ * \warning For Component manager use only.
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_unloadComponent(
+        t_component_template *reftemplate);
+
+/*!
+ * \internal
+ * \brief Instantiate a component.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_instantiateComponent(const char* templateName,
+        t_cm_domain_id domainId,
+        t_nmf_ee_priority priority,
+        const char* pathName,
+        t_elfdescription *elfhandle,
+        t_component_instance** refcomponent);
+
+struct t_client_of_singleton* cm_getClientOfSingleton(t_component_instance* component, t_bool createdIfNotExist, t_nmf_client_id clientId);
+
+/*!
+ * \internal
+ * \brief Start a component.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_startComponent(t_component_instance* component, t_nmf_client_id clientId);
+
+/*!
+ * \internal
+ * \brief Stop a component.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_stopComponent(t_component_instance* component, t_nmf_client_id clientId);
+
+/*!
+ * \internal
+ */
+typedef enum {
+    DESTROY_NORMAL,
+    DESTROY_WITHOUT_CHECK,
+    DESTROY_WITHOUT_CHECK_CALL
+} t_destroy_state;
+
+/*!
+ * \internal
+ * \brief Destroy a component instance.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_destroyInstance(t_component_instance* component, t_destroy_state forceDestroy);
+
+/*!
+ * \internal
+ * \brief Destroy a component instance.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_destroyInstanceForClient(t_component_instance* component, t_destroy_state forceDestroy, t_nmf_client_id clientId);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+void cm_delayedDestroyComponent(t_component_instance *component);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_component_instance *cm_lookupComponent(const t_cm_instance_handle hdl);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_bool cm_isComponentOnCoreId(t_nmf_core_id coreId);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_COMP_Init(void);
+
+/*!
+ * \internal
+ * \brief
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+void cm_COMP_Destroy(void);
+
+/*
+ * Table of instantiated components.
+ */
+extern t_nmf_table ComponentTable; /**< list (table) of components */
+#define componentEntry(i) ((t_component_instance *)ComponentTable.entries[i])
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/introspection.h b/drivers/staging/nmf-cm/cm/engine/component/inc/introspection.h
new file mode 100644
index 00000000000..cfb55c91779
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/introspection.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Components Management internal methods - Introspection.
+ *
+ */
+#ifndef __INC_CM_INTROSPECTION_H
+#define __INC_CM_INTROSPECTION_H
+
+#include <cm/engine/component/inc/instance.h>
+
+/*!
+ * \internal
+ * \brief Description of a required interface reference
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    const t_component_instance          *client;            //!< Component that provide this interface
+    t_uint8                             requireIndex;       //!< Index of the interface in the require array
+    t_uint8                             collectionIndex;    //!< Index in the collection if required interface is a collection
+    const char*                         origName;           //!< Name of the component interface
+} t_interface_require_description;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    const t_component_instance          *server;            //!< Component that provide this interface
+    t_uint8                             provideIndex;       //!< Index of the interface in the provide array
+    t_uint8                             collectionIndex;    //!< Index in the collection if provided interface is a collection
+    const char*                         origName;           //!< Name of the component interface
+} t_interface_provide_description;
+
+
+/*!
+ * \internal
+ * \brief Get property of a component.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_getComponentProperty(
+	const t_component_instance *component,
+	const char                 *propName,
+	char                       value[MAX_PROPERTY_VALUE_LENGTH],
+    t_uint32                   valueLength);
+
+
+t_dsp_address cm_getAttributeMpcAddress(
+        const t_component_instance  *component,
+        const char                  *attrName);
+
+t_cm_logical_address cm_getAttributeHostAddr(
+        const t_component_instance  *component,
+        const char                  *attrName);
+
+t_uint32 cm_readAttributeNoError(
+        const t_component_instance  *component,
+                const char                  *attrName);
+
+t_cm_error cm_readAttribute(
+        const t_component_instance  *component,
+        const char                  *attrName,
+        t_uint32                    *value);
+
+t_cm_error cm_writeAttribute(
+        const t_component_instance  *component,
+        const char                  *attrName,
+        t_uint32                    value);
+
+/*!
+ * \internal
+ * \brief Get internal component symbol
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_dsp_address cm_getFunction(
+        const t_component_instance* component,
+        const char* interfaceName,
+        const char* methodName);
+
+/*!
+ * \internal
+ * \brief Get interface provided by a component instance.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_getProvidedInterface(const t_component_instance* server,
+    const char* itfName,
+    t_interface_provide_description *itfProvide);
+
+/*!
+ * \internal
+ * \brief Get interface required by a component instance.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+t_cm_error cm_getRequiredInterface(const t_component_instance* server,
+    const char* itfName,
+    t_interface_require_description *itfRequire);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/nmfheaderabi.h b/drivers/staging/nmf-cm/cm/engine/component/inc/nmfheaderabi.h
new file mode 100644
index 00000000000..9eae19b2f70
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/nmfheaderabi.h
@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief NMF component description ABI
+ *
+ * \defgroup NMF_HEADER NMF Component Description ABI
+ * The NMF component description ABI is stored in the nmf_segment in the ELF component file.
+ * The NMF component description section start by the t_elf_component_header structure.
+ *
+ * \warning <B>The format of this section is not fixed and is able to be changed without concerting.</B>
+ * \note You can use the nmfHeaderVersion to check if the format has changed.
+ * \note Each pointers in this section is relative to the beginning of the section and must be relocated before used.
+ * \ingroup NMF_ABI
+ */
+#ifndef __INC_CM_NMF_HEADERABI_H
+#define __INC_CM_NMF_HEADERABI_H
+
+#include <cm/inc/cm_type.h>
+
+/*!
+ * \brief Description of a interface
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+    char                    *type;                  //!< Type of this Interface
+    t_uint8                 methodNumber;           //!< Number of method in the interfaces
+    t_uint8                 reserved1, reserved2, reserved3;
+    char                    *methodNames[1];        //!< Array of method names [methodNumber]
+} t_elf_interface_description;
+
+/*!
+ * \brief Description of required interface type (value could be combinated)
+ * \ingroup NMF_HEADER
+ */
+typedef enum {
+    COLLECTION_REQUIRE = 1,                     //!< Required interface is a collection
+    OPTIONAL_REQUIRE = 2,                       //!< Required interface if optional
+    STATIC_REQUIRE = 4,                         //!< Required interface is static
+    VIRTUAL_REQUIRE = 8,                        //!< Required interface is virtual (only for introspection purpose)
+    INTRINSEC_REQUIRE = 16                      //!< Required interface is intrinsec (bind automatically done by runtime)
+} t_elf_interface_require_type;
+
+/*!
+ * \brief Description of a required interface on a collection index
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+    t_uint32     numberOfClient;                //!< Number of interface descriptor really connected to this interface
+    t_uint32     symbols[1];          /*!< Symbol of the real name of the attribute
+                                        \note Real type symbols[numberOfClient]
+                                        \note Use relocation in order to get symbol information */
+} t_elf_interface_require_index;
+
+/*!
+ * \brief Description of an interface required
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	char        *name;		                            //!< name of the interface: offset in string segment
+    t_uint8     requireTypes;                           //!< Mask of t_elf_interface_require_type
+    t_uint8     collectionSize;                         //!< Size of the collection (1 if not a collection)
+    t_uint8     reserved1, reserved2;
+    t_elf_interface_description     *interface;         //!< Interface description
+    t_elf_interface_require_index   indexes[1];           /*!< Require information for each collection index
+                                                                        \note Real type: indexes[collectionSize],
+                                                                        available only if not static interface */
+}  t_elf_required_interface;
+
+/*!
+ * \brief Description of provided interface type (value could be combinated)
+ * \ingroup NMF_HEADER
+ */
+typedef enum {
+    COLLECTION_PROVIDE = 1,                               //!< Provided interface is a collection
+    VIRTUAL_PROVIDE = 2                         //!< Provided interface is virtual (only for introspection purpose)
+} t_elf_interface_provide_type;
+
+/*!
+ * \brief Description of an interface provided
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	char* 			                name;		            //!< name of the interface: offset in string segment
+    t_uint8                         provideTypes;           //!< Mask of t_elf_interface_provide_type
+    t_uint8                         interruptLine;          //!< Interrupt line if interrupt (0 if not)
+    t_uint8                         collectionSize;         //!< Size of the collection (1 if not a collection)
+    t_uint8                         reserved1;
+	t_elf_interface_description     *interface;             //!< Interface description
+    t_uint32                        methodSymbols[1];        /*!< Symbol of the real name of methods of the interface for each collection index
+                                                                    \note Real type: methodSymbols[collectionSize][methodNumber]
+                                                                    \note Use relocation in order to get symbol information*/
+} t_elf_provided_interface;
+
+/*!
+ * \brief Description of an attribute
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	char* 	    name;			//!< Name of this attribute
+    t_uint32    symbols;          /*!< Symbol of the real name of the attribute
+                                        \note Use relocation in order to get symbol information */
+} t_elf_attribute;
+
+/*!
+ * \brief Description of an property
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	char* 	name;			//!< Name of this attribute
+	char*	value;			//!< String of the value
+} t_elf_property;
+
+#define MAGIC_COMPONENT 0x123    //!< Magic Number for a component \ingroup NMF_HEADER
+#define MAGIC_SINGLETON 0x321    //!< Magic Number for a singleton component \ingroup NMF_HEADER
+#define MAGIC_FIRMWARE 0x456      //!< Magic Number for Execution Engine Component \ingroup NMF_HEADER
+
+/*!
+ * \brief Description of a ELF component header
+ *
+ * The NMF component description section start by this structure.
+ *
+ * \ingroup NMF_HEADER
+ */
+typedef struct {
+	t_uint32                    magic;		    //!< Magic Number
+    t_uint32                    nmfVersion;     //!< Version of the NMF Header
+
+    char*                       templateName;   //!< Name of the component template
+
+    t_uint32                    LCCConstruct;   //!< Life cycle Constructor offset
+    t_uint32                    LCCStart;       //!< Life cycle Starter offset
+    t_uint32                    LCCStop;        //!< Life cycle Stopper offset
+    t_uint32                    LCCDestroy;     //!< Life cycle Destructer offset
+
+    t_uint32                    minStackSize;   //!< Minimum stack size
+
+    t_uint32                    attributeNumber;//!< Number of attributes
+    t_elf_attribute             *attributes;    //!< Array of attributes (be careful, this reference must be relocated before use)
+
+    t_uint32                    propertyNumber; //!< Number of properties
+    t_elf_property              *properties;    //!< Array of properties (be careful, this reference must be relocated before use)
+
+    t_uint32                    provideNumber;  //!< Number of interfaces provided
+    t_elf_provided_interface    *provides;      //!< Array of interfaces provided (be careful, this reference must be relocated before use)
+
+    t_uint32                    requireNumber;     //!< Array of interfaces required
+    t_elf_required_interface    *requires;      //!< Array of interfaces required (be careful, this reference must be relocated before use)
+
+} t_elf_component_header;
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/inc/template.h b/drivers/staging/nmf-cm/cm/engine/component/inc/template.h
new file mode 100644
index 00000000000..2718d8ae9fb
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/inc/template.h
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Components Management internal methods - Template API.
+ *
+ * \defgroup COMPONENT_INTERNAL Private component instances API
+ */
+#ifndef __INC_CM_TEMPLATE_H
+#define __INC_CM_TEMPLATE_H
+
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/description.h>
+#include <cm/engine/elf/inc/elfapi.h>
+#include <cm/engine/utils/inc/string.h>
+
+
+/*!
+ * \internal
+ * \brief Class of a component
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef enum {
+	COMPONENT, 			//!< Primitive component
+	SINGLETON,          //!< Singleton component
+	FIRMWARE,			//!< Firmware composite component
+} t_component_classe;
+
+/*!
+ * \internal
+ * \brief Description of delayed relocation
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct _t_function_relocation {
+    t_dup_char                      symbol_name;
+    t_uint32                        type;
+    char                            *reloc_addr;
+    struct _t_function_relocation   *next;
+} t_function_relocation;
+
+struct t_component_instance;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface method on a collection index ; Available only when template loaded
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_uint32               methodAddresses;             //!< Address of each method
+} t_interface_provide_index_loaded;
+
+/*!
+ * \internal
+ * \brief Description of a provided interface ; Available only when template loaded
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct {
+    t_interface_provide_index_loaded    **indexesLoaded;                  //!< Provide information for each collection index
+} t_interface_provide_loaded;
+
+
+/*!
+ * \internal
+ * \brief Description of a component template
+ * \ingroup COMPONENT_INTERNAL
+ */
+typedef struct _t_component_template {
+    t_dup_char                  name;                   //!< Template name (a.k.a component type)
+
+    t_component_classe          classe;                 //!< Class of the component
+    //TODO, juraj, remove dspId
+    t_nmf_core_id               dspId;                  //!< Reference on DSP where template is loaded
+
+    t_uint8                     numberOfInstance;       //!< Number of same instance (or singleton copy) create from this template
+
+    t_uint8                     propertyNumber;         //!< Number of properties in this template
+    t_uint8                     attributeNumber;        //!< Number of attributes in this template
+    t_uint8                     provideNumber;          //!< Number of interface provided by this template
+    t_uint8                     requireNumber;          //!< Number of interface required by this template
+
+    t_uint32                    LCCConstructAddress;    //!< Life cycle Constructor address
+    t_uint32                    LCCStartAddress;        //!< Life cycle Starter address
+    t_uint32                    LCCStopAddress;         //!< Life cycle Stopper address
+    t_uint32                    LCCDestroyAddress;      //!< Life cycle Destructer address
+
+    t_uint32                    minStackSize;           //!< Minimum stack size
+
+    t_memory_handle             memories[NUMBER_OF_MMDSP_MEMORY];   //!< Reference in different memory where datas are (YES, we fix implementation to MMDSP)
+    const t_elfmemory           *thisMemory;            //!< Memory used to determine this
+    const t_elfmemory           *codeMemory;            //!< Memory used to determine code
+
+    t_function_relocation       *delayedRelocation;     //!< List of reference that can't been relocatable while appropritae binding done.
+
+    t_property	        *properties;            //!< Array of properties in this template
+    t_attribute         *attributes;            //!< Array of attributes in this template
+    t_interface_provide *provides;              //!< Array of interface provided by this template
+    t_interface_require *requires;              //!< Array of interface required by this template
+
+    t_interface_provide_loaded  *providesLoaded;    //!< Array of interface provided by this template ; Available when loaded
+
+    t_bool                      descriptionAssociatedWithTemplate;
+
+    struct _t_component_template *prev, *next;
+    struct t_component_instance  *singletonIfAvaliable;
+} t_component_template;
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/binder.c b/drivers/staging/nmf-cm/cm/engine/component/src/binder.c
new file mode 100644
index 00000000000..5f08713833b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/binder.c
@@ -0,0 +1,1313 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include "../inc/bind.h"
+#include "../inc/dspevent.h"
+#include <cm/engine/communication/fifo/inc/nmf_fifo_arm.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/component/inc/introspection.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+#include <cm/engine/utils/inc/string.h>
+
+#define CM_IT_NAME_MAX_LENGTH               8
+
+t_nmf_table Host2MpcBindingTable; /**< list (table) of host2mpc bindings */
+
+static void cm_fillItName(int interruptLine, char *itName);
+static t_uint16 getNumberOfBind(t_component_instance* component);
+
+/*
+ * Bind virtual interface, here we assume that:
+ * - client component require this interface as last one and without collection,
+ * - server component provide only this interface and without collection.
+ * Fixed in loader.c.
+ */
+static void cm_bindVirtualInterface(
+        t_component_instance* client,
+        const t_component_instance* server) {
+    t_interface_require_description itfRequire;
+
+    if(cm_getRequiredInterface(client, "coms", &itfRequire) == CM_OK)
+    {
+        t_interface_reference* itfRef = client->interfaceReferences[itfRequire.requireIndex];
+
+        /*
+         * Memorise this reference
+         */
+        itfRef->provideIndex = 0;
+        itfRef->collectionIndex = 0;
+        itfRef->instance = server;
+        itfRef->bfInfoID = (t_bf_info_ID)0;
+        itfRef->bfInfo = (void*)-1; // TODO
+    }
+    else
+    {
+        ERROR("Internal Error in cm_bindVirtualInterface\n", 0, 0, 0, 0, 0, 0);
+    }
+}
+
+static void cm_unbindVirtualInterface(
+        t_component_instance* client) {
+    t_interface_require_description itfRequire;
+
+    if(cm_getRequiredInterface(client, "coms", &itfRequire) == CM_OK)
+    {
+        t_interface_reference* itfRef = client->interfaceReferences[itfRequire.requireIndex];
+        itfRef->instance = NULL;
+    }
+    else
+    {
+        ERROR("Internal Error in cm_unbindVirtualInterface\n", 0, 0, 0, 0, 0, 0);
+    }
+}
+
+/*
+ * Bind component
+ */
+static void cm_bindLowLevelInterface(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfLocalBC,	         /* On the same DSP */
+        t_bf_info_ID bfInfoID, void* bfInfo)
+{
+    const t_component_instance* client = itfRequire->client;
+    t_component_instance* server = (t_component_instance*)itfLocalBC->server;
+    t_interface_require *require = &client->Template->requires[itfRequire->requireIndex];
+    t_interface_provide* provide = &server->Template->provides[itfLocalBC->provideIndex];
+    t_interface_provide_loaded* provideLoaded = &server->Template->providesLoaded[itfLocalBC->provideIndex];
+    int k, j;
+
+    if(require->indexes != NULL)
+    {
+        t_interface_require_index *requireindex = &require->indexes[itfRequire->collectionIndex];
+
+        for(k = 0; k < requireindex->numberOfClient; k++) {
+            t_uint32 *hostAddr;
+
+            hostAddr = (t_uint32*)(
+                    cm_DSP_GetHostLogicalAddress(client->memories[requireindex->memories[k].memory->id]) +
+                    requireindex->memories[k].offset * requireindex->memories[k].memory->memEntSize);
+
+            LOG_INTERNAL(2, "Fill ItfRef %s.%s mem=%s Off=%x @=%x\n",
+                    client->pathname, require->name,
+                    requireindex->memories[k].memory->memoryName,
+                    requireindex->memories[k].offset,
+                    hostAddr, 0);
+
+            /*
+             * Fill the interface references. We start by This then methods in order to keep
+             * Unbinded panic as long as possible and not used method with wrong This. This is
+             * relevent only for optional since we must go in stop state before rebinding other
+             * required interface.
+             *
+             * Direct write to DSP memory without go through DSP abstraction since we know we are in 24bits
+             */
+            // Write THIS reference into the Data field of the interface reference
+            // Write the interface methods reference
+
+            if(((t_uint32)hostAddr & 0x7) == 0 && require->interface->methodNumber > 0)
+            {
+                // We are 64word byte aligned, combine this write with first method
+                *(volatile t_uint64*)hostAddr =
+                        ((t_uint64)server->thisAddress << 0) |
+                        ((t_uint64)provideLoaded->indexesLoaded[itfLocalBC->collectionIndex][0].methodAddresses << 32);
+                hostAddr += 2;
+                j = 1;
+            }
+            else
+            {
+                // We are not, write this which will align us
+                *hostAddr++ = (t_uint32)server->thisAddress;
+                j = 0;
+            }
+
+            // Word align copy
+            for(; j < require->interface->methodNumber - 1; j+=2) {
+                *(volatile t_uint64*)hostAddr =
+                        ((t_uint64)provideLoaded->indexesLoaded[itfLocalBC->collectionIndex][j].methodAddresses << 0) |
+                        ((t_uint64)provideLoaded->indexesLoaded[itfLocalBC->collectionIndex][j+1].methodAddresses << 32);
+                hostAddr += 2;
+            }
+
+            // Last word align if required
+            if(j < require->interface->methodNumber)
+                *hostAddr = provideLoaded->indexesLoaded[itfLocalBC->collectionIndex][j].methodAddresses;
+        }
+    }
+    else
+    {
+        t_function_relocation *reloc = client->Template->delayedRelocation;
+        while(reloc != NULL) {
+            for(j = 0; j < provide->interface->methodNumber; j++)
+            {
+                if(provide->interface->methodNames[j] == reloc->symbol_name) {
+                    cm_ELF_performRelocation(
+                                        reloc->type,
+                                        reloc->symbol_name,
+                                        provideLoaded->indexesLoaded[itfLocalBC->collectionIndex][j].methodAddresses,
+                                        reloc->reloc_addr);
+                    break;
+                }
+            }
+
+            reloc = reloc -> next;
+        }
+    }
+
+    /*
+     * Memorise this reference
+     */
+    {
+        t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+        itfRef->provideIndex = itfLocalBC->provideIndex;
+        itfRef->collectionIndex = itfLocalBC->collectionIndex;
+        itfRef->instance = itfLocalBC->server;
+        itfRef->bfInfoID = bfInfoID;
+        itfRef->bfInfo = bfInfo;
+
+        /*
+         * Do not count binding from EE (ie interrupt line), as this will prevent
+         * cm_destroyInstance() of server to succeed (interrupt line bindings are
+         * destroyed after the check in cm_destroyInstance()
+         */
+        if (client->Template->classe != FIRMWARE)
+            server->providedItfUsedCount++;
+    }
+}
+
+static void cm_registerLowLevelInterfaceToConst(
+    const t_interface_require_description *itfRequire,
+    const t_component_instance* targetInstance)
+{
+    const t_component_instance* client = itfRequire->client;
+
+    /*
+     * Memorise this no reference
+     */
+    {
+        t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+        // This is an unbind from a true component (not to void)
+        // Do not count bindings from EE (ie interrupt line)
+        if ((targetInstance == NULL)
+                && (client->Template->classe != FIRMWARE)
+                && (itfRef->instance != (t_component_instance *)NMF_VOID_COMPONENT)
+                && (itfRef->instance != NULL))
+        {
+            ((t_component_instance*)itfRef->instance)->providedItfUsedCount--;
+        }
+
+        itfRef->instance = targetInstance;
+        itfRef->bfInfoID = BF_SYNCHRONOUS; // Just to memorize no Binding component used and unbind ToVoid happy ;-).
+    }
+}
+
+static void cm_bindLowLevelInterfaceToConst(
+        const t_interface_require_description *itfRequire,
+        const t_dsp_address functionAddress,
+        const t_component_instance* targetInstance) {
+    const t_component_instance* client = itfRequire->client;
+    t_interface_require *require = &client->Template->requires[itfRequire->requireIndex];
+    int j, k;
+
+
+    // If DSP is off/panic/... -> write nothing
+    if(
+            require->indexes != NULL
+            && cm_DSP_GetState(client->Template->dspId)->state == MPC_STATE_BOOTED)
+    {
+        t_interface_require_index *requireindex = &require->indexes[itfRequire->collectionIndex];
+
+        for(k = 0; k < requireindex->numberOfClient; k++) {
+            t_uint32 *hostAddr;
+
+            hostAddr = (t_uint32*)(
+                    cm_DSP_GetHostLogicalAddress(client->memories[requireindex->memories[k].memory->id]) +
+                    requireindex->memories[k].offset * requireindex->memories[k].memory->memEntSize);
+
+            /*
+             * Fill the interface references. We start by Methods then This in order to swith to
+             * Unbinded panic as fast as possible and not used method with wrong This. This is
+             * relevent only for optional since we must go in stop state before rebinding other
+             * required interface.
+             *
+             * Direct write to DSP memory without go through DSP abstraction since we know we are in 24bits
+             */
+            /*
+             * Write THIS reference into the Data field of the interface reference
+             * Hack for simplifying debug just to keep THIS reference with caller one
+             * (could be removed if __return_address MMDSP intrinsec provided by compiler).
+             */
+            // Write the interface methods reference
+
+            if(((t_uint32)hostAddr & 0x7) == 0 && require->interface->methodNumber > 0)
+            {
+                // We are 64word byte aligned, combine this write with first method
+                *(volatile t_uint64*)hostAddr =
+                        ((t_uint64)client->thisAddress << 0) |
+                        ((t_uint64)functionAddress << 32);
+                hostAddr += 2;
+                j = 1;
+            }
+            else
+            {
+                // We are not, write this which will align us
+                *hostAddr++ = (t_uint32)client->thisAddress;
+                j = 0;
+            }
+
+            // Word align copy
+            for(; j < require->interface->methodNumber - 1; j+=2) {
+                *(volatile t_uint64*)hostAddr =
+                        ((t_uint64)functionAddress << 0) |
+                        ((t_uint64)functionAddress << 32);
+                hostAddr += 2;
+            }
+
+            // Last word align if required
+            if(j < require->interface->methodNumber)
+                *hostAddr = functionAddress;
+        }
+    }
+
+    cm_registerLowLevelInterfaceToConst(itfRequire, targetInstance);
+}
+
+/*
+ * Bind User component though primitive binding factory
+ */
+t_cm_error cm_bindInterface(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide) {
+
+    LOG_INTERNAL(1, "\n##### Bind Synchronous %s/%x.%s -> %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName);
+
+    cm_bindLowLevelInterface(
+            itfRequire,
+            itfProvide,
+            BF_SYNCHRONOUS, NULL);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_SYNCHRONOUS,
+            itfRequire->client, itfProvide->server,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            itfProvide->server->Template->provides[itfProvide->provideIndex].name);
+
+  	return CM_OK;
+}
+
+/*
+ *
+ */
+void cm_unbindInterface(
+        const t_interface_require_description *itfRequire) {
+
+    LOG_INTERNAL(1, "\n##### UnBind synchronous %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_SYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    cm_bindLowLevelInterfaceToConst(itfRequire,
+            0x0,
+            NULL);
+}
+
+/*
+ *
+ */
+t_cm_error cm_bindInterfaceToVoid(
+        const t_interface_require_description *itfRequire) {
+    LOG_INTERNAL(1, "\n##### Bind %s/%x.%s -> Void #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_bindLowLevelInterfaceToConst(itfRequire,
+            cm_EEM_getExecutiveEngine(itfRequire->client->Template->dspId)->voidAddr,
+            (t_component_instance*)NMF_VOID_COMPONENT);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_SYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+	return CM_OK;
+}
+/*
+ * Find the server and its interface inded to a given required interface for a given component
+ */
+t_cm_error cm_lookupInterface(
+        const t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide) {
+    const t_component_instance* client = itfRequire->client;
+    t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+	if(itfRef->instance != NULL)
+	{
+	    itfProvide->server = itfRef->instance;
+    	itfProvide->provideIndex = itfRef->provideIndex;
+    	itfProvide->collectionIndex = itfRef->collectionIndex;
+
+	    return CM_OK;
+    } else {
+        itfProvide->server = NULL;
+        return CM_INTERFACE_NOT_BINDED;
+    }
+}
+
+/*
+ *
+ */
+t_cm_error cm_bindInterfaceTrace(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_elfdescription *elfhandleTrace)
+{
+    t_interface_require *require = &itfRequire->client->Template->requires[itfRequire->requireIndex];
+    t_interface_require_description bcitfRequire;
+    t_interface_provide_description bcitfProvide;
+    t_trace_bf_info *bfInfo;
+    t_cm_error error;
+
+    LOG_INTERNAL(1, "\n##### Bind Synchronous Trace %s/%x.%s -> %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName);
+
+    /* Allocate aynchronous binding factory information */
+    bfInfo = (t_trace_bf_info*)OSAL_Alloc(sizeof(t_trace_bf_info));
+    if(bfInfo == 0)
+        return CM_NO_MORE_MEMORY;
+
+    /*
+     * Instantiate related trace on dsp
+     */
+    {
+        char traceTemplateName[4 + MAX_INTERFACE_TYPE_NAME_LENGTH + 1];
+
+        cm_StringCopy(traceTemplateName,"_tr.", sizeof(traceTemplateName));
+        cm_StringConcatenate(traceTemplateName, require->interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+        if ((error = cm_instantiateComponent(
+                traceTemplateName,
+                itfRequire->client->domainId,
+                itfProvide->server->priority,
+                traceDup,
+                elfhandleTrace,
+                &bfInfo->traceInstance)) != CM_OK) {
+            OSAL_Free(bfInfo);
+            return (error == CM_COMPONENT_NOT_FOUND)?CM_BINDING_COMPONENT_NOT_FOUND : error;
+        }
+    }
+
+    /* Bind event to server interface (Error must not occure) */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->traceInstance, "target", &bcitfRequire) == CM_OK);
+
+    cm_bindLowLevelInterface(&bcitfRequire, itfProvide, BF_SYNCHRONOUS, NULL);
+
+    /* Get the event interface (Error must not occure) */
+    CM_ASSERT(cm_getProvidedInterface(bfInfo->traceInstance, "target", &bcitfProvide) == CM_OK);
+
+    /* Bind client to event (Error must not occure) */
+    cm_bindLowLevelInterface(itfRequire, &bcitfProvide, BF_TRACE, bfInfo);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_SYNCHRONOUS,
+            itfRequire->client, itfProvide->server,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            itfProvide->server->Template->provides[itfProvide->provideIndex].name);
+
+    return CM_OK;
+}
+
+void cm_unbindInterfaceTrace(
+        const t_interface_require_description *itfRequire,
+        t_trace_bf_info                       *bfInfo)
+{
+    t_interface_require_description traceitfRequire;
+
+    LOG_INTERNAL(1, "\n##### UnBind trace synchronous %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_SYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    /* Unbind Client from Event Binding Component */
+    cm_bindLowLevelInterfaceToConst(itfRequire, 0x0, NULL);
+
+    /* Unbind explicitly Event from Server Binding Component */
+    /* This is mandatory to fix the providedItfUsedCount of the server */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->traceInstance, "target", &traceitfRequire) == CM_OK);
+
+    cm_registerLowLevelInterfaceToConst(&traceitfRequire, NULL);
+
+    /* Destroy Event Binding Component */
+    cm_destroyInstance(bfInfo->traceInstance, DESTROY_WITHOUT_CHECK);
+
+    /* Free BF info */
+    OSAL_Free(bfInfo);
+}
+
+
+/*
+ *
+ */
+t_cm_error cm_bindInterfaceAsynchronous(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription *elfhandleEvent) {
+    t_interface_require *require = &itfRequire->client->Template->requires[itfRequire->requireIndex];
+    t_interface_require_description eventitfRequire;
+    t_interface_provide_description eventitfProvide;
+    t_async_bf_info *bfInfo;
+    t_cm_error error;
+
+    LOG_INTERNAL(1, "\n##### Bind Asynchronous %s/%x.%s -> %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName);
+
+    /* Allocate aynchronous binding factory information */
+    bfInfo = (t_async_bf_info*)OSAL_Alloc(sizeof(t_async_bf_info));
+    if(bfInfo == 0)
+        return CM_NO_MORE_MEMORY;
+
+    /*
+     * Instantiate related event on dsp
+     */
+    {
+        char eventTemplateName[4 + MAX_INTERFACE_TYPE_NAME_LENGTH + 1];
+
+        cm_StringCopy(eventTemplateName,"_ev.", sizeof(eventTemplateName));
+        cm_StringConcatenate(eventTemplateName, require->interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+        if ((error = cm_instantiateComponent(
+                        eventTemplateName,
+                        itfRequire->client->domainId,
+                        itfProvide->server->priority,
+                        eventDup,
+                        elfhandleEvent,
+                        &bfInfo->eventInstance)) != CM_OK) {
+            OSAL_Free(bfInfo);
+            return (error == CM_COMPONENT_NOT_FOUND)?CM_BINDING_COMPONENT_NOT_FOUND : error;
+        }
+    }
+
+    /*
+     * Initialize the event component
+     */
+    {
+        unsigned int size;
+
+        // Get fifo elem size (which was store in TOP by convention)
+        size = cm_readAttributeNoError(bfInfo->eventInstance, "TOP");
+        LOG_INTERNAL(3, "DspEvent Fifo element size = %d\n", size, 0, 0, 0, 0, 0);
+
+        // Allocate fifo
+        if ((error = dspevent_createDspEventFifo(bfInfo->eventInstance,
+                        "TOP",
+                        fifosize, size,
+                        dspEventMemType,
+                        &bfInfo->dspfifoHandle)) != CM_OK)
+        {
+            cm_destroyInstance(bfInfo->eventInstance, DESTROY_WITHOUT_CHECK);
+            OSAL_Free(bfInfo);
+            return error;
+        }
+    }
+
+    /* Bind event to server interface (Error must not occure) */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->eventInstance, "target", &eventitfRequire) == CM_OK);
+
+    cm_bindLowLevelInterface(&eventitfRequire, itfProvide, BF_SYNCHRONOUS, NULL);
+
+    /* Get the event interface (Error must not occure) */
+    CM_ASSERT(cm_getProvidedInterface(bfInfo->eventInstance, "target", &eventitfProvide) == CM_OK);
+
+    /* Bind client to event (Error must not occure) */
+    cm_bindLowLevelInterface(itfRequire, &eventitfProvide, BF_ASYNCHRONOUS, bfInfo);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS,
+            itfRequire->client, itfProvide->server,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            itfProvide->server->Template->provides[itfProvide->provideIndex].name);
+
+    return CM_OK;
+}
+
+void cm_unbindInterfaceAsynchronous(
+        const t_interface_require_description   *itfRequire,
+        t_async_bf_info                         *bfInfo)
+{
+    t_interface_require_description eventitfRequire;
+
+    LOG_INTERNAL(1, "\n##### UnBind asynchronous %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    /* Unbind Client from Event Binding Component */
+    cm_bindLowLevelInterfaceToConst(itfRequire, 0x0, NULL);
+
+    /* Unbind explicitly Event from Server Binding Component */
+    /* This is mandatory to fix the providedItfUsedCount of the server */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->eventInstance, "target", &eventitfRequire) == CM_OK);
+
+    cm_registerLowLevelInterfaceToConst(&eventitfRequire, NULL);
+
+    /* Destroy Event fifo */
+    dspevent_destroyDspEventFifo(bfInfo->dspfifoHandle);
+
+    /* Destroy Event Binding Component */
+    cm_destroyInstance(bfInfo->eventInstance, DESTROY_WITHOUT_CHECK);
+
+    /* Free BF info */
+    OSAL_Free(bfInfo);
+}
+
+/*!
+ * Create Shared FIFO and set stub and skeleton to it
+ */
+PRIVATE t_cm_error cm_createParamsFifo(t_component_instance *stub,
+        t_component_instance *skeleton,
+        t_cm_domain_id domainId,
+        t_uint32 fifosize,
+        t_nmf_fifo_arm_desc **fifo,
+        t_uint32 *fifoElemSize,
+        t_uint32 bcDescSize)
+{
+    t_nmf_core_id stubcore = (stub != NULL) ?(stub->Template->dspId): ARM_CORE_ID;
+    t_nmf_core_id skelcore = (skeleton != NULL) ?(skeleton->Template->dspId) : ARM_CORE_ID;
+    t_component_instance *bcnotnull = (stub != NULL) ? stub : skeleton;
+    int _fifoelemsize;
+
+    CM_ASSERT(bcnotnull != NULL);
+
+    /* Get fifo param elem size (which was store in FIFO by convention) */
+    _fifoelemsize = cm_readAttributeNoError(bcnotnull, "FIFO");
+    LOG_INTERNAL(3, "Fifo Params element size = %d\n", _fifoelemsize, 0, 0, 0, 0, 0);
+    if(fifoElemSize != NULL)
+        *fifoElemSize = _fifoelemsize;
+
+    /* Allocation of the fifo params */
+    *fifo = fifo_alloc(stubcore, skelcore, _fifoelemsize, fifosize, 1+bcDescSize, paramsLocation, extendedFieldLocation, domainId); /* 1+nbMethods fro hostBCThis_or_TOP space */
+    if(*fifo == NULL) {
+        ERROR("CM_NO_MORE_MEMORY: fifo_alloc() failed in cm_createParamsFifo()\n", 0, 0, 0, 0, 0, 0);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    if(stub != NULL)
+    {
+        /* Set stub FIFO attribute (Error mut not occure) */
+        cm_writeAttribute(stub, "FIFO", (*fifo)->dspAdress);
+
+        LOG_INTERNAL(2, "  FIFO param %x:%x\n", *fifo, (*fifo)->dspAdress, 0, 0, 0, 0);
+    }
+
+    if(skeleton != NULL)
+    {
+        /* Set Skeleton FIFO attribute (Error mut not occure) */
+        cm_writeAttribute(skeleton, "FIFO", (*fifo)->dspAdress);
+
+        LOG_INTERNAL(2, "  FIFO param %x:%x\n", *fifo, (*fifo)->dspAdress, 0, 0, 0, 0);
+    }
+
+    return CM_OK;
+}
+/**
+ *
+ */
+static void cm_destroyParamsFifo(t_nmf_fifo_arm_desc *fifo) {
+    fifo_free(fifo);
+}
+
+/*!
+ * Create DSP skeleton
+ */
+PRIVATE t_cm_error cm_createDSPSkeleton(
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType, //INTERNAL_XRAM24
+        t_elfdescription *elfhandleSkeleton,
+        t_dspskel_bf_info *bfInfo)
+{
+    t_interface_provide *provide = &itfProvide->server->Template->provides[itfProvide->provideIndex];
+    t_interface_require_description skelitfRequire;
+    t_cm_error error;
+    unsigned int fifoeventsize = 0;
+
+    /* Instantiate related stub on dsp */
+    {
+        char stubTemplateName[4 + MAX_INTERFACE_TYPE_NAME_LENGTH + 1];
+
+        cm_StringCopy(stubTemplateName,"_sk.", sizeof(stubTemplateName));
+        cm_StringConcatenate(stubTemplateName, provide->interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+        if ((error = cm_instantiateComponent(
+                        stubTemplateName,
+                        itfProvide->server->domainId,
+                        itfProvide->server->priority,
+                        skeletonDup,
+                        elfhandleSkeleton,
+                        &bfInfo->skelInstance)) != CM_OK) {
+            return ((error == CM_COMPONENT_NOT_FOUND)?CM_BINDING_COMPONENT_NOT_FOUND:error);
+        }
+    }
+
+    /* Get fifo elem size (which was store in TOP by convention) */
+    fifoeventsize = cm_readAttributeNoError(bfInfo->skelInstance, "TOP");
+    LOG_INTERNAL(3, "DspEvent Fifo element size = %d\n", fifoeventsize, 0, 0, 0, 0, 0);
+
+    /* Allocation of the itf event dsp fifo */
+    if ((error = dspevent_createDspEventFifo(
+                    bfInfo->skelInstance,
+                    "TOP",
+                    fifosize,
+                    fifoeventsize,
+                    dspEventMemType,
+                    &bfInfo->dspfifoHandle)) != CM_OK)
+    {
+	    cm_destroyInstance(bfInfo->skelInstance, DESTROY_WITHOUT_CHECK);
+        return error;
+    }
+
+    /* Bind stub to server component (Error must not occure) */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->skelInstance, "target", &skelitfRequire) == CM_OK);
+
+    cm_bindLowLevelInterface(&skelitfRequire, itfProvide, BF_SYNCHRONOUS, NULL);
+
+    return CM_OK;
+}
+
+/**
+ * Destroy DSP Skeleton
+ */
+PRIVATE t_cm_error cm_destroyDSPSkeleton(t_dspskel_bf_info *bfInfo) {
+    t_interface_require_description skelitfRequire;
+
+    /* Unbind explicitly stub from server component (Error must not occure) */
+    /* This is mandatory to fix the providedItfUsedCount of the server */
+    CM_ASSERT(cm_getRequiredInterface(bfInfo->skelInstance, "target", &skelitfRequire) == CM_OK);
+
+    cm_registerLowLevelInterfaceToConst(&skelitfRequire, NULL);
+
+    /* Destroy Event fifo */
+    dspevent_destroyDspEventFifo(bfInfo->dspfifoHandle);
+
+    /* Destroy Event Binding Component */
+    return cm_destroyInstance(bfInfo->skelInstance, DESTROY_WITHOUT_CHECK);
+}
+
+/*
+ *
+ */
+t_cm_error cm_bindComponentFromCMCore(
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription *elfhandleSkeleton,
+        t_host2mpc_bf_info **bfInfo) {
+    t_interface_provide *provide = &itfProvide->server->Template->provides[itfProvide->provideIndex];
+    t_dsp_offset shareVarOffset;
+    t_cm_error error;
+
+    LOG_INTERNAL(1, "\n##### Bind HOST -> %s/%x.%s #####\n",
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName, 0, 0, 0);
+
+    /* Allocate host2dsp binding factory information */
+    *bfInfo = (t_host2mpc_bf_info*)OSAL_Alloc(sizeof(t_host2mpc_bf_info));
+    if((*bfInfo) == 0)
+        return CM_NO_MORE_MEMORY;
+
+    /* Create the Skeleton */
+    if ((error = cm_createDSPSkeleton(itfProvide,
+                    fifo_normalizeDepth(fifosize), /* We SHALL create DSP Skeleton before creating the Params Fifo, but we need in advance the real depth of this fifo */
+                    dspEventMemType,
+                    elfhandleSkeleton,
+                    &(*bfInfo)->dspskeleton)) != CM_OK)
+    {
+        OSAL_Free((*bfInfo));
+        return error;
+    }
+
+    /* Create the FIFO Params */
+    if ((error = cm_createParamsFifo(NULL,
+                    (*bfInfo)->dspskeleton.skelInstance,
+                    itfProvide->server->domainId,
+                    fifosize,
+                    &(*bfInfo)->fifo,
+                    NULL,
+                    provide->interface->methodNumber)) != CM_OK)
+    {
+	cm_destroyDSPSkeleton(&(*bfInfo)->dspskeleton);
+        OSAL_Free((*bfInfo));
+        return error;
+    }
+
+    /* Set Target info in FIFO param to TOP */
+    shareVarOffset = cm_getAttributeMpcAddress((*bfInfo)->dspskeleton.skelInstance, "TOP");
+
+    /*
+     * Set Target info in FIFO param to armThis
+     * Should not return any error
+     */
+    fifo_params_setSharedField((*bfInfo)->fifo, 0, (t_shared_field)shareVarOffset /* ArmBCThis_or_TOP */);
+
+    /* Initialise FIFO Param bcDesc with Skeleton methods */
+    {
+        int i;
+        t_component_instance *skel = (*bfInfo)->dspskeleton.skelInstance;
+        for (i=0; i < provide->interface->methodNumber; i++)
+        {
+            /* should not return error */
+            fifo_params_setSharedField(
+                    (*bfInfo)->fifo,
+                    1+i,
+                    skel->Template->providesLoaded[0].indexesLoaded[0][i].methodAddresses
+                    );
+        }
+    }
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS,
+            ARM_TRACE_COMPONENT, itfProvide->server,
+            NULL,
+            itfProvide->server->Template->provides[itfProvide->provideIndex].name);
+
+    return CM_OK;
+}
+
+void cm_unbindComponentFromCMCore(
+        t_host2mpc_bf_info* bfInfo) {
+    t_component_instance *skel = bfInfo->dspskeleton.skelInstance;
+    t_interface_reference* itfProvide = &skel->interfaceReferences[0][0];
+    t_interface_provide *provide = &itfProvide->instance->Template->provides[itfProvide->provideIndex];
+
+    LOG_INTERNAL(1, "\n##### UnBind HOST -> %s/%x.%s #####\n",
+		 itfProvide->instance->pathname, itfProvide->instance, provide->name, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS,
+            ARM_TRACE_COMPONENT, itfProvide->instance,
+            NULL,
+            itfProvide->instance->Template->provides[itfProvide->provideIndex].name);
+
+    // Destroy FIFO params
+    cm_destroyParamsFifo(bfInfo->fifo);
+
+    // Destory Skeleton
+    cm_destroyDSPSkeleton(&bfInfo->dspskeleton);
+
+    // Free BF info (which contains bcDecr(==dspfct) and arm This)
+    OSAL_Free(bfInfo);
+}
+
+/**
+ * Create DSP Stub
+ */
+PRIVATE t_cm_error cm_createDSPStub(
+        const t_interface_require_description *itfRequire,
+        const char* itfType,
+        t_dspstub_bf_info* bfInfo,
+        t_elfdescription *elfhandleStub,
+        t_interface_provide_description *itfstubProvide) {
+    t_cm_error error;
+
+    /*
+     * Instantiate related skel on dsp
+     */
+    {
+        char skelTemplateName[4 + MAX_INTERFACE_TYPE_NAME_LENGTH + 1];
+
+        cm_StringCopy(skelTemplateName, "_st.", sizeof(skelTemplateName));
+        cm_StringConcatenate(skelTemplateName, itfType, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+        if ((error = cm_instantiateComponent(
+                        skelTemplateName,
+                        itfRequire->client->domainId,
+                        itfRequire->client->priority,
+                        stubDup,
+                        elfhandleStub,
+                        &bfInfo->stubInstance)) != CM_OK) {
+            return (error == CM_COMPONENT_NOT_FOUND)?CM_BINDING_COMPONENT_NOT_FOUND : error;
+        }
+    }
+
+    /* Get the internal component that serve this interface (Error must not occure) */
+    (void)cm_getProvidedInterface(bfInfo->stubInstance, "source", itfstubProvide);
+
+    return CM_OK;
+}
+
+PRIVATE t_cm_error cm_destroyDSPStub(
+        const t_interface_require_description *itfRequire,
+        t_dspstub_bf_info* bfInfo) {
+
+    /* Unbind Client from Event Binding Component */
+    cm_bindLowLevelInterfaceToConst(itfRequire,
+            0x0,
+            NULL);
+
+    /* Destroy Event Binding Component */
+    return cm_destroyInstance(bfInfo->stubInstance, DESTROY_WITHOUT_CHECK);
+}
+/*
+ *
+ */
+t_cm_error cm_bindComponentToCMCore(
+        const t_interface_require_description   *itfRequire,
+        t_uint32                                fifosize,
+        t_uint32                                context,
+        t_elfdescription                        *elfhandleStub,
+        t_mpc2host_bf_info                      ** bfInfo) {
+    t_interface_require *require = &itfRequire->client->Template->requires[itfRequire->requireIndex];
+    t_interface_provide_description itfstubProvide;
+    t_cm_error error;
+    t_uint32 fifoelemsize;
+
+    LOG_INTERNAL(1, "\n##### Bind %s/%x.%s -> HOST #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    /* Allocate dsp2host binding factory information */
+    *bfInfo = (t_mpc2host_bf_info*)OSAL_Alloc(sizeof(t_mpc2host_bf_info));
+    if(*bfInfo == 0)
+        return CM_NO_MORE_MEMORY;
+    (*bfInfo)->context = context;
+
+    if ((error = cm_createDSPStub(itfRequire,
+                    require->interface->type,
+                    &(*bfInfo)->dspstub,
+                    elfhandleStub,
+                    &itfstubProvide)) != CM_OK)
+    {
+        OSAL_Free(*bfInfo);
+        return error;
+    }
+
+    /* Create the FIFO Params */
+    if ((error = cm_createParamsFifo(
+                    (*bfInfo)->dspstub.stubInstance,
+                    NULL,
+                    itfRequire->client->domainId,
+                    fifosize,
+                    &(*bfInfo)->fifo,
+                    &fifoelemsize,
+                    1)) != CM_OK) /* 1 => we used first field as max params size */
+    {
+	    cm_destroyDSPStub(itfRequire, &(*bfInfo)->dspstub);
+        OSAL_Free(*bfInfo);
+        return error;
+    }
+
+    /* Bind client to stub component (Error must not occure) */
+    cm_bindLowLevelInterface(itfRequire, &itfstubProvide, BF_DSP2HOST, *bfInfo);
+
+    /* Bind stub component to host (virtual bind) */
+    cm_bindVirtualInterface((*bfInfo)->dspstub.stubInstance, (t_component_instance*)NMF_HOST_COMPONENT);
+
+    /*
+     * Set Target info in FIFO param to armThis
+     * Initialise FIFO Param bcDesc with  Jumptable
+     * Should not return any error
+     */
+    fifo_params_setSharedField((*bfInfo)->fifo, 0, (t_shared_field)context /* ArmBCThis_or_TOP */);
+    fifo_params_setSharedField((*bfInfo)->fifo, 1, (t_shared_field)fifoelemsize * 2/* bcDescRef */);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS,
+            itfRequire->client, ARM_TRACE_COMPONENT,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    return error;
+}
+
+void cm_unbindComponentToCMCore(
+        const t_interface_require_description   *itfRequire,
+        t_mpc2host_bf_info                      *bfInfo)
+{
+    LOG_INTERNAL(1, "\n##### UnBind %s/%x.%s -> HOST #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS,
+            itfRequire->client, ARM_TRACE_COMPONENT,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    /* Unbind virtual interface coms */
+    cm_unbindVirtualInterface(bfInfo->dspstub.stubInstance);
+
+    // Destroy FIFO params
+    cm_destroyParamsFifo(bfInfo->fifo);
+
+    // Destroy DSP Stub
+    cm_destroyDSPStub(itfRequire, &bfInfo->dspstub);
+
+    /* Free BF info */
+    OSAL_Free(bfInfo);
+}
+
+/*!
+ *
+ */
+t_cm_error cm_bindInterfaceDistributed(
+        const t_interface_require_description *itfRequire,
+        const t_interface_provide_description *itfProvide,
+        t_uint32 fifosize,
+        t_dsp_memory_type_id dspEventMemType,
+        t_elfdescription                        *elfhandleSkeleton,
+        t_elfdescription                        *elfhandleStub) {
+    t_interface_require *require = &itfRequire->client->Template->requires[itfRequire->requireIndex];
+    t_interface_provide_description itfstubProvide;
+    t_cm_error error;
+    t_mpc2mpc_bf_info *bfInfo;
+    t_dsp_offset shareVarOffset;
+
+    LOG_INTERNAL(1, "\n##### Bind Distributed %s/%x.%s -> %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+		 itfProvide->server->pathname, itfProvide->server, itfProvide->origName);
+
+    /* Allocate aynchronous binding factory information */
+    bfInfo = (t_mpc2mpc_bf_info*)OSAL_Alloc(sizeof(t_mpc2mpc_bf_info));
+    if(bfInfo == 0)
+        return CM_NO_MORE_MEMORY;
+
+    /* Create the Skeleton */
+    if ((error = cm_createDSPSkeleton(itfProvide,
+                    fifo_normalizeDepth(fifosize), /* We SHALL create DSP Skeleton before creating the Params Fifo, but we need in advance the real depth of this fifo */
+                    dspEventMemType,
+                    elfhandleSkeleton,
+                    &bfInfo->dspskeleton)) != CM_OK)
+    {
+        OSAL_Free(bfInfo);
+        return error;
+    }
+
+    // Create DSP Stub
+    if ((error = cm_createDSPStub(itfRequire,
+                    require->interface->type,
+                    &bfInfo->dspstub,
+                    elfhandleStub,
+                    &itfstubProvide)) != CM_OK)
+    {
+	cm_destroyDSPSkeleton(&bfInfo->dspskeleton);
+        OSAL_Free(bfInfo);
+        return error;
+    }
+
+    /* Bind client to stub component (Error must not occure) */
+    cm_bindLowLevelInterface(itfRequire, &itfstubProvide, BF_DSP2DSP, bfInfo);
+
+    /* Create the FIFO Params */
+    if ((error = cm_createParamsFifo(
+                    bfInfo->dspstub.stubInstance,
+                    bfInfo->dspskeleton.skelInstance,
+                    itfProvide->server->domainId,
+                    fifosize,
+                    &bfInfo->fifo,
+                    NULL,
+                    require->interface->methodNumber)) != CM_OK)
+    {
+	cm_destroyDSPStub(itfRequire, &bfInfo->dspstub);
+	cm_destroyDSPSkeleton(&bfInfo->dspskeleton);
+        OSAL_Free(bfInfo);
+        return error;
+    }
+
+    /* Bind stub component to host (virtual bind) */
+    cm_bindVirtualInterface(bfInfo->dspstub.stubInstance, bfInfo->dspskeleton.skelInstance);
+
+    /* Set Target info in FIFO param to TOP */
+    shareVarOffset = cm_getAttributeMpcAddress(bfInfo->dspskeleton.skelInstance, "TOP");
+
+    /*
+     * Set Target info in FIFO param to armThis
+     * Should not return any error
+     */
+    fifo_params_setSharedField(bfInfo->fifo, 0, (t_shared_field)shareVarOffset /* ArmBCThis_or_TOP */);
+
+    /* Initialise FIFO Param bcDesc with Skeleton methods */
+    {
+        int i;
+        t_component_instance *skel = bfInfo->dspskeleton.skelInstance;
+        for (i=0; i < require->interface->methodNumber; i++)
+        {
+            /* should not return error */
+            fifo_params_setSharedField(
+                    bfInfo->fifo,
+                    1+i,
+                    skel->Template->providesLoaded[0].indexesLoaded[0][i].methodAddresses
+                    );
+        }
+    }
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS,
+            itfRequire->client, itfProvide->server,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            itfProvide->server->Template->provides[itfProvide->provideIndex].name);
+
+    return CM_OK;
+}
+
+/*!
+ *
+ */
+void cm_unbindInterfaceDistributed(
+        const t_interface_require_description   *itfRequire,
+        t_mpc2mpc_bf_info                       *bfInfo)
+{
+    LOG_INTERNAL(1, "\n##### UnBind distributed %s/%x.%s #####\n",
+		 itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0, 0);
+
+    cm_TRC_traceBinding(TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS,
+            itfRequire->client, NULL,
+            itfRequire->client->Template->requires[itfRequire->requireIndex].name,
+            NULL);
+
+    /* Unbind virtual interface */
+    cm_unbindVirtualInterface(bfInfo->dspstub.stubInstance);
+
+    // Destroy FIFO params
+    cm_destroyParamsFifo(bfInfo->fifo);
+
+    // Destroy DSP Stub
+    cm_destroyDSPStub(itfRequire, &bfInfo->dspstub);
+
+    // Destory DSP Skeleton
+    cm_destroyDSPSkeleton(&bfInfo->dspskeleton);
+
+    // Destroy BF Info
+    OSAL_Free(bfInfo);
+}
+
+t_cm_error cm_bindInterfaceStaticInterrupt(
+        const t_nmf_core_id coreId,
+        const int interruptLine,
+        const t_component_instance *server,
+        const char* providedItfServerName
+)
+{
+    char requiredItfClientName[CM_IT_NAME_MAX_LENGTH];
+    t_component_instance *client = cm_EEM_getExecutiveEngine(coreId)->instance;
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_cm_error error;
+
+    //build it[%d] name
+    if (interruptLine < 0 || interruptLine > 255) {return CM_OUT_OF_LIMITS;}
+    cm_fillItName(interruptLine, requiredItfClientName);
+
+    //do binding
+    if ((error = cm_getRequiredInterface(client,requiredItfClientName,&itfRequire)) !=  CM_OK) {return error;}
+    if ((error = cm_getProvidedInterface(server,providedItfServerName,&itfProvide)) !=  CM_OK) {return error;}
+    if((error = cm_bindInterface(&itfRequire, &itfProvide)) != CM_OK) {return error;}
+
+    return CM_OK;
+}
+
+t_cm_error cm_unbindInterfaceStaticInterrupt(
+        const t_nmf_core_id coreId,
+        const int interruptLine
+)
+{
+    char requiredItfClientName[CM_IT_NAME_MAX_LENGTH];
+    t_component_instance *client = cm_EEM_getExecutiveEngine(coreId)->instance;
+    t_interface_require_description itfRequire;
+    t_cm_error error;
+
+    //build it[%d] name
+    if (interruptLine < 0 || interruptLine > 255) {return CM_OUT_OF_LIMITS;}
+    cm_fillItName(interruptLine, requiredItfClientName);
+
+    //do unbinding
+    if ((error = cm_getRequiredInterface(client,requiredItfClientName,&itfRequire)) !=  CM_OK) {return error;}
+    cm_unbindInterface(&itfRequire);
+
+    return CM_OK;
+}
+
+void cm_destroyRequireInterface(t_component_instance* component, t_nmf_client_id clientId)
+{
+    int i, j;
+
+    /*
+     * Special code for SINGLETON handling
+     */
+    if(component->Template->classe == SINGLETON)
+    {
+        if(getNumberOfBind(component) > 0)
+            return;
+    }
+
+    for(i = 0; i < component->Template->requireNumber; i++)
+    {
+        int nb = component->Template->requires[i].collectionSize;
+        for(j = 0; j < nb; j++)
+        {
+            if(component->interfaceReferences[i][j].instance != NULL)
+            {
+                t_interface_reference* itfRef = &component->interfaceReferences[i][j];
+                t_interface_require_description itfRequire;
+
+                itfRequire.client = component;
+                itfRequire.requireIndex = i;
+                itfRequire.collectionIndex = j;
+                itfRequire.origName = component->Template->requires[i].name;
+
+                switch (itfRef->bfInfoID) {
+                case BF_SYNCHRONOUS:
+                    /* Error ignored as it is always OK */
+                    cm_unbindInterface(&itfRequire);
+                    break;
+                case BF_TRACE:
+                    cm_unbindInterfaceTrace(&itfRequire,
+                            (t_trace_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+                    break;
+                case BF_ASYNCHRONOUS:
+                    cm_unbindInterfaceAsynchronous(&itfRequire,
+                            (t_async_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+                    break;
+                case BF_DSP2HOST: 
+                    /* This 'mpc2host handle' is provided by the host at OS Integration level.
+                           It must then be handled and released in OS specific part.
+                     */
+                    cm_unbindComponentToCMCore(&itfRequire,
+                            (t_mpc2host_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+                    break;
+                case BF_HOST2DSP:
+                    /* These bindings are from CM Core to DSP, they are not listed
+                           here and must be handles/freed by host at OS Integration level
+                     */
+                    break;
+                case BF_DSP2DSP:
+                    cm_unbindInterfaceDistributed(&itfRequire,
+                            (t_mpc2mpc_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+                    break;
+                default:
+                    break;
+                }
+            }
+        }
+    }
+}
+
+void cm_registerSingletonBinding(
+        t_component_instance*                   component,
+        t_interface_require_description*        itfRequire,
+        t_interface_provide_description*        itfProvide,
+        t_nmf_client_id                         clientId)
+{
+    if(component->Template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        if(cl != NULL)
+            cl->numberOfBind++;
+
+        if(itfProvide != NULL)
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Register binding %s/%x.%s -> %s/%x\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+                    itfProvide->server->pathname, itfProvide->server);
+        else
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Register binding %s/%x.%s -> ARM/VOID\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0);
+    }
+}
+
+t_bool cm_unregisterSingletonBinding(
+        t_component_instance*                   component,
+        t_interface_require_description*        itfRequire,
+        t_interface_provide_description*        itfProvide,
+        t_nmf_client_id                         clientId)
+{
+    if(component->Template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        if(cl != NULL)
+            cl->numberOfBind--;
+
+        if(itfProvide->server == (t_component_instance *)NMF_VOID_COMPONENT)
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Unregister binding %s/%x.%s -> ARM/VOID\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0);
+        else if(itfProvide->server == NULL)
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Unregister binding %s/%x.%s -> ?? <already unbound>\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName, 0, 0);
+        else
+            LOG_INTERNAL(1, "  -> Singleton[%d] : Unregister binding %s/%x.%s -> %s/%x\n",
+                    clientId,
+                    itfRequire->client->pathname, itfRequire->client, itfRequire->origName,
+                    itfProvide->server->pathname, itfProvide->server);
+
+        if(getNumberOfBind(component) == 0)
+        {
+            LOG_INTERNAL(1, "  -> Singleton[%d] : All required of %s/%x logically unbound, perform physical unbind\n",
+                    clientId, itfRequire->client->pathname, itfRequire->client, 0, 0, 0);
+
+            (void)cm_EEM_ForceWakeup(component->Template->dspId);
+
+            // This is the last binding unbind all !!!
+            cm_destroyRequireInterface(component, clientId);
+
+            cm_EEM_AllowSleep(component->Template->dspId);
+        }
+        else if(itfProvide->server != NULL)
+        {
+            t_interface_require* itfReq;
+            itfReq = &itfRequire->client->Template->requires[itfRequire->requireIndex];
+            if((itfReq->requireTypes & OPTIONAL_REQUIRE) != 0x0)
+                return TRUE;
+        }
+
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+static t_uint16 getNumberOfBind(t_component_instance* component)
+{
+    t_uint16 bindNumber = 0;
+    struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+    for( ; cur != NULL ; cur = cur->next)
+    {
+        bindNumber += cur->numberOfBind;
+    }
+
+    return bindNumber;
+}
+
+static void cm_fillItName(int interruptLine, char *itName)
+{
+    int divider = 10000;
+
+    *itName++ = 'i';
+    *itName++ = 't';
+    *itName++ = '[';
+
+    // Find first significant divider
+    while(divider > interruptLine)
+        divider /= 10;
+
+    // Compute number
+    do
+    {
+        *itName++ = "0123456789"[interruptLine / divider];
+        interruptLine %= divider;
+        divider /= 10;
+    } while(divider != 0);
+
+    *itName++ = ']';
+    *itName++ = '\0';
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/binder_check.c b/drivers/staging/nmf-cm/cm/engine/component/src/binder_check.c
new file mode 100644
index 00000000000..373fea0cd47
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/binder_check.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include "../inc/bind.h"
+#include <cm/engine/trace/inc/trace.h>
+
+#include <cm/engine/utils/inc/string.h>
+
+t_cm_error cm_checkValidClient(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        t_interface_require_description *itfRequire,
+        t_bool *bindable) {
+    t_cm_error error;
+
+    // Component LC state check
+    if (NULL == client)
+        return  CM_INVALID_COMPONENT_HANDLE;
+
+    // Check if the requiredItfClientName is required by client component
+    if ((error = cm_getRequiredInterface(client, requiredItfClientName, itfRequire)) != CM_OK)
+        return error;
+
+    // Check required interface not already binded
+    {
+        t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+        if(itfRef->instance != (t_component_instance*)NULL)
+        {
+            if(client->Template->classe == SINGLETON)
+            {
+                // Singleton is immutable thus we can't rebind it, nevertheless it's not an issue
+                *bindable = FALSE;
+                return CM_OK;
+            }
+            else
+            {
+                t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+
+                if(itfRef->instance == (const t_component_instance*)NMF_VOID_COMPONENT)
+                    ERROR("CM_INTERFACE_ALREADY_BINDED(): Component (%s<%s>.%s) already bound to VOID\n",
+                            client->pathname, client->Template->name, requiredItfClientName, 0, 0, 0);
+                else
+                    ERROR("CM_INTERFACE_ALREADY_BINDED(): Component (%s<%s>.%s) already bound to another server (%s<%s>.%s)\n",
+                            client->pathname, client->Template->name, requiredItfClientName,
+                            itfRef->instance->pathname, itfRef->instance->Template->name, itfRef->instance->Template->provides[itfRef->provideIndex].name);
+                return CM_INTERFACE_ALREADY_BINDED;
+            }
+        }
+    }
+
+    // Delayed Component LC state check done only if not optional required interface or intrinsic one that has been solved by loader
+    {
+        t_interface_require* itfReq = &client->Template->requires[itfRequire->requireIndex];
+
+        if((itfReq->requireTypes & (OPTIONAL_REQUIRE | INTRINSEC_REQUIRE)) == 0) {
+            if(client->state == STATE_RUNNABLE)
+                return CM_COMPONENT_NOT_STOPPED;
+        }
+    }
+
+    *bindable = TRUE;
+
+    return CM_OK;
+}
+
+t_cm_error cm_checkValidServer(
+        const t_component_instance* server,
+        const char* providedItfServerName,
+        t_interface_provide_description *itfProvide) {
+    t_cm_error error;
+
+    // Check if the components are initialized
+    //if (server->state == STATE_INSTANCIATED)
+    //    return CM_COMPONENT_NOT_INITIALIZED;
+    if(NULL == server)
+        return  CM_INVALID_COMPONENT_HANDLE;
+
+    // Check if the providedItfServerName is provided by server component
+    if((error = cm_getProvidedInterface(server, providedItfServerName, itfProvide)) != CM_OK)
+        return error;
+
+    return CM_OK;
+}
+
+t_cm_error cm_checkValidBinding(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        const t_component_instance* server,
+        const char* providedItfServerName,
+        t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide,
+        t_bool *bindable) {
+    t_interface_require *require;
+    t_interface_provide *provide;
+    t_cm_error error;
+
+    // Check Server
+    if((error = cm_checkValidServer(server, providedItfServerName, itfProvide)) != CM_OK)
+        return error;
+
+    // Check Client
+    if((error = cm_checkValidClient(client, requiredItfClientName, itfRequire, bindable)) != CM_OK)
+        return error;
+
+    // If this is a singleton which has been already bound check that next binding is at the same server
+    if(*bindable == FALSE
+            && client->Template->classe == SINGLETON)
+    {
+        t_interface_reference* itfRef = &client->interfaceReferences[itfRequire->requireIndex][itfRequire->collectionIndex];
+        while( itfRef->instance != server
+                || itfRef->provideIndex != itfProvide->provideIndex
+                || itfRef->collectionIndex != itfProvide->collectionIndex )
+        {
+            if(itfRef->instance == (const t_component_instance*)NMF_VOID_COMPONENT)
+            {
+                ERROR("CM_INTERFACE_ALREADY_BINDED(): Singleton (%s<%s>.%s) already bound to VOID\n",
+                        client->pathname, client->Template->name, requiredItfClientName, 0, 0, 0);
+                return CM_INTERFACE_ALREADY_BINDED;
+            }
+            else if(itfRef->bfInfoID == BF_ASYNCHRONOUS || itfRef->bfInfoID == BF_TRACE)
+            {
+                t_interface_require_description eventitfRequire;
+                CM_ASSERT(cm_getRequiredInterface(itfRef->instance, "target", &eventitfRequire) == CM_OK);
+                itfRef = &itfRef->instance->interfaceReferences[eventitfRequire.requireIndex][eventitfRequire.collectionIndex];
+
+                // Go to see client of event if the same
+            }
+            else
+            {
+                ERROR("CM_INTERFACE_ALREADY_BINDED(): Singleton (%s<%s>.%s) already bound to different server (%s<%s>.%s)\n",
+                        client->pathname, client->Template->name, requiredItfClientName,
+                        itfRef->instance->pathname, itfRef->instance->Template->name, itfRef->instance->Template->provides[itfRef->provideIndex].name);
+                return CM_INTERFACE_ALREADY_BINDED;
+            }
+        }
+    }
+
+    // Check if provided and required type matches
+    require = &client->Template->requires[itfRequire->requireIndex];
+    provide = &server->Template->provides[itfProvide->provideIndex];
+    if(require->interface != provide->interface)
+    {
+        ERROR("CM_ILLEGAL_BINDING(%s, %s)\n", require->interface->type, provide->interface->type, 0, 0, 0, 0);
+        return CM_ILLEGAL_BINDING;
+    }
+
+    // Check if static required interface binded to singleton component
+    if((require->requireTypes & STATIC_REQUIRE) &&
+            (server->Template->classe != SINGLETON))
+    {
+        ERROR("CM_ILLEGAL_BINDING(): Can't bind static required interface to not singleton component\n",
+                0, 0, 0, 0, 0, 0);
+        return CM_ILLEGAL_BINDING;
+    }
+
+    return CM_OK;
+}
+
+t_cm_error cm_checkValidUnbinding(
+        const t_component_instance* client,
+        const char* requiredItfClientName,
+        t_interface_require_description *itfRequire,
+        t_interface_provide_description *itfProvide) {
+    t_cm_error error;
+    t_interface_require* itfReq;
+
+    // Component LC state check
+    if (NULL == client)
+        return CM_INVALID_COMPONENT_HANDLE;
+
+    // Check if the requiredItfClientName is required by client component
+    if ((error = cm_getRequiredInterface(client, requiredItfClientName, itfRequire)) != CM_OK)
+        return error;
+
+    itfReq = &client->Template->requires[itfRequire->requireIndex];
+
+    // Check if the requiredItfClientName is required by client component
+    if ((error = cm_lookupInterface(itfRequire, itfProvide)) != CM_OK)
+    {
+        // We allow to unbind optional required of singleton even if not binded, since it could have been unbound previously but we don't
+        // want to break bind singleton reference counter
+        if((client->Template->classe == SINGLETON) &&
+                (itfReq->requireTypes & OPTIONAL_REQUIRE) != 0x0)
+            return CM_OK;
+
+        return error;
+    }
+
+    // Singleton is immutable, don't unbind it
+    if(client->Template->classe == SINGLETON)
+        return CM_OK;
+
+    /* if interface is optionnal then allow unbinding even if not stop */
+    if((itfReq->requireTypes & OPTIONAL_REQUIRE) == 0x0)
+    {
+        if(client->state == STATE_RUNNABLE)
+            return CM_COMPONENT_NOT_STOPPED;
+    }
+
+    return CM_OK;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/component_wrapper.c b/drivers/staging/nmf-cm/cm/engine/component/src/component_wrapper.c
new file mode 100644
index 00000000000..d593950cd97
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/component_wrapper.c
@@ -0,0 +1,1332 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/api/component_engine.h>
+#include <cm/engine/api/communication_engine.h>
+
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/memory/inc/domain.h>
+
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+
+/*
+ * Component mangement wrapping.
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_InstantiateComponent(
+        const char* templateName,
+        t_cm_domain_id domainId,
+        t_nmf_client_id clientId,
+        t_nmf_ee_priority priority,
+        const char localName[MAX_COMPONENT_NAME_LENGTH],
+        const char *dataFile,
+        t_cm_instance_handle *instance) {
+    t_cm_error error;
+    t_nmf_core_id coreId;
+    t_component_instance *comp;
+    t_elfdescription *elfhandle = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFile != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFile,
+                    TRUE,
+                    &elfhandle)) != CM_OK)
+        goto out;
+
+    //only allow instantiation in non-scratch domains (ie. DOMAIN_NORMAL)!
+    if ((error = cm_DM_CheckDomainWithClient(domainId, DOMAIN_NORMAL, clientId)) != CM_OK)
+        goto out;
+
+    coreId = cm_DM_GetDomainCoreId(domainId);
+
+    if(coreId < FIRST_MPC_ID || coreId > LAST_CORE_ID)
+    {
+        error = CM_INVALID_PARAMETER;
+        goto out;
+    }
+
+    if ((error = cm_CFG_CheckMpcStatus(coreId)) != CM_OK)
+        goto out;
+
+    if ((error = cm_EEM_ForceWakeup(coreId)) != CM_OK)
+        goto out;
+
+    error = cm_instantiateComponent(
+            templateName,
+            domainId,
+            priority,
+            localName,
+            elfhandle,
+            &comp);
+    if(error == CM_OK)
+        *instance = comp->instance;
+
+    cm_EEM_AllowSleep(coreId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandle);
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_StartComponent(
+        t_cm_instance_handle instance,
+        t_nmf_client_id clientId) {
+    t_cm_error error;
+    t_component_instance *component;
+
+    OSAL_LOCK_API();
+
+    component = cm_lookupComponent(instance);
+    if (NULL == component)
+        error = CM_INVALID_COMPONENT_HANDLE;
+    else
+    {
+        error = cm_startComponent(component, clientId);
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_StopComponent(
+        t_cm_instance_handle instance,
+        t_nmf_client_id clientId) {
+    t_cm_error error;
+    t_component_instance *component;
+
+    OSAL_LOCK_API();
+
+    component = cm_lookupComponent(instance);
+    if (NULL == component)
+        error = CM_INVALID_COMPONENT_HANDLE;
+    else
+    {
+        error = cm_stopComponent(component, clientId);
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_DestroyComponent(
+        t_cm_instance_handle instance,
+        t_nmf_client_id clientId)
+{
+    t_cm_error error;
+    t_component_instance *component;
+
+    OSAL_LOCK_API();
+
+    component = cm_lookupComponent(instance);
+    if (NULL == component)
+    {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    }
+    else
+    {
+        t_nmf_core_id coreId = component->Template->dspId;
+
+        (void)cm_EEM_ForceWakeup(coreId);
+
+        error = cm_destroyInstanceForClient(component, DESTROY_NORMAL, clientId);
+
+        cm_CFG_ReleaseMpc(coreId);
+
+        cm_EEM_AllowSleep(coreId);
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_FlushComponents(t_nmf_client_id clientId)
+{
+    t_cm_error error = CM_OK;
+    t_component_instance *instance;
+    t_uint32 i;
+
+    if (clientId == 0)
+    	return CM_INVALID_PARAMETER;
+
+    OSAL_LOCK_API();
+
+    // We don't know exactly where components will be, wake up everybody !!
+    (void)cm_EEM_ForceWakeup(SVA_CORE_ID);
+    (void)cm_EEM_ForceWakeup(SIA_CORE_ID);
+
+    /* Destroy all host2mpc bindings */
+    OSAL_LOCK_COM();
+    for (i=0; i<Host2MpcBindingTable.idxMax; i++)
+    {
+	    t_host2mpc_bf_info* bfInfo;
+	    bfInfo = Host2MpcBindingTable.entries[i];
+	    if ((bfInfo != NULL) && (bfInfo->clientId == clientId)) {
+		    cm_delEntry(&Host2MpcBindingTable, i);
+		    OSAL_UNLOCK_COM();
+		    cm_unbindComponentFromCMCore(bfInfo);
+                    OSAL_LOCK_COM();
+	    }
+    }
+    OSAL_UNLOCK_COM();
+
+    /* First, stop all remaining components for this client */
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	if ((instance = componentEntry(i)) == NULL)
+		continue;
+        if (/* skip EE */
+                (instance->Template->classe == FIRMWARE) ||
+                /* Skip all binding components */
+                (cm_StringCompare(instance->Template->name, "_ev.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_st.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_sk.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_tr.", 4) == 0))
+            continue;
+
+        /*
+         * Special code for SINGLETON handling
+         */
+        if(instance->Template->classe == SINGLETON)
+        {
+            struct t_client_of_singleton* cl = cm_getClientOfSingleton(instance, FALSE, clientId);
+            if(cl == NULL)
+                continue;
+
+            cl->numberOfStart = 1; // == 1 since it will go to 0 in cm_stopComponent
+            cl->numberOfInstance = 1; // == 1 since it will go to 0 in cm_destroyInstanceForClient
+            cl->numberOfBind = 0; // == 0 since we don't want anymore binding for this component
+        }
+        else if(domainDesc[instance->domainId].client != clientId)
+            /* Skip all components not belonging to our client */
+            continue;
+
+        // Stop the component
+        error = cm_stopComponent(instance, clientId);
+        if (error != CM_OK && error != CM_COMPONENT_NOT_STARTED)
+            LOG_INTERNAL(0, "Error stopping component %s/%x (%s, error=%d, client=%u)\n", instance->pathname, instance, instance->Template->name, error, clientId, 0);
+
+        // Destroy dependencies
+        cm_destroyRequireInterface(instance, clientId);
+    }
+
+    /* Destroy all remaining components for this client */
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	if ((instance = componentEntry(i)) == NULL)
+		continue;
+        if (/* skip EE */
+                (instance->Template->classe == FIRMWARE) ||
+                /* Skip all binding components */
+                (cm_StringCompare(instance->Template->name, "_ev.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_st.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_sk.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_tr.", 4) == 0)) {
+		continue;
+        }
+
+
+       /*
+        * Special code for SINGLETON handling
+        */
+       if(instance->Template->classe == SINGLETON)
+       {
+           struct t_client_of_singleton* cl = cm_getClientOfSingleton(instance, FALSE, clientId);
+           if(cl == NULL)
+               continue;
+       }
+       else if(domainDesc[instance->domainId].client != clientId)
+           /* Skip all components not belonging to our client */
+           continue;
+
+
+        // Destroy the component
+        error = cm_destroyInstanceForClient(instance, DESTROY_WITHOUT_CHECK, clientId);
+
+        if (error != CM_OK)
+        {
+            /* FIXME : add component name instance in log message but need to make a copy before cm_flushComponent()
+             *         because it's no more available after.
+             */
+            LOG_INTERNAL(0, "Error flushing component (error=%d, client=%u)\n", error, clientId, 0, 0, 0, 0);
+        }
+    }
+
+    cm_CFG_ReleaseMpc(SVA_CORE_ID);
+    cm_CFG_ReleaseMpc(SIA_CORE_ID);
+
+    cm_EEM_AllowSleep(SVA_CORE_ID);
+    cm_EEM_AllowSleep(SIA_CORE_ID);
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+/*
+ * Component binding wrapping.
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponent(
+        const t_cm_instance_handle clientInstance,
+        const char* requiredItfClientName,
+        const t_cm_instance_handle serverInstance,
+        const char* providedItfServerName,
+        t_bool traced,
+        t_nmf_client_id clientId,
+        const char *dataFileTrace) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_bool bindable;
+    t_cm_error error;
+    t_component_instance *client, *server;
+    t_elfdescription *elfhandleTrace = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFileTrace != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileTrace,
+                    TRUE,
+                    &elfhandleTrace)) != CM_OK)
+        goto out;
+
+    client = cm_lookupComponent(clientInstance);
+    server = cm_lookupComponent(serverInstance);
+    // Sanity check
+    if((error = cm_checkValidBinding(client, requiredItfClientName,
+				     server, providedItfServerName,
+				     &itfRequire, &itfProvide, &bindable)) != CM_OK)
+        goto out;
+
+    // Check that client and server component run on same DSP
+    if (itfRequire.client->Template->dspId != itfProvide.server->Template->dspId)
+    {
+        error = CM_ILLEGAL_BINDING;
+        goto out;
+    }
+
+    // Check if we really need to bind
+    if(bindable)
+    {
+        if ((error = cm_EEM_ForceWakeup(itfRequire.client->Template->dspId)) != CM_OK)
+            goto out;
+
+        /*
+         * Synchronous binding, so no binding component
+         */
+        if(traced)
+            error = cm_bindInterfaceTrace(&itfRequire, &itfProvide, elfhandleTrace);
+        else
+            error = cm_bindInterface(&itfRequire, &itfProvide);
+
+        cm_EEM_AllowSleep(itfRequire.client->Template->dspId);
+    }
+
+    cm_registerSingletonBinding(client, &itfRequire, &itfProvide, clientId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandleTrace);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_UnbindComponent(
+        const t_cm_instance_handle clientInstance,
+        const char* requiredItfClientName,
+        t_nmf_client_id clientId) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_bf_info_ID bfInfoID;
+    t_cm_error error;
+    t_component_instance *client;
+
+    OSAL_LOCK_API();
+
+    client = cm_lookupComponent(clientInstance);
+    // Sanity check
+    if((error = cm_checkValidUnbinding(client, requiredItfClientName,
+				       &itfRequire, &itfProvide)) != CM_OK)
+        goto out;
+
+    // Check if this is a Primitive binding
+    bfInfoID = itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfoID;
+    if(bfInfoID != BF_SYNCHRONOUS && bfInfoID != BF_TRACE)
+    {
+        error = CM_ILLEGAL_UNBINDING;
+        goto out;
+    }
+
+    // Check if we really need to unbind
+    if(cm_unregisterSingletonBinding(client, &itfRequire, &itfProvide, clientId))
+    {
+        (void)cm_EEM_ForceWakeup(itfRequire.client->Template->dspId);
+
+        if(bfInfoID == BF_SYNCHRONOUS)
+            cm_unbindInterface(&itfRequire);
+        else
+            cm_unbindInterfaceTrace(
+                    &itfRequire,
+                    (t_trace_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+
+        cm_EEM_AllowSleep(itfRequire.client->Template->dspId);
+
+        error = CM_OK;
+    }
+
+out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponentToVoid(
+    const t_cm_instance_handle clientInstance,
+    const char                 requiredItfClientName[MAX_INTERFACE_NAME_LENGTH],
+    t_nmf_client_id clientId)
+{
+    t_interface_require_description itfRequire;
+    t_bool bindable;
+    t_cm_error error;
+    t_component_instance *client;
+
+    OSAL_LOCK_API();
+
+    client = cm_lookupComponent(clientInstance);
+    // Check invalid binding
+    if((error = cm_checkValidClient(client, requiredItfClientName,
+                    &itfRequire, &bindable)) != CM_OK)
+        goto out;
+
+   // Check if we really need to bind
+    if(bindable)
+    {
+        if ((error = cm_EEM_ForceWakeup(itfRequire.client->Template->dspId)) != CM_OK)
+            goto out;
+
+        error = cm_bindInterfaceToVoid(&itfRequire);
+
+        cm_EEM_AllowSleep(itfRequire.client->Template->dspId);
+    }
+
+    cm_registerSingletonBinding(client, &itfRequire, NULL, clientId);
+
+out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponentAsynchronous(
+        const t_cm_instance_handle clientInstance,
+        const char* requiredItfClientName,
+        const t_cm_instance_handle serverInstance,
+        const char* providedItfServerName,
+        t_uint32 fifosize,
+        t_cm_mpc_memory_type eventMemType,
+        t_nmf_client_id clientId,
+        const char *dataFileSkeletonOrEvent,
+        const char *dataFileStub) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_dsp_memory_type_id dspEventMemType;
+    t_bool bindable;
+    t_cm_error error;
+    t_component_instance *client, *server;
+    t_elfdescription *elfhandleSkeletonOrEvent = NULL;
+    t_elfdescription *elfhandleStub = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFileSkeletonOrEvent != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileSkeletonOrEvent,
+                    TRUE,
+                    &elfhandleSkeletonOrEvent)) != CM_OK)
+        goto out;
+    if(dataFileStub != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileStub,
+                    TRUE,
+                    &elfhandleStub)) != CM_OK)
+        goto out;
+
+    client = cm_lookupComponent(clientInstance);
+    server = cm_lookupComponent(serverInstance);
+    // Check invalid binding
+    if((error = cm_checkValidBinding(client, requiredItfClientName,
+				     server, providedItfServerName,
+				     &itfRequire, &itfProvide, &bindable)) != CM_OK)
+        goto out;
+
+    switch(eventMemType)
+    {
+    case CM_MM_MPC_TCM24_X:
+        dspEventMemType = INTERNAL_XRAM24;
+        break;
+    case CM_MM_MPC_ESRAM24:
+        dspEventMemType = ESRAM_EXT24;
+        break;
+    case CM_MM_MPC_SDRAM24:
+        dspEventMemType = SDRAM_EXT24;
+        break;
+    default:
+        error = CM_INVALID_PARAMETER;
+        goto out;
+    }
+
+    // Check if we really need to bind
+    if(bindable)
+    {
+        // Create the binding and bind it to the client (or all sub-components clients ....)
+        if (itfRequire.client->Template->dspId != itfProvide.server->Template->dspId)
+        {
+            if ((error = cm_EEM_ForceWakeup(itfRequire.client->Template->dspId)) != CM_OK)
+                goto out;
+            if ((error = cm_EEM_ForceWakeup(itfProvide.server->Template->dspId)) != CM_OK)
+            {
+                cm_EEM_AllowSleep(itfRequire.client->Template->dspId);
+                goto out;
+            }
+
+            // This is a distribute communication
+            error = cm_bindInterfaceDistributed(
+                    &itfRequire,
+                    &itfProvide,
+                    fifosize,
+                    dspEventMemType,
+                    elfhandleSkeletonOrEvent,
+                    elfhandleStub);
+
+            cm_EEM_AllowSleep(itfRequire.client->Template->dspId);
+            cm_EEM_AllowSleep(itfProvide.server->Template->dspId);
+        }
+        else
+        {
+            if ((error = cm_EEM_ForceWakeup(itfRequire.client->Template->dspId)) != CM_OK)
+                goto out;
+
+            // This is a acynchronous communication
+            error = cm_bindInterfaceAsynchronous(
+                    &itfRequire,
+                    &itfProvide,
+                    fifosize,
+                    dspEventMemType,
+                    elfhandleSkeletonOrEvent);
+
+            cm_EEM_AllowSleep(itfRequire.client->Template->dspId);
+        }
+    }
+
+    cm_registerSingletonBinding(client, &itfRequire, &itfProvide, clientId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandleSkeletonOrEvent);
+    cm_ELF_CloseFile(TRUE, elfhandleStub);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentAsynchronous(
+        const t_cm_instance_handle instance,
+        const char* requiredItfClientName,
+        t_nmf_client_id clientId) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_bf_info_ID bfInfoID;
+    t_cm_error error;
+    t_component_instance *client;
+
+    OSAL_LOCK_API();
+
+    client = cm_lookupComponent(instance);
+    // Sanity check
+    if((error = cm_checkValidUnbinding(client, requiredItfClientName,
+				       &itfRequire, &itfProvide)) != CM_OK)
+        goto out;
+
+    bfInfoID = itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfoID;
+
+    // Check if we really need to unbind
+    if(cm_unregisterSingletonBinding(client, &itfRequire, &itfProvide, clientId))
+    {
+        // Check if this is a Asynchronous binding
+        if(bfInfoID == BF_DSP2DSP)
+        {
+            t_nmf_core_id clientDsp = itfRequire.client->Template->dspId;
+            t_nmf_core_id serverDsp = itfProvide.server->Template->dspId;
+
+            (void)cm_EEM_ForceWakeup(clientDsp);
+            (void)cm_EEM_ForceWakeup(serverDsp);
+
+            cm_unbindInterfaceDistributed(
+                    &itfRequire,
+                    (t_mpc2mpc_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+
+            cm_EEM_AllowSleep(clientDsp);
+            cm_EEM_AllowSleep(serverDsp);
+
+            error = CM_OK;
+        }
+        else if(bfInfoID == BF_ASYNCHRONOUS)
+        {
+            t_nmf_core_id clientDsp = itfRequire.client->Template->dspId;
+
+            (void)cm_EEM_ForceWakeup(clientDsp);
+
+            cm_unbindInterfaceAsynchronous(
+                    &itfRequire,
+                    (t_async_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo);
+
+            cm_EEM_AllowSleep(clientDsp);
+
+            error = CM_OK;
+        }
+        else
+            error = CM_ILLEGAL_UNBINDING;
+    }
+
+ out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponentFromCMCore(
+        const t_cm_instance_handle server,
+        const char* providedItfServerName,
+        t_uint32 fifosize,
+        t_cm_mpc_memory_type eventMemType,
+        t_cm_bf_host2mpc_handle *bfHost2mpcHdl,
+        t_nmf_client_id clientId,
+        const char *dataFileSkeleton) {
+    t_interface_provide_description itfProvide;
+    t_dsp_memory_type_id dspEventMemType;
+    t_cm_error error;
+    t_component_instance* component;
+    t_host2mpc_bf_info *bfInfo;
+    t_elfdescription *elfhandleSkeleton = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFileSkeleton != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileSkeleton,
+                    TRUE,
+                    &elfhandleSkeleton)) != CM_OK)
+        goto out;
+
+    component = cm_lookupComponent(server);
+    // Check server validity
+    if((error = cm_checkValidServer(component, providedItfServerName,
+                    &itfProvide)) != CM_OK)
+        goto out;
+
+    if ((error = cm_EEM_ForceWakeup(itfProvide.server->Template->dspId)) != CM_OK)
+        goto out;
+
+    switch(eventMemType)
+    {
+    case CM_MM_MPC_TCM24_X:
+        dspEventMemType = INTERNAL_XRAM24;
+        break;
+    case CM_MM_MPC_ESRAM24:
+        dspEventMemType = ESRAM_EXT24;
+        break;
+    case CM_MM_MPC_SDRAM24:
+        dspEventMemType = SDRAM_EXT24;
+        break;
+    default:
+        error = CM_INVALID_PARAMETER;
+        goto out;
+    }
+
+    error = cm_bindComponentFromCMCore(&itfProvide,
+				       fifosize,
+				       dspEventMemType,
+				       elfhandleSkeleton,
+				       &bfInfo);
+
+    cm_EEM_AllowSleep(itfProvide.server->Template->dspId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandleSkeleton);
+    OSAL_UNLOCK_API();
+
+    if (error == CM_OK) {
+	    bfInfo->clientId = clientId;
+	    OSAL_LOCK_COM();
+	    *bfHost2mpcHdl = cm_addEntry(&Host2MpcBindingTable, bfInfo);
+	    if (*bfHost2mpcHdl == 0)
+		    error = CM_NO_MORE_MEMORY;
+	    OSAL_UNLOCK_COM();
+
+	    if (error != CM_OK) {
+		    OSAL_LOCK_API();
+		    (void)cm_EEM_ForceWakeup(itfProvide.server->Template->dspId);
+		    cm_unbindComponentFromCMCore(bfInfo);
+		    cm_EEM_AllowSleep(itfProvide.server->Template->dspId);
+		    OSAL_UNLOCK_API();
+	    }
+    }
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentFromCMCore(
+        t_cm_bf_host2mpc_handle bfHost2mpcId) {
+	t_host2mpc_bf_info* bfInfo;
+	t_nmf_core_id coreId;
+
+    OSAL_LOCK_COM();
+    bfInfo = cm_lookupEntry(&Host2MpcBindingTable, bfHost2mpcId);
+    if (bfInfo)
+	    cm_delEntry(&Host2MpcBindingTable, bfHost2mpcId & INDEX_MASK);
+    OSAL_UNLOCK_COM();
+    if (NULL == bfInfo)
+	    return  CM_INVALID_PARAMETER;
+
+    OSAL_LOCK_API();
+
+    // Check if this is a DSP to Host binding
+    //if(bfInfo->id != BF_HOST2DSP)
+    //    return CM_ILLEGAL_UNBINDING;
+    coreId = bfInfo->dspskeleton.skelInstance->Template->dspId;
+
+    (void)cm_EEM_ForceWakeup(coreId);
+
+    cm_unbindComponentFromCMCore(bfInfo);
+
+    cm_EEM_AllowSleep(coreId);
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_BindComponentToCMCore(
+        const t_cm_instance_handle  instance,
+        const char                  *requiredItfClientName,
+        t_uint32                    fifosize,
+        t_nmf_mpc2host_handle       upLayerThis,
+        const char                  *dataFileStub,
+        t_cm_bf_mpc2host_handle     *mpc2hostId,
+        t_nmf_client_id             clientId) {
+    t_interface_require_description itfRequire;
+    t_bool bindable;
+    t_cm_error error;
+    t_component_instance* client;
+    t_elfdescription *elfhandleStub = NULL;
+
+    OSAL_LOCK_API();
+
+    /*
+     * Load Elf File
+     */
+    if(dataFileStub != NULL &&
+            (error = cm_ELF_CheckFile(
+                    dataFileStub,
+                    TRUE,
+                    &elfhandleStub)) != CM_OK)
+        goto out;
+
+    client = cm_lookupComponent(instance);
+    // Check invalid binding
+    if((error = cm_checkValidClient(client, requiredItfClientName,
+                    &itfRequire, &bindable)) != CM_OK)
+        goto out;
+
+    // Check if we really need to bind
+    if(bindable)
+    {
+        if ((error = cm_EEM_ForceWakeup(itfRequire.client->Template->dspId)) != CM_OK)
+            goto out;
+
+        error = cm_bindComponentToCMCore(
+                &itfRequire,
+                fifosize,
+                upLayerThis,
+                elfhandleStub,
+                (t_mpc2host_bf_info**)mpc2hostId);
+
+        cm_EEM_AllowSleep(itfRequire.client->Template->dspId);
+    }
+
+    cm_registerSingletonBinding(client, &itfRequire, NULL, clientId);
+
+out:
+    cm_ELF_CloseFile(TRUE, elfhandleStub);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_UnbindComponentToCMCore(
+        const t_cm_instance_handle      instance,
+        const char                      *requiredItfClientName,
+        t_nmf_mpc2host_handle           *upLayerThis,
+        t_nmf_client_id clientId) {
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_cm_error error;
+    t_mpc2host_bf_info *bfInfo;
+    t_component_instance* client;
+
+    OSAL_LOCK_API();
+
+    client = cm_lookupComponent(instance);
+    // Sanity check
+    if((error = cm_checkValidUnbinding(client, requiredItfClientName,
+				       &itfRequire, &itfProvide)) != CM_OK)
+        goto out;
+
+    // Check if this is a DSP to Host binding
+    if(itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfoID != BF_DSP2HOST)
+    {
+        error = CM_ILLEGAL_UNBINDING;
+        goto out;
+    }
+
+    bfInfo = (t_mpc2host_bf_info*)itfRequire.client->interfaceReferences[itfRequire.requireIndex][itfRequire.collectionIndex].bfInfo;
+
+    // Get client information
+    *upLayerThis = bfInfo->context;
+
+    // Check if we really need to unbind
+    if(cm_unregisterSingletonBinding(client, &itfRequire, &itfProvide, clientId))
+    {
+        (void)cm_EEM_ForceWakeup(itfRequire.client->Template->dspId);
+
+        cm_unbindComponentToCMCore(&itfRequire, bfInfo);
+
+        cm_EEM_AllowSleep(itfRequire.client->Template->dspId);
+
+        error = CM_OK;
+    }
+out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_event_params_handle CM_ENGINE_AllocEvent(t_cm_bf_host2mpc_handle host2mpcId) {
+    t_host2mpc_bf_info* bfInfo;
+    t_event_params_handle eventHandle;
+
+    OSAL_LOCK_COM();
+    bfInfo = cm_lookupEntry(&Host2MpcBindingTable, host2mpcId);
+    if (NULL == bfInfo) {
+	    OSAL_UNLOCK_COM();
+	    return NULL;
+    }
+
+    if(bfInfo->dspskeleton.skelInstance->interfaceReferences[0][0].instance->state != STATE_RUNNABLE) {
+        ERROR("CM_COMPONENT_NOT_STARTED: Call interface before start component %s<%s>\n",
+                bfInfo->dspskeleton.skelInstance->pathname,
+                bfInfo->dspskeleton.skelInstance->Template->name, 0, 0, 0, 0);
+    }
+
+    eventHandle = cm_AllocEvent(bfInfo->fifo);
+
+    OSAL_UNLOCK_COM();
+
+    return eventHandle;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_PushEvent(t_cm_bf_host2mpc_handle host2mpcId, t_event_params_handle h, t_uint32 methodIndex) {
+	t_host2mpc_bf_info* bfInfo;
+    t_cm_error error;
+
+    OSAL_LOCK_COM();
+    bfInfo = cm_lookupEntry(&Host2MpcBindingTable, host2mpcId);
+    if (NULL == bfInfo) {
+	    OSAL_UNLOCK_COM();
+	    return CM_INVALID_PARAMETER;
+    }
+    error = cm_PushEvent(bfInfo->fifo, h, methodIndex);
+    OSAL_UNLOCK_COM();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED void CM_ENGINE_AcknowledgeEvent(t_cm_bf_mpc2host_handle mpc2hostId) {
+    t_mpc2host_bf_info* bfInfo = (t_mpc2host_bf_info*)mpc2hostId;
+
+    //t_dsp2host_bf_info* bfInfo = (t_host2mpc_bf_info*)mpc2hostId;
+    OSAL_LOCK_COM();
+    cm_AcknowledgeEvent(bfInfo->fifo);
+    OSAL_UNLOCK_COM();
+}
+
+/*
+ * Get a reference on a given attribute of a given component
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_ReadComponentAttribute(
+        const t_cm_instance_handle instance,
+        const char* attrName,
+        t_uint24 *attrValue)
+{
+    t_cm_error error;
+    t_component_instance* component;
+
+    OSAL_LOCK_API();
+
+    component = cm_lookupComponent(instance);
+    if (NULL == component)
+        error = CM_INVALID_COMPONENT_HANDLE;
+    else
+    {
+        if ((error = cm_EEM_ForceWakeup(component->Template->dspId)) != CM_OK)
+            goto out;
+
+        // t_uint24 -> t_uint32 possible since we know it same size
+        error = cm_readAttribute(component, attrName, (t_uint32*)attrValue);
+
+        cm_EEM_AllowSleep(component->Template->dspId);
+    }
+
+out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+/*
+ * Get a reference on a given attribute of a given component
+ */
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_WriteComponentAttribute(
+        const t_cm_instance_handle instance,
+        const char* attrName,
+        t_uint24 attrValue)
+{
+    t_cm_error error;
+    t_component_instance* component;
+
+    OSAL_LOCK_API();
+
+    component = cm_lookupComponent(instance);
+    if (NULL == component)
+        error = CM_INVALID_COMPONENT_HANDLE;
+    else
+    {
+        if ((error = cm_EEM_ForceWakeup(component->Template->dspId)) != CM_OK)
+            goto out;
+
+        //t_uint24 -> t_uint32 possible since we know it same size
+        error = cm_writeAttribute(component, attrName, attrValue);
+
+        cm_EEM_AllowSleep(component->Template->dspId);
+    }
+
+out:
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+/*===============================================================================
+ * Introspection API
+ *===============================================================================*/
+/*
+ * Component
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentListHeader(
+    const t_nmf_client_id       client,
+    t_cm_instance_handle        *headerComponent) {
+    t_uint32 i;
+
+    OSAL_LOCK_API();
+
+    *headerComponent = 0;
+    for (i=0; i < ComponentTable.idxMax; i++) {
+	    if ((componentEntry(i) != NULL) &&
+		(componentEntry(i)->Template->classe != FIRMWARE) &&
+		(domainDesc[componentEntry(i)->domainId].client == client)) {
+		    *headerComponent = ENTRY2HANDLE(componentEntry(i), i);;
+		    break;  
+	    }
+    }
+
+    OSAL_UNLOCK_API();
+
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentListNext(
+    const t_nmf_client_id       client,
+    const t_cm_instance_handle  prevComponent,
+    t_cm_instance_handle        *nextComponent){
+    t_cm_error error;
+    t_uint32 i = prevComponent & INDEX_MASK;
+
+    OSAL_LOCK_API();
+
+    // Sanity check
+    if ((i >= ComponentTable.idxMax)
+	|| (((unsigned int)componentEntry(i) << INDEX_SHIFT) != (prevComponent & ~INDEX_MASK)))
+        error =  CM_INVALID_COMPONENT_HANDLE;
+    else {
+	*nextComponent = 0;
+	for (i++; i < ComponentTable.idxMax; i++) {
+	    if ((componentEntry(i) != NULL) &&
+		(componentEntry(i)->Template->classe != FIRMWARE) &&
+		(domainDesc[componentEntry(i)->domainId].client == client)) {
+		    *nextComponent = ENTRY2HANDLE(componentEntry(i), i);;
+		    break;  
+	    }
+	}
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentDescription(
+        const t_cm_instance_handle  instance,
+        char                        *templateName,
+        t_uint32                    templateNameLength,
+        t_nmf_core_id               *coreId,
+        char                        *localName,
+        t_uint32                    localNameLength,
+	t_nmf_ee_priority           *priority) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else {
+        cm_StringCopy(
+                templateName,
+                comp->Template->name,
+                templateNameLength);
+        *coreId = comp->Template->dspId;
+        cm_StringCopy(
+                localName,
+                comp->pathname,
+                localNameLength);
+	if (priority)
+		*priority = comp->priority;
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+/*
+ * Require interface
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterfaceNumber(
+    const t_cm_instance_handle  instance,
+    t_uint8                     *numberRequiredInterfaces) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else {
+        *numberRequiredInterfaces = comp->Template->requireNumber;
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterface(
+        const t_cm_instance_handle  instance,
+        const t_uint8               index,
+        char                        *itfName,
+        t_uint32                    itfNameLength,
+        char                        *itfType,
+        t_uint32                    itfTypeLength,
+        t_cm_require_state          *requireState,
+        t_sint16                    *collectionSize) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else if(index >= comp->Template->requireNumber) {
+        error = CM_NO_SUCH_REQUIRED_INTERFACE;
+    } else {
+        cm_StringCopy(
+                itfName,
+                comp->Template->requires[index].name,
+                itfNameLength);
+        cm_StringCopy(
+                itfType,
+                comp->Template->requires[index].interface->type,
+                itfTypeLength);
+        if(comp->Template->requires[index].requireTypes & COLLECTION_REQUIRE)
+            *collectionSize = comp->Template->requires[index].collectionSize;
+        else
+            *collectionSize = -1;
+
+	if(requireState != NULL) {
+		*requireState = 0;
+		if(comp->Template->requires[index].requireTypes & COLLECTION_REQUIRE)
+			*requireState |= CM_REQUIRE_COLLECTION;
+		if(comp->Template->requires[index].requireTypes & OPTIONAL_REQUIRE)
+                	*requireState |= CM_REQUIRE_OPTIONAL;
+		if(comp->Template->requires[index].requireTypes & STATIC_REQUIRE)
+                	*requireState |= CM_REQUIRE_STATIC;
+	}
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentRequiredInterfaceBinding(
+        const t_cm_instance_handle  instance,
+        const char                  *itfName,
+        t_cm_instance_handle        *server,
+        char                        *serverItfName,
+        t_uint32                    serverItfNameLength) {
+    t_component_instance *comp;
+    t_interface_require_description itfRequire;
+    t_interface_provide_description itfProvide;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if(NULL == comp) {
+         error = CM_INVALID_COMPONENT_HANDLE;
+    } else if ((error = cm_getRequiredInterface(comp, itfName, &itfRequire)) != CM_OK) {
+        // Check if the requiredItfClientName is required by client component
+    } else if ((error = cm_lookupInterface(&itfRequire, &itfProvide)) != CM_OK) {
+        // Check if the requiredItfClientName is required by client component
+    } else {
+        if ((t_cm_instance_handle)itfProvide.server == NMF_HOST_COMPONENT
+               || (t_cm_instance_handle)itfProvide.server == NMF_VOID_COMPONENT)
+            *server = (t_cm_instance_handle)itfProvide.server;
+        else
+            *server = itfProvide.server->instance;
+        if(*server == NMF_HOST_COMPONENT) {
+            cm_StringCopy(
+                    serverItfName,
+                    "unknown",
+                    serverItfNameLength);
+        } else if(*server == NMF_VOID_COMPONENT) {
+            cm_StringCopy(
+                    serverItfName,
+                    "void",
+                    serverItfNameLength);
+        } else if(*server != 0) {
+            cm_StringCopy(
+                    serverItfName,
+                    itfProvide.server->Template->provides[itfProvide.provideIndex].name,
+                    serverItfNameLength);
+            if(itfProvide.server->Template->provides[itfProvide.provideIndex].provideTypes & COLLECTION_PROVIDE) {
+                int len = cm_StringLength(serverItfName, serverItfNameLength);
+                serverItfName[len++] = '[';
+                if(itfProvide.collectionIndex >= 100)
+                    serverItfName[len++] = '0' + (itfProvide.collectionIndex / 100);
+                if(itfProvide.collectionIndex >= 10)
+                    serverItfName[len++] = '0' + ((itfProvide.collectionIndex % 100) / 10);
+                serverItfName[len++] = '0' + (itfProvide.collectionIndex % 10);
+                serverItfName[len++] = ']';
+                serverItfName[len] = 0;
+            }
+        }
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+/*
+ * Provide interface
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentProvidedInterfaceNumber(
+    const t_cm_instance_handle  instance,
+    t_uint8                     *numberProvidedInterfaces) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else {
+        *numberProvidedInterfaces = comp->Template->provideNumber;
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentProvidedInterface(
+        const t_cm_instance_handle  instance,
+        const t_uint8               index,
+        char                        *itfName,
+        t_uint32                    itfNameLength,
+        char                        *itfType,
+        t_uint32                    itfTypeLength,
+        t_sint16                    *collectionSize) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else if(index >= comp->Template->provideNumber) {
+        error = CM_NO_SUCH_PROVIDED_INTERFACE;
+    } else {
+        cm_StringCopy(
+                itfName,
+                comp->Template->provides[index].name,
+                itfNameLength);
+        cm_StringCopy(
+                itfType,
+                comp->Template->provides[index].interface->type,
+                itfTypeLength);
+        if(comp->Template->provides[index].provideTypes & COLLECTION_PROVIDE)
+            *collectionSize = comp->Template->provides[index].collectionSize;
+        else
+            *collectionSize = -1;
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+/*
+ * Component Property
+ */
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyNumber(
+    const t_cm_instance_handle  instance,
+    t_uint8                     *numberProperties) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else {
+        *numberProperties = comp->Template->propertyNumber;
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyName(
+        const t_cm_instance_handle  instance,
+        const t_uint8               index,
+        char                        *propertyName,
+        t_uint32                    propertyNameLength) {
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    // Sanity check
+    if (NULL == comp) {
+        error = CM_INVALID_COMPONENT_HANDLE;
+    } else if(index >= comp->Template->propertyNumber) {
+        error = CM_NO_SUCH_PROPERTY;
+    } else {
+        cm_StringCopy(
+                propertyName,
+                comp->Template->properties[index].name,
+                propertyNameLength);
+
+        error = CM_OK;
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetComponentPropertyValue(
+        const t_cm_instance_handle  instance,
+        const char                  *propertyName,
+        char                        *propertyValue,
+        t_uint32                    propertyValueLength)
+{
+    t_component_instance *comp;
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+
+    comp = cm_lookupComponent(instance);
+    if (NULL == comp)
+        error = CM_INVALID_COMPONENT_HANDLE;
+    else
+    {
+        error = cm_getComponentProperty(
+            comp,
+            propertyName,
+            propertyValue,
+            propertyValueLength);
+
+        if(error == CM_NO_SUCH_PROPERTY)
+            ERROR("CM_NO_SUCH_PROPERTY(%s, %s)\n", comp->pathname, propertyName, 0, 0, 0, 0);
+    }
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/dspevent.c b/drivers/staging/nmf-cm/cm/engine/component/src/dspevent.c
new file mode 100644
index 00000000000..0d5e89e0515
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/dspevent.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+#include <cm/engine/trace/inc/trace.h>
+#include "../inc/dspevent.h"
+
+
+#define DSP_REMOTE_EVENT_SIZE_IN_BYTE (4*DSP_REMOTE_EVENT_SIZE_IN_DSPWORD)
+#define DSP_REMOTE_EVENT_NEXT_FIELD_OFFSET      0
+#define DSP_REMOTE_EVENT_REACTION_FIELD_OFFSET  1
+#define DSP_REMOTE_EVENT_THIS_FIELD_OFFSET      2
+#define DSP_REMOTE_EVENT_PRIORITY_FIELD_OFFSET  3
+#define DSP_REMOTE_EVENT_DATA_FIELD_OFFSET      4
+
+t_cm_error dspevent_createDspEventFifo(
+    const t_component_instance *pComp,
+    const char* nameOfTOP,
+    t_uint32 fifoNbElem,
+    t_uint32 fifoElemSizeInWord,
+    t_dsp_memory_type_id dspEventMemType,
+    t_memory_handle *pHandle)
+{
+    t_uint32 dspElementAddr;
+    t_uint32 *elemAddr32;
+    int i;
+
+    // Allocate fifo
+    *pHandle = cm_DM_Alloc(pComp->domainId, dspEventMemType, fifoNbElem*fifoElemSizeInWord, CM_MM_ALIGN_2WORDS, TRUE);
+    if(*pHandle == INVALID_MEMORY_HANDLE) {
+        ERROR("CM_NO_MORE_MEMORY: dspevent_createDspEventFifo()\n", 0, 0, 0, 0, 0, 0);
+        return 	CM_NO_MORE_MEMORY;
+    }
+
+    cm_DSP_GetDspAddress(*pHandle, &dspElementAddr);
+
+    elemAddr32 = (t_uint32*)cm_DSP_GetHostLogicalAddress(*pHandle);
+
+    LOG_INTERNAL(2, "\n##### FIFO (dsp event): ARM=0x%x DSP=0x%x\n", elemAddr32, dspElementAddr, 0, 0, 0, 0);
+
+    // Read attribute addr (we assume that variable in XRAM)
+    cm_writeAttribute(pComp, nameOfTOP, dspElementAddr);
+
+    // Initialise the linked list (next...)
+    for (i = 0; i < fifoNbElem - 1; i++)
+    {
+        dspElementAddr += fifoElemSizeInWord;
+
+        /* Write next field  */
+        *elemAddr32 = dspElementAddr;
+        /* Write THIS field & priority field */
+        *(volatile t_uint64*)&elemAddr32[DSP_REMOTE_EVENT_THIS_FIELD_OFFSET] =
+                ((t_uint64)pComp->thisAddress | (((t_uint64)pComp->priority) << 32));
+
+        elemAddr32 += fifoElemSizeInWord;
+    }
+
+    /* Last element: Write next field */
+    *elemAddr32 = 0x0 /* NULL */;
+    /* Last element: Write THIS field & priority field */
+    *(volatile t_uint64*)&elemAddr32[DSP_REMOTE_EVENT_THIS_FIELD_OFFSET] =
+            ((t_uint64)pComp->thisAddress | (((t_uint64)pComp->priority) << 32));
+
+    return CM_OK;
+}
+
+
+
+void dspevent_destroyDspEventFifo(t_memory_handle handle)
+{
+    (void)cm_DM_Free(handle, TRUE);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/initializer.c b/drivers/staging/nmf-cm/cm/engine/component/src/initializer.c
new file mode 100644
index 00000000000..7f99b710401
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/initializer.c
@@ -0,0 +1,383 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+
+#include <cm/engine/power_mgt/inc/power.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#include <cm/engine/trace/inc/trace.h>
+
+#include "../inc/dspevent.h"
+#include "../inc/initializer.h"
+
+// Since now due to semaphore use call is synchrone so we only need a fifo size of three
+// (due to updateStack + (InstructionCacheLock or InstructionCacheUnlock))
+#define DEFAULT_INITIALIZER_FIFO_SIZE 3
+
+/* private prototype */
+PRIVATE t_cm_error cm_COMP_generic(t_nmf_core_id coreId, t_event_params_handle paramArray, t_uint32 paramNumber, t_uint32 serviceIndex);
+PRIVATE void cm_COMP_generatePanic(t_nmf_core_id coreId);
+
+/*
+ * This module is tightly coupled with cm_DSP_components one (communication/initializer)
+ */
+static struct {
+    t_nmf_fifo_arm_desc* downlinkFifo;
+    t_nmf_fifo_arm_desc* uplinkFifo;
+    t_memory_handle dspfifoHandle;
+    t_nmf_osal_sem_handle fifoSemHandle;
+    t_uint32 servicePending;                // TODO : Use sem counter instead of defining such variable (need to create new OSAL)
+} initializerDesc[NB_CORE_IDS];
+
+PUBLIC t_cm_error cm_COMP_INIT_Init(t_nmf_core_id coreId)
+{
+    t_uint32 i;
+    t_cm_error error;
+    t_component_instance *ee;
+    t_dsp_offset sharedVarOffset;
+    t_interface_provide_description itfProvide;
+    t_interface_provide* provide;
+    t_interface_provide_loaded* provideLoaded;
+
+    ee = cm_EEM_getExecutiveEngine(coreId)->instance;
+
+    // Get interface description
+    if((error = cm_getProvidedInterface(ee, "service", &itfProvide)) != CM_OK)
+        return error;
+    provide = &ee->Template->provides[itfProvide.provideIndex];
+    provideLoaded = &ee->Template->providesLoaded[itfProvide.provideIndex];
+
+
+    if ((error = dspevent_createDspEventFifo(
+                    ee, "comms/TOP",
+                    DEFAULT_INITIALIZER_FIFO_SIZE,
+                    DSP_REMOTE_EVENT_SIZE_IN_DSPWORD,
+                    INTERNAL_XRAM24,
+                    &initializerDesc[coreId].dspfifoHandle)) != CM_OK)
+        return error;
+
+    /* create fifo semaphore */
+    initializerDesc[coreId].servicePending = 0;
+    initializerDesc[coreId].fifoSemHandle = OSAL_CreateSemaphore(DEFAULT_INITIALIZER_FIFO_SIZE);
+    if (initializerDesc[coreId].fifoSemHandle == 0) {
+        dspevent_destroyDspEventFifo(initializerDesc[coreId].dspfifoHandle);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    /* static armTHis initialisation */
+    /*
+     * In the two next fifo_alloc call (1+n) means that we want to manage the hostThis_or_TOP and one method for each params fifos */
+    initializerDesc[coreId].downlinkFifo =
+        fifo_alloc(ARM_CORE_ID, coreId,
+                INIT_COMPONENT_CMD_SIZE, DEFAULT_INITIALIZER_FIFO_SIZE,
+                (1+provide->interface->methodNumber), paramsLocation,  extendedFieldLocation, cm_DSP_GetState(coreId)->domainEE);
+    if (initializerDesc[coreId].downlinkFifo == NULL)
+    {
+        OSAL_DestroySemaphore(initializerDesc[coreId].fifoSemHandle);
+        dspevent_destroyDspEventFifo(initializerDesc[coreId].dspfifoHandle);
+        ERROR("CM_NO_MORE_MEMORY: fifo_alloc() failed in cm_COMP_INIT_Init()\n", 0, 0, 0, 0, 0, 0);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    initializerDesc[coreId].uplinkFifo =
+        fifo_alloc(coreId, ARM_CORE_ID,
+                INIT_COMPONENT_ACK_SIZE, DEFAULT_INITIALIZER_FIFO_SIZE,
+                (1), paramsLocation, extendedFieldLocation, cm_DSP_GetState(coreId)->domainEE);              /* 1 is mandatory to compute internally the indexMask */
+                                                                                              /* this statement is acceptable only written by skilled man ;) */
+                                                                                              /* We don't used bcDescRef, since we assume that we don't need params size */
+    if (initializerDesc[coreId].uplinkFifo == NULL)
+    {
+        OSAL_DestroySemaphore(initializerDesc[coreId].fifoSemHandle);
+        fifo_free(initializerDesc[coreId].downlinkFifo);
+        dspevent_destroyDspEventFifo(initializerDesc[coreId].dspfifoHandle);
+        ERROR("CM_NO_MORE_MEMORY: fifo_alloc() failed in cm_COMP_INIT_Init()\n", 0, 0, 0, 0, 0, 0);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    cm_writeAttribute(ee, "comms/FIFOcmd", initializerDesc[coreId].downlinkFifo->dspAdress);
+
+    cm_writeAttribute(ee, "comms/FIFOack", initializerDesc[coreId].uplinkFifo->dspAdress);
+
+    sharedVarOffset = cm_getAttributeMpcAddress(ee, "comms/TOP");
+
+    /* HOST->DSP ParamsFifo extended fields initialisation */
+    fifo_params_setSharedField(
+            initializerDesc[coreId].downlinkFifo,
+            0,
+            (t_shared_field)sharedVarOffset /* TOP DSP Address */
+    );
+    for(i=0; i<provide->interface->methodNumber; i++)
+    {
+        fifo_params_setSharedField(
+                initializerDesc[coreId].downlinkFifo,
+                i + 1,
+                provideLoaded->indexesLoaded[itfProvide.collectionIndex][i].methodAddresses);
+    }
+
+    /* DSP->HOST ParamsFifo extended fields initialisation */
+    fifo_params_setSharedField(
+            initializerDesc[coreId].uplinkFifo,
+            0,
+            (t_shared_field)NMF_INTERNAL_USERTHIS
+            );
+
+    return CM_OK;
+}
+
+
+PUBLIC t_cm_error cm_COMP_CallService(
+        int serviceIndex,
+        t_component_instance *pComp,
+        t_uint32 methodAddress) {
+    t_cm_error error;
+    t_uint16 params[INIT_COMPONENT_CMD_SIZE];
+    t_bool isSynchronous = (serviceIndex == NMF_CONSTRUCT_SYNC_INDEX ||
+                            serviceIndex == NMF_START_SYNC_INDEX ||
+                            serviceIndex == NMF_STOP_SYNC_INDEX ||
+                            serviceIndex == NMF_DESTROY_INDEX)?TRUE:FALSE;
+
+    params[INIT_COMPONENT_CMD_HANDLE_INDEX] = (t_uint16)((unsigned int)pComp & 0xFFFF);
+    params[INIT_COMPONENT_CMD_HANDLE_INDEX+1] =  (t_uint16)((unsigned int)pComp >> 16);
+    params[INIT_COMPONENT_CMD_THIS_INDEX] =  (t_uint16)(pComp->thisAddress & 0xFFFF);
+    params[INIT_COMPONENT_CMD_THIS_INDEX+1] =  (t_uint16)(pComp->thisAddress >> 16);
+    params[INIT_COMPONENT_CMD_METHOD_INDEX] =  (t_uint16)(methodAddress & 0xFFFF);
+    params[INIT_COMPONENT_CMD_METHOD_INDEX+1] =  (t_uint16)(methodAddress >> 16);
+
+    error = cm_COMP_generic(pComp->Template->dspId, params, sizeof(params) / sizeof(t_uint16), serviceIndex);
+
+    if (isSynchronous == TRUE && error == CM_OK) {
+        if (OSAL_SEMAPHORE_WAIT_TIMEOUT(semHandle) != SYNC_OK) {
+            cm_COMP_generatePanic(pComp->Template->dspId);
+            error = CM_MPC_NOT_RESPONDING;
+        }
+    }
+
+    return error;
+}
+
+PUBLIC void cm_COMP_Flush(t_nmf_core_id coreId) {
+
+    if(initializerDesc[coreId].servicePending > 0)
+    {
+        t_uint16 params[INIT_COMPONENT_CMD_SIZE];
+        t_uint32 methodAddress = cm_EEM_getExecutiveEngine(coreId)->voidAddr;
+
+        // If service still pending on MMDSP side, send a flush command (today, we reuse Destroy to not create new empty service)
+        // When we receive the result, this mean that we have flushed all previous request.
+
+        params[INIT_COMPONENT_CMD_HANDLE_INDEX] = (t_uint16)(0x0 & 0xFFFF);
+        params[INIT_COMPONENT_CMD_HANDLE_INDEX+1] =  (t_uint16)(0x0 >> 16);
+        params[INIT_COMPONENT_CMD_THIS_INDEX] =  (t_uint16)(0x0 & 0xFFFF);
+        params[INIT_COMPONENT_CMD_THIS_INDEX+1] =  (t_uint16)(0x0 >> 16);
+        params[INIT_COMPONENT_CMD_METHOD_INDEX] =  (t_uint16)(methodAddress & 0xFFFF);
+        params[INIT_COMPONENT_CMD_METHOD_INDEX+1] =  (t_uint16)(methodAddress >> 16);
+
+        if (cm_COMP_generic(coreId, params, sizeof(params) / sizeof(t_uint16), NMF_DESTROY_INDEX) != CM_OK ||
+                OSAL_SEMAPHORE_WAIT_TIMEOUT(semHandle) != SYNC_OK)
+        {
+            cm_COMP_generatePanic(coreId);
+            ERROR("CM_MPC_NOT_RESPONDING: can't call flush service\n", 0, 0, 0, 0, 0, 0);
+        }
+    }
+}
+
+PUBLIC void cm_COMP_INIT_Close(t_nmf_core_id coreId)
+{
+    unsigned int i;
+
+    /* wait for semaphore to be sure it would not be touch later on */
+    /* in case of timeout we break and try to clean everythink */
+    for(i = 0; i < DEFAULT_INITIALIZER_FIFO_SIZE; i++) {
+        if (OSAL_SEMAPHORE_WAIT_TIMEOUT(initializerDesc[coreId].fifoSemHandle) != SYNC_OK)
+            break;
+    }
+
+    /* destroy semaphore */
+    OSAL_DestroySemaphore(initializerDesc[coreId].fifoSemHandle);
+
+    /* Unallocate initializerDesc[index].uplinkFifo */
+    /* (who is used in this particular case to store dummy (with no data space (only descriptor)) DSP->HOST params fifo */
+    fifo_free(initializerDesc[coreId].uplinkFifo);
+
+    /* Unallocate initializerDesc[index].downlinkFifo */
+    fifo_free(initializerDesc[coreId].downlinkFifo);
+
+    /* Unallocate initializerDesc[index].dspfifoHandle */
+    dspevent_destroyDspEventFifo(initializerDesc[coreId].dspfifoHandle);
+}
+
+PUBLIC void processAsyncAcknowledge(t_nmf_core_id coreId, t_event_params_handle pParam)
+{
+    cm_AcknowledgeEvent(initializerDesc[coreId].uplinkFifo);
+
+    initializerDesc[coreId].servicePending--;
+    OSAL_SemaphorePost(initializerDesc[coreId].fifoSemHandle,1);
+}
+
+PUBLIC void processSyncAcknowledge(t_nmf_core_id coreId, t_event_params_handle pParam)
+{
+    cm_AcknowledgeEvent(initializerDesc[coreId].uplinkFifo);
+
+    initializerDesc[coreId].servicePending--;
+    OSAL_SemaphorePost(initializerDesc[coreId].fifoSemHandle,1);
+    OSAL_SemaphorePost(semHandle,1);
+}
+
+PUBLIC t_cm_error cm_COMP_UpdateStack(
+    t_nmf_core_id coreId,
+    t_uint32 stackSize
+)
+{
+    t_uint16 params[2];
+
+    // Marshall parameter
+    params[0] =  (t_uint16)((unsigned int)stackSize & 0xFFFF);
+    params[1] =  (t_uint16)((unsigned int)stackSize >> 16);
+
+    return cm_COMP_generic(coreId, params, sizeof(params) / sizeof(t_uint16), NMF_UPDATE_STACK);
+}
+
+PUBLIC t_cm_error cm_COMP_ULPForceWakeup(
+    t_nmf_core_id coreId
+)
+{
+    t_cm_error error;
+
+    error = cm_COMP_generic(coreId, NULL, 0, NMF_ULP_FORCEWAKEUP);
+
+    if (error == CM_OK) {
+        if (OSAL_SEMAPHORE_WAIT_TIMEOUT(semHandle) != SYNC_OK) {
+            cm_COMP_generatePanic(coreId);
+            error = CM_MPC_NOT_RESPONDING;
+        }
+    }
+
+    return error;
+}
+
+PUBLIC t_cm_error cm_COMP_ULPAllowSleep(
+    t_nmf_core_id coreId
+)
+{
+    return cm_COMP_generic(coreId, NULL, 0, NMF_ULP_ALLOWSLEEP);
+}
+
+PUBLIC t_cm_error cm_COMP_InstructionCacheLock(
+    t_nmf_core_id coreId,
+    t_uint32 mmdspAddr,
+    t_uint32 mmdspSize
+)
+{
+    t_uint16 params[4];
+    t_uint32 startAddr = cm_DSP_GetState(coreId)->locked_offset;
+    int way;
+
+    for(way = 1; startAddr < mmdspAddr + mmdspSize; startAddr += MMDSP_CODE_CACHE_WAY_SIZE, way++)
+    {
+        if(mmdspAddr < startAddr + MMDSP_CODE_CACHE_WAY_SIZE)
+        {
+            t_cm_error error;
+
+            // Marshall parameter
+            params[0] =  (t_uint16)((unsigned int)startAddr & 0xFFFF);
+            params[1] =  (t_uint16)((unsigned int)startAddr >> 16);
+            params[2] =  (t_uint16)((unsigned int)way & 0xFFFF);
+            params[3] =  (t_uint16)((unsigned int)way >> 16);
+
+             if((error = cm_COMP_generic(coreId, params, sizeof(params) / sizeof(t_uint16), NMF_LOCK_CACHE)) != CM_OK)
+                 return error;
+        }
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_COMP_InstructionCacheUnlock(
+    t_nmf_core_id coreId,
+    t_uint32 mmdspAddr,
+    t_uint32 mmdspSize
+)
+{
+    t_uint16 params[2];
+    t_uint32 startAddr = cm_DSP_GetState(coreId)->locked_offset;
+    int way;
+
+    for(way = 1; startAddr < mmdspAddr + mmdspSize; startAddr += MMDSP_CODE_CACHE_WAY_SIZE, way++)
+    {
+        if(mmdspAddr < startAddr + MMDSP_CODE_CACHE_WAY_SIZE)
+        {
+            t_cm_error error;
+
+            // Marshall parameter
+            params[0] =  (t_uint16)((unsigned int)way & 0xFFFF);
+            params[1] =  (t_uint16)((unsigned int)way >> 16);
+
+             if((error = cm_COMP_generic(coreId, params, sizeof(params) / sizeof(t_uint16), NMF_UNLOCK_CACHE)) != CM_OK)
+                 return error;
+        }
+    }
+
+    return CM_OK;
+}
+
+/* private method */
+PRIVATE t_cm_error cm_COMP_generic(
+        t_nmf_core_id coreId,
+    t_event_params_handle paramArray,
+    t_uint32 paramNumber,
+    t_uint32 serviceIndex
+)
+{
+    t_event_params_handle _xyuv_data;
+    t_cm_error error;
+    t_uint32 i;
+
+    // wait for an event in fifo
+    if (OSAL_SEMAPHORE_WAIT_TIMEOUT(initializerDesc[coreId].fifoSemHandle) != SYNC_OK) {
+        cm_COMP_generatePanic(coreId);
+        return CM_MPC_NOT_RESPONDING;
+    }
+
+
+    // AllocEvent
+    if((_xyuv_data = cm_AllocEvent(initializerDesc[coreId].downlinkFifo)) == NULL)
+    {
+        ERROR("CM_INTERNAL_FIFO_OVERFLOW: service FIFO full\n", 0, 0, 0, 0, 0, 0);
+        error = CM_INTERNAL_FIFO_OVERFLOW;
+        goto unlock;
+    }
+
+    // Copy param
+    for(i=0;i<paramNumber;i++)
+        _xyuv_data[i] = paramArray[i];
+
+    OSAL_LOCK_COM();
+
+    // Send Command
+    error = cm_PushEventTrace(initializerDesc[coreId].downlinkFifo, _xyuv_data, serviceIndex,0);
+    if(error == CM_OK)
+        initializerDesc[coreId].servicePending++;
+
+unlock:
+    OSAL_UNLOCK_COM();
+
+    return error;
+}
+
+PRIVATE void cm_COMP_generatePanic(t_nmf_core_id coreId)
+{
+    const t_dsp_desc* pDspDesc = cm_DSP_GetState(coreId);
+
+    if (pDspDesc->state != MPC_STATE_PANIC) {
+        cm_DSP_SetStatePanic(coreId);
+        OSAL_GeneratePanic(coreId, 0);
+    }
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/instantiater.c b/drivers/staging/nmf-cm/cm/engine/component/src/instantiater.c
new file mode 100644
index 00000000000..92c28b63171
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/instantiater.c
@@ -0,0 +1,829 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/component/inc/initializer.h>
+
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/configuration/inc/configuration_status.h>
+
+#include <cm/engine/dsp/inc/dsp.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+#include <cm/engine/memory/inc/domain.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/utils/inc/mem.h>
+#include <cm/engine/utils/inc/convert.h>
+
+#include <cm/engine/power_mgt/inc/power.h>
+
+
+t_nmf_table ComponentTable; /**< list (table) of components */
+
+static t_uint32 cm_getMaxStackValue(t_component_instance *pComponent);
+static t_uint16 getNumberOfInstance(t_component_instance* component);
+static t_uint16 getNumberOfStart(t_component_instance* component);
+
+
+t_cm_error cm_COMP_Init(void) {
+	t_cm_error error;
+	error = cm_initTable(&ComponentTable);
+	if (error == CM_OK)
+		error = cm_initTable(&Host2MpcBindingTable);
+	return error;
+}
+
+void cm_COMP_Destroy(void) {
+	cm_destroyTable(&ComponentTable);
+	cm_destroyTable(&Host2MpcBindingTable);
+}
+
+/** cm_addComponent - Add an internal handler to the list
+ *
+ *  1. Increase the size of the list if it's full
+ *  2. Search an empty entry
+ *  3. Add the element to the list
+ *  4. Compute and return the "user handle" (= t_cm_instance_handle)
+ */
+static t_cm_instance_handle cm_addComponent(t_component_instance *comp)
+{
+	OSAL_DisableServiceMessages();
+	comp->instance = cm_addEntry(&ComponentTable, comp);
+	OSAL_EnableServiceMessages();
+
+	return comp->instance;
+}
+
+/** cm_delComponent - remove the given component from the list
+ *
+ *  1. Check if the handle is valid
+ *  2. Search the entry and free it
+ */
+static void cm_delComponent(t_component_instance *comp)
+{
+	if (comp == NULL)
+		return;
+
+	OSAL_DisableServiceMessages();
+	cm_delEntry(&ComponentTable, comp->instance & INDEX_MASK);
+	OSAL_EnableServiceMessages();
+}
+
+/** cm_lookupComponent - search the component corresponding to
+ *                       the component instance.
+ *
+ * 1. Check if the instance is valid
+ * 2. Return a pointer to the component
+ */
+t_component_instance *cm_lookupComponent(const t_cm_instance_handle hdl)
+{
+	return cm_lookupEntry(&ComponentTable, hdl);
+}
+
+static void cm_DestroyComponentMemory(t_component_instance *component)
+{
+    int i;
+
+    /*
+     * Remove instance from list
+     */
+    cm_delComponent(component);
+
+    /*
+     * Destroy instance
+     */
+    {
+        struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+        for( ; cur != NULL ; )
+        {
+            struct t_client_of_singleton* tmp = cur;
+            cur = cur->next;
+
+            OSAL_Free(tmp);
+        }
+    }
+
+    for(i = 0; i < component->Template->requireNumber; i++)
+    {
+        OSAL_Free(component->interfaceReferences[i]);
+    }
+
+    cm_StringRelease(component->pathname);
+
+    cm_ELF_FreeInstance(component->Template->dspId, component->Template->memories, component->memories);
+
+    cm_unloadComponent(component->Template);
+    OSAL_Free(component);
+}
+
+/**
+ * Non-Require:
+ *    - MMDSP could be sleep (Since we access it only through HSEM)
+ */
+void cm_delayedDestroyComponent(t_component_instance *component) {
+    int i;
+
+    if (osal_debug_ops.component_destroy)
+	    osal_debug_ops.component_destroy(component);
+
+    /*
+     * Remove component from load map here
+     */
+    cm_DSPABI_RemoveLoadMap(
+            component->domainId,
+            component->Template->name,
+            component->memories,
+            component->pathname,
+            component);
+
+    // Generate XTI/STM trace
+    cm_TRC_traceLoadMap(TRACE_COMPONENT_COMMAND_REMOVE, component);
+
+    /*
+     * disconnect interrupt handler if needed
+     */
+    for(i = 0; i < component->Template->provideNumber; i++)
+    {
+        if(component->Template->provides[i].interruptLine)
+        {
+            cm_unbindInterfaceStaticInterrupt(component->Template->dspId, component->Template->provides[i].interruptLine);
+        }
+    }
+
+    /*
+     * Update dsp stack size if needed
+     */
+    if (component->Template->minStackSize > MIN_STACK_SIZE)
+    {
+        if (cm_EEM_isStackUpdateNeed(component->Template->dspId, component->priority, FALSE, component->Template->minStackSize))
+        {
+            t_uint32 newStackValue;
+            t_uint32 maxComponentStackSize;
+
+            maxComponentStackSize = cm_getMaxStackValue(component);
+            cm_EEM_UpdateStack(component->Template->dspId, component->priority, maxComponentStackSize, &newStackValue);
+	    if (cm_DSP_GetState(component->Template->dspId)->state == MPC_STATE_BOOTED)
+		    cm_COMP_UpdateStack(component->Template->dspId, newStackValue);
+        }
+    }
+
+    cm_DestroyComponentMemory(component);
+}
+
+/**
+ * Pre-Require:
+ *    - MMDSP wakeup (when loading in TCM)
+ */
+t_cm_error cm_instantiateComponent(const char* templateName,
+                                   t_cm_domain_id domainId,
+                                   t_nmf_ee_priority priority,
+                                   const char* pathName,
+                                   t_elfdescription *elfhandle,
+                                   t_component_instance** refcomponent)
+{
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(domainId);
+    t_dup_char templateNameDup;
+    t_component_template* template;
+    t_component_instance *component;
+    /* coverity[var_decl] */
+    t_cm_error error;
+    int i, j, k;
+
+    *refcomponent = NULL;
+
+    templateNameDup = cm_StringDuplicate(templateName);
+    if(templateNameDup == NULL)
+        return CM_NO_MORE_MEMORY;
+
+    /*
+     * Lookup in template list
+     */
+    template = cm_lookupTemplate(coreId, templateNameDup);
+    if(template != NULL)
+    {
+        if(template->classe == SINGLETON)
+        {
+            // Return same handle for singleton component
+            struct t_client_of_singleton* cl;
+
+            cm_StringRelease(templateNameDup);
+
+            cl = cm_getClientOfSingleton(template->singletonIfAvaliable, TRUE, domainDesc[domainId].client);
+            if(cl == NULL)
+                return CM_NO_MORE_MEMORY;
+            cl->numberOfInstance++;
+
+            *refcomponent = template->singletonIfAvaliable;
+            LOG_INTERNAL(1, "##### Singleton : New handle of %s/%x component on %s provItf=%d#####\n",
+                    template->singletonIfAvaliable->pathname, template->singletonIfAvaliable, cm_getDspName(coreId),
+                    template->singletonIfAvaliable->providedItfUsedCount, 0, 0);
+            return CM_OK;
+        }
+    }
+
+    // Get the dataFile (identity if already pass as parameter)
+    if((elfhandle = cm_REP_getComponentFile(templateNameDup, elfhandle)) == NULL)
+    {
+        cm_StringRelease(templateNameDup);
+        return CM_COMPONENT_NOT_FOUND;
+    }
+
+    // Load template
+    if((error = cm_loadComponent(templateNameDup, domainId, elfhandle, &template)) != CM_OK)
+    {
+        cm_StringRelease(templateNameDup);
+        return error;
+    }
+
+    // templateNameDup no more used, release it
+    cm_StringRelease(templateNameDup);
+
+    // Allocated component
+    component = (t_component_instance*)OSAL_Alloc_Zero(
+            sizeof(t_component_instance) +
+            sizeof(t_interface_reference*) * template->requireNumber);
+    if(component == NULL)
+    {
+        cm_unloadComponent(template);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    component->interfaceReferences = (t_interface_reference**)((char*)component + sizeof(t_component_instance));
+    component->Template = template;
+
+    /*
+     * Update linked list
+     */
+    if (cm_addComponent(component) == 0) {
+        cm_unloadComponent(template);
+        OSAL_Free(component);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    // NOTE: From here use cm_DestroyComponentMemory
+
+    component->pathname = pathName ? cm_StringDuplicate(pathName) : cm_StringReference(anonymousDup);
+    if(component->pathname == NULL)
+    {
+        cm_DestroyComponentMemory(component);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    LOG_INTERNAL(1, "\n##### Instantiate %s/%x (%s) component on %s at priority %d #####\n", component->pathname, component, template->name, cm_getDspName(coreId), priority, 0);
+
+    if((error = cm_ELF_LoadInstance(domainId, elfhandle, template->memories, component->memories, template->classe == SINGLETON)) != CM_OK)
+    {
+        cm_DestroyComponentMemory(component);
+        return error;
+    }
+
+    if((error = cm_ELF_relocatePrivateSegments(
+            component->memories,
+            elfhandle,
+            template)) != CM_OK)
+    {
+        cm_DestroyComponentMemory(component);
+        return error;
+    }
+
+    cm_ELF_FlushInstance(coreId, template->memories, component->memories);
+
+    /*
+     * Create a new component instance
+     */
+    component->priority = priority;
+    component->thisAddress = 0xFFFFFFFF;
+    component->state = STATE_NONE;
+
+    if(component->Template->classe == SINGLETON)
+    {   // Return same handle for singleton component
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, TRUE, domainDesc[domainId].client);
+        if(cl == NULL)
+        {
+            cm_DestroyComponentMemory(component);
+            return CM_NO_MORE_MEMORY;
+        }
+
+        cl->numberOfInstance = 1;
+        template->singletonIfAvaliable = component;
+	if (cm_DM_GetDomainCoreId(domainId) == SVA_CORE_ID)
+		component->domainId = DEFAULT_SVA_DOMAIN;
+	else
+		component->domainId = DEFAULT_SIA_DOMAIN;
+    } else {
+        component->domainId = domainId;
+    }
+
+    if(component->memories[template->thisMemory->id] != INVALID_MEMORY_HANDLE)
+        cm_DSP_GetDspAddress(component->memories[template->thisMemory->id], &component->thisAddress);
+    else {
+        // In case of singleton or component without data
+        component->thisAddress = 0;
+    }
+
+    /*
+     * Create empty required interfaces array and set method interface to Panic
+     */
+    for(i = 0; i < template->requireNumber; i++) // For all required interface
+    {
+        component->interfaceReferences[i] =
+                (t_interface_reference*)OSAL_Alloc_Zero(sizeof(t_interface_reference) * template->requires[i].collectionSize);
+        if(component->interfaceReferences[i] == NULL)
+        {
+            cm_DestroyComponentMemory(component);
+            return CM_NO_MORE_MEMORY;
+        }
+
+        for(j = 0; j < template->requires[i].collectionSize; j++) // ... and for each index in collection (set THIS&method for each client)
+        {
+            component->interfaceReferences[i][j].instance = NULL;
+            component->interfaceReferences[i][j].bfInfoID = BF_SYNCHRONOUS; // Just to memorize no Binding component used and unbind ToVoid happy ;-).
+
+            if(template->classe == COMPONENT && template->requires[i].indexes != NULL)
+            {
+                // If component, fill THIS to itself to detect UNBINDED panic with rigth DSP
+                t_interface_require_index *requireindex = &template->requires[i].indexes[j];
+                for(k = 0; k < requireindex->numberOfClient; k++)
+                {
+                    t_uint32 *hostAddr;
+
+                    hostAddr = (t_uint32*)(
+                            cm_DSP_GetHostLogicalAddress(
+                                    component->memories[requireindex->memories[k].memory->id]) +
+                                    requireindex->memories[k].offset * requireindex->memories[k].memory->memEntSize);
+                    *hostAddr++ = (t_uint32)component->thisAddress;
+                }
+            }
+        }
+    }
+
+    /*
+     * Inform debugger about new component
+     */
+    if ((error = cm_DSPABI_AddLoadMap(
+            domainId,
+            template->name,
+            component->pathname,
+            component->memories,
+            component)) != CM_OK)
+    {
+        cm_DestroyComponentMemory(component);
+        return error;
+    }
+
+    // Generate XTI/STM trace
+    cm_TRC_traceLoadMap(TRACE_COMPONENT_COMMAND_ADD, component);
+
+    // NOTE: From here use cm_delayedDestroyComponent
+
+    /*
+     * Relocate interrupt if this is an interrupt
+     */
+    for(i = 0; i < template->provideNumber; i++)
+    {
+        if(template->provides[i].interruptLine)
+        {
+            if ((error = cm_bindInterfaceStaticInterrupt(coreId,
+                    template->provides[i].interruptLine,
+                    component,
+                    template->provides[i].name)) != CM_OK)
+            {
+                cm_delayedDestroyComponent(component);
+                return error;
+            }
+        }
+    }
+
+    /*
+     * For first instance of a component; Update ee stack size if needed
+     */
+    if(template->classe != FIRMWARE && template->numberOfInstance == 1 && template->minStackSize > MIN_STACK_SIZE)
+    {
+        t_uint32 newStackValue;
+
+        if (cm_EEM_isStackUpdateNeed(template->dspId, priority, TRUE, template->minStackSize))
+        {
+            error = cm_EEM_UpdateStack(template->dspId, priority, template->minStackSize, &newStackValue);
+            if (error != CM_OK)
+            {
+                cm_delayedDestroyComponent(component);
+                return error;
+            }
+            cm_COMP_UpdateStack(template->dspId, newStackValue);
+        }
+    }
+
+
+    /*
+     * For component or first instance
+     */
+    if(template->classe == SINGLETON || template->classe == COMPONENT)
+    {
+        /*
+         * Call init function generated by the compiler (one per .elf)
+         */
+        LOG_INTERNAL(2, "constructor call(s) <%s>\n", template->name, 0, 0, 0, 0, 0);
+        if (cm_DSP_GetState(template->dspId)->state != MPC_STATE_BOOTED)
+        {
+            cm_delayedDestroyComponent(component);
+            return CM_MPC_NOT_RESPONDING;
+        }
+        else if ((error = cm_COMP_CallService(
+                (priority > cm_EEM_getExecutiveEngine(coreId)->instance->priority)?NMF_CONSTRUCT_SYNC_INDEX:NMF_CONSTRUCT_INDEX,
+                component,
+                template->LCCConstructAddress)) != CM_OK)
+        {
+            if (error == CM_MPC_NOT_RESPONDING)
+                ERROR("CM_MPC_NOT_RESPONDING: can't call constructor '%s'\n", component->pathname, 0, 0, 0, 0, 0);
+            cm_delayedDestroyComponent(component);
+            return error;
+        }
+    }
+    else
+    {
+        /* be sure everything is write into memory, not required elsewhere since will be done by cm_COMP_CallService */
+        OSAL_mb();
+    }
+
+    // For firmware; Directly switch to STARTED state, don't need to start it
+    if (template->classe == FIRMWARE)
+        component->state = STATE_RUNNABLE;
+    else
+        component->state = STATE_STOPPED;
+
+    if (osal_debug_ops.component_create)
+	    osal_debug_ops.component_create(component);
+
+    *refcomponent = component;
+    return CM_OK;
+}
+
+struct t_client_of_singleton* cm_getClientOfSingleton(t_component_instance* component, t_bool createdIfNotExist, t_nmf_client_id clientId)
+{
+    struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+    for( ; cur != NULL ; cur = cur->next)
+    {
+        if(cur->clientId == clientId)
+        {
+            return cur;
+        }
+    }
+
+    //if(createdIfNotExist)
+    {
+        cur = OSAL_Alloc(sizeof(struct t_client_of_singleton));
+        if(cur != NULL)
+        {
+            cur->clientId = clientId;
+            cur->next = component->clientOfSingleton;
+            cur->numberOfBind = 0;
+            cur->numberOfInstance= 0;
+            cur->numberOfStart = 0;
+            component->clientOfSingleton = cur;
+        }
+    }
+    return cur;
+}
+
+/**
+ * Non-Require:
+ *    - MMDSP could be sleep (Since we access it only through HSEM)
+ */
+t_cm_error cm_startComponent(t_component_instance* component, t_nmf_client_id clientId)
+{
+    t_cm_error error;
+    char  value[MAX_PROPERTY_VALUE_LENGTH];
+    int i;
+
+    /*
+     * Special code for SINGLETON handling
+     */
+    if(component->Template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        if(cl != NULL)
+            cl->numberOfStart++;
+        // A singleton could be started twice, thus start it only if first client starter
+        if(getNumberOfStart(component) > 1)
+            return CM_OK;
+
+        // Fall through and start really the singleton.
+    }
+
+    if(component->state == STATE_RUNNABLE)
+        return CM_COMPONENT_NOT_STOPPED;
+
+    // CM_ASSERT component->state == STATE_STOPPED
+
+    /*
+     * Check that all required binding have been binded!
+     */
+    for(i = 0; i < component->Template->requireNumber; i++)
+    {
+        int nb = component->Template->requires[i].collectionSize, j;
+        for(j = 0; j < nb; j++)
+        {
+            if(component->interfaceReferences[i][j].instance == NULL  &&
+                    (component->Template->requires[i].requireTypes & (OPTIONAL_REQUIRE | INTRINSEC_REQUIRE)) == 0)
+            {
+                ERROR("CM_REQUIRE_INTERFACE_UNBINDED: Required interface '%s'.'%s' binded\n", component->pathname, component->Template->requires[i].name, 0, 0, 0, 0);
+                return CM_REQUIRE_INTERFACE_UNBINDED;
+            }
+        }
+    }
+
+    component->state = STATE_RUNNABLE;
+
+    /*
+     * Power on, HW resources if required
+     */
+    if(cm_getComponentProperty(
+            component,
+            "hardware",
+            value,
+            sizeof(value)) == CM_OK)
+    {
+        error = cm_PWR_EnableMPC(MPC_PWR_HWIP, component->Template->dspId);
+        if(error != CM_OK)
+            return error;
+    }
+
+    /*
+     * Call starter if available
+     */
+    if(component->Template->LCCStartAddress != 0)
+    {
+        if (cm_DSP_GetState(component->Template->dspId)->state != MPC_STATE_BOOTED)
+        {
+            return CM_MPC_NOT_RESPONDING;
+        }
+        else if ((error = cm_COMP_CallService(
+                (component->priority > cm_EEM_getExecutiveEngine(component->Template->dspId)->instance->priority)?NMF_START_SYNC_INDEX:NMF_START_INDEX,
+                component,
+                component->Template->LCCStartAddress)) != CM_OK)
+        {
+            if (error == CM_MPC_NOT_RESPONDING)
+                ERROR("CM_MPC_NOT_RESPONDING: can't call starter '%s'\n", component->pathname, 0, 0, 0, 0, 0);
+            return error;
+        }
+    }
+
+    return CM_OK;
+}
+
+/**
+ * Non-Require:
+ *    - MMDSP could be sleep (Since we access it only through HSEM)
+ */
+t_cm_error cm_stopComponent(t_component_instance* component, t_nmf_client_id clientId)
+{
+    char  value[MAX_PROPERTY_VALUE_LENGTH];
+    t_cm_error error = CM_OK;
+    t_bool isHwProperty;
+
+    /*
+     * Special code for SINGLETON handling
+     */
+    if(component->Template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        if(cl != NULL)
+            cl->numberOfStart--;
+        // A singleton could be started twice, thus stop it only if no more client starter
+        if(getNumberOfStart(component) > 0)
+            return CM_OK;
+
+        // Fall through and stop really the singleton.
+    }
+
+    /*
+     * Component life cycle sanity check
+     */
+    if(component->state == STATE_STOPPED)
+        return CM_COMPONENT_NOT_STARTED;
+
+    // CM_ASSERT component->state == STATE_RUNNABLE
+    component->state = STATE_STOPPED;
+
+    isHwProperty = (cm_getComponentProperty(
+                component,
+                "hardware",
+                value,
+                sizeof(value)) == CM_OK);
+
+    if (cm_DSP_GetState(component->Template->dspId)->state != MPC_STATE_BOOTED)
+    {
+        error = CM_MPC_NOT_RESPONDING;
+    }
+    else
+    {
+        /*
+         * Call stopper if available
+         */
+        if(component->Template->LCCStopAddress != 0)
+        {
+            if ((error = cm_COMP_CallService(
+                    isHwProperty ? NMF_STOP_SYNC_INDEX : NMF_STOP_INDEX,
+                    component,
+                    component->Template->LCCStopAddress)) != CM_OK)
+            {
+                if (error == CM_MPC_NOT_RESPONDING)
+                    ERROR("CM_MPC_NOT_RESPONDING: can't call stopper '%s'\n", component->pathname, 0, 0, 0, 0, 0);
+            }
+        }
+    }
+
+    /*
+     * Power on, HW resources if required
+     */
+    if(isHwProperty)
+    {
+        cm_PWR_DisableMPC(MPC_PWR_HWIP, component->Template->dspId);
+    }
+
+    return error;
+}
+
+t_cm_error cm_destroyInstance(t_component_instance* component, t_destroy_state forceDestroy)
+{
+    int i, j;
+
+    LOG_INTERNAL(1, "\n##### Destroy %s/%x (%s) component on %s #####\n",
+                 component->pathname, component, component->Template->name, cm_getDspName(component->Template->dspId), 0, 0);
+
+    /*
+     * Component life cycle sanity check; do it only when destroying last reference.
+     */
+    if(forceDestroy == DESTROY_NORMAL)
+    {
+        if (component->state == STATE_RUNNABLE)
+            return CM_COMPONENT_NOT_STOPPED;
+
+        // CM_ASSERT component->state == STATE_STOPPED
+
+        // Check that all required binding have been unbound!
+        for(i = 0; i < component->Template->requireNumber; i++)
+        {
+            int nb = component->Template->requires[i].collectionSize;
+            for(j = 0; j < nb; j++)
+            {
+                if(component->interfaceReferences[i][j].instance != NULL)
+                {
+                    ERROR("CM_COMPONENT_NOT_UNBINDED: Required interface %s/%x.%s still binded\n",
+                            component->pathname, component, component->Template->requires[i].name, 0, 0, 0);
+                    return CM_COMPONENT_NOT_UNBINDED;
+                }
+            }
+        }
+
+        // Check that all provided bindings have been unbound!
+        if (component->providedItfUsedCount != 0)
+        {
+            unsigned idx;
+
+            ERROR("CM_COMPONENT_NOT_UNBINDED: Still %d binding to %s/%x provided interface\n",
+                    component->providedItfUsedCount, component->pathname, component, 0, 0, 0);
+
+            /* Find which interface is still bound to gracefully print an error message */
+            for (idx=0; idx<ComponentTable.idxMax; idx++)
+            {
+                if ((componentEntry(idx) == NULL) || (componentEntry(idx) == component))
+                    continue;
+                for (i = 0; i < componentEntry(idx)->Template->requireNumber; i++)
+                {
+                    for (j = 0; j < componentEntry(idx)->Template->requires[i].collectionSize; j++)
+                    {
+                        if(componentEntry(idx)->interfaceReferences[i][j].instance == component
+                                && component->Template->provides[componentEntry(idx)->interfaceReferences[i][j].provideIndex].interruptLine == 0)
+                        {
+                            ERROR("  -> %s/%x.%s still used by %s/%x.%s\n",
+                                    component->pathname, component,
+                                    component->Template->provides[componentEntry(idx)->interfaceReferences[i][j].provideIndex].name,
+                                    componentEntry(idx)->pathname,
+                                    componentEntry(idx),
+                                    componentEntry(idx)->Template->requires[i].name);
+                        }
+                    }
+                }
+            }
+
+            return CM_COMPONENT_NOT_UNBINDED;
+        }
+    }
+
+    // Sanity check finished, here, we will do the JOB whatever error
+
+    if (cm_DSP_GetState(component->Template->dspId)->state == MPC_STATE_BOOTED)
+    {
+        /*
+         * Call destroy if available
+         */
+        /* Call the destructor only if we don't want to force the destruction */
+        if(forceDestroy != DESTROY_WITHOUT_CHECK_CALL && component->Template->LCCDestroyAddress != 0)
+        {
+            if (cm_COMP_CallService(
+                    NMF_DESTROY_INDEX,
+                    component,
+                    component->Template->LCCDestroyAddress) != CM_OK)
+            {
+                ERROR("CM_MPC_NOT_RESPONDING: can't call destroy '%s'\n", component->pathname, 0, 0, 0, 0, 0);
+            }
+        }
+        else
+        {
+            cm_COMP_Flush(component->Template->dspId);
+        }
+    }
+
+    cm_delayedDestroyComponent(component);
+
+    return CM_OK;
+}
+
+/**
+ * Pre-Require:
+ *    - MMDSP wakeup (when accessing loadmap)
+ */
+t_cm_error cm_destroyInstanceForClient(t_component_instance* component, t_destroy_state forceDestroy, t_nmf_client_id clientId)
+{
+    /*
+     * Special code for SINGLETON handling
+     */
+    if(component->Template->classe == SINGLETON)
+    {
+        struct t_client_of_singleton* cl = cm_getClientOfSingleton(component, FALSE, clientId);
+        int nbinstance;
+        if(cl != NULL)
+            cl->numberOfInstance--;
+
+        // A singleton could be instantiate twice, thus destroy it only if no more client constructor
+        nbinstance = getNumberOfInstance(component);
+        if(nbinstance > 0)
+        {
+            LOG_INTERNAL(1, "##### Singleton : Delete handle of %s/%x (%s) component on %s [%d] provItf=%d #####\n",
+                 component->pathname, component, component->Template->name, cm_getDspName(component->Template->dspId),
+                 nbinstance, component->providedItfUsedCount);
+            return CM_OK;
+        }
+
+        // Fall through
+    }
+
+    return cm_destroyInstance(component, forceDestroy);
+}
+
+
+static t_uint32 cm_getMaxStackValue(t_component_instance *pComponent)
+{
+    t_nmf_executive_engine_id executiveEngineId = cm_EEM_getExecutiveEngine(pComponent->Template->dspId)->executiveEngineId;
+    t_uint32 res = MIN_STACK_SIZE;
+    unsigned int i;
+
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	if ((componentEntry(i) != NULL) &&
+	    (componentEntry(i) != pComponent) &&
+            (pComponent->Template->dspId == componentEntry(i)->Template->dspId) &&
+            (executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE || componentEntry(i)->priority == pComponent->priority))
+        {
+		if (componentEntry(i)->Template->minStackSize > res)
+                res = componentEntry(i)->Template->minStackSize;
+        }
+    }
+
+    return res;
+}
+
+static t_uint16 getNumberOfInstance(t_component_instance* component)
+{
+    t_uint16 instanceNumber = 0;
+    struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+    for( ; cur != NULL ; cur = cur->next)
+    {
+        instanceNumber += cur->numberOfInstance;
+    }
+
+    return instanceNumber;
+}
+
+static t_uint16 getNumberOfStart(t_component_instance* component)
+{
+    t_uint16 startNumber = 0;
+    struct t_client_of_singleton* cur = component->clientOfSingleton;
+
+    for( ; cur != NULL ; cur = cur->next)
+    {
+        startNumber += cur->numberOfStart;
+    }
+
+    return startNumber;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/introspection.c b/drivers/staging/nmf-cm/cm/engine/component/src/introspection.c
new file mode 100644
index 00000000000..7e2f7cd65d0
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/introspection.c
@@ -0,0 +1,327 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/string.h>
+
+/*
+ *
+ */
+t_cm_error cm_getComponentProperty(
+	const t_component_instance *component,
+	const char                 *propName,
+    char                       value[MAX_PROPERTY_VALUE_LENGTH],
+    t_uint32                   valueLength){
+	t_component_template* template = component->Template;
+    int i;
+
+    for(i = 0; i < template->propertyNumber; i++) {
+        if(cm_StringCompare(template->properties[i].name, propName, MAX_PROPERTY_NAME_LENGTH) == 0) {
+        	cm_StringCopy(
+                value,
+                template->properties[i].value,
+                valueLength);
+            return CM_OK;
+        }
+    }
+
+    return CM_NO_SUCH_PROPERTY;
+}
+
+/**
+ *
+ */
+static t_attribute* cm_getAttributeDescriptor(
+        const t_component_instance  *component,
+        const char                  *attrName)
+{
+    int i;
+
+    for(i = 0; i < component->Template->attributeNumber; i++)
+    {
+        if(cm_StringCompare(component->Template->attributes[i].name, attrName, MAX_ATTRIBUTE_NAME_LENGTH) == 0)
+        {
+            return &component->Template->attributes[i];
+        }
+    }
+
+    return NULL;
+}
+
+t_dsp_address cm_getAttributeMpcAddress(
+        const t_component_instance  *component,
+        const char                  *attrName)
+{
+    t_attribute* attribute;
+    t_uint32 dspAddress;
+
+    if((attribute = cm_getAttributeDescriptor(component, attrName)) == NULL)
+        return 0x0;
+
+    cm_DSP_GetDspAddress(component->memories[attribute->memory.memory->id], &dspAddress);
+
+    return (dspAddress +
+            attribute->memory.offset);
+}
+
+t_cm_logical_address cm_getAttributeHostAddr(
+        const t_component_instance  *component,
+        const char                  *attrName)
+{
+    t_attribute* attribute;
+
+    if((attribute = cm_getAttributeDescriptor(component, attrName)) == NULL)
+        return 0x0;
+
+    // TODO JPF: component->Template->attributes[i].memory.offset could be converted in byte during load
+    return cm_DSP_GetHostLogicalAddress(component->memories[attribute->memory.memory->id]) +
+            attribute->memory.offset * attribute->memory.memory->memEntSize;
+}
+
+
+t_cm_error cm_readAttribute(
+        const t_component_instance* component,
+        const char* attrName,
+        t_uint32* value)
+{
+    t_attribute* attribute;
+    t_cm_logical_address hostAddr;
+
+    if((attribute = cm_getAttributeDescriptor(component, attrName)) == NULL)
+    {
+        ERROR("CM_NO_SUCH_ATTRIBUTE(%s, %s)\n", component->pathname, attrName, 0, 0, 0, 0);
+        return CM_NO_SUCH_ATTRIBUTE;
+    }
+
+    // TODO JPF: component->Template->attributes[i].memory.offset could be converted in byte during load
+    hostAddr = cm_DSP_GetHostLogicalAddress(component->memories[attribute->memory.memory->id]) +
+            attribute->memory.offset * attribute->memory.memory->memEntSize;
+
+    if(attribute->memory.memory->memEntSize != 2)
+        *value = *((t_uint32 *)hostAddr) & ~MASK_BYTE3;
+    else
+        *value = *((t_uint16 *)hostAddr);
+
+    LOG_INTERNAL(3, "cm_readAttribute: [%s:%s, %x]=%x\n",
+            component->pathname, attrName, hostAddr, *value, 0, 0);
+
+    return CM_OK;
+}
+
+t_uint32 cm_readAttributeNoError(
+        const t_component_instance* component,
+        const char* attrName)
+{
+    t_uint32 value;
+
+    if(cm_readAttribute(component, attrName, &value) != CM_OK)
+        value = 0;
+
+    return value;
+}
+
+t_cm_error cm_writeAttribute(
+        const t_component_instance* component,
+        const char* attrName,
+        t_uint32 value)
+{
+    t_attribute* attribute;
+    t_cm_logical_address hostAddr;
+
+    if((attribute = cm_getAttributeDescriptor(component, attrName)) == NULL)
+    {
+        ERROR("CM_NO_SUCH_ATTRIBUTE(%s, %s)\n", component->pathname, attrName, 0, 0, 0, 0);
+        return CM_NO_SUCH_ATTRIBUTE;
+    }
+
+    // TODO JPF: component->Template->attributes[i].memory.offset could be converted in byte during load
+    hostAddr = cm_DSP_GetHostLogicalAddress(component->memories[attribute->memory.memory->id]) +
+            attribute->memory.offset * attribute->memory.memory->memEntSize;
+
+    if(attribute->memory.memory->memEntSize != 2)
+        *((t_uint32 *)hostAddr) = value & ~MASK_BYTE3;
+    else
+        *((t_uint16 *)hostAddr) = value;
+
+    /* be sure attribute is write into memory */
+    OSAL_mb();
+
+    LOG_INTERNAL(3, "cm_writeAttribute: [%s:%s, %x]=%x\n",
+            component->pathname, attrName, hostAddr, value, 0, 0);
+
+    return CM_OK;
+}
+
+
+/**
+ *
+ */
+t_dsp_address cm_getFunction(
+        const t_component_instance* component,
+        const char* interfaceName,
+        const char* methodName)
+{
+    t_interface_provide_description itfProvide;
+    t_interface_provide* provide;
+    t_interface_provide_loaded* provideLoaded;
+    t_cm_error error;
+    int i;
+
+    // Get interface description
+    if((error = cm_getProvidedInterface(component, interfaceName, &itfProvide)) != CM_OK)
+        return error;
+
+    provide = &component->Template->provides[itfProvide.provideIndex];
+    provideLoaded = &component->Template->providesLoaded[itfProvide.provideIndex];
+
+    for(i = 0; i < provide->interface->methodNumber; i++)
+    {
+        if(cm_StringCompare(provide->interface->methodNames[i], methodName, MAX_INTERFACE_METHOD_NAME_LENGTH) == 0)
+        {
+            return provideLoaded->indexesLoaded[itfProvide.collectionIndex][i].methodAddresses;
+        }
+    }
+
+    return 0x0;
+}
+
+/**
+ *
+ */
+PRIVATE t_uint8 compareItfName(const char* simplename, const char* complexname, int *collectionIndex) {
+    int i;
+
+    // Search if simplename is a prefix of complexname ??
+    for(i = 0; simplename[i] != 0; i++) {
+        if(simplename[i] != complexname[i])
+            return 1;                       // NO
+    }
+
+    // YES
+    if(complexname[i] == '[') {
+        // This is a collection
+        int value = 0;
+        i++;
+        if(complexname[i] < '0' || complexname[i] > '9') {
+            return 1;
+        }
+        for(; complexname[i] >= '0' && complexname[i] <= '9'; i++) {
+            value = value * 10 + (complexname[i] - '0');
+        }
+        if(complexname[i++] != ']')
+            return 1;
+        *collectionIndex = value;
+    } else
+        *collectionIndex = -1;
+
+    if(complexname[i] != 0) {
+        // Complexe name has not been fully parsed -> different name
+        return 1;
+    }
+
+    return 0;
+}
+
+
+/**
+ *
+ */
+PUBLIC t_cm_error cm_getProvidedInterface(const t_component_instance* server,
+        const char* itfName,
+        t_interface_provide_description *itfProvide){
+    int i;
+
+    for(i = 0; i < server->Template->provideNumber; i++)
+    {
+        int collectionIndex;
+        if(compareItfName(server->Template->provides[i].name, itfName, &collectionIndex) == 0)
+        {
+            t_interface_provide *provide = &server->Template->provides[i];
+            if(collectionIndex >= 0)
+            {
+                if(! (provide->provideTypes & COLLECTION_PROVIDE)) {
+                    ERROR("CM_NO_SUCH_PROVIDED_INTERFACE(%s, %s)\n",
+                            server->pathname, itfName, 0, 0, 0, 0);
+                    goto out;
+                }
+                if(collectionIndex >= provide->collectionSize) {
+                    ERROR("CM_NO_SUCH_PROVIDED_INTERFACE(%s, %s): out of range [0..%d[\n",
+                            server->pathname, itfName, provide->collectionSize,
+                            0, 0, 0);
+                    goto out;
+                }
+            }
+            else
+            {
+                if(provide->provideTypes & COLLECTION_PROVIDE) {
+                    ERROR("CM_NO_SUCH_PROVIDED_INTERFACE(%s, %s): interface is a collection [0..%d[\n",
+                            server->pathname, itfName, provide->collectionSize,
+                            0, 0, 0);
+                    goto out;
+                }
+                collectionIndex = 0;
+            }
+            itfProvide->provideIndex = i;
+            itfProvide->server = server;
+            itfProvide->collectionIndex = collectionIndex;
+            itfProvide->origName = itfName;
+            return CM_OK;
+        }
+    }
+
+    ERROR("CM_NO_SUCH_PROVIDED_INTERFACE(%s, %s)\n", server->pathname, itfName, 0, 0, 0, 0);
+out:
+    itfProvide->provideIndex = 0;
+    itfProvide->server = NULL;
+    itfProvide->collectionIndex = 0;
+    itfProvide->origName = NULL;
+    return CM_NO_SUCH_PROVIDED_INTERFACE;
+}
+
+/**
+ *
+ */
+t_cm_error cm_getRequiredInterface(const t_component_instance* client,
+        const char* itfName,
+        t_interface_require_description *itfRequire){
+    int i;
+
+    for(i = 0; i < client->Template->requireNumber; i++) {
+        int collectionIndex;
+        if(compareItfName(client->Template->requires[i].name, itfName, &collectionIndex) == 0) {
+            t_interface_require *require = &client->Template->requires[i];
+             if(collectionIndex >= 0) {
+                if(! (require->requireTypes & COLLECTION_REQUIRE)) {
+                    ERROR("CM_NO_SUCH_REQUIRED_INTERFACE(%s, %s)\n",
+                        client->pathname, itfName, 0, 0, 0, 0);
+                    return CM_NO_SUCH_REQUIRED_INTERFACE;
+                }
+                if(collectionIndex >= require->collectionSize) {
+                    ERROR("CM_NO_SUCH_REQUIRED_INTERFACE(%s, %s): out of range [0..%d[\n",
+                        client->pathname, itfName, require->collectionSize,
+                        0, 0, 0);
+                    return CM_NO_SUCH_REQUIRED_INTERFACE;
+                }
+            } else {
+                if(require->requireTypes & COLLECTION_REQUIRE) {
+                    ERROR("CM_NO_SUCH_REQUIRED_INTERFACE(%s, %s): interface is a collection [0..%d[\n",
+                        client->pathname, itfName, require->collectionSize,
+                        0, 0, 0);
+                    return CM_NO_SUCH_REQUIRED_INTERFACE;
+                }
+                collectionIndex = 0;
+            }
+            itfRequire->client = client;
+            itfRequire->requireIndex = i;
+            itfRequire->collectionIndex = collectionIndex;
+            itfRequire->origName = itfName;
+            return CM_OK;
+        }
+    }
+
+    ERROR("CM_NO_SUCH_REQUIRED_INTERFACE(%s, %s)\n", client->pathname, itfName, 0, 0, 0, 0);
+    return CM_NO_SUCH_REQUIRED_INTERFACE;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/component/src/loader.c b/drivers/staging/nmf-cm/cm/engine/component/src/loader.c
new file mode 100644
index 00000000000..3d81e8308f9
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/component/src/loader.c
@@ -0,0 +1,384 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/component/inc/bind.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/convert.h>
+
+void START(void);
+void END(char*);
+
+#undef NHASH
+#define NHASH 79       //Use a prime number!
+#define MULT 17
+
+static t_component_template *templates[NB_CORE_IDS][NHASH];
+
+static unsigned int templateHash(const char *str)
+{
+    unsigned int h = 0;
+    for(; *str; str++)
+        h = MULT * h + *str;
+    return h % NHASH;
+}
+
+static void templateAdd(t_component_template *template)
+{
+    unsigned int h = templateHash(template->name);
+
+    if(templates[template->dspId][h] != NULL)
+        templates[template->dspId][h]->prev = template;
+    template->next = templates[template->dspId][h];
+    template->prev = NULL;
+    templates[template->dspId][h] = template;
+}
+
+static void templateRemove(t_component_template *template)
+{
+    unsigned int h = templateHash(template->name);
+
+    if(template->prev != NULL)
+        template->prev->next = template->next;
+    if(template->next != NULL)
+        template->next->prev = template->prev;
+    if(template == templates[template->dspId][h])
+        templates[template->dspId][h] = template->next;
+}
+
+
+t_component_template* cm_lookupTemplate(t_nmf_core_id dspId, t_dup_char str)
+{
+    t_component_template *template;
+
+    for(template = templates[dspId][templateHash(str)]; template != NULL; template = template->next)
+    {
+        if(str == template->name)
+            return template;
+    }
+
+    return NULL;
+}
+
+t_bool cm_isComponentOnCoreId(t_nmf_core_id coreId) {
+    t_uint32 i;
+
+    for(i = 0; i < NHASH; i++)
+    {
+	if ((templates[coreId][i] != NULL)
+	    && (templates[coreId][i]->classe != FIRMWARE)) // Skip firmware
+            return TRUE;
+    }
+
+    return FALSE;
+}
+
+
+static t_dsp_address MemoryToDspAdress(t_component_template *template, t_memory_reference *memory)
+{
+    if(memory->memory == NULL)
+        return (t_dsp_address)memory->offset;
+    else
+    {
+        t_dsp_address address;
+
+        cm_DSP_GetDspAddress(template->memories[memory->memory->id], &address);
+
+        return (t_dsp_address)(address + memory->offset);
+    }
+}
+
+/*
+ * Method callback
+ */
+t_uint32 cm_resolvSymbol(
+        void* context,
+        t_uint32 type,
+        t_dup_char symbolName,
+        char* reloc_addr)
+{
+    t_component_template *template = (t_component_template*)context;
+    t_component_instance* ee = cm_EEM_getExecutiveEngine(template->dspId)->instance;
+    int i, j;
+
+    // Search if this method is provided by EE and resolve it directly
+    for(i = 0; i < ee->Template->provideNumber; i++)
+    {
+        t_interface_provide* provide = &ee->Template->provides[i];
+        t_interface_provide_loaded* provideLoaded = &ee->Template->providesLoaded[i];
+
+        for(j = 0; j < provide->interface->methodNumber; j++)
+        {
+            if(provide->interface->methodNames[j] == symbolName)
+            {
+                return provideLoaded->indexesLoaded[0][j].methodAddresses; // Here we assume no collection provided !!
+            }
+        }
+    }
+
+    // Lookup if the method is statically required, ands delay relocation when bind occur
+    for(i = 0; i < template->requireNumber; i++)
+    {
+        if((template->requires[i].requireTypes & STATIC_REQUIRE) == 0)
+            continue;
+
+        for(j = 0; j < template->requires[i].interface->methodNumber; j++)
+        {
+            if(template->requires[i].interface->methodNames[j] == symbolName)
+            {
+                t_function_relocation* delayedRelocation = (t_function_relocation*)OSAL_Alloc(sizeof(t_function_relocation));
+                if(delayedRelocation == NULL)
+                    return 0xFFFFFFFE;
+
+                delayedRelocation->type = type;
+                delayedRelocation->symbol_name = cm_StringReference(symbolName);
+                delayedRelocation->reloc_addr = reloc_addr;
+                delayedRelocation->next = template->delayedRelocation;
+                template->delayedRelocation = delayedRelocation;
+
+                return 0xFFFFFFFF;
+            }
+        }
+    }
+
+    //Symbol not found
+    return 0x0;
+}
+
+/*
+ * Template Management
+ */
+t_cm_error cm_loadComponent(
+        t_dup_char templateName,
+        t_cm_domain_id domainId,
+        t_elfdescription* elfhandle,
+        t_component_template **reftemplate)
+{
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(domainId);
+    t_cm_error error;
+    int i, j, k;
+
+    /*
+     * Allocate new component template if first instance
+     */
+    if(*reftemplate == NULL)
+    {
+        t_component_template *template;
+
+        LOG_INTERNAL(1, "\n##### Load template %s on %s #####\n", templateName, cm_getDspName(coreId), 0, 0, 0, 0);
+
+        /*
+         * Sanity check
+         */
+        if(elfhandle->foundedTemplateName != templateName)
+        {
+            ERROR("CM_INVALID_ELF_FILE: template name %s != %s\n", templateName, elfhandle->foundedTemplateName, 0, 0, 0, 0);
+            return CM_INVALID_ELF_FILE;
+        }
+
+        // Alloc & Reset variable in order to use unloadComponent either with partial constructed template
+        *reftemplate = template = (t_component_template*)OSAL_Alloc_Zero(sizeof(t_component_template));
+        if(template == NULL)
+            return CM_NO_MORE_MEMORY;
+        template->name = cm_StringReference(elfhandle->foundedTemplateName);
+
+        // Get information from elfhandle
+        template->descriptionAssociatedWithTemplate = elfhandle->temporaryDescription;
+        template->requireNumber = elfhandle->requireNumber;
+        template->requires = elfhandle->requires;
+        template->attributeNumber = elfhandle->attributeNumber;
+        template->attributes = elfhandle->attributes;
+        template->propertyNumber = elfhandle->propertyNumber;
+        template->properties = elfhandle->properties;
+        template->provideNumber = elfhandle->provideNumber;
+        template->provides = elfhandle->provides;
+        if(template->descriptionAssociatedWithTemplate)
+        {
+            elfhandle->requires = NULL;
+            elfhandle->attributes = NULL;
+            elfhandle->properties = NULL;
+            elfhandle->provides = NULL;
+        }
+
+        // Compute simple information
+        template->numberOfInstance = 1;
+        template->dspId = coreId;
+        LOG_INTERNAL(3, "load<%x> = %s\n", (int)template, template->name, 0, 0, 0, 0);
+        switch(elfhandle->magicNumber) {
+        case MAGIC_COMPONENT:
+            template->classe = COMPONENT;
+            break;
+        case MAGIC_SINGLETON:
+            template->classe = SINGLETON;
+            break;
+        case MAGIC_FIRMWARE:
+            template->classe = FIRMWARE;
+            break;
+        }
+        template->minStackSize = elfhandle->minStackSize;
+
+        /*
+         * Load shared memory from file
+         */
+        // START();
+        if((error = cm_ELF_LoadTemplate(domainId, elfhandle, template->memories, template->classe == SINGLETON)) != CM_OK)
+            goto out;
+        MMDSP_serializeMemories(elfhandle->instanceProperty, &template->codeMemory, &template->thisMemory);
+        // END("cm_ELF_LoadTemplate");
+
+        /*
+         * Copy LCC functions information
+         * Since MMDSP require Constructor & Destructor (for cache flush and debug purpose) to be called
+         * either if not provided by user for allowing defered breakpoint, we use Void method if not provided.
+         */
+        template->LCCConstructAddress = MemoryToDspAdress(template, &elfhandle->memoryForConstruct);
+        template->LCCStartAddress = MemoryToDspAdress(template, &elfhandle->memoryForStart);
+        template->LCCStopAddress = MemoryToDspAdress(template, &elfhandle->memoryForStop);
+        template->LCCDestroyAddress = MemoryToDspAdress(template, &elfhandle->memoryForDestroy);
+        if(template->LCCConstructAddress == 0 && template->classe != FIRMWARE)
+            template->LCCConstructAddress = cm_EEM_getExecutiveEngine(coreId)->voidAddr;
+
+        // Compute provide methodIndex
+        if(template->provideNumber != 0)
+        {
+            template->providesLoaded =
+                    (t_interface_provide_loaded*)OSAL_Alloc_Zero(sizeof(t_interface_provide_loaded) * template->provideNumber);
+            if(template->providesLoaded == NULL)
+                goto oom;
+
+            for(i = 0; i < template->provideNumber; i++)
+            {
+                template->providesLoaded[i].indexesLoaded = (t_interface_provide_index_loaded**)OSAL_Alloc_Zero(
+                        sizeof(t_interface_provide_index_loaded*) * template->provides[i].collectionSize);
+                if(template->providesLoaded[i].indexesLoaded == NULL)
+                    goto oom;
+
+                if(template->provides[i].interface->methodNumber != 0)
+                {
+                    for(j = 0; j < template->provides[i].collectionSize; j++)
+                    {
+                        template->providesLoaded[i].indexesLoaded[j] = (t_interface_provide_index_loaded*)OSAL_Alloc(
+                                sizeof(t_interface_provide_index_loaded) * template->provides[i].interface->methodNumber);
+                        if(template->providesLoaded[i].indexesLoaded[j] == NULL)
+                            goto oom;
+
+                        for(k = 0; k < template->provides[i].interface->methodNumber; k++)
+                        {
+                            template->providesLoaded[i].indexesLoaded[j][k].methodAddresses =
+                                    MemoryToDspAdress(template, &template->provides[i].indexes[j][k].memory);
+
+                            LOG_INTERNAL(2, "    [%d, %d] method '%s' @ %x\n",
+                                    j, k, template->provides[i].interface->methodNames[k],
+                                    template->providesLoaded[i].indexesLoaded[j][k].methodAddresses, 0, 0);
+                        }
+
+                    }
+                }
+            }
+        }
+
+        /*
+         * TODO
+
+        if((error = elfhandle->errorOccured) != CM_OK)
+            goto out;
+            */
+
+      //  START();
+        if(template->classe != FIRMWARE)
+        {
+            if((error = cm_ELF_relocateSharedSegments(
+                    template->memories,
+                    elfhandle,
+                    template)) != CM_OK)
+                goto out;
+        }
+       // END("cm_ELF_relocateSharedSegments");
+
+        cm_ELF_FlushTemplate(coreId, template->memories);
+
+        templateAdd(template);
+
+        return CM_OK;
+    oom:
+        error = CM_NO_MORE_MEMORY;
+    out:
+        cm_unloadComponent(template);
+        return error;
+    }
+    else
+    {
+        (*reftemplate)->numberOfInstance++;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_unloadComponent(
+        t_component_template *template)
+{
+    /*
+     * Destroy template if last instance
+     */
+    if(--template->numberOfInstance == 0) {
+        t_function_relocation* reloc;
+
+        LOG_INTERNAL(3, "unload<%s>\n", template->name, 0, 0, 0, 0, 0);
+
+        templateRemove(template);
+
+        // Free delayedRelocation
+        reloc = template->delayedRelocation;
+        while(reloc != NULL)
+        {
+            t_function_relocation *tofree = reloc;
+            reloc = reloc->next;
+            cm_StringRelease(tofree->symbol_name);
+            OSAL_Free(tofree);
+        }
+
+        if(template->providesLoaded != NULL)
+        {
+            int i, j;
+
+            for(i = 0; i < template->provideNumber; i++)
+            {
+                if(template->providesLoaded[i].indexesLoaded != NULL)
+                {
+                    for(j = 0; j < template->provides[i].collectionSize; j++)
+                    {
+                        OSAL_Free(template->providesLoaded[i].indexesLoaded[j]);
+                    }
+                    OSAL_Free(template->providesLoaded[i].indexesLoaded);
+                }
+            }
+
+            OSAL_Free(template->providesLoaded);
+        }
+
+        if(template->descriptionAssociatedWithTemplate)
+        {
+            cm_ELF_ReleaseDescription(
+                    template->requireNumber, template->requires,
+                    template->attributeNumber, template->attributes,
+                    template->propertyNumber, template->properties,
+                    template->provideNumber, template->provides);
+        }
+
+        // Free shared memories
+        cm_ELF_FreeTemplate(template->dspId, template->memories);
+
+        cm_StringRelease(template->name);
+
+        OSAL_Free(template);
+    }
+
+    return CM_OK;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration.h b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration.h
new file mode 100644
index 00000000000..98d22bba743
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_CONFIGURATION_H_
+#define __INC_CONFIGURATION_H_
+
+#include <cm/engine/api/control/configuration_engine.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <inc/nmf-limits.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+/******************************************************************************/
+/************************ FUNCTIONS PROTOTYPES ********************************/
+/******************************************************************************/
+
+PUBLIC t_cm_error cm_CFG_ConfigureMediaProcessorCore(t_nmf_core_id coreId,
+        t_nmf_executive_engine_id executiveEngineId,
+        t_nmf_semaphore_type_id semaphoreTypeId, t_uint8 nbYramBanks,
+        const t_cm_system_address *mediaProcessorMappingBaseAddr,
+        const t_cm_domain_id eeDomain,
+        t_dsp_allocator_desc* sdramCodeAllocId,
+        t_dsp_allocator_desc* sdramDataAllocId
+        );
+
+PUBLIC t_cm_error cm_CFG_AddMpcSdramSegment(const t_nmf_memory_segment *pDesc,
+        const char *memoryname, t_dsp_allocator_desc **allocDesc);
+
+PUBLIC t_cm_error cm_CFG_CheckMpcStatus(t_nmf_core_id coreId);
+
+void cm_CFG_ReleaseMpc(t_nmf_core_id coreId);
+
+#endif /* __INC_CONFIGURATION_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_status.h b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_status.h
new file mode 100644
index 00000000000..0c75b9c49b0
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_status.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_CONFIGSTATUS_H_
+#define __INC_CONFIGSTATUS_H
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/utils/inc/string.h>
+
+/*
+ * Variable to active intensive check
+ *
+ * \ingroup CM_CONFIGURATION_API
+ */
+extern t_sint32 cmIntensiveCheckState;
+
+/*
+ * Variable to active trace level
+ *
+ * \ingroup CM_CONFIGURATION_API
+ */
+extern t_sint32 cm_debug_level;
+
+/*
+ * Variable to active error break
+ *
+ * \ingroup CM_CONFIGURATION_API
+ */
+extern t_sint32 cm_error_break;
+
+/*
+ * Variable to activate Ulp
+ *
+ * \ingroup CM_CONFIGURATION_API
+ */
+extern t_bool cmUlpEnable;
+
+extern t_dup_char anonymousDup, eventDup, skeletonDup, stubDup, traceDup;
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_type.h b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_type.h
new file mode 100644
index 00000000000..af29d584ba4
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/inc/configuration_type.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Configuration Component Manager API type.
+ */
+#ifndef CONFIGURATION_TYPE_H
+#define CONFIGURATION_TYPE_H
+
+#include <cm/inc/cm_type.h>
+
+/*!
+ * @defgroup t_cm_cmd_id t_cm_cmd_id
+ * \brief Definition of the command ID
+ * \ingroup CM_CONFIGURATION_API
+ *
+ * CM_CMD_XXX designates the command ID used by the \ref CM_SetMode routine.
+ *
+ * \remarks Other command IDs are not yet implemented.
+ */
+
+typedef t_uint32 t_cm_cmd_id;                                     //!< Fake enumeration type \ingroup t_cm_cmd_id
+#define CM_CMD_SYNC            ((t_cm_cmd_id)0x01)  //!< Synchronize on-going operations (no parameter) \ingroup t_cm_cmd_id
+
+#define CM_CMD_WARM_RESET      ((t_cm_cmd_id)0x02)  //!< Reset a part of the CM-engine (parameter indicates the part which must be reseted) \ingroup t_cm_cmd_id
+
+#define CM_CMD_PWR_MGR         ((t_cm_cmd_id)0x10)  //!< Enable/Disable the internal power management module (0=Disable, 1=Enable) \ingroup t_cm_cmd_id
+
+#define CM_CMD_DBG_MODE        ((t_cm_cmd_id)0x40)  //!< Enable/Disable DEBUG mode, Pwr Mgr is also disabled (0=Disable, 1=Enable) \ingroup t_cm_cmd_id
+
+#define CM_CMD_TRACE_ON             ((t_cm_cmd_id)0x41)     //!< Enable STM/XTI tracing and force network resetting and dumping \note Since MPC trace will be usable, you can enable them if not \ingroup t_cm_cmd_id
+#define CM_CMD_TRACE_OFF            ((t_cm_cmd_id)0x42)     //!< Disable STM/XTI tracing \note Since MPC trace will not be usable, you can also disable them \ingroup t_cm_cmd_id
+
+#define CM_CMD_MPC_TRACE_ON         ((t_cm_cmd_id)0x50)     //!< Enable MPC STM/XTI tracing (param == coreId). \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_TRACE_OFF        ((t_cm_cmd_id)0x51)     //!< Disable MPC STM/XTI tracing (param == coreId) This is the default configuration. \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+
+#define CM_CMD_MPC_PRINT_OFF        ((t_cm_cmd_id)0x52)     //!< Set to OFF the level of MPC traces (param == coreId) \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_PRINT_ERROR      ((t_cm_cmd_id)0x53)     //!< Set to ERROR the level of MPC traces param == coreId) \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_PRINT_WARNING    ((t_cm_cmd_id)0x54)     //!< Set to WARNING the level of MPC traces param == coreId) \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_PRINT_INFO       ((t_cm_cmd_id)0x55)     //!< Set to INFO the level of MPC traces (param == coreId) \note This command is not execute if execution engine not started on the coreId This is the default configuration. \ingroup t_cm_cmd_id
+#define CM_CMD_MPC_PRINT_VERBOSE    ((t_cm_cmd_id)0x56)     //!< Set to VERBOSE the level of MPC traces param == coreId) \note This command is not execute if execution engine not started on the coreId \ingroup t_cm_cmd_id
+
+/*!
+ * \brief  Define the level of internal CM log traces
+ *
+ * Define the level of internal CM log traces (-1 to 3)
+ *    -# <b>-1 </b> all internal LOG/ERROR traces are disabled
+ *    -# <b> 0 </b> all internal LOG traces are disabled (<b>default/reset value</b>)
+ *    -# <b> 1, 2, 3 </b> Most and most
+ *
+ * \ingroup t_cm_cmd_id
+ */
+#define CM_CMD_TRACE_LEVEL     ((t_cm_cmd_id)0x80)
+
+/*!
+ * \brief  Enable/Disable intensive internal check
+ *
+ * Enable/Disable intensive internal check (0=Disable, 1=Enable):
+ *    - Component handle checking
+ *
+ * Must be used during the integration phase (additional process is time consuming).
+ *
+ * \ingroup t_cm_cmd_id
+ */
+#define CM_CMD_INTENSIVE_CHECK ((t_cm_cmd_id)0x100)
+
+/*!
+ * \brief  Enable/Disable ulp mode
+ *
+ * Enable/Disable Ultra Low Power mode.
+ *
+ * \ingroup t_cm_cmd_id
+ */
+#define CM_CMD_ULP_MODE_ON      ((t_cm_cmd_id)0x111)    //!< Enable ULP mode \ingroup t_cm_cmd_id
+#define CM_CMD_ULP_MODE_OFF     ((t_cm_cmd_id)0x110)    //!< Deprecated (must be removed in 2.10) !!!
+
+#endif /* CONFIGURATION_TYPE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration.c b/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration.c
new file mode 100644
index 00000000000..f092c7061b4
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/component/inc/initializer.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/repository_mgt/inc/repository_mgt.h>
+#include <inc/nmf-limits.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/memory/inc/domain.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/convert.h>
+
+#include <cm/engine/power_mgt/inc/power.h>
+
+t_sint32 cmIntensiveCheckState = 0;
+t_sint32 cm_debug_level = 1;
+t_sint32 cm_error_break = 0;
+t_bool cmUlpEnable = FALSE;
+
+
+#define MAX_EE_NAME_LENGTH      32
+typedef struct {
+    char eeName[MAX_EE_NAME_LENGTH];
+    t_nmf_executive_engine_id executiveEngineId;
+    t_uint32 EEmemoryCount;
+} t_cfg_mpc_desc;
+
+static t_cfg_mpc_desc        cfgMpcDescArray[NB_CORE_IDS];
+
+PUBLIC t_cm_error cm_CFG_ConfigureMediaProcessorCore(
+        t_nmf_core_id coreId,
+        t_nmf_executive_engine_id executiveEngineId,
+        t_nmf_semaphore_type_id semaphoreTypeId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *mediaProcessorMappingBaseAddr,
+        const t_cm_domain_id eeDomain,
+        t_dsp_allocator_desc *sdramCodeAllocDesc,
+        t_dsp_allocator_desc *sdramDataAllocDesc)
+{
+    /* Process requested configuration (save it) */
+    cfgMpcDescArray[coreId].EEmemoryCount = 0;
+    cfgMpcDescArray[coreId].executiveEngineId = executiveEngineId;
+    /* Build Executive Engine Name */
+    switch(executiveEngineId)
+    {
+        case SYNCHRONOUS_EXECUTIVE_ENGINE:
+            cm_StringCopy(cfgMpcDescArray[coreId].eeName, "synchronous_", MAX_EE_NAME_LENGTH);
+            break;
+        case HYBRID_EXECUTIVE_ENGINE:
+            cm_StringCopy(cfgMpcDescArray[coreId].eeName, "hybrid_", MAX_EE_NAME_LENGTH);
+            break;
+    }
+
+    switch(semaphoreTypeId)
+    {
+        case LOCAL_SEMAPHORES:
+            cm_StringConcatenate(cfgMpcDescArray[coreId].eeName, "lsem", MAX_EE_NAME_LENGTH);
+            break;
+        case SYSTEM_SEMAPHORES:
+            cm_StringConcatenate(cfgMpcDescArray[coreId].eeName, "hsem", MAX_EE_NAME_LENGTH);
+            break;
+    }
+
+    cm_SEM_InitMpc(coreId, semaphoreTypeId);
+
+    return cm_DSP_Add(coreId, nbYramBanks, mediaProcessorMappingBaseAddr, eeDomain, sdramCodeAllocDesc, sdramDataAllocDesc);
+}
+
+// TODO JPF: Move in dsp.c
+PUBLIC t_cm_error cm_CFG_AddMpcSdramSegment(const t_nmf_memory_segment *pDesc, const char* memoryname, t_dsp_allocator_desc **allocDesc)
+{
+    t_dsp_allocator_desc *desc;
+    if ( (pDesc == NULL) ||
+            ((pDesc->systemAddr.logical & CM_MM_ALIGN_64BYTES) != 0) )
+        return CM_INVALID_PARAMETER;
+
+    //TODO, juraj, the right place and way to do this?
+    desc = (t_dsp_allocator_desc*)OSAL_Alloc(sizeof (t_dsp_allocator_desc));
+    if (desc == 0)
+        return CM_NO_MORE_MEMORY;
+
+    desc->allocDesc = cm_MM_CreateAllocator(pDesc->size, 0, memoryname);
+    if (desc->allocDesc == 0) {
+    	OSAL_Free(desc);
+    	return CM_NO_MORE_MEMORY;
+    }
+    desc->baseAddress = pDesc->systemAddr;
+    desc->referenceCounter = 0;
+
+    *allocDesc = desc;
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_CFG_CheckMpcStatus(t_nmf_core_id coreId)
+{
+    t_cm_error error;
+
+    if (cm_DSP_GetState(coreId)->state == MPC_STATE_BOOTABLE)
+    {
+        /* Allocate coms fifo for a given MPC */
+        if ((error = cm_COM_AllocateMpc(coreId)) != CM_OK)
+            return error;
+
+        /* Launch EE */
+        if ((error = cm_EEM_Init(coreId,
+                                 cfgMpcDescArray[coreId].eeName,
+                                 cfgMpcDescArray[coreId].executiveEngineId)) != CM_OK)
+        {
+            cm_COM_FreeMpc(coreId);
+            return error;
+        }
+
+        /* Initialize coms fifo for a given MPC */
+        cm_COM_InitMpc(coreId);
+
+        /* Initialisation of the dedicated communication channel for component initialization */
+        if((error = cm_COMP_INIT_Init(coreId)) != CM_OK)
+        {
+            cm_EEM_Close(coreId);
+            cm_COM_FreeMpc(coreId);
+            return error;
+        }
+
+        cfgMpcDescArray[coreId].EEmemoryCount = cm_PWR_GetMPCMemoryCount(coreId);
+
+        if(cmUlpEnable)
+        {
+            // We have finish boot, allow MMDSP to go in auto idle
+            cm_EEM_AllowSleep(coreId);
+        }
+    }
+
+    if (cm_DSP_GetState(coreId)->state != MPC_STATE_BOOTED)
+        return CM_MPC_NOT_INITIALIZED;
+
+    return CM_OK;
+}
+
+void cm_CFG_ReleaseMpc(t_nmf_core_id coreId)
+{
+    t_uint32 memoryCount = cm_PWR_GetMPCMemoryCount(coreId);
+
+    // If No more memory and no more component (to avoid switch off in case of component using no memory)
+    if(
+            cm_PWR_GetMode() == NORMAL_PWR_MODE &&
+            memoryCount != 0 /* Just to see if there is something */ &&
+            memoryCount == cfgMpcDescArray[coreId].EEmemoryCount &&
+            cm_isComponentOnCoreId(coreId) == FALSE)
+    {
+        LOG_INTERNAL(1, "\n##### Shutdown %s #####\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+
+        (void)cm_EEM_ForceWakeup(coreId);
+
+        /* remove ee from load map here */
+        cm_COMP_INIT_Close(coreId);
+        cm_EEM_Close(coreId);
+        cm_COM_FreeMpc(coreId);
+
+        cfgMpcDescArray[coreId].EEmemoryCount = 0; // For debug purpose
+    }
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration_wrapper.c b/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration_wrapper.c
new file mode 100644
index 00000000000..bc3952e63b4
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/configuration/src/configuration_wrapper.c
@@ -0,0 +1,301 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/api/configuration_engine.h>
+#include <cm/engine/communication/inc/communication.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/memory/inc/chunk_mgr.h>
+#include <cm/engine/repository_mgt/inc/repository_mgt.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/power_mgt/inc/power.h>
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/api/executive_engine_mgt_engine.h>
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+t_dup_char anonymousDup, eventDup, skeletonDup, stubDup, traceDup;
+
+PUBLIC t_cm_error CM_ENGINE_Init(
+        const t_nmf_hw_mapping_desc *pNmfHwMappingDesc,
+        const t_nmf_config_desc *pNmfConfigDesc
+        )
+{
+    t_cm_error error;
+
+    // The purpose of that is just to not free/unfree some String frequently used
+    anonymousDup = cm_StringDuplicate("anonymous");
+    eventDup = cm_StringDuplicate("event");
+    skeletonDup = cm_StringDuplicate("skeleton");
+    stubDup = cm_StringDuplicate("stub");
+    traceDup = cm_StringDuplicate("trace");
+
+    if ((
+                error = cm_OSAL_Init()
+        ) != CM_OK) { return error; }
+
+    if ((
+                error = cm_COMP_Init()
+        ) != CM_OK) { return error; }
+
+    if ((
+    		    error = cm_PWR_Init()
+         ) != CM_OK) { return error; }
+
+    cm_TRC_traceReset();
+
+    if ((
+                error = cm_DM_Init()
+         ) != CM_OK) {return error; }
+
+    if ((
+                error = cm_SEM_Init(&pNmfHwMappingDesc->hwSemaphoresMappingBaseAddr)
+        ) != CM_OK) { return error; }
+
+    if ((error = cm_COM_Init(pNmfConfigDesc->comsLocation)) != CM_OK)
+        return error;
+
+    cm_DSP_Init(&pNmfHwMappingDesc->esramDesc);
+
+    return CM_OK;
+}
+
+PUBLIC void CM_ENGINE_Destroy(void)
+{
+    t_component_instance *instance;
+    t_cm_error error;
+    t_uint32 i;
+
+    /* PP: Well, on Linux (and probably on Symbian too), this is called when driver is removed
+     * => the module (driver) can't be removed if there are some pending clients
+     * => all remaining components should have been destroyed in CM_ENGINE_FlushClient()
+     * => So, if we found some components here, we are in BIG trouble ...
+     */
+    /* First, stop all remaining components  */
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	t_nmf_client_id clientId;
+
+	if ((instance = componentEntry(i)) == NULL)
+		continue;
+	clientId = domainDesc[instance->domainId].client;
+        LOG_INTERNAL(0, "Found a remaining component %s (%s) when destroying the CM !!!\n", instance->pathname, instance->Template->name, 0, 0, 0, 0);
+        if (/* skip EE */
+                (instance->Template->classe == FIRMWARE) ||
+                /* Skip all binding components */
+                (cm_StringCompare(instance->Template->name, "_ev.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_st.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_sk.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_tr.", 4) == 0))
+            continue;
+
+        /*
+         * Special code for SINGLETON handling
+         */
+        if(instance->Template->classe == SINGLETON)
+        {
+            struct t_client_of_singleton* cl = instance->clientOfSingleton;
+
+            clientId = instance->clientOfSingleton->clientId;
+            for( ; cl != NULL ; cl = cl->next)
+            {
+                if(cl == instance->clientOfSingleton)
+                {
+                    cl->numberOfStart = 1; // == 1 since it will go to 0 in cm_stopComponent
+                    cl->numberOfInstance = 1; // == 1 since it will go to 0 in cm_destroyInstanceForClient
+                }
+                else
+                {
+                    cl->numberOfStart = 0;
+                    cl->numberOfInstance = 0;
+                }
+                cl->numberOfBind = 0;
+            }
+        }
+
+        // Stop the component
+        error = cm_stopComponent(instance, clientId);
+        if (error != CM_OK && error != CM_COMPONENT_NOT_STARTED)
+            LOG_INTERNAL(0, "Error stopping component %s/%x (%s, error=%d, client=%u)\n", instance->pathname, instance, instance->Template->name, error, clientId, 0);
+
+        // Destroy dependencies
+        cm_destroyRequireInterface(instance, clientId);
+    }
+
+    /* Destroy all remaining components */
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	t_nmf_client_id clientId;
+
+	if ((instance = componentEntry(i)) == NULL)
+		continue;
+        clientId = domainDesc[instance->domainId].client;
+
+        if (/* skip EE */
+                (instance->Template->classe == FIRMWARE) ||
+                /* Skip all binding components */
+                (cm_StringCompare(instance->Template->name, "_ev.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_st.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_sk.", 4) == 0) ||
+                (cm_StringCompare(instance->Template->name, "_tr.", 4) == 0)) {
+            continue;
+        }
+
+        if(instance->Template->classe == SINGLETON)
+        {
+            clientId = instance->clientOfSingleton->clientId;
+        }
+
+        // Destroy the component
+        error = cm_destroyInstanceForClient(instance, DESTROY_WITHOUT_CHECK, clientId);
+
+        if (error != CM_OK)
+        {
+            /* FIXME : add component name instance in log message but need to make a copy before cm_flushComponent()
+             *         because it's no more available after.
+             */
+            LOG_INTERNAL(0, "Error flushing component (error=%d, client=%u)\n", error, clientId, 0, 0, 0, 0);
+        }
+    }
+
+    /* This will power off all ressources and destroy EE */
+    cm_PWR_SetMode(NORMAL_PWR_MODE);
+    cm_DSP_Destroy();
+    cm_DM_Destroy();
+    /* Nothing to do about SEM */
+    //cm_MM_Destroy();
+    cm_REP_Destroy();
+    cm_COMP_Destroy();
+    cm_OSAL_Destroy();
+
+    cm_StringRelease(traceDup);
+    cm_StringRelease(stubDup);
+    cm_StringRelease(skeletonDup);
+    cm_StringRelease(eventDup);
+    cm_StringRelease(anonymousDup);
+}
+
+PUBLIC t_cm_error CM_ENGINE_ConfigureMediaProcessorCore(
+        t_nmf_core_id coreId,
+        t_nmf_executive_engine_id executiveEngineId,
+        t_nmf_semaphore_type_id semaphoreTypeId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *mediaProcessorMappingBaseAddr,
+        const t_cm_domain_id eeDomain,
+        const t_cfg_allocator_id sdramCodeAllocId,
+        const t_cfg_allocator_id sdramDataAllocId
+        )
+{
+    return cm_CFG_ConfigureMediaProcessorCore(
+            coreId,
+            executiveEngineId,
+            semaphoreTypeId,
+            nbYramBanks,
+            mediaProcessorMappingBaseAddr,
+            eeDomain,
+            (t_dsp_allocator_desc*)sdramCodeAllocId,
+            (t_dsp_allocator_desc*)sdramDataAllocId
+            );
+}
+
+PUBLIC t_cm_error CM_ENGINE_AddMpcSdramSegment(
+        const t_nmf_memory_segment *pDesc,
+        t_cfg_allocator_id         *id,
+        const char                 *memoryname
+        )
+{
+    return cm_CFG_AddMpcSdramSegment(pDesc, memoryname == NULL ? "" : memoryname, (t_dsp_allocator_desc**)id);
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_SetMode(t_cm_cmd_id aCmdID, t_sint32 aParam)
+{
+	t_cm_error error = CM_OK;
+	int i;
+
+    OSAL_LOCK_API();
+
+    switch(aCmdID) {
+    case CM_CMD_DBG_MODE:
+        cm_PWR_SetMode(( aParam==1 ) ? DISABLE_PWR_MODE : NORMAL_PWR_MODE);
+        switch(cm_PWR_GetMode())
+        {
+        case NORMAL_PWR_MODE:
+            // Release the MPC (which will switch it off if no more used)
+            for (i=FIRST_MPC_ID; i<NB_CORE_IDS; i++)
+            {
+                cm_CFG_ReleaseMpc(i);
+            }
+            break;
+        case DISABLE_PWR_MODE:
+            // Force the load of the EE if not already done.
+            for (i=FIRST_MPC_ID; i<NB_CORE_IDS;i++)
+            {
+                if((error = cm_CFG_CheckMpcStatus(i)) != CM_OK)
+                    break;
+            }
+            break;
+        }
+        break;
+	case CM_CMD_TRACE_LEVEL:
+	    if (aParam<-1) cm_debug_level = -1;
+	    else cm_debug_level = aParam;
+	    break;
+	case CM_CMD_INTENSIVE_CHECK:
+	    cmIntensiveCheckState = aParam;
+	    break;
+
+	case CM_CMD_TRACE_ON:
+	    cm_trace_enabled = TRUE;
+        cm_TRC_Dump();
+	    break;
+	case CM_CMD_TRACE_OFF:
+	    cm_trace_enabled = FALSE;
+	    break;
+
+	case CM_CMD_MPC_TRACE_ON:
+	    cm_EEM_setTraceMode((t_nmf_core_id)aParam, 1);
+	    break;
+	case CM_CMD_MPC_TRACE_OFF:
+	    cm_EEM_setTraceMode((t_nmf_core_id)aParam, 0);
+	    break;
+
+	case CM_CMD_MPC_PRINT_OFF:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 0);
+	    break;
+	case CM_CMD_MPC_PRINT_ERROR:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 1);
+	    break;
+	case CM_CMD_MPC_PRINT_WARNING:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 2);
+	    break;
+	case CM_CMD_MPC_PRINT_INFO:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 3);
+	    break;
+	case CM_CMD_MPC_PRINT_VERBOSE:
+	    cm_EEM_setPrintLevel((t_nmf_core_id)aParam, 4);
+	    break;
+
+	case CM_CMD_ULP_MODE_ON:
+	    cmUlpEnable = TRUE;
+	    break;
+
+	default:
+	    error = CM_INVALID_PARAMETER;
+	    break;
+	}
+
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/inc/dsp.h b/drivers/staging/nmf-cm/cm/engine/dsp/inc/dsp.h
new file mode 100644
index 00000000000..439deac115f
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/inc/dsp.h
@@ -0,0 +1,453 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief DSP abstraction layer
+ *
+ * \defgroup DSP_INTERNAL Private DSP Abstraction Layer API.
+ *
+ */
+#ifndef __INC_CM_DSP_H
+#define __INC_CM_DSP_H
+
+#include <cm/inc/cm_type.h>
+#include <share/inc/nmf.h>
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/memory/inc/remote_allocator.h>
+
+
+#define SxA_NB_BLOCK_RAM        8 /*32kworks (24-bit) */
+
+#define SxA_LOCKED_WAY          1
+
+/*
+ * Type defintion to handle dsp offset in word
+ */
+typedef t_uint32 t_dsp_offset;
+
+typedef t_uint32 t_dsp_address;
+
+typedef enum {
+    DSP2ARM_IRQ_0,
+    DSP2ARM_IRQ_1
+} t_mpc2host_irq_num;
+
+typedef enum {
+    ARM2DSP_IRQ_0,
+    ARM2DSP_IRQ_1,
+    ARM2DSP_IRQ_2,
+    ARM2DSP_IRQ_3
+} t_host2mpc_irq_num;
+
+typedef enum {
+    INTERNAL_XRAM24 = 0,  /* 24-bit XRAM */
+    INTERNAL_XRAM16 = 1,  /* 16-bit XRAM */
+    INTERNAL_YRAM24 = 2,  /* 24-bit YRAM */
+    INTERNAL_YRAM16 = 3,  /* 16-bit YRAM */
+    SDRAM_EXT24     = 4,  /* 24-bit external "X" memory */
+    SDRAM_EXT16     = 5,  /* 16-bit external "X" memory */
+    ESRAM_EXT24     = 6,  /* ESRAM24 */
+    ESRAM_EXT16     = 7,  /* ESRAM16 */
+    SDRAM_CODE      = 8,  /* Program memory */
+    ESRAM_CODE      = 9,  /* ESRAM code */
+    LOCKED_CODE     = 10, /* For way locking */
+    NB_DSP_MEMORY_TYPE,
+    DEFAULT_DSP_MEM_TYPE = MASK_ALL16
+} t_dsp_memory_type_id;
+
+typedef struct {
+    t_cm_allocator_desc  *allocDesc;
+    t_cm_system_address   baseAddress;
+    t_uint32              referenceCounter;
+} t_dsp_allocator_desc;
+
+typedef struct {
+    t_cm_system_address  base;
+    t_uint32             size;
+} t_dsp_segment;
+
+typedef enum {
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    SDRAM_CODE_EE,
+    SDRAM_CODE_USER,
+    SDRAM_DATA_EE,
+    SDRAM_DATA_USER,
+    NB_MIGRATION_SEGMENT,
+    ESRAM_CODE_EE = NB_MIGRATION_SEGMENT,
+    ESRAM_CODE_USER,
+    ESRAM_DATA_EE,
+    ESRAM_DATA_USER,
+#else
+    SDRAM_CODE_EE,
+    SDRAM_DATA_EE,
+    ESRAM_CODE_EE,
+    ESRAM_DATA_EE,
+#endif
+    NB_DSP_SEGMENT_TYPE
+} t_dsp_segment_type;
+
+typedef struct {
+    t_dsp_segment_type  segmentType;
+    t_uint32            baseOffset;
+} t_dsp_address_info;
+
+typedef enum  {
+    MPC_STATE_UNCONFIGURED,
+    MPC_STATE_BOOTABLE,
+    MPC_STATE_BOOTED,
+    MPC_STATE_PANIC,
+} t_dsp_state;
+
+typedef struct {
+    t_dsp_state             state;
+    t_uint8                 nbYramBank;
+    t_cm_domain_id          domainEE;
+    t_dsp_allocator_desc   *allocator[NB_DSP_MEMORY_TYPE];
+    t_dsp_segment           segments[NB_DSP_SEGMENT_TYPE];
+    t_uint32                yram_offset;
+    t_uint32                yram_size;
+    t_uint32                locked_offset;
+    t_uint32                locked_size;
+} t_dsp_desc;
+
+typedef struct {
+    t_nmf_core_id coreId;
+    t_dsp_memory_type_id memType; // Index in MPC desc allocator
+    t_cm_allocator_desc *alloc;
+} t_dsp_chunk_info;
+
+PUBLIC const t_dsp_desc* cm_DSP_GetState(t_nmf_core_id coreId);
+PUBLIC void cm_DSP_SetStatePanic(t_nmf_core_id coreId);
+
+PUBLIC void cm_DSP_Init(const t_nmf_memory_segment *pEsramDesc);
+PUBLIC void cm_DSP_Destroy(void);
+
+/*!
+ * \brief Initialize the memory segments management of a given MPC
+ *
+ * \param[in] coreId Identifier of the DSP to initialize
+ * \param[in] pDspMapDesc DSP mapping into host space
+ * \param[in] memConf configuration of the DSP memories (standalone or shared)
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_Add(t_nmf_core_id coreId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *pDspMapDesc,
+        const t_cm_domain_id eeDomain,
+        t_dsp_allocator_desc *sdramCodeAllocDesc,
+        t_dsp_allocator_desc *sdramDataAllocDesc);
+
+
+
+/*!
+ * \brief Configure a given Media Processor Core
+ *
+ * This routine programs the configuration (caches, ahb wrapper, ...) registers of a given MPC.
+ *
+ * \param[in] coreId Identifier of the DSP to initialize
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_Boot(t_nmf_core_id coreId);
+
+/*!
+ * \brief Boot a given DSP
+ *
+ * This routine allows after having initialized and loaded the EE into a given DSP to start it (boot it)
+ *
+ * \param[in] coreId identifier of the DSP to boot
+ * \param[in] panicReasonOffset offset of panic reason which will pass to NONE_PANIC when DSP booted.
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_ConfigureAfterBoot(t_nmf_core_id coreId);
+
+PUBLIC void cm_DSP_Start(t_nmf_core_id coreId);
+
+PUBLIC void cm_DSP_Stop(t_nmf_core_id coreId);
+
+/*!
+ * \brief Shutdown a given DSP
+ *
+ * This routine allows to stop and shutdown a given DSP
+ *
+ * \param[in] coreId identifier of the DSP to shutdown
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_Shutdown(t_nmf_core_id coreId);
+
+PUBLIC t_uint32 cm_DSP_ReadXRamWord(t_nmf_core_id coreId, t_uint32 dspOffset);
+PUBLIC void cm_DSP_WriteXRamWord(t_nmf_core_id coreId, t_uint32 dspOffset, t_uint32 value);
+
+/*!
+ * \brief Convert a Dsp address (offset inside a given DSP memory segment) into the  host address (logical)
+ *
+ * \param[in] coreId identifier of the given DSP
+ * \param[in] dspAddress dsp address to be converted
+ * \param[in] memType memory type identifier
+ *
+ * \retval t_cm_logical_address
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_logical_address cm_DSP_ConvertDspAddressToHostLogicalAddress(t_nmf_core_id coreId, t_shared_addr dspAddress);
+
+/*!
+ * \brief Acknowledge the local interrupt of a given DSP (when not using HW semaphore mechanisms)
+ *
+ * \param[in] coreId identifier of the given DSP
+ * \param[in] irqNum irq identifier
+ *
+ * \retval void
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_AcknowledgeDspIrq(t_nmf_core_id coreId, t_mpc2host_irq_num irqNum);
+
+
+/*
+ * Memory Management API routines
+ */
+
+/*!
+ * \brief Retrieve DSP information for a memory chunk.
+ *
+ * This function retrieves information stored in user-data of the allocated chunk.
+ * See also \ref{t_dsp_chunk_info}.
+ *
+ * \param[in]  memHandle Handle to the allocated chunk.
+ * \param[out] info Dsp information structure.
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_GetDspChunkInfo(t_memory_handle memHandle, t_dsp_chunk_info *info);
+
+/*!
+ * \brief Get memory allocator for a given memory type on a DSP.
+ *
+ * \param[in]  coreId Dsp identifier.
+ * \param[in]  memType Memory type identifier.
+ *
+ * \retval reference to the allocator descriptor (or null)
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_allocator_desc* cm_DSP_GetAllocator(t_nmf_core_id coreId, t_dsp_memory_type_id memType);
+
+/*!
+ * \brief Get DSP internal memory (TCM) information for allocation.
+ *
+ * For DSP-internal memories (TCMX, Y 16/24), return the offset and size of the allocation zone (for domain
+ * mechanism) and the allocation memory type.
+ *
+ * \param[in]  coreId Dsp identifier.
+ * \param[in]  memType Memory type identifier.
+ * \param[out] mem_info Memory information structure.
+ *
+ * \retval CM_OK
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_GetInternalMemoriesInfo(t_cm_domain_id domainId, t_dsp_memory_type_id memType,
+        t_uint32 *offset, t_uint32 *size);
+
+
+/*!
+ * \brief Convert word size to byte size.
+ *
+ * \param[in] memType Memory type identifier.
+ * \param[in] wordSize Word size to be converted.
+ *
+ * \retval Byte size.
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_uint32 cm_DSP_ConvertSize(t_dsp_memory_type_id memType, t_uint32 wordSize);
+
+/*!
+ * \brief Provide the Memory status of a given memory type for a given DSP
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] memType Type of memory.
+ * \param[out] pStatus requested memory status
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_GetAllocatorStatus(t_nmf_core_id coreId, t_dsp_memory_type_id memType, t_uint32 offset, t_uint32 size, t_cm_allocator_status *pStatus);
+
+/*!
+ * \brief Provide DSP memory host shared address
+ *
+ * \param[in] memHandle Allocated block handle
+ * \param[out] pAddr Returned system address.
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_GetHostSystemAddress( t_memory_handle memHandle, t_cm_system_address *pAddr);
+
+/*!
+ * \brief Get physical address of a memory chunk.
+ *
+ * \param[in] memHandle Memory handle.
+ *
+ * \retval Physical address.
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_physical_address cm_DSP_GetPhysicalAdress(t_memory_handle memHandle);
+
+/*!
+ * \brief Return Logical Address of an allocated memory chunk.
+ *
+ * \param[in] memHandle Allocated chunk handle
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_logical_address cm_DSP_GetHostLogicalAddress(t_memory_handle memHandle);
+
+/*!
+ * \brief Provide DSP memory DSP address (offset inside a given DSP memory segment)
+ *
+ * \param[in] memHandle Allocated block handle
+ * \param[out] dspAddress allocated block address seen by the given DSP
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_GetDspAddress(t_memory_handle handle, t_uint32 *pDspAddress);
+
+/*!
+ * \brief Return the adress of the DSP base associated to the memory type.
+ * Caution, this information is valid only in normal state (not when migrated).
+ *
+ * \param[in]  coreId DSP Identifier.
+ * \param[in]  memType Type of memory.
+ * \param[out] pAddr Base address.
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_GetDspBaseAddress(t_nmf_core_id coreId, t_dsp_memory_type_id memType, t_cm_system_address *pAddr);
+
+/*!
+ * \brief Return DSP memory handle offset (offset inside a given DSP memory)
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] memType Type of memory.
+ * \param[in] memHandle Allocated block handle
+ *
+ * \retval t_uint32: Offset of memory handle inside memory
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_uint32 cm_DSP_GetDspMemoryHandleOffset(
+        t_nmf_core_id coreId,
+        t_dsp_memory_type_id dspMemType,
+        t_memory_handle memHandle);
+
+/*!
+ * \brief Provide DSP memory handle size
+ *
+ * \param[in] memHandle Allocated block handle
+ * \param[out] pDspSize Size of the given memory handle
+
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC void cm_DSP_GetDspMemoryHandleSize(t_memory_handle memHandle, t_uint32 *pDspSize);
+
+/*!
+ * \brief Resize xram allocator to reserve spave for stack.
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] newStackSize New required stack size.
+
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_setStackSize(t_nmf_core_id coreId, t_uint32 newStackSize);
+
+/*!
+ * \brief Allow to know if nbYramBanks parameter is valid for coreId. This api is need since use of nbYramBanks
+ *        is deferred.
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] nbYramBanks number of yramBanks to use.
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_IsNbYramBanksValid(t_nmf_core_id coreId, t_uint8 nbYramBanks);
+
+/*!
+ * \brief Allow to know stack base address according to coreId and nbYramBanks use.
+ *
+ * \param[in] coreId dsp identifier.
+ * \param[in] nbYramBanks number of yramBanks to use.
+ *
+ * \retval t_uint32 return stack address
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_uint32 cm_DSP_getStackAddr(t_nmf_core_id coreId);
+
+/*!
+ * \brief For a give dsp adress return the offset from the hardware base that the adress is relative to.
+ *
+ * \param[in]  coreId DSP identifier.
+ * \param[in]  adr    DSP address.
+ * \param[out] info   Info structure containing (hw base id, offset)
+ *
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_GetDspDataAddressInfo(t_nmf_core_id coreId, t_uint32 adr, t_dsp_address_info *info);
+
+/*!
+ * \brief Modify the mapping of a code hardware base. Used for memory migration.
+ *
+ * The function calculates the new hardware base so that in the DSP address-space,
+ * the source address will be mapped to the destination address.
+ *
+ * \param[in] coreId DSP Identifier.
+ * \param[in] hwSegment Identifier of the hardware segment (thus hardware base).
+ * \param[in] src Source address
+ * \param[in] dst Destination address
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_updateCodeBase(t_nmf_core_id coreId, t_dsp_segment_type hwSegment, t_cm_system_address src, t_cm_system_address dst);
+
+/*!
+ * \brief Modify the mapping of a data hardware base. Used for memory migration.
+ *
+ * The function calculates the new hardware base so that in the DSP address-space,
+ * the source address will be mapped to the destination address.
+ *
+ * \param[in] coreId DSP Identifier.
+ * \param[in] hwSegment Identifier of the hardware segment (thus hardware base).
+ * \param[in] src Source address
+ * \param[in] dst Destination address
+ *
+ * \retval t_cm_error
+ * \ingroup DSP_INTERNAL
+ */
+PUBLIC t_cm_error cm_DSP_updateDataBase(t_nmf_core_id coreId, t_dsp_segment_type hwSegment, t_cm_system_address src, t_cm_system_address dst);
+
+#endif /* __INC_CM_DSP_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/inc/semaphores_dsp.h b/drivers/staging/nmf-cm/cm/engine/dsp/inc/semaphores_dsp.h
new file mode 100644
index 00000000000..1bb1c34cced
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/inc/semaphores_dsp.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_CM_SEMAPHORES_DSP_H
+#define __INC_CM_SEMAPHORES_DSP_H
+
+#include <share/semaphores/inc/semaphores.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+PUBLIC void cm_DSP_SEM_Take(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_DSP_SEM_Give(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_DSP_SEM_GenerateIrq(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_DSP_AssertDspIrq(t_nmf_core_id coreId, t_host2mpc_irq_num irqNum);
+
+PUBLIC void cm_DSP_AcknowledgeDspIrq(t_nmf_core_id coreId, t_mpc2host_irq_num irqNum);
+
+#endif /* __INC_CM_SEMAPHORES_DSP_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h b/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h
new file mode 100644
index 00000000000..0ddc71d2c4f
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h
@@ -0,0 +1,959 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_MMDSP_HWP_H
+#define __INC_MMDSP_HWP_H
+
+#include <cm/inc/cm_type.h>
+
+#define MMDSP_NB_BLOCK_RAM      8
+#define MMDSP_RAM_BLOCK_SIZE    4096    /* 0x1000 */
+#define MMDSP_NB_TIMER          3
+#define MMDSP_NB_BIT_SEM        8
+#define MMDSP_NB_DMA_IF         8
+#define MMDSP_NB_DMA_CTRL       4
+#define MMDSP_NB_ITREMAP_REG    32
+
+#define MMDSP_INSTRUCTION_WORD_SIZE     (sizeof(t_uint64))
+#define MMDSP_ICACHE_LINE_SIZE_IN_INST  (4)
+#define MMDSP_ICACHE_LINE_SIZE  (MMDSP_ICACHE_LINE_SIZE_IN_INST * MMDSP_INSTRUCTION_WORD_SIZE)
+
+#define MMDSP_DATA_WORD_SIZE     (3)
+#define MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE (sizeof(t_uint32))
+#define MMDSP_DATA_WORD_SIZE_IN_EXT24 (sizeof(t_uint32))
+#define MMDSP_DATA_WORD_SIZE_IN_EXT16 (sizeof(t_uint16))
+#define MMDSP_DCACHE_LINE_SIZE_IN_WORDS  (8)
+#define MMDSP_DCACHE_LINE_SIZE  (MMDSP_DCACHE_LINE_SIZE_IN_WORDS * sizeof(t_uint32))
+
+#define MMDSP_NB_IO             16
+
+#define MMDSP_CODE_CACHE_WAY_SIZE   256
+
+//#define MMDSP_ESRAM_DSP_BASE_ADDR         0xE0000 /* 64-bit words */
+//#define MMDSP_DATA24_DSP_BASE_ADDR        0x10000
+//#define MMDSP_DATA16_DSP_BASE_ADDR        0x800000
+//#define MMDSP_MMIO_DSP_BASE_ADDR          0xF80000
+
+/* Specified according MMDSP & ELF convention */
+/* Note: Here we assume that ESRAM is less than 2MB */
+#define SDRAMTEXT_BASE_ADDR             0x00000000
+#define ESRAMTEXT_BASE_ADDR             0x000E0000
+
+#define SDRAMMEM24_BASE_ADDR            0x00010000
+#define ESRAMMEM24_BASE_ADDR            0x00600000 /* ELF == 0x00400000 TODO: Update it in MMDSP ELF compiler */
+#define SDRAMMEM16_BASE_ADDR            0x00800000
+#define ESRAMMEM16_BASE_ADDR            0x00D80000 /* ELF == 0x00BC0000 TODO: Update it in MMDSP ELF compiler */
+
+#define MMIO_BASE_ADDR                  0x00F80000
+
+/*
+ * Definition of indirect host registers
+ */
+#define IHOST_ICACHE_FLUSH_REG                            0x0
+#define IHOST_ICACHE_FLUSH_CMD_ENABLE                     (t_uint64)MASK_BIT0
+#define IHOST_ICACHE_FLUSH_ALL_ENTRIES_CMD                (t_uint64)0x0
+#if 0
+#define IHOST_ICACHE_INVALID_ALL_UNLOCKED_L2_LINES_CMD    (t_uint64)0x8
+#define IHOST_ICACHE_INVALID_ALL_LOCKED_L2_LINES_CMD      (t_uint64)0xA
+#define IHOST_ICACHE_UNLOCK_ALL_LOCKED_L2_LINES_CMD       (t_uint64)0xC
+#define IHOST_ICACHE_LOCK_ALL_WAYS_LESSER_THAN_LOCK_V_CMD (t_uint64)0xE
+#else
+#define IHOST_ICACHE_INVALID_ALL_UNLOCKED_L2_LINES_CMD    (t_uint64)0x10
+#define IHOST_ICACHE_INVALID_ALL_LOCKED_L2_LINES_CMD      (t_uint64)0x12
+#define IHOST_ICACHE_UNLOCK_ALL_LOCKED_L2_LINES_CMD       (t_uint64)0x14
+#define IHOST_ICACHE_LOCK_ALL_WAYS_LESSER_THAN_LOCK_V_CMD (t_uint64)0x16
+#define IHOST_ICACHE_FLUSH_BY_SERVICE                     (t_uint64)0x18
+#define IHOST_ICACHE_FLUSH_OUTSIDE_RANGE                  (t_uint64)0x1A
+#endif
+
+#define IHOST_ICACHE_LOCK_V_REG                           0x1
+
+#define IHOST_ICACHE_MODE_REG                             0x2
+#define IHOST_ICACHE_MODE_PERFMETER_ON                    (t_uint64)MASK_BIT0
+#define IHOST_ICACHE_MODE_PERFMETER_OFF                   (t_uint64)0x0
+#define IHOST_ICACHE_MODE_L2_CACHE_ON                     (t_uint64)MASK_BIT1
+#define IHOST_ICACHE_MODE_L2_CACHE_OFF                    (t_uint64)0x0
+#define IHOST_ICACHE_MODE_L1_CACHE_ON                     (t_uint64)MASK_BIT2
+#define IHOST_ICACHE_MODE_L1_CACHE_OFF                    (t_uint64)0x0
+#define IHOST_ICACHE_MODE_FILL_MODE_ON                    (t_uint64)MASK_BIT3
+#define IHOST_ICACHE_MODE_FILL_MODE_OFF                   (t_uint64)0x0
+
+#define IHOST_CLEAR_PERFMETER_REG                         0x3
+#define IHOST_CLEAR_PERFMETER_ON                          (t_uint64)0x1
+#define IHOST_CLEAR_PERFMETER_OFF                         (t_uint64)0x0
+
+#define IHOST_PERF_HIT_STATUS_REG                         0x4
+
+#define IHOST_PERF_MISS_STATUS_REG                        0x5
+
+#define IHOST_FILL_START_WAY_REG                          0x6
+#define IHOST_FILL_START_ADDR_VALUE_SHIFT                 0U
+#define IHOST_FILL_WAY_NUMBER_SHIFT                       20U
+
+#define IHOST_PRG_BASE_ADDR_REG                           0x7
+#define IHOST_PRG_BASE1_ADDR_SHIFT                        0
+#define IHOST_PRG_BASE2_ADDR_SHIFT                        32
+
+#if defined(__STN_8500) && (__STN_8500>10)
+#define IHOST_PRG_BASE_34_ADDR_REG                        0x1A
+#define IHOST_PRG_BASE3_ADDR_SHIFT                        0
+#define IHOST_PRG_BASE4_ADDR_SHIFT                        32
+#endif
+
+#if defined(__STN_8815) /* __STN_8815 */
+#define IHOST_PRG_AHB_CONF_REG                            0x8
+#define IHOST_PRG_AHB_LOCKED_SHIFT                        0U
+#define IHOST_PRG_AHB_PROT_SHIFT                          1U
+
+#define AHB_LOCKED_ON                                   (t_uint64)1
+#define AHB_LOCKED_OFF                                  (t_uint64)0
+
+#define AHB_PROT_USER                                   (t_uint64)0
+#define AHB_PROT_PRIVILEGED                             (t_uint64)MASK_BIT0
+#define AHB_PROT_NONBUFFERABLE                          (t_uint64)0
+#define AHB_PROT_BUFFERABLE                             (t_uint64)MASK_BIT1
+#define AHB_PROT_NONCACHEABLE                           (t_uint64)0
+#define AHB_PROT_CACHEABLE                              (t_uint64)MASK_BIT2
+
+
+#define IHOST_DATA_AHB_CONF_REG                           0x9
+#define IHOST_DATA_AHB_LOCKED_SHIFT                       0U
+#define IHOST_DATA_AHB_PROT_SHIFT                         1U
+#else /* def __STN_8820 or __STN_8500 */
+#define IHOST_STBUS_ID_CONF_REG                           0x8
+#define SAA_STBUS_ID                                      176 /* = 0xB0 */
+#define SVA_STBUS_ID                                      4   /* = 0x4  */
+#define SIA_STBUS_ID                                      180 /* = 0xB4 */
+
+#define IHOST_STBUF_CONF_REG                              0x9  /* RESERVED */
+#endif /* __STN_8820 or __STN_8500 */
+
+#define IHOST_DATA_EXT_BUS_BASE_REG                       0xA
+#define IHOST_DATA_EXT_BUS_BASE_16_SHIFT                  32ULL
+#define IHOST_DATA_EXT_BUS_BASE_24_SHIFT                  0ULL
+
+#define IHOST_EXT_MMIO_BASE_DATA_EXT_BUS_TOP_REG          0xB
+#define IHOST_EXT_MMIO_DATA_EXT_BUS_TOP_SHIFT             0ULL
+#define IHOST_EXT_MMIO_BASE_ADDR_SHIFT                    32ULL
+
+#define IHOST_DATA_EXT_BUS_BASE2_REG                      0xC
+#define IHOST_DATA_EXT_BUS_BASE2_16_SHIFT                 32ULL
+#define IHOST_DATA_EXT_BUS_BASE2_24_SHIFT                 0ULL
+
+#if defined(__STN_8500) && (__STN_8500>10)
+
+#define IHOST_DATA_EXT_BUS_BASE3_REG                      0x1B
+#define IHOST_DATA_EXT_BUS_BASE3_16_SHIFT                 32ULL
+#define IHOST_DATA_EXT_BUS_BASE3_24_SHIFT                 0ULL
+
+#define IHOST_DATA_EXT_BUS_BASE4_REG                      0x1C
+#define IHOST_DATA_EXT_BUS_BASE4_16_SHIFT                 32ULL
+#define IHOST_DATA_EXT_BUS_BASE4_24_SHIFT                 0ULL
+
+#endif
+
+#define IHOST_ICACHE_STATE_REG                            0xD
+#define IHOST_ICACHE_STATE_RESET                          0x0
+#define IHOST_ICACHE_STATE_INITAGL2                       0x1
+#define IHOST_ICACHE_STATE_READY_TO_START                 0x2
+#define IHOST_ICACHE_STATE_WAIT_FOR_MISS                  0x3
+#define IHOST_ICACHE_STATE_FILLDATARAM0                   0x4
+#define IHOST_ICACHE_STATE_FILLDATARAM1                   0x5
+#define IHOST_ICACHE_STATE_FILLDATARAM2                   0x6
+#define IHOST_ICACHE_STATE_FILLDATARAM3                   0x7
+#define IHOST_ICACHE_STATE_FLUSH                          0x8
+#define IHOST_ICACHE_STATE_FILL_INIT                      0x9
+#define IHOST_ICACHE_STATE_FILL_LOOP                      0xA
+#define IHOST_ICACHE_STATE_FILL_LOOP0                     0xB
+#define IHOST_ICACHE_STATE_FILL_LOOP1                     0xC
+#define IHOST_ICACHE_STATE_FILL_LOOP2                     0xD
+#define IHOST_ICACHE_STATE_FILL_LOOP3                     0xE
+#define IHOST_ICACHE_STATE_FILL_END                       0xF
+#define IHOST_ICACHE_STATE_SPECIFIC_FLUSH_R               0x10
+#define IHOST_ICACHE_STATE_SPECIFIC_FLUSH_W               0x11
+#define IHOST_ICACHE_STATE_SPECIFIC_FLUSH_END             0x12
+#define IHOST_ICACHE_STATE_OTHERS                         0x1F
+
+#define IHOST_EN_EXT_BUS_TIMEOUT_REG                      0xE
+#define IHOST_TIMEOUT_ENABLE                              1ULL
+#define IHOST_TIMEOUT_DISABLE                             0ULL
+
+#define IHOST_DATA2_1624_XA_BASE_REG                      0xF
+#define IHOST_DATA2_24_XA_BASE_SHIFT                      0ULL
+#define IHOST_DATA2_16_XA_BASE_SHIFT                      32ULL
+#if defined(__STN_8500) && (__STN_8500>10)
+#define IHOST_DATA3_24_XA_BASE_SHIFT                      8ULL
+#define IHOST_DATA3_16_XA_BASE_SHIFT                      40ULL
+#define IHOST_DATA4_24_XA_BASE_SHIFT                      16ULL
+#define IHOST_DATA4_16_XA_BASE_SHIFT                      48ULL
+#endif
+
+#define IHOST_PERFMETERS_MODE_REG                         0x10
+
+#if defined(__STN_8815) /* __STN_8815 */
+#define IHOST_EXT_MMIO_AHB_CONF_REG                       0x11
+#define IHOST_EXT_MMIO_AHB_LOCKED_SHIFT                   0U
+#define IHOST_EXT_MMIO_AHB_PROT_SHIFT                     1U
+#else /* def __STN_8820 or __STN_8500 */
+#define IHOST_EXT_MMIO_STBS_CONF_REG                      0x11 /* RESERVED */
+#endif /* __STN_8820 or __STN_8500 */
+
+#define IHOST_PRG_BASE_SEL_REG                            0x12
+#define IHOST_PRG_BASE_SEL_OFF                            (t_uint64)0
+#define IHOST_PRG_BASE_SEL_ON                             (t_uint64)1
+
+#define IHOST_PRG_BASE2_ACTIV_REG                         0x13
+#define IHOST_PRG_BASE2_ACTIV_OFF                         (t_uint64)0
+#if defined(__STN_8500) && (__STN_8500>10)
+/* TODO : for the moment just divide mmdsp in fix 4 spaces */
+    #define IHOST_PRG_BASE2_ACTIV_ON                      (t_uint64)((((t_uint64)0xf0000>>10)<<48) | (((t_uint64)0xe0000>>10)<<32) | (((t_uint64)0x70000>>10)<<16) | 1)
+#else
+    #define IHOST_PRG_BASE2_ACTIV_ON                      (t_uint64)1
+#endif
+
+#define IHOST_DATA_EXT_BUS_TOP_16_24_REG                      0x14
+#define IHOST_DATA_EXT_BUS_TOP_24_SHIFT                       0ULL
+#define IHOST_DATA_EXT_BUS_TOP_16_SHIFT                       32ULL
+
+#define IHOST_DATA_TOP_16_24_CHK_REG                      0x16
+#define IHOST_DATA_TOP_16_24_CHK_OFF                      (t_uint64)0
+#define IHOST_DATA_TOP_16_24_CHK_ON                       (t_uint64)1
+
+#define IHOST_EXT_BUS_TOP2_16_24_REG                      0x15
+#define IHOST_DATA_EXT_BUS_TOP2_24_SHIFT                  0ULL
+#define IHOST_DATA_EXT_BUS_TOP2_16_SHIFT                  32ULL
+
+#if defined(__STN_8500) && (__STN_8500>10)
+
+#define IHOST_EXT_BUS_TOP3_16_24_REG                      0x1D
+#define IHOST_DATA_EXT_BUS_TOP3_24_SHIFT                  0ULL
+#define IHOST_DATA_EXT_BUS_TOP3_16_SHIFT                  32ULL
+
+#define IHOST_EXT_BUS_TOP4_16_24_REG                      0x1E
+#define IHOST_DATA_EXT_BUS_TOP4_24_SHIFT                  0ULL
+#define IHOST_DATA_EXT_BUS_TOP4_16_SHIFT                  32ULL
+
+#endif
+
+#define IHOST_DATA_BASE2_ACTIV_REG                        0x17
+#define IHOST_DATA_BASE2_ACTIV_OFF                        (t_uint64)0
+#define IHOST_DATA_BASE2_ACTIV_ON                         (t_uint64)1
+
+#define IHOST_INST_BURST_SZ_REG                           0x18
+#define IHOST_INST_BURST_SZ_ALWAYS_1_LINE                 (t_uint64)0x0
+#define IHOST_INST_BURST_SZ_ALWAYS_2_LINES                (t_uint64)0x1
+#define IHOST_INST_BURST_SZ_AUTO                          (t_uint64)0x2  /* 2 lines for SDRAM [0, 0xE0000[, 1 line for ESRAM [0xE0000, 0xFFFFF] */
+
+#define IHOST_ICACHE_END_CLEAR_REG                        0x19
+#define IHOST_ICACHE_START_CLEAR_REG                      IHOST_FILL_START_WAY_REG
+
+/*
+ * Definition of value of the ucmd register
+ */
+#define MMDSP_UCMD_WRITE                  0
+#define MMDSP_UCMD_READ                   4
+#define MMDSP_UCMD_CTRL_STATUS_ACCESS     0x10 // (MASK_BIT4 | !MASK_BIT3 | !MASK_BIT0)
+#define MMDSP_UCMD_DECREMENT_ADDR         MASK_BIT5
+#define MMDSP_UCMD_INCREMENT_ADDR         MASK_BIT1
+
+/*
+ * Definition of value of the ubkcmd register
+ */
+#define MMDSP_UBKCMD_EXT_CODE_MEM_ACCESS_ENABLE    MASK_BIT3
+#define MMDSP_UBKCMD_EXT_CODE_MEM_ACCESS_DISABLE   0
+
+/*
+ * Definition of value of the clockcmd register
+ */
+#define MMDSP_CLOCKCMD_STOP_CLOCK         MASK_BIT0
+#define MMDSP_CLOCKCMD_START_CLOCK        0
+
+/*
+ * Definition of macros used to access indirect addressed host register
+ */
+#define WRITE_INDIRECT_HOST_REG(pRegs, addr, value64) \
+{ \
+    (pRegs)->host_reg.emul_uaddrl = addr; \
+    (pRegs)->host_reg.emul_uaddrm = 0;    \
+    (pRegs)->host_reg.emul_uaddrh = 0;    \
+    (pRegs)->host_reg.emul_udata[0] = ((value64 >> 0ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[1] = ((value64 >> 8ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[2] = ((value64 >> 16ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[3] = ((value64 >> 24ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[4] = ((value64 >> 32ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[5] = ((value64 >> 40ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[6] = ((value64 >> 48ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_udata[7] = ((value64 >> 56ULL) & MASK_BYTE0);  \
+    (pRegs)->host_reg.emul_ucmd = (MMDSP_UCMD_CTRL_STATUS_ACCESS | MMDSP_UCMD_WRITE); \
+}
+
+#define READ_INDIRECT_HOST_REG(pRegs, addr, value64) \
+{ \
+    (pRegs)->host_reg.emul_udata[0] = 0;  \
+    (pRegs)->host_reg.emul_udata[1] = 0;  \
+    (pRegs)->host_reg.emul_udata[2] = 0;  \
+    (pRegs)->host_reg.emul_udata[3] = 0;  \
+    (pRegs)->host_reg.emul_udata[4] = 0;  \
+    (pRegs)->host_reg.emul_udata[5] = 0;  \
+    (pRegs)->host_reg.emul_udata[6] = 0;  \
+    (pRegs)->host_reg.emul_udata[7] = 0;  \
+    (pRegs)->host_reg.emul_uaddrl = addr; \
+    (pRegs)->host_reg.emul_uaddrm = 0;    \
+    (pRegs)->host_reg.emul_uaddrh = 0;    \
+    (pRegs)->host_reg.emul_ucmd = (MMDSP_UCMD_CTRL_STATUS_ACCESS | MMDSP_UCMD_READ); \
+    value64 = (((t_uint64)((pRegs)->host_reg.emul_udata[0])) << 0ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[1])) << 8ULL)  |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[2])) << 16ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[3])) << 24ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[4])) << 32ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[5])) << 40ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[6])) << 48ULL) |  \
+        (((t_uint64)((pRegs)->host_reg.emul_udata[7])) << 56ULL); \
+}
+
+/* Common type to handle 64-bit modulo field in 32-bit mode */
+typedef struct {
+    t_uint32 value;
+    t_uint32 dummy;
+} t_mmdsp_field_32;
+
+typedef struct {
+    t_uint16 value;
+    t_uint16 dummy;
+} t_mmdsp_field_16;
+
+/* DCache registers */
+#define DCACHE_MODE_ENABLE              MASK_BIT0
+#define DCACHE_MODE_DISABLE             0
+#define DCACHE_MODE_DIVIDE_PER_2        MASK_BIT1
+#define DCACHE_MODE_DIVIDE_PER_4        MASK_BIT2
+#define DCACHE_MODE_CHECK_TAG_ENABLE    MASK_BIT3
+#define DCACHE_MODE_CHECK_TAG_DISABLE   0
+#define DCACHE_MODE_FORCE_LOCK_MODE     MASK_BIT4
+#define DCACHE_MODE_LOCK_BIT            MASK_BIT5
+
+#define DCACHE_CONTROL_PREFETCH_LINE            MASK_BIT0
+#define DCACHE_CONTROL_NON_BLOCKING_REFILL      0
+#define DCACHE_CONTROL_FAST_READ_DISABLE        MASK_BIT1
+#define DCACHE_CONTROL_FAST_READ_ENABLE         0
+#define DCACHE_CONTROL_ON_FLY_FILL_ACCESS_OFF   MASK_BIT2
+#define DCACHE_CONTROL_ON_FLY_FILL_ACCESS_ON    0
+#define DCACHE_CONTROL_BURST_1_WRAP8            MASK_BIT3
+#define DCACHE_CONTROL_BURST_2_WRAP4            0
+#define DCACHE_CONTROL_NOT_USE_DATA_BUFFER      MASK_BIT4
+#define DCACHE_CONTROL_USE_DATA_BUFFER          0
+#define DCACHE_CONTROL_WRITE_POSTING_ENABLE     MASK_BIT5
+#define DCACHE_CONTROL_WRITE_POSTING_DISABLE    0
+
+#define DCACHE_CMD_NOP                          0
+#define DCACHE_CMD_DISCARD_WAY                  2 //see Dcache_way reg
+#define DCACHE_CMD_DISCARD_LINE                 3 //see Dcache_line reg
+#define DCACHE_CMD_FREE_WAY                     4 //see Dcache_way reg
+#define DCACHE_CMD_FREE_LINE                    5 //see Dchache_line reg
+#define DCACHE_CMD_FLUSH                        7
+
+#define DCACHE_STATUS_CURRENT_WAY_MASK          (MASK_BIT2 | MASK_BIT1 | MASK_BIT0)
+#define DCACHE_STATUS_TAG_HIT_MASK              MASK_BIT3
+#define DCACHE_STATUS_TAG_LOCKED_MASK           MASK_BIT4
+#define DCACHE_STATUS_PROTECTION_ERROR_MASK     MASK_BIT5
+
+#define DCACHE_CPTRSEL_COUNTER_1_MASK           (MASK_BIT3 | MASK_BIT2 | MASK_BIT1 | MASK_BIT0)
+#define DCACHE_CPTRSEL_COUNTER_1_SHIFT          0
+#define DCACHE_CPTRSEL_COUNTER_2_MASK           (MASK_BIT7 | MASK_BIT6 | MASK_BIT5 | MASK_BIT4)
+#define DCACHE_CPTRSEL_COUNTER_2_SHIFT          4
+#define DCACHE_CPTRSEL_COUNTER_3_MASK           (MASK_BIT11 | MASK_BIT10 | MASK_BIT9 | MASK_BIT8)
+#define DCACHE_CPTRSEL_COUNTER_3_SHIFT          8
+#define DCACHE_CPTRSEL_XBUS_ACCESS_TO_CACHE_RAM 1
+#define DCACHE_CPTRSEL_CACHE_HIT                2
+#define DCACHE_CPTRSEL_LINE_MATCH               3
+#define DCACHE_CPTRSEL_XBUS_WS                  4
+#define DCACHE_CPTRSEL_EXTMEM_WS                5
+#define DCACHE_CPTRSEL_CACHE_READ               6
+#define DCACHE_CPTRSEL_CACHE_WRITE              7
+#define DCACHE_CPTRSEL_TAG_HIT_READ             8
+#define DCACHE_CPTRSEL_TAG_LOCKED_ACCESS        9
+#define DCACHE_CPTRSEL_TAG_MEM_READ_CYCLE       10
+#define DCACHE_CPTRSEL_TAG_MEM_WRITE_CYCLE      11
+
+
+typedef volatile struct {
+    t_uint16 padding_1[5];
+    t_uint16 mode;
+    t_uint16 control;
+    t_uint16 way;
+    t_uint16 line;
+    t_uint16 command;
+    t_uint16 status;
+    t_uint16 cptr1l;
+    t_uint16 cptr1h;
+    t_uint16 cptr2l;
+    t_uint16 cptr2h;
+    t_uint16 cptr3l;
+    t_uint16 cptr3h;
+    t_uint16 cptrsel;
+    t_uint16 flush_base_lsb; /* only on STn8820 and STn8500 */
+    t_uint16 flush_base_msb; /* only on STn8820 and STn8500 */
+    t_uint16 flush_top_lsb; /* only on STn8820 and STn8500 */
+    t_uint16 flush_top_msb; /* only on STn8820 and STn8500 */
+    t_uint16 padding_2[10];
+} t_mmdsp_dcache_regs_16;
+
+typedef volatile struct {
+    t_uint32 padding_1[5];
+    t_uint32 mode;
+    t_uint32 control;
+    t_uint32 way;
+    t_uint32 line;
+    t_uint32 command;
+    t_uint32 status;
+    t_uint32 cptr1l;
+    t_uint32 cptr1h;
+    t_uint32 cptr2l;
+    t_uint32 cptr2h;
+    t_uint32 cptr3l;
+    t_uint32 cptr3h;
+    t_uint32 cptrsel;
+    t_uint32 flush_base_lsb; /* only on STn8820 and STn8500 */
+    t_uint32 flush_base_msb; /* only on STn8820 and STn8500 */
+    t_uint32 flush_top_lsb; /* only on STn8820 and STn8500 */
+    t_uint32 flush_top_msb; /* only on STn8820 and STn8500 */
+    t_uint32 padding_2[10];
+} t_mmdsp_dcache_regs_32;
+
+/* TIMER Registers */
+typedef volatile struct {
+    t_mmdsp_field_16    timer_msb;
+    t_mmdsp_field_16    timer_lsb;
+} t_mmdsp_timer_regs_16;
+
+typedef volatile struct {
+    t_mmdsp_field_32    timer_msb;
+    t_mmdsp_field_32    timer_lsb;
+} t_mmdsp_timer_regs_32;
+
+
+/* DMA interface Registers */
+typedef volatile struct {
+    t_uint16    arm_dma_sreq;        /* dma0: 5e800, dma1: +0x20 ...*/
+    t_uint16    arm_dma_breq;        /* ... 5e802 */
+    t_uint16    arm_dma_lsreq;       /* ... 5e804 */
+    t_uint16    arm_dma_lbreq;
+    t_uint16    arm_dma_maskit;
+    t_uint16    arm_dma_it;
+    t_uint16    arm_dma_auto;
+    t_uint16    arm_dma_lauto;
+    t_uint16    dma_reserved[8];
+} t_mmdsp_dma_if_regs_16;
+
+typedef volatile struct {
+    t_uint32    arm_dma_sreq;        /* dma0: 3a800, dma1: +0x40 ...*/
+    t_uint32    arm_dma_breq;        /* ... 3a804 */
+    t_uint32    arm_dma_lsreq;       /* ... 3a808 */
+    t_uint32    arm_dma_lbreq;
+    t_uint32    arm_dma_maskit;
+    t_uint32    arm_dma_it;
+    t_uint32    arm_dma_auto;
+    t_uint32    arm_dma_lauto;
+    t_uint32    dma_reserved[8];
+} t_mmdsp_dma_if_regs_32;
+
+/* MMDSP DMA controller Registers */
+typedef volatile struct {
+    t_uint16    dma_ctrl;            /* dma0: 0x5d400, dma1: +0x10 ... */
+    t_uint16    dma_int_base;        /* ... 0x5d402 */
+    t_uint16    dma_int_length;      /* ... 0x5d404 */
+    t_uint16    dma_ext_baseh;
+    t_uint16    dma_ext_basel;
+    t_uint16    dma_count;
+    t_uint16    dma_ext_length;
+    t_uint16    dma_it_status;
+} t_mmdsp_dma_ctrl_regs_16;
+
+typedef volatile struct {
+    t_uint32    dma_ctrl;            /* dma0: 0x3a800, dma1: +0x20 ... */
+    t_uint32    dma_int_base;        /* ... 0x3a804 */
+    t_uint32    dma_int_length;      /* ... 0x3a808 */
+    t_uint32    dma_ext_baseh;
+    t_uint32    dma_ext_basel;
+    t_uint32    dma_count;
+    t_uint32    dma_ext_length;
+    t_uint32    dma_it_status;
+} t_mmdsp_dma_ctrl_regs_32;
+
+/* IO registers */
+typedef volatile struct {
+    t_mmdsp_field_16 io_bit[MMDSP_NB_IO];
+    t_mmdsp_field_16 io_lsb;
+    t_mmdsp_field_16 io_msb;
+    t_mmdsp_field_16 io_all;
+    t_mmdsp_field_16 io_en;
+} t_mmdsp_io_regs_16;
+
+typedef volatile struct {
+    t_mmdsp_field_32 io_bit[MMDSP_NB_IO];
+    t_mmdsp_field_32 io_lsb;
+    t_mmdsp_field_32 io_msb;
+    t_mmdsp_field_32 io_all;
+    t_mmdsp_field_32 io_en;
+} t_mmdsp_io_regs_32;
+
+/* HOST Registers bit mapping */
+#define HOST_GATEDCLK_ITREMAP   MASK_BIT0
+#define HOST_GATEDCLK_SYSDMA     MASK_BIT1
+#define HOST_GATEDCLK_INTEG_REGS MASK_BIT2
+#define HOST_GATEDCLK_TIMER_GPIO MASK_BIT3
+#define HOST_GATEDCLK_XBUSDMA   MASK_BIT4
+#define HOST_GATEDCLK_STACKCTRL MASK_BIT5
+#define HOST_GATEDCLK_ITC       MASK_BIT6
+
+/* Only for STn8820 and STn8500 */
+#define HOST_PWR_DBG_MODE       MASK_BIT0
+#define HOST_PWR_DC_STATUS      (MASK_BIT1 | MASK_BIT2 | MASK_BIT3 | MASK_BIT4 | MASK_BIT5)
+#define HOST_PWR_DE_STATUS      MASK_BIT6
+#define HOST_PWR_STOV_STATUS    MASK_BIT7
+
+/* HOST Registers */
+typedef volatile struct {
+    t_uint16    ident;               /*0x...60000*/
+    t_uint16    identx[4];           /*0x...60002..8*/
+    t_uint16    r5;                  /*0x...6000a*/
+    t_uint16    r6;                  /*0x...6000c*/
+    t_uint16    inte[2];             /*0x...6000e..10*/
+    t_uint16    intx[2];             /*0x...60012..14*/
+    t_uint16    int_ris[2];          /*0x...60016..18*/
+    t_uint16    intpol;              /*0x...6001a*/
+    t_uint16    pwr;                 /*0x...6001c*/ /* only on STn8820 and STn8500 */
+    t_uint16    gatedclk;            /*0x...6001e*/
+    t_uint16    softreset;           /*0x...60020*/
+    t_uint16    int_icr[2];          /*0x...60022..24*/
+    t_uint16    cmd[4];              /*0x...60026..2c*/
+    t_uint16    RESERVED4;
+    t_uint16    int_mis0;            /*0x...60030*/
+    t_uint16    RESERVED5;
+    t_uint16    RESERVED6;
+    t_uint16    RESERVED7;
+    t_uint16    i2cdiv;              /*0x...60038*/
+    t_uint16    int_mis1;            /*0x...6003a*/
+    t_uint16    RESERVED8;
+    t_uint16    RESERVED9;
+    t_uint16    emul_udata[8];      /*0x...60040..4e*/
+    t_uint16    emul_uaddrl;        /*0x...60050*/
+    t_uint16    emul_uaddrm;        /*0x...60052*/
+    t_uint16    emul_ucmd;          /*0x...60054*/
+    t_uint16    emul_ubkcmd;        /*0x...60056*/
+    t_uint16    emul_bk2addl;       /*0x...60058*/
+    t_uint16    emul_bk2addm;       /*0x...6005a*/
+    t_uint16    emul_bk2addh;       /*0x...6005c*/
+    t_uint16    emul_mdata[3];      /*0x...6005e..62*/
+    t_uint16    emul_maddl;         /*0x...60064*/
+    t_uint16    emul_maddm;         /*0x...60066*/
+    t_uint16    emul_mcmd;          /*0x...60068*/
+    t_uint16    emul_maddh;         /*0x...6006a*/
+    t_uint16    emul_uaddrh;        /*0x...6006c*/
+    t_uint16    emul_bk_eql;        /*0x...6006e*/
+    t_uint16    emul_bk_eqh;        /*0x...60070*/
+    t_uint16    emul_bk_combi;      /*0x...60072*/
+    t_uint16    emul_clockcmd;      /*0x...60074*/
+    t_uint16    emul_stepcmd;       /*0x...60076*/
+    t_uint16    emul_scanreg;       /*0x...60078*/
+    t_uint16    emul_breakcountl;   /*0x...6007a*/
+    t_uint16    emul_breakcounth;   /*0x...6007c*/
+    t_uint16    emul_forcescan;     /*0x...6007e*/
+    t_uint16    user_area[(0x200 - 0x80)>>1];
+} t_mmdsp_host_regs_16;
+
+typedef volatile struct {
+    t_uint32    ident;               /*0x...60000*/
+    t_uint32    identx[4];           /*0x...60004..10*/
+    t_uint32    r5;                  /*0x...60014*/
+    t_uint32    r6;                  /*0x...60018*/
+    t_uint32    inte[2];             /*0x...6001c..20*/
+    t_uint32    intx[2];             /*0x...60024..28*/
+    t_uint32    int_ris[2];          /*0x...6002c..30*/
+    t_uint32    intpol;              /*0x...60034*/
+    t_uint32    pwr;                 /*0x...60038*/ /* only on STn8820 and STn8500 */
+    t_uint32    gatedclk;            /*0x...6003c*/
+    t_uint32    softreset;           /*0x...60040*/
+    t_uint32    int_icr[2];          /*0x...60044..48*/
+    t_uint32    cmd[4];              /*0x...6004c..58*/
+    t_uint32    RESERVED4;
+    t_uint32    int_mis0;            /*0x...60060*/
+    t_uint32    RESERVED5;
+    t_uint32    RESERVED6;
+    t_uint32    RESERVED7;
+    t_uint32    i2cdiv;              /*0x...60070*/
+    t_uint32    int_mis1;            /*0x...60074*/
+    t_uint32    RESERVED8;
+    t_uint32    RESERVED9;
+    t_uint32    emul_udata[8];      /*0x...60080..9c*/
+    t_uint32    emul_uaddrl;        /*0x...600a0*/
+    t_uint32    emul_uaddrm;        /*0x...600a4*/
+    t_uint32    emul_ucmd;          /*0x...600a8*/
+    t_uint32    emul_ubkcmd;        /*0x...600ac*/
+    t_uint32    emul_bk2addl;       /*0x...600b0*/
+    t_uint32    emul_bk2addm;       /*0x...600b4*/
+    t_uint32    emul_bk2addh;       /*0x...600b8*/
+    t_uint32    emul_mdata[3];      /*0x...600bc..c4*/
+    t_uint32    emul_maddl;         /*0x...600c8*/
+    t_uint32    emul_maddm;         /*0x...600cc*/
+    t_uint32    emul_mcmd;          /*0x...600d0*/
+    t_uint32    emul_maddh;         /*0x...600d4*/
+    t_uint32    emul_uaddrh;        /*0x...600d8*/
+    t_uint32    emul_bk_eql;        /*0x...600dc*/
+    t_uint32    emul_bk_eqh;        /*0x...600e0*/
+    t_uint32    emul_bk_combi;      /*0x...600e4*/
+    t_uint32    emul_clockcmd;      /*0x...600e8*/
+    t_uint32    emul_stepcmd;       /*0x...600ec*/
+    t_uint32    emul_scanreg;       /*0x...600f0*/
+    t_uint32    emul_breakcountl;   /*0x...600f4*/
+    t_uint32    emul_breakcounth;   /*0x...600f8*/
+    t_uint32    emul_forcescan;     /*0x...600fc*/
+    t_uint32    user_area[(0x400 - 0x100)>>2];
+} t_mmdsp_host_regs_32;
+
+/* MMIO blocks */
+#if defined(__STN_8820) || defined(__STN_8500)
+typedef volatile struct {
+    t_uint16 RESERVED1[(0xD400-0x8000)>>1];
+
+    t_mmdsp_dma_ctrl_regs_16 dma_ctrl[MMDSP_NB_DMA_CTRL];
+
+    t_uint16 RESERVED2[(0xD800-0xD440)>>1];
+
+    t_mmdsp_dcache_regs_16 dcache;
+
+    t_uint16 RESERVED3[(0xE000-0xD840)>>1];
+
+    t_mmdsp_io_regs_16 io;
+
+    t_uint16 RESERVED4[(0x60-0x50)>>1];
+
+    t_mmdsp_timer_regs_16 timer[MMDSP_NB_TIMER];
+
+    t_uint16 RESERVED5[(0x410-0x78)>>1];
+
+    t_mmdsp_field_16 sem[MMDSP_NB_BIT_SEM];
+
+    t_uint16 RESERVED6[(0x450-0x430)>>1];
+
+    t_mmdsp_field_16 ipen;
+    t_uint16 itip_0;
+    t_uint16 itip_1;
+    t_uint16 itip_2;
+    t_uint16 itip_3;
+    t_uint16 itop_0;
+    t_uint16 itop_1;
+    t_uint16 itop_2;
+    t_uint16 itop_3;
+    t_uint16 RESERVED7[(0x8a-0x64)>>1];
+    t_uint16 itip_4;
+    t_uint16 itop_4;
+
+    t_uint16 RESERVED8[(0x7e0-0x48e)>>1];
+
+    t_mmdsp_field_16 id[4];
+    t_mmdsp_field_16 idp[4];
+
+    t_mmdsp_dma_if_regs_16 dma_if[MMDSP_NB_DMA_IF];
+
+    t_uint16 RESERVED9[(0xC00-0x900)>>1];
+
+    t_mmdsp_field_16 emu_unit_maskit;
+    t_mmdsp_field_16 RESERVED[3];
+    t_mmdsp_field_16 config_data_mem;
+    t_mmdsp_field_16 compatibility;
+
+    t_uint16 RESERVED10[(0xF000-0xEC18)>>1];
+
+    t_uint16 stbus_if_config;
+    t_uint16 stbus_if_mode;
+    t_uint16 stbus_if_status;
+    t_uint16 stbus_if_security;
+    t_uint16 stbus_if_flush;
+    t_uint16 stbus_reserved;
+    t_uint16 stbus_if_priority;
+    t_uint16 stbus_msb_attribut;
+
+    t_uint16 RESERVED11[(0xFC00-0xF010)>>1];
+
+    t_mmdsp_field_16 itremap_reg[MMDSP_NB_ITREMAP_REG];
+    t_mmdsp_field_16 itmsk_l_reg;
+    t_mmdsp_field_16 itmsk_h_reg;
+
+    t_uint16 RESERVED12[(0xfc9c - 0xfc88)>>1];
+
+    t_mmdsp_field_16 itmemo_l_reg;
+    t_mmdsp_field_16 itmeme_h_reg;
+
+    t_uint16 RESERVED13[(0xfd00 - 0xfca4)>>1];
+
+    t_mmdsp_field_16 itremap1_reg[MMDSP_NB_ITREMAP_REG];
+
+    t_uint16 RESERVED14[(0x60000 - 0x5fd80)>>1];
+} t_mmdsp_mmio_regs_16;
+
+
+typedef volatile struct {
+    t_uint32 RESERVED1[(0xa800)>>2];
+
+    t_mmdsp_dma_ctrl_regs_32 dma_ctrl[MMDSP_NB_DMA_CTRL];
+
+    t_uint32 RESERVED2[(0xb000-0xa880)>>2];
+
+    t_mmdsp_dcache_regs_32 dcache;
+
+    t_uint32 RESERVED3[(0xc000-0xb080)>>2];
+
+    t_mmdsp_io_regs_32 io;
+
+    t_uint32 RESERVED4[(0xc0-0xa0)>>2];
+
+    t_mmdsp_timer_regs_32 timer[MMDSP_NB_TIMER];
+
+    t_uint32 RESERVED5[(0x820-0x0f0)>>2];
+
+    t_mmdsp_field_32 sem[MMDSP_NB_BIT_SEM];
+
+    t_uint32 RESERVED6[(0x8a0-0x860)>>2];
+
+    t_mmdsp_field_32 ipen;
+    t_uint32 itip_0;
+    t_uint32 itip_1;
+    t_uint32 itip_2;
+    t_uint32 itip_3;
+    t_uint32 itop_0;
+    t_uint32 itop_1;
+    t_uint32 itop_2;
+    t_uint32 itop_3;
+    t_uint32 RESERVED7[(0x914-0x8c8)>>2];
+    t_uint32 itip_4;
+    t_uint32 itop_4;
+
+    t_uint32 RESERVED8[(0xcfc0-0xc91c)>>2];
+
+    t_mmdsp_field_32 id[4];
+    t_mmdsp_field_32 idp[4];
+
+    t_mmdsp_dma_if_regs_32 dma_if[MMDSP_NB_DMA_IF];
+
+    t_uint32 RESERVED9[(0x800-0x200)>>2];
+
+    t_mmdsp_field_32 emu_unit_maskit;
+    t_mmdsp_field_32 RESERVED[3];
+    t_mmdsp_field_32 config_data_mem;
+    t_mmdsp_field_32 compatibility;
+
+    t_uint32 RESERVED10[(0xE000-0xD830)>>2];
+
+    t_uint32 stbus_if_config;
+    t_uint32 stbus_if_mode;
+    t_uint32 stbus_if_status;
+    t_uint32 stbus_if_security;
+    t_uint32 stbus_if_flush;
+    t_uint32 stbus_reserved;
+    t_uint32 stbus_if_priority;
+    t_uint32 stbus_msb_attribut;
+
+    t_uint32 RESERVED11[(0xF800-0xE020)>>2];
+
+    t_mmdsp_field_32 itremap_reg[MMDSP_NB_ITREMAP_REG];
+    t_mmdsp_field_32 itmsk_l_reg;
+    t_mmdsp_field_32 itmsk_h_reg;
+
+    t_uint32 RESERVED12[(0xf938 - 0xf910)>>2];
+
+    t_mmdsp_field_32 itmemo_l_reg;
+    t_mmdsp_field_32 itmeme_h_reg;
+
+    t_uint32 RESERVED13[(0xfa00 - 0xf948)>>2];
+
+    t_mmdsp_field_32 itremap1_reg[MMDSP_NB_ITREMAP_REG];
+
+    t_uint32 RESERVED14[(0x40000 - 0x3fb00)>>2];
+} t_mmdsp_mmio_regs_32;
+#endif /* __STN_8820 or __STN_8500 */
+
+#ifdef __STN_8815
+typedef volatile struct {
+    t_uint16 RESERVED1[(0xD400-0x8000)>>1];
+
+    t_mmdsp_dma_ctrl_regs_16 dma_ctrl[MMDSP_NB_DMA_CTRL];
+
+    t_uint16 RESERVED2[(0xD800-0xD440)>>1];
+
+    t_mmdsp_dcache_regs_16 dcache;
+
+    t_uint16 RESERVED3[(0xE000-0xD840)>>1];
+
+    t_mmdsp_io_regs_16 io;
+
+    t_uint16 RESERVED4[(0x60-0x50)>>1];
+
+    t_mmdsp_timer_regs_16 timer[MMDSP_NB_TIMER];
+
+    t_uint16 RESERVED5[(0x410-0x78)>>1];
+
+    t_mmdsp_field_16 sem[MMDSP_NB_BIT_SEM];
+
+    t_uint16 RESERVED6[(0x450-0x430)>>1];
+
+    t_mmdsp_field_16 ipen;
+    t_uint16 itip_0;
+    t_uint16 itip_1;
+    t_uint16 itip_2;
+    t_uint16 itip_3;
+    t_uint16 itop_0;
+    t_uint16 itop_1;
+    t_uint16 itop_2;
+    t_uint16 itop_3;
+    t_uint16 RESERVED7[(0x8a-0x64)>>1];
+    t_uint16 itip_4;
+    t_uint16 itop_4;
+
+    t_uint16 RESERVED8[(0x7e0-0x48e)>>1];
+
+    t_mmdsp_field_16 id[4];
+    t_mmdsp_field_16 idp[4];
+
+    t_mmdsp_dma_if_regs_16 dma_if[MMDSP_NB_DMA_IF];
+
+    t_uint16 RESERVED9[(0xC00-0x900)>>1];
+
+    t_mmdsp_field_16 emu_unit_maskit;
+    t_mmdsp_field_16 RESERVED[3];
+    t_mmdsp_field_16 config_data_mem;
+    t_mmdsp_field_16 compatibility;
+
+    t_uint16 RESERVED10[(0xF000-0xEC18)>>1];
+
+    t_uint16 ahb_if_config;
+    t_uint16 ahb_if_mode;
+    t_uint16 ahb_if_status;
+    t_uint16 ahb_if_security;
+    t_uint16 ahb_if_flush;
+
+    t_uint16 RESERVED11[(0xFC00-0xF00A)>>1];
+
+    t_mmdsp_field_16 itremap_reg[MMDSP_NB_ITREMAP_REG];
+    t_mmdsp_field_16 itmsk_l_reg;
+    t_mmdsp_field_16 itmsk_h_reg;
+
+    t_uint16 RESERVED12[(0xfc9c - 0xfc88)>>1];
+
+    t_mmdsp_field_16 itmemo_l_reg;
+    t_mmdsp_field_16 itmeme_h_reg;
+
+    t_uint16 RESERVED13[(0xfd00 - 0xfca4)>>1];
+
+    t_mmdsp_field_16 itremap1_reg[MMDSP_NB_ITREMAP_REG];
+
+    t_uint16 RESERVED14[(0x60000 - 0x5fd80)>>1];
+} t_mmdsp_mmio_regs_16;
+
+
+typedef volatile struct {
+    t_uint32 RESERVED1[(0xa800)>>2];
+
+    t_mmdsp_dma_ctrl_regs_32 dma_ctrl[MMDSP_NB_DMA_CTRL];
+
+    t_uint32 RESERVED2[(0xb000-0xa880)>>2];
+
+    t_mmdsp_dcache_regs_32 dcache;
+
+    t_uint32 RESERVED3[(0xc000-0xb080)>>2];
+
+    t_mmdsp_io_regs_32 io;
+
+    t_uint32 RESERVED4[(0xc0-0xa0)>>2];
+
+    t_mmdsp_timer_regs_32 timer[MMDSP_NB_TIMER];
+
+    t_uint32 RESERVED5[(0x820-0x0f0)>>2];
+
+    t_mmdsp_field_32 sem[MMDSP_NB_BIT_SEM];
+
+    t_uint32 RESERVED6[(0x8a0-0x860)>>2];
+
+    t_mmdsp_field_32 ipen;
+    t_uint32 itip_0;
+    t_uint32 itip_1;
+    t_uint32 itip_2;
+    t_uint32 itip_3;
+    t_uint32 itop_0;
+    t_uint32 itop_1;
+    t_uint32 itop_2;
+    t_uint32 itop_3;
+    t_uint32 RESERVED7[(0x914-0x8c8)>>2];
+    t_uint32 itip_4;
+    t_uint32 itop_4;
+
+    t_uint32 RESERVED8[(0xcfc0-0xc91c)>>2];
+
+    t_mmdsp_field_32 id[4];
+    t_mmdsp_field_32 idp[4];
+
+    t_mmdsp_dma_if_regs_32 dma_if[MMDSP_NB_DMA_IF];
+
+    t_uint32 RESERVED9[(0x800-0x200)>>2];
+
+    t_mmdsp_field_32 emu_unit_maskit;
+    t_mmdsp_field_32 RESERVED[3];
+    t_mmdsp_field_32 config_data_mem;
+    t_mmdsp_field_32 compatibility;
+
+    t_uint32 RESERVED10[(0xE000-0xD830)>>2];
+
+    t_uint32 ahb_if_config;
+    t_uint32 ahb_if_mode;
+    t_uint32 ahb_if_status;
+    t_uint32 ahb_if_security;
+    t_uint32 ahb_if_flush;
+
+    t_uint32 RESERVED11[(0xF800-0xE014)>>2];
+
+    t_mmdsp_field_32 itremap_reg[MMDSP_NB_ITREMAP_REG];
+    t_mmdsp_field_32 itmsk_l_reg;
+    t_mmdsp_field_32 itmsk_h_reg;
+
+    t_uint32 RESERVED12[(0xf938 - 0xf910)>>2];
+
+    t_mmdsp_field_32 itmemo_l_reg;
+    t_mmdsp_field_32 itmeme_h_reg;
+
+    t_uint32 RESERVED13[(0xfa00 - 0xf948)>>2];
+
+    t_mmdsp_field_32 itremap1_reg[MMDSP_NB_ITREMAP_REG];
+
+    t_uint32 RESERVED14[(0x40000 - 0x3fb00)>>2];
+} t_mmdsp_mmio_regs_32;
+#endif /* __STN_8815 */
+
+/* Smart xx Accelerator memory map */
+typedef volatile struct {
+    t_uint32    mem24[MMDSP_NB_BLOCK_RAM*MMDSP_RAM_BLOCK_SIZE];  /* 0x0000 -> 0x20000 */
+
+    t_uint32    RESERVED1[(0x30000 - 0x20000)>>2];
+
+    t_mmdsp_mmio_regs_32 mmio_32;
+
+    t_uint16    mem16[MMDSP_NB_BLOCK_RAM*MMDSP_RAM_BLOCK_SIZE];  /* 0x40000 -> 0x50000 */
+
+    t_uint32    RESERVED2[(0x58000 - 0x50000)>>2];
+
+    t_mmdsp_mmio_regs_16 mmio_16;
+
+    t_mmdsp_host_regs_16 host_reg;
+    /*
+    union host_reg {
+        t_mmdsp_host_regs_16 reg16;
+        t_mmdsp_host_regs_32 reg32;
+    };
+    */
+} t_mmdsp_hw_regs;
+
+#endif // __INC_MMDSP_HWP_H
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_macros.h b/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_macros.h
new file mode 100644
index 00000000000..b8911d27609
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/mmdsp/inc/mmdsp_macros.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_MMDSP_DSP_MACROS
+#define __INC_MMDSP_DSP_MACROS
+
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+
+#define MMDSP_ENABLE_WRITE_POSTING(pRegs) \
+{                                                       \
+   (pRegs)->mmio_16.dcache.control |= DCACHE_CONTROL_WRITE_POSTING_ENABLE;  \
+}
+
+#define MMDSP_FLUSH_DCACHE(pRegs) \
+{ /* Today, only full cache flush (clear all the ways) */ \
+   (pRegs)->mmio_16.dcache.command = DCACHE_CMD_FLUSH; \
+}
+
+#define MMDSP_FLUSH_DCACHE_BY_SERVICE(pRegs, startAddr, endAddr)
+
+#define MMDSP_FLUSH_ICACHE(pRegs) \
+{ /* Flush the Instruction cache */ \
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_FLUSH_REG, (IHOST_ICACHE_FLUSH_ALL_ENTRIES_CMD | IHOST_ICACHE_FLUSH_CMD_ENABLE)); \
+}
+
+#ifndef __STN_8810
+#define MMDSP_FLUSH_ICACHE_BY_SERVICE(pRegs, startAddr, endAddr) \
+{ /* Flush the Instruction cache by service */ \
+    /*t_uint64 start_clear_addr = startAddr & ~(MMDSP_ICACHE_LINE_SIZE_IN_INST - 1);*/ \
+    t_uint64 start_clear_addr = (startAddr)>>2; \
+    t_uint64 end_clear_addr = ((endAddr) + MMDSP_ICACHE_LINE_SIZE_IN_INST) & ~(MMDSP_ICACHE_LINE_SIZE_IN_INST - 1); \
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_START_CLEAR_REG, start_clear_addr); \
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_END_CLEAR_REG, end_clear_addr); \
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_FLUSH_REG, (IHOST_ICACHE_FLUSH_BY_SERVICE | IHOST_ICACHE_FLUSH_CMD_ENABLE)); \
+}
+#else
+#define MMDSP_FLUSH_ICACHE_BY_SERVICE(pRegs, startAddr, endAddr) {(void)pRegs; (void)startAddr; (void)endAddr; }
+#endif
+
+#define MMDSP_RESET_CORE(pRegs) \
+{  /* Assert DSP core soft reset */ \
+   (pRegs)->host_reg.softreset = 1; \
+}
+
+#define MMDSP_START_CORE(pRegs) \
+{ \
+   /* Enable external memory access (set bit 3 of ubkcmd) */ \
+   (pRegs)->host_reg.emul_ubkcmd |= MMDSP_UBKCMD_EXT_CODE_MEM_ACCESS_ENABLE; \
+  \
+   /* Start core clock */ \
+   (pRegs)->host_reg.emul_clockcmd = MMDSP_CLOCKCMD_START_CLOCK; \
+}
+
+#define MMDSP_STOP_CORE(pRegs) \
+{ \
+   /* Disable external memory access (reset bit 3 of ubkcmd) */ \
+   (pRegs)->host_reg.emul_ubkcmd = MMDSP_UBKCMD_EXT_CODE_MEM_ACCESS_DISABLE; \
+  \
+   /* Stop core clock */ \
+   (pRegs)->host_reg.emul_clockcmd = MMDSP_CLOCKCMD_STOP_CLOCK; \
+}
+
+#define MMDSP_ASSERT_IRQ(pRegs, irqNum) \
+{ \
+   (pRegs)->host_reg.cmd[irqNum] = 1; \
+}
+
+#define MMDSP_ACKNOWLEDGE_IRQ(pRegs, irqNum) \
+{ \
+    volatile t_uint16 dummy; \
+    dummy =(pRegs)->host_reg.intx[irqNum]; \
+}
+
+#define MMDSP_WRITE_XWORD(pRegs, offset, value) \
+{ \
+    (pRegs)->mem24[offset] = value; \
+}
+
+#define MMDSP_READ_XWORD(pRegs, offset) (pRegs)->mem24[offset]
+
+#endif /* __INC_MMDSP_DSP_MACROS */
diff --git a/drivers/staging/nmf-cm/cm/engine/dsp/src/dsp.c b/drivers/staging/nmf-cm/cm/engine/dsp/src/dsp.c
new file mode 100644
index 00000000000..ef11a5265aa
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/dsp/src/dsp.c
@@ -0,0 +1,1083 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_macros.h>
+
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/power_mgt/inc/power.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/trace/inc/trace.h>
+
+#include <share/inc/nomadik_mapping.h>
+
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/component/inc/component_type.h>
+
+static t_dsp_allocator_desc     esramDesc;
+static t_dsp_desc               mpcDesc[NB_CORE_IDS];
+static t_mmdsp_hw_regs          *pMmdspRegs[NB_CORE_IDS];
+
+struct s_base_descr
+{
+    t_uint32            startAddress[2 /* DSP16 = 0, DSP24 = 1*/];
+    t_dsp_segment_type  segmentType;
+};
+
+#if defined(__STN_8500) && (__STN_8500 > 10)
+
+#define DATA_BASE_NUMBER    4
+
+// In bytes
+#define SDRAM_CODE_SPACE_SPLIT 0x8000
+#define ESRAM_CODE_SPACE_SPLIT 0x4000
+#define SDRAM_DATA_SPACE_SPLIT 0x40000 // This is the modulo constraint of mmdsp
+#define ESRAM_DATA_SPACE_SPLIT 0x40000
+
+// In MMDSP word
+static const struct s_base_descr DATA_ADDRESS_BASE[DATA_BASE_NUMBER + 1 /* For guard */] = {
+        {{SDRAMMEM16_BASE_ADDR,                                      SDRAMMEM24_BASE_ADDR},                                 SDRAM_DATA_EE},
+        {{SDRAMMEM16_BASE_ADDR + (SDRAM_DATA_SPACE_SPLIT / 2),       SDRAMMEM24_BASE_ADDR + (SDRAM_DATA_SPACE_SPLIT / 4)},  SDRAM_DATA_USER},
+        {{ESRAMMEM16_BASE_ADDR,                                      ESRAMMEM24_BASE_ADDR},                                 ESRAM_DATA_EE},
+        {{ESRAMMEM16_BASE_ADDR + (ESRAM_DATA_SPACE_SPLIT / 2),       ESRAMMEM24_BASE_ADDR + (ESRAM_DATA_SPACE_SPLIT / 4)},  ESRAM_DATA_USER},
+        {{MMIO_BASE_ADDR,                                            SDRAMMEM16_BASE_ADDR},                                 NB_DSP_SEGMENT_TYPE /* Not used*/}
+};
+
+#else
+
+#define DATA_BASE_NUMBER    2
+
+// In MMDSP word
+static const struct s_base_descr DATA_ADDRESS_BASE[DATA_BASE_NUMBER + 1 /* For guard */] = {
+        {{SDRAMMEM16_BASE_ADDR,                                      SDRAMMEM24_BASE_ADDR},                                 SDRAM_DATA_EE},
+        {{ESRAMMEM16_BASE_ADDR,                                      ESRAMMEM24_BASE_ADDR},                                 ESRAM_DATA_EE},
+        {{MMIO_BASE_ADDR,                                            SDRAMMEM16_BASE_ADDR},                                 NB_DSP_SEGMENT_TYPE /* Not used*/}
+};
+
+#endif
+
+#if defined(__STN_8500) && (__STN_8500 > 10)
+// In word
+static const t_uint32 CODE_ADDRESS_BASE[4] = {
+        SDRAMTEXT_BASE_ADDR,
+        SDRAMTEXT_BASE_ADDR + (SDRAM_CODE_SPACE_SPLIT / 8),
+        ESRAMTEXT_BASE_ADDR,
+        ESRAMTEXT_BASE_ADDR + (ESRAM_CODE_SPACE_SPLIT / 8)
+};
+#endif
+
+static void arm_Init(void);
+static t_cm_error mmdsp_Init(const t_cm_system_address *dspSystemAddr,
+        t_uint8 nbXramBlocks, t_uint8 nbYramBlocks,
+        t_dsp_allocator_desc *sdramCodeDesc,
+        t_dsp_allocator_desc *sdramDataDesc,
+        t_cm_domain_id eeDomain,
+        t_dsp_desc *pDspDesc,
+        t_mmdsp_hw_regs **pRegs);
+static t_cm_error mmdsp_Configure(t_nmf_core_id coreId, t_mmdsp_hw_regs *pRegs, const t_dsp_desc *pDspDesc);
+static t_cm_error mmdsp_ConfigureAfterBoot(t_nmf_core_id coreId, t_uint8 nbXramBlocks, t_uint8 nbYramBlocks);
+static void cm_DSP_SEM_Init(t_nmf_core_id coreId);
+
+PUBLIC const t_dsp_desc* cm_DSP_GetState(t_nmf_core_id coreId)
+{
+    return &mpcDesc[coreId];
+}
+PUBLIC void cm_DSP_SetStatePanic(t_nmf_core_id coreId)
+{
+    mpcDesc[coreId].state = MPC_STATE_PANIC;
+}
+
+PUBLIC void cm_DSP_Init(const t_nmf_memory_segment *pEsramDesc)
+{
+    t_nmf_core_id coreId;
+    int i;
+
+    /* Create esram desc */
+    esramDesc.allocDesc = cm_MM_CreateAllocator(pEsramDesc->size, 0, "esram");
+    esramDesc.baseAddress = pEsramDesc->systemAddr;
+    esramDesc.referenceCounter = 1; // Don't free it with destroy mechanism
+
+    /* Create ARM */
+    arm_Init();
+
+    mpcDesc[ARM_CORE_ID].state = MPC_STATE_BOOTED;
+
+    /* Reset MPC configuration */
+    for (coreId = FIRST_MPC_ID; coreId <= LAST_CORE_ID; coreId++)
+    {
+        mpcDesc[coreId].state = MPC_STATE_UNCONFIGURED;
+
+        for(i = 0; i < NB_DSP_MEMORY_TYPE; i++)
+            mpcDesc[coreId].allocator[i] = NULL;
+    }
+
+}
+
+PUBLIC void cm_DSP_Destroy(void)
+{
+    t_nmf_core_id coreId;
+    int i;
+
+    for (coreId = ARM_CORE_ID; coreId <= LAST_CORE_ID; coreId++)
+    {
+        for(i = 0; i < NB_DSP_MEMORY_TYPE; i++)
+        {
+            if (mpcDesc[coreId].allocator[i] != NULL)
+            {
+                if(--mpcDesc[coreId].allocator[i]->referenceCounter == 0)
+                {
+                    cm_MM_DeleteAllocator(mpcDesc[coreId].allocator[i]->allocDesc);
+
+                    OSAL_Free(mpcDesc[coreId].allocator[i]);
+                }
+            }
+        }
+    }
+
+    cm_MM_DeleteAllocator(esramDesc.allocDesc);
+}
+
+
+PUBLIC t_cm_error cm_DSP_Add(t_nmf_core_id coreId,
+        t_uint8 nbYramBanks,
+        const t_cm_system_address *pDspMapDesc,
+        const t_cm_domain_id eeDomain,
+        t_dsp_allocator_desc *sdramCodeAllocDesc,
+        t_dsp_allocator_desc *sdramDataAllocDesc)
+{
+    t_cm_error error;
+
+    /* checking nbYramBanks is valid */
+    if (nbYramBanks >= SxA_NB_BLOCK_RAM)
+        return CM_MPC_INVALID_CONFIGURATION;
+
+    if((error = cm_DM_CheckDomain(eeDomain, DOMAIN_NORMAL)) != CM_OK)
+        return error;
+
+    mpcDesc[coreId].domainEE = eeDomain;
+    mpcDesc[coreId].nbYramBank = nbYramBanks;
+    mpcDesc[coreId].state = MPC_STATE_BOOTABLE;
+
+    return mmdsp_Init(
+            pDspMapDesc,
+            SxA_NB_BLOCK_RAM, /* nb of data tcm bank minus one (reserved for cache) */
+            nbYramBanks,
+            sdramCodeAllocDesc,
+            sdramDataAllocDesc,
+            eeDomain,
+            &mpcDesc[coreId],
+            &pMmdspRegs[coreId]
+    );
+}
+
+PUBLIC t_cm_error cm_DSP_Boot(t_nmf_core_id coreId)
+{
+    t_cm_error error;
+
+    // Enable the associated power domain
+    if((error = cm_PWR_EnableMPC(MPC_PWR_CLOCK, coreId)) != CM_OK)
+      return error;
+
+    cm_SEM_PowerOn[coreId](coreId);
+
+    if((error = mmdsp_Configure(
+            coreId,
+            pMmdspRegs[coreId],
+            &mpcDesc[coreId])) != CM_OK)
+    {
+        cm_PWR_DisableMPC(MPC_PWR_CLOCK, coreId);
+    }
+
+    // Put it in auto idle mode ; it's the default in Step 2 of power implementation
+    if((error = cm_PWR_EnableMPC(MPC_PWR_AUTOIDLE, coreId)) != CM_OK)
+      return error;
+
+    return error;
+}
+
+/*
+ * This method is required since MMDSP C bootstrap set some value that must be set differently !!!
+ */
+PUBLIC void cm_DSP_ConfigureAfterBoot(t_nmf_core_id coreId)
+{
+    mpcDesc[coreId].state = MPC_STATE_BOOTED;
+
+    mmdsp_ConfigureAfterBoot(coreId, SxA_NB_BLOCK_RAM, mpcDesc[coreId].nbYramBank);
+
+    cm_DSP_SEM_Init(coreId);
+}
+
+PUBLIC void cm_DSP_Stop(t_nmf_core_id coreId)
+{
+    MMDSP_STOP_CORE(pMmdspRegs[coreId]);
+
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+}
+
+PUBLIC void cm_DSP_Start(t_nmf_core_id coreId)
+{
+    MMDSP_START_CORE(pMmdspRegs[coreId]);
+
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+}
+
+PUBLIC void cm_DSP_Shutdown(t_nmf_core_id coreId)
+{
+    MMDSP_FLUSH_DCACHE(pMmdspRegs[coreId]);
+    MMDSP_FLUSH_ICACHE(pMmdspRegs[coreId]);
+
+    // Due to a hardware bug that breaks MTU when DSP are powered off, don't do that
+    // on mop500_ed for now
+#if !defined(__STN_8500) ||  (__STN_8500 > 10)
+    MMDSP_RESET_CORE(pMmdspRegs[coreId]);
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+    MMDSP_STOP_CORE(pMmdspRegs[coreId]);
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+#endif
+
+    mpcDesc[coreId].state = MPC_STATE_BOOTABLE;
+
+    cm_SEM_PowerOff[coreId](coreId);
+
+    cm_PWR_DisableMPC(MPC_PWR_AUTOIDLE, coreId);
+    cm_PWR_DisableMPC(MPC_PWR_CLOCK, coreId);
+}
+
+PUBLIC t_uint32 cm_DSP_ReadXRamWord(t_nmf_core_id coreId, t_uint32 dspOffset)
+{
+    t_uint32 value;
+
+    value = pMmdspRegs[coreId]->mem24[dspOffset];
+
+    LOG_INTERNAL(3, "cm_DSP_ReadXRamWord: [%x]=%x\n",
+            dspOffset, value,
+            0, 0, 0, 0);
+
+    return value;
+}
+
+
+PUBLIC void cm_DSP_WriteXRamWord(t_nmf_core_id coreId, t_uint32 dspOffset, t_uint32 value)
+{
+    LOG_INTERNAL(3, "cm_DSP_WriteXRamWord: [%x]<-%x\n",
+            dspOffset, value,
+            0, 0, 0, 0);
+
+    pMmdspRegs[coreId]->mem24[dspOffset] = value;
+}
+
+static void cm_DSP_SEM_Init(t_nmf_core_id coreId)
+{
+    pMmdspRegs[coreId]->mmio_16.sem[1].value = 1;
+}
+
+PUBLIC void cm_DSP_SEM_Take(t_nmf_core_id coreId, t_semaphore_id semId)
+{
+    /* take semaphore */
+    while(pMmdspRegs[coreId]->mmio_16.sem[1].value) ;
+}
+
+PUBLIC void cm_DSP_SEM_Give(t_nmf_core_id coreId, t_semaphore_id semId)
+{
+    /* release semaphore */
+    pMmdspRegs[coreId]->mmio_16.sem[1].value = 1;
+}
+
+PUBLIC void cm_DSP_SEM_GenerateIrq(t_nmf_core_id coreId, t_semaphore_id semId)
+{
+    MMDSP_ASSERT_IRQ(pMmdspRegs[coreId], ARM2DSP_IRQ_0);
+}
+
+
+PUBLIC void cm_DSP_AssertDspIrq(t_nmf_core_id coreId, t_host2mpc_irq_num irqNum)
+{
+    MMDSP_ASSERT_IRQ(pMmdspRegs[coreId], irqNum);
+    return;
+}
+
+PUBLIC void cm_DSP_AcknowledgeDspIrq(t_nmf_core_id coreId, t_mpc2host_irq_num irqNum)
+{
+    MMDSP_ACKNOWLEDGE_IRQ(pMmdspRegs[coreId], irqNum);
+    return;
+}
+
+//TODO, juraj, cleanup INTERNAL_XRAM vs INTERNAL_XRAM16/24
+static const t_uint32 dspMemoryTypeId2OffsetShifter[NB_DSP_MEMORY_TYPE] =
+{
+    2, /* INTERNAL_XRAM24:   Internal X memory but seen by host as 32-bit memory */
+    2, /* INTERNAL_XRAM16:   Internal X memory but seen by host as 16-bit memory */
+    2, /* INTERNAL_YRAM24:   Internal Y memory but seen by host as 32-bit memory */
+    2, /* INTERNAL_YRAM16:   Internal Y memory but seen by host as 16-bit memory */
+    2, /* SDRAM_EXT24:       24-bit external "X" memory */
+    1, /* SDRAM_EXT16:       16-bit external "X" memory */
+    2, /* ESRAM_EXT24:       ESRAM24 */
+    1, /* ESRAM_EXT16:       ESRAM16 */
+    3, /* SDRAM_CODE:        Program memory */
+    3, /* ESRAM_CODE:        ESRAM code */
+    3, /* LOCKED_CODE:        ESRAM code */
+};
+
+//TODO, juraj, use these values in mmdsp_Configure
+static const t_uint32 dspMemoryTypeId2DspAddressOffset[NB_DSP_MEMORY_TYPE] =
+{
+    0, /* INTERNAL_XRAM24 */
+    0, /* INTERNAL_XRAM16 */
+    0, /* INTERNAL_YRAM24 */
+    0, /* INTERNAL_YRAM16 */
+    SDRAMMEM24_BASE_ADDR,           /* SDRAM_EXT24:       24-bit external "X" memory */
+    SDRAMMEM16_BASE_ADDR,           /* SDRAM_EXT16:       16-bit external "X" memory */
+    ESRAMMEM24_BASE_ADDR,           /* ESRAM_EXT24:       ESRAM24 */
+    ESRAMMEM16_BASE_ADDR,           /* ESRAM_EXT16:       ESRAM16 */
+    SDRAMTEXT_BASE_ADDR,            /* SDRAM_CODE:        Program memory */
+    ESRAMTEXT_BASE_ADDR,            /* ESRAM_CODE:        ESRAM code */
+    SDRAMTEXT_BASE_ADDR,            /* ESRAM_CODE:        ESRAM code */
+};
+
+PUBLIC t_cm_allocator_desc* cm_DSP_GetAllocator(t_nmf_core_id coreId, t_dsp_memory_type_id memType)
+{
+    return mpcDesc[coreId].allocator[memType] ? mpcDesc[coreId].allocator[memType]->allocDesc : NULL;
+}
+
+PUBLIC void cm_DSP_GetDspChunkInfo(t_memory_handle memHandle, t_dsp_chunk_info *info)
+{
+    t_uint16 userData;
+
+    cm_MM_GetMemoryHandleUserData(memHandle, &userData, &info->alloc);
+
+    info->coreId            = (t_nmf_core_id)       ((userData >> SHIFT_BYTE1) & MASK_BYTE0);
+    info->memType           = (t_dsp_memory_type_id)((userData >> SHIFT_BYTE0) & MASK_BYTE0);
+}
+
+PUBLIC t_cm_error cm_DSP_GetInternalMemoriesInfo(t_cm_domain_id domainId, t_dsp_memory_type_id memType,
+        t_uint32 *offset, t_uint32 *size)
+{
+    t_nmf_core_id coreId = domainDesc[domainId].domain.coreId;
+
+    switch(memType)
+    {
+    case INTERNAL_XRAM24:
+    case INTERNAL_XRAM16:
+        *offset = 0;
+        *size = mpcDesc[coreId].yram_offset;
+        break;
+    case INTERNAL_YRAM24:
+    case INTERNAL_YRAM16:
+        *offset = mpcDesc[coreId].yram_offset;
+        *size = mpcDesc[coreId].yram_size;
+        break;
+    case LOCKED_CODE:
+        *offset = mpcDesc[coreId].locked_offset;
+        *size = mpcDesc[coreId].locked_size;
+        break;
+    case SDRAM_EXT24:
+    case SDRAM_EXT16:
+        *offset = domainDesc[domainId].domain.sdramData.offset;
+        *size   = domainDesc[domainId].domain.sdramData.size;
+        break;
+    case ESRAM_EXT24:
+    case ESRAM_EXT16:
+        *offset = domainDesc[domainId].domain.esramData.offset;
+        *size   = domainDesc[domainId].domain.esramData.size;
+        break;
+    case SDRAM_CODE:
+        *offset = domainDesc[domainId].domain.sdramCode.offset;
+        *size   = domainDesc[domainId].domain.sdramCode.size;
+
+        // update domain size to take into account .locked section
+        if(*offset + *size > mpcDesc[coreId].locked_offset)
+            *size = mpcDesc[coreId].locked_offset - *offset;
+        break;
+    case ESRAM_CODE:
+        *offset = domainDesc[domainId].domain.esramCode.offset;
+        *size   = domainDesc[domainId].domain.esramCode.size;
+        break;
+    default:
+        //return CM_INVALID_PARAMETER;
+        //params are checked at the level above, so this should never occur
+        ERROR("Invalid memType\n",0,0,0,0,0,0);
+        *offset = 0;
+        *size   = 0;
+        CM_ASSERT(0);
+    }
+
+    return CM_OK;
+}
+
+
+PUBLIC t_uint32 cm_DSP_ConvertSize(t_dsp_memory_type_id memType, t_uint32 wordSize)
+{
+    return wordSize << dspMemoryTypeId2OffsetShifter[memType];
+}
+
+PUBLIC t_cm_logical_address cm_DSP_ConvertDspAddressToHostLogicalAddress(t_nmf_core_id coreId, t_shared_addr dspAddress)
+{
+    t_dsp_address_info info;
+    cm_DSP_GetDspDataAddressInfo(coreId, dspAddress, &info);
+    return mpcDesc[coreId].segments[info.segmentType].base.logical + info.baseOffset;
+}
+
+PUBLIC t_cm_error cm_DSP_GetAllocatorStatus(t_nmf_core_id coreId, t_dsp_memory_type_id dspMemType, t_uint32 offset, t_uint32 size, t_cm_allocator_status *pStatus)
+{
+    t_cm_error error;
+
+    if(mpcDesc[coreId].allocator[dspMemType] == NULL)
+        return CM_UNKNOWN_MEMORY_HANDLE;
+
+    error = cm_MM_GetAllocatorStatus(cm_DSP_GetAllocator(coreId, dspMemType), offset, size, pStatus);
+    if (error != CM_OK)
+        return error;
+
+    // complete status with stack sizes, for all dsps
+    //NOTE, well, surely this isn't very clean, as dsp and memory allocator are different things ..
+    {
+        t_uint8 i;
+        for (i = 0; i < NB_CORE_IDS; i++) {
+            //*(pStatus->stack[i].sizes) = *(eeState[i].currentStackSize);
+            pStatus->stack[i].sizes[0] = eeState[i].currentStackSize[0];
+            pStatus->stack[i].sizes[1] = eeState[i].currentStackSize[1];
+            pStatus->stack[i].sizes[2] = eeState[i].currentStackSize[2];
+        }
+    }
+
+    // Change bytes to words
+    pStatus->global.accumulate_free_memory  = pStatus->global.accumulate_free_memory >> dspMemoryTypeId2OffsetShifter[dspMemType];
+    pStatus->global.accumulate_used_memory  = pStatus->global.accumulate_used_memory >> dspMemoryTypeId2OffsetShifter[dspMemType];
+    pStatus->global.maximum_free_size  = pStatus->global.maximum_free_size >> dspMemoryTypeId2OffsetShifter[dspMemType];
+    pStatus->global.minimum_free_size  = pStatus->global.minimum_free_size >> dspMemoryTypeId2OffsetShifter[dspMemType];
+
+    return error;
+}
+
+PUBLIC void cm_DSP_GetHostSystemAddress(t_memory_handle memHandle, t_cm_system_address *pAddr)
+{
+    t_dsp_chunk_info chunk_info;
+    t_uint32 offset; //in bytes
+
+    cm_DSP_GetDspChunkInfo(memHandle, &chunk_info);
+
+    offset = cm_MM_GetOffset(memHandle);
+
+    /* MMDSP mem16 array is very specific to host access, so .... */
+    /* We compute by hand the Host System address to take into account the specifities of the mmdsp mem16 array */
+    /* 1 dsp word = 2 host bytes AND mem16 array is "exported" by MMDSP External Bus wrapper at the 0x40000 offet */
+    if (chunk_info.memType == INTERNAL_XRAM16 || chunk_info.memType == INTERNAL_YRAM16) {
+        offset = (offset >> 1) + FIELD_OFFSET(t_mmdsp_hw_regs, mem16);
+    }
+
+    //TODO, juraj, calculate correct value here - based on segments desc etc..
+    pAddr->logical  = mpcDesc[chunk_info.coreId].allocator[chunk_info.memType]->baseAddress.logical  + offset;
+    pAddr->physical = mpcDesc[chunk_info.coreId].allocator[chunk_info.memType]->baseAddress.physical + offset;
+}
+
+
+PUBLIC t_physical_address cm_DSP_GetPhysicalAdress(t_memory_handle memHandle)
+{
+    t_cm_system_address addr;
+    cm_DSP_GetHostSystemAddress(memHandle, &addr);
+    return addr.physical;
+}
+
+PUBLIC t_cm_logical_address cm_DSP_GetHostLogicalAddress(t_memory_handle memHandle)
+{
+    t_cm_system_address addr;
+    cm_DSP_GetHostSystemAddress(memHandle, &addr);
+    return addr.logical;
+}
+
+PUBLIC void cm_DSP_GetDspAddress(t_memory_handle memHandle, t_uint32 *pDspAddress)
+{
+    t_dsp_chunk_info chunk_info;
+
+    cm_DSP_GetDspChunkInfo(memHandle, &chunk_info);
+
+    *pDspAddress =
+            (cm_MM_GetOffset(memHandle) >> dspMemoryTypeId2OffsetShifter[chunk_info.memType]) +
+            dspMemoryTypeId2DspAddressOffset[chunk_info.memType];
+}
+
+PUBLIC t_cm_error cm_DSP_GetDspBaseAddress(t_nmf_core_id coreId, t_dsp_memory_type_id memType, t_cm_system_address *pAddr)
+{
+    cm_migration_check_state(coreId, STATE_NORMAL);
+    if (mpcDesc[coreId].allocator[memType] == NULL)
+	    return CM_INVALID_PARAMETER;
+    *pAddr = mpcDesc[coreId].allocator[memType]->baseAddress;
+    return CM_OK;
+}
+
+PUBLIC void cm_DSP_GetDspMemoryHandleSize(t_memory_handle memHandle, t_uint32 *pDspSize)
+{
+    t_dsp_chunk_info chunk_info;
+    cm_DSP_GetDspChunkInfo(memHandle, &chunk_info);
+    *pDspSize = cm_MM_GetSize(memHandle) >> dspMemoryTypeId2OffsetShifter[chunk_info.memType];
+}
+
+PUBLIC t_cm_error cm_DSP_setStackSize(t_nmf_core_id coreId, t_uint32 newStackSize)
+{
+    t_uint8 nbXramBanks;
+    t_uint32 xramSize;
+    t_cm_error error;
+
+    /* compute size of xram allocator */
+    nbXramBanks = SxA_NB_BLOCK_RAM - mpcDesc[coreId].nbYramBank;
+
+    /* check first that required stack size is less then xram memory ....*/
+    if (newStackSize >= nbXramBanks * 4 * ONE_KB) {
+        ERROR("CM_NO_MORE_MEMORY: cm_DSP_setStackSize(), required stack size doesn't fit in XRAM.\n", 0, 0, 0, 0, 0, 0);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    /* compute new xram allocator size */
+    xramSize = nbXramBanks * 4 * ONE_KB - newStackSize;
+
+    /* try to resize it */
+    if ((error = cm_MM_ResizeAllocator(cm_DSP_GetAllocator(coreId, INTERNAL_XRAM24),
+                                 xramSize << dspMemoryTypeId2OffsetShifter[INTERNAL_XRAM24])) == CM_NO_MORE_MEMORY) {
+        ERROR("CM_NO_MORE_MEMORY: Couldn't resize stack in cm_DSP_setStackSize()\n", 0, 0, 0, 0, 0, 0);
+    }
+
+    return error;
+}
+
+PUBLIC t_cm_error cm_DSP_IsNbYramBanksValid(t_nmf_core_id coreId, t_uint8 nbYramBanks)
+{
+    /* we use one bank for cache */
+    t_uint8 nbOfRamBanksWithCacheReserved = SxA_NB_BLOCK_RAM;
+
+    /* we want to keep at least one bank of xram */
+    if (nbYramBanks < nbOfRamBanksWithCacheReserved) {return CM_OK;}
+    else {return CM_MPC_INVALID_CONFIGURATION;}
+}
+
+PUBLIC t_uint32 cm_DSP_getStackAddr(t_nmf_core_id coreId)
+{
+    /* we use one bank for cache */
+    //t_uint8 nbOfRamBanksWithCacheReserved = SxA_NB_BLOCK_RAM;
+    /* */
+    //return ((nbOfRamBanksWithCacheReserved * MMDSP_RAM_BLOCK_SIZE * MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE) - mpcDesc[coreId].yram_offset);
+    return mpcDesc[coreId].yram_offset / MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE;
+}
+
+static void arm_Init(void)
+{
+    mpcDesc[ARM_CORE_ID].allocator[INTERNAL_XRAM24] = 0;
+    mpcDesc[ARM_CORE_ID].allocator[INTERNAL_XRAM16] = 0;
+
+    mpcDesc[ARM_CORE_ID].allocator[INTERNAL_YRAM24] = 0;
+    mpcDesc[ARM_CORE_ID].allocator[INTERNAL_YRAM16] = 0;
+
+    mpcDesc[ARM_CORE_ID].allocator[SDRAM_CODE] = 0;
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_CODE] = 0;
+
+    mpcDesc[ARM_CORE_ID].allocator[SDRAM_EXT16] = 0;
+    mpcDesc[ARM_CORE_ID].allocator[SDRAM_EXT24] = 0;
+
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_EXT16] = &esramDesc;
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_EXT16]->referenceCounter++;
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_EXT24] = &esramDesc;
+    mpcDesc[ARM_CORE_ID].allocator[ESRAM_EXT24]->referenceCounter++;
+}
+
+static void _init_Segment(
+        t_dsp_segment *seg,
+        const t_cm_system_address base, const t_uint32 arm_offset,
+        const t_uint32 size)
+{
+    seg->base.logical = base.logical + arm_offset;
+    seg->base.physical = base.physical + arm_offset;
+    seg->size = size;
+}
+
+static t_cm_error mmdsp_Init(
+        const t_cm_system_address *dspSystemAddr,
+        t_uint8 nbXramBlocks, t_uint8 nbYramBlocks,
+        t_dsp_allocator_desc *sdramCodeDesc,
+        t_dsp_allocator_desc *sdramDataDesc,
+        t_cm_domain_id eeDomain,
+        t_dsp_desc *pDspDesc,
+        t_mmdsp_hw_regs **pRegs)
+{
+    t_cm_system_address xramSysAddr;
+    t_uint32 sizeInBytes;
+
+    /* Initialize reference on hw ressources */
+    *pRegs = (t_mmdsp_hw_regs *) dspSystemAddr->logical;
+
+    /* Initialize memory segments management */
+    xramSysAddr.logical = (t_cm_logical_address)(((t_mmdsp_hw_regs *)dspSystemAddr->logical)->mem24);
+    xramSysAddr.physical = (t_cm_physical_address)(((t_mmdsp_hw_regs *)dspSystemAddr->physical)->mem24);
+
+    /* The last (x)ram block will be used by cache, so ... */
+    /* And the NB_YRAM_BLOCKS last available block(s) will be used as YRAM */
+
+    /* XRAM*/
+    pDspDesc->allocator[INTERNAL_XRAM16] = pDspDesc->allocator[INTERNAL_XRAM24] = (t_dsp_allocator_desc*)OSAL_Alloc(sizeof (t_dsp_allocator_desc));
+    if (pDspDesc->allocator[INTERNAL_XRAM24] == NULL)
+        return CM_NO_MORE_MEMORY;
+
+    pDspDesc->allocator[INTERNAL_XRAM24]->allocDesc = cm_MM_CreateAllocator(
+            ((nbXramBlocks-nbYramBlocks)*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE,
+            0,
+            "XRAM");
+    pDspDesc->allocator[INTERNAL_XRAM24]->baseAddress = xramSysAddr;
+    pDspDesc->allocator[INTERNAL_XRAM24]->referenceCounter = 2;
+
+    /* YRAM */
+    pDspDesc->allocator[INTERNAL_YRAM16] = pDspDesc->allocator[INTERNAL_YRAM24] = (t_dsp_allocator_desc*)OSAL_Alloc(sizeof (t_dsp_allocator_desc));
+    if (pDspDesc->allocator[INTERNAL_YRAM24] == 0) {
+    	OSAL_Free(pDspDesc->allocator[INTERNAL_XRAM24]);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    pDspDesc->allocator[INTERNAL_YRAM24]->allocDesc = cm_MM_CreateAllocator(
+            (nbYramBlocks*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE,
+            ((nbXramBlocks-nbYramBlocks)*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE,
+            "YRAM");
+    pDspDesc->allocator[INTERNAL_YRAM24]->baseAddress = xramSysAddr; /* use xram base address but offset is not null */
+    pDspDesc->allocator[INTERNAL_YRAM24]->referenceCounter = 2;
+
+    pDspDesc->yram_offset = ((nbXramBlocks-nbYramBlocks)*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE;
+    pDspDesc->yram_size = (nbYramBlocks*MMDSP_RAM_BLOCK_SIZE)*MMDSP_DATA_WORD_SIZE_IN_HOST_SPACE;
+
+    /* SDRAM & ESRAM */
+    pDspDesc->allocator[SDRAM_CODE] = sdramCodeDesc;
+    pDspDesc->allocator[SDRAM_CODE]->referenceCounter++;
+    pDspDesc->allocator[ESRAM_CODE] = &esramDesc;
+    pDspDesc->allocator[ESRAM_CODE]->referenceCounter++;
+
+    /* LOCKED CODE at end of SDRAM code*/
+    pDspDesc->allocator[LOCKED_CODE] = sdramCodeDesc;
+    pDspDesc->allocator[LOCKED_CODE]->referenceCounter++;
+
+    pDspDesc->locked_offset = cm_MM_GetAllocatorSize(pDspDesc->allocator[SDRAM_CODE]->allocDesc) - MMDSP_CODE_CACHE_WAY_SIZE * 8 * SxA_LOCKED_WAY;
+    pDspDesc->locked_size = MMDSP_CODE_CACHE_WAY_SIZE * 8 * SxA_LOCKED_WAY;
+
+    /* Data_16/24 memory management */
+    pDspDesc->allocator[SDRAM_EXT16] = sdramDataDesc;
+    pDspDesc->allocator[SDRAM_EXT16]->referenceCounter++;
+    pDspDesc->allocator[SDRAM_EXT24] = sdramDataDesc;
+    pDspDesc->allocator[SDRAM_EXT24]->referenceCounter++;
+
+    pDspDesc->allocator[ESRAM_EXT16] = &esramDesc;
+    pDspDesc->allocator[ESRAM_EXT16]->referenceCounter++;
+    pDspDesc->allocator[ESRAM_EXT24] = &esramDesc;
+    pDspDesc->allocator[ESRAM_EXT24]->referenceCounter++;
+
+    sizeInBytes = cm_MM_GetAllocatorSize(pDspDesc->allocator[SDRAM_CODE]->allocDesc);
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    _init_Segment(&pDspDesc->segments[SDRAM_CODE_EE],
+            pDspDesc->allocator[SDRAM_CODE]->baseAddress,
+            domainDesc[eeDomain].domain.sdramCode.offset,
+            domainDesc[eeDomain].domain.sdramCode.size);
+    _init_Segment(&pDspDesc->segments[SDRAM_CODE_USER],
+            pDspDesc->allocator[SDRAM_CODE]->baseAddress,
+            domainDesc[eeDomain].domain.sdramCode.offset + domainDesc[eeDomain].domain.sdramCode.size,
+            sizeInBytes - domainDesc[eeDomain].domain.sdramCode.size);
+#else
+    _init_Segment(&pDspDesc->segments[SDRAM_CODE_EE],
+            pDspDesc->allocator[SDRAM_CODE]->baseAddress,
+            0x0,
+            sizeInBytes);
+#endif
+
+    sizeInBytes = cm_MM_GetAllocatorSize(pDspDesc->allocator[ESRAM_CODE]->allocDesc);
+#if defined(__STN_8500) && (__STN_8500 > 10)
+   _init_Segment(&pDspDesc->segments[ESRAM_CODE_EE],
+            pDspDesc->allocator[ESRAM_CODE]->baseAddress,
+            domainDesc[eeDomain].domain.esramCode.offset,
+            domainDesc[eeDomain].domain.esramCode.size);
+    _init_Segment(&pDspDesc->segments[ESRAM_CODE_USER],
+            pDspDesc->allocator[ESRAM_CODE]->baseAddress,
+            domainDesc[eeDomain].domain.esramCode.offset + domainDesc[eeDomain].domain.esramCode.size,
+            sizeInBytes - domainDesc[eeDomain].domain.esramCode.size);
+#else
+    _init_Segment(&pDspDesc->segments[ESRAM_CODE_EE],
+            pDspDesc->allocator[ESRAM_CODE]->baseAddress,
+            0x0,
+            sizeInBytes);
+#endif
+
+    //the difference in the following code is the segment size used to calculate the top!!
+    sizeInBytes = cm_MM_GetAllocatorSize(pDspDesc->allocator[SDRAM_EXT16]->allocDesc);
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    _init_Segment(&pDspDesc->segments[SDRAM_DATA_EE],
+            pDspDesc->allocator[SDRAM_EXT16]->baseAddress,
+            domainDesc[eeDomain].domain.sdramData.offset,
+            domainDesc[eeDomain].domain.sdramData.size);
+    _init_Segment(&pDspDesc->segments[SDRAM_DATA_USER],
+            pDspDesc->allocator[SDRAM_EXT16]->baseAddress,
+            domainDesc[eeDomain].domain.sdramData.offset + domainDesc[eeDomain].domain.sdramData.size,
+            sizeInBytes - domainDesc[eeDomain].domain.sdramData.size);
+#else
+    _init_Segment(&pDspDesc->segments[SDRAM_DATA_EE],
+            pDspDesc->allocator[SDRAM_EXT16]->baseAddress,
+            0x0,
+            sizeInBytes);
+#endif
+
+    sizeInBytes = cm_MM_GetAllocatorSize(pDspDesc->allocator[ESRAM_EXT16]->allocDesc);
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    _init_Segment(&pDspDesc->segments[ESRAM_DATA_EE],
+            pDspDesc->allocator[ESRAM_EXT16]->baseAddress,
+            domainDesc[eeDomain].domain.esramData.offset,
+            domainDesc[eeDomain].domain.esramData.size);
+    _init_Segment(&pDspDesc->segments[ESRAM_DATA_USER],
+            pDspDesc->allocator[ESRAM_EXT16]->baseAddress,
+            domainDesc[eeDomain].domain.esramData.offset + domainDesc[eeDomain].domain.esramData.size,
+            sizeInBytes - domainDesc[eeDomain].domain.esramData.size);
+#else
+    _init_Segment(&pDspDesc->segments[ESRAM_DATA_EE],
+            pDspDesc->allocator[ESRAM_EXT16]->baseAddress,
+            0x0,
+            sizeInBytes);
+#endif
+
+    return CM_OK;
+}
+
+//TODO, juraj, reuse cm_DSP_UpdateBase functions
+static t_cm_error mmdsp_Configure(t_nmf_core_id coreId, t_mmdsp_hw_regs *pRegs, const t_dsp_desc *pDspDesc)
+{
+    t_uint64 regValue;
+    static const t_uint64 coreId2stbusId[NB_CORE_IDS] =
+    {
+        0, /* ARM_CORE_ID no meaning */
+        SVA_STBUS_ID, /* SVA_CORE_ID */
+        SIA_STBUS_ID  /* SIA_CORE_ID */
+    };
+
+    //t_cm_system_address sysAddr;
+    //t_cm_size sizeInBytes;
+
+    /* Stop core (stop clock) */
+    MMDSP_RESET_CORE(pRegs);
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+    MMDSP_STOP_CORE(pRegs);
+    {
+        volatile t_uint32 loopme = 0xfff;
+        while(loopme--) ;
+    }
+
+#if 0
+    /* Reset DSP internal memory (xram) */
+    {
+        t_uint32 *pSrc = (t_uint32 *)(pRegs->mem24);
+        t_uint32 tcmSize;
+        int i;
+        cm_MM_GetAllocatorSize(pDspDesc->allocator[INTERNAL_XRAM], &sizeInBytes);
+        tcmSize = sizeInBytes;
+        cm_MM_GetAllocatorSize(pDspDesc->allocator[INTERNAL_YRAM], &sizeInBytes);
+        tcmSize += sizeInBytes;
+        for (i = 0; i < (tcmSize/sizeof(t_uint32)); i++)
+            *(pSrc++) = 0;
+    }
+#endif
+
+    /* Configure all blocks as X only, except the Y ones (MOVED TO mmdsp_InitAfterBoot()) */
+
+    /* __STN_8815 --> __STN_8820 or __STN_8500 */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_STBUS_ID_CONF_REG, coreId2stbusId[coreId]);
+
+    /* Configure External Bus timeout reg */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EN_EXT_BUS_TIMEOUT_REG, IHOST_TIMEOUT_ENABLE);
+
+    /* Program memory management */
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    {
+        const t_uint32 r0 = CODE_ADDRESS_BASE[1] >> 10;
+        const t_uint32 r1 = CODE_ADDRESS_BASE[2] >> 10;
+        const t_uint32 r2 = CODE_ADDRESS_BASE[3] >> 10;
+        const t_uint32 sdram0 = pDspDesc->segments[SDRAM_CODE_EE].base.physical;
+        const t_uint32 sdram1 = pDspDesc->segments[SDRAM_CODE_USER].base.physical;
+        const t_uint32 esram0 = pDspDesc->segments[ESRAM_CODE_EE].base.physical;
+        const t_uint32 esram1 = pDspDesc->segments[ESRAM_CODE_USER].base.physical;
+
+        /* Bases for first two segments, going to sdram */
+        regValue = ((t_uint64)(sdram1) << IHOST_PRG_BASE2_ADDR_SHIFT) + (t_uint64)sdram0;
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE_ADDR_REG, regValue);
+
+        /* Bases for second two segments, going to esram */
+        regValue = ((t_uint64)(esram1) << IHOST_PRG_BASE4_ADDR_SHIFT) + (t_uint64)esram0;
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE_34_ADDR_REG, regValue);
+
+        /* Split mmdsp program adress-space and activate the mechanism */
+        regValue =  (t_uint64)((t_uint64)(r2) << 48 | (t_uint64)(r1) <<32 | (t_uint64)(r0) << 16 | 1);
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE2_ACTIV_REG, regValue);
+    }
+#else
+    {
+        const t_uint32 sdram0 = pDspDesc->segments[SDRAM_CODE_EE].base.physical;
+        const t_uint32 esram0 = pDspDesc->segments[ESRAM_CODE_EE].base.physical;
+
+        regValue = (t_uint64)sdram0 | ( ((t_uint64)esram0) << IHOST_PRG_BASE2_ADDR_SHIFT );
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE_ADDR_REG, regValue);
+
+        WRITE_INDIRECT_HOST_REG(pRegs, IHOST_PRG_BASE2_ACTIV_REG, IHOST_PRG_BASE2_ACTIV_ON);
+    }
+#endif
+
+    /* Data_16/24 memory management */
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    /* Segments 1 and 2 for 16/24 map to sdram continuously */
+    /* Base 1 */
+    regValue = (((t_uint64)pDspDesc->segments[SDRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[SDRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE_REG, regValue);
+    /* Top 1 */
+    regValue = (((t_uint64)(pDspDesc->segments[SDRAM_DATA_EE].base.physical + pDspDesc->segments[SDRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[SDRAM_DATA_EE].base.physical + pDspDesc->segments[SDRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_TOP_16_24_REG, regValue);
+
+    /* Base 2 */
+    regValue = (((t_uint64)pDspDesc->segments[SDRAM_DATA_USER].base.physical) << IHOST_DATA_EXT_BUS_BASE2_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[SDRAM_DATA_USER].base.physical) << IHOST_DATA_EXT_BUS_BASE2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE2_REG, regValue);
+    /* Top 2 */
+    regValue = (((t_uint64)(pDspDesc->segments[SDRAM_DATA_USER].base.physical + pDspDesc->segments[SDRAM_DATA_USER].size - 1)) << IHOST_DATA_EXT_BUS_TOP2_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[SDRAM_DATA_USER].base.physical + pDspDesc->segments[SDRAM_DATA_USER].size - 1)) << IHOST_DATA_EXT_BUS_TOP2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_BUS_TOP2_16_24_REG, regValue);
+
+    /* Segments 3 and 4 for 16/24 map to esram continuously */
+    /* Base 3 */
+    regValue = (((t_uint64)pDspDesc->segments[ESRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE3_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[ESRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE3_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE3_REG, regValue);
+    /* Top 3 */
+    regValue = (((t_uint64)(pDspDesc->segments[ESRAM_DATA_EE].base.physical + pDspDesc->segments[ESRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP3_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[ESRAM_DATA_EE].base.physical + pDspDesc->segments[ESRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP3_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_BUS_TOP3_16_24_REG, regValue);
+
+    /* Base 4 */
+    regValue = (((t_uint64)pDspDesc->segments[ESRAM_DATA_USER].base.physical) << IHOST_DATA_EXT_BUS_BASE4_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[ESRAM_DATA_USER].base.physical) << IHOST_DATA_EXT_BUS_BASE4_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE4_REG, regValue);
+    /* Top 4 */
+    regValue = (((t_uint64)(pDspDesc->segments[ESRAM_DATA_USER].base.physical + pDspDesc->segments[ESRAM_DATA_USER].size - 1)) << IHOST_DATA_EXT_BUS_TOP4_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[ESRAM_DATA_USER].base.physical + pDspDesc->segments[ESRAM_DATA_USER].size - 1)) << IHOST_DATA_EXT_BUS_TOP4_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_BUS_TOP4_16_24_REG, regValue);
+
+    /* Define base 2 thresholds/offset (1MB for each up segment) */
+    regValue =  ((t_uint64)DATA_ADDRESS_BASE[1].startAddress[1]>>SHIFT_HALFWORD1)<< IHOST_DATA2_24_XA_BASE_SHIFT;
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[1].startAddress[0]>>SHIFT_HALFWORD1)<< IHOST_DATA2_16_XA_BASE_SHIFT;
+
+    /* Define base 3 thresholds/offset (1MB for each up segment) */
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[2].startAddress[1]>>SHIFT_HALFWORD1)<< IHOST_DATA3_24_XA_BASE_SHIFT;
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[2].startAddress[0]>>SHIFT_HALFWORD1)<< IHOST_DATA3_16_XA_BASE_SHIFT;
+
+    /* Define base 4 thresholds/offset (1MB for each up segment) */
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[3].startAddress[1]>>SHIFT_HALFWORD1)<< IHOST_DATA4_24_XA_BASE_SHIFT;
+    regValue |= ((t_uint64)DATA_ADDRESS_BASE[3].startAddress[0]>>SHIFT_HALFWORD1)<< IHOST_DATA4_16_XA_BASE_SHIFT;
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA2_1624_XA_BASE_REG, regValue);
+
+#else
+    /* Program data24/16 base 1 */
+    regValue = (((t_uint64)pDspDesc->segments[SDRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE_24_SHIFT) |
+               (((t_uint64)pDspDesc->segments[SDRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE_REG, regValue);
+
+    /* Program data24/16 top 1 */
+    regValue = (((t_uint64)(pDspDesc->segments[SDRAM_DATA_EE].base.physical + pDspDesc->segments[SDRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[SDRAM_DATA_EE].base.physical + pDspDesc->segments[SDRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_TOP_16_24_REG, regValue);
+
+    /* Program data24/16 base 2 */
+    regValue = (((t_uint64)pDspDesc->segments[ESRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE2_24_SHIFT) |
+                (((t_uint64)pDspDesc->segments[ESRAM_DATA_EE].base.physical) << IHOST_DATA_EXT_BUS_BASE2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_EXT_BUS_BASE2_REG, regValue);
+
+    /* Program data24/16 top 2 */
+    regValue = (((t_uint64)(pDspDesc->segments[ESRAM_DATA_EE].base.physical + pDspDesc->segments[ESRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP2_24_SHIFT) |
+               (((t_uint64)(pDspDesc->segments[ESRAM_DATA_EE].base.physical + pDspDesc->segments[ESRAM_DATA_EE].size - 1)) << IHOST_DATA_EXT_BUS_TOP2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_BUS_TOP2_16_24_REG, regValue);
+
+    /* Define base 2 thresholds/offset (1MB for each up segment) */
+    regValue = ((t_uint64)(DATA_ADDRESS_BASE[1].startAddress[1]>>SHIFT_HALFWORD1))<< IHOST_DATA2_24_XA_BASE_SHIFT; // Top address minus ONE_MB => 256KW (24/32-bit)
+    regValue |= ((t_uint64)(DATA_ADDRESS_BASE[1].startAddress[0]>>SHIFT_HALFWORD1))<< IHOST_DATA2_16_XA_BASE_SHIFT; // Top address minus ONE_MB => 512KW (16-bit)
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA2_1624_XA_BASE_REG, regValue);
+#endif
+
+    /* Enable top check */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_TOP_16_24_CHK_REG, IHOST_DATA_TOP_16_24_CHK_ON);
+
+    /* Enable both bases */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_DATA_BASE2_ACTIV_REG, IHOST_DATA_BASE2_ACTIV_ON);
+
+    /* MMIO management */
+    regValue = (((t_uint64)STM_BASE_ADDR) << IHOST_EXT_MMIO_BASE_ADDR_SHIFT) |
+            (((t_uint64)DMA_CTRL_END_ADDR) << IHOST_EXT_MMIO_DATA_EXT_BUS_TOP_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_EXT_MMIO_BASE_DATA_EXT_BUS_TOP_REG, regValue);
+
+    /* Configure Icache */
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_INST_BURST_SZ_REG, IHOST_INST_BURST_SZ_AUTO);
+
+    regValue = (t_uint64)(IHOST_ICACHE_MODE_PERFMETER_OFF | IHOST_ICACHE_MODE_L2_CACHE_ON |
+            IHOST_ICACHE_MODE_L1_CACHE_ON | IHOST_ICACHE_MODE_FILL_MODE_OFF);
+    WRITE_INDIRECT_HOST_REG(pRegs, IHOST_ICACHE_MODE_REG, regValue);
+    
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DSP_updateCodeBase(
+        t_nmf_core_id coreId,
+        t_dsp_segment_type hwSegment,
+        t_cm_system_address src,
+        t_cm_system_address dst
+        )
+{
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    t_mmdsp_hw_regs *pRegs = pMmdspRegs[coreId];
+    t_uint32 offset = src.physical - mpcDesc[coreId].segments[hwSegment].base.physical;
+    t_cm_system_address base;
+    t_uint32 altBase = 0;
+    t_uint64 regValue = 0;
+    t_uint8  reg = 0;
+
+    base.physical = dst.physical - offset;
+    base.logical  = dst.logical  - offset;
+
+    switch(hwSegment) {
+    case SDRAM_CODE_EE:
+        altBase  = mpcDesc[coreId].segments[SDRAM_CODE_USER].base.physical;
+        regValue = ((t_uint64)(altBase) << IHOST_PRG_BASE2_ADDR_SHIFT) + (t_uint64)base.physical;
+        reg = IHOST_PRG_BASE_ADDR_REG;
+        break;
+    case SDRAM_CODE_USER:
+        altBase  = mpcDesc[coreId].segments[SDRAM_CODE_EE].base.physical;
+        regValue = ((t_uint64)(base.physical) << IHOST_PRG_BASE2_ADDR_SHIFT) + (t_uint64)altBase;
+        reg = IHOST_PRG_BASE_ADDR_REG;
+        break;
+    case ESRAM_CODE_EE:
+        altBase = mpcDesc[coreId].segments[ESRAM_CODE_USER].base.physical;
+        regValue = ((t_uint64)(altBase) << IHOST_PRG_BASE4_ADDR_SHIFT) + (t_uint64)base.physical;
+        reg = IHOST_PRG_BASE_34_ADDR_REG;
+        break;
+    case ESRAM_CODE_USER:
+        altBase = mpcDesc[coreId].segments[ESRAM_CODE_EE].base.physical;
+        regValue = ((t_uint64)(base.physical) << IHOST_PRG_BASE4_ADDR_SHIFT) + (t_uint64)altBase;
+        reg = IHOST_PRG_BASE_34_ADDR_REG;
+        break;
+    default:
+        CM_ASSERT(0);
+    }
+
+    LOG_INTERNAL(1, "##### DSP Code Base Update [%d]: 0x%x -> 0x%x (0x%x)\n",
+            hwSegment, mpcDesc[coreId].segments[hwSegment].base.physical, base.physical, base.logical, 0, 0);
+
+    WRITE_INDIRECT_HOST_REG(pRegs, reg, regValue);
+
+    mpcDesc[coreId].segments[hwSegment].base = base;
+#endif
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DSP_updateDataBase(
+        t_nmf_core_id coreId,
+        t_dsp_segment_type hwSegment,
+        t_cm_system_address src,
+        t_cm_system_address dst
+        )
+{
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    t_mmdsp_hw_regs *pRegs = pMmdspRegs[coreId];
+    t_uint32 offset = src.physical - mpcDesc[coreId].segments[hwSegment].base.physical;
+    t_cm_system_address base;
+    t_uint32 size = mpcDesc[coreId].segments[hwSegment].size; //in bytes
+    t_uint64 regValue;
+    t_uint8  reg = 0;
+    t_uint8  top = 0;
+
+    base.physical = dst.physical - offset;
+    base.logical  = dst.logical  - offset;
+
+    switch(hwSegment) {
+    case SDRAM_DATA_EE:
+        reg = IHOST_DATA_EXT_BUS_BASE_REG;
+        top = IHOST_DATA_EXT_BUS_TOP_16_24_REG;
+        break;
+    case SDRAM_DATA_USER:
+        reg = IHOST_DATA_EXT_BUS_BASE2_REG;
+        top = IHOST_EXT_BUS_TOP2_16_24_REG;
+        break;
+    case ESRAM_DATA_EE:
+        reg = IHOST_DATA_EXT_BUS_BASE3_REG;
+        top = IHOST_EXT_BUS_TOP3_16_24_REG;
+        break;
+    case ESRAM_DATA_USER:
+        reg = IHOST_DATA_EXT_BUS_BASE4_REG;
+        top = IHOST_EXT_BUS_TOP4_16_24_REG;
+        break;
+    default:
+        CM_ASSERT(0);
+    }
+
+    LOG_INTERNAL(1, "##### DSP Data Base Update [%d]: 0x%x -> 0x%x (0x%x)\n",
+            hwSegment, mpcDesc[coreId].segments[hwSegment].base.physical, base.physical, base.logical, 0, 0);
+
+    /* Program data24/16 base */
+    regValue = (((t_uint64)(base.physical)) << IHOST_DATA_EXT_BUS_BASE2_24_SHIFT) |
+               (((t_uint64)(base.physical)) << IHOST_DATA_EXT_BUS_BASE2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, reg, regValue);
+
+    /* Program data24/16 top */
+    regValue = (((t_uint64)(base.physical + size - 1)) << IHOST_DATA_EXT_BUS_TOP2_24_SHIFT) |
+               (((t_uint64)(base.physical + size - 1)) << IHOST_DATA_EXT_BUS_TOP2_16_SHIFT);
+    WRITE_INDIRECT_HOST_REG(pRegs, top, regValue);
+
+    mpcDesc[coreId].segments[hwSegment].base = base;
+#endif
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DSP_GetDspDataAddressInfo(t_nmf_core_id coreId, t_uint32 addr, t_dsp_address_info *info)
+{
+    t_uint32 i, j;
+
+    for(j = 0; j < 2; j++)
+    {
+        for(i = 0; i < DATA_BASE_NUMBER; i++)
+        {
+            if(DATA_ADDRESS_BASE[i].startAddress[j] <= addr && addr < DATA_ADDRESS_BASE[i + 1].startAddress[j])
+            {
+                info->segmentType = DATA_ADDRESS_BASE[i].segmentType;
+                info->baseOffset = (addr - DATA_ADDRESS_BASE[i].startAddress[j]) * (2 + j * 2);
+
+                return CM_OK;
+            }
+        }
+    }
+
+    CM_ASSERT(0);
+    //return CM_INVALID_PARAMETER;
+}
+
+static t_cm_error mmdsp_ConfigureAfterBoot(t_nmf_core_id coreId, t_uint8 nbXramBlocks, t_uint8 nbYramBlocks)
+{
+    /* Configure all blocks as X only, except the Y ones */
+    pMmdspRegs[coreId]->mmio_16.config_data_mem.value = (t_uint16)(~(((1U << nbYramBlocks) - 1) << (nbXramBlocks-nbYramBlocks)));
+    
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    /* enable write posting */
+    MMDSP_ENABLE_WRITE_POSTING(pMmdspRegs[coreId]);
+#endif
+    
+    return CM_OK;
+}
+
+
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/bfd.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/bfd.h
new file mode 100644
index 00000000000..2bccf9c073b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/bfd.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf bfd relocation.
+ *
+ * \defgroup ELFLOADER MMDSP ELF loader.
+ */
+#ifndef __INC_CM_ELF_BFD_H
+#define __INC_CM_ELF_BFD_H
+
+#include <cm/inc/cm_type.h>
+
+/*
+ * Relocation spcification
+ */
+enum complain_overflow
+{
+  /* Do not complain on overflow.  */
+  complain_overflow_dont,
+
+  /* Complain if the bitfield overflows, whether it is considered
+     as signed or unsigned.  */
+  complain_overflow_bitfield,
+
+  /* Complain if the value overflows when considered as signed
+     number.  */
+  complain_overflow_signed,
+
+  /* Complain if the value overflows when considered as an
+     unsigned number.  */
+  complain_overflow_unsigned
+};
+
+struct reloc_howto_struct
+{
+  /*  The type field has mainly a documentary use - the back end can
+      do what it wants with it, though normally the back end's
+      external idea of what a reloc number is stored
+      in this field.  For example, a PC relative word relocation
+      in a coff environment has the type 023 - because that's
+      what the outside world calls a R_PCRWORD reloc.  */
+  unsigned int type;
+
+  /*  The value the final relocation is shifted right by.  This drops
+      unwanted data from the relocation.  */
+  unsigned int rightshift;
+
+  /*  The size of the item to be relocated.  This is *not* a
+      power-of-two measure.  To get the number of bytes operated
+      on by a type of relocation, use bfd_get_reloc_size.  */
+  int size;
+
+  /*  The number of bits in the item to be relocated.  This is used
+      when doing overflow checking.  */
+  unsigned int bitsize;
+
+  /*  Notes that the relocation is relative to the location in the
+      data section of the addend.  The relocation function will
+      subtract from the relocation value the address of the location
+      being relocated.  */
+  t_uint64 pc_relative;
+
+  /*  The bit position of the reloc value in the destination.
+      The relocated value is left shifted by this amount.  */
+  unsigned int bitpos;
+
+  /* What type of overflow error should be checked for when
+     relocating.  */
+  enum complain_overflow complain_on_overflow;
+
+  void (*special_function)(void);
+
+  /* The textual name of the relocation type.  */
+  char *name;
+
+  /* Some formats record a relocation addend in the section contents
+     rather than with the relocation.  For ELF formats this is the
+     distinction between USE_REL and USE_RELA (though the code checks
+     for USE_REL == 1/0).  The value of this field is TRUE if the
+     addend is recorded with the section contents; when performing a
+     partial link (ld -r) the section contents (the data) will be
+     modified.  The value of this field is FALSE if addends are
+     recorded with the relocation (in arelent.addend); when performing
+     a partial link the relocation will be modified.
+     All relocations for all ELF USE_RELA targets should set this field
+     to FALSE (values of TRUE should be looked on with suspicion).
+     However, the converse is not true: not all relocations of all ELF
+     USE_REL targets set this field to TRUE.  Why this is so is peculiar
+     to each particular target.  For relocs that aren't used in partial
+     links (e.g. GOT stuff) it doesn't matter what this is set to.  */
+  char partial_inplace;
+
+  /* src_mask selects the part of the instruction (or data) to be used
+     in the relocation sum.  If the target relocations don't have an
+     addend in the reloc, eg. ELF USE_REL, src_mask will normally equal
+     dst_mask to extract the addend from the section contents.  If
+     relocations do have an addend in the reloc, eg. ELF USE_RELA, this
+     field should be zero.  Non-zero values for ELF USE_RELA targets are
+     bogus as in those cases the value in the dst_mask part of the
+     section contents should be treated as garbage.  */
+  t_uint64 src_mask;
+
+  /* dst_mask selects which parts of the instruction (or data) are
+     replaced with a relocated value.  */
+  t_uint64 dst_mask;
+
+  /* When some formats create PC relative instructions, they leave
+     the value of the pc of the place being relocated in the offset
+     slot of the instruction, so that a PC relative relocation can
+     be made just by adding in an ordinary offset (e.g., sun3 a.out).
+     Some formats leave the displacement part of an instruction
+     empty (e.g., m88k bcs); this flag signals the fact.  */
+  char pcrel_offset;
+};
+
+#define HOWTO(C, R, S, B, P, BI, O, SF, NAME, INPLACE, MASKSRC, MASKDST, PC) \
+  { (unsigned) C, R, S, B, P, BI, O, SF, NAME, INPLACE, MASKSRC, MASKDST, PC }
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/common.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/common.h
new file mode 100644
index 00000000000..c51845d5f96
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/common.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf common definition.
+ */
+#ifndef __INC_CM_ELF_COMMON_H
+#define __INC_CM_ELF_COMMON_H
+
+#include <cm/engine/component/inc/nmfheaderabi.h>
+#include <cm/engine/elf/inc/elfabi.h>
+#include <cm/engine/elf/inc/reloc.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/component/inc/description.h>
+#include <cm/engine/utils/inc/string.h>
+
+
+#define MAX_SEGMENT  20 // Just in order to not allocate them dynamically
+
+struct XXElf;
+
+/**
+ * \brief Structure used as database of pushed component.
+ */
+typedef struct {
+    t_instance_property instanceProperty;
+    t_uint32            magicNumber;          //!< Magic Number
+    t_dup_char          foundedTemplateName;
+    t_uint32            minStackSize;           //!< Minimum stack size
+
+    struct XXElf        *ELF;
+
+    t_elfSegment segments[NUMBER_OF_MMDSP_MEMORY];
+
+    t_bool              temporaryDescription;
+
+    t_memory_reference          memoryForConstruct;
+    t_memory_reference          memoryForStart;
+    t_memory_reference          memoryForStop;
+    t_memory_reference          memoryForDestroy;
+
+    t_uint8                     requireNumber;          //!< Number of interface required by this template
+    t_uint8                     attributeNumber;        //!< Number of attributes in this template
+    t_uint8                     propertyNumber;         //!< Number of properties in this template
+    t_uint8                     provideNumber;          //!< Number of interface provided by this template
+
+    t_interface_require         *requires;              //!< Array of interface required by this template
+    t_attribute                 *attributes;            //!< Array of attributes in this template
+    t_property                  *properties;            //!< Array of properties in this template
+    t_interface_provide         *provides;              //!< Array of interface provided by this template
+
+} t_elfdescription;
+
+/**
+ * \brief Temporary structure used as database when pushing component.
+ */
+typedef struct
+{
+    const char              *elfdata;
+    const char              *sectionData[50]; // YES it must be dynamic, but i'm tired.
+
+    t_bool                  isExecutable;
+
+    t_sint32                nmfSectionIndex;
+    const void              *relaNmfSegment, *relaNmfSegmentEnd;
+    const void              *relaNmfSegmentSymbols;
+    const char              *relaNmfSegmentStrings;
+
+    const t_elf_component_header*elfheader;
+
+
+} t_tmp_elfdescription;
+
+
+t_cm_error ELF64_LoadComponent(
+        t_uint16                    e_machine,
+        const char                  *elfdata,
+        t_elfdescription            **elfhandlePtr,
+        t_tmp_elfdescription        *elftmp);
+t_cm_error ELF64_ComputeSegment(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp);
+
+void ELF64_UnloadComponent(
+        t_elfdescription            *elfhandle);
+
+t_cm_error ELF64_loadSegment(
+        t_elfdescription            *elfhandle,
+        t_memory_handle             *memory,
+        t_memory_property           property);
+t_cm_error ELF64_relocateSegments(
+        t_memory_handle             *memories,
+        t_elfdescription            *elf,
+        t_memory_property           property,
+        void                        *cbContext);
+t_cm_error ELF64_getRelocationMemory(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp,
+        t_uint32                    offsetInNmf,
+        t_memory_reference          *memory);
+
+const t_elfmemory* MMDSP_getMappingById(t_memory_id memId);
+const t_elfmemory* MMDSP_getMappingByName(const char* sectionName, t_instance_property property);
+void MMDSP_serializeMemories(t_instance_property property,
+        const t_elfmemory** codeMemory, const t_elfmemory** thisMemory);
+void MMDSP_copySection(t_uint32 origAddr, t_uint32 remoteAddr, t_uint32 sizeInByte);
+void MMDSP_bzeroSection(t_uint32 remoteAddr, t_uint32 sizeInByte);
+void MMDSP_loadedSection(t_nmf_core_id coreId, t_memory_id memId, t_memory_handle handle);
+void MMDSP_unloadedSection(t_nmf_core_id coreId, t_memory_id memId, t_memory_handle handle);
+
+void MMDSP_copyCode(t_uint64 * remoteAddr64, const char* origAddr, int nb);
+void MMDSP_copyData24(t_uint32 * remoteAddr32, const char* origAddr, int nb);
+void MMDSP_copyData16(t_uint16 * remoteAddr16, const char* origAddr, int nb);
+
+t_uint32 cm_resolvSymbol(
+        void* context,
+        t_uint32 type,
+        t_dup_char symbolName,
+        char* reloc_addr);
+
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/elfabi.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/elfabi.h
new file mode 100644
index 00000000000..cbcc6db3b9b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/elfabi.h
@@ -0,0 +1,539 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#ifndef _CM_ELF_H
+#define _CM_ELF_H 1
+
+typedef t_uint16 Elf32_Half;
+typedef t_uint16 Elf64_Half;
+
+typedef t_uint32 Elf32_Word;
+typedef t_sint32 Elf32_Sword;
+typedef t_uint32 Elf64_Word;
+typedef t_sint32 Elf64_Sword;
+
+typedef t_uint64 Elf32_Xword;
+typedef t_sint64 Elf32_Sxword;
+typedef t_uint64 Elf64_Xword;
+typedef t_sint64 Elf64_Sxword;
+
+typedef t_uint32 Elf32_Addr;
+typedef t_uint64 Elf64_Addr;
+
+typedef t_uint32 Elf32_Off;
+typedef t_uint64 Elf64_Off;
+
+typedef t_uint16 Elf32_Section;
+typedef t_uint16 Elf64_Section;
+
+typedef Elf32_Half Elf32_Versym;
+typedef Elf64_Half Elf64_Versym;
+
+
+/*********************************************
+ * Header
+ *********************************************/
+#define EI_NIDENT (16)                              //!< Size of e_ident[]
+
+#define EI_MAG0         0               //!< File identification
+#define ELFMAG0             0x7f
+
+#define EI_MAG1         1               //!< File identification
+#define ELFMAG1             'E'
+
+#define EI_MAG2         2               //!< File identification
+#define ELFMAG2             'L'
+
+#define EI_MAG3         3               //!< File identification
+#define ELFMAG3             'F'
+
+#define EI_CLASS        4               //!< File class
+#define ELFCLASSNONE        0               //!< Invalid class
+#define ELFCLASS32          1               //!< 32-bit objects
+#define ELFCLASS64          2               //!< 64-bit objects
+
+#define EI_DATA         5               //!< Data encoding
+#define ELFDATANONE         0               //!< Invalid data encoding
+#define ELFDATA2LSB         1               //!< 2's complement, little endian
+#define ELFDATA2MSB         2               //!< 2's complement, big endian
+
+#define EI_VERSION      6               //!< File version
+
+#define EI_OSABI        7               //!< OS ABI identification
+#define ELFOSABI_NONE       0               //!<  No extension
+#define ELFOSABI_HPUX       1               //!<  HP-UX
+#define ELFOSABI_NETBSD     2               //!<  NetBSD
+#define ELFOSABI_LINUX      3               //!<  Linux
+#define ELFOSABI_SOLARIS    6               //!<  Sun Solaris
+#define ELFOSABI_AIX        7               //!<  AIX
+#define ELFOSABI_IRIX       8               //!<  IRIX
+#define ELFOSABI_FREEBSD    9               //!<  FreeBSD
+#define ELFOSABI_TRU64      10              //!<  Compaq TRU64 UNIX
+#define ELFOSABI_MODESTO    11              //!<  Novell Modesto
+#define ELFOSABI_OPENBSD    12              //!<  Open BSD
+#define ELFOSABI_OPENVMS    13              //!<  Open VMS
+#define ELFOSABI_NSK        14              //!<  HP Non-Stop-Kernel
+
+#define EI_ABIVERSION   8               //!< ABI version
+
+#define EI_PAD          9               //!< Start of padding byte
+
+
+typedef struct
+{
+    unsigned char       e_ident[EI_NIDENT];             //!< The initial bytes mark the file as an object file and provide machine-independent data with which to decode and interpret the file's contents
+    Elf32_Half          e_type;                         //!< This member identifies the object file type
+    Elf32_Half          e_machine;                      //!< This member's value specifies the required architecture for an individual file
+    Elf32_Word          e_version;                      //!< This member identifies the object file version
+    Elf32_Addr          e_entry;                        //!< This member gives the virtual address to which the system first transfers control, thus starting the process
+    Elf32_Off           e_phoff;                        //!< This member holds the program header table's file offset in bytes
+    Elf32_Off           e_shoff;                        //!< This member holds the section header table's file offset in bytes
+    Elf32_Word          e_flags;                        //!< This member holds processor-specific flags associated with the file
+    Elf32_Half          e_ehsize;                       //!< This member holds the ELF header's size in bytes
+    Elf32_Half          e_phentsize;                    //!< This member holds the size in bytes of one entry in the file's program header table; all entries are the same size
+    Elf32_Half          e_phnum;                        //!< This member holds the number of entries in the program header table
+    Elf32_Half          e_shentsize;                    //!< This member holds a section header's size in bytes
+    Elf32_Half          e_shnum;                        //!< This member holds the number of entries in the section header table
+    Elf32_Half          e_shstrndx;                     //!< This member holds the section header table index of the entry associated with the section name string table
+} Elf32_Ehdr;       //!< 32bit Entry Header
+
+typedef struct
+{
+    unsigned char       e_ident[EI_NIDENT];             //!< The initial bytes mark the file as an object file and provide machine-independent data with which to decode and interpret the file's contents
+    Elf64_Half          e_type;                         //!< This member identifies the object file type
+    Elf64_Half          e_machine;                      //!< This member's value specifies the required architecture for an individual file
+    Elf64_Word          e_version;                      //!< This member identifies the object file version
+    Elf64_Addr          e_entry;                        //!< This member gives the virtual address to which the system first transfers control, thus starting the process
+    Elf64_Off           e_phoff;                        //!< This member holds the program header table's file offset in bytes
+    Elf64_Off           e_shoff;                        //!< This member holds the section header table's file offset in bytes
+    Elf64_Word          e_flags;                        //!< This member holds processor-specific flags associated with the file
+    Elf64_Half          e_ehsize;                       //!< This member holds the ELF header's size in bytes
+    Elf64_Half          e_phentsize;                    //!< This member holds the size in bytes of one entry in the file's program header table; all entries are the same size
+    Elf64_Half          e_phnum;                        //!< This member holds the number of entries in the program header table
+    Elf64_Half          e_shentsize;                    //!< This member holds a section header's size in bytes
+    Elf64_Half          e_shnum;                        //!< This member holds the number of entries in the section header table
+    Elf64_Half          e_shstrndx;                     //!< This member holds the section header table index of the entry associated with the section name string table
+} Elf64_Ehdr;       //!< 64bit Entry Header
+
+/*
+ * e_type
+ */
+#define ET_NONE         0                   //!< No file type
+#define ET_REL          1                   //!< Relocatable file
+#define ET_EXEC         2                   //!< Executable file
+#define ET_DYN          3                   //!< Shared object file
+#define ET_CORE         4                   //!< Core file
+#define ET_LOOS         0xfe00              //!< Operating system-specific
+#define ET_HIOS         0xfeff              //!< Operating system-specific
+#define ET_LOPROC       0xff00              //!< Processor-specific
+#define ET_HIPROC       0xffff              //!< Processor-specific
+
+/*
+ * e_machine
+ */
+#define EM_NONE             0               //!< No machine
+#define EM_M32              1               //!< AT&T WE 32100
+#define EM_SPARC            2               //!< SUN SPARC
+#define EM_386              3               //!< Intel 80386
+#define EM_68K              4               //!< Motorola 68000
+#define EM_88K              5               //!< Motorola 88000
+#define EM_860              7               //!< Intel 80860
+#define EM_MIPS             8               //!< MIPS I architecture
+#define EM_S370             9               //!< IBM System/370
+#define EM_MIPS_RS3_LE      10              //!< MIPS R3000 little-endian
+#define EM_PARISC           15              //!< HPPA
+#define EM_VPP500           17              //!< Fujitsu VPP500
+#define EM_SPARC32PLUS      18              //!< Enhanced instruction set SPARC
+#define EM_960              19              //!< Intel 80960
+#define EM_PPC              20              //!< PowerPC
+#define EM_PPC64            21              //!< 64-bit PowerPC
+#define EM_S390             22              //!< IBM System/390 Processor
+#define EM_V800             36              //!< NEC V800
+#define EM_FR20             37              //!< Fujitsu FR20
+#define EM_RH32             38              //!< TRW RH-32
+#define EM_RCE              39              //!< Motorola RCE
+#define EM_ARM              40              //!< Advanced RISC Machines ARM
+#define EM_FAKE_ALPHA       41              //!< Digital Alpha
+#define EM_SH               42              //!< Hitachi SH
+#define EM_SPARCV9          43              //!< SPARC Version 9
+#define EM_TRICORE          44              //!< Siemens TriCore embedded processor
+#define EM_ARC              45              //!< Argonaut RISC Core, Argonaut Technologies Inc
+#define EM_H8_300           46              //!< Hitachi H8/300
+#define EM_H8_300H          47              //!< Hitachi H8/300H
+#define EM_H8S              48              //!< Hitachi H8S
+#define EM_H8_500           49              //!< Hitachi H8/500
+#define EM_IA_64            50              //!< Intel IA-64 processor architecture
+#define EM_MIPS_X           51              //!< Stanford MIPS-X
+#define EM_COLDFIRE         52              //!< Motorola ColdFire
+#define EM_68HC12           53              //!< Motorola M68HC12
+#define EM_MMA              54              //!< Fujitsu MMA Multimedia Accelerator
+#define EM_PCP              55              //!< Siemens PCP
+#define EM_NCPU             56              //!< Sony nCPU embedded RISC processor
+#define EM_NDR1             57              //!< Denso NDR1 microprocessor
+#define EM_STARCORE         58              //!< Motorola Start*Core processor
+#define EM_ME16             59              //!< Toyota ME16 processor
+#define EM_ST100            60              //!< STMicroelectronics ST100 processor
+#define EM_TINYJ            61              //!< Advanced Logic Corp. TinyJ embedded processor family
+#define EM_X86_64           62              //!< AMD x86-64 architecture
+#define EM_PDSP             63              //!< Sony DSP Processor
+#define EM_PDP10            64              //!< Digital Equipment Corp. PDP-10
+#define EM_PDP11            65              //!< Digital Equipment Corp. PDP-11
+#define EM_FX66             66              //!< Siemens FX66 microcontroller
+#define EM_ST9PLUS          67              //!< STMicroelectronics ST9+ 8/16 bit microcontroller
+#define EM_ST7              68              //!< STMicroelectronics ST7 8-bit microcontroller
+#define EM_68HC16           69              //!< Motorola MC68HC16 Microcontroller
+#define EM_68HC11           70              //!< Motorola MC68HC11 Microcontroller
+#define EM_68HC08           71              //!< Motorola MC68HC08 Microcontroller
+#define EM_68HC05           72              //!< Motorola MC68HC05 Microcontroller
+#define EM_SVX              73              //!< Silicon Graphics SVx
+#define EM_ST19             74              //!< STMicroelectronics ST19 8-bit microcontroller
+#define EM_VAX              75              //!< Digital VAX
+#define EM_CRIS             76              //!< Axis Communications 32-bit embedded processor
+#define EM_JAVELIN          77              //!< Infifineon Technologies 32-bit embedded processor
+#define EM_FIREPATH         78              //!< Element 14 64-bit DSP Processor
+#define EM_ZSP              79              //!< LSI Logic 16-bit DSP Processor
+#define EM_MMIX             80              //!< Donald Knuth's educational 64-bit processor
+#define EM_HUANY            81              //!< Harvard University machine-independent object files
+#define EM_PRISM            82              //!< SiTera Prism
+#define EM_AVR              83              //!< Atmel AVR 8-bit microcontroller
+#define EM_FR30             84              //!< Fujitsu FR30
+#define EM_D10V             85              //!< Mitsubishi D10V
+#define EM_D30V             86              //!< Mitsubishi D30V
+#define EM_V850             87              //!< NEC v850
+#define EM_M32R             88              //!< Mitsubishi M32R
+#define EM_MN10300          89              //!< Matsushita MN10300
+#define EM_MN10200          90              //!< Matsushita MN10200
+#define EM_PJ               91              //!< picoJava
+#define EM_OPENRISC         92              //!< OpenRISC 32-bit embedded processor
+#define EM_ARC_A5           93              //!< ARC International ARCompact processor (old spelling/synonym: EM_ARC_A5)
+#define EM_XTENSA           94              //!< Tensilica Xtensa Architecture
+#define EM_VIDEOCORE        95              //!< Alphamosaic VideoCore processor
+#define EM_TMM_GPP          96              //!< Thompson Multimedia General Purpose Processor
+#define EM_NS32K            97              //!< National Semiconductor 32000 series
+#define EM_TPC              98              //!< Tenor Network TPC processor
+#define EM_SNP1K            99              //!< Trebia SNP 1000 processor
+#define EM_ST200            100             //!< STMicroelectronics (www.st.com) ST200 microcontroller
+#define EM_IP2K             101             //!< Ubicom IP2xxx microcontroller family
+#define EM_MAX              102             //!< MAX Processor
+#define EM_CR               103             //!< National Semiconductor CompactRISC microprocessor
+#define EM_F2MC16           104             //!< Fujitsu F2MC16
+#define EM_MSP430           105             //!< Texaxas Instruments embedded microcontroller msp430
+#define EM_BLACKFIN         106             //!< Analog Devices Blackfin (DSP) processor
+#define EM_SE_C33           107             //!< S1C33 Family of Seiko Epspson processors
+#define EM_SEP              108             //!< Sharp embedded microprocessor
+#define EM_ARCA             109             //!< Arca RISC Microprocessor
+#define EM_UNICORE          110             //!< Microprocessor series from PKU-Unity Ltd. and MPRC of Peking University
+#define EM_EXCESS           111             //!< eXcess: 16/32/64-bit configurable embedded CPU
+#define EM_DXP              112             //!< Icera Semiconductor Inc. Deep Execution Processor
+#define EM_ALTERA_NIOS2     113             //!< Altera Nios II soft-core processor
+#define EM_CRX              114             //!< National Semiconductor CompactRISC CRX microprocessor
+#define EM_XGATE            115             //!< Motorola XGATE embedded processor
+#define EM_C166             116             //!< Infifineon C16x/XC16x processor
+#define EM_M16C             117             //!< Renesas M16C series microprocessors
+#define EM_DSPIC30F         118             //!< Microchip Technology dsPIC30F Digital Signal Controller
+#define EM_CE               119             //!< Freescale Communication Engine RISC core
+#define EM_M32C             120             //!< Renesas M32C series microprocessors
+#define EM_TSK3000          131             //!< Altium TSK3000 core
+#define EM_RS08             132             //!< Freescale RS08 embedded processor
+#define EM_ECOG2            134             //!< Cyan Technology eCOG2 microprocessor
+#define EM_SCORE7           135             //!< Sunplus S+core7 RISC processor
+#define EM_DSP24            136             //!< New Japan Radio (NJR) 24-bit DSP Processor
+#define EM_VIDEOCORE3       137             //!< Broadcom VideoCore III processor
+#define EM_LATTICEMICO32    138             //!< RISC processor for Lattice FPGA architecture
+#define EM_SE_C17           139             //!< Seiko Epspson C17 family
+#define EM_TI_C6000         140             //!< The Texaxas Instruments TMS320C6000 DSP family
+#define EM_TI_C2000         141             //!< The Texaxas Instruments TMS320C2000 DSP family
+#define EM_TI_C5500         142             //!< The Texaxas Instruments TMS320C55x DSP family
+#define EM_MMDSP_PLUS       160             //!< STMicroelectronics 64bit VLIW Data Signal Processor
+#define EM_CYPRESS_M8C      161             //!< Cypress M8C microprocessor
+#define EM_R32C             162             //!< Renesas R32C series microprocessors
+#define EM_TRIMEDIA         163             //!< NXP Semiconductors TriMedia architecture family
+#define EM_QDSP6            164             //!< QUALCOMM DSP6 Processor
+#define EM_8051             165             //!< Intel 8051 and variants
+#define EM_STXP7X           166             //!< STMicroelectronics STxP7x family of configurable and extensible RISC processors
+#define EM_NDS32            167             //!< Andes Technology compact code size embedded RISC processor family
+#define EM_ECOG1            168             //!< Cyan Technology eCOG1X family
+#define EM_ECOG1X           168             //!< Cyan Technology eCOG1X family
+#define EM_MAXQ30           169             //!< Dallas Semiconductor MAXQ30 Core Micro-controllers
+#define EM_XIMO16           170             //!< New Japan Radio (NJR) 16-bit DSP Processor
+#define EM_MANIK            171             //!< M2000 Reconfigurable RISC Microprocessor
+#define EM_CRAYNV2          172             //!< Cray Inc. NV2 vector architecture
+#define EM_RX               173             //!< Renesas RX family
+#define EM_METAG            174             //!< Imagination Technologies META processor architecture
+#define EM_MCST_ELBRUS      175             //!< MCST Elbrus general purpose hardware architecture
+#define EM_ECOG16           176             //!< Cyan Technology eCOG16 family
+#define EM_CR16             177             //!< National Semiconductor CompactRISC CR16 16-bit microprocessor
+#define EM_ETPU             178             //!< Freescale Extended Time Processing Unit
+#define EM_SLE9X            179             //!< Infifineon Technologies SLE9X core
+#define EM_AVR32            185             //!< Atmel Corporation 32-bit microprocessor family
+#define EM_STM8             186             //!< STMicroeletronics STM8 8-bit microcontroller
+#define EM_TILE64           187             //!< Tilera TILE64 multicore architecture family
+#define EM_TILEPRO          188             //!< Tilera TILEPro multicore architecture family
+#define EM_MICROBLAZE       189             //!< Xilinx MicroBlaze 32-bit RISC soft processor core
+#define EM_CUDA             190             //!< NVIDIA CUDA architecture
+#define EM_TILEGX           191             //!< Tilera TILE-Gx multicore architecture family
+
+/*
+ * e_version (version)
+ */
+#define EV_NONE             0               //!< Invalid version
+#define EV_CURRENT          1               //!< Current version
+
+
+/*********************************************
+ * Section
+ *********************************************/
+typedef struct
+{
+  Elf32_Word            sh_name;                    //!< This member specifies the name of the section
+  Elf32_Word            sh_type;                    //!< This member categorizes the section's contents and semantics
+  Elf32_Word            sh_flags;                   //!< Sections support 1-bit flags that describe miscellaneous attributes
+  Elf32_Addr            sh_addr;                    //!< If the section will appear in the memory image of a process, this member gives the address at which the section's first byte should reside
+  Elf32_Off             sh_offset;                  //!< This member's value gives the byte offset from the beginning of the file to the first byte in the section
+  Elf32_Word            sh_size;                    //!< This member gives the section's size in bytes
+  Elf32_Word            sh_link;                    //!< This member holds a section header table index link, whose interpretation depends on the section type
+  Elf32_Word            sh_info;                    //!< This member holds extra information, whose interpretation depends on the section type
+  Elf32_Word            sh_addralign;               //!< Some sections have address alignment constraints
+  Elf32_Word            sh_entsize;                 //!< Some sections hold a table of fixed-size entries, such as a symbol table
+} Elf32_Shdr;       //!< 32bit Section header
+
+typedef struct
+{
+  Elf64_Word            sh_name;                    //!< This member specifies the name of the section
+  Elf64_Word            sh_type;                    //!< This member categorizes the section's contents and semantics
+  Elf64_Xword           sh_flags;                   //!< Sections support 1-bit flags that describe miscellaneous attributes
+  Elf64_Addr            sh_addr;                    //!< If the section will appear in the memory image of a process, this member gives the address at which the section's first byte should reside
+  Elf64_Off             sh_offset;                  //!< This member's value gives the byte offset from the beginning of the file to the first byte in the section
+  Elf64_Xword           sh_size;                    //!< This member gives the section's size in bytes
+  Elf64_Word            sh_link;                    //!< This member holds a section header table index link, whose interpretation depends on the section type
+  Elf64_Word            sh_info;                    //!< This member holds extra information, whose interpretation depends on the section type
+  Elf64_Xword           sh_addralign;               //!< Some sections have address alignment constraints
+  Elf64_Xword           sh_entsize;                 //!< Some sections hold a table of fixed-size entries, such as a symbol table
+} Elf64_Shdr;       //!< 64bit Section header
+
+/*
+ * Special Section Indexes
+ */
+#define SHN_UNDEF           0                   //!< This value marks an undefined, missing, irrelevant, or otherwise meaningless section reference
+#define SHN_LORESERVE       0xff00              //!< This value specifies the lower bound of the range of reserved indexes
+#define SHN_LOPROC          0xff00              //!< Values in this inclusive range are reserved for processor-specific semantics
+#define SHN_HIPROC          0xff1f              //!< Values in this inclusive range are reserved for processor-specific semantics
+#define SHN_LOOS            0xff20              //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define SHN_HIOS            0xff3f              //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define SHN_ABS             0xfff1              //!< This value specifies absolute values for the corresponding reference
+#define SHN_COMMON          0xfff2              //!< Symbols defined relative to this section are common symbols
+#define SHN_XINDEX          0xffff              //!< This value is an escape value
+#define SHN_HIRESERVE       0xffff              //!< This value specifies the upper bound of the range of reserved indexes
+
+/*
+ * sh_type
+ */
+#define SHT_NULL            0                   //!< This value marks the section header as inactive
+#define SHT_PROGBITS        1                   //!< The section holds information defined by the program
+#define SHT_SYMTAB          2                   //!< These sections hold a symbol table
+#define SHT_STRTAB          3                   //!< The section holds a string table
+#define SHT_RELA            4                   //!< The section holds relocation entries with explicit addends, such as type Elf32_Rela for the 32-bit class of object files or type Elf64_Rela for the 64-bit class of object files
+#define SHT_HASH            5                   //!< The section holds a symbol hash table
+#define SHT_DYNAMIC         6                   //!< The section holds information for dynamic linking
+#define SHT_NOTE            7                   //!< The section holds information that marks the file in some way
+#define SHT_NOBITS          8                   //!< A section of this type occupies no space in the file but otherwise resembles SHT_PROGBITS
+#define SHT_REL             9                   //!< The section holds relocation entries without explicit addends, such as type Elf32_Rel for the 32-bit class of object files or type Elf64_Rel for the 64-bit class of object files
+#define SHT_SHLIB           10                  //!< This section type is reserved but has unspecified semantics
+#define SHT_DYNSYM          11                  //!<
+#define SHT_INIT_ARRAY      14                  //!< This section contains an array of pointers to initialization functions
+#define SHT_FINI_ARRAY      15                  //!< This section contains an array of pointers to termination functions
+#define SHT_PREINIT_ARRAY   16                  //!< This section contains an array of pointers to functions that are invoked before all other initialization functions
+#define SHT_GROUP           17                  //!< This section defines a section group
+#define SHT_SYMTAB_SHNDX    18                  //!< This section is associated with a section of type SHT_SYMTAB and is required if any of the section header indexes referenced by that symbol table contain the escape value SHN_XINDEX
+#define SHT_LOOS            0x60000000          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define SHT_HIOS            0x6fffffff          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define SHT_LOPROC          0x70000000          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define SHT_HIPROC          0x7fffffff          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define SHT_LOUSER          0x80000000          //!< This value specifies the upper bound of the range of indexes reserved for application programs
+#define SHT_HIUSER          0x8fffffff          //!< This value specifies the upper bound of the range of indexes reserved for application programs
+
+/*
+ * sh_flags
+ */
+#define SHF_WRITE               0x1             //!< The section contains data that should be writable during process execution
+#define SHF_ALLOC               0x2             //!< The section occupies memory during process execution
+#define SHF_EXECINSTR           0x4             //!< The section contains executable machine instructions
+#define SHF_MERGE               0x10            //!< The data in the section may be merged to eliminate duplication
+#define SHF_STRINGS             0x20            //!< The data elements in the section consist of null-terminated character strings
+#define SHF_INFO_LINK           0x40            //!< The sh_info field of this section header holds a section header table index
+#define SHF_LINK_ORDER          0x80            //!< This flag adds special ordering requirements for link editors
+#define SHF_OS_NONCONFORMING    0x100           //!< This section requires special OS-specific processing (beyond the standard linking rules) to avoid incorrect behavior
+#define SHF_GROUP               0x200           //!< This section is a member (perhaps the only one) of a section group
+#define SHF_TLS                 0x400           //!< This section holds Thread-Local Storage, meaning that each separate execution flow has its own distinct instance of this data
+#define SHF_MASKOS              0x0ff00000      //!< All bits included in this mask are reserved for operating system-specific semantics
+#define SHF_MASKPROC            0xf0000000      //!< All bits included in this mask are reserved for processor-specific semantics
+
+
+/*********************************************
+ * Symbol
+ *********************************************/
+typedef struct
+{
+    Elf32_Word          st_name;                //!< This member holds an index into the object file's symbol string table, which holds the character representations of the symbol names
+    Elf32_Addr          st_value;               //!< This member gives the value of the associated symbol
+    Elf32_Word          st_size;                //!< Many symbols have associated sizes
+    unsigned char       st_info;                //!< This member specifies the symbol's type and binding attributes
+    unsigned char       st_other;               //!< This member currently specifies a symbol's visibility
+    Elf32_Section       st_shndx;               //!< Every symbol table entry is defined in relation to some section
+} Elf32_Sym;
+
+typedef struct
+{
+    Elf64_Word          st_name;                //!< This member holds an index into the object file's symbol string table, which holds the character representations of the symbol names
+    unsigned char       st_info;                //!< This member specifies the symbol's type and binding attributes
+    unsigned char       st_other;               //!< This member currently specifies a symbol's visibility
+    Elf64_Section       st_shndx;               //!< Every symbol table entry is defined in relation to some section
+    Elf64_Addr          st_value;               //!< This member gives the value of the associated symbol
+    Elf64_Xword         st_size;                //!< Many symbols have associated sizes
+} Elf64_Sym;
+
+/*
+ * st_info
+ */
+#define ELF32_ST_BIND(i)            ((i)>>4)
+#define ELF32_ST_TYPE(i)            ((i)&0xf)
+#define ELF32_ST_INFO(b,t)          (((b)<<4)+((t)&0xf))
+
+#define ELF64_ST_BIND(i)            ((i)>>4)
+#define ELF64_ST_TYPE(i)            ((i)&0xf)
+#define ELF64_ST_INFO(b,t)          (((b)<<4)+((t)&0xf))
+
+
+/* st_info (symbol binding) */
+#define STB_LOCAL               0           //!< Local symbols are not visible outside the object file containing their definition
+#define STB_GLOBAL              1           //!< Global symbols are visible to all object files being combined
+#define STB_WEAK                2           //!< Weak symbols resemble global symbols, but their definitions have lower precedence
+#define STB_LOOS                10          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define STB_HIOS                12          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define STB_LOPROC              13          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define STB_HIPROC              15          //!< Values in this inclusive range are reserved for processor-specific semantics
+
+/* st_info (symbol type)  */
+#define STT_NOTYPE              0           //!< The symbol's type is not specified
+#define STT_OBJECT              1           //!< The symbol is associated with a data object, such as a variable, an array, and so on
+#define STT_FUNC                2           //!< The symbol is associated with a function or other executable code
+#define STT_SECTION             3           //!< The symbol is associated with a section
+#define STT_FILE                4           //!< Conventionally, the symbol's name gives the name of the source file associated with the object file
+#define STT_COMMON              5           //!< The symbol labels an uninitialized common block
+#define STT_TLS                 6           //!<  The symbol specifies a Thread-Local Storage entity
+#define STT_LOOS                10          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define STT_HIOS                12          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define STT_LOPROC              13          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define STT_HIPROC              15          //!< Values in this inclusive range are reserved for processor-specific semantics
+
+/*
+ * st_other
+ */
+#define ELF32_ST_VISIBILITY(o)          ((o)&0x3)
+#define ELF64_ST_VISIBILITY(o)          ((o)&0x3)
+
+
+#define STV_DEFAULT             0         //!< The visibility of symbols with the STV_DEFAULT attribute is as specified by the symbol's binding type
+#define STV_INTERNAL            1         //!< A symbol defined in the current component is protected if it is visible in other components but not preemptable, meaning that any reference to such a symbol from within the defining component must be resolved to the definition in that component, even if there is a definition in another component that would preempt by the default rules
+#define STV_HIDDEN              2         //!< A symbol defined in the current component is hidden if its name is not visible to other components
+#define STV_PROTECTED           3         //!< The meaning of this visibility attribute may be defined by processor supplements to further constrain hidden symbols
+
+
+/*********************************************
+ * Relocation
+ *********************************************/
+typedef struct
+{
+    Elf32_Addr          r_offset;           //!< This member gives the location at which to apply the relocation action
+    Elf32_Word          r_info;             //!< This member gives both the symbol table index with respect to which the relocation must be made, and the type of relocation to apply
+} Elf32_Rel;        //!< 32bits Relocation Entries
+
+typedef struct
+{
+    Elf64_Addr          r_offset;           //!< This member gives the location at which to apply the relocation action
+    Elf64_Xword         r_info;             //!< This member gives both the symbol table index with respect to which the relocation must be made, and the type of relocation to apply
+} Elf64_Rel;        //!< 32bits Relocation Entries
+
+typedef struct
+{
+    Elf32_Addr          r_offset;           //!< This member gives the location at which to apply the relocation action
+    Elf32_Word          r_info;             //!< This member gives both the symbol table index with respect to which the relocation must be made, and the type of relocation to apply
+    Elf32_Sword         r_addend;           //!< This member specifies a constant addend used to compute the value to be stored into the relocatable field
+} Elf32_Rela;        //!< 32bits Relocation Addend Entries
+
+typedef struct
+{
+    Elf64_Addr          r_offset;           //!< This member gives the location at which to apply the relocation action
+    Elf64_Xword         r_info;             //!< This member gives both the symbol table index with respect to which the relocation must be made, and the type of relocation to apply
+    Elf64_Sxword        r_addend;           //!< This member specifies a constant addend used to compute the value to be stored into the relocatable field
+} Elf64_Rela;        //!< 32bits Relocation Addend Entries
+
+
+/*
+ * r_info
+ */
+#define ELF32_R_SYM(i)              ((i)>>8)
+#define ELF32_R_TYPE(i)             ((unsigned char)(i))
+#define ELF32_R_INFO(s,t)           (((s)<<8)+(unsigned char)(t))
+
+#define ELF64_R_SYM(i)              ((i)>>32)
+#define ELF64_R_TYPE(i)             ((i)&0xffffffffL)
+#define ELF64_R_INFO(s,t)           (((s)<<32)+((t)&0xffffffffL))
+
+
+
+/*********************************************
+ * Program
+ *********************************************/
+typedef struct
+{
+    Elf32_Word          p_type;             //!< This member tells what kind of segment this array element describes or how to interpret the array element's information
+    Elf32_Off           p_offset;           //!< This member gives the offset from the beginning of the file at which the first byte of the segment resides
+    Elf32_Addr          p_vaddr;            //!< This member gives the virtual address at which the first byte of the segment resides in memory
+    Elf32_Addr          p_paddr;            //!< On systems for which physical addressing is relevant, this member is reserved for the segment's physical address
+    Elf32_Word          p_filesz;           //!< This member gives the number of bytes in the file image of the segment; it may be zero
+    Elf32_Word          p_memsz;            //!< This member gives the number of bytes in the memory image of the segment; it may be zero
+    Elf32_Word          p_flags;            //!< This member gives flags relevant to the segment
+    Elf32_Word          p_align;            //!< As ``Program Loading'' describes in this chapter of the processor supplement, loadable process segments must have congruent values for p_vaddr and p_offset, modulo the page size
+} Elf32_Phdr;       //!< 32bits Program header
+
+typedef struct
+{
+    Elf64_Word          p_type;             //!< This member tells what kind of segment this array element describes or how to interpret the array element's information
+    Elf64_Word          p_flags;            //!< This member gives flags relevant to the segment
+    Elf64_Off           p_offset;           //!< This member gives the offset from the beginning of the file at which the first byte of the segment resides
+    Elf64_Addr          p_vaddr;            //!< This member gives the virtual address at which the first byte of the segment resides in memory
+    Elf64_Addr          p_paddr;            //!< On systems for which physical addressing is relevant, this member is reserved for the segment's physical address
+    Elf64_Xword         p_filesz;           //!< This member gives the number of bytes in the file image of the segment; it may be zero
+    Elf64_Xword         p_memsz;            //!< This member gives the number of bytes in the memory image of the segment; it may be zero
+    Elf64_Xword         p_align;            //!< As ``Program Loading'' describes in this chapter of the processor supplement, loadable process segments must have congruent values for p_vaddr and p_offset, modulo the page size
+} Elf64_Phdr;       //!< 64bits Program header
+
+/*
+ * p_type
+ */
+#define PT_NULL                 0                   //!< The array element is unused; other members' values are undefined
+#define PT_LOAD                 1                   //!< The array element specifies a loadable segment, described by p_filesz and p_memsz
+#define PT_DYNAMIC              2                   //!< The array element specifies dynamic linking information
+#define PT_INTERP               3                   //!< The array element specifies the location and size of a null-terminated path name to invoke as an interpreter
+#define PT_NOTE                 4                   //!< The array element specifies the location and size of auxiliary information
+#define PT_SHLIB                5                   //!< This segment type is reserved but has unspecified semantics
+#define PT_PHDR                 6                   //!< The array element, if present, specifies the location and size of the program header table itself, both in the file and in the memory image of the program
+#define PT_TLS                  7                   //!< The array element specifies the Thread-Local Storage template
+#define PT_LOOS                 0x60000000          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define PT_HIOS                 0x6fffffff          //!< Values in this inclusive range are reserved for operating system-specific semantics
+#define PT_LOPROC               0x70000000          //!< Values in this inclusive range are reserved for processor-specific semantics
+#define PT_HIPROC               0x7fffffff          //!< Values in this inclusive range are reserved for processor-specific semantics
+
+/*
+ * p_flags
+ */
+#define PF_X                    (1 << 0)            //!< Execute
+#define PF_W                    (1 << 1)            //!< Write
+#define PF_R                    (1 << 2)            //!< Read
+#define PF_MASKOS               0x0ff00000          //!< Unspecified
+#define PF_MASKPROC             0xf0000000          //!< Unspecified
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/elfapi.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/elfapi.h
new file mode 100644
index 00000000000..cce6d158b4e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/elfapi.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf loder internal methods.
+ *
+ * \defgroup ELFLOADER MMDSP ELF loader.
+ */
+#ifndef __INC_CM_ELFLOADER_H
+#define __INC_CM_ELFLOADER_H
+
+#include <cm/engine/elf/inc/common.h>
+
+/*!
+ * \internal
+ * \brief ELF Parsing & checking
+ * \ingroup ELFLOADER
+ */
+t_cm_error cm_ELF_CheckFile(
+        const char              *elfdata,
+        t_bool                  temporaryDescription,
+        t_elfdescription        **elfhandlePtr);
+
+void cm_ELF_ReleaseDescription(
+        t_uint32 requireNumber, t_interface_require *requires,
+        t_uint32 attributeNumber, t_attribute *attributes,
+        t_uint32 propertyNumber, t_property *properties,
+        t_uint32 provideNumber, t_interface_provide *provides);
+
+/*!
+ * \internal
+ * \brief ELF closing
+ * \ingroup ELFLOADER
+ */
+void cm_ELF_CloseFile(
+        t_bool                  temporaryDescription,
+        t_elfdescription        *elfhandle);
+
+/*!
+ * \internal
+ * \brief Load a component template shared memories.
+ *
+ * \note In case of error, part of memory could have been allocated and must be free by calling cm_DSPABI_FreeTemplate.
+ */
+t_cm_error cm_ELF_LoadTemplate(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_bool                  isSingleton);
+
+/*!
+ * \internal
+ * \brief Clean cache memory of a component template shared code.
+ */
+void cm_ELF_FlushTemplate(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY]);
+
+void cm_ELF_FlushInstance(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle privateMemories[NUMBER_OF_MMDSP_MEMORY]);
+
+/*!
+ * \internal
+ * \brief Load a component instance private memories.
+ *
+ * \note In case of error, part of memory could have been allocated and must be free by calling cm_DSPABI_FreeInstance.
+ */
+t_cm_error cm_ELF_LoadInstance(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle         privateMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_bool                  isSingleton);
+
+void cm_ELF_FreeInstance(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle privateMemories[NUMBER_OF_MMDSP_MEMORY]);
+void cm_ELF_FreeTemplate(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY]);
+
+
+t_cm_error cm_ELF_relocateSharedSegments(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        void                        *cbContext);
+t_cm_error cm_ELF_relocatePrivateSegments(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        void                        *cbContext);
+void cm_ELF_performRelocation(
+        t_uint32                    type,
+        const char                  *symbol_name,
+        t_uint32                    symbol_addr,
+        char                        *reloc_addr);
+t_cm_error cm_ELF_GetMemory(
+        t_elfdescription            *elf,
+        t_tmp_elfdescription        *elftmp,
+        t_uint32                    address,
+        t_memory_purpose            purpose,
+        t_memory_reference          *memory);
+
+
+#include <cm/engine/component/inc/component_type.h>
+
+t_cm_error cm_DSPABI_AddLoadMap(
+        t_cm_domain_id domainId,
+        const char* templateName,
+        const char* localname,
+        t_memory_handle *memories,
+        void *componentHandle);
+t_cm_error cm_DSPABI_RemoveLoadMap(
+        t_cm_domain_id domainId,
+        const char* templateName,
+        t_memory_handle *memories,
+        const char* localname,
+        void *componentHandle);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/memory.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/memory.h
new file mode 100644
index 00000000000..9eab94f173c
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/memory.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf memory.
+ */
+#ifndef __INC_CM_ELF_MEMORY_H
+#define __INC_CM_ELF_MEMORY_H
+
+#include <cm/engine/dsp/inc/dsp.h>
+
+/**
+ * \brief Memory identifier
+ */
+typedef t_uint8 t_memory_id;
+
+/**
+ * \brief Memory property
+ */
+typedef enum {
+    MEM_FOR_MULTIINSTANCE,
+    MEM_FOR_SINGLETON,
+    MEM_FOR_LAST
+} t_instance_property;
+
+/**
+ * \brief Memory prupose (for processor with different address space for code and data/
+ */
+typedef enum {
+    MEM_CODE,
+    MEM_DATA
+} t_memory_purpose;
+
+/**
+ * \brief Memory property
+ */
+typedef enum {
+    MEM_PRIVATE,
+    MEM_SHARABLE,
+} t_memory_property;
+
+/**
+ * \brief Elf memory mapping description
+ */
+typedef struct
+{
+    t_memory_id             id;
+    t_dsp_memory_type_id    dspMemType;
+    t_uint32                startAddr;
+    t_cm_memory_alignment   memAlignement;
+    t_memory_property       property;
+    t_memory_purpose        purpose;
+    t_uint8                 fileEntSize;
+    t_uint8                 memEntSize;
+    char*                   memoryName;
+} t_elfmemory;
+
+#define NUMBER_OF_MMDSP_MEMORY  15
+
+/*
+ * \brief Elf segment description
+ */
+typedef struct {
+    // Data in Bytes
+    t_uint32                sumSize;
+    t_bool                  sumSizeSetted;
+    t_cm_logical_address    hostAddr;   // Valid only if section Load in memory
+    t_uint32                maxAlign;
+    // Data in word
+    t_uint32                mpcAddr;    // Valid only if section Load in memory
+} t_elfSegment;
+
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp-loadmap.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp-loadmap.h
new file mode 100644
index 00000000000..bb65c0b1244
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp-loadmap.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf writer internal methods.
+ *
+ * \defgroup LOADMAP MMDSP ELF writer (a linker in fact).
+ */
+#ifndef __INC_CM_LOADMAP_H
+#define __INC_CM_LOADMAP_H
+
+#include <cm/inc/cm_type.h>
+
+/*
+ * Align with loadmap :
+ * https://codex.cro.st.com/wiki/index.php?pagename=Specification%2FLoadmap%2Fv1.2&group_id=310
+ */
+#define LOADMAP_MAGIC_NUMBER 0xFBBF
+
+#define LOADMAP_VERSION_MSB 1
+#define LOADMAP_VERSION_LSB 2
+
+struct LoadMapItem
+{
+  const char*  pSolibFilename;  // Filename of shared library object
+  void*        pAddrProg;       // Load address of program section
+  void*        pAddrEmbProg;    // Load address of embedded program section
+  void*        pThis;           // Data base address of component instance
+  void*        pARMThis;        // ARM component debug ID
+  const char*  pComponentName;  // Pretty name of the component instance, NULL if none.
+  struct LoadMapItem* pNextItem;// Pointer on the next list item, NULL if last one.
+  void*        pXROM;           // Start address of XROM
+  void*        pYROM;           // Start address of YROM
+};
+
+struct LoadMapHdr
+{
+  t_uint16      nMagicNumber;    // Equal to 0xFBBF.
+  t_uint16      nVersion;        // The version of the load map format.
+  t_uint32      nRevision;       // A counter incremented at each load map list modification.
+  struct LoadMapItem* pFirstItem;// Pointer on the first item, NULL if no shared library loaded.
+};
+
+#endif /* __INC_CM_LOADMAP_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp.h
new file mode 100644
index 00000000000..1662def6c1a
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/mmdsp.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief MMDSP elf.
+ */
+#ifndef __INC_CM_ELF_MMDSP_H
+#define __INC_CM_ELF_MMDSP_H
+
+#include <cm/engine/elf/inc/common.h>
+
+#define CODE_MEMORY_INDEX           0
+#define ECODE_MEMORY_INDEX          7
+
+#define XROM_MEMORY_INDEX           1
+#define YROM_MEMORY_INDEX           2
+#define PRIVATE_DATA_MEMORY_INDEX   8
+#define SHARE_DATA_MEMORY_INDEX     1
+
+/*
+ * Relocation
+ */
+#define R_MMDSP_IMM16       5
+#define R_MMDSP_IMM20_16    6
+#define R_MMDSP_IMM20_4     7
+#define R_MMDSP_24          13
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/mpcal.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/mpcal.h
new file mode 100644
index 00000000000..718b7f61ceb
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/mpcal.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief MPC Abraction Layer.
+ *
+ * \defgroup MPCAL MPC Abraction Layer.
+ */
+#ifndef __INC_CM_DSP_MPCAL_H
+#define __INC_CM_DSP_MPCAL_H
+
+#include <cm/inc/cm_type.h>
+#include <share/inc/nmf.h>
+
+#include <cm/engine/elf/inc/common.h>
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/inc/reloc.h b/drivers/staging/nmf-cm/cm/engine/elf/inc/reloc.h
new file mode 100644
index 00000000000..b38be48d689
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/inc/reloc.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Elf relocation.
+ */
+#ifndef __INC_CM_ELF_RELOC_H
+#define __INC_CM_ELF_RELOC_H
+
+
+void MMDSP_performRelocation(
+        t_uint32 type,
+        const char* symbol_name,
+        t_uint32 symbol_addr,
+        char* reloc_addr,
+        const char* inPlaceAddr,
+        t_uint32 reloc_offset);
+
+/*
+ *
+ * Return:
+ * 0x0 returned if symbol not found
+ * 0xFFFFFFFE returned if out of memory
+ * 0xFFFFFFFF returned if symbol found in static required binding
+ */
+typedef t_uint32 (*CBresolvSymbol)(
+        void* context,
+        t_uint32 type,
+        const char* symbolName,
+        char* reloc_addr);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elf64.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elf64.c
new file mode 100644
index 00000000000..2e0f5928ffd
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elf64.c
@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/elf/inc/common.h>
+#include <cm/engine/elf/inc/elfabi.h>
+
+#include <cm/engine/utils/inc/swap.h>
+#include <cm/engine/trace/inc/trace.h>
+
+typedef Elf64_Half ElfXX_Half;
+typedef Elf64_Word ElfXX_Word;
+typedef Elf64_Addr ElfXX_Addr;
+typedef Elf64_Off ElfXX_Off;
+
+typedef Elf64_Xword ElfXX_Xword;
+
+typedef Elf64_Ehdr ElfXX_Ehdr;
+typedef Elf64_Shdr ElfXX_Shdr;
+typedef Elf64_Sym ElfXX_Sym;
+typedef Elf64_Rela ElfXX_Rela;
+
+#undef ELFXX_R_SYM
+#define ELFXX_R_SYM ELF64_R_SYM
+#undef ELFXX_R_TYPE
+#define ELFXX_R_TYPE ELF64_R_TYPE
+#undef ELFXX_R_INFO
+#define ELFXX_R_INFO ELF64_R_INFO
+
+// TODO Here we assume big endian (MMDSP !)
+static Elf64_Half swapHalf(Elf64_Half half)
+{
+    return (Elf64_Half)swap16(half);
+}
+
+static Elf64_Word swapWord(Elf64_Word word)
+{
+    return (Elf64_Word)swap32(word);
+}
+
+static Elf64_Xword swapXword(Elf64_Xword xword)
+{
+    return (Elf64_Xword)swap64(xword);
+}
+
+#include "elfxx.c"
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elfload.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elfload.c
new file mode 100644
index 00000000000..466d04508d1
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elfload.c
@@ -0,0 +1,773 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/elf/inc/elfapi.h>
+#include <cm/engine/elf/inc/mpcal.h>
+#include <cm/inc/cm_def.h>
+
+//#include <cm/engine/component/inc/introspection.h>
+
+#include <cm/engine/utils/inc/mem.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/swap.h>
+#include <cm/engine/utils/inc/string.h>
+
+static void* getElfHeaderReference(t_tmp_elfdescription *elftmp, void* hdrref)
+{
+    if(hdrref != NULL)
+        return (void*)((int)swap32((t_uint32)hdrref) + (int)elftmp->elfheader);
+    else
+        return NULL;
+}
+
+static t_dup_char copyElfString(t_tmp_elfdescription *elftmp, char* idx)
+{
+    return cm_StringDuplicate((char*)getElfHeaderReference(elftmp, (void*)idx));
+}
+
+static t_cm_error getMemoryOffset(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp,
+        t_memory_purpose            purpose,
+        const t_uint32              *addressInNmf,
+        t_memory_reference          *memory) {
+
+    if(elftmp->isExecutable) {
+        return cm_ELF_GetMemory(elfhandle, elftmp,
+                swap32(*addressInNmf),
+                purpose,
+                memory);
+    } else {
+        return ELF64_getRelocationMemory(elfhandle, elftmp,
+                (t_uint32)addressInNmf - (t_uint32)elftmp->elfheader,
+                memory);
+    }
+}
+
+static t_cm_error getAdressForExecutableOffsetElsewhere(
+        t_elfdescription                *elfhandle,
+        t_tmp_elfdescription            *elftmp,
+        const t_uint32                  *addressInNmf,
+        t_memory_reference              *memory) {
+    t_uint32 address;
+
+    address = swap32(*addressInNmf);
+    if(address == 0xFFFFFFFF)
+    {
+        memory->offset = 0x0;
+        memory->memory = NULL;
+        return CM_OK;
+    }
+
+    if(elftmp->isExecutable)
+    {
+        memory->offset = address;
+        memory->memory = NULL;
+        return CM_OK;
+    }
+
+    // Error log in elfhandle by previous call will be check in loadTemplate
+    return ELF64_getRelocationMemory(elfhandle, elftmp,
+            (t_uint32)addressInNmf - (t_uint32)elftmp->elfheader,
+            memory);
+}
+
+/*
+ * Interface Management
+ */
+static t_interface_description* interfaceList = NULL;
+
+static t_interface_description* getInterfaceDescription(t_tmp_elfdescription *elftmp, t_elf_interface_description* elfitf) {
+    t_dup_char itfType;
+    t_interface_description* itf;
+    int i;
+
+    itfType = copyElfString(elftmp, elfitf->type);
+    if(itfType == NULL)
+        return NULL;
+
+    // Search if interfane already loaded
+    for(itf = interfaceList; itf != NULL; itf = itf->next) {
+        if(itf->type == itfType) {
+	    if (itf->methodNumber != elfitf->methodNumber) {
+		    ERROR("When loading component template %s:\n\tNumber of methods in interface type %s\n\tdiffers from previous declaration: was %d, found %d\n",
+			  getElfHeaderReference(elftmp, (void*)elftmp->elfheader->templateName), itfType, itf->methodNumber, elfitf->methodNumber, 0, 0);
+		    //Do not fail for now for compatibility reason
+		    //goto out_itf_type;
+	    }
+	    if (cmIntensiveCheckState) {
+		    for(i = 0; i < itf->methodNumber; i++) {
+			    if (cm_StringCompare(itf->methodNames[i], getElfHeaderReference(elftmp, (void*)elfitf->methodNames[i]), MAX_INTERNAL_STRING_LENGTH) != 0) {
+				    ERROR("When loading component template %s:\n"
+					  "\tName of method number %d in interface type %s\n"
+					  "\tdiffers from previous declaration: previous name was %s, new name found is %s\n",
+					  getElfHeaderReference(elftmp, (void*)elftmp->elfheader->templateName), i,
+					  itfType, itf->methodNames[i],
+					  getElfHeaderReference(elftmp, (void*)elfitf->methodNames[i]), 0);
+				    //Do not fail for now for compatibility reason
+				    //goto out_itf_type;
+			    }
+		    }
+	    }
+            itf->referenceCounter++;
+            cm_StringRelease(itfType);
+            return itf;
+        }
+    }
+
+    // Create a new interface if not exists
+    itf = (t_interface_description*)OSAL_Alloc_Zero(sizeof(t_interface_description) + sizeof(t_dup_char) * (elfitf->methodNumber - 1));
+    if(itf == NULL)
+        goto out_itf_type;
+    itf->referenceCounter = 1;
+    itf->type = itfType;
+    itf->methodNumber = elfitf->methodNumber;
+    for(i = 0; i < itf->methodNumber; i++) {
+        itf->methodNames[i] = copyElfString(elftmp, elfitf->methodNames[i]);
+        if(itf->methodNames[i] == NULL)
+            goto out_method;
+    }
+
+    // Put it in Top
+    itf->next = interfaceList;
+    interfaceList = itf;
+
+    return itf;
+
+out_method:
+    for(i = 0; i < itf->methodNumber; i++)
+        cm_StringRelease(itf->methodNames[i]);
+    OSAL_Free(itf);
+out_itf_type:
+    cm_StringRelease(itfType);
+    return NULL;
+}
+
+static void releaseInterfaceDescription(t_interface_description* itf) {
+    if(itf == NULL)
+        return;
+
+    if(--itf->referenceCounter == 0) {
+        int i;
+
+        // Remove it from list
+        if(interfaceList == itf) {
+            interfaceList = interfaceList->next;
+        } else {
+            t_interface_description* prev = interfaceList;
+            while(prev->next != itf)
+                prev = prev->next;
+            prev->next = itf->next;
+        }
+
+        // Destroy interface description
+        for(i = 0; i < itf->methodNumber; i++) {
+            cm_StringRelease(itf->methodNames[i]);
+        }
+        cm_StringRelease(itf->type);
+        OSAL_Free(itf);
+    }
+}
+
+
+t_cm_error cm_ELF_CheckFile(
+        const char              *elfdata,
+        t_bool                  temporaryDescription,
+        t_elfdescription        **elfhandlePtr)
+{
+    t_elfdescription        *elfhandle;
+    t_tmp_elfdescription    elftmp = {0,};
+    t_cm_error error;
+    t_uint32 version;
+    t_uint32 compatibleVersion;
+    int i, j, k;
+
+    /*
+     * Sanity check
+     */
+    if (elfdata[EI_MAG0] != ELFMAG0 ||
+            elfdata[EI_MAG1] != ELFMAG1 ||
+            elfdata[EI_MAG2] != ELFMAG2 ||
+            elfdata[EI_MAG3] != ELFMAG3 ||
+            elfdata[EI_CLASS] != ELFCLASS64)
+    {
+        ERROR("CM_INVALID_ELF_FILE: component file is not a MMDSP ELF file\n", 0, 0, 0, 0, 0, 0);
+        return CM_INVALID_ELF_FILE;
+    }
+
+    /*
+     * Create elf data
+     */
+    if((error = ELF64_LoadComponent(EM_MMDSP_PLUS, elfdata, elfhandlePtr, &elftmp)) != CM_OK)
+        return error;
+
+    elfhandle = *elfhandlePtr;
+
+    elfhandle->temporaryDescription = temporaryDescription;
+
+    version = swap32(elftmp.elfheader->nmfVersion);
+
+    compatibleVersion = (VERSION_MAJOR(version) == VERSION_MAJOR(NMF_VERSION));
+    if(compatibleVersion)
+    {
+        switch(VERSION_MINOR(NMF_VERSION))
+        {
+        case 10: // Compatible with 2.9, 2.10
+            compatibleVersion =
+                    (VERSION_MINOR(version) == 9) ||
+                    (VERSION_MINOR(version) == 10);
+            break;
+        default: // Strict compatibility 2.x == 2.x
+            compatibleVersion = (VERSION_MINOR(version) == VERSION_MINOR(NMF_VERSION));
+        }
+    }
+
+    if(! compatibleVersion)
+    {
+        ERROR("CM_INVALID_ELF_FILE: incompatible version for Component %d.%d.x != CM:%d.%d.x\n",
+                VERSION_MAJOR(version), VERSION_MINOR(version),
+                VERSION_MAJOR(NMF_VERSION), VERSION_MINOR(NMF_VERSION), 0, 0);
+        error = CM_INVALID_ELF_FILE;
+        goto onerror;
+    }
+
+
+    /*
+     * Commented since to many noise !!!!
+    if(VERSION_PATCH(version) != VERSION_PATCH(NMF_VERSION))
+    {
+        WARNING("CM_INVALID_ELF_FILE: incompatible version, Component:%d.%d.%d != CM:%d.%d.%d\n",
+                VERSION_MAJOR(version), VERSION_MINOR(version), VERSION_PATCH(version),
+                VERSION_MAJOR(NMF_VERSION), VERSION_MINOR(NMF_VERSION), VERSION_PATCH(NMF_VERSION));
+    }
+     */
+
+    if((error = ELF64_ComputeSegment(elfhandle, &elftmp)) != CM_OK)
+        goto onerror;
+
+    //
+    elfhandle->foundedTemplateName =  copyElfString(&elftmp, elftmp.elfheader->templateName);
+    if(elfhandle->foundedTemplateName == NULL)
+        goto oom;
+    elfhandle->minStackSize = swap32(elftmp.elfheader->minStackSize);
+
+    // Get Life-cycle memory
+    if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp, &elftmp.elfheader->LCCConstruct, &elfhandle->memoryForConstruct)) != CM_OK)
+        goto onerror;
+    if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp, &elftmp.elfheader->LCCStart, &elfhandle->memoryForStart)) != CM_OK)
+        goto onerror;
+    if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp, &elftmp.elfheader->LCCStop, &elfhandle->memoryForStop)) != CM_OK)
+        goto onerror;
+    if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp, &elftmp.elfheader->LCCDestroy, &elfhandle->memoryForDestroy)) != CM_OK)
+        goto onerror;
+
+    // Copy attributes information
+    elfhandle->attributeNumber = swap32(elftmp.elfheader->attributeNumber);
+    if(elfhandle->attributeNumber > 0)
+    {
+        elfhandle->attributes =
+                (t_attribute*)OSAL_Alloc_Zero(sizeof(t_attribute) * elfhandle->attributeNumber);
+        if(elfhandle->attributes == NULL)
+            goto oom;
+
+        if(elfhandle->attributeNumber >  0)
+        {
+            t_elf_attribute *attributes = (t_elf_attribute*)getElfHeaderReference(&elftmp, (void*)elftmp.elfheader->attributes);
+
+            for(i = 0; i < elfhandle->attributeNumber; i++)
+            {
+                elfhandle->attributes[i].name = copyElfString(&elftmp, attributes[i].name);
+                if(elfhandle->attributes[i].name == NULL)
+                    goto oom;
+
+                if((error = getMemoryOffset(elfhandle, &elftmp,
+                        MEM_DATA,
+                        &attributes[i].symbols,
+                        &elfhandle->attributes[i].memory)) != CM_OK)
+                        goto onerror;
+                LOG_INTERNAL(2, "  attribute %s mem=%s offset=%x\n",
+                        elfhandle->attributes[i].name,
+                        elfhandle->attributes[i].memory.memory->memoryName,
+                        elfhandle->attributes[i].memory.offset,
+                        0, 0, 0);
+            }
+        }
+    }
+
+    // Copy properties information
+    elfhandle->propertyNumber = swap32(elftmp.elfheader->propertyNumber);
+    if(elfhandle->propertyNumber > 0)
+    {
+        elfhandle->properties =
+                (t_property*)OSAL_Alloc_Zero(sizeof(t_property) * elfhandle->propertyNumber);
+        if(elfhandle->properties == NULL)
+            goto oom;
+
+        if(elfhandle->propertyNumber > 0)
+        {
+            t_elf_property *properties = (t_elf_property*)getElfHeaderReference(&elftmp, (void*)elftmp.elfheader->properties);
+
+            for(i = 0; i < elfhandle->propertyNumber; i++)
+            {
+                elfhandle->properties[i].name = copyElfString(&elftmp, properties[i].name);
+                if(elfhandle->properties[i].name == NULL)
+                    goto oom;
+
+                elfhandle->properties[i].value = copyElfString(&elftmp, properties[i].value);
+                if(elfhandle->properties[i].value == NULL)
+                    goto oom;
+
+                LOG_INTERNAL(3, "  property %s = %s\n",
+                        elfhandle->properties[i].name,
+                        elfhandle->properties[i].value,
+                        0, 0, 0, 0);
+            }
+        }
+    }
+
+    // Copy requires information
+    elfhandle->requireNumber = swap32(elftmp.elfheader->requireNumber);
+    if(elfhandle->requireNumber > 0)
+    {
+        char *ref = getElfHeaderReference(&elftmp, (void*)elftmp.elfheader->requires);
+
+        elfhandle->requires = (t_interface_require*)OSAL_Alloc_Zero(sizeof(t_interface_require) * elfhandle->requireNumber);
+        if(elfhandle->requires == NULL)
+            goto oom;
+
+        for(i = 0; i < elfhandle->requireNumber; i++)
+        {
+            t_elf_required_interface *require = (t_elf_required_interface*)ref;
+            t_elf_interface_description *interface = (t_elf_interface_description*)getElfHeaderReference(&elftmp, (void*)require->interface);
+
+            elfhandle->requires[i].name = copyElfString(&elftmp, require->name);
+            if(elfhandle->requires[i].name == NULL)
+                goto oom;
+
+            elfhandle->requires[i].requireTypes = require->requireTypes;
+            elfhandle->requires[i].collectionSize = require->collectionSize;
+            elfhandle->requires[i].interface = getInterfaceDescription(&elftmp, interface);
+            if(elfhandle->requires[i].interface == NULL)
+                goto oom;
+
+            LOG_INTERNAL(2, "  require %s <%s> %x\n",
+                    elfhandle->requires[i].name,
+                    elfhandle->requires[i].interface->type,
+                    elfhandle->requires[i].requireTypes, 0, 0, 0);
+            CM_ASSERT(elfhandle->requires[i].collectionSize != 0);
+
+            ref = (char*)&require->indexes[0];
+
+            if((elfhandle->requires[i].requireTypes & VIRTUAL_REQUIRE) == 0 &&
+                    (elfhandle->requires[i].requireTypes & STATIC_REQUIRE) == 0)
+            {
+                elfhandle->requires[i].indexes =
+                        (t_interface_require_index*)OSAL_Alloc_Zero(sizeof(t_interface_require_index) * elfhandle->requires[i].collectionSize);
+                if(elfhandle->requires[i].indexes == NULL)
+                    goto oom;
+
+                for(j = 0; j < elfhandle->requires[i].collectionSize; j++)
+                {
+                    t_elf_interface_require_index* index = (t_elf_interface_require_index*)ref;
+
+                    elfhandle->requires[i].indexes[j].numberOfClient = swap32(index->numberOfClient);
+                    if(elfhandle->requires[i].indexes[j].numberOfClient != 0)
+                    {
+                        elfhandle->requires[i].indexes[j].memories =
+                                (t_memory_reference*)OSAL_Alloc(sizeof(t_memory_reference) *  elfhandle->requires[i].indexes[j].numberOfClient);
+                        if(elfhandle->requires[i].indexes[j].memories == NULL)
+                            goto oom;
+
+                        for(k = 0; k < elfhandle->requires[i].indexes[j].numberOfClient; k++) {
+                            if((error = getMemoryOffset(elfhandle,&elftmp,
+                                    MEM_DATA,
+                                    &index->symbols[k],
+                                    &elfhandle->requires[i].indexes[j].memories[k])) != CM_OK)
+                                goto onerror;
+                            LOG_INTERNAL(2, "    [%d, %d] mem=%s offset=%x\n",
+                                    j, k,
+                                    elfhandle->requires[i].indexes[j].memories[k].memory->memoryName,
+                                    elfhandle->requires[i].indexes[j].memories[k].offset,
+                                    0, 0);
+                        }
+                    }
+
+                    ref += sizeof(index->numberOfClient) + elfhandle->requires[i].indexes[j].numberOfClient * sizeof(index->symbols[0]);
+                }
+            }
+        }
+    }
+
+    // Copy provides informations
+    elfhandle->provideNumber = swap32(elftmp.elfheader->provideNumber);
+    if(elfhandle->provideNumber != 0)
+    {
+        elfhandle->provides =
+                (t_interface_provide*)OSAL_Alloc_Zero(sizeof(t_interface_provide) * elfhandle->provideNumber);
+        if(elfhandle->provides == NULL)
+            goto oom;
+
+        if(elfhandle->provideNumber > 0)
+        {
+            char *ref = getElfHeaderReference(&elftmp, (void*)elftmp.elfheader->provides);
+
+            for(i = 0; i < elfhandle->provideNumber; i++)
+            {
+                t_elf_provided_interface *provide = (t_elf_provided_interface*)ref;
+                t_elf_interface_description *interface = (t_elf_interface_description*)getElfHeaderReference(&elftmp, (void*)provide->interface);
+
+                elfhandle->provides[i].name = copyElfString(&elftmp, provide->name);
+                if(elfhandle->provides[i].name == NULL)
+                    goto oom;
+
+                elfhandle->provides[i].provideTypes = provide->provideTypes;
+                elfhandle->provides[i].interruptLine = provide->interruptLine;
+                elfhandle->provides[i].collectionSize = provide->collectionSize;
+                elfhandle->provides[i].interface = getInterfaceDescription(&elftmp, interface);
+                if(elfhandle->provides[i].interface == NULL)
+                    goto oom;
+
+                LOG_INTERNAL(2, "  provide %s <%s>\n",
+                        elfhandle->provides[i].name,
+                        elfhandle->provides[i].interface->type,
+                        0,0, 0, 0);
+                CM_ASSERT(elfhandle->provides[i].collectionSize != 0);
+
+                ref = (char*)&provide->methodSymbols[0];
+
+                {
+                    t_uint32 *methodSymbols = (t_uint32*)ref;
+
+                    elfhandle->provides[i].indexes = (t_interface_provide_index**)OSAL_Alloc_Zero(
+                            sizeof(t_interface_provide_index*) * elfhandle->provides[i].collectionSize);
+                    if(elfhandle->provides[i].indexes == NULL)
+                        goto oom;
+
+                    if(elfhandle->provides[i].interface->methodNumber != 0)
+                    {
+                        for(j = 0; j < elfhandle->provides[i].collectionSize; j++)
+                        {
+                            elfhandle->provides[i].indexes[j] = (t_interface_provide_index*)OSAL_Alloc(
+                                    sizeof(t_interface_provide_index) * elfhandle->provides[i].interface->methodNumber);
+                            if(elfhandle->provides[i].indexes[j] == NULL)
+                                goto oom;
+
+                            for(k = 0; k < elfhandle->provides[i].interface->methodNumber; k++)
+                            {
+                                if((error = getAdressForExecutableOffsetElsewhere(elfhandle, &elftmp,
+                                        methodSymbols++,
+                                        &elfhandle->provides[i].indexes[j][k].memory)) != CM_OK)
+                                        goto onerror;
+
+                                if(elfhandle->provides[i].indexes[j][k].memory.memory != NULL)
+                                    LOG_INTERNAL(2, "    [%d, %d] method '%s' mem=%s offset=%x\n",
+                                        j, k,
+                                        elfhandle->provides[i].interface->methodNames[k],
+                                        elfhandle->provides[i].indexes[j][k].memory.memory->memoryName,
+                                        elfhandle->provides[i].indexes[j][k].memory.offset,
+                                        0);
+                                else
+                                    LOG_INTERNAL(2, "    [%d, %d] method '%s' address=%x\n",
+                                        j, k,
+                                        elfhandle->provides[i].interface->methodNames[k],
+                                        elfhandle->provides[i].indexes[j][k].memory.offset,
+                                        0, 0);
+                            }
+                        }
+                    }
+
+                    ref += elfhandle->provides[i].collectionSize * elfhandle->provides[i].interface->methodNumber * sizeof(methodSymbols[0]);
+                }
+            }
+        }
+    }
+
+    return CM_OK;
+
+oom:
+    error = CM_NO_MORE_MEMORY;
+onerror:
+    cm_ELF_CloseFile(temporaryDescription, elfhandle);
+    *elfhandlePtr = NULL;
+    return error;
+}
+
+void cm_ELF_ReleaseDescription(
+        t_uint32 requireNumber, t_interface_require *requires,
+        t_uint32 attributeNumber, t_attribute *attributes,
+        t_uint32 propertyNumber, t_property *properties,
+        t_uint32 provideNumber, t_interface_provide *provides)
+{
+    int i, j;
+
+    // Free provides (Number set when array allocated)
+    if(provides != NULL)
+    {
+        for(i = 0; i < provideNumber; i++)
+        {
+            if(provides[i].indexes != NULL)
+            {
+                for(j = 0; j < provides[i].collectionSize; j++)
+                {
+                    OSAL_Free(provides[i].indexes[j]);
+                }
+                OSAL_Free(provides[i].indexes);
+            }
+            releaseInterfaceDescription(provides[i].interface);
+            cm_StringRelease(provides[i].name);
+        }
+        OSAL_Free(provides);
+    }
+
+    // Free requires (Number set when array allocated)
+    if(requires != NULL)
+    {
+        for(i = 0; i < requireNumber; i++)
+        {
+            if(requires[i].indexes != 0)
+            {
+                for(j = 0; j < requires[i].collectionSize; j++)
+                {
+                    OSAL_Free(requires[i].indexes[j].memories);
+                }
+                OSAL_Free(requires[i].indexes);
+            }
+            releaseInterfaceDescription(requires[i].interface);
+            cm_StringRelease(requires[i].name);
+        }
+        OSAL_Free(requires);
+    }
+
+    // Free properties (Number set when array allocated)
+    if(properties != NULL)
+    {
+        for(i = 0; i < propertyNumber; i++)
+        {
+            cm_StringRelease(properties[i].value);
+            cm_StringRelease(properties[i].name);
+        }
+        OSAL_Free(properties);
+    }
+
+    // Free Attributes (Number set when array allocated)
+    if(attributes != NULL)
+    {
+        for(i = 0; i < attributeNumber; i++)
+        {
+            cm_StringRelease(attributes[i].name);
+        }
+        OSAL_Free(attributes);
+    }
+}
+
+void cm_ELF_CloseFile(
+        t_bool                  temporaryDescription,
+        t_elfdescription        *elfhandle)
+{
+    if(elfhandle == NULL)
+        return;
+
+    if(temporaryDescription && ! elfhandle->temporaryDescription)
+        return;
+
+    // Release description if not moved to template
+    cm_ELF_ReleaseDescription(
+            elfhandle->requireNumber, elfhandle->requires,
+            elfhandle->attributeNumber, elfhandle->attributes,
+            elfhandle->propertyNumber, elfhandle->properties,
+            elfhandle->provideNumber, elfhandle->provides);
+
+    cm_StringRelease(elfhandle->foundedTemplateName);
+
+    ELF64_UnloadComponent(elfhandle);
+}
+
+
+static t_cm_error allocSegment(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         memories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_property       property,
+        t_bool                  isSingleton) {
+    t_memory_id memId;
+    const t_elfmemory           *thisMemory;            //!< Memory used to determine this
+    const t_elfmemory           *codeMemory;            //!< Memory used to determine code
+
+    MMDSP_serializeMemories(elfhandle->instanceProperty, &codeMemory, &thisMemory);
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        const t_elfmemory* mapping;
+
+        if(elfhandle->segments[memId].sumSize == 0x0)
+            continue;
+
+        mapping = MMDSP_getMappingById(memId);
+
+        if(
+                (mapping->property == property && elfhandle->instanceProperty != MEM_FOR_SINGLETON) ||
+                (property == MEM_SHARABLE && elfhandle->instanceProperty == MEM_FOR_SINGLETON) )
+        {
+            // Allocate segment
+            memories[memId] = cm_DM_Alloc(domainId, mapping->dspMemType,
+                    elfhandle->segments[memId].sumSize / mapping->fileEntSize,
+                    mapping->memAlignement, TRUE);
+
+            if(memories[memId] == INVALID_MEMORY_HANDLE)
+            {
+                ERROR("CM_NO_MORE_MEMORY(%s): %x too big\n", mapping->memoryName, elfhandle->segments[memId].sumSize / mapping->fileEntSize, 0, 0, 0, 0);
+                return CM_NO_MORE_MEMORY;
+            }
+
+            // Get reference in memory
+            elfhandle->segments[memId].hostAddr = cm_DSP_GetHostLogicalAddress(memories[memId]);
+
+            cm_DSP_GetDspAddress(memories[memId], &elfhandle->segments[memId].mpcAddr);
+
+	    if (isSingleton)
+		    cm_DM_SetDefaultDomain(memories[memId], cm_DM_GetDomainCoreId(domainId));
+
+            // Log it
+            LOG_INTERNAL(1, "\t%s%s: 0x%x..+0x%x (0x%x)\n",
+                    mapping->memoryName,
+                    (thisMemory == mapping) ? "(THIS)" : "",
+                    elfhandle->segments[memId].mpcAddr,
+                    elfhandle->segments[memId].sumSize / mapping->fileEntSize,
+                    elfhandle->segments[memId].hostAddr, 0);
+        }
+        else if(property == MEM_PRIVATE) // Since we allocate private segment, if not allocate, it's a share one
+        {
+            // In order to allow further relocation based on cached address like mpcAddr & hostAddr,
+            // initialize them also !
+
+            // Get reference in memory
+            elfhandle->segments[memId].hostAddr = cm_DSP_GetHostLogicalAddress(memories[memId]);
+
+            cm_DSP_GetDspAddress(memories[memId], &elfhandle->segments[memId].mpcAddr);
+        }
+    }
+
+    return CM_OK;
+}
+
+/*
+ * Note: in case of error, part of memory could have been allocated and must be free by calling cm_DSPABI_FreeTemplate
+ */
+t_cm_error cm_ELF_LoadTemplate(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_bool                  isSingleton)
+{
+    t_cm_error error;
+
+    if((error = allocSegment(domainId, elfhandle, sharedMemories, MEM_SHARABLE, isSingleton)) != CM_OK)
+        return error;
+
+    // Load each readonly segment
+    if((error = ELF64_loadSegment(elfhandle, sharedMemories, MEM_SHARABLE)) != CM_OK)
+        return error;
+
+    return CM_OK;
+}
+
+t_cm_error cm_ELF_LoadInstance(
+        t_cm_domain_id          domainId,
+        t_elfdescription        *elfhandle,
+        t_memory_handle         sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle         privateMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_bool                  isSingleton)
+{
+    t_memory_id memId;
+    t_cm_error error;
+
+    // Erase whole memories to make free in case of error
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        privateMemories[memId] = sharedMemories[memId];
+    }
+
+    if((error = allocSegment(domainId, elfhandle, privateMemories, MEM_PRIVATE, isSingleton)) != CM_OK)
+        return error;
+
+    // Load each writable memory
+    if((error = ELF64_loadSegment(elfhandle, privateMemories, MEM_PRIVATE)) != CM_OK)
+        return error;
+
+    return CM_OK;
+}
+
+void cm_ELF_FlushTemplate(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_memory_id memId;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        if(sharedMemories[memId] != INVALID_MEMORY_HANDLE)
+            MMDSP_loadedSection(
+                    coreId, memId,
+                    sharedMemories[memId]);
+    }
+}
+
+void cm_ELF_FlushInstance(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle privateMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_memory_id memId;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        if(privateMemories[memId] != INVALID_MEMORY_HANDLE && privateMemories[memId] != sharedMemories[memId])
+            MMDSP_loadedSection(
+                    coreId, memId,
+                    privateMemories[memId]);
+    }
+}
+
+void cm_ELF_FreeInstance(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY],
+        t_memory_handle privateMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_memory_id memId;
+
+    if(privateMemories == NULL)
+        return;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        if(privateMemories[memId] != INVALID_MEMORY_HANDLE && privateMemories[memId] != sharedMemories[memId])
+        {
+            MMDSP_unloadedSection(coreId, memId, privateMemories[memId]);
+            cm_DM_Free(privateMemories[memId], TRUE);
+        }
+    }
+}
+
+void cm_ELF_FreeTemplate(
+        t_nmf_core_id coreId,
+        t_memory_handle sharedMemories[NUMBER_OF_MMDSP_MEMORY])
+{
+    t_memory_id memId;
+
+    if(sharedMemories == NULL)
+        return;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        if(sharedMemories[memId] != INVALID_MEMORY_HANDLE)
+        {
+            MMDSP_unloadedSection(coreId, memId, sharedMemories[memId]);
+            cm_DM_Free(sharedMemories[memId], TRUE);
+        }
+    }
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elfmmdsp.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elfmmdsp.c
new file mode 100644
index 00000000000..5f6641b188d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elfmmdsp.c
@@ -0,0 +1,575 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/elf/inc/mmdsp.h>
+#include <cm/engine/elf/inc/bfd.h>
+#include <cm/engine/elf/inc/mpcal.h>
+
+#include <cm/engine/component/inc/initializer.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/utils/inc/swap.h>
+#include <cm/engine/trace/inc/trace.h>
+
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+
+static const t_elfmemory mmdspMemories[NUMBER_OF_MMDSP_MEMORY] = {
+    {0,  SDRAM_CODE,      SDRAMTEXT_BASE_ADDR,        CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_CODE, 8, 8, "SDRAM_CODE"},  /* 0: Program memory */
+    {1,  INTERNAL_XRAM24, 0,                          CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 4, "XROM"},        /* 1: Internal X memory */
+    {2,  INTERNAL_YRAM24, 0,                          CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 4, "YROM"},        /* 2: Y memory */
+    {3,  SDRAM_EXT24,     SDRAMMEM24_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 4, "SDR0M24"},     /* 5: SDRAM24 */
+    {4,  SDRAM_EXT16,     SDRAMMEM16_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 2, "SDROM16"},     /* 6: SDRAM16 */
+    {5,  ESRAM_EXT24,     ESRAMMEM24_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 4, "ESROM24"},     /* 8: ESRAM24 */
+    {6,  ESRAM_EXT16,     ESRAMMEM16_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_DATA, 3, 2, "ESROM16"},     /* 9: ESRAM16 */
+    {7,  ESRAM_CODE,      ESRAMTEXT_BASE_ADDR,        CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_CODE, 8, 8, "ESRAM_CODE"},  /*10: ESRAM code */
+    {8,  INTERNAL_XRAM24, 0,                          CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 4, "XRAM"},        /* 1: Internal X memory */
+    {9,  INTERNAL_YRAM24, 0,                          CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 4, "YRAM"},        /* 2: Y memory */
+    {10, SDRAM_EXT24,     SDRAMMEM24_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 4, "SDRAM24"},     /* 5: SDRAM24 */
+    {11, SDRAM_EXT16,     SDRAMMEM16_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 2, "SDRAM16"},     /* 6: SDRAM16 */
+    {12, ESRAM_EXT24,     ESRAMMEM24_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 4, "ESRAM24"},     /* 8: ESRAM24 */
+    {13, ESRAM_EXT16,     ESRAMMEM16_BASE_ADDR,       CM_MM_ALIGN_2WORDS, MEM_PRIVATE,  MEM_DATA, 3, 2, "ESRAM16"},     /* 9: ESRAM16 */
+    {14, LOCKED_CODE,     SDRAMTEXT_BASE_ADDR,        CM_MM_ALIGN_2WORDS, MEM_SHARABLE, MEM_CODE, 8, 8, "LOCKED_CODE"},  /*  : .locked */
+};
+
+#define MAX_ELFSECTIONNAME  10
+struct memoryMapping {
+    char        *elfSectionName;
+    t_uint32    memoryIndex[MEM_FOR_LAST]; // memoryIndex[t_instance_property]
+};
+
+static const struct memoryMapping mappingmem0[] = {
+        {"mem0.0",  {0,  0}},
+        {"mem0.1",  {0,  0}},
+        {"mem0.2",  {0,  0}}
+};
+static const struct memoryMapping mappingmem10 =
+        {"mem10",   {7,  7}};
+static const struct memoryMapping mappinglocked =
+        {".locked", {14,  14}};
+static const struct memoryMapping mappingmem1[] = {
+        {"",  {0xff,  0xff}},
+        {"mem1.1",  {1,  1}},
+        {"mem1.2",  {8,  1}},
+        {"mem1.3",  {1,  1}},
+        {"mem1.4",  {8,  1}},
+        {"mem1.stack", {8, 1}}
+};
+static const struct memoryMapping mappingmem2[] = {
+        {"",  {0xff,  0xff}},
+        {"mem2.1",  {2,  2}},
+        {"mem2.2",  {9,  2}},
+        {"mem2.3",  {2,  2}},
+        {"mem2.4",  {9,  2}}
+};
+static const struct memoryMapping mappingmem5[] = {
+        {"",  {0xff,  0xff}},
+        {"mem5.1",  {3,  3}},
+        {"mem5.2",  {10, 3}},
+        {"mem5.3",  {3,  3}},
+        {"mem5.4",  {10, 3}}
+};
+static const struct memoryMapping mappingmem6[] = {
+        {"",  {0xff,  0xff}},
+        {"mem6.1",  {4,  4}},
+        {"mem6.2",  {11, 4}},
+        {"mem6.3",  {4,  4}},
+        {"mem6.4",  {11, 4}}
+};
+static const struct memoryMapping mappingmem8[] = {
+        {"",  {0xff,  0xff}},
+        {"mem8.1",  {5,  5}},
+        {"mem8.2",  {12, 5}},
+        {"mem8.3",  {5,  5}},
+        {"mem8.4",  {12, 5}}
+};
+static const struct memoryMapping mappingmem9[] = {
+        {"",  {0xff,  0xff}},
+        {"mem9.1",  {6,  6}},
+        {"mem9.2",  {13, 6}},
+        {"mem9.3",  {6,  6}},
+        {"mem9.4",  {13, 6}}
+};
+
+static const struct {
+    const struct memoryMapping* mapping;
+    unsigned int        number;
+} hashMappings[10] = {
+        {mappingmem0, sizeof(mappingmem0) / sizeof(mappingmem0[0])},
+        {mappingmem1, sizeof(mappingmem1) / sizeof(mappingmem1[0])},
+        {mappingmem2, sizeof(mappingmem2) / sizeof(mappingmem2[0])},
+        {0x0, 0},
+        {0x0, 0},
+        {mappingmem5, sizeof(mappingmem5) / sizeof(mappingmem5[0])},
+        {mappingmem6, sizeof(mappingmem6) / sizeof(mappingmem6[0])},
+        {0x0, 0},
+        {mappingmem8, sizeof(mappingmem8) / sizeof(mappingmem8[0])},
+        {mappingmem9, sizeof(mappingmem9) / sizeof(mappingmem9[0])},
+};
+
+const t_elfmemory* MMDSP_getMappingById(t_memory_id memId)
+{
+    return &mmdspMemories[memId];
+}
+
+const t_elfmemory* MMDSP_getMappingByName(const char* sectionName, t_instance_property property)
+{
+    if(sectionName[0] == 'm' && sectionName[1] == 'e' && sectionName[2] == 'm')
+    {
+        if(sectionName[4] == '.')
+        {
+            if(sectionName[5] >= '0' && sectionName[5] <= '9')
+            {
+                if(sectionName[3] >= '0' && sectionName[3] <= '9')
+                {
+                    unsigned int m, sm;
+
+                    m = sectionName[3] - '0';
+                    sm = sectionName[5] - '0';
+                    if(sm < hashMappings[m].number)
+                        return &mmdspMemories[hashMappings[m].mapping[sm].memoryIndex[property]];
+                }
+            } else if(sectionName[3] == '1' && sectionName[5] == 's')
+                return &mmdspMemories[mappingmem1[5].memoryIndex[property]];
+        }
+        else if(sectionName[3] == '1' && sectionName[4] == '0')
+            return &mmdspMemories[mappingmem10.memoryIndex[property]];
+    }
+    else if(sectionName[0] == '.' && sectionName[1] == 'l' && sectionName[2] == 'o' && sectionName[3] == 'c' &&
+            sectionName[4] == 'k' && sectionName[5] == 'e' && sectionName[6] == 'd')
+    {
+        return &mmdspMemories[mappinglocked.memoryIndex[property]];
+    }
+
+    return NULL;
+}
+
+void MMDSP_serializeMemories(t_instance_property property,
+        const t_elfmemory** codeMemory, const t_elfmemory** thisMemory) {
+    // Return meory reference
+    *codeMemory = &mmdspMemories[0];
+    if(property == MEM_FOR_SINGLETON)
+    {
+        *thisMemory = &mmdspMemories[1];
+    }
+    else
+    {
+        *thisMemory = &mmdspMemories[8];
+    }
+}
+
+void MMDSP_copyCode(t_uint64 * remoteAddr64, const char* origAddr, int nb)
+{
+    int m;
+
+    // Linux allow unaligned access
+#ifdef LINUX
+    t_uint64  *origAddr64 = (t_uint64*)origAddr;
+#else
+    __packed t_uint64  *origAddr64 = (__packed t_uint64*)origAddr;
+#endif
+
+    for (m = 0; m < nb; m += 8)
+    {
+        *remoteAddr64++ = swap64(*origAddr64++);
+    }
+}
+
+void MMDSP_copyData24(t_uint32 * remoteAddr32, const char* origAddr, int nb)
+{
+    int m;
+
+    for (m = 0; m < nb; m+=4)
+    {
+        t_uint32 value1;
+
+        value1  = (*origAddr++ << 16);
+        value1 |= (*origAddr++ << 8);
+        value1 |= (*origAddr++ << 0);
+        *remoteAddr32++ = value1;
+    }
+}
+
+void MMDSP_copyData16(t_uint16 * remoteAddr16, const char* origAddr, int nb)
+{
+    int m;
+
+    for (m = 0; m < nb; m+=2)
+    {
+        t_uint16 value1;
+
+        origAddr++; // Skip this byte (which is put in elf file for historical reason)
+        value1 = (*origAddr++ << 8);
+        value1 |= (*origAddr++ << 0);
+        *remoteAddr16++ = value1;
+    }
+}
+
+#if 0
+__asm void MMDSP_copyCode(void* dst, const void* src, int nb)
+{
+    PUSH     {r4-r8, lr}
+    SUBS     r2,r2,#0x20
+    BCC      l4
+
+l5
+    SETEND   BE
+    LDR      r4, [r1], #0x4
+    LDR      r3, [r1], #0x4
+    LDR      r6, [r1], #0x4
+    LDR      r5, [r1], #0x4
+    LDR      r8, [r1], #0x4
+    LDR      r7, [r1], #0x4
+    LDR      lr, [r1], #0x4
+    LDR      r12, [r1], #0x4
+
+    SETEND   LE
+    STM      r0!,{r3-r8,r12, lr}
+    SUBS     r2,r2,#0x20
+    BCS      l5
+
+l4
+    LSLS     r12,r2,#28
+
+    SETEND   BE
+    LDRCS    r4, [r1], #0x4
+    LDRCS    r3, [r1], #0x4
+    LDRCS    r6, [r1], #0x4
+    LDRCS    r5, [r1], #0x4
+    SETEND   LE
+    STMCS    r0!,{r3-r6}
+
+    SETEND   BE
+    LDRMI    r4, [r1], #0x4
+    LDRMI    r3, [r1], #0x4
+    SETEND   LE
+    STMMI    r0!,{r3-r4}
+
+    POP      {r4-r8, pc}
+}
+#endif
+
+#ifdef LINUX
+static void PLD5(int r)
+{
+    asm volatile (
+	    "PLD      [r0, #0x20]     \n\t"
+	    "PLD      [r0, #0x40]     \n\t"
+	    "PLD      [r0, #0x60]     \n\t"
+	    "PLD      [r0, #0x80]     \n\t"
+	    "PLD      [r0, #0xA0]" );
+}
+
+static void PLD1(int r)
+{
+    asm volatile (
+	    "PLD      [r0, #0xC0]" );
+}
+#else /* Symbian, Think -> We assume ARMCC */
+static __asm void PLD5(int r)
+{
+    PLD      [r0, #0x20]
+    PLD      [r0, #0x40]
+    PLD      [r0, #0x60]
+    PLD      [r0, #0x80]
+    PLD      [r0, #0xA0]
+
+              bx lr
+}
+
+static __asm void PLD1(int r)
+{
+    PLD      [r0, #0xC0]
+
+              bx lr
+}
+#endif
+
+#if 0
+__asm void COPY(void* dst, const void* src, int nb)
+{
+    PUSH     {r4-r8, lr}
+    SUBS     r2,r2,#0x20
+    BCC      l4a
+    PLD      [r1, #0x20]
+    PLD      [r1, #0x40]
+    PLD      [r1, #0x60]
+    PLD      [r1, #0x80]
+    PLD      [r1, #0xA0]
+
+l5a
+    PLD      [r1, #0xC0]
+    LDM      r1!,{r3-r8,r12,lr}
+    STM      r0!,{r3-r8,r12,lr}
+    SUBS     r2,r2,#0x20
+    BCS      l5a
+
+l4a
+    LSLS     r12,r2,#28
+    LDMCS    r1!,{r3,r4,r12,lr}
+    STMCS    r0!,{r3,r4,r12,lr}
+    LDMMI    r1!,{r3,r4}
+    STMMI    r0!,{r3,r4}
+    POP      {r4-r8,lr}
+    LSLS     r12,r2,#30
+    LDRCS    r3,[r1],#4
+    STRCS    r3,[r0],#4
+    BXEQ     lr
+l6b
+    LSLS     r2,r2,#31
+    LDRHCS   r3,[r1],#2
+    LDRBMI   r2,[r1],#1
+    STRHCS   r3,[r0],#2
+    STRBMI   r2,[r0],#1
+    BX       lr
+}
+#endif
+
+
+void MMDSP_copySection(t_uint32 origAddr, t_uint32 remoteAddr, t_uint32 sizeInByte) {
+    t_uint32 endAddr = remoteAddr + sizeInByte;
+
+    PLD5(origAddr);
+
+    // Align on 32bits
+    if((remoteAddr & 0x3) != 0)
+    {
+        *(t_uint16*)remoteAddr = *(t_uint16*)origAddr;
+        remoteAddr += sizeof(t_uint16);
+        origAddr += sizeof(t_uint16);
+    }
+
+    // Align on 64bits
+    if((remoteAddr & 0x7) != 0 && (remoteAddr <= endAddr - sizeof(t_uint32)))
+    {
+        *(t_uint32*)remoteAddr = *(t_uint32*)origAddr;
+        remoteAddr += sizeof(t_uint32);
+        origAddr += sizeof(t_uint32);
+    }
+
+    // 64bits burst access
+    for(; remoteAddr <= endAddr - sizeof(t_uint64); remoteAddr += sizeof(t_uint64), origAddr += sizeof(t_uint64))
+    {
+        PLD1(origAddr);
+        *(volatile t_uint64*)remoteAddr = *(t_uint64*)origAddr;
+    }
+
+    // Remain 32bits access
+    if(remoteAddr <= endAddr - sizeof(t_uint32))
+    {
+        *(t_uint32*)remoteAddr = *(t_uint32*)origAddr;
+        remoteAddr += sizeof(t_uint32);
+        origAddr += sizeof(t_uint32);
+    }
+
+    // Remain 16bits access
+    if(remoteAddr <= endAddr - sizeof(t_uint16))
+        *(t_uint16*)remoteAddr = *(t_uint16*)origAddr;
+}
+
+
+void MMDSP_bzeroSection(t_uint32 remoteAddr, t_uint32 sizeInByte) {
+    t_uint32 endAddr = remoteAddr + sizeInByte;
+
+    // Align on 32bits
+    if((remoteAddr & 0x3) != 0)
+    {
+        *(t_uint16*)remoteAddr = 0;
+        remoteAddr += sizeof(t_uint16);
+    }
+
+    // Align on 64bits
+    if((remoteAddr & 0x7) != 0 && (remoteAddr <= endAddr - sizeof(t_uint32)))
+    {
+        *(t_uint32*)remoteAddr = 0;
+        remoteAddr += sizeof(t_uint32);
+    }
+
+    // 64bits burst access
+    for(; remoteAddr <= endAddr - sizeof(t_uint64); remoteAddr += sizeof(t_uint64))
+        *(volatile t_uint64*)remoteAddr = 0ULL;
+
+    // Remain 32bits access
+    if(remoteAddr <= endAddr - sizeof(t_uint32))
+    {
+        *(t_uint32*)remoteAddr = 0;
+        remoteAddr += sizeof(t_uint32);
+    }
+
+    // Remain 16bits access
+    if(remoteAddr <= endAddr - sizeof(t_uint16))
+        *(t_uint16*)remoteAddr = 0;
+}
+
+void MMDSP_loadedSection(t_nmf_core_id coreId, t_memory_id memId, t_memory_handle handle)
+{
+    if(mmdspMemories[memId].purpose == MEM_CODE)
+    {
+        OSAL_CleanDCache(cm_DSP_GetHostLogicalAddress(handle), cm_MM_GetSize(handle));
+    }
+
+    if(memId == LOCKED_CODE)
+    {
+        t_uint32 DspAddress, DspSize;
+
+        cm_DSP_GetDspMemoryHandleSize(handle, &DspSize);
+        cm_DSP_GetDspAddress(handle, &DspAddress);
+
+        cm_COMP_InstructionCacheLock(coreId, DspAddress,  DspSize);
+    }
+}
+
+void MMDSP_unloadedSection(t_nmf_core_id coreId, t_memory_id memId, t_memory_handle handle)
+{
+    if(memId == LOCKED_CODE)
+    {
+        t_uint32 DspAddress, DspSize;
+
+        cm_DSP_GetDspMemoryHandleSize(handle, &DspSize);
+        cm_DSP_GetDspAddress(handle, &DspAddress);
+
+        cm_COMP_InstructionCacheUnlock(coreId, DspAddress, DspSize);
+    }
+
+}
+
+static struct reloc_howto_struct elf64_mmdsp_howto_table[] =
+{
+  HOWTO (R_MMDSP_IMM20_16,  /* type */
+     0,         /* rightshift */
+     4,         /* size (0 = byte, 1 = short, 2 = long) */
+     16,            /* bitsize */
+     FALSE,         /* pc_relative */
+     8,         /* bitpos */
+     complain_overflow_dont, /* complain_on_overflow */
+     0x0, /* special_function */
+     "R_MMDSP_IMM20_16",    /* name */
+     FALSE,         /* partial_inplace */
+     0x0,           /* src_mask */
+     0x0000000000ffff00,    /* dst_mask */
+     FALSE),        /* pcrel_offset */
+
+    /* A 4-bit absolute relocation for splitted 20 bits immediate, shifted by 56 */
+
+  HOWTO (R_MMDSP_IMM20_4,   /* type */
+     16,            /* rightshift */
+     4,         /* size (0 = byte, 1 = short, 2 = long) */
+     4,         /* bitsize */
+     FALSE,         /* pc_relative */
+     56,            /* bitpos */
+     complain_overflow_dont, /* complain_on_overflow */
+     0x0, /* special_function */
+     "R_MMDSP_IMM20_4", /* name */
+     FALSE,         /* partial_inplace */
+     0x0,           /* src_mask */
+     0x0f00000000000000LL,  /* dst_mask */
+     FALSE),        /* pcrel_offset */
+
+  HOWTO (R_MMDSP_24,           /* type */
+        0,             /* rightshift */
+        2,             /* size (0 = byte, 1 = short, 2 = long) */
+        24,                /* bitsize */
+        FALSE,             /* pc_relative */
+        0,             /* bitpos */
+        complain_overflow_bitfield,    /* complain_on_overflow */
+        0x0,     /* special_function */
+        "R_MMDSP_24",          /* name */
+        FALSE,             /* partial_inplace */
+        0x0,            /* src_mask */
+        0xffffffff,            /* dst_mask */
+        FALSE),            /* pcrel_offset */
+
+  HOWTO (R_MMDSP_IMM16,       /* type */
+             0,         /* rightshift */
+             4,         /* size (0 = byte, 1 = short, 2 = long) */
+             16,            /* bitsize */
+             FALSE,         /* pc_relative */
+             8,         /* bitpos */
+             complain_overflow_bitfield, /* complain_on_overflow */
+             0x0, /* special_function */
+             "R_MMDSP_IMM16",   /* name */
+             FALSE,         /* partial_inplace */
+             0x0,               /* src_mask */
+             0x0000000000ffff00,    /* dst_mask */
+             FALSE),        /* pcrel_offset */
+};
+
+static const char* lastInPlaceAddr = 0;
+static long long lastInPlaceValue;
+
+void MMDSP_performRelocation(
+        t_uint32 type,
+        const char* symbol_name,
+        t_uint32 symbol_addr,
+        char* reloc_addr,
+        const char* inPlaceAddr,
+        t_uint32 reloc_offset) {
+    int i;
+
+    for(i = 0; i < sizeof(elf64_mmdsp_howto_table) / sizeof(elf64_mmdsp_howto_table[0]); i++)
+    {
+        struct reloc_howto_struct* howto = &elf64_mmdsp_howto_table[i];
+        if(howto->type == type)
+        {
+            t_uint64 relocation;
+
+            LOG_INTERNAL(2, "reloc '%s:0x%x' type %s at 0x%x (0x%x)\n",
+                    symbol_name ? symbol_name : "??", symbol_addr,
+                    howto->name,
+                    reloc_offset, reloc_addr, 0);
+
+            relocation = symbol_addr;
+
+            if (howto->pc_relative) {
+                // Not handle yet
+            }
+
+            if (howto->complain_on_overflow != complain_overflow_dont) {
+                // Not handle yet
+            }
+
+            relocation >>= howto->rightshift;
+
+            relocation <<= howto->bitpos;
+
+#define DOIT(x) \
+    x = ( (x & ~howto->dst_mask) | (((x & howto->src_mask) +  relocation) & howto->dst_mask))
+
+            switch (howto->size) {
+            case 2: {
+                long x = *(long*)inPlaceAddr;
+
+               // CM_ASSERT(*(long*)inPlaceAddr == *(long*)reloc_addr);
+
+                DOIT (x);
+                *(long*)reloc_addr = x;
+            }
+            break;
+            case 4: {
+                long long x;
+                if(lastInPlaceAddr == inPlaceAddr)
+                {
+                    x = lastInPlaceValue;
+                }
+                else
+                {
+                   //  CM_ASSERT(*(__packed long long*)inPlaceAddr == *(long long*)reloc_addr);
+                    x = *(long long*)inPlaceAddr;
+                    lastInPlaceAddr = inPlaceAddr;
+                }
+
+                DOIT (x);
+                *(long long*)reloc_addr = lastInPlaceValue = x;
+            }
+            break;
+            default:
+                CM_ASSERT(0);
+            }
+
+            return;
+        }
+    }
+
+    ERROR("Relocation type %d not supported for '%s'\n", type, symbol_name, 0, 0, 0, 0);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elfrelocate.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elfrelocate.c
new file mode 100644
index 00000000000..b08ac6a361e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elfrelocate.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/elf/inc/bfd.h>
+#include <cm/engine/elf/inc/mpcal.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/string.h>
+
+t_cm_error cm_ELF_relocateSharedSegments(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        void                        *cbContext)
+{
+    return ELF64_relocateSegments(
+        memories,
+        elfhandle,
+        MEM_SHARABLE,
+        cbContext);
+}
+
+t_cm_error cm_ELF_relocatePrivateSegments(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        void                        *cbContext)
+{
+    return ELF64_relocateSegments(
+        memories,
+        elfhandle,
+        MEM_PRIVATE,
+        cbContext);
+}
+
+void cm_ELF_performRelocation(
+        t_uint32 type,
+        const char* symbol_name,
+        t_uint32 symbol_addr,
+        char* reloc_addr)
+{
+    MMDSP_performRelocation(
+                        type,
+                        symbol_name,
+                        symbol_addr,
+                        reloc_addr,
+                        reloc_addr,
+                        0xBEEF);
+
+    OSAL_CleanDCache((t_uint32)reloc_addr, 8);
+}
+
+t_cm_error cm_ELF_GetMemory(
+        t_elfdescription            *elf,
+        t_tmp_elfdescription        *elftmp,
+        t_uint32                    address,
+        t_memory_purpose            purpose,
+        t_memory_reference          *memory) {
+    t_memory_id memId;
+
+    for(memId = 0; memId < NUMBER_OF_MMDSP_MEMORY; memId++)
+    {
+        const t_elfmemory* mem = MMDSP_getMappingById(memId);
+
+        if(mem->purpose == purpose &&    // Memory correspond
+                elf->segments[mem->id].sumSize != 0 && // Segment allocated
+                (elf->segments[mem->id].mpcAddr <= address) &&
+                (address < elf->segments[mem->id].mpcAddr + elf->segments[mem->id].sumSize / mem->fileEntSize)) {
+            memory->memory = mem;
+            memory->offset = address - elf->segments[mem->id].mpcAddr;
+            return CM_OK;
+        }
+    }
+
+    ERROR("Memory %x,%d not found\n", address, purpose, 0, 0, 0, 0);
+    return CM_INVALID_ELF_FILE;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/elfxx.c b/drivers/staging/nmf-cm/cm/engine/elf/src/elfxx.c
new file mode 100644
index 00000000000..4a2976a6bc1
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/elfxx.c
@@ -0,0 +1,591 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/elf/inc/mpcal.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/utils/inc/mem.h>
+
+
+static t_uint32 max(t_uint32 a, t_uint32 b)
+{
+    return (a >= b) ? a : b;
+}
+/*
+static t_uint32 min(t_uint32 a, t_uint32 b)
+{
+    return (a <= b) ? a : b;
+}
+*/
+
+struct XXrelocation
+{
+    t_uint32            st_value;
+    ElfXX_Half          st_shndx;
+    Elf64_Sxword        r_addend;
+    t_uint32            OffsetInElf;
+    t_uint32            type;
+
+    t_dup_char          symbol_name;    // Valid only if st_shndx == SHN_UNDEF
+};
+
+struct XXSection {
+    ElfXX_Word      sh_type;        /* Section type */
+    t_uint32        sh_size;        /* Section size in bytes */
+    ElfXX_Word      sh_info;        /* Additional section information */
+    ElfXX_Word      sh_link;        /* Link to another section */
+    t_uint32        sh_addralign;   /* Some sections have address alignment constraints */
+    t_uint32        sh_addr;        /* Section addr */
+    ElfXX_Xword     sh_flags;       /* Section flags */
+
+    const char      *data;
+    t_uint32        trueDataSize;   /* Valid if different from sh_size */
+    const char      *sectionName;
+
+    t_uint32            offsetInSegment;
+    const t_elfmemory   *meminfo;
+
+    t_uint32                relocationNumber;
+    struct XXrelocation     *relocations;
+};
+
+struct XXElf {
+    t_uint32            e_shnum;
+    struct XXSection                sectionss[1];
+};
+
+t_cm_error ELF64_LoadComponent(
+        t_uint16                    e_machine,
+        const char                  *elfdata,
+        t_elfdescription            **elfhandlePtr,
+        t_tmp_elfdescription        *elftmp)
+{
+    t_elfdescription *elfhandle;
+    const ElfXX_Ehdr *header = (ElfXX_Ehdr*)elfdata;
+    const ElfXX_Shdr *sections;
+    const char *strings;
+    struct XXElf* ELF;
+    int i, nb;
+
+    elftmp->elfdata = elfdata;
+
+    /* Sanity check */
+    if (swapHalf(header->e_machine) != e_machine)
+    {
+        ERROR("This is not a executable for such MPC\n", 0, 0, 0, 0, 0, 0);
+        return CM_INVALID_ELF_FILE;
+    }
+
+    // Cache elf file informations
+    nb = swapHalf(header->e_shnum);
+    elftmp->isExecutable = (swapHalf(header->e_type) == ET_EXEC);
+
+    elfhandle = (t_elfdescription*)OSAL_Alloc_Zero(
+            sizeof(t_elfdescription) + sizeof(struct XXElf) + sizeof(struct XXSection) * (nb - 1));
+    if(elfhandle == NULL)
+        return CM_NO_MORE_MEMORY;
+
+    ELF = elfhandle->ELF = (struct XXElf*)(elfhandle + 1);
+
+    ELF->e_shnum = nb;
+
+    sections = (ElfXX_Shdr*)&elfdata[swapXword(header->e_shoff)];
+    // Compute and swap section infromation
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ELF->sectionss[i].sh_type = swapWord(sections[i].sh_type);
+        ELF->sectionss[i].sh_info = swapWord(sections[i].sh_info);
+        ELF->sectionss[i].sh_link = swapWord(sections[i].sh_link);
+        ELF->sectionss[i].sh_size = (t_uint32)swapXword(sections[i].sh_size);
+        ELF->sectionss[i].sh_addralign = (t_uint32)swapXword(sections[i].sh_addralign);
+        ELF->sectionss[i].sh_addr = (t_uint32)swapXword(sections[i].sh_addr);
+        ELF->sectionss[i].sh_flags = swapXword(sections[i].sh_flags);
+
+        elftmp->sectionData[i] = &elfdata[(t_uint32)swapXword(sections[i].sh_offset)];
+    }
+
+    /*
+     * search nmf_segment
+     */
+    strings = elftmp->sectionData[swapHalf(header->e_shstrndx)];
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ELF->sectionss[i].sectionName = &strings[swapWord(sections[i].sh_name)];
+
+        // Found nmf_segment to see if it's
+        if(cm_StringCompare("nmf_segment", ELF->sectionss[i].sectionName, 11) == 0) {
+            elftmp->nmfSectionIndex = i;
+            elftmp->elfheader = (const t_elf_component_header*)elftmp->sectionData[i];
+        }
+    }
+
+    if(elftmp->nmfSectionIndex == 0)
+    {
+        ERROR("This is not a NMF component\n", 0, 0, 0, 0, 0, 0);
+        goto invalid;
+    }
+
+    /*
+     * Determine component type
+     */
+    elfhandle->magicNumber = swap32(elftmp->elfheader->magic);
+    switch(elfhandle->magicNumber) {
+    case MAGIC_COMPONENT:
+        elfhandle->instanceProperty = MEM_FOR_MULTIINSTANCE;
+        break;
+    case MAGIC_SINGLETON:
+    case MAGIC_FIRMWARE:
+        elfhandle->instanceProperty = MEM_FOR_SINGLETON;
+        break;
+    }
+
+    // Copy content
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ELF->sectionss[i].meminfo = MMDSP_getMappingByName(
+                ELF->sectionss[i].sectionName,
+                elfhandle->instanceProperty);
+
+        if(ELF->sectionss[i].meminfo != NULL)
+            ELF->sectionss[i].trueDataSize = (ELF->sectionss[i].sh_size / ELF->sectionss[i].meminfo->fileEntSize) * ELF->sectionss[i].meminfo->memEntSize;
+
+        if(ELF->sectionss[i].sh_size != 0 &&
+                ELF->sectionss[i].sh_type == SHT_PROGBITS &&
+                (ELF->sectionss[i].sh_flags & SHF_ALLOC) != 0)
+        {
+            const char* elfAddr = elftmp->sectionData[i];
+
+            ELF->sectionss[i].data = OSAL_Alloc(ELF->sectionss[i].trueDataSize);
+            if(ELF->sectionss[i].data == NULL)
+                goto oom;
+
+            if(ELF->sectionss[i].meminfo->purpose == MEM_CODE)
+            {
+                MMDSP_copyCode(
+                        (t_uint64*)ELF->sectionss[i].data,
+                        elfAddr,
+                        ELF->sectionss[i].trueDataSize);
+            }
+            else if(ELF->sectionss[i].meminfo->purpose == MEM_DATA &&
+                    // Always 3 for data ELF->sectionss[i].meminfo->fileEntSize == 3 &&
+                    ELF->sectionss[i].meminfo->memEntSize == 4)
+            {
+                MMDSP_copyData24(
+                        (t_uint32*)ELF->sectionss[i].data,
+                        elfAddr,
+                        ELF->sectionss[i].trueDataSize);
+            }
+            else if(ELF->sectionss[i].meminfo->purpose == MEM_DATA &&
+                    // Always 3 for data ELF->sectionss[i].meminfo->fileEntSize == 3 &&
+                    ELF->sectionss[i].meminfo->memEntSize == 2)
+            {
+                MMDSP_copyData16(
+                        (t_uint16*)ELF->sectionss[i].data,
+                        elfAddr,
+                        ELF->sectionss[i].trueDataSize);
+            }
+            else
+                CM_ASSERT(0);
+        }
+    }
+
+    // Copy relocation
+    // Loop on all relocation section
+    for(i=0; i < ELF->e_shnum; i++)
+    {
+        int sh_info;
+
+        // Does this section is a relocation table (only RELA supported)
+        if((ELF->sectionss[i].sh_type != SHT_RELA) ||
+                ELF->sectionss[i].sh_size == 0) continue;
+
+        // Copy only relocation for loaded section
+        sh_info = ELF->sectionss[i].sh_info;
+        if(ELF->sectionss[sh_info].meminfo != NULL)
+        {
+            const ElfXX_Sym* symtab;
+            const char* strtab;
+            ElfXX_Rela* rel_start;
+            int n;
+
+            ELF->sectionss[sh_info].relocationNumber = ELF->sectionss[i].sh_size / sizeof(ElfXX_Rela);
+            ELF->sectionss[sh_info].relocations = (struct XXrelocation*)OSAL_Alloc_Zero(sizeof(struct XXrelocation) * ELF->sectionss[sh_info].relocationNumber);
+            if(ELF->sectionss[sh_info].relocations == NULL)
+                goto oom;
+
+            symtab = (ElfXX_Sym *)elftmp->sectionData[ELF->sectionss[i].sh_link];
+            strtab = elftmp->sectionData[ELF->sectionss[ELF->sectionss[i].sh_link].sh_link];
+            rel_start = (ElfXX_Rela*)elftmp->sectionData[i];
+            for(n = 0; n < ELF->sectionss[sh_info].relocationNumber; n++, rel_start++)
+            {
+                struct XXrelocation* relocation = &ELF->sectionss[sh_info].relocations[n];
+                ElfXX_Xword r_info = swapXword(rel_start->r_info);
+                int strtab_index = ELFXX_R_SYM(r_info);
+                const char* symbol_name = &strtab[swapWord(symtab[strtab_index].st_name)];
+
+                relocation->st_shndx = swapHalf(symtab[strtab_index].st_shndx);
+                relocation->st_value = (t_uint32)swapXword(symtab[strtab_index].st_value);
+                relocation->r_addend = swapXword(rel_start->r_addend);
+                relocation->OffsetInElf = (t_uint32)swapXword(rel_start->r_offset) / ELF->sectionss[sh_info].meminfo->fileEntSize;
+                relocation->type = ELFXX_R_TYPE(r_info);
+
+                switch(relocation->st_shndx) {
+                case SHN_UNDEF:
+                    relocation->symbol_name = cm_StringDuplicate(symbol_name + 1); /* Remove '_' prefix */
+                    if(relocation->symbol_name == NULL)
+                        goto oom;
+                    break;
+                case SHN_COMMON:
+                    ERROR("SHN_COMMON not handle for %s\n", symbol_name, 0, 0, 0, 0, 0);
+                    goto invalid;
+                }
+            }
+        }
+    }
+
+    *elfhandlePtr = elfhandle;
+    return CM_OK;
+invalid:
+    ELF64_UnloadComponent(elfhandle);
+    return CM_INVALID_ELF_FILE;
+oom:
+    ELF64_UnloadComponent(elfhandle);
+    return CM_NO_MORE_MEMORY;
+}
+
+t_cm_error ELF64_ComputeSegment(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp)
+{
+    struct XXElf* ELF = elfhandle->ELF;
+    int i;
+
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ELF->sectionss[i].offsetInSegment =  0xFFFFFFFF;
+
+        if(ELF->sectionss[i].sh_type == SHT_PROGBITS || ELF->sectionss[i].sh_type == SHT_NOBITS) {
+            // This is a loadable memory (memory size could be zero since we can have symbol on it)...
+            const t_elfmemory* meminfo = ELF->sectionss[i].meminfo;
+
+            if(meminfo != NULL) {
+                // Which correspond to MPC memory
+
+                if(elftmp->isExecutable)
+                {
+                    if(! elfhandle->segments[meminfo->id].sumSizeSetted)
+                    {
+                        CM_ASSERT(ELF->sectionss[i].sh_addr >= meminfo->startAddr * meminfo->fileEntSize);
+
+                        elfhandle->segments[meminfo->id].sumSizeSetted = TRUE;
+                        elfhandle->segments[meminfo->id].sumSize = ELF->sectionss[i].sh_addr - meminfo->startAddr * meminfo->fileEntSize;
+                    }
+                    else
+                        CM_ASSERT(elfhandle->segments[meminfo->id].sumSize == ELF->sectionss[i].sh_addr - meminfo->startAddr * meminfo->fileEntSize);
+                }
+                else
+                {
+                    while(elfhandle->segments[meminfo->id].sumSize % ELF->sectionss[i].sh_addralign != 0)
+                        elfhandle->segments[meminfo->id].sumSize++;
+                }
+
+                elfhandle->segments[meminfo->id].maxAlign = max(elfhandle->segments[meminfo->id].maxAlign, ELF->sectionss[i].sh_addralign);
+                ELF->sectionss[i].offsetInSegment = elfhandle->segments[meminfo->id].sumSize / meminfo->fileEntSize;
+                elfhandle->segments[meminfo->id].sumSize += ELF->sectionss[i].sh_size;
+            }
+         } else if(ELF->sectionss[i].sh_type == SHT_RELA && ELF->sectionss[i].sh_info == elftmp->nmfSectionIndex) {
+             int secsym = ELF->sectionss[i].sh_link;
+             elftmp->relaNmfSegment = (ElfXX_Rela*)elftmp->sectionData[i];
+             elftmp->relaNmfSegmentEnd = (ElfXX_Rela*)((t_uint32)elftmp->relaNmfSegment + ELF->sectionss[i].sh_size);
+             elftmp->relaNmfSegmentSymbols = (ElfXX_Sym*)elftmp->sectionData[secsym];
+             elftmp->relaNmfSegmentStrings = elftmp->sectionData[ELF->sectionss[secsym].sh_link];
+         }
+    }
+
+    return CM_OK;
+}
+
+void ELF64_UnloadComponent(
+        t_elfdescription            *elfhandle)
+{
+    struct XXElf* ELF = elfhandle->ELF;
+    int i, n;
+
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        if(ELF->sectionss[i].relocations != NULL)
+        {
+            for(n = 0; n < ELF->sectionss[i].relocationNumber; n++)
+                cm_StringRelease(ELF->sectionss[i].relocations[n].symbol_name);
+            OSAL_Free(ELF->sectionss[i].relocations);
+        }
+
+        OSAL_Free((void*)ELF->sectionss[i].data);
+    }
+    OSAL_Free(elfhandle);
+}
+
+t_cm_error ELF64_loadSegment(
+        t_elfdescription            *elfhandle,
+        t_memory_handle             *memory,
+        t_memory_property           property)
+{
+    struct XXElf* ELF = elfhandle->ELF;
+    int i;
+
+    /*
+     * Copy ELF data in this segment
+     */
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        const t_elfmemory* mapping = ELF->sectionss[i].meminfo;
+
+        if(mapping == NULL)
+            continue;
+        if((! (ELF->sectionss[i].sh_flags & SHF_ALLOC)) || (ELF->sectionss[i].sh_size == 0))
+            continue;
+
+        // This is a loadable memory ...
+        if(
+                (mapping->property == property && elfhandle->instanceProperty != MEM_FOR_SINGLETON) ||
+                (property == MEM_SHARABLE && elfhandle->instanceProperty == MEM_FOR_SINGLETON) )
+        {
+            // Where memory exist and waited share/private correspond
+            t_uint32 remoteData = elfhandle->segments[mapping->id].hostAddr +
+                    ELF->sectionss[i].offsetInSegment * mapping->memEntSize;
+
+            if(ELF->sectionss[i].sh_type != SHT_NOBITS)
+            {
+                LOG_INTERNAL(2, "loadSection(%s, 0x%x, 0x%x, 0x%08x)\n",
+                        ELF->sectionss[i].sectionName, remoteData, ELF->sectionss[i].trueDataSize,
+                        (t_uint32)ELF->sectionss[i].data, 0, 0);
+
+                MMDSP_copySection((t_uint32)ELF->sectionss[i].data, remoteData, ELF->sectionss[i].trueDataSize);
+            }
+            else
+            {
+                LOG_INTERNAL(2, "bzeroSection(%s, 0x%x, 0x%x)\n",
+                        ELF->sectionss[i].sectionName, remoteData, ELF->sectionss[i].trueDataSize, 0, 0, 0);
+
+                MMDSP_bzeroSection(remoteData, ELF->sectionss[i].trueDataSize);
+            }
+        }
+    }
+
+    return CM_OK;
+}
+
+
+
+static const t_elfmemory* getSectionAddress(
+        t_memory_handle *memories,
+        t_elfdescription *elfhandle,
+        t_uint32 sectionIdx,
+        t_uint32 *sectionOffset,
+        t_cm_logical_address *sectionAddr) {
+    struct XXElf* ELF = elfhandle->ELF;
+    const t_elfmemory* mapping = ELF->sectionss[sectionIdx].meminfo;
+
+    if(mapping != NULL) {
+        *sectionOffset = (elfhandle->segments[mapping->id].mpcAddr +
+                ELF->sectionss[sectionIdx].offsetInSegment);
+
+        *sectionAddr = (t_cm_logical_address)(elfhandle->segments[mapping->id].hostAddr +
+                ELF->sectionss[sectionIdx].offsetInSegment * mapping->memEntSize);
+    }
+
+    return mapping;
+}
+
+static t_uint32 getSymbolAddress(
+        t_memory_handle             *memories,
+        t_elfdescription            *elfhandle,
+        t_uint32                    symbolSectionIdx,
+        t_uint32                    symbolOffet) {
+    struct XXElf* ELF = elfhandle->ELF;
+    const t_elfmemory* mapping = ELF->sectionss[symbolSectionIdx].meminfo;
+
+    if(mapping == NULL)
+        return 0xFFFFFFFF;
+    // CM_ASSERT(elfhandle->segments[mapping->id].sumSize != 0);
+    // CM_ASSERT(elfhandle->sections[symbolSectionIdx].offsetInSegment != 0xFFFFFFFF);
+
+    return elfhandle->segments[mapping->id].mpcAddr +
+        ELF->sectionss[symbolSectionIdx].offsetInSegment +
+        symbolOffet;
+}
+
+#if 0
+t_bool ELFXX_getSymbolLocation(
+        const t_mpcal_memory    *mpcalmemory,
+        t_elfdescription        *elf,
+        char                    *symbolName,
+        const t_elfmemory       **memory,
+        t_uint32                *offset) {
+    const ElfXX_Ehdr *header = (ElfXX_Ehdr*)elf->elfdata;
+    const ElfXX_Shdr *sections = (ElfXX_Shdr*)&elf->elfdata[swapXword(header->e_shoff)];
+    const char *strings = &elf->elfdata[swapXword(sections[swapHalf(header->e_shstrndx)].sh_offset)];
+    int len = cm_StringLength(symbolName, 256); // TO BE FIXED
+    int i;
+
+    for(i = 0; i < ELF->e_shnum; i++)
+    {
+        ElfXX_Sym* symtab;
+        const char* strtab;
+        unsigned int size, j;
+
+        if(ELF->sectionss[i].sh_type != SHT_SYMTAB && ELF->sectionss[i].sh_type != SHT_DYNSYM) continue;
+
+        // Section is a symbol table
+        symtab = (ElfXX_Sym*)&elf->elfdata[swapXword(sections[i].sh_offset)];
+        strtab = &elf->elfdata[swapXword(sections[swapWord(sections[i].sh_link)].sh_offset)];
+        size = ELF->sectionss[i].sh_size / (unsigned int)swapXword(sections[i].sh_entsize);
+
+        for(j = 0; j < size; j++) {
+            const char* foundName = &strtab[swapWord(symtab[j].st_name)];
+
+            if(cm_StringCompare(symbolName, foundName, len) == 0) {
+                if(swapHalf(symtab[j].st_shndx) != SHN_UNDEF) {
+                    int sectionIdx = (int)swapHalf(symtab[j].st_shndx);
+                    ElfXX_Xword sh_flags = swapXword(sections[sectionIdx].sh_flags);
+
+                    *memory = mpcalmemory->getMappingByName(&strings[swapWord(sections[sectionIdx].sh_name)],
+                            sh_flags & SHF_WRITE ? MEM_RW : (sh_flags & SHF_EXECINSTR ? MEM_X : MEM_RO));
+                    *offset = (t_uint32)swapXword(symtab[j].st_value);
+
+                    return 1;
+                }
+            }
+        }
+    }
+    return 0;
+}
+#endif
+
+t_cm_error ELF64_relocateSegments(
+        t_memory_handle             *memories,
+        t_elfdescription            *elfhandle,
+        t_memory_property           property,
+        void                        *cbContext) {
+    struct XXElf* ELF = elfhandle->ELF;
+    int sec, n;
+
+    // Loop on all relocation section
+    for(sec=0; sec < ELF->e_shnum; sec++)
+    {
+        t_cm_logical_address sectionAddr = 0;
+        t_uint32 sectionOffset = 0;
+        const t_elfmemory* mapping;
+
+        if(ELF->sectionss[sec].relocations == NULL)
+            continue;
+
+        // Relocate only section in memory
+        mapping = getSectionAddress(memories,
+                elfhandle,
+                sec,
+                &sectionOffset,
+                &sectionAddr);
+        if(mapping == NULL)
+            continue;
+
+        if(
+                (mapping->property == property && elfhandle->instanceProperty != MEM_FOR_SINGLETON) ||
+                (property == MEM_SHARABLE && elfhandle->instanceProperty == MEM_FOR_SINGLETON) )
+        {
+            LOG_INTERNAL(2, "relocSection(%s)\n", ELF->sectionss[sec].sectionName, 0, 0, 0, 0, 0);
+
+            for(n = 0; n < ELF->sectionss[sec].relocationNumber; n++)
+            {
+                struct XXrelocation* relocation = &ELF->sectionss[sec].relocations[n];
+                t_uint32 symbol_addr;
+                char* relocAddr = (char*)(sectionAddr + relocation->OffsetInElf * mapping->memEntSize);
+
+                switch(relocation->st_shndx) {
+                case SHN_ABS:              // Absolute external reference
+                    symbol_addr = relocation->st_value;
+                    break;
+                case SHN_UNDEF:            // External reference
+                     // LOG_INTERNAL(0, "cm_resolvSymbol(%d, %s)\n", relocation->type, relocation->symbol_name, 0,0, 0, 0);
+                    symbol_addr = cm_resolvSymbol(cbContext,
+                            relocation->type,
+                            relocation->symbol_name,
+                            relocAddr);
+                    if(symbol_addr == 0x0) { // Not defined symbol
+                        ERROR("Symbol %s not found\n", relocation->symbol_name, 0, 0, 0, 0, 0);
+                        return CM_INVALID_ELF_FILE;
+                    } else if(symbol_addr == 0xFFFFFFFE) { // OOM
+                        return CM_NO_MORE_MEMORY;
+                    } else if(symbol_addr == 0xFFFFFFFF) { // Defined inside static binding
+                        continue;
+                    }
+                    break;
+                default:                   // Internal reference in loaded section
+                    symbol_addr = getSymbolAddress(
+                            memories,
+                            elfhandle,
+                            (t_uint32)relocation->st_shndx,
+                            relocation->st_value);
+                    if(symbol_addr == 0xFFFFFFFF) {
+                        ERROR("Symbol in section %s+%d not loaded\n",
+                                ELF->sectionss[relocation->st_shndx].sectionName,
+                                relocation->st_value, 0, 0, 0, 0);
+                        return CM_INVALID_ELF_FILE;
+                    }
+                    break;
+                }
+
+                symbol_addr += relocation->r_addend;
+
+                MMDSP_performRelocation(
+                        relocation->type,
+                        relocation->symbol_name,
+                        symbol_addr,
+                        relocAddr,
+                        ELF->sectionss[sec].data + relocation->OffsetInElf * mapping->memEntSize,
+                        sectionOffset + relocation->OffsetInElf);
+            }
+        }
+    }
+
+    return CM_OK;
+}
+
+t_cm_error ELF64_getRelocationMemory(
+        t_elfdescription            *elfhandle,
+        t_tmp_elfdescription        *elftmp,
+        t_uint32                    offsetInNmf,
+        t_memory_reference          *memory) {
+    struct XXElf* ELF = elfhandle->ELF;
+    const ElfXX_Rela* rel_start;
+    const ElfXX_Sym* relaNmfSegmentSymbols = (ElfXX_Sym*)elftmp->relaNmfSegmentSymbols;
+
+    for(rel_start = (ElfXX_Rela*)elftmp->relaNmfSegment; rel_start < (ElfXX_Rela*)elftmp->relaNmfSegmentEnd; rel_start++)
+    {
+        if((t_uint32)swapXword(rel_start->r_offset) == offsetInNmf)
+        {
+            int strtab_index = ELFXX_R_SYM(swapXword(rel_start->r_info));
+            int sectionIdx = (int)swapHalf(relaNmfSegmentSymbols[strtab_index].st_shndx);
+
+            memory->memory = ELF->sectionss[sectionIdx].meminfo;
+
+            if(memory->memory != NULL) {
+                memory->offset = (
+                        ELF->sectionss[sectionIdx].offsetInSegment +                           // Offset in Segment
+                        (t_uint32)swapXword(relaNmfSegmentSymbols[strtab_index].st_value) +    // Offset in Elf Section
+                        (t_uint32)swapXword(rel_start->r_addend));                                  // Addend
+
+                return CM_OK;
+            } else {
+                const char* symbol_name = &elftmp->relaNmfSegmentStrings[swapWord(relaNmfSegmentSymbols[strtab_index].st_name)];
+                ERROR("Symbol %s not found\n", symbol_name, 0, 0, 0, 0, 0);
+                return CM_INVALID_ELF_FILE;
+            }
+        }
+    }
+
+    ERROR("Unknown relocation error\n", 0, 0, 0, 0, 0, 0);
+    return CM_INVALID_ELF_FILE;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/mmdsp-debug.c b/drivers/staging/nmf-cm/cm/engine/elf/src/mmdsp-debug.c
new file mode 100644
index 00000000000..c6c316046b3
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/mmdsp-debug.c
@@ -0,0 +1,435 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/elf/inc/mmdsp-loadmap.h>
+#include <cm/engine/elf/inc/mmdsp.h>
+#include <cm/engine/dsp/inc/semaphores_dsp.h>
+#include <cm/engine/dsp/mmdsp/inc/mmdsp_hwp.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#include <cm/engine/power_mgt/inc/power.h>
+
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/component/inc/component_type.h>
+#include <inc/nmf-limits.h>
+
+#define LOADMAP_SEMAPHORE_USE_NB                        7
+
+static t_memory_handle headerHandle[NB_CORE_IDS] = {INVALID_MEMORY_HANDLE, };
+static struct LoadMapHdr *headerAddresses[NB_CORE_IDS] = {0, };
+static t_uint32 headerOffsets[NB_CORE_IDS] = {0, };
+static t_uint32 entryNumber[NB_CORE_IDS] = {0, };
+
+#undef myoffsetof
+#define myoffsetof(TYPE, MEMBER) ((unsigned int) &((TYPE *)0)->MEMBER)
+
+t_cm_error cm_DSPABI_AddLoadMap(
+        t_cm_domain_id domainId,
+        const char* templateName,
+        const char* localname,
+        t_memory_handle *memories,
+        void *componentHandle)
+{
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(domainId);
+    int count=0;
+    struct LoadMapItem* curItem = NULL;
+
+    if (headerHandle[coreId] == 0) /* Create loadmap header */
+    {
+        headerHandle[coreId] = cm_DM_Alloc(domainId, SDRAM_EXT16,
+                                           sizeof(struct LoadMapHdr)/2, CM_MM_ALIGN_2WORDS, TRUE);
+        if (headerHandle[coreId] == INVALID_MEMORY_HANDLE) {
+            ERROR("CM_NO_MORE_MEMORY: Unable to allocate loadmap in cm_DSPABI_AddLoadMap()\n", 0, 0, 0, 0, 0, 0);
+            return CM_NO_MORE_MEMORY;
+        }
+
+        headerAddresses[coreId] = (struct LoadMapHdr*)cm_DSP_GetHostLogicalAddress(headerHandle[coreId]);
+
+        headerAddresses[coreId]->nMagicNumber = LOADMAP_MAGIC_NUMBER;
+        headerAddresses[coreId]->nVersion = (LOADMAP_VERSION_MSB<<8)|(LOADMAP_VERSION_LSB);
+        headerAddresses[coreId]->nRevision = 0;
+        headerAddresses[coreId]->pFirstItem = 0;
+
+        //Register Header into XRAM:2
+        cm_DSP_GetDspAddress(headerHandle[coreId], &headerOffsets[coreId]);
+        cm_DSP_WriteXRamWord(coreId, 2, headerOffsets[coreId]);
+    }
+
+    // update Header nRevision field
+    headerAddresses[coreId]->nRevision++;
+
+    /*
+     * Build loadmap entry
+     */
+    {
+        t_memory_handle handle;
+        struct LoadMapItem* pItem;
+        t_uint32 dspentry;
+        unsigned char* pos;
+        t_uint32 fnlen, lnlen;
+        t_uint32 fnlenaligned, lnlenaligned;
+        t_uint32 address;
+        t_uint32 postStringLength;
+        int i;
+
+        postStringLength = cm_StringLength(".elf", 16);
+        fnlenaligned = fnlen = cm_StringLength(templateName, MAX_COMPONENT_FILE_PATH_LENGTH) + postStringLength + 2;
+        if((fnlenaligned % 2) != 0) fnlenaligned++;
+        lnlenaligned = lnlen = cm_StringLength(localname, MAX_TEMPLATE_NAME_LENGTH);
+        if((lnlenaligned % 2) != 0) lnlenaligned++;
+
+        // Allocate new loap map
+        handle = cm_DM_Alloc(domainId, SDRAM_EXT16,
+                             sizeof(struct LoadMapItem)/2 + (1 + fnlenaligned/2) + (1 + lnlenaligned/2),
+                             CM_MM_ALIGN_2WORDS, TRUE);
+        if (handle == INVALID_MEMORY_HANDLE) {
+            ERROR("CM_NO_MORE_MEMORY: Unable to allocate loadmap entry in cm_DSPABI_AddLoadMap\n", 0, 0, 0, 0, 0, 0);
+            return CM_NO_MORE_MEMORY;
+        }
+
+        pItem = (struct LoadMapItem*)cm_DSP_GetHostLogicalAddress(handle);
+        cm_DSP_GetDspAddress(handle, &dspentry);
+	count++;
+	entryNumber[coreId]++;
+
+        // Link this new loadmap with the previous one
+        if(headerAddresses[coreId]->pFirstItem == NULL)
+            headerAddresses[coreId]->pFirstItem = (struct LoadMapItem *)dspentry;
+        else
+        {
+            const t_dsp_desc* pDspDesc = cm_DSP_GetState(coreId);
+            t_uint32 endSegmentAddr = SDRAMMEM16_BASE_ADDR + pDspDesc->segments[SDRAM_DATA_USER].size / 2;
+            struct LoadMapItem* curItem, *prevItem = NULL;
+            t_uint32  curItemDspAdress;
+
+            if(
+                    ((t_uint32)headerAddresses[coreId]->pFirstItem < SDRAMMEM16_BASE_ADDR) ||
+                    ((t_uint32)headerAddresses[coreId]->pFirstItem > endSegmentAddr))
+            {
+                ERROR("Memory corruption in MMDSP: at data DSP address=%x or ARM address=%x\n",
+                        headerOffsets[coreId], &headerAddresses[coreId]->pFirstItem, 0, 0, 0, 0);
+
+                return CM_INVALID_DATA;
+            }
+            curItemDspAdress = (t_uint32)headerAddresses[coreId]->pFirstItem;
+            curItem = (struct LoadMapItem*)((curItemDspAdress - headerOffsets[coreId]) * 2 + (t_uint32)headerAddresses[coreId]); // To ARM address
+            count++;
+            while(curItem->pNextItem != NULL)
+            {
+                if(((t_uint32)curItem->pNextItem < SDRAMMEM16_BASE_ADDR) || ((t_uint32)curItem->pNextItem > endSegmentAddr))
+                {
+			if (prevItem == NULL)
+				ERROR("AddLoadMap: Memory corruption in MMDSP: at data DSP address=%x or ARM address=%x\n",
+				      curItemDspAdress + myoffsetof(struct LoadMapItem, pNextItem), &curItem->pNextItem,
+				      0, 0, 0, 0);
+			else
+				ERROR("AddLoadMap: Memory corruption in MMDSP: at data DSP address=%x or ARM address=%x\n",
+				      curItemDspAdress + myoffsetof(struct LoadMapItem, pNextItem), &curItem->pNextItem,
+				      0, 0, 0, 0);
+			return CM_INVALID_DATA;
+                }
+                curItemDspAdress = (t_uint32)curItem->pNextItem;
+		prevItem = curItem;
+                curItem = (struct LoadMapItem*)((curItemDspAdress - headerOffsets[coreId]) * 2 + (t_uint32)headerAddresses[coreId]); // To ARM address
+		count++;
+            }
+            curItem->pNextItem = (struct LoadMapItem *)dspentry;
+        }
+
+        // DSP Address of the string at the end of the load map
+        pos = (unsigned char*)pItem + sizeof(struct LoadMapItem);
+
+        /*
+         * Set SolibFilename address information
+         *   ->  string = "./origfilename"
+         */
+        pItem->pSolibFilename = (char*)(dspentry + sizeof(struct LoadMapItem) / 2);
+        *(t_uint16*)pos = fnlen;
+        pos += 2;
+        *pos++ = '.';
+        *pos++ = '\\';
+        for(i = 0; i < fnlen - 2 - postStringLength; i++)
+        {
+            *pos++ = (templateName[i] == '.') ? '\\' : templateName[i];
+        }
+        *pos++ = '.';
+        *pos++ = 'e';
+        *pos++ = 'l';
+        *pos++ = 'f';
+        // add padding if needed
+        if ((t_uint32)pos & 1)
+            *pos++ = '\0';
+
+        /*
+         * Set Component Name  address information
+         */
+        if (lnlen != 0)
+        {
+            pItem->pComponentName = (char*)(dspentry + sizeof(struct LoadMapItem) / 2 + 1 + fnlenaligned / 2);
+
+            *(t_uint16*)pos = lnlen;
+            pos += 2;
+            for(i = 0; i < lnlenaligned; i++)
+            {
+                // If not aligned null ending copied
+                *pos++ = localname[i];
+            }
+        }
+        else
+        {
+            pItem->pComponentName = 0;
+        }
+
+        /*
+         * Set PROG information
+         */
+        if(memories[CODE_MEMORY_INDEX] == INVALID_MEMORY_HANDLE)
+            address = 0;
+        else
+            cm_DSP_GetDspAddress(memories[CODE_MEMORY_INDEX], &address);
+        pItem->pAddrProg = (void*)address;
+
+        /*
+         * Set ERAMCODE information
+         */
+        if(memories[ECODE_MEMORY_INDEX] == INVALID_MEMORY_HANDLE)
+            address = 0;
+        else
+            cm_DSP_GetDspAddress(memories[ECODE_MEMORY_INDEX], &address);
+        pItem->pAddrEmbProg = (void*)address;
+
+        /*
+         * Set THIS information
+         */
+        if(memories[PRIVATE_DATA_MEMORY_INDEX] != INVALID_MEMORY_HANDLE) {
+            // Standard component
+            cm_DSP_GetDspAddress(memories[PRIVATE_DATA_MEMORY_INDEX], &address);
+        } else if(memories[SHARE_DATA_MEMORY_INDEX] != INVALID_MEMORY_HANDLE) {
+            // Singleton component where data are shared (simulate THIS with shared memory)
+            cm_DSP_GetDspAddress(memories[SHARE_DATA_MEMORY_INDEX], &address);
+        } else {
+            // Component without data (take unique identifier -> arbitrary take host component handle)
+		address = (t_uint32)componentHandle;
+        }
+        pItem->pThis = (void*)address;
+
+        /*
+         * Set ARM THIS information
+         */
+        pItem->pARMThis = componentHandle;
+
+        /*
+         * Set Link to null (end of list)
+         */
+        pItem->pNextItem = 0;
+
+        /*
+         * Set XROM information
+         */
+        if(memories[XROM_MEMORY_INDEX] == INVALID_MEMORY_HANDLE)
+            address = 0;
+        else
+            cm_DSP_GetDspAddress(memories[XROM_MEMORY_INDEX], &address);
+        pItem->pXROM = (void*)address;
+
+        /*
+         * Set YROM information
+         */
+        if(memories[YROM_MEMORY_INDEX] == INVALID_MEMORY_HANDLE)
+            address = 0;
+        else
+            cm_DSP_GetDspAddress(memories[YROM_MEMORY_INDEX], &address);
+        pItem->pYROM = (void*)address;
+
+        /*
+         * Set memory handle (not used externally)
+         */
+        ((t_component_instance *)componentHandle)->loadMapHandle = handle;
+    }
+
+    OSAL_mb();
+
+    if (count != entryNumber[coreId]) {
+	    ERROR("AddLoadMap: corrumption, number of component differs: count=%d, expected %d (last item @ %p)\n",
+		  count, entryNumber[coreId], curItem, 0, 0, 0);
+	    return CM_INVALID_DATA;
+    }
+    return CM_OK;
+}
+
+t_cm_error cm_DSPABI_RemoveLoadMap(
+        t_cm_domain_id domainId,
+        const char* templateName,
+        t_memory_handle *memories,
+        const char* localname,
+        void *componentHandle)
+{
+    struct LoadMapItem **prevItemReference;
+    t_uint32  prevItemReferenceDspAddress, curItemDspAdress;
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(domainId);
+    const t_dsp_desc* pDspDesc = cm_DSP_GetState(coreId);
+    t_uint32 endSegmentAddr = SDRAMMEM16_BASE_ADDR + pDspDesc->segments[SDRAM_DATA_USER].size / 2;
+    struct LoadMapItem* curItem = NULL;
+
+    CM_ASSERT (headerHandle[coreId] != INVALID_MEMORY_HANDLE);
+
+    /* parse list until we find this */
+    prevItemReferenceDspAddress = 0x2;                          // DSP address of load map head pointer
+    prevItemReference = &headerAddresses[coreId]->pFirstItem;
+    curItemDspAdress = (t_uint32)*prevItemReference;
+    while(curItemDspAdress != 0x0)
+    {
+        if((curItemDspAdress < SDRAMMEM16_BASE_ADDR) || (curItemDspAdress > endSegmentAddr))
+        {
+            ERROR("Memory corruption in MMDSP: at data DSP address=%x or ARM address=%x\n",
+                    prevItemReferenceDspAddress, prevItemReference, 0, 0, 0, 0);
+
+	    /* free the entry anyway to avoid leakage */
+            cm_DM_Free(((t_component_instance *)componentHandle)->loadMapHandle, TRUE);
+
+	    return CM_OK;
+        }
+
+        curItem = (struct LoadMapItem*)((curItemDspAdress - headerOffsets[coreId]) * 2 + (t_uint32)headerAddresses[coreId]); // To ARM address
+
+        if(curItem->pARMThis == componentHandle)
+        {
+            // Remove component from loadmap
+
+            /* take local semaphore */
+            cm_DSP_SEM_Take(coreId,LOADMAP_SEMAPHORE_USE_NB);
+
+            /* remove element from list */
+            *prevItemReference = curItem->pNextItem;
+
+            /* update nRevision field in header */
+            headerAddresses[coreId]->nRevision++;
+
+            /* If this is the last item, deallocate !!! */
+            if(headerAddresses[coreId]->pFirstItem == NULL)
+            {
+                // Deallocate memory
+                cm_DM_Free(headerHandle[coreId], TRUE);
+                headerHandle[coreId] = INVALID_MEMORY_HANDLE;
+
+                //Register Header into XRAM:2
+                cm_DSP_WriteXRamWord(coreId, 2, 0);
+            }
+
+            /* deallocate memory */
+            cm_DM_Free(((t_component_instance *)componentHandle)->loadMapHandle, TRUE);
+
+            /* be sure memory is updated before releasing local semaphore */
+            OSAL_mb();
+
+            /* release local semaphore */
+            cm_DSP_SEM_Give(coreId,LOADMAP_SEMAPHORE_USE_NB);
+
+	    entryNumber[coreId]--;
+
+            return CM_OK;
+        }
+
+        prevItemReferenceDspAddress = curItemDspAdress + myoffsetof(struct LoadMapItem, pNextItem);
+        prevItemReference = &curItem->pNextItem;
+        curItemDspAdress = (t_uint32)*prevItemReference;
+    };
+
+    ERROR("Memory corruption in MMDSP: component not in LoadMap %s\n", localname, 0, 0, 0, 0, 0);
+
+    /* free the entry anyway to avoid leakage */
+    cm_DM_Free(((t_component_instance *)componentHandle)->loadMapHandle, TRUE);
+
+    return CM_OK;
+}
+
+#if 0
+t_cm_error cm_DSPABI_CheckLoadMap_nolock(t_nmf_core_id coreId)
+{
+    int count=0;
+    static int dump = 5;
+    struct LoadMapItem* curItem = NULL;
+
+    if (!dump)
+            return CM_OK;
+    if (headerHandle[coreId] == 0) /* No load map yet */
+            return CM_OK;
+
+    {
+        // No entry in loadmap
+        if(headerAddresses[coreId]->pFirstItem == NULL)
+            return CM_OK;
+
+        {
+            const t_dsp_desc* pDspDesc = cm_DSP_GetState(coreId);
+            t_uint32 endSegmentAddr = SDRAMMEM16_BASE_ADDR + pDspDesc->segments[SDRAM_DATA_USER].size / 2;
+            struct LoadMapItem *prevItem=NULL;
+            t_uint32  curItemDspAdress;
+
+            if (((t_uint32)headerAddresses[coreId]->pFirstItem < SDRAMMEM16_BASE_ADDR) ||
+		((t_uint32)headerAddresses[coreId]->pFirstItem > endSegmentAddr))
+            {
+                ERROR("CheckLoadMap: Memory corruption in MMDSP at first item: at data DSP address=%x or ARM address=%x\n",
+                      headerOffsets[coreId], &headerAddresses[coreId]->pFirstItem, 0, 0, 0, 0);
+                dump--;
+                return CM_INVALID_COMPONENT_HANDLE;
+            }
+            curItemDspAdress = (t_uint32)headerAddresses[coreId]->pFirstItem;
+            curItem = (struct LoadMapItem*)((curItemDspAdress - headerOffsets[coreId]) * 2 + (t_uint32)headerAddresses[coreId]);
+	    count++;
+            while(curItem->pNextItem != NULL)
+            {
+                if(((t_uint32)curItem->pNextItem < SDRAMMEM16_BASE_ADDR) || ((t_uint32)curItem->pNextItem > endSegmentAddr))
+                {
+                    if (!prevItem)
+                        ERROR("CheckLoadMap: Memory corruption in MMDSP (count=%d): at data DSP address=%x or ARM address=%x\n"
+                        "Previous (first) component name %s<%s>\n",
+                        count,
+                        curItemDspAdress + myoffsetof(struct LoadMapItem, pNextItem), &curItem->pNextItem,
+                        (char*)(((t_component_instance *)&curItem->pARMThis)->pathname),
+                        (char*)(((t_component_instance *)&curItem->pARMThis)->Template->name), 0);
+                    else
+                        ERROR("CheckLoadMap: Memory corruption in MMDSP (count=%d): at data DSP address=%x or ARM address=%x\n"
+                        "Previous valid component name %s<%s>",
+                        count,
+                        curItemDspAdress + myoffsetof(struct LoadMapItem, pNextItem), &curItem->pNextItem,
+                        (char*)(((t_component_instance *)&prevItem->pARMThis)->pathname),
+                        (char*)(((t_component_instance *)&prevItem->pARMThis)->Template->name), 0);
+                    dump--;
+                return CM_INVALID_COMPONENT_HANDLE;
+                }
+                curItemDspAdress = (t_uint32)curItem->pNextItem;
+                prevItem = curItem;
+                curItem = (struct LoadMapItem*)((curItemDspAdress - headerOffsets[coreId]) * 2 + (t_uint32)headerAddresses[coreId]); // To ARM address
+		count++;
+            }
+        }
+
+    }
+
+    if (count != entryNumber[coreId]) {
+	    ERROR("CheckLoadMap: number of component differs: count=%d, expected %d (last item @ %p)\n", count, entryNumber[coreId],
+		  curItem, 0, 0, 0);
+	    dump--;
+	    return CM_INVALID_COMPONENT_HANDLE;
+    }
+    return CM_OK;
+}
+
+t_cm_error cm_DSPABI_CheckLoadMap(t_nmf_core_id coreId)
+{
+	t_cm_error error;
+	OSAL_LOCK_API();
+	error = cm_DSPABI_CheckLoadMap_nolock(coreId);
+	OSAL_UNLOCK_API();
+	return error;
+}
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/elf/src/mpcal.c b/drivers/staging/nmf-cm/cm/engine/elf/src/mpcal.c
new file mode 100644
index 00000000000..93d910a5ed6
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/elf/src/mpcal.c
@@ -0,0 +1,6 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/elf/inc/mpcal.h>
diff --git a/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h b/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h
new file mode 100644
index 00000000000..0894410ae0d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_EE_MGT_H
+#define __INC_EE_MGT_H
+
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+typedef struct {
+    t_component_instance        *instance;
+    t_nmf_executive_engine_id   executiveEngineId;
+    t_uint32                    currentStackSize[NMF_SCHED_URGENT + 1];
+    t_uint32                    voidAddr;
+    t_uint32                    traceState;
+    t_uint32                    printLevel;
+    t_uint32                    nbOfForceWakeup;
+    struct {
+        t_memory_handle         handle;
+        t_cm_logical_address    addr;
+    } panicArea;
+
+    // Trace Management
+    t_uint32                    readTracePointer;
+    t_uint32                    lastReadedTraceRevision;
+    t_memory_handle             traceDataHandle;
+    struct t_nmf_trace          *traceDataAddr;
+} t_ee_state;
+
+//TODO, juraj, this should be done more properly, like accessor method, instead making this global variable..
+extern t_ee_state eeState[NB_CORE_IDS];
+
+/******************************************************************************/
+/************************ FUNCTIONS PROTOTYPES ********************************/
+/******************************************************************************/
+
+PUBLIC t_cm_error cm_EEM_Init(t_nmf_core_id coreId, const char *eeName, t_nmf_executive_engine_id executiveEngineId);
+PUBLIC void cm_EEM_Close(t_nmf_core_id coreId);
+PUBLIC t_uint32 cm_EEM_isStackUpdateNeed(t_nmf_core_id coreId, t_nmf_ee_priority priority, t_uint32 isInstantiate, t_uint32 needMinStackSize);
+PUBLIC t_cm_error cm_EEM_UpdateStack(t_nmf_core_id coreId, t_nmf_ee_priority priority, t_uint32 needMinStackSize, t_uint32 *pNewStackValue);
+PUBLIC t_ee_state* cm_EEM_getExecutiveEngine(t_nmf_core_id coreId);
+PUBLIC void cm_EEM_setTraceMode(t_nmf_core_id coreId, t_uint32 state);
+PUBLIC void cm_EEM_setPrintLevel(t_nmf_core_id coreId, t_uint32 level);
+t_cm_error cm_EEM_ForceWakeup(t_nmf_core_id coreId);
+void cm_EEM_AllowSleep(t_nmf_core_id coreId);
+
+#endif /* __INC_EE_MGT_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/src/executive_engine_mgt.c b/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/src/executive_engine_mgt.c
new file mode 100644
index 00000000000..a00ca4c4683
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/executive_engine_mgt/src/executive_engine_mgt.c
@@ -0,0 +1,410 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*----------------------------------------------------------------------------*
+ * This module provides functions that allow to manage DSPs' Firmwares.       *
+ ******************************************************************************/
+
+
+/******************************************************************* Includes
+ ****************************************************************************/
+
+#include "../inc/executive_engine_mgt.h"
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/utils/inc/convert.h>
+#include <cm/engine/component/inc/initializer.h>
+#include <cm/engine/power_mgt/inc/power.h>
+#include <cm/engine/perfmeter/inc/mpcload.h>
+
+#include <cm/engine/trace/inc/xtitrace.h>
+
+#include <share/communication/inc/nmf_service.h>
+
+t_ee_state eeState[NB_CORE_IDS];
+
+/****************************************************************** Functions
+ ****************************************************************************/
+static t_cm_error cm_EEM_allocPanicArea(t_nmf_core_id coreId, t_cm_domain_id domainId);
+static void cm_EEM_freePanicArea(t_nmf_core_id coreId);
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetExecutiveEngineHandle(
+        t_cm_domain_id domainId,
+        t_cm_instance_handle *executiveEngineHandle)
+{
+    t_nmf_core_id coreId;
+
+    if (cm_DM_CheckDomain(domainId, DOMAIN_NORMAL) != CM_OK) {
+        return CM_INVALID_DOMAIN_HANDLE;
+    }
+
+    coreId = cm_DM_GetDomainCoreId(domainId);
+    //in case someone ask for ee on component manager !!!!
+    if (coreId == ARM_CORE_ID) {*executiveEngineHandle = 0;}
+    else {*executiveEngineHandle = eeState[coreId].instance->instance;}
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_EEM_Init(
+    t_nmf_core_id coreId,
+    const char *eeName,
+    t_nmf_executive_engine_id executiveEngineId)
+{
+    t_rep_component *pRepComponent;
+    t_cm_error error;
+    t_uint32 i;
+
+    eeState[coreId].instance = (t_component_instance *)0;
+    eeState[coreId].executiveEngineId = executiveEngineId;
+    for(i = NMF_SCHED_BACKGROUND; i < NMF_SCHED_URGENT + 1;i++)
+    {
+        eeState[coreId].currentStackSize[i] = MIN_STACK_SIZE;
+    }
+
+    // Try to load component file
+    if((error = cm_REP_lookupComponent(eeName, &pRepComponent)) != CM_OK)
+    {
+        if (error == CM_COMPONENT_NOT_FOUND)
+            ERROR("CM_COMPONENT_NOT_FOUND: Execution Engine %s\n", eeName, 0, 0, 0, 0, 0);
+        return error;
+    }
+
+    // Set to 1 during bootstrap since MMDSP forceWakeup is to one also in order to not go in idle state
+    // while configuration not finish !!!
+    eeState[coreId].nbOfForceWakeup = 1;
+
+    if ((error = cm_DSP_Boot(coreId)) != CM_OK)
+        return error;
+
+    if((error = cm_instantiateComponent(
+            eeName,
+            cm_DSP_GetState(coreId)->domainEE,
+            NMF_SCHED_URGENT,
+            eeName,
+            pRepComponent->elfhandle,
+            &eeState[coreId].instance)) != CM_OK)
+    {
+        cm_DSP_Shutdown(coreId);
+        return error;
+    }
+
+    /* Get Void Function */
+    eeState[coreId].voidAddr = cm_getFunction(eeState[coreId].instance, "helper", "Void");
+
+    /* allocate xram space for stack */
+    if (executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE)
+    {
+        error = cm_DSP_setStackSize(coreId, MIN_STACK_SIZE);
+    }
+    else
+    {
+        error = cm_DSP_setStackSize(coreId, (NMF_SCHED_URGENT + 1) * MIN_STACK_SIZE);
+    }
+    if (error != CM_OK)
+    {
+        cm_delayedDestroyComponent(eeState[coreId].instance);
+        eeState[coreId].instance = (t_component_instance *)0;
+        cm_DSP_Shutdown(coreId);
+        return error;
+    }
+
+    /* allocate sdram memory for panic area */
+    error = cm_EEM_allocPanicArea(coreId, cm_DSP_GetState(coreId)->domainEE);
+    if (error != CM_OK) {
+        cm_delayedDestroyComponent(eeState[coreId].instance);
+        eeState[coreId].instance = (t_component_instance *)0;
+        cm_DSP_Shutdown(coreId);
+        return error;
+    }
+
+    /* allocate sdram memory to share perfmeters data */
+    error = cm_PFM_allocatePerfmeterDataMemory(coreId, cm_DSP_GetState(coreId)->domainEE);
+    if (error != CM_OK) {
+        cm_EEM_freePanicArea(coreId);
+        cm_delayedDestroyComponent(eeState[coreId].instance);
+        eeState[coreId].instance = (t_component_instance *)0;
+        cm_DSP_Shutdown(coreId);
+        return error;
+    }
+
+    if((error = cm_SRV_allocateTraceBufferMemory(coreId, cm_DSP_GetState(coreId)->domainEE)) != CM_OK)
+    {
+        cm_PFM_deallocatePerfmeterDataMemory(coreId);
+        cm_EEM_freePanicArea(coreId);
+        cm_delayedDestroyComponent(eeState[coreId].instance);
+        eeState[coreId].instance = (t_component_instance *)0;
+        cm_DSP_Shutdown(coreId);
+        return error;
+    }
+
+    /* set initial stack value */
+    cm_writeAttribute(eeState[coreId].instance, "rtos/scheduler/topOfStack", cm_DSP_getStackAddr(coreId));
+
+    /* set myCoreId for trace */
+    cm_writeAttribute(eeState[coreId].instance, "xti/myCoreId", coreId - 1);
+    
+#if defined(__STN_8500) && (__STN_8500 > 10)
+    /* set myCoreId for prcmu if exist */
+    cm_writeAttribute(eeState[coreId].instance, "sleep/prcmu/myCoreId", coreId + 1);
+#endif
+
+    /* go go go ... */
+    cm_DSP_Start(coreId);
+    
+    /* Waiting for End Of Boot */
+    //TODO : remove infinite while loop
+    //TODO : to be paranoiac, add a read to serviceReasonOffset before starting core and check value is MPC_SERVICE_BOOT as it should be
+    {
+        while(cm_readAttributeNoError(eeState[coreId].instance, "rtos/commonpart/serviceReason") == MPC_SERVICE_BOOT)
+        {
+            volatile t_uint32 i;
+            for (i=0; i < 1000; i++);
+        }
+    }
+
+    /* set some attributes after boot to avoid being erase by mmdsp boot */
+    cm_writeAttribute(eeState[coreId].instance, "xti/traceActive", eeState[coreId].traceState);
+    cm_writeAttribute(eeState[coreId].instance, "rtos/commonpart/printLevel", eeState[coreId].printLevel);
+
+    cm_DSP_ConfigureAfterBoot(coreId);
+
+    return CM_OK;
+}
+
+/****************************************************************************/
+/* NAME:        cm_EEM_Close                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Inform us that ee for coreId has been destroyed             */
+/*                                                                          */
+/* PARAMETERS:  id: dsp identifier                                          */
+/*                                                                          */
+/* RETURN:      none                                                        */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC void cm_EEM_Close(t_nmf_core_id coreId)
+{
+    cm_DSP_setStackSize(coreId, 0);
+    cm_delayedDestroyComponent(eeState[coreId].instance);
+    eeState[coreId].instance = (t_component_instance *)0;
+    cm_SRV_deallocateTraceBufferMemory(coreId);
+    cm_PFM_deallocatePerfmeterDataMemory(coreId);
+    cm_EEM_freePanicArea(coreId);
+    cm_DSP_Shutdown(coreId);
+}
+
+/****************************************************************************/
+/* NAME: cm_EEM_isStackUpdateNeed(                                          */
+/*                                t_nmf_core_id id,                         */
+/*                                t_nmf_ee_priority priority,               */
+/*                                t_uint32 isInstantiate,                   */
+/*                                t_uint32 needMinStackSize                 */
+/*                               )                                          */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Return a boolean to inform if a ee stack size update is need*/
+/*              when instantiate or destroying a component                  */
+/****************************************************************************/
+PUBLIC t_uint32 cm_EEM_isStackUpdateNeed(
+    t_nmf_core_id coreId,
+    t_nmf_ee_priority priority,
+    t_uint32 isInstantiate,
+    t_uint32 needMinStackSize)
+{
+    /* in case of SYNCHRONOUS_EXECUTIVE_ENGINE we only use currentStackSize[NMF_SCHED_BACKGROUND] */
+    if (eeState[coreId].executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE) {priority = NMF_SCHED_BACKGROUND;}
+    if (isInstantiate)
+    {
+        if (needMinStackSize > eeState[coreId].currentStackSize[priority]) {return TRUE;}
+    }
+    else
+    {
+        if (needMinStackSize == eeState[coreId].currentStackSize[priority]) {return TRUE;}
+    }
+
+    return FALSE;
+}
+
+/****************************************************************************/
+/* NAME: cm_EEM_UpdateStack(                                                */
+/*                          t_nmf_core_id id,                               */
+/*                          t_nmf_ee_priority priority,                     */
+/*                          t_uint32 needMinStackSize,                      */
+/*                          t_uint32 *pNewStackValue                        */
+/*                         )                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: If cm_EEM_isStackUpdateNeed() has return true then caller   */
+/*              must inform EEM about new stack value for priority.         */
+/*              cm_EEM_UpdateStack() will return new global stack size to   */
+/*              provide to ee.                                              */
+/****************************************************************************/
+PUBLIC t_cm_error cm_EEM_UpdateStack(
+    t_nmf_core_id coreId,
+    t_nmf_ee_priority priority,
+    t_uint32 needMinStackSize,
+    t_uint32 *pNewStackValue)
+{
+    t_cm_error error;
+    t_uint32 recoveryStackSize = eeState[coreId].currentStackSize[priority];
+    t_uint32 i;
+
+    /* in case of SYNCHRONOUS_EXECUTIVE_ENGINE we only use currentStackSize[NMF_SCHED_BACKGROUND] */
+    if (eeState[coreId].executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE) {priority = NMF_SCHED_BACKGROUND;}
+    eeState[coreId].currentStackSize[priority] = needMinStackSize;
+    if (eeState[coreId].executiveEngineId == SYNCHRONOUS_EXECUTIVE_ENGINE) {*pNewStackValue = needMinStackSize;}
+    else
+    {
+        *pNewStackValue = 0;
+        for(i = NMF_SCHED_BACKGROUND; i < NMF_SCHED_URGENT + 1;i++)
+        {
+            *pNewStackValue += eeState[coreId].currentStackSize[i];
+        }
+    }
+
+    /* try to increase size of stack by modifying xram allocator size */
+    error = cm_DSP_setStackSize(coreId, *pNewStackValue);
+    if (error != CM_OK) {
+        eeState[coreId].currentStackSize[priority] = recoveryStackSize;
+    } else {
+        LOG_INTERNAL(1, "\n##### Stack update: size=%d, prio=%d on %s #####\n", *pNewStackValue, priority, cm_getDspName(coreId), 0, 0, 0);
+    }
+
+    return error;
+}
+
+/****************************************************************************/
+/* NAME: t_nmf_executive_engine_id(                                         */
+/*                          t_nmf_core_id id                                */
+/*                         )                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: return executive engine load on id core.                    */
+/****************************************************************************/
+PUBLIC t_ee_state * cm_EEM_getExecutiveEngine(t_nmf_core_id coreId)
+{
+    return &eeState[coreId];
+}
+
+/****************************************************************************/
+/* NAME: cm_EEM_setTraceMode(                                               */
+/*                          t_nmf_core_id id,                               */
+/*                          t_uint32 state                                  */
+/*                         )                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: activate/deactivate trace for ee running on id. In case ee  */
+/*          is not yet load then information is store.                      */
+/****************************************************************************/
+PUBLIC void cm_EEM_setTraceMode(t_nmf_core_id coreId, t_uint32 state)
+{
+    eeState[coreId].traceState = state;
+    if (eeState[coreId].instance)
+    {
+        if(cm_EEM_ForceWakeup(coreId) == CM_OK)
+        {
+            cm_writeAttribute(eeState[coreId].instance, "xti/traceActive", eeState[coreId].traceState);
+
+            cm_EEM_AllowSleep(coreId);
+        }
+    }
+}
+
+/****************************************************************************/
+/* NAME: cm_EEM_setPrintLevel(                                              */
+/*                          t_nmf_core_id id,                               */
+/*                          t_uint32 level                                  */
+/*                         )                                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: set print level for ee running on id. In case ee            */
+/*          is not yet load then information is store.                      */
+/****************************************************************************/
+PUBLIC void cm_EEM_setPrintLevel(t_nmf_core_id coreId, t_uint32 level)
+{
+    eeState[coreId].printLevel = level;
+    if (eeState[coreId].instance)
+    {
+        if(cm_EEM_ForceWakeup(coreId) == CM_OK)
+        {
+            cm_writeAttribute(eeState[coreId].instance, "rtos/commonpart/printLevel", eeState[coreId].printLevel);
+
+            cm_EEM_AllowSleep(coreId);
+        }
+    }
+}
+
+t_cm_error cm_EEM_ForceWakeup(t_nmf_core_id coreId)
+{
+    if(eeState[coreId].nbOfForceWakeup++ == 0)
+    {
+        t_cm_error error;
+
+        LOG_INTERNAL(1, "ARM: Try to wake up on core id : %d\n", coreId, 0, 0, 0, 0, 0);
+
+        if (cm_DSP_GetState(coreId)->state != MPC_STATE_BOOTED)
+        {
+            return CM_MPC_NOT_RESPONDING;
+        }
+        else if ((error = cm_COMP_ULPForceWakeup(coreId)) != CM_OK)
+        {
+            if (error == CM_MPC_NOT_RESPONDING) {
+                if(cm_DSP_GetState(coreId)->state == MPC_STATE_PANIC)
+                    /* Don't print error which has been done by Panic handling */;
+                else
+                {
+                    ERROR("CM_MPC_NOT_RESPONDING: DSP %s can't be wakeup'ed\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+                    cm_DSP_SetStatePanic(coreId);
+                }
+            }
+            return error;
+        }
+    }
+    else
+        LOG_INTERNAL(1, "ARM: Not Try to wake up on core id : %d (nbOfForceWakeup = %d)\n", coreId, eeState[coreId].nbOfForceWakeup, 0, 0, 0, 0);
+
+    return CM_OK;
+}
+
+void cm_EEM_AllowSleep(t_nmf_core_id coreId)
+{
+    if(--eeState[coreId].nbOfForceWakeup == 0)
+    {
+        LOG_INTERNAL(1, "ARM: Allow sleep on core id : %d\n", coreId, 0, 0, 0, 0, 0);
+
+        if (cm_DSP_GetState(coreId)->state != MPC_STATE_BOOTED)
+        {
+        }
+        else if (cm_COMP_ULPAllowSleep(coreId) != CM_OK)
+        {
+            ERROR("CM_MPC_NOT_RESPONDING: DSP %s can't be allow sleep'ed\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+        }
+    }
+    else
+        LOG_INTERNAL(1, "ARM: Not Allow sleep on core id : %d (nbOfForceWakeup = %d)\n", coreId, eeState[coreId].nbOfForceWakeup, 0, 0, 0, 0);
+}
+
+/* internal api */
+t_cm_error cm_EEM_allocPanicArea(t_nmf_core_id coreId, t_cm_domain_id domainId)
+{
+    t_cm_error error = CM_OK;
+
+    eeState[coreId].panicArea.handle = cm_DM_Alloc(cm_DSP_GetState(coreId)->domainEE, SDRAM_EXT24, 45 /* 42 registers, pc, 2 magic words */,CM_MM_ALIGN_WORD, TRUE);
+    if (eeState[coreId].panicArea.handle == INVALID_MEMORY_HANDLE)
+        error = CM_NO_MORE_MEMORY;
+    else {
+        t_uint32 mmdspAddr;
+
+        eeState[coreId].panicArea.addr = cm_DSP_GetHostLogicalAddress(eeState[coreId].panicArea.handle);
+        cm_DSP_GetDspAddress(eeState[coreId].panicArea.handle, &mmdspAddr);
+
+        cm_writeAttribute(eeState[coreId].instance, "rtos/commonpart/panicDataAddr", mmdspAddr);
+    }
+
+    return error;
+}
+
+void cm_EEM_freePanicArea(t_nmf_core_id coreId)
+{
+    eeState[coreId].panicArea.addr = 0;
+    cm_DM_Free(eeState[coreId].panicArea.handle, TRUE);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/chunk_mgr.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/chunk_mgr.h
new file mode 100644
index 00000000000..340301a9259
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/chunk_mgr.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef CHUNK_MGR_H_
+#define CHUNK_MGR_H_
+
+#include <cm/engine/memory/inc/remote_allocator.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+t_cm_error allocChunkPool(void);
+t_cm_error fillChunkPool(void);
+void freeChunkPool(void);
+
+/***************************************************************************/
+/*
+ * allocChunk
+ * param current  : Pointer on chunck to free
+ *
+ * Add a chunk in the chunck list
+ *
+ */
+/***************************************************************************/
+t_cm_chunk* allocChunk(void);
+
+/***************************************************************************/
+/*
+ * freeChunk
+ * param current  : Pointer on chunck to free
+ *
+ * Remove a chunk in the chunck list
+ *
+ */
+/***************************************************************************/
+void freeChunk(t_cm_chunk *chunk);
+
+#endif /*CHUNK_MGR_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/domain.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/domain.h
new file mode 100644
index 00000000000..c9b39956c63
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/domain.h
@@ -0,0 +1,163 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/***************************************************************************/
+/* file    : domain.h
+ * author  : NMF team
+ * version : 1.0
+ *
+ * brief : NMF domain definitions
+ */
+/***************************************************************************/
+
+#ifndef DOMAIN_H_
+#define DOMAIN_H_
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+/* These default domains are used for singleton only ! */
+#define DEFAULT_SVA_DOMAIN (t_cm_domain_id)1
+#define DEFAULT_SIA_DOMAIN (t_cm_domain_id)2
+
+/*!
+ * \brief Domain type.
+ * \internal
+ * \ingroup CM_DOMAIN_API
+ */
+typedef enum {
+    DOMAIN_ANY = 0,
+    DOMAIN_NORMAL,
+    DOMAIN_SCRATCH_PARENT,
+    DOMAIN_SCRATCH_CHILD
+} t_cm_domain_type;
+
+/*!
+ * \brief Domain descriptor. Holds offsets for all memory types present in the system.
+ * \internal
+ * \ingroup CM_DOMAIN_API
+ */
+typedef struct {
+    t_cm_domain_memory            domain;      // the actual memory ranges
+    t_cm_domain_type              type;        // domain type
+    t_uint32                      refcount;    // reference counter for scratch domain dependencies
+    t_nmf_client_id               client;      // client id for cleaning
+
+    union {
+        struct {
+            t_memory_handle       handle;      // memory handle of the allocated chunk the covers the esram-data scratch region
+        } parent;
+        struct {
+            t_cm_allocator_desc  *alloc;       //allocator descriptor for the scratch domain
+            t_cm_domain_id        parent_ref;  //parent domain reference
+        } child;
+    } scratch;
+    void *dbgCooky;                            //pointer to OS internal data
+} t_cm_domain_desc;
+
+#ifdef DEBUG
+#define DOMAIN_DEBUG(handle) \
+    handle = handle & ~0xc0;
+#else
+#define DOMAIN_DEBUG(handle)
+#endif
+
+/*!
+ * \brief Domain descriptor array.
+ */
+extern t_cm_domain_desc domainDesc[];
+
+typedef struct {
+    t_cm_domain_id parentId;
+    t_cm_domain_id domainId;
+    t_cm_allocator_desc *allocDesc;
+} t_cm_domain_scratch_desc;
+
+extern t_cm_domain_scratch_desc domainScratchDesc[];
+
+typedef struct {
+    t_cm_system_address sdramCode;
+    t_cm_system_address sdramData;
+    t_cm_system_address esramCode;
+    t_cm_system_address esramData;
+} t_cm_domain_info;
+
+/*!
+ * \brief Init of the domain subsystem.
+ */
+PUBLIC t_cm_error cm_DM_Init(void);
+
+/*!
+ * \brief Clean-up of the domain subsystem.
+ */
+PUBLIC void cm_DM_Destroy(void);
+
+/*!
+ * \brief Domain creation.
+ *
+ * Allocates in slot in the domain descriptors array and copies segment infos from the domain
+ * parameter to the descriptor. The resulting handle is returned via @param handle.
+ *
+ * Returns: CM_DOMAIN_INVALID in case of error, otherwise CM_OK.
+ */
+PUBLIC t_cm_error cm_DM_CreateDomain(const t_nmf_client_id client, const t_cm_domain_memory *domain, t_cm_domain_id *handle);
+
+/*!
+ * \brief Scratch (or overlap) domain creation.
+ *
+ * Create a scratch domain, ie domain where allocation may overlap.
+ */
+PUBLIC t_cm_error cm_DM_CreateDomainScratch(const t_nmf_client_id client, const t_cm_domain_id parentId, const t_cm_domain_memory *domain, t_cm_domain_id *handle);
+
+/* !
+ * \brief Retrieve the coreId from a given domain. Utility.
+ */
+PUBLIC t_nmf_core_id cm_DM_GetDomainCoreId(const t_cm_domain_id domainId);
+
+/*!
+ * \brief Destroy all domains belonging to a given client.
+ */
+PUBLIC t_cm_error cm_DM_DestroyDomains(const t_nmf_client_id client);
+
+/*!
+ * \brief Destroy a given domain.
+ */
+PUBLIC t_cm_error cm_DM_DestroyDomain(t_cm_domain_id handle);
+
+/*!
+ * \brief Check if the handle is valid.
+ */
+PUBLIC t_cm_error cm_DM_CheckDomain(t_cm_domain_id handle, t_cm_domain_type type);
+PUBLIC t_cm_error cm_DM_CheckDomainWithClient(t_cm_domain_id handle, t_cm_domain_type type, t_nmf_client_id client);
+
+/*!
+ * \brief Memory allocation in a given domain, for a given memory type (see CM_AllocMpcMemory).
+ */
+PUBLIC t_memory_handle cm_DM_Alloc(t_cm_domain_id domainId, t_dsp_memory_type_id memType, t_uint32 size, t_cm_mpc_memory_alignment memAlignment, t_bool powerOn);
+
+/*!
+ * \brief Memory free using a given domain handle
+ */
+PUBLIC void cm_DM_FreeWithInfo(t_memory_handle memHandle, t_nmf_core_id *coreId, t_dsp_memory_type_id *memType, t_bool powerOff);
+
+/*!
+ * \brief Memory free using a given domain handle
+ */
+PUBLIC void cm_DM_Free(t_memory_handle memHandle, t_bool powerOff);
+
+/*!
+ * \brief Wrapper function for CM_GetMpcMemoryStatus.
+ */
+PUBLIC t_cm_error cm_DM_GetAllocatorStatus(t_cm_domain_id domainId, t_dsp_memory_type_id memType, t_cm_allocator_status *pStatus);
+
+PUBLIC t_cm_error cm_DM_GetDomainAbsAdresses(t_cm_domain_id domainId, t_cm_domain_info *info);
+
+/*!
+ * \brief Change the domain for the given allocated chunk
+ */
+PUBLIC void cm_DM_SetDefaultDomain(t_memory_handle memHandle, t_nmf_core_id coreId);
+#endif /* DOMAIN_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/domain_type.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/domain_type.h
new file mode 100644
index 00000000000..354315d4d72
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/domain_type.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/***************************************************************************/
+/* file    : domain.h
+ * author  : NMF team
+ * version : 1.0
+ *
+ * brief : NMF domain definitions
+ */
+/***************************************************************************/
+
+#ifndef DOMAIN_TYPE_H_
+#define DOMAIN_TYPE_H_
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+
+/*!
+ * \brief Domain identifier
+ * \ingroup CM_DOMAIN_API
+ */
+typedef t_uint8 t_cm_domain_id;
+
+/*!
+ * \brief Client identifier
+ *        0 (zero) is considered as an invalid or 'NO' client identifier
+ * \ingroup CM_DOMAIN_API
+ */
+typedef t_uint32 t_nmf_client_id;
+#define NMF_CORE_CLIENT (t_nmf_client_id)-1
+#define NMF_CURRENT_CLIENT (t_nmf_client_id)0
+
+typedef struct {
+    t_uint32 offset;       //!< offset relativ to segment start in memory (in bytes)
+    t_uint32 size;         //!< size in bytes of the domain segment
+} t_cm_domain_segment;
+
+/*!
+ * \brief Domain memory description structure
+ * \ingroup CM_DOMAIN_API
+ */
+typedef struct {
+    t_nmf_core_id         coreId;     //!< MMDSP Core Id for this domain (used for TCM-X and TCM-Y at instantiate)
+    t_cm_domain_segment   esramCode;  //!< ESRAM code segment
+    t_cm_domain_segment   esramData;  //!< ESRAM data segment
+    t_cm_domain_segment   sdramCode;  //!< SDRAM code segment
+    t_cm_domain_segment   sdramData;  //!< SDRAM data segment
+} t_cm_domain_memory;
+
+#define INIT_DOMAIN_SEGMENT {0, 0}
+#define INIT_DOMAIN {MASK_ALL8, INIT_DOMAIN_SEGMENT, INIT_DOMAIN_SEGMENT, INIT_DOMAIN_SEGMENT, INIT_DOMAIN_SEGMENT}
+
+
+#endif /* DOMAIN_TYPE_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/memory.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/memory.h
new file mode 100644
index 00000000000..c6d1fb2dfff
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/memory.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Internal Memory Management API.
+ *
+ * \defgroup MEMORY_INTERNAL Private Memory API.
+ *
+ */
+#ifndef __INC_MEMORY_H
+#define __INC_MEMORY_H
+
+#include <cm/engine/api/control/configuration_engine.h>
+#include <cm/engine/memory/inc/remote_allocator.h>
+
+#endif /* __INC_MEMORY_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/memory_type.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/memory_type.h
new file mode 100644
index 00000000000..dbdba4c3d9d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/memory_type.h
@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Public Component Manager Memory API type.
+ *
+ * This file contains the Component Manager API type for manipulating memory.
+ */
+#ifndef __INC_MEMORY_TYPE_H
+#define __INC_MEMORY_TYPE_H
+
+#include <cm/inc/cm_type.h>
+
+/*!
+ * @defgroup t_cm_mpc_memory_type t_cm_mpc_memory_type
+ * \brief Definition of symbols used to reference the various type of Media Processor Core adressable memory
+ * @{
+ * \ingroup MEMORY
+ */
+typedef t_uint8 t_cm_mpc_memory_type;                            //!< Fake enumeration type
+#define CM_MM_MPC_TCM16_X           ((t_cm_mpc_memory_type)0)
+#define CM_MM_MPC_TCM24_X           ((t_cm_mpc_memory_type)1)
+#define CM_MM_MPC_ESRAM16           ((t_cm_mpc_memory_type)2)
+#define CM_MM_MPC_ESRAM24           ((t_cm_mpc_memory_type)3)
+#define CM_MM_MPC_SDRAM16           ((t_cm_mpc_memory_type)4)
+#define CM_MM_MPC_SDRAM24           ((t_cm_mpc_memory_type)5)
+#define CM_MM_MPC_TCM16_Y           ((t_cm_mpc_memory_type)6)
+#define CM_MM_MPC_TCM24_Y           ((t_cm_mpc_memory_type)7)
+#define CM_MM_MPC_TCM16             CM_MM_MPC_TCM16_X
+#define CM_MM_MPC_TCM24             CM_MM_MPC_TCM24_X
+
+/* @} */
+
+/*!
+ * @defgroup t_cm_memory_alignment t_cm_memory_alignment
+ * \brief Definition of symbols used to constraint the alignment of the allocated memory
+ * @{
+ * \ingroup MEMORY
+ */
+typedef t_uint16 t_cm_memory_alignment;                            //!< Fake enumeration type
+#define CM_MM_ALIGN_NONE            ((t_cm_memory_alignment)0x00000000)
+#define CM_MM_ALIGN_BYTE            ((t_cm_memory_alignment)CM_MM_ALIGN_NONE)
+#define CM_MM_ALIGN_HALFWORD        ((t_cm_memory_alignment)0x00000001)
+#define CM_MM_ALIGN_WORD            ((t_cm_memory_alignment)0x00000003)
+#define CM_MM_ALIGN_2WORDS          ((t_cm_memory_alignment)0x00000007)
+#define CM_MM_ALIGN_16BYTES         ((t_cm_memory_alignment)0x0000000F)
+#define CM_MM_ALIGN_4WORDS          ((t_cm_memory_alignment)0x0000000F)
+#define CM_MM_ALIGN_AHB_BURST       ((t_cm_memory_alignment)0x0000000F)
+#define CM_MM_ALIGN_32BYTES         ((t_cm_memory_alignment)0x0000001F)
+#define CM_MM_ALIGN_8WORDS          ((t_cm_memory_alignment)0x0000001F)
+#define CM_MM_ALIGN_64BYTES         ((t_cm_memory_alignment)0x0000003F)
+#define CM_MM_ALIGN_16WORDS         ((t_cm_memory_alignment)0x0000003F)
+#define CM_MM_ALIGN_128BYTES        ((t_cm_memory_alignment)0x0000007F)
+#define CM_MM_ALIGN_32WORDS         ((t_cm_memory_alignment)0x0000007F)
+#define CM_MM_ALIGN_256BYTES        ((t_cm_memory_alignment)0x000000FF)
+#define CM_MM_ALIGN_64WORDS         ((t_cm_memory_alignment)0x000000FF)
+#define CM_MM_ALIGN_512BYTES        ((t_cm_memory_alignment)0x000001FF)
+#define CM_MM_ALIGN_128WORDS        ((t_cm_memory_alignment)0x000001FF)
+#define CM_MM_ALIGN_1024BYTES       ((t_cm_memory_alignment)0x000003FF)
+#define CM_MM_ALIGN_256WORDS        ((t_cm_memory_alignment)0x000003FF)
+#define CM_MM_ALIGN_2048BYTES       ((t_cm_memory_alignment)0x000007FF)
+#define CM_MM_ALIGN_512WORDS        ((t_cm_memory_alignment)0x000007FF)
+#define CM_MM_ALIGN_4096BYTES       ((t_cm_memory_alignment)0x00000FFF)
+#define CM_MM_ALIGN_1024WORDS       ((t_cm_memory_alignment)0x00000FFF)
+#define CM_MM_ALIGN_65536BYTES      ((t_cm_memory_alignment)0x0000FFFF)
+#define CM_MM_ALIGN_16384WORDS      ((t_cm_memory_alignment)0x0000FFFF)
+/* @} */
+
+/*!
+ * @defgroup t_cm_mpc_memory_alignment t_cm_mpc_memory_alignment
+ * \brief Definition of symbols used to constraint the alignment of the allocated mpc memory
+ * @{
+ * \ingroup MEMORY
+ */
+typedef t_uint16 t_cm_mpc_memory_alignment;                            //!< Fake enumeration type
+#define CM_MM_MPC_ALIGN_NONE        ((t_cm_mpc_memory_alignment)0x00000000)
+#define CM_MM_MPC_ALIGN_HALFWORD    ((t_cm_mpc_memory_alignment)0x00000001)
+#define CM_MM_MPC_ALIGN_WORD        ((t_cm_mpc_memory_alignment)0x00000003)
+#define CM_MM_MPC_ALIGN_2WORDS      ((t_cm_mpc_memory_alignment)0x00000007)
+#define CM_MM_MPC_ALIGN_4WORDS      ((t_cm_mpc_memory_alignment)0x0000000F)
+#define CM_MM_MPC_ALIGN_8WORDS      ((t_cm_mpc_memory_alignment)0x0000001F)
+#define CM_MM_MPC_ALIGN_16WORDS     ((t_cm_mpc_memory_alignment)0x0000003F)
+#define CM_MM_MPC_ALIGN_32WORDS     ((t_cm_mpc_memory_alignment)0x0000007F)
+#define CM_MM_MPC_ALIGN_64WORDS     ((t_cm_mpc_memory_alignment)0x000000FF)
+#define CM_MM_MPC_ALIGN_128WORDS    ((t_cm_mpc_memory_alignment)0x000001FF)
+#define CM_MM_MPC_ALIGN_256WORDS    ((t_cm_mpc_memory_alignment)0x000003FF)
+#define CM_MM_MPC_ALIGN_512WORDS    ((t_cm_mpc_memory_alignment)0x000007FF)
+#define CM_MM_MPC_ALIGN_1024WORDS   ((t_cm_mpc_memory_alignment)0x00000FFF)
+#define CM_MM_MPC_ALIGN_65536BYTES  ((t_cm_mpc_memory_alignment)0x0000FFFF)
+#define CM_MM_MPC_ALIGN_16384WORDS  ((t_cm_mpc_memory_alignment)0x0000FFFF)
+/* @} */
+
+/*!
+ * \brief Identifier of a memory handle
+ * \ingroup MEMORY
+ */
+typedef t_uint32 t_cm_memory_handle;
+
+/*!
+ * \brief Description of a memory segment
+ *
+ * <=> allocable addressable space
+ * \ingroup MEMORY
+ */
+typedef struct {
+    t_cm_system_address systemAddr; //!< Logical AND physical segment start address
+    t_uint32 size;                //!< segment size (in bytes)
+} t_nmf_memory_segment;
+#define INIT_MEMORY_SEGMENT {{0, 0}, 0}
+
+/*!
+ * \brief Definition of structure used for an allocator status
+ * \ingroup MEMORY
+ */
+typedef struct
+{
+    struct {
+        t_uint32 size;              //!< size of the allocator
+        /* Block counters */
+        t_uint16 used_block_number; //!< used block number
+        t_uint16 free_block_number; //!< free block number
+
+        /* Free memory min/max */
+        t_uint32 maximum_free_size; //!< maximum free size
+        t_uint32 minimum_free_size; //!< minimum free size
+
+        /* Accumulation of free and used memory */
+        t_uint32 accumulate_free_memory; //!< accumulate free memory
+        t_uint32 accumulate_used_memory; //!< accumulate used memory
+    } global;
+
+    struct {
+        t_uint32 size;       //!< size of the domain
+        t_uint32 maximum_free_size; //!< maximum free size in the given domain
+        t_uint32 minimum_free_size; //!< minimum free size in the given domain
+        t_uint32 accumulate_free_memory; //all free memory of the given domain
+        t_uint32 accumulate_used_memory; //all used memory of the given domain
+    } domain;
+
+    struct {
+        t_uint32 sizes[3];
+    } stack[NB_CORE_IDS];
+
+} t_cm_allocator_status;
+
+#endif /* __INC_MEMORY_TYPE_H */
+
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/migration.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/migration.h
new file mode 100644
index 00000000000..824d25374b3
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/migration.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Migration API.
+ *
+ * \defgroup
+ *
+ */
+#ifndef __INC_MIGRATION_H
+#define __INC_MIGRATION_H
+
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+typedef enum {
+    STATE_MIGRATED = 1,
+    STATE_NORMAL   = 0,
+} t_cm_migration_state;
+
+PUBLIC t_cm_error cm_migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst);
+
+PUBLIC t_cm_error cm_unmigrate(void);
+
+PUBLIC t_uint32 cm_migration_translate(t_dsp_segment_type segmentType, t_uint32 addr);
+
+PUBLIC void cm_migration_check_state(t_nmf_core_id coreId, t_cm_migration_state expected);
+
+#endif /* __INC_MIGRATION_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator.h
new file mode 100644
index 00000000000..36994f37daa
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator.h
@@ -0,0 +1,275 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ *
+ * \note: In this module, we assume that parameters were checked !!
+ */
+#ifndef __REMOTE_ALLOCATOR_H_
+#define __REMOTE_ALLOCATOR_H_
+
+/*
+ * Include
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+
+
+/*
+ * Description of the memory block status
+ */
+typedef enum {
+    MEM_USED = 0,     /* Memory block is used */
+    MEM_FREE = 1     /* Memory block is free */
+} t_mem_status;
+
+/*
+ * Chunk structure.
+ */
+struct cm_allocator_desc;
+typedef struct chunk_struct
+{
+    /* Double linked list of chunks */
+    struct chunk_struct *prev;
+    struct chunk_struct *next;
+
+    /* Double linked list of free memory */
+    struct chunk_struct *prev_free_mem;
+    struct chunk_struct *next_free_mem;
+
+    /* Offset of the block memory */
+    t_uint32 offset;
+
+    /* Size of the block memory */
+    t_cm_size size;
+
+    /* Status of the block memory */
+    t_mem_status status;
+
+    /* User data */
+    t_uint16 userData;
+
+    /* Alloc debug info*/
+    t_uint32 domainId;
+
+    /* Alloc desc backlink */
+    struct cm_allocator_desc *alloc;
+} t_cm_chunk;
+
+/*!
+ * \brief Identifier of an internal memory handle
+ * \ingroup MEMORY_INTERNAL
+ */
+typedef t_cm_chunk* t_memory_handle;
+
+#define INVALID_MEMORY_HANDLE ((t_cm_chunk*)NULL)
+
+
+/*
+ * Context structure
+ */
+#define BINS                    63
+
+//TODO, juraj, add memType to alloc struct ?
+typedef struct cm_allocator_desc {
+    const char     *pAllocName;             /* Name of the allocator */
+    t_uint32        maxSize;                /* Max size of the allocator -> Potentially increase/decrease by stack management */
+    t_uint32        sbrkSize;               /* Current size of allocator */
+    t_uint32        offset;                 /* Offset of the allocator */
+    t_cm_chunk     *chunks;                 /* Array of chunk */
+    t_cm_chunk     *lastChunk;              /* Null terminated last chunk of previous array declaration */
+    t_cm_chunk     *free_mem_chunks[BINS];  /* List of free memory */
+    struct cm_allocator_desc* next;         /* List of allocator */
+} t_cm_allocator_desc;
+
+int bin_index(unsigned int sz);
+
+/*
+ * Functions
+ */
+/*!
+ * \brief Create a new allocator for a piece of memory (hw mapped (xram, yram))
+ * Any further allocation into this piece of memory will return an offset inside it.
+ * (a constant offset value can be added to this offset)
+ *
+ * \retval t_cm_allocator_desc*  new memory allocator identifier
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_cm_allocator_desc* cm_MM_CreateAllocator(
+        t_cm_size size,                     //!< [in] Size of the addressable space in bytes
+        t_uint32 offset,                    //!< [in] Constant offset to add to each allocated block base address
+        const char* name                    //!< [in] Name of the allocator
+        );
+
+/*!
+ * \brief Free a memory allocator descriptor
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_cm_error cm_MM_DeleteAllocator(
+        t_cm_allocator_desc* alloc          //!< [in] Identifier of the memory allocator to be freed
+        );
+
+
+/*!
+ * \brief Resize an allocator to the size value.
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_cm_error cm_MM_ResizeAllocator(
+        t_cm_allocator_desc* alloc,         //!< [in] Identifier of the memory allocator used to allocate the piece of memory
+        t_cm_size size                      //!< [in] Size of the addressable space in allocDesc granularity
+        );
+
+/*!
+ * \brief Check validity of a user handle
+ */
+t_cm_error cm_MM_getValidMemoryHandle(t_cm_memory_handle handle, t_memory_handle* validHandle);
+
+/*!
+ * \brief Wrapper routine to allocate some memory into a given allocator
+ *
+ * \retval t_memory_handle handle on the new allocated piece of memory
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_memory_handle cm_MM_Alloc(
+        t_cm_allocator_desc* alloc,             //!< [in] Identifier of the memory allocator
+        t_cm_size size,                         //!< [in] Size of the addressable space
+        t_cm_memory_alignment memAlignment,     //!< [in] Alignment constraint
+        t_uint32 seg_offset,                    //!< [in] Offset of range where allocating
+        t_uint32 seg_size,                      //!< [in] Size of range where allocating
+        t_uint32 domainId
+        );
+
+
+/*!
+ * \brief Routine to reallocate memory for a given handle
+ *
+ * Routine to reallocate memory for a given handle. The chunk can be extended or shrinked in both
+ * directions - top and bottom, depending on the offset and size arguments.
+ *
+ * \retval t_memory_handle handle on the reallocated piece of memory
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_cm_error cm_MM_Realloc(
+                t_cm_allocator_desc* alloc,
+                const t_cm_size size,
+                const t_uint32 offset,
+                t_memory_handle *handle);
+/*!
+ * \brief Frees the allocated chunk
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC void cm_MM_Free(
+        t_cm_allocator_desc* alloc,             //!< [in] Identifier of the memory allocator
+        t_memory_handle memHandle               //!< [in] Memory handle to free
+        );
+
+
+/*!
+ * \brief Get the allocator status
+ *
+ * \param[in] alloc Identifier of the memory allocator
+ * \param[out] pStatus Status of the allocator
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_cm_error cm_MM_GetAllocatorStatus(t_cm_allocator_desc* alloc, t_uint32 offset, t_uint32 size, t_cm_allocator_status *pStatus);
+
+/*!
+ * \brief Returns the offset into a given memory allocator of an allocated piece of memory
+ *
+ * \param[in] memHandle handle on the given memory
+ *
+ * \retval t_uint32  offset into the given memory allocator
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_uint32 cm_MM_GetOffset(t_memory_handle memHandle);
+
+
+/*!
+ * \brief Returns the size in word size for a given memory allocator of an allocated piece of memory
+ *
+ * \param[in] memHandle handle on the given memory
+ *
+ * \retval t_uint32  size in wordsize for the given memory allocator
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_uint32 cm_MM_GetSize(t_memory_handle memHandle);
+
+/*!
+ * \brief Returns the size in bytes for a given memory allocator
+ *
+ * \param[in] allocDesc Identifier of the memory allocator
+ * \retval size
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC t_uint32 cm_MM_GetAllocatorSize(t_cm_allocator_desc* allocDesc);
+
+
+/*!
+ * \brief Set the user data of an allocated piece of memory
+ *
+ * \param[in] memHandle handle on the given memory
+ * \param[in] userData UsedData of the given memory piece
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC void cm_MM_SetMemoryHandleUserData (t_memory_handle memHandle, t_uint16 userData);
+
+
+/*!
+ * \brief Return the user data of an allocated piece of memory
+ *
+ * \param[in] memHandle handle on the given memory
+ * \param[out] pUserData returned UsedData of the given memory piece
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC void cm_MM_GetMemoryHandleUserData(t_memory_handle memHandle, t_uint16 *pUserData, t_cm_allocator_desc **alloc);
+
+/*!
+ * \brief Dump chunkd in the range of [start:end]
+ *
+ * \param[in] alloc Allocator descriptor
+ * \param[in] start Range start
+ * \param[in] end Range end
+ *
+ * \retval void
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC void cm_MM_DumpMemory(t_cm_allocator_desc* alloc, t_uint32 start, t_uint32 end);
+
+/*!
+ * \brief Change the domain for the given chunk of memory
+ *
+ * \param[in] memHandle The given chunk of memory
+ * \param[in] domainId  The new domain id to set
+ *
+ * \retval void
+ *
+ * \ingroup MEMORY_INTERNAL
+ */
+PUBLIC void cm_MM_SetDefaultDomain(t_memory_handle memHandle, t_uint32 domainId);
+#endif /* _REMOTE_ALLOCATOR_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator_utils.h b/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator_utils.h
new file mode 100644
index 00000000000..0a7c901187b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/inc/remote_allocator_utils.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef REMOTE_ALLOCATOR_UTILS_H_
+#define REMOTE_ALLOCATOR_UTILS_H_
+
+#include <cm/engine/memory/inc/remote_allocator.h>
+#include <cm/engine/memory/inc/chunk_mgr.h>
+
+typedef enum {
+    FREE_CHUNK_BEFORE,
+    FREE_CHUNK_AFTER,
+} t_mem_split_position;
+
+
+PUBLIC void updateFreeList(t_cm_allocator_desc* alloc, t_cm_chunk* chunk);
+
+PUBLIC void linkChunk(t_cm_allocator_desc* alloc, t_cm_chunk* prev,t_cm_chunk* add);
+PUBLIC void unlinkChunk(t_cm_allocator_desc* alloc,t_cm_chunk* current);
+
+PUBLIC void unlinkFreeMem(t_cm_allocator_desc* alloc,t_cm_chunk* current);
+PUBLIC void linkFreeMemBefore(t_cm_chunk* add, t_cm_chunk* next);
+PUBLIC void linkFreeMemAfter(t_cm_chunk* prev,t_cm_chunk* add);
+
+PUBLIC t_cm_chunk* splitChunk(t_cm_allocator_desc* alloc, t_cm_chunk *chunk, t_uint32 offset, t_mem_split_position position);
+
+#endif /*REMOTE_ALLOCATOR_UTILS_H_*/
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/chunk_mgr.c b/drivers/staging/nmf-cm/cm/engine/memory/src/chunk_mgr.c
new file mode 100644
index 00000000000..78a549a74ce
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/chunk_mgr.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ * Include
+ */
+#include <cm/inc/cm_type.h>
+#include "../inc/chunk_mgr.h"
+#include <cm/engine/trace/inc/trace.h>
+
+#define CHUNKS_PER_PAGE 500
+#define CHUNK_THRESOLD 5
+
+struct t_page_chuncks {
+    struct t_page_chuncks   *nextPage;
+  //  unsigned int            freeChunkInPage;
+    t_cm_chunk              chunks[CHUNKS_PER_PAGE];
+};
+
+static struct t_page_chuncks   *firstPage = 0;
+
+static unsigned int            freeChunks = 0;
+static t_cm_chunk              *firstFreeChunk = 0;
+
+t_cm_chunk* allocChunk()
+{
+    t_cm_chunk* chunk = firstFreeChunk;
+
+    firstFreeChunk = chunk->next;
+
+    chunk->next_free_mem    = 0;
+    chunk->prev_free_mem    = 0;
+    chunk->prev             = 0;
+    chunk->next             = 0;
+    chunk->status           = MEM_FREE;
+    // chunk->offset = 0;
+    // chunk->size   = 0;
+    // chunk->alloc  = 0;
+    // chunk->userData = 0;
+
+    freeChunks--;
+
+    return chunk;
+}
+
+void freeChunk(t_cm_chunk* chunk)
+{
+    // Link chunk in free list
+    chunk->next = firstFreeChunk;
+    firstFreeChunk = chunk;
+
+    // Increase counter
+    freeChunks++;
+}
+
+t_cm_error allocChunkPool(void)
+{
+    struct t_page_chuncks* newPage;
+    int i;
+
+    newPage = (struct t_page_chuncks*)OSAL_Alloc(sizeof(struct t_page_chuncks));
+    if(newPage == NULL)
+        return CM_NO_MORE_MEMORY;
+
+    // Link page
+    newPage->nextPage = firstPage;
+    firstPage = newPage;
+
+    // Put chunk in free list
+    for(i = 0; i < CHUNKS_PER_PAGE; i++)
+        freeChunk(&newPage->chunks[i]);
+
+    return CM_OK;
+}
+
+t_cm_error fillChunkPool(void)
+{
+    if(freeChunks < CHUNK_THRESOLD)
+        return allocChunkPool();
+
+    return CM_OK;
+}
+
+void freeChunkPool(void)
+{
+    while(firstPage != NULL)
+    {
+        struct t_page_chuncks* tofree = firstPage;
+        firstPage = firstPage->nextPage;
+        OSAL_Free(tofree);
+    }
+
+    firstFreeChunk = 0;
+    freeChunks = 0;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/domain.c b/drivers/staging/nmf-cm/cm/engine/memory/src/domain.c
new file mode 100644
index 00000000000..a605cc475a9
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/domain.c
@@ -0,0 +1,608 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <inc/nmf-limits.h>
+
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/memory/inc/chunk_mgr.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/power_mgt/inc/power.h>
+#include <cm/engine/trace/inc/trace.h>
+
+/*
+ * domain_memory structure is all we need
+ */
+#define MAX_USER_DOMAIN_NB 64
+#define MAX_SCRATCH_DOMAIN_NB 16
+
+t_cm_domain_desc domainDesc[MAX_USER_DOMAIN_NB];
+t_cm_domain_scratch_desc domainScratchDesc[MAX_SCRATCH_DOMAIN_NB];
+
+static t_cm_allocator_desc *cm_DM_getAllocator(t_cm_domain_id domainId, t_dsp_memory_type_id memType);
+static void cm_DM_DomainError(const t_cm_domain_id parentId, const t_nmf_client_id client);
+
+#define INIT_DOMAIN_STRUCT(domainDesc)        do {       \
+    domainDesc.client                   = 0;             \
+    domainDesc.type                     = DOMAIN_NORMAL; \
+    domainDesc.refcount                 = 0;             \
+    domainDesc.domain.coreId            = MASK_ALL8;     \
+    domainDesc.domain.esramCode.offset  = 0;             \
+    domainDesc.domain.esramCode.size    = 0;             \
+    domainDesc.domain.esramData.offset  = 0;             \
+    domainDesc.domain.esramData.size    = 0;             \
+    domainDesc.domain.sdramCode.offset  = 0;             \
+    domainDesc.domain.sdramCode.size    = 0;             \
+    domainDesc.domain.sdramData.offset  = 0;             \
+    domainDesc.domain.sdramData.size    = 0;             \
+    domainDesc.scratch.parent.handle    = 0;             \
+    domainDesc.scratch.child.alloc      = 0;             \
+    domainDesc.scratch.child.parent_ref = 0;             \
+    domainDesc.dbgCooky                 = NULL;          \
+    } while (0)
+
+#define FIND_DOMAIN_ID(domainId)                                                           \
+    {                                                                                      \
+        domainId = 0;                                                                      \
+        while (domainDesc[domainId].client != 0 && domainId < MAX_USER_DOMAIN_NB) {     \
+            domainId++;                                                                    \
+        }                                                                                  \
+        if (domainId >= MAX_USER_DOMAIN_NB) {                                              \
+            return CM_INTERNAL_DOMAIN_OVERFLOW;                                            \
+        }                                                                                  \
+    }
+
+#define FIND_SCRATCH_DOMAIN_ID(domainId)                                                   \
+    {                                                                                      \
+        domainId = 0;                                                                      \
+        while (domainScratchDesc[domainId].allocDesc != 0 && domainId < MAX_SCRATCH_DOMAIN_NB) {     \
+            domainId++;                                                                    \
+        }                                                                                  \
+        if (domainId >= MAX_SCRATCH_DOMAIN_NB) {                                           \
+            return CM_INTERNAL_DOMAIN_OVERFLOW;                                            \
+        }                                                                                  \
+    }
+
+PUBLIC t_cm_error cm_DM_CheckDomain(t_cm_domain_id handle, t_cm_domain_type type)
+{
+    if ((handle <= 3)
+            || (handle >= MAX_USER_DOMAIN_NB)) { //remember, domain[0-3] are reserved
+        return CM_INVALID_DOMAIN_HANDLE;
+    }
+
+    if (domainDesc[handle].client == 0) {
+        return CM_INVALID_DOMAIN_HANDLE;
+    }
+
+    if (type != DOMAIN_ANY) {
+        if (domainDesc[handle].type != type) {
+            return CM_INVALID_DOMAIN_HANDLE;
+        }
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DM_CheckDomainWithClient(t_cm_domain_id handle, t_cm_domain_type type, t_nmf_client_id client)
+{
+    t_cm_error error;
+
+    if((error = cm_DM_CheckDomain(handle, type)) != CM_OK)
+        return error;
+
+#ifdef CHECK_TO_BE_REACTIVATED_IN_2_11
+    if(domainDesc[handle].client != client)
+    {
+        ERROR("CM_DOMAIN_VIOLATION: domain %d created by client %d not usable by client %d.", handle, domainDesc[handle].client, client, 0, 0, 0);
+        return CM_DOMAIN_VIOLATION;
+    }
+#endif
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_DM_Init(void)
+{
+    t_cm_error error;
+
+    int i = 0;
+    for(i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+        INIT_DOMAIN_STRUCT(domainDesc[i]);
+    }
+
+    //domains[0-3] are reserved - allows to catch some cases of incorrect usage,
+    //especially when user uses coreId instead of domainId, ie id = 1, 2, 3
+    domainDesc[0].client = NMF_CORE_CLIENT;
+    domainDesc[1].client = NMF_CORE_CLIENT;
+    domainDesc[2].client = NMF_CORE_CLIENT;
+    domainDesc[3].client = NMF_CORE_CLIENT;
+
+    /* We use domain 1 and 2 for the singleton, only used for components structure */
+    domainDesc[DEFAULT_SVA_DOMAIN].type = DOMAIN_NORMAL;
+    domainDesc[DEFAULT_SVA_DOMAIN].domain.coreId= SVA_CORE_ID;
+    domainDesc[DEFAULT_SVA_DOMAIN].domain.esramCode.size = (t_uint32)-1;
+    domainDesc[DEFAULT_SVA_DOMAIN].domain.esramData.size = (t_uint32)-1;
+    domainDesc[DEFAULT_SVA_DOMAIN].domain.sdramCode.size = (t_uint32)-1;
+    domainDesc[DEFAULT_SVA_DOMAIN].domain.sdramData.size = (t_uint32)-1;
+    domainDesc[DEFAULT_SIA_DOMAIN].type = DOMAIN_NORMAL;
+    domainDesc[DEFAULT_SIA_DOMAIN].domain.coreId= SIA_CORE_ID;
+    domainDesc[DEFAULT_SIA_DOMAIN].domain.esramCode.size = (t_uint32)-1;
+    domainDesc[DEFAULT_SIA_DOMAIN].domain.esramData.size = (t_uint32)-1;
+    domainDesc[DEFAULT_SIA_DOMAIN].domain.sdramCode.size = (t_uint32)-1;
+    domainDesc[DEFAULT_SIA_DOMAIN].domain.sdramData.size = (t_uint32)-1;
+
+    for(i = 0; i < MAX_SCRATCH_DOMAIN_NB; i++) {
+        domainScratchDesc[i].domainId  = 0;
+        domainScratchDesc[i].parentId  = 0;
+        domainScratchDesc[i].allocDesc = 0;
+    }
+
+    // Alloc twice for having comfortable chunk
+    if((error = allocChunkPool()) != CM_OK)
+        return error;
+    if((error = allocChunkPool()) != CM_OK)
+    {
+        freeChunkPool();
+        return error;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_DM_Destroy(void)
+{
+    //cm_DM_Init();
+    freeChunkPool();
+}
+
+PUBLIC t_nmf_core_id cm_DM_GetDomainCoreId(const t_cm_domain_id domainId)
+{
+
+    return domainDesc[domainId].domain.coreId;
+}
+
+#if 0
+static t_uint32 cm_DM_isSegmentOverlaping(const t_cm_domain_segment *d0, const t_cm_domain_segment *d1)
+{
+    t_uint32 min0 = d0->offset;
+    t_uint32 max0 = d0->offset + d0->size;
+    t_uint32 min1 = d1->offset;
+    t_uint32 max1 = d1->offset + d1->size;
+
+    if ( (min0 < min1) && (min1 < max0) ){    /* min0 < min1 < max0  OR  min1 in [min0:max0] */
+        return 1;
+    }
+    if ( (min1 < min0) && (min0 <= max1) ){   /* min1 < min0 < max0  OR  min0 in [min1:max1] */
+        return 1;
+    }
+
+    return 0;
+}
+{
+    ...
+
+    t_uint32 i;
+    //check non-overlapp with other domains
+    for (i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+        if (domainDesc[i].client != 0) {
+            if (cm_DM_isSegmentOverlaping(&domainDesc[i].domain.esramData, &domain->esramData)) {
+                return CM_DOMAIN_OVERLAP;
+            }
+            /*
+            if (cm_DM_isSegmentOverlaping(&domainDesc[i].domain.esramData, &domain->esramData)) {
+                return CM_DOMAIN_OVERLAP;
+            }
+            */
+        }
+    }
+
+    ...
+}
+#endif
+
+PUBLIC t_cm_error cm_DM_CreateDomain(const t_nmf_client_id client, const t_cm_domain_memory *domain, t_cm_domain_id *handle)
+{
+    t_cm_domain_id domainId;
+    FIND_DOMAIN_ID(domainId);
+
+    if (client == 0)
+        return CM_INVALID_PARAMETER;
+
+    if (domain->coreId > LAST_CORE_ID)
+        return CM_INVALID_DOMAIN_DEFINITION;
+
+    //FIXME, juraj, check invalid domain definition
+    domainDesc[domainId].client = client;
+    domainDesc[domainId].domain = *domain;
+
+    if (osal_debug_ops.domain_create)
+	    osal_debug_ops.domain_create(domainId);
+
+    *handle = domainId;
+
+    return CM_OK;
+}
+
+//TODO, juraj, add assert to cm_MM_GetOffset(), if domain is scratch parent
+PUBLIC t_cm_error cm_DM_CreateDomainScratch(const t_nmf_client_id client, const t_cm_domain_id parentId, const t_cm_domain_memory *domain, t_cm_domain_id *handle)
+{
+    t_cm_error error;
+    t_memory_handle memhandle;
+    t_cm_allocator_desc *alloc;
+    t_uint32 parentMin, parentMax;
+    t_uint32 scratchMin, scratchMax;
+
+    /* check if the parent domain exists */
+    /* parent could be DOMAIN_NORMAL (1st call) or DOMAIN_SCRATCH_PARENT (other calls) */
+    if ((error = cm_DM_CheckDomain(parentId, DOMAIN_ANY)) != CM_OK) {
+        return error;
+    }
+
+    parentMin = domainDesc[parentId].domain.esramData.offset;
+    parentMax = domainDesc[parentId].domain.esramData.offset + domainDesc[parentId].domain.esramData.size;
+    scratchMin = domain->esramData.offset;
+    scratchMax = domain->esramData.offset + domain->esramData.size;
+    /* check if the scratch domain respects the parent domain (esram data only )*/
+    if ( (parentMin > scratchMin) || (parentMax < scratchMax) ) {
+        return CM_INVALID_DOMAIN_DEFINITION;
+    }
+
+    /* create the scratch domain */
+    if ((error = cm_DM_CreateDomain(client, domain, handle)) != CM_OK) {
+        return error;
+    }
+
+    /* check if this is the first scratch domain */
+    if (domainDesc[parentId].scratch.parent.handle == 0) {
+        /* 1st scratch domain */
+        t_cm_domain_segment tmp;
+
+        /* reserve the zone for the scratch domain */
+        tmp = domainDesc[parentId].domain.esramData;
+        domainDesc[parentId].domain.esramData = domain->esramData;
+        memhandle = cm_DM_Alloc(parentId, ESRAM_EXT16, domain->esramData.size / 2, CM_MM_ALIGN_NONE, FALSE); //note byte to 16bit-word conversion
+        domainDesc[parentId].domain.esramData = tmp;
+        if (memhandle == 0) {
+            cm_DM_DestroyDomain(*handle);
+            cm_DM_DomainError(parentId, client);
+            return CM_NO_MORE_MEMORY;
+        }
+
+        domainDesc[parentId].type = DOMAIN_SCRATCH_PARENT;
+        domainDesc[parentId].refcount = 0; //reinit the refcount
+        domainDesc[parentId].scratch.parent.handle = memhandle;
+
+    } else {
+        /* nth scratch domain */
+        t_uint32 i;
+        t_uint32 oldMin = domainDesc[parentId].domain.esramData.offset + domainDesc[parentId].domain.esramData.offset;
+        t_uint32 oldMax = 0;
+
+        /* compute the new scratch zone size */
+        for(i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+            if ((domainDesc[i].type == DOMAIN_SCRATCH_CHILD) && (domainDesc[i].scratch.child.parent_ref == parentId)) {
+                /* ok, here we have a scratch domain created from the same child domain */
+                t_uint32 min = domainDesc[i].domain.esramData.offset;
+                t_uint32 max = domainDesc[i].domain.esramData.offset + domainDesc[i].domain.esramData.size;
+
+                oldMin = (min < oldMin)?min:oldMin;
+                oldMax = (max > oldMax)?max:oldMax;
+            }
+        }
+
+        /* resize the scratch zone */
+        if ((oldMin > scratchMin) || (oldMax < scratchMax)) {
+            t_uint32 newMin = (oldMin > scratchMin)?scratchMin:oldMin;
+            t_uint32 newMax = (oldMax < scratchMax)?scratchMax:oldMax;
+
+            if(cm_MM_Realloc(cm_DM_getAllocator(parentId, ESRAM_EXT16), newMax - newMin, newMin,
+                                      &domainDesc[parentId].scratch.parent.handle) != CM_OK)
+            {
+                /* failed to extend the zone */
+                cm_DM_DestroyDomain(*handle);
+                cm_DM_DomainError(parentId, client);
+                return CM_NO_MORE_MEMORY;
+            }
+        }
+    }
+
+    /* create esram-data allocator in the scratch domain */
+    alloc = cm_MM_CreateAllocator(domainDesc[*handle].domain.esramData.size,
+                                  domainDesc[*handle].domain.esramData.offset,
+                                  "scratch");
+
+    domainDesc[*handle].type = DOMAIN_SCRATCH_CHILD;
+    domainDesc[*handle].scratch.child.parent_ref = parentId;
+    domainDesc[*handle].scratch.child.alloc = alloc;
+    domainDesc[parentId].refcount++;
+
+    return error;
+}
+
+PUBLIC t_cm_error cm_DM_DestroyDomains(const t_nmf_client_id client)
+{
+    t_cm_domain_id handle;
+    t_cm_error error, status=CM_OK;
+
+    for (handle=0; handle<MAX_USER_DOMAIN_NB; handle++) {
+        if ((domainDesc[handle].client == client)
+            && ((error=cm_DM_DestroyDomain(handle)) != CM_OK)) {
+            LOG_INTERNAL(0, "Error (%d) destroying remaining domainId %d for client %u\n", error, handle, client, 0, 0, 0);
+            status = error;
+        }
+    }
+    return status;
+}
+
+PUBLIC t_cm_error cm_DM_DestroyDomain(t_cm_domain_id handle)
+{
+    t_cm_error error = CM_OK;
+    t_uint32 i;
+
+    if ((error = cm_DM_CheckDomain(handle, DOMAIN_ANY)) != CM_OK) {
+        return error;
+    }
+
+    //forbid destruction of cm domains
+    //if (handle == cm_DSP_GetState(domainDesc[handle].domain.coreId)->domainEE)
+    //    return CM_INVALID_DOMAIN_HANDLE;
+
+    /* loop all components and check if there are still components instantiated with this handle */
+    //actually this check is redundant with the usage counters as component instantiations allocate memory
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	    if (NULL != componentEntry(i) && componentEntry(i)->domainId == handle) {
+            return CM_ILLEGAL_DOMAIN_OPERATION;
+        }
+    }
+
+    //perform check based on usage counters
+    if (domainDesc[handle].refcount != 0) {
+        return CM_ILLEGAL_DOMAIN_OPERATION;
+    }
+
+    if (domainDesc[handle].type == DOMAIN_SCRATCH_PARENT) {
+        return CM_ILLEGAL_DOMAIN_OPERATION; //parent destroyed implicitly with the last scratch
+    } else if (domainDesc[handle].type == DOMAIN_SCRATCH_CHILD) {
+        t_cm_allocator_status status;
+        t_cm_domain_id parentId = domainDesc[handle].scratch.child.parent_ref;
+
+        cm_MM_GetAllocatorStatus(domainDesc[handle].scratch.child.alloc, 0, 0xffff, &status);
+        if (status.global.accumulate_used_memory != 0) {
+            //something is still allocated
+            return CM_ILLEGAL_DOMAIN_OPERATION;
+        }
+
+        domainDesc[parentId].refcount--;
+        cm_MM_DeleteAllocator(domainDesc[handle].scratch.child.alloc); //returns no error
+
+        if (domainDesc[parentId].refcount == 0) {
+            /* last scratch domain */
+            cm_DM_Free(domainDesc[parentId].scratch.parent.handle, FALSE);
+            domainDesc[parentId].scratch.parent.handle = 0;
+            domainDesc[parentId].type = DOMAIN_NORMAL;
+        } else {
+            /* other child scratch domains exist, check if the reserved zone needs resize, ie reduce */
+
+            t_uint32 i;
+            /* init oldMin and oldMax to values we are sure will get overwritten below */
+            t_uint32 oldMin = 0xffffffff;
+            t_uint32 oldMax = 0x0;
+            t_uint32 scratchMin = domainDesc[handle].domain.esramData.offset;
+            t_uint32 scratchMax = domainDesc[handle].domain.esramData.offset + domainDesc[handle].domain.esramData.size;
+
+            /* compute the remaining reserved zone size */
+            for(i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+                if (i == handle)
+                    continue; //do not consider the current domain to be destroyed later in this function
+                if ((domainDesc[i].type == DOMAIN_SCRATCH_CHILD) && (domainDesc[i].scratch.child.parent_ref == parentId)) {
+                    /* ok, here we have a scratch domain created from the same child domain */
+                    t_uint32 min = domainDesc[i].domain.esramData.offset;
+                    t_uint32 max = domainDesc[i].domain.esramData.offset + domainDesc[i].domain.esramData.size;
+
+                    oldMin = (min < oldMin)?min:oldMin;
+                    oldMax = (max > oldMax)?max:oldMax;
+                }
+            }
+
+            /* resize the scratch zone */
+            if ((oldMin > scratchMin) || (oldMax < scratchMax)) {
+                CM_ASSERT(cm_MM_Realloc(cm_DM_getAllocator(parentId, ESRAM_EXT16), oldMax - oldMin, oldMin,
+                                          &domainDesc[parentId].scratch.parent.handle) == CM_OK); //the realloc shouldn't fail..
+            }
+        }
+    }
+
+    if (osal_debug_ops.domain_destroy)
+	    osal_debug_ops.domain_destroy(handle);
+
+    //reset the domain desc
+    INIT_DOMAIN_STRUCT(domainDesc[handle]);
+
+    return CM_OK;
+}
+
+/*
+ *   - if the domainId is scratch parent, all allocations are done as in normal domains
+ *   - if the domainId is scratch child
+ *          if allocation type is esram, retrieve the allocator from the domainDesc
+ *          else allocation is done as for normal domain
+ *   - if the domainId is normal, allocator is retrieved from mpcDesc via cm_DSP_GetAllocator()
+ */
+static t_cm_allocator_desc *cm_DM_getAllocator(t_cm_domain_id domainId, t_dsp_memory_type_id memType)
+{
+    t_cm_allocator_desc *alloc = 0;
+
+    if ((domainDesc[domainId].type == DOMAIN_SCRATCH_CHILD)
+        && ((memType == ESRAM_EXT16) || (memType == ESRAM_EXT24))) {
+        alloc = domainDesc[domainId].scratch.child.alloc;
+    } else {
+        alloc = cm_DSP_GetAllocator(domainDesc[domainId].domain.coreId, memType);
+    }
+
+    return alloc;
+}
+
+void START(void);
+void END(const char*);
+
+//TODO, juraj, alloc would need to return finer errors then 0
+PUBLIC t_memory_handle cm_DM_Alloc(t_cm_domain_id domainId, t_dsp_memory_type_id memType, t_uint32 wordSize, t_cm_mpc_memory_alignment memAlignment,  t_bool powerOn)
+{
+    t_nmf_core_id coreId = domainDesc[domainId].domain.coreId;
+    t_memory_handle handle;
+    t_cm_allocator_desc *alloc;
+    t_uint32 offset;
+    t_uint32 size;
+
+    cm_DSP_GetInternalMemoriesInfo(domainId, memType, &offset, &size);
+
+    if ((alloc = cm_DM_getAllocator(domainId, memType)) == 0) {
+        return 0;
+    }
+
+    handle = cm_MM_Alloc(alloc,
+            cm_DSP_ConvertSize(memType, wordSize),
+            (t_cm_memory_alignment) memAlignment,
+            offset, size, domainId);
+
+    if(handle != INVALID_MEMORY_HANDLE)
+    {
+        cm_MM_SetMemoryHandleUserData(handle, (coreId << SHIFT_BYTE1) | (memType << SHIFT_BYTE0));
+
+        if (powerOn) {
+            // [Pwr] The associated power domain can be enabled only after the Alloc request.
+            //       Associated MPC memory chunk is not accessed (Remote allocator feature)
+            cm_PWR_EnableMemory(
+                    coreId,
+                    memType,
+                    /*
+                     * Compute physical address based on cm_DSP_GetHostSystemAddress but in optimized way
+                     * -> See it for information
+                     * -> Note TCM memory is not correctly compute, but it's not used
+                     */
+                    cm_DSP_GetState(coreId)->allocator[memType]->baseAddress.physical + cm_MM_GetOffset(handle),
+                    cm_MM_GetSize(handle));
+        }
+    } else {
+        LOG_INTERNAL(0, "CM_NO_MORE_MEMORY domainId: %d, memType %d, wordSize %d, alignement %d\n",
+                domainId, memType, wordSize, memAlignment, 0, 0);
+        cm_MM_DumpMemory(alloc, offset, offset + size);
+    }
+
+    return handle;
+}
+
+PUBLIC void cm_DM_FreeWithInfo(t_memory_handle memHandle, t_nmf_core_id *coreId, t_dsp_memory_type_id *memType, t_bool powerOff)
+{
+    t_dsp_chunk_info chunk_info;
+
+    cm_DSP_GetDspChunkInfo(memHandle, &chunk_info);
+
+    if (powerOff) {
+        cm_PWR_DisableMemory(
+                chunk_info.coreId,
+                chunk_info.memType,
+                cm_DSP_GetPhysicalAdress(memHandle),
+                cm_MM_GetSize(memHandle));
+    }
+
+    cm_MM_Free(chunk_info.alloc, memHandle);
+
+    *coreId = chunk_info.coreId;
+    *memType = chunk_info.memType;
+}
+
+PUBLIC void cm_DM_Free(t_memory_handle memHandle, t_bool powerOff)
+{
+    t_nmf_core_id coreId;
+    t_dsp_memory_type_id memType;
+
+    cm_DM_FreeWithInfo(memHandle, &coreId, &memType, powerOff);
+}
+
+PUBLIC t_cm_error cm_DM_GetAllocatorStatus(t_cm_domain_id domainId, t_dsp_memory_type_id memType, t_cm_allocator_status *pStatus)
+{
+    t_cm_error error;
+    t_uint32 dOffset;
+    t_uint32 dSize;
+
+    //TODO, scratch
+    error = cm_DM_CheckDomain(domainId, DOMAIN_ANY);
+    if (error != CM_OK) {
+        return error;
+    }
+
+    cm_DSP_GetInternalMemoriesInfo(domainId, memType, &dOffset, &dSize);
+
+    return cm_DSP_GetAllocatorStatus(domainDesc[domainId].domain.coreId, memType,
+            dOffset, dSize, pStatus);
+}
+
+//WARNING: this function is only correct *before* migration! because
+//the computation of absolute adresses of a domain is based on the allocator for the given
+//segment (this is hidden in cm_DSP_GetDspBaseAddress and this info is not valid
+//after migration (non-contiguous address-space from the ARM-side)
+PUBLIC t_cm_error cm_DM_GetDomainAbsAdresses(t_cm_domain_id domainId, t_cm_domain_info *info)
+{
+    t_cm_error error;
+    t_nmf_core_id coreId = domainDesc[domainId].domain.coreId;
+
+    cm_migration_check_state(coreId, STATE_NORMAL);
+
+    error = cm_DM_CheckDomain(domainId, DOMAIN_NORMAL);
+    if (error != CM_OK) {
+        return error;
+    }
+
+    cm_DSP_GetDspBaseAddress(coreId, SDRAM_CODE,  &info->sdramCode);
+    cm_DSP_GetDspBaseAddress(coreId, ESRAM_CODE,  &info->esramCode);
+    cm_DSP_GetDspBaseAddress(coreId, SDRAM_EXT24, &info->sdramData);
+    cm_DSP_GetDspBaseAddress(coreId, ESRAM_EXT24, &info->esramData);
+
+    info->sdramCode.physical += domainDesc[domainId].domain.sdramCode.offset;
+    info->sdramCode.logical  += domainDesc[domainId].domain.sdramCode.offset;
+    info->esramCode.physical += domainDesc[domainId].domain.esramCode.offset;
+    info->esramCode.logical  += domainDesc[domainId].domain.esramCode.offset;
+    info->sdramData.physical += domainDesc[domainId].domain.sdramData.offset;
+    info->sdramData.logical  += domainDesc[domainId].domain.sdramData.offset;
+    info->esramData.physical += domainDesc[domainId].domain.esramData.offset;
+    info->esramData.logical  += domainDesc[domainId].domain.esramData.offset;
+
+    return CM_OK;
+}
+
+static void cm_DM_DomainError(const t_cm_domain_id parentId, const t_nmf_client_id client)
+{
+    int i;
+    LOG_INTERNAL(0, "NMF_DEBUG_SCRATCH failed to allocate domain (client %u): 0x%08x -> 0x%08x\n",
+            client,
+            domainDesc[parentId].domain.esramData.offset,
+            domainDesc[parentId].domain.esramData.offset + domainDesc[parentId].domain.esramData.size,
+            0, 0, 0);
+    for(i = 0; i < MAX_USER_DOMAIN_NB; i++) {
+        if (domainDesc[i].type == DOMAIN_SCRATCH_CHILD) {
+            LOG_INTERNAL(0, "NMF_DEBUG_SCRATCH scratch domain %d allocated (client %u): 0x%08x -> 0x%08x\n",
+                    i, domainDesc[i].client,
+                    domainDesc[i].domain.esramData.offset,
+                    domainDesc[i].domain.esramData.offset + domainDesc[i].domain.esramData.size,
+                    0, 0);
+        }
+    }
+    cm_MM_DumpMemory(cm_DM_getAllocator(parentId, ESRAM_EXT16),
+            domainDesc[parentId].domain.esramData.offset,
+            domainDesc[parentId].domain.esramData.offset + domainDesc[parentId].domain.esramData.size);
+}
+
+PUBLIC void cm_DM_SetDefaultDomain(t_memory_handle memHandle, t_nmf_core_id coreId)
+{
+	if (coreId == SVA_CORE_ID)
+		cm_MM_SetDefaultDomain(memHandle, DEFAULT_SVA_DOMAIN);
+	else if (coreId == SIA_CORE_ID)
+		cm_MM_SetDefaultDomain(memHandle, DEFAULT_SIA_DOMAIN);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/domain_wrapper.c b/drivers/staging/nmf-cm/cm/engine/memory/src/domain_wrapper.c
new file mode 100644
index 00000000000..ec305812f15
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/domain_wrapper.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/api/domain_engine.h>
+#include <cm/engine/api/migration_engine.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_CreateMemoryDomain(
+		const t_nmf_client_id     client,
+        const t_cm_domain_memory  *domain,
+        t_cm_domain_id            *handle
+        )
+{
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = cm_DM_CreateDomain(client, domain, handle);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_CreateMemoryDomainScratch(
+        const t_nmf_client_id     client,
+        const t_cm_domain_id parentId,
+        const t_cm_domain_memory  *domain,
+        t_cm_domain_id            *handle
+        )
+{
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = cm_DM_CreateDomainScratch(client, parentId, domain, handle);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_DestroyMemoryDomain(
+        t_cm_domain_id handle)
+{
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = cm_DM_DestroyDomain(handle);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_FlushMemoryDomains(
+        t_nmf_client_id client)
+{
+    t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = cm_DM_DestroyDomains(client);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetDomainCoreId(const t_cm_domain_id domainId, t_nmf_core_id *coreId)
+{
+    t_cm_error error;
+    OSAL_LOCK_API();
+    //TODO, scratch
+    error = cm_DM_CheckDomain(domainId, DOMAIN_NORMAL);
+    if (error != CM_OK) {
+        OSAL_UNLOCK_API();
+        return error;
+    }
+
+    *coreId = cm_DM_GetDomainCoreId(domainId);
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_Migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst)
+{
+    t_cm_error error;
+    OSAL_LOCK_API();
+    error = cm_migrate(srcShared, src, dst);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_Unmigrate(void)
+{
+    t_cm_error error;
+    OSAL_LOCK_API();
+    error = cm_unmigrate();
+    OSAL_UNLOCK_API();
+    return error;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/memory_wrapper.c b/drivers/staging/nmf-cm/cm/engine/memory/src/memory_wrapper.c
new file mode 100644
index 00000000000..37bea690e48
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/memory_wrapper.c
@@ -0,0 +1,222 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/component/inc/instance.h>
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/trace/inc/trace.h>
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_AllocMpcMemory(
+        t_cm_domain_id domainId,
+        t_nmf_client_id clientId,
+        t_cm_mpc_memory_type memType,
+        t_cm_size size,
+        t_cm_mpc_memory_alignment memAlignment,
+        t_cm_memory_handle *pHandle
+        )
+{
+    t_dsp_memory_type_id dspMemType;
+
+    switch(memType)
+    {
+        case CM_MM_MPC_TCM16_X:
+            dspMemType = INTERNAL_XRAM16;
+            break;
+        case CM_MM_MPC_TCM24_X:
+            dspMemType = INTERNAL_XRAM24;
+            break;
+        case CM_MM_MPC_TCM16_Y:
+            dspMemType = INTERNAL_YRAM16;
+            break;
+        case CM_MM_MPC_TCM24_Y:
+            dspMemType = INTERNAL_YRAM24;
+            break;
+#ifndef __STN_8810
+        case CM_MM_MPC_ESRAM16:
+            dspMemType = ESRAM_EXT16;
+            break;
+        case CM_MM_MPC_ESRAM24:
+            dspMemType = ESRAM_EXT24;
+            break;
+#endif /* ndef __STN_8810 */
+        case CM_MM_MPC_SDRAM16:
+            dspMemType = SDRAM_EXT16;
+            break;
+        case CM_MM_MPC_SDRAM24:
+            dspMemType = SDRAM_EXT24;
+            break;
+        default:
+            return CM_INVALID_PARAMETER;
+    }
+
+    OSAL_LOCK_API();
+    {
+        t_cm_error error;
+        error = cm_DM_CheckDomainWithClient(domainId, DOMAIN_ANY, clientId);
+        if (error != CM_OK) {
+            OSAL_UNLOCK_API();
+            return error;
+        }
+    }
+
+    switch(memAlignment) {
+    case CM_MM_MPC_ALIGN_NONE        :
+    case CM_MM_MPC_ALIGN_HALFWORD    :
+    case CM_MM_MPC_ALIGN_WORD        :
+    case CM_MM_MPC_ALIGN_2WORDS      :
+    case CM_MM_MPC_ALIGN_4WORDS      :
+    case CM_MM_MPC_ALIGN_8WORDS      :
+    case CM_MM_MPC_ALIGN_16WORDS     :
+    case CM_MM_MPC_ALIGN_32WORDS     :
+    case CM_MM_MPC_ALIGN_64WORDS     :
+    case CM_MM_MPC_ALIGN_128WORDS    :
+    case CM_MM_MPC_ALIGN_256WORDS    :
+    case CM_MM_MPC_ALIGN_512WORDS    :
+    case CM_MM_MPC_ALIGN_1024WORDS   :
+    case CM_MM_MPC_ALIGN_65536BYTES  :
+    //case CM_MM_MPC_ALIGN_16384WORDS  : maps to the same value as above
+        break;
+    default:
+        OSAL_UNLOCK_API();
+        return CM_INVALID_PARAMETER;
+    }
+
+    /* in case we allocate in tcm x be sure ee is load before */
+    if ( memType == CM_MM_MPC_TCM16_X || memType == CM_MM_MPC_TCM24_X ||
+         memType == CM_MM_MPC_TCM16_Y || memType == CM_MM_MPC_TCM24_Y )
+    {
+        t_cm_error error;
+        if ((error = cm_CFG_CheckMpcStatus(cm_DM_GetDomainCoreId(domainId))) != CM_OK)
+        {
+            OSAL_UNLOCK_API();
+            return error;
+        }
+    }
+
+    /* alloc memory */
+    *pHandle = (t_cm_memory_handle)cm_DM_Alloc(domainId, dspMemType, size, memAlignment, TRUE);
+    if(*pHandle == (t_cm_memory_handle)INVALID_MEMORY_HANDLE)
+    {
+        OSAL_UNLOCK_API();
+        ERROR("CM_NO_MORE_MEMORY: CM_AllocMpcMemory() failed\n", 0, 0, 0, 0, 0, 0);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_FreeMpcMemory(t_cm_memory_handle handle)
+{
+    t_cm_error error;
+    t_memory_handle validHandle;
+    t_nmf_core_id coreId;
+    t_dsp_memory_type_id memType;
+
+    OSAL_LOCK_API();
+
+    if((error = cm_MM_getValidMemoryHandle(handle, &validHandle)) != CM_OK)
+    {
+        OSAL_UNLOCK_API();
+        return error;
+    }
+
+    cm_DM_FreeWithInfo(validHandle, &coreId, &memType, TRUE);
+
+    /* in case we allocate in tcm x be sure ee is load before */
+    if ( memType == INTERNAL_XRAM16 || memType == INTERNAL_XRAM24 ||
+            memType == INTERNAL_YRAM16 || memType == INTERNAL_YRAM24 )
+    {
+        cm_CFG_ReleaseMpc(coreId);
+    }
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemorySystemAddress(t_cm_memory_handle handle, t_cm_system_address *pSystemAddress)
+{
+    t_cm_error error;
+    t_memory_handle validHandle;
+
+    OSAL_LOCK_API();
+
+    if((error = cm_MM_getValidMemoryHandle(handle, &validHandle)) != CM_OK)
+    {
+        OSAL_UNLOCK_API();
+        return error;
+    }
+
+    cm_DSP_GetHostSystemAddress(validHandle, pSystemAddress);
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemoryMpcAddress(t_cm_memory_handle handle, t_uint32 *pDspAddress)
+{
+    t_cm_error error;
+    t_memory_handle validHandle;
+
+    OSAL_LOCK_API();
+
+    if((error = cm_MM_getValidMemoryHandle(handle, &validHandle)) != CM_OK)
+    {
+        OSAL_UNLOCK_API();
+        return error;
+    }
+
+    cm_DSP_GetDspAddress(validHandle, pDspAddress);
+
+    OSAL_UNLOCK_API();
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetMpcMemoryStatus(t_nmf_core_id coreId, t_cm_mpc_memory_type memType, t_cm_allocator_status *pStatus)
+{
+    t_dsp_memory_type_id dspMemType;
+    t_cm_error error;
+
+    switch(memType)
+    {
+        case CM_MM_MPC_TCM16_X:
+            dspMemType = INTERNAL_XRAM16;
+            break;
+        case CM_MM_MPC_TCM24_X:
+            dspMemType = INTERNAL_XRAM24;
+            break;
+        case CM_MM_MPC_TCM16_Y:
+            dspMemType = INTERNAL_YRAM16;
+            break;
+        case CM_MM_MPC_TCM24_Y:
+            dspMemType = INTERNAL_YRAM24;
+            break;
+#ifndef __STN_8810
+        case CM_MM_MPC_ESRAM16:
+            dspMemType = ESRAM_EXT16;
+            break;
+        case CM_MM_MPC_ESRAM24:
+            dspMemType = ESRAM_EXT24;
+            break;
+#endif /* ndef __STN_8810 */
+        case CM_MM_MPC_SDRAM16:
+            dspMemType = SDRAM_EXT16;
+            break;
+        case CM_MM_MPC_SDRAM24:
+            dspMemType = SDRAM_EXT24;
+            break;
+        default:
+            return CM_INVALID_PARAMETER;
+    }
+
+    OSAL_LOCK_API();
+    error = cm_DSP_GetAllocatorStatus(coreId, dspMemType, 0, 0, pStatus);
+    OSAL_UNLOCK_API();
+
+    return error;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/migration.c b/drivers/staging/nmf-cm/cm/engine/memory/src/migration.c
new file mode 100644
index 00000000000..d68898d830e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/migration.c
@@ -0,0 +1,392 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <inc/type.h>
+#include <inc/nmf-limits.h>
+
+#include <cm/engine/communication/fifo/inc/nmf_fifo_arm.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/memory/inc/domain.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/memory/inc/migration.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/mem.h>
+
+#if defined(__STN_8500) && (__STN_8500 > 10)
+
+typedef enum {
+    CM_MIGRATION_OK = 0,
+    CM_MIGRATION_INVALID_ARGUMENT = 1,
+    CM_MIGRATION_ERROR = 2,
+} t_cm_migration_error;
+
+extern t_nmf_fifo_arm_desc* mpc2mpcComsFifoId[NB_CORE_IDS][NB_CORE_IDS];
+
+/*!
+ * \brief Data structure representing a segment to migrate
+ *
+ * segment:
+ *      - used to determine which mmdsp-hw base is to be updated, index in mpcDesc->segments[] structure
+ *      * this is hard-coded in cm_migrate(), could be computed (would be nice) LIMITATION
+ * srcAdr.physical:
+ *      - new base setting
+ *      * computed from the src domain in cm_DM_GetAbsAdresses() which uses the start of the allocator for the memory
+ *        this is a LIMITATION, as this information is valid only before migration
+ * srcAdr.logical:
+ *      - cm_MemCopy()
+ *      * computed as srcAdr.logical
+ * dstAdr.physical: see srcAdr.physical
+ * dstAdr.logical:  see srcAdr.logical
+ * size:
+ *      - cm_MemCopy()
+ *      - setting the top when new base is set
+ */
+typedef struct {
+    t_dsp_segment_type       segment; //!< the link to the segment type
+    t_cm_system_address      srcAdr;  //!< source address
+    t_cm_system_address      dstAdr;  //!< destination address
+    t_uint32                 size;    //!< size of the segment
+} t_cm_migration_segment;
+
+/*!
+ * \brief Internal data structure 1/ during migration, and 2/ between migration and unmigration calls
+ *
+ * all needed information are computed before calling _cm_migration_move()
+ */
+typedef struct {
+    t_cm_migration_state    state;                            //!< migration state
+    t_nmf_core_id           coreId;                           //!< migration only on one mpc
+    t_cm_migration_segment  segments[NB_MIGRATION_SEGMENT];   //!< segments to migrate (selected on migration_move)
+    t_memory_handle         handles[NB_MIGRATION_SEGMENT];    //!< memory handles for destination chunks allocated prior migration
+} t_cm_migration_internal_state;
+
+static t_cm_migration_internal_state migrationState = {STATE_NORMAL, };
+
+static t_cm_error _cm_migration_initSegment(
+        t_dsp_segment_type      dspSegment,
+        t_cm_system_address    *srcAdr,
+        t_uint32                size,
+        t_cm_domain_id          dst,
+        t_cm_migration_internal_state *info
+        )
+{
+    t_cm_system_address      dstAdr;
+    t_cm_migration_segment  *segment = &info->segments[dspSegment];
+    t_memory_handle          handle;
+
+    handle = cm_DM_Alloc(dst, ESRAM_EXT16, size >> 1, CM_MM_ALIGN_AHB_BURST, TRUE); //note: byte to half-word conversion
+    if (handle == 0) {
+        ERROR("CM_NO_MORE_MEMORY: Unable to init segment for migration\n", 0, 0, 0, 0, 0, 0);
+        return CM_NO_MORE_MEMORY;
+    }
+
+    info->handles[dspSegment] = handle;
+
+    cm_DSP_GetHostSystemAddress(handle, &dstAdr);
+
+    segment->segment   = dspSegment; //this is redundant and could be avoided by recoding move(), but nice to have for debug
+    segment->size      = size;
+    segment->srcAdr    = *srcAdr;
+    segment->dstAdr    = dstAdr;
+
+    return CM_OK;
+}
+
+static void _cm_migration_releaseSegment(t_cm_migration_internal_state *info, t_dsp_segment_type segId)
+{
+    cm_DM_Free(info->handles[segId], TRUE);
+}
+
+static t_cm_migration_error _cm_migration_release(t_cm_migration_internal_state *info)
+{
+    t_uint32 i = 0;
+    for (i = 0; i < NB_MIGRATION_SEGMENT; i++) {
+        cm_DM_Free(info->handles[i], TRUE);
+    }
+
+    return CM_MIGRATION_OK;
+}
+
+#define SEGMENT_START(seg) \
+    seg.offset
+
+#define SEGMENT_END(seg) \
+    seg.offset + seg.size
+
+static t_cm_error _cm_migration_check(
+        const t_cm_domain_id srcShared,
+        const t_cm_domain_id src,
+        const t_cm_domain_id dst,
+        t_cm_migration_internal_state *info
+        )
+{
+    t_cm_error error = CM_OK;
+    t_cm_domain_info domainInfoSrc;
+    t_cm_domain_info domainInfoShared;
+    t_cm_domain_desc *domainEE;
+    t_cm_domain_desc *domainShared;
+    t_nmf_core_id coreId = cm_DM_GetDomainCoreId(src);
+
+    //coreIds in src, srcShared and dst match
+    if (!((domainDesc[src].domain.coreId == domainDesc[srcShared].domain.coreId)
+        && (domainDesc[src].domain.coreId == domainDesc[dst].domain.coreId))) {
+        return CM_INVALID_PARAMETER;
+    }
+
+    //check srcShared starts at 0
+    //FIXME, juraj, today EE code is in SDRAM, but this is flexible, so must find out where EE is instantiated
+    if (domainDesc[srcShared].domain.sdramCode.offset != 0x0) {
+        return CM_INVALID_PARAMETER;
+    }
+
+    //check srcShared contains EE domain
+    domainEE = &domainDesc[cm_DSP_GetState(coreId)->domainEE];
+    domainShared = &domainDesc[srcShared];
+    if ((SEGMENT_START(domainEE->domain.esramCode) < SEGMENT_START(domainShared->domain.esramCode))
+      ||(SEGMENT_END(domainEE->domain.esramCode) > SEGMENT_END(domainShared->domain.esramCode))
+      ||(SEGMENT_START(domainEE->domain.esramData) < SEGMENT_START(domainShared->domain.esramData))
+      ||(SEGMENT_END(domainEE->domain.esramData) > SEGMENT_END(domainShared->domain.esramData))
+      ||(SEGMENT_START(domainEE->domain.sdramCode) < SEGMENT_START(domainShared->domain.sdramCode))
+      ||(SEGMENT_END(domainEE->domain.sdramCode) > SEGMENT_END(domainShared->domain.sdramCode))
+      ||(SEGMENT_START(domainEE->domain.sdramData) < SEGMENT_START(domainShared->domain.sdramData))
+      ||(SEGMENT_END(domainEE->domain.sdramData) > SEGMENT_END(domainShared->domain.sdramData))
+        ) {
+        return CM_INVALID_PARAMETER;
+    }
+
+    info->coreId = coreId;
+    cm_DM_GetDomainAbsAdresses(srcShared, &domainInfoShared);
+    cm_DM_GetDomainAbsAdresses(src, &domainInfoSrc);
+
+    if ((error = _cm_migration_initSegment(SDRAM_CODE_EE, &domainInfoShared.sdramCode,
+                                           domainDesc[srcShared].domain.sdramCode.size, dst, info)) != CM_OK)
+            goto _migration_error1;
+    if ((error = _cm_migration_initSegment(SDRAM_CODE_USER, &domainInfoSrc.sdramCode,
+                                           domainDesc[src].domain.sdramCode.size, dst, info)) != CM_OK)
+            goto _migration_error2;
+    if ((error = _cm_migration_initSegment(SDRAM_DATA_EE, &domainInfoShared.sdramData,
+                                           domainDesc[srcShared].domain.sdramData.size, dst, info)) != CM_OK)
+            goto _migration_error3;
+    if ((error = _cm_migration_initSegment(SDRAM_DATA_USER, &domainInfoSrc.sdramData,
+                                           domainDesc[src].domain.sdramData.size, dst, info)) != CM_OK)
+            goto _migration_error4;
+    return error;
+
+_migration_error4: _cm_migration_releaseSegment(info, SDRAM_DATA_EE);
+_migration_error3: _cm_migration_releaseSegment(info, SDRAM_CODE_USER);
+_migration_error2: _cm_migration_releaseSegment(info, SDRAM_CODE_EE);
+_migration_error1:
+    OSAL_Log("Couldn't allocate memory for migration\n", 0, 0, 0, 0, 0, 0);
+    return CM_NO_MORE_MEMORY;
+}
+
+typedef t_cm_error (*updateBase_t)(t_nmf_core_id, t_dsp_segment_type, t_cm_system_address, t_cm_system_address);
+
+static t_cm_migration_error _cm_migration_move(
+        t_nmf_core_id           coreId,
+        t_cm_migration_segment *seg,
+        updateBase_t            updateBase,
+        char*                   name
+        )
+{
+    LOG_INTERNAL(1, "##### Migration %s: 0x%x -> 0x%x\n", name, seg->srcAdr.logical, seg->dstAdr.logical, 0, 0, 0);
+    cm_MemCopy((void*)seg->dstAdr.logical, (void*)seg->srcAdr.logical, seg->size);
+    updateBase(coreId, seg->segment, seg->srcAdr, seg->dstAdr);
+    cm_MemSet((void*)seg->srcAdr.logical, 0xdead, seg->size); //for debug, to be sure that we have actually moved the code and bases
+
+    return CM_MIGRATION_OK;
+}
+
+static t_cm_migration_error _cm_migration_update_internal(
+        t_cm_migration_internal_state *info,
+        t_cm_migration_state state
+        )
+{
+    t_nmf_fifo_arm_desc *pArmFifo;
+
+    migrationState.state = state;
+
+    switch(state) {
+    case STATE_MIGRATED:
+        //move fifos
+        pArmFifo = mpc2mpcComsFifoId[ARM_CORE_ID][info->coreId];
+        pArmFifo->fifoDesc = (t_nmf_fifo_desc*)cm_migration_translate(pArmFifo->dspAddressInfo.segmentType, (t_shared_addr)pArmFifo->fifoDescShadow);
+        pArmFifo = mpc2mpcComsFifoId[info->coreId][ARM_CORE_ID];
+        pArmFifo->fifoDesc = (t_nmf_fifo_desc*)cm_migration_translate(pArmFifo->dspAddressInfo.segmentType, (t_shared_addr)pArmFifo->fifoDescShadow);
+        break;
+
+    case STATE_NORMAL:
+        //move fifos
+        pArmFifo = mpc2mpcComsFifoId[ARM_CORE_ID][info->coreId];
+        pArmFifo->fifoDesc = pArmFifo->fifoDescShadow;
+        pArmFifo = mpc2mpcComsFifoId[info->coreId][ARM_CORE_ID];
+        pArmFifo->fifoDesc = pArmFifo->fifoDescShadow;
+        break;
+
+    default:
+        OSAL_Log("unknown state", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    return CM_MIGRATION_OK;
+}
+
+PUBLIC t_cm_error cm_migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst)
+{
+    t_cm_migration_error mError;
+    t_cm_error error;
+
+    if ((error = _cm_migration_check(srcShared, src, dst, &migrationState)) != CM_OK) {
+        return error;
+    }
+
+    /* stop DSP execution */
+    cm_DSP_Stop(migrationState.coreId);
+
+    /* migrate EE and FX */
+    mError = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_CODE_EE], cm_DSP_updateCodeBase, "code");
+    if (mError) {
+        OSAL_Log("EE code migration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    mError = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_DATA_EE], cm_DSP_updateDataBase, "data");
+    if (mError) {
+        OSAL_Log("EE data migration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    /* migrate user domain */
+    mError = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_CODE_USER], cm_DSP_updateCodeBase, "code");
+    if (mError) {
+        OSAL_Log("User code migration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    mError = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_DATA_USER], cm_DSP_updateDataBase, "data");
+    if (mError) {
+        OSAL_Log("User data migration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+   }
+    /* update CM internal structures */
+    mError = _cm_migration_update_internal(&migrationState, STATE_MIGRATED);
+    if (mError) {
+        OSAL_Log("Update internal data failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    /* Be sure everything has been write before restarting mmdsp */
+    OSAL_mb();
+
+    /* resume DSP execution */
+    cm_DSP_Start(migrationState.coreId);
+
+    return CM_OK;
+}
+
+static void _cm_migration_swapSegments(
+        t_cm_migration_segment *segment
+        )
+{
+    t_cm_system_address tmp;
+    tmp = segment->dstAdr;
+    segment->dstAdr = segment->srcAdr;
+    segment->srcAdr = tmp;
+}
+
+PUBLIC t_cm_error cm_unmigrate(void)
+{
+    t_cm_migration_error merror;
+
+    if (migrationState.state != STATE_MIGRATED)
+        return CM_INVALID_PARAMETER; //TODO, juraj, define a proper error for this migration case
+
+    cm_DSP_Stop(migrationState.coreId);
+
+    _cm_migration_swapSegments(&migrationState.segments[SDRAM_CODE_EE]);
+    _cm_migration_swapSegments(&migrationState.segments[SDRAM_DATA_EE]);
+    _cm_migration_swapSegments(&migrationState.segments[SDRAM_CODE_USER]);
+    _cm_migration_swapSegments(&migrationState.segments[SDRAM_DATA_USER]);
+
+    merror = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_CODE_EE], cm_DSP_updateCodeBase, "code");
+    if (merror) {
+        OSAL_Log("EE code unmigration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    merror = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_DATA_EE], cm_DSP_updateDataBase, "data");
+    if (merror) {
+        OSAL_Log("EE data unmigration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    merror = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_CODE_USER], cm_DSP_updateCodeBase, "code");
+    if (merror) {
+        OSAL_Log("User code unmigration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+    merror = _cm_migration_move(migrationState.coreId, &migrationState.segments[SDRAM_DATA_USER], cm_DSP_updateDataBase, "data");
+    if (merror) {
+        OSAL_Log("User data unmigration failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    /* update CM internal structures */
+    merror = _cm_migration_update_internal(&migrationState, STATE_NORMAL);
+    if (merror) {
+        OSAL_Log("Update internal data failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    /* Be sure everything has been write before restarting mmdsp */
+    OSAL_mb();
+
+    cm_DSP_Start(migrationState.coreId);
+
+    /* update CM internal structures */
+    merror = _cm_migration_release(&migrationState);
+    if (merror) {
+        OSAL_Log("Update internal data failed", 0, 0, 0, 0, 0, 0);
+        CM_ASSERT(0);
+    }
+
+    return CM_OK;
+}
+
+// here we make the assumption that the offset doesn't depend from the dsp!!
+PUBLIC t_uint32 cm_migration_translate(t_dsp_segment_type segmentType, t_uint32 addr)
+{
+    //TODO, juraj, save delta instead of recalculating it
+    t_sint32 offset;
+    if (migrationState.state == STATE_MIGRATED) {
+        offset = migrationState.segments[segmentType].dstAdr.logical - migrationState.segments[segmentType].srcAdr.logical;
+    } else {
+        offset = 0;
+    }
+    return addr + offset;
+}
+
+PUBLIC void cm_migration_check_state(t_nmf_core_id coreId, t_cm_migration_state expected)
+{
+    CM_ASSERT(migrationState.state == expected);
+}
+
+#else
+PUBLIC t_cm_error cm_migrate(const t_cm_domain_id srcShared, const t_cm_domain_id src, const t_cm_domain_id dst)
+{
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_unmigrate(void)
+{
+    return CM_OK;
+}
+
+PUBLIC t_uint32 cm_migration_translate(t_dsp_segment_type segmentType, t_uint32 addr)
+{
+    return addr;
+}
+
+PUBLIC void cm_migration_check_state(t_nmf_core_id coreId, t_cm_migration_state expected)
+{
+    return;
+}
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator.c b/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator.c
new file mode 100644
index 00000000000..0d000d37371
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator.c
@@ -0,0 +1,656 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ * Include
+ */
+#include "../inc/remote_allocator.h"
+#include "../inc/remote_allocator_utils.h"
+#include "../inc/chunk_mgr.h"
+
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/trace/inc/xtitrace.h>
+
+static void cm_MM_RA_checkAllocator(t_cm_allocator_desc* alloc);
+//static void cm_MM_RA_checkAlloc(t_cm_allocator_desc* alloc, t_uint32 size, t_uint32 align, t_uint32 min, t_uint32 max);
+
+int bin_index(unsigned int sz) {
+    /*
+     * 32 bins of size       2
+     * 16 bins of size      16
+     *  8 bins of size     128
+     *  4 bins of size    1024
+     *  2 bins of size    8192
+     *  1 bin  of size what's left
+     *
+     */
+    return (((sz >> 6) ==    0) ?       (sz >>  1): // 0        -> 0 .. 31
+            ((sz >> 6) <=    4) ?  28 + (sz >>  4): // 64       -> 32 .. 47
+            ((sz >> 6) <=   20) ?  46 + (sz >>  7): // 320      -> 48 .. 55
+            ((sz >> 6) <=   84) ?  55 + (sz >> 10): // 1344     -> 56 .. 59
+            ((sz >> 6) <=  340) ?  59 + (sz >> 13): // 5440     -> 60 .. 61
+            62);                                    // 21824..
+}
+
+static t_cm_allocator_desc* ListOfAllocators = NULL;
+
+PUBLIC t_cm_allocator_desc* cm_MM_CreateAllocator(t_cm_size size, t_uint32 offset, const char* name)
+{
+    t_cm_allocator_desc *alloc;
+
+    CM_ASSERT(fillChunkPool() == CM_OK);
+
+    /* Alloc structure */
+    alloc = (t_cm_allocator_desc*)OSAL_Alloc_Zero(sizeof(t_cm_allocator_desc));
+    CM_ASSERT(alloc != NULL);
+
+    // Add allocator in list
+    alloc->next = ListOfAllocators;
+    ListOfAllocators = alloc;
+
+    /* assign name */
+    alloc->pAllocName = name;
+
+    alloc->maxSize = size;
+    alloc->sbrkSize = 0;
+    alloc->offset = offset;
+
+    //TODO, juraj, alloc impacts trace format
+    cm_TRC_traceMemAlloc(TRACE_ALLOCATOR_COMMAND_CREATE, 0, size, name);
+
+    return alloc;
+}
+
+PUBLIC t_cm_error cm_MM_DeleteAllocator(t_cm_allocator_desc *alloc)
+{
+    t_cm_chunk *chunk, *next_cm_chunk;
+
+    cm_TRC_traceMemAlloc(TRACE_ALLOCATOR_COMMAND_DESTROY, 0, 0, alloc->pAllocName);
+
+    /* Parse all chunks and free them */
+    chunk = alloc->chunks;
+    while(chunk != 0)
+    {
+        next_cm_chunk = chunk->next;
+        unlinkChunk(alloc, chunk);
+        freeChunk(chunk);
+
+        chunk = next_cm_chunk;
+    }
+
+    // Remove allocator from the list
+    if(ListOfAllocators == alloc)
+        ListOfAllocators = alloc->next;
+    else {
+        t_cm_allocator_desc *prev = ListOfAllocators;
+        while(prev->next != alloc)
+            prev = prev->next;
+        prev->next = alloc->next;
+    }
+
+
+    /* Free allocator descriptor */
+    OSAL_Free(alloc);
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_MM_ResizeAllocator(t_cm_allocator_desc *alloc, t_cm_size size)
+{
+    /* sanity check */
+    if (size == 0)
+        return CM_INVALID_PARAMETER;
+
+    if(alloc->sbrkSize > size)
+        return CM_NO_MORE_MEMORY;
+
+    alloc->maxSize = size;
+
+    if (cmIntensiveCheckState)
+        cm_MM_RA_checkAllocator(alloc);
+
+    return CM_OK;
+}
+
+t_cm_error cm_MM_getValidMemoryHandle(t_cm_memory_handle handle, t_memory_handle* validHandle)
+{
+#ifdef LINUX
+    /* On linux, there is already a check within the linux part
+     * => we don't need to check twice */
+    *validHandle = (t_memory_handle)handle;
+    return CM_OK;
+#else
+    t_cm_allocator_desc *alloc = ListOfAllocators;
+
+    for(; alloc != NULL; alloc = alloc->next)
+    {
+        t_cm_chunk* chunk = alloc->chunks;
+
+        /* Parse all chunks */
+        for(; chunk != NULL; chunk = chunk->next)
+        {
+            if(chunk == (t_memory_handle)handle)
+            {
+                if(chunk->status == MEM_FREE)
+                    return CM_MEMORY_HANDLE_FREED;
+
+                *validHandle = (t_memory_handle)handle;
+
+                return CM_OK;
+            }
+        }
+    }
+
+    return CM_UNKNOWN_MEMORY_HANDLE;
+#endif
+}
+
+//TODO, juraj, add appartenance to allocHandle (of chunk) and degage setUserData
+PUBLIC t_memory_handle cm_MM_Alloc(
+        t_cm_allocator_desc* alloc,
+        t_cm_size size,
+        t_cm_memory_alignment memAlignment,
+        t_uint32 seg_offset,
+        t_uint32 seg_size,
+        t_uint32 domainId)
+{
+    t_cm_chunk* chunk;
+    t_uint32 aligned_offset;
+    t_uint32 aligned_end;
+    t_uint32 seg_end = seg_offset + seg_size;
+    int i;
+
+    /* Sanity check */
+    if ( (size == 0) || (size > seg_size) )
+        return INVALID_MEMORY_HANDLE;
+
+    if(fillChunkPool() != CM_OK)
+        return INVALID_MEMORY_HANDLE;
+
+    /* Get first chunk available for the specific size */
+    // Search a list with a free chunk
+    for(i = bin_index(size); i < BINS; i++)
+    {
+        chunk = alloc->free_mem_chunks[i];
+        while (chunk != 0)
+        {
+            /* Alignment of the lower boundary */
+            aligned_offset = ALIGN_VALUE(MAX(chunk->offset, seg_offset), (memAlignment + 1));
+
+            aligned_end = aligned_offset + size;
+
+            if ((aligned_end <= seg_end)
+                    && aligned_end <= (chunk->offset + chunk->size)
+                    && aligned_offset >= seg_offset
+                    && aligned_offset >= chunk->offset)
+                goto found;
+
+            chunk = chunk->next_free_mem;
+        }
+    }
+
+    // Try to increase sbrkSize through maxSize
+    aligned_offset = ALIGN_VALUE(MAX((alloc->offset + alloc->sbrkSize), seg_offset), (memAlignment + 1));
+
+    aligned_end = aligned_offset + size;
+
+    if ((aligned_end <= seg_end)
+            && aligned_end <= (alloc->offset + alloc->maxSize)
+            && aligned_offset >= seg_offset
+            && aligned_offset >= (alloc->offset + alloc->sbrkSize))
+    {
+        /* If that fit requirement, create a new free chunk at the end of current allocator */
+        chunk = allocChunk();
+
+        /* Update chunk size */
+        chunk->offset = alloc->offset + alloc->sbrkSize; // offset start at end of current allocator
+        chunk->size = aligned_end - chunk->offset;
+        chunk->alloc = alloc;
+
+        /* Chain it with latest chunk */
+        linkChunk(alloc, alloc->lastChunk, chunk);
+
+        /* Increase sbrkSize to end of this new chunk */
+        alloc->sbrkSize += chunk->size;
+
+        goto foundNew;
+   }
+
+    return INVALID_MEMORY_HANDLE;
+
+found:
+    /* Remove chunk from free list */
+    unlinkFreeMem(alloc, chunk);
+
+foundNew:
+    //create an empty chunk before the allocated one
+    if (chunk->offset < aligned_offset) {
+        chunk = splitChunk(alloc, chunk, aligned_offset, FREE_CHUNK_BEFORE);
+    }
+    //create an empty chunk after the allocated one
+    if (chunk->offset + chunk->size > aligned_end) {
+        splitChunk(alloc, chunk, aligned_end, FREE_CHUNK_AFTER);
+    }
+
+    chunk->status = MEM_USED;
+    chunk->prev_free_mem = 0;
+    chunk->next_free_mem = 0;
+    chunk->domainId = domainId;
+
+    //TODO, juraj, alloc impacts trace format
+    cm_TRC_traceMem(TRACE_ALLOC_COMMAND_ALLOC, 0, chunk->offset, chunk->size);
+
+    if (cmIntensiveCheckState) {
+        cm_MM_RA_checkAllocator(alloc);
+    }
+
+    return (t_memory_handle) chunk;
+}
+
+//caution - if successfull, the chunk offset will be aligned with seg_offset
+//caution++ the offset of the allocated chunk changes implicitly
+PUBLIC t_cm_error cm_MM_Realloc(
+                t_cm_allocator_desc* alloc,
+                const t_cm_size size,
+                const t_uint32 offset,
+                t_memory_handle *handle)
+{
+    t_cm_chunk *chunk = (t_cm_chunk*)*handle;
+    t_uint32 oldOffset = chunk->offset;
+    t_uint32 oldSize = chunk->size;
+    t_uint32 oldDomainId = chunk->domainId;
+    t_uint16 userData = chunk->userData;
+
+    cm_MM_Free(alloc, *handle);
+
+    *handle = cm_MM_Alloc(alloc, size, CM_MM_ALIGN_NONE, offset, size, oldDomainId);
+
+    if(*handle == INVALID_MEMORY_HANDLE)
+    {
+        *handle = cm_MM_Alloc(alloc, oldSize, CM_MM_ALIGN_NONE, oldOffset, oldSize, oldDomainId);
+
+        CM_ASSERT(*handle != INVALID_MEMORY_HANDLE);
+
+        chunk = (t_cm_chunk*)*handle;
+        chunk->userData = userData;
+
+        return CM_NO_MORE_MEMORY;
+    }
+
+    chunk = (t_cm_chunk*)*handle;
+    chunk->userData = userData;
+
+    return CM_OK;
+
+#if 0
+    /* check reallocation is related to this chunk! */
+    CM_ASSERT(chunk->offset <= (offset + size));
+    CM_ASSERT(offset <= (chunk->offset + chunk->size));
+    CM_ASSERT(size);
+
+    /* check if extend low */
+    if (offset < chunk->offset) {
+        /* note: it is enough to check only the previous chunk,
+         *      because adjacent chunks of same status are merged
+         */
+        if ((chunk->prev == 0)
+           ||(chunk->prev->status != MEM_FREE)
+           ||(chunk->prev->offset > offset)) {
+            return INVALID_MEMORY_HANDLE;
+        }
+    }
+
+    /* check if extend high, extend sbrk if necessary */
+    if ( (offset + size) > (chunk->offset + chunk->size)) {
+        if(chunk->next == 0)
+        {
+            // check if allocator can be extended to maxSize
+            if((offset + size) > (alloc->offset + alloc->maxSize))
+                return INVALID_MEMORY_HANDLE;
+        }
+        else
+        {
+            if ((chunk->next->status != MEM_FREE)
+                    ||( (chunk->next->offset + chunk->next->size) < (offset + size))) {
+                return INVALID_MEMORY_HANDLE;
+            }
+        }
+    }
+
+    if(fillChunkPool() != CM_OK)
+        return INVALID_MEMORY_HANDLE;
+
+
+    /* extend low
+     *      all conditions should have been checked
+     *      this must not fail
+     */
+    if (offset < chunk->offset) {
+        t_uint32 delta = chunk->prev->offset + chunk->prev->size - offset;
+        t_cm_chunk *prev = chunk->prev;
+
+        chunk->offset -= delta;
+        chunk->size += delta;
+
+        CM_ASSERT(prev->status == MEM_FREE); //TODO, juraj, already checked
+        unlinkFreeMem(alloc, prev);
+        prev->size -= delta;
+        if(prev->size == 0)
+        {
+            unlinkChunk(alloc, prev);
+            freeChunk(prev);
+        } else {
+            updateFreeList(alloc, prev);
+        }
+    }
+
+    /* extend high */
+    if ( (offset + size) > (chunk->offset + chunk->size)) {
+        t_uint32 delta = size - chunk->size;
+        t_cm_chunk *next = chunk->next;
+
+        chunk->size += delta;
+
+        if(next == 0)
+        {
+            alloc->sbrkSize += delta;
+        } else {
+            CM_ASSERT(next->status == MEM_FREE);
+            unlinkFreeMem(alloc, next);
+            next->offset += delta;
+            next->size -= delta;
+            if(next->size == 0)
+            {
+                unlinkChunk(alloc, next);
+                freeChunk(next);
+            } else {
+                updateFreeList(alloc, next);
+            }
+        }
+    }
+
+    /* reduce top */
+    if ((offset + size) < (chunk->offset + chunk->size)) {
+        t_uint32 delta = chunk->size - size;
+
+        if(chunk->next == 0) {
+            alloc->sbrkSize -= delta;
+            chunk->size -= delta;
+
+        } else if (chunk->next->status == MEM_FREE) {
+            unlinkFreeMem(alloc, chunk->next);
+            chunk->size -= delta;
+            chunk->next->offset -= delta;
+            chunk->next->size += delta;
+            updateFreeList(alloc, chunk->next);
+        } else {
+            t_cm_chunk *tmp = splitChunk(alloc, chunk, offset + size, FREE_CHUNK_AFTER); //tmp = chunk, chunk = result
+            tmp->status = MEM_USED;
+            tmp->next->status = MEM_FREE;
+        }
+    }
+
+    /* reduce bottom */
+    if (offset > chunk->offset) {
+        if (chunk->prev->status == MEM_FREE) {
+            t_uint32 delta = offset - chunk->offset;
+            unlinkFreeMem(alloc, chunk->prev);
+            chunk->prev->size += delta;
+            chunk->offset = offset;
+            chunk->size -= delta;
+            updateFreeList(alloc, chunk->prev);
+        } else {
+            t_cm_chunk *tmp = splitChunk(alloc, chunk, offset, FREE_CHUNK_BEFORE); //tmp->next = chunk, tmp = result
+            tmp->status = MEM_USED;
+            tmp->prev->status = MEM_FREE;
+        }
+    }
+
+    cm_MM_RA_checkAllocator(alloc);
+
+    return (t_memory_handle)chunk;
+#endif
+}
+
+PUBLIC void cm_MM_Free(t_cm_allocator_desc* alloc, t_memory_handle memHandle)
+{
+    t_cm_chunk* chunk = (t_cm_chunk*)memHandle;
+
+    //TODO, juraj, alloc impacts trace format
+    cm_TRC_traceMem(TRACE_ALLOC_COMMAND_FREE, 0,
+            chunk->offset, chunk->size);
+
+    /* Update chunk status */
+    chunk->status = MEM_FREE;
+    chunk->domainId = 0x0;
+
+    // Invariant: Current chunk is free but not in free list
+
+    /* Check if the previous chunk is free */
+    if((chunk->prev != 0) && (chunk->prev->status == MEM_FREE))
+    {
+        t_cm_chunk* prev = chunk->prev;
+
+        // Remove chunk to be freed from memory list
+        unlinkChunk(alloc, chunk);
+
+        // Remove previous from free list
+        unlinkFreeMem(alloc, prev);
+
+        // Update previous size
+        prev->size += chunk->size;
+
+        freeChunk(chunk);
+
+        chunk = prev;
+    }
+
+    /* Check if the next chunk is free */
+    if((chunk->next != 0) && (chunk->next->status == MEM_FREE))
+    {
+        t_cm_chunk* next = chunk->next;
+
+        // Remove next from memory list
+        unlinkChunk(alloc, next);
+
+        // Remove next from free list
+        unlinkFreeMem(alloc, next);
+
+        // Update previous size
+        chunk->size += next->size;
+
+        freeChunk(next);
+    }
+
+    if(chunk->next == 0)
+    {
+        // If we are the last one, decrease sbrkSize
+        alloc->sbrkSize -= chunk->size;
+
+        unlinkChunk(alloc, chunk);
+        freeChunk(chunk);
+
+    }
+    else
+    {
+        // Add it in free list
+        updateFreeList(alloc, chunk);
+    }
+
+    if (cmIntensiveCheckState) {
+        cm_MM_RA_checkAllocator(alloc);
+    }
+}
+
+PUBLIC t_cm_error cm_MM_GetAllocatorStatus(t_cm_allocator_desc* alloc, t_uint32 offset, t_uint32 size, t_cm_allocator_status *pStatus)
+{
+    t_cm_chunk* chunk = alloc->chunks;
+    t_uint32 sbrkFree = alloc->maxSize - alloc->sbrkSize;
+    t_uint8 min_free_size_updated = FALSE;
+
+    /* Init status */
+    pStatus->global.used_block_number = 0;
+    pStatus->global.free_block_number = 0;
+    pStatus->global.maximum_free_size = 0;
+    pStatus->global.minimum_free_size = 0xFFFFFFFF;
+    pStatus->global.accumulate_free_memory = 0;
+    pStatus->global.accumulate_used_memory = 0;
+    pStatus->global.size = alloc->maxSize;
+    pStatus->domain.maximum_free_size = 0;
+    pStatus->domain.minimum_free_size = 0xFFFFFFFF;
+    pStatus->domain.accumulate_free_memory = 0;
+    pStatus->domain.accumulate_used_memory = 0;
+    pStatus->domain.size= size;
+
+    /* Parse all chunks */
+    while(chunk != 0)
+    {
+
+        /* Chunk is free */
+        if (chunk->status == MEM_FREE) {
+            pStatus->global.free_block_number++;
+            pStatus->global.accumulate_free_memory += chunk->size;
+
+            /* Check max size */
+            if (chunk->size > pStatus->global.maximum_free_size)
+            {
+                pStatus->global.maximum_free_size = chunk->size;
+            }
+
+            /* Check min size */
+            if (chunk->size < pStatus->global.minimum_free_size)
+            {
+                pStatus->global.minimum_free_size = chunk->size;
+                min_free_size_updated = TRUE;
+            }
+        } else {/* Chunk used */
+            pStatus->global.used_block_number++;
+            pStatus->global.accumulate_used_memory += chunk->size;
+        }
+
+        chunk = chunk->next;
+    }
+
+    /* Accumulate free space between sbrkSize and maxSize */
+    pStatus->global.accumulate_free_memory += sbrkFree;
+    if (sbrkFree > 0)
+        pStatus->global.free_block_number++;
+    if (pStatus->global.maximum_free_size < sbrkFree)
+        pStatus->global.maximum_free_size = sbrkFree;
+    if (pStatus->global.minimum_free_size > sbrkFree) {
+        pStatus->global.minimum_free_size = sbrkFree;
+        min_free_size_updated = TRUE;
+    }
+
+    /* Put max free size to min free size */
+    if (min_free_size_updated == FALSE) {
+        pStatus->global.minimum_free_size = pStatus->global.maximum_free_size;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC t_uint32 cm_MM_GetOffset(t_memory_handle memHandle)
+{
+    /* Provide offset */
+    return ((t_cm_chunk*)memHandle)->offset;
+}
+
+PUBLIC t_uint32 cm_MM_GetSize(t_memory_handle memHandle)
+{
+    return ((t_cm_chunk*)memHandle)->size;
+}
+
+PUBLIC t_uint32 cm_MM_GetAllocatorSize(t_cm_allocator_desc* alloc)
+{
+    return alloc->maxSize;
+}
+
+PUBLIC void cm_MM_SetMemoryHandleUserData(t_memory_handle memHandle, t_uint16 userData)
+{
+    ((t_cm_chunk*)memHandle)->userData = userData;
+}
+
+PUBLIC void cm_MM_GetMemoryHandleUserData(t_memory_handle memHandle, t_uint16 *pUserData, t_cm_allocator_desc **alloc)
+{
+    *pUserData = ((t_cm_chunk*)memHandle)->userData;
+    if (alloc)
+        *alloc = ((t_cm_chunk*)memHandle)->alloc;
+}
+
+/*
+ * check free list is ordered
+ * check all chunks are correctly linked
+ * check adjacent chunks are not FREE
+ */
+static void cm_MM_RA_checkAllocator(t_cm_allocator_desc* alloc)
+{
+    t_cm_chunk *chunk = alloc->chunks;
+    t_uint32 size = 0;
+    int i;
+
+    CM_ASSERT(alloc->sbrkSize <= alloc->maxSize);
+
+    while(chunk != 0) {
+        if(chunk == alloc->chunks)
+            CM_ASSERT(chunk->prev == 0);
+        if(chunk == alloc->lastChunk)
+            CM_ASSERT(chunk->next == 0);
+
+        CM_ASSERT(chunk->alloc == alloc);
+
+        if (chunk->next != 0) {
+            CM_ASSERT(!((chunk->status == MEM_FREE) && (chunk->next->status == MEM_FREE))); //two free adjacent blocks
+            CM_ASSERT(chunk->offset < chunk->next->offset); //offsets reverted
+            CM_ASSERT(chunk->offset + chunk->size == chunk->next->offset); // Not hole in allocator
+        }
+        size += chunk->size;
+        chunk = chunk->next;
+    }
+
+    CM_ASSERT(size == alloc->sbrkSize);
+
+    for(i = 0; i < BINS; i++)
+    {
+        chunk = alloc->free_mem_chunks[i];
+        while(chunk != 0) {
+            if (chunk->next_free_mem != 0) {
+                CM_ASSERT(chunk->size <= chunk->next_free_mem->size); //free list not ordered
+            }
+            chunk = chunk->next_free_mem;
+        }
+    }
+}
+
+PUBLIC void cm_MM_DumpMemory(t_cm_allocator_desc* alloc, t_uint32 start, t_uint32 end)
+{
+    t_cm_chunk *chunk = alloc->chunks;
+
+    LOG_INTERNAL(0, "ALLOCATOR Dumping allocator \"%s\" [0x%08x:0x%08x]\n", alloc->pAllocName, start, end, 0, 0, 0);
+    while(chunk != 0) {
+        if (((chunk->offset < start) && (chunk->offset + chunk->size > start))
+          || ((chunk->offset < end) && (chunk->offset + chunk->size > end))
+          || ((chunk->offset > start) && (chunk->offset + chunk->size < end))
+          || ((chunk->offset < start) && (chunk->offset + chunk->size > end)))
+        {
+            LOG_INTERNAL(0, "ALLOCATOR chunk [0x%08x -> 0x%08x[: status:%s, domainId: 0x%x\n",
+                    chunk->offset,
+                    chunk->offset + chunk->size,
+                    chunk->status?"FREE":"USED",
+                    chunk->domainId, 0, 0);
+        }
+        chunk = chunk->next;
+    }
+}
+
+PUBLIC void cm_MM_SetDefaultDomain(t_memory_handle memHandle, t_uint32 domainId)
+{
+	((t_cm_chunk *) memHandle)->domainId = domainId;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator_utils.c b/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator_utils.c
new file mode 100644
index 00000000000..4e800376dbb
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/memory/src/remote_allocator_utils.c
@@ -0,0 +1,250 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/memory/inc/remote_allocator_utils.h>
+#include <cm/engine/trace/inc/trace.h>
+
+/***************************************************************************/
+/*
+ * linkChunk
+ * param prev     : Pointer on previous chunk where the chunk will be added
+ * param add      : Pointer on chunk to add
+ *
+ * Add a chunk in the memory list
+ *
+ */
+/***************************************************************************/
+PUBLIC void linkChunk(t_cm_allocator_desc* alloc, t_cm_chunk* prev, t_cm_chunk* add)
+{
+    // Link previous
+    if(prev == 0)
+    {
+        add->next = alloc->chunks;
+        alloc->chunks = add;
+    }
+    else
+    {
+        add->prev = prev;
+        add->next = prev->next;
+        prev->next = add;
+    }
+
+    // Link next
+    if(add->next == 0)
+    {
+        // Link at the end
+        alloc->lastChunk = add;
+    }
+    else
+        add->next->prev = add;
+}
+
+/***************************************************************************/
+/*
+ * unlinkChunk
+ * param allocHandle : Allocator handle
+ * param current     : Pointer on chunk to remove
+ *
+ * Remove a chunk in the memory list and update first pointer
+ *
+ */
+/***************************************************************************/
+PUBLIC void unlinkChunk(t_cm_allocator_desc* alloc, t_cm_chunk* current)
+{
+    /* Link previous with next */
+    if (current->prev != 0)
+        current->prev->next = current->next;
+    else
+    {
+        CM_ASSERT(alloc->chunks == current);
+
+        // We remove the first, update chunks
+        alloc->chunks = current->next;
+    }
+
+    /* Link next with previous */
+    if(current->next != 0)
+        current->next->prev= current->prev;
+    else
+    {
+        CM_ASSERT(alloc->lastChunk == current);
+
+        // We remove the last, update lastChunk
+        alloc->lastChunk = current->prev;
+    }
+}
+
+
+/***************************************************************************/
+/*
+ * unlinkFreeMem() unlinks chunk from free memory double-linked list
+ *   makes the previous and next chunk in the list point to each other..
+ * param allocHandle : Allocator handle
+ * param current     : Pointer on chunk to remove
+ *
+ * Remove a chunk in the free memory list and update pointer
+ *
+ */
+/***************************************************************************/
+PUBLIC void unlinkFreeMem(t_cm_allocator_desc* alloc ,t_cm_chunk* current)
+{
+    int bin = bin_index(current->size);
+
+    /* unlink previous */
+    if (current->prev_free_mem != 0)
+    {
+        current->prev_free_mem->next_free_mem = current->next_free_mem;
+    }
+
+    /* Unlink next */
+    if (current->next_free_mem !=0 )
+    {
+        current->next_free_mem->prev_free_mem = current->prev_free_mem;
+    }
+
+    /* update first free pointer */
+    if (alloc->free_mem_chunks[bin] == current)
+    {
+        alloc->free_mem_chunks[bin] = current->next_free_mem;
+    }
+
+    current->prev_free_mem = 0;
+    current->next_free_mem = 0;
+}
+
+/***************************************************************************/
+/*
+ * linkFreeMemBefore
+ * param add      : Pointer on chunk to add
+ * param next     : Pointer on next chunk where the chunk will be added before
+ *
+ * Add a chunk in the free memory list
+ *
+ */
+/***************************************************************************/
+PUBLIC void linkFreeMemBefore(t_cm_chunk* add, t_cm_chunk* next)
+{
+    /* Link next */
+    add->prev_free_mem = next->prev_free_mem;
+    add->next_free_mem = next;
+
+    /* Link previous */
+    if (next->prev_free_mem != 0)
+    {
+        next->prev_free_mem->next_free_mem = add;
+    }
+    next->prev_free_mem = add;
+}
+
+/***************************************************************************/
+/*
+ * linkFreeMemAfter
+ * param add      : Pointer on chunk to add
+ * param prev     : Pointer on previous chunk where the chunk will be added after
+ *
+ * Add a chunk in the free memory list
+ *
+ */
+/***************************************************************************/
+PUBLIC void linkFreeMemAfter(t_cm_chunk* prev,t_cm_chunk* add)
+{
+    /* Link previous */
+    add->prev_free_mem = prev;
+    add->next_free_mem = prev->next_free_mem;
+
+    /* Link next */
+    if (prev->next_free_mem != 0)
+    {
+        prev->next_free_mem->prev_free_mem = add;
+    }
+    prev->next_free_mem = add;
+}
+
+
+/***************************************************************************/
+/*
+ * updateFreeList
+ * param allocHandle : Allocator handle
+ * param offset      : Pointer on chunk
+ *
+ * Update free memory list, ordered by size
+ *
+ */
+/***************************************************************************/
+PUBLIC void updateFreeList(t_cm_allocator_desc* alloc , t_cm_chunk* chunk)
+{
+    t_cm_chunk* free_chunk;
+    int bin = bin_index(chunk->size);
+
+    /* check case with no more free block */
+    if (alloc->free_mem_chunks[bin] == 0)
+    {
+        alloc->free_mem_chunks[bin] = chunk;
+        return ;
+    }
+
+    /* order list */
+    free_chunk = alloc->free_mem_chunks[bin];
+    while ((free_chunk->next_free_mem != 0) && (chunk->size > free_chunk->size))
+    {
+        free_chunk = free_chunk->next_free_mem;
+    }
+
+    /* Add after free chunk if smaller -> we are the last */
+    if(free_chunk->size <= chunk->size)
+    {
+        linkFreeMemAfter(free_chunk,chunk);
+    }
+    else // This mean that we are smaller
+    {
+        linkFreeMemBefore(chunk,free_chunk);
+
+        /* Update first free chunk */
+        if (alloc->free_mem_chunks[bin] == free_chunk)
+        {
+            alloc->free_mem_chunks[bin] = chunk;
+        }
+    }
+}
+
+
+/***************************************************************************/
+/*
+ * splitChunk
+ * param allocHandle : Allocator handle
+ * param chunk       : Current chunk (modified in place)
+ * param offset      : Offset address of the start memory
+ * return            : New chunk handle or 0 if an error occurs
+ *
+ * Create new chunk before/after the current chunk with the size
+ */
+/***************************************************************************/
+PUBLIC t_cm_chunk* splitChunk(t_cm_allocator_desc* alloc ,t_cm_chunk *chunk,
+        t_uint32 offset, t_mem_split_position position)
+{
+    t_cm_chunk *free;
+    t_cm_chunk *returned;
+
+    t_cm_chunk* new_chunk = allocChunk();
+
+    if (position == FREE_CHUNK_AFTER) {
+        returned = chunk;
+        free = new_chunk;
+    } else { //FREE_CHUNK_BEFORE
+        returned = new_chunk;
+        free = chunk;
+    }
+
+    new_chunk->offset = offset;
+    new_chunk->size   = chunk->offset + chunk->size - offset;
+    new_chunk->alloc  = alloc;
+    chunk->size = offset - chunk->offset;
+
+    linkChunk(alloc, chunk, new_chunk);
+    unlinkFreeMem(alloc, free);
+    updateFreeList(alloc, free);
+
+    return returned;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h b/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h
new file mode 100644
index 00000000000..c9ec864795f
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h
@@ -0,0 +1,498 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \brief OS Adaptation Layer API
+ *
+ * \defgroup CM_ENGINE_OSAL_API CM Engine OSAL (Operating System Abstraction Layer) API
+ * \ingroup CM_ENGINE_MODULE
+ */
+#ifndef __INC_CM_OSAL_H
+#define __INC_CM_OSAL_H
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+#include <cm/engine/component/inc/instance.h>
+
+/*!
+ * \brief Identifier of a trace channel (id in [0..255])
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef t_uint8 t_nmf_trace_channel;
+
+/*!
+ * \brief Identifier of lock create by OSAL
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef t_uint32 t_nmf_osal_sync_handle;
+
+/*!
+ * \brief Identifier of semaphore create by OSAL
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef t_uint32 t_nmf_osal_sem_handle;
+
+/*!
+ * \brief Identifier of semaphore wait error return by semaphore OSAL API
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef t_uint8 t_nmf_osal_sync_error;
+#define SYNC_ERROR_TIMEOUT      ((t_nmf_osal_sync_error)-1)
+#define SYNC_OK                 ((t_nmf_osal_sync_error)0)
+#define SEM_TIMEOUT_NORMAL      3000
+#define SEM_TIMEOUT_DEBUG       300000
+
+/*!
+ * \brief Operations used to support additionnal OS-specific debug feature
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+struct osal_debug_operations {
+	void (*component_create)(t_component_instance *component);
+	void (*component_destroy)(t_component_instance *component);
+	void (*domain_create)(t_cm_domain_id id);
+	void (*domain_destroy)(t_cm_domain_id id);
+};
+
+extern struct osal_debug_operations osal_debug_ops;
+
+/*!
+ * \brief Description of the Scheduling part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Support of uplink communication path (from Media Processors to Host (ARM))
+ *
+ * Post a function call outside of Host CPU Interrupt mode in order to minimize ISR execution time
+ * \param[in] upLayerTHIS : this one provided by user when calling CM_ENGINE_BindComponentToCMCore() (first field of the interface context) \n
+ * \param[in] methodIndex : index method to be called  \n
+ * \param[in] anyPtr : internal NMF marshaled parameters block (to be passed as second parameter when calling the previous pSkeleton method) \n
+ * \param[in] ptrSize : size of anyPtr in bytes \n
+ *
+ * Called by:
+ *  - CM_ProcessMpcEvent() call (shall be bound by OS integrator to HSEM IRQ)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+
+PUBLIC void OSAL_PostDfc(
+        t_nmf_mpc2host_handle   upLayerTHIS,
+        t_uint32                    methodIndex,
+        t_event_params_handle       anyPtr,
+        t_uint32                    ptrSize);
+
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to protect global variable against multiple call. Interrupt and scheduler function are use when
+ *       we take hardware/local semaphore. Scheduler lock functions can have empty implementation but this may
+ *       impact performance (dsp waiting semaphore because host thread was preempted whereas it has already take semaphore
+ *       but not yet release it).
+ *
+ * \return handle of the Mutex created
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_nmf_osal_sync_handle OSAL_CreateLock(void);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to protect global variable against multiple call. Interrupt and scheduler function are use when
+ *       we take hardware/local semaphore. Scheduler lock functions can have empty implementation but this may
+ *       impact performance (dsp waiting semaphore because host thread was preempted whereas it has already take semaphore
+ *       but not yet release it).
+ *
+ * \param[in] handle handle of the Mutex to be locked
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Lock(
+        t_nmf_osal_sync_handle handle);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to protect global variable against multiple call. Interrupt and scheduler function are use when
+ *       we take hardware/local semaphore. Scheduler lock functions can have empty implementation but this may
+ *       impact performance (dsp waiting semaphore because host thread was preempted whereas it has already take semaphore
+ *       but not yet release it).
+ *
+ * \param[in] handle handle of the Mutex to be unlocked
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Unlock(
+        t_nmf_osal_sync_handle handle);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to protect global variable against multiple call. Interrupt and scheduler function are use when
+ *       we take hardware/local semaphore. Scheduler lock functions can have empty implementation but this may
+ *       impact performance (dsp waiting semaphore because host thread was preempted whereas it has already take semaphore
+ *       but not yet release it).
+ *
+ * \param[in] handle handle of the Mutex to be destroyed
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DestroyLock(
+        t_nmf_osal_sync_handle handle);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * \param[in] value : Initial value of semaphore.
+ *
+ * \return handle of the Semaphore created
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_nmf_osal_sem_handle OSAL_CreateSemaphore(
+        t_uint32 value);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to allow to synchronize with code running on mpc side. This function can be call under
+ *       Irq context by CM.
+ *
+ * \param[in] handle handle of the Semaphore for which we increase value and so potentially wake up thread.
+ *
+ * \param[in] aCtx is a hint to indicate to os that we are in a none normal context (e.g under interruption).
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_SemaphorePost(
+        t_nmf_osal_sem_handle handle,
+        t_uint8 aCtx);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * \param[in] handle of the Semaphore for which we decrease value and so potentially block current thread.
+ *
+ * \param[in] timeOutInMs maximun time in ms after which the block thread is wake up. In this case function return SYNC_ERROR_TIMEOUT value.
+ *
+ * \return error number: SYNC_ERROR_TIMEOUT in case semaphore is not release withing timeOutInMs.
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_nmf_osal_sync_error OSAL_SemaphoreWaitTimed(
+        t_nmf_osal_sem_handle handle,
+        t_uint32 timeOutInMs);
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * \param[in] handle handle of the Semaphore to be destroyed
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DestroySemaphore(
+        t_nmf_osal_sem_handle handle);
+
+/*!
+ * \brief Description of the System Memory Allocator part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Allocate CM some cacheable and bufferable memory (SDRAM) for internal usage \n
+ * This memory will be accessed only by Host CPU (ARM)
+ *
+ * This function provide a simple, general-purpose  memory  allocation. The
+ * OSAL_Alloc macro returns a pointer to a block of at least size bytes
+ * suitably aligned for any use. If there  is  no  available  memory, this
+ * function returns a null pointer.
+ *
+ * \param[in] size   size in bytes, of memory to be allocated
+ * \return pointer on the beginning of the allocated memory
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void* OSAL_Alloc(
+        t_cm_size size);
+
+/*!
+ * \brief Description of the System Memory Allocator part of the OS Adaptation Layer with memory set to zero
+ *
+ * Compare to \see OSAL_Alloc, same allocation is done but memory is set with zero before returning.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void* OSAL_Alloc_Zero(
+        t_cm_size size);
+
+/*!
+ * \brief Description of the System Memory Allocator part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Free CM some cacheable and bufferable memory (SDRAM) for internal usage \n
+ * This memory will be accessed only by Host CPU (ARM)
+ *
+ * \param[in] pHandle  pointer on the begining of the memory previously allocated
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void  OSAL_Free(
+        void *pHandle);
+
+/*!
+ * \brief Clean data cache in DDR in order to be accessible from peripheral.
+ *
+ * This method must be synchronized with MMDSP Code cache attribute.
+ *   Strongly Ordered -> nothing
+ *   Shared device -> dsb + L2 Write buffer drain
+ *   Non cacheable, Bufferable -> dsb + L2 Write buffer drain
+ *   WT or WB -> Flush cache range + dsb + L2 Write buffer drain
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_CleanDCache(
+        t_uint32                startAddr,          //!< [in] Start data address of range to clean
+        t_uint32                Size                //!< [in] Size of range to clean
+        );
+
+/*!
+ * \brief Flush write buffer.
+ *
+ * This method must be synchronized with MMDSP Data cache attribute.
+ *   Strongly Ordered -> nothing
+ *   Shared device -> dsb + L2 Write buffer drain
+ *   Non cacheable, Bufferable -> dsb + L2 Write buffer drain
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_mb(void);
+
+/*!
+ * \brief Description of the System Memory part of the OS Adaptation Layer
+ *
+ * <B>Goal:</B> Copy some cacheable and bufferable memory (SDRAM) provided by a client to\n
+ * internal memory.
+ *
+ * \param[in] dst  : pointer on the begining of the internal memory previously allocated
+ * \param[in] src  : pointer on the begining of the client's memory
+ * \param[in] size : The size of the data to copy
+ *
+ * Called by:
+ *  - CM_ENGINE_PushComponent()
+ *
+ * \note This API is mainly provided for the OS were the client application does execute in the same
+ *       address space as the CM.
+ *       For example in Linux or Symbian, the client's address space is userland but the CM execute in
+ *       kernel space. Thus, 'dst' is supposed to be a kernel address but src is supposed to be a user
+ *       space address
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_cm_error OSAL_Copy(
+        void *dst,
+	const void *src,
+	t_cm_size size);
+
+/*!
+ * \brief Description of the internal log traces configuration of the Component Manager
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Log(
+        const char *format,
+        int param1,
+        int param2,
+        int param3,
+        int param4,
+        int param5,
+        int param6);
+
+/*!
+ * \brief Generate an OS-Panic. Called in from CM_ASSERT().
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Panic(void);
+
+/*!
+ * \brief Description of the configuration of the trace features
+ *
+ * (trace output itself is provided by user through his custom implementation of the generic APIs)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_Write64(
+        t_nmf_trace_channel channel,
+        t_uint8 isTimestamped,
+        t_uint64 value);
+
+/*!
+ * \brief Power enabling/disabling commands description.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+typedef enum
+{
+    CM_OSAL_POWER_SxA_CLOCK,     //!< SxA Power & Clock, firstParam contains Core ID
+    CM_OSAL_POWER_SxA_AUTOIDLE,  //!< SxA AutoIdle, firstParam contains Core ID
+    CM_OSAL_POWER_SxA_HARDWARE,  //!< SxA Hardware Power, firstParam contains Core ID
+    CM_OSAL_POWER_HSEM,          //!< HSEM Power
+    CM_OSAL_POWER_SDRAM,         //!< SDRAM memory, firstParam contains physical resource address, secondParam contains size
+    CM_OSAL_POWER_ESRAM          //!< ESRAM memory, firstParam contains physical resource address, secondParam contains size
+} t_nmf_power_resource;
+
+/*!
+ * \brief Description of the Power Management part of the OS Adaptation Layer
+ *
+ * Use by CM engine to disable a logical power domain  (see \ref t_nmf_power_resource)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DisablePwrRessource(
+        t_nmf_power_resource    resource,           //!< [in] Describe the domain which must be disabled
+        t_uint32                firstParam,         //!< [in] Eventual first parameter to power to disable
+        t_uint32                secondParam         //!< [in] Eventual second parameter to power to disable
+        );
+
+/*!
+ * \brief Description of the Power Management part of the OS Adaptation Layer
+ *
+ * Use by CM engine to enable a logical power domain  (see \ref t_nmf_power_resource)
+ *
+ * \return
+ *   - \ref CM_OK
+ *   - \ref CM_PWR_NOT_AVAILABLE A specified power domain is not managed (see returned value in aPowerMask)
+ *
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_cm_error OSAL_EnablePwrRessource(
+        t_nmf_power_resource    resource,           //!< [in] Describing the domains which must be enabled
+        t_uint32                firstParam,         //!< [in] Eventual first parameter to power to disable
+        t_uint32                secondParam         //!< [in] Eventual second parameter to power to disable
+        );
+
+
+/*!
+ * \brief return prcmu timer value.
+ *
+ * This is need for perfmeter api  (see \ref t_nmf_power_resource)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC t_uint64 OSAL_GetPrcmuTimer(void);
+
+/*!
+ * \brief Disable the service message handling (panic, etc)
+ *
+ * It must disable the handling of all service messages
+ * If a service message is currently handled, it must wait till the end
+ * of its managment before returning.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DisableServiceMessages(void);
+
+/*!
+ * \brief Enable the service message handling (panic, etc)
+ *
+ * It enables the handling of all service messages
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_EnableServiceMessages(void);
+
+/*!
+ * \brief Generate 'software' panic due to dsp crash
+ *
+ * We request that the os part generate a panic to notify cm users
+*  that a problem occur but not dsp panic has been sent (for example
+*  a dsp crash)
+ *
+ * \param[in] t_nmf_core_id  : core_id is the id of dsp for which we need to generate a panic.
+ * \param[in] reason  : additional information. Today only 0 is valid.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_GeneratePanic(t_nmf_core_id coreId, t_uint32 reason);
+
+extern /*const*/ t_nmf_osal_sync_handle lockHandleApi;
+extern /*const*/ t_nmf_osal_sync_handle lockHandleCom;
+extern /*const*/ t_nmf_osal_sem_handle semHandle;
+
+/*!
+ * \brief Take a lock before entering critical section. Can suspend current thread if lock already taken. \n
+ *        Use this macro in api function. For com function use OSAL_LOCK_COM.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_LOCK_API() OSAL_Lock(lockHandleApi)
+
+/*!
+ * \brief Release lock before leaving critical section.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_UNLOCK_API() OSAL_Unlock((lockHandleApi))
+
+/*!
+ * \brief Take a lock before entering critical section. Can suspend current thread if lock already taken. \n
+ *        Use this macro in com function. For com function use OSAL_LOCK_API.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_LOCK_COM() OSAL_Lock(lockHandleCom)
+
+/*!
+ * \brief Release lock before leaving critical section.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_UNLOCK_COM() OSAL_Unlock((lockHandleCom))
+
+/*!
+ * \brief Go to sleep untill post done on semaphore or timeout expire. In that case SYNC_ERROR_TIMEOUT is return.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+#define OSAL_SEMAPHORE_WAIT_TIMEOUT(semHandle) OSAL_SemaphoreWaitTimed(semHandle, (cm_PWR_GetMode() == NORMAL_PWR_MODE)?SEM_TIMEOUT_NORMAL:SEM_TIMEOUT_DEBUG)
+
+/****************/
+/* Generic part */
+/****************/
+t_cm_error cm_OSAL_Init(void);
+void cm_OSAL_Destroy(void);
+
+#endif /* __INC_CM_OSAL_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/src/os_adaptation_layer.c b/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/src/os_adaptation_layer.c
new file mode 100644
index 00000000000..380692e3cd8
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/os_adaptation_layer/src/os_adaptation_layer.c
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/utils/inc/mem.h>
+
+t_nmf_osal_sync_handle lockHandleApi;
+t_nmf_osal_sync_handle lockHandleCom;
+t_nmf_osal_sem_handle semHandle;
+struct osal_debug_operations osal_debug_ops;
+
+/****************/
+/* Generic part */
+/****************/
+PUBLIC t_cm_error cm_OSAL_Init(void)
+{
+
+	/* create locks */
+	lockHandleApi = OSAL_CreateLock();
+	if (lockHandleApi == 0) {return CM_INVALID_PARAMETER;}
+	lockHandleCom = OSAL_CreateLock();
+	if (lockHandleCom == 0) {return CM_INVALID_PARAMETER;}
+
+	/* create semaphore */
+	semHandle = OSAL_CreateSemaphore(0);
+	if (semHandle == 0) {return CM_INVALID_PARAMETER;}
+
+	/* init to zero */
+	cm_MemSet(&osal_debug_ops, 0, sizeof(osal_debug_ops));
+
+	return CM_OK;
+}
+
+PUBLIC void cm_OSAL_Destroy(void)
+{
+	/* destroy locks */
+	OSAL_DestroyLock(lockHandleApi);
+	OSAL_DestroyLock(lockHandleCom);
+
+	/* destroy semaphore */
+	OSAL_DestroySemaphore(semHandle);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/mpcload.h b/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/mpcload.h
new file mode 100644
index 00000000000..0831f1940ca
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/mpcload.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ */
+#ifndef MPCLOAD_H_
+#define MPCLOAD_H_
+
+#include <cm/engine/component/inc/instance.h>
+
+/******************************************************************************/
+/************************ FUNCTIONS PROTOTYPES ********************************/
+/******************************************************************************/
+
+PUBLIC t_cm_error cm_PFM_allocatePerfmeterDataMemory(t_nmf_core_id coreId, t_cm_domain_id domainId);
+PUBLIC void cm_PFM_deallocatePerfmeterDataMemory(t_nmf_core_id coreId);
+
+#endif /* MPCLOAD_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/perfmeter_type.h b/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/perfmeter_type.h
new file mode 100644
index 00000000000..8733c20b21b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/perfmeter/inc/perfmeter_type.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Public Component Manager Performance Meter API type.
+ *
+ * This file contains the Component Manager API type for performance meter.
+ *
+ * \defgroup PERFMETER CM Monitoring API
+ * \ingroup CM_USER_API
+ */
+#ifndef CM_COMMON_PERFMETER_TYPE_H_
+#define CM_COMMON_PERFMETER_TYPE_H_
+
+#include <cm/inc/cm_type.h>
+/*!
+ * \brief Description of mpc load structure.
+ *
+ * This contain mpc load value.
+ *
+ * \ingroup PERFMETER
+ */
+typedef struct {
+    t_uint64 totalCounter;
+    t_uint64 loadCounter;
+} t_cm_mpc_load_counter;
+
+
+#endif /* CM_COMMON_PERFMETER_TYPE_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/perfmeter/src/mpcload.c b/drivers/staging/nmf-cm/cm/engine/perfmeter/src/mpcload.c
new file mode 100644
index 00000000000..193d155b97b
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/perfmeter/src/mpcload.c
@@ -0,0 +1,119 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/engine/perfmeter/inc/mpcload.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+
+#include <cm/engine/api/perfmeter_engine.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#include <cm/engine/trace/inc/trace.h>
+
+#define PERFMETER_MAX_RETRIES                           32
+#define PERFMETER_DATA_WORD_NB                          7
+
+/* private type */
+typedef struct {
+    t_memory_handle perfmeterDataHandle;
+    t_cm_logical_address perfmeterDataAddr;
+} t_mpcLoad;
+
+/* private globals */
+t_mpcLoad mpcLoad_i[NB_CORE_IDS];
+
+/* engine api */
+PUBLIC EXPORT_SHARED t_cm_error CM_GetMpcLoadCounter(
+    t_nmf_core_id coreId,
+    t_cm_mpc_load_counter *pMpcLoadCounter
+)
+{
+    t_uint24 data[PERFMETER_DATA_WORD_NB];
+    t_uint32 i;
+    t_uint64 prcmuBeforeAttributes;
+    t_uint32 retryCounter = 0;
+    volatile t_uint32 *pData;
+
+    pMpcLoadCounter->totalCounter = 0;
+    pMpcLoadCounter->loadCounter = 0;
+    /* check core id is an mpc */
+    if (coreId < FIRST_MPC_ID || coreId > LAST_CORE_ID) {return CM_INVALID_PARAMETER;}
+
+    /* check core has been booted */
+    pData = (t_uint32 *) mpcLoad_i[coreId].perfmeterDataAddr;
+    if (pData == NULL) {return CM_OK;}
+
+    do {
+        prcmuBeforeAttributes = OSAL_GetPrcmuTimer();
+        /* get attributes */
+        do
+        {
+            for(i = 0;i < PERFMETER_DATA_WORD_NB;i++)
+                data[i] = pData[i];
+        }
+        while(((data[0] & 0xff0000) != (data[1] & 0xff0000) || (data[0] & 0xff0000) != (data[2] & 0xff0000) ||
+               (data[0] & 0xff0000) != (data[3] & 0xff0000) || (data[0] & 0xff0000) != (data[4] & 0xff0000) ||
+               (data[0] & 0xff0000) != (data[5] & 0xff0000) || (data[0] & 0xff0000) != (data[6] & 0xff0000) ||
+               (data[0] & 0xff0000) != (data[6] & 0xff0000))
+               && retryCounter-- < PERFMETER_MAX_RETRIES); // check data coherence
+        if (retryCounter >= PERFMETER_MAX_RETRIES)
+            return CM_MPC_NOT_RESPONDING;
+
+        /* read forever counter for totalCounter */
+        pMpcLoadCounter->totalCounter = OSAL_GetPrcmuTimer();
+    } while(pMpcLoadCounter->totalCounter - prcmuBeforeAttributes >= 32); //we loop until it seems we have not be preempt too long (< 1ms)
+
+    /* we got coherent data, use them */
+    pMpcLoadCounter->loadCounter = ((data[0] & (t_uint64)0xffff) << 32) + ((data[1] & (t_uint64)0xffff) << 16) + ((data[2] & (t_uint64)0xffff) << 0);
+    //fix load counter if needed
+    if ((data[6] & 0xffff) == 1) {
+        t_uint64 lastEvent;
+
+        lastEvent = ((data[3] & (t_uint64)0xffff) << 32) + ((data[4] & (t_uint64)0xffff) << 16) + ((data[5] & (t_uint64)0xffff) << 0);
+        pMpcLoadCounter->loadCounter += pMpcLoadCounter->totalCounter - lastEvent;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_getMpcLoadCounter(
+    t_nmf_core_id coreId,
+    t_cm_mpc_load_counter *pMpcLoadCounter
+)
+{
+	t_cm_error error;
+
+    OSAL_LOCK_API();
+    error = CM_GetMpcLoadCounter(coreId, pMpcLoadCounter);
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+/* internal api */
+PUBLIC t_cm_error cm_PFM_allocatePerfmeterDataMemory(t_nmf_core_id coreId, t_cm_domain_id domainId)
+{
+    t_cm_error error = CM_OK;
+    t_mpcLoad *pMpcLoad = (t_mpcLoad *) &mpcLoad_i[coreId];
+
+    pMpcLoad->perfmeterDataHandle = cm_DM_Alloc(domainId, SDRAM_EXT24, PERFMETER_DATA_WORD_NB, CM_MM_ALIGN_WORD, TRUE);
+    if (pMpcLoad->perfmeterDataHandle == INVALID_MEMORY_HANDLE) {
+        error = CM_NO_MORE_MEMORY;
+        ERROR("CM_NO_MORE_MEMORY: Unable to allocate perfmeter\n", 0, 0, 0, 0, 0, 0);
+    } else {
+        t_uint32 mmdspAddr;
+
+        pMpcLoad->perfmeterDataAddr = cm_DSP_GetHostLogicalAddress(pMpcLoad->perfmeterDataHandle);
+        cm_DSP_GetDspAddress(pMpcLoad->perfmeterDataHandle, &mmdspAddr);
+        cm_writeAttribute(cm_EEM_getExecutiveEngine(coreId)->instance, "rtos/perfmeter/perfmeterDataAddr", mmdspAddr);
+    }
+
+    return error;
+}
+
+PUBLIC void cm_PFM_deallocatePerfmeterDataMemory(t_nmf_core_id coreId)
+{
+    mpcLoad_i[coreId].perfmeterDataAddr = 0;
+    cm_DM_Free(mpcLoad_i[coreId].perfmeterDataHandle, TRUE);
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/power_mgt/inc/power.h b/drivers/staging/nmf-cm/cm/engine/power_mgt/inc/power.h
new file mode 100644
index 00000000000..942805df2f3
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/power_mgt/inc/power.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Enable a CM power domain by CoreID.
+ *
+ * \ingroup COMPONENT_INTERNAL
+ */
+#ifndef __INC_NMF_POWER
+#define __INC_NMF_POWER
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/dsp/inc/dsp.h>
+
+typedef enum
+{
+    DISABLE_PWR_MODE = 0x0, //!< Disable mode - CM Power management is disabled. CM Power domain are always enabled and the EEs are loaded by default
+    NORMAL_PWR_MODE = 0x1   //!< Normal mode
+} t_nmf_power_mode;
+
+extern t_nmf_power_mode powerMode;
+
+PUBLIC t_cm_error cm_PWR_Init(void);
+void cm_PWR_SetMode(t_nmf_power_mode aMode);
+t_nmf_power_mode cm_PWR_GetMode(void);
+t_uint32 cm_PWR_GetMPCMemoryCount(t_nmf_core_id coreId);
+
+typedef enum
+{
+    MPC_PWR_CLOCK,
+    MPC_PWR_AUTOIDLE,
+    MPC_PWR_HWIP
+} t_mpc_power_request;
+
+PUBLIC t_cm_error cm_PWR_EnableMPC(
+        t_mpc_power_request             request,
+        t_nmf_core_id                   coreId);
+PUBLIC void cm_PWR_DisableMPC(
+        t_mpc_power_request             request,
+        t_nmf_core_id                   coreId);
+
+PUBLIC t_cm_error cm_PWR_EnableHSEM(void);
+PUBLIC void cm_PWR_DisableHSEM(void);
+
+PUBLIC t_cm_error cm_PWR_EnableMemory(
+        t_nmf_core_id                   coreId,
+        t_dsp_memory_type_id            dspMemType,
+        t_cm_physical_address           address,
+        t_cm_size                       size);
+PUBLIC void cm_PWR_DisableMemory(
+        t_nmf_core_id                   coreId,
+        t_dsp_memory_type_id            dspMemType,
+        t_cm_physical_address           address,
+        t_cm_size                       size);
+
+
+#endif /* __INC_NMF_POWER */
diff --git a/drivers/staging/nmf-cm/cm/engine/power_mgt/src/cmpower.c b/drivers/staging/nmf-cm/cm/engine/power_mgt/src/cmpower.c
new file mode 100644
index 00000000000..a104486db6c
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/power_mgt/src/cmpower.c
@@ -0,0 +1,244 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include "../inc/power.h"
+
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/convert.h>
+#include <cm/engine/dsp/inc/dsp.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+
+// -------------------------------------------------------------------------------
+// Compilation flags
+// -------------------------------------------------------------------------------
+#define __PWR_DEBUG_TRACE_LEVEL 2  					// Debug trave level for CM power module
+
+// -------------------------------------------------------------------------------
+// Internal counter to store the TCM allocated chunk (by MPC)
+// -------------------------------------------------------------------------------
+static t_uint32 _pwrMPCHWIPCountT[NB_CORE_IDS];
+
+// -------------------------------------------------------------------------------
+// Internal counter to store the TCM allocated chunk (by MPC)
+// -------------------------------------------------------------------------------
+static t_uint32 _pwrMPCMemoryCountT[NB_CORE_IDS];
+
+
+// -------------------------------------------------------------------------------
+// Internal data to store the global Power Manager mode (see cm_PWR_Init fct)
+// -------------------------------------------------------------------------------
+t_nmf_power_mode powerMode = NORMAL_PWR_MODE;
+
+// -------------------------------------------------------------------------------
+// cm_PWR_Init
+// -------------------------------------------------------------------------------
+PUBLIC t_cm_error cm_PWR_Init(void)
+{
+	int i;
+
+	for (i=0; i<NB_CORE_IDS;i++)
+	{
+        _pwrMPCHWIPCountT[i] = 0;
+        _pwrMPCMemoryCountT[i] = 0;
+	}
+
+	return CM_OK;
+}
+
+// -------------------------------------------------------------------------------
+// cm_PWR_SetMode
+// -------------------------------------------------------------------------------
+void cm_PWR_SetMode(t_nmf_power_mode aMode)
+{
+	powerMode = aMode;
+}
+
+t_nmf_power_mode cm_PWR_GetMode()
+{
+    return powerMode;
+}
+
+t_uint32 cm_PWR_GetMPCMemoryCount(t_nmf_core_id coreId)
+{
+    return _pwrMPCMemoryCountT[coreId];
+}
+
+
+PUBLIC t_cm_error cm_PWR_EnableMPC(
+        t_mpc_power_request             request,
+        t_nmf_core_id                   coreId)
+{
+    t_cm_error error;
+
+    switch(request)
+    {
+    case MPC_PWR_CLOCK:
+        LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] MPC %s enable clock\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+        if((error = OSAL_EnablePwrRessource(CM_OSAL_POWER_SxA_CLOCK, coreId, 0)) != CM_OK)
+        {
+            ERROR("[Pwr] MPC %s clock can't be enabled\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+            return error;
+        }
+        break;
+    case MPC_PWR_AUTOIDLE:
+        if((error = OSAL_EnablePwrRessource(CM_OSAL_POWER_SxA_AUTOIDLE, coreId, 0)) != CM_OK)
+        {
+            ERROR("[Pwr] MPC %s clock can't be auto-idle\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+            return error;
+        }
+        break;
+    case MPC_PWR_HWIP:
+        if(_pwrMPCHWIPCountT[coreId]++ == 0)
+        {
+            LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] MPC %s HW IP enable clock\n",cm_getDspName(coreId), 0, 0, 0, 0, 0);
+
+            // The PRCMU seem not supporting the transition of asking HW IP on while DSP in retention
+            // -> Thus force wake up of the MMDSP before asking the transition
+            if ((error = cm_EEM_ForceWakeup(coreId)) != CM_OK)
+                return error;
+
+            if((error = OSAL_EnablePwrRessource(CM_OSAL_POWER_SxA_HARDWARE, coreId, 0)) != CM_OK)
+            {
+                ERROR("[Pwr] MPC %s HW IP clock can't be enabled\n", cm_getDspName(coreId), 0, 0, 0, 0, 0);
+                cm_EEM_AllowSleep(coreId);
+                return error;
+            }
+
+            cm_EEM_AllowSleep(coreId);
+        }
+        break;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_PWR_DisableMPC(
+        t_mpc_power_request             request,
+        t_nmf_core_id                   coreId)
+{
+    switch(request)
+    {
+    case MPC_PWR_CLOCK:
+        LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] MPC %s disable clock\n",cm_getDspName(coreId), 0, 0, 0, 0, 0);
+        OSAL_DisablePwrRessource(CM_OSAL_POWER_SxA_CLOCK, coreId, 0);
+        break;
+    case MPC_PWR_AUTOIDLE:
+        OSAL_DisablePwrRessource(CM_OSAL_POWER_SxA_AUTOIDLE, coreId, 0);
+        break;
+    case MPC_PWR_HWIP:
+        if(--_pwrMPCHWIPCountT[coreId] == 0)
+        {
+            LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] MPC %s HW IP disable clock\n",cm_getDspName(coreId), 0, 0, 0, 0, 0);
+
+            // The PRCMU seem not supporting the transition of asking HW IP on while DSP in retention
+            // -> Thus force wake up of the MMDSP before asking the transition
+            if (cm_EEM_ForceWakeup(coreId) != CM_OK)
+                return;
+
+            OSAL_DisablePwrRessource(CM_OSAL_POWER_SxA_HARDWARE, coreId, 0);
+
+            cm_EEM_AllowSleep(coreId);
+        }
+        break;
+    }
+}
+
+PUBLIC t_cm_error cm_PWR_EnableHSEM(void)
+{
+    t_cm_error error;
+
+    LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] HSEM enable clock\n",0 , 0, 0, 0, 0, 0);
+    if((error = OSAL_EnablePwrRessource(CM_OSAL_POWER_HSEM, 0, 0)) != CM_OK)
+    {
+        ERROR("[Pwr] HSEM clock can't be enabled\n", 0, 0, 0, 0, 0, 0);
+        return error;
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_PWR_DisableHSEM(void)
+{
+    LOG_INTERNAL(__PWR_DEBUG_TRACE_LEVEL, "[Pwr] HSEM disable clock\n",0 , 0, 0, 0, 0, 0);
+    OSAL_DisablePwrRessource(CM_OSAL_POWER_HSEM, 0, 0);
+}
+
+PUBLIC t_cm_error cm_PWR_EnableMemory(
+        t_nmf_core_id                   coreId,
+        t_dsp_memory_type_id            dspMemType,
+        t_cm_physical_address           address,
+        t_cm_size                       size)
+{
+    switch(dspMemType)
+    {
+    case INTERNAL_XRAM24:
+    case INTERNAL_XRAM16:
+    case INTERNAL_YRAM24:
+    case INTERNAL_YRAM16:
+        _pwrMPCMemoryCountT[coreId]++;
+        break;
+    case SDRAM_EXT24:
+    case SDRAM_EXT16:
+    case SDRAM_CODE:
+    case LOCKED_CODE:
+        return OSAL_EnablePwrRessource(
+                CM_OSAL_POWER_SDRAM,
+                address,
+                size);
+    case ESRAM_EXT24:
+    case ESRAM_EXT16:
+    case ESRAM_CODE:
+        return OSAL_EnablePwrRessource(
+                CM_OSAL_POWER_ESRAM,
+                address,
+                size);
+    default:
+	    CM_ASSERT(0);
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_PWR_DisableMemory(
+        t_nmf_core_id                   coreId,
+        t_dsp_memory_type_id            dspMemType,
+        t_cm_physical_address           address,
+        t_cm_size                       size)
+{
+    switch(dspMemType)
+    {
+    case INTERNAL_XRAM24:
+    case INTERNAL_XRAM16:
+    case INTERNAL_YRAM24:
+    case INTERNAL_YRAM16:
+        _pwrMPCMemoryCountT[coreId]--;
+        break;
+    case SDRAM_EXT24:
+    case SDRAM_EXT16:
+    case SDRAM_CODE:
+    case LOCKED_CODE:
+        OSAL_DisablePwrRessource(
+                CM_OSAL_POWER_SDRAM,
+                address,
+                size);
+        break;
+    case ESRAM_EXT24:
+    case ESRAM_EXT16:
+    case ESRAM_CODE:
+        OSAL_DisablePwrRessource(
+                CM_OSAL_POWER_ESRAM,
+                address,
+                size);
+        break;
+    default:
+	    CM_ASSERT(0);
+    }
+}
+
+
+
+
+
diff --git a/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_mgt.h b/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_mgt.h
new file mode 100644
index 00000000000..d2c7185b24f
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_mgt.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Component repository internal methods.
+ *
+ * \defgroup REPOSITORY_INTERNAL Component repository.
+ */
+#ifndef __INC_CM_REP_MGT_H
+#define __INC_CM_REP_MGT_H
+
+#include <cm/inc/cm_type.h>
+#include <inc/nmf-limits.h>
+
+/*!
+ * \brief Identification of a component entry.
+ * \ingroup REPOSITORY_INTERNAL
+ */
+typedef struct t_rep_component {
+    t_dup_char             name;
+	struct t_rep_component *prev;
+	struct t_rep_component *next;
+	t_elfdescription *elfhandle;                      //!< Must be last as data will be stored here
+} t_rep_component;
+
+/*!
+ * \brief Search a component entry by name.
+ *
+ * \param[in]  name The name of the component to look for.
+ * \param[out] component The corresponding component entry in the repository
+ *
+ * \retval t_cm_error
+ *
+ * \ingroup REPOSITORY_INTERNAL
+ */
+PUBLIC t_cm_error cm_REP_lookupComponent(const char *name, t_rep_component **component);
+
+/*!
+ * \brief Helper method that return the dataFile found in parameter or in the cache
+ */
+t_elfdescription* cm_REP_getComponentFile(t_dup_char templateName, t_elfdescription* elfhandle);
+
+/*!
+ * \brief Destroy the full repository (remove and free all components)
+ *
+ * \retval none
+ *
+ * \ingroup REPOSITORY_INTERNAL
+ */
+PUBLIC void cm_REP_Destroy(void);
+
+#endif /* __INC_CM_REP_MGT_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_type.h b/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_type.h
new file mode 100644
index 00000000000..30ef8004c48
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/repository_mgt/inc/repository_type.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Components Component Manager API type.
+ *
+ * \defgroup COMPONENT CM Components API
+ * \ingroup CM_USER_API
+ */
+
+#ifndef REPOSITORY_TYPE_H_
+#define REPOSITORY_TYPE_H_
+
+typedef enum
+{
+    BIND_ASYNC,
+    BIND_TRACE,
+    BIND_FROMUSER,
+    BIND_TOUSER
+} t_action_to_do;
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/repository_mgt/src/repository_mgt.c b/drivers/staging/nmf-cm/cm/engine/repository_mgt/src/repository_mgt.c
new file mode 100644
index 00000000000..f6ccb4a9992
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/repository_mgt/src/repository_mgt.c
@@ -0,0 +1,322 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/utils/inc/string.h>
+
+#include <cm/engine/component/inc/component_type.h>
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/configuration/inc/configuration.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/repository_mgt/inc/repository_mgt.h>
+#include <cm/engine/api/repository_mgt_engine.h>
+#include <cm/engine/trace/inc/trace.h>
+
+
+#undef NHASH
+#define NHASH 157       //Use a prime number!
+#define MULT 17
+
+static t_rep_component *componentCaches[NHASH];
+
+static unsigned int repcomponentHash(const char *str)
+{
+    unsigned int h = 0;
+    for(; *str; str++)
+        h = MULT * h + *str;
+    return h % NHASH;
+}
+
+static void repcomponentAdd(t_rep_component *component)
+{
+    unsigned int h = repcomponentHash(component->name);
+
+    if(componentCaches[h] != NULL)
+        componentCaches[h]->prev = component;
+    component->next = componentCaches[h];
+    component->prev = NULL;
+    componentCaches[h] = component;
+}
+
+static void repcomponentRemove(t_rep_component *component)
+{
+    unsigned int h = repcomponentHash(component->name);
+
+    if(component->prev != NULL)
+        component->prev->next = component->next;
+    if(component->next != NULL)
+        component->next->prev = component->prev;
+    if(component == componentCaches[h])
+        componentCaches[h] = component->next;
+}
+
+
+PUBLIC t_cm_error cm_REP_lookupComponent(const char *name, t_rep_component **component)
+{
+    t_rep_component *tmp;
+
+    for(tmp = componentCaches[repcomponentHash(name)]; tmp != NULL; tmp = tmp->next)
+    {
+        if(cm_StringCompare(name, tmp->name, MAX_TEMPLATE_NAME_LENGTH) == 0)
+        {
+            if(component != NULL)
+                *component = tmp;
+            return CM_OK;
+        }
+    }
+
+    return CM_COMPONENT_NOT_FOUND;
+}
+
+t_elfdescription* cm_REP_getComponentFile(t_dup_char templateName, t_elfdescription* elfhandle)
+{
+    if(elfhandle == NULL)
+    {
+        t_rep_component *pRepComponent;
+
+        for(pRepComponent = componentCaches[repcomponentHash(templateName)]; pRepComponent != NULL; pRepComponent = pRepComponent->next)
+        {
+            if(pRepComponent->name == templateName)
+                return pRepComponent->elfhandle;
+        }
+
+        return NULL;
+    }
+
+    return elfhandle;
+}
+
+
+PUBLIC void cm_REP_Destroy(void)
+{
+	t_rep_component *component, *next;
+	int i;
+
+	for(i = 0; i < NHASH; i++)
+	{
+	    for (component = componentCaches[i]; component != NULL; component = next)
+	    {
+	        next = component->next;
+	        cm_ELF_CloseFile(FALSE, component->elfhandle);
+	        cm_StringRelease(component->name);
+	        OSAL_Free(component);
+	    }
+	    componentCaches[i] = NULL;
+	}
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_GetRequiredComponentFiles(
+        // IN
+        t_action_to_do action,
+        const t_cm_instance_handle client,
+        const char *requiredItfClientName,
+        const t_cm_instance_handle server,
+        const char *providedItfServerName,
+        // OUT component to be pushed
+        char fileList[][MAX_INTERFACE_TYPE_NAME_LENGTH],
+        t_uint32 listSize,
+        // OUT interface information
+        char type[MAX_INTERFACE_TYPE_NAME_LENGTH],
+        t_uint32 *methodNumber)
+{
+    t_cm_error error;
+    t_component_instance* compClient, *compServer;
+    int n;
+
+    OSAL_LOCK_API();
+
+    // No component required
+    for(n = 0; n < listSize; n++)
+        fileList[n][0] = 0;
+
+    compClient = cm_lookupComponent(client);
+    compServer = cm_lookupComponent(server);
+    switch(action)
+    {
+    case BIND_FROMUSER:{
+        t_interface_provide_description itfProvide;
+
+        // Check server validity
+        if((error = cm_checkValidServer(compServer, providedItfServerName,
+                &itfProvide)) == CM_OK)
+        {
+            cm_StringCopy(type, itfProvide.server->Template->provides[itfProvide.provideIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+            cm_StringCopy(fileList[0], "_sk.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+            cm_StringConcatenate(fileList[0], itfProvide.server->Template->provides[itfProvide.provideIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+        }
+    } break;
+
+    case BIND_TOUSER: {
+        /* Get Components names for a BindComponentToCMCore */
+        t_interface_require_description itfRequire;
+        t_bool bindable;
+
+        // Check client validity
+        if((error = cm_checkValidClient(compClient, requiredItfClientName,
+                &itfRequire, &bindable)) == CM_OK)
+        {
+            cm_StringCopy(type, itfRequire.client->Template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+            *methodNumber = itfRequire.client->Template->requires[itfRequire.requireIndex].interface->methodNumber;
+
+            cm_StringCopy(fileList[0], "_st.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+            cm_StringConcatenate(fileList[0], itfRequire.client->Template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+        }
+    }; break;
+
+    case BIND_ASYNC: {
+        /* Get Components names for an asynchronous binding */
+        t_interface_require_description itfRequire;
+        t_interface_provide_description itfProvide;
+        t_bool bindable;
+
+        // Check invalid binding
+        if((error = cm_checkValidBinding(compClient, requiredItfClientName,
+                compServer, providedItfServerName,
+                &itfRequire, &itfProvide, &bindable)) == CM_OK)
+        {
+                if(compClient->Template->dspId != compServer->Template->dspId)
+                {
+                    cm_StringCopy(fileList[0], "_sk.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+                    cm_StringConcatenate(fileList[0], itfRequire.client->Template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+
+                    cm_StringCopy(fileList[1], "_st.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+                    cm_StringConcatenate(fileList[1], itfRequire.client->Template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+                }
+                else
+                {
+                    cm_StringCopy(fileList[0], "_ev.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+                    cm_StringConcatenate(fileList[0], itfRequire.client->Template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+                }
+        }
+    }; break;
+
+    case BIND_TRACE: {
+        /* Get Components names for an asynchronous binding */
+        t_interface_require_description itfRequire;
+        t_interface_provide_description itfProvide;
+        t_bool bindable;
+
+        // Check invalid binding
+        if((error = cm_checkValidBinding(compClient, requiredItfClientName,
+                compServer, providedItfServerName,
+                &itfRequire, &itfProvide, &bindable)) == CM_OK)
+        {
+            cm_StringCopy(fileList[0], "_tr.", MAX_INTERFACE_TYPE_NAME_LENGTH);
+            cm_StringConcatenate(fileList[0], itfRequire.client->Template->requires[itfRequire.requireIndex].interface->type, MAX_INTERFACE_TYPE_NAME_LENGTH);
+        }
+    }; break;
+
+    default:
+        error = CM_OK;
+        break;
+    }
+
+    if(error == CM_OK)
+    {
+        for(n = 0; n < listSize; n++)
+        {
+            t_rep_component *comp;
+
+            // If already loaded, don't ask to load it and put the name to NULL
+            if (fileList[n][0] != 0 &&
+                    cm_REP_lookupComponent(fileList[n], &comp) == CM_OK)
+                fileList[n][0] = 0;
+        }
+    }
+
+
+    OSAL_UNLOCK_API();
+    return error;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_PushComponent(const char *name, const void *data, t_cm_size size)
+{
+	t_rep_component *comp;
+	t_cm_error error;
+
+	OSAL_LOCK_API();
+
+	if (cm_REP_lookupComponent(name, &comp) == CM_OK) {
+		/* Component is already there: silently ignore it */
+		OSAL_UNLOCK_API();
+		return CM_OK;
+	}
+
+	comp = OSAL_Alloc(sizeof(*comp));
+	if (comp == NULL) {
+		OSAL_UNLOCK_API();
+		return CM_NO_MORE_MEMORY;
+	}
+
+	comp->name = cm_StringDuplicate(name);
+	if(comp->name == NULL)
+	{
+        OSAL_Free(comp);
+        OSAL_UNLOCK_API();
+        return CM_NO_MORE_MEMORY;
+	}
+
+    if((error = cm_ELF_CheckFile(
+            data,
+            FALSE,
+            &comp->elfhandle)) != CM_OK) {
+        cm_StringRelease(comp->name);
+        OSAL_Free(comp);
+        OSAL_UNLOCK_API();
+        return error;
+    }
+/*
+	if (OSAL_Copy(comp->data, data, size)) {
+		OSAL_Free(comp);
+		OSAL_UNLOCK_API();
+		return CM_UNKNOWN_MEMORY_HANDLE;
+	}*/
+
+    repcomponentAdd(comp);
+
+	OSAL_UNLOCK_API();
+	return CM_OK;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ENGINE_ReleaseComponent (const char *name)
+{
+	t_rep_component *component;
+	t_cm_error err;
+
+	OSAL_LOCK_API();
+	err = cm_REP_lookupComponent(name , &component);
+
+	if (CM_OK == err)
+	{
+	    repcomponentRemove(component);
+
+	    cm_ELF_CloseFile(FALSE, component->elfhandle);
+	    cm_StringRelease(component->name);
+	    OSAL_Free(component);
+	}
+
+	OSAL_UNLOCK_API();
+
+	return err;
+}
+
+PUBLIC EXPORT_SHARED t_bool CM_ENGINE_IsComponentCacheEmpty(void)
+{
+	int i;
+
+	OSAL_LOCK_API();
+	for(i = 0; i < NHASH; i++) {
+		if (componentCaches[i] != NULL) {
+			OSAL_UNLOCK_API();
+			return FALSE;
+		}
+	}
+	OSAL_UNLOCK_API();
+	return TRUE;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h b/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h
new file mode 100644
index 00000000000..bd914195b6d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_HW_SEMA_H_
+#define __INC_HW_SEMA_H_
+
+#include <cm/inc/cm_type.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <share/semaphores/inc/hwsem_hwp.h>
+
+
+/******************************************************************************/
+/************************ FUNCTIONS PROTOTYPES ********************************/
+/******************************************************************************/
+
+PUBLIC t_cm_error cm_HSEM_Init(const t_cm_system_address *pSystemAddr);
+PUBLIC t_cm_error cm_HSEM_EnableSemIrq(t_semaphore_id semId, t_nmf_core_id toCoreId);
+PUBLIC void cm_HSEM_Take(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_HSEM_Give(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_HSEM_GiveWithInterruptGeneration(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC void cm_HSEM_GenerateIrq(t_nmf_core_id coreId, t_semaphore_id semId);
+PUBLIC t_nmf_core_id cm_HSEM_GetCoreIdFromIrqSrc(void);
+
+PUBLIC t_cm_error cm_HSEM_PowerOn(t_nmf_core_id coreId);
+PUBLIC void cm_HSEM_PowerOff(t_nmf_core_id coreId);
+
+#endif /* __INC_HW_SEMA_H_ */
diff --git a/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/src/hw_semaphores.c b/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/src/hw_semaphores.c
new file mode 100644
index 00000000000..932058cd4f2
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/semaphores/hw_semaphores/src/hw_semaphores.c
@@ -0,0 +1,171 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/******************************************************************* Includes
+ ****************************************************************************/
+
+#include "../inc/hw_semaphores.h"
+#include <share/semaphores/inc/hwsem_hwp.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/power_mgt/inc/power.h>
+static t_hw_semaphore_regs *pHwSemRegs = (t_hw_semaphore_regs *)0;
+
+static t_uint32 semaphoreUseCounter = 0;
+static t_uint32 imsc[HSEM_MAX_INTR];
+PRIVATE void restoreMask(void);
+
+/****************************************************************************/
+/* NAME:   t_cm_error cm_HSEM_Init(const t_cm_system_address *pSystemAddr)     */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Initialize the HW Semaphores module                         */
+/*                                                                          */
+/* PARAMETERS:                                                              */
+/*       (in)   pSystemAddr: system base address of the HW semaphores IP    */
+/*                                                                          */
+/* RETURN:      CM_OK always                                                */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC t_cm_error cm_HSEM_Init(const t_cm_system_address *pSystemAddr)
+{
+    t_uint8 i;
+
+    pHwSemRegs = (t_hw_semaphore_regs *)pSystemAddr->logical;
+
+    for (i=HSEM_FIRST_INTR; i < HSEM_MAX_INTR; i++)
+    {
+        imsc[i] = 0; // Mask all interrupt
+    }
+
+    return CM_OK;
+}
+
+static void cm_HSEM_ReInit(void)
+{
+    t_uint8 i;
+
+    pHwSemRegs->icrall = MASK_ALL16;
+
+    for (i=HSEM_FIRST_INTR; i < HSEM_MAX_INTR; i++)
+    {
+        pHwSemRegs->it[i].imsc = imsc[i];
+        pHwSemRegs->it[i].icr = MASK_ALL16;
+    }
+
+    for (i=0; i < NUM_HW_SEMAPHORES; i++)
+    {
+        pHwSemRegs->sem[i] = 0;
+    }
+}
+
+/****************************************************************************/
+/* NAME:   t_cm_error cm_HSEM_EnableSemIrq(                                */
+/*                                          t_semaphore_id semId,           */
+/*                                          t_nmf_core_id toCoreId           */
+/*                                         )                                */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Enable Irq for a given coreId (communication receiver)      */
+/*                                                                          */
+/* PARAMETERS:                                                              */
+/*       (in)   semId: identifier of the semaphore                          */
+/*       (in)   toCoreId: identifier of coreId destination of the coms      */
+/*                                                                          */
+/* RETURN:      CM_OK always                                                */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC t_cm_error cm_HSEM_EnableSemIrq(t_semaphore_id semId, t_nmf_core_id toCoreId)
+{
+    static t_uint32 CoreIdToIntr[NB_CORE_IDS] = {0, 2, 3};
+    int i = CoreIdToIntr[toCoreId];
+
+    imsc[i] |= (1UL << semId);
+
+    // Allow cm_HSEM_EnableSemIrq to be called before real start in order to save power
+    if(semaphoreUseCounter > 0)
+    {
+        pHwSemRegs->it[i].imsc = imsc[i];
+    }
+
+    return CM_OK;
+}
+
+/****************************************************************************/
+/* NAME:  void cm_HSEM_GenerateIrq(t_semaphore_id semId)                    */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Generate an irq toward correct core according to semId      */
+/*                                                                          */
+/* PARAMETERS:                                                              */
+/*       (in)   semId: identifier of the semaphore to handle                */
+/*                                                                          */
+/* RETURN:      none                                                        */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC void cm_HSEM_GenerateIrq(t_nmf_core_id coreId, t_semaphore_id semId)
+{
+   // TODO Move restore in OS BSP or in PRCMU in order to to it only when wake-up, for now do it always !!!!!!!!!!!!
+            restoreMask();
+
+    pHwSemRegs->sem[semId] = CORE_ID_2_HW_CORE_ID(ARM_CORE_ID);
+    pHwSemRegs->sem[semId] = (HSEM_INTRA_MASK|HSEM_INTRB_MASK|HSEM_INTRC_MASK|HSEM_INTRD_MASK);
+}
+
+/****************************************************************************/
+/* NAME:  t_nmf_core_id cm_HSEM_GetCoreIdFromIrqSrc(void)                   */
+/*--------------------------------------------------------------------------*/
+/* DESCRIPTION: Check Masked Interrupt Status to know which semaphore(s)    */
+/*        have pending interrupt and return the identifier of the given dsp */
+/*                                                                          */
+/* PARAMETERS:  none                                                        */
+/*                                                                          */
+/* RETURN:      none                                                        */
+/*                                                                          */
+/****************************************************************************/
+PUBLIC t_nmf_core_id cm_HSEM_GetCoreIdFromIrqSrc(void)
+{
+    t_uword misValue = pHwSemRegs->it[ARM_CORE_ID].mis;
+    t_uint32 mask = 1 << FIRST_NEIGHBOR_SEMID(ARM_CORE_ID) /* == 0 here */;
+    t_nmf_core_id coreId = FIRST_MPC_ID;
+
+    while ((misValue & mask) == 0)
+    {
+        mask <<= 1;
+
+        coreId++;
+        if(coreId > LAST_MPC_ID)
+            return coreId;
+    }
+
+    /* Acknowledge Hsem interrupt */
+    pHwSemRegs->it[ARM_CORE_ID].icr = mask;
+
+    return coreId;
+}
+
+PUBLIC t_cm_error cm_HSEM_PowerOn(t_nmf_core_id coreId)
+{
+    if(semaphoreUseCounter++ == 0)
+    {
+        cm_PWR_EnableHSEM();
+
+        cm_HSEM_ReInit();  // HSEM is called one time only when the HSEM is switched ON
+    }
+
+    return CM_OK;
+}
+
+PUBLIC void cm_HSEM_PowerOff(t_nmf_core_id coreId)
+{
+    if(--semaphoreUseCounter == 0)
+    {
+        cm_PWR_DisableHSEM();
+    }
+}
+
+PRIVATE void restoreMask()
+{
+    t_uint8 i;
+    
+    for (i=HSEM_FIRST_INTR; i < HSEM_MAX_INTR; i++)
+        pHwSemRegs->it[i].imsc = imsc[i];
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/semaphores/inc/semaphores.h b/drivers/staging/nmf-cm/cm/engine/semaphores/inc/semaphores.h
new file mode 100644
index 00000000000..7636d8e7c9d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/semaphores/inc/semaphores.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/**
+ * \internal
+ */
+#ifndef __INC_NMF_SEMAPHORE_H
+#define __INC_NMF_SEMAPHORE_H
+
+#include <cm/engine/api/control/configuration_engine.h>
+#include <share/semaphores/inc/semaphores.h>
+#include <cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h>
+
+PUBLIC t_cm_error cm_SEM_Init(const t_cm_system_address *pSystemAddr);
+PUBLIC t_cm_error cm_SEM_InitMpc(t_nmf_core_id coreId, t_nmf_semaphore_type_id semTypeId);
+PUBLIC t_semaphore_id cm_SEM_Alloc(t_nmf_core_id fromCoreId, t_nmf_core_id toCoreId);
+
+/* Semaphores management virtualized functions */
+extern void (*cm_SEM_GenerateIrq[NB_CORE_IDS])(t_nmf_core_id coreId, t_semaphore_id semId);
+extern t_cm_error (*cm_SEM_PowerOn[NB_CORE_IDS])(t_nmf_core_id coreId);
+extern void (*cm_SEM_PowerOff[NB_CORE_IDS])(t_nmf_core_id coreId);
+
+#endif /* __INC_NMF_SEMAPHORE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/semaphores/src/semaphores.c b/drivers/staging/nmf-cm/cm/engine/semaphores/src/semaphores.c
new file mode 100644
index 00000000000..daf95355a56
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/semaphores/src/semaphores.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/semaphores/inc/semaphores.h>
+#include <cm/engine/semaphores/hw_semaphores/inc/hw_semaphores.h>
+#include <cm/engine/dsp/inc/semaphores_dsp.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <share/inc/nmf.h>
+
+void (*cm_SEM_GenerateIrq[NB_CORE_IDS])(t_nmf_core_id coreId, t_semaphore_id semId);
+t_cm_error (*cm_SEM_PowerOn[NB_CORE_IDS])(t_nmf_core_id coreId);
+void (*cm_SEM_PowerOff[NB_CORE_IDS])(t_nmf_core_id coreId);
+
+#define SEM_TYPE_ID_DEFAULT_VALUE ((t_nmf_semaphore_type_id)MASK_ALL32)
+static t_nmf_semaphore_type_id semaphoreTypePerCoreId[NB_CORE_IDS];
+
+static t_cm_error cm_LSEM_PowerOn(t_nmf_core_id coreId)
+{
+    return CM_OK;
+}
+
+static void cm_LSEM_PowerOff(t_nmf_core_id coreId)
+{
+}
+
+PUBLIC t_cm_error cm_SEM_Init(const t_cm_system_address *pSystemAddr)
+{
+    t_nmf_core_id coreId;
+
+    for (coreId = ARM_CORE_ID; coreId < NB_CORE_IDS; coreId++)
+    {
+        semaphoreTypePerCoreId[coreId] = SEM_TYPE_ID_DEFAULT_VALUE;
+
+        /* By default, we suppose that we use a full feature NMF ;) */
+        cm_SEM_GenerateIrq[coreId] = NULL;
+        cm_SEM_PowerOn[coreId] = NULL;
+        cm_SEM_PowerOff[coreId] = NULL;
+   }
+
+    cm_HSEM_Init(pSystemAddr);
+    /* if needed local semaphore init will be done coreId per coreId */
+
+    return CM_OK;
+}
+
+PUBLIC t_cm_error cm_SEM_InitMpc(t_nmf_core_id coreId, t_nmf_semaphore_type_id semTypeId)
+{
+    if (semaphoreTypePerCoreId[coreId] != SEM_TYPE_ID_DEFAULT_VALUE)
+        return CM_MPC_ALREADY_INITIALIZED;
+
+    if(semTypeId == SYSTEM_SEMAPHORES)
+    {
+        cm_SEM_GenerateIrq[coreId] = cm_HSEM_GenerateIrq;
+        cm_SEM_PowerOn[coreId] = cm_HSEM_PowerOn;
+        cm_SEM_PowerOff[coreId] = cm_HSEM_PowerOff;
+    }
+    else if (semTypeId == LOCAL_SEMAPHORES)
+    {
+        cm_SEM_GenerateIrq[coreId] = cm_DSP_SEM_GenerateIrq;
+        cm_SEM_PowerOn[coreId] = cm_LSEM_PowerOn;
+        cm_SEM_PowerOff[coreId] = cm_LSEM_PowerOff;
+    }
+
+    semaphoreTypePerCoreId[coreId] =  semTypeId;
+
+    return CM_OK;
+}
+
+PUBLIC t_semaphore_id cm_SEM_Alloc(t_nmf_core_id fromCoreId, t_nmf_core_id toCoreId)
+{
+    t_semaphore_id semId;
+    t_nmf_core_id corex;
+
+    semId = FIRST_NEIGHBOR_SEMID(toCoreId);
+    for (corex = FIRST_CORE_ID; corex < fromCoreId; corex++)
+    {
+        if (corex == toCoreId)
+            continue;
+        semId++;
+    }
+
+    if (
+            (toCoreId == ARM_CORE_ID && semaphoreTypePerCoreId[fromCoreId] == SYSTEM_SEMAPHORES) ||
+            (semaphoreTypePerCoreId[toCoreId] == SYSTEM_SEMAPHORES)
+       )
+    {
+        cm_HSEM_EnableSemIrq(semId, toCoreId);
+    }
+
+    return semId;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/trace/inc/trace.h b/drivers/staging/nmf-cm/cm/engine/trace/inc/trace.h
new file mode 100644
index 00000000000..111eaf1324e
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/trace/inc/trace.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Trace facilities management API
+ *
+ * \defgroup Trace Facilities
+ */
+#ifndef __INC_CM_TRACE_H
+#define __INC_CM_TRACE_H
+
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/configuration/inc/configuration_status.h>
+
+/*********************/
+/* Log related stuff */
+/*********************/
+#define ERROR(format, param1, param2, param3, param4, param5, param6) \
+do { \
+    if (cm_debug_level != -1)    \
+    OSAL_Log("Error: " format, (int)(param1), (int)(param2), (int)(param3), (int)(param4), (int)(param5), (int)(param6)); \
+    while(cm_error_break);\
+} while(0)
+
+#define WARNING(format, param1, param2, param3, param4, param5, param6) \
+do { \
+    if (cm_debug_level != -1)    \
+    OSAL_Log("Warning: " format, (int)(param1), (int)(param2), (int)(param3), (int)(param4), (int)(param5), (int)(param6)); \
+} while(0)
+
+#define LOG_INTERNAL(level, format, param1, param2, param3, param4, param5, param6) \
+do { \
+    if (level <= cm_debug_level)    \
+        OSAL_Log((const char *)format, (int)(param1), (int)(param2), (int)(param3), (int)(param4), (int)(param5), (int)(param6)); \
+} while(0)
+
+/*************************/
+/* Panic related stuff   */
+/*************************/
+#define CM_ASSERT(cond) \
+do { \
+	if(!(cond)) { OSAL_Log("CM_ASSERT at %s:%d\n", (int)__FILE__, (int)__LINE__, 0, 0, 0, 0); OSAL_Panic(); while(1); } \
+} while (0)
+
+#endif /* __INC_CM_TRACE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/trace/inc/xtitrace.h b/drivers/staging/nmf-cm/cm/engine/trace/inc/xtitrace.h
new file mode 100644
index 00000000000..1efd4f1e699
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/trace/inc/xtitrace.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#ifndef __INC_CM_XTITRACE_H
+#define __INC_CM_XTITRACE_H
+
+#include <cm/engine/component/inc/instance.h>
+
+#include <inc/nmf-tracedescription.h>
+
+extern t_bool cm_trace_enabled;
+
+/*************************/
+/* Trace   related stuff */
+/*************************/
+void cm_TRC_Dump(void);
+
+void cm_TRC_traceReset(void);
+
+void cm_TRC_traceLoadMap(
+        t_nmfTraceComponentCommandDescription cmd,
+        const t_component_instance* component);
+
+#define ARM_TRACE_COMPONENT ((const t_component_instance*)0xFFFFFFFF)
+
+void cm_TRC_traceBinding(
+        t_nmfTraceBindCommandDescription command,
+        const t_component_instance* clientComponent, const t_component_instance* serverComponent,
+        const char *requiredItfName, const char *providedItfName);
+
+void cm_TRC_traceCommunication(
+        t_nmfTraceCommunicationCommandDescription command,
+        t_nmf_core_id coreId,
+        t_nmf_core_id remoteCoreId);
+
+void cm_TRC_traceMemAlloc(t_nmfTraceAllocatorCommandDescription command, t_uint8 allocId, t_uint32 memorySize, const char *allocname);
+
+void cm_TRC_traceMem(t_nmfTraceAllocCommandDescription command, t_uint8 allocId, t_uint32 startAddress, t_uint32 memorySize);
+
+/*************************/
+/* MMDSP trace buffer    */
+/*************************/
+PUBLIC t_cm_error cm_SRV_allocateTraceBufferMemory(t_nmf_core_id coreId, t_cm_domain_id domainId);
+PUBLIC void cm_SRV_deallocateTraceBufferMemory(t_nmf_core_id coreId);
+
+
+
+#endif /* __INC_CM_TRACE_H */
diff --git a/drivers/staging/nmf-cm/cm/engine/trace/src/panic.c b/drivers/staging/nmf-cm/cm/engine/trace/src/panic.c
new file mode 100644
index 00000000000..e59d9f8b1ba
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/trace/src/panic.c
@@ -0,0 +1,331 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include <cm/inc/cm_type.h>
+#include <cm/engine/component/inc/introspection.h>
+#include <cm/engine/component/inc/bind.h>
+#include <cm/engine/executive_engine_mgt/inc/executive_engine_mgt.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/api/control/irq_engine.h>
+
+#include <cm/engine/utils/inc/convert.h>
+#include <share/communication/inc/nmf_service.h>
+
+PUBLIC t_cm_error cm_SRV_allocateTraceBufferMemory(t_nmf_core_id coreId, t_cm_domain_id domainId)
+{
+    t_ee_state *state = cm_EEM_getExecutiveEngine(coreId);
+
+    state->traceDataHandle = cm_DM_Alloc(domainId, SDRAM_EXT16,
+            TRACE_BUFFER_SIZE * sizeof(struct t_nmf_trace) / 2, CM_MM_ALIGN_WORD, TRUE);
+    if (state->traceDataHandle == INVALID_MEMORY_HANDLE)
+        return CM_NO_MORE_MEMORY;
+    else
+    {
+        t_uint32 mmdspAddr;
+        int i;
+
+        state->traceDataAddr = (struct t_nmf_trace*)cm_DSP_GetHostLogicalAddress(state->traceDataHandle);
+        cm_DSP_GetDspAddress(state->traceDataHandle, &mmdspAddr);
+        cm_writeAttribute(state->instance, "rtos/commonpart/traceDataAddr", mmdspAddr);
+
+        eeState[coreId].readTracePointer = 0;
+        eeState[coreId].lastReadedTraceRevision = 0;
+
+        for(i = 0; i < TRACE_BUFFER_SIZE; i++)
+            state->traceDataAddr[i].revision = 0;
+
+        return CM_OK;
+    }
+}
+
+PUBLIC void cm_SRV_deallocateTraceBufferMemory(t_nmf_core_id coreId)
+{
+    t_ee_state *state = cm_EEM_getExecutiveEngine(coreId);
+
+    state->traceDataAddr = 0;
+    cm_DM_Free(state->traceDataHandle, TRUE);
+}
+
+static t_uint32 swapHalfWord(t_uint32 word)
+{
+    return (word >> 16) | (word << 16);
+}
+
+PUBLIC EXPORT_SHARED t_cm_trace_type CM_ENGINE_GetNextTrace(
+        t_nmf_core_id               coreId,
+        struct t_nmf_trace          *trace)
+{
+    t_ee_state *state = cm_EEM_getExecutiveEngine(coreId);
+    t_uint32 foundRevision;
+    t_cm_trace_type type;
+
+    OSAL_LOCK_API();
+    if (state->traceDataAddr == NULL) {
+        type = CM_MPC_TRACE_NONE;
+	goto out;
+    }
+
+    foundRevision = swapHalfWord(state->traceDataAddr[state->readTracePointer].revision);
+
+    if(foundRevision <= state->lastReadedTraceRevision)
+    {
+        // It's an old trace forgot it
+        type = CM_MPC_TRACE_NONE;
+    }
+    else
+    {
+        struct t_nmf_trace          *traceRaw;
+
+        if(foundRevision == state->lastReadedTraceRevision + 1)
+        {
+            type = CM_MPC_TRACE_READ;
+        }
+        else
+        {
+            type = CM_MPC_TRACE_READ_OVERRUN;
+            /*
+             * If we find that revision is bigger, thus we are in overrun, then we take the writePointer + 1 which
+             * correspond to the older one.
+             * => Here there is a window where the MMDSP could update writePointer just after
+             */
+            state->readTracePointer = (cm_readAttributeNoError(state->instance, "rtos/commonpart/writePointer") + 1) % TRACE_BUFFER_SIZE;
+        }
+
+        traceRaw = &state->traceDataAddr[state->readTracePointer];
+
+        trace->timeStamp = swapHalfWord(traceRaw->timeStamp);
+        trace->componentId = swapHalfWord(traceRaw->componentId);
+        trace->traceId = swapHalfWord(traceRaw->traceId);
+        trace->paramOpt = swapHalfWord(traceRaw->paramOpt);
+        trace->componentHandle = swapHalfWord(traceRaw->componentHandle);
+        trace->parentHandle = swapHalfWord(traceRaw->parentHandle);
+
+        trace->params[0] = swapHalfWord(traceRaw->params[0]);
+        trace->params[1] = swapHalfWord(traceRaw->params[1]);
+        trace->params[2] = swapHalfWord(traceRaw->params[2]);
+        trace->params[3] = swapHalfWord(traceRaw->params[3]);
+
+        state->readTracePointer = (state->readTracePointer + 1) % TRACE_BUFFER_SIZE;
+        state->lastReadedTraceRevision = swapHalfWord(traceRaw->revision);
+        trace->revision = state->lastReadedTraceRevision;
+    }
+
+out:
+    OSAL_UNLOCK_API();
+
+    return type;
+}
+
+
+/*
+ * Panic
+ */
+const struct {
+	char* name;
+    unsigned int info1:1;
+    unsigned int PC:1;
+	unsigned int SP:1;
+	unsigned int interface:1;
+} reason_descrs[] = {
+		{"NONE_PANIC",                0,  0,  0,  0},
+		{"INTERNAL_PANIC",            1,  0,  0,  0},
+		{"MPC_NOT_RESPONDING_PANIC",  0,  0,  0,  0}, /* Should not be useful since in that case CM_getServiceDescription() not call */
+		{"USER_STACK_OVERFLOW",       0,  1,  1,  0},
+		{"SYSTEM_STACK_OVERFLOW",     0,  1,  1,  0},
+		{"UNALIGNED_LONG_ACCESS",     0,  1,  0,  0},
+		{"EVENT_FIFO_OVERFLOW",       0,  0,  0,  1},
+		{"PARAM_FIFO_OVERFLOW",       0,  0,  0,  1},
+		{"INTERFACE_NOT_BINDED",      0,  1,  0,  0},
+		{"USER_PANIC",                1,  0,  0,  0}
+};
+
+static t_component_instance* getCorrespondingInstance(
+        t_panic_reason panicReason,
+        t_uint32 panicThis,
+        t_dup_char *itfName,
+	t_cm_instance_handle *instHandle) {
+    t_component_instance *instance;
+    t_uint32 k;
+
+    for (k=0; k<ComponentTable.idxMax; k++) {
+        if ((instance = componentEntry(k)) == NULL)
+            continue;
+        if(panicReason == PARAM_FIFO_OVERFLOW ||
+                panicReason == EVENT_FIFO_OVERFLOW) {
+            // Panic has been generated by binding component, search the client who has call it
+            // and return the client handle (not the BC one).
+            int i;
+
+            if(instance->thisAddress == panicThis && panicThis == 0) {
+                *itfName = "Internal NMF service";
+                *instHandle = ENTRY2HANDLE(instance, k);
+                return instance;
+            }
+
+            for(i = 0; i < instance->Template->requireNumber; i++) {
+                int nb = instance->Template->requires[i].collectionSize, j;
+                for(j = 0; j < nb; j++) {
+                    if(instance->interfaceReferences[i][j].instance != NULL &&
+                            instance->interfaceReferences[i][j].instance != (t_component_instance *)NMF_HOST_COMPONENT &&
+                            instance->interfaceReferences[i][j].instance != (t_component_instance *)NMF_VOID_COMPONENT &&
+                            instance->interfaceReferences[i][j].instance->thisAddress == panicThis)
+                    {
+                        *itfName = instance->Template->requires[i].name;
+                        *instHandle = ENTRY2HANDLE(instance, k);
+                        return instance;
+                    }
+                }
+            }
+        } else {
+            // The component which has generated the panic is the good one.
+
+            if(instance->thisAddress == panicThis) {
+		*itfName = "?";
+                *instHandle = ENTRY2HANDLE(instance, k);
+                return instance;
+            }
+        }
+    }
+
+    *itfName = "?";
+    *instHandle = 0;
+    return 0;
+}
+
+PUBLIC EXPORT_SHARED t_cm_error CM_ReadMPCString(
+        t_nmf_core_id               coreId,
+        t_uint32                    dspAddress,
+        char *                      buffer,
+        t_uint32                    bufferSize) {
+
+    while(--bufferSize > 0)
+    {
+        char ch = cm_DSP_ReadXRamWord(coreId, dspAddress++);
+        if(ch == 0)
+            break;
+
+        *buffer++ = ch;
+    };
+
+    *buffer = 0;
+
+    // Reset panicReason
+    cm_writeAttribute(cm_EEM_getExecutiveEngine(coreId)->instance,
+            "rtos/commonpart/serviceReason", MPC_SERVICE_NONE);
+
+    return CM_OK;
+}
+
+/****************/
+/* Generic part */
+/****************/
+PUBLIC EXPORT_SHARED t_cm_error CM_getServiceDescription(
+        t_nmf_core_id               coreId,
+        t_cm_service_type           *srcType,
+        t_cm_service_description    *srcDescr)
+{
+    t_uint32 serviceReason;
+    t_component_instance *ee;
+
+    // Acknowledge interrupt (do it before resetting panicReason)
+    cm_DSP_AcknowledgeDspIrq(coreId, DSP2ARM_IRQ_1);
+
+    ee = cm_EEM_getExecutiveEngine(coreId)->instance;
+
+    // Read panicReason
+    serviceReason = cm_readAttributeNoError(ee, "rtos/commonpart/serviceReason");
+    if(serviceReason == MPC_SERVICE_PRINT)
+    {
+        *srcType = CM_MPC_SERVICE_PRINT;
+
+        srcDescr->u.print.dspAddress = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo0");
+        srcDescr->u.print.value1 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo1");
+        srcDescr->u.print.value2 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo2");
+    }
+    else if(serviceReason == MPC_SERVICE_TRACE)
+    {
+        *srcType = CM_MPC_SERVICE_TRACE;
+    }
+    else if(serviceReason != MPC_SERVICE_NONE)
+    {
+        t_uint32 panicThis;
+        t_dup_char itfName;
+        t_component_instance *instance;
+
+        *srcType = CM_MPC_SERVICE_PANIC;
+        srcDescr->u.panic.panicReason = (t_panic_reason)serviceReason;
+        srcDescr->u.panic.panicSource = MPC_EE;
+        srcDescr->u.panic.info.mpc.coreid = coreId;
+
+        // Read panicThis
+        panicThis = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo0");
+
+        instance = getCorrespondingInstance(srcDescr->u.panic.panicReason, panicThis, &itfName, &srcDescr->u.panic.info.mpc.faultingComponent);
+
+        LOG_INTERNAL(0, "Error: Panic(%s, %s), This=%x", cm_getDspName(coreId),
+                reason_descrs[srcDescr->u.panic.panicReason].name, (void*)panicThis, 0, 0, 0);
+
+        if(reason_descrs[srcDescr->u.panic.panicReason].interface != 0)
+        {
+            LOG_INTERNAL(0, ", interface=%s", itfName, 0, 0, 0, 0, 0);
+        }
+
+        if(reason_descrs[srcDescr->u.panic.panicReason].info1 != 0)
+        {
+            // Info 1
+            srcDescr->u.panic.info.mpc.panicInfo1 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo1");
+
+            LOG_INTERNAL(0, ", Info=%x", srcDescr->u.panic.info.mpc.panicInfo1, 0, 0, 0, 0, 0);
+        }
+
+        if(reason_descrs[srcDescr->u.panic.panicReason].PC != 0)
+        {
+            t_uint32 DspAddress = 0xFFFFFFFF;
+            t_uint32 DspSize = 0x0;
+
+            // PC need to be read in rtos/commonpart/serviceInfo1
+            srcDescr->u.panic.info.mpc.panicInfo1 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo1");
+
+            if(instance != 0)
+            {
+                cm_DSP_GetDspAddress(instance->memories[instance->Template->codeMemory->id], &DspAddress);
+                cm_DSP_GetDspMemoryHandleSize(instance->memories[instance->Template->codeMemory->id], &DspSize);
+            }
+
+            if(DspAddress <= srcDescr->u.panic.info.mpc.panicInfo1 &&
+                    srcDescr->u.panic.info.mpc.panicInfo1 < (DspAddress + DspSize))
+                LOG_INTERNAL(0, ", PC:off=%x <abs=%x>",
+                        srcDescr->u.panic.info.mpc.panicInfo1 - DspAddress,
+                        srcDescr->u.panic.info.mpc.panicInfo1, 0, 0, 0, 0);
+            else
+                LOG_INTERNAL(0, ", PC:<abs=%x>", srcDescr->u.panic.info.mpc.panicInfo1, 0, 0, 0, 0, 0);
+        }
+
+        if(reason_descrs[srcDescr->u.panic.panicReason].SP != 0)
+        {
+            srcDescr->u.panic.info.mpc.panicInfo2 = cm_readAttributeNoError(ee, "rtos/commonpart/serviceInfo2");
+
+            LOG_INTERNAL(0, ", SP=%x", srcDescr->u.panic.info.mpc.panicInfo2, 0, 0, 0, 0, 0);
+        }
+
+        LOG_INTERNAL(0, "\n", 0, 0, 0, 0, 0, 0);
+
+        if(instance != 0)
+        {
+            LOG_INTERNAL(0, "Error:  Component=%s<%s>\n",
+                    instance->pathname, instance->Template->name, 0, 0, 0, 0);
+        }
+
+        // We don't set rtos/commonpart/serviceReason = MPC_SERVICE_NONE, since we don't want the
+        // MMDSP to continue execution, and we put in in Panic state
+        cm_DSP_SetStatePanic(coreId);
+    }
+    else
+    {
+        *srcType = CM_MPC_SERVICE_NONE;
+    }
+
+    return CM_OK;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/trace/src/trace.c b/drivers/staging/nmf-cm/cm/engine/trace/src/trace.c
new file mode 100644
index 00000000000..e27d3284ed2
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/trace/src/trace.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+#include "../inc/trace.h"
+#include "../inc/xtitrace.h"
+#include <inc/nmf-tracedescription.h>
+#include <inc/nmf-limits.h>
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+t_bool cm_trace_enabled = FALSE;
+
+/*
+ * STM message dump
+ */
+#define HEADER(t, s) ((t) | (s << 16))
+
+static void writeN(struct t_nmfTraceChannelHeader* header)
+{
+    t_uint64* data = (t_uint64*)header;
+    t_uint64 *end = (t_uint64*)(((unsigned int)data) + header->traceSize - sizeof(t_uint64));
+
+    while(data < end)
+    {
+        OSAL_Write64(CM_CHANNEL, 0,  *data++);
+    }
+
+    OSAL_Write64(CM_CHANNEL, 1, *data);
+}
+
+void cm_TRC_Dump(void)
+{
+    t_uint32 i;
+
+    cm_TRC_traceReset();
+
+    for (i=0; i<ComponentTable.idxMax; i++)
+    {
+	if (componentEntry(i) != NULL)
+	    cm_TRC_traceLoadMap(TRACE_COMPONENT_COMMAND_ADD, componentEntry(i));
+    }
+}
+
+void cm_TRC_traceReset(void)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceReset   trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_RESET, sizeof(trace));
+
+        trace.minorVersion = TRACE_MINOR_VERSION;
+        trace.majorVersion = TRACE_MAJOR_VERSION;
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
+void cm_TRC_traceLoadMap(
+        t_nmfTraceComponentCommandDescription command,
+        const t_component_instance* component)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceComponent   trace;
+
+        /*
+         * Generate instantiate trace
+         */
+        trace.header.v = HEADER(TRACE_TYPE_COMPONENT, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+        trace.domainId = (t_uint16)component->Template->dspId + 1;
+        trace.componentContext = (t_uint32)component->thisAddress;
+        trace.componentUserContext = (t_uint32)component;
+        cm_StringCopy((char*)trace.componentLocalName, component->pathname, MAX_COMPONENT_NAME_LENGTH);
+        cm_StringCopy((char*)trace.componentTemplateName, component->Template->name, MAX_TEMPLATE_NAME_LENGTH);
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+
+        if(command == TRACE_COMPONENT_COMMAND_ADD)
+        {
+            struct t_nmfTraceMethod tracemethod;
+            int i, j, k;
+
+            /*
+             * Generate method trace
+             */
+            tracemethod.header.v = HEADER(TRACE_TYPE_METHOD, sizeof(tracemethod));
+
+            tracemethod.domainId = (t_uint16)component->Template->dspId + 1;
+            tracemethod.componentContext = (t_uint32)component->thisAddress;
+
+            for(i = 0; i < component->Template->provideNumber; i++)
+            {
+                t_interface_provide* provide = &component->Template->provides[i];
+                t_interface_provide_loaded* provideLoaded = &component->Template->providesLoaded[i];
+
+                for(j = 0; j < provide->collectionSize; j++)
+                {
+                    for(k = 0; k < provide->interface->methodNumber; k++)
+                    {
+                        tracemethod.methodId = provideLoaded->indexesLoaded[j][k].methodAddresses;
+
+                        cm_StringCopy((char*)tracemethod.methodName, provide->interface->methodNames[k], MAX_INTERFACE_METHOD_NAME_LENGTH);
+
+                        writeN((struct t_nmfTraceChannelHeader*)&tracemethod);
+                    }
+                }
+            }
+        }
+    }
+}
+
+void cm_TRC_traceBinding(
+        t_nmfTraceBindCommandDescription command,
+        const t_component_instance* clientComponent, const t_component_instance* serverComponent,
+        const char *requiredItfName, const char *providedItfName)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceBind trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_BIND, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+
+        if(clientComponent == ARM_TRACE_COMPONENT) // ARM
+        {
+            trace.clientDomainId = 0x1;
+            trace.clientComponentContext = 0x0;
+        }
+        else
+        {
+            trace.clientDomainId = (t_uint16)clientComponent->Template->dspId + 1;
+            trace.clientComponentContext = (t_uint32)clientComponent->thisAddress;
+        }
+        if(requiredItfName != NULL)
+            cm_StringCopy((char*)trace.requiredItfName, requiredItfName, MAX_INTERFACE_NAME_LENGTH);
+        else
+            trace.requiredItfName[0] = 0;
+
+        if(serverComponent == NULL)
+        { // Unbind or VOID
+            trace.serverDomainId = 0;
+            trace.serverComponentContext = 0x0;
+        }
+        else if(serverComponent == ARM_TRACE_COMPONENT)
+        { // ARM
+            trace.serverDomainId = 0x1;
+            trace.serverComponentContext = 0x0;
+        }
+        else
+        {
+            trace.serverDomainId = (t_uint16)serverComponent->Template->dspId + 1;
+            trace.serverComponentContext = (t_uint32)serverComponent->thisAddress;
+        }
+        if(providedItfName != NULL)
+            cm_StringCopy((char*)trace.providedItfName, providedItfName, MAX_INTERFACE_NAME_LENGTH);
+        else
+            trace.providedItfName[0] = 0;
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
+void cm_TRC_traceCommunication(
+        t_nmfTraceCommunicationCommandDescription command,
+        t_nmf_core_id coreId,
+        t_nmf_core_id remoteCoreId)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceCommunication trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_COMMUNICATION, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+        trace.domainId = (t_uint16)coreId + 1;
+        trace.remoteDomainId = (t_uint16)remoteCoreId + 1;
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
+void cm_TRC_traceMemAlloc(t_nmfTraceAllocatorCommandDescription command, t_uint8 allocId, t_uint32 memorySize, const char *allocname)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceAllocator trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_ALLOCATOR, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+        trace.allocId = (t_uint16)allocId;
+        trace.size = memorySize;
+        cm_StringCopy((char*)trace.name, allocname, sizeof(trace.name));
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
+void cm_TRC_traceMem(t_nmfTraceAllocCommandDescription command, t_uint8 allocId, t_uint32 startAddress, t_uint32 memorySize)
+{
+    if(cm_trace_enabled)
+    {
+        struct t_nmfTraceAlloc trace;
+
+        trace.header.v = HEADER(TRACE_TYPE_ALLOC, sizeof(trace));
+
+        trace.command = (t_uint16)command;
+        trace.allocId = (t_uint16)allocId;
+        trace.offset = startAddress;
+        trace.size = memorySize;
+
+        writeN((struct t_nmfTraceChannelHeader*)&trace);
+    }
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/convert.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/convert.h
new file mode 100644
index 00000000000..d6912e58687
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/convert.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Conversion utility methods.
+ */
+#ifndef H_CM_CONVERTS_MEM
+#define H_CM_CONVERTS_MEM
+
+#include <share/inc/nmf.h>
+
+/*
+ * Utils convert methods
+ */
+const char* cm_getDspName(t_nmf_core_id dsp);
+
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/mem.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/mem.h
new file mode 100644
index 00000000000..c950a94023d
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/mem.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Memory manipulation.
+ */
+#ifndef H_CM_UTILS_MEM
+#define H_CM_UTILS_MEM
+
+/*
+ * Utils libc methods
+ */
+void cm_MemCopy(void* dest, const void *src, int count);
+void cm_MemSet(void *str, int c, int count);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/string.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/string.h
new file mode 100644
index 00000000000..d2b7c0b0823
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/string.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief String manipulation.
+ */
+#ifndef H_CM_UTILS_STRING
+#define H_CM_UTILS_STRING
+
+#include <cm/engine/memory/inc/memory.h>
+
+#define MAX_INTERNAL_STRING_LENGTH 2048
+
+typedef const char *t_dup_char;
+
+t_dup_char cm_StringGet(const char* str);
+t_dup_char cm_StringReference(t_dup_char str);
+t_dup_char cm_StringDuplicate(const char* orig);
+void cm_StringRelease(t_dup_char orig);
+
+/*
+ * Utils libc methods
+ */
+void        cm_StringCopy(char* dest, const char* src, int count);
+int         cm_StringCompare(const char* str1, const char* str2, int count);
+int         cm_StringLength(const char * str, int count);
+void        cm_StringConcatenate(char* dest, const char* src, int count);
+char*       cm_StringSearch(const char* str, int c);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/swap.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/swap.h
new file mode 100644
index 00000000000..e4f5acb3010
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/swap.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Swap integer manipulation.
+ */
+#ifndef H_CM_UTILS_SWAP
+#define H_CM_UTILS_SWAP
+
+#include <cm/inc/cm_type.h>
+
+/*
+ * Swap methods
+ */
+t_uint16 swap16(t_uint16 x);
+t_uint32 swap32(t_uint32 x);
+t_uint64 swap64(t_uint64 x);
+t_uint32 noswap32(t_uint32 x);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/inc/table.h b/drivers/staging/nmf-cm/cm/engine/utils/inc/table.h
new file mode 100644
index 00000000000..9d9828a81f6
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/inc/table.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*!
+ * \internal
+ * \brief Dynamic table manipulation.
+ */
+#ifndef H_CM_UTILS_TABLE
+#define H_CM_UTILS_TABLE
+
+#include <cm/inc/cm_type.h>
+
+/*
+  This implement a (generic) dynamic table (the size is dynamic)
+  to register some pointers of a given kind of elements
+  
+  It also allows to compute/convert each kernel pointer registered in the
+  table to a user handler, that can be checked.
+
+  The "user" handler is composed by the index in this table
+  (the low INDEX_SHIFT bits) and the low bits of the "local" pointer
+  shifted by INDEX_SHIFT are stored in the high bits:
+
+  handle bits: 31 ................................ 12 11 ...... 0
+              | lower bits of of the local pointer   |   index   |
+
+  This allows a straight translation from a user handle to a local pointer
+  + a strong check to validate the value of a user handle.
+  The reverse translation from pointer to a user handle is
+  slower as it requires an explicit search in the list.
+ */
+
+
+/* INDEX_SHIFT determines the index size and thus the max index */
+#define INDEX_SHIFT    12
+#define INDEX_MAX      (1UL << INDEX_SHIFT)
+#define INDEX_MASK     (INDEX_MAX-1)
+#define ENTRY2HANDLE(pointer, index) (((unsigned int)pointer << INDEX_SHIFT) | index)
+#define TABLE_DEF_SIZE 0x1000
+
+typedef struct {
+	t_uint32 idxNb;  /**< number of entries used */
+	t_uint32 idxCur; /**< current index: point to next supposed
+			    free entry: used to look for the next
+			    free entry */
+	t_uint32 idxMax; /**< index max currently allowed */
+	void **entries;   /**< table itself */
+} t_nmf_table;
+
+t_cm_error cm_initTable(t_nmf_table* table);
+void cm_destroyTable(t_nmf_table* table);
+t_uint32 cm_addEntry(t_nmf_table *table, void *entry);
+void cm_delEntry(t_nmf_table *table, t_uint32 idx);
+void *cm_lookupEntry(const t_nmf_table *table, const t_uint32 hdl);
+t_uint32 cm_lookupHandle(const t_nmf_table *table, const void *entry);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/convert.c b/drivers/staging/nmf-cm/cm/engine/utils/src/convert.c
new file mode 100644
index 00000000000..ad6e097bfe6
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/convert.c
@@ -0,0 +1,20 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/utils/inc/convert.h>
+
+const char* dspNames[NB_CORE_IDS] = {
+    "ARM",
+    "SVA",
+    "SIA"
+};
+
+
+const char* cm_getDspName(t_nmf_core_id dsp) {
+	return dspNames[dsp];
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/mem.c b/drivers/staging/nmf-cm/cm/engine/utils/src/mem.c
new file mode 100644
index 00000000000..130a044bbf8
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/mem.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/utils/inc/mem.h>
+
+
+/*
+ * Methods
+ */
+void cm_MemCopy(void* dest, const void *src, int count) {
+    char *tmp = (char *) dest, *s = (char *) src;
+
+    while (count--)
+        *tmp++ = *s++;
+}
+
+void cm_MemSet(void *str, int c, int count) {
+  char *tmp = (char *)str;
+
+  while (count--)
+    *tmp++ = c;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/string.c b/drivers/staging/nmf-cm/cm/engine/utils/src/string.c
new file mode 100644
index 00000000000..89058d5825a
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/string.c
@@ -0,0 +1,231 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ * Shared string manipulation.
+ * TODO This is a list today, must be a hash later !!!!!
+ */
+#include <cm/engine/utils/inc/string.h>
+#include <cm/engine/trace/inc/trace.h>
+
+#include <cm/engine/memory/inc/memory.h>
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#undef NHASH
+#define NHASH 257       //Use a prime number!
+#define MULT 17
+
+/*
+ * Data
+ */
+struct t_linkedstring
+{
+    struct t_linkedstring	*next;
+    int						referencer;
+    char					string[1];
+};
+
+static struct t_linkedstring *list[NHASH];
+
+#undef myoffsetof
+#define myoffsetof(st, m) \
+     ((int) ( (char *)&((st *)(0))->m - (char *)0 ))
+
+unsigned int hash(const char *str)
+{
+    unsigned int h = 0;
+    for(; *str; str++)
+        h = MULT * h + *str;
+    return h % NHASH;
+}
+/*
+ * Methods
+ */
+PRIVATE struct t_linkedstring *lookupString(
+        const char* str,
+        struct t_linkedstring   *first)
+{
+    while(first != 0)
+    {
+        if(cm_StringCompare(str, first->string, MAX_INTERNAL_STRING_LENGTH) == 0)
+            break;
+        first = first->next;
+    }
+
+    return first;
+}
+
+t_dup_char cm_StringGet(const char* str)
+{
+    struct t_linkedstring *entry;
+
+    entry = lookupString(str, list[hash(str)]);
+    CM_ASSERT(entry != 0);
+
+    return (t_dup_char)entry->string;
+}
+
+t_dup_char cm_StringReference(t_dup_char str)
+{
+    struct t_linkedstring* entry = (struct t_linkedstring*)((t_uint32)str - myoffsetof(struct t_linkedstring, string));
+
+    // One more referencer
+    entry->referencer++;
+
+    return (t_dup_char)entry->string;
+}
+
+t_dup_char cm_StringDuplicate(const char* str)
+{
+    struct t_linkedstring *entry;
+    unsigned int h;
+
+    h = hash(str);
+    entry = lookupString(str, list[h]);
+    if(entry != 0)
+    {
+        // One more referencer
+        entry->referencer++;
+    }
+    else
+    {
+        // Allocate new entry
+        entry = (struct t_linkedstring *)OSAL_Alloc(sizeof(struct t_linkedstring)-1 + cm_StringLength(str, MAX_INTERNAL_STRING_LENGTH)+1);
+        if(entry == NULL)
+            return NULL;
+
+        entry->referencer = 1;
+        cm_StringCopy(entry->string, str, MAX_INTERNAL_STRING_LENGTH);
+
+        // Link it in list
+        entry->next = list[h];
+        list[h] = entry;
+    }
+
+    return (t_dup_char)entry->string;
+}
+
+void cm_StringRelease(t_dup_char str)
+{
+    if(str != NULL)
+    {
+        struct t_linkedstring* entry = (struct t_linkedstring*)((t_uint32)str - myoffsetof(struct t_linkedstring, string));
+
+        // One less referencer
+        entry->referencer--;
+
+        if(entry->referencer == 0)
+        {
+            int h = hash(entry->string);
+
+            if(list[h] == entry) // This first first one
+            {
+                list[h] = entry->next;
+            }
+            else
+            {
+                struct t_linkedstring *tmp = list[h];
+
+                // Here we assume that entry is in the list
+                while(/*tmp != NULL && */tmp->next != entry)
+                    tmp = tmp->next;
+
+                tmp->next = entry->next;
+            }
+            OSAL_Free(entry);
+        }
+    }
+}
+
+#if 0
+void checkString()
+{
+    struct t_linkedstring *tmp = list;
+
+    while(tmp != 0)
+    {
+        printf("  stay %s %d\n", tmp->string, tmp->referencer);
+        tmp = tmp->next;
+    }
+}
+#endif
+
+/*
+ * LibC method
+ */
+void cm_StringCopy(char* dest, const char *src, int count)
+{
+    while (count-- && (*dest++ = *src++) != '\0')
+        /* nothing */
+        ;
+}
+#define DETECTNULL(X) (((X) - 0x01010101) & ~(X) & 0x80808080)
+
+int cm_StringCompare(const char* str1, const char* str2, int count)
+{
+    /* If s1 and s2 are word-aligned, compare them a word at a time. */
+    if ((((int)str1 & 3) | ((int)str2 & 3)) == 0)
+    {
+        unsigned int *a1 = (unsigned int*)str1;
+        unsigned int *a2 = (unsigned int*)str2;
+
+        while (count >= sizeof (unsigned int) && *a1 == *a2)
+        {
+            count -= sizeof (unsigned int);
+
+            /* If we've run out of bytes or hit a null, return zero since we already know *a1 == *a2.  */
+            if (count == 0 || DETECTNULL (*a1))
+                return 0;
+
+            a1++;
+            a2++;
+        }
+
+        /* A difference was detected in last few bytes of s1, so search bytewise */
+        str1 = (char*)a1;
+        str2 = (char*)a2;
+    }
+
+    while (count-- > 0 && *str1 == *str2)
+    {
+        /* If we've run out of bytes or hit a null, return zero
+       since we already know *s1 == *s2.  */
+        if (count == 0 || *str1 == '\0')
+            return 0;
+        str1++;
+        str2++;
+    }
+
+    return (*(unsigned char *) str1) - (*(unsigned char *) str2);
+}
+
+int cm_StringLength(const char * str, int count)
+{
+    const char *sc;
+
+    for (sc = str; count-- && *sc != '\0'; ++sc)
+        /* nothing */
+        ;
+    return sc - str;
+}
+
+void cm_StringConcatenate(char* dest, const char* src, int count)
+{
+    while ((*dest) != '\0')
+    {
+        dest++;
+        count--;
+    }
+    cm_StringCopy(dest, src, count);
+}
+
+char* cm_StringSearch(const char* str, int c)
+{
+    for(; *str != (char) c; ++str)
+        if (*str == '\0')
+            return 0;
+    return (char *) str;
+}
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/swap.c b/drivers/staging/nmf-cm/cm/engine/utils/src/swap.c
new file mode 100644
index 00000000000..e3e2d536144
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/swap.c
@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/utils/inc/swap.h>
+
+
+/*
+ * Methods
+ */
+t_uint16 swap16(t_uint16 x)
+{
+    return ((x >> 8) |
+            ((x << 8) & 0xff00U));
+}
+
+#ifdef LINUX
+
+#if defined(__STN_8815) /* __STN_8815 -> ARMv5*/
+t_uint32 swap32(t_uint32 x)
+{
+    asm volatile (
+            "EOR      r1, r0, r0, ROR #16           \n\t"
+            "BIC      r1, r1, #0xFF0000             \n\t"
+            "MOV      r0, r0, ROR #8                \n\t"
+            "EOR      r0, r0, r1, LSR #8"
+            :  :  : "r3" );
+
+    return x;
+}
+
+t_uint64 swap64(t_uint64 x)
+{
+    asm volatile (
+            "MOV      r2, r1                        \n\t"
+            "                                       \n\t"
+            "EOR      r3, r0, r0, ROR #16           \n\t"
+            "BIC      r3, r3, #0xFF0000             \n\t"
+            "MOV      r0, r0, ROR #8                \n\t"
+            "EOR      r1, r0, r3, LSR #8            \n\t"
+            "                                       \n\t"
+            "EOR      r3, r2, r2, ROR #16           \n\t"
+            "BIC      r3, r3, #0xFF0000             \n\t"
+            "MOV      r2, r2, ROR #8                \n\t"
+            "EOR      r0, r2, r3, LSR #8"
+            :  :  : "r3", "r2" );
+
+    return x;
+}
+#else /* -> ARMv6 or later */
+
+t_uint32 swap32(t_uint32 x)
+{
+    asm volatile (
+            "REV      %0, %0"
+            :  "+r"(x) : );
+
+    return x;
+}
+
+t_uint64 swap64(t_uint64 x)
+{
+    asm volatile (
+            "REV      r2, %Q0                \n\t"
+            "REV      %Q0, %R0                \n\t"
+            "MOV      %R0, r2"
+            :  "+&r" (x) : : "r2" );
+
+    return x;
+}
+
+#endif
+
+#else /* Symbian, Think -> We assume ARMCC */
+
+#if defined(__thumb__)
+
+t_uint32 swap32(t_uint32 x)
+{
+    return ((x >> 24) |
+            ((x >> 8) & 0xff00U) |
+            ((x << 8) & 0xff0000U) |
+            ((x << 24) & 0xff000000U));
+}
+
+t_uint64 swap64(t_uint64 x)
+{
+    return ((x >> 56) |
+            ((x >> 40) & 0xff00UL) |
+            ((x >> 24) & 0xff0000UL) |
+            ((x >> 8) & 0xff000000UL) |
+            ((x << 8) & 0xff00000000ULL) |
+            ((x << 24) & 0xff0000000000ULL) |
+            ((x << 40) & 0xff000000000000ULL) |
+            ((x << 56)));
+}
+
+#elif (__TARGET_ARCH_ARM < 6)
+
+__asm t_uint32 swap32(t_uint32 x)
+{
+    EOR      r1, r0, r0, ROR #16
+    BIC      r1, r1, #0xFF0000
+    MOV      r0, r0, ROR #8
+    EOR      r0, r0, r1, LSR #8
+
+    BX       lr
+}
+
+__asm t_uint64 swap64(t_uint64 x)
+{
+    MOV      r2, r1
+
+    EOR      r3, r0, r0, ROR #16    // Swap low (r0) and store it in high (r1)
+    BIC      r3, r3, #0xFF0000
+    MOV      r0, r0, ROR #8
+    EOR      r1, r0, r3, LSR #8
+
+    EOR      r3, r2, r2, ROR #16    // Swap high (r2 = ex r1) and store it in low (r0)
+    BIC      r3, r3, #0xFF0000
+    MOV      r2, r2, ROR #8
+    EOR      r0, r2, r3, LSR #8
+
+    BX       lr
+}
+
+#else /* -> ARMv6 or later */
+
+__asm t_uint32 swap32(t_uint32 x)
+{
+    REV      r0, r0
+
+    BX       lr
+}
+
+__asm t_uint64 swap64(t_uint64 x)
+{
+    REV      r2, r0
+    REV      r0, r1
+    MOV      r1, r2
+
+    BX       lr
+}
+
+#endif
+
+#endif
+
+t_uint32 noswap32(t_uint32 x) {
+    return x;
+}
+
diff --git a/drivers/staging/nmf-cm/cm/engine/utils/src/table.c b/drivers/staging/nmf-cm/cm/engine/utils/src/table.c
new file mode 100644
index 00000000000..708396a01b2
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/engine/utils/src/table.c
@@ -0,0 +1,155 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL) version 2.
+ */
+/*
+ *
+ */
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+#include <cm/engine/trace/inc/trace.h>
+#include <cm/engine/utils/inc/mem.h>
+#include <cm/engine/utils/inc/table.h>
+
+/*
+ * Methods
+ */
+t_cm_error cm_initTable(t_nmf_table* table)
+{
+	table->idxMax = TABLE_DEF_SIZE / sizeof(table->entries);
+
+	table->entries = OSAL_Alloc_Zero(table->idxMax*sizeof(table->entries));
+
+	if (table->entries == NULL) {
+		table->idxMax = 0;
+		return CM_NO_MORE_MEMORY;
+	}
+
+	return CM_OK;
+}
+
+void cm_destroyTable(t_nmf_table* table)
+{
+	if (table->idxNb) {
+		ERROR("Attempt to free non-empty table !!!\n", 0, 0, 0, 0, 0, 0);
+		return;
+	}
+	OSAL_Free(table->entries);
+	table->idxMax = 0;
+}
+
+static t_cm_error cm_increaseTable(t_nmf_table* table)
+{
+	t_uint32 new_max;
+	void *mem;
+
+	if (table->idxMax == INDEX_MASK) {
+		ERROR("CM_NO_MORE_MEMORY: Maximum table entries reached\n", 0, 0, 0, 0, 0, 0);
+		return CM_NO_MORE_MEMORY;
+	}
+
+	new_max = table->idxMax
+		+ TABLE_DEF_SIZE / sizeof(table->entries);
+
+	if (new_max > INDEX_MAX)
+		new_max = INDEX_MAX;
+
+	mem = OSAL_Alloc(new_max * sizeof(table->entries));
+
+	if (mem == NULL) {
+		ERROR("CM_NO_MORE_MEMORY: Unable to allocate memory for a table\n", 0, 0, 0, 0, 0, 0);
+		return CM_NO_MORE_MEMORY;
+	}
+
+	cm_MemCopy(mem, table->entries,
+		   table->idxMax*sizeof(table->entries));
+	cm_MemSet((void *)((t_uint32) mem + table->idxMax*sizeof(*table->entries)), 0,
+		  (new_max-table->idxMax) * sizeof(*table->entries));
+
+	OSAL_Free(table->entries);
+	table->entries = mem;
+	table->idxMax = new_max;
+
+	return CM_OK;
+}
+
+/** cm_addEntry - Add an local pointer to an element to the list
+ *
+ *  1. Increase the size of the list if it's full
+ *  2. Search an empty entry
+ *  3. Add the element to the list
+ *  4. Compute and return the "user handle"
+ */
+t_uint32 cm_addEntry(t_nmf_table *table, void *entry)
+{
+	unsigned int i;
+	t_uint32 hdl = 0;
+
+	if (table->idxNb == table->idxMax)
+		cm_increaseTable(table);
+
+	for (i = table->idxCur;
+	     table->entries[i] != 0 && i != (table->idxCur-1);
+	     i = (i+1)%table->idxMax);
+
+	if (table->entries[i] == 0) {
+		table->entries[i] = entry;
+		table->idxCur = (i+1) % table->idxMax;
+		table->idxNb++;
+		hdl = ENTRY2HANDLE(entry, i);
+	} else
+		ERROR("No free entry found in table\n", 0, 0, 0, 0, 0, 0);
+
+	return hdl;
+}
+
+/** cm_delEntry - remove the given element from the list
+ *
+ *  1. Check if the handle is valid
+ *  2. Search the entry and free it
+ */
+void cm_delEntry(t_nmf_table *table, t_uint32 idx)
+{
+	table->entries[idx] = NULL;
+	table->idxNb--;
+}
+
+/** cm_lookupEntry - search the entry corresponding to
+ *                   the user handle.
+ *
+ * 1. Check if the handle is valid
+ * 2. Return a pointer to the element
+ */
+void *cm_lookupEntry(const t_nmf_table *table, const t_uint32 hdl)
+{
+	unsigned int idx = hdl & INDEX_MASK;
+
+	if ((idx >= table->idxMax)
+	    || (((unsigned int)table->entries[idx] << INDEX_SHIFT) != (hdl & ~INDEX_MASK)))
+		return NULL;
+	else
+		return table->entries[idx];
+}
+
+/** cm_lookupHandle - search the handle corresponding
+ *                    to the given element
+ *
+ * 1. Check if the handler is valid or is a special handler
+ * 2. Loop in the table to retrieve the entry matching and return its value
+ */
+t_uint32 cm_lookupHandle(const t_nmf_table *table, const void *entry)
+{
+	t_uint32 i;
+
+	/* NULL is an invalid value that must be handle separatly
+	   as it'll match all used/free entries value */
+	if (entry == NULL)
+		return 0;
+
+	for (i=0; i < table->idxMax; i++) {
+		if (table->entries[i] == entry)
+			return ENTRY2HANDLE(table->entries[i], i);
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/nmf-cm/cm/inc/cm.h b/drivers/staging/nmf-cm/cm/inc/cm.h
new file mode 100644
index 00000000000..37ccb36a5ee
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/inc/cm.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_CM_H
+#define __INC_CM_H
+
+#include <cm/inc/cm_def.h>
+
+/********************************************************************************/
+/*       Component Manager API prototypes                                       */
+/********************************************************************************/
+
+/*
+ * User level wrapper
+ */
+#include <cm/proxy/api/cm_proxy.h>
+
+#endif /* __INC_CM_H */
diff --git a/drivers/staging/nmf-cm/cm/inc/cm_def.h b/drivers/staging/nmf-cm/cm/inc/cm_def.h
new file mode 100644
index 00000000000..dc7a1fdad66
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/inc/cm_def.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+/*!
+ * \brief Component Manager API.
+ *
+ * This file contains the Component Manager API for manipulating components.
+ *
+ */
+
+#ifndef __INC_CM_DEF_H
+#define __INC_CM_DEF_H
+
+#include <cm/inc/cm_type.h>
+#include <inc/nmf-def.h>
+
+/*!
+ * \brief Get the version of the NMF CM engine at runtime
+ *
+ * This method should be used to query the version number  of  the
+ * NMF Component Manager engine at runtime. This is useful when using
+ * to check if version of the engine linked with application correspond
+ * to engine used for development. 
+ *
+ * Such code can be used to check compatibility: \code
+    t_uint32 nmfversion;
+
+    // Print NMF version
+    CM_GetVersion(&nmfversion);
+    LOG("NMF Version %d-%d-%d\n",
+            VERSION_MAJOR(nmfversion),
+            VERSION_MINOR(nmfversion),
+            VERSION_PATCH(nmfversion));
+    if(NMF_VERSION != nmfversion) {
+        LOG("Error: Incompatible API version %d != %d\n", NMF_VERSION, nmfversion);
+        EXIT();
+    }
+ * \endcode
+ *
+ * \param[out] version Internal hardcoded version (use \ref VERSION_MAJOR, \ref  VERSION_MINOR, \ref VERSION_PATCH  macros to decode it).
+ *
+ * \ingroup CM
+ */
+PUBLIC IMPORT_SHARED void CM_GetVersion(t_uint32 *version);
+
+#endif /* __INC_CM_H */
diff --git a/drivers/staging/nmf-cm/cm/inc/cm_macros.h b/drivers/staging/nmf-cm/cm/inc/cm_macros.h
new file mode 100644
index 00000000000..2279c204a20
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/inc/cm_macros.h
@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+/*!
+ * \brief Component Manager Macros.
+ *
+ * \defgroup CM_MACROS NMF Macros (ANSI C99)
+ * The Component Manager Macros are provided to ease FromHost interface call and ToHost callback definition.
+ * \attention <b>These macros are only ANSI C99 compliant</b> (ARM RVCT 2.x/3.x, GNU gcc 4.x, ...)
+ * \ingroup CM_USER_API
+ */
+
+#ifndef __INC_CM_MACROS_H
+#define __INC_CM_MACROS_H
+
+/*
+ * The next macros are supported only with C Ansi 99, so....
+ */
+
+/*
+ * The Symbian environment dependency, computation which uses an old gnu cpp,
+ * does not accept "..." parameters.
+ * However the actual compiler (armcc) does.
+ * So remove the macro definitions when computing dependencies.
+ */
+#if ( defined(__CC_ARM) && !defined(__STRICT_ANSI__) ) || !defined(__SYMBIAN32__)
+
+/*
+ * Only for skilled eyes ;)
+ * The following macros are used to implement NMFCALL[VOID] and NMFMETH[VOID] macros in an elegant way
+ */
+#define WITH_PARAM(...)  __VA_ARGS__)
+#define WITH_NOPARAM(...) )
+
+/*!
+ * \brief Macro to ease Host to Dsp interface calling
+ *
+ * \attention <b>This macro is only ANSI C99 compliant</b>
+ *
+ * The <i>NMFCALL</i> macro can be used to call one method of any previously FromHost bounded interface.\n
+ * From Host side, today, we have no way to mask the multi-instance handling, so
+ * this macro is provided to ease FromHost interface calling and to avoid any mistake into the THIS parameter passing.
+ *
+ * So, any fromHost interface method call like: \code
+ * itf.method(itf.THIS, param1, param2, ...);
+ * \endcode
+ * can be replaced by: \code
+ * NMFCALL(itf, method)(param1, param2, ...);
+ * \endcode
+ *
+ * \warning Don't forget to use NMFCALLVOID macro when declaring a FromHost interface method having none application parameter,
+ * else it will lead to erroneous C code expansion
+ * \see NMFCALLVOID
+ * \hideinitializer
+ * \ingroup CM_MACROS
+ */
+#define NMFCALL(itfHandle, itfMethodName)  \
+    (itfHandle).itfMethodName((itfHandle).THIS, WITH_PARAM
+
+/*!
+ * \brief Macro to ease Host to Dsp interface calling (method without any user parameter)
+ *
+ * \attention <b>This macro is only ANSI C99 compliant</b>
+ *
+ * The <i>NMFCALLVOID</i> macro can be used to call one method (those without any user parameter) of any previously FromHost bounded interface.\n
+ * From Host side, today, we have no way to mask the multi-instance handling, so
+ * this macro is provided to ease FromHost interface calling and to avoid any mistake into the THIS parameter passing.
+ *
+ * So, any FromHost interface method call without any application parameter like:\code
+ * itf.method(itf.THIS);
+ * \endcode
+ * can be replaced by: \code
+ * NMFCALLVOID(itf, method)();
+ * \endcode
+ * \see NMFCALL
+ * \hideinitializer
+ * \ingroup CM_MACROS
+ */
+#define NMFCALLVOID(itfHandle, itfMethodName)  \
+    (itfHandle).itfMethodName((itfHandle).THIS WITH_NOPARAM
+
+/*!
+ * \brief Macro to ease Dsp to Host interface method declaration
+ *
+ * \attention <b>This macro definition is only ANSI C99 compliant</b>
+ *
+ * The <i>NMFMETH</i> macro can be used to ease the ToHost interface method declaration.\n
+ * From Host side, today, we have no way to mask the multi-intance handling, so the user shall handle it by hand
+ * by passing the "component" context as first parameter of each ToHost interface method through the void *THIS parameter.
+ * This macro could avoid any mistake into the THIS parameter declaration when never used by the user code.
+ *
+ * So, any ToHost interface method declaration like:\code
+ * void mynotify(void *THIS, mytype1 myparam1, mytype2 myparam2, ...) {
+ * <body of the interface routine>
+ * }
+ * \endcode
+ * can be replaced by: \code
+ * void NMFMETH(mynotify)(mytype1 myparam1, mytype2 myparam2, ...) {
+ * <body of the interface routine>
+ * }
+ * \endcode
+ *
+ * \warning Don't forget to use NMFMETHVOID macro when declaring a ToHost interface method having none application parameter,
+ * else it will lead to erroneous C code expansion
+ *
+ * \see NMFMETHVOID
+ * \hideinitializer
+ * \ingroup CM_MACROS
+ */
+#define NMFMETH(itfMethodName) \
+    itfMethodName(void *THIS, WITH_PARAM
+
+/*!
+ * \brief Macro to ease Dsp to Host interface method declaration (method without any user parameter)
+ *
+ * \attention <b>This macro is only ANSI C99 compliant</b>
+ *
+ * The <i>NMFMETHVOID</i> macro can be used to ease the ToHost interface method (those without any user parameter) declaration.\n
+ * From Host side, today, we have no way to mask the multi-intance handling, so the user shall handle it by hand
+ * by passing the "component" context as first parameter of each ToHost interface method through the void *THIS parameter.
+ * This macro could avoid any mistake into the THIS parameter declaration when never used by the user code.
+ *
+ * So, any ToHost interface method declaration having none application parameter like:\code
+ * void mynotify(void *THIS) {
+ * <body of the interface routine>
+ * }
+ * \endcode
+ * can be replaced by: \code
+ * void NMFMETHVOID(mynotify)(void) {
+ * <body of the interface routine>
+ * }
+ * \endcode
+ *
+ * \see NMFMETH
+ * \hideinitializer
+ * \ingroup CM_MACROS
+ */
+#define NMFMETHVOID(itfMethodName) \
+    itfMethodName(void *THIS WITH_NOPARAM
+
+#endif /* not Symbian environment or compiling with ARMCC and not in strict ANSI */
+
+#endif /* __INC_CM_MACROS_H */
+
diff --git a/drivers/staging/nmf-cm/cm/inc/cm_type.h b/drivers/staging/nmf-cm/cm/inc/cm_type.h
new file mode 100644
index 00000000000..780e27ca600
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm/inc/cm_type.h
@@ -0,0 +1,147 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Component Manager types.
+ *
+ * This file contains the Component Manager  types.
+ *
+ * \defgroup CM CM Type Definitions
+ * \ingroup CM_USER_API
+ */
+#ifndef _CM_TYPE_H_
+#define _CM_TYPE_H_
+
+#include <share/inc/nmf.h>
+#include <share/inc/macros.h>
+
+#include <nmf/inc/channel_type.h>
+
+/*!
+ * @defgroup t_cm_error t_cm_error
+ * \brief Description of the various errors returned by CM API routines
+ * @{
+ * \ingroup CM
+ */
+typedef t_nmf_error t_cm_error;                                             //!< Error type returned by CM API routines
+
+/*********************************************************************************/
+/* WARNING: UPDATE CM_StringError() func each time an error is added/removed !!! */
+/* CM_StringError() is defined twice in:                                         */
+/*   nmf_core/host/cm/proxy/common/wrapper/src/wrapper.c                         */
+/*   tests/src/common/nte/src/nte.c                                              */
+/*********************************************************************************/
+#define CM_LAST_ERROR_ID                    ((t_cm_error)-128)
+#define CM_INTEGRATION_ERROR                NMF_INTEGRATION_ERROR0      //!< \ref NMF_INTEGRATION_ERROR0
+
+    /* Communication */
+#define CM_FLUSH_MESSAGE                    NMF_FLUSH_MESSAGE           //!< Message send after call to CM_FlushChannel()
+#define CM_BUFFER_OVERFLOW                  ((t_cm_error)-105)          //!< Buffer overflow (interface binding message bigger than buffer)
+#define CM_USER_NOT_REGISTERED              ((t_cm_error)-104)          //!< User not registered
+#define CM_NO_MESSAGE                       NMF_NO_MESSAGE              //!< \ref NMF_NO_MESSAGE
+#define CM_PARAM_FIFO_OVERFLOW              ((t_cm_error)-102)          //!< Param fifo overflow
+#define CM_INTERNAL_FIFO_OVERFLOW           ((t_cm_error)-101)          //!< Internal services fifo overflow (not returned to user)
+#define CM_MPC_NOT_RESPONDING               ((t_cm_error)-100)          //!< MPC not responding (either crash, interrupt handler too long, internal NMF fifo coms overflow, ...).
+
+    /* ELF & File system */
+#define CM_FS_ERROR                         ((t_cm_error)-96)           //!< FileSystem error
+#define CM_NO_SUCH_FILE                     ((t_cm_error)-95)           //!< No such file or directory
+#define CM_INVALID_ELF_FILE                 ((t_cm_error)-94)           //!< File isn't a valid MMDSP ELF file
+#define CM_NO_SUCH_BASE                     ((t_cm_error)-93)           //!< The memory base doesn't exist
+
+    /* Introspection */
+#define CM_NO_SUCH_ATTRIBUTE                NMF_NO_SUCH_ATTRIBUTE       //!< \ref NMF_NO_SUCH_ATTRIBUTE
+#define CM_NO_SUCH_PROPERTY                 NMF_NO_SUCH_PROPERTY        //!< \ref NMF_NO_SUCH_PROPERTY
+
+    /* Component Life Cycle */
+#define CM_COMPONENT_NOT_STOPPED            NMF_COMPONENT_NOT_STOPPED   //!< \ref NMF_COMPONENT_NOT_STOPPED
+#define CM_COMPONENT_NOT_UNBINDED           ((t_cm_error)-79)           //!< Component must be fully unbinded before perform operation
+#define CM_COMPONENT_NOT_STARTED            ((t_cm_error)-78)           //!< Component must be started to perform operation
+#define CM_COMPONENT_WAIT_RUNNABLE          ((t_cm_error)-76)           //!< Component need acknowlegdment of life cycle start function before perform operation
+#define CM_REQUIRE_INTERFACE_UNBINDED       ((t_cm_error)-75)           //!< Required component interfaces must be binded before perform operation
+#define CM_INVALID_COMPONENT_HANDLE         ((t_cm_error)-74)           //!< Try to access a component already destroyed
+
+    /* Binder */
+#define CM_NO_SUCH_PROVIDED_INTERFACE       NMF_NO_SUCH_PROVIDED_INTERFACE   //!< \ref NMF_NO_SUCH_PROVIDED_INTERFACE
+#define CM_NO_SUCH_REQUIRED_INTERFACE       NMF_NO_SUCH_REQUIRED_INTERFACE   //!< \ref NMF_NO_SUCH_REQUIRED_INTERFACE
+#define CM_ILLEGAL_BINDING                  ((t_cm_error)-62)           //!< Client and server interface type mismatch
+#define CM_ILLEGAL_UNBINDING                ((t_cm_error)-61)           //!< Try to unbind component with bad binding Factories
+#define CM_INTERFACE_ALREADY_BINDED         NMF_INTERFACE_ALREADY_BINDED//!< \ref NMF_INTERFACE_ALREADY_BINDED
+#define CM_INTERFACE_NOT_BINDED             NMF_INTERFACE_NOT_BINDED    //!< \ref NMF_INTERFACE_NOT_BINDED
+
+    /* Loader */
+#define CM_BINDING_COMPONENT_NOT_FOUND      ((t_cm_error)-48)           //!< Binding Component template name don't exist on components repository (should be generated thanks nkitf tool)
+#define CM_COMPONENT_NOT_FOUND              ((t_cm_error)-47)           //!< Component template name doesn't exist on components repository
+#define CM_NO_SUCH_SYMBOL                   ((t_cm_error)-46)           //!< Symbol name doesn't exported by the underlying component
+#define CM_COMPONENT_EXIST                  ((t_cm_error)-45)           //!< Component name already exists in the component cache
+
+    /* Fifo management related ones */
+#define CM_FIFO_FULL                        ((t_cm_error)-40)           //!< Fifo is full
+#define CM_FIFO_EMPTY                       ((t_cm_error)-39)           //!< Fifo is empty
+#define CM_UNKNOWN_FIFO_ID                  ((t_cm_error)-38)           //!< Fifo handle doesn't exist
+
+    /* Memory management related ones */
+#define CM_DOMAIN_VIOLATION                 ((t_cm_error)-33)           //!< Domain violation
+#define CM_CREATE_ALLOC_ERROR               ((t_cm_error)-32)           //!< Error during allocator creation
+#define CM_UNKNOWN_MEMORY_HANDLE            ((t_cm_error)-31)           //!< Handle doesn't exists
+#define CM_NO_MORE_MEMORY                   NMF_NO_MORE_MEMORY          //!< \ref NMF_NO_MORE_MEMORY
+#define CM_BAD_MEMORY_ALIGNMENT             ((t_cm_error)-29)           //!< Memory alignment wanted is not correct
+#define CM_MEMORY_HANDLE_FREED              ((t_cm_error)-28)           //!< Handle was alread freed
+#define CM_INVALID_DOMAIN_DEFINITION        ((t_cm_error)-27)           //!< Domain to be created is not correctly defined
+#define CM_INTERNAL_DOMAIN_OVERFLOW         ((t_cm_error)-26)           //!< Internal domain descriptor overflow (too many domains) //TODO, juraj, remove this error
+#define CM_INVALID_DOMAIN_HANDLE            ((t_cm_error)-25)           //!< Invalid domain handle
+#define CM_ILLEGAL_DOMAIN_OPERATION         ((t_cm_error)-21)           //!< Operation on a domain is illegal (like destroy of a domain with referenced components)
+
+    /* Media Processor related ones */
+#define CM_MPC_INVALID_CONFIGURATION        ((t_cm_error)-24)           //!< Media Processor Core invalid configuration
+#define CM_MPC_NOT_INITIALIZED              ((t_cm_error)-23)           //!< Media Processor Core not yet initialized
+#define CM_MPC_ALREADY_INITIALIZED          ((t_cm_error)-22)           //!< Media Processor Core already initialized
+//ERROR 21 is defined above, with the domains
+
+    /* Power Mgt related ones */
+#define CM_PWR_NOT_AVAILABLE                ((t_cm_error)-16)           //!< No modification of the state of the power input
+
+    /* Common errors */
+#define CM_INVALID_DATA                     ((t_cm_error)-4)            //!< Invalid internal data encountered
+#define CM_OUT_OF_LIMITS                    ((t_cm_error)-3)            //!< User reach an internal nmf limits of limits.h file
+#define CM_INVALID_PARAMETER                NMF_INVALID_PARAMETER       //!< \ref NMF_INVALID_PARAMETER
+#define CM_NOT_YET_IMPLEMENTED              ((t_cm_error)-1)            //!< CM API not yet implemented
+#define CM_OK                               NMF_OK                      //!< \ref NMF_OK
+
+/** @} */
+
+/*!
+ * \brief Definition of a physical memory address
+ * \ingroup MEMORY
+ */
+typedef t_uint32 t_cm_physical_address;
+
+/*!
+ * \brief Definition of a logical memory address
+ * \ingroup MEMORY
+ */
+typedef t_uint32 t_cm_logical_address;
+
+/*!
+ * \brief Definition of a system address into a system with MMU
+ * \ingroup MEMORY
+ */
+typedef struct {
+    t_cm_physical_address physical; //!< Physical memory address
+    t_cm_logical_address logical;       //!< Logical memory address
+} t_cm_system_address;
+#define INVALID_SYSTEM_ADDRESS {(t_cm_physical_address)MASK_ALL32, (t_cm_logical_address)MASK_ALL32}
+
+
+/*!
+ * \brief Define a type used to manipulate size of various buffers
+ * \ingroup MEMORY
+ */
+typedef t_uint32 t_cm_size;
+
+#endif /* _CM_TYPE_H_ */
+
diff --git a/drivers/staging/nmf-cm/cm_debug.c b/drivers/staging/nmf-cm/cm_debug.c
new file mode 100644
index 00000000000..ffb067c65d9
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_debug.c
@@ -0,0 +1,840 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/proc_fs.h>
+
+#include "osal-kernel.h"
+#include "cm_debug.h"
+
+#ifdef CONFIG_DEBUG_FS
+#include <linux/sched.h>
+
+static struct dentry *cm_dir;        /* nmf-cm/            */
+static struct dentry *proc_dir;      /* nmf-cm/proc/       */
+static struct dentry *core_dir;      /* nmf-cm/dsp/        */
+static struct dentry *domain_dir;    /* nmf-cm/domains/    */
+
+/* components data managment */
+struct cm_debug_component_cooky {
+	struct dentry *comp_file; /* entry in nmf-cm/dsp/sxa/components/ */
+	struct dentry *proc_link; /* entry in nmf-cm/proc/       */
+};
+
+static ssize_t component_read(struct file *file, char __user *userbuf,
+			      size_t count, loff_t *ppos) {
+	t_component_instance *component = file->f_dentry->d_inode->i_private;
+	char buf[640];
+	int ret=0;
+
+	OSAL_LOCK_API();
+	if ((component != NULL) && (component->dbgCooky != NULL)) {
+		char nb_i[16] = "";
+		int i;
+
+		if (component->Template->classe == SINGLETON)
+			snprintf(nb_i, sizeof(nb_i), " (%d)",
+				 component->Template->numberOfInstance);
+
+		ret = snprintf(buf, sizeof(buf),
+			       "Name:\t\t%s <%s>\n"
+			       "Class:\t\t%s%s\n"
+			       "State:\t\t%s\n"
+			       "Priority:\t%u\n"
+			       "Domain:\t\t%u\n\n"
+			       "Memory    : Physical address  Logical address"
+			       "   DSP address           Size\n"
+			       "---------------------------------------------"
+			       "-----------------------------\n",
+			       component->pathname,
+			       component->Template->name,
+			       component->Template->classe == COMPONENT ?
+			       "Component" :
+			       (component->Template->classe == SINGLETON ?
+				"Singleton" :
+				(component->Template->classe == FIRMWARE ?
+				 "Firmware" :
+				 "?")),
+			       nb_i,
+			       component->state == STATE_RUNNABLE ? "Runnable" :
+			       (component->state == STATE_STOPPED ? "Sopped" :
+				"None"),
+			       (unsigned)component->priority,
+			       component->domainId
+			);
+
+		for (i=0; i<NUMBER_OF_MMDSP_MEMORY && ret<sizeof(buf); i++) {
+			if (component->memories[i]) {
+				t_cm_system_address addr;
+				t_uint32 dspAddr, dspSize;
+				cm_DSP_GetHostSystemAddress(
+					component->memories[i], &addr);
+				cm_DSP_GetDspAddress(
+					component->memories[i], &dspAddr);
+				cm_DSP_GetDspMemoryHandleSize(
+					component->memories[i], &dspSize);
+				ret += snprintf(
+					&buf[ret], sizeof(buf)-ret,
+					"%-10s: %p-%p %p-%p %p-%p %8lu\n",
+					MMDSP_getMappingById(i)->memoryName,
+					(void *)addr.physical,
+					(void *)addr.physical
+					+ component->memories[i]->size-1,
+					(void *)addr.logical,
+					(void *)addr.logical
+					+ component->memories[i]->size-1,
+					(void *)dspAddr,
+					(void *)dspAddr + dspSize - 1,
+					component->memories[i]->size);
+			}
+		}
+	}
+
+	OSAL_UNLOCK_API();
+	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations component_fops = {
+	.read = component_read,
+};
+
+static void cm_debug_component_create(t_component_instance *component)
+{
+        char tmp[12+MAX_COMPONENT_NAME_LENGTH];
+	struct cm_debug_component_cooky *cooky;
+	struct mpcConfig *mpc;
+	mpc = &osalEnv.mpc[COREIDX(component->Template->dspId)];
+
+	cooky = OSAL_Alloc_Zero(sizeof(*cooky));
+	if (cooky == NULL)
+		return;
+
+	component->dbgCooky = cooky;
+	sprintf(tmp, "%s-%08x", component->pathname,
+		(unsigned int)component->instance);
+	cooky->comp_file = debugfs_create_file(tmp, S_IRUSR|S_IRGRP,
+					       mpc->comp_dir,
+					       component, &component_fops);
+	if (IS_ERR(cooky->comp_file)) {
+		if (PTR_ERR(cooky->comp_file) != -ENODEV)
+			pr_info("CM: Can't create dsp/%s/components/%s"
+				"debugfs file: %ld\n",
+				mpc->name,
+				tmp,
+				PTR_ERR(cooky->comp_file));
+		cooky->comp_file = NULL;
+	} else {
+		char target_lnk[40+MAX_COMPONENT_NAME_LENGTH];
+		sprintf(target_lnk, "../../../dsp/%s/components/%s-%08x",
+			mpc->name,
+			component->pathname,
+			(unsigned int)component->instance);
+
+		/* Some firmware, like Executive Engine, do not belong
+		   to any process */
+		if (domainDesc[component->domainId].client == current->tgid) {
+			struct list_head* head;
+			struct cm_process_priv *entry = NULL;
+			/* Search the entry for the calling process */
+			list_for_each(head, &process_list) {
+				entry = list_entry(head,
+						   struct cm_process_priv,
+						   entry);
+				if (entry->pid == current->tgid)
+					break;
+			}
+
+			if (entry) {
+				cooky->proc_link = debugfs_create_symlink(
+					tmp,
+					entry->comp_dir,
+					target_lnk);
+				if (IS_ERR(cooky->proc_link)) {
+					long err = PTR_ERR(cooky->proc_link);
+					if (err != -ENODEV)
+						pr_info("CM: Can't create "
+							"proc/%d/%s "
+							"debugfs link: %ld\n",
+							entry->pid, tmp, err);
+					cooky->proc_link = NULL;
+				}
+			}
+		}
+	}
+}
+
+static void cm_debug_component_destroy(t_component_instance *component)
+{
+	struct cm_debug_component_cooky *cooky = component->dbgCooky;
+
+	if (cooky) {
+		component->dbgCooky = NULL;
+		debugfs_remove(cooky->proc_link);
+		debugfs_remove(cooky->comp_file);
+		OSAL_Free(cooky);
+	}
+}
+
+/* domain data managment */
+struct cm_debug_domain_cooky {
+	struct dentry *domain_file; /* entry in nmf-cm/components/ */
+	struct dentry *proc_link;   /* entry in nmf-cm/proc/       */
+	struct dentry *dsp_link;    /* entry in nmf-cm/dsp/sxa/domains */
+};
+
+static ssize_t domain_read(struct file *file, char __user *userbuf,
+			   size_t count, loff_t *ppos)
+{
+	t_cm_domain_id id =
+		(t_cm_domain_id)(long)file->f_dentry->d_inode->i_private;
+	t_cm_domain_desc *domain = &domainDesc[id];
+
+	char buf[640];
+	int ret=0;
+
+	OSAL_LOCK_API();
+	if ((domain->domain.coreId != MASK_ALL8)
+	    && (domain->dbgCooky != NULL)) {
+		t_cm_allocator_status status;
+		t_uint32 dOffset;
+		t_uint32 dSize;
+		if (domain->domain.coreId != ARM_CORE_ID) {
+			t_cm_domain_info info;
+
+			cm_DM_GetDomainAbsAdresses(id, &info);
+			cm_DSP_GetInternalMemoriesInfo(id, ESRAM_CODE,
+						       &dOffset, &dSize);
+			cm_MM_GetAllocatorStatus(
+				cm_DSP_GetAllocator(domain->domain.coreId,
+						    ESRAM_CODE),
+				dOffset, dSize, &status);
+			ret = snprintf(
+				buf, sizeof(buf),
+				"Core:\t%s\n\n"
+				"Memory    : Physical address  Logical address"
+				"     Size     Free     Used\n"
+				"---------------------------------------------"
+				"-----------------------------\n"
+				"ESRAM Code: %08x-%08lx %08x-%08lx\t%8lu %8lu "
+				"%8lu\n",
+				osalEnv.mpc[COREIDX(domain->domain.coreId)].name,
+				(unsigned int)info.esramCode.physical,
+				domain->domain.esramCode.size ?
+				info.esramCode.physical
+				+ domain->domain.esramCode.size - 1 : 0,
+				(unsigned int)info.esramCode.logical,
+				domain->domain.esramCode.size ?
+				info.esramCode.logical
+				+ domain->domain.esramCode.size - 1 : 0,
+				domain->domain.esramCode.size,
+				status.global.accumulate_free_memory,
+				status.global.accumulate_used_memory);
+
+			cm_DSP_GetInternalMemoriesInfo(id, ESRAM_EXT24,
+						       &dOffset, &dSize);
+			cm_MM_GetAllocatorStatus(
+				cm_DSP_GetAllocator(domain->domain.coreId,
+						    ESRAM_EXT24),
+				dOffset, dSize, &status);
+			ret += snprintf(
+				&buf[ret], sizeof(buf)-ret,
+				"ESRAM Data: %08x-%08lx "
+				"%08x-%08lx\t%8lu %8lu %8lu\n",
+				(unsigned int)info.esramData.physical,
+				domain->domain.esramData.size ?
+				info.esramData.physical
+				+ domain->domain.esramData.size - 1 : 0,
+				(unsigned int)info.esramData.logical,
+				domain->domain.esramData.size ?
+				info.esramData.logical
+				+ domain->domain.esramData.size - 1 : 0,
+				domain->domain.esramData.size,
+				status.global.accumulate_free_memory,
+				status.global.accumulate_used_memory);
+
+			cm_DSP_GetInternalMemoriesInfo(id, SDRAM_CODE,
+						       &dOffset, &dSize);
+			cm_MM_GetAllocatorStatus(
+				cm_DSP_GetAllocator(domain->domain.coreId,
+						    SDRAM_CODE),
+				dOffset, dSize, &status);
+			ret += snprintf(
+				&buf[ret], sizeof(buf)-ret,
+				"SDRAM Code: %08x-%08lx "
+				"%08x-%08lx\t%8lu %8lu %8lu\n",
+				(unsigned int)info.sdramCode.physical,
+				domain->domain.sdramCode.size ?
+				info.sdramCode.physical +
+				domain->domain.sdramCode.size - 1 : 0,
+				(unsigned int)info.sdramCode.logical,
+				domain->domain.sdramCode.size ?
+				info.sdramCode.logical +
+				domain->domain.sdramCode.size - 1 : 0,
+				domain->domain.sdramCode.size,
+				status.global.accumulate_free_memory,
+				status.global.accumulate_used_memory);
+
+			cm_DSP_GetInternalMemoriesInfo(id, SDRAM_EXT24,
+						       &dOffset, &dSize);
+			cm_MM_GetAllocatorStatus(
+				cm_DSP_GetAllocator(domain->domain.coreId,
+						    SDRAM_EXT24),
+				dOffset, dSize, &status);
+			ret += snprintf(
+				&buf[ret], sizeof(buf)-ret,
+				"SDRAM Data: %08x-%08lx "
+				"%08x-%08lx\t%8lu %8lu %8lu\n",
+				(unsigned int)info.sdramData.physical,
+				domain->domain.sdramData.size ?
+				info.sdramData.physical +
+				domain->domain.sdramData.size - 1 : 0,
+				(unsigned int)info.sdramData.logical,
+				domain->domain.sdramData.size ?
+				info.sdramData.logical +
+				domain->domain.sdramData.size - 1 : 0,
+				domain->domain.sdramData.size,
+				status.global.accumulate_free_memory,
+				status.global.accumulate_used_memory);
+		} else {
+			t_cm_system_address addr;
+			ret = snprintf(
+				buf, sizeof(buf),
+				"Core:\tarm\n\n"
+				"Memory    : Physical address  Logical "
+				"address     Size     Free     Used\n"
+				"---------------------------------------"
+				"-----------------------------------\n");
+			if (domain->domain.esramCode.size &&
+			    cm_DSP_GetDspBaseAddress(ARM_CORE_ID,
+						     ESRAM_CODE,
+						     &addr) == CM_OK) {
+				cm_DSP_GetInternalMemoriesInfo(id, ESRAM_CODE,
+							       &dOffset,
+							       &dSize);
+				cm_MM_GetAllocatorStatus(
+					cm_DSP_GetAllocator(ARM_CORE_ID,
+							    ESRAM_CODE),
+					dOffset, dSize, &status);
+				ret += snprintf(
+					&buf[ret], sizeof(buf)-ret,
+					"ESRAM Code: %08x-%08lx "
+					"%08x-%08lx\t%8lu %8lu %8lu\n",
+					(unsigned int)addr.physical,
+					addr.physical +
+					domain->domain.esramCode.size - 1,
+					(unsigned int)addr.logical,
+					addr.logical +
+					domain->domain.esramCode.size - 1,
+					domain->domain.esramCode.size,
+					status.global.accumulate_free_memory,
+					status.global.accumulate_used_memory);
+			}
+			if (domain->domain.esramData.size &&
+			    cm_DSP_GetDspBaseAddress(ARM_CORE_ID,
+						     ESRAM_EXT24,
+						     &addr) == CM_OK) {
+				cm_DSP_GetInternalMemoriesInfo(id, ESRAM_EXT24,
+							       &dOffset,
+							       &dSize);
+				cm_MM_GetAllocatorStatus(
+					cm_DSP_GetAllocator(ARM_CORE_ID,
+							    ESRAM_EXT24),
+					dOffset, dSize, &status);
+				ret += snprintf(
+					&buf[ret], sizeof(buf)-ret,
+					"ESRAM Data: %08x-%08lx "
+					"%08x-%08lx\t%8lu %8lu %8lu\n",
+					(unsigned int)addr.physical,
+					addr.physical +
+					domain->domain.esramData.size - 1,
+					(unsigned int)addr.logical,
+					addr.logical +
+					domain->domain.esramData.size - 1,
+					domain->domain.esramData.size,
+					status.global.accumulate_free_memory,
+					status.global.accumulate_used_memory);
+			}
+			if (domain->domain.sdramCode.size &&
+			    cm_DSP_GetDspBaseAddress(ARM_CORE_ID,
+						     SDRAM_CODE,
+						     &addr) == CM_OK) {
+				cm_DSP_GetInternalMemoriesInfo(id, SDRAM_CODE,
+							       &dOffset,
+							       &dSize);
+				cm_MM_GetAllocatorStatus(
+					cm_DSP_GetAllocator(ARM_CORE_ID,
+							    SDRAM_CODE),
+					dOffset, dSize, &status);
+				ret += snprintf(
+					&buf[ret], sizeof(buf)-ret,
+					"SDRAM Code: %08x-%08lx %08x-%08lx\t"
+					"%8lu %8lu %8lu\n",
+					(unsigned int)addr.physical,
+					addr.physical +
+					domain->domain.sdramCode.size - 1,
+					(unsigned int)addr.logical,
+					addr.logical +
+					domain->domain.sdramCode.size - 1,
+					domain->domain.sdramCode.size,
+					status.global.accumulate_free_memory,
+					status.global.accumulate_used_memory);
+			}
+			if (domain->domain.sdramData.size &&
+			    cm_DSP_GetDspBaseAddress(ARM_CORE_ID,
+						     SDRAM_EXT24,
+						     &addr) == CM_OK) {
+				cm_DSP_GetInternalMemoriesInfo(id, SDRAM_EXT24,
+							       &dOffset,
+							       &dSize);
+				cm_MM_GetAllocatorStatus(
+					cm_DSP_GetAllocator(ARM_CORE_ID,
+							    SDRAM_EXT24),
+					dOffset, dSize, &status);
+				ret += snprintf(
+					&buf[ret], sizeof(buf)-ret,
+					"SDRAM Data: %08x-%08lx %08x-%08lx\t"
+					"%8lu %8lu %8lu\n",
+					(unsigned int)addr.physical,
+					addr.physical +
+					domain->domain.sdramData.size - 1,
+					(unsigned int)addr.logical,
+					addr.logical +
+					domain->domain.sdramData.size - 1,
+					domain->domain.sdramData.size,
+					status.global.accumulate_free_memory,
+					status.global.accumulate_used_memory);
+			}
+		}
+	}
+	OSAL_UNLOCK_API();
+	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);;
+}
+
+static const struct file_operations domain_fops = {
+	.read = domain_read,
+};
+
+static void cm_debug_domain_create(t_cm_domain_id id)
+{
+        char tmp[12];
+	struct cm_debug_domain_cooky *cooky;
+
+	cooky = OSAL_Alloc_Zero(sizeof(*cooky));
+	if (cooky == NULL)
+		return;
+
+	domainDesc[id].dbgCooky = cooky;
+	sprintf(tmp, "%u", id);
+	cooky->domain_file = debugfs_create_file(tmp, S_IRUSR|S_IRGRP,
+						 domain_dir,
+						 (void *)(long)id,
+						 &domain_fops);
+	if (IS_ERR(cooky->domain_file)) {
+		if (PTR_ERR(cooky->domain_file) != -ENODEV)
+			pr_err("CM: Can't create domains/%s debugfs "
+			       "file: %ld\n", tmp,
+			       PTR_ERR(cooky->domain_file));
+		cooky->domain_file = NULL;
+	} else {
+		char target_lnk[40];
+		sprintf(target_lnk, "../../../domains/%u", id);
+
+		if (domainDesc[id].client != NMF_CORE_CLIENT) {
+			struct list_head* head;
+			struct cm_process_priv *entry = NULL;
+
+			/* Search the entry for the target process */
+			list_for_each(head, &process_list) {
+				entry = list_entry(head,
+						   struct cm_process_priv,
+						   entry);
+				if (entry->pid == domainDesc[id].client)
+					break;
+			}
+
+			if (entry) {
+				cooky->proc_link = debugfs_create_symlink(
+					tmp,
+					entry->domain_dir,
+					target_lnk);
+				if (IS_ERR(cooky->proc_link)) {
+					long err = PTR_ERR(cooky->proc_link);
+					if (err != -ENODEV)
+						pr_err("CM: Can't create "
+						       "proc/%d/domains/%s "
+						       "debugfs link: %ld\n",
+						       entry->pid, tmp, err);
+					cooky->proc_link = NULL;
+				}
+			}
+		}
+		if (domainDesc[id].domain.coreId != ARM_CORE_ID) {
+			cooky->dsp_link =
+				debugfs_create_symlink(
+					tmp,
+					osalEnv.mpc[COREIDX(domainDesc[id].domain.coreId)].domain_dir,
+					target_lnk);
+			if (IS_ERR(cooky->dsp_link)) {
+				if (PTR_ERR(cooky->dsp_link) != -ENODEV)
+					pr_err("CM: Can't create dsp/%s/domains/%s "
+					       "debugfs link: %ld\n",
+					       osalEnv.mpc[COREIDX(domainDesc[id].domain.coreId)].name,
+					       tmp,
+					       PTR_ERR(cooky->dsp_link));
+				cooky->dsp_link = NULL;
+			}
+		}
+	}
+}
+
+static void cm_debug_domain_destroy(t_cm_domain_id id)
+{
+	struct cm_debug_domain_cooky *cooky = domainDesc[id].dbgCooky;
+	if (cooky) {
+		domainDesc[id].dbgCooky = NULL;
+		debugfs_remove(cooky->proc_link);
+		debugfs_remove(cooky->dsp_link);
+		debugfs_remove(cooky->domain_file);
+		OSAL_Free(cooky);
+	}
+}
+
+/* proc directory */
+void cm_debug_proc_init(struct cm_process_priv *entry)
+{
+        char tmp[PROC_NUMBUF];
+        sprintf(tmp, "%d", entry->pid);
+	entry->dir = debugfs_create_dir(tmp, proc_dir);
+	if (IS_ERR(entry->dir)) {
+		if (PTR_ERR(entry->dir) != -ENODEV)
+			pr_info("CM: Can't create proc/%d debugfs directory: "
+				"%ld\n", entry->pid, PTR_ERR(entry->dir));
+		entry->dir = NULL;
+		return;
+	}
+	entry->comp_dir = debugfs_create_dir("components", entry->dir);
+	if (IS_ERR(entry->comp_dir)) {
+		if (PTR_ERR(entry->comp_dir) != -ENODEV)
+			pr_info("CM: Can't create proc/%d/components debugfs "
+				"directory: %ld\n", entry->pid,
+				PTR_ERR(entry->comp_dir));
+		entry->comp_dir = NULL;
+	}
+	entry->domain_dir = debugfs_create_dir("domains", entry->dir);
+	if (IS_ERR(entry->domain_dir)) {
+		if (PTR_ERR(entry->domain_dir) != -ENODEV)
+			pr_info("CM: Can't create proc/%d/domains debugfs "
+				"directory: %ld\n", entry->pid,
+				PTR_ERR(entry->domain_dir));
+		entry->domain_dir = NULL;
+	}
+}
+
+/* DSP meminfo */
+static ssize_t meminfo_read(struct file *file, char __user *userbuf,
+			    size_t count, loff_t *ppos)
+{
+	const t_nmf_core_id id =
+		*(t_nmf_core_id *)file->f_dentry->d_inode->i_private;
+	char buf[640];
+	int ret=0;
+	t_cm_allocator_status status;
+	t_cm_system_address addr;
+
+	OSAL_LOCK_API();
+	cm_MM_GetAllocatorStatus(cm_DSP_GetAllocator(id, ESRAM_CODE),
+				 0, 0, &status);
+	cm_DSP_GetDspBaseAddress(id, ESRAM_CODE,  &addr);
+	ret = snprintf(buf, sizeof(buf),
+		       "Memory    : Physical address  Logical address     Size "
+		       "    Free     Used\n"
+		       "-------------------------------------------------------"
+		       "-------------------\n"
+		       "ESRAM Code: %08x-%08lx %08x-%08lx\t%8lu %8lu %8lu\n",
+		       (unsigned int)addr.physical,
+		       addr.physical + status.global.size - 1,
+		       (unsigned int)addr.logical,
+		       addr.logical + status.global.size - 1,
+		       status.global.size,
+		       status.global.accumulate_free_memory,
+		       status.global.accumulate_used_memory);
+
+	cm_MM_GetAllocatorStatus(cm_DSP_GetAllocator(id, ESRAM_EXT24),
+				 0, 0, &status);
+	cm_DSP_GetDspBaseAddress(id, ESRAM_EXT24, &addr);
+	ret += snprintf(&buf[ret], sizeof(buf)-ret,
+			"ESRAM Data: %08x-%08lx %08x-%08lx\t%8lu %8lu %8lu\n",
+			(unsigned int)addr.physical,
+			addr.physical + status.global.size - 1,
+			(unsigned int)addr.logical,
+			addr.logical + status.global.size - 1,
+			status.global.size,
+			status.global.accumulate_free_memory,
+			status.global.accumulate_used_memory);
+
+	cm_MM_GetAllocatorStatus(cm_DSP_GetAllocator(id, SDRAM_CODE),
+				 0, 0, &status);
+	cm_DSP_GetDspBaseAddress(id, SDRAM_CODE,  &addr);
+	ret += snprintf(&buf[ret], sizeof(buf)-ret,
+			"SDRAM Code: %08x-%08lx %08x-%08lx\t%8lu %8lu %8lu\n",
+			(unsigned int)addr.physical,
+			addr.physical + status.global.size - 1,
+			(unsigned int)addr.logical,
+			addr.logical + status.global.size - 1,
+			status.global.size,
+			status.global.accumulate_free_memory,
+			status.global.accumulate_used_memory);
+
+	cm_MM_GetAllocatorStatus(cm_DSP_GetAllocator(id, SDRAM_EXT24),
+				 0, 0, &status);
+	cm_DSP_GetDspBaseAddress(id, SDRAM_EXT24, &addr);
+	ret += snprintf(&buf[ret], sizeof(buf)-ret,
+			"SDRAM Data: %08x-%08lx %08x-%08lx\t%8lu %8lu %8lu\n",
+			(unsigned int)addr.physical,
+			addr.physical + status.global.size - 1,
+			(unsigned int)addr.logical,
+			addr.logical + status.global.size - 1,
+			status.global.size,
+			status.global.accumulate_free_memory,
+			status.global.accumulate_used_memory);
+
+	cm_MM_GetAllocatorStatus(cm_DSP_GetAllocator(id, INTERNAL_XRAM24),
+				 0, 0, &status);
+	cm_DSP_GetDspBaseAddress(id, INTERNAL_XRAM24, &addr);
+	ret += snprintf(&buf[ret], sizeof(buf)-ret,
+			"TCM XRAM  : %08x-%08lx %08x-%08lx\t%8lu %8lu %8lu\n",
+			(unsigned int)addr.physical,
+			addr.physical + status.global.size - 1,
+			(unsigned int)addr.logical,
+			addr.logical + status.global.size - 1,
+			status.global.size,
+			status.global.accumulate_free_memory,
+			status.global.accumulate_used_memory);
+
+	cm_MM_GetAllocatorStatus(cm_DSP_GetAllocator(id, INTERNAL_YRAM24),
+				 0, 0, &status);
+	cm_DSP_GetDspBaseAddress(id, INTERNAL_YRAM24, &addr);
+	ret += snprintf(&buf[ret], sizeof(buf)-ret,
+			"TCM YRAM  : %08x-%08lx %08x-%08lx\t%8lu %8lu %8lu\n",
+			(unsigned int)addr.physical,
+			addr.physical + status.global.size - 1,
+			(unsigned int)addr.logical,
+			addr.logical + status.global.size - 1,
+			status.global.size,
+			status.global.accumulate_free_memory,
+			status.global.accumulate_used_memory);
+
+	OSAL_UNLOCK_API();
+	return simple_read_from_buffer(userbuf, count, ppos, buf, ret);;
+}
+
+static const struct file_operations mem_fops = {
+	.read = meminfo_read,
+};
+
+/* ESRAM file operations */
+static int esram_open(struct inode *inode, struct file *file)
+{
+	int i, err=0;
+	for (i=0; i<NB_ESRAM; i++) {
+		if (regulator_enable(osalEnv.esram_regulator[i]) < 0) {
+			pr_err("CM (%s): can't enable regulator"
+			       "for esram bank %s\n", __func__,
+			       i ? "34" : "12");
+			err = -EIO;
+			break;
+		}
+	}
+
+	if (err) {
+		for (i--; i>=0; i--)
+			regulator_disable(osalEnv.esram_regulator[i]);
+	}
+
+	return err;
+}
+
+static int esram_release(struct inode *inode, struct file *file)
+{
+	int i;
+	for (i=0; i<NB_ESRAM; i++)
+		regulator_disable(osalEnv.esram_regulator[i]);
+	return 0;
+}
+
+static ssize_t esram_read(struct file *file, char __user *user_buf,
+			  size_t count, loff_t *ppos)
+{
+	return simple_read_from_buffer(user_buf, count, ppos,
+				       osalEnv.esram_base,
+				       cfgESRAMSize*ONE_KB);
+}
+
+static const struct file_operations esram_fops = {
+	.read    =	esram_read,
+	.open    =	esram_open,
+	.release =	esram_release,
+};
+
+
+/* TCM file */
+void cm_debug_create_tcm_file(unsigned mpc_index)
+{
+	osalEnv.mpc[mpc_index].tcm_file = debugfs_create_blob(
+		"tcm24", S_IRUSR|S_IRGRP|S_IROTH,
+		osalEnv.mpc[mpc_index].snapshot_dir,
+		&osalEnv.mpc[mpc_index].base);
+	if (IS_ERR(osalEnv.mpc[mpc_index].tcm_file)) {
+		if (PTR_ERR(osalEnv.mpc[mpc_index].tcm_file) != -ENODEV)
+			pr_info("CM: Can't create dsp/%s/tcm24 debugfs "
+				"directory: %ld\n", osalEnv.mpc[mpc_index].name,
+				PTR_ERR(osalEnv.mpc[mpc_index].tcm_file));
+		osalEnv.mpc[mpc_index].tcm_file = NULL;
+	}
+}
+
+void cm_debug_destroy_tcm_file(unsigned mpc_index)
+{
+	debugfs_remove(osalEnv.mpc[mpc_index].tcm_file);
+}
+
+/* Global init */
+void cm_debug_init(void)
+{
+	int i;
+
+	cm_dir = debugfs_create_dir(DEBUGFS_ROOT, NULL);
+	if (IS_ERR(cm_dir)) {
+		if (PTR_ERR(cm_dir) != -ENODEV)
+			pr_info("CM: Can't create root debugfs directory: "
+				"%ld\n", PTR_ERR(cm_dir));
+		cm_dir = NULL;
+		return;
+	}
+
+	proc_dir = debugfs_create_dir("proc", cm_dir);
+	if (IS_ERR(proc_dir)) {
+		if (PTR_ERR(proc_dir) != -ENODEV)
+			pr_info("CM: Can't create 'proc' debugfs directory: "
+				"%ld\n", PTR_ERR(proc_dir));
+		proc_dir = NULL;
+	}
+
+	core_dir = debugfs_create_dir("dsp", cm_dir);
+	if (IS_ERR(core_dir)) {
+		if (PTR_ERR(core_dir) != -ENODEV)
+			pr_info("CM: Can't create 'dsp' debugfs directory: %ld\n",
+				PTR_ERR(core_dir));
+		core_dir = NULL;
+	}
+
+	domain_dir = debugfs_create_dir("domains", cm_dir);
+	if (IS_ERR(domain_dir)) {
+		if (PTR_ERR(domain_dir) != -ENODEV)
+			pr_info("CM: Can't create 'domains' debugfs directory: "
+				"%ld\n",
+				PTR_ERR(domain_dir));
+		domain_dir = NULL;
+	} else {
+		osal_debug_ops.domain_create  = cm_debug_domain_create;
+		osal_debug_ops.domain_destroy = cm_debug_domain_destroy;
+	}
+
+	for (i=0; i<NB_MPC; i++) {
+		osalEnv.mpc[i].dir = debugfs_create_dir(osalEnv.mpc[i].name,
+							core_dir);
+		if (IS_ERR(osalEnv.mpc[i].dir)) {
+			if (PTR_ERR(osalEnv.mpc[i].dir) != -ENODEV)
+				pr_info("CM: Can't create %s debugfs directory: "
+					"%ld\n",
+					osalEnv.mpc[i].name,
+					PTR_ERR(osalEnv.mpc[i].dir));
+			osalEnv.mpc[i].dir = NULL;
+		} else {
+			osalEnv.mpc[i].mem_file =
+				debugfs_create_file("meminfo", S_IRUSR|S_IRGRP,
+						    osalEnv.mpc[i].dir,
+						    (void*)&osalEnv.mpc[i].coreId,
+						    &mem_fops);
+			if (IS_ERR(osalEnv.mpc[i].mem_file)) {
+				if (PTR_ERR(osalEnv.mpc[i].mem_file) != -ENODEV)
+					pr_err("CM: Can't create dsp/%s/meminfo "
+					       "debugfs file: %ld\n",
+					       osalEnv.mpc[i].name,
+					       PTR_ERR(osalEnv.mpc[i].mem_file));
+				osalEnv.mpc[i].mem_file = NULL;
+			}
+
+			osalEnv.mpc[i].comp_dir = debugfs_create_dir(
+				"components",
+				osalEnv.mpc[i].dir);
+			if (IS_ERR(osalEnv.mpc[i].comp_dir)) {
+				if (PTR_ERR(osalEnv.mpc[i].comp_dir) != -ENODEV)
+					pr_info("CM: Can't create "
+						"'dsp/%s/components' debugfs "
+						"directory: %ld\n",
+						osalEnv.mpc[i].name,
+						PTR_ERR(osalEnv.mpc[i].comp_dir));
+				osalEnv.mpc[i].comp_dir = NULL;
+			}
+
+			osalEnv.mpc[i].domain_dir =
+				debugfs_create_dir("domains",
+						   osalEnv.mpc[i].dir);
+			if (IS_ERR(osalEnv.mpc[i].domain_dir)) {
+				if (PTR_ERR(osalEnv.mpc[i].domain_dir) != -ENODEV)
+					pr_info("CM: Can't create "
+						"'dsp/%s/domains' "
+						"debugfs directory: %ld\n",
+						osalEnv.mpc[i].name,
+						PTR_ERR(osalEnv.mpc[i].domain_dir));
+				osalEnv.mpc[i].domain_dir = NULL;
+			}
+
+			osalEnv.mpc[i].snapshot_dir = debugfs_create_dir(
+				"snapshot",
+				osalEnv.mpc[i].dir);
+			if (IS_ERR(osalEnv.mpc[i].snapshot_dir)) {
+				if (PTR_ERR(osalEnv.mpc[i].snapshot_dir) != -ENODEV)
+					pr_info("CM: Can't create "
+						"'dsp/%s/snapshot' debugfs "
+						"directory: %ld\n",
+						osalEnv.mpc[i].name,
+						PTR_ERR(osalEnv.mpc[i].snapshot_dir));
+				osalEnv.mpc[i].snapshot_dir = NULL;
+			} else {
+				debugfs_create_file("esram", S_IRUSR|S_IRGRP|S_IROTH,
+						    osalEnv.mpc[i].snapshot_dir,
+						    &osalEnv.esram_base,
+						    &esram_fops);
+				debugfs_create_blob("sdram_data", S_IRUSR|S_IRGRP|S_IROTH,
+						    osalEnv.mpc[i].snapshot_dir,
+						    &osalEnv.mpc[i].sdram_data);
+				debugfs_create_blob("sdram_code", S_IRUSR|S_IRGRP|S_IROTH,
+						    osalEnv.mpc[i].snapshot_dir,
+						    &osalEnv.mpc[i].sdram_code);
+			}
+
+			debugfs_create_bool("running", S_IRUSR|S_IRGRP,
+					    osalEnv.mpc[i].dir,
+					    (u32 *)&osalEnv.mpc[i].monitor_tsk);
+			debugfs_create_u8("load", S_IRUSR|S_IRGRP,
+					  osalEnv.mpc[i].dir,
+					  &osalEnv.mpc[i].load);
+			debugfs_create_u8("requested_opp", S_IRUSR|S_IRGRP,
+					  osalEnv.mpc[i].dir,
+					  &osalEnv.mpc[i].opp_request);
+		}
+	}
+	osal_debug_ops.component_create  = cm_debug_component_create;
+	osal_debug_ops.component_destroy = cm_debug_component_destroy;
+}
+
+void cm_debug_exit(void)
+{
+	debugfs_remove_recursive(cm_dir);
+}
+
+#endif /* CONFIG_DEBUG_FS */
diff --git a/drivers/staging/nmf-cm/cm_debug.h b/drivers/staging/nmf-cm/cm_debug.h
new file mode 100644
index 00000000000..26c80682d11
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_debug.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef CM_DEBUG_H
+#define CM_DEBUG_H
+
+#ifdef CONFIG_DEBUG_FS
+#include "cmld.h"
+
+void cm_debug_init(void);
+void cm_debug_exit(void);
+void cm_debug_proc_init(struct cm_process_priv *entry);
+void cm_debug_create_tcm_file(unsigned mpc_index);
+void cm_debug_destroy_tcm_file(unsigned mpc_index);
+
+#else
+
+#define cm_debug_init()
+#define cm_debug_exit()
+#define cm_debug_proc_init(entry)
+#define cm_debug_create_tcm_file(mpc_index)
+#define cm_debug_destroy_tcm_file(mpc_index)
+
+#endif /* CONFIG_DEBUG_FS */
+#endif /* CM_DEBUG_H */
diff --git a/drivers/staging/nmf-cm/cm_dma.c b/drivers/staging/nmf-cm/cm_dma.c
new file mode 100644
index 00000000000..652b504324c
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_dma.c
@@ -0,0 +1,226 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <asm/io.h>
+#include <mach/db8500-regs.h>
+
+#include "cm_dma.h"
+
+#define CMDMA_LIDX (2)
+#define CMDMA_REG_LCLA (0x024)
+
+static void __iomem *virtbase = NULL;
+
+static int cmdma_write_cyclic_list_mem2per(
+    unsigned int from_addr,
+    unsigned int to_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS);
+
+static int cmdma_write_cyclic_list_per2mem(
+    unsigned int from_addr,
+    unsigned int to_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS);
+
+static bool cmdma_setup_relink_area_called = false;
+
+int cmdma_setup_relink_area( unsigned int mem_addr,
+    unsigned int per_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS,
+    enum cmdma_type type)
+{
+    if (!cmdma_setup_relink_area_called)
+	    cmdma_setup_relink_area_called = true;
+
+    switch (type) {
+
+    case CMDMA_MEM_2_PER:
+        return cmdma_write_cyclic_list_mem2per(
+            mem_addr,
+            per_addr,
+            segments,
+            segmentsize,
+            LOS);
+
+    case CMDMA_PER_2_MEM:
+        return cmdma_write_cyclic_list_per2mem(
+            per_addr,
+            mem_addr,
+            segments,
+            segmentsize,
+            LOS);
+
+    default :
+        return -EINVAL;
+    }
+ }
+
+ static unsigned int cmdma_getlcla( void) {
+
+    if(!virtbase)
+        virtbase = ioremap(U8500_DMA_BASE, CMDMA_REG_LCLA + sizeof(int) );
+
+    return readl(virtbase + CMDMA_REG_LCLA);
+ }
+
+ static void cmdma_write_relink_params_mem2per (
+    int * relink,
+    unsigned int LOS,
+    unsigned int nb_element,
+    unsigned int src_addr,
+    unsigned int dst_addr,
+    unsigned int burst_size) {
+
+    relink[0] =  (((long)(nb_element & 0xFFFF)) << 16) |
+        (src_addr & 0xFFFF);
+
+    relink[1] = (((src_addr >> 16)  & 0xFFFFUL) << 16) |
+        (0x1200UL | (LOS << 1) | (burst_size<<10));
+
+    relink[2] = ((nb_element & 0xFFFF) << 16)  |
+        (dst_addr & 0xFFFF);
+
+    relink[3] = (((dst_addr >> 16)  & 0xFFFFUL) << 16 ) |
+        0x8201UL | ((LOS+1) << 1) | (burst_size<<10);
+}
+
+static void cmdma_write_relink_params_per2mem (
+    int * relink,
+    unsigned int LOS,
+    unsigned int nb_element,
+    unsigned int src_addr,
+    unsigned int dst_addr,
+    unsigned int burst_size) {
+
+    relink[0] =  (((long)(nb_element & 0xFFFF)) << 16) |
+        (src_addr & 0xFFFF);
+
+    relink[1] = (((src_addr >> 16)  & 0xFFFFUL) << 16) |
+        (0x8201UL | (LOS << 1) | (burst_size<<10));
+
+    relink[2] = ((nb_element & 0xFFFF) << 16)  |
+        (dst_addr & 0xFFFF);
+
+    relink[3] = (((dst_addr >> 16)  & 0xFFFFUL) << 16 ) |
+        0x1200UL | ((LOS+1) << 1) | (burst_size<<10);
+}
+
+static int cmdma_write_cyclic_list_mem2per(
+    unsigned int from_addr,
+    unsigned int to_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS) {
+
+    unsigned int i,j;
+    int *relink;
+
+    j = LOS;
+
+    for ( i = 0; i < segments; i++) {
+        relink = ioremap_nocache (cmdma_getlcla() + 1024 * CMDMA_LIDX + 8 * j, 4 * sizeof(int));
+
+        if (i == (segments-1))
+                j = LOS;
+            else
+                j += 2;
+
+        cmdma_write_relink_params_mem2per (
+            relink,
+            j,
+            segmentsize / 4,
+            from_addr,
+            to_addr,
+            0x2);
+
+	iounmap(relink);
+
+        from_addr += segmentsize;
+	}
+
+    return 0;
+}
+
+static int cmdma_write_cyclic_list_per2mem(
+    unsigned int from_addr,
+    unsigned int to_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS) {
+
+    unsigned int i,j;
+    int *relink;
+    j = LOS;
+
+    for ( i = 0; i < segments; i++) {
+        relink = ioremap_nocache (cmdma_getlcla() + 1024 * CMDMA_LIDX + 8 * j, 4 * sizeof(int));
+
+        if (i == (segments-1))
+            j = LOS;
+        else
+            j += 2;
+
+        cmdma_write_relink_params_per2mem (
+            relink,
+            j,
+            segmentsize / 4,
+            from_addr,
+            to_addr,
+            0x2);
+
+	iounmap(relink);
+
+        to_addr += segmentsize;
+    }
+
+    return 0;
+}
+
+static void __iomem *dmabase = 0;
+int cmdma_init(void)
+{
+	dmabase = ioremap_nocache(U8500_DMA_BASE, PAGE_SIZE);
+	if (dmabase == NULL)
+		return -ENOMEM;
+	else
+		return 0;
+}
+
+void cmdma_destroy(void)
+{
+	iounmap(dmabase);
+}
+
+#define SSLNK_CHAN_2 (0x40C + 0x20 * 2)
+#define SDLNK_CHAN_2 (0x41C + 0x20 * 2)
+
+void cmdma_stop_dma(void)
+{
+    if(cmdma_setup_relink_area_called) {
+        cmdma_setup_relink_area_called = false;
+        if (readl(dmabase + SSLNK_CHAN_2) & (0x3 << 28)) {
+            printk(KERN_ERR "CM: ERROR - RX DMA was running\n");
+        }
+        if (readl(dmabase + SDLNK_CHAN_2) & (0x3 << 28)) {
+            printk(KERN_ERR "CM: ERROR - TX DMA was running\n");
+        }
+
+        writel(~(1 << 28), dmabase + SSLNK_CHAN_2);
+        while (readl(dmabase + SSLNK_CHAN_2) & (0x3 << 28));
+
+        writel(~(1 << 28), dmabase + SDLNK_CHAN_2);
+        while (readl(dmabase + SDLNK_CHAN_2) & (0x3 << 28));
+    }
+}
diff --git a/drivers/staging/nmf-cm/cm_dma.h b/drivers/staging/nmf-cm/cm_dma.h
new file mode 100644
index 00000000000..4fccef03830
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_dma.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+
+#ifndef __CMDMA_H
+#define __CMDMA_H
+#include "cmioctl.h"
+
+int cmdma_setup_relink_area(
+    unsigned int mem_addr,
+    unsigned int per_addr,
+    unsigned int segments,
+    unsigned int segmentsize,
+    unsigned int LOS,
+    enum cmdma_type type);
+
+void cmdma_stop_dma(void);
+int cmdma_init(void);
+void cmdma_destroy(void);
+#endif
diff --git a/drivers/staging/nmf-cm/cm_service.c b/drivers/staging/nmf-cm/cm_service.c
new file mode 100644
index 00000000000..7335cccbd6a
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_service.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file cm_service.c
+ *
+ * Nomadik Multiprocessing Framework Linux Driver
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/plist.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock_types.h>
+
+#include <cm/engine/api/control/irq_engine.h>
+
+#include "osal-kernel.h"
+#include "cmld.h"
+#include "cm_service.h"
+#include "cm_dma.h"
+
+/* Panic managment */
+static void service_tasklet_func(unsigned long);
+unsigned long service_tasklet_data = 0;
+DECLARE_TASKLET(cmld_service_tasklet, service_tasklet_func, 0);
+
+void dispatch_service_msg(struct osal_msg *msg)
+{
+	struct list_head *head, *next;
+#ifdef CONFIG_DEBUG_FS
+	bool dump_flag_to_set = true;
+#endif
+	/*
+	 * Note: no lock needed to protect the channel_list against list
+	 * changes, as the current tasklet is disabled each time we modify
+	 * the list
+	 */
+	list_for_each_safe(head, next, &channel_list) {
+		struct cm_channel_priv *channelPriv = list_entry(head, struct cm_channel_priv, entry);
+		struct osal_msg *new_msg;
+		size_t msg_size;
+
+		if (channelPriv->state == CHANNEL_CLOSED)
+			continue;
+		msg_size = sizeof(new_msg->hdr) + sizeof(new_msg->d.srv);
+		new_msg = kmalloc(msg_size, GFP_ATOMIC);
+		if (new_msg == NULL) {
+			pr_err("[CM] %s: can't allocate memory, service"
+			       " message not dispatched !!\n", __func__);
+			continue;
+		}
+		memcpy(new_msg, msg, msg_size);
+		plist_node_init(&new_msg->msg_entry, 0);
+#ifdef CONFIG_DEBUG_FS
+		if (cmld_user_has_debugfs && dump_flag_to_set
+		    && (new_msg->d.srv.srvType == NMF_SERVICE_PANIC)) {
+			/*
+			 * The reciever of this message will do the DSP
+			 * memory dump
+			 */
+			new_msg->d.srv.srvData.panic.panicSource
+				|= DEBUGFS_DUMP_FLAG;
+			dump_flag_to_set = false;
+			cmld_dump_ongoing = channelPriv->proc->pid;
+		}
+#endif
+		spin_lock_bh(&channelPriv->bh_lock);
+		plist_add(&new_msg->msg_entry, &channelPriv->messageQueue);
+		spin_unlock_bh(&channelPriv->bh_lock);
+		wake_up(&channelPriv->waitq);
+	}
+}
+
+static void service_tasklet_func(unsigned long unused)
+{
+	t_cm_service_type type;
+	t_cm_service_description desc;
+	int i=0;
+
+	do {
+		if (test_and_clear_bit(i, &service_tasklet_data)) { 
+			CM_getServiceDescription(osalEnv.mpc[i].coreId, &type, &desc);
+
+			switch (type) {
+			case CM_MPC_SERVICE_PANIC: {
+				struct osal_msg msg;
+
+				msg.msg_type = MSG_SERVICE;
+				msg.d.srv.srvType = NMF_SERVICE_PANIC;
+				msg.d.srv.srvData.panic = desc.u.panic;
+
+				dispatch_service_msg(&msg);
+				/*
+				 * Stop DMA directly before shutdown, to avoid
+				 * bad sound. Should be called after DSP has
+				 * stopped executing, to avoid the DSP
+				 * re-starting DMA
+				 */
+				if (osalEnv.mpc[i].coreId == SIA_CORE_ID)
+					cmdma_stop_dma();
+				break;
+			}
+			case CM_MPC_SERVICE_PRINT: {
+				char msg[256];
+				if (CM_ReadMPCString(osalEnv.mpc[i].coreId,
+						     desc.u.print.dspAddress, msg,
+						     sizeof(msg)) == CM_OK)
+					printk(msg, desc.u.print.value1,
+					       desc.u.print.value2);
+				break;
+			}
+			case CM_MPC_SERVICE_TRACE:
+				spin_lock_bh(&osalEnv.mpc[i].trace_reader_lock);
+				if (osalEnv.mpc[i].trace_reader)
+					wake_up_process(osalEnv.mpc[i].trace_reader);
+				spin_unlock_bh(&osalEnv.mpc[i].trace_reader_lock);
+				break;
+			default:
+				pr_err("[CM] %s: MPC Service Type %d not supported\n", __func__, type);
+			}
+			enable_irq(osalEnv.mpc[i].interrupt1);
+		}
+		i = (i+1) % NB_MPC;
+	} while (service_tasklet_data != 0);
+}
diff --git a/drivers/staging/nmf-cm/cm_service.h b/drivers/staging/nmf-cm/cm_service.h
new file mode 100644
index 00000000000..39582eae573
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_service.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file cm_service.c
+ *
+ * Nomadik Multiprocessing Framework Linux Driver
+ *
+ */
+
+#ifndef CM_SERVICE_H
+#define CM_SERVICE_H
+
+#include <linux/interrupt.h>
+
+extern unsigned long service_tasklet_data;
+extern struct tasklet_struct cmld_service_tasklet;
+void dispatch_service_msg(struct osal_msg *msg);
+
+#endif
diff --git a/drivers/staging/nmf-cm/cm_syscall.c b/drivers/staging/nmf-cm/cm_syscall.c
new file mode 100644
index 00000000000..304b86ffee7
--- /dev/null
+++ b/drivers/staging/nmf-cm/cm_syscall.c
@@ -0,0 +1,1440 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include <cm/engine/api/cm_engine.h>
+#include "cmioctl.h"
+#include "osal-kernel.h"
+#include "cmld.h"
+#include "cm_dma.h"
+
+/** Dequeue and free per-process messages for specific binding
+ *
+ * \note
+ * This is only safe if the per process mutex is held when called.
+ */
+static inline void freeMessages(struct cm_channel_priv* cPriv, t_skelwrapper* binding)
+{
+	struct osal_msg *this, *next;
+
+	spin_lock_bh(&cPriv->bh_lock);
+
+	/* free any pending messages */
+         plist_for_each_entry_safe(this, next, &cPriv->messageQueue, msg_entry) {
+		if (this->msg_type == MSG_INTERFACE
+		    && this->d.itf.skelwrap == binding) {
+			plist_del(&this->msg_entry, &cPriv->messageQueue);
+			kfree(this);
+		}
+	}
+	spin_unlock_bh(&cPriv->bh_lock);
+}
+
+static t_cm_error copy_string_from_user(char *dst, const char __user *src, int len)
+{
+	int ret;
+
+	ret = strncpy_from_user(dst, src, len);
+	if (ret < 0) /* -EFAULT */
+		return CM_INVALID_PARAMETER;
+
+	if (ret >= len)
+		return CM_OUT_OF_LIMITS;
+
+	return 0;
+}
+
+inline int cmld_InstantiateComponent(struct cm_process_priv* procPriv,
+				     CM_InstantiateComponent_t __user *param)
+{
+	CM_InstantiateComponent_t data;
+	char templateName[MAX_TEMPLATE_NAME_LENGTH];
+	char localName[MAX_COMPONENT_NAME_LENGTH];
+	char *dataFile = NULL;
+
+	/* Copy all user data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (data.in.dataFile != NULL) {
+		dataFile = vmalloc(data.in.dataFileSize);
+		if (dataFile == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFile, data.in.dataFile, data.in.dataFileSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	if ((data.out.error = copy_string_from_user(templateName,
+						    data.in.templateName,
+						    sizeof(templateName))))
+		goto out;
+
+	if ((data.in.localName != NULL) &&
+	    (data.out.error = copy_string_from_user(localName,
+						    data.in.localName,
+						    sizeof(localName))))
+		goto out;
+
+	/* Do appropriate CM Engine call */
+	data.out.error = CM_ENGINE_InstantiateComponent(templateName,
+							data.in.domainId,
+							procPriv->pid,
+							data.in.priority,
+							data.in.localName ? localName : NULL,
+							dataFile,
+							&data.out.component);
+
+out:
+	if (dataFile)
+		vfree(dataFile);
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponentFromCMCore(struct cm_process_priv* procPriv,
+					CM_BindComponentFromCMCore_t __user *param)
+{
+	CM_BindComponentFromCMCore_t data;
+	char providedItfServerName[MAX_INTERFACE_NAME_LENGTH];
+	char *dataFileSkeleton = NULL;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(providedItfServerName,
+						    data.in.providedItfServerName,
+						    sizeof(providedItfServerName))))
+		goto out;
+
+	if (data.in.dataFileSkeleton != NULL) {
+		dataFileSkeleton = OSAL_Alloc(data.in.dataFileSkeletonSize);
+		if (dataFileSkeleton == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileSkeleton, data.in.dataFileSkeleton,
+				   data.in.dataFileSkeletonSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	data.out.error = CM_ENGINE_BindComponentFromCMCore(data.in.server,
+							   providedItfServerName,
+							   data.in.fifosize,
+							   data.in.eventMemType,
+							   &data.out.host2mpcId,
+							   procPriv->pid,
+							   dataFileSkeleton);
+out:
+	if (dataFileSkeleton)
+		OSAL_Free(dataFileSkeleton);
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+
+	return 0;
+}
+
+inline int cmld_UnbindComponentFromCMCore(CM_UnbindComponentFromCMCore_t __user *param)
+{
+	CM_UnbindComponentFromCMCore_t data;
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_UnbindComponentFromCMCore(data.in.host2mpcId);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponentToCMCore(struct cm_channel_priv* channelPriv,
+				      CM_BindComponentToCMCore_t __user *param)
+{
+	CM_BindComponentToCMCore_t data;
+	t_skelwrapper *skelwrapper;
+	struct cm_process_priv *procPriv = channelPriv->proc;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+	char *dataFileStub = NULL;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	/* Do appropriate CM Engine call */
+	skelwrapper = (t_skelwrapper *)OSAL_Alloc(sizeof(*skelwrapper));
+	if (skelwrapper == NULL) {
+		data.out.error = CM_NO_MORE_MEMORY;
+		goto out;
+	}
+
+	if (data.in.dataFileStub != NULL) {
+		dataFileStub = OSAL_Alloc(data.in.dataFileStubSize);
+		if (dataFileStub == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileStub, data.in.dataFileStub, data.in.dataFileStubSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	if ((data.out.error = CM_ENGINE_BindComponentToCMCore(
+		     data.in.client,
+		     requiredItfClientName,
+		     data.in.fifosize,
+		     (t_nmf_mpc2host_handle)skelwrapper,
+		     dataFileStub,
+		     &data.out.mpc2hostId,
+		     procPriv->pid)) != CM_OK) {
+		OSAL_Free(skelwrapper);
+		goto out;
+	}
+
+	skelwrapper->upperLayerThis = data.in.upLayerThis;
+	skelwrapper->mpc2hostId = data.out.mpc2hostId;
+	skelwrapper->channelPriv = channelPriv;
+	mutex_lock(&channelPriv->skelListLock);
+	list_add(&skelwrapper->entry, &channelPriv->skelList);
+	mutex_unlock(&channelPriv->skelListLock);
+out:
+	if (dataFileStub != NULL)
+		OSAL_Free(dataFileStub);
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_UnbindComponentToCMCore(struct cm_process_priv* procPriv,
+					CM_UnbindComponentToCMCore_t __user *param)
+{
+	CM_UnbindComponentToCMCore_t data;
+	t_skelwrapper *skelwrapper;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_UnbindComponentToCMCore(
+			data.in.client, requiredItfClientName,
+			(t_nmf_mpc2host_handle*)&skelwrapper,
+								procPriv->pid);
+
+	if (data.out.error != CM_OK && data.out.error != CM_MPC_NOT_RESPONDING)
+		goto out;
+
+	data.out.upLayerThis = skelwrapper->upperLayerThis;
+
+	mutex_lock(&skelwrapper->channelPriv->msgQueueLock);
+	freeMessages(skelwrapper->channelPriv, skelwrapper);
+	mutex_lock(&skelwrapper->channelPriv->skelListLock);
+	list_del(&skelwrapper->entry);
+	mutex_unlock(&skelwrapper->channelPriv->skelListLock);
+	mutex_unlock(&skelwrapper->channelPriv->msgQueueLock);
+	OSAL_Free(skelwrapper);
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponentAsynchronous(struct cm_process_priv* procPriv,
+					  CM_BindComponentAsynchronous_t __user *param)
+{
+	CM_BindComponentAsynchronous_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+	char providedItfServerName[MAX_INTERFACE_NAME_LENGTH];
+	char *dataFileSkeletonOrEvent = NULL;
+	char *dataFileStub = NULL;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	if ((data.out.error = copy_string_from_user(providedItfServerName,
+						    data.in.providedItfServerName,
+						    sizeof(providedItfServerName))))
+		goto out;
+
+	if (data.in.dataFileSkeletonOrEvent != NULL) {
+		dataFileSkeletonOrEvent =
+			OSAL_Alloc(data.in.dataFileSkeletonOrEventSize);
+		if (dataFileSkeletonOrEvent == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileSkeletonOrEvent, data.in.dataFileSkeletonOrEvent, data.in.dataFileSkeletonOrEventSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	if (data.in.dataFileStub != NULL) {
+		dataFileStub = OSAL_Alloc(data.in.dataFileStubSize);
+		if (dataFileStub == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileStub, data.in.dataFileStub, data.in.dataFileStubSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	/* Do appropriate CM Engine call */
+	data.out.error = CM_ENGINE_BindComponentAsynchronous(data.in.client,
+							     requiredItfClientName,
+							     data.in.server,
+							     providedItfServerName,
+							     data.in.fifosize,
+							     data.in.eventMemType,
+							     procPriv->pid,
+							     dataFileSkeletonOrEvent,
+							     dataFileStub);
+
+out:
+	if (dataFileSkeletonOrEvent != NULL)
+		OSAL_Free(dataFileSkeletonOrEvent);
+	if (dataFileStub != NULL)
+		OSAL_Free(dataFileStub);
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_UnbindComponentAsynchronous(struct cm_process_priv* procPriv,
+					    CM_UnbindComponentAsynchronous_t __user *param)
+{
+	CM_UnbindComponentAsynchronous_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	/* Do appropriate CM Engine call */
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_UnbindComponentAsynchronous(data.in.client,
+								requiredItfClientName,
+								procPriv->pid);
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponent(struct cm_process_priv* procPriv,
+			      CM_BindComponent_t __user *param)
+{
+	CM_BindComponent_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+	char providedItfServerName[MAX_INTERFACE_NAME_LENGTH];
+	char *dataFileTrace = NULL;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	if ((data.out.error = copy_string_from_user(providedItfServerName,
+						    data.in.providedItfServerName,
+						    sizeof(providedItfServerName))))
+		goto out;
+
+	if (data.in.dataFileTrace != NULL) {
+		dataFileTrace = OSAL_Alloc(data.in.dataFileTraceSize);
+		if (dataFileTrace == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFileTrace, data.in.dataFileTrace,
+				   data.in.dataFileTraceSize)) {
+			data.out.error = CM_INVALID_PARAMETER;
+			goto out;
+		}
+	}
+
+	/* Do appropriate CM Engine call */
+	data.out.error = CM_ENGINE_BindComponent(data.in.client,
+						 requiredItfClientName,
+						 data.in.server,
+						 providedItfServerName,
+						 data.in.traced,
+						 procPriv->pid,
+						 dataFileTrace);
+out:
+	if (dataFileTrace != NULL)
+		OSAL_Free(dataFileTrace);
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_UnbindComponent(struct cm_process_priv* procPriv,
+				CM_UnbindComponent_t __user *param)
+{
+	CM_UnbindComponent_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	/* Do appropriate CM Engine call */
+	data.out.error = CM_ENGINE_UnbindComponent(data.in.client,
+						   requiredItfClientName,
+						   procPriv->pid);
+
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_BindComponentToVoid(struct cm_process_priv* procPriv,
+				    CM_BindComponentToVoid_t __user *param)
+{
+	CM_BindComponentToVoid_t data;
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_BindComponentToVoid(data.in.client,
+							requiredItfClientName,
+								procPriv->pid);
+
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_DestroyComponent(struct cm_process_priv* procPriv,
+				 CM_DestroyComponent_t __user *param)
+{
+	CM_DestroyComponent_t data;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_DestroyComponent(data.in.component,
+						    procPriv->pid);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_CreateMemoryDomain(struct cm_process_priv *procPriv,
+				   CM_CreateMemoryDomain_t __user *param)
+{
+	CM_CreateMemoryDomain_t data;
+	t_cm_domain_memory domain;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (copy_from_user(&domain, data.in.domain, sizeof(domain)))
+		return -EFAULT;
+
+	if (data.in.client == NMF_CURRENT_CLIENT)
+		data.out.error = CM_ENGINE_CreateMemoryDomain(procPriv->pid,
+							      &domain,
+							      &data.out.handle);
+	else {
+		/* Check if client is valid (ie already registered) */
+		struct list_head* head;
+		struct cm_process_priv *entry;
+
+		list_for_each(head, &process_list) {
+			entry = list_entry(head, struct cm_process_priv,
+					   entry);
+			if (entry->pid == data.in.client)
+				break;
+		}
+		if (head == &process_list)
+			data.out.error = CM_INVALID_PARAMETER;
+		else
+			data.out.error =
+				CM_ENGINE_CreateMemoryDomain(data.in.client,
+							     &domain,
+							     &data.out.handle);
+	}
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_CreateMemoryDomainScratch(struct cm_process_priv *procPriv,
+					  CM_CreateMemoryDomainScratch_t __user *param)
+{
+	CM_CreateMemoryDomainScratch_t data;
+	t_cm_domain_memory domain;
+
+	/* Copy all user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (copy_from_user(&domain, data.in.domain, sizeof(domain)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_CreateMemoryDomainScratch(procPriv->pid,
+							     data.in.parentId,
+							     &domain,
+							     &data.out.handle);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_DestroyMemoryDomain(CM_DestroyMemoryDomain_t __user *param)
+{
+	CM_DestroyMemoryDomain_t data;
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_DestroyMemoryDomain(data.in.domainId);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetDomainCoreId(CM_GetDomainCoreId_t __user *param)
+{
+	CM_GetDomainCoreId_t data;
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_GetDomainCoreId(data.in.domainId,
+						   &data.out.coreId);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_AllocMpcMemory(struct cm_process_priv *procPriv,
+			       CM_AllocMpcMemory_t __user *param)
+{
+	t_cm_error err;
+	CM_AllocMpcMemory_t data;
+	t_cm_memory_handle handle = 0;
+	struct memAreaDesc_t* memAreaDesc;
+	t_cm_system_address systemAddress;
+	t_uint32 mpcAddress;
+
+	/* Copy all user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* Disregard alignment information and force 4kB memory alignment,
+	   in any case (see devnotes.txt) */
+	/* PP: Disable this 'force' for now, because of the low amount of
+	   available MPC Memory */
+	//data.in.memAlignment = CM_MM_MPC_ALIGN_1024WORDS;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_AllocMpcMemory(data.in.domainId,
+						  procPriv->pid,
+						  data.in.memType,
+						  data.in.size,
+						  data.in.memAlignment,
+						  &handle);
+
+	data.out.pHandle = handle;
+
+	if (data.out.error != CM_OK)
+		goto out;
+
+	/* Get memory area decriptors in advance
+	   so to fill in list elements right now */
+	err = CM_ENGINE_GetMpcMemorySystemAddress(handle, &systemAddress);
+	if (err != CM_OK) {
+		pr_err("%s: failed CM_ENGINE_GetMpcMemorySystemAddress (%i)\n", __func__, err);
+		/* If we can't manage internally this allocated memory latter, it's
+		   better to report the error now.
+		   Free the handle to not let the driver in an inconsistent state */
+		CM_ENGINE_FreeMpcMemory(handle);
+		return -EFAULT;
+	}
+
+	/* Get MPC address in advance so to fill in list elements right now */
+	err = CM_ENGINE_GetMpcMemoryMpcAddress(handle, &mpcAddress);
+	if (err != CM_OK) {
+		pr_err("%s: failed CM_ENGINE_GetMpcMemoryMpcAddress (%i)\n", __func__, err);
+		/* see comments above */
+		CM_ENGINE_FreeMpcMemory(handle);
+		return -EFAULT;
+	}
+
+	/* Allocate and fill a new memory area descriptor. Add it to the list */
+	memAreaDesc = OSAL_Alloc(sizeof(struct memAreaDesc_t));
+	if (memAreaDesc == NULL) {
+		pr_err("%s: failed allocating memAreaDesc\n", __func__);
+		/* see comments above */
+		CM_ENGINE_FreeMpcMemory(handle);
+		return -ENOMEM;
+	}
+
+	memAreaDesc->procPriv = procPriv;
+	memAreaDesc->handle = handle;
+	memAreaDesc->tid = 0;
+	memAreaDesc->physAddr = systemAddress.physical;
+	memAreaDesc->kernelLogicalAddr = systemAddress.logical;
+	memAreaDesc->userLogicalAddr = 0;
+	memAreaDesc->mpcPhysAddr = mpcAddress;
+	memAreaDesc->size = data.in.size * ((data.in.memType % 2) ? 4 : 2); // betzw: set size in bytes for host (ugly version)
+	atomic_set(&memAreaDesc->count, 0);
+
+	if (lock_process(procPriv)) {
+		/* may be rather call lock_process_uninterruptible() */
+		CM_ENGINE_FreeMpcMemory(handle);
+		OSAL_Free(memAreaDesc);
+		return -ERESTARTSYS;
+	}
+	list_add(&memAreaDesc->list, &procPriv->memAreaDescList);
+	unlock_process(procPriv);
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_FreeMpcMemory(struct cm_process_priv *procPriv,
+			      CM_FreeMpcMemory_t __user *param)
+{
+	CM_FreeMpcMemory_t data;
+	struct list_head *cursor, *next;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* check that it is actually owned by the process */
+	data.out.error = CM_UNKNOWN_MEMORY_HANDLE;
+
+	if (lock_process(procPriv))
+		return -ERESTARTSYS;
+	list_for_each_safe(cursor, next, &procPriv->memAreaDescList){
+		struct memAreaDesc_t* curr;
+		curr = list_entry(cursor, struct memAreaDesc_t, list);
+		if (curr->handle == data.in.handle){
+			if (atomic_read(&curr->count) != 0) {
+				pr_err("%s: Memory area (phyAddr: %x, size: %d) "
+				       "still in use (count=%d)!\n", __func__,
+				       curr->physAddr, curr->size,
+				       atomic_read(&curr->count));
+				data.out.error = CM_INVALID_PARAMETER;
+			} else {
+				data.out.error =
+					CM_ENGINE_FreeMpcMemory(data.in.handle);
+				if (data.out.error == CM_OK) {
+					list_del(cursor);
+					OSAL_Free(curr);
+				}
+			}
+			break;
+		}
+	}
+	unlock_process(procPriv);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetMpcMemoryStatus(CM_GetMpcMemoryStatus_t __user *param)
+{
+	CM_GetMpcMemoryStatus_t data;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_GetMpcMemoryStatus(data.in.coreId,
+						      data.in.memType,
+						      &data.out.pStatus);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_StartComponent(struct cm_process_priv *procPriv,
+			       CM_StartComponent_t __user *param)
+{
+	CM_StartComponent_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_StartComponent(data.in.client,
+						  procPriv->pid);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_StopComponent(struct cm_process_priv *procPriv,
+			      CM_StopComponent_t __user *param)
+{
+	CM_StopComponent_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_StopComponent(data.in.client,
+						 procPriv->pid);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetMpcLoadCounter(CM_GetMpcLoadCounter_t __user *param)
+{
+	CM_GetMpcLoadCounter_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_getMpcLoadCounter(data.in.coreId,
+						      &data.out.pMpcLoadCounter);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentDescription(struct cm_process_priv *procPriv,
+					CM_GetComponentDescription_t __user *param)
+{
+	CM_GetComponentDescription_t data;
+	char templateName[MAX_TEMPLATE_NAME_LENGTH];
+	char localName[MAX_COMPONENT_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentDescription(data.in.component,
+							   templateName,
+							   data.in.templateNameLength,
+							   &data.out.coreId,
+							   localName,
+							   data.in.localNameLength,
+							   &data.out.priority);
+
+	/* Copy results back to userspace */
+	if (data.out.error == CM_OK) {
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.templateName, templateName, data.in.templateNameLength))
+			return -EFAULT;
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.localName, localName, data.in.localNameLength))
+			return -EFAULT;
+	}
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentListHeader(struct cm_process_priv *procPriv,
+				       CM_GetComponentListHeader_t __user *param)
+{
+	CM_GetComponentListHeader_t data;
+
+	data.out.error = CM_ENGINE_GetComponentListHeader(procPriv->pid,
+							  &data.out.headerComponent);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentListNext(struct cm_process_priv *procPriv,
+				     CM_GetComponentListNext_t __user *param)
+{
+	CM_GetComponentListNext_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentListNext(procPriv->pid,
+							data.in.prevComponent,
+							&data.out.nextComponent);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentRequiredInterfaceNumber(struct cm_process_priv *procPriv,
+						    CM_GetComponentRequiredInterfaceNumber_t __user *param)
+{
+	CM_GetComponentRequiredInterfaceNumber_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentRequiredInterfaceNumber(data.in.component,
+								       &data.out.numberRequiredInterfaces);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentRequiredInterface(struct cm_process_priv *procPriv,
+					      CM_GetComponentRequiredInterface_t __user *param)
+{
+	CM_GetComponentRequiredInterface_t data;
+	char itfName[MAX_INTERFACE_NAME_LENGTH];
+	char itfType[MAX_INTERFACE_TYPE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentRequiredInterface(data.in.component,
+								 data.in.index,
+								 itfName,
+								 data.in.itfNameLength,
+								 itfType,
+								 data.in.itfTypeLength,
+								 &data.out.requireState,
+								 &data.out.collectionSize);
+
+	/* Copy results back to userspace */
+	if (data.out.error == CM_OK) {
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.itfName, itfName, data.in.itfNameLength))
+			return -EFAULT;
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.itfType, itfType, data.in.itfTypeLength))
+			return -EFAULT;
+	}
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentRequiredInterfaceBinding(struct cm_process_priv *procPriv,
+						     CM_GetComponentRequiredInterfaceBinding_t __user *param)
+{
+	CM_GetComponentRequiredInterfaceBinding_t data;
+	char itfName[MAX_INTERFACE_NAME_LENGTH];
+	char serverItfName[MAX_INTERFACE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+	if ((data.out.error = copy_string_from_user(itfName,
+						    data.in.itfName,
+						    sizeof(itfName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_GetComponentRequiredInterfaceBinding(data.in.component,
+									itfName,
+									&data.out.server,
+									serverItfName,
+									data.in.serverItfNameLength);
+
+	/* Copy results back to userspace */
+	if (data.out.error != CM_OK)
+		goto out;
+
+	/* coverity[tainted_data : FALSE] */
+	if (copy_to_user(data.in.serverItfName, serverItfName, data.in.serverItfNameLength))
+		return -EFAULT;
+out:
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentProvidedInterfaceNumber(struct cm_process_priv *procPriv,
+						    CM_GetComponentProvidedInterfaceNumber_t __user *param)
+{
+	CM_GetComponentProvidedInterfaceNumber_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentProvidedInterfaceNumber(data.in.component,
+								       &data.out.numberProvidedInterfaces);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentProvidedInterface(struct cm_process_priv *procPriv,
+					      CM_GetComponentProvidedInterface_t __user *param)
+{
+	CM_GetComponentProvidedInterface_t data;
+	char itfName[MAX_INTERFACE_NAME_LENGTH];
+	char itfType[MAX_INTERFACE_TYPE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentProvidedInterface(data.in.component,
+								 data.in.index,
+								 itfName,
+								 data.in.itfNameLength,
+								 itfType,
+								 data.in.itfTypeLength,
+								 &data.out.collectionSize);
+
+	/* Copy results back to userspace */
+	if (data.out.error == CM_OK) {
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.itfName, itfName, data.in.itfNameLength))
+			return -EFAULT;
+		/* coverity[tainted_data : FALSE] */
+		if (copy_to_user(data.in.itfType, itfType, data.in.itfTypeLength))
+			return -EFAULT;
+	}
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentPropertyNumber(struct cm_process_priv *procPriv,
+					   CM_GetComponentPropertyNumber_t __user *param)
+{
+	CM_GetComponentPropertyNumber_t data;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentPropertyNumber(data.in.component,
+							      &data.out.numberProperties);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentPropertyName(struct cm_process_priv *procPriv,
+					 CM_GetComponentPropertyName_t __user *param)
+{
+	CM_GetComponentPropertyName_t data;
+	char propertyName[MAX_PROPERTY_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_GetComponentPropertyName(data.in.component,
+							    data.in.index,
+							    propertyName,
+							    data.in.propertyNameLength);
+
+	/* Copy results back to userspace */
+	/* coverity[tainted_data : FALSE] */
+	if ((data.out.error == CM_OK) &&
+	    copy_to_user(data.in.propertyName, propertyName, data.in.propertyNameLength))
+		return -EFAULT;
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetComponentPropertyValue(struct cm_process_priv *procPriv,
+					  CM_GetComponentPropertyValue_t __user *param)
+{
+	CM_GetComponentPropertyValue_t data;
+	char propertyName[MAX_PROPERTY_NAME_LENGTH];
+	char propertyValue[MAX_PROPERTY_VALUE_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(propertyName,
+						    data.in.propertyName,
+						    sizeof(propertyName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_GetComponentPropertyValue(data.in.component,
+							     propertyName,
+							     propertyValue,
+							     data.in.propertyValueLength);
+	/* Copy results back to userspace */
+	/* coverity[tainted_data : FALSE] */
+	if ((data.out.error == CM_OK) &&
+	    copy_to_user(data.in.propertyValue, propertyValue, data.in.propertyValueLength))
+		return -EFAULT;
+out:
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_ReadComponentAttribute(struct cm_process_priv *procPriv,
+				       CM_ReadComponentAttribute_t __user *param)
+{
+	CM_ReadComponentAttribute_t data;
+	char attrName[MAX_ATTRIBUTE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(attrName,
+						    data.in.attrName,
+						    sizeof(attrName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_ReadComponentAttribute(data.in.component,
+							  attrName,
+							  &data.out.value);
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+
+inline int cmld_WriteComponentAttribute(struct cm_process_priv *procPriv,
+                               CM_WriteComponentAttribute_t __user *param)
+{
+    CM_WriteComponentAttribute_t data;
+    char attrName[MAX_ATTRIBUTE_NAME_LENGTH];
+
+    /* Copy user input data in kernel space */
+    if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+        return -EFAULT;
+
+    if ((data.out.error = copy_string_from_user(attrName,
+                            data.in.attrName,
+                            sizeof(attrName))))
+        goto out;
+
+    data.out.error = CM_ENGINE_WriteComponentAttribute(data.in.component,
+                            attrName,
+                            data.in.value);
+out:
+    /* Copy results back to userspace */
+    if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+        return -EFAULT;
+    return 0;
+}
+
+
+inline int cmld_GetExecutiveEngineHandle(struct cm_process_priv *procPriv,
+					 CM_GetExecutiveEngineHandle_t __user *param)
+{
+	CM_GetExecutiveEngineHandle_t data;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_GetExecutiveEngineHandle(data.in.domainId,
+							     &data.out.executiveEngineHandle);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_SetMode(CM_SetMode_t __user *param)
+{
+	CM_SetMode_t data;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_SetMode(data.in.aCmdID, data.in.aParam);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_GetRequiredComponentFiles(struct cm_process_priv *procPriv,
+					  CM_GetRequiredComponentFiles_t __user *param)
+{
+	CM_GetRequiredComponentFiles_t data;
+	char components[4][MAX_INTERFACE_TYPE_NAME_LENGTH];
+	char requiredItfClientName[MAX_INTERFACE_NAME_LENGTH];
+	char providedItfServerName[MAX_INTERFACE_NAME_LENGTH];
+	char type[MAX_INTERFACE_TYPE_NAME_LENGTH];
+	unsigned int i;
+	int err;
+
+	/* Copy user input data in kernel space */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (data.in.requiredItfClientName &&
+	    (data.out.error = copy_string_from_user(requiredItfClientName,
+						    data.in.requiredItfClientName,
+						    sizeof(requiredItfClientName))))
+		goto out;
+
+	if (data.in.providedItfServerName &&
+	    (data.out.error = copy_string_from_user(providedItfServerName,
+						    data.in.providedItfServerName,
+						    sizeof(providedItfServerName))))
+		goto out;
+
+	data.out.error = CM_ENGINE_GetRequiredComponentFiles(data.in.action,
+							     data.in.client,
+							     requiredItfClientName,
+							     data.in.server,
+							     providedItfServerName,
+							     components,
+							     data.in.listSize,
+							     data.in.type ? type : NULL,
+							     &data.out.methodNumber);
+
+	if (data.out.error)
+		goto out;
+
+	if (data.in.fileList) {
+		/* Copy results back to userspace */
+		for (i=0; i<data.in.listSize; i++) {
+			err = copy_to_user(&((char*)data.in.fileList)[i*MAX_INTERFACE_TYPE_NAME_LENGTH], components[i], MAX_INTERFACE_TYPE_NAME_LENGTH);
+			if (err)
+				return -EFAULT;
+		}
+	}
+	if (data.in.type
+	    && copy_to_user(data.in.type, type, MAX_INTERFACE_TYPE_NAME_LENGTH))
+			return -EFAULT;
+out:
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_Migrate(CM_Migrate_t __user *param)
+{
+	CM_Migrate_t data;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	data.out.error = CM_ENGINE_Migrate(data.in.srcShared, data.in.src, data.in.dst);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_Unmigrate(CM_Unmigrate_t __user *param)
+{
+	CM_Unmigrate_t data;
+
+	data.out.error = CM_ENGINE_Unmigrate();
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+int cmld_SetupRelinkArea(struct cm_process_priv *procPriv,
+			 CM_SetupRelinkArea_t __user *param)
+{
+	CM_SetupRelinkArea_t data;
+	struct list_head *cursor, *next;
+	struct memAreaDesc_t *entry = NULL;
+
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+
+	/* check that it is actually owned by the process */
+	data.out.error = CM_UNKNOWN_MEMORY_HANDLE;
+
+	if (lock_process(procPriv))
+		return -ERESTARTSYS;
+	list_for_each_safe(cursor, next, &procPriv->memAreaDescList){
+		entry = list_entry(cursor, struct memAreaDesc_t, list);
+		if (entry->handle == data.in.mem_handle)
+			break;
+	}
+	unlock_process(procPriv);
+
+	if ((entry == NULL) || (entry->handle != data.in.mem_handle))
+		goto out;
+
+	if (entry->size < data.in.segments * data.in.segmentsize)
+	{
+		data.out.error = CM_INVALID_PARAMETER;
+		goto out;
+	}
+
+	data.out.error = cmdma_setup_relink_area(
+		entry->physAddr,
+		data.in.peripheral_addr,
+		data.in.segments,
+		data.in.segmentsize,
+		data.in.LOS,
+		data.in.type);
+out:
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+
+	return 0;
+}
+
+
+inline int cmld_PushComponent(CM_PushComponent_t __user *param)
+{
+	CM_PushComponent_t data;
+	char name[MAX_INTERFACE_TYPE_NAME_LENGTH];
+	void *dataFile = NULL;
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(name,
+						    data.in.name,
+						    sizeof(name))))
+		goto out;
+
+	if (data.in.data != NULL) {
+		dataFile = OSAL_Alloc(data.in.size);
+		if (dataFile == NULL) {
+			data.out.error = CM_NO_MORE_MEMORY;
+			goto out;
+		}
+		/* coverity[tainted_data : FALSE] */
+		if (copy_from_user(dataFile, data.in.data, data.in.size))
+			data.out.error = CM_INVALID_PARAMETER;
+		else
+			data.out.error = CM_ENGINE_PushComponent(name, dataFile,
+								 data.in.size);
+		OSAL_Free(dataFile);
+	}
+
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_ReleaseComponent(CM_ReleaseComponent_t __user *param)
+{
+	CM_ReleaseComponent_t data;
+	char name[MAX_INTERFACE_TYPE_NAME_LENGTH];
+
+	/* Copy user input data in kernel space */
+	/* coverity[tainted_data_argument : FALSE] */
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if ((data.out.error = copy_string_from_user(name,
+						    data.in.name,
+						    sizeof(name))))
+		goto out;
+
+	/* coverity[tainted_data : FALSE] */
+	data.out.error = CM_ENGINE_ReleaseComponent(name);
+
+out:
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+inline int cmld_PrivGetMPCMemoryDesc(struct cm_process_priv *procPriv, CM_PrivGetMPCMemoryDesc_t __user *param)
+{
+	CM_PrivGetMPCMemoryDesc_t data;
+	struct list_head* cursor;
+
+	if (copy_from_user(&data.in, &param->in, sizeof(data.in)))
+		return -EFAULT;
+
+	if (lock_process(procPriv))
+		return -ERESTARTSYS;
+	/* Scan the memory descriptors list looking for the requested handle */
+	data.out.error = CM_UNKNOWN_MEMORY_HANDLE;
+	list_for_each(cursor, &procPriv->memAreaDescList) {
+		struct memAreaDesc_t* curr;
+		curr = list_entry(cursor, struct memAreaDesc_t, list);
+		if (curr->handle == data.in.handle) {
+			data.out.size = curr->size;
+			data.out.physAddr = curr->physAddr;
+			data.out.kernelLogicalAddr = curr->kernelLogicalAddr;
+			data.out.userLogicalAddr = curr->userLogicalAddr;
+			data.out.mpcPhysAddr = curr->mpcPhysAddr;
+			data.out.error = CM_OK;
+			break;
+		}
+	}
+	unlock_process(procPriv);
+
+	/* Copy results back to userspace */
+	if (copy_to_user(&param->out, &data.out, sizeof(data.out)))
+		return -EFAULT;
+	return 0;
+}
+
+int cmld_PrivReserveMemory(struct cm_process_priv *procPriv, unsigned int physAddr)
+{
+	struct list_head* cursor;
+	struct memAreaDesc_t* curr;
+	int err = -ENXIO;
+
+	if (lock_process(procPriv))
+		return -ERESTARTSYS;
+	list_for_each(cursor, &procPriv->memAreaDescList) {
+		curr = list_entry(cursor, struct memAreaDesc_t, list);
+		if (curr->physAddr == physAddr) {
+			/* Mark this memory area reserved for a mapping for this thread ID */
+			/* It must not be already reserved but this should not happen */
+			if (curr->tid) {
+				pr_err("%s: thread %d can't reseveved memory %x already "
+				       "reserved for %d\n",
+				       __func__, current->pid, physAddr, curr->tid);
+				err = -EBUSY;
+			} else {
+				curr->tid = current->pid;
+				err = 0;
+			}
+			break;
+		}
+	}
+	unlock_process(procPriv);
+	return err;
+}
diff --git a/drivers/staging/nmf-cm/cmioctl.h b/drivers/staging/nmf-cm/cmioctl.h
new file mode 100644
index 00000000000..eafeb8acff1
--- /dev/null
+++ b/drivers/staging/nmf-cm/cmioctl.h
@@ -0,0 +1,616 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+
+#ifndef __CMIOCTL_H
+#define __CMIOCTL_H
+
+#ifndef __KERNEL__
+#define BITS_PER_BYTE 8
+#endif
+
+#include <cm/engine/component/inc/component_type.h>
+#include <cm/engine/communication/inc/communication_type.h>
+#include <cm/engine/configuration/inc/configuration_type.h>
+#include <cm/engine/memory/inc/domain_type.h>
+#include <cm/engine/memory/inc/memory_type.h>
+#include <cm/engine/perfmeter/inc/perfmeter_type.h>
+#include <cm/engine/repository_mgt/inc/repository_type.h>
+
+#define DEBUGFS_ROOT "nmf-cm"
+#define DEBUGFS_DUMP_FLAG (1 << (sizeof(t_panic_source)*BITS_PER_BYTE - 1))
+
+enum cmdma_type {
+    CMDMA_MEM_2_PER,
+    CMDMA_PER_2_MEM
+};
+
+#define CMLD_DEV_NAME     \
+	{ "cm_control",   \
+	  "cm_channel",	  \
+	  "cm_sia_trace", \
+	  "cm_sva_trace", \
+	}
+
+/*
+ * The following structures are used to exchange CM_SYSCALL parameters with
+ * the driver. There is one structure per ioctl command, ie per CM_SYSCAL.
+ * Each of them contains:
+ *  - One set of fields placed in a struture 'in' which are all input
+ *    parameters of the syscall (parameters that kernel side must retrieve
+ *    from user space)
+ *  - One set of fields placed in a struture 'out' which contains all output
+ *    parameters of the syscall plus the error code.
+ *
+ * NOTE: all pointers to (user) buffer are always placed in struct 'in', including
+ * buffers used as output parameters; because the pointer itself is considered as
+ * an input parameter, as it is directly accessed from kernel space.
+ */
+typedef struct{
+	struct {
+		const char * templateName;
+		t_cm_domain_id domainId;
+		t_nmf_ee_priority priority;
+		const char * localName;
+		const char *dataFile;
+		t_uint32 dataFileSize;
+	} in;
+	struct {
+		t_cm_instance_handle component; /** < Output parameter */
+		t_cm_error error;
+	} out;
+} CM_InstantiateComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_bf_host2mpc_handle host2mpcId;
+		t_event_params_handle h;
+		t_uint32 size;
+		t_uint32 methodIndex;
+	} in;
+	struct {
+		t_cm_error error;
+	} out;
+} CM_PushEventWithSize_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle server;
+		const char * providedItfServerName;
+		t_uint32 fifosize;
+		t_cm_mpc_memory_type eventMemType;
+		const char *dataFileSkeleton;
+		t_uint32 dataFileSkeletonSize;
+	} in;
+	struct {
+		t_cm_bf_host2mpc_handle host2mpcId;  /** < Output parameter */
+		t_cm_error error;
+	} out;
+} CM_BindComponentFromCMCore_t;
+
+typedef struct {
+	struct {
+		t_cm_bf_host2mpc_handle host2mpcId;
+	} in;
+	struct {
+		t_cm_error error;
+	} out;
+} CM_UnbindComponentFromCMCore_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+		t_uint32 fifosize;
+		t_nmf_mpc2host_handle upLayerThis;
+		const char *dataFileStub;
+		t_uint32 dataFileStubSize;
+	} in;
+	struct {
+		t_cm_bf_mpc2host_handle mpc2hostId; /** < Output parameter */
+		t_cm_error error;
+	} out;
+} CM_BindComponentToCMCore_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+	} in;
+	struct {
+		t_nmf_mpc2host_handle upLayerThis;  /** < Output parameter */
+		t_cm_error error;
+	} out;
+} CM_UnbindComponentToCMCore_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+	} in;
+	struct {
+		t_cm_error error;  /** < Output parameter */
+	} out;  /** < Output parameter */
+} CM_DestroyComponent_t;
+
+typedef struct {
+	struct {
+		const t_cm_domain_memory  *domain;
+		t_nmf_client_id            client;
+	} in;
+	struct {
+		t_cm_domain_id handle; /** < Out parameter */
+		t_cm_error error;  /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_CreateMemoryDomain_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id parentId;
+		const t_cm_domain_memory *domain;
+	} in;
+	struct {
+		t_cm_domain_id handle; /** < Out parameter */
+		t_cm_error error;  /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_CreateMemoryDomainScratch_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id domainId;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_DestroyMemoryDomain_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id domainId; /** < In parameter */
+	} in;
+	struct {
+		t_nmf_core_id coreId; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_GetDomainCoreId_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id domainId;
+		t_cm_mpc_memory_type memType;
+		t_cm_size size;
+		t_cm_memory_alignment memAlignment;
+	} in;
+	struct {
+		t_cm_memory_handle pHandle; /** < Output parameter */
+		t_cm_error error;
+	} out;  /** < Output parameter */
+} CM_AllocMpcMemory_t;
+
+typedef struct{
+	struct {
+		t_cm_memory_handle handle;
+	} in;
+	struct {
+		t_cm_error error;
+	} out;  /** < Output parameter */
+} CM_FreeMpcMemory_t;
+
+typedef struct {
+	struct {
+		t_cm_memory_handle handle;
+	} in;
+	struct {
+		t_uint32 size; /** < Out parameter */
+		t_uint32 physAddr; /** < Out parameter */
+		t_uint32 kernelLogicalAddr; /** < Out parameter */
+		t_uint32 userLogicalAddr; /** < Out parameter */
+		t_uint32 mpcPhysAddr; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;  /** < Output parameter */
+} CM_PrivGetMPCMemoryDesc_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+		t_cm_instance_handle server;
+		const char *providedItfServerName;
+		t_uint32 fifosize;
+		t_cm_mpc_memory_type eventMemType;
+		const char *dataFileSkeletonOrEvent;
+		t_uint32 dataFileSkeletonOrEventSize;
+		const char *dataFileStub;
+		t_uint32 dataFileStubSize;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_BindComponentAsynchronous_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char* requiredItfClientName;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_UnbindComponentAsynchronous_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+		t_cm_instance_handle server;
+		const char *providedItfServerName;
+		t_bool traced;
+		const char *dataFileTrace;
+		t_uint32 dataFileTraceSize;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_BindComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char* requiredItfClientName;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_UnbindComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+		const char* requiredItfClientName;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_BindComponentToVoid_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_StartComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle client;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_StopComponent_t;
+
+typedef struct {
+	struct {
+		t_nmf_core_id coreId;
+	} in;
+	struct {
+		t_cm_mpc_load_counter pMpcLoadCounter; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetMpcLoadCounter_t;
+
+typedef struct {
+	struct {
+		t_nmf_core_id coreId;
+		t_cm_mpc_memory_type memType;
+	} in;
+	struct {
+		t_cm_allocator_status pStatus; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_GetMpcMemoryStatus_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		t_uint32 templateNameLength;
+		t_uint32 localNameLength;
+		char *templateName; /** < Out parameter */
+		char *localName; /** < Out parameter */
+	} in;
+	struct {
+		t_nmf_core_id coreId; /** < Out parameter */
+		t_nmf_ee_priority priority; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_GetComponentDescription_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle headerComponent; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out; /** < Out parameter */
+} CM_GetComponentListHeader_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle prevComponent;
+	} in;
+	struct {
+		t_cm_instance_handle nextComponent; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentListNext_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+	} in;
+	struct {
+		t_uint8 numberRequiredInterfaces; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentRequiredInterfaceNumber_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		t_uint8 index;
+		t_uint32 itfNameLength;
+		t_uint32 itfTypeLength;
+		char *itfName; /** < Out parameter */
+		char *itfType; /** < Out parameter */
+	} in;
+	struct {
+		t_cm_require_state requireState; /** < Out parameter */
+		t_sint16 collectionSize; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentRequiredInterface_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		const char *itfName;
+		t_uint32 serverItfNameLength;
+		char *serverItfName; /** < Out parameter */
+	} in;
+	struct {
+		t_cm_instance_handle server; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentRequiredInterfaceBinding_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+	} in;
+	struct {
+		t_uint8 numberProvidedInterfaces; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentProvidedInterfaceNumber_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		t_uint8 index;
+		t_uint32 itfNameLength;
+		t_uint32 itfTypeLength;
+		char *itfName; /** < Out parameter */
+		char *itfType; /** < Out parameter */
+	} in;
+	struct {
+		t_sint16 collectionSize; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentProvidedInterface_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+	} in;
+	struct {
+		t_uint8 numberProperties; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentPropertyNumber_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		const char *attrName;
+		t_uint8 index;
+		t_uint32 propertyNameLength;
+		char *propertyName; /** < Out parameter */
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentPropertyName_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		const char *propertyName;
+		t_uint32 propertyValueLength;
+		char *propertyValue; /** < Out parameter */
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetComponentPropertyValue_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		const char *attrName;
+	} in;
+	struct {
+		t_uint32 value; /** < Out parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_ReadComponentAttribute_t;
+
+typedef struct {
+	struct {
+		t_cm_instance_handle component;
+		const char *attrName;
+		t_uint32 value; /** < In parameter */
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_WriteComponentAttribute_t;
+
+typedef struct{
+	struct {
+		t_cm_domain_id domainId;
+	} in;
+	struct {
+		t_cm_instance_handle executiveEngineHandle;
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetExecutiveEngineHandle_t;
+
+typedef struct {
+	struct {
+		t_cm_cmd_id aCmdID;
+		t_sint32 aParam;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_SetMode_t;
+
+typedef struct {
+	struct {
+		t_action_to_do action;
+		t_cm_instance_handle client;
+		const char *requiredItfClientName;
+		t_cm_instance_handle server;
+		const char *providedItfServerName;
+		char **fileList;
+		unsigned int listSize;
+		char *type;
+	} in;
+	struct {
+		t_uint32 methodNumber; /** < Output parameter */
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_GetRequiredComponentFiles_t;
+
+typedef struct {
+	struct {
+		const char *name;
+		const void *data;
+		t_cm_size size;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_PushComponent_t;
+
+typedef struct {
+	struct {
+		const char *name;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_ReleaseComponent_t;
+
+typedef struct {
+	struct {
+		t_cm_domain_id srcShared;
+		t_cm_domain_id src;
+		t_cm_domain_id dst;
+	} in;
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_Migrate_t;
+
+typedef struct {
+	struct {
+		t_cm_error error; /** < Out parameter */
+	} out;
+} CM_Unmigrate_t;
+
+typedef struct{
+    struct {
+        t_cm_memory_handle mem_handle;
+        unsigned int peripheral_addr;
+        unsigned int segments;
+        unsigned int segmentsize;
+        unsigned int LOS;
+        enum cmdma_type type;
+    } in;
+    struct {
+        t_cm_error error;
+    } out;
+} CM_SetupRelinkArea_t;
+
+#define CM_PUSHEVENTWITHSIZE 			_IOWR('c', 0, CM_PushEventWithSize_t)
+#define CM_GETVERSION 				_IOR('c', 1, t_uint32)
+#define CM_INSTANTIATECOMPONENT 		_IOWR('c', 2, CM_InstantiateComponent_t)
+#define CM_BINDCOMPONENTFROMCMCORE 		_IOWR('c', 3, CM_BindComponentFromCMCore_t)
+#define CM_UNBINDCOMPONENTFROMCMCORE 		_IOWR('c', 4, CM_UnbindComponentFromCMCore_t)
+#define CM_BINDCOMPONENTTOCMCORE 		_IOWR('c', 5, CM_BindComponentToCMCore_t)
+#define CM_UNBINDCOMPONENTTOCMCORE 		_IOWR('c', 6, CM_UnbindComponentToCMCore_t)
+#define CM_DESTROYCOMPONENT 			_IOWR('c', 7, CM_DestroyComponent_t)
+#define CM_CREATEMEMORYDOMAIN 			_IOWR('c', 8, CM_CreateMemoryDomain_t)
+#define CM_CREATEMEMORYDOMAINSCRATCH 		_IOWR('c', 9, CM_CreateMemoryDomainScratch_t)
+#define CM_DESTROYMEMORYDOMAIN 			_IOWR('c', 10, CM_DestroyMemoryDomain_t)
+#define CM_GETDOMAINCOREID 			_IOWR('c', 11, CM_GetDomainCoreId_t)
+#define CM_ALLOCMPCMEMORY 			_IOWR('c', 12, CM_AllocMpcMemory_t)
+#define CM_FREEMPCMEMORY 			_IOWR('c', 13, CM_FreeMpcMemory_t)
+#define CM_BINDCOMPONENTASYNCHRONOUS 		_IOWR('c', 14, CM_BindComponentAsynchronous_t)
+#define CM_UNBINDCOMPONENTASYNCHRONOUS 		_IOWR('c', 15, CM_UnbindComponentAsynchronous_t)
+#define CM_BINDCOMPONENT 			_IOWR('c', 16, CM_BindComponent_t)
+#define CM_UNBINDCOMPONENT 			_IOWR('c', 17, CM_UnbindComponent_t)
+#define CM_BINDCOMPONENTTOVOID 			_IOWR('c', 18, CM_BindComponentToVoid_t)
+#define CM_STARTCOMPONENT 			_IOWR('c', 19, CM_StartComponent_t)
+#define CM_STOPCOMPONENT 			_IOWR('c', 20, CM_StopComponent_t)
+#define CM_GETMPCLOADCOUNTER 			_IOWR('c', 21, CM_GetMpcLoadCounter_t)
+#define CM_GETMPCMEMORYSTATUS 			_IOWR('c', 22, CM_GetMpcMemoryStatus_t)
+#define CM_GETCOMPONENTDESCRIPTION 		_IOWR('c', 23, CM_GetComponentDescription_t)
+#define CM_GETCOMPONENTLISTHEADER 		_IOWR('c', 24, CM_GetComponentListHeader_t)
+#define CM_GETCOMPONENTLISTNEXT 		_IOWR('c', 25, CM_GetComponentListNext_t)
+#define CM_GETCOMPONENTREQUIREDINTERFACENUMBER  _IOWR('c', 26, CM_GetComponentRequiredInterfaceNumber_t)
+#define CM_GETCOMPONENTREQUIREDINTERFACE        _IOWR('c', 27, CM_GetComponentRequiredInterface_t)
+#define CM_GETCOMPONENTREQUIREDINTERFACEBINDING _IOWR('c', 28, CM_GetComponentRequiredInterfaceBinding_t)
+#define CM_GETCOMPONENTPROVIDEDINTERFACENUMBER 	_IOWR('c', 29, CM_GetComponentProvidedInterfaceNumber_t)
+#define CM_GETCOMPONENTPROVIDEDINTERFACE 	_IOWR('c', 30, CM_GetComponentProvidedInterface_t)
+#define CM_GETCOMPONENTPROPERTYNUMBER 		_IOWR('c', 31, CM_GetComponentPropertyNumber_t)
+#define CM_GETCOMPONENTPROPERTYNAME 		_IOWR('c', 32, CM_GetComponentPropertyName_t)
+#define CM_GETCOMPONENTPROPERTYVALUE 		_IOWR('c', 33, CM_GetComponentPropertyValue_t)
+#define CM_READCOMPONENTATTRIBUTE 		_IOWR('c', 34, CM_ReadComponentAttribute_t)
+#define CM_WRITECOMPONENTATTRIBUTE 		_IOWR('c', 44, CM_WriteComponentAttribute_t)
+#define CM_GETEXECUTIVEENGINEHANDLE 		_IOWR('c', 35, CM_GetExecutiveEngineHandle_t)
+#define CM_SETMODE 				_IOWR('c', 36, CM_SetMode_t)
+#define CM_GETREQUIREDCOMPONENTFILES 		_IOWR('c', 37, CM_GetRequiredComponentFiles_t)
+#define CM_PUSHCOMPONENT 			_IOWR('c', 38, CM_PushComponent_t)
+#define CM_FLUSHCHANNEL 			_IO('c', 39)
+#define CM_MIGRATE 				_IOWR('c', 40, CM_Migrate_t)
+#define CM_UNMIGRATE 				_IOR('c', 41, CM_Unmigrate_t)
+#define CM_RELEASECOMPONENT 			_IOWR('c', 42, CM_ReleaseComponent_t)
+#define CM_SETUPRELINKAREA             		_IOWR('c', 43, CM_SetupRelinkArea_t)
+
+#define CM_PRIVGETMPCMEMORYDESC			_IOWR('c', 100, CM_PrivGetMPCMemoryDesc_t)
+#define CM_PRIVRESERVEMEMORY 			_IOW('c', 101, unsigned int)
+#define CM_PRIV_GETBOARDVERSION 		_IOR('c', 102, unsigned int)
+#define CM_PRIV_ISCOMPONENTCACHEEMPTY 		_IO('c', 103)
+#define CM_PRIV_DEBUGFS_READY 			_IO('c', 104)
+#define CM_PRIV_DEBUGFS_WAIT_DUMP 		_IO('c', 105)
+#define CM_PRIV_DEBUGFS_DUMP_DONE 		_IO('c', 106)
+
+enum board_version {
+	U8500_V2
+};
+#endif
diff --git a/drivers/staging/nmf-cm/cmld.c b/drivers/staging/nmf-cm/cmld.c
new file mode 100644
index 00000000000..8d3cbc037be
--- /dev/null
+++ b/drivers/staging/nmf-cm/cmld.c
@@ -0,0 +1,1413 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file cmld.c
+ *
+ * Nomadik Multiprocessing Framework Linux Driver
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/cdev.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+#include <cm/inc/cm_def.h>
+#include <cm/engine/api/cm_engine.h>
+#include <cm/engine/api/control/irq_engine.h>
+
+#include "osal-kernel.h"
+#include "cmld.h"
+#include "cmioctl.h"
+#include "cm_debug.h"
+#include "cm_service.h"
+#include "cm_dma.h"
+
+#define CMDRIVER_PATCH_VERSION 122
+#define O_FLUSH 0x1000000
+
+static int cmld_major;
+static struct cdev cmld_cdev;
+static struct class cmld_class = {
+	.name = "cm",
+	.owner = THIS_MODULE,
+};
+const char *cmld_devname[] = CMLD_DEV_NAME;
+static struct device *cmld_dev[ARRAY_SIZE(cmld_devname)];
+
+/* List of per process structure (struct cm_process_priv list) */
+LIST_HEAD(process_list);
+static DEFINE_MUTEX(process_lock); /* lock used to protect previous list */
+/* List of per channel structure (struct cm_channel_priv list).
+   A channel == One file descriptor */
+LIST_HEAD(channel_list);
+static DEFINE_MUTEX(channel_lock); /* lock used to protect previous list */
+
+#ifdef CONFIG_DEBUG_FS
+/* Debugfs support */
+bool cmld_user_has_debugfs = false;
+pid_t cmld_dump_ongoing = 0;
+module_param(cmld_dump_ongoing, uint, S_IWUSR|S_IRUGO);
+static DECLARE_WAIT_QUEUE_HEAD(dump_waitq);
+#endif
+
+static inline struct cm_process_priv *getProcessPriv(void)
+{
+	struct list_head* head;
+	struct cm_process_priv *entry;
+
+	mutex_lock(&process_lock);
+
+	/* Look for an entry for the calling process */
+	list_for_each(head, &process_list) {
+		entry = list_entry(head, struct cm_process_priv, entry);
+		if (entry->pid == current->tgid) {
+			kref_get(&entry->ref);
+			goto out;
+		}
+	}
+	mutex_unlock(&process_lock);
+
+	/* Allocate, init and register a new one otherwise */
+	entry = OSAL_Alloc(sizeof(*entry));
+	if (entry == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	/* init host2mpcLock */
+	mutex_init(&entry->host2mpcLock);
+
+	INIT_LIST_HEAD(&entry->memAreaDescList);
+	kref_init(&entry->ref);
+	mutex_init(&entry->mutex);
+
+	entry->pid = current->tgid;
+	mutex_lock(&process_lock);
+	list_add(&entry->entry, &process_list);
+	cm_debug_proc_init(entry);
+out:
+	mutex_unlock(&process_lock);
+	return entry;
+}
+
+/* Free all messages */
+static inline void freeMessages(struct cm_channel_priv* channelPriv)
+{
+	struct osal_msg *this, *next;
+	int warn = 0;
+
+	spin_lock_bh(&channelPriv->bh_lock);
+	plist_for_each_entry_safe(this, next, &channelPriv->messageQueue, msg_entry) {
+		plist_del(&this->msg_entry, &channelPriv->messageQueue);
+		kfree(this);
+		warn = 1;
+	}
+	spin_unlock_bh(&channelPriv->bh_lock);
+	if (warn)
+		pr_err("[CM - PID=%d]: Some remaining"
+		       " message(s) freed\n", current->tgid);
+}
+
+/* Free all pending memory areas and relative descriptors  */
+static inline void freeMemHandles(struct cm_process_priv* processPriv)
+{
+	struct list_head* head, *next;
+	int warn = 0;
+			
+	list_for_each_safe(head, next, &processPriv->memAreaDescList) {
+		struct memAreaDesc_t* curr;
+		int err;
+		curr = list_entry(head, struct memAreaDesc_t, list);
+		err=CM_ENGINE_FreeMpcMemory(curr->handle);
+		if (err)
+			pr_err("[CM - PID=%d]: Error (%d) freeing remaining memory area "
+			       "handle\n", current->tgid, err);
+		list_del(head);
+		OSAL_Free(curr);
+		warn = 1;
+	}
+	if (warn) {
+		pr_err("[CM - PID=%d]: Some remaining memory area "
+		       "handle(s) freed\n", current->tgid);
+		warn = 0;
+	}
+}
+
+/* Free any skeleton, called when freeing the process entry */
+static inline void freeSkelList(struct list_head* skelList)
+{
+	struct list_head* head, *next;
+	int warn = 0;
+
+	/* No lock held, we know that we are the only and the last user
+	   of the list */
+	list_for_each_safe(head, next, skelList) {
+		t_skelwrapper* curr;
+		curr = list_entry(head, t_skelwrapper, entry);
+		list_del(head);
+		OSAL_Free(curr);
+		warn = 1;
+	}
+	if (warn)
+		pr_err("[CM - PID=%d]: Some remaining skeleton "
+		       "wrapper(s) freed\n", current->tgid);
+}
+
+/* Free any remaining channels belonging to this process */
+/* Called _only_ when freeing the process entry, once the network constructed by
+   this process has been destroyed.
+   See cmld_release() to see why there can be some remaining non-freed channels */
+static inline void freeChannels(struct cm_process_priv* processPriv)
+{
+	struct list_head* head, *next;
+	int warn = 0;
+
+	mutex_lock(&channel_lock);
+	list_for_each_safe(head, next, &channel_list) {
+		struct cm_channel_priv *channelPriv;
+		channelPriv = list_entry(head, struct cm_channel_priv, entry);
+		/* Only channels belonging to this process are concerned */
+		if (channelPriv->proc == processPriv) {
+			tasklet_disable(&cmld_service_tasklet);
+			list_del(&channelPriv->entry);
+			tasklet_enable(&cmld_service_tasklet);
+
+			/* Free all remaining messages if any
+			   (normally none, but double check) */
+			freeMessages(channelPriv);
+
+			/* Free any pending skeleton wrapper */
+			/* Here it's safe, we know that all bindings have been undone */
+			freeSkelList(&channelPriv->skelList);
+
+			/* Free the per-channel descriptor */
+			OSAL_Free(channelPriv);
+		}
+		warn = 1;
+	}
+	mutex_unlock(&channel_lock);
+
+	if (warn)
+		pr_err("[CM - PID=%d]: Some remaining channel entries "
+		       "freed\n", current->tgid);
+}
+
+/* Free the process priv structure and all related stuff */
+/* Called only when the last ref to this structure is released */
+static void freeProcessPriv(struct kref *ref)
+{
+	struct cm_process_priv *entry = container_of(ref, struct cm_process_priv, ref);
+	t_nmf_error err;
+
+	mutex_lock(&process_lock);
+	list_del(&entry->entry);
+	mutex_unlock(&process_lock);
+
+	/* Destroy all remaining components */
+	err=CM_ENGINE_FlushComponents(entry->pid);
+	if (err != NMF_OK)
+		pr_err("[CM - PID=%d]: Error while flushing some remaining"
+		       " components: error=%d\n", current->tgid, err);
+
+	freeChannels(entry);
+
+	/* Free any pending memory areas and relative descriptors */
+	freeMemHandles(entry);
+
+	/* Destroy all remaining domains */
+	err=CM_ENGINE_FlushMemoryDomains(entry->pid);
+	if (err != NMF_OK)
+		pr_err("[CM - PID=%d]: Error while flushing some remaining"
+		       " domains: error=%d\n", current->tgid, err);
+
+#ifdef CONFIG_DEBUG_FS
+	debugfs_remove_recursive(entry->dir);
+	if (cmld_dump_ongoing == entry->pid) {
+		cmld_dump_ongoing = 0;
+		wake_up(&dump_waitq);
+	}
+#endif
+
+	/* Free the per-process descriptor */
+	OSAL_Free(entry);
+}
+
+/** Reads Component Manager messages destinated to this process.
+ * The message is composed by three fields:
+ * 1) mpc2host handle (distinguishes interfaces)
+ * 2) methodIndex (distinguishes interface's methods)
+ * 3) Variable length parameters (method's parameters values)
+ *
+ * \note cfr GetEvent()
+ * \return POSIX error code
+ */
+static ssize_t cmld_channel_read(struct file *file, char *buf, size_t count, loff_t *ppos)
+{
+	int err = 0;
+	struct cm_channel_priv* channelPriv = file->private_data;
+	int msgSize = 0;
+	struct plist_head* messageQueue;
+	struct osal_msg* msg;
+	t_os_message *os_msg = (t_os_message *)buf;
+	int block = !(file->f_flags & O_NONBLOCK);
+
+	messageQueue = &channelPriv->messageQueue;
+ 
+	if (mutex_lock_killable(&channelPriv->msgQueueLock))
+		return -ERESTARTSYS;
+
+wait:
+	while (plist_head_empty(messageQueue)) {
+		mutex_unlock(&channelPriv->msgQueueLock);
+		if (block == 0)
+			return -EAGAIN;
+		/* Wait until there is a message to ferry up */
+		if (wait_event_interruptible(channelPriv->waitq, ((!plist_head_empty(messageQueue)) || (file->f_flags & O_FLUSH))))
+			return -ERESTARTSYS;
+		if (file->f_flags & O_FLUSH) {
+			file->f_flags &= ~O_FLUSH;
+			return 0;
+		}
+		if (mutex_lock_killable(&channelPriv->msgQueueLock))
+			return -ERESTARTSYS;
+	}
+
+	/* Pick up the first message from the queue, making sure that the
+	 * hwsem tasklet does not wreak havoc the queue in the meantime
+	 */
+	spin_lock_bh(&channelPriv->bh_lock);
+	msg = plist_first_entry(messageQueue, struct osal_msg, msg_entry);
+	plist_del(&msg->msg_entry, messageQueue);
+	spin_unlock_bh(&channelPriv->bh_lock);
+
+	switch (msg->msg_type) {
+	case MSG_INTERFACE: {
+
+		/* Check if enough space is available */
+		msgSize = sizeof(msg->msg_type) + msg->d.itf.ptrSize + sizeof(os_msg->data.itf) - sizeof(os_msg->data.itf.params) ;
+		if (msgSize > count) {
+			mutex_unlock(&channelPriv->msgQueueLock);
+			pr_err("CM: message size bigger than buffer size silently ignored!\n");
+			err = -EMSGSIZE;
+			goto out;
+		}
+
+		/* Copy to user message type */
+		err = put_user(msg->msg_type, &os_msg->type);
+		if (err) goto ack_evt;
+	
+		/* Copy to user the t_nmf_mpc2host_handle */
+		err = put_user(msg->d.itf.skelwrap->upperLayerThis, &os_msg->data.itf.THIS);
+		if (err) goto ack_evt;
+
+		/* The methodIndex  */
+		err = put_user(msg->d.itf.methodIdx, &os_msg->data.itf.methodIndex);
+		if (err) goto ack_evt;
+
+		/* And the parameters */
+		err = copy_to_user(os_msg->data.itf.params, msg->d.itf.anyPtr, msg->d.itf.ptrSize);
+	
+		ack_evt:
+		/* This call is void */
+		/* Note: that we cannot release the lock before having called this function
+		   as acknowledgements MUST be executed in the same order as their
+		   respective messages have arrived! */
+		CM_ENGINE_AcknowledgeEvent(msg->d.itf.skelwrap->mpc2hostId);
+
+		mutex_unlock(&channelPriv->msgQueueLock);
+		break;
+	}
+	case MSG_SERVICE: {
+		mutex_unlock(&channelPriv->msgQueueLock);
+		msgSize = sizeof(msg->msg_type) + sizeof(msg->d.srv.srvType)
+			+ sizeof(msg->d.srv.srvData);
+		if (count < msgSize) {
+			pr_err("CM: service message size bigger than buffer size - silently ignored!\n");
+			err = -EMSGSIZE;
+		}
+
+		/* Copy to user message type */
+		err = put_user(msg->msg_type, &os_msg->type);
+		if (err) goto out;
+		err = copy_to_user(&os_msg->data.srv, &msg->d.srv,
+				   sizeof(msg->d.srv.srvType) + sizeof(msg->d.srv.srvData));
+		break;
+	}
+	default:
+		mutex_unlock(&channelPriv->msgQueueLock);
+		pr_err("CM: invalid message type %d discarded\n", msg->msg_type);
+		goto wait;
+	}
+out:
+	/* Destroy the message */
+	kfree(msg);
+
+	return err ? err : msgSize;
+}
+
+/** Part of driver's release method. (ie userspace close())
+ * It wakes up all waiter.
+ *
+ * \return POSIX error code
+ */
+static int cmld_channel_flush(struct file *file, fl_owner_t id)
+{
+	struct cm_channel_priv* channelPriv = file->private_data;
+	file->f_flags |= O_FLUSH;
+	wake_up(&channelPriv->waitq);
+	return 0;
+}
+
+static long cmld_channel_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct cm_channel_priv *channelPriv = file->private_data;
+#ifdef CONFIG_DEBUG_FS
+	if (wait_event_interruptible(dump_waitq, (!cmld_dump_ongoing)))
+		return -ERESTARTSYS;
+#endif
+
+	switch(cmd) {
+		/*
+		 * All channel CM SYSCALL
+		 */
+	case CM_BINDCOMPONENTTOCMCORE:
+		return cmld_BindComponentToCMCore(channelPriv, (CM_BindComponentToCMCore_t *)arg);
+	case CM_FLUSHCHANNEL:
+		return cmld_channel_flush(file, 0);
+	default:
+		pr_err("CM(%s): unsupported command %i\n", __func__, cmd);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static long cmld_control_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct cm_process_priv* procPriv = file->private_data;
+#ifdef CONFIG_DEBUG_FS
+	if (cmd == CM_PRIV_DEBUGFS_DUMP_DONE) {
+		cmld_dump_ongoing = 0;
+		wake_up(&dump_waitq);
+		return 0;
+	} else if (wait_event_interruptible(dump_waitq, (!cmld_dump_ongoing)))
+		return -ERESTARTSYS;
+#endif
+
+	switch(cmd) {
+		/*
+		 * All wrapped CM SYSCALL
+		 */
+	case CM_INSTANTIATECOMPONENT:
+		return cmld_InstantiateComponent(procPriv,
+						 (CM_InstantiateComponent_t *)arg);
+
+	case CM_BINDCOMPONENTFROMCMCORE:
+		return cmld_BindComponentFromCMCore(procPriv,
+						    (CM_BindComponentFromCMCore_t *)arg);
+
+	case CM_UNBINDCOMPONENTFROMCMCORE:
+		return cmld_UnbindComponentFromCMCore((CM_UnbindComponentFromCMCore_t *)arg);
+
+	case CM_UNBINDCOMPONENTTOCMCORE:
+		return cmld_UnbindComponentToCMCore(procPriv, (CM_UnbindComponentToCMCore_t *)arg);
+
+	case CM_BINDCOMPONENTASYNCHRONOUS:
+		return cmld_BindComponentAsynchronous(procPriv, (CM_BindComponentAsynchronous_t *)arg);
+
+	case CM_UNBINDCOMPONENTASYNCHRONOUS:
+		return cmld_UnbindComponentAsynchronous(procPriv, (CM_UnbindComponentAsynchronous_t *)arg);
+
+	case CM_BINDCOMPONENT:
+		return cmld_BindComponent(procPriv, (CM_BindComponent_t *)arg);
+
+	case CM_UNBINDCOMPONENT:
+		return cmld_UnbindComponent(procPriv, (CM_UnbindComponent_t *)arg);
+
+	case CM_BINDCOMPONENTTOVOID:
+		return cmld_BindComponentToVoid(procPriv, (CM_BindComponentToVoid_t *)arg);
+
+	case CM_DESTROYCOMPONENT:
+		return cmld_DestroyComponent(procPriv, (CM_DestroyComponent_t *)arg);
+
+	case CM_CREATEMEMORYDOMAIN:
+		return cmld_CreateMemoryDomain(procPriv, (CM_CreateMemoryDomain_t *)arg);
+
+	case CM_CREATEMEMORYDOMAINSCRATCH:
+		return cmld_CreateMemoryDomainScratch(procPriv, (CM_CreateMemoryDomainScratch_t *)arg);
+
+	case CM_DESTROYMEMORYDOMAIN:
+		return cmld_DestroyMemoryDomain((CM_DestroyMemoryDomain_t *)arg);
+
+	case CM_GETDOMAINCOREID:
+		return cmld_GetDomainCoreId((CM_GetDomainCoreId_t *)arg);
+
+	case CM_ALLOCMPCMEMORY:
+		return cmld_AllocMpcMemory(procPriv, (CM_AllocMpcMemory_t *)arg);
+
+	case CM_FREEMPCMEMORY:
+		return cmld_FreeMpcMemory(procPriv, (CM_FreeMpcMemory_t *)arg);
+
+	case CM_GETMPCMEMORYSTATUS:
+		return cmld_GetMpcMemoryStatus((CM_GetMpcMemoryStatus_t *)arg);
+
+	case CM_STARTCOMPONENT:
+		return cmld_StartComponent(procPriv, (CM_StartComponent_t *)arg);
+
+	case CM_STOPCOMPONENT:
+		return cmld_StopComponent(procPriv, (CM_StopComponent_t *)arg);
+
+	case CM_GETMPCLOADCOUNTER:
+		return cmld_GetMpcLoadCounter((CM_GetMpcLoadCounter_t *)arg);
+
+	case CM_GETCOMPONENTDESCRIPTION:
+		return cmld_GetComponentDescription(procPriv, (CM_GetComponentDescription_t *)arg);
+
+	case CM_GETCOMPONENTLISTHEADER:
+		return cmld_GetComponentListHeader(procPriv, (CM_GetComponentListHeader_t *)arg);
+
+	case CM_GETCOMPONENTLISTNEXT:
+		return cmld_GetComponentListNext(procPriv, (CM_GetComponentListNext_t *)arg);
+
+	case CM_GETCOMPONENTREQUIREDINTERFACENUMBER:
+		return cmld_GetComponentRequiredInterfaceNumber(procPriv,
+								(CM_GetComponentRequiredInterfaceNumber_t *)arg);
+
+	case CM_GETCOMPONENTREQUIREDINTERFACE:
+		return cmld_GetComponentRequiredInterface(procPriv,
+							  (CM_GetComponentRequiredInterface_t *)arg);
+
+	case CM_GETCOMPONENTREQUIREDINTERFACEBINDING:
+		return cmld_GetComponentRequiredInterfaceBinding(procPriv,
+								 (CM_GetComponentRequiredInterfaceBinding_t *)arg);
+
+	case CM_GETCOMPONENTPROVIDEDINTERFACENUMBER:
+		return cmld_GetComponentProvidedInterfaceNumber(procPriv,
+								(CM_GetComponentProvidedInterfaceNumber_t *)arg);
+
+	case CM_GETCOMPONENTPROVIDEDINTERFACE:
+		return cmld_GetComponentProvidedInterface(procPriv,
+							  (CM_GetComponentProvidedInterface_t *)arg);
+
+	case CM_GETCOMPONENTPROPERTYNUMBER:
+		return cmld_GetComponentPropertyNumber(procPriv,
+						       (CM_GetComponentPropertyNumber_t *)arg);
+
+	case CM_GETCOMPONENTPROPERTYNAME:
+		return cmld_GetComponentPropertyName(procPriv,
+						     (CM_GetComponentPropertyName_t *)arg);
+
+	case CM_GETCOMPONENTPROPERTYVALUE:
+		return cmld_GetComponentPropertyValue(procPriv,
+						      (CM_GetComponentPropertyValue_t *)arg);
+
+	case CM_READCOMPONENTATTRIBUTE:
+		return cmld_ReadComponentAttribute(procPriv,
+						   (CM_ReadComponentAttribute_t *)arg);
+
+	case CM_GETEXECUTIVEENGINEHANDLE:
+		return cmld_GetExecutiveEngineHandle(procPriv,
+						     (CM_GetExecutiveEngineHandle_t *)arg);
+
+	case CM_SETMODE:
+		return cmld_SetMode((CM_SetMode_t *)arg);
+
+	case CM_GETREQUIREDCOMPONENTFILES:
+		return cmld_GetRequiredComponentFiles(procPriv,
+						      (CM_GetRequiredComponentFiles_t *)arg);
+
+	case CM_MIGRATE:
+		return cmld_Migrate((CM_Migrate_t *)arg);
+
+	case CM_UNMIGRATE:
+		return cmld_Unmigrate((CM_Unmigrate_t *)arg);
+
+	case CM_SETUPRELINKAREA:
+		return cmld_SetupRelinkArea(procPriv,
+					    (CM_SetupRelinkArea_t *)arg);
+
+	case CM_PUSHCOMPONENT:
+		return cmld_PushComponent((CM_PushComponent_t *)arg);
+
+	case CM_RELEASECOMPONENT:
+		return cmld_ReleaseComponent((CM_ReleaseComponent_t *)arg);
+
+		/*
+		 * NMF CALLS (Host->MPC bindings)
+		 */
+	case CM_PUSHEVENTWITHSIZE: {
+		CM_PushEventWithSize_t data;
+		t_event_params_handle event;
+
+		/* coverity[tainted_data_argument : FALSE] */
+		if (copy_from_user(&data.in, (CM_PushEventWithSize_t*)arg, sizeof(data.in)))
+			return -EFAULT;
+
+		/* Take the lock to synchronize CM_ENGINE_AllocEvent()
+		 * and CM_ENGINE_PushEvent()
+		 */
+		if (mutex_lock_killable(&procPriv->host2mpcLock))
+			return -ERESTARTSYS;
+
+		event = CM_ENGINE_AllocEvent(data.in.host2mpcId);
+		if (event == NULL) {
+			mutex_unlock(&procPriv->host2mpcLock);
+			return put_user(CM_PARAM_FIFO_OVERFLOW,
+					&((CM_PushEventWithSize_t*)arg)->out.error);
+		}
+		if (data.in.size != 0)
+			/* coverity[tainted_data : FALSE] */
+			if (copy_from_user(event, data.in.h, data.in.size)) {
+				mutex_unlock(&procPriv->host2mpcLock);
+				return -EFAULT; // TODO: what about the already allocated and acknowledged event!?!
+			}
+
+		data.out.error = CM_ENGINE_PushEvent(data.in.host2mpcId, event, data.in.methodIndex);
+		mutex_unlock(&procPriv->host2mpcLock);
+ 
+		/* copy error value back */
+		return put_user(data.out.error, &((CM_PushEventWithSize_t*)arg)->out.error);
+	}
+	
+		/*
+		 * All private (internal) commands
+		 */
+	case CM_PRIVGETMPCMEMORYDESC:
+		return cmld_PrivGetMPCMemoryDesc(procPriv, (CM_PrivGetMPCMemoryDesc_t *)arg);
+
+	case CM_PRIVRESERVEMEMORY:
+		return cmld_PrivReserveMemory(procPriv, arg);
+
+	case CM_GETVERSION: {
+		t_uint32 nmfversion = NMF_VERSION;
+		return copy_to_user((void*)arg, &nmfversion, sizeof(nmfversion));
+	}
+	case CM_PRIV_GETBOARDVERSION: {
+		enum board_version v = U8500_V2;
+		return copy_to_user((void*)arg, &v, sizeof(v));
+	}
+	case CM_PRIV_ISCOMPONENTCACHEEMPTY:
+		if (CM_ENGINE_IsComponentCacheEmpty())
+			return 0;
+		else
+			return -ENOENT;
+	case CM_PRIV_DEBUGFS_READY:
+#ifdef CONFIG_DEBUG_FS
+		cmld_user_has_debugfs = true;
+#endif
+		return 0;
+	case CM_PRIV_DEBUGFS_WAIT_DUMP:
+		return 0;
+
+	case CM_WRITECOMPONENTATTRIBUTE:
+		return cmld_WriteComponentAttribute(procPriv,
+            (CM_WriteComponentAttribute_t *)arg);
+
+	default:
+		pr_err("CM(%s): unsupported command %i\n", __func__, cmd);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/** VMA open callback function
+ */
+static void cmld_vma_open(struct vm_area_struct* vma) {
+	struct memAreaDesc_t* curr = (struct memAreaDesc_t*)vma->vm_private_data;
+
+	atomic_inc(&curr->count);
+}
+
+/** VMA close callback function
+ */
+static void cmld_vma_close(struct vm_area_struct* vma) {
+	struct memAreaDesc_t* curr = (struct memAreaDesc_t*)vma->vm_private_data;
+	
+	atomic_dec(&curr->count);
+}
+
+static struct vm_operations_struct cmld_remap_vm_ops = {
+	.open = cmld_vma_open,
+	.close = cmld_vma_close,
+};
+
+/** mmap implementation.
+ * Remaps just once.
+ * 
+ * \return POSIX error code
+ */
+static int cmld_control_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	struct list_head* listHead;
+	struct list_head* cursor;
+	struct cm_process_priv* procPriv = file->private_data;
+	struct memAreaDesc_t* curr = NULL;
+	unsigned int vma_size = vma->vm_end-vma->vm_start;
+
+	listHead = &procPriv->memAreaDescList;
+
+	if (lock_process(procPriv)) return -ERESTARTSYS;
+	/* Make sure the memory area has not already been remapped */
+	list_for_each(cursor, listHead) {
+		curr = list_entry(cursor, struct memAreaDesc_t, list);
+		/* For now, the user space aligns any requested physaddr to a page-size limit
+		   This is not safe and must be fixed. But this is the only way to
+		   minimize the allocated TCM memory, needed because of low amount of
+		   TCM memory 
+		   Another way is to add some more check before doing this mmap()
+		   to allow this mmap, for example.
+		   NOTE: this memory must be first reserved via the CM_PRIVRESERVEMEMORY ioctl()
+		*/
+		if ((curr->physAddr&PAGE_MASK) == offset &&
+		    curr->tid == current->pid) {
+			if (curr->userLogicalAddr) {
+				unlock_process(procPriv);
+				return -EINVAL; // already mapped!
+			}
+			/* reset the thread id value, to not confuse any further mmap() */
+			curr->tid = 0;
+			break;
+		}
+	}
+
+	if (cursor == listHead) {
+		unlock_process(procPriv);
+		return -EINVAL; // no matching memory area descriptor found!
+	}
+	
+	/* Very, very important to have consistent buffer transition */
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_flags |= VM_RESERVED | VM_IO | VM_DONTEXPAND | VM_DONTCOPY;
+	
+	if (remap_pfn_range(vma, vma->vm_start, offset>>PAGE_SHIFT,
+			    vma_size, vma->vm_page_prot)) {
+		unlock_process(procPriv);
+		return -EAGAIN;
+	}
+ 
+	/* Offset represents the physical address.
+	 * Update the list entry filling in the logical address assigned to the user
+	 */
+	/*
+	 * NOTE: here the useLogicalAddr is page-aligned, but not necessaly the
+	 *       phycical address. We mmap() more than originaly requested by the
+	 *       user, see in CM User Proxy (file cmsyscallwrapper.c)
+	 */
+	curr->userLogicalAddr = vma->vm_start;
+
+	/* increment reference counter */
+	atomic_inc(&curr->count);
+	
+	unlock_process(procPriv);
+
+	/* set private data structure and callbacks */
+	vma->vm_private_data = (void *)curr;
+	vma->vm_ops = &cmld_remap_vm_ops;
+
+	return 0;
+}
+
+/* Driver's release method for /dev/cm_channel */
+static int cmld_channel_release(struct inode *inode, struct file *file)
+{
+	struct cm_channel_priv* channelPriv = file->private_data;
+	struct cm_process_priv* procPriv = channelPriv->proc;
+
+	/*
+	 * The driver must guarantee that all related resources are released.
+	 * Thus all these checks below are necessary to release all remaining
+	 * resources still linked to this 'client', in case of abnormal process
+	 * exit.
+	 * => These are error cases !
+	 * In the usual case, nothing should be done except the free of
+	 * the cmPriv itself
+	 */
+
+	/* We don't need to synchronize here by using the skelListLock:
+	   the list is only accessed during ioctl() and we can't be here
+	   if an ioctl() is on-going */
+	if (list_empty(&channelPriv->skelList)) {
+		/* There is no pending MPC->HOST binding
+		   => we can quietly delete the channel */
+		tasklet_disable(&cmld_service_tasklet);
+		mutex_lock(&channel_lock);
+		list_del(&channelPriv->entry);
+		mutex_unlock(&channel_lock);
+		tasklet_enable(&cmld_service_tasklet);
+
+		/* Free all remaining messages if any */
+		freeMessages(channelPriv);
+
+		/* Free the per-channel descriptor */
+		OSAL_Free(channelPriv);
+	} else {
+		/*
+		 * Uh: there are still some MPC->HOST binding but we don't have
+		 * the required info to unbind them.
+		 * => we must keep all skel structures because possibly used in
+		 * OSAL_PostDfc (incoming callback msg). We flag the channel as
+		 * closed to discard any new msg that will never be read anyway
+		 */
+		channelPriv->state = CHANNEL_CLOSED;
+
+		/* Already Free all remaining messages if any,
+		   they will never be read anyway */
+		freeMessages(channelPriv);
+	}
+
+	kref_put(&procPriv->ref, freeProcessPriv);
+	file->private_data = NULL;
+
+	return 0;
+}
+
+/* Driver's release method for /dev/cm_control */
+static int cmld_control_release(struct inode *inode, struct file *file)
+{
+	struct cm_process_priv* procPriv = file->private_data;
+
+	kref_put(&procPriv->ref, freeProcessPriv);
+	file->private_data = NULL;
+
+	return 0;
+}
+
+static struct file_operations cmld_control_fops = {
+	.owner		=	THIS_MODULE,
+	.unlocked_ioctl	=	cmld_control_ioctl,
+	.mmap		=	cmld_control_mmap,
+	.release	=	cmld_control_release,
+};
+
+static int cmld_control_open(struct file *file)
+{
+	struct cm_process_priv *procPriv = getProcessPriv();
+	if (IS_ERR(procPriv))
+		return PTR_ERR(procPriv);
+	file->private_data = procPriv;
+	file->f_op = &cmld_control_fops;
+	return 0;
+}
+
+static struct file_operations cmld_channel_fops = {
+	.owner		=	THIS_MODULE,
+	.read		=	cmld_channel_read,
+	.unlocked_ioctl	=	cmld_channel_ioctl,
+	.flush		=	cmld_channel_flush,
+	.release	=	cmld_channel_release,
+};
+
+static int cmld_channel_open(struct file *file)
+{
+	struct cm_process_priv *procPriv = getProcessPriv();
+	struct cm_channel_priv *channelPriv;
+
+	if (IS_ERR(procPriv))
+		return PTR_ERR(procPriv);
+
+	channelPriv = (struct cm_channel_priv*)OSAL_Alloc(sizeof(*channelPriv));
+	if (channelPriv == NULL) {
+		kref_put(&procPriv->ref, freeProcessPriv);
+		return -ENOMEM;
+	}
+
+	channelPriv->proc = procPriv;
+	channelPriv->state = CHANNEL_OPEN;
+
+	/* Initialize wait_queue, lists and mutexes */
+	init_waitqueue_head(&channelPriv->waitq);
+	plist_head_init(&channelPriv->messageQueue);
+	INIT_LIST_HEAD(&channelPriv->skelList);
+	spin_lock_init(&channelPriv->bh_lock);
+	mutex_init(&channelPriv->msgQueueLock);
+	mutex_init(&channelPriv->skelListLock);
+
+	tasklet_disable(&cmld_service_tasklet);
+	mutex_lock(&channel_lock);
+	list_add(&channelPriv->entry, &channel_list);
+	mutex_unlock(&channel_lock);
+	tasklet_enable(&cmld_service_tasklet);
+
+	file->private_data = channelPriv;
+	file->f_op = &cmld_channel_fops;
+	return 0;
+}
+
+static ssize_t cmld_sxa_trace_read(struct file *file, char *buf, size_t count, loff_t *ppos)
+{
+	struct mpcConfig *mpc = file->private_data;
+	size_t written = 0;
+	struct t_nmf_trace trace;
+	t_cm_trace_type traceType;
+	struct mmdsp_trace mmdsp_tr = {
+		.media = TB_MEDIA_FILE,
+		.receiver_dev = TB_DEV_PC,
+		.sender_dev = TB_DEV_TRACEBOX,
+		.unused = TB_TRACEBOX,
+		.receiver_obj = DEFAULT_RECEIVERR_OBJ,
+		.sender_obj = DEFAULT_SENDER_OBJ,
+		.transaction_id = 0,
+		.message_id = TB_TRACE_MSG,
+		.master_id = mpc->coreId+1,
+		.channel_id = 0,
+		.ost_version = OST_VERSION,
+		.entity = ENTITY,
+		.protocol_id = PROTOCOL_ID,
+		.btrace_hdr_flag = 0,
+		.btrace_hdr_subcategory = 0,
+	};
+
+	while ((count - written) >= sizeof(mmdsp_tr)) {
+		traceType = CM_ENGINE_GetNextTrace(mpc->coreId, &trace);
+
+		switch (traceType) {
+		case CM_MPC_TRACE_READ_OVERRUN:
+			mmdsp_tr.size =
+				cpu_to_be16(offsetof(struct mmdsp_trace,
+						     ost_version)
+					    -offsetof(struct mmdsp_trace,
+						      receiver_obj));
+			mmdsp_tr.message_id = TB_TRACE_EXCEPTION_MSG;
+			mmdsp_tr.ost_master_id = TB_EXCEPTION_LONG_OVRF_PACKET;
+			if (copy_to_user(&buf[written], &mmdsp_tr,
+					 offsetof(struct mmdsp_trace,
+						  ost_version)))
+				return -EFAULT;
+			written += offsetof(struct mmdsp_trace, ost_version);
+			if ((count - written) < sizeof(mmdsp_tr))
+				break;
+		case CM_MPC_TRACE_READ: {
+			u16 param_nr = (u16)trace.paramOpt;
+			u16 handle_valid = (u16)(trace.paramOpt >> 16);
+			u32 to_write = offsetof(struct mmdsp_trace,
+						parent_handle);
+			mmdsp_tr.transaction_id = trace.revision%256;
+			mmdsp_tr.message_id = TB_TRACE_MSG;
+			mmdsp_tr.ost_master_id = OST_MASTERID;
+			mmdsp_tr.timestamp = cpu_to_be64(trace.timeStamp);
+			mmdsp_tr.timestamp2 = cpu_to_be64(trace.timeStamp);
+			mmdsp_tr.component_id = cpu_to_be32(trace.componentId);
+			mmdsp_tr.trace_id = cpu_to_be32(trace.traceId);
+			mmdsp_tr.btrace_hdr_category = (trace.traceId>>16)&0xFF;
+			mmdsp_tr.btrace_hdr_size = BTRACE_HEADER_SIZE
+				+ sizeof(trace.params[0]) * param_nr;
+			if (handle_valid) {
+				mmdsp_tr.parent_handle = trace.parentHandle;
+				mmdsp_tr.component_handle =
+					trace.componentHandle;
+				to_write += sizeof(trace.parentHandle)
+					+ sizeof(trace.componentHandle);
+				mmdsp_tr.btrace_hdr_size +=
+					sizeof(trace.parentHandle)
+					+ sizeof(trace.componentHandle);
+			}
+			mmdsp_tr.size =
+				cpu_to_be16(to_write
+					    + (sizeof(trace.params[0])*param_nr)
+					    - offsetof(struct mmdsp_trace,
+						       receiver_obj));
+			mmdsp_tr.length = to_write
+				+ (sizeof(trace.params[0])*param_nr)
+				- offsetof(struct mmdsp_trace,
+					   timestamp2);
+			if (copy_to_user(&buf[written], &mmdsp_tr, to_write))
+				return -EFAULT;
+			written += to_write;
+			/* write param */
+			to_write = sizeof(trace.params[0]) * param_nr;
+			if (copy_to_user(&buf[written], trace.params, to_write))
+				return -EFAULT;
+			written += to_write;
+			break;
+		}
+		case CM_MPC_TRACE_NONE:
+		default:
+			if ((file->f_flags & O_NONBLOCK) || written)
+				return written;
+			spin_lock_bh(&mpc->trace_reader_lock);
+			mpc->trace_reader = current;
+			spin_unlock_bh(&mpc->trace_reader_lock);
+			schedule_timeout_killable(msecs_to_jiffies(200));
+			spin_lock_bh(&mpc->trace_reader_lock);
+			mpc->trace_reader = NULL;
+			spin_unlock_bh(&mpc->trace_reader_lock);
+			if (signal_pending(current))
+				return -ERESTARTSYS;
+		}
+	}
+	return written;
+}
+
+/* Driver's release method for /dev/cm_sxa_trace */
+static int cmld_sxa_trace_release(struct inode *inode, struct file *file)
+{
+	struct mpcConfig *mpc = file->private_data;
+	atomic_dec(&mpc->trace_read_count);
+	return 0;
+}
+
+static struct file_operations cmld_sxa_trace_fops = {
+	.owner		=	THIS_MODULE,
+	.read		=	cmld_sxa_trace_read,
+	.release	=	cmld_sxa_trace_release,
+};
+
+static int cmld_sxa_trace_open(struct file *file, struct mpcConfig *mpc)
+{
+	if (atomic_add_unless(&mpc->trace_read_count, 1, 1) == 0)
+		return -EBUSY;
+
+	file->private_data = mpc;
+	file->f_op = &cmld_sxa_trace_fops;
+	return 0;
+}
+
+/* driver open() call: specific */
+static int cmld_open(struct inode *inode, struct file *file)
+{
+	switch (iminor(inode)) {
+	case 0:
+		return cmld_control_open(file);
+	case 1:
+		return cmld_channel_open(file);
+	case 2:
+		return cmld_sxa_trace_open(file, &osalEnv.mpc[SIA]);
+	case 3:
+		return cmld_sxa_trace_open(file, &osalEnv.mpc[SVA]);
+	default:
+		return -ENOSYS;
+	}
+}
+
+/** MPC Events tasklet
+ *  The parameter is used to know from which interrupts we're comming
+ *  and which core to pass to CM_ProcessMpcEvent():
+ *  0 means HSEM => ARM_CORE_ID
+ *  otherwise, it gives the index+1 of MPC within osalEnv.mpc table
+ */
+static void mpc_events_tasklet_handler(unsigned long core)
+{
+	/* This serves internal events directly. No propagation to user space.
+	 * Calls OSAL_PostDfc implementation for user interface events */
+	if (core == 0) {
+		CM_ProcessMpcEvent(ARM_CORE_ID);
+		enable_irq(IRQ_DB8500_HSEM);
+	} else {
+		--core;
+		CM_ProcessMpcEvent(osalEnv.mpc[core].coreId);
+		enable_irq(osalEnv.mpc[core].interrupt0);
+	}
+}
+
+/** Hardware semaphore and MPC interrupt handler
+ * 'data' param is the one given when registering the IRQ hanlder,
+ * contains the source core (ARM or MPC), and follows the same logic
+ * as for mpc_events_tasklet_handler()
+ * This handler is used for all IRQ handling some com (ie HSEM or
+ * all MPC IRQ line0)
+ */
+static irqreturn_t mpc_events_irq_handler(int irq, void *data)
+{
+	unsigned core = (unsigned)data;
+
+	if (core != 0)
+		--core;
+	disable_irq_nosync(irq);
+	tasklet_schedule(&osalEnv.mpc[core].tasklet);
+
+	return IRQ_HANDLED;
+}
+
+/** MPC panic handler
+ * 'idx' contains the index of the core within the osalEnv.mpc table.
+ * This handler is used for all MPC IRQ line1
+ */
+static irqreturn_t panic_handler(int irq, void *idx)
+{
+	set_bit((int)idx, &service_tasklet_data);
+	disable_irq_nosync(irq);
+	tasklet_schedule(&cmld_service_tasklet);
+	return IRQ_HANDLED;
+}
+
+/** Driver's operations
+ */
+static struct file_operations cmld_fops = {
+	.owner		=	THIS_MODULE,
+	.open		=	cmld_open,
+};
+
+/**
+ * Configure a MPC, called for each MPC to configure
+ *
+ * \param i            index of the MPC to configure (refer to the index
+ *                     of the MPC within the osalEnvironment.mpc table)
+ * \param dataAllocId  allocId of the data segment, passed through each call of
+ *                     this function, and initialized at the first call in case
+ *                     shared data segment
+ */
+static int configureMpc(unsigned i, t_cfg_allocator_id *dataAllocId)
+{
+	int err;
+	t_cm_system_address mpcSystemAddress;
+	t_nmf_memory_segment codeSegment, dataSegment;
+	t_cfg_allocator_id codeAllocId;
+	t_cm_domain_id eeDomainId;
+	t_cm_domain_memory eeDomain = INIT_DOMAIN;
+	char regulator_name[14];
+
+	getMpcSystemAddress(i, &mpcSystemAddress);
+	getMpcSdramSegments(i, &codeSegment, &dataSegment);
+
+	/* Create code segment */
+	err = CM_ENGINE_AddMpcSdramSegment(&codeSegment, &codeAllocId, "Code");
+	if (err != CM_OK) {
+		pr_err("CM_ENGINE_AddMpcSdramSegment() error code: %d\n", err);
+		return -EAGAIN;
+	}
+
+	/* Create data segment
+	 * NOTE: in case of shared data segment, all MPC point to the same data segment
+	 * (see in remapRegions()) and we need to create the segment only at first call.
+	 * => we reuse the same allocId for the following MPCs
+	 */
+	if ((osalEnv.mpc[i].sdram_data.data != osalEnv.mpc[0].sdram_data.data)
+	    || *dataAllocId == -1) {
+		err = CM_ENGINE_AddMpcSdramSegment(&dataSegment, dataAllocId, "Data");
+		if (err != CM_OK) {
+			pr_err("CM_ENGINE_AddMpcSdramSegment() error code: %d\n", err);
+			return -EAGAIN;
+		}
+	}
+
+	/* create default domain for the given coreId
+	 * this serves for instanciating EE and the LoadMap, only sdram segment is present
+	 * this domain will probably overlap with other user domains
+	 */
+	eeDomain.coreId            = osalEnv.mpc[i].coreId;
+	eeDomain.sdramCode.offset  = 0x0;
+	eeDomain.sdramData.offset  = 0x0;
+	eeDomain.sdramCode.size    = 0x8000;
+	eeDomain.sdramData.size    = 0x40000;
+	eeDomain.esramCode.size    = 0x4000;
+	eeDomain.esramData.size    = 0x40000;
+	err = CM_ENGINE_CreateMemoryDomain(NMF_CORE_CLIENT, &eeDomain, &eeDomainId);
+	if (err != CM_OK) {
+		pr_err("Create EE domain on %s failed with error code: %d\n", osalEnv.mpc[i].name, err);
+		return -EAGAIN;
+	}
+
+	err = CM_ENGINE_ConfigureMediaProcessorCore(
+		osalEnv.mpc[i].coreId,
+		osalEnv.mpc[i].eeId,
+		(cfgSemaphoreTypeHSEM ? SYSTEM_SEMAPHORES : LOCAL_SEMAPHORES),
+		osalEnv.mpc[i].nbYramBanks,
+		&mpcSystemAddress,
+		eeDomainId,
+		codeAllocId,
+		*dataAllocId);
+
+	if (err != CM_OK) {
+		pr_err("CM_ConfigureMediaProcessorCore failed with error code: %d\n", err);	
+		return -EAGAIN;
+	}
+
+        // Communication channel
+        if (! cfgSemaphoreTypeHSEM) {
+		tasklet_init(&osalEnv.mpc[i].tasklet, mpc_events_tasklet_handler, i+1);
+		err = request_irq(osalEnv.mpc[i].interrupt0, mpc_events_irq_handler, IRQF_DISABLED, osalEnv.mpc[i].name, (void*)(i+1));
+		if (err != 0) {
+			pr_err("CM: request_irq failed to register irq0 %i for %s (%i)\n", osalEnv.mpc[i].interrupt0, osalEnv.mpc[i].name, err);
+			return err;
+		}
+        }
+
+        // Panic channel
+	err = request_irq(osalEnv.mpc[i].interrupt1, panic_handler, IRQF_DISABLED, osalEnv.mpc[i].name, (void*)i);
+	if (err != 0) {
+		pr_err("CM: request_irq failed to register irq1 %i for %s (%i)\n", osalEnv.mpc[i].interrupt1, osalEnv.mpc[i].name, err);
+		free_irq(osalEnv.mpc[i].interrupt0, (void*)(i+1));
+		return err;
+	}
+
+	// Retrieve the regulators used for this MPCs
+	sprintf(regulator_name, "%s-mmdsp", osalEnv.mpc[i].name);
+	osalEnv.mpc[i].mmdsp_regulator = regulator_get(cmld_dev[0], regulator_name);
+	if (IS_ERR(osalEnv.mpc[i].mmdsp_regulator)) {
+		long err = PTR_ERR(osalEnv.mpc[i].mmdsp_regulator);
+		pr_err("CM: Error while retrieving the regulator %s: %ld\n", regulator_name, err);
+		osalEnv.mpc[i].mmdsp_regulator = NULL;
+		return err;
+	}
+	sprintf(regulator_name, "%s-pipe", osalEnv.mpc[i].name);
+	osalEnv.mpc[i].pipe_regulator = regulator_get(cmld_dev[0], regulator_name);
+	if (IS_ERR(osalEnv.mpc[i].pipe_regulator)) {
+		long err = PTR_ERR(osalEnv.mpc[i].pipe_regulator);
+		pr_err("CM: Error while retrieving the regulator %s: %ld\n", regulator_name, err);
+		osalEnv.mpc[i].pipe_regulator = NULL;
+		return err;
+	}
+#ifdef CONFIG_HAS_WAKELOCK
+	wake_lock_init(&osalEnv.mpc[i].wakelock, WAKE_LOCK_SUSPEND, osalEnv.mpc[i].name);
+#endif
+	return 0;
+}
+
+/* Free all used MPC irqs and clocks.
+ * max_mpc allows it to be called from init_module and free
+ * only the already configured irqs.
+ */
+static void free_mpc_irqs(int max_mpc)
+{
+	int i;
+	for (i=0; i<max_mpc; i++) {
+		if (! cfgSemaphoreTypeHSEM)
+			free_irq(osalEnv.mpc[i].interrupt0, (void*)(i+1));
+		free_irq(osalEnv.mpc[i].interrupt1, (void*)i);
+		if (osalEnv.mpc[i].mmdsp_regulator)
+			regulator_put(osalEnv.mpc[i].mmdsp_regulator);
+		if (osalEnv.mpc[i].pipe_regulator)
+			regulator_put(osalEnv.mpc[i].pipe_regulator);
+#ifdef CONFIG_HAS_WAKELOCK
+		wake_lock_destroy(&osalEnv.mpc[i].wakelock);
+#endif
+	}
+}
+
+/** Module entry point
+ * Allocate memory chunks. Register hardware semaphore, SIA and SVA interrupts.
+ * Initialize Component Manager. Register hotplug for components download.
+ *
+ * \return POSIX error code
+ */
+static int __init cmld_init_module(void)
+{
+	int err;
+	unsigned i=0;
+	dev_t dev;
+	t_cfg_allocator_id dataAllocId = -1;
+	void *htim_base=NULL;
+
+	/* Component manager initialization descriptors */
+	t_nmf_hw_mapping_desc nmfHwMappingDesc;
+	t_nmf_config_desc nmfConfigDesc = { cfgCommunicationLocationInSDRAM ? COMS_IN_SDRAM : COMS_IN_ESRAM };
+
+	/* OSAL_*Resources() assumes the following, so check that it is correct */
+	if (SVA != COREIDX((int)SVA_CORE_ID)) {
+		pr_err("SVA and (SVA_CORE_ID-1) differs : code must be fixed !\n");
+		return -EIO;
+	}
+	if (SIA != COREIDX((int)SIA_CORE_ID)) {
+		pr_err("SIA and (SIA_CORE_ID-1) differs : code must be fixed !\n");
+		return -EIO;
+	}
+
+#ifdef CM_DEBUG_ALLOC
+	init_debug_alloc();
+#endif
+
+	err = -EIO;
+	prcmu_base = __io_address(U8500_PRCMU_BASE);
+
+	/* power on a clock/timer 90KHz used on SVA */
+	htim_base = ioremap_nocache(U8500_CR_BASE /*0xA03C8000*/, SZ_4K);
+	prcmu_tcdm_base = __io_address(U8500_PRCMU_TCDM_BASE);
+
+	/* Activate SVA 90 KHz timer */
+	if (htim_base == NULL)
+		goto out;
+	iowrite32((1<<26) | ioread32(htim_base), htim_base);
+	iounmap(htim_base);
+
+	/*i = ioread32(PRCM_SVAMMDSPCLK_MGT) & 0xFF;
+	if (i != 0x22)
+		pr_alert("CM: Looks like SVA is not clocked at 200MHz (PRCM_SVAMMDSPCLK_MGT=%x)\n", i);
+	i = ioread32(PRCM_SIAMMDSPCLK_MGT) & 0xFF;
+	if (i != 0x22)
+		pr_alert("CM: Looks like SIA is not clocked at 200MHz (PRCM_SIAMMDSPCLK_MGT=%x)\n", i);
+
+	i = 0;*/
+	err = init_config();
+	if (err)
+		goto out;
+
+	/* Remap all needed regions and store in osalEnv base addresses */
+	err = remapRegions();
+	if (err != 0)
+		goto out;
+	
+	/* Initialize linux devices */
+	err = class_register(&cmld_class);
+	if (err) {
+		pr_err("CM: class_register failed (%d)\n", err);
+                goto out;
+	}
+
+	/* Register char device */
+	err = alloc_chrdev_region(&dev, 0, ARRAY_SIZE(cmld_devname), "cm");
+	if (err) {
+		pr_err("CM: alloc_chrdev_region failed (%d)\n", err);
+		goto out_destroy_class;
+	}
+	cmld_major =  MAJOR(dev);
+
+	cdev_init(&cmld_cdev, &cmld_fops);
+	cmld_cdev.owner = THIS_MODULE;
+	err = cdev_add (&cmld_cdev, dev, ARRAY_SIZE(cmld_devname));
+	if (err) {
+		pr_err("CM: cdev_add failed (%d)\n", err);
+		goto out_destroy_chrdev;
+	}
+
+	for (i=0; i<ARRAY_SIZE(cmld_devname); i++) {
+		cmld_dev[i] = device_create(&cmld_class, NULL, MKDEV(cmld_major, i), NULL,
+					    "%s", cmld_devname[i]);
+		if (IS_ERR(cmld_dev[i])) {
+			err = PTR_ERR(cmld_dev[i]);
+			pr_err("CM: device_create failed (%d)\n", err);
+			goto out_destroy_device;
+		}
+	}
+
+	osalEnv.esram_regulator[ESRAM_12] = regulator_get(cmld_dev[0], "esram12");
+	if (IS_ERR(osalEnv.esram_regulator[ESRAM_12])) {
+		err = PTR_ERR(osalEnv.esram_regulator[ESRAM_12]);
+		pr_err("CM: Error while retrieving the regulator for esram12: %d\n", err);
+		osalEnv.esram_regulator[ESRAM_12] = NULL;
+		goto out_destroy_device;
+	}
+	osalEnv.esram_regulator[ESRAM_34] = regulator_get(cmld_dev[0], "esram34");
+	if (IS_ERR(osalEnv.esram_regulator[ESRAM_34])) {
+		err = PTR_ERR(osalEnv.esram_regulator[ESRAM_34]);
+		pr_err("CM: Error while retrieving the regulator for esram34: %d\n", err);
+		osalEnv.esram_regulator[ESRAM_34] = NULL;
+		goto out_destroy_device;
+	}
+
+	/* Fill in the descriptors needed by CM_ENGINE_Init() */
+	getNmfHwMappingDesc(&nmfHwMappingDesc);
+
+	/* Initialize Component Manager */
+	err = CM_ENGINE_Init(&nmfHwMappingDesc, &nmfConfigDesc);
+	if (err != CM_OK) {
+		pr_err("CM: CM_Init failed with error code: %d\n", err);
+		err = -EAGAIN;
+		goto out_destroy_device;
+	} else {
+		pr_info("Initialize NMF %d.%d.%d Component Manager......\n",
+			VERSION_MAJOR(NMF_VERSION),
+			VERSION_MINOR(NMF_VERSION),
+			VERSION_PATCH(NMF_VERSION));
+		pr_info("[ CM Linux Driver %d.%d.%d ]\n",
+			VERSION_MAJOR(NMF_VERSION),
+			VERSION_MINOR(NMF_VERSION),
+			CMDRIVER_PATCH_VERSION);
+	}
+
+	cm_debug_init();
+	if (osal_debug_ops.domain_create) {
+		osal_debug_ops.domain_create(DEFAULT_SVA_DOMAIN);
+		osal_debug_ops.domain_create(DEFAULT_SIA_DOMAIN);
+	}
+
+	/* Configure MPC Cores */
+	for (i=0; i<NB_MPC; i++) {
+		err = configureMpc(i, &dataAllocId);
+		if (err)
+			goto out_all;
+	}
+	/* End of Component Manager initialization phase */
+
+
+        if (cfgSemaphoreTypeHSEM) {
+		/* We use tasklet of mpc[0]. See comments above osalEnvironnent struct */
+		tasklet_init(&osalEnv.mpc[0].tasklet, mpc_events_tasklet_handler, 0);
+		err = request_irq(IRQ_DB8500_HSEM, mpc_events_irq_handler, IRQF_DISABLED,
+				  "hwsem", 0);
+		if (err) {
+			pr_err("CM: request_irq failed to register hwsem irq %i (%i)\n", 
+			       IRQ_DB8500_HSEM, err);
+			goto out_all;
+		}
+	}
+
+	err = cmdma_init();
+	if (err == 0)
+		return 0;
+
+out_all:
+	cm_debug_exit();
+	free_mpc_irqs(i);
+	CM_ENGINE_Destroy();
+	i=ARRAY_SIZE(cmld_devname);
+out_destroy_device:
+	if (osalEnv.esram_regulator[ESRAM_12])
+		regulator_put(osalEnv.esram_regulator[ESRAM_12]);
+	if (osalEnv.esram_regulator[ESRAM_34])
+		regulator_put(osalEnv.esram_regulator[ESRAM_34]);
+	while (i--)
+		device_destroy(&cmld_class, MKDEV(cmld_major, i));
+	cdev_del(&cmld_cdev);
+out_destroy_chrdev:
+	unregister_chrdev_region(dev, ARRAY_SIZE(cmld_devname));
+out_destroy_class:
+	class_unregister(&cmld_class);
+out:
+	unmapRegions();
+#ifdef 	CM_DEBUG_ALLOC
+	cleanup_debug_alloc();
+#endif
+	return err;
+}
+
+/** Module exit point
+ * Unregister the driver. This will lead to a 'remove' call.
+ */
+static void __exit cmld_cleanup_module(void)
+{
+	unsigned i;
+
+	if (!list_empty(&channel_list))
+		pr_err("CM Driver ending with non empty channel list\n");
+	if (!list_empty(&process_list))
+		pr_err("CM Driver ending with non empty process list\n");
+	
+        if (cfgSemaphoreTypeHSEM)
+		free_irq(IRQ_DB8500_HSEM, NULL);
+	free_mpc_irqs(NB_MPC);
+	tasklet_kill(&cmld_service_tasklet);
+
+	if (osalEnv.esram_regulator[ESRAM_12])
+		regulator_put(osalEnv.esram_regulator[ESRAM_12]);
+	if (osalEnv.esram_regulator[ESRAM_34])
+		regulator_put(osalEnv.esram_regulator[ESRAM_34]);
+	for (i=0; i<ARRAY_SIZE(cmld_devname); i++)
+		device_destroy(&cmld_class, MKDEV(cmld_major, i));
+	cdev_del(&cmld_cdev);
+	unregister_chrdev_region(MKDEV(cmld_major, 0), ARRAY_SIZE(cmld_devname));
+	class_unregister(&cmld_class);
+
+	CM_ENGINE_Destroy();
+
+	cmdma_destroy();
+	unmapRegions();
+#ifdef 	CM_DEBUG_ALLOC
+	cleanup_debug_alloc();
+#endif
+	cm_debug_exit();
+}
+module_init(cmld_init_module);
+module_exit(cmld_cleanup_module);
+
+MODULE_AUTHOR("David Siorpaes");
+MODULE_AUTHOR("Wolfgang Betz");
+MODULE_AUTHOR("Pierre Peiffer");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Nomadik Multiprocessing Framework Component Manager Linux driver");
diff --git a/drivers/staging/nmf-cm/cmld.h b/drivers/staging/nmf-cm/cmld.h
new file mode 100644
index 00000000000..57dc3a4e8f7
--- /dev/null
+++ b/drivers/staging/nmf-cm/cmld.h
@@ -0,0 +1,190 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef CMLD_H
+#define CMLD_H
+
+#include <linux/kref.h>
+#include <linux/mutex.h>
+#include <linux/version.h>
+#include <linux/wait.h>
+#include <inc/nmf-limits.h>
+#include "cmioctl.h"
+
+/** Channel state used within the per-channel private structure 'cm_channel_priv'
+ */
+enum channel_state {
+	CHANNEL_CLOSED = 0,	/**< Channel already closed */
+	CHANNEL_OPEN,		/**< Channel still open */
+};
+
+/** Component Manager per-process private structure
+ *  It is created the first time a process opens /dev/cm0 or /dev/cm1
+ */
+struct cm_process_priv
+{
+	struct kref ref;                  /**< ref count */
+	struct list_head entry;           /**< This entry */
+	pid_t pid;                        /**< pid of process owner */
+	struct mutex mutex;               /**< per process mutex: protect memAreaDescList */
+	struct list_head memAreaDescList; /**< memAreaDesc_t list */
+	struct mutex host2mpcLock;        /**< used to synchronize each AllocEvent + PushEvent */
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dir;               /**< debugfs dir entry under nmf-cm/proc */
+	struct dentry *comp_dir;          /**< debugfs dir entry under nmf-cm/proc/..%components */
+	struct dentry *domain_dir;        /**< debugfs dir entry under nmf-cm/proc/..%domains */
+#endif
+};
+
+/** Component Manager per-channel private structure
+ * It is created when a user opens /dev/cm1
+ */
+struct cm_channel_priv
+{
+	enum channel_state state;         /**< Channel state */
+	struct list_head entry;           /**< This entry */
+	struct cm_process_priv *proc;     /**< Pointer to the owner process structure */
+	struct list_head skelList;        /**< t_skelwrapper list */
+	struct mutex skelListLock;        /**< skelList mutex */
+	struct plist_head messageQueue;   /**< queueelem_t list */
+	struct mutex msgQueueLock;        /**< lock used to synchronize MPC to HOST bindings
+					      in case of multiple read (see cmld_read comments) */
+	spinlock_t bh_lock;               /**< lock used to synchronize add/removal of element in/from
+					      the message queue in both user context and tasklet */
+	wait_queue_head_t waitq;          /**< wait queue used to block read() call */
+};
+
+/** Memory area descriptor.
+ */
+struct memAreaDesc_t {
+	struct list_head list;            /**< Doubly linked list descriptor */
+	atomic_t count;                   /**< Reference counter */
+	pid_t tid;                        /**< tid of the process this area is reserved for */
+	t_cm_memory_handle handle;        /**< Component Manager handle */
+	unsigned int size;                /**< Size */
+	unsigned int physAddr;            /**< Physical address */
+	unsigned int kernelLogicalAddr;   /**< Logical address as seen by kernel */
+	unsigned int userLogicalAddr;     /**< Logical address as seen by user */
+	unsigned int mpcPhysAddr;         /**< Physicaladdress as seen by MPC */
+	struct cm_process_priv* procPriv; /**< link to per process private structure */
+};
+
+extern struct list_head channel_list; /**< List of all allocated channel structures */
+extern struct list_head process_list; /**< List of all allocated process private structure */
+#ifdef CONFIG_DEBUG_FS
+extern bool cmld_user_has_debugfs; /**< Whether user side has proper support of debugfs to take a dump */
+extern pid_t cmld_dump_ongoing; /**< If a dump is on-going, store pid of process doing the dump */
+#endif
+
+/* Structure used to embed DSP traces */
+#define TB_MEDIA_FILE 0x1C
+#define TB_DEV_PC 0x10
+#define TB_DEV_TRACEBOX 0x4C
+#define TB_TRACEBOX 0x7C
+#define DEFAULT_RECEIVERR_OBJ 0x0
+#define DEFAULT_SENDER_OBJ 0x0A
+#define TB_TRACE_MSG 0x94
+#define TB_TRACE_EXCEPTION_MSG 0x95
+#define TB_EXCEPTION_LONG_OVRF_PACKET 0x07
+#define OST_MASTERID 0x08
+#define OST_VERSION 0x05
+#define ENTITY 0xAA
+#define PROTOCOL_ID 0x03
+#define BTRACE_HEADER_SIZE 4
+
+struct __attribute__ ((__packed__)) mmdsp_trace {
+	u8 media;
+	u8 receiver_dev;
+	u8 sender_dev;
+	u8 unused;
+	u16 size;
+	u8 receiver_obj;
+	u8 sender_obj;
+	u8 transaction_id;
+	u8 message_id;
+	u8 master_id;
+	u8 channel_id;
+	u64 timestamp;
+	u8 ost_master_id;
+	u8 ost_version;
+	u8 entity;
+	u8 protocol_id;
+	u8 length;
+	u64 timestamp2;
+	u32 component_id;
+	u32 trace_id;
+	u8 btrace_hdr_size;
+	u8 btrace_hdr_flag;
+	u8 btrace_hdr_category;
+	u8 btrace_hdr_subcategory;
+	u32 parent_handle;
+	u32 component_handle;
+	u32 params[4];
+};
+
+/** Lock/unlock per process mutex
+ *
+ * \note Must be taken before tasklet_disable (if necessary)!
+ */
+#define lock_process_uninterruptible(proc) (mutex_lock(&proc->mutex))
+#define lock_process(proc) (mutex_lock_killable(&proc->mutex))
+#define unlock_process(proc) (mutex_unlock(&proc->mutex))
+
+
+
+int cmld_InstantiateComponent(struct cm_process_priv *, CM_InstantiateComponent_t __user *);
+int cmld_BindComponentFromCMCore(struct cm_process_priv *,
+				 CM_BindComponentFromCMCore_t __user *);
+int cmld_UnbindComponentFromCMCore(CM_UnbindComponentFromCMCore_t __user *);
+int cmld_BindComponentToCMCore(struct cm_channel_priv *, CM_BindComponentToCMCore_t __user *);
+int cmld_UnbindComponentToCMCore(struct cm_process_priv*, CM_UnbindComponentToCMCore_t __user *);
+int cmld_BindComponentAsynchronous(struct cm_process_priv*, CM_BindComponentAsynchronous_t __user *);
+int cmld_UnbindComponentAsynchronous(struct cm_process_priv*, CM_UnbindComponentAsynchronous_t __user *);
+int cmld_BindComponent(struct cm_process_priv*, CM_BindComponent_t __user *);
+int cmld_UnbindComponent(struct cm_process_priv*, CM_UnbindComponent_t __user *);
+int cmld_BindComponentToVoid(struct cm_process_priv*, CM_BindComponentToVoid_t  __user *);
+int cmld_DestroyComponent(struct cm_process_priv*, CM_DestroyComponent_t __user *);
+int cmld_CreateMemoryDomain(struct cm_process_priv*, CM_CreateMemoryDomain_t __user *);
+int cmld_CreateMemoryDomainScratch(struct cm_process_priv*, CM_CreateMemoryDomainScratch_t __user *);
+int cmld_DestroyMemoryDomain(CM_DestroyMemoryDomain_t __user *);
+int cmld_GetDomainCoreId(CM_GetDomainCoreId_t __user *);
+int cmld_AllocMpcMemory(struct cm_process_priv *, CM_AllocMpcMemory_t __user *);
+int cmld_FreeMpcMemory(struct cm_process_priv *, CM_FreeMpcMemory_t __user *);
+int cmld_GetMpcMemoryStatus(CM_GetMpcMemoryStatus_t __user *);
+int cmld_StartComponent(struct cm_process_priv *, CM_StartComponent_t __user *);
+int cmld_StopComponent(struct cm_process_priv *, CM_StopComponent_t __user *);
+int cmld_GetMpcLoadCounter(CM_GetMpcLoadCounter_t __user *);
+int cmld_GetComponentDescription(struct cm_process_priv *, CM_GetComponentDescription_t __user *);
+int cmld_GetComponentListHeader(struct cm_process_priv *, CM_GetComponentListHeader_t __user *);
+int cmld_GetComponentListNext(struct cm_process_priv *, CM_GetComponentListNext_t __user *);
+int cmld_GetComponentRequiredInterfaceNumber(struct cm_process_priv *,
+					     CM_GetComponentRequiredInterfaceNumber_t __user *);
+int cmld_GetComponentRequiredInterface(struct cm_process_priv *,
+				       CM_GetComponentRequiredInterface_t __user *);
+int cmld_GetComponentRequiredInterfaceBinding(struct cm_process_priv *,
+					      CM_GetComponentRequiredInterfaceBinding_t __user *);
+int cmld_GetComponentProvidedInterfaceNumber(struct cm_process_priv *,
+					     CM_GetComponentProvidedInterfaceNumber_t __user *);
+int cmld_GetComponentProvidedInterface(struct cm_process_priv *,
+				       CM_GetComponentProvidedInterface_t __user *);
+int cmld_GetComponentPropertyNumber(struct cm_process_priv *,
+				    CM_GetComponentPropertyNumber_t __user *);
+int cmld_GetComponentPropertyName(struct cm_process_priv *, CM_GetComponentPropertyName_t __user *);
+int cmld_GetComponentPropertyValue(struct cm_process_priv *, CM_GetComponentPropertyValue_t __user *);
+int cmld_ReadComponentAttribute(struct cm_process_priv *, CM_ReadComponentAttribute_t __user *);
+int cmld_WriteComponentAttribute(struct cm_process_priv *, CM_WriteComponentAttribute_t __user *);
+int cmld_GetExecutiveEngineHandle(struct cm_process_priv *, CM_GetExecutiveEngineHandle_t __user *);
+int cmld_SetMode(CM_SetMode_t __user *);
+int cmld_GetRequiredComponentFiles(struct cm_process_priv *cmPriv,
+				   CM_GetRequiredComponentFiles_t __user *);
+int cmld_Migrate(CM_Migrate_t __user *);
+int cmld_Unmigrate(CM_Unmigrate_t __user *);
+int cmld_SetupRelinkArea(struct cm_process_priv *, CM_SetupRelinkArea_t __user *);
+int cmld_PushComponent(CM_PushComponent_t __user *);
+int cmld_ReleaseComponent(CM_ReleaseComponent_t __user *);
+int cmld_PrivGetMPCMemoryDesc(struct cm_process_priv *, CM_PrivGetMPCMemoryDesc_t __user *);
+int cmld_PrivReserveMemory(struct cm_process_priv *, unsigned int);
+#endif
diff --git a/drivers/staging/nmf-cm/configuration.c b/drivers/staging/nmf-cm/configuration.c
new file mode 100644
index 00000000000..523874fc586
--- /dev/null
+++ b/drivers/staging/nmf-cm/configuration.c
@@ -0,0 +1,146 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file configuration.c
+ *
+ * Nomadik Multiprocessing Framework Linux Driver
+ *
+ */
+
+#include <linux/module.h>
+#include <cm/engine/api/configuration_engine.h>
+#include <cm/engine/configuration/inc/configuration_status.h>
+#include <cm/engine/power_mgt/inc/power.h>
+#include "osal-kernel.h"
+
+/* Per-driver environment */
+struct OsalEnvironment osalEnv =
+{
+	.mpc = {
+		{
+			.coreId           = SVA_CORE_ID,
+			.name             = "sva",
+			.base_phys        = (void*)U8500_SVA_BASE,
+			.interrupt0       = IRQ_DB8500_SVA,
+			.interrupt1       = IRQ_DB8500_SVA2,
+			.mmdsp_regulator  = NULL,
+			.pipe_regulator   = NULL,
+			.monitor_tsk      = NULL,
+			.hwmem_code       = NULL,
+			.hwmem_data       = NULL,
+			.trace_read_count = ATOMIC_INIT(0),
+		},
+		{
+			.coreId           = SIA_CORE_ID,
+			.name             = "sia",
+			.base_phys        = (void*)U8500_SIA_BASE,
+			.interrupt0       = IRQ_DB8500_SIA,
+			.interrupt1       = IRQ_DB8500_SIA2,
+			.mmdsp_regulator  = NULL,
+			.pipe_regulator   = NULL,
+			.monitor_tsk      = NULL,
+			.hwmem_code       = NULL,
+			.hwmem_data       = NULL,
+			.trace_read_count = ATOMIC_INIT(0),
+		}
+	},
+	.esram_regulator = { NULL, NULL},
+	.dsp_sleep = {
+		.sia_auto_pm_enable = PRCMU_AUTO_PM_OFF,
+		.sia_power_on       = 0,
+		.sia_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF,
+		.sva_auto_pm_enable = PRCMU_AUTO_PM_OFF,
+		.sva_power_on       = 0,
+		.sva_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF,		
+	},
+	.dsp_idle = {
+		.sia_auto_pm_enable = PRCMU_AUTO_PM_OFF,
+		.sia_power_on       = 0,
+		.sia_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF,
+		.sva_auto_pm_enable = PRCMU_AUTO_PM_OFF,
+		.sva_power_on       = 0,
+		.sva_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF,		
+	},
+};
+
+module_param_call(cm_debug_level, param_set_int, param_get_int,
+                  &cm_debug_level, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(cm_debug_level, "Debug level of NMF Core");
+
+module_param_call(cm_error_break, param_set_bool, param_get_bool,
+                  &cm_error_break, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(cm_error_break, "Stop on error (in an infinite loop, for debugging purpose)");
+
+module_param_call(cmIntensiveCheckState, param_set_bool, param_get_bool,
+                  &cmIntensiveCheckState, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(cmIntensiveCheckState, "Add additional intensive checks");
+
+DECLARE_MPC_PARAM(SVA, SDRAM_DATA_SIZE, "", 1);
+
+DECLARE_MPC_PARAM(SIA, 0, "\n\t\t\t(0 means shared with SVA)", 2);
+
+bool cfgCommunicationLocationInSDRAM = true;
+module_param(cfgCommunicationLocationInSDRAM, bool, S_IRUGO);
+MODULE_PARM_DESC(cfgCommunicationLocationInSDRAM, "Location of communications (SDRAM or ESRAM)");
+
+bool cfgSemaphoreTypeHSEM = true;
+module_param(cfgSemaphoreTypeHSEM, bool, S_IRUGO);
+MODULE_PARM_DESC(cfgSemaphoreTypeHSEM, "Semaphore used (HSEM or LSEM)");
+
+int cfgESRAMSize = ESRAM_SIZE;
+module_param(cfgESRAMSize, uint, S_IRUGO);
+MODULE_PARM_DESC(cfgESRAMSize, "Size of ESRAM used in the CM (in Kb)");
+
+static int set_param_powerMode(const char *val, const struct kernel_param *kp)
+{
+	/* No equals means "set"... */
+	if (!val) val = "1";
+
+	/* One of =[yYnN01] */
+	switch (val[0]) {
+	case 'y': case 'Y': case '1':
+		CM_ENGINE_SetMode(CM_CMD_DBG_MODE, 0);
+		break;
+	case 'n': case 'N': case '0':
+		CM_ENGINE_SetMode(CM_CMD_DBG_MODE, 1);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+module_param_call(powerMode, set_param_powerMode, param_get_bool, &powerMode, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(powerMode, "DSP power mode enable");
+
+int init_config(void)
+{
+	if (cfgMpcSDRAMCodeSize_SVA == 0 || cfgMpcSDRAMCodeSize_SIA == 0) {
+		pr_err("SDRAM code size must be greater than 0\n");
+		return -EINVAL;
+	}
+
+	if (cfgMpcSDRAMDataSize_SVA == 0) {
+		pr_err("SDRAM data size for SVA must be greater than 0\n");
+		return -EINVAL;
+	}
+
+	osalEnv.mpc[SVA].nbYramBanks     = cfgMpcYBanks_SVA;
+	osalEnv.mpc[SVA].eeId            = cfgSchedulerTypeHybrid_SVA ? HYBRID_EXECUTIVE_ENGINE : SYNCHRONOUS_EXECUTIVE_ENGINE;
+	osalEnv.mpc[SVA].sdram_code.size = cfgMpcSDRAMCodeSize_SVA * ONE_KB;
+	osalEnv.mpc[SVA].sdram_data.size = cfgMpcSDRAMDataSize_SVA * ONE_KB;
+	osalEnv.mpc[SVA].base.size       = 128*ONE_KB; //we expose only TCM24
+	spin_lock_init(&osalEnv.mpc[SVA].trace_reader_lock);
+
+	osalEnv.mpc[SIA].nbYramBanks     = cfgMpcYBanks_SIA;
+	osalEnv.mpc[SIA].eeId            = cfgSchedulerTypeHybrid_SIA ? HYBRID_EXECUTIVE_ENGINE : SYNCHRONOUS_EXECUTIVE_ENGINE;
+	osalEnv.mpc[SIA].sdram_code.size = cfgMpcSDRAMCodeSize_SIA * ONE_KB;
+	osalEnv.mpc[SIA].sdram_data.size = cfgMpcSDRAMDataSize_SIA * ONE_KB;
+	osalEnv.mpc[SIA].base.size       = 128*ONE_KB; //we expose only TCM24
+	spin_lock_init(&osalEnv.mpc[SIA].trace_reader_lock);
+
+	return 0;
+}
diff --git a/drivers/staging/nmf-cm/configuration.h b/drivers/staging/nmf-cm/configuration.h
new file mode 100644
index 00000000000..39416cde686
--- /dev/null
+++ b/drivers/staging/nmf-cm/configuration.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef CONFIGURATION_H
+#define CONFIGURATION_H
+
+/** Peripherals description.
+ * Some of these values are taken from kernel header description (which should be the
+ * right place of these definition); the missing ones are defined here.
+ */
+
+#include <linux/version.h>
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,32)
+#include <generated/autoconf.h>
+#else
+#include <linux/autoconf.h>
+#endif
+
+/* Embedded Static RAM base address */
+/* config: 0-64k: secure */
+#define ESRAM_BASE (U8500_ESRAM_BASE + U8500_ESRAM_DMA_LCPA_OFFSET)
+
+/*
+ * Embedded ram size for CM (in Kb)
+ * 5 banks of 128k: skip the first half bank (secure) and the last
+ * one (used for MCDE/B2R2), but include DMA part (4k after the secure part)
+ * to give access from DSP side
+ */
+#define ESRAM_SIZE 448
+enum {
+	ESRAM_12,
+	ESRAM_34,
+	NB_ESRAM,
+};
+
+/** MPCs */
+enum {
+	SVA,
+	SIA,
+	NB_MPC,
+};
+#define COREIDX(id) (id-1)
+
+/** Base address of shared SDRAM: use upper SDRAM. We reserve a rather */
+#define SDRAM_CODE_SIZE_SVA (2*ONE_KB)
+#define SDRAM_CODE_SIZE_SIA (2*ONE_KB)
+#define SDRAM_DATA_SIZE     (8*ONE_KB)
+
+extern bool cfgCommunicationLocationInSDRAM;
+extern bool cfgSemaphoreTypeHSEM;
+extern int cfgESRAMSize;
+
+int init_config(void);
+
+#define DECLARE_MPC_PARAM(mpc, sdramDataSize, extension, ybank)		\
+	static unsigned int cfgMpcYBanks_##mpc = ybank;			\
+	module_param(cfgMpcYBanks_##mpc, uint, S_IRUGO);		\
+	MODULE_PARM_DESC(cfgMpcYBanks_##mpc, "Nb of Y-Ram banks used on " #mpc); \
+									\
+	static bool         cfgSchedulerTypeHybrid_##mpc = 1;		\
+	module_param(cfgSchedulerTypeHybrid_##mpc, bool, S_IRUGO);	\
+	MODULE_PARM_DESC(cfgSchedulerTypeHybrid_##mpc, "Scheduler used on " #mpc " (Hybrid or Synchronous)"); \
+									\
+	static unsigned int cfgMpcSDRAMCodeSize_##mpc = SDRAM_CODE_SIZE_##mpc; \
+	module_param(cfgMpcSDRAMCodeSize_##mpc, uint, S_IRUGO);		\
+	MODULE_PARM_DESC(cfgMpcSDRAMCodeSize_##mpc, "Size of code segment on " #mpc " (in Kb)"); \
+									\
+	static unsigned int cfgMpcSDRAMDataSize_##mpc = sdramDataSize;	\
+	module_param(cfgMpcSDRAMDataSize_##mpc, uint, S_IRUGO);		\
+	MODULE_PARM_DESC(cfgMpcSDRAMDataSize_##mpc, "Size of data segment on " #mpc " (in Kb)" extension)
+
+#endif
diff --git a/drivers/staging/nmf-cm/ee/api/panic.idt b/drivers/staging/nmf-cm/ee/api/panic.idt
new file mode 100644
index 00000000000..71996b8a55e
--- /dev/null
+++ b/drivers/staging/nmf-cm/ee/api/panic.idt
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \defgroup NMF_EE_TYPE Execution Engine Common Type Definitions
+ * \ingroup COMMON
+ */
+
+#ifndef __INC_PANIC_IDT
+#define __INC_PANIC_IDT
+
+/*!
+ * \brief Panic reason type
+ *
+ * For values, see \ref t_panic_reasonDescription.
+ *
+ * \ingroup NMF_EE_TYPE
+ */
+typedef t_uint8 t_panic_reason;
+
+/*!
+ * \brief The different panic reasons
+ *
+ * \verbatim
+ *  Reason                   | Information                       | Behavior
+ *  -------------------------------------------------------------------
+ *  INTERNAL_PANIC           | Not interpreted                   | Fatal panic, stop MPC
+ *  MPC_NOT_RESPONDING_PANIC | Not interpreted                   | Fatal panic, stop MPC
+ *  USER_STACK_OVERFLOW      | Faulting address & SPu            | Fatal panic, stop MPC
+ *  SYSTEM_STACK_OVERFLOW    | Faulting address &  SPu           | Fatal panic, stop MPC
+ *  UNALIGNED_LONG_ACCESS    | Indicative Faulting address & SPu | Fatal panic, stop MPC
+ *  EVENT_FIFO_OVERFLOW      | 0                                 | Abort current task, stop MPC
+ *  PARAM_FIFO_OVERFLOW      | 0                                 | idem
+ *  INTERFACE_NOT_BINDED     | 0                                 | idem
+ *  USER_PANIC               | Not interpreted                   | idem
+ *  UNBIND_INTERRUPT         | Interrupt number                  | Do nothing, just return from interrupt.
+ *  EVENT_FIFO_IN_USE        | Destroy event Fifo while event already schedule (only for HostEE)
+ * \endverbatim
+ *
+ * \ingroup NMF_EE_TYPE
+ */
+typedef enum {
+    INTERNAL_PANIC =                 1,
+    MPC_NOT_RESPONDING_PANIC =       2,
+    USER_STACK_OVERFLOW =            3,
+    SYSTEM_STACK_OVERFLOW =          4,
+    UNALIGNED_LONG_ACCESS =          5,
+    EVENT_FIFO_OVERFLOW =            6,
+    PARAM_FIFO_OVERFLOW =            7,
+    INTERFACE_NOT_BINDED =           8,
+    USER_PANIC =                     9,
+    UNBIND_INTERRUPT =               10,
+    EVENT_FIFO_IN_USE =              11,
+    RESERVED_PANIC =                 2 //for COMPATIBILITY with previous versions of NMF, to be deprecated
+} t_panic_reasonDescription;
+
+/*!
+ * \brief Define the source of the panic
+ *
+ * It indicates the source core of the panic message.\n
+ * It gives the member to use within \ref t_nmf_panic_data (which is a member of the t_nmf_service_data service data structure).
+
+ * \ingroup NMF_EE_TYPE
+ */
+typedef enum {
+	HOST_EE, //!< If the source is the Executive Engine running on the ARM Core
+	MPC_EE   //!< If the source is the Executive Engine running on one of the MPC Core
+} t_panic_source;
+
+#endif
diff --git a/drivers/staging/nmf-cm/ee/api/trace.idt b/drivers/staging/nmf-cm/ee/api/trace.idt
new file mode 100644
index 00000000000..f4d4c8615e2
--- /dev/null
+++ b/drivers/staging/nmf-cm/ee/api/trace.idt
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \defgroup NMF_EE_TYPE Execution Engine Common Type Definitions
+ * \ingroup COMMON
+ */
+
+#ifndef __INC_TRACE_IDT
+#define __INC_TRACE_IDT
+
+struct t_nmf_trace
+{
+    t_uint32 revision;
+    t_uint32 timeStamp;
+    t_uint32 componentId;
+    t_uint32 traceId;
+    t_uint32 paramOpt;
+    t_uint32 componentHandle;
+    t_uint32 parentHandle;
+    t_uint32 params[4];
+};
+
+#define TRACE_BUFFER_SIZE                          128
+
+#endif
diff --git a/drivers/staging/nmf-cm/inc/nmf-def.h b/drivers/staging/nmf-cm/inc/nmf-def.h
new file mode 100644
index 00000000000..439adf8ef56
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/nmf-def.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ /*!
+ * \brief NMF Version.
+ *
+ * This file contains the NMF Version.
+ *
+ * \defgroup NMF_VERSION NMF Version
+ * \ingroup COMMON
+ */
+
+#ifndef __INC_NMF_DEF_H
+#define __INC_NMF_DEF_H
+
+/*!
+ * \brief Current NMF version number
+ *
+ * \ingroup NMF_VERSION
+ */
+#define NMF_VERSION ((2 << 16) | (10 << 8) | (123))
+
+/*!
+ * \brief Get NMF major version corresponding to NMF version number
+ * \ingroup NMF_VERSION
+ */
+#define VERSION_MAJOR(version)  (((version) >> 16) & 0xFF)
+/*!
+ * \brief Get NMF minor version corresponding to NMF version number
+ * \ingroup NMF_VERSION
+ */
+#define VERSION_MINOR(version)  (((version) >> 8) & 0xFF)
+/*!
+ * \brief Get NMF patch version corresponding to NMF version number
+ * \ingroup NMF_VERSION
+ */
+#define VERSION_PATCH(version)  (((version) >> 0) & 0xFF)
+
+#endif /* __INC_NMF_DEF_H */
diff --git a/drivers/staging/nmf-cm/inc/nmf-limits.h b/drivers/staging/nmf-cm/inc/nmf-limits.h
new file mode 100644
index 00000000000..374795f91e0
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/nmf-limits.h
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Common Nomadik Multiprocessing Framework limits definition
+ *
+ * This file contains the limit definitions used into NMF.
+ *
+ * \warning Don't modify it since it is also hardcoded in tools
+ *
+ * \defgroup NMF_LIMITS NMF limits definition
+ * \ingroup COMMON
+ */
+#ifndef __INC_NMF_LIMITS_H
+#define __INC_NMF_LIMITS_H
+
+/*!
+ * \brief Maximum interface name length
+ *
+ * Define the maximum interface name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_INTERFACE_NAME_LENGTH               32
+
+/*!
+ * \brief Maximum interface method name length
+ *
+ * Define the maximum interface method name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_INTERFACE_METHOD_NAME_LENGTH        64
+
+/*!
+ * \brief Maximum interface type name length
+ *
+ * Define the maximum interface type name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_INTERFACE_TYPE_NAME_LENGTH          128
+
+
+/*!
+ * \brief Maximum template name length
+ *
+ * Define the maximum template name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_TEMPLATE_NAME_LENGTH                128
+
+/*!
+ * \brief Maximum component local name length
+ *
+ * Define the maximum component local name length inside a composite component allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_COMPONENT_NAME_LENGTH               32
+
+/*!
+ * \brief Maximum property name length
+ *
+ * Define the maximum property name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_PROPERTY_NAME_LENGTH                32
+
+/*!
+ * \brief Maximum property value length
+ *
+ * Define the maximum property value length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_PROPERTY_VALUE_LENGTH               128
+
+/*!
+ * \brief Maximum attribute name length
+ *
+ * Define the maximum attribute name length allowed by NMF.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_ATTRIBUTE_NAME_LENGTH               32
+
+/*!
+ * \brief Maximum fifo size allowed for binding component
+ *
+ * Define the maximum fifo size allowed for binding component allowed by NMF when calling
+ * CM_BindComponentFromHost and CM_BindComponentAsynchronous.
+ *
+ * \ingroup NMF_LIMITS
+ */
+#define MAX_COMMUNICATION_FIFO_SIZE             256
+
+#define MAX_COMPONENT_FILE_PATH_LENGTH          1024
+
+#endif /* __INC_NMF_LIMITS_H */
diff --git a/drivers/staging/nmf-cm/inc/nmf-tracedescription.h b/drivers/staging/nmf-cm/inc/nmf-tracedescription.h
new file mode 100644
index 00000000000..bce589079b9
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/nmf-tracedescription.h
@@ -0,0 +1,323 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief NMF xti/stm trace format description
+ *
+ * \defgroup NMF_TRACE_FORMAT NMF xti/stm trace format description
+ *
+ * The NMF trace is output by either xti ip on 8815 or stm ip on 8820 and 8500.
+ * Each type of trace is output on a dedicated channel. Following is a description
+ * of each of this traces.
+ *
+ * Traces have generally a timestamp added by hardware but is not described here.
+ * \ingroup NMF_ABI
+ */
+#ifndef TRACE_FORMAT_H_
+#define TRACE_FORMAT_H_
+
+#include <inc/nmf-limits.h>
+
+/*!
+ * \brief XTI/STM Channel where trace are dumped
+ *
+ * \note This type is only for defining constants, please not reference it.
+ *
+ * \note Ever if this format is able to be generated on same channel, Host EE & CM channel are separated
+ * in order to avoir concurrency and access STM IP without require mutual exclusion.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    MPC_EE_CHANNEL      = 100,                  //!< MPC EE channel (MPC activity) in 32bits bundle
+    CM_CHANNEL          = 101,                  //!< CM channel (MPC deployment) in 64bits bundle
+    HOST_EE_CHANNEL     = 151                   //!< Host EE channel (deployment & activity) in 64bits bundle
+} t_nmfTraceChannelDescription;
+
+/*!
+ * \brief Message trace type
+ *
+ * \note This type is only for defining constants, please not reference it.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_TYPE_RESET            = 1,                //!< Reset trace type
+    TRACE_TYPE_COMPONENT        = 2,                //!< Component instantiate trace type
+    TRACE_TYPE_BIND             = 3,                //!< Component bind trace type
+    TRACE_TYPE_METHOD           = 4,                //!< Component method trace type
+    TRACE_TYPE_ACTIVITY         = 5,                //!< Activity trace type
+    TRACE_TYPE_PANIC            = 6,                //!< Panic trace type
+    TRACE_TYPE_COMMUNICATION    = 7,                //!< Communication trace type
+    TRACE_TYPE_ALLOCATOR        = 8,                //!< Allocator trace type
+    TRACE_TYPE_ALLOC            = 9,                //!< Alloc trace type
+    TRACE_TYPE_USER             = 10                //!< User trace type
+} t_nmfTraceTypeDescription;
+
+#define TRACE_MAJOR_VERSION         1       //!< Current major trace version number \ingroup NMF_TRACE_FORMAT
+#define TRACE_MINOR_VERSION         2       //!< Current minor trace version number \ingroup NMF_TRACE_FORMAT
+
+/*!
+ * \brief Trace header description.
+ *
+ * \note XTI will add 64bits time-stamp in first field of this structure, but not generated by us !
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceChannelHeader {
+    // t_uint64           timeStamp;
+    t_uint8             traceType;          //!< Trace type
+    t_uint8             reserved;
+    t_uint16            traceSize;          //!< Trace size (depending on trace type description)
+};
+
+/*!
+ * \brief Trace header union
+ *
+ * The purpose of this is to optimize header setting in one instruction.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef union {
+    struct t_nmfTraceChannelHeader   s;
+    t_uint32                         v;
+} t_nmfTraceChannelHeaderUnion;
+
+
+/*!
+ * \brief Trace reset description
+ *
+ * Inform tools to reset their internal state because network will be dumped new time.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceReset {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16                       minorVersion;                        //!< NMF trace format minor version
+    t_uint16                       majorVersion;                        //!< NMF trace format major version
+};
+
+/**
+ * \brief Component instantiation trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_COMPONENT_COMMAND_ADD         = 0x1,
+    TRACE_COMPONENT_COMMAND_REMOVE      = 0x2
+} t_nmfTraceComponentCommandDescription;
+
+
+/*!
+ * \brief Component instantiation trace description
+ *
+ * Component instantiation trace is generated each time an instance of a component is added or removed.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceComponent {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceComponentCommandDescription
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA, in SMPEE: 0x1
+    t_uint32            componentContext;                               //!< Component context belonging domain (DSP this or ARM class this)
+    t_uint32            componentUserContext;                           //!< User friendly component Id belonging the channel (CM handle or ARM class this)
+    t_uint8             componentLocalName[MAX_COMPONENT_NAME_LENGTH];  //!< local name of component as given by user (null terminated)
+    t_uint8             componentTemplateName[MAX_TEMPLATE_NAME_LENGTH];//!< template name of component  (null terminated)
+};
+
+/**
+ * \brief Component binding trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_BIND_COMMAND_BIND_SYNCHRONOUS         = 0x1,
+    TRACE_BIND_COMMAND_UNBIND_SYNCHRONOUS       = 0x2,
+    TRACE_BIND_COMMAND_BIND_ASYNCHRONOUS        = 0x3,
+    TRACE_BIND_COMMAND_UNBIND_ASYNCHRONOUS      = 0x4
+} t_nmfTraceBindCommandDescription;
+
+/**
+ * \brief Component binding trace description
+ *
+ * \note clientComponentContext & serverComponentContext take value 0xffffffff when client or server are Component Manager.
+ * \note serverComponentContext take value 0x00000000 when binding to void.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceBind {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceBindCommandDescription
+    t_uint16            reserved;
+    t_uint16            clientDomainId;                                 //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA, in SMPEE: 0x1
+    t_uint16            serverDomainId;                                 //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA, in SMPEE: 0x1
+    t_uint32            clientComponentContext;                         //!< Component context belonging domain (DSP this or ARM class this)
+    t_uint32            serverComponentContext;                         //!< Component context belonging domain (DSP this or ARM class this)
+    t_uint8             requiredItfName[MAX_INTERFACE_NAME_LENGTH];     //!< Required interface name
+    t_uint8             providedItfName[MAX_INTERFACE_NAME_LENGTH];     //!< Provided interface name
+};
+
+/*!
+ * \brief Component interface method name trace description
+ *
+ * For each methods of each interfaces provided by a component, one such trace is dumped.
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceMethod {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA, in SMPEE: 0x1
+    t_uint16            reserved;
+    t_uint32            methodId;                                       //!< Unique Method Id belonging the component
+    t_uint32            componentContext;                               //!< Component context belonging domain (DSP this or ARM class this)
+    t_uint8             methodName[MAX_INTERFACE_METHOD_NAME_LENGTH];   //!< Symbolic method name
+};
+
+/**
+ * \brief Activity trace trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_ACTIVITY_START                    = 0x1,                  //!< Start method
+    TRACE_ACTIVITY_END                      = 0x2,                  //!< End method
+    TRACE_ACTIVITY_POST                     = 0x3,                  //!< Post method
+    TRACE_ACTIVITY_CALL                     = 0x4,                  //!< Synchronous call method
+    TRACE_ACTIVITY_RETURN                   = 0x5                   //!< Synchronous return method
+} t_nmfTraceActivityCommandDescription;
+
+/*!
+ * \brief Execution Engine scheduling activity trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceActivity {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceActivityCommandDescription
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+    t_uint32            componentContext;                               //!< Unique component Id (Component Handle for CM, Component this for EE)
+    t_uint32            methodId;                                       //!< Unique Method Id belonging the component
+};
+
+/**
+ * \brief Component instantiation trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_COMMUNICATION_COMMAND_SEND         = 0x1,
+    TRACE_COMMUNICATION_COMMAND_RECEIVE      = 0x2
+} t_nmfTraceCommunicationCommandDescription;
+
+/**
+ * \brief Inter-processor communication signaling trace description
+ *
+ * Use when trigging interrupt through core.
+ *
+ * \note Not used on SMP EE
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceCommunication {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceCommunicationCommandDescription
+    t_uint16            reserved_0;
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+    t_uint16            remoteDomainId;                                 //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+};
+
+/**
+ * \brief Component instantiation trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_ALLOCATOR_COMMAND_CREATE      = 0x1,
+    TRACE_ALLOCATOR_COMMAND_DESTROY     = 0x2
+} t_nmfTraceAllocatorCommandDescription;
+
+/*!
+ * \brief Panic trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceAllocator {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceAllocatorCommandDescription
+    t_uint16            allocId;                                        //!< Memory allocator ID
+    t_uint32            size;                                           //!< Memory allocator size
+    t_uint8             name[32];                                       //!< Memory allocator name
+};
+
+/**
+ * \brief Component instantiation trace command description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+typedef enum {
+    TRACE_ALLOC_COMMAND_ALLOC           = 0x1,
+    TRACE_ALLOC_COMMAND_FREE            = 0x2
+} t_nmfTraceAllocCommandDescription;
+
+/*!
+ * \brief Panic trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceAlloc {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            command;                                        //!< See \ref t_nmfTraceAllocatorCommandDescription
+    t_uint16            allocId;                                        //!< Memory allocator ID
+    t_uint32            offset;                                         //!< Memory chunk offet
+    t_uint32            size;                                           //!< Memory chunk size
+};
+
+/*!
+ * \brief Panic trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTracePanic {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint16            reason;                                         //!< See \ref t_panic_reason for description
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+    t_uint32            componentContext;                               //!< Unique component Id (Component Handle for CM, Component this for EE)
+    t_uint32            information1;                                   //!< Reason dependent information 1st
+    t_uint32            information2;                                   //!< Reason dependent information 2nd
+};
+
+/*!
+ * \brief User trace description
+ *
+ * \ingroup NMF_TRACE_FORMAT
+ */
+struct t_nmfTraceUser {
+    t_nmfTraceChannelHeaderUnion   header;                              //!< Trace header
+
+    t_uint32            key;                                            //!< User key
+    t_uint16            domainId;                                       //!< In CM: 0x01:Arm | 0x02:SAA | 0x03:SVA | 0x04:SIA
+    t_uint16            reserved;
+    t_uint32            componentContext;                               //!< Unique component Id (Component Handle for CM, Component this for EE)
+    t_uint32            callerAddress;                                  //!< Unique code address belonging the component
+};
+
+/*
+struct t_nmfTracePower{
+    struct t_nmfTraceChannelHeader   header;
+};
+*/
+
+#endif /* TRACE_FORMAT_H_ */
diff --git a/drivers/staging/nmf-cm/inc/nmf_type.idt b/drivers/staging/nmf-cm/inc/nmf_type.idt
new file mode 100644
index 00000000000..dda547a463e
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/nmf_type.idt
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+#ifndef NMF_TYPE_H_
+#define NMF_TYPE_H_
+
+/*!
+ * \defgroup NMF_COMMON_TYPE NMF Common Type
+ * \ingroup COMMON
+ */
+
+/*!
+ * \brief Error type returned by NMF  API routines
+ *
+ * Possible value describe by \ref t_nmf_errorDescription
+ *
+ * \ingroup NMF_COMMON_TYPE
+ */
+typedef t_sint8 t_nmf_error;
+
+/*!
+ * \brief Error type values
+ *
+ * \ingroup NMF_COMMON_TYPE
+ */
+typedef enum {
+    NMF_OK =                                 0,        //!< No error
+    NMF_INVALID_PARAMETER =                  -2,       //!< Invalid parameter
+    NMF_NO_MORE_MEMORY =                     -30,      //!< Out of memory
+    NMF_INTERFACE_NOT_BINDED =               -59,      //!< Try to unbind not binded interface
+    NMF_INTERFACE_ALREADY_BINDED =           -60,      //!< Try to bind already binded interface
+    NMF_NO_SUCH_REQUIRED_INTERFACE =         -63,      //!< Interface name not required by a component
+    NMF_NO_SUCH_PROVIDED_INTERFACE =         -64,      //!< Interface name not provided by a component
+    NMF_COMPONENT_NOT_STOPPED =              -80,      //!< Component must be stopped to perform operation
+    NMF_INVALID_COMPONENT_STATE_TRANSITION = -81,      //!< Invalid component state transition caused by user action
+    NMF_NO_SUCH_PROPERTY =                   -87,      //!< Property name doesn't exported by the underlying component
+    NMF_NO_SUCH_ATTRIBUTE =                  -88,      //!< Attribute name not shared (exported) by a component
+    NMF_NO_MESSAGE =                         -103,     //!< No message available
+    NMF_FLUSH_MESSAGE =                      -106,     //!< Message send after call to EE_FlushChannel()
+    NMF_INTEGRATION_ERROR0 =                 -112,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR1 =                 -113,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR2 =                 -114,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR3 =                 -115,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR4 =                 -116,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR5 =                 -117,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR6 =                 -118,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR7 =                 -119,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR8 =                 -120,     //!< OS dependent integration Error [-112 -> -121]
+    NMF_INTEGRATION_ERROR9 =                 -121      //!< OS dependent integration Error [-112 -> -121]
+} t_nmf_errorDescription;
+
+/*!
+ * \brief Define t_nmf_channel type that identify a communication channel between nmf and user.
+ *
+ * \ingroup NMF_COMMON_TYPE
+ */
+typedef t_uint32 t_nmf_channel;
+
+#endif /* NMF_TYPE_H_ */
diff --git a/drivers/staging/nmf-cm/inc/type.h b/drivers/staging/nmf-cm/inc/type.h
new file mode 100644
index 00000000000..3075505aee5
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/type.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/* inc/type.h - Programming Model.
+ *
+ * Copyright (c) 2006, 2007, 2008 STMicroelectronics.
+ *
+ * Reproduction and Communication of this document is strictly prohibited
+ * unless specifically authorized in writing by STMicroelectronics.
+ *
+ * Written by NMF team.
+ */
+#ifndef _NMF_TYPE_H_
+#define _NMF_TYPE_H_
+
+#include <inc/typedef.h>
+
+PUBLIC IMPORT_SHARED void NMF_LOG(const char* fmt, ...);
+PUBLIC IMPORT_SHARED void NMF_PANIC(const char* fmt, ...);
+
+#define NMF_ASSERT(cond)  do { if(!(cond)) NMF_PANIC("NMF_ASSERT at %s:%d\n", (int)__FILE__, (int)__LINE__); } while(0)
+
+#ifndef EXPORT_NMF_COMPONENT
+    #define EXPORT_NMF_COMPONENT    EXPORT_SHARED
+#endif
+
+#ifndef IMPORT_NMF_COMPONENT
+    #define IMPORT_NMF_COMPONENT    IMPORT_SHARED
+#endif
+
+#endif /* _NMF_TYPE_H_ */
diff --git a/drivers/staging/nmf-cm/inc/typedef.h b/drivers/staging/nmf-cm/inc/typedef.h
new file mode 100644
index 00000000000..a29e6b88fde
--- /dev/null
+++ b/drivers/staging/nmf-cm/inc/typedef.h
@@ -0,0 +1,192 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \defgroup COMMON Common types and definitions
+ *
+ * \defgroup NMF_COMMON NMF common definition
+ * \ingroup COMMON
+ *
+ * \defgroup NMF_ABI NMF ABI specification
+ * \warning This page is not for multimedia developers !
+ */
+/*!
+ * \brief Primitive Type Definition
+ *
+ * \defgroup NMF_PRIMITIVE_TYPE Primitive type definition
+ * \ingroup COMMON
+ */
+
+#ifndef NMF_TYPEDEF_H_
+#define NMF_TYPEDEF_H_
+
+#undef PRIVATE
+#define PRIVATE  static                 //!< Private macro declaration \ingroup NMF_PRIMITIVE_TYPE
+
+#undef PUBLIC
+#ifdef __cplusplus
+#define PUBLIC extern "C"               //!< Public macro declaration \ingroup NMF_PRIMITIVE_TYPE
+#else
+#define PUBLIC extern                   //!< Public macro declaration \ingroup NMF_PRIMITIVE_TYPE
+#endif
+
+#if defined(__SYMBIAN32__)
+/*!
+ * \brief Declared IMPORT_SHARED to allow dll/shared library creation
+ *
+ * \note Value depend on OS.
+ *
+ * \ingroup NMF_PRIMITIVE_TYPE
+ */
+    #ifndef IMPORT_SHARED
+        #define IMPORT_SHARED   IMPORT_C
+    #endif
+/*!
+ * \brief Declared EXPORT_SHARED to allow dll/shared library creation
+ *
+ * \note Value depend on OS.
+ *
+ * \ingroup NMF_PRIMITIVE_TYPE
+ */
+    #ifndef EXPORT_SHARED
+        #define EXPORT_SHARED   EXPORT_C
+    #endif
+#elif defined(LINUX)
+    #ifndef IMPORT_SHARED
+        #define IMPORT_SHARED
+    #endif
+    #ifndef EXPORT_SHARED
+        #define EXPORT_SHARED   __attribute__ ((visibility ("default")))
+    #endif
+#else
+    #ifndef IMPORT_SHARED
+        #define IMPORT_SHARED
+    #endif
+
+    #ifndef EXPORT_SHARED
+        #define EXPORT_SHARED
+    #endif
+#endif
+
+/*
+ * Definition of type that are used by interface.
+ */
+
+typedef unsigned int t_uword;
+typedef signed int t_sword;
+
+#ifdef __flexcc2__
+
+typedef unsigned char t_bool;
+
+#ifdef __mode16__
+
+typedef signed char t_sint8;
+typedef signed int t_sint16;
+typedef signed long t_sint24;
+typedef signed long t_sint32;
+typedef signed long long t_sint40;
+// bigger type are not handle on this mode
+
+typedef unsigned char t_uint8;
+typedef unsigned int t_uint16;
+typedef unsigned long t_uint24;
+typedef unsigned long t_uint32;
+typedef unsigned long long t_uint40;
+// bigger type are not handle on this mode
+
+// shared addr type definition
+//typedef __SHARED16 t_uint16 * t_shared_addr;
+typedef void * t_shared_field;
+
+#else /* __mode16__ -> __mode24__ */
+
+typedef signed char t_sint8;
+typedef signed short t_sint16;
+typedef signed int t_sint24;
+typedef signed long t_sint32;
+typedef signed long t_sint40;
+typedef signed long t_sint48;
+typedef signed long long t_sint56;
+
+typedef unsigned char t_uint8;
+typedef unsigned short t_uint16;
+typedef unsigned int t_uint24;
+typedef unsigned long t_uint32;
+typedef unsigned long t_uint40;
+typedef unsigned long t_uint48;
+typedef unsigned long long t_uint56;
+
+// shared addr type definition
+//typedef __SHARED16 t_uint16 * t_shared_addr;
+typedef t_uint24 t_shared_field;
+
+#endif /* MMDSP mode24 */
+
+// shared register (ARM world) type definition
+#if 0
+typedef struct {
+    t_uint16 lsb;
+    t_uint16 msb;
+} t_shared_reg;
+#endif
+typedef t_uint32 t_shared_reg;
+
+typedef t_uint32 t_physical_address;
+
+#include <stwdsp.h>
+
+#else /* __flexcc2__ -> RISC 32 Bits */
+
+#ifndef _HCL_DEFS_H
+typedef unsigned char t_bool;                   //!< Boolean primitive type \ingroup NMF_PRIMITIVE_TYPE
+
+typedef unsigned char t_uint8;                  //!< Unsigned 8 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef signed char t_sint8;                    //!< Signed 8 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef unsigned short t_uint16;                //!< Unsigned 16 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef signed short t_sint16;                  //!< Signed 16 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef unsigned long t_uint32;                 //!< Unsigned 32 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef signed long t_sint32;                   //!< Signed 32 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef unsigned long long t_uint64;            //!< Unsigned 64 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+typedef signed long long t_sint64;              //!< Signed 64 bits primitive type \ingroup NMF_PRIMITIVE_TYPE
+
+typedef t_uint32 t_physical_address;
+#endif /* _HCL_DEFS_H */
+
+typedef unsigned long t_uint24;
+typedef signed long t_sint24;
+typedef unsigned long long t_uint48;
+typedef signed long long t_sint48;
+
+// shared addr type definition
+typedef t_uint32 t_shared_addr;
+
+// shared register (ARM world) type definition
+typedef t_uint32 t_shared_reg;
+typedef t_uint32 t_shared_field;
+
+#endif /* RISC 32 Bits */
+
+/*
+ * Define boolean type
+ */
+#undef FALSE
+#define FALSE  0                                            //!< Boolean FALSE value
+#undef TRUE
+#define TRUE  1                                             //!< Boolean TRUE value
+
+#ifndef NULL
+    #if defined __flexcc2__ || defined __SYMBIAN32__
+        #define NULL (0x0)                                  //!< Null type \ingroup NMF_PRIMITIVE_TYPE
+    #else
+        #define NULL ((void*)0x0)                           //!< Null type \ingroup NMF_PRIMITIVE_TYPE
+    #endif
+#endif
+
+typedef t_uint32 t_nmf_component_handle;
+
+#endif /* NMF_TYPEDEF_H_ */
diff --git a/drivers/staging/nmf-cm/nmf/inc/channel_type.h b/drivers/staging/nmf-cm/nmf/inc/channel_type.h
new file mode 100644
index 00000000000..91a733dbbbf
--- /dev/null
+++ b/drivers/staging/nmf-cm/nmf/inc/channel_type.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Common Nomadik Multiprocessing Framework type definition
+ *
+ * This file contains the shared between cm and ee type definitions used into NMF for callback.
+ */
+/*!
+ * \defgroup _t_nmf_channel_flag t_nmf_channel_flag
+ * \ingroup NMF_COMMON
+ */
+
+#ifndef __INC_CHANNEL_TYPE_H
+#define __INC_CHANNEL_TYPE_H
+
+#include <inc/typedef.h>
+#include <inc/nmf_type.idt>
+
+/*!
+ * \brief Define t_nmf_channel_flag type that allow to control if/how a new communication channel is created.
+ * \ingroup _t_nmf_channel_flag
+ */
+typedef t_uint32 t_nmf_channel_flag;
+
+#define NMF_CHANNEL_SHARED                      ((t_nmf_channel_flag)0) //!< \ingroup _t_nmf_channel_flag
+#define NMF_CHANNEL_PRIVATE                     ((t_nmf_channel_flag)1) //!< \ingroup _t_nmf_channel_flag
+
+/*!
+ * \brief Define t_nmf_virtualInterruptHandler function type to allow to dispatch virtual interrupt
+ * \ingroup VIRTUAL_INTERRUPT
+ */
+typedef void (*t_nmf_virtualInterruptHandler)(void *interruptContext); 
+
+#endif
+
diff --git a/drivers/staging/nmf-cm/nmf/inc/component_type.h b/drivers/staging/nmf-cm/nmf/inc/component_type.h
new file mode 100644
index 00000000000..26217554158
--- /dev/null
+++ b/drivers/staging/nmf-cm/nmf/inc/component_type.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Common Nomadik Multiprocessing Framework type definition
+ *
+ * This file contains the shared between cm and ee type definitions used into NMF for callback.
+ */
+#ifndef __INC_COMPONENT_TYPE_H
+#define __INC_COMPONENT_TYPE_H
+
+#include <inc/typedef.h>
+
+/*!
+ * \brief Identifier of a component instance handle
+ *
+ * \ingroup NMF_COMMON
+ */
+typedef t_nmf_component_handle t_cm_instance_handle;
+
+#endif
+
diff --git a/drivers/staging/nmf-cm/nmf/inc/service_type.h b/drivers/staging/nmf-cm/nmf/inc/service_type.h
new file mode 100644
index 00000000000..06d5c72dce9
--- /dev/null
+++ b/drivers/staging/nmf-cm/nmf/inc/service_type.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Service type and data used through service callback.
+ * \defgroup NMF_SERVICE NMF Service Callback types and data definition
+ * \ingroup NMF_COMMON
+ */
+#ifndef SERVICE_TYPE_H
+#define SERVICE_TYPE_H
+
+#include <ee/api/panic.idt>
+#include <nmf/inc/component_type.h>
+#include <share/inc/nmf.h>
+
+/*!
+ * \brief Define t_nmf_service_type type
+ *
+ * It gives the type of service message passed to service callback.
+ * \ingroup NMF_SERVICE
+ */
+typedef t_uint32 t_nmf_service_type;
+#define NMF_SERVICE_PANIC                          ((t_nmf_service_type)0) //!< \ingroup NMF_SERVICE
+#define NMF_SERVICE_SHUTDOWN                       ((t_nmf_service_type)1) //!< \ingroup NMF_SERVICE
+
+/*
+ * The following structured define each data structure used for each service type
+ * and given to each serviceCallback
+ */
+
+/*!
+ * \brief Define t_nmf_panic_data type
+ *
+ * This is the data structure passed to the service callback (inside \ref t_nmf_service_data)
+ * when t_nmf_service_type == NMF_SERVICE_PANIC
+ * \ingroup NMF_SERVICE
+ */
+typedef struct {
+	t_panic_reason       panicReason; //!< The reason of the panic
+	t_panic_source       panicSource; //!< THe source of the panic (One of the MPC or the ARM-EE)
+	/*!
+	 * union of structures containing specific info, depending on the panicSource
+	 */
+	union {
+		struct {
+			t_nmf_core_id        coreid; //!< The coreId of the MPC on which the panic occured
+			t_cm_instance_handle faultingComponent; //!< The faulting component handle
+			t_uint32             panicInfo1; //!< First info (depend on \ref panicReason)
+			t_uint32             panicInfo2; //!< Second info (depend on \ref panicReason)
+		} mpc; //!< member to use if panicSource == MPC_EE
+		struct {
+			void *               faultingComponent; //!< The faulting component handle
+			t_uint32             panicInfo1; //!< First info (depend on \ref panicReason)
+			t_uint32             panicInfo2; //!< Second info (depend on \ref panicReason)
+		} host; //!< member to use if panicSource == HOST_EE
+	} info; //!< union of structures containing specific info, depending on the panicSource
+} t_nmf_panic_data;
+
+/*!
+ * \brief Define t_nmf_shutdown_data type
+ *
+ * This is the data structure passed to the service callback (inside \ref t_nmf_service_data)
+ * when t_nmf_service_type == NMF_SERVICE_SHUTDOWN
+ * \ingroup NMF_SERVICE
+ */
+typedef struct {
+    t_nmf_core_id        coreid; //!< The coreId of the MPC on which has been shutdown
+} t_nmf_shutdown_data;
+
+/*!
+ * \brief Define t_nmf_service_data type
+ *
+ * It gives the data passed to the service callbacks for each service type
+ * This is an union whose member to use is defined by the given \ref t_nmf_service_type
+ *
+ * \ingroup NMF_SERVICE
+ */
+typedef union {
+	t_nmf_panic_data panic; //!< if service_type == NMF_SERVICE_PANIC
+    t_nmf_shutdown_data shutdown; //!< if service_type == NMF_SERVICE_SHUTDOWN
+} t_nmf_service_data;
+
+/*!
+ * \brief Define t_nmf_serviceCallback function type to allow to dispatch service message to user.
+ * \ingroup NMF_SERVICE
+ */
+typedef void (*t_nmf_serviceCallback)(void *contextHandler, t_nmf_service_type serviceType, t_nmf_service_data *serviceData);
+
+#endif //SERVICE_TYPE_H
diff --git a/drivers/staging/nmf-cm/osal-kernel.c b/drivers/staging/nmf-cm/osal-kernel.c
new file mode 100644
index 00000000000..c1d8640b11b
--- /dev/null
+++ b/drivers/staging/nmf-cm/osal-kernel.c
@@ -0,0 +1,1223 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/** \file osal-kernel.c
+ * 
+ * Implements NMF OSAL for Linux kernel-space environment
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kthread.h>
+#include <linux/mm.h>
+#include <linux/semaphore.h>
+#include <linux/slab.h>
+#include <linux/timer.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+
+#ifdef CONFIG_STM_TRACE
+#include <trace/stm.h>
+#endif
+
+#include <cm/engine/configuration/inc/configuration_status.h>
+
+#include "cmioctl.h"
+#include "osal-kernel.h"
+#include "cm_service.h"
+#include "cmld.h"
+#include "cm_debug.h"
+#include "cm_dma.h"
+
+__iomem void *prcmu_base = NULL;
+__iomem void *prcmu_tcdm_base = NULL;
+
+/* DSP Load Monitoring */
+#define FULL_OPP 100
+#define HALF_OPP 50
+static unsigned long running_dsp = 0;
+static unsigned int dspLoadMonitorPeriod = 1000;
+module_param(dspLoadMonitorPeriod, uint, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(dspLoadMonitorPeriod, "Period of the DSP-Load monitoring in ms");
+static unsigned int dspLoadHighThreshold = 85;
+module_param(dspLoadHighThreshold, uint, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(dspLoadHighThreshold, "Threshold above which 100 APE OPP is requested");
+static unsigned int dspLoadLowThreshold = 35;
+module_param(dspLoadLowThreshold, uint, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(dspLoadLowThreshold, "Threshold below which 100 APE OPP request is removed");
+static bool cm_use_ftrace;
+module_param(cm_use_ftrace, bool, S_IWUSR|S_IRUGO);
+MODULE_PARM_DESC(cm_use_ftrace, "Whether all CM debug traces goes through ftrace or normal kernel output");
+
+/** \defgroup ENVIRONMENT_INITIALIZATION Environment initialization
+ * Includes functions that initialize the Linux OSAL itself plus functions that
+ * are responsible to factor configuration objects needed to initialize Component Manager library
+ */
+
+/** \defgroup OSAL_IMPLEMENTATION OSAL implementation
+ *	 Linux-specific implementation of the Component Manager OSAL interface.
+ */
+
+
+/** \ingroup ENVIRONMENT_INITIALIZATION
+ * Remaps IO, SDRAM and ESRAM regions
+ *
+ * \osalEnvironment NMF-Osal descriptor
+ * \return POSIX error code 
+ */
+int remapRegions(void)
+{
+	unsigned i;
+
+	/* Remap DSP base areas */
+	for (i=0; i<NB_MPC; i++) {
+		osalEnv.mpc[i].base.data = ioremap_nocache((int)osalEnv.mpc[i].base_phys, ONE_MB);
+		if(osalEnv.mpc[i].base.data == NULL){
+			pr_err("%s: could not remap base address for %s\n", __func__, osalEnv.mpc[i].name);
+			return -ENOMEM;
+		}
+	}
+
+	/* Remap hardware semaphores */
+	osalEnv.hwsem_base = ioremap_nocache(U8500_HSEM_BASE, (4*ONE_KB));
+	if(osalEnv.hwsem_base == NULL){
+		pr_err("%s: could not remap HWSEM Base\n", __func__);
+		return -ENOMEM;
+	}
+
+	/* Remap _all_ ESRAM banks */
+	osalEnv.esram_base = ioremap_nocache(ESRAM_BASE, cfgESRAMSize*ONE_KB);
+	if(osalEnv.esram_base == NULL){
+		pr_err("%s: could not remap ESRAM Base\n", __func__);
+		return -ENOMEM;
+	}
+
+	/* Allocate code and data sections for MPC (SVA, SIA) */
+        for (i=0; i<NB_MPC; i++) {
+		/* Allocate MPC SDRAM code area */
+		struct hwmem_mem_chunk mem_chunk;
+		size_t mem_chunk_length;
+		osalEnv.mpc[i].hwmem_code = hwmem_alloc(osalEnv.mpc[i].sdram_code.size,
+						       //HWMEM_ALLOC_HINT_CACHE_WB,
+						       HWMEM_ALLOC_HINT_WRITE_COMBINE | HWMEM_ALLOC_HINT_UNCACHED,
+						       HWMEM_ACCESS_READ | HWMEM_ACCESS_WRITE,
+						       HWMEM_MEM_CONTIGUOUS_SYS);
+		if (IS_ERR(osalEnv.mpc[i].hwmem_code)) {
+			int err = PTR_ERR(osalEnv.mpc[i].hwmem_code);
+			osalEnv.mpc[i].hwmem_code = NULL;
+			pr_err("%s: could not allocate SDRAM Code for %s\n",
+			       __func__, osalEnv.mpc[i].name);
+			return err;
+		}
+		osalEnv.mpc[i].sdram_code.data = hwmem_kmap(osalEnv.mpc[i].hwmem_code);
+		if (IS_ERR(osalEnv.mpc[i].sdram_code.data)) {
+			int err = PTR_ERR(osalEnv.mpc[i].sdram_code.data);
+			osalEnv.mpc[i].sdram_code.data = NULL;
+			pr_err("%s: could not map SDRAM Code for %s\n", __func__, osalEnv.mpc[i].name);
+			return err;
+		}
+		mem_chunk_length = 1;
+		(void)hwmem_pin(osalEnv.mpc[i].hwmem_code, &mem_chunk, &mem_chunk_length);
+		osalEnv.mpc[i].sdram_code_phys = mem_chunk.paddr;
+		/* Allocate MPC SDRAM data area by taking care wether the data are shared or not */
+		if (osalEnv.mpc[i].sdram_data.size == 0) {
+			/* size of 0 means shared data segment, reuse the same param as for first MPC */
+			osalEnv.mpc[i].sdram_data_phys = osalEnv.mpc[0].sdram_data_phys;
+			osalEnv.mpc[i].sdram_data.data = osalEnv.mpc[0].sdram_data.data;
+			osalEnv.mpc[i].sdram_data.size = osalEnv.mpc[0].sdram_data.size;
+		} else {
+			/* If we do not share the data segment or if this is the first MPC */
+			osalEnv.mpc[i].hwmem_data = hwmem_alloc(osalEnv.mpc[i].sdram_data.size,
+							       HWMEM_ALLOC_HINT_WRITE_COMBINE | HWMEM_ALLOC_HINT_UNCACHED,
+							       HWMEM_ACCESS_READ | HWMEM_ACCESS_WRITE,
+							       HWMEM_MEM_CONTIGUOUS_SYS);
+			if (IS_ERR(osalEnv.mpc[i].hwmem_data)) {
+				int err = PTR_ERR(osalEnv.mpc[i].hwmem_data);
+				osalEnv.mpc[i].hwmem_data = NULL;
+				pr_err("%s: could not allocate SDRAM Data for %s\n",
+				       __func__, osalEnv.mpc[i].name);
+				return err;
+			}
+			mem_chunk_length = 1;
+			(void)hwmem_pin(osalEnv.mpc[i].hwmem_data,
+					&mem_chunk, &mem_chunk_length);
+			osalEnv.mpc[i].sdram_data_phys = mem_chunk.paddr;
+			osalEnv.mpc[i].sdram_data.data = hwmem_kmap(osalEnv.mpc[i].hwmem_data);
+			if (IS_ERR(osalEnv.mpc[i].sdram_data.data)) {
+				int err = PTR_ERR(osalEnv.mpc[i].sdram_data.data);
+				osalEnv.mpc[i].sdram_data.data = NULL;
+				pr_err("%s: could not map SDRAM Data for %s\n",
+				       __func__, osalEnv.mpc[i].name);
+				return err;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/** \ingroup ENVIRONMENT_INITIALIZATION
+ * Unmaps IO, SDRAM and ESRAM regions
+ *
+ * \return POSIX error code 
+ */
+void unmapRegions(void)
+{
+	unsigned i;
+
+	/* Release SVA, SIA, Hardware sempahores and embedded SRAM mappings */
+	for (i=0; i<NB_MPC; i++) {
+		if(osalEnv.mpc[i].base.data != NULL)
+			iounmap(osalEnv.mpc[i].base.data);
+	}
+
+	if(osalEnv.hwsem_base != NULL)
+		iounmap(osalEnv.hwsem_base);
+	
+	if(osalEnv.esram_base != NULL)
+		iounmap(osalEnv.esram_base);
+
+	/*
+	 * Free SVA and SIA code and data sections or release their mappings
+	 * according on how memory allocations has been achieved
+	 */
+	for (i=0; i<NB_MPC; i++) {
+		if (osalEnv.mpc[i].sdram_code.data != NULL) {
+			hwmem_unpin(osalEnv.mpc[i].hwmem_code);
+			hwmem_kunmap(osalEnv.mpc[i].hwmem_code);
+			if (osalEnv.mpc[i].hwmem_code != NULL)
+				hwmem_release(osalEnv.mpc[i].hwmem_code);
+		}
+
+		/* If data segment is shared, we must free only the first data segment */
+		if (((i == 0) || (osalEnv.mpc[i].sdram_data.data != osalEnv.mpc[0].sdram_data.data))
+		    && (osalEnv.mpc[i].sdram_data.data != NULL)) {
+			hwmem_unpin(osalEnv.mpc[i].hwmem_data);
+			hwmem_kunmap(osalEnv.mpc[i].hwmem_data);
+			if (osalEnv.mpc[i].hwmem_data != NULL)
+				hwmem_release(osalEnv.mpc[i].hwmem_data);
+		}
+	}
+}
+
+
+/** \ingroup ENVIRONMENT_INITIALIZATION
+ *	Fills a t_nmf_hw_mapping_desc object
+ *
+ * \param nmfHwMappingDesc Pointer to a t_nmf_hw_mapping_desc object
+ * \return POSIX error code
+ */
+int getNmfHwMappingDesc(t_nmf_hw_mapping_desc* nmfHwMappingDesc)
+{
+
+	if (nmfHwMappingDesc == NULL)
+		return -ENXIO;
+
+	nmfHwMappingDesc->esramDesc.systemAddr.physical = ESRAM_BASE;
+	nmfHwMappingDesc->esramDesc.systemAddr.logical = (t_cm_logical_address)osalEnv.esram_base;
+	nmfHwMappingDesc->esramDesc.size = cfgESRAMSize*ONE_KB;
+
+	nmfHwMappingDesc->hwSemaphoresMappingBaseAddr.physical = U8500_HSEM_BASE;
+	nmfHwMappingDesc->hwSemaphoresMappingBaseAddr.logical = (t_cm_logical_address)osalEnv.hwsem_base;
+
+	return 0;
+}
+
+/**  \ingroup ENVIRONMENT_INITIALIZATION
+ * Fills a t_cm_system_address object
+ *
+ * \param mpcSystemAddress Pointer to a t_cm_system_address object
+ * \return POSIX error code
+ */
+void getMpcSystemAddress(unsigned i, t_cm_system_address* mpcSystemAddress)
+{
+	mpcSystemAddress->physical = (t_cm_physical_address)osalEnv.mpc[i].base_phys;
+	mpcSystemAddress->logical  = (t_cm_logical_address)osalEnv.mpc[i].base.data;
+}
+
+
+/** \ingroup ENVIRONMENT_INITIALIZATION
+ *	Fills t_nmf_memory_segment objects for MPC code and data segments
+ *
+ * \param i           Index of the MPC to initialize
+ * \param codeSegment Pointer to a t_nmf_memory_segment (code segment)
+ * \param dataSegment Pointer to a t_nmf_memory_segment (data segment)
+ * \return Always 0
+ */
+void getMpcSdramSegments(unsigned i, t_nmf_memory_segment* codeSegment, t_nmf_memory_segment* dataSegment)
+{
+	codeSegment->systemAddr.logical = (t_cm_logical_address)osalEnv.mpc[i].sdram_code.data;
+	codeSegment->systemAddr.physical = osalEnv.mpc[i].sdram_code_phys;
+	codeSegment->size = osalEnv.mpc[i].sdram_code.size;
+
+	dataSegment->systemAddr.logical = (t_cm_logical_address)osalEnv.mpc[i].sdram_data.data;
+	dataSegment->systemAddr.physical = osalEnv.mpc[i].sdram_data_phys;
+	dataSegment->size = osalEnv.mpc[i].sdram_data.size;
+}
+
+#ifdef CM_DEBUG_ALLOC
+#include <linux/kallsyms.h>
+struct cm_alloc cm_alloc;
+
+/**
+ * These routines initializes the structures used to trace all alloc/free.
+ * These are used in debug mode to track all memory leak about the allocations
+ * done through the OSAL.
+ */
+void init_debug_alloc(void)
+{
+	INIT_LIST_HEAD(&cm_alloc.chain);
+	spin_lock_init(&cm_alloc.lock);
+}
+
+void cleanup_debug_alloc(void)
+{
+	struct cm_alloc_elem *entry, *next;
+	char buffer[128];
+
+	list_for_each_entry_safe(entry, next, &cm_alloc.chain, elem) {
+		sprint_symbol(buffer, (int)entry->caller);
+		pr_err("/!\\ ALLOC(size=%d) not freed from: 0x%p (%s)\n",
+		       entry->size, entry->caller, buffer);
+		list_del(&entry->elem);
+		if ((void*)entry >= (void*)VMALLOC_START
+		    && (void*)entry < (void*)VMALLOC_END)
+			vfree(entry);
+		else
+			kfree(entry);
+	}
+}
+
+void dump_debug_alloc(void)
+{
+	struct cm_alloc_elem *entry, *next;
+	char buffer[128];
+
+	pr_err("Current allocated memory:\n");
+	list_for_each_entry_safe(entry, next, &cm_alloc.chain, elem) {
+		sprint_symbol(buffer, (int)entry->caller);
+		pr_err("=> Alloc of size=%d from: 0x%p (%s)\n",
+		       entry->size, entry->caller, buffer);
+	}
+}
+#endif
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ * Called by CM_ProcessMpcEvent in interrupt/tasklet context. Schedules the DFC. 
+ * Enqueues the new event in the process' message queue.
+ *
+ * \note This is _not_ called in response to internal events such as in
+ * response to a CM_InstantiateComponent. It is called when user-defined
+ * functions need to be called in skeletons. This behavior is different
+ * from 0.8.1 version.
+ */
+void OSAL_PostDfc(t_nmf_mpc2host_handle upLayerTHIS, t_uint32 methodIndex, t_event_params_handle ptr, t_uint32 size)
+{
+	/* skelwrapper has been created in CM_SYSCALL_BindComponentToCMCore and conveys per-process private data */
+	t_skelwrapper* skelwrapper = (t_skelwrapper*)upLayerTHIS;
+	struct osal_msg* message;
+
+	/* If the clannel has been closed, no more reader exists
+	   => discard the message */
+	if (skelwrapper->channelPriv->state == CHANNEL_CLOSED) {
+		pr_warning("%s: message discarded (channel closed)\n",
+			   __func__ );
+		return;
+	}
+
+	/* Create a new message */
+	message = kmalloc(sizeof(*message), GFP_ATOMIC);
+	if (!message) {
+		pr_err("%s: message discarded (alloc failed)\n", __func__ );
+		return;
+	}
+
+	/* Stuff it */
+	plist_node_init(&message->msg_entry, 0);
+	message->msg_type = MSG_INTERFACE;
+	message->d.itf.skelwrap = skelwrapper;
+	message->d.itf.methodIdx = methodIndex;
+	message->d.itf.anyPtr = ptr;
+	message->d.itf.ptrSize = size;
+
+	/* Enqueue it */
+	/* Should be protected with the cmPriv->msgQueueLock held
+	   But we know by design that we are safe here. (Alone here in
+	   tasklet (soft-interrupt) context.
+	   When accessed in process context, soft-irq are disable)
+	*/
+	spin_lock_bh(&skelwrapper->channelPriv->bh_lock);
+	plist_add(&message->msg_entry, &skelwrapper->channelPriv->messageQueue);
+	spin_unlock_bh(&skelwrapper->channelPriv->bh_lock);
+	
+	/* Wake up process' wait queue */
+	wake_up(&skelwrapper->channelPriv->waitq);
+}
+
+
+#define MAX_LOCKS 8 // max number of locks/semaphores creatable
+static unsigned long semused = 0; // bit field for used semaphores
+static unsigned long lockused = 0; // bit field for used mutexes
+static struct mutex cmld_locks[MAX_LOCKS];
+
+/** \ingroup OSAL_IMPLEMENTATION
+ */
+t_nmf_osal_sync_handle OSAL_CreateLock(void)
+{
+	int i;
+
+	for (i=0; i<MAX_LOCKS; i++)
+		if (!test_and_set_bit(i, &lockused)) {
+			struct mutex* mutex = &cmld_locks[i];		
+			mutex_init(mutex);
+			return (t_nmf_osal_sync_handle)mutex;
+		}
+
+	return (t_nmf_osal_sync_handle)NULL;
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ */
+void OSAL_Lock(t_nmf_osal_sync_handle handle)
+{
+		// unfortunately there is no return value to this function
+		// so we cannot use 'mutex_lock_killable()'
+		mutex_lock((struct mutex*)handle);
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ */
+void OSAL_Unlock(t_nmf_osal_sync_handle handle)
+{
+		mutex_unlock((struct mutex*)handle);
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ */
+void OSAL_DestroyLock(t_nmf_osal_sync_handle handle)
+{
+	int i;
+
+	// clear the bit in the bits field about used locks
+	i = ((struct mutex*)handle - cmld_locks);
+
+	clear_bit(i, &lockused);
+}
+
+static struct semaphore cmld_semaphores[MAX_LOCKS];
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * Goal: Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * \param[in] value : Initial value of semaphore.
+ * 
+ * \return handle of the Semaphore created 
+ * 
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup OSAL
+ */ 
+t_nmf_osal_sem_handle OSAL_CreateSemaphore(t_uint32 value)
+{
+	int i;
+
+	for (i=0; i<MAX_LOCKS; i++)
+		if (!test_and_set_bit(i, &semused)) {
+			struct semaphore* sem = &cmld_semaphores[i];		
+			sema_init(sem, value);
+			return (t_nmf_osal_sem_handle)sem;
+		}
+
+	return (t_nmf_osal_sem_handle)NULL;
+}
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * Goal: Use by CM to allow to synchronize with code running on mpc side. This function can be called under
+ *       Irq context by CM.
+ *
+ * param[in] : handle of the Semaphore for which we increase value and so potentially wake up thread.
+ * 
+ * param[in] : aCtx is a hint to indicate to os that we are in a none normal context (e.g under interruption).
+ *  
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup OSAL
+ */ 
+void OSAL_SemaphorePost(t_nmf_osal_sem_handle handle, t_uint8 aCtx)
+{
+		up((struct semaphore*)handle);
+}
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * Goal: Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * param[in] : handle of the Semaphore for which we decrease value and so potentially block current thread.
+ *  
+ * param[in] : maximun time in ms after which the block thread is wake up. In this case function return SYNC_ERROR_TIMEOUT value.
+ * 
+ * \return error number: SYNC_ERROR_TIMEOUT in case semaphore is not release withing timeOutInMs.
+ * 
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup OSAL
+ */ 
+t_nmf_osal_sync_error OSAL_SemaphoreWaitTimed(t_nmf_osal_sem_handle handle,
+					      t_uint32 timeOutInMs)
+{
+	if (down_timeout((struct semaphore*)handle, msecs_to_jiffies(timeOutInMs)))
+		return SYNC_ERROR_TIMEOUT;
+	else
+		return SYNC_OK;
+}
+
+/*!
+ * \brief Description of the Synchronization part of the OS Adaptation Layer
+ *
+ * Goal: Use by CM to allow to synchronize with code running on mpc side.
+ *
+ * param[in] : handle of the Semaphore to be destroyed
+ * 
+ * Called by:
+ *  - any CM API call
+ *
+ * \ingroup OSAL
+ */
+void OSAL_DestroySemaphore(t_nmf_osal_sem_handle handle)
+{
+	int i;
+
+	// clear the bit in the bits field about used locks
+	i = ((struct semaphore*)handle - cmld_semaphores);
+
+	clear_bit(i, &semused);
+}
+
+/** \ingroup OSAL_IMPLEMENTATION
+ * OSAL alloc implementation
+ *
+ * In both OSAL_Alloc() and OSAL_Alloc_Zero() function, the strategy is to use
+ * kmalloc() as it is the more efficient and most common way to allocate memory.
+ * For big allocation, kmalloc may fail because memory is very fragmented
+ * (kmalloc() allocates contiguous memory). In that case, we fall to vmalloc()
+ * instead.
+ * In OSAL_Free(), we rely on the virtual address to know which of kfree() or
+ * vfree() to use (vmalloc() use its own range of virtual addresses)
+ */
+void* OSAL_Alloc(t_cm_size size)
+{
+#ifdef CM_DEBUG_ALLOC
+	struct cm_alloc_elem *entry;
+
+	if (size == 0)
+		return NULL;
+
+	entry = kmalloc(size + sizeof(*entry), GFP_KERNEL | __GFP_NOWARN);
+
+	if (entry == NULL) {
+		entry = vmalloc(size + sizeof(*entry));
+
+		if (entry == NULL) {
+			pr_alert("%s: kmalloc(%d) and vmalloc(%d) failed\n",
+				 __func__, (int)size, (int)size);
+			dump_debug_alloc();
+			return NULL;
+		}
+	}
+	/* return address of the caller */
+	entry->caller = __builtin_return_address(0);
+	entry->size = size;
+
+	spin_lock(&cm_alloc.lock);
+	list_add_tail(&entry->elem, &cm_alloc.chain);
+	spin_unlock(&cm_alloc.lock);
+
+	return entry->addr;
+#else
+	void* mem;
+
+	if (size == 0)
+		return NULL;
+	mem = kmalloc(size, GFP_KERNEL | __GFP_NOWARN);
+	if (mem == NULL) {
+		mem = vmalloc(size);
+		if (mem == NULL)
+			pr_alert("CM (%s): No more memory (requested "
+				 "size=%d) !!!\n", __func__, (int)size);
+	}
+	return mem;
+#endif
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ * OSAL alloc implementation
+ */ 
+void* OSAL_Alloc_Zero(t_cm_size size)
+{
+#ifdef CM_DEBUG_ALLOC
+	struct cm_alloc_elem *entry;
+
+	if (size == 0)
+		return NULL;
+
+	entry = kzalloc(size + sizeof(*entry), GFP_KERNEL | __GFP_NOWARN);
+	if (entry == NULL) {
+		entry = vmalloc(size + sizeof(*entry));
+		if (entry == NULL) {
+			pr_alert("%s: kmalloc(%d) and vmalloc(%d) failed\n",
+				 __func__, (int)size, (int)size);
+			dump_debug_alloc();
+			return NULL;
+		} else {
+			memset(entry, 0, size + sizeof(*entry));
+		}
+	}
+
+	/* return address of the caller */
+	entry->caller = __builtin_return_address(0);
+	entry->size = size;
+
+	spin_lock(&cm_alloc.lock);
+	list_add_tail(&entry->elem, &cm_alloc.chain);
+	spin_unlock(&cm_alloc.lock);
+
+	return entry->addr;
+#else
+	void* mem;
+
+	if (size == 0)
+		return NULL;
+	mem = kzalloc(size, GFP_KERNEL | __GFP_NOWARN);
+	if (mem == NULL) {
+		mem = vmalloc(size);
+		if (mem == NULL)
+			pr_alert("CM (%s): No more memory (requested "
+				 "size=%d) !!!\n", __func__, (int)size);
+		else
+			memset(mem, 0, size);
+	}
+
+	return mem;
+#endif
+}
+
+
+/** \ingroup OSAL_IMPLEMENTATION
+ *	OSAL free implementation
+ */
+void OSAL_Free(void* mem)
+{
+#ifdef CM_DEBUG_ALLOC
+	struct cm_alloc_elem *entry = container_of(mem, struct cm_alloc_elem, addr);
+	unsigned int i;
+	char pattern[4] = { 0xEF, 0xBE, 0xAD, 0xDE };
+
+	if (mem == NULL)
+		return;
+
+	/* fill with a pattern to detect bad re-use of this area */
+	for (i=0; i<entry->size; i++)
+		entry->addr[i] = pattern[i%4];
+
+	spin_lock(&cm_alloc.lock);
+	list_del(&entry->elem);
+	spin_unlock(&cm_alloc.lock);
+
+	if ((void*)entry >= (void*)VMALLOC_START
+	    && (void*)entry < (void*)VMALLOC_END)
+		vfree(entry);
+	else
+		kfree(entry);
+#else
+	if (mem >= (void*)VMALLOC_START && mem < (void*)VMALLOC_END)
+		vfree(mem);
+	else
+		kfree(mem);
+#endif
+}
+
+/** \ingroup OSAL_IMPLEMENTATION
+ *	OSAL Copy implementation
+ * This copy some data from userspace (address to kernel space.
+ * This implementation differs on Symbian.
+ */
+t_cm_error OSAL_Copy(void *dst, const void *src, t_cm_size size)
+{
+	if (copy_from_user(dst, src, size))
+		return CM_UNKNOWN_MEMORY_HANDLE;
+	return CM_OK;
+}
+
+/**  \ingroup OSAL_IMPLEMENTATION
+ *	OSAL write64 function implementation
+ */
+void OSAL_Write64(t_nmf_trace_channel channel, t_uint8 isTimestamped, t_uint64 value)
+{
+#ifdef CONFIG_STM_TRACE
+	if (isTimestamped)
+		stm_tracet_64(channel, value);
+	else
+		stm_trace_64(channel, value);
+#endif
+}
+
+
+/**  \ingroup OSAL_IMPLEMENTATION
+ *	OSAL log function implementation
+ */
+void OSAL_Log(const char *format, int param1, int param2, int param3, int param4, int param5, int param6)
+{
+	if (cm_use_ftrace)
+		trace_printk(format,
+			     param1, param2, param3, param4, param5, param6);
+	else
+		printk(format, param1, param2, param3, param4, param5, param6);
+}
+
+/**
+ * compute the dsp load
+ * 
+ * return -1 if in case of failure, a value between 0 and 100 otherwise
+ */
+static s8 computeDspLoad(t_cm_mpc_load_counter *oldCounter, t_cm_mpc_load_counter *counter)
+{
+	u32 t, l;
+
+	if ((oldCounter->totalCounter == 0) && (oldCounter->loadCounter == 0))
+		return -1; // Failure or not started ?
+	if ((counter->totalCounter == 0) && (counter->loadCounter == 0))
+		return -1; // Failure or already stopped ?
+
+	if (counter->totalCounter < oldCounter->totalCounter)
+		t = (u32)((((u64)-1) - oldCounter->totalCounter)
+			  + counter->totalCounter + 1);
+	else
+		t = (u32)(counter->totalCounter - oldCounter->totalCounter);
+
+	if (counter->loadCounter < oldCounter->loadCounter)
+		l = (u32)((((u64)-1) - oldCounter->loadCounter)
+			  + counter->loadCounter  + 1);
+	else
+		l = (u32)(counter->loadCounter - oldCounter->loadCounter);
+
+	if (t == 0) // not significant
+		return -1;
+
+	if (l > t) // not significant
+		return -1;
+
+	return (l*100) / t;
+}
+
+static void wakeup_process(unsigned long data)
+{
+	wake_up_process((struct task_struct *)data);
+}
+
+/**
+ * Thread function entry for monitorin the CPU load
+ */
+static int dspload_monitor(void *idx)
+{
+	int i = (int)idx;
+	unsigned char current_opp_request = FULL_OPP;
+	struct mpcConfig *mpc = &osalEnv.mpc[i];
+	struct timer_list timer;
+
+	timer.function = wakeup_process;
+	timer.data = (unsigned long)current;
+	init_timer_deferrable(&timer);
+
+#ifdef CONFIG_DEBUG_FS
+	mpc->opp_request = current_opp_request;
+#endif
+	if (prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+				      (char*)mpc->name,
+				      current_opp_request))
+		pr_err("CM Driver: Add QoS failed\n");
+
+	/*
+	 * Wait for 500ms before initializing the counter,
+	 * to let the DSP boot (init of counter will failed if
+	 * DSP is not booted).
+	 */
+	schedule_timeout_uninterruptible(msecs_to_jiffies(500));
+
+	/* init counter */
+	if (CM_GetMpcLoadCounter(mpc->coreId,
+				 &mpc->oldLoadCounter) != CM_OK)
+		pr_warn("CM Driver: Failed to init load counter for %s\n",
+			mpc->name);
+
+	while (!kthread_should_stop()) {
+		t_cm_mpc_load_counter loadCounter;
+		s8 load = -1;
+		unsigned long expire;
+
+		__set_current_state(TASK_UNINTERRUPTIBLE);
+
+		expire = msecs_to_jiffies(dspLoadMonitorPeriod) + jiffies;
+
+		mod_timer(&timer, expire);
+		schedule();
+		/* We can be woken up before the expiration of the timer
+		   but we don't need to handle that case as the
+		   computation of the DSP load takes that into account */
+
+		if (!test_bit(i, &running_dsp))
+			continue;
+
+		if (CM_GetMpcLoadCounter(mpc->coreId,
+					 &loadCounter) != CM_OK)
+			loadCounter = mpc->oldLoadCounter;
+
+#ifdef CONFIG_DEBUG_FS
+		mpc->load =
+#endif
+		load = computeDspLoad(&mpc->oldLoadCounter, &loadCounter);
+		mpc->oldLoadCounter = loadCounter;
+
+		if (load == -1)
+			continue;
+		/* check if we must request more opp */
+		if ((current_opp_request == HALF_OPP)
+		    && (load > dspLoadHighThreshold)) {
+#ifdef CONFIG_DEBUG_FS
+			mpc->opp_request =
+#endif
+			current_opp_request = FULL_OPP;
+			if (cm_debug_level)
+				pr_info("CM Driver: Request QoS OPP %d for %s\n",
+					current_opp_request, mpc->name);
+			prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP,
+						     (char*)mpc->name,
+						     current_opp_request);
+		}
+		/* check if we can request less opp */
+		else if ((current_opp_request == FULL_OPP)
+			 && (load < dspLoadLowThreshold)) {
+#ifdef CONFIG_DEBUG_FS
+			mpc->opp_request =
+#endif
+			current_opp_request = HALF_OPP;
+			if (cm_debug_level)
+				pr_info("CM Driver: Request QoS OPP %d for %s\n",
+					current_opp_request, mpc->name);
+			prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP,
+						     (char*)mpc->name,
+						     current_opp_request);
+		}
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	mpc->opp_request = mpc->load = 0;
+#endif
+	del_singleshot_timer_sync(&timer);
+	if (cm_debug_level)
+		pr_info("CM Driver: Remove QoS OPP for %s\n", mpc->name);
+	prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+				     (char*)mpc->name);
+ 	return 0;
+}
+
+static bool enable_auto_pm = 1;
+module_param(enable_auto_pm, bool, S_IWUSR|S_IRUGO);
+
+/** \ingroup OSAL_IMPLEMENTATION
+ *     Used by CM to disable a power resource
+ */
+void OSAL_DisablePwrRessource(t_nmf_power_resource resource, t_uint32 firstParam, t_uint32 secondParam)
+{
+	switch (resource) {
+	case CM_OSAL_POWER_SxA_CLOCK: {
+		unsigned idx = COREIDX(firstParam);
+		struct osal_msg msg;
+
+		if (idx >= NB_MPC) {
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n",
+			       __func__, (int)resource, (unsigned)firstParam);
+			return;
+		}
+
+		cm_debug_destroy_tcm_file(idx);
+
+		/* Stop the DSP load monitoring */
+		clear_bit(idx, &running_dsp);
+		if (osalEnv.mpc[idx].monitor_tsk) {
+			kthread_stop(osalEnv.mpc[idx].monitor_tsk);
+			osalEnv.mpc[idx].monitor_tsk = NULL;
+		}
+
+		/* Stop the DMA (normally done on DSP side, but be safe) */
+		if (firstParam == SIA_CORE_ID)
+			cmdma_stop_dma();
+
+		/* Stop the DSP */
+		if (regulator_disable(osalEnv.mpc[idx].mmdsp_regulator) < 0)
+			pr_err("CM Driver(%s): can't disable regulator %s-mmsdp\n",
+			       __func__, osalEnv.mpc[idx].name);
+#ifdef CONFIG_HAS_WAKELOCK
+		wake_unlock(&osalEnv.mpc[idx].wakelock);
+#endif
+
+		/* Create and dispatch a shutdown service message */
+		msg.msg_type = MSG_SERVICE;
+		msg.d.srv.srvType = NMF_SERVICE_SHUTDOWN;
+		msg.d.srv.srvData.shutdown.coreid = firstParam;
+		dispatch_service_msg(&msg);
+		break;
+	}
+	case CM_OSAL_POWER_SxA_AUTOIDLE:
+		switch (firstParam) {
+		case SVA_CORE_ID:
+			osalEnv.dsp_sleep.sva_auto_pm_enable = PRCMU_AUTO_PM_OFF;
+			osalEnv.dsp_sleep.sva_power_on       = 0;
+			osalEnv.dsp_sleep.sva_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF;
+			break;
+		case SIA_CORE_ID:
+			osalEnv.dsp_sleep.sia_auto_pm_enable = PRCMU_AUTO_PM_OFF;
+			osalEnv.dsp_sleep.sia_power_on       = 0;
+			osalEnv.dsp_sleep.sia_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_HWP_OFF;
+			break;
+		default:
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n", __func__, (int)resource, (unsigned)firstParam);
+			return;
+		}
+		if (enable_auto_pm)
+			prcmu_configure_auto_pm(&osalEnv.dsp_sleep, &osalEnv.dsp_idle);
+		break;
+	case CM_OSAL_POWER_SxA_HARDWARE: {
+		unsigned idx = COREIDX(firstParam);
+		if (idx >= NB_MPC) {
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n",
+			       __func__, (int)resource, (unsigned)firstParam);
+			return;
+		}
+		if (regulator_disable(osalEnv.mpc[idx].pipe_regulator) < 0)
+			pr_err("CM Driver(%s): can't disable regulator %s-pipe\n",
+			       __func__, osalEnv.mpc[idx].name);
+		break;
+	}
+	case CM_OSAL_POWER_HSEM:
+		break;
+	case CM_OSAL_POWER_SDRAM:
+		break;
+	case CM_OSAL_POWER_ESRAM: {
+		int i;
+		/* firstParam: base address; secondParam: size
+		   U8500_ESRAM_BASE is the start address of BANK 0,
+		   BANK size=0x20000 */
+
+		/* Compute the relative end address of the range,
+		   relative to base address of BANK1 */
+		secondParam = (firstParam+secondParam-U8500_ESRAM_BANK1-1);
+
+		/* if end is below base address of BANK1, it means that full
+		   range of addresses is on Bank0 */
+		if (((int)secondParam) < 0)
+			break;
+		/* Compute the index of the last bank accessed among
+		   esram 1+2 and esram 3+4 banks */
+		secondParam /= (2*U8500_ESRAM_BANK_SIZE);
+		WARN_ON(secondParam > 1);
+
+		/* Compute the index of the first bank accessed among esram 1+2
+		   and esram 3+4 banks
+		   Do not manage Bank 0 (secured, must be always ON) */
+		if (firstParam < U8500_ESRAM_BANK1)
+			firstParam  = 0;
+		else
+			firstParam  = (firstParam-U8500_ESRAM_BANK1)/(2*U8500_ESRAM_BANK_SIZE);
+
+		/* power off the banks 1+2 and 3+4 if accessed. */
+		for (i=firstParam; i<=secondParam; i++) {
+			if (regulator_disable(osalEnv.esram_regulator[i]) < 0)
+				pr_err("CM Driver(%s): can't disable regulator"
+				       "for esram bank %s\n", __func__,
+				       i ? "34" : "12");
+		}
+		break;
+	}
+	default:
+		pr_err("CM Driver(%s): resource %d unknown/not supported\n",
+		       __func__, (int)resource);
+	}
+}
+
+/** \ingroup OSAL_IMPLEMENTATION
+ *     Used by CM to enable a power resource 
+ */
+t_cm_error OSAL_EnablePwrRessource(t_nmf_power_resource resource, t_uint32 firstParam, t_uint32 secondParam)
+{
+	switch (resource) {
+	case CM_OSAL_POWER_SxA_CLOCK: {
+		unsigned idx = COREIDX(firstParam);
+
+		if (idx > NB_MPC) {
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n", __func__, (int)resource, (unsigned)firstParam);
+			return CM_INVALID_PARAMETER;
+		}
+
+		/* Start the DSP */
+#ifdef CONFIG_HAS_WAKELOCK
+		wake_lock(&osalEnv.mpc[idx].wakelock);
+#endif
+		if (regulator_enable(osalEnv.mpc[idx].mmdsp_regulator) < 0)
+			pr_err("CM Driver(%s): can't enable regulator %s-mmsdp\n", __func__, osalEnv.mpc[idx].name);
+
+		/* Start the DSP load monitoring for this dsp */
+		set_bit(idx, &running_dsp);
+		osalEnv.mpc[idx].monitor_tsk = kthread_run(&dspload_monitor,
+							   (void*)idx,
+							   "%s-loadd",
+							   osalEnv.mpc[idx].name);
+		if (IS_ERR(osalEnv.mpc[idx].monitor_tsk)) {
+			pr_err("CM Driver: failed to start dspmonitord "
+			       "thread: %ld\n", PTR_ERR(osalEnv.mpc[idx].monitor_tsk));
+			osalEnv.mpc[idx].monitor_tsk = NULL;
+		}
+
+		cm_debug_create_tcm_file(idx);
+		break;
+	}
+	case CM_OSAL_POWER_SxA_AUTOIDLE:
+		switch (firstParam) {
+		case SVA_CORE_ID:
+			osalEnv.dsp_sleep.sva_auto_pm_enable = PRCMU_AUTO_PM_ON;
+			osalEnv.dsp_sleep.sva_power_on       = PRCMU_AUTO_PM_POWER_ON_HSEM | PRCMU_AUTO_PM_POWER_ON_ABB_FIFO_IT;
+			osalEnv.dsp_sleep.sva_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_RAMRET_HWP_OFF;
+			break;
+		case SIA_CORE_ID:
+			osalEnv.dsp_sleep.sia_auto_pm_enable = PRCMU_AUTO_PM_ON;
+			osalEnv.dsp_sleep.sia_power_on       = PRCMU_AUTO_PM_POWER_ON_HSEM | PRCMU_AUTO_PM_POWER_ON_ABB_FIFO_IT;
+			osalEnv.dsp_sleep.sia_policy         = PRCMU_AUTO_PM_POLICY_DSP_OFF_RAMRET_HWP_OFF;
+			break;
+		default:
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n", __func__, (int)resource, (unsigned)firstParam);
+			return CM_INVALID_PARAMETER;
+		}
+		if (enable_auto_pm)
+			prcmu_configure_auto_pm(&osalEnv.dsp_sleep, &osalEnv.dsp_idle);
+		break;
+	case CM_OSAL_POWER_SxA_HARDWARE: {
+		unsigned idx = COREIDX(firstParam);
+
+		if (idx > NB_MPC) {
+			pr_err("CM Driver(%s(res=%d)): core %u unknown\n", __func__, (int)resource, (unsigned)firstParam);
+			return CM_INVALID_PARAMETER;
+		}
+		if (regulator_enable(osalEnv.mpc[idx].pipe_regulator) < 0)
+			pr_err("CM Driver(%s): can't enable regulator %s-pipe\n", __func__, osalEnv.mpc[idx].name);
+		break;
+	}
+	case CM_OSAL_POWER_HSEM:
+		return CM_OK;
+	case CM_OSAL_POWER_SDRAM:
+		break;
+	case CM_OSAL_POWER_ESRAM:
+	{
+		int i;
+		/* firstParam: base address; secondParam: size
+		   U8500_ESRAM_BASE is the start address of BANK 0,
+		   BANK size=0x20000 */
+
+		/* Compute the relative end address of the range, relative
+		   to base address of BANK1 */
+		secondParam = (firstParam+secondParam-U8500_ESRAM_BANK1-1);
+
+		/* if end is below base address of BANK1, it means that full
+		   range of addresses is on Bank0 */
+		if (((int)secondParam) < 0)
+			break;
+		/* Compute the index of the last bank accessed among esram 1+2
+		   and esram 3+4 banks */
+		secondParam /= (2*U8500_ESRAM_BANK_SIZE);
+		WARN_ON(secondParam > 1);
+
+		/* Compute the index of the first bank accessed among esram 1+2
+		   and esram 3+4 banks
+		   Do not manage Bank 0 (secured, must be always ON) */
+		if (firstParam < U8500_ESRAM_BANK1)
+			firstParam  = 0;
+		else
+			firstParam  = (firstParam-U8500_ESRAM_BANK1)/(2*U8500_ESRAM_BANK_SIZE);
+
+		/* power on the banks 1+2 and 3+4 if accessed. */
+		for (i=firstParam; i<=secondParam; i++) {
+			if (regulator_enable(osalEnv.esram_regulator[i]) < 0)
+				pr_err("CM Driver(%s): can't enable regulator "
+				       "for esram bank %s\n", __func__,
+				       i ? "34" : "12");
+		}
+		break;
+	}
+	default:
+		pr_err("CM Driver(%s): resource %x unknown/not supported\n",
+		       __func__, (int)resource);
+		return CM_INVALID_PARAMETER;
+	}
+
+	return CM_OK;
+}
+
+/*!
+  * \brief Generate 'software' panic to notify cm users
+  *  that a problem occurs but no dsp panic has been sent yet
+  * (for example a dsp crash)
+  * \ingroup CM_ENGINE_OSAL_API
+  */
+void OSAL_GeneratePanic(t_nmf_core_id coreId, t_uint32 reason)
+{
+	struct osal_msg msg;
+
+	/* Create and dispatch a shutdown service message */
+	msg.msg_type = MSG_SERVICE;
+	msg.d.srv.srvType = NMF_SERVICE_PANIC;
+	msg.d.srv.srvData.panic.panicReason = MPC_NOT_RESPONDING_PANIC;
+	msg.d.srv.srvData.panic.panicSource = MPC_EE;
+	msg.d.srv.srvData.panic.info.mpc.coreid = coreId;
+	msg.d.srv.srvData.panic.info.mpc.faultingComponent = 0;
+	msg.d.srv.srvData.panic.info.mpc.panicInfo1 = reason;
+	msg.d.srv.srvData.panic.info.mpc.panicInfo2 = 0;
+	dispatch_service_msg(&msg);
+}
+
+/*!
+ * \brief Generate an OS-Panic. Called in from CM_ASSERT().
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+void OSAL_Panic(void)
+{
+	panic("FATAL ISSUE IN THE CM DRIVER !!");
+}
+#include <mach/dcache.h>
+/*!
+ * \brief Clean data cache in DDR in order to be accessible from peripheral.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+void OSAL_CleanDCache(t_uint32 startAddr, t_uint32 size)
+{
+#if 0
+	/*
+	 * Currently, the code sections are non-cached/buffered,
+	 * which normally doesn't required the maintenance done below.
+	 * As the cost is low (doesn't do much thing), I keep it in case
+	 * of the memory settings are changed later.
+	 */
+
+	struct hwmem_region region;
+	struct mpcConfig *mpc;
+	t_uint32 endAddr = startAddr + size;
+
+	if (startAddr >= (u32)osalEnv.mpc[0].sdram_code.data
+	    && endAddr <= (u32)(osalEnv.mpc[0].sdram_code.data
+				+ osalEnv.mpc[0].sdram_code.size)) {
+		mpc = &osalEnv.mpc[0];
+	} else if (startAddr >= (u32)osalEnv.mpc[1].sdram_code.data
+		   && endAddr <= (u32)(osalEnv.mpc[1].sdram_code.data
+				       + osalEnv.mpc[1].sdram_code.size)) {
+		mpc = &osalEnv.mpc[1];
+	} else {
+		/* The code may be in esram, in that case, nothing to do */
+		return;
+	}
+
+	region.offset = startAddr - (u32)mpc->sdram_code.data;
+	region.count  = 1;
+	region.start  = 0;
+	region.end    = size;
+	region.size   = size;
+	hwmem_set_domain(mpc->hwmem_code, HWMEM_ACCESS_READ,
+			 HWMEM_DOMAIN_SYNC, &region);
+	/*
+	 * The hwmem keep track of region being sync or not.
+	 * Mark the region as being write-accessed here right now
+	 * to let following clean being done as expected. Today,
+	 * there is no other place to do that in CM Core right now
+	 */
+	hwmem_set_domain(mpc->hwmem_code, HWMEM_ACCESS_WRITE,
+			 HWMEM_DOMAIN_CPU, &region);
+#else
+	dsb();
+	outer_cache.sync();
+#endif
+}
+
+/*!
+ * \brief Flush write-buffer of L2 cache
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+void OSAL_mb(void)
+{
+	mb();
+}
+
+/*!
+ * \brief return prcmu timer value.
+ *
+ * This is need for perfmeter api  (see \ref t_nmf_power_resource)
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+t_uint64 OSAL_GetPrcmuTimer()
+{
+	t_uint64 msbBefore;
+	t_uint32 lsb;
+	t_uint64 msbAfter;
+
+	/* read prcmu timers */
+	msbBefore = ~ioread32(prcmu_tcdm_base+0xDE4);
+	lsb = ~ioread32(prcmu_base+0x454);
+	msbAfter = ~ioread32(prcmu_tcdm_base+0xDE4);
+
+	/* handle rollover test case */
+	// NOTE : there is still a window in prcmu side between counter rollover
+	// and prcmu interrupt handling
+	// to update msb register => this can lead to erroneous value return here
+	if (msbBefore == msbAfter || lsb >= 0x80000000UL)
+		return (((msbBefore & 0xffffffUL) << 32) + lsb);
+	else
+		return (((msbAfter & 0xffffffUL) << 32) + lsb);
+}
+
+/*!
+ * \brief Disable the service message handling (panic, etc)
+ *
+ * It must disable the handling of all service messages
+ * If a service message is currently handled, it must wait till the end
+ * of its managment before returning.
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_DisableServiceMessages(void) {
+	tasklet_disable(&cmld_service_tasklet);
+}
+
+/*!
+ * \brief Enable the service message handling (panic, etc)
+ *
+ * It enables the handling of all service messages
+ *
+ * \ingroup CM_ENGINE_OSAL_API
+ */
+PUBLIC void OSAL_EnableServiceMessages(void) {
+	tasklet_enable(&cmld_service_tasklet);
+}
diff --git a/drivers/staging/nmf-cm/osal-kernel.h b/drivers/staging/nmf-cm/osal-kernel.h
new file mode 100644
index 00000000000..29b82368d8d
--- /dev/null
+++ b/drivers/staging/nmf-cm/osal-kernel.h
@@ -0,0 +1,168 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Pierre Peiffer <pierre.peiffer@stericsson.com> for ST-Ericsson.
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef OSAL_KERNEL_H
+#define OSAL_KERNEL_H
+
+#include <linux/debugfs.h>
+#include <linux/interrupt.h>
+#include <linux/hwmem.h>
+#include <linux/regulator/consumer.h>
+#include <linux/plist.h>
+#include <linux/version.h>
+#ifdef CONFIG_HAS_WAKELOCK
+#include <linux/wakelock.h>
+#endif
+#include <linux/mfd/dbx500-prcmu.h>
+#include <cm/engine/api/channel_engine.h>
+#include <cm/engine/api/control/configuration_engine.h>
+#include <cm/engine/api/perfmeter_engine.h>
+/*
+ * Do not include ELF definition from cm/engine/elf/inc/elfapi.h file
+ * because it conflicts with definition from linux/elf.h file
+ */
+#define _CM_ELF_H
+#include <cm/engine/os_adaptation_layer/inc/os_adaptation_layer.h>
+
+#include "configuration.h"
+
+/*
+ * Per-MPC configuration structure
+ * Use struct debugfs_blob_wrapper to store pointer and size of section
+ * to allow easy re-use of data through debugfs
+ */
+struct mpcConfig {
+	const t_nmf_core_id coreId;      /**< MPC coreId */
+	const char *name;                /**< MPC name */
+        t_uint8    nbYramBanks;          /**< number of TCM ram banks to reserve for y memory */
+	t_nmf_executive_engine_id eeId;  /**< Type of Executive Engine */
+	const void *base_phys;           /**< Physical base address of the MPC */
+	struct debugfs_blob_wrapper base;/**< Remapped base address of the MPC and size of TCM24 */
+	struct hwmem_alloc *hwmem_code;  /**< hwmem code segment */
+	u32        sdram_code_phys;      /**< Physical base address for MPC SDRAM Code region */
+	struct debugfs_blob_wrapper sdram_code; /**< Remapped base address and size for MPC SDRAM Code */
+	struct hwmem_alloc *hwmem_data;  /**< hwmem data segment */
+	u32        sdram_data_phys;      /**< Physical base address for MPC SDRAM Data region */
+	struct debugfs_blob_wrapper sdram_data; /**< Remapped base address and size for MPC SDRAM Data */
+	const unsigned int interrupt0;   /**< interrupt line triggered by the MPC, for MPC events (if HSEM not used) */
+	const unsigned int interrupt1;   /**< interrupt line triggered by the MPC, for PANIC events */
+	struct tasklet_struct tasklet;   /**< taskket used to process MPC events */
+	struct regulator *mmdsp_regulator; /**< mmdsp regulator linked to this MPC */
+	struct regulator *pipe_regulator;  /**< hardware pipe linked to this MPC */
+#ifdef CONFIG_HAS_WAKELOCK
+	struct wake_lock wakelock;       /**< wakelock for this MPC to prevent ARM to go in APSLEEP state */
+#endif
+	struct task_struct *monitor_tsk; /**< task to monitor the dsp load; */
+	t_cm_mpc_load_counter oldLoadCounter; /**< previous load counter of the DSP */
+	atomic_t trace_read_count;       /**< number of trace reader */
+	spinlock_t trace_reader_lock;
+	struct task_struct *trace_reader;/**< current reader task */
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dir;              /**< debugfs dir entry */
+	struct dentry *comp_dir;         /**< debugfs component dir entry */
+	struct dentry *domain_dir;       /**< debugfs domain dir entry */
+	struct dentry *snapshot_dir;     /**< debugfs snapshot dir entry */
+	struct dentry *mem_file;         /**< debugfs meminfo file entry */
+	struct dentry *tcm_file;         /**< debugfs meminfo file entry */
+	struct dentry *esram_file;       /**< debugfs meminfo file entry */
+	s8         load;                 /**< current load of the DSP */
+	s8         opp_request;          /**< current requested opp of the DSP */
+#endif
+};
+
+/** Describes current Kernel OSAL environment
+ *
+ * Note about mpc.tasklet : we declare one tasklet per MPC but their usage depends
+ * on cfgSemaphoreTypeHSEM.
+ *
+ * This tasklet is scheduled by the interrupt handler to process MPC Events.
+ * - If we use Hardware Semaphore, there is only one interrupt handler used
+ * and thus only one tasklet, tasklet of MPC 0 (ie osalEnv.mpc[0].tasklet)
+ * - If we use local semaphore, there is one interrupt handler and tasklet per mpc
+ */
+struct OsalEnvironment
+{
+	struct mpcConfig mpc[NB_MPC];
+	void* hwsem_base;  /** < Remapped base address of the hardware semaphores */
+	void* esram_base;  /**< Remapped base address embedded RAM used within the CM */
+	struct regulator *esram_regulator[NB_ESRAM]; /**< regulator for ESRAM bank 1+2 and 3+4 */
+	struct prcmu_auto_pm_config dsp_sleep;
+	struct prcmu_auto_pm_config dsp_idle;
+};
+
+
+/** Structure used to store the skeleton related data.
+ *  It is used for communicattion from a MPC to a user process (=host)
+ */
+typedef struct {
+	struct list_head entry; /**< Doubly linked list descriptor */
+	t_cm_bf_mpc2host_handle mpc2hostId;  /**< mpc2host ID */
+	t_nmf_mpc2host_handle upperLayerThis;/**< upper-layer handle */
+	struct cm_channel_priv* channelPriv; /**< Per-channel private data. The actual message queue is hold here */
+} t_skelwrapper;
+
+/** Message description for MPC to HOST communication
+ */
+struct osal_msg {
+	struct {
+		struct plist_node entry; /**< Doubly linked list descriptor */
+		t_message_type type;     /**< Type of message (callback, service or interrupt for now) */
+	} hdr; /**< Header of the message */
+#define msg_entry hdr.entry
+#define msg_type hdr.type
+	union {
+		struct {
+			t_skelwrapper *skelwrap; /**< Link to the skelwrapper, to retrieve the channel on which this message has to be forwarded */
+			t_uint32 methodIdx; /**< callback data: method index*/
+			t_event_params_handle anyPtr;  /**< callback data: method parameters */
+			t_uint32 ptrSize;  /**< size of the parameters */
+		} itf; /**< structure holding callback data */
+		struct {
+			t_nmf_service_type srvType; /**< Type of the service */
+			t_nmf_service_data srvData; /**< Data of the service */
+		} srv; /**< structure holding service data */
+	} d; /**< data */
+};
+
+extern struct OsalEnvironment osalEnv;
+
+/** Environment initialization/deinitialization */
+int remapRegions(void);
+void unmapRegions(void);
+
+/** Component manager configuration getters for CM_ENGINE_Init() */
+int getNmfHwMappingDesc(t_nmf_hw_mapping_desc* nmfHwMappingDesc);
+
+/** Component manager configuration getters for CM_ConfigureMediaProcessorCore (SVA and SIA) */
+void getMpcSystemAddress(unsigned i, t_cm_system_address* mpcSystemAddress);
+void getMpcSdramSegments(unsigned i, t_nmf_memory_segment* codeSegment, t_nmf_memory_segment* dataSegment);
+
+#ifdef CM_DEBUG_ALLOC
+struct cm_alloc {
+	spinlock_t        lock;
+	struct list_head  chain;
+};
+
+struct cm_alloc_elem {
+	struct list_head elem;
+	void *caller;
+	size_t size;
+	char addr[0];
+};
+
+void init_debug_alloc(void);
+void cleanup_debug_alloc(void);
+#endif /* CM_DEBUG_ALLOC */
+
+/* TODO: To remove later */
+extern __iomem void *prcmu_base;
+extern __iomem void *prcmu_tcdm_base;
+extern const char *cmld_devname[];
+
+#define PRCM_SVAMMDSPCLK_MGT                (prcmu_base + 0x008)
+#define PRCM_SIAMMDSPCLK_MGT                (prcmu_base + 0x00c)
+
+#endif /* OSAL_KERNEL_H */
diff --git a/drivers/staging/nmf-cm/share/communication/inc/communication_fifo.h b/drivers/staging/nmf-cm/share/communication/inc/communication_fifo.h
new file mode 100644
index 00000000000..ea24e82ceae
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/communication/inc/communication_fifo.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_NMF_COM_FIFO
+#define __INC_NMF_COM_FIFO
+
+#include <inc/typedef.h>
+
+#define EVENT_ELEM_METHOD_IDX       0
+#define EVENT_ELEM_PARAM_IDX        1
+#define EVENT_ELEM_EXTFIELD_IDX     2
+
+#define EVENT_ELEM_SIZE_IN_BYTE     (3 * sizeof(t_shared_field))
+
+#endif /* __INC_NMF_COM_FIFO */
diff --git a/drivers/staging/nmf-cm/share/communication/inc/initializer.h b/drivers/staging/nmf-cm/share/communication/inc/initializer.h
new file mode 100644
index 00000000000..10985c3981a
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/communication/inc/initializer.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_SHARE_INITIALIZER
+#define __INC_SHARE_INITIALIZER
+
+#define NMF_CONSTRUCT_INDEX         0
+#define NMF_START_INDEX             1
+#define NMF_STOP_INDEX              2
+#define NMF_DESTROY_INDEX           3
+#define NMF_UPDATE_STACK            4
+#define NMF_LOCK_CACHE              5
+#define NMF_UNLOCK_CACHE            6
+#define NMF_ULP_FORCEWAKEUP         7
+#define NMF_ULP_ALLOWSLEEP          8
+#define NMF_CONSTRUCT_SYNC_INDEX    9
+#define NMF_START_SYNC_INDEX        10
+#define NMF_STOP_SYNC_INDEX         11
+
+/*
+ * Index of datas in command parameter format
+ */
+#define INIT_COMPONENT_CMD_HANDLE_INDEX         0
+#define INIT_COMPONENT_CMD_THIS_INDEX           2
+#define INIT_COMPONENT_CMD_METHOD_INDEX         4
+#define INIT_COMPONENT_CMD_SIZE                 6
+
+/*
+ * Index of datas in acknowledge parameter format
+ */
+#define INIT_COMPONENT_ACK_HANDLE_INDEX         0
+#define INIT_COMPONENT_ACK_SIZE                 2
+
+#endif /* __INC_SHARE_INITIALIZER */
diff --git a/drivers/staging/nmf-cm/share/communication/inc/nmf_fifo_desc.h b/drivers/staging/nmf-cm/share/communication/inc/nmf_fifo_desc.h
new file mode 100644
index 00000000000..99caa48b05c
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/communication/inc/nmf_fifo_desc.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_NMF_FIFO_DESC
+#define __INC_NMF_FIFO_DESC
+
+#include <inc/typedef.h>
+#include <share/semaphores/inc/semaphores.h>
+
+/*
+ * SHOULD be mapped onto a AHB burst (16 bytes=8x16-bit)
+ */
+typedef struct {
+    t_semaphore_id semId;
+
+    t_uint16 elemSize;
+    t_uint16 fifoFullValue;
+    t_uint16 readIndex;
+    t_uint16 writeIndex;
+    t_uint16 wrappingValue;
+
+    t_uint32 extendedField; /* in DSP 24 memory when to MPC in Logical Host when to ARM */
+} t_nmf_fifo_desc;
+
+#define EXTENDED_FIELD_BCTHIS_OR_TOP        0       //<! This field will be used:
+                                                    //<! - as hostBCThis for DSP->HOST binding
+                                                    //<! - as TOP else
+#define EXTENDED_FIELD_BCDESC               1       //<! This field will be used for:
+                                                    //<! - interface method address for ->MPC binding
+                                                    //<! - for params size for ->Host binding (today only [0] is used as max size)
+
+#endif /* __INC_NMF_FIFO */
diff --git a/drivers/staging/nmf-cm/share/communication/inc/nmf_service.h b/drivers/staging/nmf-cm/share/communication/inc/nmf_service.h
new file mode 100644
index 00000000000..71dfc534f97
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/communication/inc/nmf_service.h
@@ -0,0 +1,17 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_NMF_SERVICE_H
+#define __INC_NMF_SERVICE_H
+
+/* 1 - 0xff Reserved for Panic Reason */
+#define MPC_SERVICE_NONE                    0
+#define MPC_SERVICE_BOOT                    0xB001
+#define MPC_SERVICE_PRINT                   0x1234
+#define MPC_SERVICE_TRACE                   0x789
+
+#endif
diff --git a/drivers/staging/nmf-cm/share/inc/macros.h b/drivers/staging/nmf-cm/share/inc/macros.h
new file mode 100644
index 00000000000..7d2c2289cd3
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/inc/macros.h
@@ -0,0 +1,213 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief NMF Macro API.
+ */
+
+#ifndef _COMMON_MACROS_H_
+#define _COMMON_MACROS_H_
+
+#undef ALIGN_VALUE
+#define ALIGN_VALUE(value, alignment) (((value) + (alignment - 1)) & ~(alignment - 1))
+
+#undef MIN
+#define MIN(a,b) (((a)>(b))?(b):(a))
+
+#undef MAX
+#define MAX(a,b) (((a)<(b))?(b):(a))
+
+/*-----------------------------------------------------------------------------
+ * endianess switch macros (32 bits and 16 bits)
+ *---------------------------------------------------------------------------*/
+#define ENDIANESS_32_SWITCH(value) ( \
+        (((value) & MASK_BYTE3) >> SHIFT_BYTE3) | \
+        (((value) & MASK_BYTE2) >> SHIFT_BYTE1) | \
+        (((value) & MASK_BYTE1) << SHIFT_BYTE1) | \
+        (((value) & MASK_BYTE0) << SHIFT_BYTE3)   \
+        )
+
+#define ENDIANESS_16_SWITCH(value) ( \
+        (((value) & MASK_BYTE0) << SHIFT_BYTE1) | \
+        (((value) & MASK_BYTE1) >> SHIFT_BYTE1)   \
+        )
+
+/*-----------------------------------------------------------------------------
+ * field offset extraction from a structure
+ *---------------------------------------------------------------------------*/
+#undef FIELD_OFFSET
+#define FIELD_OFFSET(typeName, fieldName) ((t_uint32)(&(((typeName *)0)->fieldName)))
+
+#undef MASK_BIT
+#define MASK_BIT(n)  				(1UL << ((n) - 1))
+
+/*-----------------------------------------------------------------------------
+ * Misc definition
+ *---------------------------------------------------------------------------*/
+
+#undef ONE_KB
+#define ONE_KB (1024)
+#undef ONE_MB
+#define ONE_MB (ONE_KB * ONE_KB)
+
+/*-----------------------------------------------------------------------------
+ * Bit mask definition
+ *---------------------------------------------------------------------------*/
+#undef MASK_NULL8
+#define MASK_NULL8     0x00U
+#undef MASK_NULL16
+#define MASK_NULL16    0x0000U
+#undef MASK_NULL32
+#define MASK_NULL32    0x00000000UL
+#undef MASK_ALL8
+#define MASK_ALL8      0xFFU
+#undef MASK_ALL16
+#define MASK_ALL16     0xFFFFU
+#undef MASK_ALL32
+#define MASK_ALL32     0xFFFFFFFFUL
+
+#undef MASK_BIT0
+#define MASK_BIT0      (1UL<<0)
+#undef MASK_BIT1
+#define MASK_BIT1      (1UL<<1)
+#undef MASK_BIT2
+#define MASK_BIT2      (1UL<<2)
+#undef MASK_BIT3
+#define MASK_BIT3      (1UL<<3)
+#undef MASK_BIT4
+#define MASK_BIT4      (1UL<<4)
+#undef MASK_BIT5
+#define MASK_BIT5      (1UL<<5)
+#undef MASK_BIT6
+#define MASK_BIT6      (1UL<<6)
+#undef MASK_BIT7
+#define MASK_BIT7      (1UL<<7)
+#undef MASK_BIT8
+#define MASK_BIT8      (1UL<<8)
+#undef MASK_BIT9
+#define MASK_BIT9      (1UL<<9)
+#undef MASK_BIT10
+#define MASK_BIT10     (1UL<<10)
+#undef MASK_BIT11
+#define MASK_BIT11     (1UL<<11)
+#undef MASK_BIT12
+#define MASK_BIT12     (1UL<<12)
+#undef MASK_BIT13
+#define MASK_BIT13     (1UL<<13)
+#undef MASK_BIT14
+#define MASK_BIT14     (1UL<<14)
+#undef MASK_BIT15
+#define MASK_BIT15     (1UL<<15)
+#undef MASK_BIT16
+#define MASK_BIT16     (1UL<<16)
+#undef MASK_BIT17
+#define MASK_BIT17     (1UL<<17)
+#undef MASK_BIT18
+#define MASK_BIT18     (1UL<<18)
+#undef MASK_BIT19
+#define MASK_BIT19     (1UL<<19)
+#undef MASK_BIT20
+#define MASK_BIT20     (1UL<<20)
+#undef MASK_BIT21
+#define MASK_BIT21     (1UL<<21)
+#undef MASK_BIT22
+#define MASK_BIT22     (1UL<<22)
+#undef MASK_BIT23
+#define MASK_BIT23     (1UL<<23)
+#undef MASK_BIT24
+#define MASK_BIT24     (1UL<<24)
+#undef MASK_BIT25
+#define MASK_BIT25     (1UL<<25)
+#undef MASK_BIT26
+#define MASK_BIT26     (1UL<<26)
+#undef MASK_BIT27
+#define MASK_BIT27     (1UL<<27)
+#undef MASK_BIT28
+#define MASK_BIT28     (1UL<<28)
+#undef MASK_BIT29
+#define MASK_BIT29     (1UL<<29)
+#undef MASK_BIT30
+#define MASK_BIT30     (1UL<<30)
+#undef MASK_BIT31
+#define MASK_BIT31     (1UL<<31)
+
+/*-----------------------------------------------------------------------------
+ * quartet shift definition
+ *---------------------------------------------------------------------------*/
+#undef MASK_QUARTET
+#define MASK_QUARTET     (0xFUL)
+#undef SHIFT_QUARTET0
+#define SHIFT_QUARTET0   0
+#undef SHIFT_QUARTET1
+#define SHIFT_QUARTET1   4
+#undef SHIFT_QUARTET2
+#define SHIFT_QUARTET2   8
+#undef SHIFT_QUARTET3
+#define SHIFT_QUARTET3   12
+#undef SHIFT_QUARTET4
+#define SHIFT_QUARTET4   16
+#undef SHIFT_QUARTET5
+#define SHIFT_QUARTET5   20
+#undef SHIFT_QUARTET6
+#define SHIFT_QUARTET6   24
+#undef SHIFT_QUARTET7
+#define SHIFT_QUARTET7   28
+#undef MASK_QUARTET0
+#define MASK_QUARTET0    (MASK_QUARTET << SHIFT_QUARTET0)
+#undef MASK_QUARTET1
+#define MASK_QUARTET1    (MASK_QUARTET << SHIFT_QUARTET1)
+#undef MASK_QUARTET2
+#define MASK_QUARTET2    (MASK_QUARTET << SHIFT_QUARTET2)
+#undef MASK_QUARTET3
+#define MASK_QUARTET3    (MASK_QUARTET << SHIFT_QUARTET3)
+#undef MASK_QUARTET4
+#define MASK_QUARTET4    (MASK_QUARTET << SHIFT_QUARTET4)
+#undef MASK_QUARTET5
+#define MASK_QUARTET5    (MASK_QUARTET << SHIFT_QUARTET5)
+#undef MASK_QUARTET6
+#define MASK_QUARTET6    (MASK_QUARTET << SHIFT_QUARTET6)
+#undef MASK_QUARTET7
+#define MASK_QUARTET7    (MASK_QUARTET << SHIFT_QUARTET7)
+
+/*-----------------------------------------------------------------------------
+ * Byte shift definition
+ *---------------------------------------------------------------------------*/
+#undef MASK_BYTE
+#define MASK_BYTE       (0xFFUL)
+#undef SHIFT_BYTE0
+#define SHIFT_BYTE0 	0U
+#undef SHIFT_BYTE1
+#define SHIFT_BYTE1 	8U
+#undef SHIFT_BYTE2
+#define SHIFT_BYTE2 	16U
+#undef SHIFT_BYTE3
+#define SHIFT_BYTE3 	24U
+#undef MASK_BYTE0
+#define MASK_BYTE0       (MASK_BYTE << SHIFT_BYTE0)
+#undef MASK_BYTE1
+#define MASK_BYTE1       (MASK_BYTE << SHIFT_BYTE1)
+#undef MASK_BYTE2
+#define MASK_BYTE2       (MASK_BYTE << SHIFT_BYTE2)
+#undef MASK_BYTE3
+#define MASK_BYTE3       (MASK_BYTE << SHIFT_BYTE3)
+
+/*-----------------------------------------------------------------------------
+ * Halfword shift definition
+ *---------------------------------------------------------------------------*/
+#undef MASK_HALFWORD
+#define MASK_HALFWORD        (0xFFFFUL)
+#undef SHIFT_HALFWORD0
+#define SHIFT_HALFWORD0 	    0U
+#undef SHIFT_HALFWORD1
+#define SHIFT_HALFWORD1 	    16U
+#undef MASK_HALFWORD0
+#define MASK_HALFWORD0       (MASK_HALFWORD << SHIFT_HALFWORD0)
+#undef MASK_HALFWORD1
+#define MASK_HALFWORD1       (MASK_HALFWORD << SHIFT_HALFWORD1)
+
+#endif /* _COMMON_MACROS_H_ */
+
diff --git a/drivers/staging/nmf-cm/share/inc/nmf.h b/drivers/staging/nmf-cm/share/inc/nmf.h
new file mode 100644
index 00000000000..2f73311c2f3
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/inc/nmf.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Jean-Philippe FASSINO <jean-philippe.fassino@stericsson.com> for ST-Ericsson.
+ * License terms:  GNU General Public License (GPL), version 2, with
+ * user space exemption described in the top-level COPYING file in
+ * the Linux kernel source tree.
+ */
+/*!
+ * \brief Common Nomadik Multiprocessing Framework type definition
+ *
+ * This file contains the shared type definitions used into NMF.
+ */
+
+#ifndef __INC_NMF_H
+#define __INC_NMF_H
+
+#include <inc/typedef.h>
+
+/*!
+ * \brief Identification of the various cores (host cpu and Media Processors) into Nomadik Platform
+ * In order to improve performance, these ids are those used to interconnect HW Semaphores IP with Cores (Interrupt lines)
+ * \ingroup NMF_COMMON
+ */
+#if defined(__STN_8500)
+    //#warning "TODO : mapping below is not correct, need to think how to change it"
+#endif
+typedef t_uint8 t_nmf_core_id;
+#define ARM_CORE_ID                 ((t_nmf_core_id)0)                  //!< HOST CPU Id
+#define SVA_CORE_ID                 ((t_nmf_core_id)1)                  //!< Smart Video Accelerator Media Processor Code Id
+#define SIA_CORE_ID                 ((t_nmf_core_id)2)                  //!< Smart Imaging Accelerator Media Processor Code Id
+#define NB_CORE_IDS                 ((t_nmf_core_id)3)
+
+#define FIRST_CORE_ID               ((t_nmf_core_id)ARM_CORE_ID)
+#define FIRST_MPC_ID                ((t_nmf_core_id)SVA_CORE_ID)
+#define LAST_CORE_ID                ((t_nmf_core_id)SIA_CORE_ID)
+#define LAST_MPC_ID                 ((t_nmf_core_id)SIA_CORE_ID)
+
+
+/*!
+ * \brief Define minimal stack size use by execution engine
+ */
+#define MIN_STACK_SIZE              128
+
+
+
+#endif /* __INC_NMF_H */
diff --git a/drivers/staging/nmf-cm/share/inc/nomadik_mapping.h b/drivers/staging/nmf-cm/share/inc/nomadik_mapping.h
new file mode 100644
index 00000000000..bec221aa111
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/inc/nomadik_mapping.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_NOMADIK_MAPPING_H
+#define __INC_NOMADIK_MAPPING_H
+
+/*--------------------------------------------------------------------------*/
+#if defined(__STN_8810)
+
+/* XTI (CPU OSMO/OSMOT address space) */
+#define XTI_CPU_BASE_ADDR	0x10000000
+#define XTI_CPU_END_ADDR	0x100FFFFF
+
+/* XTI configuration registers */
+#define XTI_CFG_REG_BASE_ADDR	0x101A0000
+#define XTI_CFG_REG_END_ADDR	0x101AFFFF
+
+/* Core APB Peripherals */
+#define CORE_APB_BASE_ADDR	0x101E0000
+#define CORE_APB_END_ADDR	0x101EFFFF
+
+/* DMA APB Peripherals */
+#define DMA_APB_BASE_ADDR	0x101F0000
+#define DMA_APB_END_ADDR	0x101FFFFF
+
+/* XTI (DSP OSMO/OSMOT address space) */
+#define XTI_DSP_BASE_ADDR	0x10200000
+#define XTI_DSP_END_ADDR	0x1020FFFF
+
+#endif  /*  defined(__STN_8810)   */
+
+/*--------------------------------------------------------------------------*/
+#if defined(__STN_8815)
+
+/* XTI (CPU OSMO/OSMOT address space) */
+#define XTI_CPU_BASE_ADDR	0x10000000
+#define XTI_CPU_END_ADDR	0x100FFFFF
+
+/* XTI configuration registers */
+#define XTI_CFG_REG_BASE_ADDR	0x101A0000
+#define XTI_CFG_REG_END_ADDR	0x101AFFFF
+
+/* Core APB Peripherals */
+#define CORE_APB_BASE_ADDR	0x101E0000
+#define CORE_APB_END_ADDR	0x101EFFFF
+
+/* DMA APB Peripherals */
+#define DMA_APB_BASE_ADDR	0x101F0000
+#define DMA_APB_END_ADDR	0x101FFFFF
+
+/* XTI (DSP OSMO/OSMOT address space) */
+#define XTI_DSP_BASE_ADDR	0x10220000
+#define XTI_DSP_END_ADDR	0x1022FFFF
+
+#endif  /*  defined(__STN_8815)   */
+
+
+/*--------------------------------------------------------------------------*/
+#if defined(__STN_8820)
+
+/* STM (System Trace Module address space) */
+#define STM_BASE_ADDR	0x700F0000
+#define STM_END_ADDR	0x700FFFFF
+
+/* AHB2 Peripherals */
+#define AHB2_PERIPH_BASE_ADDR	0x70100000
+#define AHB2_PERIPH_END_ADDR	0x7010FFFF
+
+/* APB2 Peripherals */
+#define APB2_PERIPH_BASE_ADDR	0x70110000
+#define APB2_PERIPH_END_ADDR	0x7011FFFF
+
+/* APB1 Peripherals */
+#define APB1_PERIPH_BASE_ADDR	0x70120000
+#define APB1_PERIPH_END_ADDR	0x7012FFFF
+
+#endif  /*  defined(__STN_8820)   */
+
+/*--------------------------------------------------------------------------*/
+#if defined(__STN_8500)
+/* STM (System Trace Module address space) */
+#define STM_BASE_ADDR       0x80100000
+#define STM_END_ADDR        0x8010FFFF
+
+#define HSEM_BASE_ADDR      0x80140000
+#define HSEM_END_ADDR       0x8014FFFF
+
+#define DMA_CTRL_BASE_ADDR  0x801C0000
+#define DMA_CTRL_END_ADDR   0x801C0FFF
+
+
+#endif  /*  defined(__STN_8500)   */
+
+#endif /*__INC_NOMADIK_MAPPING_H */
diff --git a/drivers/staging/nmf-cm/share/semaphores/inc/hwsem_hwp.h b/drivers/staging/nmf-cm/share/semaphores/inc/hwsem_hwp.h
new file mode 100644
index 00000000000..b573627beae
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/semaphores/inc/hwsem_hwp.h
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_HWSEM_HWP_H
+#define __INC_HWSEM_HWP_H
+
+#include <share/semaphores/inc/semaphores.h>
+
+#define CORE_ID_2_HW_CORE_ID(coreId) (1U << (coreId))
+
+/*
+ * Definition of the number of hw semaphores into the Nomadik IP
+ */
+#define NUM_HW_SEMAPHORES       32
+
+
+/*
+ * Definition of how HSEM IP interrupts are interconnected with cores
+ */
+typedef enum {
+    HSEM_FIRST_INTR = 0,
+    HSEM_INTRA = HSEM_FIRST_INTR,
+    HSEM_INTRB = 1,
+    HSEM_INTRC = 2,
+    HSEM_INTRD = 3,
+    HSEM_INTRE = 4,
+    HSEM_MAX_INTR
+} t_hw_semaphore_irq_id;
+
+/*
+ * Description of the registers of the HW Sem IP
+ */
+#define HSEM_INTRA_MASK  (1<<(4+HSEM_INTRA))
+#define HSEM_INTRB_MASK  (1<<(4+HSEM_INTRB))
+#define HSEM_INTRC_MASK  (1<<(4+HSEM_INTRC))
+#define HSEM_INTRD_MASK  (1<<(4+HSEM_INTRD))
+#define HSEM_INTRE_MASK  (1<<(4+HSEM_INTRE))
+
+typedef struct {
+    t_shared_reg imsc;
+    t_shared_reg ris;
+    t_shared_reg mis;
+    t_shared_reg icr;
+} t_hsem_it_regs;
+
+typedef volatile struct {
+#if defined(__STN_8500)
+    t_shared_reg cr;
+    t_shared_reg dummy;
+#endif
+    t_shared_reg sem[NUM_HW_SEMAPHORES];
+#if defined(__STN_8820)
+    t_shared_reg RESERVED1[(0x90 - 0x80)>>2];
+#elif defined(__STN_8500)
+    t_shared_reg RESERVED1[(0x90 - 0x88)>>2];
+#else /* __STN_8820 or __STN_8500 -> _STN_8815 */
+    t_shared_reg RESERVED1[(0x90 - 0x40)>>2];
+#endif /* __STN_8820 or __STN_8500 -> _STN_8815 */
+    t_shared_reg icrall;
+    t_shared_reg RESERVED2[(0xa0 - 0x94)>>2];
+    t_hsem_it_regs it[HSEM_MAX_INTR];
+#if defined(__STN_8820) || defined(__STN_8500)
+    t_shared_reg RESERVED3[(0x100 - 0xf0)>>2];
+#else /* __STN_8820 or __STN_8500 -> _STN_8815 */
+    t_shared_reg RESERVED3[(0x100 - 0xe0)>>2];
+#endif /* __STN_8820 or __STN_8500 -> _STN_8815 */
+    t_shared_reg itcr;
+    t_shared_reg RESERVED4;
+    t_shared_reg itop;
+    t_shared_reg RESERVED5[(0xfe0 - 0x10c)>>2];
+    t_shared_reg pid0;
+    t_shared_reg pid1;
+    t_shared_reg pid2;
+    t_shared_reg pid3;
+    t_shared_reg pcid0;
+    t_shared_reg pcid1;
+    t_shared_reg pcid2;
+    t_shared_reg pcid3;
+} t_hw_semaphore_regs, *tp_hw_semaphore_regs;
+
+#endif /* __INC_HWSEM_HWP_H */
diff --git a/drivers/staging/nmf-cm/share/semaphores/inc/semaphores.h b/drivers/staging/nmf-cm/share/semaphores/inc/semaphores.h
new file mode 100644
index 00000000000..c72b64cd709
--- /dev/null
+++ b/drivers/staging/nmf-cm/share/semaphores/inc/semaphores.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010. All rights reserved.
+ * This code is ST-Ericsson proprietary and confidential.
+ * Any use of the code for whatever purpose is subject to
+ * specific written permission of ST-Ericsson SA.
+ */
+ 
+#ifndef __INC_SHARED_SEMAPHORE_H
+#define __INC_SHARED_SEMAPHORE_H
+
+#include <share/inc/nmf.h>
+
+typedef t_uint16 t_semaphore_id;
+
+/*
+ * HW semaphore allocation
+ * -----------------------
+ *  We want to optimize interrupt demultiplexing at dsp interrupt handler level
+ *  so a good solution would be to have sequentially the semaphores for each neighbors
+ *
+ * STn8500 :
+ * ---------
+ * ARM <- SVA  COMS => 0
+ * ARM <- SIA  COMS => 1
+ * SVA <- ARM  COMS => 2
+ * SVA <- SIA  COMS => 3
+ * SIA <- ARM  COMS => 4
+ * SIA <- SVA  COMS => 5
+
+ * The first neighbor is always the ARM, then the other ones (SVA,SIA)
+ */
+
+/*
+ * Local semaphore allocation
+ * -----------------------
+ * 0 : ARM <- DSP
+ * 1 : DSP <- ARM
+ */
+
+#define NB_USED_HSEM_PER_CORE (NB_CORE_IDS - 1)
+#define FIRST_NEIGHBOR_SEMID(coreId) ((coreId)*NB_USED_HSEM_PER_CORE)
+
+#endif /* __INC_SHARED_SEMAPHORE_H */
diff --git a/drivers/staging/ste_rmi4/board-mop500-u8500uib-rmi4.c b/drivers/staging/ste_rmi4/board-mop500-u8500uib-rmi4.c
index a272e488e5b..545e03d31fc 100644
--- a/drivers/staging/ste_rmi4/board-mop500-u8500uib-rmi4.c
+++ b/drivers/staging/ste_rmi4/board-mop500-u8500uib-rmi4.c
@@ -20,8 +20,9 @@
 static struct synaptics_rmi4_platform_data rmi4_i2c_dev_platformdata = {
 	.irq_number     = NOMADIK_GPIO_TO_IRQ(84),
 	.irq_type       = (IRQF_TRIGGER_FALLING | IRQF_SHARED),
-	.x_flip		= false,
-	.y_flip		= true,
+	.x_flip		= true,
+	.y_flip		= false,
+	.regulator_en   = true,
 };
 
 struct i2c_board_info __initdata mop500_i2c3_devices_u8500[] = {
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
index 11728a03f8a..fd7fed743f7 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
@@ -1,11 +1,10 @@
-/**
- *
+/*
  * Synaptics Register Mapped Interface (RMI4) I2C Physical Layer Driver.
  * Copyright (c) 2007-2010, Synaptics Incorporated
  *
  * Author: Js HA <js.ha@stericsson.com> for ST-Ericsson
  * Author: Naveen Kumar G <naveen.gaddipati@stericsson.com> for ST-Ericsson
- * Copyright 2010 (c) ST-Ericsson AB
+ * Copyright 2010 (c) ST-Ericsson SA
  */
 /*
  * This file is licensed under the GPL2 license.
@@ -27,6 +26,7 @@
 
 #include <linux/input.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/regulator/consumer.h>
@@ -36,8 +36,10 @@
 /* TODO: for multiple device support will need a per-device mutex */
 #define DRIVER_NAME "synaptics_rmi4_i2c"
 
+#define DELTA			8
 #define MAX_ERROR_REPORT	6
-#define MAX_TOUCH_MAJOR		15
+#define	TIMEOUT_PERIOD		1
+#define MAX_WIDTH_MAJOR		255
 #define MAX_RETRY_COUNT		5
 #define STD_QUERY_LEN		21
 #define PAGE_LEN		2
@@ -45,6 +47,7 @@
 #define BUF_LEN			37
 #define QUERY_LEN		9
 #define DATA_LEN		12
+#define RESUME_DELAY		100 /* msecs */
 #define HAS_TAP			0x01
 #define HAS_PALMDETECT		0x01
 #define HAS_ROTATE		0x02
@@ -164,6 +167,8 @@ struct synaptics_rmi4_device_info {
  * @regulator: pointer to the regulator structure
  * @wait: wait queue structure variable
  * @touch_stopped: flag to stop the thread function
+ * @enable: flag to enable/disable the driver event.
+ * @resume_wq_handler: work queue for resume the device
  *
  * This structure gives the device data information.
  */
@@ -184,6 +189,8 @@ struct synaptics_rmi4_data {
 	struct regulator	*regulator;
 	wait_queue_head_t	wait;
 	bool			touch_stopped;
+	bool			enable;
+	struct work_struct	resume_wq_handler;
 };
 
 /**
@@ -291,6 +298,133 @@ exit:
 }
 
 /**
+ * synaptics_rmi4_enable() - enable the touchpad driver event
+ * @pdata: pointer to synaptics_rmi4_data structure
+ *
+ * This function is to enable the touchpad driver event and returns integer.
+ */
+static int synaptics_rmi4_enable(struct synaptics_rmi4_data *pdata)
+{
+	int retval;
+	unsigned char intr_status;
+
+	if (pdata->board->regulator_en)
+		regulator_enable(pdata->regulator);
+	enable_irq(pdata->board->irq_number);
+	pdata->touch_stopped = false;
+
+	msleep(RESUME_DELAY);
+	retval = synaptics_rmi4_i2c_block_read(pdata,
+					pdata->fn01_data_base_addr + 1,
+					&intr_status,
+					pdata->number_of_interrupt_register);
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_i2c_byte_write(pdata,
+					pdata->fn01_ctrl_base_addr + 1,
+					(intr_status | TOUCHPAD_CTRL_INTR));
+	if (retval < 0)
+		return retval;
+
+	return 0;
+}
+
+/**
+ * synaptics_rmi4_disable() - disable the touchpad driver event
+ * @pdata: pointer to synaptics_rmi4_data structure
+ *
+ * This function is to disable the driver event and returns integer.
+ */
+
+static int synaptics_rmi4_disable(struct synaptics_rmi4_data *pdata)
+{
+	int retval;
+	unsigned char intr_status;
+
+	pdata->touch_stopped = true;
+	disable_irq(pdata->board->irq_number);
+
+	retval = synaptics_rmi4_i2c_block_read(pdata,
+					pdata->fn01_data_base_addr + 1,
+					&intr_status,
+					pdata->number_of_interrupt_register);
+	if (retval < 0)
+		return retval;
+
+	retval = synaptics_rmi4_i2c_byte_write(pdata,
+					pdata->fn01_ctrl_base_addr + 1,
+					(intr_status & ~TOUCHPAD_CTRL_INTR));
+	if (retval < 0)
+		return retval;
+	if (pdata->board->regulator_en)
+		regulator_disable(pdata->regulator);
+
+	return 0;
+}
+
+/**
+ * synaptics_rmi4_show_attr_enable() - show the touchpad enable value
+ * @dev: pointer to device data structure
+ * @attr: pointer to attribute structure
+ * @buf: pointer to character buffer
+ *
+ * This function is to show the touchpad enable value and returns ssize_t.
+ */
+static ssize_t synaptics_rmi4_show_attr_enable(struct device *dev,
+			struct device_attribute *attr, char *buf)
+{
+	struct synaptics_rmi4_data *pdata = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", pdata->enable);
+}
+
+/**
+ * synaptics_rmi4_store_attr_enable() - store the touchpad enable value
+ * @dev: pointer to device data structure
+ * @attr: pointer to attribute structure
+ * @buf: pointer to character buffer
+ * @count: number fo arguments
+ *
+ * This function is to store the touchpad enable value and returns ssize_t.
+ */
+static ssize_t synaptics_rmi4_store_attr_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct synaptics_rmi4_data *pdata = dev_get_drvdata(dev);
+	unsigned long val;
+	int retval = 0;
+
+	if (strict_strtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if ((val != 0) && (val != 1))
+		return -EINVAL;
+
+	if (pdata->enable != val) {
+		pdata->enable = val ? true : false;
+		if (pdata->enable)
+			retval = synaptics_rmi4_enable(pdata);
+		else
+			retval = synaptics_rmi4_disable(pdata);
+
+	}
+	return ((retval < 0) ? retval : count);
+}
+
+static DEVICE_ATTR(enable, S_IWUSR | S_IRUGO,
+	synaptics_rmi4_show_attr_enable, synaptics_rmi4_store_attr_enable);
+
+static struct attribute *synaptics_rmi4_attrs[] = {
+	&dev_attr_enable.attr,
+	NULL,
+};
+
+static struct attribute_group synaptics_rmi4_attr_group = {
+	.attrs = synaptics_rmi4_attrs,
+};
+
+/**
  * synpatics_rmi4_touchpad_report() - reports for the rmi4 touchpad device
  * @pdata: pointer to synaptics_rmi4_data structure
  * @rfi: pointer to synaptics_rmi4_fn structure
@@ -316,8 +450,9 @@ static int synpatics_rmi4_touchpad_report(struct synaptics_rmi4_data *pdata,
 	unsigned char	data[DATA_LEN];
 	int	x[RMI4_NUMBER_OF_MAX_FINGERS];
 	int	y[RMI4_NUMBER_OF_MAX_FINGERS];
-	int	wx[RMI4_NUMBER_OF_MAX_FINGERS];
-	int	wy[RMI4_NUMBER_OF_MAX_FINGERS];
+	int	w[RMI4_NUMBER_OF_MAX_FINGERS];
+	static int	prv_x[RMI4_NUMBER_OF_MAX_FINGERS];
+	static int	prv_y[RMI4_NUMBER_OF_MAX_FINGERS];
 	struct	i2c_client *client = pdata->i2c_client;
 
 	/* get 2D sensor finger data */
@@ -376,11 +511,7 @@ static int synpatics_rmi4_touchpad_report(struct synaptics_rmi4_data *pdata,
 				y[touch_count]	=
 					(data[1] << 4) |
 					((data[2] >> 4) & MASK_4BIT);
-				wy[touch_count]	=
-						(data[3] >> 4) & MASK_4BIT;
-				wx[touch_count]	=
-						(data[3] & MASK_4BIT);
-
+				w[touch_count]	= data[3];
 				if (pdata->board->x_flip)
 					x[touch_count] =
 						pdata->sensor_max_x -
@@ -389,6 +520,25 @@ static int synpatics_rmi4_touchpad_report(struct synaptics_rmi4_data *pdata,
 					y[touch_count] =
 						pdata->sensor_max_y -
 								y[touch_count];
+				if (x[touch_count] < 0)
+					x[touch_count]	=	0;
+				else if (x[touch_count] >= pdata->sensor_max_x)
+					x[touch_count]	=
+							pdata->sensor_max_x - 1;
+
+				if (y[touch_count] < 0)
+					y[touch_count]	=	0;
+				else if (y[touch_count] >= pdata->sensor_max_y)
+					y[touch_count]	=
+							pdata->sensor_max_y - 1;
+			}
+			if ((abs(x[finger] - prv_x[finger]) < DELTA) &&
+				(abs(y[finger] - prv_y[finger]) < DELTA)) {
+				x[finger] = prv_x[finger];
+				y[finger] = prv_y[finger];
+			} else {
+				prv_x[finger] = x[finger];
+				prv_y[finger] = y[finger];
 			}
 			/* number of active touch points */
 			touch_count++;
@@ -399,7 +549,9 @@ static int synpatics_rmi4_touchpad_report(struct synaptics_rmi4_data *pdata,
 	if (touch_count) {
 		for (finger = 0; finger < touch_count; finger++) {
 			input_report_abs(pdata->input_dev, ABS_MT_TOUCH_MAJOR,
-						max(wx[finger] , wy[finger]));
+						max(x[finger] , y[finger]));
+			input_report_abs(pdata->input_dev, ABS_MT_WIDTH_MAJOR,
+								w[finger]);
 			input_report_abs(pdata->input_dev, ABS_MT_POSITION_X,
 								x[finger]);
 			input_report_abs(pdata->input_dev, ABS_MT_POSITION_Y,
@@ -502,7 +654,7 @@ static irqreturn_t synaptics_rmi4_irq(int irq, void *data)
 		touch_count = synaptics_rmi4_sensor_report(pdata);
 		if (touch_count)
 			wait_event_timeout(pdata->wait, pdata->touch_stopped,
-							msecs_to_jiffies(1));
+					msecs_to_jiffies(TIMEOUT_PERIOD));
 		else
 			break;
 	} while (!pdata->touch_stopped);
@@ -881,9 +1033,27 @@ static int synaptics_rmi4_i2c_query_device(struct synaptics_rmi4_data *pdata)
 }
 
 /**
+ * synaptics_rmi4_resume_handler() - work queue for resume handler
+ * @work:work_struct structure pointer
+ *
+ * This work queue handler used to resume the device and returns none
+ */
+static void synaptics_rmi4_resume_handler(struct work_struct *work)
+{
+	struct synaptics_rmi4_data *prmi4_data = container_of(work,
+				struct synaptics_rmi4_data, resume_wq_handler);
+	struct i2c_client *client = prmi4_data->i2c_client;
+	int retval;
+
+	retval = synaptics_rmi4_enable(prmi4_data);
+	if (retval < 0)
+		dev_err(&client->dev, "%s: resume failed\n", __func__);
+}
+
+/**
  * synaptics_rmi4_probe() - Initialze the i2c-client touchscreen driver
- * @i2c: i2c client structure pointer
- * @id:i2c device id pointer
+ * @client: i2c client structure pointer
+ * @dev_id:i2c device id pointer
  *
  * This function will allocate and initialize the instance
  * data and request the irq and set the instance data as the clients
@@ -927,19 +1097,17 @@ static int __devinit synaptics_rmi4_probe
 		goto err_input;
 	}
 
-	rmi4_data->regulator = regulator_get(&client->dev, "vdd");
-	if (IS_ERR(rmi4_data->regulator)) {
-		dev_err(&client->dev, "%s:get regulator failed\n",
-							__func__);
-		retval = PTR_ERR(rmi4_data->regulator);
-		goto err_get_regulator;
-	}
-	retval = regulator_enable(rmi4_data->regulator);
-	if (retval < 0) {
-		dev_err(&client->dev, "%s:regulator enable failed\n",
-							__func__);
-		goto err_regulator_enable;
+	if (platformdata->regulator_en) {
+		rmi4_data->regulator = regulator_get(&client->dev, "vdd");
+		if (IS_ERR(rmi4_data->regulator)) {
+			dev_err(&client->dev, "%s:get regulator failed\n",
+								__func__);
+			retval = PTR_ERR(rmi4_data->regulator);
+			goto err_regulator;
+		}
+		regulator_enable(rmi4_data->regulator);
 	}
+
 	init_waitqueue_head(&rmi4_data->wait);
 	/*
 	 * Copy i2c_client pointer into RTID's i2c_client pointer for
@@ -987,7 +1155,16 @@ static int __devinit synaptics_rmi4_probe
 	input_set_abs_params(rmi4_data->input_dev, ABS_MT_POSITION_Y, 0,
 					rmi4_data->sensor_max_y, 0, 0);
 	input_set_abs_params(rmi4_data->input_dev, ABS_MT_TOUCH_MAJOR, 0,
-						MAX_TOUCH_MAJOR, 0, 0);
+			max(rmi4_data->sensor_max_y, rmi4_data->sensor_max_y),
+							0, 0);
+	input_set_abs_params(rmi4_data->input_dev, ABS_MT_WIDTH_MAJOR, 0,
+						MAX_WIDTH_MAJOR, 0, 0);
+
+	retval = input_register_device(rmi4_data->input_dev);
+	if (retval) {
+		dev_err(&client->dev, "%s:input register failed\n", __func__);
+		goto err_input_register;
+	}
 
 	/* Clear interrupts */
 	synaptics_rmi4_i2c_block_read(rmi4_data,
@@ -1000,24 +1177,34 @@ static int __devinit synaptics_rmi4_probe
 	if (retval) {
 		dev_err(&client->dev, "%s:Unable to get attn irq %d\n",
 				__func__, platformdata->irq_number);
-		goto err_query_dev;
+		goto err_request_irq;
 	}
 
-	retval = input_register_device(rmi4_data->input_dev);
+	INIT_WORK(&rmi4_data->resume_wq_handler, synaptics_rmi4_resume_handler);
+
+	/* sysfs implementation for dynamic enable/disable the input event */
+	retval = sysfs_create_group(&client->dev.kobj,
+					&synaptics_rmi4_attr_group);
 	if (retval) {
-		dev_err(&client->dev, "%s:input register failed\n", __func__);
-		goto err_free_irq;
+		dev_err(&client->dev, "failed to create sysfs entries\n");
+		goto err_sysfs;
 	}
-
+	rmi4_data->enable		= true;
 	return retval;
 
-err_free_irq:
+err_sysfs:
+	cancel_work_sync(&rmi4_data->resume_wq_handler);
+err_request_irq:
 	free_irq(platformdata->irq_number, rmi4_data);
+	input_unregister_device(rmi4_data->input_dev);
+err_input_register:
+	i2c_set_clientdata(client, NULL);
 err_query_dev:
-	regulator_disable(rmi4_data->regulator);
-err_regulator_enable:
-	regulator_put(rmi4_data->regulator);
-err_get_regulator:
+	if (platformdata->regulator_en) {
+		regulator_disable(rmi4_data->regulator);
+		regulator_put(rmi4_data->regulator);
+	}
+err_regulator:
 	input_free_device(rmi4_data->input_dev);
 	rmi4_data->input_dev = NULL;
 err_input:
@@ -1037,12 +1224,16 @@ static int __devexit synaptics_rmi4_remove(struct i2c_client *client)
 	struct synaptics_rmi4_data *rmi4_data = i2c_get_clientdata(client);
 	const struct synaptics_rmi4_platform_data *pdata = rmi4_data->board;
 
+	sysfs_remove_group(&client->dev.kobj, &synaptics_rmi4_attr_group);
 	rmi4_data->touch_stopped = true;
 	wake_up(&rmi4_data->wait);
+	cancel_work_sync(&rmi4_data->resume_wq_handler);
 	free_irq(pdata->irq_number, rmi4_data);
 	input_unregister_device(rmi4_data->input_dev);
-	regulator_disable(rmi4_data->regulator);
-	regulator_put(rmi4_data->regulator);
+	if (pdata->regulator_en) {
+		regulator_disable(rmi4_data->regulator);
+		regulator_put(rmi4_data->regulator);
+	}
 	kfree(rmi4_data);
 
 	return 0;
@@ -1059,31 +1250,11 @@ static int __devexit synaptics_rmi4_remove(struct i2c_client *client)
 static int synaptics_rmi4_suspend(struct device *dev)
 {
 	/* Touch sleep mode */
-	int retval;
-	unsigned char intr_status;
 	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
-	const struct synaptics_rmi4_platform_data *pdata = rmi4_data->board;
 
-	rmi4_data->touch_stopped = true;
-	disable_irq(pdata->irq_number);
-
-	retval = synaptics_rmi4_i2c_block_read(rmi4_data,
-				rmi4_data->fn01_data_base_addr + 1,
-				&intr_status,
-				rmi4_data->number_of_interrupt_register);
-	if (retval < 0)
-		return retval;
-
-	retval = synaptics_rmi4_i2c_byte_write(rmi4_data,
-					rmi4_data->fn01_ctrl_base_addr + 1,
-					(intr_status & ~TOUCHPAD_CTRL_INTR));
-	if (retval < 0)
-		return retval;
-
-	regulator_disable(rmi4_data->regulator);
-
-	return 0;
+	return synaptics_rmi4_disable(rmi4_data);
 }
+
 /**
  * synaptics_rmi4_resume() - resume the touch screen controller
  * @dev: pointer to device structure
@@ -1093,28 +1264,9 @@ static int synaptics_rmi4_suspend(struct device *dev)
  */
 static int synaptics_rmi4_resume(struct device *dev)
 {
-	int retval;
-	unsigned char intr_status;
 	struct synaptics_rmi4_data *rmi4_data = dev_get_drvdata(dev);
-	const struct synaptics_rmi4_platform_data *pdata = rmi4_data->board;
-
-	regulator_enable(rmi4_data->regulator);
 
-	enable_irq(pdata->irq_number);
-	rmi4_data->touch_stopped = false;
-
-	retval = synaptics_rmi4_i2c_block_read(rmi4_data,
-				rmi4_data->fn01_data_base_addr + 1,
-				&intr_status,
-				rmi4_data->number_of_interrupt_register);
-	if (retval < 0)
-		return retval;
-
-	retval = synaptics_rmi4_i2c_byte_write(rmi4_data,
-					rmi4_data->fn01_ctrl_base_addr + 1,
-					(intr_status | TOUCHPAD_CTRL_INTR));
-	if (retval < 0)
-		return retval;
+	schedule_work(&rmi4_data->resume_wq_handler);
 
 	return 0;
 }
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h
index 384436ef806..973abc97374 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.h
@@ -42,6 +42,7 @@ struct synaptics_rmi4_platform_data {
 	int irq_type;
 	bool x_flip;
 	bool y_flip;
+	bool regulator_en;
 };
 
 #endif
diff --git a/drivers/tee/Kconfig b/drivers/tee/Kconfig
new file mode 100644
index 00000000000..a452e888d77
--- /dev/null
+++ b/drivers/tee/Kconfig
@@ -0,0 +1,13 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Martin Hovang (martin.xm.hovang@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+# Trursted Execution Environment Configuration
+config TEE_SUPPORT
+	bool "Trusted Execution Environment Support"
+	default y
+	---help---
+	  This implements the Trusted Execution Environment (TEE) Client
+	  API Specification from GlobalPlatform Device Technology.
diff --git a/drivers/tee/Makefile b/drivers/tee/Makefile
new file mode 100644
index 00000000000..b937eb19d72
--- /dev/null
+++ b/drivers/tee/Makefile
@@ -0,0 +1,8 @@
+#
+# Copyright (C) ST-Ericsson SA 2010
+# Author: Martin Hovang (martin.xm.hovang@stericsson.com)
+# License terms: GNU General Public License (GPL) version 2
+#
+
+obj-$(CONFIG_TEE_SUPPORT) += tee_service.o
+obj-$(CONFIG_TEE_SUPPORT) += tee_driver.o
diff --git a/drivers/tee/tee_driver.c b/drivers/tee/tee_driver.c
new file mode 100644
index 00000000000..442dec5fe06
--- /dev/null
+++ b/drivers/tee/tee_driver.c
@@ -0,0 +1,692 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Martin Hovang <martin.xm.hovang@stericsson.com>
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/mutex.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/tee.h>
+#include <linux/slab.h>
+#include <linux/hwmem.h>
+
+#define TEED_NAME "tee"
+#define TEED_PFX "TEE: "
+
+#define TEED_STATE_OPEN_DEV 0
+#define TEED_STATE_OPEN_SESSION 1
+
+static struct mutex sync;
+
+static int tee_open(struct inode *inode, struct file *file);
+static int tee_release(struct inode *inode, struct file *file);
+static int tee_read(struct file *filp, char __user *buffer,
+		    size_t length, loff_t *offset);
+static int tee_write(struct file *filp, const char __user *buffer,
+		     size_t length, loff_t *offset);
+
+static inline void set_emsg(struct tee_session *ts, u32 msg, int line)
+{
+	pr_err(TEED_PFX "msg: 0x%08x at line: %d\n", msg, line);
+	ts->err = msg;
+	ts->origin = TEED_ORIGIN_DRIVER;
+}
+
+static void reset_session(struct tee_session *ts)
+{
+	int i;
+
+	ts->state = TEED_STATE_OPEN_DEV;
+	ts->err = TEED_SUCCESS;
+	ts->origin = TEED_ORIGIN_DRIVER;
+	ts->id = 0;
+	for (i = 0; i < TEEC_CONFIG_PAYLOAD_REF_COUNT; i++)
+		ts->vaddr[i] = NULL;
+	ts->ta = NULL;
+	ts->uuid = NULL;
+	ts->cmd = 0;
+	ts->driver_cmd = TEED_OPEN_SESSION;
+	ts->ta_size = 0;
+	ts->op = NULL;
+}
+
+static int copy_ta(struct tee_session *ts,
+		   struct tee_session *ku_buffer)
+{
+	ts->ta = kmalloc(ku_buffer->ta_size, GFP_KERNEL);
+	if (ts->ta == NULL) {
+		pr_err(TEED_PFX "[%s] error, out of memory (ta)\n",
+		       __func__);
+		set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY, __LINE__);
+		return -ENOMEM;
+	}
+
+	ts->ta_size = ku_buffer->ta_size;
+
+	memcpy(ts->ta, ku_buffer->ta, ku_buffer->ta_size);
+	return 0;
+}
+
+static int copy_uuid(struct tee_session *ts,
+		     struct tee_session *ku_buffer)
+{
+	ts->uuid = kmalloc(sizeof(struct tee_uuid), GFP_KERNEL);
+
+	if (ts->uuid == NULL) {
+		pr_err(TEED_PFX "[%s] error, out of memory (uuid)\n",
+		       __func__);
+		set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY, __LINE__);
+		return -ENOMEM;
+	}
+
+	memcpy(ts->uuid, ku_buffer->uuid, sizeof(struct tee_uuid));
+
+	return 0;
+}
+
+static inline void free_operation(struct tee_session *ts,
+				  struct hwmem_alloc **alloc,
+				  int memrefs_allocated)
+{
+	int i;
+
+	for (i = 0; i < memrefs_allocated; ++i) {
+		if (ts->op->shm[i].buffer) {
+			hwmem_kunmap(alloc[i]);
+			hwmem_unpin(alloc[i]);
+			hwmem_release(alloc[i]);
+			ts->op->shm[i].buffer = NULL;
+		}
+
+		if (ts->vaddr[i])
+			ts->vaddr[i] = NULL;
+	}
+
+	kfree(ts->op);
+	ts->op = NULL;
+}
+
+static inline void memrefs_phys_to_virt(struct tee_session *ts)
+{
+	int i;
+
+	for (i = 0; i < TEEC_CONFIG_PAYLOAD_REF_COUNT; ++i) {
+		if (ts->op->flags & (1 << i)) {
+			ts->op->shm[i].buffer =
+				phys_to_virt((unsigned long)
+					     ts->op->shm[i].buffer);
+		}
+	}
+}
+
+static inline void memrefs_virt_to_phys(struct tee_session *ts)
+{
+	int i;
+
+	for (i = 0; i < TEEC_CONFIG_PAYLOAD_REF_COUNT; ++i) {
+		if (ts->op->flags & (1 << i)) {
+			ts->op->shm[i].buffer =
+				(void *)virt_to_phys(ts->op->shm[i].buffer);
+		}
+	}
+}
+
+static int copy_memref_to_user(struct tee_session *ts,
+			       struct tee_operation __user *ubuf_op,
+			       int memref)
+{
+	unsigned long bytes_left;
+
+	bytes_left = copy_to_user(ubuf_op->shm[memref].buffer,
+				  ts->vaddr[memref],
+				  ts->op->shm[memref].size);
+
+	if (bytes_left != 0) {
+		pr_err(TEED_PFX "[%s] failed to copy result to user space (%lu "
+		       "bytes left of buffer).\n", __func__, bytes_left);
+		return bytes_left;
+	}
+
+	bytes_left = put_user(ts->op->shm[memref].size,
+			      &ubuf_op->shm[memref].size);
+
+	if (bytes_left != 0) {
+		pr_err(TEED_PFX "[%s] failed to copy result to user space (%lu "
+		       "bytes left of size).\n", __func__, bytes_left);
+		return -EINVAL;
+	}
+
+	bytes_left = put_user(ts->op->shm[memref].flags,
+			      &ubuf_op->shm[memref].flags);
+	if (bytes_left != 0) {
+		pr_err(TEED_PFX "[%s] failed to copy result to user space (%lu "
+		       "bytes left of flags).\n", __func__, bytes_left);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int copy_memref_to_kernel(struct tee_session *ts,
+				 struct tee_session *ku_buffer,
+				 struct hwmem_alloc **alloc,
+				 int memref)
+{
+	int ret = -EINVAL;
+	size_t mem_chunks_length = 1;
+	struct hwmem_mem_chunk mem_chunks;
+
+	if (ku_buffer->op->shm[memref].size == 0) {
+		pr_err(TEED_PFX "[%s] error, size of memref is zero "
+		       "(memref: %d)\n", __func__, memref);
+		return ret;
+	}
+
+	alloc[memref] = hwmem_alloc(ku_buffer->op->shm[memref].size,
+				    (HWMEM_ALLOC_HINT_WRITE_COMBINE |
+				     HWMEM_ALLOC_HINT_CACHED |
+				     HWMEM_ALLOC_HINT_CACHE_WB |
+				     HWMEM_ALLOC_HINT_CACHE_AOW |
+				     HWMEM_ALLOC_HINT_INNER_AND_OUTER_CACHE),
+				    (HWMEM_ACCESS_READ | HWMEM_ACCESS_WRITE |
+				     HWMEM_ACCESS_IMPORT),
+				    HWMEM_MEM_CONTIGUOUS_SYS);
+
+	if (IS_ERR(alloc[memref])) {
+		pr_err(TEED_PFX "[%s] couldn't alloc hwmem_alloc (memref: %d)"
+		       "\n", __func__, memref);
+		return PTR_ERR(alloc[memref]);
+	}
+
+	ret = hwmem_pin(alloc[memref], &mem_chunks, &mem_chunks_length);
+	if (ret) {
+		pr_err(TEED_PFX "[%s] couldn't pin buffer (memref: %d)\n",
+		       __func__, memref);
+		return ret;
+	}
+
+	/*
+	 * Since phys_to_virt is not working for hwmem memory we are storing the
+	 * virtual addresses in separate array in tee_session and we keep the
+	 * address of the physical pointers in the memref buffer.
+	 */
+	ts->op->shm[memref].buffer = (void *)mem_chunks.paddr;
+	ts->vaddr[memref] = hwmem_kmap(alloc[memref]);
+
+	/* Buffer unmapped/freed in invoke_command if this function fails. */
+	if (!ts->op->shm[memref].buffer || !ts->vaddr[memref]) {
+		pr_err(TEED_PFX "[%s] out of memory (memref: %d)\n",
+		       __func__, memref);
+		return -ENOMEM;
+	}
+
+	if (ku_buffer->op->shm[memref].flags & TEEC_MEM_INPUT)
+		memcpy(ts->vaddr[memref],
+		       ku_buffer->op->shm[memref].buffer,
+		       ku_buffer->op->shm[memref].size);
+
+	ts->op->shm[memref].size = ku_buffer->op->shm[memref].size;
+	ts->op->shm[memref].flags = ku_buffer->op->shm[memref].flags;
+
+	return 0;
+}
+
+static int open_tee_device(struct tee_session *ts,
+			   struct tee_session *ku_buffer)
+{
+	int ret;
+
+	if (ku_buffer->driver_cmd != TEED_OPEN_SESSION) {
+		set_emsg(ts, TEED_ERROR_BAD_STATE, __LINE__);
+		return -EINVAL;
+	}
+
+	if (ku_buffer->ta) {
+		ret = copy_ta(ts, ku_buffer);
+	} else if (ku_buffer->uuid) {
+		ret = copy_uuid(ts, ku_buffer);
+	} else {
+		set_emsg(ts, TEED_ERROR_COMMUNICATION, __LINE__);
+		return -EINVAL;
+	}
+
+	ts->id = 0;
+	ts->state = TEED_STATE_OPEN_SESSION;
+	return ret;
+}
+
+static int invoke_command(struct tee_session *ts,
+			  struct tee_session *ku_buffer,
+			  struct tee_session __user *u_buffer)
+{
+	int i;
+	int ret = 0;
+	/* To keep track of which memrefs to free when failure occurs. */
+	int memrefs_allocated = 0;
+	struct hwmem_alloc *alloc[TEEC_CONFIG_PAYLOAD_REF_COUNT];
+
+	ts->op = kmalloc(sizeof(struct tee_operation), GFP_KERNEL);
+
+	if (!ts->op) {
+		if (ts->op == NULL) {
+			pr_err(TEED_PFX "[%s] error, out of memory "
+			       "(op)\n", __func__);
+			set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY, __LINE__);
+			return -ENOMEM;
+		}
+	}
+
+	ts->op->flags = ku_buffer->op->flags;
+	ts->cmd = ku_buffer->cmd;
+
+	for (i = 0; i < TEEC_CONFIG_PAYLOAD_REF_COUNT; ++i) {
+		ts->op->shm[i].buffer = NULL;
+		memrefs_allocated++;
+
+		/* We only want to copy memrefs in use to kernel space. */
+		if (ku_buffer->op->flags & (1 << i)) {
+			ret = copy_memref_to_kernel(ts, ku_buffer, alloc, i);
+			if (ret) {
+				pr_err(TEED_PFX "[%s] failed copy memref[%d] "
+				       "to kernel", __func__, i);
+				goto err;
+			}
+		} else {
+			ts->op->shm[i].size = 0;
+			ts->op->shm[i].flags = 0;
+		}
+	}
+
+	if (call_sec_world(ts, TEED_INVOKE)) {
+		set_emsg(ts, TEED_ERROR_COMMUNICATION, __LINE__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	for (i = 0; i < TEEC_CONFIG_PAYLOAD_REF_COUNT; ++i) {
+		if ((ku_buffer->op->flags & (1 << i)) &&
+		    (ku_buffer->op->shm[i].flags & TEEC_MEM_OUTPUT)) {
+			ret = copy_memref_to_user(ts, u_buffer->op, i);
+			if (ret) {
+				pr_err(TEED_PFX "[%s] failed copy memref[%d] "
+				       "to user", __func__, i);
+				goto err;
+			}
+		}
+	}
+err:
+	free_operation(ts, alloc, memrefs_allocated);
+
+	return ret;
+}
+
+static int tee_open(struct inode *inode, struct file *filp)
+{
+	struct tee_session *ts;
+	filp->private_data = kmalloc(sizeof(struct tee_session),
+				     GFP_KERNEL);
+
+	if (filp->private_data == NULL) {
+		pr_err(TEED_PFX "[%s] allocation failed", __func__);
+		return -ENOMEM;
+	}
+
+	ts = (struct tee_session *)(filp->private_data);
+	reset_session(ts);
+
+	return 0;
+}
+
+static int tee_release(struct inode *inode, struct file *filp)
+{
+	kfree(filp->private_data);
+	filp->private_data = NULL;
+
+	return 0;
+}
+
+/*
+ * Called when a process, which already opened the dev file, attempts
+ * to read from it. This function gets the current status of the session.
+ */
+static int tee_read(struct file *filp, char __user *buffer,
+		    size_t length, loff_t *offset)
+{
+	struct tee_read buf;
+	struct tee_session *ts;
+
+	if (length != sizeof(struct tee_read)) {
+		pr_err(TEED_PFX "[%s] error, incorrect input length\n",
+		       __func__);
+		return -EINVAL;
+	}
+
+	ts = (struct tee_session *)(filp->private_data);
+
+	if (ts == NULL) {
+		pr_err(TEED_PFX "[%s] error, private_data not "
+		       "initialized\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&sync);
+
+	buf.err = ts->err;
+	buf.origin = ts->origin;
+
+	mutex_unlock(&sync);
+
+	if (copy_to_user(buffer, &buf, length)) {
+		pr_err(TEED_PFX "[%s] error, copy_to_user failed!\n",
+		       __func__);
+		return -EINVAL;
+	}
+
+	return length;
+}
+
+/*
+ * Called when a process writes to a dev file.
+ */
+static int tee_write(struct file *filp, const char __user *buffer,
+		     size_t length, loff_t *offset)
+{
+	struct tee_session ku_buffer;
+	struct tee_session *ts;
+	int ret = 0;
+
+	if (length != sizeof(struct tee_session)) {
+		pr_err(TEED_PFX "[%s] error, incorrect input length\n",
+		       __func__);
+		return -EINVAL;
+	}
+
+	if (copy_from_user(&ku_buffer, buffer, length)) {
+		pr_err(TEED_PFX "[%s] error, tee_session "
+		       "copy_from_user failed\n", __func__);
+		return -EINVAL;
+	}
+
+	ts = (struct tee_session *)(filp->private_data);
+
+	if (ts == NULL) {
+		pr_err(TEED_PFX "[%s] error, private_data not "
+		       "initialized\n", __func__);
+		return -EINVAL;
+	}
+
+	mutex_lock(&sync);
+
+	switch (ts->state) {
+	case TEED_STATE_OPEN_DEV:
+		ret = open_tee_device(ts, &ku_buffer);
+		break;
+
+	case TEED_STATE_OPEN_SESSION:
+		switch (ku_buffer.driver_cmd) {
+		case TEED_INVOKE:
+			ret = invoke_command(ts, &ku_buffer,
+					     (struct tee_session *)buffer);
+			break;
+
+		case TEED_CLOSE_SESSION:
+			/* no caching implemented yet... */
+			if (call_sec_world(ts, TEED_CLOSE_SESSION)) {
+				set_emsg(ts, TEED_ERROR_COMMUNICATION,
+					 __LINE__);
+				ret = -EINVAL;
+			}
+
+			kfree(ts->ta);
+			ts->ta = NULL;
+
+			reset_session(ts);
+			break;
+
+		default:
+			set_emsg(ts, TEED_ERROR_BAD_PARAMETERS, __LINE__);
+			ret = -EINVAL;
+		}
+		break;
+	default:
+		pr_err(TEED_PFX "[%s] unknown state\n", __func__);
+		set_emsg(ts, TEED_ERROR_BAD_STATE, __LINE__);
+		ret = -EINVAL;
+	}
+
+	/*
+	 * We expect that ret has value zero when reaching the end here.
+	 * If it has any other value some error must have occured.
+	 */
+	if (!ret) {
+		ret = length;
+	} else {
+		pr_err(TEED_PFX "[%s], forcing error to -EINVAL\n", __func__);
+		ret = -EINVAL;
+	}
+
+	mutex_unlock(&sync);
+
+	return ret;
+}
+
+int teec_initialize_context(const char *name, struct tee_context *context)
+{
+	return TEED_SUCCESS;
+}
+EXPORT_SYMBOL(teec_initialize_context);
+
+int teec_finalize_context(struct tee_context *context)
+{
+	return TEED_SUCCESS;
+}
+EXPORT_SYMBOL(teec_finalize_context);
+
+int teec_open_session(struct tee_context *context,
+		      struct tee_session *session,
+		      const struct tee_uuid *destination,
+		      unsigned int connection_method,
+		      void *connection_data, struct tee_operation *operation,
+		      unsigned int *error_origin)
+{
+	int res = TEED_SUCCESS;
+
+	if (session == NULL || destination == NULL) {
+		pr_err(TEED_PFX "[%s] session or destination == NULL\n",
+		       __func__);
+		if (error_origin != NULL)
+			*error_origin = TEED_ORIGIN_DRIVER;
+		res = TEED_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	reset_session(session);
+
+	/*
+	 * Open a session towards an application already loaded inside
+	 * the TEE.
+	 */
+	session->uuid = kmalloc(sizeof(struct tee_uuid), GFP_KERNEL);
+
+	if (session->uuid == NULL) {
+		pr_err(TEED_PFX "[%s] error, out of memory (uuid)\n",
+		       __func__);
+		if (error_origin != NULL)
+			*error_origin = TEED_ORIGIN_DRIVER;
+		res = TEED_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	memcpy(session->uuid, destination, sizeof(struct tee_uuid));
+
+	session->ta = NULL;
+	session->id = 0;
+
+exit:
+	return res;
+}
+EXPORT_SYMBOL(teec_open_session);
+
+int teec_close_session(struct tee_session *session)
+{
+	int res = TEED_SUCCESS;
+
+	mutex_lock(&sync);
+
+	if (session == NULL) {
+		pr_err(TEED_PFX "[%s] error, session == NULL\n", __func__);
+		res = TEED_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	if (call_sec_world(session, TEED_CLOSE_SESSION)) {
+		pr_err(TEED_PFX "[%s] error, call_sec_world failed\n",
+		       __func__);
+		res = TEED_ERROR_GENERIC;
+		goto exit;
+	}
+
+exit:
+	if (session != NULL) {
+		kfree(session->uuid);
+		session->uuid = NULL;
+	}
+
+	mutex_unlock(&sync);
+	return res;
+}
+EXPORT_SYMBOL(teec_close_session);
+
+int teec_invoke_command(
+	struct tee_session *session, unsigned int command_id,
+	struct tee_operation *operation,
+	unsigned int *error_origin)
+{
+	int res = TEED_SUCCESS;
+	int i;
+
+	mutex_lock(&sync);
+
+	if (session == NULL || operation == NULL || error_origin == NULL) {
+		pr_err(TEED_PFX "[%s] error, input parameters == NULL\n",
+		       __func__);
+		if (error_origin != NULL)
+			*error_origin = TEED_ORIGIN_DRIVER;
+		res = TEED_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	for (i = 0; i < 4; ++i) {
+		/* We only want to translate memrefs in use. */
+		if (operation->flags & (1 << i)) {
+			operation->shm[i].buffer =
+				(void *)virt_to_phys(
+					operation->shm[i].buffer);
+		}
+	}
+	session->op = operation;
+	session->cmd = command_id;
+
+	/*
+	 * Call secure world
+	 */
+	if (call_sec_world(session, TEED_INVOKE)) {
+		pr_err(TEED_PFX "[%s] error, call_sec_world failed\n",
+		       __func__);
+		if (error_origin != NULL)
+			*error_origin = TEED_ORIGIN_DRIVER;
+		res = TEED_ERROR_GENERIC;
+	}
+	if (session->err != TEED_SUCCESS) {
+		pr_err(TEED_PFX "[%s] error, call_sec_world failed\n",
+		       __func__);
+		if (error_origin != NULL)
+			*error_origin = session->origin;
+		res = session->err;
+	}
+
+	memrefs_phys_to_virt(session);
+	session->op = NULL;
+
+exit:
+	mutex_unlock(&sync);
+	return res;
+}
+EXPORT_SYMBOL(teec_invoke_command);
+
+int teec_allocate_shared_memory(struct tee_context *context,
+				struct tee_sharedmemory *shared_memory)
+{
+	int res = TEED_SUCCESS;
+
+	if (shared_memory == NULL) {
+		res = TEED_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	shared_memory->buffer = kmalloc(shared_memory->size,
+					GFP_KERNEL);
+
+	if (shared_memory->buffer == NULL) {
+		res = TEED_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+exit:
+	return res;
+}
+EXPORT_SYMBOL(teec_allocate_shared_memory);
+
+void teec_release_shared_memory(struct tee_sharedmemory *shared_memory)
+{
+	kfree(shared_memory->buffer);
+}
+EXPORT_SYMBOL(teec_release_shared_memory);
+
+static const struct file_operations tee_fops = {
+	.owner = THIS_MODULE,
+	.read = tee_read,
+	.write = tee_write,
+	.open = tee_open,
+	.release = tee_release,
+};
+
+static struct miscdevice tee_dev = {
+	MISC_DYNAMIC_MINOR,
+	TEED_NAME,
+	&tee_fops
+};
+
+static int __init tee_init(void)
+{
+	int err = 0;
+
+	err = misc_register(&tee_dev);
+
+	if (err) {
+		pr_err(TEED_PFX "[%s] error %d adding character device "
+		       "TEE\n", __func__, err);
+	}
+
+	mutex_init(&sync);
+
+	return err;
+}
+
+static void __exit tee_exit(void)
+{
+	misc_deregister(&tee_dev);
+}
+
+subsys_initcall(tee_init);
+module_exit(tee_exit);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("Trusted Execution Enviroment driver");
diff --git a/drivers/tee/tee_service.c b/drivers/tee/tee_service.c
new file mode 100644
index 00000000000..b01e9d0ac39
--- /dev/null
+++ b/drivers/tee/tee_service.c
@@ -0,0 +1,17 @@
+/*
+ * TEE service to handle the calls to trusted applications.
+ *
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Joakim Bech <joakim.xx.bech@stericsson.com>
+ * License terms: GNU General Public License (GPL) version 2
+ */
+#include <linux/kernel.h>
+#include <linux/tee.h>
+#include <linux/device.h>
+
+int __weak call_sec_world(struct tee_session *ts, int sec_cmd)
+{
+	pr_info("[%s] Generic call_sec_world called!\n", __func__);
+
+	return 0;
+}
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 070b442c1f8..b2831ed01bb 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -63,6 +63,14 @@ config SERIAL_AMBA_PL011_CONSOLE
 	  your boot loader (lilo or loadlin) about how to pass options to the
 	  kernel at boot time.)
 
+config SERIAL_AMBA_PL011_CLOCK_CONTROL
+	bool "Support for clock control on AMBA serial port"
+	depends on SERIAL_AMBA_PL011
+	select CONSOLE_POLL
+	---help---
+	  Say Y here if you wish to use amba set_termios function to control
+	  the pl011 clock. Any positive baudrate passed enables clock,
+
 config SERIAL_SB1250_DUART
 	tristate "BCM1xxx on-chip DUART serial support"
 	depends on SIBYTE_SB1xxx_SOC=y
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index 3d569cd68f5..d356b940382 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -47,11 +47,13 @@
 #include <linux/amba/serial.h>
 #include <linux/clk.h>
 #include <linux/slab.h>
+#include <linux/regulator/consumer.h>
 #include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
 #include <linux/scatterlist.h>
 #include <linux/delay.h>
 #include <linux/types.h>
+#include <linux/pm_runtime.h>
 
 #include <asm/io.h>
 #include <asm/sizes.h>
@@ -67,6 +69,37 @@
 #define UART_DR_ERROR		(UART011_DR_OE|UART011_DR_BE|UART011_DR_PE|UART011_DR_FE)
 #define UART_DUMMY_DR_RX	(1 << 16)
 
+/*
+ * The console UART is handled differently for power management (it doesn't
+ * take the regulator, in order to allow the system to go to sleep even if the
+ * console is open).  This should be removed once cable detect is in place.
+ */
+#ifdef CONFIG_SERIAL_CORE_CONSOLE
+#define uart_console(port)	((port)->cons \
+		&& (port)->cons->index == (port)->line)
+#else
+#define uart_console(port)	(0)
+#endif
+
+/* Available amba pl011 port clock states */
+enum pl011_clk_states {
+	PL011_CLK_OFF = 0,	/* clock disabled */
+	PL011_CLK_REQUEST_OFF,	/* disable after TX flushed */
+	PL011_CLK_ON,		/* clock enabled */
+	PL011_PORT_OFF,		/* port disabled */
+};
+
+/*
+ * Backup registers to be used during regulator startup/shutdown
+ */
+static const u32 backup_regs[] = {
+	UART011_IBRD,
+	UART011_FBRD,
+	ST_UART011_LCRH_RX,
+	ST_UART011_LCRH_TX,
+	UART011_CR,
+	UART011_IMSC,
+};
 
 #define UART_WA_SAVE_NR 14
 
@@ -89,7 +122,9 @@ static const u32 uart_wa_reg[UART_WA_SAVE_NR] = {
 };
 
 static u32 uart_wa_regdata[UART_WA_SAVE_NR];
-static DECLARE_TASKLET(pl011_lockup_tlet, pl011_lockup_wa, 0);
+static unsigned int uart_wa_tlet_line;
+static DECLARE_TASKLET(pl011_lockup_tlet, pl011_lockup_wa,
+		(unsigned long) &uart_wa_tlet_line);
 
 /* There is by now at least one vendor with differing details, so handle it */
 struct vendor_data {
@@ -158,10 +193,18 @@ struct uart_amba_port {
 	unsigned int		im;		/* interrupt mask */
 	unsigned int		old_status;
 	unsigned int		fifosize;	/* vendor-specific */
+	unsigned int		ifls;		/* vendor-specific */
 	unsigned int		lcrh_tx;	/* vendor-specific */
 	unsigned int		lcrh_rx;	/* vendor-specific */
 	unsigned int		old_cr;		/* state during shutdown */
 	bool			autorts;
+#ifdef CONFIG_SERIAL_AMBA_PL011_CLOCK_CONTROL
+	enum pl011_clk_states	clk_state;	/* actual clock state */
+	struct delayed_work	clk_off_work;	/* work used for clock off */
+	unsigned int		clk_off_delay;	/* clock off delay */
+#endif
+	struct regulator        *regulator;
+	u32 backup[ARRAY_SIZE(backup_regs)];
 	char			type[12];
 	bool			interrupt_may_hang; /* vendor-specific */
 #ifdef CONFIG_DMA_ENGINE
@@ -1070,13 +1113,17 @@ static inline bool pl011_dma_rx_running(struct uart_amba_port *uap)
  */
 static void pl011_lockup_wa(unsigned long data)
 {
-	struct uart_amba_port *uap = amba_ports[0];
+	struct uart_amba_port *uap = amba_ports[*(unsigned int *)data];
 	void __iomem *base = uap->port.membase;
 	struct circ_buf *xmit = &uap->port.state->xmit;
 	struct tty_struct *tty = uap->port.state->port.tty;
 	int buf_empty_retries = 200;
 	int loop;
 
+	/* Exit early if there is no tty */
+	if (!tty)
+		return;
+
 	/* Stop HCI layer from submitting data for tx */
 	tty->hw_stopped = 1;
 	while (!uart_circ_empty(xmit)) {
@@ -1117,6 +1164,260 @@ static void pl011_lockup_wa(unsigned long data)
 	tty->hw_stopped = 0;
 }
 
+static void __pl011_startup(struct uart_amba_port *uap)
+{
+   unsigned int cr;
+
+   writew(uap->ifls, uap->port.membase + UART011_IFLS);
+
+   /*
+    * Provoke TX FIFO interrupt into asserting.
+    */
+   cr = UART01x_CR_UARTEN | UART011_CR_TXE | UART011_CR_LBE;
+   writew(cr, uap->port.membase + UART011_CR);
+   writew(0, uap->port.membase + UART011_FBRD);
+   writew(1, uap->port.membase + UART011_IBRD);
+   writew(0, uap->port.membase + uap->lcrh_rx);
+   if (uap->lcrh_tx != uap->lcrh_rx) {
+       int i;
+       /*
+        * Wait 10 PCLKs before writing LCRH_TX register,
+        * to get this delay write read only register 10 times
+        */
+       for (i = 0; i < 10; ++i)
+           writew(0xff, uap->port.membase + UART011_MIS);
+       writew(0, uap->port.membase + uap->lcrh_tx);
+   }
+   writew(0, uap->port.membase + UART01x_DR);
+   while (readw(uap->port.membase + UART01x_FR) & UART01x_FR_BUSY)
+       barrier();
+}
+
+/* Backup the registers during regulator startup/shutdown */
+#ifdef CONFIG_SERIAL_AMBA_PL011_CLOCK_CONTROL
+static int pl011_backup(struct uart_amba_port *uap, bool suspend)
+{
+	int i, cnt;
+
+    if (!suspend) {
+        __pl011_startup(uap);
+        writew(0, uap->port.membase + UART011_CR);
+    }
+
+	for (i = 0; i < ARRAY_SIZE(backup_regs); i++) {
+		if (suspend)
+			uap->backup[i] = readw(uap->port.membase +
+						    backup_regs[i]);
+		else {
+			if (backup_regs[i] == ST_UART011_LCRH_TX) {
+				/*
+				 * Wait 10 PCLKs before writing LCRH_TX
+				 * register, to get this delay write read
+				 * only register 10 times
+				 */
+				for (cnt = 0; cnt < 10; ++cnt)
+					writew(0xff, uap->port.membase +
+						UART011_MIS);
+			}
+
+			writew(uap->backup[i],
+			       uap->port.membase + backup_regs[i]);
+		}
+	}
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_SERIAL_AMBA_PL011_CLOCK_CONTROL
+/* Turn clock off if TX buffer is empty, otherwise reschedule */
+static void pl011_clock_off(struct work_struct *work)
+{
+	struct uart_amba_port *uap = container_of(work, struct uart_amba_port,
+							clk_off_work.work);
+	struct uart_port *port = &uap->port;
+	struct circ_buf *xmit = &port->state->xmit;
+	unsigned long flags;
+	bool disable_regulator = false;
+	bool runtime_put = false;
+	unsigned int busy, interrupt_status;
+
+	spin_lock_irqsave(&port->lock, flags);
+	
+	interrupt_status = readw(uap->port.membase + UART011_MIS);
+	busy = readw(uap->port.membase + UART01x_FR) & UART01x_FR_BUSY;
+
+	if (uap->clk_state == PL011_CLK_REQUEST_OFF) {
+			if (uart_circ_empty(xmit) && !interrupt_status && !busy) {
+				if (!uart_console(&uap->port) && uap->regulator) {
+					pl011_backup(uap, true);
+					disable_regulator = true;
+				}
+			runtime_put = true;
+			uap->clk_state = PL011_CLK_OFF;
+			clk_disable(uap->clk);
+		} else
+			schedule_delayed_work(&uap->clk_off_work,
+						uap->clk_off_delay);
+	}
+
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	if (disable_regulator)
+		regulator_disable(uap->regulator);
+	if (runtime_put)
+		pm_runtime_put_sync(uap->port.dev);
+}
+
+/* Request to turn off uart clock once pending TX is flushed */
+static void pl011_clock_request_off(struct uart_port *port)
+{
+	unsigned long flags;
+	struct uart_amba_port *uap = (struct uart_amba_port *)(port);
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	if (uap->clk_state == PL011_CLK_ON) {
+		uap->clk_state = PL011_CLK_REQUEST_OFF;
+		/* Turn off later */
+		schedule_delayed_work(&uap->clk_off_work,
+			uap->clk_off_delay);
+	}
+
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+/* Request to immediately turn on uart clock */
+static void pl011_clock_on(struct uart_port *port)
+{
+	unsigned long flags;
+	struct uart_amba_port *uap = (struct uart_amba_port *)(port);
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	switch (uap->clk_state) {
+	case PL011_CLK_OFF:
+		pm_runtime_get_sync(uap->port.dev);
+		clk_enable(uap->clk);
+		if (!uart_console(&uap->port) && uap->regulator) {
+			spin_unlock_irqrestore(&port->lock, flags);
+			regulator_enable(uap->regulator);
+			spin_lock_irqsave(&port->lock, flags);
+			pl011_backup(uap, false);
+		}
+		/* fallthrough */
+	case PL011_CLK_REQUEST_OFF:
+		__cancel_delayed_work(&uap->clk_off_work);
+		uap->clk_state = PL011_CLK_ON;
+		break;
+	default:
+		break;
+	}
+
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static void pl011_clock_check(struct uart_amba_port *uap)
+{
+	/* Reshedule work during off request  */
+	if (uap->clk_state == PL011_CLK_REQUEST_OFF)
+		/* New TX - restart work */
+		if (__cancel_delayed_work(&uap->clk_off_work))
+			schedule_delayed_work(&uap->clk_off_work,
+						uap->clk_off_delay);
+}
+
+static int pl011_power_startup(struct uart_amba_port *uap)
+{
+	int retval = 0;
+
+	if (uap->clk_state == PL011_PORT_OFF) {
+		pm_runtime_get_sync(uap->port.dev);
+		if (!uart_console(&uap->port) && uap->regulator)
+			regulator_enable(uap->regulator);
+		retval = clk_enable(uap->clk);
+		if (!retval) {
+			uap->clk_state = PL011_CLK_ON;
+		} else {
+			uap->clk_state = PL011_PORT_OFF;
+			pm_runtime_put_sync(uap->port.dev);
+		}
+	}
+
+	return retval;
+}
+
+static void pl011_power_shutdown(struct uart_amba_port *uap)
+{
+	bool disable_regulator = false;
+	bool runtime_put = false;
+
+	cancel_delayed_work_sync(&uap->clk_off_work);
+
+	spin_lock_irq(&uap->port.lock);
+	if (uap->clk_state == PL011_CLK_ON || 
+		uap->clk_state ==  PL011_CLK_REQUEST_OFF) {
+		clk_disable(uap->clk);
+		runtime_put = true;
+		if (!uart_console(&uap->port) && uap->regulator)
+			disable_regulator = true;
+	}
+	uap->clk_state = PL011_PORT_OFF;
+	spin_unlock_irq(&uap->port.lock);
+	
+	if (disable_regulator)
+		regulator_disable(uap->regulator);
+	if (runtime_put)
+		pm_runtime_put_sync(uap->port.dev);
+}
+
+static void
+pl011_clock_control(struct uart_port *port, struct ktermios *termios,
+		     struct ktermios *old)
+{
+	speed_t new_baud = tty_termios_baud_rate(termios);
+		
+	if (new_baud == 0)
+		pl011_clock_request_off(port);
+	else
+		pl011_clock_on(port);
+}
+
+static void pl011_clock_control_init(struct uart_amba_port *uap)
+{
+	uap->clk_state = PL011_PORT_OFF;
+	INIT_DELAYED_WORK(&uap->clk_off_work, pl011_clock_off);
+	uap->clk_off_delay = HZ / 10;  /* 100 ms */
+}
+
+#else
+/* Blank functions for clock control */
+static inline void pl011_clock_check(struct uart_amba_port *uap)
+{
+}
+
+static inline int pl011_power_startup(struct uart_amba_port *uap)
+{
+	pm_runtime_get_sync(uap->port.dev);
+	return clk_enable(uap->clk);
+}
+
+static inline void pl011_power_shutdown(struct uart_amba_port *uap)
+{
+	clk_disable(uap->clk);
+	pm_runtime_put_sync(uap->port.dev);
+}
+
+static inline void
+pl011_clock_control(struct uart_port *port, struct ktermios *termios,
+		     struct ktermios *old)
+{
+}
+
+static inline void pl011_clock_control_init(struct uart_amba_port *uap)
+{
+}
+#endif
+
 static void pl011_stop_tx(struct uart_port *port)
 {
 	struct uart_amba_port *uap = (struct uart_amba_port *)port;
@@ -1208,6 +1509,9 @@ static void pl011_tx_chars(struct uart_amba_port *uap)
 			break;
 	} while (--count > 0);
 
+	if (count)
+		pl011_clock_check(uap);
+
 	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
 		uart_write_wakeup(&uap->port);
 
@@ -1253,7 +1557,7 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 		do {
 			writew(status & ~(UART011_TXIS|UART011_RTIS|
 					  UART011_RXIS),
-			       uap->port.membase + UART011_ICR);
+				uap->port.membase + UART011_ICR);
 
 			if (status & (UART011_RTIS|UART011_RXIS)) {
 				if (pl011_dma_rx_running(uap))
@@ -1268,8 +1572,10 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 				pl011_tx_chars(uap);
 
 			if (pass_counter-- == 0) {
-				if (uap->interrupt_may_hang)
+				if (uap->interrupt_may_hang) {
+					uart_wa_tlet_line = uap->port.line;
 					tasklet_schedule(&pl011_lockup_tlet);
+				}
 				break;
 			}
 
@@ -1389,9 +1695,9 @@ static int pl011_startup(struct uart_port *port)
 		goto out;
 
 	/*
-	 * Try to enable the clock producer.
+	 * Try to enable the clock producer and the regulator.
 	 */
-	retval = clk_enable(uap->clk);
+	retval = pl011_power_startup(uap);
 	if (retval)
 		goto clk_unprep;
 
@@ -1408,29 +1714,7 @@ static int pl011_startup(struct uart_port *port)
 	if (retval)
 		goto clk_dis;
 
-	writew(uap->vendor->ifls, uap->port.membase + UART011_IFLS);
-
-	/*
-	 * Provoke TX FIFO interrupt into asserting.
-	 */
-	cr = UART01x_CR_UARTEN | UART011_CR_TXE | UART011_CR_LBE;
-	writew(cr, uap->port.membase + UART011_CR);
-	writew(0, uap->port.membase + UART011_FBRD);
-	writew(1, uap->port.membase + UART011_IBRD);
-	writew(0, uap->port.membase + uap->lcrh_rx);
-	if (uap->lcrh_tx != uap->lcrh_rx) {
-		int i;
-		/*
-		 * Wait 10 PCLKs before writing LCRH_TX register,
-		 * to get this delay write read only register 10 times
-		 */
-		for (i = 0; i < 10; ++i)
-			writew(0xff, uap->port.membase + UART011_MIS);
-		writew(0, uap->port.membase + uap->lcrh_tx);
-	}
-	writew(0, uap->port.membase + UART01x_DR);
-	while (readw(uap->port.membase + UART01x_FR) & UART01x_FR_BUSY)
-		barrier();
+	__pl011_startup(uap);
 
 	/* restore RTS and DTR */
 	cr = uap->old_cr & (UART011_CR_RTS | UART011_CR_DTR);
@@ -1471,7 +1755,7 @@ static int pl011_startup(struct uart_port *port)
 	return 0;
 
  clk_dis:
-	clk_disable(uap->clk);
+	pl011_power_shutdown(uap);
  clk_unprep:
 	clk_unprepare(uap->clk);
  out:
@@ -1529,10 +1813,18 @@ static void pl011_shutdown(struct uart_port *port)
 	if (uap->lcrh_rx != uap->lcrh_tx)
 		pl011_shutdown_channel(uap, uap->lcrh_tx);
 
+	if (uap->port.dev->platform_data) {
+		struct amba_pl011_data *plat;
+
+		plat = uap->port.dev->platform_data;
+		if (plat->exit)
+			plat->exit();
+	}
+
 	/*
-	 * Shut down the clock producer
+	 * Shut down the clock producer and the producer
 	 */
-	clk_disable(uap->clk);
+	pl011_power_shutdown(uap);
 	clk_unprepare(uap->clk);
 
 	if (uap->port.dev->platform_data) {
@@ -1545,6 +1837,32 @@ static void pl011_shutdown(struct uart_port *port)
 
 }
 
+/* Power/Clock management. */
+static void pl011_serial_pm(struct uart_port *port, unsigned int state,
+unsigned int oldstate)
+{
+	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+
+	switch (state) {
+		case 0: /*fully on */
+			/*
+			 * Enable the peripheral clock for this serial port.
+			 * This is called on uart_open() or a resume event.
+			 */
+			pl011_power_startup(uap);
+			break;
+		case 3: /* powered down */
+			/*
+			 * Disable the peripheral clock for this serial port.
+			 * This is called on uart_close() or a suspend event.
+			 */
+			pl011_power_shutdown(uap);
+			break;
+		default:
+			printk(KERN_ERR "pl011_serial: unknown pm %d\n", state);
+	}
+}
+
 static void
 pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 		     struct ktermios *old)
@@ -1558,7 +1876,12 @@ pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 		clkdiv = 8;
 	else
 		clkdiv = 16;
-
+	/*
+	 * Must be before uart_get_baud_rate() call, because
+	 * this function changes baudrate to default in case of 0
+	 * B0 hangup !!!
+	 */
+	pl011_clock_control(port, termios, old);
 	/*
 	 * Ask the core to calculate the divisor for us.
 	 */
@@ -1746,14 +2069,13 @@ static struct uart_ops amba_pl011_pops = {
 	.request_port	= pl010_request_port,
 	.config_port	= pl010_config_port,
 	.verify_port	= pl010_verify_port,
+	.pm				= pl011_serial_pm,
 #ifdef CONFIG_CONSOLE_POLL
 	.poll_get_char = pl010_get_poll_char,
 	.poll_put_char = pl010_put_poll_char,
 #endif
 };
 
-static struct uart_amba_port *amba_ports[UART_NR];
-
 #ifdef CONFIG_SERIAL_AMBA_PL011_CONSOLE
 
 static void pl011_console_putchar(struct uart_port *port, int ch)
@@ -1897,6 +2219,13 @@ static struct console amba_console = {
 	.data		= &amba_reg,
 };
 
+static int __init pl011_console_init(void)
+{
+	register_console(&amba_console);
+	return 0;
+}
+console_initcall(pl011_console_init);
+
 #define AMBA_CONSOLE	(&amba_console)
 #else
 #define AMBA_CONSOLE	NULL
@@ -1911,7 +2240,6 @@ static struct uart_driver amba_reg = {
 	.nr			= UART_NR,
 	.cons			= AMBA_CONSOLE,
 };
-
 static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 {
 	struct uart_amba_port *uap;
@@ -1940,6 +2268,12 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 		goto free;
 	}
 
+	uap->regulator = regulator_get(&dev->dev, "v-uart");
+	if (IS_ERR(uap->regulator)) {
+		dev_warn(&dev->dev, "could not get uart regulator\n");
+		uap->regulator = NULL;
+	}
+
 	uap->clk = clk_get(&dev->dev, NULL);
 	if (IS_ERR(uap->clk)) {
 		ret = PTR_ERR(uap->clk);
@@ -1947,6 +2281,7 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 	}
 
 	uap->vendor = vendor;
+	uap->ifls = vendor->ifls;
 	uap->lcrh_rx = vendor->lcrh_rx;
 	uap->lcrh_tx = vendor->lcrh_tx;
 	uap->old_cr = 0;
@@ -1972,18 +2307,30 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 	amba_ports[i] = uap;
 
 	amba_set_drvdata(dev, uap);
+
+	pm_runtime_irq_safe(&dev->dev);
+
+	pl011_clock_control_init(uap);
+
 	ret = uart_add_one_port(&amba_reg, &uap->port);
+
+	if (!ret)
+		pm_runtime_put(&dev->dev);
+
 	if (ret) {
 		amba_set_drvdata(dev, NULL);
 		amba_ports[i] = NULL;
 		pl011_dma_remove(uap);
 		clk_put(uap->clk);
  unmap:
+		if (uap->regulator)
+			regulator_put(uap->regulator);
 		iounmap(base);
  free:
 		kfree(uap);
 	}
  out:
+
 	return ret;
 }
 
@@ -1994,6 +2341,8 @@ static int pl011_remove(struct amba_device *dev)
 
 	amba_set_drvdata(dev, NULL);
 
+	pm_runtime_get_sync(uap->port.dev);
+
 	uart_remove_one_port(&amba_reg, &uap->port);
 
 	for (i = 0; i < ARRAY_SIZE(amba_ports); i++)
@@ -2002,6 +2351,8 @@ static int pl011_remove(struct amba_device *dev)
 
 	pl011_dma_remove(uap);
 	iounmap(uap->port.membase);
+	if (uap->regulator)
+		regulator_put(uap->regulator);
 	clk_put(uap->clk);
 	kfree(uap);
 	return 0;
@@ -2014,7 +2365,12 @@ static int pl011_suspend(struct amba_device *dev, pm_message_t state)
 
 	if (!uap)
 		return -EINVAL;
+#ifdef CONFIG_SERIAL_AMBA_PL011_CLOCK_CONTROL
+    cancel_delayed_work_sync(&uap->clk_off_work);
 
+    if (uap->clk_state == PL011_CLK_OFF)
+       return 0;
+#endif
 	return uart_suspend_port(&amba_reg, &uap->port);
 }
 
@@ -2024,6 +2380,10 @@ static int pl011_resume(struct amba_device *dev)
 
 	if (!uap)
 		return -EINVAL;
+#ifdef CONFIG_SERIAL_AMBA_PL011_CLOCK_CONTROL
+    if (uap->clk_state == PL011_CLK_OFF)
+        return 0;
+#endif
 
 	return uart_resume_port(&amba_reg, &uap->port);
 }
@@ -2082,7 +2442,7 @@ static void __exit pl011_exit(void)
  * While this can be a module, if builtin it's most likely the console
  * So let's leave module_exit but move module_init to an earlier place
  */
-arch_initcall(pl011_init);
+subsys_initcall(pl011_init);
 module_exit(pl011_exit);
 
 MODULE_AUTHOR("ARM Ltd/Deep Blue Solutions Ltd");
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 1ab2fd8c3e9..2923752b858 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -1767,10 +1767,45 @@ musb_srp_store(struct device *dev, struct device_attribute *attr,
 }
 static DEVICE_ATTR(srp, 0644, NULL, musb_srp_store);
 
+static ssize_t
+ux500_set_extvbus(struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t n)
+{
+	struct musb_hdrc_platform_data *plat = dev->platform_data;
+	unsigned short	extvbus;
+
+	if (sscanf(buf, "%hu", &extvbus) != 1
+			|| ((extvbus != 1) && (extvbus != 0))) {
+		dev_err(dev, "Invalid value EXTVBUS must be 1 or 0\n");
+		return -EINVAL;
+	}
+
+	plat->extvbus = extvbus;
+
+	return n;
+}
+
+static ssize_t
+ux500_get_extvbus(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct musb_hdrc_platform_data *plat = dev->platform_data;
+	int		extvbus;
+
+	/* FIXME get_vbus_status() is normally #defined as false...
+	 * and is effectively TUSB-specific.
+	 */
+	extvbus = plat->extvbus;
+
+	return sprintf(buf, "EXTVBUS is %s\n",
+			extvbus ? "on" : "off");
+}
+static DEVICE_ATTR(extvbus, 0644, ux500_get_extvbus, ux500_set_extvbus);
+
 static struct attribute *musb_attributes[] = {
 	&dev_attr_mode.attr,
 	&dev_attr_vbus.attr,
 	&dev_attr_srp.attr,
+	&dev_attr_extvbus.attr,
 	NULL
 };
 
diff --git a/drivers/usb/musb/musb_debugfs.c b/drivers/usb/musb/musb_debugfs.c
index 40a37c91cc1..19e3c91c22e 100644
--- a/drivers/usb/musb/musb_debugfs.c
+++ b/drivers/usb/musb/musb_debugfs.c
@@ -68,6 +68,7 @@ static const struct musb_register_map musb_regmap[] = {
 	{ "RxFIFOadd",		0x66,	16 },
 	{ "VControl",		0x68,	32 },
 	{ "HWVers",		0x6C,	16 },
+	{ "EXTVBUS",		0x70,	8 },
 	{ "EPInfo",		0x78,	8 },
 	{ "RAMInfo",		0x79,	8 },
 	{ "LinkInfo",		0x7A,	8 },
diff --git a/drivers/usb/musb/ux500.c b/drivers/usb/musb/ux500.c
index 2f089ddcd46..c1a205cd5e3 100644
--- a/drivers/usb/musb/ux500.c
+++ b/drivers/usb/musb/ux500.c
@@ -311,6 +311,7 @@ static void ux500_musb_set_vbus(struct musb *musb, int is_on)
 	u8		devctl;
 	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
 	int ret = 1;
+	struct musb_hdrc_platform_data *plat = musb->controller->platform_data;
 #ifdef	CONFIG_USB_OTG_20
 	int val = 0;
 #endif
@@ -323,6 +324,17 @@ static void ux500_musb_set_vbus(struct musb *musb, int is_on)
 	val |= 0x1C;
 	musb_writeb(musb->mregs, MUSB_MISC, val);
 #endif
+
+	/* Use EXTVBUS */
+	u8 busctl = musb_read_ulpi_buscontrol(musb->mregs);
+	if (plat->extvbus) {
+		busctl |= MUSB_ULPI_USE_EXTVBUS;
+		musb_write_ulpi_buscontrol(musb->mregs, busctl);
+	} else {
+		busctl &= ~MUSB_ULPI_USE_EXTVBUS;
+		musb_write_ulpi_buscontrol(musb->mregs, busctl);
+	}
+
 	devctl = musb_readb(musb->mregs, MUSB_DEVCTL);
 
 	if (is_on) {
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index a290be51a1f..c775ac2419d 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -23,6 +23,8 @@ source "drivers/gpu/drm/Kconfig"
 
 source "drivers/gpu/stub/Kconfig"
 
+source "drivers/gpu/mali/Kconfig"
+
 config VGASTATE
        tristate
        default n
@@ -286,6 +288,8 @@ config FB_CIRRUS
 	  Say N unless you have such a graphics board or plan to get one
 	  before you next recompile the kernel.
 
+source "drivers/video/mcde/Kconfig"
+
 config FB_PM2
 	tristate "Permedia2 support"
 	depends on FB && ((AMIGA && BROKEN) || PCI)
@@ -2415,6 +2419,8 @@ source "drivers/video/omap/Kconfig"
 source "drivers/video/omap2/Kconfig"
 source "drivers/video/exynos/Kconfig"
 source "drivers/video/backlight/Kconfig"
+source "drivers/video/av8100/Kconfig"
+source "drivers/video/b2r2/Kconfig"
 
 if VT
 	source "drivers/video/console/Kconfig"
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 9356add945b..3a4ad2ebe33 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -139,6 +139,9 @@ obj-$(CONFIG_FB_SH_MOBILE_MERAM)  += sh_mobile_meram.o
 obj-$(CONFIG_FB_SH_MOBILE_LCDC)	  += sh_mobile_lcdcfb.o
 obj-$(CONFIG_FB_OMAP)             += omap/
 obj-y                             += omap2/
+obj-$(CONFIG_FB_MCDE)		  += mcde/
+obj-$(CONFIG_AV8100)		  += av8100/
+obj-y				  += b2r2/
 obj-$(CONFIG_XEN_FBDEV_FRONTEND)  += xen-fbfront.o
 obj-$(CONFIG_FB_CARMINE)          += carminefb.o
 obj-$(CONFIG_FB_MB862XX)	  += mb862xx/
diff --git a/drivers/video/av8100/Kconfig b/drivers/video/av8100/Kconfig
new file mode 100644
index 00000000000..40b9943aaa9
--- /dev/null
+++ b/drivers/video/av8100/Kconfig
@@ -0,0 +1,48 @@
+config AV8100
+       tristate "AV8100 driver support(HDMI/CVBS)"
+       default n
+       help
+           Please enable this feature if hdmi/tvout driver support is required.
+
+config HDMI_AV8100_DEBUG
+	bool "HDMI and AV8100 debug messages"
+	default n
+	depends on AV8100
+	---help---
+	  Say Y here if you want the HDMI and AV8100 driver to
+	  output debug messages.
+
+choice
+	prompt "AV8100 HW trig method"
+	default AV8100_HWTRIG_DSI_TE
+
+config AV8100_HWTRIG_INT
+	bool "AV8100 HW trig on INT"
+	depends on AV8100
+	---help---
+	  If you say Y here AV8100 will use HW triggering
+	  from AV8100 INT to MCDE sync0.
+
+config AV8100_HWTRIG_I2SDAT3
+	bool "AV8100 HW trig on I2SDAT3"
+	depends on AV8100
+	---help---
+	  If you say Y here AV8100 will use HW triggering
+	  from AV8100 I2SDAT3 to MCDE sync1.
+
+config AV8100_HWTRIG_DSI_TE
+	bool "AV8100 HW trig on DSI"
+	depends on AV8100
+	---help---
+	  If you say Y here AV8100 will use HW triggering
+	  using DSI TE polling between AV8100 and MCDE.
+
+config AV8100_HWTRIG_NONE
+	bool "AV8100 SW trig"
+	depends on AV8100
+	---help---
+	  If you say Y here AV8100 will use SW triggering
+	  between AV8100 and MCDE.
+
+endchoice
+
diff --git a/drivers/video/av8100/Makefile b/drivers/video/av8100/Makefile
new file mode 100644
index 00000000000..2d3028b18ca
--- /dev/null
+++ b/drivers/video/av8100/Makefile
@@ -0,0 +1,10 @@
+# Make file for compiling and loadable module HDMI
+
+obj-$(CONFIG_AV8100)		+= av8100.o hdmi.o
+
+ifdef CONFIG_HDMI_AV8100_DEBUG
+EXTRA_CFLAGS += -DDEBUG
+endif
+
+clean-files := av8100.o hdmi.o built-in.o modules.order
+
diff --git a/drivers/video/av8100/av8100.c b/drivers/video/av8100/av8100.c
new file mode 100644
index 00000000000..d5d159079c3
--- /dev/null
+++ b/drivers/video/av8100/av8100.c
@@ -0,0 +1,4166 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * AV8100 driver
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+#include <linux/fs.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/timer.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+#include <linux/clk.h>
+#include <linux/list.h>
+#include <linux/regulator/consumer.h>
+#include <linux/mfd/dbx500-prcmu.h>
+
+#include "av8100_regs.h"
+#include <video/av8100.h>
+#include <video/hdmi.h>
+#include <linux/firmware.h>
+
+#define AV8100_FW_FILENAME "av8100.fw"
+#define CUT_STR_0 "2.1"
+#define CUT_STR_1 "2.2"
+#define CUT_STR_3 "2.3"
+#define CUT_STR_30 "3.0"
+#define CUT_STR_UNKNOWN ""
+#define AV8100_DEVNR_DEFAULT 0
+
+/* Interrupts */
+#define AV8100_INT_EVENT 0x1
+#define AV8100_PLUGSTARTUP_EVENT 0x4
+
+#define AV8100_PLUGSTARTUP_TIME 100
+
+/* Standby search time */
+#define AV8100_ON_TIME 1	/* 9 ms step */
+#define AV8100_DENC_OFF_TIME 3	/* 275 ms step if > V1. Not used if V1 */
+#define AV8100_HDMI_OFF_TIME 2	/* 140 ms step if V2. 80 ms step if V1 */
+
+/* Command offsets */
+#define AV8100_COMMAND_OFFSET		0x10
+#define AV8100_CUTVER_OFFSET		0x11
+#define AV8100_COMMAND_MAX_LENGTH	0x81
+#define AV8100_CMD_BUF_OFFSET		(AV8100_COMMAND_OFFSET + 1)
+#define AV8100_2ND_RET_BYTE_OFFSET	(AV8100_COMMAND_OFFSET + 1)
+#define AV8100_CEC_RET_BUF_OFFSET	(AV8100_COMMAND_OFFSET + 4)
+#define AV8100_HDCP_RET_BUF_OFFSET	(AV8100_COMMAND_OFFSET + 2)
+#define AV8100_EDID_RET_BUF_OFFSET	(AV8100_COMMAND_OFFSET + 1)
+#define AV8100_FUSE_CRC_OFFSET		(AV8100_COMMAND_OFFSET + 2)
+#define AV8100_FUSE_PRGD_OFFSET		(AV8100_COMMAND_OFFSET + 3)
+#define AV8100_CRC32_OFFSET		(AV8100_COMMAND_OFFSET + 2)
+#define AV8100_CEC_ADDR_OFFSET		(AV8100_COMMAND_OFFSET + 3)
+
+/* Tearing effect line numbers */
+#define AV8100_TE_LINE_NB_14	14
+#define AV8100_TE_LINE_NB_17	17
+#define AV8100_TE_LINE_NB_18	18
+#define AV8100_TE_LINE_NB_21	21
+#define AV8100_TE_LINE_NB_22	22
+#define AV8100_TE_LINE_NB_24	24
+#define AV8100_TE_LINE_NB_25	25
+#define AV8100_TE_LINE_NB_26	26
+#define AV8100_TE_LINE_NB_29	29
+#define AV8100_TE_LINE_NB_30	30
+#define AV8100_TE_LINE_NB_32	32
+#define AV8100_TE_LINE_NB_38	38
+#define AV8100_TE_LINE_NB_40	40
+#define AV8100_UI_X4_DEFAULT	6
+
+#define HDMI_REQUEST_FOR_REVOCATION_LIST_INPUT 2
+#define HDMI_CEC_MESSAGE_WRITE_BUFFER_SIZE 16
+#define HDMI_HDCP_SEND_KEY_SIZE 7
+#define HDMI_INFOFRAME_DATA_SIZE 28
+#define HDMI_FUSE_AES_KEY_SIZE 16
+#define HDMI_FUSE_AES_KEY_RET_SIZE 2
+#define HDMI_LOADAES_END_BLK_NR 145
+#define HDMI_CRC32_SIZE 4
+#define HDMI_HDCP_MGMT_BKSV_SIZE 5
+#define HDMI_HDCP_MGMT_SHA_SIZE 20
+#define HDMI_HDCP_MGMT_MAX_DEVICES_SIZE 20
+#define HDMI_HDCP_MGMT_DEVICE_MASK 0x7F
+#define HDMI_EDIDREAD_SIZE 0x7F
+
+#define HPDS_INVALID 0xF
+#define CPDS_INVALID 0xF
+#define CECRX_INVALID 0xF
+
+#define REG_16_8_LSB(p)		((u8)(p & 0xFF))
+#define REG_16_8_MSB(p)		((u8)((p & 0xFF00)>>8))
+#define REG_32_8_MSB(p)		((u8)((p & 0xFF000000)>>24))
+#define REG_32_8_MMSB(p)	((u8)((p & 0x00FF0000)>>16))
+#define REG_32_8_MLSB(p)	((u8)((p & 0x0000FF00)>>8))
+#define REG_32_8_LSB(p)		((u8)(p & 0x000000FF))
+#define REG_10_8_MSB(p)		((u8)((p & 0x300)>>8))
+#define REG_12_8_MSB(p)		((u8)((p & 0xf00)>>8))
+
+#define AV8100_WAITTIME_1MS 1
+#define AV8100_WAITTIME_5MS 5
+#define AV8100_WAITTIME_10MS 10
+#define AV8100_WAITTIME_50MS 50
+#define AV8100_WATTIME_100US 100
+
+static DEFINE_MUTEX(av8100_hw_mutex);
+#define LOCK_AV8100_HW mutex_lock(&av8100_hw_mutex)
+#define UNLOCK_AV8100_HW mutex_unlock(&av8100_hw_mutex)
+static DEFINE_MUTEX(av8100_fwdl_mutex);
+#define LOCK_AV8100_FWDL mutex_lock(&av8100_fwdl_mutex)
+#define UNLOCK_AV8100_FWDL mutex_unlock(&av8100_fwdl_mutex)
+
+struct color_conversion_cmd {
+	unsigned short	c0;
+	unsigned short	c1;
+	unsigned short	c2;
+	unsigned short	c3;
+	unsigned short	c4;
+	unsigned short	c5;
+	unsigned short	c6;
+	unsigned short	c7;
+	unsigned short	c8;
+	unsigned short	aoffset;
+	unsigned short	boffset;
+	unsigned short	coffset;
+	unsigned char	lmax;
+	unsigned char	lmin;
+	unsigned char	cmax;
+	unsigned char	cmin;
+};
+
+struct av8100_config {
+	struct i2c_client *client;
+	struct i2c_device_id *id;
+	struct av8100_video_input_format_cmd hdmi_video_input_cmd;
+	struct av8100_audio_input_format_cmd hdmi_audio_input_cmd;
+	struct av8100_video_output_format_cmd hdmi_video_output_cmd;
+	struct av8100_video_scaling_format_cmd hdmi_video_scaling_cmd;
+	enum av8100_color_transform color_transform;
+	struct av8100_cec_message_write_format_cmd
+		hdmi_cec_message_write_cmd;
+	struct av8100_cec_message_read_back_format_cmd
+		hdmi_cec_message_read_back_cmd;
+	struct av8100_denc_format_cmd hdmi_denc_cmd;
+	struct av8100_hdmi_cmd hdmi_cmd;
+	struct av8100_hdcp_send_key_format_cmd hdmi_hdcp_send_key_cmd;
+	struct av8100_hdcp_management_format_cmd
+		hdmi_hdcp_management_format_cmd;
+	struct av8100_infoframes_format_cmd	hdmi_infoframes_cmd;
+	struct av8100_edid_section_readback_format_cmd
+		hdmi_edid_section_readback_cmd;
+	struct av8100_pattern_generator_format_cmd hdmi_pattern_generator_cmd;
+	struct av8100_fuse_aes_key_format_cmd hdmi_fuse_aes_key_cmd;
+};
+
+enum av8100_plug_state {
+	AV8100_UNPLUGGED,
+	AV8100_PLUGGED_STARTUP,
+	AV8100_PLUGGED
+};
+
+struct av8100_params {
+	int denc_off_time;/* 5 volt time */
+	int hdmi_off_time;/* 5 volt time */
+	int on_time;/* 5 volt time */
+	u8 hpdm;/*stby_int_mask*/
+	u8 cpdm;/*stby_int_mask*/
+	u8 cecm;/*gen_int_mask*/
+	u8 hdcpm;/*gen_int_mask*/
+	u8 uovbm;/*gen_int_mask*/
+	void (*hdmi_ev_cb)(enum av8100_hdmi_event);
+	enum av8100_plug_state plug_state;
+	struct clk *inputclk;
+	bool inputclk_requested;
+	bool opp_requested;
+	struct regulator *regulator_pwr;
+	bool regulator_requested;
+	bool pre_suspend_power;
+	bool ints_enabled;
+	bool irq_requested;
+};
+
+/**
+ * struct av8100_cea - CEA(consumer electronic access) standard structure
+ * @cea_id:
+ * @cea_nb:
+ * @vtotale:
+ **/
+
+struct av8100_cea {
+	char cea_id[40];
+	int cea_nb;
+	int vtotale;
+	int vactive;
+	int vsbp;
+	int vslen;
+	int vsfp;
+	char vpol[5];
+	int htotale;
+	int hactive;
+	int hbp;
+	int hslen;
+	int hfp;
+	int frequence;
+	char hpol[5];
+	int reg_line_duration;
+	int blkoel_duration;
+	int uix4;
+	int pll_mult;
+	int pll_div;
+};
+
+enum av8100_command_size {
+	AV8100_COMMAND_VIDEO_INPUT_FORMAT_SIZE  = 0x17,
+	AV8100_COMMAND_AUDIO_INPUT_FORMAT_SIZE  = 0x8,
+	AV8100_COMMAND_VIDEO_OUTPUT_FORMAT_SIZE = 0x1E,
+	AV8100_COMMAND_VIDEO_SCALING_FORMAT_SIZE = 0x11,
+	AV8100_COMMAND_COLORSPACECONVERSION_SIZE = 0x1D,
+	AV8100_COMMAND_CEC_MESSAGE_WRITE_SIZE = 0x12,
+	AV8100_COMMAND_CEC_MESSAGE_READ_BACK_SIZE = 0x1,
+	AV8100_COMMAND_DENC_SIZE = 0x6,
+	AV8100_COMMAND_HDMI_SIZE = 0x4,
+	AV8100_COMMAND_HDCP_SENDKEY_SIZE = 0xA,
+	AV8100_COMMAND_HDCP_MANAGEMENT_SIZE = 0x3,
+	AV8100_COMMAND_INFOFRAMES_SIZE = 0x21,
+	AV8100_COMMAND_EDID_SECTION_READBACK_SIZE  = 0x3,
+	AV8100_COMMAND_PATTERNGENERATOR_SIZE  = 0x4,
+	AV8100_COMMAND_FUSE_AES_KEY_SIZE = 0x12,
+	AV8100_COMMAND_FUSE_AES_CHK_SIZE = 0x2,
+};
+
+struct av8100_device {
+	struct list_head	list;
+	struct miscdevice	miscdev;
+	struct device		*dev;
+	struct av8100_config	config;
+	struct av8100_status	status;
+	struct timer_list	timer;
+	wait_queue_head_t	event;
+	int			flag;
+	struct av8100_params	params;
+	u8			chip_version;
+};
+
+static const unsigned int waittime_retry[10] =	{
+					1, 2, 4, 6, 8, 10, 10, 10, 10, 10};
+
+static int av8100_5V_w(u8 denc_off, u8 hdmi_off, u8 on);
+static void clr_plug_status(struct av8100_device *adev,
+	enum av8100_plugin_status status);
+static void set_plug_status(struct av8100_device *adev,
+	enum av8100_plugin_status status);
+static void cec_rx(struct av8100_device *adev);
+static void cec_tx(struct av8100_device *adev);
+static void cec_txerr(struct av8100_device *adev);
+static void hdcp_changed(struct av8100_device *adev);
+static const struct color_conversion_cmd *get_color_transform_cmd(
+				struct av8100_device *adev,
+				enum av8100_color_transform transform);
+static int av8100_open(struct inode *inode, struct file *filp);
+static int av8100_release(struct inode *inode, struct file *filp);
+static long av8100_ioctl(struct file *file,
+				unsigned int cmd, unsigned long arg);
+static int __devinit av8100_probe(struct i2c_client *i2c_client,
+	const struct i2c_device_id *id);
+static int __devexit av8100_remove(struct i2c_client *i2c_client);
+
+static const struct file_operations av8100_fops = {
+	.owner =    THIS_MODULE,
+	.open =     av8100_open,
+	.release =  av8100_release,
+	.unlocked_ioctl = av8100_ioctl
+};
+
+/* List of devices */
+static LIST_HEAD(av8100_device_list);
+
+static const struct av8100_cea av8100_all_cea[29] = {
+/* cea id
+ *	cea_nr	vtot	vact	vsbpp	vslen
+ *	vsfp	vpol	htot	hact	hbp	hslen	hfp	freq
+ *	hpol	rld	bd	uix4	pm	pd */
+{ "0  CUSTOM                            ",
+	0,	0,	0,	0,	0,
+	0,	"-",	800,	640,	16,	96,	10,	25200000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be defined*/
+{ "1  CEA 1 VESA 4 640x480p @ 60 Hz     ",
+	1,	525,	480,	33,	2,
+	10,	"-",	800,	640,	49,	290,	146,	25200000,
+	"-",	2438,	1270,	6,	32,	1},/*RGB888*/
+{ "2  CEA 2 - 3    720x480p @ 60 Hz 4:3 ",
+	2,	525,	480,	30,	6,
+	9,	"-",	858,	720,	34,	130,	128,	27027000,
+	"-",	1828,	0x3C0,	8,	24,	1},/*RGB565*/
+{ "3  CEA 4        1280x720p @ 60 Hz    ",
+	4,	750,	720,	20,	5,
+	5,	"+",	1650,	1280,	114,	39,	228,	74250000,
+	"+",	1706,	164,	6,	32,	1},/*RGB565*/
+{ "4  CEA 5        1920x1080i @ 60 Hz   ",
+	5,	1125,	540,	20,	5,
+	0,	"+",	2200,	1920,	88,	44,	10,	74250000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "5  CEA 6-7      480i (NTSC)          ",
+	6,	525,	240,	44,	5,
+	0,	"-",	858,	720,	12,	64,	10,	13513513,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "6  CEA 14-15    480p @ 60 Hz         ",
+	14,	525,	480,	44,	5,
+	0,	"-",	858,	720,	12,	64,	10,	27027000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "7  CEA 16       1920x1080p @ 60 Hz   ",
+	16,	1125,	1080,	36,	5,
+	0,	"+",	1980,	1280,	440,	40,	10,	133650000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "8  CEA 17-18    720x576p @ 50 Hz     ",
+	17,	625,	576,	44,	5,
+	0,	"-",	864,	720,	12,	64,	10,	27000000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "9  CEA 19       1280x720p @ 50 Hz    ",
+	19,	750,	720,	25,	5,
+	0,	"+",	1980,	1280,	440,	40,	10,	74250000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "10 CEA 20       1920 x 1080i @ 50 Hz ",
+	20,	1125,	540,	20,	5,
+	0,	"+",	2640,	1920,	528,	44,	10,	74250000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "11 CEA 21-22    576i (PAL)           ",
+	21,	625,	288,	44,	5,
+	0,	"-",	1728,	1440,	12,	64,	10,	27000000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "12 CEA 29/30    576p                 ",
+	29,	625,	576,	44,	5,
+	0,	"-",	864,	720,	12,	64,	10,	27000000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "13 CEA 31       1080p 50Hz           ",
+	31,	1125,	1080,	44,	5,
+	0,	"-",	2640,	1920,	12,	64,	10,	148500000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "14 CEA 32       1920x1080p @ 24 Hz   ",
+	32,	1125,	1080,	36,	5,
+	4,	"+",	2750,	1920,	660,	44,	153,	74250000,
+	"+",	2844,	0x530,	6,	32,	1},/*RGB565*/
+{ "15 CEA 33       1920x1080p @ 25 Hz   ",
+	33,	1125,	1080,	36,	5,
+	4,	"+",	2640,	1920,	528,	44,	10,	74250000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "16 CEA 34       1920x1080p @ 30Hz    ",
+	34,	1125,	1080,	36,	5,
+	4,	"+",	2200,	1920,	91,	44,	153,	74250000,
+	"+",	2275,	0xAB,	6,	32,	1},/*RGB565*/
+{ "17 CEA 60       1280x720p @ 24 Hz    ",
+	60,	750,	720,	20,	5,
+	5,	"+",	3300,	1280,	284,	50,	2276,	59400000,
+	"+",	4266,	0xAD0,	5,	32,	1},/*RGB565*/
+{ "18 CEA 61       1280x720p @ 25 Hz    ",
+	61,	750,	720,	20,	5,
+	5,	"+",	3960,	1280,	228,	39,	2503,	74250000,
+	"+",	4096,	0x500,	5,	32,	1},/*RGB565*/
+{ "19 CEA 62       1280x720p @ 30 Hz    ",
+	62,	750,	720,	20,	5,
+	5,	"+",	3300,	1280,	228,	39,	1820,	74250000,
+	"+",	3413,	0x770,	5,	32,	1},/*RGB565*/
+{ "20 VESA 9       800x600 @ 60 Hz      ",
+	109,	628,	600,	28,	4,
+	0,	"+",	1056,	800,	40,	128,	10,	40000000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "21 VESA 14      848x480  @ 60 Hz     ",
+	114,	517,	480,	20,	5,
+	0,	"+",	1088,	848,	24,	80,	10,	33750000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "22 VESA 16      1024x768 @ 60 Hz     ",
+	116,	806,	768,	38,	6,
+	0,	"-",	1344,	1024,	24,	135,	10,	65000000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "23 VESA 22      1280x768 @ 60 Hz     ",
+	122,	790,	768,	34,	4,
+	0,	"+",	1440,	1280,	48,	160,	10,	68250000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "24 VESA 23      1280x768 @ 60 Hz     ",
+	123,	798,	768,	30,	7,
+	0,	"+",	1664,	1280,	64,	128,	10,	79500000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "25 VESA 27      1280x800 @ 60 Hz     ",
+	127,	823,	800,	23,	6,
+	0,	"+",	1440,	1280,	48,	32,	10,	71000000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "26 VESA 28      1280x800 @ 60 Hz     ",
+	128,	831,	800,	31,	6,
+	0,	"+",	1680,	1280,	72,	128,	10,	83500000,
+	"-",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "27 VESA 39      1360x768 @ 60 Hz     ",
+	139,	795,	768,	22,	5,
+	0,	"-",	1792,	1360,	48,	32,	10,	85500000,
+	"+",	0,	0,	0,	0,	0},/*Settings to be define*/
+{ "28 VESA 81      1366x768 @ 60 Hz     ",
+	181,	798,	768,	30,	5,
+	0,	"+",	1792,	1366,	72,	136,	10,	85750000,
+	"-",	0,	0,	0,	0,	0} /*Settings to be define*/
+};
+
+const struct color_conversion_cmd col_trans_identity = {
+	.c0      = 0x0100, .c1      = 0x0000, .c2      = 0x0000,
+	.c3      = 0x0000, .c4      = 0x0100, .c5      = 0x0000,
+	.c6      = 0x0000, .c7      = 0x0000, .c8      = 0x0100,
+	.aoffset = 0x0000, .boffset = 0x0000, .coffset = 0x0000,
+	.lmax    = 0xff,
+	.lmin    = 0x00,
+	.cmax    = 0xff,
+	.cmin    = 0x00,
+};
+
+const struct color_conversion_cmd col_trans_identity_clamp_yuv = {
+	.c0      = 0x0100, .c1      = 0x0000, .c2      = 0x0000,
+	.c3      = 0x0000, .c4      = 0x0100, .c5      = 0x0000,
+	.c6      = 0x0000, .c7      = 0x0000, .c8      = 0x0100,
+	.aoffset = 0x0000, .boffset = 0x0000, .coffset = 0x0000,
+	.lmax    = 0xeb,
+	.lmin    = 0x10,
+	.cmax    = 0xf0,
+	.cmin    = 0x10,
+};
+
+const struct color_conversion_cmd col_trans_yuv_to_rgb_v1 = {
+	.c0      = 0x0087, .c1      = 0x0000, .c2      = 0x00ba,
+	.c3      = 0x0087, .c4      = 0xffd3, .c5      = 0xffa1,
+	.c6      = 0x0087, .c7      = 0x00eb, .c8      = 0x0000,
+	.aoffset = 0xffab, .boffset = 0x004e, .coffset = 0xff92,
+	.lmax    = 0xff,
+	.lmin    = 0x00,
+	.cmax    = 0xff,
+	.cmin    = 0x00,
+};
+
+const struct color_conversion_cmd col_trans_yuv_to_rgb_v2 = {
+	.c0      = 0x0198, .c1      = 0x012a, .c2      = 0x0000,
+	.c3      = 0xff30, .c4      = 0x012a, .c5      = 0xff9c,
+	.c6      = 0x0000, .c7      = 0x012a, .c8      = 0x0204,
+	.aoffset = 0xff21, .boffset = 0x0088, .coffset = 0xfeeb,
+	.lmax    = 0xff,
+	.lmin    = 0x00,
+	.cmax    = 0xff,
+	.cmin    = 0x00,
+};
+
+const struct color_conversion_cmd col_trans_yuv_to_denc = {
+	.c0      = 0x0100, .c1      = 0x0000, .c2      = 0x0000,
+	.c3      = 0x0000, .c4      = 0x0100, .c5      = 0x0000,
+	.c6      = 0x0000, .c7      = 0x0000, .c8      = 0x0100,
+	.aoffset = 0x0000, .boffset = 0x0000, .coffset = 0x0000,
+	.lmax    = 0xeb,
+	.lmin    = 0x10,
+	.cmax    = 0xf0,
+	.cmin    = 0x10,
+};
+
+const struct color_conversion_cmd col_trans_rgb_to_denc = {
+	.c0      = 0x0070, .c1      = 0xffb6, .c2      = 0xffda,
+	.c3      = 0x0042, .c4      = 0x0081, .c5      = 0x0019,
+	.c6      = 0xffee, .c7      = 0xffa2, .c8      = 0x0070,
+	.aoffset = 0x007f, .boffset = 0x0010, .coffset = 0x007f,
+	.lmax    = 0xff,
+	.lmin    = 0x00,
+	.cmax    = 0xff,
+	.cmin    = 0x00,
+};
+
+static const struct i2c_device_id av8100_id[] = {
+	{ "av8100", 0 },
+	{ }
+};
+
+static struct av8100_device *devnr_to_adev(int devnr)
+{
+	/* Get device from list of devices */
+	struct list_head *element;
+	struct av8100_device *av8100_dev;
+	int cnt = 0;
+
+	list_for_each(element, &av8100_device_list) {
+		av8100_dev = list_entry(element, struct av8100_device, list);
+		if (cnt == devnr)
+			return av8100_dev;
+		cnt++;
+	}
+
+	return NULL;
+}
+
+static struct av8100_device *dev_to_adev(struct device *dev)
+{
+	/* Get device from list of devices */
+	struct list_head *element;
+	struct av8100_device *av8100_dev;
+	int cnt = 0;
+
+	list_for_each(element, &av8100_device_list) {
+		av8100_dev = list_entry(element, struct av8100_device, list);
+		if (av8100_dev->dev == dev)
+			return av8100_dev;
+		cnt++;
+	}
+
+	return NULL;
+}
+
+static int adev_to_devnr(struct av8100_device *adev)
+{
+	/* Get devnr from list of devices */
+	struct list_head *element;
+	struct av8100_device *av8100_dev;
+	int cnt = 0;
+
+	list_for_each(element, &av8100_device_list) {
+		av8100_dev = list_entry(element, struct av8100_device, list);
+		if (av8100_dev == adev)
+			return cnt;
+		cnt++;
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_PM
+static int av8100_suspend(struct device *dev)
+{
+	int ret = 0;
+	struct av8100_device *adev;
+
+	adev = dev_to_adev(dev);
+	if (!adev)
+		return -EFAULT;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	adev->params.pre_suspend_power =
+		(av8100_status_get().av8100_state > AV8100_OPMODE_SHUTDOWN);
+
+	if (adev->params.pre_suspend_power) {
+		ret = av8100_powerdown();
+		if (ret)
+			dev_err(dev, "av8100_powerdown failed\n");
+	}
+
+	return ret;
+}
+
+static int av8100_resume(struct device *dev)
+{
+	int ret;
+	u8 hpds = 0;
+	struct av8100_device *adev;
+
+	adev = dev_to_adev(dev);
+	if (!adev)
+		return -EFAULT;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (adev->params.pre_suspend_power) {
+		ret = av8100_powerup();
+		if (ret) {
+			dev_err(dev, "av8100_powerup failed\n");
+			return ret;
+		}
+
+		/* Check HDMI plug status */
+		if (av8100_reg_stby_r(NULL, NULL, &hpds, NULL, NULL)) {
+			dev_warn(dev, "av8100_reg_stby_r failed\n");
+			goto av8100_resume_end;
+		}
+
+		if (hpds)
+			set_plug_status(adev, AV8100_HDMI_PLUGIN); /* Plugged*/
+		else
+			clr_plug_status(adev,
+					AV8100_HDMI_PLUGIN); /* Unplugged*/
+
+		adev->params.hpdm = AV8100_STANDBY_INTERRUPT_MASK_HPDM_HIGH;
+		av8100_enable_interrupt();
+	}
+
+av8100_resume_end:
+	return 0;
+}
+
+static const struct dev_pm_ops av8100_dev_pm_ops = {
+	.suspend = av8100_suspend,
+	.resume = av8100_resume,
+};
+#endif
+
+static struct i2c_driver av8100_driver = {
+	.probe	= av8100_probe,
+	.remove = av8100_remove,
+	.driver = {
+		.name	= "av8100",
+#ifdef CONFIG_PM
+		.pm	= &av8100_dev_pm_ops,
+#endif
+	},
+	.id_table	= av8100_id,
+};
+
+static void av8100_plugtimer_int(unsigned long value)
+{
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev((int)value);
+	adev->flag |= AV8100_PLUGSTARTUP_EVENT;
+	wake_up_interruptible(&adev->event);
+	del_timer(&adev->timer);
+}
+
+static int av8100_int_event_handle(struct av8100_device *adev)
+{
+	u8 hpdi = 0;
+	u8 cpdi = 0;
+	u8 uovbi = 0;
+	u8 hdcpi = 0;
+	u8 ceci = 0;
+	u8 hpds = 0;
+	u8 cpds = 0;
+	u8 hdcps = 0;
+	u8 onuvb = 0;
+	u8 cectxerr = 0;
+	u8 cecrx = 0;
+	u8 cectx = 0;
+
+	/* STANDBY_PENDING_INTERRUPT reg */
+	if (av8100_reg_stby_pend_int_r(&hpdi, &cpdi, NULL, NULL)) {
+		dev_dbg(adev->dev, "av8100_reg_stby_pend_int_r failed\n");
+		goto av8100_int_event_handle_1;
+	}
+
+	/* Plug event */
+	if (hpdi | cpdi) {
+		/* Clear pending interrupts */
+		(void)av8100_reg_stby_pend_int_w(1, 1, 1, 0);
+
+		/* STANDBY reg */
+		if (av8100_reg_stby_r(NULL, NULL, &hpds, &cpds, NULL)) {
+			dev_dbg(adev->dev, "av8100_reg_stby_r failed\n");
+			goto av8100_int_event_handle_1;
+		}
+	}
+
+	if (cpdi & adev->params.cpdm) {
+		/* TVout plugin change */
+		if (cpds) {
+			dev_dbg(adev->dev, "cpds 1\n");
+			set_plug_status(adev, AV8100_CVBS_PLUGIN);
+		} else {
+			dev_dbg(adev->dev, "cpds 0\n");
+			clr_plug_status(adev, AV8100_CVBS_PLUGIN);
+		}
+	}
+
+	if (hpdi & adev->params.hpdm) {
+		/* HDMI plugin change */
+		if (hpds) {
+			/* Plugged */
+			/* Set 5V always on */
+			av8100_5V_w(adev->params.denc_off_time,
+					0,
+					adev->params.on_time);
+			dev_dbg(adev->dev, "hpds 1\n");
+			set_plug_status(adev, AV8100_HDMI_PLUGIN);
+		} else {
+			/* Unplugged */
+			av8100_5V_w(adev->params.denc_off_time,
+					adev->params.hdmi_off_time,
+					adev->params.on_time);
+			dev_dbg(adev->dev, "hpds 0\n");
+			clr_plug_status(adev, AV8100_HDMI_PLUGIN);
+		}
+	}
+
+av8100_int_event_handle_1:
+	/* GENERAL_INTERRUPT reg */
+	if (av8100_reg_gen_int_r(NULL, NULL, NULL, &ceci,
+			&hdcpi, &uovbi, NULL)) {
+		dev_dbg(adev->dev, "av8100_reg_gen_int_r failed\n");
+		return -EINVAL;
+	}
+
+	/* CEC or HDCP event */
+	if (ceci | hdcpi | uovbi) {
+		/* Clear pending interrupts */
+		av8100_reg_gen_int_w(1, 1, 1, 1, 1, 1);
+
+		/* GENERAL_STATUS reg */
+		if (av8100_reg_gen_status_r(&cectxerr, &cecrx, &cectx, NULL,
+				&onuvb, &hdcps) != 0) {
+			dev_dbg(adev->dev, "av8100_reg_gen_status_r fail\n");
+			return -EINVAL;
+		}
+	}
+
+	/* Underflow or overflow */
+	if (uovbi)
+		dev_dbg(adev->dev, "uovbi %d\n", onuvb);
+
+	/* CEC received */
+	if (ceci && cecrx) {
+		u8 val;
+
+		dev_dbg(adev->dev, "cecrx\n");
+
+		/* Clear cecrx in status reg*/
+		if (av8100_reg_r(AV8100_GENERAL_STATUS, &val) == 0) {
+			if (av8100_reg_w(AV8100_GENERAL_STATUS,
+				val & ~AV8100_GENERAL_STATUS_CECREC_MASK))
+				dev_info(adev->dev, "gen_stat write error\n");
+		} else {
+			dev_info(adev->dev, "gen_stat read error\n");
+		}
+
+		/* Report CEC event */
+		cec_rx(adev);
+	}
+
+	/* CEC tx error */
+	if (ceci && cectx && cectxerr) {
+		dev_dbg(adev->dev, "cectxerr\n");
+		/* Report CEC tx error event */
+		cec_txerr(adev);
+	} else if (ceci && cectx) {
+		dev_dbg(adev->dev, "cectx\n");
+		/* Report CEC tx event */
+		cec_tx(adev);
+	}
+
+	/* HDCP event */
+	if (hdcpi) {
+		dev_dbg(adev->dev, "hdcpch:%0x\n", hdcps);
+		/* Report HDCP status change event */
+		hdcp_changed(adev);
+	}
+
+	return 0;
+}
+
+static int av8100_plugstartup_event_handle(struct av8100_device *adev)
+{
+	u8 hpds = 0;
+	u8 cpds = 0;
+
+	switch (adev->params.plug_state) {
+	case AV8100_UNPLUGGED:
+	case AV8100_PLUGGED:
+	default:
+		break;
+
+	case AV8100_PLUGGED_STARTUP:
+		/* Unmask interrupt */
+		adev->params.hpdm = AV8100_STANDBY_INTERRUPT_MASK_HPDM_HIGH;
+		if (av8100_reg_stby_int_mask_w(adev->params.hpdm,
+				adev->params.cpdm,
+			AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT,
+			AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW)) {
+			dev_dbg(adev->dev,
+				"av8100_reg_stby_int_mask_w fail\n");
+		}
+
+		mdelay(AV8100_WAITTIME_1MS);
+
+		/* Get actual plug status */
+		if (av8100_reg_stby_r(NULL, NULL, &hpds, &cpds,	NULL))
+			dev_dbg(adev->dev, "av8100_reg_stby_r fail\n");
+
+		/* Set plugstate */
+		if (hpds) {
+			adev->params.plug_state = AV8100_PLUGGED;
+			dev_dbg(adev->dev, "plug_state:2\n");
+		} else {
+			adev->params.plug_state = AV8100_UNPLUGGED;
+			dev_dbg(adev->dev, "plug_state:0\n");
+
+			if (adev->params.hdmi_ev_cb)
+				adev->params.hdmi_ev_cb(
+					AV8100_HDMI_EVENT_HDMI_PLUGOUT);
+		}
+		break;
+	}
+
+	return 0;
+}
+
+static int av8100_thread(void *p)
+{
+	u8			flags;
+	struct av8100_device	*adev = p;
+
+	while (1) {
+		wait_event_interruptible(adev->event, (adev->flag != 0));
+		flags = adev->flag;
+		adev->flag = 0;
+
+		if (adev->status.av8100_state < AV8100_OPMODE_STANDBY)
+			continue;
+
+		if (flags & AV8100_INT_EVENT)
+			(void)av8100_int_event_handle(adev);
+
+		if (flags & AV8100_PLUGSTARTUP_EVENT)
+			(void)av8100_plugstartup_event_handle(adev);
+	}
+
+	return 0;
+}
+
+static irqreturn_t av8100_intr_handler(int irq, void *p)
+{
+	struct av8100_device *adev;
+
+	adev = (struct av8100_device *) p;
+	adev->flag |= AV8100_INT_EVENT;
+	wake_up_interruptible(&adev->event);
+
+	return IRQ_HANDLED;
+}
+
+static u16 av8100_get_te_line_nb(
+	enum av8100_output_CEA_VESA output_video_format)
+{
+	u16 retval;
+
+	switch (output_video_format) {
+	case AV8100_CEA1_640X480P_59_94HZ:
+	case AV8100_CEA2_3_720X480P_59_94HZ:
+	case AV8100_VESA16_1024X768P_60HZ:
+		retval = AV8100_TE_LINE_NB_30;
+		break;
+
+	case AV8100_CEA4_1280X720P_60HZ:
+	case AV8100_CEA60_1280X720P_24HZ:
+	case AV8100_CEA61_1280X720P_25HZ:
+	case AV8100_CEA62_1280X720P_30HZ:
+		retval = AV8100_TE_LINE_NB_21;
+		break;
+
+	case AV8100_CEA5_1920X1080I_60HZ:
+	case AV8100_CEA6_7_NTSC_60HZ:
+	case AV8100_CEA20_1920X1080I_50HZ:
+	case AV8100_CEA21_22_576I_PAL_50HZ:
+	case AV8100_VESA27_1280X800P_59_91HZ:
+		retval = AV8100_TE_LINE_NB_18;
+		break;
+
+	case AV8100_CEA14_15_480p_60HZ:
+		retval = AV8100_TE_LINE_NB_32;
+		break;
+
+	case AV8100_CEA17_18_720X576P_50HZ:
+	case AV8100_CEA29_30_576P_50HZ:
+		retval = AV8100_TE_LINE_NB_40;
+		break;
+
+	case AV8100_CEA19_1280X720P_50HZ:
+	case AV8100_VESA39_1360X768P_60_02HZ:
+		retval = AV8100_TE_LINE_NB_22;
+		break;
+
+	case AV8100_CEA32_1920X1080P_24HZ:
+	case AV8100_CEA33_1920X1080P_25HZ:
+	case AV8100_CEA34_1920X1080P_30HZ:
+		retval = AV8100_TE_LINE_NB_38;
+		break;
+
+	case AV8100_VESA9_800X600P_60_32HZ:
+		retval = AV8100_TE_LINE_NB_24;
+		break;
+
+	case AV8100_VESA14_848X480P_60HZ:
+		retval = AV8100_TE_LINE_NB_29;
+		break;
+
+	case AV8100_VESA22_1280X768P_59_99HZ:
+		retval = AV8100_TE_LINE_NB_17;
+		break;
+
+	case AV8100_VESA23_1280X768P_59_87HZ:
+	case AV8100_VESA81_1366X768P_59_79HZ:
+		retval = AV8100_TE_LINE_NB_25;
+		break;
+
+	case AV8100_VESA28_1280X800P_59_81HZ:
+		retval = AV8100_TE_LINE_NB_26;
+		break;
+
+	case AV8100_CEA16_1920X1080P_60HZ:
+	case AV8100_CEA31_1920x1080P_50Hz:
+	default:
+		/* TODO */
+		retval = AV8100_TE_LINE_NB_38;
+		break;
+	}
+
+	return retval;
+}
+
+static u16 av8100_get_ui_x4(
+	enum av8100_output_CEA_VESA output_video_format)
+{
+	return AV8100_UI_X4_DEFAULT;
+}
+
+static int av8100_config_video_output_dep(
+		enum av8100_output_CEA_VESA output_format)
+{
+	int retval;
+	union av8100_configuration config;
+
+	/* video input */
+	config.video_input_format.dsi_input_mode =
+		AV8100_HDMI_DSI_COMMAND_MODE;
+	config.video_input_format.input_pixel_format = AV8100_INPUT_PIX_RGB565;
+	config.video_input_format.total_horizontal_pixel =
+		av8100_all_cea[output_format].htotale;
+	config.video_input_format.total_horizontal_active_pixel =
+		av8100_all_cea[output_format].hactive;
+	config.video_input_format.total_vertical_lines =
+		av8100_all_cea[output_format].vtotale;
+	config.video_input_format.total_vertical_active_lines =
+		av8100_all_cea[output_format].vactive;
+
+	switch (output_format) {
+	case AV8100_CEA5_1920X1080I_60HZ:
+	case AV8100_CEA20_1920X1080I_50HZ:
+	case AV8100_CEA21_22_576I_PAL_50HZ:
+	case AV8100_CEA6_7_NTSC_60HZ:
+		config.video_input_format.video_mode =
+			AV8100_VIDEO_INTERLACE;
+		break;
+
+	default:
+		config.video_input_format.video_mode =
+			AV8100_VIDEO_PROGRESSIVE;
+		break;
+	}
+
+	config.video_input_format.nb_data_lane =
+		AV8100_DATA_LANES_USED_2;
+	config.video_input_format.nb_virtual_ch_command_mode = 0;
+	config.video_input_format.nb_virtual_ch_video_mode = 0;
+	config.video_input_format.ui_x4 = av8100_get_ui_x4(output_format);
+	config.video_input_format.TE_line_nb = av8100_get_te_line_nb(
+		output_format);
+	config.video_input_format.TE_config = AV8100_TE_OFF;
+	config.video_input_format.master_clock_freq = 0;
+
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_VIDEO_INPUT_FORMAT, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* DENC */
+	switch (output_format) {
+	case AV8100_CEA21_22_576I_PAL_50HZ:
+		config.denc_format.cvbs_video_format = AV8100_CVBS_625;
+		config.denc_format.standard_selection = AV8100_PAL_BDGHI;
+		break;
+
+	case AV8100_CEA6_7_NTSC_60HZ:
+		config.denc_format.cvbs_video_format = AV8100_CVBS_525;
+		config.denc_format.standard_selection = AV8100_NTSC_M;
+		break;
+
+	default:
+		/* Not supported */
+		break;
+	}
+
+	return 0;
+}
+
+static int av8100_config_init(struct av8100_device *adev)
+{
+	int retval;
+	union av8100_configuration config;
+
+	dev_dbg(adev->dev, "%s\n", __func__);
+
+	memset(&config, 0, sizeof(union av8100_configuration));
+	memset(&adev->config, 0, sizeof(struct av8100_config));
+
+	/* Color conversion */
+	config.color_transform = AV8100_COLOR_TRANSFORM_INDENTITY;
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_COLORSPACECONVERSION, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* DENC */
+	config.denc_format.cvbs_video_format = AV8100_CVBS_625;
+	config.denc_format.standard_selection = AV8100_PAL_BDGHI;
+	config.denc_format.enable = 0;
+	config.denc_format.macrovision_enable = 0;
+	config.denc_format.internal_generator = 0;
+	retval = av8100_conf_prep(AV8100_COMMAND_DENC, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* Video output */
+	config.video_output_format.video_output_cea_vesa =
+		AV8100_CEA4_1280X720P_60HZ;
+
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_VIDEO_OUTPUT_FORMAT, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* Video input */
+	av8100_config_video_output_dep(
+		config.video_output_format.video_output_cea_vesa);
+
+	/* Pattern generator */
+	config.pattern_generator_format.pattern_audio_mode =
+		AV8100_PATTERN_AUDIO_OFF;
+	config.pattern_generator_format.pattern_type =
+		AV8100_PATTERN_GENERATOR;
+	config.pattern_generator_format.pattern_video_format =
+		AV8100_PATTERN_720P;
+	retval = av8100_conf_prep(AV8100_COMMAND_PATTERNGENERATOR,
+		&config);
+	if (retval)
+		return -EFAULT;
+
+	/* Audio input */
+	config.audio_input_format.audio_input_if_format	=
+		AV8100_AUDIO_I2SDELAYED_MODE;
+	config.audio_input_format.i2s_input_nb = 1;
+	config.audio_input_format.sample_audio_freq = AV8100_AUDIO_FREQ_48KHZ;
+	config.audio_input_format.audio_word_lg = AV8100_AUDIO_16BITS;
+	config.audio_input_format.audio_format = AV8100_AUDIO_LPCM_MODE;
+	config.audio_input_format.audio_if_mode = AV8100_AUDIO_MASTER;
+	config.audio_input_format.audio_mute = AV8100_AUDIO_MUTE_DISABLE;
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_AUDIO_INPUT_FORMAT, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* HDMI mode */
+	config.hdmi_format.hdmi_mode	= AV8100_HDMI_ON;
+	config.hdmi_format.hdmi_format	= AV8100_HDMI;
+	config.hdmi_format.dvi_format	= AV8100_DVI_CTRL_CTL0;
+	retval = av8100_conf_prep(AV8100_COMMAND_HDMI, &config);
+	if (retval)
+		return -EFAULT;
+
+	/* EDID section readback */
+	config.edid_section_readback_format.address = 0xA0;
+	config.edid_section_readback_format.block_number = 0;
+	retval = av8100_conf_prep(
+		AV8100_COMMAND_EDID_SECTION_READBACK, &config);
+	if (retval)
+		return -EFAULT;
+
+	return 0;
+}
+
+static int av8100_params_init(struct av8100_device *adev)
+{
+	dev_dbg(adev->dev, "%s\n", __func__);
+
+	memset(&adev->params, 0, sizeof(struct av8100_params));
+
+	adev->params.denc_off_time = AV8100_DENC_OFF_TIME;
+	adev->params.hdmi_off_time = AV8100_HDMI_OFF_TIME;
+	adev->params.on_time = AV8100_ON_TIME;
+
+	adev->params.hpdm = AV8100_STANDBY_INTERRUPT_MASK_HPDM_HIGH;
+	adev->params.cpdm = AV8100_STANDBY_INTERRUPT_MASK_CPDM_HIGH;
+	adev->params.hdcpm = AV8100_GENERAL_INTERRUPT_MASK_HDCPM_HIGH;
+	adev->params.cecm = AV8100_GENERAL_INTERRUPT_MASK_CECM_HIGH;
+	adev->params.uovbm = AV8100_GENERAL_INTERRUPT_MASK_UOVBM_HIGH;
+
+	return 0;
+}
+
+static void clr_plug_status(struct av8100_device *adev,
+			enum av8100_plugin_status status)
+{
+	adev->status.av8100_plugin_status &= ~status;
+
+	switch (status) {
+	case AV8100_HDMI_PLUGIN:
+		switch (adev->params.plug_state) {
+		case AV8100_UNPLUGGED:
+		case AV8100_PLUGGED_STARTUP:
+		default:
+			break;
+
+		case AV8100_PLUGGED:
+			adev->params.plug_state =
+				AV8100_UNPLUGGED;
+			dev_dbg(adev->dev, "plug_state:0\n");
+
+			if (adev->params.hdmi_ev_cb)
+				adev->params.hdmi_ev_cb(
+					AV8100_HDMI_EVENT_HDMI_PLUGOUT);
+			break;
+		}
+		break;
+
+	case AV8100_CVBS_PLUGIN:
+		/* TODO */
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void set_plug_status(struct av8100_device *adev,
+			enum av8100_plugin_status status)
+{
+	adev->status.av8100_plugin_status |= status;
+
+	switch (status) {
+	case AV8100_HDMI_PLUGIN:
+		switch (adev->params.plug_state) {
+		case AV8100_UNPLUGGED:
+			adev->params.plug_state =
+				AV8100_PLUGGED_STARTUP;
+
+			dev_dbg(adev->dev, "plug_state:1\n");
+
+			/*
+			 * Mask interrupts to avoid plug detect during
+			 * startup
+			 *  */
+			adev->params.hpdm =
+				AV8100_STANDBY_INTERRUPT_MASK_HPDM_LOW;
+			if (av8100_reg_stby_int_mask_w(
+					adev->params.hpdm,
+					adev->params.cpdm,
+				AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT,
+				AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW)) {
+				dev_dbg(adev->dev,
+					"av8100_reg_stby_int_mask_w fail\n");
+			}
+
+			/* Set plug startup timer */
+			init_timer(&adev->timer);
+			adev->timer.expires = jiffies +
+					AV8100_PLUGSTARTUP_TIME;
+			adev->timer.function =
+					av8100_plugtimer_int;
+			adev->timer.data = 0;
+			adev->timer.data = adev_to_devnr(adev);
+			mod_timer(&adev->timer, adev->timer.expires);
+
+			if (adev->params.hdmi_ev_cb)
+				adev->params.hdmi_ev_cb(
+					AV8100_HDMI_EVENT_HDMI_PLUGIN);
+			break;
+
+		case AV8100_PLUGGED_STARTUP:
+		case AV8100_PLUGGED:
+		default:
+			break;
+		}
+		break;
+
+	case AV8100_CVBS_PLUGIN:
+		/* TODO */
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void cec_rx(struct av8100_device *adev)
+{
+	if (adev->params.hdmi_ev_cb)
+		adev->params.hdmi_ev_cb(AV8100_HDMI_EVENT_CEC);
+}
+
+static void cec_tx(struct av8100_device *adev)
+{
+	if (adev->params.hdmi_ev_cb)
+		adev->params.hdmi_ev_cb(AV8100_HDMI_EVENT_CECTX);
+}
+
+static void cec_txerr(struct av8100_device *adev)
+{
+	if (adev->params.hdmi_ev_cb)
+		adev->params.hdmi_ev_cb(AV8100_HDMI_EVENT_CECTXERR);
+}
+
+static void hdcp_changed(struct av8100_device *adev)
+{
+	if (adev->params.hdmi_ev_cb)
+		adev->params.hdmi_ev_cb(AV8100_HDMI_EVENT_HDCP);
+}
+
+static void av8100_set_state(struct av8100_device *adev,
+			enum av8100_operating_mode state)
+{
+	adev->status.av8100_state = state;
+
+	if (state <= AV8100_OPMODE_STANDBY) {
+		clr_plug_status(adev, AV8100_HDMI_PLUGIN);
+		clr_plug_status(adev, AV8100_CVBS_PLUGIN);
+		adev->status.hdmi_on = false;
+	}
+}
+
+/**
+ * write_single_byte() - Write a single byte to av8100
+ * through i2c interface.
+ * @client:	i2c client structure
+ * @reg:	register offset
+ * @data:	data byte to be written
+ *
+ * This funtion uses smbus byte write API to write a single byte to av8100
+ **/
+static int write_single_byte(struct i2c_client *client, u8 reg,
+	u8 data)
+{
+	int ret;
+	struct device *dev = &client->dev;
+
+	ret = i2c_smbus_write_byte_data(client, reg, data);
+	if (ret < 0)
+		dev_dbg(dev, "i2c smbus write byte failed\n");
+
+	return ret;
+}
+
+/**
+ * read_single_byte() - read single byte from av8100
+ * through i2c interface
+ * @client:     i2c client structure
+ * @reg:        register offset
+ * @val:	register value
+ *
+ * This funtion uses smbus read block API to read single byte from the reg
+ * offset.
+ **/
+static int read_single_byte(struct i2c_client *client, u8 reg, u8 *val)
+{
+	int value;
+	struct device *dev = &client->dev;
+
+	value = i2c_smbus_read_byte_data(client, reg);
+	if (value < 0) {
+		dev_dbg(dev, "i2c smbus read byte failed,read data = %x "
+			"from offset:%x\n" , value, reg);
+		return -EFAULT;
+	}
+
+	*val = (u8) value;
+	return 0;
+}
+
+/**
+ * write_multi_byte() - Write a multiple bytes to av8100 through
+ * i2c interface.
+ * @client:	i2c client structure
+ * @buf:	buffer to be written
+ * @nbytes:	nunmber of bytes to be written
+ *
+ * This funtion uses smbus block write API's to write n number of bytes to the
+ * av8100
+ **/
+static int write_multi_byte(struct i2c_client *client, u8 reg,
+		u8 *buf, u8 nbytes)
+{
+	int ret;
+	struct device *dev = &client->dev;
+
+	ret = i2c_smbus_write_i2c_block_data(client, reg, nbytes, buf);
+	if (ret < 0)
+		dev_dbg(dev, "i2c smbus write multi byte error\n");
+
+	return ret;
+}
+
+static int configuration_video_input_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_video_input_cmd.dsi_input_mode;
+	buffer[1] = adev->config.hdmi_video_input_cmd.input_pixel_format;
+	buffer[2] = REG_16_8_MSB(adev->config.hdmi_video_input_cmd.
+		total_horizontal_pixel);
+	buffer[3] = REG_16_8_LSB(adev->config.hdmi_video_input_cmd.
+		total_horizontal_pixel);
+	buffer[4] = REG_16_8_MSB(adev->config.hdmi_video_input_cmd.
+		total_horizontal_active_pixel);
+	buffer[5] = REG_16_8_LSB(adev->config.hdmi_video_input_cmd.
+		total_horizontal_active_pixel);
+	buffer[6] = REG_16_8_MSB(adev->config.hdmi_video_input_cmd.
+		total_vertical_lines);
+	buffer[7] = REG_16_8_LSB(adev->config.hdmi_video_input_cmd.
+		total_vertical_lines);
+	buffer[8] = REG_16_8_MSB(adev->config.hdmi_video_input_cmd.
+		total_vertical_active_lines);
+	buffer[9] = REG_16_8_LSB(adev->config.hdmi_video_input_cmd.
+		total_vertical_active_lines);
+	buffer[10] = adev->config.hdmi_video_input_cmd.video_mode;
+	buffer[11] = adev->config.hdmi_video_input_cmd.nb_data_lane;
+	buffer[12] = adev->config.hdmi_video_input_cmd.
+		nb_virtual_ch_command_mode;
+	buffer[13] = adev->config.hdmi_video_input_cmd.
+		nb_virtual_ch_video_mode;
+	buffer[14] = REG_16_8_MSB(adev->config.hdmi_video_input_cmd.
+		TE_line_nb);
+	buffer[15] = REG_16_8_LSB(adev->config.hdmi_video_input_cmd.
+		TE_line_nb);
+	buffer[16] = adev->config.hdmi_video_input_cmd.TE_config;
+	buffer[17] = REG_32_8_MSB(adev->config.hdmi_video_input_cmd.
+		master_clock_freq);
+	buffer[18] = REG_32_8_MMSB(adev->config.hdmi_video_input_cmd.
+		master_clock_freq);
+	buffer[19] = REG_32_8_MLSB(adev->config.hdmi_video_input_cmd.
+		master_clock_freq);
+	buffer[20] = REG_32_8_LSB(adev->config.hdmi_video_input_cmd.
+		master_clock_freq);
+	buffer[21] = adev->config.hdmi_video_input_cmd.ui_x4;
+
+	*length = AV8100_COMMAND_VIDEO_INPUT_FORMAT_SIZE - 1;
+	return 0;
+
+}
+
+static int configuration_audio_input_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_audio_input_cmd.audio_input_if_format;
+	buffer[1] = adev->config.hdmi_audio_input_cmd.i2s_input_nb;
+	buffer[2] = adev->config.hdmi_audio_input_cmd.sample_audio_freq;
+	buffer[3] = adev->config.hdmi_audio_input_cmd.audio_word_lg;
+	buffer[4] = adev->config.hdmi_audio_input_cmd.audio_format;
+	buffer[5] = adev->config.hdmi_audio_input_cmd.audio_if_mode;
+	buffer[6] = adev->config.hdmi_audio_input_cmd.audio_mute;
+
+	*length = AV8100_COMMAND_AUDIO_INPUT_FORMAT_SIZE - 1;
+	return 0;
+}
+
+static int configuration_video_output_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_video_output_cmd.
+		video_output_cea_vesa;
+
+	if (buffer[0] == AV8100_CUSTOM) {
+		buffer[1] = adev->config.hdmi_video_output_cmd.
+			vsync_polarity;
+		buffer[2] = adev->config.hdmi_video_output_cmd.
+			hsync_polarity;
+		buffer[3] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.total_horizontal_pixel);
+		buffer[4] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.total_horizontal_pixel);
+		buffer[5] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.total_horizontal_active_pixel);
+		buffer[6] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.total_horizontal_active_pixel);
+		buffer[7] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.total_vertical_in_half_lines);
+		buffer[8] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.total_vertical_in_half_lines);
+		buffer[9] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.
+				total_vertical_active_in_half_lines);
+		buffer[10] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.
+				total_vertical_active_in_half_lines);
+		buffer[11] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.hsync_start_in_pixel);
+		buffer[12] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.hsync_start_in_pixel);
+		buffer[13] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.hsync_length_in_pixel);
+		buffer[14] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.hsync_length_in_pixel);
+		buffer[15] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.vsync_start_in_half_line);
+		buffer[16] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.vsync_start_in_half_line);
+		buffer[17] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.vsync_length_in_half_line);
+		buffer[18] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.vsync_length_in_half_line);
+		buffer[19] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.hor_video_start_pixel);
+		buffer[20] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.hor_video_start_pixel);
+		buffer[21] = REG_16_8_MSB(adev->config.
+			hdmi_video_output_cmd.vert_video_start_pixel);
+		buffer[22] = REG_16_8_LSB(adev->config.
+			hdmi_video_output_cmd.vert_video_start_pixel);
+		buffer[23] = adev->config.hdmi_video_output_cmd.video_type;
+		buffer[24] = adev->config.hdmi_video_output_cmd.pixel_repeat;
+		buffer[25] = REG_32_8_MSB(adev->config.
+			hdmi_video_output_cmd.pixel_clock_freq_Hz);
+		buffer[26] = REG_32_8_MMSB(adev->config.
+			hdmi_video_output_cmd.pixel_clock_freq_Hz);
+		buffer[27] = REG_32_8_MLSB(adev->config.
+			hdmi_video_output_cmd.pixel_clock_freq_Hz);
+		buffer[28] = REG_32_8_LSB(adev->config.
+			hdmi_video_output_cmd.pixel_clock_freq_Hz);
+
+		*length = AV8100_COMMAND_VIDEO_OUTPUT_FORMAT_SIZE - 1;
+	} else {
+		*length = 1;
+	}
+
+	return 0;
+}
+
+static int configuration_video_scaling_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = REG_16_8_MSB(adev->config.hdmi_video_scaling_cmd.
+		h_start_in_pixel);
+	buffer[1] = REG_16_8_LSB(adev->config.hdmi_video_scaling_cmd.
+		h_start_in_pixel);
+	buffer[2] = REG_16_8_MSB(adev->config.hdmi_video_scaling_cmd.
+		h_stop_in_pixel);
+	buffer[3] = REG_16_8_LSB(adev->config.hdmi_video_scaling_cmd.
+		h_stop_in_pixel);
+	buffer[4] = REG_16_8_MSB(adev->config.hdmi_video_scaling_cmd.
+		v_start_in_line);
+	buffer[5] = REG_16_8_LSB(adev->config.hdmi_video_scaling_cmd.
+		v_start_in_line);
+	buffer[6] = REG_16_8_MSB(adev->config.hdmi_video_scaling_cmd.
+		v_stop_in_line);
+	buffer[7] = REG_16_8_LSB(adev->config.hdmi_video_scaling_cmd.
+		v_stop_in_line);
+	buffer[8] = REG_16_8_MSB(adev->config.hdmi_video_scaling_cmd.
+		h_start_out_pixel);
+	buffer[9] = REG_16_8_LSB(adev->config.hdmi_video_scaling_cmd
+		.h_start_out_pixel);
+	buffer[10] = REG_16_8_MSB(adev->config.hdmi_video_scaling_cmd.
+		h_stop_out_pixel);
+	buffer[11] = REG_16_8_LSB(adev->config.hdmi_video_scaling_cmd.
+		h_stop_out_pixel);
+	buffer[12] = REG_16_8_MSB(adev->config.hdmi_video_scaling_cmd.
+		v_start_out_line);
+	buffer[13] = REG_16_8_LSB(adev->config.hdmi_video_scaling_cmd.
+		v_start_out_line);
+	buffer[14] = REG_16_8_MSB(adev->config.hdmi_video_scaling_cmd.
+		v_stop_out_line);
+	buffer[15] = REG_16_8_LSB(adev->config.hdmi_video_scaling_cmd.
+		v_stop_out_line);
+
+	*length = AV8100_COMMAND_VIDEO_SCALING_FORMAT_SIZE - 1;
+	return 0;
+}
+
+static int configuration_colorspace_conversion_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	const struct color_conversion_cmd *hdmi_color_space_conversion_cmd;
+
+	hdmi_color_space_conversion_cmd =
+		get_color_transform_cmd(adev, adev->config.color_transform);
+
+	buffer[0] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c0);
+	buffer[1] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c0);
+	buffer[2] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c1);
+	buffer[3] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c1);
+	buffer[4] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c2);
+	buffer[5] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c2);
+	buffer[6] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c3);
+	buffer[7] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c3);
+	buffer[8] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c4);
+	buffer[9] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c4);
+	buffer[10] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c5);
+	buffer[11] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c5);
+	buffer[12] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c6);
+	buffer[13] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c6);
+	buffer[14] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c7);
+	buffer[15] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c7);
+	buffer[16] = REG_12_8_MSB(hdmi_color_space_conversion_cmd->c8);
+	buffer[17] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->c8);
+	buffer[18] = REG_10_8_MSB(hdmi_color_space_conversion_cmd->aoffset);
+	buffer[19] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->aoffset);
+	buffer[20] = REG_10_8_MSB(hdmi_color_space_conversion_cmd->boffset);
+	buffer[21] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->boffset);
+	buffer[22] = REG_10_8_MSB(hdmi_color_space_conversion_cmd->coffset);
+	buffer[23] = REG_16_8_LSB(hdmi_color_space_conversion_cmd->coffset);
+	buffer[24] = hdmi_color_space_conversion_cmd->lmax;
+	buffer[25] = hdmi_color_space_conversion_cmd->lmin;
+	buffer[26] = hdmi_color_space_conversion_cmd->cmax;
+	buffer[27] = hdmi_color_space_conversion_cmd->cmin;
+
+	*length = AV8100_COMMAND_COLORSPACECONVERSION_SIZE - 1;
+	return 0;
+}
+
+static int configuration_cec_message_write_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_cec_message_write_cmd.buffer_length;
+	memcpy(&buffer[1], adev->config.hdmi_cec_message_write_cmd.buffer,
+			adev->config.hdmi_cec_message_write_cmd.buffer_length);
+
+	*length = adev->config.hdmi_cec_message_write_cmd.buffer_length + 1;
+
+	return 0;
+}
+
+static int configuration_cec_message_read_get(char *buffer,
+	unsigned int *length)
+{
+	/* No buffer data */
+	*length = AV8100_COMMAND_CEC_MESSAGE_READ_BACK_SIZE - 1;
+	return 0;
+}
+
+static int configuration_denc_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_denc_cmd.cvbs_video_format;
+	buffer[1] = adev->config.hdmi_denc_cmd.standard_selection;
+	buffer[2] = adev->config.hdmi_denc_cmd.enable;
+	buffer[3] = adev->config.hdmi_denc_cmd.macrovision_enable;
+	buffer[4] = adev->config.hdmi_denc_cmd.internal_generator;
+
+	*length = AV8100_COMMAND_DENC_SIZE - 1;
+	return 0;
+}
+
+static int configuration_hdmi_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_cmd.hdmi_mode;
+	buffer[1] = adev->config.hdmi_cmd.hdmi_format;
+	buffer[2] = adev->config.hdmi_cmd.dvi_format;
+
+	*length = AV8100_COMMAND_HDMI_SIZE - 1;
+	return 0;
+}
+
+static int configuration_hdcp_sendkey_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_hdcp_send_key_cmd.key_number;
+	memcpy(&buffer[1], adev->config.hdmi_hdcp_send_key_cmd.data,
+			adev->config.hdmi_hdcp_send_key_cmd.data_len);
+
+	*length = adev->config.hdmi_hdcp_send_key_cmd.data_len + 1;
+	return 0;
+}
+
+static int configuration_hdcp_management_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_hdcp_management_format_cmd.req_type;
+	buffer[1] = adev->config.hdmi_hdcp_management_format_cmd.encr_use;
+
+	*length = AV8100_COMMAND_HDCP_MANAGEMENT_SIZE - 1;
+	return 0;
+}
+
+static int configuration_infoframe_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_infoframes_cmd.type;
+	buffer[1] = adev->config.hdmi_infoframes_cmd.version;
+	buffer[2] = adev->config.hdmi_infoframes_cmd.length;
+	buffer[3] = adev->config.hdmi_infoframes_cmd.crc;
+	memcpy(&buffer[4], adev->config.hdmi_infoframes_cmd.data,
+	HDMI_INFOFRAME_DATA_SIZE);
+
+	*length = adev->config.hdmi_infoframes_cmd.length + 4;
+	return 0;
+}
+
+static int av8100_edid_section_readback_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_edid_section_readback_cmd.address;
+	buffer[1] = adev->config.hdmi_edid_section_readback_cmd.
+		block_number;
+
+	*length = AV8100_COMMAND_EDID_SECTION_READBACK_SIZE - 1;
+	return 0;
+}
+
+static int configuration_pattern_generator_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_pattern_generator_cmd.pattern_type;
+	buffer[1] = adev->config.hdmi_pattern_generator_cmd.
+		pattern_video_format;
+	buffer[2] = adev->config.hdmi_pattern_generator_cmd.
+		pattern_audio_mode;
+
+	*length = AV8100_COMMAND_PATTERNGENERATOR_SIZE - 1;
+	return 0;
+}
+
+static int configuration_fuse_aes_key_get(struct av8100_device *adev,
+			char *buffer, unsigned int *length)
+{
+	buffer[0] = adev->config.hdmi_fuse_aes_key_cmd.fuse_operation;
+	if (adev->config.hdmi_fuse_aes_key_cmd.fuse_operation) {
+		/* Write key command */
+		memcpy(&buffer[1], adev->config.hdmi_fuse_aes_key_cmd.key,
+			HDMI_FUSE_AES_KEY_SIZE);
+
+		*length = AV8100_COMMAND_FUSE_AES_KEY_SIZE - 1;
+	} else {
+		/* Check key command */
+		*length = AV8100_COMMAND_FUSE_AES_CHK_SIZE - 1;
+	}
+	return 0;
+}
+
+static int get_command_return_first(struct i2c_client *i2c,
+		enum av8100_command_type command_type) {
+	int retval = 0;
+	char val;
+	struct device *dev = &i2c->dev;
+
+	retval = read_single_byte(i2c, AV8100_COMMAND_OFFSET, &val);
+	if (retval) {
+		dev_dbg(dev, "%s 1st ret failed\n", __func__);
+		return retval;
+	}
+
+	if (val != (0x80 | command_type)) {
+		dev_dbg(dev, "%s 1st ret wrong:%x\n", __func__, val);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int get_command_return_data(struct i2c_client *i2c,
+	enum av8100_command_type command_type,
+	u8 *command_buffer,
+	u8 *buffer_length,
+	u8 *buffer)
+{
+	int retval = 0;
+	char val;
+	int index = 0;
+	struct device *dev = &i2c->dev;
+
+	if (buffer_length)
+		*buffer_length = 0;
+
+	switch (command_type) {
+	case AV8100_COMMAND_VIDEO_INPUT_FORMAT:
+	case AV8100_COMMAND_AUDIO_INPUT_FORMAT:
+	case AV8100_COMMAND_VIDEO_OUTPUT_FORMAT:
+	case AV8100_COMMAND_VIDEO_SCALING_FORMAT:
+	case AV8100_COMMAND_COLORSPACECONVERSION:
+	case AV8100_COMMAND_CEC_MESSAGE_WRITE:
+	case AV8100_COMMAND_DENC:
+	case AV8100_COMMAND_HDMI:
+	case AV8100_COMMAND_INFOFRAMES:
+	case AV8100_COMMAND_PATTERNGENERATOR:
+		/* Get the second return byte */
+		retval = read_single_byte(i2c,
+			AV8100_2ND_RET_BYTE_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail2r;
+
+		if (val) {
+			retval = -EFAULT;
+			goto get_command_return_data_fail2v;
+		}
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_READ_BACK:
+		if ((buffer == NULL) ||	(buffer_length == NULL)) {
+			retval = -EINVAL;
+			goto get_command_return_data_fail;
+		}
+
+		/* Get the return buffer length */
+		retval = read_single_byte(i2c, AV8100_CEC_ADDR_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail;
+
+		dev_dbg(dev, "cec buflen:%d\n", val);
+		*buffer_length = val;
+
+		if (*buffer_length >
+			HDMI_CEC_READ_MAXSIZE) {
+			dev_dbg(dev, "CEC size too large %d\n",
+				*buffer_length);
+			*buffer_length = HDMI_CEC_READ_MAXSIZE;
+		}
+
+		dev_dbg(dev, "return data: ");
+
+		/* Get the return buffer */
+		for (index = 0; index < *buffer_length; ++index) {
+			retval = read_single_byte(i2c,
+				AV8100_CEC_RET_BUF_OFFSET + index, &val);
+			if (retval) {
+				*buffer_length = 0;
+				goto get_command_return_data_fail;
+			} else {
+				*(buffer + index) = val;
+				dev_dbg(dev, "%02x ", *(buffer + index));
+			}
+		}
+
+		dev_dbg(dev, "\n");
+		break;
+
+	case AV8100_COMMAND_HDCP_MANAGEMENT:
+		{
+		u8 nrdev;
+		u8 devcnt;
+		int cnt;
+
+		/* Get the second return byte */
+		retval = read_single_byte(i2c,
+			AV8100_2ND_RET_BYTE_OFFSET, &val);
+		if (retval) {
+			goto get_command_return_data_fail2r;
+		} else {
+			/* Check the second return byte */
+			if (val)
+				goto get_command_return_data_fail2v;
+		}
+
+		if ((buffer == NULL) || (buffer_length == NULL))
+			/* Ignore return data */
+			break;
+
+		dev_dbg(dev, "req_type:%02x ", command_buffer[0]);
+
+		/* Check if revoc list data is requested */
+		if (command_buffer[0] !=
+			HDMI_REQUEST_FOR_REVOCATION_LIST_INPUT) {
+			*buffer_length = 0;
+			break;
+		}
+
+		dev_dbg(dev, "return data: ");
+
+		/* Get the return buffer */
+		for (cnt = 0; cnt < HDMI_HDCP_MGMT_BKSV_SIZE; cnt++) {
+			retval = read_single_byte(i2c,
+				AV8100_HDCP_RET_BUF_OFFSET + index, &val);
+			if (retval) {
+				*buffer_length = 0;
+				goto get_command_return_data_fail;
+			} else {
+				*(buffer + index) = val;
+				dev_dbg(dev, "%02x ", *(buffer + index));
+			}
+			index++;
+		}
+
+		/* Get Device count */
+		retval = read_single_byte(i2c,
+			AV8100_HDCP_RET_BUF_OFFSET + index, &nrdev);
+		if (retval) {
+			*buffer_length = 0;
+			goto get_command_return_data_fail;
+		} else {
+			*(buffer + index) = nrdev;
+			dev_dbg(dev, "%02x ", *(buffer + index));
+		}
+		index++;
+
+		/* Determine number of devices */
+		nrdev &= HDMI_HDCP_MGMT_DEVICE_MASK;
+		if (nrdev > HDMI_HDCP_MGMT_MAX_DEVICES_SIZE)
+			nrdev = HDMI_HDCP_MGMT_MAX_DEVICES_SIZE;
+
+		/* Get Bksv for each connected equipment */
+		for (devcnt = 0; devcnt < nrdev; devcnt++)
+			for (cnt = 0; cnt < HDMI_HDCP_MGMT_BKSV_SIZE; cnt++) {
+				retval = read_single_byte(i2c,
+					AV8100_HDCP_RET_BUF_OFFSET + index,
+									&val);
+				if (retval) {
+					*buffer_length = 0;
+					goto get_command_return_data_fail;
+				} else {
+					*(buffer + index) = val;
+					dev_dbg(dev, "%02x ",
+							*(buffer + index));
+				}
+				index++;
+			}
+
+		if (nrdev == 0)
+			goto hdcp_management_end;
+
+		/* Get SHA signature */
+		for (cnt = 0; cnt < HDMI_HDCP_MGMT_SHA_SIZE - 1; cnt++) {
+			retval = read_single_byte(i2c,
+				AV8100_HDCP_RET_BUF_OFFSET + index, &val);
+			if (retval) {
+				*buffer_length = 0;
+				goto get_command_return_data_fail;
+			} else {
+				*(buffer + index) = val;
+				dev_dbg(dev, "%02x ", *(buffer + index));
+			}
+			index++;
+		}
+
+hdcp_management_end:
+		*buffer_length = index;
+
+		dev_dbg(dev, "\n");
+		}
+		break;
+
+	case AV8100_COMMAND_EDID_SECTION_READBACK:
+		if ((buffer == NULL) ||	(buffer_length == NULL)) {
+			retval = -EINVAL;
+			goto get_command_return_data_fail;
+		}
+
+		/* Return buffer length is fixed */
+		*buffer_length = HDMI_EDIDREAD_SIZE;
+
+		dev_dbg(dev, "return data: ");
+
+		/* Get the return buffer */
+		for (index = 0; index < *buffer_length; ++index) {
+			retval = read_single_byte(i2c,
+				AV8100_EDID_RET_BUF_OFFSET + index, &val);
+			if (retval) {
+				*buffer_length = 0;
+				goto get_command_return_data_fail;
+			} else {
+				*(buffer + index) = val;
+				dev_dbg(dev, "%02x ", *(buffer + index));
+			}
+		}
+
+		dev_dbg(dev, "\n");
+		break;
+
+	case AV8100_COMMAND_FUSE_AES_KEY:
+		if ((buffer == NULL) ||	(buffer_length == NULL)) {
+			retval = -EINVAL;
+			goto get_command_return_data_fail;
+		}
+
+		/* Get the second return byte */
+		retval = read_single_byte(i2c,
+			AV8100_2ND_RET_BYTE_OFFSET, &val);
+
+		if (retval)
+			goto get_command_return_data_fail2r;
+
+		/* Check the second return byte */
+		if (val) {
+			retval = -EFAULT;
+			goto get_command_return_data_fail2v;
+		}
+
+		/* Return buffer length is fixed */
+		*buffer_length = HDMI_FUSE_AES_KEY_RET_SIZE;
+
+		/* Get CRC */
+		retval = read_single_byte(i2c,
+			AV8100_FUSE_CRC_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail;
+
+		*buffer = val;
+		dev_dbg(dev, "CRC:%02x ", val);
+
+		/* Get programmed status */
+		retval = read_single_byte(i2c,
+			AV8100_FUSE_PRGD_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail;
+
+		*(buffer + 1) = val;
+
+		dev_dbg(dev, "programmed:%02x ", val);
+		break;
+
+	case AV8100_COMMAND_HDCP_SENDKEY:
+		if ((command_buffer[0] == HDMI_LOADAES_END_BLK_NR) &&
+			((buffer == NULL) || (buffer_length == NULL))) {
+			retval = -EINVAL;
+			goto get_command_return_data_fail;
+		}
+
+		/* Get the second return byte */
+		retval = read_single_byte(i2c,
+			AV8100_2ND_RET_BYTE_OFFSET, &val);
+		if (retval)
+			goto get_command_return_data_fail2r;
+
+		if (val) {
+			retval = -EFAULT;
+			goto get_command_return_data_fail2v;
+		}
+
+		if (command_buffer[0] == HDMI_LOADAES_END_BLK_NR) {
+			/* Return CRC32 if last AES block */
+			int cnt;
+
+			dev_dbg(dev, "CRC32:");
+			for (cnt = 0; cnt < HDMI_CRC32_SIZE; cnt++) {
+				if (read_single_byte(i2c,
+					AV8100_CRC32_OFFSET + cnt, &val))
+					goto get_command_return_data_fail;
+				*(buffer + cnt) = val;
+				dev_dbg(dev, "%02x", val);
+			}
+
+			*buffer_length = HDMI_CRC32_SIZE;
+		}
+		break;
+
+	default:
+		retval = -EFAULT;
+		break;
+	}
+
+	return retval;
+get_command_return_data_fail2r:
+	dev_dbg(dev, "%s Reading 2nd return byte failed\n", __func__);
+	return retval;
+get_command_return_data_fail2v:
+	dev_dbg(dev, "%s 2nd return byte is wrong:%x\n", __func__, val);
+	return retval;
+get_command_return_data_fail:
+	dev_dbg(dev, "%s FAIL\n", __func__);
+	return retval;
+}
+
+static int av8100_powerup1(struct av8100_device *adev)
+{
+	int retval;
+	struct av8100_platform_data *pdata = adev->dev->platform_data;
+
+	/* Regulator enable */
+	if ((adev->params.regulator_pwr) &&
+			(adev->params.regulator_requested == false)) {
+		retval = regulator_enable(adev->params.regulator_pwr);
+		if (retval < 0) {
+			dev_warn(adev->dev, "%s: regulator_enable failed\n",
+			__func__);
+			return retval;
+		}
+		dev_dbg(adev->dev, "regulator_enable ok\n");
+		adev->params.regulator_requested = true;
+	}
+
+	/* Reset av8100 */
+	gpio_set_value_cansleep(pdata->reset, 1);
+
+	/* Need to wait before proceeding */
+	mdelay(AV8100_WAITTIME_1MS);
+
+	av8100_set_state(adev, AV8100_OPMODE_STANDBY);
+
+	if (pdata->alt_powerupseq) {
+		dev_dbg(adev->dev, "powerup seq alt\n");
+		retval = av8100_5V_w(0, 0, AV8100_ON_TIME);
+		if (retval) {
+			dev_err(adev->dev, "%s reg_wr err 1\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		udelay(AV8100_WATTIME_100US);
+
+		retval = av8100_reg_stby_pend_int_w(
+				AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_HIGH);
+		if (retval) {
+			dev_err(adev->dev, "%s reg_wr err 2\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		udelay(AV8100_WATTIME_100US);
+
+		retval = av8100_reg_stby_w(AV8100_STANDBY_CPD_LOW,
+				AV8100_STANDBY_STBY_HIGH, pdata->mclk_freq);
+		if (retval) {
+			dev_err(adev->dev, "%s reg_wr err 3\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		mdelay(AV8100_WAITTIME_1MS);
+
+		retval = av8100_reg_stby_w(AV8100_STANDBY_CPD_LOW,
+				AV8100_STANDBY_STBY_LOW, pdata->mclk_freq);
+		if (retval) {
+			dev_err(adev->dev, "%s reg_wr err 4\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		mdelay(AV8100_WAITTIME_1MS);
+
+		retval = av8100_reg_stby_pend_int_w(
+				AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_LOW);
+		if (retval) {
+			dev_err(adev->dev, "%s reg_wr err 5\n", __func__);
+			goto av8100_powerup1_err;
+		}
+
+		mdelay(AV8100_WAITTIME_1MS);
+	}
+
+	retval = request_irq(pdata->irq, av8100_intr_handler,
+			IRQF_TRIGGER_RISING, "av8100", adev);
+	if (retval == 0)
+		adev->params.irq_requested = true;
+	else
+		dev_err(adev->dev, "request_irq %d failed %d\n",
+				pdata->irq, retval);
+
+	return retval;
+
+av8100_powerup1_err:
+	av8100_powerdown();
+	return -EFAULT;
+}
+
+static int av8100_powerup2(struct av8100_device *adev)
+{
+	int retval;
+
+	/* ON time & OFF time on 5v HDMI plug detect */
+	retval = av8100_5V_w(adev->params.denc_off_time,
+				adev->params.hdmi_off_time,
+				adev->params.on_time);
+	if (retval) {
+		dev_err(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		return retval;
+	}
+
+	mdelay(AV8100_WAITTIME_1MS);
+
+	av8100_set_state(adev, AV8100_OPMODE_SCAN);
+
+	return 0;
+}
+
+static int register_read_internal(u8 offset, u8 *value)
+{
+	int retval = 0;
+	struct i2c_client *i2c;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EFAULT;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	i2c = adev->config.client;
+
+	/* Read from register */
+	retval = read_single_byte(i2c, offset, value);
+	if (retval)	{
+		dev_dbg(adev->dev,
+			"Failed to read the value from av8100 register\n");
+		return -EFAULT;
+	}
+
+	return retval;
+}
+
+static int register_write_internal(u8 offset, u8 value)
+{
+	int retval;
+	struct i2c_client *i2c;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EFAULT;
+
+	i2c = adev->config.client;
+
+	/* Write to register */
+	retval = write_single_byte(i2c, offset, value);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+int av8100_powerscan(void)
+{
+	int retval;
+	struct av8100_device *adev;
+	struct av8100_platform_data *pdata;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EFAULT;
+
+	pdata = adev->dev->platform_data;
+
+	dev_dbg(adev->dev, "%s\n", __func__);
+
+	if (av8100_status_get().av8100_state > AV8100_OPMODE_SCAN) {
+		dev_dbg(adev->dev, "set to scan mode\n");
+
+		av8100_disable_interrupt();
+
+		/* Stby mode */
+		retval = av8100_reg_stby_w(AV8100_STANDBY_CPD_LOW,
+			AV8100_STANDBY_STBY_LOW, pdata->mclk_freq);
+		if (retval) {
+			dev_err(adev->dev,
+				"Failed to write to av8100 register\n");
+			return retval;
+		}
+
+		/* Remove APE OPP requirement */
+		if (adev->params.opp_requested) {
+			prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+					(char *)adev->miscdev.name);
+			prcmu_qos_remove_requirement(PRCMU_QOS_DDR_OPP,
+					(char *)adev->miscdev.name);
+			adev->params.opp_requested = false;
+		}
+
+		/* Clock disable */
+		if (adev->params.inputclk &&
+				adev->params.inputclk_requested) {
+			clk_disable(adev->params.inputclk);
+			adev->params.inputclk_requested = false;
+		}
+
+		mdelay(AV8100_WAITTIME_1MS);
+
+		av8100_enable_interrupt();
+
+		av8100_set_state(adev, AV8100_OPMODE_SCAN);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_powerscan);
+
+int av8100_powerup(void)
+{
+	int ret = 0;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EFAULT;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state < AV8100_OPMODE_STANDBY) {
+		ret = av8100_powerup1(adev);
+		if (ret) {
+			dev_err(adev->dev, "av8100_powerup1 fail\n");
+			return -EFAULT;
+		}
+	}
+
+	if (av8100_status_get().av8100_state < AV8100_OPMODE_SCAN)
+		ret = av8100_powerup2(adev);
+
+	av8100_enable_interrupt();
+
+	return ret;
+}
+EXPORT_SYMBOL(av8100_powerup);
+
+int av8100_powerdown(void)
+{
+	int retval = 0;
+	struct av8100_device *adev;
+	struct av8100_platform_data *pdata;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EFAULT;
+
+	pdata = adev->dev->platform_data;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		goto av8100_powerdown_end;
+
+	av8100_disable_interrupt();
+
+	if (adev->params.irq_requested)
+		free_irq(pdata->irq, adev);
+	adev->params.irq_requested = false;
+
+	if (pdata->alt_powerupseq) {
+		retval = av8100_reg_stby_pend_int_w(
+				AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+				AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_HIGH);
+
+		if (retval)
+			dev_err(adev->dev, "%s reg_wr err\n", __func__);
+		msleep(AV8100_WAITTIME_50MS);
+	}
+
+	/* Remove APE OPP requirement */
+	if (adev->params.opp_requested) {
+		prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+				(char *)adev->miscdev.name);
+		prcmu_qos_remove_requirement(PRCMU_QOS_DDR_OPP,
+				(char *)adev->miscdev.name);
+		adev->params.opp_requested = false;
+	}
+
+	/* Clock disable */
+	if (adev->params.inputclk && adev->params.inputclk_requested) {
+		clk_disable(adev->params.inputclk);
+		adev->params.inputclk_requested = false;
+	}
+
+	av8100_set_state(adev, AV8100_OPMODE_SHUTDOWN);
+
+	gpio_set_value_cansleep(pdata->reset, 0);
+
+	/* Regulator disable */
+	if ((adev->params.regulator_pwr) &&
+			(adev->params.regulator_requested)) {
+		dev_dbg(adev->dev, "regulator_disable\n");
+		regulator_disable(adev->params.regulator_pwr);
+		adev->params.regulator_requested = false;
+	}
+
+	if (pdata->alt_powerupseq)
+		mdelay(AV8100_WAITTIME_5MS);
+
+av8100_powerdown_end:
+	return retval;
+}
+EXPORT_SYMBOL(av8100_powerdown);
+
+int av8100_download_firmware(enum interface_type if_type)
+{
+	int retval;
+	int temp = 0x0;
+	int increment = 15;
+	int index = 0;
+	int size = 0x0;
+	char val = 0x0;
+	char checksum = 0;
+	int cnt;
+	int cnt_max;
+	struct i2c_client *i2c;
+	u8 uc;
+	u8 fdl;
+	u8 hld;
+	u8 wa;
+	u8 ra;
+	struct av8100_platform_data *pdata;
+	const struct firmware *fw_file;
+	u8 *fw_buff;
+	int fw_bytes;
+	struct av8100_device *adev;
+	struct av8100_status status;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	LOCK_AV8100_FWDL;
+
+	status = av8100_status_get();
+	if (status.av8100_state <= AV8100_OPMODE_SHUTDOWN) {
+		retval = -EINVAL;
+		goto av8100_download_firmware_err2;
+	}
+
+	if (status.av8100_state >= AV8100_OPMODE_INIT) {
+		dev_dbg(adev->dev, "FW already ok\n");
+		retval = 0;
+		goto av8100_download_firmware_err2;
+	}
+
+	av8100_set_state(adev, AV8100_OPMODE_INIT);
+
+	pdata = adev->dev->platform_data;
+
+	/* Request firmware */
+	if (request_firmware(&fw_file,
+			       AV8100_FW_FILENAME,
+			       adev->dev)) {
+		dev_err(adev->dev, "fw request failed\n");
+		retval = -EFAULT;
+		goto av8100_download_firmware_err2;
+	}
+
+	/* Master clock timing, running */
+	retval = av8100_reg_stby_w(AV8100_STANDBY_CPD_LOW,
+		AV8100_STANDBY_STBY_HIGH, pdata->mclk_freq);
+	if (retval) {
+		dev_err(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		goto av8100_download_firmware_err;
+	}
+
+	mdelay(AV8100_WAITTIME_1MS);
+
+	/* Clock enable */
+	if (adev->params.inputclk &&
+			adev->params.inputclk_requested == false) {
+		if (clk_enable(adev->params.inputclk)) {
+			dev_err(adev->dev, "inputclk en failed\n");
+			retval = -EFAULT;
+			goto av8100_download_firmware_err;
+		}
+
+		adev->params.inputclk_requested = true;
+	}
+
+	/* Request 100% APE OPP */
+	if (adev->params.opp_requested == false) {
+		if (prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP,
+				(char *)adev->miscdev.name, 100)) {
+			dev_err(adev->dev, "APE OPP 100 failed\n");
+			retval = -EFAULT;
+			goto av8100_download_firmware_err;
+		}
+		if (prcmu_qos_add_requirement(PRCMU_QOS_DDR_OPP,
+				(char *)adev->miscdev.name, 100)) {
+			dev_err(adev->dev, "DDR OPP 100 failed\n");
+			prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+					(char *)adev->miscdev.name);
+			retval = -EFAULT;
+			goto av8100_download_firmware_err;
+		}
+
+		adev->params.opp_requested = true;
+	}
+
+	msleep(AV8100_WAITTIME_10MS);
+
+	/* Prepare firmware data */
+	fw_bytes = fw_file->size;
+	fw_buff = (u8 *)fw_file->data;
+	dev_dbg(adev->dev, "fw size:%d\n", fw_bytes);
+
+	i2c = adev->config.client;
+
+	/* Enable firmware download */
+	retval = av8100_reg_gen_ctrl_w(
+		AV8100_GENERAL_CONTROL_FDL_HIGH,
+		AV8100_GENERAL_CONTROL_HLD_HIGH,
+		AV8100_GENERAL_CONTROL_WA_LOW,
+		AV8100_GENERAL_CONTROL_RA_LOW);
+	if (retval) {
+		dev_err(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		retval = -EFAULT;
+		goto av8100_download_firmware_err;
+	}
+
+	retval = av8100_reg_gen_ctrl_r(&fdl, &hld, &wa, &ra);
+	if (retval) {
+		dev_err(adev->dev,
+			"Failed to read the value from av8100 register\n");
+		retval = -EFAULT;
+		goto av8100_download_firmware_err;
+	} else {
+		dev_dbg(adev->dev, "GENERAL_CONTROL_REG register fdl:%d "
+			"hld:%d wa:%d ra:%d\n", fdl, hld, wa, ra);
+	}
+
+	LOCK_AV8100_HW;
+
+	temp = fw_bytes % increment;
+	for (size = 0; size < (fw_bytes-temp); size = size + increment,
+		index += increment) {
+		if (if_type == I2C_INTERFACE) {
+			retval = write_multi_byte(i2c,
+				AV8100_FIRMWARE_DOWNLOAD_ENTRY, fw_buff + size,
+				increment);
+			if (retval) {
+				dev_dbg(adev->dev, "Failed to download the "
+					"av8100 firmware\n");
+				UNLOCK_AV8100_HW;
+				retval = -EFAULT;
+				goto av8100_download_firmware_err;
+			}
+		} else if (if_type == DSI_INTERFACE) {
+			dev_dbg(adev->dev,
+				"DSI_INTERFACE is currently not supported\n");
+			UNLOCK_AV8100_HW;
+			retval = -EINVAL;
+			goto av8100_download_firmware_err;
+		} else {
+			UNLOCK_AV8100_HW;
+			retval = -EINVAL;
+			goto av8100_download_firmware_err;
+		}
+	}
+
+	/* Transfer last firmware bytes */
+	if (if_type == I2C_INTERFACE) {
+		retval = write_multi_byte(i2c,
+			AV8100_FIRMWARE_DOWNLOAD_ENTRY, fw_buff + size, temp);
+		if (retval) {
+			dev_dbg(adev->dev,
+				"Failed to download the av8100 firmware\n");
+			UNLOCK_AV8100_HW;
+			retval = -EFAULT;
+			goto av8100_download_firmware_err;
+		}
+	} else if (if_type == DSI_INTERFACE) {
+		/* TODO: Add support for DSI firmware download */
+		UNLOCK_AV8100_HW;
+		retval = -EINVAL;
+		goto av8100_download_firmware_err;
+	} else {
+		UNLOCK_AV8100_HW;
+		retval = -EINVAL;
+		goto av8100_download_firmware_err;
+	}
+
+	/* check transfer*/
+	for (size = 0; size < fw_bytes; size++)
+		checksum = checksum ^ fw_buff[size];
+
+	UNLOCK_AV8100_HW;
+
+	retval = av8100_reg_fw_dl_entry_r(&val);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to read the value from the av8100 register\n");
+		retval = -EFAULT;
+		goto av8100_download_firmware_err;
+	}
+
+	dev_dbg(adev->dev, "checksum:%x,val:%x\n", checksum, val);
+
+	if (checksum != val) {
+		dev_dbg(adev->dev,
+			">Fw downloading.... FAIL checksum issue\n");
+		dev_dbg(adev->dev, "checksum = %d\n", checksum);
+		dev_dbg(adev->dev, "checksum read: %d\n", val);
+		retval = -EFAULT;
+		goto av8100_download_firmware_err;
+	} else {
+		dev_dbg(adev->dev, ">Fw downloading.... success\n");
+	}
+
+	/* Set to idle mode */
+	av8100_reg_gen_ctrl_w(AV8100_GENERAL_CONTROL_FDL_LOW,
+		AV8100_GENERAL_CONTROL_HLD_LOW,	AV8100_GENERAL_CONTROL_WA_LOW,
+		AV8100_GENERAL_CONTROL_RA_LOW);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to write the value to the av8100 register\n");
+		retval = -EFAULT;
+		goto av8100_download_firmware_err;
+	}
+
+	/* Wait Internal Micro controler ready */
+	cnt = 0;
+	cnt_max = sizeof(waittime_retry) / sizeof(waittime_retry[0]);
+	retval = av8100_reg_gen_status_r(NULL, NULL, NULL, &uc,
+		NULL, NULL);
+	while ((retval == 0) && (uc != 0x1) && (cnt < cnt_max)) {
+		mdelay(waittime_retry[cnt]);
+		retval = av8100_reg_gen_status_r(NULL, NULL, NULL,
+			&uc, NULL, NULL);
+		cnt++;
+	}
+	dev_dbg(adev->dev, "av8100 fwdl cnt:%d\n", cnt);
+
+	if (retval)	{
+		dev_dbg(adev->dev,
+			"Failed to read the value from the av8100 register\n");
+		retval = -EFAULT;
+		goto av8100_download_firmware_err;
+	}
+
+	if (uc != 0x1)
+		dev_dbg(adev->dev, "UC is not ready\n");
+
+	release_firmware(fw_file);
+
+	if (adev->chip_version != 1) {
+		char *cut_str;
+
+		/* Get cut version */
+		retval = read_single_byte(i2c, AV8100_CUTVER_OFFSET, &val);
+		if (retval) {
+			dev_err(adev->dev, "Read cut ver failed\n");
+			return retval;
+		}
+
+		switch (val) {
+		case 0x00:
+			cut_str = CUT_STR_0;
+			break;
+		case 0x01:
+			cut_str = CUT_STR_1;
+			break;
+		case 0x03:
+			cut_str = CUT_STR_3;
+			break;
+		case 0x30:
+			cut_str = CUT_STR_30;
+			break;
+		default:
+			cut_str = CUT_STR_UNKNOWN;
+			break;
+		}
+		dev_dbg(adev->dev, "Cut ver %d %s\n", val, cut_str);
+	}
+
+	av8100_set_state(adev, AV8100_OPMODE_IDLE);
+
+	UNLOCK_AV8100_FWDL;
+	return 0;
+
+av8100_download_firmware_err:
+	release_firmware(fw_file);
+
+	/* Remove APE OPP requirement */
+	if (adev->params.opp_requested) {
+		prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP,
+				(char *)adev->miscdev.name);
+		prcmu_qos_remove_requirement(PRCMU_QOS_DDR_OPP,
+				(char *)adev->miscdev.name);
+		adev->params.opp_requested = false;
+	}
+
+	/* Clock disable */
+	if (adev->params.inputclk && adev->params.inputclk_requested) {
+		clk_disable(adev->params.inputclk);
+		adev->params.inputclk_requested = false;
+	}
+
+av8100_download_firmware_err2:
+	UNLOCK_AV8100_FWDL;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_download_firmware);
+
+int av8100_disable_interrupt(void)
+{
+	int retval;
+	u8 hpdm = 0;
+	u8 cpdm = 0;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	if (!adev->params.ints_enabled)
+		return 0;
+
+	retval = av8100_reg_stby_pend_int_w(
+			AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_LOW);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	retval = av8100_reg_gen_int_mask_w(
+			AV8100_GENERAL_INTERRUPT_MASK_EOCM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_VSIM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_VSOM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_CECM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_HDCPM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_UOVBM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_TEM_LOW);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	hpdm = adev->params.hpdm;
+	cpdm = adev->params.cpdm;
+
+	retval = av8100_reg_stby_int_mask_w(
+			AV8100_STANDBY_INTERRUPT_MASK_HPDM_LOW,
+			AV8100_STANDBY_INTERRUPT_MASK_CPDM_LOW,
+			AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT,
+			AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	adev->params.hpdm = hpdm;
+	adev->params.cpdm = cpdm;
+	adev->params.ints_enabled = false;
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_disable_interrupt);
+
+int av8100_enable_interrupt(void)
+{
+	int retval;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	if (adev->params.ints_enabled)
+		return 0;
+
+	retval = av8100_reg_stby_pend_int_w(
+			AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW,
+			AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_LOW);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	retval = av8100_reg_gen_int_mask_w(
+			AV8100_GENERAL_INTERRUPT_MASK_EOCM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_VSIM_LOW,
+			AV8100_GENERAL_INTERRUPT_MASK_VSOM_LOW,
+			adev->params.cecm,
+			adev->params.hdcpm,
+			adev->params.uovbm,
+			AV8100_GENERAL_INTERRUPT_MASK_TEM_LOW);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	retval = av8100_reg_stby_int_mask_w(
+			adev->params.hpdm,
+			adev->params.cpdm,
+			AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT,
+			AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		return -EFAULT;
+	}
+
+	adev->params.ints_enabled = true;
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_enable_interrupt);
+
+int av8100_reg_stby_w(
+		u8 cpd, u8 stby, u8 mclkrng)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_STANDBY_CPD(cpd) | AV8100_STANDBY_STBY(stby) |
+		AV8100_STANDBY_MCLKRNG(mclkrng);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_STANDBY, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_w);
+
+static int av8100_5V_w(u8 denc_off, u8 hdmi_off, u8 on)
+{
+	u8 val;
+	int retval;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value.
+	 * chip_version == 1 have one common off time
+	 * chip_version > 1 support different off time for hdmi and tvout. */
+	if (adev->chip_version == 1)
+		val = AV8100_HDMI_5_VOLT_TIME_OFF_TIME(hdmi_off) |
+			AV8100_HDMI_5_VOLT_TIME_ON_TIME(on);
+	else
+		val = AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME(denc_off) |
+			AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME(hdmi_off) |
+			AV8100_HDMI_5_VOLT_TIME_ON_TIME(on);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_HDMI_5_VOLT_TIME, val);
+
+	UNLOCK_AV8100_HW;
+
+	return retval;
+}
+
+int av8100_reg_hdmi_5_volt_time_w(u8 denc_off, u8 hdmi_off, u8 on)
+{
+	int retval;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	retval = av8100_5V_w(denc_off, hdmi_off, on);
+
+	/* Set vars */
+	if (adev->chip_version > 1)
+		adev->params.denc_off_time = denc_off;
+
+	adev->params.hdmi_off_time = hdmi_off;
+	if (on)
+		adev->params.on_time = on;
+
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_hdmi_5_volt_time_w);
+
+int av8100_reg_stby_int_mask_w(
+		u8 hpdm, u8 cpdm, u8 stbygpiocfg, u8 ipol)
+{
+	int retval;
+	u8 val;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_STANDBY_INTERRUPT_MASK_HPDM(hpdm) |
+		AV8100_STANDBY_INTERRUPT_MASK_CPDM(cpdm) |
+		AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG(stbygpiocfg) |
+		AV8100_STANDBY_INTERRUPT_MASK_IPOL(ipol);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_STANDBY_INTERRUPT_MASK, val);
+
+	adev->params.hpdm = hpdm;
+	adev->params.cpdm = cpdm;
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_int_mask_w);
+
+int av8100_reg_stby_pend_int_w(
+		u8 hpdi, u8 cpdi, u8 oni, u8 bpdig)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_STANDBY_PENDING_INTERRUPT_HPDI(hpdi) |
+		AV8100_STANDBY_PENDING_INTERRUPT_CPDI(cpdi) |
+		AV8100_STANDBY_PENDING_INTERRUPT_ONI(oni) |
+		AV8100_STANDBY_PENDING_INTERRUPT_BPDIG(bpdig);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_STANDBY_PENDING_INTERRUPT, val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_pend_int_w);
+
+int av8100_reg_gen_int_mask_w(
+		u8 eocm, u8 vsim, u8 vsom, u8 cecm, u8 hdcpm, u8 uovbm, u8 tem)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_GENERAL_INTERRUPT_MASK_EOCM(eocm) |
+		AV8100_GENERAL_INTERRUPT_MASK_VSIM(vsim) |
+		AV8100_GENERAL_INTERRUPT_MASK_VSOM(vsom) |
+		AV8100_GENERAL_INTERRUPT_MASK_CECM(cecm) |
+		AV8100_GENERAL_INTERRUPT_MASK_HDCPM(hdcpm) |
+		AV8100_GENERAL_INTERRUPT_MASK_UOVBM(uovbm) |
+		AV8100_GENERAL_INTERRUPT_MASK_TEM(tem);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_GENERAL_INTERRUPT_MASK,	val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_int_mask_w);
+
+int av8100_reg_gen_int_w(
+		u8 eoci, u8 vsii, u8 vsoi, u8 ceci, u8 hdcpi, u8 uovbi)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_GENERAL_INTERRUPT_EOCI(eoci) |
+		AV8100_GENERAL_INTERRUPT_VSII(vsii) |
+		AV8100_GENERAL_INTERRUPT_VSOI(vsoi) |
+		AV8100_GENERAL_INTERRUPT_CECI(ceci) |
+		AV8100_GENERAL_INTERRUPT_HDCPI(hdcpi) |
+		AV8100_GENERAL_INTERRUPT_UOVBI(uovbi);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_GENERAL_INTERRUPT, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_int_w);
+
+int av8100_reg_gpio_conf_w(
+		u8 dat3dir, u8 dat3val, u8 dat2dir, u8 dat2val,	u8 dat1dir,
+		u8 dat1val, u8 ucdbg)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_GPIO_CONFIGURATION_DAT3DIR(dat3dir) |
+		AV8100_GPIO_CONFIGURATION_DAT3VAL(dat3val) |
+		AV8100_GPIO_CONFIGURATION_DAT2DIR(dat2dir) |
+		AV8100_GPIO_CONFIGURATION_DAT2VAL(dat2val) |
+		AV8100_GPIO_CONFIGURATION_DAT1DIR(dat1dir) |
+		AV8100_GPIO_CONFIGURATION_DAT1VAL(dat1val) |
+		AV8100_GPIO_CONFIGURATION_UCDBG(ucdbg);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_GPIO_CONFIGURATION, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gpio_conf_w);
+
+int av8100_reg_gen_ctrl_w(
+		u8 fdl, u8 hld, u8 wa, u8 ra)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_GENERAL_CONTROL_FDL(fdl) |
+		AV8100_GENERAL_CONTROL_HLD(hld) |
+		AV8100_GENERAL_CONTROL_WA(wa) |
+		AV8100_GENERAL_CONTROL_RA(ra);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_GENERAL_CONTROL, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_ctrl_w);
+
+int av8100_reg_fw_dl_entry_w(
+	u8 mbyte_code_entry)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Set register value */
+	val = AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY(
+		mbyte_code_entry);
+
+	/* Write to register */
+	retval = register_write_internal(AV8100_FIRMWARE_DOWNLOAD_ENTRY, val);
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_fw_dl_entry_w);
+
+int av8100_reg_w(
+		u8 offset, u8 value)
+{
+	int retval = 0;
+	struct i2c_client *i2c;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	i2c = adev->config.client;
+
+	/* Write to register */
+	retval = write_single_byte(i2c, offset, value);
+	if (retval) {
+		dev_dbg(adev->dev,
+			"Failed to write the value to av8100 register\n");
+		UNLOCK_AV8100_HW;
+		return -EFAULT;
+	}
+
+	UNLOCK_AV8100_HW;
+	return 0;
+}
+EXPORT_SYMBOL(av8100_reg_w);
+
+int av8100_reg_stby_r(
+		u8 *cpd, u8 *stby, u8 *hpds, u8 *cpds, u8 *mclkrng)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_STANDBY, &val);
+
+	/* Set return params */
+	if (cpd)
+		*cpd = AV8100_STANDBY_CPD_GET(val);
+	if (stby)
+		*stby = AV8100_STANDBY_STBY_GET(val);
+	if (hpds)
+		*hpds = AV8100_STANDBY_HPDS_GET(val);
+	if (cpds)
+		*cpds = AV8100_STANDBY_CPDS_GET(val);
+	if (mclkrng)
+		*mclkrng = AV8100_STANDBY_MCLKRNG_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_r);
+
+int av8100_reg_hdmi_5_volt_time_r(
+		u8 *denc_off_time, u8 *hdmi_off_time, u8 *on_time)
+{
+	int retval;
+	u8 val;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_HDMI_5_VOLT_TIME, &val);
+
+	/* Set return params */
+	if (adev->chip_version == 1) {
+		if (denc_off_time)
+			*denc_off_time = 0;
+		if (hdmi_off_time)
+			*hdmi_off_time =
+				AV8100_HDMI_5_VOLT_TIME_OFF_TIME_GET(val);
+	} else {
+		if (denc_off_time)
+			*denc_off_time =
+				AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME_GET(val);
+		if (hdmi_off_time)
+			*hdmi_off_time =
+				AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME_GET(val);
+	}
+
+	if (on_time)
+		*on_time = AV8100_HDMI_5_VOLT_TIME_ON_TIME_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_hdmi_5_volt_time_r);
+
+int av8100_reg_stby_int_mask_r(
+		u8 *hpdm, u8 *cpdm, u8 *stbygpiocfg, u8 *ipol)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_STANDBY_INTERRUPT_MASK, &val);
+
+	/* Set return params */
+	if (hpdm)
+		*hpdm = AV8100_STANDBY_INTERRUPT_MASK_HPDM_GET(val);
+	if (cpdm)
+		*cpdm = AV8100_STANDBY_INTERRUPT_MASK_CPDM_GET(val);
+	if (stbygpiocfg)
+		*stbygpiocfg =
+			AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_GET(val);
+	if (ipol)
+		*ipol = AV8100_STANDBY_INTERRUPT_MASK_IPOL_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_int_mask_r);
+
+int av8100_reg_stby_pend_int_r(
+		u8 *hpdi, u8 *cpdi, u8 *oni, u8 *sid)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_STANDBY_PENDING_INTERRUPT,
+			&val);
+
+	/* Set return params */
+	if (hpdi)
+		*hpdi = AV8100_STANDBY_PENDING_INTERRUPT_HPDI_GET(val);
+	if (cpdi)
+		*cpdi = AV8100_STANDBY_PENDING_INTERRUPT_CPDI_GET(val);
+	if (oni)
+		*oni = AV8100_STANDBY_PENDING_INTERRUPT_ONI_GET(val);
+	if (sid)
+		*sid = AV8100_STANDBY_PENDING_INTERRUPT_SID_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_stby_pend_int_r);
+
+int av8100_reg_gen_int_mask_r(
+		u8 *eocm,
+		u8 *vsim,
+		u8 *vsom,
+		u8 *cecm,
+		u8 *hdcpm,
+		u8 *uovbm,
+		u8 *tem)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GENERAL_INTERRUPT_MASK, &val);
+
+	/* Set return params */
+	if (eocm)
+		*eocm = AV8100_GENERAL_INTERRUPT_MASK_EOCM_GET(val);
+	if (vsim)
+		*vsim = AV8100_GENERAL_INTERRUPT_MASK_VSIM_GET(val);
+	if (vsom)
+		*vsom = AV8100_GENERAL_INTERRUPT_MASK_VSOM_GET(val);
+	if (cecm)
+		*cecm = AV8100_GENERAL_INTERRUPT_MASK_CECM_GET(val);
+	if (hdcpm)
+		*hdcpm = AV8100_GENERAL_INTERRUPT_MASK_HDCPM_GET(val);
+	if (uovbm)
+		*uovbm = AV8100_GENERAL_INTERRUPT_MASK_UOVBM_GET(val);
+	if (tem)
+		*tem = AV8100_GENERAL_INTERRUPT_MASK_TEM_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_int_mask_r);
+
+int av8100_reg_gen_int_r(
+		u8 *eoci,
+		u8 *vsii,
+		u8 *vsoi,
+		u8 *ceci,
+		u8 *hdcpi,
+		u8 *uovbi,
+		u8 *tei)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GENERAL_INTERRUPT, &val);
+
+	/* Set return params */
+	if (eoci)
+		*eoci = AV8100_GENERAL_INTERRUPT_EOCI_GET(val);
+	if (vsii)
+		*vsii = AV8100_GENERAL_INTERRUPT_VSII_GET(val);
+	if (vsoi)
+		*vsoi = AV8100_GENERAL_INTERRUPT_VSOI_GET(val);
+	if (ceci)
+		*ceci = AV8100_GENERAL_INTERRUPT_CECI_GET(val);
+	if (hdcpi)
+		*hdcpi = AV8100_GENERAL_INTERRUPT_HDCPI_GET(val);
+	if (uovbi)
+		*uovbi = AV8100_GENERAL_INTERRUPT_UOVBI_GET(val);
+	if (tei)
+		*tei = AV8100_GENERAL_INTERRUPT_TEI_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_int_r);
+
+int av8100_reg_gen_status_r(
+		u8 *cectxerr,
+		u8 *cecrec,
+		u8 *cectrx,
+		u8 *uc,
+		u8 *onuvb,
+		u8 *hdcps)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GENERAL_STATUS, &val);
+
+	/* Set return params */
+	if (cectxerr)
+		*cectxerr = AV8100_GENERAL_STATUS_CECTXERR_GET(val);
+	if (cecrec)
+		*cecrec	= AV8100_GENERAL_STATUS_CECREC_GET(val);
+	if (cectrx)
+		*cectrx	= AV8100_GENERAL_STATUS_CECTRX_GET(val);
+	if (uc)
+		*uc = AV8100_GENERAL_STATUS_UC_GET(val);
+	if (onuvb)
+		*onuvb = AV8100_GENERAL_STATUS_ONUVB_GET(val);
+	if (hdcps)
+		*hdcps = AV8100_GENERAL_STATUS_HDCPS_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_status_r);
+
+int av8100_reg_gpio_conf_r(
+		u8 *dat3dir,
+		u8 *dat3val,
+		u8 *dat2dir,
+		u8 *dat2val,
+		u8 *dat1dir,
+		u8 *dat1val,
+		u8 *ucdbg)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GPIO_CONFIGURATION, &val);
+
+	/* Set return params */
+	if (dat3dir)
+		*dat3dir = AV8100_GPIO_CONFIGURATION_DAT3DIR_GET(val);
+	if (dat3val)
+		*dat3val = AV8100_GPIO_CONFIGURATION_DAT3VAL_GET(val);
+	if (dat2dir)
+		*dat2dir = AV8100_GPIO_CONFIGURATION_DAT2DIR_GET(val);
+	if (dat2val)
+		*dat2val = AV8100_GPIO_CONFIGURATION_DAT2VAL_GET(val);
+	if (dat1dir)
+		*dat1dir = AV8100_GPIO_CONFIGURATION_DAT1DIR_GET(val);
+	if (dat1val)
+		*dat1val = AV8100_GPIO_CONFIGURATION_DAT1VAL_GET(val);
+	if (ucdbg)
+		*ucdbg = AV8100_GPIO_CONFIGURATION_UCDBG_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gpio_conf_r);
+
+int av8100_reg_gen_ctrl_r(
+		u8 *fdl,
+		u8 *hld,
+		u8 *wa,
+		u8 *ra)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_GENERAL_CONTROL, &val);
+	/* Set return params */
+	if (fdl)
+		*fdl = AV8100_GENERAL_CONTROL_FDL_GET(val);
+	if (hld)
+		*hld = AV8100_GENERAL_CONTROL_HLD_GET(val);
+	if (wa)
+		*wa = AV8100_GENERAL_CONTROL_WA_GET(val);
+	if (ra)
+		*ra = AV8100_GENERAL_CONTROL_RA_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_gen_ctrl_r);
+
+int av8100_reg_fw_dl_entry_r(
+	u8 *mbyte_code_entry)
+{
+	int retval;
+	u8 val;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	/* Read from register */
+	retval = register_read_internal(AV8100_FIRMWARE_DOWNLOAD_ENTRY,	&val);
+
+	/* Set return params */
+	if (mbyte_code_entry)
+		*mbyte_code_entry =
+		AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY_GET(val);
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_fw_dl_entry_r);
+
+int av8100_reg_r(
+		u8 offset,
+		u8 *value)
+{
+	int retval = 0;
+	struct i2c_client *i2c;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	i2c = adev->config.client;
+
+	/* Read from register */
+	retval = read_single_byte(i2c, offset, value);
+	if (retval)	{
+		dev_dbg(adev->dev,
+			"Failed to read the value from av8100 register\n");
+		retval = -EFAULT;
+		goto av8100_register_read_out;
+	}
+
+av8100_register_read_out:
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_reg_r);
+
+int av8100_conf_get(enum av8100_command_type command_type,
+	union av8100_configuration *config)
+{
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	/* Put configuration data to the corresponding data struct depending
+	 * on command type */
+	switch (command_type) {
+	case AV8100_COMMAND_VIDEO_INPUT_FORMAT:
+		memcpy(&config->video_input_format,
+			&adev->config.hdmi_video_input_cmd,
+			sizeof(struct av8100_video_input_format_cmd));
+		break;
+
+	case AV8100_COMMAND_AUDIO_INPUT_FORMAT:
+		memcpy(&config->audio_input_format,
+			&adev->config.hdmi_audio_input_cmd,
+			sizeof(struct av8100_audio_input_format_cmd));
+		break;
+
+	case AV8100_COMMAND_VIDEO_OUTPUT_FORMAT:
+		memcpy(&config->video_output_format,
+			&adev->config.hdmi_video_output_cmd,
+			sizeof(struct av8100_video_output_format_cmd));
+		break;
+
+	case AV8100_COMMAND_VIDEO_SCALING_FORMAT:
+		memcpy(&config->video_scaling_format,
+			&adev->config.hdmi_video_scaling_cmd,
+			sizeof(struct av8100_video_scaling_format_cmd));
+		break;
+
+	case AV8100_COMMAND_COLORSPACECONVERSION:
+		config->color_transform = adev->config.color_transform;
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_WRITE:
+		memcpy(&config->cec_message_write_format,
+			&adev->config.hdmi_cec_message_write_cmd,
+			sizeof(struct av8100_cec_message_write_format_cmd));
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_READ_BACK:
+		memcpy(&config->cec_message_read_back_format,
+			&adev->config.hdmi_cec_message_read_back_cmd,
+			sizeof(struct av8100_cec_message_read_back_format_cmd));
+		break;
+
+	case AV8100_COMMAND_DENC:
+		memcpy(&config->denc_format, &adev->config.hdmi_denc_cmd,
+				sizeof(struct av8100_denc_format_cmd));
+		break;
+
+	case AV8100_COMMAND_HDMI:
+		memcpy(&config->hdmi_format, &adev->config.hdmi_cmd,
+				sizeof(struct av8100_hdmi_cmd));
+		break;
+
+	case AV8100_COMMAND_HDCP_SENDKEY:
+		memcpy(&config->hdcp_send_key_format,
+			&adev->config.hdmi_hdcp_send_key_cmd,
+			sizeof(struct av8100_hdcp_send_key_format_cmd));
+		break;
+
+	case AV8100_COMMAND_HDCP_MANAGEMENT:
+		memcpy(&config->hdcp_management_format,
+			&adev->config.hdmi_hdcp_management_format_cmd,
+			sizeof(struct av8100_hdcp_management_format_cmd));
+		break;
+
+	case AV8100_COMMAND_INFOFRAMES:
+		memcpy(&config->infoframes_format,
+			&adev->config.hdmi_infoframes_cmd,
+			sizeof(struct av8100_infoframes_format_cmd));
+		break;
+
+	case AV8100_COMMAND_EDID_SECTION_READBACK:
+		memcpy(&config->edid_section_readback_format,
+			&adev->config.hdmi_edid_section_readback_cmd,
+			sizeof(struct
+				av8100_edid_section_readback_format_cmd));
+		break;
+
+	case AV8100_COMMAND_PATTERNGENERATOR:
+		memcpy(&config->pattern_generator_format,
+			&adev->config.hdmi_pattern_generator_cmd,
+			sizeof(struct av8100_pattern_generator_format_cmd));
+		break;
+
+	case AV8100_COMMAND_FUSE_AES_KEY:
+		memcpy(&config->fuse_aes_key_format,
+			&adev->config.hdmi_fuse_aes_key_cmd,
+			sizeof(struct av8100_fuse_aes_key_format_cmd));
+		break;
+
+	default:
+		return -EINVAL;
+		break;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_conf_get);
+
+int av8100_conf_prep(enum av8100_command_type command_type,
+	union av8100_configuration *config)
+{
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!config || !adev)
+		return -EINVAL;
+
+	/* Put configuration data to the corresponding data struct depending
+	 * on command type */
+	switch (command_type) {
+	case AV8100_COMMAND_VIDEO_INPUT_FORMAT:
+		memcpy(&adev->config.hdmi_video_input_cmd,
+			&config->video_input_format,
+			sizeof(struct av8100_video_input_format_cmd));
+		break;
+
+	case AV8100_COMMAND_AUDIO_INPUT_FORMAT:
+		memcpy(&adev->config.hdmi_audio_input_cmd,
+			&config->audio_input_format,
+			sizeof(struct av8100_audio_input_format_cmd));
+		break;
+
+	case AV8100_COMMAND_VIDEO_OUTPUT_FORMAT:
+		memcpy(&adev->config.hdmi_video_output_cmd,
+			&config->video_output_format,
+			sizeof(struct av8100_video_output_format_cmd));
+
+		/* Set params that depend on video output */
+		av8100_config_video_output_dep(adev->config.
+			hdmi_video_output_cmd.video_output_cea_vesa);
+		break;
+
+	case AV8100_COMMAND_VIDEO_SCALING_FORMAT:
+		memcpy(&adev->config.hdmi_video_scaling_cmd,
+			&config->video_scaling_format,
+			sizeof(struct av8100_video_scaling_format_cmd));
+		break;
+
+	case AV8100_COMMAND_COLORSPACECONVERSION:
+		adev->config.color_transform = config->color_transform;
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_WRITE:
+		memcpy(&adev->config.hdmi_cec_message_write_cmd,
+			&config->cec_message_write_format,
+			sizeof(struct av8100_cec_message_write_format_cmd));
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_READ_BACK:
+		memcpy(&adev->config.hdmi_cec_message_read_back_cmd,
+			&config->cec_message_read_back_format,
+			sizeof(struct av8100_cec_message_read_back_format_cmd));
+		break;
+
+	case AV8100_COMMAND_DENC:
+		memcpy(&adev->config.hdmi_denc_cmd, &config->denc_format,
+				sizeof(struct av8100_denc_format_cmd));
+		break;
+
+	case AV8100_COMMAND_HDMI:
+		memcpy(&adev->config.hdmi_cmd, &config->hdmi_format,
+				sizeof(struct av8100_hdmi_cmd));
+		break;
+
+	case AV8100_COMMAND_HDCP_SENDKEY:
+		memcpy(&adev->config.hdmi_hdcp_send_key_cmd,
+			&config->hdcp_send_key_format,
+			sizeof(struct av8100_hdcp_send_key_format_cmd));
+		break;
+
+	case AV8100_COMMAND_HDCP_MANAGEMENT:
+		memcpy(&adev->config.hdmi_hdcp_management_format_cmd,
+			&config->hdcp_management_format,
+			sizeof(struct av8100_hdcp_management_format_cmd));
+		break;
+
+	case AV8100_COMMAND_INFOFRAMES:
+		memcpy(&adev->config.hdmi_infoframes_cmd,
+			&config->infoframes_format,
+			sizeof(struct av8100_infoframes_format_cmd));
+		break;
+
+	case AV8100_COMMAND_EDID_SECTION_READBACK:
+		memcpy(&adev->config.hdmi_edid_section_readback_cmd,
+			&config->edid_section_readback_format,
+			sizeof(struct
+				av8100_edid_section_readback_format_cmd));
+		break;
+
+	case AV8100_COMMAND_PATTERNGENERATOR:
+		memcpy(&adev->config.hdmi_pattern_generator_cmd,
+			&config->pattern_generator_format,
+			sizeof(struct av8100_pattern_generator_format_cmd));
+		break;
+
+	case AV8100_COMMAND_FUSE_AES_KEY:
+		memcpy(&adev->config.hdmi_fuse_aes_key_cmd,
+			&config->fuse_aes_key_format,
+			sizeof(struct av8100_fuse_aes_key_format_cmd));
+		break;
+
+	default:
+		return -EINVAL;
+		break;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(av8100_conf_prep);
+
+int av8100_conf_w(enum av8100_command_type command_type,
+	u8 *return_buffer_length,
+	u8 *return_buffer, enum interface_type if_type)
+{
+	int retval = 0;
+	u8 cmd_buffer[AV8100_COMMAND_MAX_LENGTH];
+	u32 cmd_length = 0;
+	struct i2c_client *i2c;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	if (return_buffer_length)
+		*return_buffer_length = 0;
+
+	i2c = adev->config.client;
+
+	memset(&cmd_buffer, 0x00, AV8100_COMMAND_MAX_LENGTH);
+
+#define PRNK_MODE(_m) dev_dbg(adev->dev, "cmd: " #_m "\n");
+
+	/* Fill the command buffer with configuration data */
+	switch (command_type) {
+	case AV8100_COMMAND_VIDEO_INPUT_FORMAT:
+		PRNK_MODE(AV8100_COMMAND_VIDEO_INPUT_FORMAT);
+		configuration_video_input_get(adev, cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_AUDIO_INPUT_FORMAT:
+		PRNK_MODE(AV8100_COMMAND_AUDIO_INPUT_FORMAT);
+		configuration_audio_input_get(adev, cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_VIDEO_OUTPUT_FORMAT:
+		PRNK_MODE(AV8100_COMMAND_VIDEO_OUTPUT_FORMAT);
+		configuration_video_output_get(adev, cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_VIDEO_SCALING_FORMAT:
+		PRNK_MODE(AV8100_COMMAND_VIDEO_SCALING_FORMAT);
+		configuration_video_scaling_get(adev, cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_COLORSPACECONVERSION:
+		PRNK_MODE(AV8100_COMMAND_COLORSPACECONVERSION);
+		configuration_colorspace_conversion_get(adev, cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_WRITE:
+		PRNK_MODE(AV8100_COMMAND_CEC_MESSAGE_WRITE);
+		configuration_cec_message_write_get(adev, cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_CEC_MESSAGE_READ_BACK:
+		PRNK_MODE(AV8100_COMMAND_CEC_MESSAGE_READ_BACK);
+		configuration_cec_message_read_get(cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_DENC:
+		PRNK_MODE(AV8100_COMMAND_DENC);
+		configuration_denc_get(adev, cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_HDMI:
+		PRNK_MODE(AV8100_COMMAND_HDMI);
+		configuration_hdmi_get(adev, cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_HDCP_SENDKEY:
+		PRNK_MODE(AV8100_COMMAND_HDCP_SENDKEY);
+		configuration_hdcp_sendkey_get(adev, cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_HDCP_MANAGEMENT:
+		PRNK_MODE(AV8100_COMMAND_HDCP_MANAGEMENT);
+		configuration_hdcp_management_get(adev, cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_INFOFRAMES:
+		PRNK_MODE(AV8100_COMMAND_INFOFRAMES);
+		configuration_infoframe_get(adev, cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_EDID_SECTION_READBACK:
+		PRNK_MODE(AV8100_COMMAND_EDID_SECTION_READBACK);
+		av8100_edid_section_readback_get(adev, cmd_buffer, &cmd_length);
+		break;
+
+	case AV8100_COMMAND_PATTERNGENERATOR:
+		PRNK_MODE(AV8100_COMMAND_PATTERNGENERATOR);
+		configuration_pattern_generator_get(adev, cmd_buffer,
+			&cmd_length);
+		break;
+
+	case AV8100_COMMAND_FUSE_AES_KEY:
+		PRNK_MODE(AV8100_COMMAND_FUSE_AES_KEY);
+		configuration_fuse_aes_key_get(adev, cmd_buffer, &cmd_length);
+		break;
+
+	default:
+		dev_dbg(adev->dev, "Invalid command type\n");
+		retval = -EFAULT;
+		break;
+	}
+
+	LOCK_AV8100_HW;
+
+	if (if_type == I2C_INTERFACE) {
+		int cnt = 0;
+		int cnt_max;
+
+		dev_dbg(adev->dev, "av8100_conf_w cmd_type:%02x length:%02x ",
+			command_type, cmd_length);
+		dev_dbg(adev->dev, "buffer: ");
+		while (cnt < cmd_length) {
+			dev_dbg(adev->dev, "%02x ", cmd_buffer[cnt]);
+			cnt++;
+		}
+
+		/* Write the command buffer */
+		retval = write_multi_byte(i2c,
+			AV8100_CMD_BUF_OFFSET, cmd_buffer, cmd_length);
+		if (retval) {
+			UNLOCK_AV8100_HW;
+			return retval;
+		}
+
+		/* Write the command */
+		retval = write_single_byte(i2c, AV8100_COMMAND_OFFSET,
+			command_type);
+		if (retval) {
+			UNLOCK_AV8100_HW;
+			return retval;
+		}
+
+
+		/* Get the first return byte */
+		mdelay(AV8100_WAITTIME_1MS);
+		cnt = 0;
+		cnt_max = sizeof(waittime_retry) / sizeof(waittime_retry[0]);
+		retval = get_command_return_first(i2c, command_type);
+		while (retval && (cnt < cnt_max)) {
+			mdelay(waittime_retry[cnt]);
+			retval = get_command_return_first(i2c, command_type);
+			cnt++;
+		}
+		dev_dbg(adev->dev, "first return cnt:%d\n", cnt);
+
+		if (retval) {
+			UNLOCK_AV8100_HW;
+			return retval;
+		}
+
+		retval = get_command_return_data(i2c, command_type, cmd_buffer,
+			return_buffer_length, return_buffer);
+	} else if (if_type == DSI_INTERFACE) {
+		/* TODO */
+	} else {
+		retval = -EINVAL;
+		dev_dbg(adev->dev, "Invalid command type\n");
+	}
+
+	if (command_type == AV8100_COMMAND_HDMI) {
+		adev->status.hdmi_on = ((adev->config.hdmi_cmd.
+			hdmi_mode == AV8100_HDMI_ON) &&
+			(adev->config.hdmi_cmd.hdmi_format == AV8100_HDMI));
+	}
+
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_conf_w);
+
+int av8100_conf_w_raw(enum av8100_command_type command_type,
+	u8 buffer_length,
+	u8 *buffer,
+	u8 *return_buffer_length,
+	u8 *return_buffer)
+{
+	int retval = 0;
+	struct i2c_client *i2c;
+	int cnt;
+	int cnt_max;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	if (av8100_status_get().av8100_state <= AV8100_OPMODE_SHUTDOWN)
+		return -EINVAL;
+
+	LOCK_AV8100_HW;
+
+	if (return_buffer_length)
+		*return_buffer_length = 0;
+
+	i2c = adev->config.client;
+
+	/* Write the command buffer */
+	retval = write_multi_byte(i2c,
+		AV8100_CMD_BUF_OFFSET, buffer, buffer_length);
+	if (retval)
+		goto av8100_conf_w_raw_out;
+
+	/* Write the command */
+	retval = write_single_byte(i2c, AV8100_COMMAND_OFFSET,
+		command_type);
+	if (retval)
+		goto av8100_conf_w_raw_out;
+
+
+	/* Get the first return byte */
+	mdelay(AV8100_WAITTIME_1MS);
+	cnt = 0;
+	cnt_max = sizeof(waittime_retry) / sizeof(waittime_retry[0]);
+	retval = get_command_return_first(i2c, command_type);
+	while (retval && (cnt < cnt_max)) {
+		mdelay(waittime_retry[cnt]);
+		retval = get_command_return_first(i2c, command_type);
+		cnt++;
+	}
+	dev_dbg(adev->dev, "first return cnt:%d\n", cnt);
+	if (retval)
+		goto av8100_conf_w_raw_out;
+
+	retval = get_command_return_data(i2c, command_type, buffer,
+		return_buffer_length, return_buffer);
+
+av8100_conf_w_raw_out:
+	UNLOCK_AV8100_HW;
+	return retval;
+}
+EXPORT_SYMBOL(av8100_conf_w_raw);
+
+struct av8100_status av8100_status_get(void)
+{
+	struct av8100_status status = {0};
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (adev)
+		return adev->status;
+	else
+		return status;
+}
+EXPORT_SYMBOL(av8100_status_get);
+
+enum av8100_output_CEA_VESA av8100_video_output_format_get(int xres,
+	int yres,
+	int htot,
+	int vtot,
+	int pixelclk,
+	bool interlaced)
+{
+	enum av8100_output_CEA_VESA index = 1;
+	int yres_div = !interlaced ? 1 : 2;
+	int hres_div = 1;
+	long freq1;
+	long freq2;
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	/*
+	* 720_576_I need a divider for hact and htot since
+	* these params need to be twice as large as expected in av8100_all_cea,
+	* which is used as input parameter to video input config.
+	*/
+	if ((xres == 720) && (yres == 576) && (interlaced == true))
+		hres_div = 2;
+
+	freq1 = 1000000 / htot * 1000000 / vtot / pixelclk + 1;
+	while (index < sizeof(av8100_all_cea)/sizeof(struct av8100_cea)) {
+		freq2 = av8100_all_cea[index].frequence /
+			av8100_all_cea[index].htotale /
+			av8100_all_cea[index].vtotale;
+
+		dev_dbg(adev->dev, "freq1:%ld freq2:%ld\n", freq1, freq2);
+		if ((xres == av8100_all_cea[index].hactive / hres_div) &&
+			(yres == av8100_all_cea[index].vactive * yres_div) &&
+			(htot == av8100_all_cea[index].htotale / hres_div) &&
+			(vtot == av8100_all_cea[index].vtotale) &&
+			(abs(freq1 - freq2) < 2)) {
+			goto av8100_video_output_format_get_out;
+		}
+		index++;
+	}
+
+av8100_video_output_format_get_out:
+	dev_dbg(adev->dev, "av8100_video_output_format_get %d %d %d %d %d\n",
+		xres, yres, htot, vtot, index);
+	return index;
+}
+EXPORT_SYMBOL(av8100_video_output_format_get);
+
+void av8100_hdmi_event_cb_set(void (*hdmi_ev_cb)(enum av8100_hdmi_event))
+{
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (adev)
+		adev->params.hdmi_ev_cb = hdmi_ev_cb;
+}
+EXPORT_SYMBOL(av8100_hdmi_event_cb_set);
+
+u8 av8100_ver_get(void)
+{
+	struct av8100_device *adev;
+
+	adev = devnr_to_adev(AV8100_DEVNR_DEFAULT);
+	if (!adev)
+		return -EINVAL;
+
+	return adev->chip_version;
+}
+EXPORT_SYMBOL(av8100_ver_get);
+
+static const struct color_conversion_cmd *get_color_transform_cmd(
+			struct av8100_device *adev,
+			enum av8100_color_transform transform)
+{
+	const struct color_conversion_cmd *result;
+
+	switch (transform) {
+	case AV8100_COLOR_TRANSFORM_INDENTITY:
+		result = &col_trans_identity;
+		break;
+	case AV8100_COLOR_TRANSFORM_INDENTITY_CLAMP_YUV:
+		result = &col_trans_identity_clamp_yuv;
+		break;
+	case AV8100_COLOR_TRANSFORM_YUV_TO_RGB:
+		if (adev->chip_version == AV8100_CHIPVER_1)
+			result = &col_trans_yuv_to_rgb_v1;
+		else
+			result = &col_trans_yuv_to_rgb_v2;
+		break;
+	case AV8100_COLOR_TRANSFORM_YUV_TO_DENC:
+		result = &col_trans_yuv_to_denc;
+		break;
+	case AV8100_COLOR_TRANSFORM_RGB_TO_DENC:
+		result = &col_trans_rgb_to_denc;
+		break;
+	default:
+		dev_warn(adev->dev, "Unknown color space transform\n");
+		result = &col_trans_identity;
+		break;
+	}
+	return result;
+}
+
+static int av8100_open(struct inode *inode, struct file *filp)
+{
+	pr_debug("%s\n", __func__);
+	return 0;
+}
+
+static int av8100_release(struct inode *inode, struct file *filp)
+{
+	pr_debug("%s\n", __func__);
+	return 0;
+}
+
+static long av8100_ioctl(struct file *file,
+				unsigned int cmd, unsigned long arg)
+{
+	return 0;
+}
+
+int av8100_device_register(struct av8100_device *adev)
+{
+	adev->miscdev.minor = MISC_DYNAMIC_MINOR;
+	adev->miscdev.name = "av8100";
+	adev->miscdev.fops = &av8100_fops;
+
+	if (misc_register(&adev->miscdev)) {
+		pr_err("av8100 misc_register failed\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+int av8100_init_device(struct av8100_device *adev, struct device *dev)
+{
+	adev->dev = dev;
+
+	if (av8100_config_init(adev)) {
+		dev_info(dev, "av8100_config_init failed\n");
+		return -EFAULT;
+	}
+
+	if (av8100_params_init(adev)) {
+		dev_info(dev, "av8100_params_init failed\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static int __devinit av8100_probe(struct i2c_client *i2c_client,
+	const struct i2c_device_id *id)
+{
+	int ret = 0;
+	struct av8100_platform_data *pdata = i2c_client->dev.platform_data;
+	struct device *dev;
+	struct av8100_device *adev;
+
+	dev = &i2c_client->dev;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	/* Allocate device data */
+	adev = kzalloc(sizeof(struct av8100_device), GFP_KERNEL);
+	if (!adev) {
+		dev_info(dev, "%s: Alloc failure\n", __func__);
+		return -ENOMEM;
+	}
+
+	/* Add to list */
+	list_add_tail(&adev->list, &av8100_device_list);
+
+	av8100_device_register(adev);
+
+	av8100_init_device(adev, dev);
+
+	av8100_set_state(adev, AV8100_OPMODE_UNDEFINED);
+
+	if (!i2c_check_functionality(i2c_client->adapter,
+			I2C_FUNC_SMBUS_BYTE_DATA |
+			I2C_FUNC_SMBUS_READ_WORD_DATA)) {
+		ret = -ENODEV;
+		dev_info(dev, "av8100 i2c_check_functionality failed\n");
+		goto err1;
+	}
+
+	init_waitqueue_head(&adev->event);
+
+	adev->config.client = i2c_client;
+	adev->config.id = (struct i2c_device_id *) id;
+	i2c_set_clientdata(i2c_client, &adev->config);
+
+	kthread_run(av8100_thread, adev, "av8100_thread");
+
+	/* Get regulator resource */
+	if (pdata->regulator_pwr_id) {
+		adev->params.regulator_pwr = regulator_get(dev,
+				pdata->regulator_pwr_id);
+		if (IS_ERR(adev->params.regulator_pwr)) {
+			ret = PTR_ERR(adev->params.regulator_pwr);
+			dev_warn(dev,
+				"%s: Failed to get regulator '%s'\n",
+				__func__, pdata->regulator_pwr_id);
+			adev->params.regulator_pwr = NULL;
+			goto err1;
+		}
+	}
+
+	/* Get clock resource */
+	if (pdata->inputclk_id) {
+		adev->params.inputclk = clk_get(NULL, pdata->inputclk_id);
+		if (IS_ERR(adev->params.inputclk)) {
+			adev->params.inputclk = NULL;
+			dev_warn(dev, "%s: Failed to get clock '%s'\n",
+					__func__, pdata->inputclk_id);
+		}
+	}
+
+	av8100_set_state(adev, AV8100_OPMODE_SHUTDOWN);
+
+
+	if (av8100_powerup1(adev)) {
+		dev_err(adev->dev, "av8100_powerup1 fail\n");
+		ret = -EFAULT;
+		goto err1;
+	}
+
+	/* Obtain the chip version */
+	ret = av8100_reg_stby_pend_int_r(NULL, NULL, NULL,
+					 &adev->chip_version);
+	if (ret) {
+		dev_err(adev->dev, "Failed to read chip version\n");
+		goto err2;
+	}
+
+	dev_info(adev->dev, "chip version:%d\n", adev->chip_version);
+
+	switch (adev->chip_version) {
+	case AV8100_CHIPVER_1:
+	case AV8100_CHIPVER_2:
+		break;
+
+	default:
+		dev_err(adev->dev, "Unsupported chip version:%d\n",
+				adev->chip_version);
+		ret = -EINVAL;
+		goto err2;
+		break;
+	}
+err2:
+	(void) av8100_powerdown();
+err1:
+	return ret;
+}
+
+static int __devexit av8100_remove(struct i2c_client *i2c_client)
+{
+	struct av8100_device *adev;
+
+	adev = dev_to_adev(&i2c_client->dev);
+	if (!adev)
+		return -EFAULT;
+
+	dev_dbg(adev->dev, "%s\n", __func__);
+
+	if (adev->params.inputclk)
+		clk_put(adev->params.inputclk);
+
+	/* Release regulator resource */
+	if (adev->params.regulator_pwr)
+		regulator_put(adev->params.regulator_pwr);
+
+	misc_deregister(&adev->miscdev);
+
+	/* Remove from list */
+	list_del(&adev->list);
+
+	/* Free device data */
+	kfree(adev);
+
+	return 0;
+}
+
+int av8100_init(void)
+{
+	pr_debug("%s\n", __func__);
+
+	if (i2c_add_driver(&av8100_driver)) {
+		pr_err("av8100 i2c_add_driver failed\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+module_init(av8100_init);
+
+void av8100_exit(void)
+{
+	pr_debug("%s\n", __func__);
+
+	i2c_del_driver(&av8100_driver);
+}
+module_exit(av8100_exit);
+
+MODULE_AUTHOR("Per Persson <per.xb.persson@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson hdmi display driver");
diff --git a/drivers/video/av8100/av8100_regs.h b/drivers/video/av8100/av8100_regs.h
new file mode 100644
index 00000000000..6ed9000987a
--- /dev/null
+++ b/drivers/video/av8100/av8100_regs.h
@@ -0,0 +1,346 @@
+
+#define AV8100_VAL2REG(__reg, __fld, __val) \
+	(((__val) << __reg##_##__fld##_SHIFT) & __reg##_##__fld##_MASK)
+#define AV8100_REG2VAL(__reg, __fld, __val) \
+	(((__val) & __reg##_##__fld##_MASK) >> __reg##_##__fld##_SHIFT)
+
+#define AV8100_STANDBY 0x00000000
+#define AV8100_STANDBY_CPD_SHIFT 0
+#define AV8100_STANDBY_CPD_MASK 0x00000001
+#define AV8100_STANDBY_CPD_HIGH 1
+#define AV8100_STANDBY_CPD_LOW 0
+#define AV8100_STANDBY_CPD(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, CPD, __x)
+#define AV8100_STANDBY_CPD_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, CPD, __x)
+#define AV8100_STANDBY_STBY_SHIFT 1
+#define AV8100_STANDBY_STBY_MASK 0x00000002
+#define AV8100_STANDBY_STBY_HIGH 1
+#define AV8100_STANDBY_STBY_LOW 0
+#define AV8100_STANDBY_STBY(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, STBY, __x)
+#define AV8100_STANDBY_STBY_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, STBY, __x)
+#define AV8100_STANDBY_HPDS_SHIFT 2
+#define AV8100_STANDBY_HPDS_MASK 0x00000004
+#define AV8100_STANDBY_HPDS(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, HPDS, __x)
+#define AV8100_STANDBY_HPDS_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, HPDS, __x)
+#define AV8100_STANDBY_CPDS_SHIFT 3
+#define AV8100_STANDBY_CPDS_MASK 0x00000008
+#define AV8100_STANDBY_CPDS(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, CPDS, __x)
+#define AV8100_STANDBY_CPDS_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, CPDS, __x)
+#define AV8100_STANDBY_MCLKRNG_SHIFT 4
+#define AV8100_STANDBY_MCLKRNG_MASK 0x000000F0
+#define AV8100_STANDBY_MCLKRNG(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY, MCLKRNG, __x)
+#define AV8100_STANDBY_MCLKRNG_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY, MCLKRNG, __x)
+#define AV8100_HDMI_5_VOLT_TIME 0x00000001
+#define AV8100_HDMI_5_VOLT_TIME_OFF_TIME_SHIFT 0
+#define AV8100_HDMI_5_VOLT_TIME_OFF_TIME_MASK 0x0000001F
+#define AV8100_HDMI_5_VOLT_TIME_OFF_TIME(__x) \
+	AV8100_VAL2REG(AV8100_HDMI_5_VOLT_TIME, OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_OFF_TIME_GET(__x) \
+	AV8100_REG2VAL(AV8100_HDMI_5_VOLT_TIME, OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME_SHIFT 0
+#define AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME_MASK 0x00000003
+#define AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME(__x) \
+	AV8100_VAL2REG(AV8100_HDMI_5_VOLT_TIME, DAC_OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_DAC_OFF_TIME_GET(__x) \
+	AV8100_REG2VAL(AV8100_HDMI_5_VOLT_TIME, DAC_OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME_SHIFT 2
+#define AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME_MASK 0x0000001C
+#define AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME(__x) \
+	AV8100_VAL2REG(AV8100_HDMI_5_VOLT_TIME, SU_OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_SU_OFF_TIME_GET(__x) \
+	AV8100_REG2VAL(AV8100_HDMI_5_VOLT_TIME, SU_OFF_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_ON_TIME_SHIFT 5
+#define AV8100_HDMI_5_VOLT_TIME_ON_TIME_MASK 0x000000E0
+#define AV8100_HDMI_5_VOLT_TIME_ON_TIME(__x) \
+	AV8100_VAL2REG(AV8100_HDMI_5_VOLT_TIME, ON_TIME, __x)
+#define AV8100_HDMI_5_VOLT_TIME_ON_TIME_GET(__x) \
+	AV8100_REG2VAL(AV8100_HDMI_5_VOLT_TIME, ON_TIME, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK 0x00000002
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_SHIFT 0
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_MASK 0x00000001
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_HIGH 1
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_LOW 0
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_INTERRUPT_MASK, HPDM, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_HPDM_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_INTERRUPT_MASK, HPDM, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_SHIFT 1
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_MASK 0x00000002
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_HIGH 1
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_LOW 0
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_INTERRUPT_MASK, CPDM, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_CPDM_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_INTERRUPT_MASK, CPDM, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_SHIFT 2
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_MASK 0x0000000C
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_INPUT 0x00
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_ALT 0x01
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_OUTPUT0 0x02
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_OUTPUT1 0x03
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_INTERRUPT_MASK, STBYGPIOCFG, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_STBYGPIOCFG_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_INTERRUPT_MASK, STBYGPIOCFG, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_SHIFT 7
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_MASK 0x00000080
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_HIGH 1
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_LOW 0
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_INTERRUPT_MASK, IPOL, __x)
+#define AV8100_STANDBY_INTERRUPT_MASK_IPOL_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_INTERRUPT_MASK, IPOL, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT 0x00000003
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_SHIFT 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_MASK 0x00000001
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_HIGH 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_LOW 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, HPDI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_HPDI_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_PENDING_INTERRUPT, HPDI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_SHIFT 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_MASK 0x00000002
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_HIGH 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_LOW 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, CPDI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_CPDI_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_PENDING_INTERRUPT, CPDI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_SHIFT 2
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_MASK 0x00000004
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_HIGH 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_LOW 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, ONI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_ONI_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_PENDING_INTERRUPT, ONI, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_SID_SHIFT 4
+#define AV8100_STANDBY_PENDING_INTERRUPT_SID_MASK 0x000000F0
+#define AV8100_STANDBY_PENDING_INTERRUPT_SID(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, SID, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_SID_GET(__x) \
+	AV8100_REG2VAL(AV8100_STANDBY_PENDING_INTERRUPT, SID, __x)
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_SHIFT 6
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_MASK 0x00000040
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_HIGH 1
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG_LOW 0
+#define AV8100_STANDBY_PENDING_INTERRUPT_BPDIG(__x) \
+	AV8100_VAL2REG(AV8100_STANDBY_PENDING_INTERRUPT, BPDIG, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK 0x00000004
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_SHIFT 0
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_MASK 0x00000001
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, EOCM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_EOCM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, EOCM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_SHIFT 1
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_MASK 0x00000002
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, VSIM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_VSIM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, VSIM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_SHIFT 2
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_MASK 0x00000004
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, VSOM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_VSOM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, VSOM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_SHIFT 3
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_MASK 0x00000008
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, CECM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_CECM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, CECM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_SHIFT 4
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_MASK 0x00000010
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, HDCPM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_HDCPM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, HDCPM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_SHIFT 5
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_MASK 0x00000020
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, UOVBM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_UOVBM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, UOVBM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_SHIFT 6
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_MASK 0x00000040
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_HIGH 1
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_LOW 0
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT_MASK, TEM, __x)
+#define AV8100_GENERAL_INTERRUPT_MASK_TEM_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT_MASK, TEM, __x)
+#define AV8100_GENERAL_INTERRUPT 0x00000005
+#define AV8100_GENERAL_INTERRUPT_EOCI_SHIFT 0
+#define AV8100_GENERAL_INTERRUPT_EOCI_MASK 0x00000001
+#define AV8100_GENERAL_INTERRUPT_EOCI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, EOCI, __x)
+#define AV8100_GENERAL_INTERRUPT_EOCI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, EOCI, __x)
+#define AV8100_GENERAL_INTERRUPT_VSII_SHIFT 1
+#define AV8100_GENERAL_INTERRUPT_VSII_MASK 0x00000002
+#define AV8100_GENERAL_INTERRUPT_VSII(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, VSII, __x)
+#define AV8100_GENERAL_INTERRUPT_VSII_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, VSII, __x)
+#define AV8100_GENERAL_INTERRUPT_VSOI_SHIFT 2
+#define AV8100_GENERAL_INTERRUPT_VSOI_MASK 0x00000004
+#define AV8100_GENERAL_INTERRUPT_VSOI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, VSOI, __x)
+#define AV8100_GENERAL_INTERRUPT_VSOI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, VSOI, __x)
+#define AV8100_GENERAL_INTERRUPT_CECI_SHIFT 3
+#define AV8100_GENERAL_INTERRUPT_CECI_MASK 0x00000008
+#define AV8100_GENERAL_INTERRUPT_CECI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, CECI, __x)
+#define AV8100_GENERAL_INTERRUPT_CECI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, CECI, __x)
+#define AV8100_GENERAL_INTERRUPT_HDCPI_SHIFT 4
+#define AV8100_GENERAL_INTERRUPT_HDCPI_MASK 0x00000010
+#define AV8100_GENERAL_INTERRUPT_HDCPI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, HDCPI, __x)
+#define AV8100_GENERAL_INTERRUPT_HDCPI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, HDCPI, __x)
+#define AV8100_GENERAL_INTERRUPT_UOVBI_SHIFT 5
+#define AV8100_GENERAL_INTERRUPT_UOVBI_MASK 0x00000020
+#define AV8100_GENERAL_INTERRUPT_UOVBI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, UOVBI, __x)
+#define AV8100_GENERAL_INTERRUPT_UOVBI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, UOVBI, __x)
+#define AV8100_GENERAL_INTERRUPT_TEI_SHIFT 6
+#define AV8100_GENERAL_INTERRUPT_TEI_MASK 0x00000040
+#define AV8100_GENERAL_INTERRUPT_TEI(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_INTERRUPT, TEI, __x)
+#define AV8100_GENERAL_INTERRUPT_TEI_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_INTERRUPT, TEI, __x)
+#define AV8100_GENERAL_STATUS 0x00000006
+#define AV8100_GENERAL_STATUS_CECTXERR_SHIFT 0
+#define AV8100_GENERAL_STATUS_CECTXERR_MASK 0x00000001
+#define AV8100_GENERAL_STATUS_CECTXERR_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, CECTXERR, __x)
+#define AV8100_GENERAL_STATUS_CECREC_SHIFT 1
+#define AV8100_GENERAL_STATUS_CECREC_MASK 0x00000002
+#define AV8100_GENERAL_STATUS_CECREC_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, CECREC, __x)
+#define AV8100_GENERAL_STATUS_CECTRX_SHIFT 2
+#define AV8100_GENERAL_STATUS_CECTRX_MASK 0x00000004
+#define AV8100_GENERAL_STATUS_CECTRX_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, CECTRX, __x)
+#define AV8100_GENERAL_STATUS_UC_SHIFT 3
+#define AV8100_GENERAL_STATUS_UC_MASK 0x00000008
+#define AV8100_GENERAL_STATUS_UC_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, UC, __x)
+#define AV8100_GENERAL_STATUS_ONUVB_SHIFT 4
+#define AV8100_GENERAL_STATUS_ONUVB_MASK 0x00000010
+#define AV8100_GENERAL_STATUS_ONUVB_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, ONUVB, __x)
+#define AV8100_GENERAL_STATUS_HDCPS_SHIFT 5
+#define AV8100_GENERAL_STATUS_HDCPS_MASK 0x000000E0
+#define AV8100_GENERAL_STATUS_HDCPS_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_STATUS, HDCPS, __x)
+#define AV8100_GPIO_CONFIGURATION 0x00000007
+#define AV8100_GPIO_CONFIGURATION_DAT3DIR_SHIFT 0
+#define AV8100_GPIO_CONFIGURATION_DAT3DIR_MASK 0x00000001
+#define AV8100_GPIO_CONFIGURATION_DAT3DIR(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT3DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT3DIR_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT3DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT3VAL_SHIFT 1
+#define AV8100_GPIO_CONFIGURATION_DAT3VAL_MASK 0x00000002
+#define AV8100_GPIO_CONFIGURATION_DAT3VAL(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT3VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT3VAL_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT3VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT2DIR_SHIFT 2
+#define AV8100_GPIO_CONFIGURATION_DAT2DIR_MASK 0x00000004
+#define AV8100_GPIO_CONFIGURATION_DAT2DIR(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT2DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT2DIR_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT2DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT2VAL_SHIFT 3
+#define AV8100_GPIO_CONFIGURATION_DAT2VAL_MASK 0x00000008
+#define AV8100_GPIO_CONFIGURATION_DAT2VAL(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT2VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT2VAL_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT2VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT1DIR_SHIFT 4
+#define AV8100_GPIO_CONFIGURATION_DAT1DIR_MASK 0x00000010
+#define AV8100_GPIO_CONFIGURATION_DAT1DIR(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT1DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT1DIR_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT1DIR, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT1VAL_SHIFT 5
+#define AV8100_GPIO_CONFIGURATION_DAT1VAL_MASK 0x00000020
+#define AV8100_GPIO_CONFIGURATION_DAT1VAL(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, DAT1VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_DAT1VAL_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, DAT1VAL, __x)
+#define AV8100_GPIO_CONFIGURATION_UCDBG_SHIFT 6
+#define AV8100_GPIO_CONFIGURATION_UCDBG_MASK 0x00000040
+#define AV8100_GPIO_CONFIGURATION_UCDBG(__x) \
+	AV8100_VAL2REG(AV8100_GPIO_CONFIGURATION, UCDBG, __x)
+#define AV8100_GPIO_CONFIGURATION_UCDBG_GET(__x) \
+	AV8100_REG2VAL(AV8100_GPIO_CONFIGURATION, UCDBG, __x)
+#define AV8100_GENERAL_CONTROL 0x00000008
+#define AV8100_GENERAL_CONTROL_FDL_SHIFT 4
+#define AV8100_GENERAL_CONTROL_FDL_MASK 0x00000010
+#define AV8100_GENERAL_CONTROL_FDL_HIGH 1
+#define AV8100_GENERAL_CONTROL_FDL_LOW 0
+#define AV8100_GENERAL_CONTROL_FDL(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_CONTROL, FDL, __x)
+#define AV8100_GENERAL_CONTROL_FDL_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_CONTROL, FDL, __x)
+#define AV8100_GENERAL_CONTROL_HLD_SHIFT 5
+#define AV8100_GENERAL_CONTROL_HLD_MASK 0x00000020
+#define AV8100_GENERAL_CONTROL_HLD_HIGH 1
+#define AV8100_GENERAL_CONTROL_HLD_LOW 0
+#define AV8100_GENERAL_CONTROL_HLD(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_CONTROL, HLD, __x)
+#define AV8100_GENERAL_CONTROL_HLD_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_CONTROL, HLD, __x)
+#define AV8100_GENERAL_CONTROL_WA_SHIFT 6
+#define AV8100_GENERAL_CONTROL_WA_MASK 0x00000040
+#define AV8100_GENERAL_CONTROL_WA_HIGH 1
+#define AV8100_GENERAL_CONTROL_WA_LOW 0
+#define AV8100_GENERAL_CONTROL_WA(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_CONTROL, WA, __x)
+#define AV8100_GENERAL_CONTROL_WA_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_CONTROL, WA, __x)
+#define AV8100_GENERAL_CONTROL_RA_SHIFT 7
+#define AV8100_GENERAL_CONTROL_RA_MASK 0x00000080
+#define AV8100_GENERAL_CONTROL_RA_HIGH 1
+#define AV8100_GENERAL_CONTROL_RA_LOW 0
+#define AV8100_GENERAL_CONTROL_RA(__x) \
+	AV8100_VAL2REG(AV8100_GENERAL_CONTROL, RA, __x)
+#define AV8100_GENERAL_CONTROL_RA_GET(__x) \
+	AV8100_REG2VAL(AV8100_GENERAL_CONTROL, RA, __x)
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY 0x0000000F
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY_SHIFT 0
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY_MASK 0x000000FF
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY(__x) \
+	AV8100_VAL2REG(AV8100_FIRMWARE_DOWNLOAD_ENTRY, MBYTE_CODE_ENTRY, __x)
+#define AV8100_FIRMWARE_DOWNLOAD_ENTRY_MBYTE_CODE_ENTRY_GET(__x) \
+	AV8100_REG2VAL(AV8100_FIRMWARE_DOWNLOAD_ENTRY, MBYTE_CODE_ENTRY, __x)
diff --git a/drivers/video/av8100/hdmi.c b/drivers/video/av8100/hdmi.c
new file mode 100644
index 00000000000..3159c4446f1
--- /dev/null
+++ b/drivers/video/av8100/hdmi.c
@@ -0,0 +1,2479 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson HDMI driver
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/miscdevice.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/ioctl.h>
+#include <linux/uaccess.h>
+#include <video/av8100.h>
+#include <video/hdmi.h>
+#include <linux/poll.h>
+#include <linux/mutex.h>
+#include <linux/ctype.h>
+#include "hdmi_loc.h"
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+#include <linux/list.h>
+
+#define SYSFS_EVENT_FILENAME "evread"
+#define HDMI_DEVNR_DEFAULT	0
+
+DEFINE_MUTEX(hdmi_events_mutex);
+#define LOCK_HDMI_EVENTS mutex_lock(&hdmi_events_mutex)
+#define UNLOCK_HDMI_EVENTS mutex_unlock(&hdmi_events_mutex)
+#define EVENTS_MASK 0xFF
+
+struct hdmi_device {
+	struct list_head	list;
+	struct miscdevice	miscdev;
+	struct device		*dev;
+	struct hdmi_sysfs_data	sysfs_data;
+	int			events;
+	int			events_mask;
+	wait_queue_head_t	event_wq;
+	bool			events_received;
+	int			devnr;
+};
+
+/* List of devices */
+static LIST_HEAD(hdmi_device_list);
+
+static ssize_t store_storeastext(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_plugdeten(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_edidread(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_edidread(struct device *dev, struct device_attribute *attr,
+				char *buf);
+static ssize_t store_ceceven(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_cecread(struct device *dev, struct device_attribute *attr,
+				char *buf);
+static ssize_t store_cecsend(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_infofrsend(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_hdcpeven(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_hdcpchkaesotp(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_hdcpfuseaes(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_hdcpfuseaes(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_hdcploadaes(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_hdcploadaes(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_hdcpauthencr(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_hdcpauthencr(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t show_hdcpstateget(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t show_evread(struct device *dev, struct device_attribute *attr,
+				char *buf);
+static ssize_t store_evclr(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_audiocfg(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_plugstatus(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_poweronoff(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_poweronoff(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_evwakeup(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+
+static const struct device_attribute hdmi_sysfs_attrs[] = {
+	__ATTR(storeastext, S_IWUSR, NULL, store_storeastext),
+	__ATTR(plugdeten, S_IWUSR, NULL, store_plugdeten),
+	__ATTR(edidread, S_IRUGO | S_IWUSR, show_edidread, store_edidread),
+	__ATTR(ceceven, S_IWUSR, NULL, store_ceceven),
+	__ATTR(cecread, S_IRUGO, show_cecread, NULL),
+	__ATTR(cecsend, S_IWUSR, NULL, store_cecsend),
+	__ATTR(infofrsend, S_IWUSR, NULL, store_infofrsend),
+	__ATTR(hdcpeven, S_IWUSR, NULL, store_hdcpeven),
+	__ATTR(hdcpchkaesotp, S_IRUGO, show_hdcpchkaesotp, NULL),
+	__ATTR(hdcpfuseaes, S_IRUGO | S_IWUSR, show_hdcpfuseaes,
+			store_hdcpfuseaes),
+	__ATTR(hdcploadaes, S_IRUGO | S_IWUSR, show_hdcploadaes,
+			store_hdcploadaes),
+	__ATTR(hdcpauthencr, S_IRUGO | S_IWUSR, show_hdcpauthencr,
+			store_hdcpauthencr),
+	__ATTR(hdcpstateget, S_IRUGO, show_hdcpstateget, NULL),
+	__ATTR(evread, S_IRUGO, show_evread, NULL),
+	__ATTR(evclr, S_IWUSR, NULL, store_evclr),
+	__ATTR(audiocfg, S_IWUSR, NULL, store_audiocfg),
+	__ATTR(plugstatus, S_IRUGO, show_plugstatus, NULL),
+	__ATTR(poweronoff, S_IRUGO | S_IWUSR, show_poweronoff,
+			store_poweronoff),
+	__ATTR(evwakeup, S_IWUSR, NULL, store_evwakeup),
+	__ATTR_NULL
+};
+
+/* Hex to int conversion */
+static unsigned int htoi(const char *ptr)
+{
+	unsigned int value = 0;
+	char ch;
+
+	if (!ptr)
+		return 0;
+
+	ch = *ptr;
+	if (isdigit(ch))
+		value = ch - '0';
+	else
+		value = toupper(ch) - 'A' + 10;
+
+	value <<= 4;
+	ch = *(++ptr);
+
+	if (isdigit(ch))
+		value += ch - '0';
+	else
+		value += toupper(ch) - 'A' + 10;
+
+	return value;
+}
+
+static struct hdmi_device *dev_to_hdev(struct device *dev)
+{
+	/* Get device from list of devices */
+	struct list_head *element;
+	struct hdmi_device *hdmi_dev;
+	int cnt = 0;
+
+	list_for_each(element, &hdmi_device_list) {
+		hdmi_dev = list_entry(element, struct hdmi_device, list);
+		if (hdmi_dev->dev == dev)
+			return hdmi_dev;
+		cnt++;
+	}
+
+	return NULL;
+}
+
+static struct hdmi_device *devnr_to_hdev(int devnr)
+{
+	/* Get device from list of devices */
+	struct list_head *element;
+	struct hdmi_device *hdmi_dev;
+	int cnt = 0;
+
+	list_for_each(element, &hdmi_device_list) {
+		hdmi_dev = list_entry(element, struct hdmi_device, list);
+		if (cnt == devnr)
+			return hdmi_dev;
+		cnt++;
+	}
+
+	return NULL;
+}
+
+static int event_enable(struct hdmi_device *hdev, bool enable,
+		enum hdmi_event ev)
+{
+	struct kobject *kobj = &hdev->dev->kobj;
+
+	dev_dbg(hdev->dev, "enable_event %d %02x\n", enable, ev);
+	if (enable)
+		hdev->events_mask |= ev;
+	else
+		hdev->events_mask &= ~ev;
+
+	if (hdev->events & ev) {
+		/* Report pending event */
+		/* Wake up application waiting for event via call to poll() */
+		sysfs_notify(kobj, NULL, SYSFS_EVENT_FILENAME);
+
+		LOCK_HDMI_EVENTS;
+		hdev->events_received = true;
+		UNLOCK_HDMI_EVENTS;
+
+		wake_up_interruptible(&hdev->event_wq);
+	}
+
+	return 0;
+}
+
+static int plugdeten(struct hdmi_device *hdev, struct plug_detect *pldet)
+{
+	struct av8100_status status;
+	u8 denc_off_time = 0;
+	int retval;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	event_enable(hdev, pldet->hdmi_detect_enable != 0,
+		HDMI_EVENT_HDMI_PLUGIN);
+	event_enable(hdev, pldet->hdmi_detect_enable != 0,
+		HDMI_EVENT_HDMI_PLUGOUT);
+
+	av8100_reg_hdmi_5_volt_time_r(&denc_off_time, NULL, NULL);
+
+	retval = av8100_reg_hdmi_5_volt_time_w(
+			denc_off_time,
+			pldet->hdmi_off_time,
+			pldet->on_time);
+
+	if (retval) {
+		dev_err(hdev->dev, "Failed to write the value to av8100 "
+			"register\n");
+		return -EFAULT;
+	}
+
+	return retval;
+}
+
+static int edidread(struct hdmi_device *hdev, struct edid_read *edidread,
+			u8 *len, u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.edid_section_readback_format.address = edidread->address;
+	config.edid_section_readback_format.block_number = edidread->block_nr;
+
+	dev_dbg(hdev->dev, "addr:%0x blnr:%0x",
+		config.edid_section_readback_format.address,
+		config.edid_section_readback_format.block_number);
+
+	if (av8100_conf_prep(AV8100_COMMAND_EDID_SECTION_READBACK,
+		&config) != 0) {
+		dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_EDID_SECTION_READBACK,
+		len, data, I2C_INTERFACE) != 0) {
+		dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(hdev->dev, "len:%0x\n", *len);
+
+	return 0;
+}
+
+static int cecread(struct hdmi_device *hdev, u8 *src, u8 *dest, u8 *data_len,
+			u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+	u8 buf_len;
+	u8 buff[HDMI_CEC_READ_MAXSIZE];
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	if (av8100_conf_prep(AV8100_COMMAND_CEC_MESSAGE_READ_BACK,
+			&config) != 0) {
+		dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_CEC_MESSAGE_READ_BACK,
+		&buf_len, buff, I2C_INTERFACE) != 0) {
+		dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	if (buf_len > 0) {
+		*src = (buff[0] & 0xF0) >> 4;
+		*dest = buff[0] & 0x0F;
+		*data_len = buf_len - 1;
+		memcpy(data, &buff[1], buf_len - 1);
+	} else
+		*data_len = 0;
+
+	return 0;
+}
+
+/* CEC tx status can be set or read */
+static bool cec_tx_status(struct hdmi_device *hdev,
+				enum cec_tx_status_action action)
+{
+	static bool cec_tx_busy;
+
+	switch (action) {
+	case CEC_TX_SET_FREE:
+		cec_tx_busy = false;
+		dev_dbg(hdev->dev, "cec_tx_busy set:%d\n", cec_tx_busy);
+		break;
+
+	case CEC_TX_SET_BUSY:
+		cec_tx_busy = true;
+		dev_dbg(hdev->dev, "cec_tx_busy set:%d\n", cec_tx_busy);
+		break;
+
+	case CEC_TX_CHECK:
+	default:
+		dev_dbg(hdev->dev, "cec_tx_busy chk:%d\n", cec_tx_busy);
+		break;
+	}
+
+	return cec_tx_busy;
+}
+
+static int cecsend(struct hdmi_device *hdev, u8 src, u8 dest, u8 data_len,
+			u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+	int cnt;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.cec_message_write_format.buffer[0] = ((src & 0x0F) << 4) +
+			(dest & 0x0F);
+	config.cec_message_write_format.buffer_length = data_len + 1;
+	memcpy(&config.cec_message_write_format.buffer[1], data, data_len);
+
+	if (av8100_conf_prep(AV8100_COMMAND_CEC_MESSAGE_WRITE,
+		&config) != 0) {
+		dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_enable_interrupt() != 0) {
+		dev_err(hdev->dev, "av8100_ei FAIL\n");
+		return -EINVAL;
+	}
+
+	cnt = 0;
+	while ((cnt < CECTX_TRY) && cec_tx_status(hdev, CEC_TX_CHECK)) {
+		/* Wait for pending CEC to be finished */
+		msleep(CECTX_WAITTIME);
+		cnt++;
+	}
+	dev_dbg(hdev->dev, "cectxcnt:%d\n", cnt);
+
+	if (av8100_conf_w(AV8100_COMMAND_CEC_MESSAGE_WRITE,
+		NULL, NULL, I2C_INTERFACE) != 0) {
+		dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+	cec_tx_status(hdev, CEC_TX_SET_BUSY);
+
+	return 0;
+}
+
+static int infofrsend(struct hdmi_device *hdev, u8 type, u8 version, u8 crc,
+			u8 data_len, u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	if ((data_len < 1) || (data_len > HDMI_INFOFRAME_MAX_SIZE))
+		return -EINVAL;
+
+	config.infoframes_format.type = type;
+	config.infoframes_format.version = version;
+	config.infoframes_format.crc = crc;
+	config.infoframes_format.length = data_len;
+	memcpy(&config.infoframes_format.data, data, data_len);
+	if (av8100_conf_prep(AV8100_COMMAND_INFOFRAMES,
+		&config) != 0) {
+		dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_INFOFRAMES,
+		NULL, NULL, I2C_INTERFACE) != 0) {
+		dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int hdcpchkaesotp(struct hdmi_device *hdev, u8 *crc, u8 *progged)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+	u8 buf_len;
+	u8 buf[2];
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.fuse_aes_key_format.fuse_operation = AV8100_FUSE_READ;
+	memset(config.fuse_aes_key_format.key, 0, AV8100_FUSE_KEY_SIZE);
+	if (av8100_conf_prep(AV8100_COMMAND_FUSE_AES_KEY,
+		&config) != 0) {
+		dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_FUSE_AES_KEY,
+		&buf_len, buf, I2C_INTERFACE) != 0) {
+		dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	if (buf_len == 2) {
+		*crc = buf[0];
+		*progged = buf[1];
+	}
+
+	return 0;
+}
+
+static int hdcpfuseaes(struct hdmi_device *hdev, u8 *key, u8 crc, u8 *result)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+	u8 buf_len;
+	u8 buf[2];
+
+	/* Default not OK */
+	*result = HDMI_RESULT_NOT_OK;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.fuse_aes_key_format.fuse_operation = AV8100_FUSE_WRITE;
+	memcpy(config.fuse_aes_key_format.key, key, AV8100_FUSE_KEY_SIZE);
+	if (av8100_conf_prep(AV8100_COMMAND_FUSE_AES_KEY,
+		&config) != 0) {
+		dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_FUSE_AES_KEY,
+		&buf_len, buf, I2C_INTERFACE) != 0) {
+		dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	if (buf_len == 2) {
+		dev_dbg(hdev->dev, "buf[0]:%02x buf[1]:%02x\n", buf[0], buf[1]);
+		if ((crc == buf[0]) && (buf[1] == 1))
+			/* OK */
+			*result = HDMI_RESULT_OK;
+		else
+			*result = HDMI_RESULT_CRC_MISMATCH;
+	}
+
+	return 0;
+}
+
+static int hdcploadaes(struct hdmi_device *hdev, u8 block, u8 key_len, u8 *key,
+			u8 *result, u8 *crc32)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+	u8 buf_len;
+	u8 buf[CRC32_SIZE];
+
+	/* Default not OK */
+	*result = HDMI_RESULT_NOT_OK;
+
+	dev_dbg(hdev->dev, "%s block:%d\n", __func__, block);
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.hdcp_send_key_format.key_number = block;
+	config.hdcp_send_key_format.data_len = key_len;
+	memcpy(config.hdcp_send_key_format.data, key, key_len);
+	if (av8100_conf_prep(AV8100_COMMAND_HDCP_SENDKEY, &config) != 0) {
+		dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_HDCP_SENDKEY,
+		&buf_len, buf, I2C_INTERFACE) != 0) {
+		dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	if ((buf_len == CRC32_SIZE) && (crc32)) {
+		memcpy(crc32, buf, CRC32_SIZE);
+		dev_dbg(hdev->dev, "crc32:%02x%02x%02x%02x\n",
+			crc32[0], crc32[1], crc32[2], crc32[3]);
+	}
+
+	*result = HDMI_RESULT_OK;
+
+	return 0;
+}
+
+static int hdcpauthencr(struct hdmi_device *hdev, u8 auth_type, u8 encr_type,
+			u8 *len, u8 *data)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	switch (auth_type) {
+	case HDMI_HDCP_AUTH_OFF:
+	default:
+		config.hdcp_management_format.req_type =
+				AV8100_HDCP_AUTH_REQ_OFF;
+		break;
+
+	case HDMI_HDCP_AUTH_START:
+		config.hdcp_management_format.req_type =
+				AV8100_HDCP_AUTH_REQ_ON;
+		break;
+
+	case HDMI_HDCP_AUTH_REV_LIST_REQ:
+		config.hdcp_management_format.req_type =
+				AV8100_HDCP_REV_LIST_REQ;
+		break;
+	case HDMI_HDCP_AUTH_CONT:
+		config.hdcp_management_format.req_type =
+				AV8100_HDCP_AUTH_CONT;
+		break;
+	}
+
+	switch (encr_type) {
+	case HDMI_HDCP_ENCR_OESS:
+	default:
+		config.hdcp_management_format.encr_use =
+				AV8100_HDCP_ENCR_USE_OESS;
+		break;
+
+	case HDMI_HDCP_ENCR_EESS:
+		config.hdcp_management_format.encr_use =
+				AV8100_HDCP_ENCR_USE_EESS;
+		break;
+	}
+
+	if (av8100_conf_prep(AV8100_COMMAND_HDCP_MANAGEMENT,
+		&config) != 0) {
+		dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_HDCP_MANAGEMENT,
+		len, data, I2C_INTERFACE) != 0) {
+		dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static u8 events_read(struct hdmi_device *hdev)
+{
+	int ret;
+
+	LOCK_HDMI_EVENTS;
+	ret = hdev->events;
+	dev_dbg(hdev->dev, "%s %02x\n", __func__, hdev->events);
+	UNLOCK_HDMI_EVENTS;
+
+	return ret;
+}
+
+static int events_clear(struct hdmi_device *hdev, u8 ev)
+{
+	dev_dbg(hdev->dev, "%s %02x\n", __func__, ev);
+
+	LOCK_HDMI_EVENTS;
+	hdev->events &= ~ev & EVENTS_MASK;
+	UNLOCK_HDMI_EVENTS;
+
+	return 0;
+}
+
+static int event_wakeup(struct hdmi_device *hdev)
+{
+	struct kobject *kobj = &hdev->dev->kobj;
+
+	dev_dbg(hdev->dev, "%s", __func__);
+
+	LOCK_HDMI_EVENTS;
+	hdev->events |= HDMI_EVENT_WAKEUP;
+	hdev->events_received = true;
+	UNLOCK_HDMI_EVENTS;
+
+	/* Wake up application waiting for event via call to poll() */
+	sysfs_notify(kobj, NULL, SYSFS_EVENT_FILENAME);
+	wake_up_interruptible(&hdev->event_wq);
+
+	return 0;
+}
+
+static int audiocfg(struct hdmi_device *hdev, struct audio_cfg *cfg)
+{
+	union av8100_configuration config;
+	struct av8100_status status;
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup failed\n");
+			return -EINVAL;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_INIT) {
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	config.audio_input_format.audio_input_if_format	= cfg->if_format;
+	config.audio_input_format.i2s_input_nb		= cfg->i2s_entries;
+	config.audio_input_format.sample_audio_freq	= cfg->freq;
+	config.audio_input_format.audio_word_lg		= cfg->word_length;
+	config.audio_input_format.audio_format		= cfg->format;
+	config.audio_input_format.audio_if_mode		= cfg->if_mode;
+	config.audio_input_format.audio_mute		= cfg->mute;
+
+	if (av8100_conf_prep(AV8100_COMMAND_AUDIO_INPUT_FORMAT,
+		&config) != 0) {
+		dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_AUDIO_INPUT_FORMAT,
+		NULL, NULL, I2C_INTERFACE) != 0) {
+		dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* sysfs */
+static ssize_t store_storeastext(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if ((count != HDMI_STOREASTEXT_BIN_SIZE) &&
+		(count != HDMI_STOREASTEXT_TEXT_SIZE) &&
+		(count != HDMI_STOREASTEXT_TEXT_SIZE + 1))
+		return -EINVAL;
+
+	if ((count == HDMI_STOREASTEXT_BIN_SIZE) && (*buf == 0x1))
+		hdev->sysfs_data.store_as_hextext = true;
+	else if (((count == HDMI_STOREASTEXT_TEXT_SIZE) ||
+		(count == HDMI_STOREASTEXT_TEXT_SIZE + 1)) && (*buf == '0') &&
+			(*(buf + 1) == '1')) {
+		hdev->sysfs_data.store_as_hextext = true;
+	} else {
+		hdev->sysfs_data.store_as_hextext = false;
+	}
+
+	dev_dbg(hdev->dev, "store_as_hextext:%0d\n",
+		hdev->sysfs_data.store_as_hextext);
+
+	return count;
+}
+
+static ssize_t store_plugdeten(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	struct plug_detect plug_detect;
+	int index = 0;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_PLUGDETEN_TEXT_SIZE) &&
+			(count != HDMI_PLUGDETEN_TEXT_SIZE + 1))
+			return -EINVAL;
+		plug_detect.hdmi_detect_enable = htoi(buf + index);
+		index += 2;
+		plug_detect.on_time = htoi(buf + index);
+		index += 2;
+		plug_detect.hdmi_off_time = htoi(buf + index);
+		index += 2;
+	} else {
+		if (count != HDMI_PLUGDETEN_BIN_SIZE)
+			return -EINVAL;
+		plug_detect.hdmi_detect_enable = *(buf + index++);
+		plug_detect.on_time = *(buf + index++);
+		plug_detect.hdmi_off_time = *(buf + index++);
+	}
+
+	if (plugdeten(hdev, &plug_detect))
+		return -EINVAL;
+
+	return count;
+}
+
+static ssize_t store_edidread(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	struct edid_read edid_read;
+	int index = 0;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+	dev_dbg(hdev->dev, "count:%d\n", count);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_EDIDREAD_TEXT_SIZE) &&
+			(count != HDMI_EDIDREAD_TEXT_SIZE + 1))
+			return -EINVAL;
+		edid_read.address = htoi(buf + index);
+		index += 2;
+		edid_read.block_nr = htoi(buf + index);
+		index += 2;
+	} else {
+		if (count != HDMI_EDIDREAD_BIN_SIZE)
+			return -EINVAL;
+		edid_read.address = *(buf + index++);
+		edid_read.block_nr = *(buf + index++);
+	}
+
+	if (edidread(hdev, &edid_read, &hdev->sysfs_data.edid_data.buf_len,
+			hdev->sysfs_data.edid_data.buf))
+		return -EINVAL;
+
+	return count;
+}
+
+static ssize_t show_edidread(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	int len;
+	int index = 0;
+	int cnt;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	len = hdev->sysfs_data.edid_data.buf_len;
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", len);
+		index += 2;
+	} else
+		*(buf + index++) = len;
+
+	dev_dbg(hdev->dev, "len:%02x\n", len);
+
+	cnt = 0;
+	while (cnt < len) {
+		if (hdev->sysfs_data.store_as_hextext) {
+			snprintf(buf + index, 3, "%02x",
+				hdev->sysfs_data.edid_data.buf[cnt]);
+			index += 2;
+		} else
+			*(buf + index++) =
+				hdev->sysfs_data.edid_data.buf[cnt];
+
+		dev_dbg(hdev->dev, "%02x ",
+			hdev->sysfs_data.edid_data.buf[cnt]);
+
+		cnt++;
+	}
+
+	if (hdev->sysfs_data.store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_ceceven(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	bool enable = false;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_CECEVEN_TEXT_SIZE) &&
+			(count != HDMI_CECEVEN_TEXT_SIZE + 1))
+			return -EINVAL;
+		if ((*buf == '0') && (*(buf + 1) == '1'))
+			enable = true;
+	} else {
+		if (count != HDMI_CECEVEN_BIN_SIZE)
+			return -EINVAL;
+		if (*buf == 0x01)
+			enable = true;
+	}
+
+	event_enable(hdev, enable, HDMI_EVENT_CEC | HDMI_EVENT_CECTXERR |
+						HDMI_EVENT_CECTX);
+
+	return count;
+}
+
+static ssize_t show_cecread(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	struct cec_rw cec_read;
+	int index = 0;
+	int cnt;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (cecread(hdev, &cec_read.src, &cec_read.dest, &cec_read.length,
+		cec_read.data))
+		return -EINVAL;
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", cec_read.src);
+		index += 2;
+		snprintf(buf + index, 3, "%02x", cec_read.dest);
+		index += 2;
+		snprintf(buf + index, 3, "%02x", cec_read.length);
+		index += 2;
+	} else {
+		*(buf + index++) = cec_read.src;
+		*(buf + index++) = cec_read.dest;
+		*(buf + index++) = cec_read.length;
+	}
+
+	dev_dbg(hdev->dev, "len:%02x\n", cec_read.length);
+
+	cnt = 0;
+	while (cnt < cec_read.length) {
+		if (hdev->sysfs_data.store_as_hextext) {
+			snprintf(buf + index, 3, "%02x", cec_read.data[cnt]);
+			index += 2;
+		} else
+			*(buf + index++) = cec_read.data[cnt];
+
+		dev_dbg(hdev->dev, "%02x ", cec_read.data[cnt]);
+
+		cnt++;
+	}
+
+	if (hdev->sysfs_data.store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_cecsend(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	struct cec_rw cec_w;
+	int index = 0;
+	int cnt;
+	int store_as_text;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if ((*buf == 'F') || (*buf == 'f'))
+		/* To be able to override bin format for test purpose */
+		store_as_text = 1;
+	else
+		store_as_text = hdev->sysfs_data.store_as_hextext;
+
+	if (store_as_text) {
+		if ((count < HDMI_CECSEND_TEXT_SIZE_MIN) ||
+			(count > HDMI_CECSEND_TEXT_SIZE_MAX))
+			return -EINVAL;
+
+		cec_w.src = htoi(buf + index) & 0x0F;
+		index += 2;
+		cec_w.dest = htoi(buf + index);
+		index += 2;
+		cec_w.length = htoi(buf + index);
+		index += 2;
+		if (cec_w.length > HDMI_CEC_WRITE_MAXSIZE)
+			return -EINVAL;
+		cnt = 0;
+		while (cnt < cec_w.length) {
+			cec_w.data[cnt] = htoi(buf + index);
+			index += 2;
+			dev_dbg(hdev->dev, "%02x ", cec_w.data[cnt]);
+			cnt++;
+		}
+	} else {
+		if ((count < HDMI_CECSEND_BIN_SIZE_MIN) ||
+			(count > HDMI_CECSEND_BIN_SIZE_MAX))
+			return -EINVAL;
+
+		cec_w.src = *(buf + index++);
+		cec_w.dest = *(buf + index++);
+		cec_w.length = *(buf + index++);
+		if (cec_w.length > HDMI_CEC_WRITE_MAXSIZE)
+			return -EINVAL;
+		memcpy(cec_w.data, buf + index, cec_w.length);
+	}
+
+	if (cecsend(hdev, cec_w.src, cec_w.dest, cec_w.length, cec_w.data))
+		return -EINVAL;
+
+	return count;
+}
+
+static ssize_t store_infofrsend(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	struct info_fr info_fr;
+	int index = 0;
+	int cnt;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count < HDMI_INFOFRSEND_TEXT_SIZE_MIN) ||
+			(count > HDMI_INFOFRSEND_TEXT_SIZE_MAX))
+			return -EINVAL;
+
+		info_fr.type = htoi(&buf[index]);
+		index += 2;
+		info_fr.ver = htoi(&buf[index]);
+		index += 2;
+		info_fr.crc = htoi(&buf[index]);
+		index += 2;
+		info_fr.length = htoi(&buf[index]);
+		index += 2;
+
+		if (info_fr.length > HDMI_INFOFRAME_MAX_SIZE)
+			return -EINVAL;
+		cnt = 0;
+		while (cnt < info_fr.length) {
+			info_fr.data[cnt] = htoi(buf + index);
+			index += 2;
+			dev_dbg(hdev->dev, "%02x ", info_fr.data[cnt]);
+			cnt++;
+		}
+	} else {
+		if ((count < HDMI_INFOFRSEND_BIN_SIZE_MIN) ||
+			(count > HDMI_INFOFRSEND_BIN_SIZE_MAX))
+			return -EINVAL;
+
+		info_fr.type = *(buf + index++);
+		info_fr.ver = *(buf + index++);
+		info_fr.crc = *(buf + index++);
+		info_fr.length = *(buf + index++);
+
+		if (info_fr.length > HDMI_INFOFRAME_MAX_SIZE)
+			return -EINVAL;
+		memcpy(info_fr.data, buf + index, info_fr.length);
+	}
+
+	if (infofrsend(hdev, info_fr.type, info_fr.ver, info_fr.crc,
+		info_fr.length, info_fr.data))
+		return -EINVAL;
+
+	return count;
+}
+
+static ssize_t store_hdcpeven(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	bool enable = false;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_HDCPEVEN_TEXT_SIZE) &&
+			(count != HDMI_HDCPEVEN_TEXT_SIZE + 1))
+			return -EINVAL;
+		if ((*buf == '0') && (*(buf + 1) == '1'))
+			enable = true;
+	} else {
+		if (count != HDMI_HDCPEVEN_BIN_SIZE)
+			return -EINVAL;
+		if (*buf == 0x01)
+			enable = true;
+	}
+
+	event_enable(hdev, enable, HDMI_EVENT_HDCP);
+
+	return count;
+}
+
+static ssize_t show_hdcpchkaesotp(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	u8 crc;
+	u8 progged;
+	int index = 0;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdcpchkaesotp(hdev, &crc, &progged))
+		return -EINVAL;
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", progged);
+		index += 2;
+	} else {
+		*(buf + index++) = progged;
+	}
+
+	dev_dbg(hdev->dev, "progged:%02x\n", progged);
+
+	if (hdev->sysfs_data.store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_hdcpfuseaes(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	struct hdcp_fuseaes hdcp_fuseaes;
+	int index = 0;
+	int cnt;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	/* Default not OK */
+	hdev->sysfs_data.fuse_result = HDMI_RESULT_NOT_OK;
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_HDCP_FUSEAES_TEXT_SIZE) &&
+			(count != HDMI_HDCP_FUSEAES_TEXT_SIZE + 1))
+			return -EINVAL;
+
+		cnt = 0;
+		while (cnt < HDMI_HDCP_FUSEAES_KEYSIZE) {
+			hdcp_fuseaes.key[cnt] = htoi(buf + index);
+			index += 2;
+			dev_dbg(hdev->dev, "%02x ", hdcp_fuseaes.key[cnt]);
+			cnt++;
+		}
+		hdcp_fuseaes.crc = htoi(&buf[index]);
+		index += 2;
+		dev_dbg(hdev->dev, "%02x ", hdcp_fuseaes.crc);
+	} else {
+		if (count != HDMI_HDCP_FUSEAES_BIN_SIZE)
+			return -EINVAL;
+
+		memcpy(hdcp_fuseaes.key, buf + index,
+				HDMI_HDCP_FUSEAES_KEYSIZE);
+		index += HDMI_HDCP_FUSEAES_KEYSIZE;
+		hdcp_fuseaes.crc = *(buf + index++);
+	}
+
+	if (hdcpfuseaes(hdev, hdcp_fuseaes.key, hdcp_fuseaes.crc,
+		&hdcp_fuseaes.result))
+		return -EINVAL;
+
+	dev_dbg(hdev->dev, "fuseresult:%02x ", hdcp_fuseaes.result);
+
+	hdev->sysfs_data.fuse_result = hdcp_fuseaes.result;
+
+	return count;
+}
+
+static ssize_t show_hdcpfuseaes(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	int index = 0;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", hdev->sysfs_data.fuse_result);
+		index += 2;
+	} else
+		*(buf + index++) = hdev->sysfs_data.fuse_result;
+
+	dev_dbg(hdev->dev, "status:%02x\n", hdev->sysfs_data.fuse_result);
+
+	if (hdev->sysfs_data.store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_hdcploadaes(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	struct hdcp_loadaesone hdcp_loadaes;
+	int index = 0;
+	int block_cnt;
+	int cnt;
+	u8 crc32_rcvd[CRC32_SIZE];
+	u8 crc;
+	u8 progged;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	/* Default not OK */
+	hdev->sysfs_data.loadaes_result = HDMI_RESULT_NOT_OK;
+
+	if (hdcpchkaesotp(hdev, &crc, &progged))
+		return -EINVAL;
+
+	if (!progged) {
+		/* AES is not fused */
+		hdcp_loadaes.result = HDMI_AES_NOT_FUSED;
+		goto store_hdcploadaes_err;
+	}
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_HDCP_LOADAES_TEXT_SIZE) &&
+			(count != HDMI_HDCP_LOADAES_TEXT_SIZE + 1)) {
+			dev_err(hdev->dev, "%s", "count mismatch\n");
+			return -EINVAL;
+		}
+
+		/* AES */
+		block_cnt = 0;
+		while (block_cnt < HDMI_HDCP_AES_NR_OF_BLOCKS) {
+			cnt = 0;
+			while (cnt < HDMI_HDCP_AES_KEYSIZE) {
+				hdcp_loadaes.key[cnt] =	htoi(buf + index);
+				index += 2;
+				dev_dbg(hdev->dev, "%02x ",
+					hdcp_loadaes.key[cnt]);
+				cnt++;
+			}
+
+			if (hdcploadaes(hdev,
+					block_cnt + HDMI_HDCP_AES_BLOCK_START,
+					HDMI_HDCP_AES_KEYSIZE,
+					hdcp_loadaes.key,
+					&hdcp_loadaes.result,
+					crc32_rcvd)) {
+				dev_err(hdev->dev, "%s %d\n",
+					"hdcploadaes err aes block",
+					block_cnt + HDMI_HDCP_AES_BLOCK_START);
+				return -EINVAL;
+			}
+
+			if (hdcp_loadaes.result)
+				goto store_hdcploadaes_err;
+
+			block_cnt++;
+		}
+
+		/* KSV */
+		memset(hdcp_loadaes.key, 0, HDMI_HDCP_AES_KSVZEROESSIZE);
+		cnt = HDMI_HDCP_AES_KSVZEROESSIZE;
+		while (cnt < HDMI_HDCP_AES_KSVSIZE +
+				HDMI_HDCP_AES_KSVZEROESSIZE) {
+			hdcp_loadaes.key[cnt] =
+					htoi(&buf[index]);
+			index += 2;
+			dev_dbg(hdev->dev, "%02x ", hdcp_loadaes.key[cnt]);
+			cnt++;
+		}
+
+		if (hdcploadaes(hdev, HDMI_HDCP_KSV_BLOCK,
+				HDMI_HDCP_AES_KSVSIZE +
+				HDMI_HDCP_AES_KSVZEROESSIZE,
+				hdcp_loadaes.key,
+				&hdcp_loadaes.result,
+				NULL)) {
+			dev_err(hdev->dev,
+				"%s %d\n", "hdcploadaes err in ksv\n",
+				block_cnt + HDMI_HDCP_AES_BLOCK_START);
+			return -EINVAL;
+		}
+
+		if (hdcp_loadaes.result)
+			goto store_hdcploadaes_err;
+
+		/* CRC32 */
+		for (cnt = 0; cnt < CRC32_SIZE; cnt++) {
+			hdcp_loadaes.crc32[cnt] = htoi(buf + index);
+			index += 2;
+		}
+
+		if (memcmp(hdcp_loadaes.crc32, crc32_rcvd, CRC32_SIZE)) {
+			dev_dbg(hdev->dev, "crc32exp:%02x%02x%02x%02x\n",
+				hdcp_loadaes.crc32[0],
+				hdcp_loadaes.crc32[1],
+				hdcp_loadaes.crc32[2],
+				hdcp_loadaes.crc32[3]);
+			hdcp_loadaes.result = HDMI_RESULT_CRC_MISMATCH;
+			goto store_hdcploadaes_err;
+		}
+	} else {
+		if (count != HDMI_HDCP_LOADAES_BIN_SIZE) {
+			dev_err(hdev->dev, "%s", "count mismatch\n");
+			return -EINVAL;
+		}
+
+		/* AES */
+		block_cnt = 0;
+		while (block_cnt < HDMI_HDCP_AES_NR_OF_BLOCKS) {
+			memcpy(hdcp_loadaes.key, buf + index,
+					HDMI_HDCP_AES_KEYSIZE);
+			index += HDMI_HDCP_AES_KEYSIZE;
+
+			if (hdcploadaes(hdev,
+					block_cnt + HDMI_HDCP_AES_BLOCK_START,
+					HDMI_HDCP_AES_KEYSIZE,
+					hdcp_loadaes.key,
+					&hdcp_loadaes.result,
+					crc32_rcvd)) {
+				dev_err(hdev->dev, "%s %d\n",
+					"hdcploadaes err aes block",
+					block_cnt + HDMI_HDCP_AES_BLOCK_START);
+				return -EINVAL;
+			}
+
+			if (hdcp_loadaes.result)
+				goto store_hdcploadaes_err;
+
+			block_cnt++;
+		}
+
+		/* KSV */
+		memset(hdcp_loadaes.key, 0, HDMI_HDCP_AES_KSVZEROESSIZE);
+		memcpy(hdcp_loadaes.key + HDMI_HDCP_AES_KSVZEROESSIZE,
+				buf + index,
+				HDMI_HDCP_AES_KSVSIZE);
+		index += HDMI_HDCP_AES_KSVSIZE;
+
+		if (hdcploadaes(hdev, HDMI_HDCP_KSV_BLOCK,
+				HDMI_HDCP_AES_KSVSIZE +
+				HDMI_HDCP_AES_KSVZEROESSIZE,
+				hdcp_loadaes.key,
+				&hdcp_loadaes.result,
+				NULL)) {
+			dev_err(hdev->dev, "%s %d\n",
+				"hdcploadaes err in ksv\n",
+				block_cnt + HDMI_HDCP_AES_BLOCK_START);
+			return -EINVAL;
+		}
+
+		memcpy(hdcp_loadaes.crc32, buf + index, CRC32_SIZE);
+		index += CRC32_SIZE;
+
+		/* CRC32 */
+		if (memcmp(hdcp_loadaes.crc32, crc32_rcvd, CRC32_SIZE)) {
+			dev_dbg(hdev->dev, "crc32exp:%02x%02x%02x%02x\n",
+				hdcp_loadaes.crc32[0],
+				hdcp_loadaes.crc32[1],
+				hdcp_loadaes.crc32[2],
+				hdcp_loadaes.crc32[3]);
+			hdcp_loadaes.result = HDMI_RESULT_CRC_MISMATCH;
+		}
+	}
+
+store_hdcploadaes_err:
+	hdev->sysfs_data.loadaes_result = hdcp_loadaes.result;
+	return count;
+}
+
+static ssize_t show_hdcploadaes(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	int index = 0;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x",
+			hdev->sysfs_data.loadaes_result);
+		index += 2;
+	} else
+		*(buf + index++) = hdev->sysfs_data.loadaes_result;
+
+	dev_dbg(hdev->dev, "result:%02x\n", hdev->sysfs_data.loadaes_result);
+
+	if (hdev->sysfs_data.store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_hdcpauthencr(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	struct hdcp_authencr hdcp_authencr;
+	int index = 0;
+	u8 crc;
+	u8 progged;
+	int result = HDMI_RESULT_NOT_OK;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	/* Default */
+	hdev->sysfs_data.authencr.buf_len = 0;
+
+	if (hdcpchkaesotp(hdev, &crc, &progged)) {
+		result = HDMI_AES_NOT_FUSED;
+		goto store_hdcpauthencr_end;
+	}
+
+	if (!progged) {
+		/* AES is not fused */
+		result = HDMI_AES_NOT_FUSED;
+		goto store_hdcpauthencr_end;
+	}
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_HDCPAUTHENCR_TEXT_SIZE) &&
+			(count != HDMI_HDCPAUTHENCR_TEXT_SIZE + 1))
+			goto store_hdcpauthencr_end;
+
+		hdcp_authencr.auth_type = htoi(buf + index);
+		index += 2;
+		hdcp_authencr.encr_type = htoi(buf + index);
+		index += 2;
+	} else {
+		if (count != HDMI_HDCPAUTHENCR_BIN_SIZE)
+			goto store_hdcpauthencr_end;
+
+		hdcp_authencr.auth_type = *(buf + index++);
+		hdcp_authencr.encr_type = *(buf + index++);
+	}
+
+	if (hdcpauthencr(hdev, hdcp_authencr.auth_type, hdcp_authencr.encr_type,
+		 &hdev->sysfs_data.authencr.buf_len,
+		 hdev->sysfs_data.authencr.buf))
+		goto store_hdcpauthencr_end;
+
+	result = HDMI_RESULT_OK;
+
+store_hdcpauthencr_end:
+	hdev->sysfs_data.authencr.result = result;
+	return count;
+}
+
+static ssize_t show_hdcpauthencr(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	int len;
+	int index = 0;
+	int cnt;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	/* result */
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x",
+			hdev->sysfs_data.authencr.result);
+		index += 2;
+	} else
+		*(buf + index++) = hdev->sysfs_data.authencr.result;
+
+	dev_dbg(hdev->dev, "result:%02x\n", hdev->sysfs_data.authencr.result);
+
+	/* resp_size */
+	len = hdev->sysfs_data.authencr.buf_len;
+	if (len > AUTH_BUF_LEN)
+		len = AUTH_BUF_LEN;
+	dev_dbg(hdev->dev, "resp_size:%d\n", len);
+
+	/* resp */
+	cnt = 0;
+	while (cnt < len) {
+		if (hdev->sysfs_data.store_as_hextext) {
+			snprintf(buf + index, 3, "%02x",
+				hdev->sysfs_data.authencr.buf[cnt]);
+			index += 2;
+
+			dev_dbg(hdev->dev, "%02x ",
+				hdev->sysfs_data.authencr.buf[cnt]);
+
+		} else
+			*(buf + index++) = hdev->sysfs_data.authencr.buf[cnt];
+
+		cnt++;
+	}
+
+	if (hdev->sysfs_data.store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t show_hdcpstateget(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	u8 hdcp_state;
+	int index = 0;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (av8100_reg_gen_status_r(NULL, NULL, NULL, NULL, NULL, &hdcp_state))
+			return -EINVAL;
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", hdcp_state);
+		index += 2;
+	} else
+		*(buf + index++) = hdcp_state;
+
+	dev_dbg(hdev->dev, "status:%02x\n", hdcp_state);
+
+	if (hdev->sysfs_data.store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t show_evread(struct device *dev, struct device_attribute *attr,
+		char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	int index = 0;
+	u8 ev;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	ev = events_read(hdev);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", ev);
+		index += 2;
+	} else
+		*(buf + index++) = ev;
+
+	if (hdev->sysfs_data.store_as_hextext)
+		index++;
+
+	/* Events are read: clear events */
+	events_clear(hdev, EVENTS_MASK);
+
+	return index;
+}
+
+static ssize_t store_evclr(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	u8 ev;
+	int index = 0;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_EVCLR_TEXT_SIZE) &&
+			(count != HDMI_EVCLR_TEXT_SIZE + 1))
+			return -EINVAL;
+
+		ev = htoi(&buf[index]);
+		index += 2;
+	} else {
+		if (count != HDMI_EVCLR_BIN_SIZE)
+			return -EINVAL;
+
+		ev = *(buf + index++);
+	}
+
+	events_clear(hdev, ev);
+
+	return count;
+}
+
+static ssize_t store_audiocfg(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	struct audio_cfg audio_cfg;
+	int index = 0;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_AUDIOCFG_TEXT_SIZE) &&
+			(count != HDMI_AUDIOCFG_TEXT_SIZE + 1))
+			return -EINVAL;
+
+		audio_cfg.if_format = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.i2s_entries = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.freq = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.word_length = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.format = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.if_mode = htoi(&buf[index]);
+		index += 2;
+		audio_cfg.mute = htoi(&buf[index]);
+		index += 2;
+	} else {
+		if (count != HDMI_AUDIOCFG_BIN_SIZE)
+			return -EINVAL;
+
+		audio_cfg.if_format = *(buf + index++);
+		audio_cfg.i2s_entries = *(buf + index++);
+		audio_cfg.freq = *(buf + index++);
+		audio_cfg.word_length = *(buf + index++);
+		audio_cfg.format = *(buf + index++);
+		audio_cfg.if_mode = *(buf + index++);
+		audio_cfg.mute = *(buf + index++);
+	}
+
+	audiocfg(hdev, &audio_cfg);
+
+	return count;
+}
+
+static ssize_t show_plugstatus(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	int index = 0;
+	struct av8100_status av8100_status;
+	u8 plstat;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	av8100_status = av8100_status_get();
+	plstat = av8100_status.av8100_plugin_status == AV8100_HDMI_PLUGIN;
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", plstat);
+		index += 2;
+	} else
+		*(buf + index++) = plstat;
+
+	if (hdev->sysfs_data.store_as_hextext)
+		index++;
+
+	return index;
+}
+
+static ssize_t store_poweronoff(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	bool enable = false;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		if ((count != HDMI_POWERONOFF_TEXT_SIZE) &&
+			(count != HDMI_POWERONOFF_TEXT_SIZE + 1))
+			return -EINVAL;
+		if ((*buf == '0') && (*(buf + 1) == '1'))
+			enable = true;
+	} else {
+		if (count != HDMI_POWERONOFF_BIN_SIZE)
+			return -EINVAL;
+		if (*buf == 0x01)
+			enable = true;
+	}
+
+	if (enable == 0) {
+		if (av8100_powerdown() != 0) {
+			dev_err(hdev->dev, "av8100_powerdown FAIL\n");
+			return -EINVAL;
+		}
+	} else {
+		if (av8100_powerup() != 0) {
+			dev_err(hdev->dev, "av8100_powerup FAIL\n");
+			return -EINVAL;
+		}
+	}
+
+	return count;
+}
+
+static ssize_t show_poweronoff(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+	int index = 0;
+	struct av8100_status status;
+	u8 power_state;
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_SCAN)
+		power_state = 0;
+	else
+		power_state = 1;
+
+	if (hdev->sysfs_data.store_as_hextext) {
+		snprintf(buf + index, 3, "%02x", power_state);
+		index += 3;
+	} else {
+		*(buf + index++) = power_state;
+	}
+
+	return index;
+}
+
+static ssize_t store_evwakeup(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct hdmi_device *hdev = dev_to_hdev(dev);
+
+	if (!hdev)
+		return -EFAULT;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	event_wakeup(hdev);
+
+	return count;
+}
+
+static int hdmi_open(struct inode *inode, struct file *filp)
+{
+	return 0;
+}
+
+static int hdmi_release(struct inode *inode, struct file *filp)
+{
+	return 0;
+}
+
+/* ioctl */
+static long hdmi_ioctl(struct file *file,
+		       unsigned int cmd, unsigned long arg)
+{
+	u8 value = 0;
+	struct hdmi_register reg;
+	struct av8100_status status;
+	u8 aes_status;
+	struct hdmi_device *hdev = devnr_to_hdev(HDMI_DEVNR_DEFAULT);
+
+	switch (cmd) {
+	case IOC_PLUG_DETECT_ENABLE:
+		{
+		struct plug_detect plug_detect;
+
+		if (copy_from_user(&plug_detect, (void *)arg,
+			sizeof(struct plug_detect)))
+			return -EINVAL;
+
+		if (plugdeten(hdev, &plug_detect))
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_EDID_READ:
+		{
+		struct edid_read edid_read;
+
+		if (copy_from_user(&edid_read, (void *)arg,
+				sizeof(struct edid_read)))
+			return -EINVAL;
+
+		if (edidread(hdev, &edid_read, &edid_read.data_length,
+				edid_read.data))
+			return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&edid_read,
+			sizeof(struct edid_read))) {
+			return -EINVAL;
+		}
+		}
+		break;
+
+	case IOC_CEC_EVENT_ENABLE:
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EINVAL;
+
+		event_enable(hdev, value != 0,
+					HDMI_EVENT_CEC | HDMI_EVENT_CECTXERR |
+							HDMI_EVENT_CECTX);
+		break;
+
+	case IOC_CEC_READ:
+		{
+		struct cec_rw cec_read;
+
+		if (cecread(hdev, &cec_read.src, &cec_read.dest,
+				&cec_read.length, cec_read.data))
+			return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&cec_read,
+			sizeof(struct cec_rw))) {
+			return -EINVAL;
+		}
+		}
+		break;
+
+	case IOC_CEC_SEND:
+		{
+		struct cec_rw cec_send;
+
+		if (copy_from_user(&cec_send, (void *)arg,
+				sizeof(struct cec_rw)))
+			return -EINVAL;
+
+		if (cecsend(hdev, cec_send.src, cec_send.dest, cec_send.length,
+				cec_send.data))
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_INFOFRAME_SEND:
+		{
+		struct info_fr info_fr;
+
+		if (copy_from_user(&info_fr, (void *)arg,
+				sizeof(struct info_fr)))
+			return -EINVAL;
+
+		if (infofrsend(hdev, info_fr.type, info_fr.ver, info_fr.crc,
+			info_fr.length, info_fr.data))
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDCP_EVENT_ENABLE:
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EINVAL;
+
+		event_enable(hdev, value != 0, HDMI_EVENT_HDCP);
+		break;
+
+	case IOC_HDCP_CHKAESOTP:
+		if (hdcpchkaesotp(hdev, &value, &aes_status))
+			return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&aes_status,
+			sizeof(u8))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDCP_FUSEAES:
+		{
+		struct hdcp_fuseaes hdcp_fuseaes;
+
+		if (copy_from_user(&hdcp_fuseaes, (void *)arg,
+				sizeof(struct hdcp_fuseaes)))
+			return -EINVAL;
+
+		if (hdcpfuseaes(hdev, hdcp_fuseaes.key, hdcp_fuseaes.crc,
+				&hdcp_fuseaes.result))
+				return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&hdcp_fuseaes,
+			sizeof(struct hdcp_fuseaes))) {
+			return -EINVAL;
+		}
+		}
+		break;
+
+	case IOC_HDCP_LOADAES:
+		{
+		int block_cnt;
+		struct hdcp_loadaesone hdcp_loadaesone;
+		struct hdcp_loadaesall hdcp_loadaesall;
+
+		if (copy_from_user(&hdcp_loadaesall, (void *)arg,
+				sizeof(struct hdcp_loadaesall)))
+			return -EINVAL;
+
+		if (hdcpchkaesotp(hdev, &value, &aes_status))
+			return -EINVAL;
+
+		if (!aes_status) {
+			/* AES is not fused */
+			hdcp_loadaesone.result = HDMI_AES_NOT_FUSED;
+			goto ioc_hdcploadaes_err;
+		}
+
+		/* AES */
+		block_cnt = 0;
+		while (block_cnt < HDMI_HDCP_AES_NR_OF_BLOCKS) {
+			memcpy(hdcp_loadaesone.key, hdcp_loadaesall.key +
+					block_cnt * HDMI_HDCP_AES_KEYSIZE,
+					HDMI_HDCP_AES_KEYSIZE);
+
+			if (hdcploadaes(hdev,
+					block_cnt + HDMI_HDCP_AES_BLOCK_START,
+					HDMI_HDCP_AES_KEYSIZE,
+					hdcp_loadaesone.key,
+					&hdcp_loadaesone.result,
+					hdcp_loadaesone.crc32))
+				return -EINVAL;
+
+			if (hdcp_loadaesone.result)
+				return -EINVAL;
+
+			block_cnt++;
+		}
+
+		/* KSV */
+		memset(hdcp_loadaesone.key, 0, HDMI_HDCP_AES_KSVZEROESSIZE);
+		memcpy(hdcp_loadaesone.key + HDMI_HDCP_AES_KSVZEROESSIZE,
+				hdcp_loadaesall.ksv, HDMI_HDCP_AES_KSVSIZE);
+
+		if (hdcploadaes(hdev, HDMI_HDCP_KSV_BLOCK,
+				HDMI_HDCP_AES_KSVSIZE +
+				HDMI_HDCP_AES_KSVZEROESSIZE,
+				hdcp_loadaesone.key,
+				&hdcp_loadaesone.result,
+				NULL))
+			return -EINVAL;
+
+		if (hdcp_loadaesone.result)
+			return -EINVAL;
+
+		/* CRC32 */
+		if (memcmp(hdcp_loadaesall.crc32, hdcp_loadaesone.crc32,
+				CRC32_SIZE)) {
+			dev_dbg(hdev->dev, "crc32exp:%02x%02x%02x%02x\n",
+				hdcp_loadaesall.crc32[0],
+				hdcp_loadaesall.crc32[1],
+				hdcp_loadaesall.crc32[2],
+				hdcp_loadaesall.crc32[3]);
+			hdcp_loadaesone.result = HDMI_RESULT_CRC_MISMATCH;
+			goto ioc_hdcploadaes_err;
+		}
+
+ioc_hdcploadaes_err:
+		hdcp_loadaesall.result = hdcp_loadaesone.result;
+
+		if (copy_to_user((void *)arg, (void *)&hdcp_loadaesall,
+			sizeof(struct hdcp_loadaesall))) {
+			return -EINVAL;
+		}
+		}
+		break;
+
+	case IOC_HDCP_AUTHENCR_REQ:
+		{
+		struct hdcp_authencr hdcp_authencr;
+		int result = HDMI_RESULT_NOT_OK;
+
+		u8 buf[AUTH_BUF_LEN];
+
+		if (copy_from_user(&hdcp_authencr, (void *)arg,
+				sizeof(struct hdcp_authencr)))
+			return -EINVAL;
+
+		/* Default not OK */
+		hdcp_authencr.resp_size = 0;
+
+		if (hdcpchkaesotp(hdev, &value, &aes_status)) {
+			result = HDMI_AES_NOT_FUSED;
+			goto hdcp_authencr_end;
+		}
+
+		if (!aes_status) {
+			/* AES is not fused */
+			result = HDMI_AES_NOT_FUSED;
+			goto hdcp_authencr_end;
+		}
+
+		if (hdcpauthencr(hdev, hdcp_authencr.auth_type,
+				hdcp_authencr.encr_type,
+				&value,
+				buf)) {
+			result = HDMI_RESULT_NOT_OK;
+			goto hdcp_authencr_end;
+		}
+
+		if (value > AUTH_BUF_LEN)
+			value = AUTH_BUF_LEN;
+
+		result = HDMI_RESULT_OK;
+		hdcp_authencr.resp_size = value;
+		memcpy(hdcp_authencr.resp, buf, value);
+
+hdcp_authencr_end:
+		hdcp_authencr.result = result;
+		if (copy_to_user((void *)arg, (void *)&hdcp_authencr,
+			sizeof(struct hdcp_authencr)))
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDCP_STATE_GET:
+		if (av8100_reg_gen_status_r(NULL, NULL, NULL, NULL, NULL,
+				&value))
+				return -EINVAL;
+
+		if (copy_to_user((void *)arg, (void *)&value,
+			sizeof(u8))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_EVENTS_READ:
+		value = events_read(hdev);
+
+		if (copy_to_user((void *)arg, (void *)&value,
+			sizeof(u8))) {
+			return -EINVAL;
+		}
+
+		/* Events are read: clear events */
+		events_clear(hdev, EVENTS_MASK);
+		break;
+
+	case IOC_EVENTS_CLEAR:
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EINVAL;
+
+		events_clear(hdev, value);
+		break;
+
+	case IOC_AUDIO_CFG:
+		{
+		struct audio_cfg audio_cfg;
+
+		if (copy_from_user(&audio_cfg, (void *)arg,
+				sizeof(struct audio_cfg)))
+			return -EINVAL;
+
+		audiocfg(hdev, &audio_cfg);
+		}
+		break;
+
+	case IOC_PLUG_STATUS:
+		status = av8100_status_get();
+		value = status.av8100_plugin_status == AV8100_HDMI_PLUGIN;
+
+		if (copy_to_user((void *)arg, (void *)&value,
+			sizeof(u8))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_POWERONOFF:
+		/* Get desired power state on or off */
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EINVAL;
+
+		if (value == 0) {
+			if (av8100_powerdown() != 0) {
+				dev_err(hdev->dev, "av8100_powerdown FAIL\n");
+				return -EINVAL;
+			}
+		} else {
+			if (av8100_powerup() != 0) {
+				dev_err(hdev->dev, "av8100_powerup FAIL\n");
+				return -EINVAL;
+			}
+		}
+		break;
+
+	case IOC_EVENT_WAKEUP:
+		/* Trigger event */
+		event_wakeup(hdev);
+		break;
+
+	case IOC_POWERSTATE:
+		status = av8100_status_get();
+		value = status.av8100_state >= AV8100_OPMODE_SCAN;
+
+		if (copy_to_user((void *)arg, (void *)&value,
+						sizeof(u8))) {
+			return -EINVAL;
+		}
+		break;
+
+	/* Internal */
+	case IOC_HDMI_ENABLE_INTERRUPTS:
+		av8100_disable_interrupt();
+		if (av8100_enable_interrupt() != 0) {
+			dev_err(hdev->dev, "av8100_ei FAIL\n");
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_DOWNLOAD_FW:
+		if (av8100_download_firmware(I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100 dl fw FAIL\n");
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_ONOFF:
+		{
+		union av8100_configuration config;
+
+		/* Get desired HDMI mode on or off */
+		if (copy_from_user(&value, (void *)arg, sizeof(u8)))
+			return -EFAULT;
+
+		if (av8100_conf_get(AV8100_COMMAND_HDMI, &config) != 0) {
+			dev_err(hdev->dev, "av8100_conf_get FAIL\n");
+			return -EINVAL;
+		}
+		if (value == 0)
+			config.hdmi_format.hdmi_mode = AV8100_HDMI_OFF;
+		else
+			config.hdmi_format.hdmi_mode = AV8100_HDMI_ON;
+
+		if (av8100_conf_prep(AV8100_COMMAND_HDMI, &config) != 0) {
+			dev_err(hdev->dev, "av8100_conf_prep FAIL\n");
+			return -EINVAL;
+		}
+		if (av8100_conf_w(AV8100_COMMAND_HDMI, NULL, NULL,
+			I2C_INTERFACE) != 0) {
+			dev_err(hdev->dev, "av8100_conf_w FAIL\n");
+			return -EINVAL;
+		}
+		}
+		break;
+
+	case IOC_HDMI_REGISTER_WRITE:
+		if (copy_from_user(&reg, (void *)arg,
+			sizeof(struct hdmi_register))) {
+			return -EINVAL;
+		}
+
+		if (av8100_reg_w(reg.offset, reg.value) != 0) {
+			dev_err(hdev->dev, "hdmi_register_write FAIL\n");
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_REGISTER_READ:
+		if (copy_from_user(&reg, (void *)arg,
+			sizeof(struct hdmi_register))) {
+			return -EINVAL;
+		}
+
+		if (av8100_reg_r(reg.offset, &reg.value) != 0) {
+			dev_err(hdev->dev, "hdmi_register_write FAIL\n");
+			return -EINVAL;
+		}
+
+		if (copy_to_user((void *)arg, (void *)&reg,
+			sizeof(struct hdmi_register))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_STATUS_GET:
+		status = av8100_status_get();
+
+		if (copy_to_user((void *)arg, (void *)&status,
+			sizeof(struct av8100_status))) {
+			return -EINVAL;
+		}
+		break;
+
+	case IOC_HDMI_CONFIGURATION_WRITE:
+		{
+		struct hdmi_command_register command_reg;
+
+		if (copy_from_user(&command_reg, (void *)arg,
+				sizeof(struct hdmi_command_register)) != 0) {
+			dev_err(hdev->dev, "IOC_HDMI_CONFIGURATION_WRITE "
+				"fail 1\n");
+			command_reg.return_status = EINVAL;
+		} else {
+			command_reg.return_status = 0;
+			if (av8100_conf_w_raw(command_reg.cmd_id,
+					command_reg.buf_len,
+					command_reg.buf,
+					&(command_reg.buf_len),
+					command_reg.buf) != 0) {
+				dev_err(hdev->dev,
+					"IOC_HDMI_CONFIGURATION_WRITE "
+					"fail 2\n");
+				command_reg.return_status = EINVAL;
+			}
+		}
+
+		if (copy_to_user((void *)arg, (void *)&command_reg,
+			sizeof(struct hdmi_command_register)) != 0) {
+			return -EINVAL;
+		}
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static unsigned int
+hdmi_poll(struct file *filp, poll_table *wait)
+{
+	unsigned int mask = 0;
+	struct hdmi_device *hdev;
+
+	hdev = devnr_to_hdev(HDMI_DEVNR_DEFAULT);
+	if (!hdev)
+		return 0;
+
+	dev_dbg(hdev->dev, "%s\n", __func__);
+
+	poll_wait(filp, &hdev->event_wq , wait);
+
+	LOCK_HDMI_EVENTS;
+	if (hdev->events_received == true) {
+		hdev->events_received = false;
+		mask = POLLIN | POLLRDNORM;
+	}
+	UNLOCK_HDMI_EVENTS;
+
+	return mask;
+}
+
+static const struct file_operations hdmi_fops = {
+	.owner =    THIS_MODULE,
+	.open =     hdmi_open,
+	.release =  hdmi_release,
+	.unlocked_ioctl = hdmi_ioctl,
+	.poll = hdmi_poll
+};
+
+/* Event callback function called by hw driver */
+void hdmi_event(enum av8100_hdmi_event ev)
+{
+	int events_old;
+	int events_new;
+	struct hdmi_device *hdev;
+	struct kobject *kobj;
+
+	hdev = devnr_to_hdev(HDMI_DEVNR_DEFAULT);
+	if (!hdev)
+		return;
+
+	dev_dbg(hdev->dev, "hdmi_event %02x\n", ev);
+
+	kobj = &(hdev->dev->kobj);
+
+	LOCK_HDMI_EVENTS;
+
+	events_old = hdev->events;
+
+	/* Set event */
+	switch (ev) {
+	case AV8100_HDMI_EVENT_HDMI_PLUGIN:
+		hdev->events &= ~HDMI_EVENT_HDMI_PLUGOUT;
+		hdev->events |= HDMI_EVENT_HDMI_PLUGIN;
+		break;
+
+	case AV8100_HDMI_EVENT_HDMI_PLUGOUT:
+		hdev->events &= ~HDMI_EVENT_HDMI_PLUGIN;
+		hdev->events |= HDMI_EVENT_HDMI_PLUGOUT;
+		cec_tx_status(hdev, CEC_TX_SET_FREE);
+		break;
+
+	case AV8100_HDMI_EVENT_CEC:
+		hdev->events |= HDMI_EVENT_CEC;
+		break;
+
+	case AV8100_HDMI_EVENT_HDCP:
+		hdev->events |= HDMI_EVENT_HDCP;
+		break;
+
+	case AV8100_HDMI_EVENT_CECTXERR:
+		hdev->events |= HDMI_EVENT_CECTXERR;
+		cec_tx_status(hdev, CEC_TX_SET_FREE);
+		break;
+
+	case AV8100_HDMI_EVENT_CECTX:
+		hdev->events |= HDMI_EVENT_CECTX;
+		cec_tx_status(hdev, CEC_TX_SET_FREE);
+		break;
+
+	default:
+		break;
+	}
+
+	events_new = hdev->events_mask & hdev->events;
+
+	UNLOCK_HDMI_EVENTS;
+
+	dev_dbg(hdev->dev, "hdmi events:%02x, events_old:%02x mask:%02x\n",
+			events_new, events_old, hdev->events_mask);
+
+	if (events_new != events_old) {
+		/* Wake up application waiting for event via call to poll() */
+		sysfs_notify(kobj, NULL, SYSFS_EVENT_FILENAME);
+
+		LOCK_HDMI_EVENTS;
+		hdev->events_received = true;
+		UNLOCK_HDMI_EVENTS;
+
+		wake_up_interruptible(&hdev->event_wq);
+	}
+}
+EXPORT_SYMBOL(hdmi_event);
+
+int hdmi_device_register(struct hdmi_device *hdev)
+{
+	hdev->miscdev.minor = MISC_DYNAMIC_MINOR;
+	hdev->miscdev.name = "hdmi";
+	hdev->miscdev.fops = &hdmi_fops;
+
+	if (misc_register(&hdev->miscdev)) {
+		pr_err("hdmi misc_register failed\n");
+		return -EFAULT;
+	}
+
+	hdev->dev = hdev->miscdev.this_device;
+
+	return 0;
+}
+
+int __init hdmi_init(void)
+{
+	struct hdmi_device *hdev;
+	int i;
+	int ret;
+
+	/* Allocate device data */
+	hdev = kzalloc(sizeof(struct hdmi_device), GFP_KERNEL);
+	if (!hdev) {
+		pr_err("%s: Alloc failure\n", __func__);
+		return -ENOMEM;
+	}
+
+	/* Add to list */
+	list_add_tail(&hdev->list, &hdmi_device_list);
+
+	if (hdmi_device_register(hdev)) {
+		pr_err("%s: Alloc failure\n", __func__);
+		return -EFAULT;
+	}
+
+	hdev->devnr = HDMI_DEVNR_DEFAULT;
+
+	/* Default sysfs file format is hextext */
+	hdev->sysfs_data.store_as_hextext = true;
+
+	init_waitqueue_head(&hdev->event_wq);
+
+	/* Create sysfs attrs */
+	for (i = 0; attr_name(hdmi_sysfs_attrs[i]); i++) {
+		ret = device_create_file(hdev->dev, &hdmi_sysfs_attrs[i]);
+		if (ret)
+			dev_err(hdev->dev,
+				"Unable to create sysfs attr %s (%d)\n",
+				hdmi_sysfs_attrs[i].attr.name, ret);
+	}
+
+	/* Register event callback */
+	av8100_hdmi_event_cb_set(hdmi_event);
+
+	return 0;
+}
+late_initcall(hdmi_init);
+
+void hdmi_exit(void)
+{
+	struct hdmi_device *hdev = NULL;
+	int i;
+
+	if (list_empty(&hdmi_device_list))
+		return;
+	else
+		hdev = list_entry(hdmi_device_list.next,
+				struct hdmi_device, list);
+
+	/* Deregister event callback */
+	av8100_hdmi_event_cb_set(NULL);
+
+	/* Remove sysfs attrs */
+	for (i = 0; attr_name(hdmi_sysfs_attrs[i]); i++)
+		device_remove_file(hdev->dev, &hdmi_sysfs_attrs[i]);
+
+	misc_deregister(&hdev->miscdev);
+
+	/* Remove from list */
+	list_del(&hdev->list);
+
+	/* Free device data */
+	kfree(hdev);
+}
diff --git a/drivers/video/av8100/hdmi_loc.h b/drivers/video/av8100/hdmi_loc.h
new file mode 100644
index 00000000000..20314910db9
--- /dev/null
+++ b/drivers/video/av8100/hdmi_loc.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#ifndef __HDMI_LOC__H__
+#define __HDMI_LOC__H__
+
+#define EDID_BUF_LEN	128
+#define COMMAND_BUF_LEN	128
+#define AES_KEY_SIZE	16
+#define CRC32_SIZE	4
+#define AUTH_BUF_LEN	126
+#define CECTX_TRY	20
+#define CECTX_WAITTIME	25
+
+struct edid_data {
+	u8 buf_len;
+	u8 buf[EDID_BUF_LEN];
+};
+
+struct authencr {
+	int result;
+	u8 buf_len;
+	u8 buf[AUTH_BUF_LEN];
+};
+
+struct hdmi_register {
+	unsigned char value;
+	unsigned char offset;
+};
+
+struct hdcp_loadaesone {
+	u8 key[AES_KEY_SIZE];
+	u8 result;
+	u8 crc32[CRC32_SIZE];
+};
+
+struct hdmi_sysfs_data {
+	bool			store_as_hextext;
+	struct plug_detect	plug_detect;
+	bool			enable_cec_event;
+	struct edid_data	edid_data;
+	struct cec_rw		cec_read;
+	bool			fuse_result;
+	int			loadaes_result;
+	struct authencr         authencr;
+};
+
+struct hdmi_command_register {
+	unsigned char cmd_id;			/* input */
+	unsigned char buf_len;			/* input, output */
+	unsigned char buf[COMMAND_BUF_LEN];	/* input, output */
+	unsigned char return_status;		/* output */
+};
+
+enum cec_tx_status_action {
+	CEC_TX_SET_FREE,
+	CEC_TX_SET_BUSY,
+	CEC_TX_CHECK
+};
+
+/* Internal */
+#define IOC_HDMI_ENABLE_INTERRUPTS	_IOWR(HDMI_IOC_MAGIC, 32, int)
+#define IOC_HDMI_DOWNLOAD_FW		_IOWR(HDMI_IOC_MAGIC, 33, int)
+#define IOC_HDMI_ONOFF			_IOWR(HDMI_IOC_MAGIC, 34, int)
+#define IOC_HDMI_REGISTER_WRITE		_IOWR(HDMI_IOC_MAGIC, 35, int)
+#define IOC_HDMI_REGISTER_READ		_IOWR(HDMI_IOC_MAGIC, 36, int)
+#define IOC_HDMI_STATUS_GET		_IOWR(HDMI_IOC_MAGIC, 37, int)
+#define IOC_HDMI_CONFIGURATION_WRITE	_IOWR(HDMI_IOC_MAGIC, 38, int)
+
+#endif /* __HDMI_LOC__H__ */
diff --git a/drivers/video/b2r2/Kconfig b/drivers/video/b2r2/Kconfig
new file mode 100644
index 00000000000..8cc81876de7
--- /dev/null
+++ b/drivers/video/b2r2/Kconfig
@@ -0,0 +1,134 @@
+config FB_B2R2
+	tristate "B2R2 engine support"
+	default n
+	help
+	  B2R2 engine does various bit-blitting operations,post-processor operations
+	  and various compositions.
+
+config B2R2_PLUG_CONF
+	bool "B2R2 bus plug configuration"
+	depends on FB_B2R2
+	default n
+	help
+	  Configures how B2R2 access the memory bus. Enabling this will increase
+	  the performance of B2R2 at the cost of using the bus more heavily.
+
+	  If this is set to 'n', the hardware defaults will be used.
+
+choice
+	prompt "Opcode size"
+	depends on B2R2_PLUG_CONF
+	default B2R2_OPSIZE_64
+
+	config B2R2_OPSIZE_8
+		bool "8 bytes"
+	config B2R2_OPSIZE_16
+		bool "16 bytes"
+	config B2R2_OPSIZE_32
+		bool "32 bytes"
+	config B2R2_OPSIZE_64
+		bool "64 bytes"
+
+endchoice
+
+choice
+	prompt "Chunk size"
+	depends on B2R2_PLUG_CONF
+	default B2R2_CHSIZE_128
+
+	config B2R2_CHSIZE_1
+		bool "1 op"
+	config B2R2_CHSIZE_2
+		bool "2 ops"
+	config B2R2_CHSIZE_4
+		bool "4 ops"
+	config B2R2_CHSIZE_8
+		bool "8 ops"
+	config B2R2_CHSIZE_16
+		bool "16 ops"
+	config B2R2_CHSIZE_32
+		bool "32 ops"
+	config B2R2_CHSIZE_64
+		bool "64 ops"
+	config B2R2_CHSIZE_128
+		bool "128 ops"
+endchoice
+
+choice
+	prompt "Message size"
+	depends on B2R2_PLUG_CONF
+	default B2R2_MGSIZE_128
+
+	config B2R2_MGSIZE_1
+		bool "1 chunk"
+	config B2R2_MGSIZE_2
+		bool "2 chunks"
+	config B2R2_MGSIZE_4
+		bool "4 chunks"
+	config B2R2_MGSIZE_8
+		bool "8 s"
+	config B2R2_MGSIZE_16
+		bool "16 chunks"
+	config B2R2_MGSIZE_32
+		bool "32 chunks"
+	config B2R2_MGSIZE_64
+		bool "64 chunks"
+	config B2R2_MGSIZE_128
+		bool "128 chunks"
+endchoice
+
+choice
+	prompt "Page size"
+	depends on B2R2_PLUG_CONF
+	default B2R2_PGSIZE_256
+
+	config B2R2_PGSIZE_64
+		bool "64 bytes"
+	config B2R2_PGSIZE_128
+		bool "128 bytes"
+	config B2R2_PGSIZE_256
+		bool "256 bytes"
+endchoice
+
+config B2R2_DEBUG
+	bool "B2R2 debugging"
+	default n
+	depends on FB_B2R2
+	help
+	  Enable debugging features for the B2R2 driver.
+
+config B2R2_PROFILER
+	tristate "B2R2 profiler"
+	default n
+	depends on FB_B2R2
+	help
+	  Enables the profiler for the B2R2 driver.
+
+	  It is recommended to build this as a module, since the configuration
+	  of filters etc. is done at load time.
+
+config B2R2_GENERIC
+	bool "B2R2 generic path"
+	default y
+	depends on FB_B2R2
+	help
+	  Enables support for the generic path in the B2R2 driver. This path should
+	  be used when there is no optimized implementation for a request.
+
+choice
+	prompt "Generic usage mode"
+	depends on B2R2_GENERIC
+	default B2R2_GENERIC_FALLBACK
+
+	config B2R2_GENERIC_FALLBACK
+		bool "Fallback"
+		help
+		  The optimized path will be used for all supported operations, and the
+		  generic path will be used as a fallback for the ones not implemented.
+
+	config B2R2_GENERIC_ONLY
+		bool "Always"
+		help
+		  The generic path will be used for all operations.
+
+endchoice
diff --git a/drivers/video/b2r2/Makefile b/drivers/video/b2r2/Makefile
new file mode 100644
index 00000000000..f271f4e7ea1
--- /dev/null
+++ b/drivers/video/b2r2/Makefile
@@ -0,0 +1,15 @@
+# Make file for compiling and loadable module B2R2
+
+obj-$(CONFIG_FB_B2R2) += b2r2.o
+
+b2r2-objs = b2r2_api.o b2r2_blt_main.o b2r2_core.o b2r2_mem_alloc.o b2r2_generic.o b2r2_node_gen.o b2r2_node_split.o b2r2_profiler_socket.o b2r2_timing.o b2r2_filters.o b2r2_utils.o b2r2_input_validation.o b2r2_hw_convert.o
+
+ifdef CONFIG_B2R2_DEBUG
+b2r2-objs += b2r2_debug.o
+endif
+
+ifeq ($(CONFIG_FB_B2R2),m)
+obj-y += b2r2_kernel_if.o
+endif
+
+obj-$(CONFIG_B2R2_PROFILER) += b2r2_profiler/
diff --git a/drivers/video/b2r2/b2r2_api.c b/drivers/video/b2r2/b2r2_api.c
new file mode 100644
index 00000000000..0361e85ebf3
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_api.c
@@ -0,0 +1,1643 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010/2012
+ *
+ * ST-Ericsson B2R2 Blitter module API
+ *
+ * Author: Jorgen Nilsson <jorgen.nilsson@stericsson.com>
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/list.h>
+#ifdef CONFIG_ANDROID_PMEM
+#include <linux/android_pmem.h>
+#endif
+#include <linux/fb.h>
+#include <linux/uaccess.h>
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#endif
+#include <asm/cacheflush.h>
+#include <linux/smp.h>
+#include <linux/dma-mapping.h>
+#include <linux/sched.h>
+#include <linux/err.h>
+#include <linux/hwmem.h>
+#include <linux/kref.h>
+
+#include "b2r2_internal.h"
+#include "b2r2_control.h"
+#include "b2r2_core.h"
+#include "b2r2_timing.h"
+#include "b2r2_utils.h"
+#include "b2r2_debug.h"
+#include "b2r2_input_validation.h"
+#include "b2r2_profiler_socket.h"
+#include "b2r2_hw.h"
+
+/*
+ * TODO:
+ * Implementation of query cap
+ * Support for user space virtual pointer to physically consecutive memory
+ * Support for user space virtual pointer to physically scattered memory
+ * Callback reads lagging behind in blt_api_stress app
+ * Store smaller items in the report list instead of the whole request
+ * Support read of many report records at once.
+ */
+
+#define DATAS_START_SIZE 10
+#define DATAS_GROW_SIZE 5
+
+/**
+ * @miscdev: The miscdev presenting b2r2 to the system
+ */
+struct b2r2_blt {
+	spinlock_t                      lock;
+	int                             next_job_id;
+	struct miscdevice               miscdev;
+	struct device                   *dev;
+	struct mutex                    datas_lock;
+	/**
+	 * datas - Stores the b2r2_blt_data mapped to the cliend handle
+	 */
+	struct b2r2_blt_data **datas;
+	/**
+	 * data_count - The current maximum of active datas
+	 */
+	int data_count;
+};
+
+struct b2r2_blt_data {
+	struct b2r2_control_instance *ctl_instace[B2R2_MAX_NBR_DEVICES];
+};
+
+/**
+ * Used to keep track of coming and going b2r2 cores and
+ * the number of active instance references
+ */
+struct b2r2_control_ref {
+	struct b2r2_control *b2r2_control;
+	spinlock_t lock;
+};
+
+/**
+ * b2r2_blt - The blitter device, /dev/b2r2_blt
+ */
+struct kref blt_refcount;
+static struct b2r2_blt *b2r2_blt;
+
+/**
+ * b2r2_control - The core controls and synchronization mechanism
+ */
+static struct b2r2_control_ref b2r2_controls[B2R2_MAX_NBR_DEVICES];
+
+/**
+ * b2r2_blt_add_control - Add the b2r2 core control
+ */
+void b2r2_blt_add_control(struct b2r2_control *cont)
+{
+	unsigned long flags;
+	BUG_ON(cont->id < 0 || cont->id >= B2R2_MAX_NBR_DEVICES);
+
+	spin_lock_irqsave(&b2r2_controls[cont->id].lock, flags);
+	if (b2r2_controls[cont->id].b2r2_control == NULL)
+		b2r2_controls[cont->id].b2r2_control = cont;
+	spin_unlock_irqrestore(&b2r2_controls[cont->id].lock, flags);
+}
+
+/**
+ * b2r2_blt_remove_control - Remove the b2r2 core control
+ */
+void b2r2_blt_remove_control(struct b2r2_control *cont)
+{
+	unsigned long flags;
+	BUG_ON(cont->id < 0 || cont->id >= B2R2_MAX_NBR_DEVICES);
+
+	spin_lock_irqsave(&b2r2_controls[cont->id].lock, flags);
+	b2r2_controls[cont->id].b2r2_control = NULL;
+	spin_unlock_irqrestore(&b2r2_controls[cont->id].lock, flags);
+}
+
+/**
+ * b2r2_blt_get_control - Lock control for writing/removal
+ */
+static struct b2r2_control *b2r2_blt_get_control(int i)
+{
+	struct b2r2_control *cont;
+	unsigned long flags;
+	BUG_ON(i < 0 || i >= B2R2_MAX_NBR_DEVICES);
+
+	spin_lock_irqsave(&b2r2_controls[i].lock, flags);
+	cont = (struct b2r2_control *) b2r2_controls[i].b2r2_control;
+	if (cont != NULL) {
+		if (!cont->enabled)
+			cont = NULL;
+		else
+			kref_get(&cont->ref);
+	}
+	spin_unlock_irqrestore(&b2r2_controls[i].lock, flags);
+
+	return cont;
+}
+
+/**
+ * b2r2_blt_release_control - Unlock control for writing/removal
+ */
+static void b2r2_blt_release_control(int i)
+{
+	struct b2r2_control *cont;
+	unsigned long flags;
+	BUG_ON(i < 0 || i >= B2R2_MAX_NBR_DEVICES);
+
+	spin_lock_irqsave(&b2r2_controls[i].lock, flags);
+	cont = (struct b2r2_control *) b2r2_controls[i].b2r2_control;
+	spin_unlock_irqrestore(&b2r2_controls[i].lock, flags);
+	if (cont != NULL)
+		kref_put(&cont->ref, b2r2_core_release);
+}
+
+/**
+ * Increase size of array containing b2r2 handles
+ */
+static int grow_datas(void)
+{
+	struct b2r2_blt_data **new_datas = NULL;
+	int new_data_count = b2r2_blt->data_count + DATAS_GROW_SIZE;
+	int ret = 0;
+
+	new_datas = kzalloc(new_data_count * sizeof(*new_datas), GFP_KERNEL);
+	if (new_datas == NULL) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	memcpy(new_datas, b2r2_blt->datas,
+		b2r2_blt->data_count * sizeof(*b2r2_blt->datas));
+
+	kfree(b2r2_blt->datas);
+
+	b2r2_blt->data_count = new_data_count;
+	b2r2_blt->datas = new_datas;
+exit:
+	return ret;
+}
+
+/**
+ * Allocate and/or reserve a b2r2 handle
+ */
+static int alloc_handle(struct b2r2_blt_data *blt_data)
+{
+	int handle;
+	int ret;
+
+	mutex_lock(&b2r2_blt->datas_lock);
+
+	if (b2r2_blt->datas == NULL) {
+		b2r2_blt->datas = kzalloc(
+			DATAS_START_SIZE * sizeof(*b2r2_blt->datas),
+			GFP_KERNEL);
+		if (b2r2_blt->datas == NULL) {
+			ret = -ENOMEM;
+			goto exit;
+		}
+		b2r2_blt->data_count = DATAS_START_SIZE;
+	}
+
+	for (handle = 0; handle < b2r2_blt->data_count; handle++) {
+		if (b2r2_blt->datas[handle] == NULL) {
+			b2r2_blt->datas[handle] = blt_data;
+			break;
+		}
+
+		if (handle == b2r2_blt->data_count - 1) {
+			ret = grow_datas();
+			if (ret < 0)
+				goto exit;
+		}
+	}
+	ret = handle;
+exit:
+	mutex_unlock(&b2r2_blt->datas_lock);
+
+	return ret;
+}
+
+/**
+ * Get b2r2 data from b2r2 handle
+ */
+static struct b2r2_blt_data *get_data(int handle)
+{
+	if (handle >= b2r2_blt->data_count || handle < 0)
+		return NULL;
+	else
+		return b2r2_blt->datas[handle];
+}
+
+/**
+ * Unreserve b2r2 handle
+ */
+static void free_handle(int handle)
+{
+	if (handle < b2r2_blt->data_count && handle >= 0)
+		b2r2_blt->datas[handle] = NULL;
+}
+
+/**
+ * Get the next job number. This is the one returned to the client
+ * if the blit request was successful.
+ */
+static int get_next_job_id(void)
+{
+	int job_id;
+	unsigned long flags;
+
+	spin_lock_irqsave(&b2r2_blt->lock, flags);
+	if (b2r2_blt->next_job_id < 1)
+		b2r2_blt->next_job_id = 1;
+	job_id = b2r2_blt->next_job_id++;
+	spin_unlock_irqrestore(&b2r2_blt->lock, flags);
+
+	return job_id;
+}
+
+/**
+ * Limit the number of cores used in some "easy" and impossible cases
+ */
+static int limit_blits(int n_split, struct b2r2_blt_req *user_req)
+{
+	if (n_split <= 1)
+		return n_split;
+
+	if (user_req->dst_rect.width < 24 && user_req->dst_rect.height < 24)
+		return 1;
+
+	if (user_req->src_rect.width < n_split &&
+			user_req->src_rect.height < n_split)
+		return 1;
+
+	return n_split;
+}
+
+/**
+ * Check if the format inherently requires the b2r2 scaling engine to be active
+ */
+static bool is_scaling_fmt(enum b2r2_blt_fmt fmt)
+{
+	/* Plane separated formats must be treated as scaling */
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * Check for macroblock formats
+ */
+static bool is_mb_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * Split a request rectangle on available cores
+ */
+static int b2r2_blt_split_request(struct b2r2_blt_data *blt_data,
+		struct b2r2_blt_req *user_req,
+		struct b2r2_blt_request **split_requests,
+		struct b2r2_control_instance **ctl,
+		int *n_split)
+{
+	int sstep_x, sstep_y, dstep_x, dstep_y;
+	int dstart_x, dstart_y;
+	int bstart_x, bstart_y;
+	int dpos_x, dpos_y;
+	int bpos_x, bpos_y;
+	int dso_x = 1;
+	int dso_y = 1;
+	int sf_x, sf_y;
+	int i;
+	int srw, srh;
+	int drw, drh;
+	bool ssplit_x = true;
+	bool dsplit_x = true;
+	enum b2r2_blt_transform transform;
+	bool is_rotation = false;
+	bool is_scaling = false;
+	bool bg_blend = false;
+	u32 core_mask = 0;
+
+	srw = user_req->src_rect.width;
+	srh = user_req->src_rect.height;
+	drw = user_req->dst_rect.width;
+	drh = user_req->dst_rect.height;
+	transform = user_req->transform;
+
+	/* Early exit in the basic cases */
+	if (*n_split == 0) {
+		return -ENOSYS;
+	} else if (*n_split == 1 ||
+			(srw < *n_split && srh < *n_split) ||
+			(drw < *n_split && drh < *n_split) ||
+			is_mb_fmt(user_req->src_img.fmt)) {
+		/* Handle macroblock formats with one
+		 * core for now since there seems to be some bug
+		 * related to macroblock access patterns
+		 */
+		memcpy(&split_requests[0]->user_req,
+				user_req,
+				sizeof(*user_req));
+		split_requests[0]->core_mask = 1;
+		*n_split = 1;
+		return 0;
+	}
+
+	/*
+	 * TODO: fix the load balancing algorithm
+	 */
+
+	is_rotation = (transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) != 0;
+
+	/* Check for scaling */
+	if (is_rotation) {
+		is_scaling = (user_req->src_rect.width !=
+				user_req->dst_rect.height) ||
+			(user_req->src_rect.height !=
+				user_req->dst_rect.width);
+	} else {
+		is_scaling = (user_req->src_rect.width !=
+				user_req->dst_rect.width) ||
+			(user_req->src_rect.height !=
+				user_req->dst_rect.height);
+	}
+
+	is_scaling = is_scaling ||
+			is_scaling_fmt(user_req->src_img.fmt) ||
+			is_scaling_fmt(user_req->dst_img.fmt);
+
+	bg_blend = ((user_req->flags & B2R2_BLT_FLAG_BG_BLEND) != 0);
+
+	/*
+	 * Split the request
+	 */
+
+	b2r2_log_info(b2r2_blt->dev, "%s: In (t:0x%08X, f:0x%08X):\n"
+		"\tsrc_rect x:%d, y:%d, w:%d, h:%d src fmt:0x%x\n"
+		"\tdst_rect x:%d, y:%d, w:%d, h:%d dst fmt:0x%x\n",
+		__func__,
+		user_req->transform,
+		user_req->flags,
+		user_req->src_rect.x,
+		user_req->src_rect.y,
+		user_req->src_rect.width,
+		user_req->src_rect.height,
+		user_req->src_img.fmt,
+		user_req->dst_rect.x,
+		user_req->dst_rect.y,
+		user_req->dst_rect.width,
+		user_req->dst_rect.height,
+		user_req->dst_img.fmt);
+
+	/* TODO: We need sub pixel precision here,
+	 * or a better way to split rects */
+	dstart_x = user_req->dst_rect.x;
+	dstart_y = user_req->dst_rect.y;
+	if (bg_blend) {
+		bstart_x = user_req->bg_rect.x;
+		bstart_y = user_req->bg_rect.y;
+	}
+
+	if (srw && srh) {
+		if ((srw < srh) && !is_scaling) {
+			ssplit_x = false;
+			sstep_y = srh / *n_split;
+			/* Round up */
+			if (srh % (*n_split))
+				sstep_y++;
+
+			if (srh > 16)
+				sstep_y = ((sstep_y + 16) >> 4) << 4;
+
+			if (transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+				sf_y = (drw << 10) / srh;
+				dstep_x = (sf_y * sstep_y) >> 10;
+			} else {
+				dsplit_x = false;
+				sf_y = (drh << 10) / srh;
+				dstep_y = (sf_y * sstep_y) >> 10;
+			}
+		} else {
+			sstep_x = srw / *n_split;
+			/* Round up */
+			if (srw % (*n_split))
+				sstep_x++;
+
+			if (is_scaling) {
+				int scale_step_size =
+						B2R2_RESCALE_MAX_WIDTH - 1;
+				int pad = (scale_step_size -
+						(sstep_x % scale_step_size));
+				if ((sstep_x + pad) < srw)
+					sstep_x += pad;
+			} else {
+				/* Aim for even 16px multiples */
+				if ((sstep_x & 0xF) && ((sstep_x + 16) < srw))
+					sstep_x = ((sstep_x + 16) >> 4) << 4;
+			}
+
+			if (transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+				dsplit_x = false;
+				sf_x = (drh << 10) / srw;
+				dstep_y = (sf_x * sstep_x) >> 10;
+			} else {
+				sf_x = (drw << 10) / srw;
+				dstep_x = (sf_x * sstep_x) >> 10;
+			}
+		}
+
+	} else {
+		sstep_x = sstep_y = 0;
+
+		if (drw < drh) {
+			dsplit_x = false;
+			dstep_y = drh / *n_split;
+			/* Round up */
+			if (drh % *n_split)
+				dstep_y++;
+
+			/* Aim for even 16px multiples */
+			if ((dstep_y & 0xF) && ((dstep_y + 16) < drh))
+				dstep_y = ((dstep_y + 16) >> 4) << 4;
+		} else {
+			dstep_x = drw / *n_split;
+			/* Round up */
+			if (drw % *n_split)
+				dstep_x++;
+
+			/* Aim for even 16px multiples */
+			if ((dstep_x & 0xF) && ((dstep_x + 16) < drw))
+				dstep_x = ((dstep_x + 16) >> 4) << 4;
+		}
+	}
+
+	/* Check for flip and rotate to establish destination
+	 * step order */
+	if (transform & B2R2_BLT_TRANSFORM_FLIP_H) {
+		dstart_x +=  drw;
+		if (bg_blend)
+			bstart_x +=  drw;
+		dso_x = -1;
+	}
+	if ((transform & B2R2_BLT_TRANSFORM_FLIP_V) ||
+			(transform & B2R2_BLT_TRANSFORM_CCW_ROT_90)) {
+		dstart_y += drh;
+		if (bg_blend)
+			bstart_y += drh;
+		dso_y = -1;
+	}
+
+	/* Set scan starting position */
+	dpos_x = dstart_x;
+	dpos_y = dstart_y;
+	if (bg_blend) {
+		bpos_x = bstart_x;
+		bpos_y = bstart_y;
+	}
+
+	for (i = 0; i < *n_split; i++) {
+		struct b2r2_blt_req *sreq =
+				&split_requests[i]->user_req;
+
+		/* First mimic all */
+		memcpy(sreq, user_req, sizeof(*user_req));
+
+		/* Then change the rects */
+		if (srw && srh) {
+			if (ssplit_x) {
+				if (sstep_x > 0) {
+					sreq->src_rect.width =
+						min(sstep_x, srw);
+					sreq->src_rect.x += i*sstep_x;
+					srw -= sstep_x;
+				} else {
+					sreq->src_rect.width = srw;
+				}
+			} else {
+				if (sstep_y > 0) {
+					sreq->src_rect.y += i*sstep_y;
+					sreq->src_rect.height =
+						min(sstep_y, srh);
+					srh -= sstep_y;
+				} else {
+					sreq->src_rect.height = srh;
+				}
+			}
+		}
+
+		if (dsplit_x) {
+			int sx = min(dstep_x, drw);
+			if (dso_x < 0) {
+				dpos_x += dso_x * sx;
+				if (bg_blend)
+					bpos_x += dso_x * sx;
+			}
+			sreq->dst_rect.width = sx;
+			sreq->dst_rect.x = dpos_x;
+			if (bg_blend) {
+				sreq->bg_rect.width = sx;
+				sreq->bg_rect.x = bpos_x;
+			}
+			if (dso_x > 0) {
+				dpos_x += dso_x * sx;
+				if (bg_blend)
+					bpos_x += dso_x * sx;
+			}
+			drw -= sx;
+		} else {
+			int sy = min(dstep_y, drh);
+			if (dso_y < 0) {
+				dpos_y += dso_y * sy;
+				if (bg_blend)
+					bpos_y += dso_y * sy;
+			}
+			sreq->dst_rect.height = sy;
+			sreq->dst_rect.y = dpos_y;
+			if (bg_blend) {
+				sreq->bg_rect.height = sy;
+				sreq->bg_rect.y = bpos_y;
+			}
+			if (dso_y > 0) {
+				dpos_y += dso_y * sy;
+				if (bg_blend)
+					bpos_y += dso_y * sy;
+			}
+			drh -= sy;
+		}
+
+		b2r2_log_info(b2r2_blt->dev, "%s: Out:\n"
+				"\tsrc_rect x:%d, y:%d, w:%d, h:%d\n"
+				"\tdst_rect x:%d, y:%d, w:%d, h:%d\n"
+				"\tbg_rect  x:%d, y:%d, w:%d, h:%d\n",
+				__func__,
+				sreq->src_rect.x,
+				sreq->src_rect.y,
+				sreq->src_rect.width,
+				sreq->src_rect.height,
+				sreq->dst_rect.x,
+				sreq->dst_rect.y,
+				sreq->dst_rect.width,
+				sreq->dst_rect.height,
+				sreq->bg_rect.x,
+				sreq->bg_rect.y,
+				sreq->bg_rect.width,
+				sreq->bg_rect.height);
+
+		core_mask |= (1 << i);
+	}
+
+	for (i = 0; i < *n_split; i++)
+		split_requests[i]->core_mask = core_mask;
+
+	return 0;
+}
+
+/**
+ * Get available b2r2 control instances. It will be limited
+ * to the number of cores available at the current point in time.
+ * It will also cause the cores to stay active during the time until
+ * release_control_instances is called.
+ */
+static void get_control_instances(struct b2r2_blt_data *blt_data,
+		struct b2r2_control_instance **ctl, int max_size,
+		int *count)
+{
+	int i;
+
+	*count = 0;
+	for (i = 0; i < max_size; i++) {
+		struct b2r2_control_instance *ci = blt_data->ctl_instace[i];
+		if (ci) {
+			struct b2r2_control *cont =
+				b2r2_blt_get_control(ci->control_id);
+			if (cont) {
+				ctl[*count] = ci;
+				*count += 1;
+			}
+		}
+	}
+}
+
+/**
+ * Release b2r2 control instances. The cores allocated for the request
+ * are given back.
+ */
+static void release_control_instances(struct b2r2_control_instance **ctl,
+		int count)
+{
+	int i;
+
+	/* Release the handles to the core controls */
+	for (i = 0; i < count; i++) {
+		if (ctl[i])
+			b2r2_blt_release_control(ctl[i]->control_id);
+	}
+}
+
+/**
+ * Free b2r2 request
+ */
+static void b2r2_free_request(struct b2r2_blt_request *request)
+{
+	if (request) {
+		/* Free requests in split_requests */
+		if (request->clut)
+			dma_free_coherent(b2r2_blt->dev,
+				CLUT_SIZE,
+				request->clut,
+				request->clut_phys_addr);
+		request->clut = NULL;
+		request->clut_phys_addr = 0;
+		kfree(request);
+	}
+}
+
+/**
+ * Allocate internal b2r2 request based on user input.
+ */
+static int b2r2_alloc_request(struct b2r2_blt_req *user_req,
+		bool us_req, struct b2r2_blt_request **request_out)
+{
+	int ret = 0;
+	struct b2r2_blt_request *request =
+			kzalloc(sizeof(*request), GFP_KERNEL);
+	if (!request)
+		return -ENOMEM;
+
+	/* Initialize the structure */
+	INIT_LIST_HEAD(&request->list);
+
+	/*
+	 * If the user specified a color look-up table,
+	 * make a copy that the HW can use.
+	 */
+	if ((user_req->flags &
+		B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) != 0) {
+		request->clut = dma_alloc_coherent(
+			b2r2_blt->dev,
+			CLUT_SIZE,
+			&(request->clut_phys_addr),
+			GFP_DMA | GFP_KERNEL);
+		if (request->clut == NULL) {
+			b2r2_log_err(b2r2_blt->dev,
+				"%s CLUT allocation "
+				"failed.\n", __func__);
+			ret = -ENOMEM;
+			goto exit;
+		}
+
+		if (us_req) {
+			if (copy_from_user(request->clut,
+				user_req->clut, CLUT_SIZE)) {
+				b2r2_log_err(b2r2_blt->dev, "%s: CLUT "
+					"copy_from_user failed\n",
+					__func__);
+				ret = -EFAULT;
+				goto exit;
+			}
+		} else {
+			memcpy(request->clut, user_req->clut,
+					CLUT_SIZE);
+		}
+	}
+
+	request->profile = is_profiler_registered_approx();
+
+	*request_out = request;
+exit:
+	if (ret != 0)
+		b2r2_free_request(request);
+
+	return ret;
+}
+
+/**
+ * Do the blit job split on available cores.
+ */
+static int b2r2_blt_blit_internal(int handle,
+		struct b2r2_blt_req *user_req,
+		bool us_req)
+{
+	int request_id;
+	int i;
+	int n_instance = 0;
+	int n_blit = 0;
+	int ret = 0;
+	struct b2r2_blt_data *blt_data;
+	struct b2r2_blt_req ureq;
+
+	/* The requests and the designated workers */
+	struct b2r2_blt_request *split_requests[B2R2_MAX_NBR_DEVICES];
+	struct b2r2_control_instance *ctl[B2R2_MAX_NBR_DEVICES];
+
+	blt_data = get_data(handle);
+	if (blt_data == NULL) {
+		b2r2_log_warn(b2r2_blt->dev,
+			"%s, blitter instance not found (handle=%d)\n",
+			__func__, handle);
+		return -ENOSYS;
+	}
+
+	/* Get the b2r2 core controls for the job */
+	get_control_instances(blt_data, ctl, B2R2_MAX_NBR_DEVICES, &n_instance);
+	if (n_instance == 0) {
+		b2r2_log_err(b2r2_blt->dev, "%s: No b2r2 cores available.\n",
+			__func__);
+		return -ENOSYS;
+	}
+
+	/* Get the user data */
+	if (us_req) {
+		if (copy_from_user(&ureq, user_req, sizeof(ureq))) {
+			b2r2_log_err(b2r2_blt->dev,
+				"%s: copy_from_user failed\n",
+				__func__);
+			ret = -EFAULT;
+			goto exit;
+		}
+	} else {
+		memcpy(&ureq, user_req, sizeof(ureq));
+	}
+
+	/*
+	 * B2R2 cannot handle destination clipping on buffers
+	 * allocated close to 64MiB bank boundaries.
+	 * recalculate src_ and dst_rect to avoid clipping.
+	 *
+	 * Also this is needed to ensure the request split
+	 * operates on visible areas
+	 */
+	b2r2_recalculate_rects(b2r2_blt->dev, &ureq);
+
+	if (!b2r2_validate_user_req(b2r2_blt->dev, &ureq)) {
+		b2r2_log_warn(b2r2_blt->dev,
+			"%s: b2r2_validate_user_req failed.\n",
+			__func__);
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	/* Don't split small requests */
+	n_blit = limit_blits(n_instance, &ureq);
+
+	/* The id needs to be universal on
+	 * all cores */
+	request_id = get_next_job_id();
+
+#ifdef CONFIG_B2R2_GENERIC_ONLY
+	/* Limit the generic only solution to one core (for now) */
+	n_blit = 1;
+#endif
+
+	for (i = 0; i < n_blit; i++) {
+		ret = b2r2_alloc_request(&ureq, us_req, &split_requests[i]);
+		if (ret < 0 || !split_requests[i]) {
+			b2r2_log_err(b2r2_blt->dev, "%s: Failed to alloc mem\n",
+				__func__);
+			ret = -ENOMEM;
+			break;
+		}
+		split_requests[i]->instance = ctl[i];
+		split_requests[i]->job.job_id = request_id;
+		split_requests[i]->job.data = (int) ctl[i]->control->data;
+	}
+
+	/* Split the request */
+	if (ret >= 0)
+		ret = b2r2_blt_split_request(blt_data, &ureq,
+				&split_requests[0], &ctl[0], &n_blit);
+
+	/* If anything failed, clean up allocated memory */
+	if (ret < 0) {
+		for (i = 0; i < n_blit; i++)
+			b2r2_free_request(split_requests[i]);
+		b2r2_log_err(b2r2_blt->dev,
+				"%s: b2r2_blt_split_request failed.\n",
+				__func__);
+		goto exit;
+	}
+
+#ifdef CONFIG_B2R2_GENERIC_ONLY
+	if (ureq.flags & B2R2_BLT_FLAG_BG_BLEND) {
+		/* No support for BG BLEND in generic
+		 * implementation yet */
+		b2r2_log_warn(b2r2_blt->dev, "%s: Unsupported: "
+			"Background blend in b2r2_generic_blt\n",
+			__func__);
+		ret = -ENOSYS;
+		b2r2_free_request(split_requests[0]);
+		goto exit;
+	}
+		/* Use the generic path for all operations */
+	ret = b2r2_generic_blt(split_requests[0]);
+#else
+	/* Call each blitter control */
+	for (i = 0; i < n_blit; i++) {
+		ret = b2r2_control_blt(split_requests[i]);
+		if (ret < 0) {
+			b2r2_log_warn(b2r2_blt->dev,
+				"%s: b2r2_control_blt failed.\n", __func__);
+			break;
+		}
+	}
+	if (ret != -ENOSYS) {
+		int j;
+		/* TODO: if one blitter fails then cancel the jobs added */
+
+		/* Call waitjob for successful jobs
+		 * (synchs if specified in request) */
+		if (ureq.flags & B2R2_BLT_FLAG_DRY_RUN)
+			goto exit;
+
+		for (j = 0; j < i; j++) {
+			int rtmp;
+
+			rtmp = b2r2_control_waitjob(split_requests[j]);
+			if (rtmp < 0) {
+				b2r2_log_err(b2r2_blt->dev,
+					"%s: b2r2_control_waitjob failed.\n",
+					__func__);
+			}
+
+			/* Save just the one error */
+			ret = (ret >= 0) ? rtmp : ret;
+		}
+	}
+#endif
+#ifdef CONFIG_B2R2_GENERIC_FALLBACK
+	if (ret == -ENOSYS) {
+		struct b2r2_blt_request *request_gen = NULL;
+		if (ureq.flags & B2R2_BLT_FLAG_BG_BLEND) {
+			/* No support for BG BLEND in generic
+			 * implementation yet */
+			b2r2_log_warn(b2r2_blt->dev, "%s: Unsupported: "
+				"Background blend in b2r2_generic_blt\n",
+				__func__);
+			goto exit;
+		}
+
+		b2r2_log_info(b2r2_blt->dev,
+			"b2r2_blt=%d Going generic.\n", ret);
+		ret = b2r2_alloc_request(&ureq, us_req, &request_gen);
+		if (ret < 0 || !request_gen) {
+			b2r2_log_err(b2r2_blt->dev,
+				"%s: Failed to alloc mem for "
+				"request_gen\n", __func__);
+			ret = -ENOMEM;
+			goto exit;
+		}
+
+		/* Initialize the structure */
+		request_gen->instance = ctl[0];
+		memcpy(&request_gen->user_req, &ureq,
+				sizeof(request_gen->user_req));
+		request_gen->core_mask = 1;
+		request_gen->job.job_id = request_id;
+		request_gen->job.data = (int) ctl[0]->control->data;
+
+		ret = b2r2_generic_blt(request_gen);
+		b2r2_log_info(b2r2_blt->dev, "\nb2r2_generic_blt=%d "
+			"Generic done.\n", ret);
+	}
+#endif
+exit:
+	release_control_instances(ctl, n_instance);
+
+	ret = ret >= 0 ? request_id : ret;
+
+	return ret;
+}
+
+/**
+ * Free the memory used for the b2r2_blt device
+ */
+static void b2r2_blt_release(struct kref *ref)
+{
+	BUG_ON(b2r2_blt == NULL);
+	if (b2r2_blt == NULL)
+		return;
+	kfree(b2r2_blt->datas);
+	kfree(b2r2_blt);
+	b2r2_blt = NULL;
+}
+
+int b2r2_blt_open(void)
+{
+	int ret = 0;
+	struct b2r2_blt_data *blt_data = NULL;
+	int i;
+
+	if (!atomic_inc_not_zero(&blt_refcount.refcount))
+		return -ENOSYS;
+
+	/* Allocate blitter instance data structure */
+	blt_data = (struct b2r2_blt_data *)
+			kzalloc(sizeof(*blt_data), GFP_KERNEL);
+	if (!blt_data) {
+		b2r2_log_err(b2r2_blt->dev, "%s: Failed to alloc\n", __func__);
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	for (i = 0; i < B2R2_MAX_NBR_DEVICES; i++) {
+		struct b2r2_control *control = b2r2_blt_get_control(i);
+		if (control != NULL) {
+			struct b2r2_control_instance *ci;
+
+			/* Allocate and initialize the control instance */
+			ci = kzalloc(sizeof(*ci), GFP_KERNEL);
+			if (!ci) {
+				b2r2_log_err(b2r2_blt->dev,
+						"%s: Failed to alloc\n",
+						__func__);
+				ret = -ENOMEM;
+				b2r2_blt_release_control(i);
+				goto err;
+			}
+			ci->control_id = i;
+			ci->control = control;
+			ret = b2r2_control_open(ci);
+			if (ret < 0) {
+				b2r2_log_err(b2r2_blt->dev,
+					"%s: Failed to open b2r2 control %d\n",
+					__func__, i);
+				kfree(ci);
+				b2r2_blt_release_control(i);
+				goto err;
+			}
+			blt_data->ctl_instace[i] = ci;
+			b2r2_blt_release_control(i);
+		} else {
+			blt_data->ctl_instace[i] = NULL;
+		}
+	}
+
+	/* TODO: Create kernel worker kthread */
+
+	ret = alloc_handle(blt_data);
+	if (ret < 0)
+		goto err;
+
+	kref_put(&blt_refcount, b2r2_blt_release);
+
+	return ret;
+
+err:
+	/* Destroy the blitter instance data structure */
+	if (blt_data) {
+		for (i = 0; i < B2R2_MAX_NBR_DEVICES; i++)
+			kfree(blt_data->ctl_instace[i]);
+		kfree(blt_data);
+	}
+
+	kref_put(&blt_refcount, b2r2_blt_release);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_blt_open);
+
+int b2r2_blt_close(int handle)
+{
+	int i;
+	struct b2r2_blt_data *blt_data;
+	int ret = 0;
+
+	if (!atomic_inc_not_zero(&blt_refcount.refcount))
+		return -ENOSYS;
+
+	b2r2_log_info(b2r2_blt->dev, "%s\n", __func__);
+
+	blt_data = get_data(handle);
+	if (blt_data == NULL) {
+		b2r2_log_warn(b2r2_blt->dev,
+			"%s, blitter data not found (handle=%d)\n",
+			__func__, handle);
+		ret = -ENOSYS;
+		goto exit;
+	}
+	free_handle(handle);
+
+	for (i = 0; i < B2R2_MAX_NBR_DEVICES; i++) {
+		struct b2r2_control_instance *ci =
+				blt_data->ctl_instace[i];
+		if (ci != NULL) {
+			struct b2r2_control *cont =
+				b2r2_blt_get_control(ci->control_id);
+			if (cont) {
+				/* Release the instance */
+				b2r2_control_release(ci);
+				b2r2_blt_release_control(ci->control_id);
+			}
+			kfree(ci);
+		}
+	}
+	kfree(blt_data);
+
+exit:
+	kref_put(&blt_refcount, b2r2_blt_release);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_blt_close);
+
+int b2r2_blt_request(int handle,
+		struct b2r2_blt_req *user_req)
+{
+	int ret = 0;
+
+	if (!atomic_inc_not_zero(&blt_refcount.refcount))
+		return -ENOSYS;
+
+	/* Exclude some currently unsupported cases */
+	if ((user_req->flags & B2R2_BLT_FLAG_REPORT_WHEN_DONE) ||
+			(user_req->flags & B2R2_BLT_FLAG_REPORT_PERFORMANCE) ||
+			(user_req->report1 != 0)) {
+		b2r2_log_err(b2r2_blt->dev,
+				"%s No callback support in the kernel API\n",
+			__func__);
+		ret = -ENOSYS;
+		goto exit;
+	}
+
+	ret = b2r2_blt_blit_internal(handle, user_req, false);
+
+exit:
+	kref_put(&blt_refcount, b2r2_blt_release);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_blt_request);
+
+int b2r2_blt_synch(int handle, int request_id)
+{
+	int ret = 0;
+	int i;
+	int n_synch = 0;
+	struct b2r2_control_instance *ctl[B2R2_MAX_NBR_DEVICES];
+	struct b2r2_blt_data *blt_data;
+
+	if (!atomic_inc_not_zero(&blt_refcount.refcount))
+		return -ENOSYS;
+
+	b2r2_log_info(b2r2_blt->dev, "%s\n", __func__);
+
+	blt_data = get_data(handle);
+	if (blt_data == NULL) {
+		b2r2_log_warn(b2r2_blt->dev,
+			"%s, blitter data not found (handle=%d)\n",
+			__func__, handle);
+		ret = -ENOSYS;
+		goto exit;
+	}
+
+	/* Get the b2r2 core controls for the job */
+	get_control_instances(blt_data, ctl, B2R2_MAX_NBR_DEVICES, &n_synch);
+	if (n_synch == 0) {
+		b2r2_log_err(b2r2_blt->dev, "%s: No b2r2 cores available.\n",
+			__func__);
+		ret = -ENOSYS;
+		goto exit;
+	}
+
+	for (i = 0; i < n_synch; i++) {
+		ret = b2r2_control_synch(ctl[i], request_id);
+		if (ret != 0) {
+			b2r2_log_err(b2r2_blt->dev,
+				"%s: b2r2_control_synch failed.\n",
+				__func__);
+			break;
+		}
+	}
+
+	/* Release the handles to the core controls */
+	release_control_instances(ctl, n_synch);
+
+exit:
+	kref_put(&blt_refcount, b2r2_blt_release);
+
+	b2r2_log_info(b2r2_blt->dev,
+		"%s, request_id=%d, returns %d\n", __func__, request_id, ret);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_blt_synch);
+
+/**
+ * The user space API
+ */
+
+/**
+ * b2r2_blt_open_us - Implements file open on the b2r2_blt device
+ *
+ * @inode: File system inode
+ * @filp: File pointer
+ *
+ * A b2r2_blt_data handle is created and stored in the file structure.
+ */
+static int b2r2_blt_open_us(struct inode *inode, struct file *filp)
+{
+	int ret = 0;
+	int handle;
+
+	handle = b2r2_blt_open();
+	if (handle < 0) {
+		b2r2_log_err(b2r2_blt->dev, "%s: Failed to open handle\n",
+			__func__);
+		ret = handle;
+		goto exit;
+	}
+	filp->private_data = (void *) handle;
+exit:
+	return ret;
+}
+
+/**
+ * b2r2_blt_release_us - Implements last close on an instance of
+ *                    the b2r2_blt device
+ *
+ * @inode: File system inode
+ * @filp: File pointer
+ *
+ * All active jobs are finished or cancelled and allocated data
+ * is released.
+ */
+static int b2r2_blt_release_us(struct inode *inode, struct file *filp)
+{
+	int ret;
+	ret = b2r2_blt_close((int) filp->private_data);
+	return ret;
+}
+
+/**
+ * Query B2R2 capabilities
+ *
+ * @blt_data: The B2R2 BLT instance
+ * @query_cap: The structure receiving the capabilities
+ */
+static int b2r2_blt_query_cap(struct b2r2_blt_data *blt_data,
+		struct b2r2_blt_query_cap *query_cap)
+{
+	/* FIXME: Not implemented yet */
+	return -ENOSYS;
+}
+
+/**
+ * b2r2_blt_ioctl_us - This routine implements b2r2_blt ioctl interface
+ *
+ * @file: file pointer.
+ * @cmd	:ioctl command.
+ * @arg: input argument for ioctl.
+ *
+ * Returns 0 if OK else negative error code
+ */
+static long b2r2_blt_ioctl_us(struct file *file,
+		unsigned int cmd, unsigned long arg)
+{
+	int ret = 0;
+	int handle = (int) file->private_data;
+
+	/** Process actual ioctl */
+	b2r2_log_info(b2r2_blt->dev, "%s\n", __func__);
+
+	/* Get the instance from the file structure */
+	switch (cmd) {
+	case B2R2_BLT_IOC: {
+		/* arg is user pointer to struct b2r2_blt_request */
+		ret = b2r2_blt_blit_internal(handle,
+				(struct b2r2_blt_req *) arg, true);
+		break;
+	}
+
+	case B2R2_BLT_SYNCH_IOC:
+		/* arg is request_id */
+		ret = b2r2_blt_synch(handle, (int) arg);
+		break;
+
+	case B2R2_BLT_QUERY_CAP_IOC: {
+		/* Arg is struct b2r2_blt_query_cap */
+		struct b2r2_blt_query_cap query_cap;
+		struct b2r2_blt_data *blt_data = get_data(handle);
+
+		/* Get the user data */
+		if (copy_from_user(&query_cap, (void *)arg,
+				sizeof(query_cap))) {
+			b2r2_log_err(b2r2_blt->dev,
+				"%s: copy_from_user failed\n",
+				__func__);
+			ret = -EFAULT;
+			goto exit;
+		}
+
+		/* Fill in our capabilities */
+		ret = b2r2_blt_query_cap(blt_data, &query_cap);
+
+		/* Return data to user */
+		if (copy_to_user((void *)arg, &query_cap,
+				sizeof(query_cap))) {
+			b2r2_log_err(b2r2_blt->dev,
+				"%s: copy_to_user failed\n",
+				__func__);
+			ret = -EFAULT;
+			goto exit;
+		}
+		break;
+	}
+
+	default:
+		/* Unknown command */
+		b2r2_log_err(b2r2_blt->dev, "%s: Unknown cmd %d\n",
+			__func__, cmd);
+		ret = -EINVAL;
+		break;
+
+	}
+
+exit:
+	if (ret < 0)
+		b2r2_log_err(b2r2_blt->dev, "%s: Return with error %d!\n",
+				__func__, -ret);
+
+	return ret;
+}
+
+/**
+ * b2r2_blt_poll - Support for user-space poll, select & epoll.
+ *                 Used for user-space callback
+ *
+ * @filp: File to poll on
+ * @wait: Poll table to wait on
+ *
+ * This function checks if there are anything to read
+ */
+static unsigned b2r2_blt_poll_us(struct file *filp, poll_table *wait)
+{
+	struct b2r2_blt_data *blt_data =
+			(struct b2r2_blt_data *) filp->private_data;
+	struct b2r2_control_instance *ctl[B2R2_MAX_NBR_DEVICES];
+	unsigned int ret = POLLIN | POLLRDNORM;
+	int n_poll = 0;
+	int i;
+
+	b2r2_log_info(b2r2_blt->dev, "%s\n", __func__);
+
+	/* Get the b2r2 core controls for the job */
+	get_control_instances(blt_data, ctl, B2R2_MAX_NBR_DEVICES, &n_poll);
+	if (n_poll == 0) {
+		b2r2_log_err(b2r2_blt->dev, "%s: No b2r2 cores available.\n",
+			__func__);
+		ret = -ENOSYS;
+		goto exit;
+	}
+
+	/* Poll each core control instance */
+	for (i = 0; i < n_poll && ret != 0; i++) {
+		poll_wait(filp, &ctl[i]->report_list_waitq, wait);
+		mutex_lock(&ctl[i]->lock);
+		if (list_empty(&ctl[i]->report_list))
+			ret = 0; /* No reports */
+		mutex_unlock(&ctl[i]->lock);
+	}
+
+	/* Release the handles to the core controls */
+	release_control_instances(ctl, n_poll);
+
+exit:
+	b2r2_log_info(b2r2_blt->dev, "%s: returns %d, n_poll: %d\n",
+		__func__, ret, n_poll);
+
+	return ret;
+}
+
+/**
+ * b2r2_blt_read - Read report data, user for user-space callback
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static ssize_t b2r2_blt_read_us(struct file *filp,
+		char __user *buf, size_t count,	loff_t *f_pos)
+{
+	int ret = 0;
+	int n_read = 0;
+	int i;
+	int first_index = 0;
+	struct b2r2_blt_report report;
+	struct b2r2_blt_request *requests[B2R2_MAX_NBR_DEVICES];
+	struct b2r2_blt_data *blt_data =
+		(struct b2r2_blt_data *) filp->private_data;
+	struct b2r2_control_instance *ctl[B2R2_MAX_NBR_DEVICES];
+	struct b2r2_control_instance *first = NULL;
+	bool block = ((filp->f_flags & O_NONBLOCK) == 0);
+	u32 core_mask = 0;
+
+	b2r2_log_info(b2r2_blt->dev, "%s\n", __func__);
+
+	/*
+	 * We return only complete report records, one at a time.
+	 * Might be more efficient to support read of many.
+	 */
+	count = (count / sizeof(struct b2r2_blt_report)) *
+		sizeof(struct b2r2_blt_report);
+	if (count > sizeof(struct b2r2_blt_report))
+		count = sizeof(struct b2r2_blt_report);
+	if (count == 0)
+		return count;
+
+	memset(ctl, 0, sizeof(*ctl) * B2R2_MAX_NBR_DEVICES);
+	/* Get the b2r2 core controls for the job */
+	for (i = 0; i < B2R2_MAX_NBR_DEVICES; i++) {
+		struct b2r2_control_instance *ci = blt_data->ctl_instace[i];
+		if (ci) {
+			struct b2r2_control *cont =
+				b2r2_blt_get_control(ci->control_id);
+			if (cont) {
+				ctl[i] = ci;
+				n_read++;
+			}
+		}
+	}
+	if (n_read == 0) {
+		b2r2_log_err(b2r2_blt->dev, "%s: No b2r2 cores available.\n",
+			__func__);
+		return -ENOSYS;
+	}
+
+	/* Find which control to ask for a report first */
+	for (i = 0; i < B2R2_MAX_NBR_DEVICES; i++) {
+		if (ctl[i] != NULL) {
+			first = ctl[i];
+			first_index = i;
+			break;
+		}
+	}
+	if (!first) {
+		b2r2_log_err(b2r2_blt->dev, "%s: Internal error.\n",
+			__func__);
+		return -ENOSYS;
+	}
+
+	memset(requests, 0, sizeof(*requests) * B2R2_MAX_NBR_DEVICES);
+	/* Read report from core 0 */
+	ret = b2r2_control_read(first, &requests[first_index], block);
+	if (ret <= 0 || requests[0] == NULL) {
+		b2r2_log_err(b2r2_blt->dev, "%s: b2r2_control_read failed.\n",
+			__func__);
+		ret = -EFAULT;
+		goto exit;
+	}
+	core_mask = requests[first_index]->core_mask >> 1;
+	core_mask &= ~(1 << first_index);
+
+	/* If there are any more cores, try reading the report
+	 * with the specific ID from the other cores */
+	for (i = 0; i < B2R2_MAX_NBR_DEVICES; i++) {
+		if ((core_mask & 1) && (ctl[i] != NULL)) {
+			/* TODO: Do we need to wait here? */
+			ret = b2r2_control_read_id(ctl[i], &requests[i], block,
+					requests[first_index]->request_id);
+			if (ret <= 0 || requests[i] == NULL) {
+				b2r2_log_err(b2r2_blt->dev,
+					"%s: b2r2_control_read failed.\n",
+					__func__);
+				break;
+			}
+		}
+		core_mask = core_mask >> 1;
+	}
+
+	if (ret > 0) {
+		/* Construct a report and copy to userland */
+		report.request_id = requests[0]->request_id;
+		report.report1 = requests[0]->user_req.report1;
+		report.report2 = requests[0]->user_req.report2;
+		report.usec_elapsed = 0; /* TBD */
+
+		if (copy_to_user(buf, &report, sizeof(report))) {
+			b2r2_log_err(b2r2_blt->dev,
+				"%s: copy_to_user failed.\n",
+				__func__);
+			ret = -EFAULT;
+		}
+	}
+
+	if (ret > 0) {
+		for (i = 0; i < B2R2_MAX_NBR_DEVICES; i++)
+			/*
+			 * Release matching the addref when the job was put
+			 * into the report list
+			 */
+			if (requests[i] != NULL)
+				b2r2_core_job_release(&requests[i]->job,
+						__func__);
+	} else {
+		/* We failed at one core or copy to user failed */
+		for (i = 0; i < B2R2_MAX_NBR_DEVICES; i++)
+			if (requests[i] != NULL)
+				list_add(&requests[i]->list,
+					&ctl[i]->report_list);
+		goto exit;
+	}
+
+	ret = count;
+
+exit:
+	/* Release the handles to the core controls */
+	release_control_instances(ctl, n_read);
+
+	return ret;
+}
+
+/**
+ * b2r2_blt_fops - File operations for b2r2_blt
+ */
+static const struct file_operations b2r2_blt_fops = {
+	.owner =          THIS_MODULE,
+	.open =           b2r2_blt_open_us,
+	.release =        b2r2_blt_release_us,
+	.unlocked_ioctl = b2r2_blt_ioctl_us,
+	.poll  =          b2r2_blt_poll_us,
+	.read  =          b2r2_blt_read_us,
+};
+
+
+/**
+ * b2r2_probe() - This routine loads the B2R2 core driver
+ *
+ * @pdev: platform device.
+ */
+static int b2r2_blt_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	int i;
+
+	BUG_ON(pdev == NULL);
+
+	dev_info(&pdev->dev, "%s start.\n", __func__);
+
+	if (!b2r2_blt) {
+		b2r2_blt = kzalloc(sizeof(*b2r2_blt), GFP_KERNEL);
+		if (!b2r2_blt) {
+			dev_err(&pdev->dev, "b2r2_blt alloc failed\n");
+			ret = -EINVAL;
+			goto error_exit;
+		}
+
+		/* Init b2r2 core control reference counters */
+		for (i = 0; i < B2R2_MAX_NBR_DEVICES; i++)
+			spin_lock_init(&b2r2_controls[i].lock);
+	}
+
+	mutex_init(&b2r2_blt->datas_lock);
+	spin_lock_init(&b2r2_blt->lock);
+	b2r2_blt->dev = &pdev->dev;
+
+	/* Register b2r2 driver */
+	b2r2_blt->miscdev.parent = b2r2_blt->dev;
+	b2r2_blt->miscdev.minor = MISC_DYNAMIC_MINOR;
+	b2r2_blt->miscdev.name = "b2r2_blt";
+	b2r2_blt->miscdev.fops = &b2r2_blt_fops;
+
+	ret = misc_register(&b2r2_blt->miscdev);
+	if (ret != 0) {
+		printk(KERN_WARNING "%s: registering misc device fails\n",
+				__func__);
+		goto error_exit;
+	}
+
+	b2r2_blt->dev = b2r2_blt->miscdev.this_device;
+	b2r2_blt->dev->coherent_dma_mask = 0xFFFFFFFF;
+
+	kref_init(&blt_refcount);
+
+	dev_info(&pdev->dev, "%s done.\n", __func__);
+
+	return ret;
+
+/** Recover from any error if something fails */
+error_exit:
+
+	kfree(b2r2_blt);
+
+	dev_info(&pdev->dev, "%s done with errors (%d).\n", __func__, ret);
+
+	return ret;
+}
+
+/**
+ * b2r2_blt_remove - This routine unloads b2r2_blt driver
+ *
+ * @pdev: platform device.
+ */
+static int b2r2_blt_remove(struct platform_device *pdev)
+{
+	BUG_ON(pdev == NULL);
+	dev_info(&pdev->dev, "%s started.\n", __func__);
+	misc_deregister(&b2r2_blt->miscdev);
+	kref_put(&blt_refcount, b2r2_blt_release);
+	return 0;
+}
+
+/**
+ * b2r2_blt_suspend() - This routine puts the B2R2 blitter in to sustend state.
+ * @pdev: platform device.
+ *
+ * This routine stores the current state of the b2r2 device and puts in to
+ * suspend state.
+ *
+ */
+int b2r2_blt_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	return 0;
+}
+
+/**
+ * b2r2_blt_resume() - This routine resumes the B2R2 blitter from sustend state.
+ * @pdev: platform device.
+ *
+ * This routine restore back the current state of the b2r2 device resumes.
+ *
+ */
+int b2r2_blt_resume(struct platform_device *pdev)
+{
+	return 0;
+}
+
+/**
+ * struct platform_b2r2_driver - Platform driver configuration for the
+ * B2R2 core driver
+ */
+static struct platform_driver platform_b2r2_blt_driver = {
+	.remove = b2r2_blt_remove,
+	.driver = {
+		.name	= "b2r2_blt",
+	},
+	.suspend = b2r2_blt_suspend,
+	.resume =  b2r2_blt_resume,
+};
+
+/**
+ * b2r2_init() - Module init function for the B2R2 core module
+ */
+static int __init b2r2_blt_init(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	return platform_driver_probe(&platform_b2r2_blt_driver, b2r2_blt_probe);
+}
+module_init(b2r2_blt_init);
+
+/**
+ * b2r2_exit() - Module exit function for the B2R2 core module
+ */
+static void __exit b2r2_blt_exit(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	platform_driver_unregister(&platform_b2r2_blt_driver);
+	return;
+}
+module_exit(b2r2_blt_exit);
+
+MODULE_AUTHOR("Robert Fekete <robert.fekete@stericsson.com>");
+MODULE_DESCRIPTION("ST-Ericsson B2R2 Blitter module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/b2r2/b2r2_blt_main.c b/drivers/video/b2r2/b2r2_blt_main.c
new file mode 100644
index 00000000000..3727f742bf1
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_blt_main.c
@@ -0,0 +1,2840 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 Blitter module
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/poll.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/list.h>
+#ifdef CONFIG_ANDROID_PMEM
+#include <linux/android_pmem.h>
+#endif
+#include <linux/fb.h>
+#include <linux/uaccess.h>
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#endif
+#include <asm/cacheflush.h>
+#include <linux/smp.h>
+#include <linux/dma-mapping.h>
+#include <linux/sched.h>
+#include <linux/err.h>
+#include <linux/hwmem.h>
+
+#include "b2r2_internal.h"
+#include "b2r2_control.h"
+#include "b2r2_node_split.h"
+#include "b2r2_generic.h"
+#include "b2r2_mem_alloc.h"
+#include "b2r2_profiler_socket.h"
+#include "b2r2_timing.h"
+#include "b2r2_debug.h"
+#include "b2r2_utils.h"
+#include "b2r2_input_validation.h"
+#include "b2r2_core.h"
+#include "b2r2_filters.h"
+
+#define B2R2_HEAP_SIZE (4 * PAGE_SIZE)
+#define MAX_TMP_BUF_SIZE (128 * PAGE_SIZE)
+
+/*
+ * TODO:
+ * Implementation of query cap
+ * Support for user space virtual pointer to physically consecutive memory
+ * Support for user space virtual pointer to physically scattered memory
+ * Callback reads lagging behind in blt_api_stress app
+ * Store smaller items in the report list instead of the whole request
+ * Support read of many report records at once.
+ */
+
+/* Local functions */
+static void inc_stat(struct b2r2_control *cont, unsigned long *stat);
+static void dec_stat(struct b2r2_control *cont, unsigned long *stat);
+
+#ifndef CONFIG_B2R2_GENERIC_ONLY
+static void job_callback(struct b2r2_core_job *job);
+static void job_release(struct b2r2_core_job *job);
+static int job_acquire_resources(struct b2r2_core_job *job, bool atomic);
+static void job_release_resources(struct b2r2_core_job *job, bool atomic);
+#endif
+
+#ifdef CONFIG_B2R2_GENERIC
+static void job_callback_gen(struct b2r2_core_job *job);
+static void job_release_gen(struct b2r2_core_job *job);
+static int job_acquire_resources_gen(struct b2r2_core_job *job, bool atomic);
+static void job_release_resources_gen(struct b2r2_core_job *job, bool atomic);
+static void tile_job_callback_gen(struct b2r2_core_job *job);
+static void tile_job_release_gen(struct b2r2_core_job *job);
+#endif
+
+
+static int resolve_buf(struct b2r2_control *cont,
+		struct b2r2_blt_img *img, struct b2r2_blt_rect *rect_2b_used,
+		bool is_dst, struct b2r2_resolved_buf *resolved);
+static void unresolve_buf(struct b2r2_control *cont,
+		struct b2r2_blt_buf *buf, struct b2r2_resolved_buf *resolved);
+static void sync_buf(struct b2r2_control *cont, struct b2r2_blt_img *img,
+		struct b2r2_resolved_buf *resolved, bool is_dst,
+		struct b2r2_blt_rect *rect);
+static bool is_report_list_empty(struct b2r2_control_instance *instance);
+static bool is_synching(struct b2r2_control_instance *instance);
+static void get_actual_dst_rect(struct b2r2_blt_req *req,
+		struct b2r2_blt_rect *actual_dst_rect);
+static void set_up_hwmem_region(struct b2r2_control *cont,
+		struct b2r2_blt_img *img, struct b2r2_blt_rect *rect,
+		struct hwmem_region *region);
+static int resolve_hwmem(struct b2r2_control *cont, struct b2r2_blt_img *img,
+		struct b2r2_blt_rect *rect_2b_used, bool is_dst,
+		struct b2r2_resolved_buf *resolved_buf);
+static void unresolve_hwmem(struct b2r2_resolved_buf *resolved_buf);
+
+/**
+ * struct sync_args - Data for clean/flush
+ *
+ * @start: Virtual start address
+ * @end: Virtual end address
+ */
+struct sync_args {
+	unsigned long start;
+	unsigned long end;
+};
+/**
+ * flush_l1_cache_range_curr_cpu() - Cleans and invalidates L1 cache on the
+ * current CPU
+ *
+ * @arg: Pointer to sync_args structure
+ */
+static inline void flush_l1_cache_range_curr_cpu(void *arg)
+{
+	struct sync_args *sa = (struct sync_args *)arg;
+
+	dmac_flush_range((void *)sa->start, (void *)sa->end);
+}
+
+#ifdef CONFIG_SMP
+/**
+ * inv_l1_cache_range_all_cpus() - Cleans and invalidates L1 cache on all CPU:s
+ *
+ * @sa: Pointer to sync_args structure
+ */
+static void flush_l1_cache_range_all_cpus(struct sync_args *sa)
+{
+	on_each_cpu(flush_l1_cache_range_curr_cpu, sa, 1);
+}
+#endif
+
+/**
+ * clean_l1_cache_range_curr_cpu() - Cleans L1 cache on current CPU
+ *
+ * Ensures that data is written out from the CPU:s L1 cache,
+ * it will still be in the cache.
+ *
+ * @arg: Pointer to sync_args structure
+ */
+static inline void clean_l1_cache_range_curr_cpu(void *arg)
+{
+	struct sync_args *sa = (struct sync_args *)arg;
+
+	dmac_map_area((void *)sa->start,
+		(void *)sa->end - (void *)sa->start,
+		DMA_TO_DEVICE);
+}
+
+#ifdef CONFIG_SMP
+/**
+ * clean_l1_cache_range_all_cpus() - Cleans L1 cache on all CPU:s
+ *
+ * Ensures that data is written out from all CPU:s L1 cache,
+ * it will still be in the cache.
+ *
+ * @sa: Pointer to sync_args structure
+ */
+static void clean_l1_cache_range_all_cpus(struct sync_args *sa)
+{
+	on_each_cpu(clean_l1_cache_range_curr_cpu, sa, 1);
+}
+#endif
+
+/**
+ * b2r2_blt_open - Implements file open on the b2r2_blt device
+ *
+ * @inode: File system inode
+ * @filp: File pointer
+ *
+ * A B2R2 BLT instance is created and stored in the file structure.
+ */
+int b2r2_control_open(struct b2r2_control_instance *instance)
+{
+	int ret = 0;
+	struct b2r2_control *cont = instance->control;
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+	inc_stat(cont, &cont->stat_n_in_open);
+
+	INIT_LIST_HEAD(&instance->report_list);
+	mutex_init(&instance->lock);
+	init_waitqueue_head(&instance->report_list_waitq);
+	init_waitqueue_head(&instance->synch_done_waitq);
+	dec_stat(cont, &cont->stat_n_in_open);
+
+	return ret;
+}
+
+/**
+ * b2r2_blt_release - Implements last close on an instance of
+ *                    the b2r2_blt device
+ *
+ * @inode: File system inode
+ * @filp: File pointer
+ *
+ * All active jobs are finished or cancelled and allocated data
+ * is released.
+ */
+int b2r2_control_release(struct b2r2_control_instance *instance)
+{
+	int ret;
+	struct b2r2_control *cont = instance->control;
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	inc_stat(cont, &cont->stat_n_in_release);
+
+	/* Finish all outstanding requests */
+	ret = b2r2_control_synch(instance, 0);
+	if (ret < 0)
+		b2r2_log_warn(cont->dev, "%s: b2r2_blt_sync failed with %d\n",
+			__func__, ret);
+
+	/* Now cancel any remaining outstanding request */
+	if (instance->no_of_active_requests) {
+		struct b2r2_core_job *job;
+
+		b2r2_log_warn(cont->dev, "%s: %d active requests\n", __func__,
+			instance->no_of_active_requests);
+
+		/* Find and cancel all jobs belonging to us */
+		job = b2r2_core_job_find_first_with_tag(cont,
+			(int) instance);
+		while (job) {
+			b2r2_core_job_cancel(job);
+			/* Matches addref in b2r2_core_job_find... */
+			b2r2_core_job_release(job, __func__);
+			job = b2r2_core_job_find_first_with_tag(cont,
+				(int) instance);
+		}
+
+		b2r2_log_warn(cont->dev, "%s: %d active requests after "
+			"cancel\n", __func__, instance->no_of_active_requests);
+	}
+
+	/* Release jobs in report list */
+	mutex_lock(&instance->lock);
+	while (!list_empty(&instance->report_list)) {
+		struct b2r2_blt_request *request = list_first_entry(
+			&instance->report_list,
+			struct b2r2_blt_request,
+			list);
+		list_del_init(&request->list);
+		mutex_unlock(&instance->lock);
+		/*
+		 * This release matches the addref when the job was put into
+		 * the report list
+		 */
+		b2r2_core_job_release(&request->job, __func__);
+		mutex_lock(&instance->lock);
+	}
+	mutex_unlock(&instance->lock);
+
+	dec_stat(cont, &cont->stat_n_in_release);
+
+	return 0;
+}
+
+size_t b2r2_control_read(struct b2r2_control_instance *instance,
+		struct b2r2_blt_request **request_out, bool block)
+{
+	struct b2r2_blt_request *request = NULL;
+#ifdef CONFIG_B2R2_DEBUG
+	struct b2r2_control *cont = instance->control;
+#endif
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	/*
+	 * Loop and wait here until we have anything to return or
+	 * until interrupted
+	 */
+	mutex_lock(&instance->lock);
+	while (list_empty(&instance->report_list)) {
+		mutex_unlock(&instance->lock);
+
+		/* Return if non blocking read */
+		if (!block)
+			return -EAGAIN;
+
+		b2r2_log_info(cont->dev, "%s - Going to sleep\n", __func__);
+		if (wait_event_interruptible(
+				instance->report_list_waitq,
+				!is_report_list_empty(instance)))
+			/* signal: tell the fs layer to handle it */
+			return -ERESTARTSYS;
+
+		/* Otherwise loop, but first reaquire the lock */
+		mutex_lock(&instance->lock);
+	}
+
+	if (!list_empty(&instance->report_list))
+		request = list_first_entry(
+			&instance->report_list, struct b2r2_blt_request, list);
+
+	if (request) {
+		/* Remove from list to avoid reading twice */
+		list_del_init(&request->list);
+
+		*request_out = request;
+	}
+	mutex_unlock(&instance->lock);
+
+	if (request)
+		return 1;
+
+	/* No report returned */
+	return 0;
+}
+
+size_t b2r2_control_read_id(struct b2r2_control_instance *instance,
+		struct b2r2_blt_request **request_out, bool block,
+		int request_id)
+{
+	struct b2r2_blt_request *request = NULL;
+#ifdef CONFIG_B2R2_DEBUG
+	struct b2r2_control *cont = instance->control;
+#endif
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	/*
+	 * Loop and wait here until we have anything to return or
+	 * until interrupted
+	 */
+	mutex_lock(&instance->lock);
+	while (list_empty(&instance->report_list)) {
+		mutex_unlock(&instance->lock);
+
+		/* Return if non blocking read */
+		if (!block)
+			return -EAGAIN;
+
+		b2r2_log_info(cont->dev, "%s - Going to sleep\n", __func__);
+		if (wait_event_interruptible(
+				instance->report_list_waitq,
+				!is_report_list_empty(instance)))
+			/* signal: tell the fs layer to handle it */
+			return -ERESTARTSYS;
+
+		/* Otherwise loop, but first reaquire the lock */
+		mutex_lock(&instance->lock);
+	}
+
+	if (!list_empty(&instance->report_list)) {
+		struct b2r2_blt_request *pos;
+		list_for_each_entry(pos, &instance->report_list, list) {
+			if (pos->request_id)
+				request = pos;
+		}
+	}
+
+	if (request) {
+		/* Remove from list to avoid reading twice */
+		list_del_init(&request->list);
+		*request_out = request;
+	}
+	mutex_unlock(&instance->lock);
+
+	if (request)
+		return 1;
+
+	/* No report returned */
+	return 0;
+}
+
+#ifndef CONFIG_B2R2_GENERIC_ONLY
+/**
+ * b2r2_blt - Implementation of the B2R2 blit request
+ *
+ * @instance: The B2R2 BLT instance
+ * @request; The request to perform
+ */
+int b2r2_control_blt(struct b2r2_blt_request *request)
+{
+	int ret = 0;
+	struct b2r2_blt_rect actual_dst_rect;
+	int request_id = 0;
+	struct b2r2_node *last_node = request->first_node;
+	int node_count;
+	struct b2r2_control_instance *instance = request->instance;
+	struct b2r2_control *cont = instance->control;
+
+	u32 thread_runtime_at_start = 0;
+
+	if (request->profile) {
+		request->start_time_nsec = b2r2_get_curr_nsec();
+		thread_runtime_at_start = (u32)task_sched_runtime(current);
+	}
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	inc_stat(cont, &cont->stat_n_in_blt);
+
+	/* Debug prints of incoming request */
+	b2r2_log_info(cont->dev,
+		"src.fmt=%#010x src.buf={%d,%d,%d} "
+		"src.w,h={%d,%d} src.rect={%d,%d,%d,%d}\n",
+		request->user_req.src_img.fmt,
+		request->user_req.src_img.buf.type,
+		request->user_req.src_img.buf.fd,
+		request->user_req.src_img.buf.offset,
+		request->user_req.src_img.width,
+		request->user_req.src_img.height,
+		request->user_req.src_rect.x,
+		request->user_req.src_rect.y,
+		request->user_req.src_rect.width,
+		request->user_req.src_rect.height);
+
+	if (request->user_req.flags & B2R2_BLT_FLAG_BG_BLEND)
+		b2r2_log_info(cont->dev,
+			"bg.fmt=%#010x bg.buf={%d,%d,%d} "
+			"bg.w,h={%d,%d} bg.rect={%d,%d,%d,%d}\n",
+			request->user_req.bg_img.fmt,
+			request->user_req.bg_img.buf.type,
+			request->user_req.bg_img.buf.fd,
+			request->user_req.bg_img.buf.offset,
+			request->user_req.bg_img.width,
+			request->user_req.bg_img.height,
+			request->user_req.bg_rect.x,
+			request->user_req.bg_rect.y,
+			request->user_req.bg_rect.width,
+			request->user_req.bg_rect.height);
+
+	b2r2_log_info(cont->dev,
+		"dst.fmt=%#010x dst.buf={%d,%d,%d} "
+		"dst.w,h={%d,%d} dst.rect={%d,%d,%d,%d}\n",
+		request->user_req.dst_img.fmt,
+		request->user_req.dst_img.buf.type,
+		request->user_req.dst_img.buf.fd,
+		request->user_req.dst_img.buf.offset,
+		request->user_req.dst_img.width,
+		request->user_req.dst_img.height,
+		request->user_req.dst_rect.x,
+		request->user_req.dst_rect.y,
+		request->user_req.dst_rect.width,
+		request->user_req.dst_rect.height);
+
+	inc_stat(cont, &cont->stat_n_in_blt_synch);
+
+	/* Wait here if synch is ongoing */
+	ret = wait_event_interruptible(instance->synch_done_waitq,
+			!is_synching(instance));
+	if (ret) {
+		b2r2_log_warn(cont->dev, "%s: Sync wait interrupted, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		dec_stat(cont, &cont->stat_n_in_blt_synch);
+		goto synch_interrupted;
+	}
+
+	dec_stat(cont, &cont->stat_n_in_blt_synch);
+
+	/* Resolve the buffers */
+
+	/* Source buffer */
+	ret = resolve_buf(cont, &request->user_req.src_img,
+		&request->user_req.src_rect,
+		false, &request->src_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(cont->dev, "%s: Resolve src buf failed, %d\n",
+				__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_src_buf_failed;
+	}
+
+	/* Background buffer */
+	if (request->user_req.flags & B2R2_BLT_FLAG_BG_BLEND) {
+		ret = resolve_buf(cont, &request->user_req.bg_img,
+			&request->user_req.bg_rect,
+			false, &request->bg_resolved);
+		if (ret < 0) {
+			b2r2_log_warn(cont->dev, "%s: Resolve bg buf failed,"
+				" %d\n", __func__, ret);
+			ret = -EAGAIN;
+			goto resolve_bg_buf_failed;
+		}
+	}
+
+	/* Source mask buffer */
+	ret = resolve_buf(cont, &request->user_req.src_mask,
+			&request->user_req.src_rect, false,
+			&request->src_mask_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(cont->dev, "%s: Resolve src mask buf failed,"
+			" %d\n", __func__, ret);
+		ret = -EAGAIN;
+		goto resolve_src_mask_buf_failed;
+	}
+
+	/* Destination buffer */
+	get_actual_dst_rect(&request->user_req, &actual_dst_rect);
+	ret = resolve_buf(cont, &request->user_req.dst_img, &actual_dst_rect,
+		true, &request->dst_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(cont->dev, "%s: Resolve dst buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_dst_buf_failed;
+	}
+
+	/* Debug prints of resolved buffers */
+	b2r2_log_info(cont->dev, "src.rbuf={%X,%p,%d} {%p,%X,%X,%d}\n",
+		request->src_resolved.physical_address,
+		request->src_resolved.virtual_address,
+		request->src_resolved.is_pmem,
+		request->src_resolved.filep,
+		request->src_resolved.file_physical_start,
+		request->src_resolved.file_virtual_start,
+		request->src_resolved.file_len);
+
+	if (request->user_req.flags & B2R2_BLT_FLAG_BG_BLEND)
+		b2r2_log_info(cont->dev, "bg.rbuf={%X,%p,%d} {%p,%X,%X,%d}\n",
+			request->bg_resolved.physical_address,
+			request->bg_resolved.virtual_address,
+			request->bg_resolved.is_pmem,
+			request->bg_resolved.filep,
+			request->bg_resolved.file_physical_start,
+			request->bg_resolved.file_virtual_start,
+			request->bg_resolved.file_len);
+
+	b2r2_log_info(cont->dev, "dst.rbuf={%X,%p,%d} {%p,%X,%X,%d}\n",
+		request->dst_resolved.physical_address,
+		request->dst_resolved.virtual_address,
+		request->dst_resolved.is_pmem,
+		request->dst_resolved.filep,
+		request->dst_resolved.file_physical_start,
+		request->dst_resolved.file_virtual_start,
+		request->dst_resolved.file_len);
+
+	/* Calculate the number of nodes (and resources) needed for this job */
+	ret = b2r2_node_split_analyze(request, MAX_TMP_BUF_SIZE, &node_count,
+		&request->bufs, &request->buf_count,
+		&request->node_split_job);
+	if (ret == -ENOSYS) {
+		/* There was no optimized path for this request */
+		b2r2_log_info(cont->dev, "%s: No optimized path for request\n",
+			__func__);
+		goto no_optimized_path;
+
+	} else if (ret < 0) {
+		b2r2_log_warn(cont->dev, "%s: Failed to analyze request,"
+			" ret = %d\n", __func__, ret);
+#ifdef CONFIG_DEBUG_FS
+		{
+			/* Failed, dump job to dmesg */
+			char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+			b2r2_log_info(cont->dev, "%s: Analyze failed for:\n",
+				__func__);
+			if (Buf != NULL) {
+				sprintf_req(request, Buf, sizeof(char) * 4096);
+				b2r2_log_info(cont->dev, "%s", Buf);
+				kfree(Buf);
+			} else {
+				b2r2_log_info(cont->dev, "Unable to print the"
+					" request. Message buffer"
+					" allocation failed.\n");
+			}
+		}
+#endif
+		goto generate_nodes_failed;
+	}
+
+	/* Allocate the nodes needed */
+#ifdef B2R2_USE_NODE_GEN
+	request->first_node = b2r2_blt_alloc_nodes(cont,
+		node_count);
+	if (request->first_node == NULL) {
+		b2r2_log_warn(cont->dev, "%s: Failed to allocate nodes,"
+			" ret = %d\n", __func__, ret);
+		goto generate_nodes_failed;
+	}
+#else
+	ret = b2r2_node_alloc(cont, node_count, &(request->first_node));
+	if (ret < 0 || request->first_node == NULL) {
+		b2r2_log_warn(cont->dev,
+			"%s: Failed to allocate nodes, ret = %d\n",
+			__func__, ret);
+		goto generate_nodes_failed;
+	}
+#endif
+
+	/* Build the B2R2 node list */
+	ret = b2r2_node_split_configure(cont, &request->node_split_job,
+			request->first_node);
+
+	if (ret < 0) {
+		b2r2_log_warn(cont->dev, "%s:"
+			" Failed to perform node split, ret = %d\n",
+			__func__, ret);
+		goto generate_nodes_failed;
+	}
+
+	/* Exit here if dry run */
+	if (request->user_req.flags & B2R2_BLT_FLAG_DRY_RUN)
+		goto exit_dry_run;
+
+	/* Configure the request */
+	last_node = request->first_node;
+	while (last_node && last_node->next)
+		last_node = last_node->next;
+
+	request->job.tag = (int) instance;
+	request->job.data = (int) cont->data;
+	request->job.prio = request->user_req.prio;
+	request->job.first_node_address =
+		request->first_node->physical_address;
+	request->job.last_node_address =
+		last_node->physical_address;
+	request->job.callback = job_callback;
+	request->job.release = job_release;
+	request->job.acquire_resources = job_acquire_resources;
+	request->job.release_resources = job_release_resources;
+
+	/* Synchronize memory occupied by the buffers */
+
+	/* Source buffer */
+	if (!(request->user_req.flags &
+				B2R2_BLT_FLAG_SRC_NO_CACHE_FLUSH) &&
+			(request->user_req.src_img.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.src_img.fmt))
+		/* MB formats are never touched by SW */
+		sync_buf(cont, &request->user_req.src_img,
+			&request->src_resolved, false,
+			&request->user_req.src_rect);
+
+	/* Background buffer */
+	if ((request->user_req.flags & B2R2_BLT_FLAG_BG_BLEND) &&
+			!(request->user_req.flags &
+				B2R2_BLT_FLAG_BG_NO_CACHE_FLUSH) &&
+			(request->user_req.bg_img.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.bg_img.fmt))
+		/* MB formats are never touched by SW */
+		sync_buf(cont, &request->user_req.bg_img,
+			&request->bg_resolved, false,
+			&request->user_req.bg_rect);
+
+	/* Source mask buffer */
+	if (!(request->user_req.flags &
+				B2R2_BLT_FLAG_SRC_MASK_NO_CACHE_FLUSH) &&
+			(request->user_req.src_mask.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.src_mask.fmt))
+		/* MB formats are never touched by SW */
+		sync_buf(cont, &request->user_req.src_mask,
+			&request->src_mask_resolved, false, NULL);
+
+	/* Destination buffer */
+	if (!(request->user_req.flags &
+				B2R2_BLT_FLAG_DST_NO_CACHE_FLUSH) &&
+			(request->user_req.dst_img.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.dst_img.fmt))
+		/* MB formats are never touched by SW */
+		sync_buf(cont, &request->user_req.dst_img,
+			&request->dst_resolved, true,
+			&request->user_req.dst_rect);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Remember latest request for debugfs */
+	cont->debugfs_latest_request = *request;
+#endif
+
+	/* Submit the job */
+	b2r2_log_info(cont->dev, "%s: Submitting job\n", __func__);
+
+	inc_stat(cont, &cont->stat_n_in_blt_add);
+
+	if (request->profile)
+		request->nsec_active_in_cpu =
+			(s32)((u32)task_sched_runtime(current) -
+					thread_runtime_at_start);
+
+	mutex_lock(&instance->lock);
+
+	/* Add the job to b2r2_core */
+	request_id = b2r2_core_job_add(cont, &request->job);
+	request->request_id = request_id;
+
+	dec_stat(cont, &cont->stat_n_in_blt_add);
+
+	if (request_id < 0) {
+		b2r2_log_warn(cont->dev, "%s: Failed to add job, ret = %d\n",
+			__func__, request_id);
+		ret = request_id;
+		mutex_unlock(&instance->lock);
+		goto job_add_failed;
+	}
+
+	inc_stat(cont, &cont->stat_n_jobs_added);
+
+	instance->no_of_active_requests++;
+	mutex_unlock(&instance->lock);
+
+	return ret >= 0 ? request_id : ret;
+
+job_add_failed:
+exit_dry_run:
+no_optimized_path:
+generate_nodes_failed:
+	unresolve_buf(cont, &request->user_req.dst_img.buf,
+		&request->dst_resolved);
+resolve_dst_buf_failed:
+	unresolve_buf(cont, &request->user_req.src_mask.buf,
+		&request->src_mask_resolved);
+resolve_src_mask_buf_failed:
+	if (request->user_req.flags & B2R2_BLT_FLAG_BG_BLEND)
+		unresolve_buf(cont, &request->user_req.bg_img.buf,
+				&request->bg_resolved);
+resolve_bg_buf_failed:
+	unresolve_buf(cont, &request->user_req.src_img.buf,
+		&request->src_resolved);
+resolve_src_buf_failed:
+synch_interrupted:
+	if ((request->user_req.flags & B2R2_BLT_FLAG_DRY_RUN) == 0 || ret)
+		b2r2_log_warn(cont->dev, "%s returns with error %d\n",
+			__func__, ret);
+	job_release(&request->job);
+	dec_stat(cont, &cont->stat_n_jobs_released);
+
+	dec_stat(cont, &cont->stat_n_in_blt);
+
+	return ret;
+}
+
+int b2r2_control_waitjob(struct b2r2_blt_request *request)
+{
+	int ret = 0;
+	struct b2r2_control_instance *instance = request->instance;
+	struct b2r2_control *cont = instance->control;
+
+	/* Wait for the job to be done if synchronous */
+	if ((request->user_req.flags & B2R2_BLT_FLAG_ASYNCH) == 0) {
+		b2r2_log_info(cont->dev, "%s: Synchronous, waiting\n",
+			__func__);
+
+		inc_stat(cont, &cont->stat_n_in_blt_wait);
+
+		ret = b2r2_core_job_wait(&request->job);
+
+		dec_stat(cont, &cont->stat_n_in_blt_wait);
+
+		if (ret < 0 && ret != -ENOENT)
+			b2r2_log_warn(cont->dev, "%s: Failed to wait job,"
+				" ret = %d\n", __func__, ret);
+		else
+			b2r2_log_info(cont->dev, "%s: Synchronous wait done\n",
+				__func__);
+	}
+
+	/*
+	 * Release matching the addref in b2r2_core_job_add,
+	 * the request must not be accessed after this call
+	 */
+	b2r2_core_job_release(&request->job, __func__);
+	dec_stat(cont, &cont->stat_n_in_blt);
+
+	return ret;
+}
+
+/**
+ * Called when job is done or cancelled
+ *
+ * @job: The job
+ */
+static void job_callback(struct b2r2_core_job *job)
+{
+	struct b2r2_blt_request *request = NULL;
+	struct b2r2_core *core = NULL;
+	struct b2r2_control *cont = NULL;
+
+	request = container_of(job, struct b2r2_blt_request, job);
+	core = (struct b2r2_core *) job->data;
+	cont = core->control;
+
+	if (cont->dev)
+		b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	/* Local addref / release within this func */
+	b2r2_core_job_addref(job, __func__);
+
+	/* Unresolve the buffers */
+	unresolve_buf(cont, &request->user_req.src_img.buf,
+		&request->src_resolved);
+	unresolve_buf(cont, &request->user_req.src_mask.buf,
+		&request->src_mask_resolved);
+	unresolve_buf(cont, &request->user_req.dst_img.buf,
+		&request->dst_resolved);
+	if (request->user_req.flags & B2R2_BLT_FLAG_BG_BLEND)
+		unresolve_buf(cont, &request->user_req.bg_img.buf,
+			&request->bg_resolved);
+
+	/* Move to report list if the job shall be reported */
+	/* FIXME: Use a smaller struct? */
+	/* TODO: In the case of kernel API call, feed an asynch task to the
+	 * instance worker (kthread) instead of polling for a report */
+	mutex_lock(&request->instance->lock);
+	if (request->user_req.flags & B2R2_BLT_FLAG_REPORT_WHEN_DONE) {
+		/* Move job to report list */
+		list_add_tail(&request->list,
+			&request->instance->report_list);
+		inc_stat(cont, &cont->stat_n_jobs_in_report_list);
+
+		/* Wake up poll */
+		wake_up_interruptible(
+			&request->instance->report_list_waitq);
+
+		/* Add a reference because we put the job in the report list */
+		b2r2_core_job_addref(job, __func__);
+	}
+
+	/*
+	 * Decrease number of active requests and wake up
+	 * synching threads if active requests reaches zero
+	 */
+	BUG_ON(request->instance->no_of_active_requests == 0);
+	request->instance->no_of_active_requests--;
+	if (request->instance->synching &&
+			request->instance->no_of_active_requests == 0) {
+		request->instance->synching = false;
+		/* Wake up all syncing */
+
+		wake_up_interruptible_all(
+			&request->instance->synch_done_waitq);
+	}
+	mutex_unlock(&request->instance->lock);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Dump job if cancelled */
+	if (job->job_state == B2R2_CORE_JOB_CANCELED) {
+		char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+		b2r2_log_info(cont->dev, "%s: Job cancelled:\n", __func__);
+		if (Buf != NULL) {
+			sprintf_req(request, Buf, sizeof(char) * 4096);
+			b2r2_log_info(cont->dev, "%s", Buf);
+			kfree(Buf);
+		} else {
+			b2r2_log_info(cont->dev, "Unable to print the request."
+				" Message buffer allocation failed.\n");
+		}
+	}
+#endif
+
+	if (request->profile) {
+		request->total_time_nsec =
+			(s32)(b2r2_get_curr_nsec() - request->start_time_nsec);
+		b2r2_call_profiler_blt_done(request);
+	}
+
+	/* Local addref / release within this func */
+	b2r2_core_job_release(job, __func__);
+}
+
+/**
+ * Called when job should be released (free memory etc.)
+ *
+ * @job: The job
+ */
+static void job_release(struct b2r2_core_job *job)
+{
+	struct b2r2_blt_request *request = NULL;
+	struct b2r2_core *core = NULL;
+	struct b2r2_control *cont = NULL;
+
+	request = container_of(job, struct b2r2_blt_request, job);
+	core = (struct b2r2_core *) job->data;
+	cont = core->control;
+
+	inc_stat(cont, &cont->stat_n_jobs_released);
+
+	b2r2_log_info(cont->dev, "%s, first_node=%p, ref_count=%d\n",
+		__func__, request->first_node, request->job.ref_count);
+
+	b2r2_node_split_cancel(cont, &request->node_split_job);
+
+	if (request->first_node) {
+		b2r2_debug_job_done(cont, request->first_node);
+#ifdef B2R2_USE_NODE_GEN
+		b2r2_blt_free_nodes(cont, request->first_node);
+#else
+		b2r2_node_free(cont, request->first_node);
+#endif
+	}
+
+	/* Release memory for the request */
+	if (request->clut != NULL) {
+		dma_free_coherent(cont->dev, CLUT_SIZE, request->clut,
+				request->clut_phys_addr);
+		request->clut = NULL;
+		request->clut_phys_addr = 0;
+	}
+	kfree(request);
+}
+
+/**
+ * Tells the job to try to allocate the resources needed to execute the job.
+ * Called just before execution of a job.
+ *
+ * @job: The job
+ * @atomic: true if called from atomic (i.e. interrupt) context. If function
+ *          can't allocate in atomic context it should return error, it
+ *          will then be called later from non-atomic context.
+ */
+static int job_acquire_resources(struct b2r2_core_job *job, bool atomic)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+	struct b2r2_core *core = (struct b2r2_core *) job->data;
+	struct b2r2_control *cont = core->control;
+	int ret;
+	int i;
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	if (request->buf_count == 0)
+		return 0;
+
+	if (request->buf_count > MAX_TMP_BUFS_NEEDED) {
+		b2r2_log_err(cont->dev,
+				"%s: request->buf_count > MAX_TMP_BUFS_NEEDED\n",
+				__func__);
+		return -ENOMSG;
+	}
+
+	/*
+	 * 1 to 1 mapping between request temp buffers and temp buffers
+	 * (request temp buf 0 is always temp buf 0, request temp buf 1 is
+	 * always temp buf 1 and so on) to avoid starvation of jobs that
+	 * require multiple temp buffers. Not optimal in terms of memory
+	 * usage but we avoid get into a situation where lower prio jobs can
+	 * delay higher prio jobs that require more temp buffers.
+	 */
+	if (cont->tmp_bufs[0].in_use)
+		return -EAGAIN;
+
+	for (i = 0; i < request->buf_count; i++) {
+		if (cont->tmp_bufs[i].buf.size < request->bufs[i].size) {
+			b2r2_log_err(cont->dev, "%s: "
+					"cont->tmp_bufs[i].buf.size < "
+					"request->bufs[i].size\n", __func__);
+			ret = -ENOMSG;
+			goto error;
+		}
+
+		cont->tmp_bufs[i].in_use = true;
+		request->bufs[i].phys_addr = cont->tmp_bufs[i].buf.phys_addr;
+		request->bufs[i].virt_addr = cont->tmp_bufs[i].buf.virt_addr;
+
+		b2r2_log_info(cont->dev, "%s: phys=%p, virt=%p\n",
+				__func__, (void *)request->bufs[i].phys_addr,
+				request->bufs[i].virt_addr);
+
+		ret = b2r2_node_split_assign_buffers(cont,
+				&request->node_split_job,
+				request->first_node, request->bufs,
+				request->buf_count);
+		if (ret < 0)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	for (i = 0; i < request->buf_count; i++)
+		cont->tmp_bufs[i].in_use = false;
+
+	return ret;
+}
+
+/**
+ * Tells the job to free the resources needed to execute the job.
+ * Called after execution of a job.
+ *
+ * @job: The job
+ * @atomic: true if called from atomic (i.e. interrupt) context. If function
+ *          can't allocate in atomic context it should return error, it
+ *          will then be called later from non-atomic context.
+ */
+static void job_release_resources(struct b2r2_core_job *job, bool atomic)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+	struct b2r2_core *core = (struct b2r2_core *) job->data;
+	struct b2r2_control *cont = core->control;
+	int i;
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	/* Free any temporary buffers */
+	for (i = 0; i < request->buf_count; i++) {
+
+		b2r2_log_info(cont->dev, "%s: freeing %d bytes\n",
+				__func__, request->bufs[i].size);
+		cont->tmp_bufs[i].in_use = false;
+		memset(&request->bufs[i], 0, sizeof(request->bufs[i]));
+	}
+	request->buf_count = 0;
+
+	/*
+	 * Early release of nodes
+	 * FIXME: If nodes are to be reused we don't want to release here
+	 */
+	if (!atomic && request->first_node) {
+		b2r2_debug_job_done(cont, request->first_node);
+
+#ifdef B2R2_USE_NODE_GEN
+		b2r2_blt_free_nodes(cont, request->first_node);
+#else
+		b2r2_node_free(cont, request->first_node);
+#endif
+		request->first_node = NULL;
+	}
+}
+
+#endif /* !CONFIG_B2R2_GENERIC_ONLY */
+
+#ifdef CONFIG_B2R2_GENERIC
+/**
+ * Called when job for one tile is done or cancelled
+ * in the generic path.
+ *
+ * @job: The job
+ */
+static void tile_job_callback_gen(struct b2r2_core_job *job)
+{
+#ifdef CONFIG_B2R2_DEBUG
+	struct b2r2_core *core =
+			(struct b2r2_core *) job->data;
+	struct b2r2_control *cont = core->control;
+#endif
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	/* Local addref / release within this func */
+	b2r2_core_job_addref(job, __func__);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Notify if a tile job is cancelled */
+	if (job->job_state == B2R2_CORE_JOB_CANCELED)
+		b2r2_log_info(cont->dev, "%s: Tile job cancelled:\n",
+				__func__);
+#endif
+
+	/* Local addref / release within this func */
+	b2r2_core_job_release(job, __func__);
+}
+
+/**
+ * Called when job is done or cancelled.
+ * Used for the last tile in the generic path
+ * to notify waiting clients.
+ *
+ * @job: The job
+ */
+static void job_callback_gen(struct b2r2_core_job *job)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+	struct b2r2_core *core = (struct b2r2_core *) job->data;
+	struct b2r2_control *cont = core->control;
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	/* Local addref / release within this func */
+	b2r2_core_job_addref(job, __func__);
+
+	/* Unresolve the buffers */
+	unresolve_buf(cont, &request->user_req.src_img.buf,
+		&request->src_resolved);
+	unresolve_buf(cont, &request->user_req.src_mask.buf,
+		&request->src_mask_resolved);
+	unresolve_buf(cont, &request->user_req.dst_img.buf,
+		&request->dst_resolved);
+
+	/* Move to report list if the job shall be reported */
+	/* FIXME: Use a smaller struct? */
+	/* TODO: In the case of kernel API call, feed an asynch task to the
+	 * instance worker (kthread) instead of polling for a report */
+	mutex_lock(&request->instance->lock);
+	if (request->user_req.flags & B2R2_BLT_FLAG_REPORT_WHEN_DONE) {
+		/* Move job to report list */
+		list_add_tail(&request->list,
+			&request->instance->report_list);
+		inc_stat(cont, &cont->stat_n_jobs_in_report_list);
+
+		/* Wake up poll */
+		wake_up_interruptible(
+			&request->instance->report_list_waitq);
+
+		/*
+		 * Add a reference because we put the
+		 * job in the report list
+		 */
+		b2r2_core_job_addref(job, __func__);
+	}
+
+	/*
+	 * Decrease number of active requests and wake up
+	 * synching threads if active requests reaches zero
+	 */
+	BUG_ON(request->instance->no_of_active_requests == 0);
+	request->instance->no_of_active_requests--;
+	if (request->instance->synching &&
+			request->instance->no_of_active_requests == 0) {
+		request->instance->synching = false;
+		/* Wake up all syncing */
+
+		wake_up_interruptible_all(
+			&request->instance->synch_done_waitq);
+	}
+	mutex_unlock(&request->instance->lock);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Dump job if cancelled */
+	if (job->job_state == B2R2_CORE_JOB_CANCELED) {
+		char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+		b2r2_log_info(cont->dev, "%s: Job cancelled:\n", __func__);
+		if (Buf != NULL) {
+			sprintf_req(request, Buf, sizeof(char) * 4096);
+			b2r2_log_info(cont->dev, "%s", Buf);
+			kfree(Buf);
+		} else {
+			b2r2_log_info(cont->dev, "Unable to print the request."
+				" Message buffer allocation failed.\n");
+		}
+	}
+#endif
+
+	/* Local addref / release within this func */
+	b2r2_core_job_release(job, __func__);
+}
+
+/**
+ * Called when tile job should be released (free memory etc.)
+ * Should be used only for tile jobs. Tile jobs should only be used
+ * by b2r2_core, thus making ref_count trigger their release.
+ *
+ * @job: The job
+ */
+
+static void tile_job_release_gen(struct b2r2_core_job *job)
+{
+	struct b2r2_core *core =
+			(struct b2r2_core *) job->data;
+	struct b2r2_control *cont = core->control;
+
+	inc_stat(cont, &cont->stat_n_jobs_released);
+
+	b2r2_log_info(cont->dev, "%s, first_node_address=0x%.8x, ref_count="
+		"%d\n", __func__, job->first_node_address,
+		job->ref_count);
+
+	/* Release memory for the job */
+	kfree(job);
+}
+
+/**
+ * Called when job should be released (free memory etc.)
+ *
+ * @job: The job
+ */
+
+static void job_release_gen(struct b2r2_core_job *job)
+{
+	struct b2r2_blt_request *request =
+		container_of(job, struct b2r2_blt_request, job);
+	struct b2r2_core *core = (struct b2r2_core *) job->data;
+	struct b2r2_control *cont = core->control;
+
+	inc_stat(cont, &cont->stat_n_jobs_released);
+
+	b2r2_log_info(cont->dev, "%s, first_node=%p, ref_count=%d\n",
+			__func__, request->first_node, request->job.ref_count);
+
+	if (request->first_node) {
+		b2r2_debug_job_done(cont, request->first_node);
+
+		/* Free nodes */
+#ifdef B2R2_USE_NODE_GEN
+		b2r2_blt_free_nodes(cont, request->first_node);
+#else
+		b2r2_node_free(cont, request->first_node);
+#endif
+	}
+
+	/* Release memory for the request */
+	if (request->clut != NULL) {
+		dma_free_coherent(cont->dev, CLUT_SIZE, request->clut,
+				request->clut_phys_addr);
+		request->clut = NULL;
+		request->clut_phys_addr = 0;
+	}
+	kfree(request);
+}
+
+static int job_acquire_resources_gen(struct b2r2_core_job *job, bool atomic)
+{
+	/* Nothing so far. Temporary buffers are pre-allocated */
+	return 0;
+}
+static void job_release_resources_gen(struct b2r2_core_job *job, bool atomic)
+{
+	/* Nothing so far. Temporary buffers are pre-allocated */
+}
+
+/**
+ * b2r2_generic_blt - Generic implementation of the B2R2 blit request
+ *
+ * @request; The request to perform
+ */
+int b2r2_generic_blt(struct b2r2_blt_request *request)
+{
+	int ret = 0;
+	struct b2r2_blt_rect actual_dst_rect;
+	int request_id = 0;
+	struct b2r2_node *last_node = request->first_node;
+	int node_count;
+	s32 tmp_buf_width = 0;
+	s32 tmp_buf_height = 0;
+	u32 tmp_buf_count = 0;
+	s32 x;
+	s32 y;
+	const struct b2r2_blt_rect *dst_rect = &(request->user_req.dst_rect);
+	const s32 dst_img_width = request->user_req.dst_img.width;
+	const s32 dst_img_height = request->user_req.dst_img.height;
+	const enum b2r2_blt_flag flags = request->user_req.flags;
+	/* Descriptors for the temporary buffers */
+	struct b2r2_work_buf work_bufs[4];
+	struct b2r2_blt_rect dst_rect_tile;
+	int i;
+	struct b2r2_control_instance *instance = request->instance;
+	struct b2r2_control *cont = instance->control;
+
+	u32 thread_runtime_at_start = 0;
+	s32 nsec_active_in_b2r2 = 0;
+
+	/*
+	 * Early exit if zero blt.
+	 * dst_rect outside of dst_img or
+	 * dst_clip_rect outside of dst_img.
+	 */
+	if (dst_rect->x + dst_rect->width <= 0 ||
+			dst_rect->y + dst_rect->height <= 0 ||
+			dst_img_width <= dst_rect->x ||
+			dst_img_height <= dst_rect->y ||
+			((flags & B2R2_BLT_FLAG_DESTINATION_CLIP) != 0 &&
+			(dst_img_width <= request->user_req.dst_clip_rect.x ||
+			dst_img_height <= request->user_req.dst_clip_rect.y ||
+			request->user_req.dst_clip_rect.x +
+			request->user_req.dst_clip_rect.width <= 0 ||
+			request->user_req.dst_clip_rect.y +
+			request->user_req.dst_clip_rect.height <= 0))) {
+		goto zero_blt;
+	}
+
+	if (request->profile) {
+		request->start_time_nsec = b2r2_get_curr_nsec();
+		thread_runtime_at_start = (u32)task_sched_runtime(current);
+	}
+
+	memset(work_bufs, 0, sizeof(work_bufs));
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	inc_stat(cont, &cont->stat_n_in_blt);
+
+	/* Debug prints of incoming request */
+	b2r2_log_info(cont->dev,
+		"src.fmt=%#010x flags=0x%.8x src.buf={%d,%d,0x%.8x}\n"
+		"src.w,h={%d,%d} src.rect={%d,%d,%d,%d}\n",
+		request->user_req.src_img.fmt,
+		request->user_req.flags,
+		request->user_req.src_img.buf.type,
+		request->user_req.src_img.buf.fd,
+		request->user_req.src_img.buf.offset,
+		request->user_req.src_img.width,
+		request->user_req.src_img.height,
+		request->user_req.src_rect.x,
+		request->user_req.src_rect.y,
+		request->user_req.src_rect.width,
+		request->user_req.src_rect.height);
+	b2r2_log_info(cont->dev,
+		"dst.fmt=%#010x dst.buf={%d,%d,0x%.8x}\n"
+		"dst.w,h={%d,%d} dst.rect={%d,%d,%d,%d}\n"
+		"dst_clip_rect={%d,%d,%d,%d}\n",
+		request->user_req.dst_img.fmt,
+		request->user_req.dst_img.buf.type,
+		request->user_req.dst_img.buf.fd,
+		request->user_req.dst_img.buf.offset,
+		request->user_req.dst_img.width,
+		request->user_req.dst_img.height,
+		request->user_req.dst_rect.x,
+		request->user_req.dst_rect.y,
+		request->user_req.dst_rect.width,
+		request->user_req.dst_rect.height,
+		request->user_req.dst_clip_rect.x,
+		request->user_req.dst_clip_rect.y,
+		request->user_req.dst_clip_rect.width,
+		request->user_req.dst_clip_rect.height);
+
+	inc_stat(cont, &cont->stat_n_in_blt_synch);
+
+	/* Wait here if synch is ongoing */
+	ret = wait_event_interruptible(instance->synch_done_waitq,
+				!is_synching(instance));
+	if (ret) {
+		b2r2_log_warn(cont->dev, "%s: Sync wait interrupted, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		dec_stat(cont, &cont->stat_n_in_blt_synch);
+		goto synch_interrupted;
+	}
+
+	dec_stat(cont, &cont->stat_n_in_blt_synch);
+
+	/* Resolve the buffers */
+
+	/* Source buffer */
+	ret = resolve_buf(cont, &request->user_req.src_img,
+		&request->user_req.src_rect, false, &request->src_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(cont->dev, "%s: Resolve src buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_src_buf_failed;
+	}
+
+	/* Source mask buffer */
+	ret = resolve_buf(cont, &request->user_req.src_mask,
+					&request->user_req.src_rect, false,
+						&request->src_mask_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(cont->dev,
+			"%s: Resolve src mask buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_src_mask_buf_failed;
+	}
+
+	/* Destination buffer */
+	get_actual_dst_rect(&request->user_req, &actual_dst_rect);
+	ret = resolve_buf(cont, &request->user_req.dst_img, &actual_dst_rect,
+						true, &request->dst_resolved);
+	if (ret < 0) {
+		b2r2_log_warn(cont->dev, "%s: Resolve dst buf failed, %d\n",
+			__func__, ret);
+		ret = -EAGAIN;
+		goto resolve_dst_buf_failed;
+	}
+
+	/* Debug prints of resolved buffers */
+	b2r2_log_info(cont->dev, "src.rbuf={%X,%p,%d} {%p,%X,%X,%d}\n",
+		request->src_resolved.physical_address,
+		request->src_resolved.virtual_address,
+		request->src_resolved.is_pmem,
+		request->src_resolved.filep,
+		request->src_resolved.file_physical_start,
+		request->src_resolved.file_virtual_start,
+		request->src_resolved.file_len);
+
+	b2r2_log_info(cont->dev, "dst.rbuf={%X,%p,%d} {%p,%X,%X,%d}\n",
+		request->dst_resolved.physical_address,
+		request->dst_resolved.virtual_address,
+		request->dst_resolved.is_pmem,
+		request->dst_resolved.filep,
+		request->dst_resolved.file_physical_start,
+		request->dst_resolved.file_virtual_start,
+		request->dst_resolved.file_len);
+
+	/* Calculate the number of nodes (and resources) needed for this job */
+	ret = b2r2_generic_analyze(request, &tmp_buf_width,
+			&tmp_buf_height, &tmp_buf_count, &node_count);
+	if (ret < 0) {
+		b2r2_log_warn(cont->dev,
+			"%s: Failed to analyze request, ret = %d\n",
+			__func__, ret);
+#ifdef CONFIG_DEBUG_FS
+		{
+			/* Failed, dump job to dmesg */
+			char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+			b2r2_log_info(cont->dev,
+				"%s: Analyze failed for:\n", __func__);
+			if (Buf != NULL) {
+				sprintf_req(request, Buf, sizeof(char) * 4096);
+				b2r2_log_info(cont->dev, "%s", Buf);
+				kfree(Buf);
+			} else {
+				b2r2_log_info(cont->dev,
+					"Unable to print the request. "
+					"Message buffer allocation failed.\n");
+			}
+		}
+#endif
+		goto generate_nodes_failed;
+	}
+
+	/* Allocate the nodes needed */
+#ifdef B2R2_USE_NODE_GEN
+	request->first_node = b2r2_blt_alloc_nodes(cont, node_count);
+	if (request->first_node == NULL) {
+		b2r2_log_warn(cont->dev,
+			"%s: Failed to allocate nodes, ret = %d\n",
+			__func__, ret);
+		goto generate_nodes_failed;
+	}
+#else
+	ret = b2r2_node_alloc(cont, node_count, &(request->first_node));
+	if (ret < 0 || request->first_node == NULL) {
+		b2r2_log_warn(cont->dev,
+			"%s: Failed to allocate nodes, ret = %d\n",
+			__func__, ret);
+		goto generate_nodes_failed;
+	}
+#endif
+
+	/* Allocate the temporary buffers */
+	for (i = 0; i < tmp_buf_count; i++) {
+		void *virt;
+		work_bufs[i].size = tmp_buf_width * tmp_buf_height * 4;
+
+		virt = dma_alloc_coherent(cont->dev,
+				work_bufs[i].size,
+				&(work_bufs[i].phys_addr),
+				GFP_DMA | GFP_KERNEL);
+		if (virt == NULL) {
+			ret = -ENOMEM;
+			goto alloc_work_bufs_failed;
+		}
+
+		work_bufs[i].virt_addr = virt;
+		memset(work_bufs[i].virt_addr, 0xff, work_bufs[i].size);
+	}
+	ret = b2r2_generic_configure(request,
+			request->first_node, &work_bufs[0], tmp_buf_count);
+
+	if (ret < 0) {
+		b2r2_log_warn(cont->dev,
+			"%s: Failed to perform generic configure, ret = %d\n",
+			__func__, ret);
+		goto generic_conf_failed;
+	}
+
+	/* Exit here if dry run */
+	if (flags & B2R2_BLT_FLAG_DRY_RUN)
+		goto exit_dry_run;
+
+	/*
+	 * Configure the request and make sure
+	 * that its job is run only for the LAST tile.
+	 * This is when the request is complete
+	 * and waiting clients should be notified.
+	 */
+	last_node = request->first_node;
+	while (last_node && last_node->next)
+		last_node = last_node->next;
+
+	request->job.tag = (int) instance;
+	request->job.data = (int) cont->data;
+	request->job.prio = request->user_req.prio;
+	request->job.first_node_address =
+		request->first_node->physical_address;
+	request->job.last_node_address =
+		last_node->physical_address;
+	request->job.callback = job_callback_gen;
+	request->job.release = job_release_gen;
+	/* Work buffers and nodes are pre-allocated */
+	request->job.acquire_resources = job_acquire_resources_gen;
+	request->job.release_resources = job_release_resources_gen;
+
+	/* Flush the L1/L2 cache for the buffers */
+
+	/* Source buffer */
+	if (!(flags & B2R2_BLT_FLAG_SRC_NO_CACHE_FLUSH) &&
+			(request->user_req.src_img.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.src_img.fmt))
+		/* MB formats are never touched by SW */
+		sync_buf(cont, &request->user_req.src_img,
+			&request->src_resolved,
+			false, /*is_dst*/
+			&request->user_req.src_rect);
+
+	/* Source mask buffer */
+	if (!(flags & B2R2_BLT_FLAG_SRC_MASK_NO_CACHE_FLUSH) &&
+			(request->user_req.src_mask.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.src_mask.fmt))
+		/* MB formats are never touched by SW */
+		sync_buf(cont, &request->user_req.src_mask,
+			&request->src_mask_resolved,
+			false, /*is_dst*/
+			NULL);
+
+	/* Destination buffer */
+	if (!(flags & B2R2_BLT_FLAG_DST_NO_CACHE_FLUSH) &&
+			(request->user_req.dst_img.buf.type !=
+				B2R2_BLT_PTR_PHYSICAL) &&
+			!b2r2_is_mb_fmt(request->user_req.dst_img.fmt))
+		/* MB formats are never touched by SW */
+		sync_buf(cont, &request->user_req.dst_img,
+			&request->dst_resolved,
+			true, /*is_dst*/
+			&request->user_req.dst_rect);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Remember latest request */
+	cont->debugfs_latest_request = *request;
+#endif
+
+	/*
+	 * Same nodes are reused for all the jobs needed to complete the blit.
+	 * Nodes are NOT released together with associated job,
+	 * as is the case with optimized b2r2_blt() path.
+	 */
+	mutex_lock(&instance->lock);
+	instance->no_of_active_requests++;
+	mutex_unlock(&instance->lock);
+	/*
+	 * Process all but the last row in the destination rectangle.
+	 * Consider only the tiles that will actually end up inside
+	 * the destination image.
+	 * dst_rect->height - tmp_buf_height being <=0 is allright.
+	 * The loop will not be entered since y will always be equal to or
+	 * greater than zero.
+	 * Early exit check at the beginning handles the cases when nothing
+	 * at all should be processed.
+	 */
+	y = 0;
+	if (dst_rect->y < 0)
+		y = -dst_rect->y;
+
+	for (; y < dst_rect->height - tmp_buf_height &&
+			y + dst_rect->y < dst_img_height - tmp_buf_height;
+			y += tmp_buf_height) {
+		/* Tile in the destination rectangle being processed */
+		struct b2r2_blt_rect dst_rect_tile;
+		dst_rect_tile.y = y;
+		dst_rect_tile.width = tmp_buf_width;
+		dst_rect_tile.height = tmp_buf_height;
+
+		x = 0;
+		if (dst_rect->x < 0)
+			x = -dst_rect->x;
+
+		for (; x < dst_rect->width && x + dst_rect->x < dst_img_width;
+				x += tmp_buf_width) {
+			/*
+			 * Tile jobs are freed by the supplied release function
+			 * when ref_count on a tile_job reaches zero.
+			 */
+			struct b2r2_core_job *tile_job =
+				kmalloc(sizeof(*tile_job), GFP_KERNEL);
+			if (tile_job == NULL) {
+				/*
+				 * Skip this tile. Do not abort,
+				 * just hope for better luck
+				 * with rest of the tiles.
+				 * Memory might become available.
+				 */
+				b2r2_log_info(cont->dev, "%s: Failed to alloc "
+						"job. Skipping tile at (x, y)="
+						"(%d, %d)\n", __func__, x, y);
+				continue;
+			}
+			tile_job->job_id = request->job.job_id;
+			tile_job->tag = request->job.tag;
+			tile_job->data = request->job.data;
+			tile_job->prio = request->job.prio;
+			tile_job->first_node_address =
+					request->job.first_node_address;
+			tile_job->last_node_address =
+					request->job.last_node_address;
+			tile_job->callback = tile_job_callback_gen;
+			tile_job->release = tile_job_release_gen;
+			/* Work buffers and nodes are pre-allocated */
+			tile_job->acquire_resources =
+				job_acquire_resources_gen;
+			tile_job->release_resources =
+				job_release_resources_gen;
+
+			dst_rect_tile.x = x;
+			if (x + dst_rect->x + tmp_buf_width > dst_img_width) {
+				/*
+				 * Only a part of the tile can be written.
+				 * Limit imposed by buffer size.
+				 */
+				dst_rect_tile.width =
+					dst_img_width - (x + dst_rect->x);
+			} else if (x + tmp_buf_width > dst_rect->width) {
+				/*
+				 * Only a part of the tile can be written.
+				 * In this case limit imposed by dst_rect size.
+				 */
+				dst_rect_tile.width = dst_rect->width - x;
+			} else {
+				/* Whole tile can be written. */
+				dst_rect_tile.width = tmp_buf_width;
+			}
+			/*
+			 * Where applicable, calculate area in src buffer
+			 * that is needed to generate the specified part
+			 * of destination rectangle.
+			 */
+			b2r2_generic_set_areas(request,
+				request->first_node, &dst_rect_tile);
+			/* Submit the job */
+			b2r2_log_info(cont->dev,
+				"%s: Submitting job\n", __func__);
+
+			inc_stat(cont, &cont->stat_n_in_blt_add);
+
+			mutex_lock(&instance->lock);
+
+			request_id = b2r2_core_job_add(cont, tile_job);
+
+			dec_stat(cont, &cont->stat_n_in_blt_add);
+
+			if (request_id < 0) {
+				b2r2_log_warn(cont->dev, "%s: "
+					"Failed to add tile job, ret = %d\n",
+					__func__, request_id);
+				ret = request_id;
+				mutex_unlock(&instance->lock);
+				goto job_add_failed;
+			}
+
+			inc_stat(cont, &cont->stat_n_jobs_added);
+
+			mutex_unlock(&instance->lock);
+
+			/* Wait for the job to be done */
+			b2r2_log_info(cont->dev, "%s: Synchronous, waiting\n",
+				__func__);
+
+			inc_stat(cont, &cont->stat_n_in_blt_wait);
+
+			ret = b2r2_core_job_wait(tile_job);
+
+			dec_stat(cont, &cont->stat_n_in_blt_wait);
+
+			if (ret < 0 && ret != -ENOENT)
+				b2r2_log_warn(cont->dev,
+					"%s: Failed to wait job, ret = %d\n",
+					__func__, ret);
+			else {
+				b2r2_log_info(cont->dev,
+					"%s: Synchronous wait done\n",
+					__func__);
+
+				nsec_active_in_b2r2 +=
+					tile_job->nsec_active_in_hw;
+			}
+			/* Release matching the addref in b2r2_core_job_add */
+			b2r2_core_job_release(tile_job, __func__);
+		}
+	}
+
+	x = 0;
+	if (dst_rect->x < 0)
+		x = -dst_rect->x;
+
+	for (; x < dst_rect->width &&
+			x + dst_rect->x < dst_img_width; x += tmp_buf_width) {
+		struct b2r2_core_job *tile_job = NULL;
+		if (x + tmp_buf_width < dst_rect->width &&
+				x + dst_rect->x + tmp_buf_width <
+				dst_img_width) {
+			/*
+			 * Tile jobs are freed by the supplied release function
+			 * when ref_count on a tile_job reaches zero.
+			 * Do NOT allocate a tile_job for the last tile.
+			 * Send the job from the request. This way clients
+			 * will be notified when the whole blit is complete
+			 * and not just part of it.
+			 */
+			tile_job = kmalloc(sizeof(*tile_job), GFP_KERNEL);
+			if (tile_job == NULL) {
+				b2r2_log_info(cont->dev, "%s: Failed to alloc "
+					"job. Skipping tile at (x, y)="
+					"(%d, %d)\n", __func__, x, y);
+				continue;
+			}
+			tile_job->job_id = request->job.job_id;
+			tile_job->tag = request->job.tag;
+			tile_job->data = request->job.data;
+			tile_job->prio = request->job.prio;
+			tile_job->first_node_address =
+				request->job.first_node_address;
+			tile_job->last_node_address =
+				request->job.last_node_address;
+			tile_job->callback = tile_job_callback_gen;
+			tile_job->release = tile_job_release_gen;
+			tile_job->acquire_resources =
+				job_acquire_resources_gen;
+			tile_job->release_resources =
+				job_release_resources_gen;
+		}
+
+		dst_rect_tile.x = x;
+		if (x + dst_rect->x + tmp_buf_width > dst_img_width) {
+			/*
+			 * Only a part of the tile can be written.
+			 * Limit imposed by buffer size.
+			 */
+			dst_rect_tile.width = dst_img_width - (x + dst_rect->x);
+		} else if (x + tmp_buf_width > dst_rect->width) {
+			/*
+			 * Only a part of the tile can be written.
+			 * In this case limit imposed by dst_rect size.
+			 */
+			dst_rect_tile.width = dst_rect->width - x;
+		} else {
+			/* Whole tile can be written. */
+			dst_rect_tile.width = tmp_buf_width;
+		}
+		/*
+		 * y is now the last row. Either because the whole dst_rect
+		 * has been processed, or because the last row that will be
+		 * written to dst_img has been reached. Limits imposed in
+		 * the same way as for width.
+		 */
+		dst_rect_tile.y = y;
+		if (y + dst_rect->y + tmp_buf_height > dst_img_height)
+			dst_rect_tile.height =
+				dst_img_height - (y + dst_rect->y);
+		else if (y + tmp_buf_height > dst_rect->height)
+			dst_rect_tile.height = dst_rect->height - y;
+		else
+			dst_rect_tile.height = tmp_buf_height;
+
+		b2r2_generic_set_areas(request,
+			request->first_node, &dst_rect_tile);
+
+		b2r2_log_info(cont->dev, "%s: Submitting job\n", __func__);
+		inc_stat(cont, &cont->stat_n_in_blt_add);
+
+		mutex_lock(&instance->lock);
+		if (x + tmp_buf_width < dst_rect->width &&
+				x + dst_rect->x + tmp_buf_width <
+				dst_img_width) {
+			request_id = b2r2_core_job_add(cont, tile_job);
+		} else {
+			/*
+			 * Last tile. Send the job-struct from the request.
+			 * Clients will be notified once it completes.
+			 */
+			request_id = b2r2_core_job_add(cont, &request->job);
+		}
+
+		dec_stat(cont, &cont->stat_n_in_blt_add);
+
+		if (request_id < 0) {
+			b2r2_log_warn(cont->dev, "%s: Failed to add tile job, "
+				"ret = %d\n", __func__, request_id);
+			ret = request_id;
+			mutex_unlock(&instance->lock);
+			if (tile_job != NULL)
+				kfree(tile_job);
+			goto job_add_failed;
+		}
+
+		inc_stat(cont, &cont->stat_n_jobs_added);
+		mutex_unlock(&instance->lock);
+
+		b2r2_log_info(cont->dev, "%s: Synchronous, waiting\n",
+			__func__);
+
+		inc_stat(cont, &cont->stat_n_in_blt_wait);
+		if (x + tmp_buf_width < dst_rect->width &&
+				x + dst_rect->x + tmp_buf_width <
+				dst_img_width) {
+			ret = b2r2_core_job_wait(tile_job);
+		} else {
+			/*
+			 * This is the last tile. Wait for the job-struct from
+			 * the request.
+			 */
+			ret = b2r2_core_job_wait(&request->job);
+		}
+		dec_stat(cont, &cont->stat_n_in_blt_wait);
+
+		if (ret < 0 && ret != -ENOENT)
+			b2r2_log_warn(cont->dev,
+				"%s: Failed to wait job, ret = %d\n",
+				__func__, ret);
+		else {
+			b2r2_log_info(cont->dev,
+				"%s: Synchronous wait done\n", __func__);
+
+			if (x + tmp_buf_width < dst_rect->width &&
+					x + dst_rect->x + tmp_buf_width <
+					dst_img_width)
+				nsec_active_in_b2r2 +=
+					tile_job->nsec_active_in_hw;
+			else
+				nsec_active_in_b2r2 +=
+					request->job.nsec_active_in_hw;
+		}
+
+		/*
+		 * Release matching the addref in b2r2_core_job_add.
+		 * Make sure that the correct job-struct is released
+		 * when the last tile is processed.
+		 */
+		if (x + tmp_buf_width < dst_rect->width &&
+				x + dst_rect->x + tmp_buf_width <
+				dst_img_width) {
+			b2r2_core_job_release(tile_job, __func__);
+		} else {
+			/*
+			 * Update profiling information before
+			 * the request is released together with
+			 * its core_job.
+			 */
+			if (request->profile) {
+				request->nsec_active_in_cpu =
+					(s32)((u32)task_sched_runtime(current) -
+					thread_runtime_at_start);
+				request->total_time_nsec =
+					(s32)(b2r2_get_curr_nsec() -
+					request->start_time_nsec);
+				request->job.nsec_active_in_hw =
+					nsec_active_in_b2r2;
+
+				b2r2_call_profiler_blt_done(request);
+			}
+
+			b2r2_core_job_release(&request->job, __func__);
+		}
+	}
+
+	dec_stat(cont, &cont->stat_n_in_blt);
+
+	for (i = 0; i < tmp_buf_count; i++) {
+		dma_free_coherent(cont->dev,
+			work_bufs[i].size,
+			work_bufs[i].virt_addr,
+			work_bufs[i].phys_addr);
+		memset(&(work_bufs[i]), 0, sizeof(work_bufs[i]));
+	}
+
+	return request_id;
+
+job_add_failed:
+exit_dry_run:
+generic_conf_failed:
+alloc_work_bufs_failed:
+	for (i = 0; i < 4; i++) {
+		if (work_bufs[i].virt_addr != 0) {
+			dma_free_coherent(cont->dev,
+				work_bufs[i].size,
+				work_bufs[i].virt_addr,
+				work_bufs[i].phys_addr);
+			memset(&(work_bufs[i]), 0, sizeof(work_bufs[i]));
+		}
+	}
+
+generate_nodes_failed:
+	unresolve_buf(cont, &request->user_req.dst_img.buf,
+		&request->dst_resolved);
+resolve_dst_buf_failed:
+	unresolve_buf(cont, &request->user_req.src_mask.buf,
+		&request->src_mask_resolved);
+resolve_src_mask_buf_failed:
+	unresolve_buf(cont, &request->user_req.src_img.buf,
+		&request->src_resolved);
+resolve_src_buf_failed:
+synch_interrupted:
+zero_blt:
+	job_release_gen(&request->job);
+	dec_stat(cont, &cont->stat_n_jobs_released);
+	dec_stat(cont, &cont->stat_n_in_blt);
+
+	b2r2_log_info(cont->dev, "b2r2:%s ret=%d", __func__, ret);
+	return ret;
+}
+#endif /* CONFIG_B2R2_GENERIC */
+
+/**
+ * b2r2_blt_synch - Implements wait for all or a specified job
+ *
+ * @instance: The B2R2 BLT instance
+ * @request_id: If 0, wait for all requests on this instance to finish.
+ *              Else wait for request with given request id to finish.
+ */
+int b2r2_control_synch(struct b2r2_control_instance *instance,
+			int request_id)
+{
+	int ret = 0;
+	struct b2r2_control *cont = instance->control;
+
+	b2r2_log_info(cont->dev, "%s, request_id=%d\n", __func__, request_id);
+
+	if (request_id == 0) {
+		/* Wait for all requests */
+		inc_stat(cont, &cont->stat_n_in_synch_0);
+
+		/* Enter state "synching" if we have any active request */
+		mutex_lock(&instance->lock);
+		if (instance->no_of_active_requests)
+			instance->synching = true;
+		mutex_unlock(&instance->lock);
+
+		/* Wait until no longer in state synching */
+		ret = wait_event_interruptible(instance->synch_done_waitq,
+				!is_synching(instance));
+		dec_stat(cont, &cont->stat_n_in_synch_0);
+	} else {
+		struct b2r2_core_job *job;
+
+		inc_stat(cont, &cont->stat_n_in_synch_job);
+
+		/* Wait for specific job */
+		job = b2r2_core_job_find(cont, request_id);
+		if (job) {
+			/* Wait on find job */
+			ret = b2r2_core_job_wait(job);
+			/* Release matching the addref in b2r2_core_job_find */
+			b2r2_core_job_release(job, __func__);
+		}
+
+		/* If job not found we assume that is has been run */
+		dec_stat(cont, &cont->stat_n_in_synch_job);
+	}
+
+	b2r2_log_info(cont->dev,
+		"%s, request_id=%d, returns %d\n", __func__, request_id, ret);
+
+	return ret;
+}
+
+static void get_actual_dst_rect(struct b2r2_blt_req *req,
+					struct b2r2_blt_rect *actual_dst_rect)
+{
+	struct b2r2_blt_rect dst_img_bounds;
+
+	b2r2_get_img_bounding_rect(&req->dst_img, &dst_img_bounds);
+
+	b2r2_intersect_rects(&req->dst_rect, &dst_img_bounds, actual_dst_rect);
+
+	if (req->flags & B2R2_BLT_FLAG_DESTINATION_CLIP)
+		b2r2_intersect_rects(actual_dst_rect, &req->dst_clip_rect,
+				actual_dst_rect);
+}
+
+static void set_up_hwmem_region(struct b2r2_control *cont,
+		struct b2r2_blt_img *img, struct b2r2_blt_rect *rect,
+		struct hwmem_region *region)
+{
+	s32 img_size;
+
+	memset(region, 0, sizeof(*region));
+
+	if (b2r2_is_zero_area_rect(rect))
+		return;
+
+	img_size = b2r2_get_img_size(cont->dev, img);
+
+	if (b2r2_is_single_plane_fmt(img->fmt) &&
+			b2r2_is_independent_pixel_fmt(img->fmt)) {
+		int img_fmt_bpp = b2r2_get_fmt_bpp(cont->dev, img->fmt);
+		u32 img_pitch = b2r2_get_img_pitch(cont->dev, img);
+
+		region->offset = (u32)(img->buf.offset + (rect->y *
+				img_pitch));
+		region->count = (u32)rect->height;
+		region->start = (u32)((rect->x * img_fmt_bpp) / 8);
+		region->end = (u32)b2r2_div_round_up(
+				(rect->x + rect->width) * img_fmt_bpp, 8);
+		region->size = img_pitch;
+	} else {
+		/*
+		 * TODO: Locking entire buffer as a quick safe solution. In the
+		 * future we should lock less to avoid unecessary cache
+		 * synching. Pixel interleaved YCbCr formats should be quite
+		 * easy, just align start and stop points on 2.
+		 */
+		region->offset = (u32)img->buf.offset;
+		region->count = 1;
+		region->start = 0;
+		region->end = (u32)img_size;
+		region->size = (u32)img_size;
+	}
+}
+
+static int resolve_hwmem(struct b2r2_control *cont,
+		struct b2r2_blt_img *img,
+		struct b2r2_blt_rect *rect_2b_used,
+		bool is_dst,
+		struct b2r2_resolved_buf *resolved_buf)
+{
+	int return_value = 0;
+	enum hwmem_mem_type mem_type;
+	enum hwmem_access access;
+	enum hwmem_access required_access;
+	struct hwmem_mem_chunk mem_chunk;
+	size_t mem_chunk_length = 1;
+	struct hwmem_region region;
+
+	resolved_buf->hwmem_alloc =
+			hwmem_resolve_by_name(img->buf.hwmem_buf_name);
+	if (IS_ERR(resolved_buf->hwmem_alloc)) {
+		return_value = PTR_ERR(resolved_buf->hwmem_alloc);
+		b2r2_log_info(cont->dev, "%s: hwmem_resolve_by_name failed, "
+			"error code: %i\n", __func__, return_value);
+		goto resolve_failed;
+	}
+
+	hwmem_get_info(resolved_buf->hwmem_alloc, &resolved_buf->file_len,
+			&mem_type, &access);
+
+	required_access = (is_dst ? HWMEM_ACCESS_WRITE : HWMEM_ACCESS_READ) |
+			HWMEM_ACCESS_IMPORT;
+	if ((required_access & access) != required_access) {
+		b2r2_log_info(cont->dev,
+			"%s: Insufficient access to hwmem (%d, requires %d)"
+			"buffer.\n", __func__, access, required_access);
+		return_value = -EACCES;
+		goto access_check_failed;
+	}
+
+	if (mem_type != HWMEM_MEM_CONTIGUOUS_SYS) {
+		b2r2_log_info(cont->dev, "%s: Hwmem buffer is scattered.\n",
+			__func__);
+		return_value = -EINVAL;
+		goto buf_scattered;
+	}
+
+	if (resolved_buf->file_len <
+			img->buf.offset +
+			(__u32)b2r2_get_img_size(cont->dev, img)) {
+		b2r2_log_info(cont->dev, "%s: Hwmem buffer too small. (%d < "
+			"%d)\n", __func__, resolved_buf->file_len,
+			img->buf.offset +
+			(__u32)b2r2_get_img_size(cont->dev, img));
+		return_value = -EINVAL;
+		goto size_check_failed;
+	}
+
+	return_value = hwmem_pin(resolved_buf->hwmem_alloc, &mem_chunk,
+							 &mem_chunk_length);
+	if (return_value < 0) {
+		b2r2_log_info(cont->dev, "%s: hwmem_pin failed, "
+			"error code: %i\n", __func__, return_value);
+		goto pin_failed;
+	}
+	resolved_buf->file_physical_start = mem_chunk.paddr;
+
+	set_up_hwmem_region(cont, img, rect_2b_used, &region);
+	return_value = hwmem_set_domain(resolved_buf->hwmem_alloc,
+		required_access, HWMEM_DOMAIN_SYNC, &region);
+	if (return_value < 0) {
+		b2r2_log_info(cont->dev, "%s: hwmem_set_domain failed, "
+			"error code: %i\n", __func__, return_value);
+		goto set_domain_failed;
+	}
+
+	resolved_buf->physical_address =
+			resolved_buf->file_physical_start + img->buf.offset;
+
+	goto out;
+
+set_domain_failed:
+	hwmem_unpin(resolved_buf->hwmem_alloc);
+pin_failed:
+size_check_failed:
+buf_scattered:
+access_check_failed:
+	hwmem_release(resolved_buf->hwmem_alloc);
+resolve_failed:
+
+out:
+	return return_value;
+}
+
+static void unresolve_hwmem(struct b2r2_resolved_buf *resolved_buf)
+{
+	hwmem_unpin(resolved_buf->hwmem_alloc);
+	hwmem_release(resolved_buf->hwmem_alloc);
+}
+
+/**
+ * unresolve_buf() - Must be called after resolve_buf
+ *
+ * @buf: The buffer specification as supplied from user space
+ * @resolved: Gathered information about the buffer
+ *
+ * Returns 0 if OK else negative error code
+ */
+static void unresolve_buf(struct b2r2_control *cont,
+		struct b2r2_blt_buf *buf,
+		struct b2r2_resolved_buf *resolved)
+{
+#ifdef CONFIG_ANDROID_PMEM
+	if (resolved->is_pmem && resolved->filep)
+		put_pmem_file(resolved->filep);
+#endif
+	if (resolved->hwmem_alloc != NULL)
+		unresolve_hwmem(resolved);
+}
+
+/**
+ * get_fb_info() - Fill buf with framebuffer info
+ *
+ * @file: The framebuffer file
+ * @buf: Gathered information about the buffer
+ * @img_offset: Image offset info frame buffer
+ *
+ * Returns 0 if OK else negative error code
+ */
+static int get_fb_info(struct file *file,
+		struct b2r2_resolved_buf *buf,
+		__u32 img_offset)
+{
+#ifdef CONFIG_FB
+	if (file && buf &&
+			MAJOR(file->f_dentry->d_inode->i_rdev) == FB_MAJOR) {
+		int i;
+		/*
+		 * (OK to do it like this, no locking???)
+		 */
+		for (i = 0; i < num_registered_fb; i++) {
+			struct fb_info *info = registered_fb[i];
+
+			if (info && info->dev &&
+					MINOR(info->dev->devt) ==
+					MINOR(file->f_dentry->d_inode->i_rdev)) {
+				buf->file_physical_start = info->fix.smem_start;
+				buf->file_virtual_start = (u32)info->screen_base;
+				buf->file_len = info->fix.smem_len;
+				buf->physical_address = buf->file_physical_start +
+						img_offset;
+				buf->virtual_address =
+						(void *) (buf->file_virtual_start +
+						img_offset);
+				return 0;
+			}
+		}
+	}
+#endif
+	return -EINVAL;
+}
+
+/**
+ * resolve_buf() - Returns the physical & virtual addresses of a B2R2 blt buffer
+ *
+ * @img: The image specification as supplied from user space
+ * @rect_2b_used: The part of the image b2r2 will use.
+ * @usage: Specifies how the buffer will be used.
+ * @resolved: Gathered information about the buffer
+ *
+ * Returns 0 if OK else negative error code
+ */
+static int resolve_buf(struct b2r2_control *cont,
+		struct b2r2_blt_img *img,
+		struct b2r2_blt_rect *rect_2b_used,
+		bool is_dst,
+		struct b2r2_resolved_buf *resolved)
+{
+	int ret = 0;
+
+	memset(resolved, 0, sizeof(*resolved));
+
+	switch (img->buf.type) {
+	case B2R2_BLT_PTR_NONE:
+		break;
+
+	case B2R2_BLT_PTR_PHYSICAL:
+		resolved->physical_address = img->buf.offset;
+		resolved->file_len = img->buf.len;
+		break;
+
+		/* FD + OFFSET type */
+	case B2R2_BLT_PTR_FD_OFFSET: {
+		/*
+		 * TODO: Do we need to check if the process is allowed to
+		 * read/write (depending on if it's dst or src) to the file?
+		 */
+#ifdef CONFIG_ANDROID_PMEM
+		if (!get_pmem_file(
+				img->buf.fd,
+				(unsigned long *) &resolved->file_physical_start,
+				(unsigned long *) &resolved->file_virtual_start,
+				(unsigned long *) &resolved->file_len,
+				&resolved->filep)) {
+			resolved->physical_address =
+				resolved->file_physical_start +
+				img->buf.offset;
+			resolved->virtual_address = (void *)
+				(resolved->file_virtual_start +
+				img->buf.offset);
+			resolved->is_pmem = true;
+		} else
+#endif
+		{
+			int fput_needed;
+			struct file *file;
+
+			file = fget_light(img->buf.fd, &fput_needed);
+			if (file == NULL)
+				return -EINVAL;
+
+			ret = get_fb_info(file, resolved,
+					img->buf.offset);
+			fput_light(file, fput_needed);
+			if (ret < 0)
+				return ret;
+		}
+
+		/* Check bounds */
+		if (img->buf.offset + img->buf.len >
+				resolved->file_len) {
+			ret = -ESPIPE;
+			unresolve_buf(cont, &img->buf, resolved);
+		}
+
+		break;
+	}
+
+	case B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET:
+		ret = resolve_hwmem(cont, img, rect_2b_used, is_dst, resolved);
+		break;
+
+	default:
+		b2r2_log_warn(cont->dev, "%s: Failed to resolve buf type %d\n",
+			__func__, img->buf.type);
+
+		ret = -EINVAL;
+		break;
+
+	}
+
+	return ret;
+}
+
+/**
+ * sync_buf - Synchronizes the memory occupied by an image buffer.
+ *
+ * @buf: User buffer specification
+ * @resolved_buf: Gathered info (physical address etc.) about buffer
+ * @is_dst: true if the buffer is a destination buffer, false if the buffer is a
+ *          source buffer.
+ * @rect: rectangle in the image buffer that should be synced.
+ *        NULL if the buffer is a source mask.
+ * @img_width: width of the complete image buffer
+ * @fmt: buffer format
+*/
+static void sync_buf(struct b2r2_control *cont,
+		struct b2r2_blt_img *img,
+		struct b2r2_resolved_buf *resolved,
+		bool is_dst,
+		struct b2r2_blt_rect *rect)
+{
+	struct sync_args sa;
+	u32 start_phys, end_phys;
+
+	if (B2R2_BLT_PTR_NONE == img->buf.type ||
+			B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET == img->buf.type)
+		return;
+
+	start_phys = resolved->physical_address;
+	end_phys = resolved->physical_address + img->buf.len;
+
+	/*
+	 * TODO: Very ugly. We should find out whether the memory is coherent in
+	 * some generic way but cache handling will be rewritten soon so there
+	 * is no use spending time on it. In the new design this will probably
+	 * not be a problem.
+	 */
+	/* Frame buffer is coherent, at least now. */
+	if (!resolved->is_pmem) {
+		/*
+		 * Drain the write buffers as they are not always part of the
+		 * coherent concept.
+		 */
+		wmb();
+
+		return;
+	}
+
+	/*
+	 * src_mask does not have rect.
+	 * Also flush full buffer for planar and semiplanar YUV formats
+	 */
+	if (rect == NULL ||
+			(img->fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR) ||
+			(img->fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR) ||
+			(img->fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR) ||
+			(img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR) ||
+			(img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR) ||
+			(img->fmt ==
+				B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE) ||
+			(img->fmt ==
+				B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE)) {
+		sa.start = (unsigned long)resolved->virtual_address;
+		sa.end = (unsigned long)resolved->virtual_address +
+			img->buf.len;
+		start_phys = resolved->physical_address;
+		end_phys = resolved->physical_address + img->buf.len;
+	} else {
+		/*
+		 * buffer is not a src_mask so make use of rect when
+		 * clean & flush caches
+		 */
+		u32 bpp;	/* Bits per pixel */
+		u32 pitch;
+
+		switch (img->fmt) {
+		case B2R2_BLT_FMT_16_BIT_ARGB4444: /* Fall through */
+		case B2R2_BLT_FMT_16_BIT_ARGB1555: /* Fall through */
+		case B2R2_BLT_FMT_16_BIT_RGB565:   /* Fall through */
+		case B2R2_BLT_FMT_Y_CB_Y_CR:       /* Fall through */
+		case B2R2_BLT_FMT_CB_Y_CR_Y:
+			bpp = 16;
+			break;
+		case B2R2_BLT_FMT_24_BIT_RGB888:   /* Fall through */
+		case B2R2_BLT_FMT_24_BIT_ARGB8565: /* Fall through */
+		case B2R2_BLT_FMT_24_BIT_YUV888:
+		case B2R2_BLT_FMT_24_BIT_VUY888:
+			bpp =  24;
+			break;
+		case B2R2_BLT_FMT_32_BIT_ARGB8888: /* Fall through */
+		case B2R2_BLT_FMT_32_BIT_ABGR8888: /* Fall through */
+		case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		case B2R2_BLT_FMT_32_BIT_VUYA8888:
+			bpp = 32;
+			break;
+		default:
+			bpp = 12;
+		}
+		if (img->pitch == 0)
+			pitch = (img->width * bpp) / 8;
+		else
+			pitch = img->pitch;
+
+		/*
+		 * For 422I formats 2 horizontal pixels share color data.
+		 * Thus, the x position must be aligned down to closest even
+		 * number and width must be aligned up.
+		 */
+		{
+			s32 x;
+			s32 width;
+
+			switch (img->fmt) {
+			case B2R2_BLT_FMT_Y_CB_Y_CR:       /* Fall through */
+			case B2R2_BLT_FMT_CB_Y_CR_Y:
+				x = (rect->x / 2) * 2;
+				width = ((rect->width + 1) / 2) * 2;
+				break;
+			default:
+				x = rect->x;
+				width = rect->width;
+				break;
+			}
+
+			sa.start = (unsigned long)resolved->virtual_address +
+					rect->y * pitch + (x * bpp) / 8;
+			sa.end = (unsigned long)sa.start +
+					(rect->height - 1) * pitch +
+					(width * bpp) / 8;
+
+			start_phys = resolved->physical_address +
+					rect->y * pitch + (x * bpp) / 8;
+			end_phys = start_phys +
+					(rect->height - 1) * pitch +
+					(width * bpp) / 8;
+		}
+	}
+
+	/*
+	 * The virtual address to a pmem buffer is retrieved from ioremap, not
+	 * sure if it's	ok to use such an address as a kernel virtual address.
+	 * When doing it at a higher level such as dma_map_single it triggers an
+	 * error but at lower levels such as dmac_clean_range it seems to work,
+	 * hence the low level stuff.
+	 */
+
+	if (is_dst) {
+		/*
+		 * According to ARM's docs you must clean before invalidating
+		 * (ie flush) to avoid loosing data.
+		 */
+
+		/* Flush L1 cache */
+#ifdef CONFIG_SMP
+		flush_l1_cache_range_all_cpus(&sa);
+#else
+		flush_l1_cache_range_curr_cpu(&sa);
+#endif
+
+		/* Flush L2 cache */
+		outer_flush_range(start_phys, end_phys);
+	} else {
+		/* Clean L1 cache */
+#ifdef CONFIG_SMP
+		clean_l1_cache_range_all_cpus(&sa);
+#else
+		clean_l1_cache_range_curr_cpu(&sa);
+#endif
+
+		/* Clean L2 cache */
+		outer_clean_range(start_phys, end_phys);
+	}
+}
+
+/**
+ * is_report_list_empty() - Spin lock protected check of report list
+ *
+ * @instance: The B2R2 BLT instance
+ */
+static bool is_report_list_empty(struct b2r2_control_instance *instance)
+{
+	bool is_empty;
+
+	mutex_lock(&instance->lock);
+	is_empty = list_empty(&instance->report_list);
+	mutex_unlock(&instance->lock);
+
+	return is_empty;
+}
+
+/**
+ * is_synching() - Spin lock protected check if synching
+ *
+ * @instance: The B2R2 BLT instance
+ */
+static bool is_synching(struct b2r2_control_instance *instance)
+{
+	bool is_synching;
+
+	mutex_lock(&instance->lock);
+	is_synching = instance->synching;
+	mutex_unlock(&instance->lock);
+
+	return is_synching;
+}
+
+/**
+ * inc_stat() - Spin lock protected increment of statistics variable
+ *
+ * @stat: Pointer to statistics variable that should be incremented
+ */
+static void inc_stat(struct b2r2_control *cont, unsigned long *stat)
+{
+	mutex_lock(&cont->stat_lock);
+	(*stat)++;
+	mutex_unlock(&cont->stat_lock);
+}
+
+/**
+ * inc_stat() - Spin lock protected decrement of statistics variable
+ *
+ * @stat: Pointer to statistics variable that should be decremented
+ */
+static void dec_stat(struct b2r2_control *cont, unsigned long *stat)
+{
+	mutex_lock(&cont->stat_lock);
+	(*stat)--;
+	mutex_unlock(&cont->stat_lock);
+}
+
+
+#ifdef CONFIG_DEBUG_FS
+/**
+ * debugfs_b2r2_blt_request_read() - Implements debugfs read for B2R2 register
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_blt_request_read(struct file *filp, char __user *buf,
+			size_t count, loff_t *f_pos)
+{
+	size_t dev_size = 0;
+	int ret = 0;
+	char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+	struct b2r2_control *cont = filp->f_dentry->d_inode->i_private;
+
+	if (Buf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	dev_size = sprintf_req(&cont->debugfs_latest_request, Buf,
+		sizeof(char) * 4096);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (Buf != NULL)
+		kfree(Buf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_blt_request_fops - File operations for B2R2 request debugfs
+ */
+static const struct file_operations debugfs_b2r2_blt_request_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_blt_request_read,
+};
+
+/**
+ * struct debugfs_reg - Represents a B2R2 node "register"
+ *
+ * @name: Register name
+ * @offset: Offset within the node
+ */
+struct debugfs_reg {
+	const char  name[30];
+	u32   offset;
+};
+
+/**
+ * debugfs_node_regs - Array with all the registers in a B2R2 node, for debug
+ */
+static const struct debugfs_reg debugfs_node_regs[] = {
+	{"GROUP0.B2R2_NIP", offsetof(struct b2r2_link_list, GROUP0.B2R2_NIP)},
+	{"GROUP0.B2R2_CIC", offsetof(struct b2r2_link_list, GROUP0.B2R2_CIC)},
+	{"GROUP0.B2R2_INS", offsetof(struct b2r2_link_list, GROUP0.B2R2_INS)},
+	{"GROUP0.B2R2_ACK", offsetof(struct b2r2_link_list, GROUP0.B2R2_ACK)},
+
+	{"GROUP1.B2R2_TBA", offsetof(struct b2r2_link_list, GROUP1.B2R2_TBA)},
+	{"GROUP1.B2R2_TTY", offsetof(struct b2r2_link_list, GROUP1.B2R2_TTY)},
+	{"GROUP1.B2R2_TXY", offsetof(struct b2r2_link_list, GROUP1.B2R2_TXY)},
+	{"GROUP1.B2R2_TSZ", offsetof(struct b2r2_link_list, GROUP1.B2R2_TSZ)},
+
+	{"GROUP2.B2R2_S1CF", offsetof(struct b2r2_link_list, GROUP2.B2R2_S1CF)},
+	{"GROUP2.B2R2_S2CF", offsetof(struct b2r2_link_list, GROUP2.B2R2_S2CF)},
+
+	{"GROUP3.B2R2_SBA", offsetof(struct b2r2_link_list, GROUP3.B2R2_SBA)},
+	{"GROUP3.B2R2_STY", offsetof(struct b2r2_link_list, GROUP3.B2R2_STY)},
+	{"GROUP3.B2R2_SXY", offsetof(struct b2r2_link_list, GROUP3.B2R2_SXY)},
+	{"GROUP3.B2R2_SSZ", offsetof(struct b2r2_link_list, GROUP3.B2R2_SSZ)},
+
+	{"GROUP4.B2R2_SBA", offsetof(struct b2r2_link_list, GROUP4.B2R2_SBA)},
+	{"GROUP4.B2R2_STY", offsetof(struct b2r2_link_list, GROUP4.B2R2_STY)},
+	{"GROUP4.B2R2_SXY", offsetof(struct b2r2_link_list, GROUP4.B2R2_SXY)},
+	{"GROUP4.B2R2_SSZ", offsetof(struct b2r2_link_list, GROUP4.B2R2_SSZ)},
+
+	{"GROUP5.B2R2_SBA", offsetof(struct b2r2_link_list, GROUP5.B2R2_SBA)},
+	{"GROUP5.B2R2_STY", offsetof(struct b2r2_link_list, GROUP5.B2R2_STY)},
+	{"GROUP5.B2R2_SXY", offsetof(struct b2r2_link_list, GROUP5.B2R2_SXY)},
+	{"GROUP5.B2R2_SSZ", offsetof(struct b2r2_link_list, GROUP5.B2R2_SSZ)},
+
+	{"GROUP6.B2R2_CWO", offsetof(struct b2r2_link_list, GROUP6.B2R2_CWO)},
+	{"GROUP6.B2R2_CWS", offsetof(struct b2r2_link_list, GROUP6.B2R2_CWS)},
+
+	{"GROUP7.B2R2_CCO", offsetof(struct b2r2_link_list, GROUP7.B2R2_CCO)},
+	{"GROUP7.B2R2_CML", offsetof(struct b2r2_link_list, GROUP7.B2R2_CML)},
+
+	{"GROUP8.B2R2_FCTL", offsetof(struct b2r2_link_list, GROUP8.B2R2_FCTL)},
+	{"GROUP8.B2R2_PMK", offsetof(struct b2r2_link_list, GROUP8.B2R2_PMK)},
+
+	{"GROUP9.B2R2_RSF", offsetof(struct b2r2_link_list, GROUP9.B2R2_RSF)},
+	{"GROUP9.B2R2_RZI", offsetof(struct b2r2_link_list, GROUP9.B2R2_RZI)},
+	{"GROUP9.B2R2_HFP", offsetof(struct b2r2_link_list, GROUP9.B2R2_HFP)},
+	{"GROUP9.B2R2_VFP", offsetof(struct b2r2_link_list, GROUP9.B2R2_VFP)},
+
+	{"GROUP10.B2R2_RSF", offsetof(struct b2r2_link_list, GROUP10.B2R2_RSF)},
+	{"GROUP10.B2R2_RZI", offsetof(struct b2r2_link_list, GROUP10.B2R2_RZI)},
+	{"GROUP10.B2R2_HFP", offsetof(struct b2r2_link_list, GROUP10.B2R2_HFP)},
+	{"GROUP10.B2R2_VFP", offsetof(struct b2r2_link_list, GROUP10.B2R2_VFP)},
+
+	{"GROUP11.B2R2_FF0", offsetof(struct b2r2_link_list,
+					GROUP11.B2R2_FF0)},
+	{"GROUP11.B2R2_FF1", offsetof(struct b2r2_link_list,
+					GROUP11.B2R2_FF1)},
+	{"GROUP11.B2R2_FF2", offsetof(struct b2r2_link_list,
+					GROUP11.B2R2_FF2)},
+	{"GROUP11.B2R2_FF3", offsetof(struct b2r2_link_list,
+					GROUP11.B2R2_FF3)},
+
+	{"GROUP12.B2R2_KEY1", offsetof(struct b2r2_link_list,
+				GROUP12.B2R2_KEY1)},
+	{"GROUP12.B2R2_KEY2", offsetof(struct b2r2_link_list,
+				GROUP12.B2R2_KEY2)},
+
+	{"GROUP13.B2R2_XYL", offsetof(struct b2r2_link_list, GROUP13.B2R2_XYL)},
+	{"GROUP13.B2R2_XYP", offsetof(struct b2r2_link_list, GROUP13.B2R2_XYP)},
+
+	{"GROUP14.B2R2_SAR", offsetof(struct b2r2_link_list, GROUP14.B2R2_SAR)},
+	{"GROUP14.B2R2_USR", offsetof(struct b2r2_link_list, GROUP14.B2R2_USR)},
+
+	{"GROUP15.B2R2_VMX0", offsetof(struct b2r2_link_list,
+				GROUP15.B2R2_VMX0)},
+	{"GROUP15.B2R2_VMX1", offsetof(struct b2r2_link_list,
+				GROUP15.B2R2_VMX1)},
+	{"GROUP15.B2R2_VMX2", offsetof(struct b2r2_link_list,
+				GROUP15.B2R2_VMX2)},
+	{"GROUP15.B2R2_VMX3", offsetof(struct b2r2_link_list,
+				GROUP15.B2R2_VMX3)},
+
+	{"GROUP16.B2R2_VMX0", offsetof(struct b2r2_link_list,
+				GROUP16.B2R2_VMX0)},
+	{"GROUP16.B2R2_VMX1", offsetof(struct b2r2_link_list,
+				GROUP16.B2R2_VMX1)},
+	{"GROUP16.B2R2_VMX2", offsetof(struct b2r2_link_list,
+				GROUP16.B2R2_VMX2)},
+	{"GROUP16.B2R2_VMX3", offsetof(struct b2r2_link_list,
+				GROUP16.B2R2_VMX3)},
+};
+
+/**
+ * debugfs_b2r2_blt_stat_read() - Implements debugfs read for B2R2 BLT
+ *                                statistics
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_blt_stat_read(struct file *filp, char __user *buf,
+				size_t count, loff_t *f_pos)
+{
+	size_t dev_size = 0;
+	int ret = 0;
+	char *Buf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+	struct b2r2_control *cont = filp->f_dentry->d_inode->i_private;
+
+	if (Buf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	mutex_lock(&cont->stat_lock);
+	dev_size += sprintf(Buf + dev_size, "Added jobs           : %lu\n",
+		cont->stat_n_jobs_added);
+	dev_size += sprintf(Buf + dev_size, "Released jobs        : %lu\n",
+		cont->stat_n_jobs_released);
+	dev_size += sprintf(Buf + dev_size, "Jobs in report list  : %lu\n",
+		cont->stat_n_jobs_in_report_list);
+	dev_size += sprintf(Buf + dev_size, "Clients in open      : %lu\n",
+		cont->stat_n_in_open);
+	dev_size += sprintf(Buf + dev_size, "Clients in release   : %lu\n",
+		cont->stat_n_in_release);
+	dev_size += sprintf(Buf + dev_size, "Clients in blt       : %lu\n",
+		cont->stat_n_in_blt);
+	dev_size += sprintf(Buf + dev_size, "         synch       : %lu\n",
+		cont->stat_n_in_blt_synch);
+	dev_size += sprintf(Buf + dev_size, "           add       : %lu\n",
+		cont->stat_n_in_blt_add);
+	dev_size += sprintf(Buf + dev_size, "          wait       : %lu\n",
+		cont->stat_n_in_blt_wait);
+	dev_size += sprintf(Buf + dev_size, "Clients in synch 0   : %lu\n",
+		cont->stat_n_in_synch_0);
+	dev_size += sprintf(Buf + dev_size, "Clients in synch job : %lu\n",
+		cont->stat_n_in_synch_job);
+	dev_size += sprintf(Buf + dev_size, "Clients in query_cap : %lu\n",
+		cont->stat_n_in_query_cap);
+	mutex_unlock(&cont->stat_lock);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (Buf != NULL)
+		kfree(Buf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_blt_stat_fops() - File operations for B2R2 BLT
+ *                                statistics debugfs
+ */
+static const struct file_operations debugfs_b2r2_blt_stat_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_blt_stat_read,
+};
+#endif
+
+static void init_tmp_bufs(struct b2r2_control *cont)
+{
+	int i = 0;
+
+	for (i = 0; i < (sizeof(cont->tmp_bufs) / sizeof(struct tmp_buf));
+			i++) {
+		cont->tmp_bufs[i].buf.virt_addr = dma_alloc_coherent(
+			cont->dev, MAX_TMP_BUF_SIZE,
+			&cont->tmp_bufs[i].buf.phys_addr, GFP_DMA);
+		if (cont->tmp_bufs[i].buf.virt_addr != NULL)
+			cont->tmp_bufs[i].buf.size = MAX_TMP_BUF_SIZE;
+		else {
+			b2r2_log_err(cont->dev, "%s: Failed to allocate temp "
+				"buffer %i\n", __func__, i);
+			cont->tmp_bufs[i].buf.size = 0;
+		}
+	}
+}
+
+static void destroy_tmp_bufs(struct b2r2_control *cont)
+{
+	int i = 0;
+
+	for (i = 0; i < MAX_TMP_BUFS_NEEDED; i++) {
+		if (cont->tmp_bufs[i].buf.size != 0) {
+			dma_free_coherent(cont->dev,
+					cont->tmp_bufs[i].buf.size,
+					cont->tmp_bufs[i].buf.virt_addr,
+					cont->tmp_bufs[i].buf.phys_addr);
+
+			cont->tmp_bufs[i].buf.size = 0;
+		}
+	}
+}
+
+/**
+ * b2r2_blt_module_init() - Module init function
+ *
+ * Returns 0 if OK else negative error code
+ */
+int b2r2_control_init(struct b2r2_control *cont)
+{
+	int ret;
+
+	mutex_init(&cont->stat_lock);
+
+#ifdef CONFIG_B2R2_GENERIC
+	/* Initialize generic path */
+	b2r2_generic_init(cont);
+#endif
+	/* Initialize node splitter */
+	ret = b2r2_node_split_init(cont);
+	if (ret) {
+		printk(KERN_WARNING "%s: node split init fails\n", __func__);
+		goto b2r2_node_split_init_fail;
+	}
+
+	b2r2_log_info(cont->dev, "%s: device registered\n", __func__);
+
+	cont->dev->coherent_dma_mask = 0xFFFFFFFF;
+	init_tmp_bufs(cont);
+	ret = b2r2_filters_init(cont);
+	if (ret) {
+		b2r2_log_warn(cont->dev, "%s: failed to init filters\n",
+			__func__);
+		goto b2r2_filter_init_fail;
+	}
+
+	/* Initialize memory allocator */
+	ret = b2r2_mem_init(cont, B2R2_HEAP_SIZE,
+			4, sizeof(struct b2r2_node));
+	if (ret) {
+		printk(KERN_WARNING "%s: initializing B2R2 memhandler fails\n",
+				__func__);
+		goto b2r2_mem_init_fail;
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	/* Register debug fs */
+	if (!IS_ERR_OR_NULL(cont->debugfs_root_dir)) {
+		debugfs_create_file("last_request", 0666,
+			cont->debugfs_root_dir,
+			cont, &debugfs_b2r2_blt_request_fops);
+		debugfs_create_file("stats", 0666,
+			cont->debugfs_root_dir,
+			cont, &debugfs_b2r2_blt_stat_fops);
+	}
+#endif
+
+	b2r2_log_info(cont->dev, "%s: done\n", __func__);
+
+	return ret;
+
+b2r2_mem_init_fail:
+	b2r2_filters_exit(cont);
+b2r2_filter_init_fail:
+	b2r2_node_split_exit(cont);
+b2r2_node_split_init_fail:
+#ifdef CONFIG_B2R2_GENERIC
+	b2r2_generic_exit(cont);
+#endif
+	return ret;
+}
+
+/**
+ * b2r2_control_exit() - Module exit function
+ */
+void b2r2_control_exit(struct b2r2_control *cont)
+{
+	if (cont) {
+		b2r2_log_info(cont->dev, "%s\n", __func__);
+#ifdef CONFIG_DEBUG_FS
+		if (!IS_ERR_OR_NULL(cont->debugfs_root_dir)) {
+			debugfs_remove_recursive(cont->debugfs_root_dir);
+			cont->debugfs_root_dir = NULL;
+		}
+#endif
+		b2r2_mem_exit(cont);
+		destroy_tmp_bufs(cont);
+		b2r2_node_split_exit(cont);
+#if defined(CONFIG_B2R2_GENERIC)
+		b2r2_generic_exit(cont);
+#endif
+		b2r2_filters_exit(cont);
+	}
+}
+
+MODULE_AUTHOR("Robert Fekete <robert.fekete@stericsson.com>");
+MODULE_DESCRIPTION("ST-Ericsson B2R2 Blitter module");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/b2r2/b2r2_control.h b/drivers/video/b2r2/b2r2_control.h
new file mode 100644
index 00000000000..d13d2188618
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_control.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2012
+ *
+ * ST-Ericsson B2R2 internal definitions
+ *
+ * Author: Jorgen Nilsson <jorgen.nilsson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_CONTROL_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_CONTROL_H_
+
+#include "b2r2_internal.h"
+
+int b2r2_control_init(struct b2r2_control *cont);
+void b2r2_control_exit(struct b2r2_control *cont);
+int b2r2_control_open(struct b2r2_control_instance *instance);
+int b2r2_control_release(struct b2r2_control_instance *instance);
+
+int b2r2_control_blt(struct b2r2_blt_request *request);
+int b2r2_generic_blt(struct b2r2_blt_request *request);
+int b2r2_control_waitjob(struct b2r2_blt_request *request);
+int b2r2_control_synch(struct b2r2_control_instance *instance,
+			int request_id);
+size_t b2r2_control_read(struct b2r2_control_instance *instance,
+		struct b2r2_blt_request **request_out, bool block);
+size_t b2r2_control_read_id(struct b2r2_control_instance *instance,
+		struct b2r2_blt_request **request_out, bool block,
+		int request_id);
+
+#endif
diff --git a/drivers/video/b2r2/b2r2_core.c b/drivers/video/b2r2/b2r2_core.c
new file mode 100644
index 00000000000..02071d5f989
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_core.c
@@ -0,0 +1,2763 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 core driver
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+/*
+ * TODO: Clock address from platform data
+ * Platform data should have string id instead of numbers
+ * b2r2_remove, some type of runtime problem when kernel hacking
+ * debug features on
+ *
+ * Is there already a priority list in kernel?
+ * Is it possible to handle clock using clock framework?
+ * uTimeOut, use mdelay instead?
+ * Measure performance
+ *
+ * Exchange our home-cooked ref count with kernel kref? See
+ *    http://lwn.net/Articles/336224/
+ *
+ * B2R2:
+ * Source fill 2 bug
+ * Check with Symbian?
+ */
+
+/* include file */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/uaccess.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#ifdef CONFIG_DEBUG_FS
+#include <linux/debugfs.h>
+#endif
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/kref.h>
+
+#include "b2r2_internal.h"
+#include "b2r2_core.h"
+#include "b2r2_global.h"
+#include "b2r2_structures.h"
+#include "b2r2_control.h"
+#include "b2r2_profiler_api.h"
+#include "b2r2_timing.h"
+#include "b2r2_debug.h"
+
+/**
+ * B2R2 Hardware defines below
+ */
+
+/* - BLT_AQ_CTL */
+#define	B2R2_AQ_Enab			(0x80000000)
+#define B2R2_AQ_PRIOR_0 		(0x0)
+#define	B2R2_AQ_PRIOR_1 		(0x1)
+#define	B2R2_AQ_PRIOR_2			(0x2)
+#define	B2R2_AQ_PRIOR_3			(0x3)
+#define B2R2_AQ_NODE_REPEAT_INT	(0x100000)
+#define B2R2_AQ_STOP_INT		(0x200000)
+#define B2R2_AQ_LNA_REACH_INT	(0x400000)
+#define B2R2_AQ_COMPLETED_INT	(0x800000)
+
+/* - BLT_CTL */
+#define	B2R2BLT_CTLGLOBAL_soft_reset	(0x80000000)
+#define	B2R2BLT_CTLStep_By_Step		(0x20000000)
+#define	B2R2BLT_CTLBig_not_little	(0x10000000)
+#define	B2R2BLT_CTLMask			(0xb0000000)
+#define	B2R2BLT_CTLTestMask		(0xb0000000)
+#define	B2R2BLT_CTLInitialValue		(0x0)
+#define	B2R2BLT_CTLAccessType		(INITIAL_TEST)
+#define	B2R2BLT_CTL			(0xa00)
+
+/* - BLT_ITS */
+#define	B2R2BLT_ITSRLD_ERROR		(0x80000000)
+#define	B2R2BLT_ITSAQ4_Node_Notif	(0x8000000)
+#define	B2R2BLT_ITSAQ4_Node_repeat	(0x4000000)
+#define	B2R2BLT_ITSAQ4_Stopped		(0x2000000)
+#define	B2R2BLT_ITSAQ4_LNA_Reached	(0x1000000)
+#define	B2R2BLT_ITSAQ3_Node_Notif	(0x800000)
+#define	B2R2BLT_ITSAQ3_Node_repeat	(0x400000)
+#define	B2R2BLT_ITSAQ3_Stopped		(0x200000)
+#define	B2R2BLT_ITSAQ3_LNA_Reached	(0x100000)
+#define	B2R2BLT_ITSAQ2_Node_Notif	(0x80000)
+#define	B2R2BLT_ITSAQ2_Node_repeat	(0x40000)
+#define	B2R2BLT_ITSAQ2_Stopped		(0x20000)
+#define	B2R2BLT_ITSAQ2_LNA_Reached	(0x10000)
+#define	B2R2BLT_ITSAQ1_Node_Notif	(0x8000)
+#define	B2R2BLT_ITSAQ1_Node_repeat	(0x4000)
+#define	B2R2BLT_ITSAQ1_Stopped		(0x2000)
+#define	B2R2BLT_ITSAQ1_LNA_Reached	(0x1000)
+#define	B2R2BLT_ITSCQ2_Repaced		(0x80)
+#define	B2R2BLT_ITSCQ2_Node_Notif	(0x40)
+#define	B2R2BLT_ITSCQ2_retriggered	(0x20)
+#define	B2R2BLT_ITSCQ2_completed	(0x10)
+#define	B2R2BLT_ITSCQ1_Repaced		(0x8)
+#define	B2R2BLT_ITSCQ1_Node_Notif	(0x4)
+#define	B2R2BLT_ITSCQ1_retriggered	(0x2)
+#define	B2R2BLT_ITSCQ1_completed	(0x1)
+#define	B2R2BLT_ITSMask			(0x8ffff0ff)
+#define	B2R2BLT_ITSTestMask		(0x8ffff0ff)
+#define	B2R2BLT_ITSInitialValue		(0x0)
+#define	B2R2BLT_ITSAccessType		(INITIAL_TEST)
+#define	B2R2BLT_ITS			(0xa04)
+
+/* - BLT_STA1 */
+#define	B2R2BLT_STA1BDISP_IDLE		(0x1)
+#define	B2R2BLT_STA1Mask		(0x1)
+#define	B2R2BLT_STA1TestMask		(0x1)
+#define	B2R2BLT_STA1InitialValue	(0x1)
+#define	B2R2BLT_STA1AccessType		(INITIAL_TEST)
+#define	B2R2BLT_STA1			(0xa08)
+
+/**
+ * b2r2_core - Quick link to administration data for B2R2
+ */
+static struct b2r2_core   *b2r2_core[B2R2_MAX_NBR_DEVICES];
+
+/* Local functions */
+static void check_prio_list(struct b2r2_core *core, bool atomic);
+static void  clear_interrupts(struct b2r2_core *core);
+static void trigger_job(struct b2r2_core *core, struct b2r2_core_job *job);
+static void exit_job_list(struct b2r2_core *core,
+		struct list_head *job_list);
+static void job_work_function(struct work_struct *ptr);
+static void init_job(struct b2r2_core_job *job);
+static void insert_into_prio_list(struct b2r2_core *core,
+		struct b2r2_core_job *job);
+static struct b2r2_core_job *find_job_in_list(int job_id,
+		struct list_head *list);
+static struct b2r2_core_job *find_job_in_active_jobs(struct b2r2_core *core,
+		int job_id);
+static struct b2r2_core_job *find_tag_in_list(struct b2r2_core *core,
+		int tag, struct list_head *list);
+static struct b2r2_core_job *find_tag_in_active_jobs(struct b2r2_core *core,
+		int tag);
+
+static int domain_enable(struct b2r2_core *core);
+static void domain_disable(struct b2r2_core *core);
+
+static void stop_queue(enum b2r2_core_queue queue);
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+static void printk_regs(struct b2r2_core *core);
+static int hw_reset(struct b2r2_core *core);
+static void timeout_work_function(struct work_struct *ptr);
+#endif
+
+static void reset_hw_timer(struct b2r2_core_job *job);
+static void start_hw_timer(struct b2r2_core_job *job);
+static void stop_hw_timer(struct b2r2_core *core,
+		struct b2r2_core_job *job);
+
+static int init_hw(struct b2r2_core *core);
+static void exit_hw(struct b2r2_core *core);
+
+/* Tracking release bug... */
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+/**
+ * ar_add() - Adds an addref or a release to the array
+ *
+ * @core: The b2r2 core entity
+ * @job: The job that has been referenced
+ * @caller: The caller of addref / release
+ * @addref: true if it is an addref else false for release
+ */
+static void ar_add(struct b2r2_core *core, struct b2r2_core_job *job,
+		const char *caller, bool addref)
+{
+	core->ar[core->ar_write].addref = addref;
+	core->ar[core->ar_write].job = job;
+	core->ar[core->ar_write].caller = caller;
+	core->ar[core->ar_write].ref_count = job->ref_count;
+	core->ar_write = (core->ar_write + 1) %
+		ARRAY_SIZE(core->ar);
+	if (core->ar_write == core->ar_read)
+		core->ar_read = (core->ar_read + 1) %
+			ARRAY_SIZE(core->ar);
+}
+
+/**
+ * sprintf_ar() - Writes all addref / release to a string buffer
+ *
+ * @core: The b2r2 core entity
+ * @buf: Receiving character bufefr
+ * @job: Which job to write or NULL for all
+ *
+ * NOTE! No buffer size check!!
+ */
+static char *sprintf_ar(struct b2r2_core *core, char *buf,
+		struct b2r2_core_job *job)
+{
+	int i;
+	int size = 0;
+
+	for (i = core->ar_read; i != core->ar_write;
+			i = (i + 1) % ARRAY_SIZE(core->ar)) {
+		struct addref_release *ar = &core->ar[i];
+		if (!job || job == ar->job)
+			size += sprintf(buf + size,
+					"%s on %p from %s, ref = %d\n",
+					ar->addref ? "addref" : "release",
+					ar->job, ar->caller, ar->ref_count);
+	}
+
+	return buf;
+}
+
+/**
+ * printk_ar() - Writes all addref / release using dev_info
+ *
+ * @core: The b2r2 core entity
+ * @job: Which job to write or NULL for all
+ */
+static void printk_ar(struct b2r2_core *core, struct b2r2_core_job *job)
+{
+	int i;
+
+	for (i = core->ar_read; i != core->ar_write;
+			i = (i + 1) % ARRAY_SIZE(core->ar)) {
+		struct addref_release *ar = &core->ar[i];
+		if (!job || job == ar->job)
+			b2r2_log_info(core->dev, "%s on %p from %s,"
+				" ref = %d\n",
+				ar->addref ? "addref" : "release",
+				ar->job, ar->caller, ar->ref_count);
+	}
+}
+#endif
+
+/**
+ * internal_job_addref() - Increments the reference count for a job
+ *
+ * @core: The b2r2 core entity
+ * @job: Which job to increment reference count for
+ * @caller: Name of function calling addref (for debug)
+ *
+ * Note that core->lock _must_ be held
+ */
+static void internal_job_addref(struct b2r2_core *core,
+		struct b2r2_core_job *job, const char *caller)
+{
+	u32 ref_count;
+
+	/* Sanity checks */
+	BUG_ON(core == NULL);
+	BUG_ON(job == NULL);
+
+	b2r2_log_info(core->dev, "%s (core: %p, job: %p) (from %s)\n",
+		__func__, core, job, caller);
+
+
+	if (job->start_sentinel != START_SENTINEL ||
+			job->end_sentinel != END_SENTINEL ||
+			job->ref_count == 0 || job->ref_count > 10)	{
+		b2r2_log_info(core->dev, "%s: (core: %p, job: %p) "
+			"start=%X end=%X ref_count=%d\n",
+			__func__, core, job, job->start_sentinel,
+			job->end_sentinel, job->ref_count);
+
+	/* Something is wrong, print the addref / release array */
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+		printk_ar(core, NULL);
+#endif
+	}
+
+
+	BUG_ON(job->start_sentinel != START_SENTINEL);
+	BUG_ON(job->end_sentinel != END_SENTINEL);
+
+	/* Do the actual reference count increment */
+	ref_count = ++job->ref_count;
+
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+	/* Keep track of addref / release */
+	ar_add(core, job, caller, true);
+#endif
+
+	b2r2_log_info(core->dev, "%s called from %s (core: %p, job: %p): Ref "
+		"Count is %d\n", __func__, caller, core, job, job->ref_count);
+}
+
+/**
+ * internal_job_release() - Decrements the reference count for a job
+ *
+ * @core: The b2r2 core entity
+ * @job: Which job to decrement reference count for
+ * @caller: Name of function calling release (for debug)
+ *
+ * Returns true if job_release should be called by caller
+ * (reference count reached zero).
+ *
+ * Note that core->lock _must_ be held
+ */
+static bool internal_job_release(struct b2r2_core *core,
+		struct b2r2_core_job *job, const char *caller)
+{
+	u32 ref_count;
+	bool call_release = false;
+
+	/* Sanity checks */
+	BUG_ON(job == NULL);
+
+	b2r2_log_info(core->dev, "%s (core: %p, job: %p) (from %s)\n",
+		__func__, core, job, caller);
+
+	if (job->start_sentinel != START_SENTINEL ||
+			job->end_sentinel != END_SENTINEL ||
+			job->ref_count == 0 || job->ref_count > 10) {
+		b2r2_log_info(core->dev, "%s: (core: %p, job: %p) start=%X "
+			"end=%X ref_count=%d\n", __func__, core, job,
+			job->start_sentinel, job->end_sentinel,
+			job->ref_count);
+
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+		printk_ar(core, NULL);
+#endif
+	}
+
+	BUG_ON(job->start_sentinel != START_SENTINEL);
+	BUG_ON(job->end_sentinel != END_SENTINEL);
+	BUG_ON(job->ref_count == 0 || job->ref_count > 10);
+
+	/* Do the actual decrement */
+	ref_count = --job->ref_count;
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+	ar_add(core, job, caller, false);
+#endif
+	b2r2_log_info(core->dev, "%s called from %s (core: %p, job: %p) "
+		"Ref Count is %d\n", __func__, caller, core, job, ref_count);
+
+	if (!ref_count && job->release) {
+		call_release = true;
+		/* Job will now cease to exist */
+		job->start_sentinel = 0xFFFFFFFF;
+		job->end_sentinel = 0xFFFFFFFF;
+	}
+	return call_release;
+}
+
+
+
+/* Exported functions */
+
+/**
+ * core->lock _must_ _NOT_ be held when calling this function
+ */
+void b2r2_core_job_addref(struct b2r2_core_job *job, const char *caller)
+{
+	unsigned long flags;
+	struct b2r2_core *core;
+
+	BUG_ON(job == NULL || job->data == 0);
+	core = (struct b2r2_core *) job->data;
+
+	spin_lock_irqsave(&core->lock, flags);
+	internal_job_addref(core, job, caller);
+	spin_unlock_irqrestore(&core->lock, flags);
+}
+
+/**
+ * core->lock _must_ _NOT_ be held when calling this function
+ */
+void b2r2_core_job_release(struct b2r2_core_job *job, const char *caller)
+{
+	unsigned long flags;
+	bool call_release = false;
+	struct b2r2_core *core;
+
+	BUG_ON(job == NULL || job->data == 0);
+	core = (struct b2r2_core *) job->data;
+
+	spin_lock_irqsave(&core->lock, flags);
+	call_release = internal_job_release(core, job, caller);
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	if (call_release)
+		job->release(job);
+}
+
+/**
+ * core->lock _must_ _NOT_ be held when calling this function
+ */
+int b2r2_core_job_add(struct b2r2_control *control,
+		struct b2r2_core_job *job)
+{
+	unsigned long flags;
+	struct b2r2_core *core = control->data;
+
+	b2r2_log_info(core->dev, "%s (core: %p, job: %p)\n",
+		__func__, core, job);
+
+	/* Enable B2R2 */
+	domain_enable(core);
+
+	spin_lock_irqsave(&core->lock, flags);
+	/* Check that we have not been powered down */
+	if (!core->domain_enabled) {
+		spin_unlock_irqrestore(&core->lock, flags);
+		return -ENOSYS;
+	}
+
+	core->stat_n_jobs_added++;
+
+	/* Initialise internal job data */
+	init_job(job);
+
+	/* Initial reference, should be released by caller of this function */
+	job->ref_count = 1;
+
+	/* Insert job into prio list */
+	insert_into_prio_list(core, job);
+
+	/* Check if we can dispatch job */
+	check_prio_list(core, false);
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	return job->job_id;
+}
+
+/**
+ * core->lock _must_ _NOT_ be held when calling this function
+ */
+struct b2r2_core_job *b2r2_core_job_find(struct b2r2_control *control,
+		int job_id)
+{
+	unsigned long flags;
+	struct b2r2_core_job *job;
+	struct b2r2_core *core = control->data;
+
+	b2r2_log_info(core->dev, "%s (core: %p, job_id: %d)\n",
+		__func__, core, job_id);
+
+	spin_lock_irqsave(&core->lock, flags);
+	/* Look through prio queue */
+	job = find_job_in_list(job_id, &core->prio_queue);
+
+	if (!job)
+		job = find_job_in_active_jobs(core, job_id);
+
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	return job;
+}
+
+/**
+ * core->lock _must_ _NOT_ be held when calling this function
+ */
+struct b2r2_core_job *b2r2_core_job_find_first_with_tag(
+		struct b2r2_control *control, int tag)
+{
+	unsigned long flags;
+	struct b2r2_core_job *job;
+	struct b2r2_core *core = control->data;
+
+	b2r2_log_info(core->dev,
+		"%s (core: %p, tag: %d)\n", __func__, core, tag);
+
+	spin_lock_irqsave(&core->lock, flags);
+	/* Look through prio queue */
+	job = find_tag_in_list(core, tag, &core->prio_queue);
+
+	if (!job)
+		job = find_tag_in_active_jobs(core, tag);
+
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	return job;
+}
+
+/**
+ * is_job_done() - Spin lock protected check if job is done
+ *
+ * @job: Job to check
+ *
+ * Returns true if job is done or cancelled
+ *
+ * core->lock must _NOT_ be held when calling this function
+ */
+static bool is_job_done(struct b2r2_core_job *job)
+{
+	unsigned long flags;
+	bool job_is_done;
+	struct b2r2_core *core = (struct b2r2_core *) job->data;
+
+	spin_lock_irqsave(&core->lock, flags);
+	job_is_done =
+		job->job_state != B2R2_CORE_JOB_QUEUED &&
+		job->job_state != B2R2_CORE_JOB_RUNNING;
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	return job_is_done;
+}
+
+/**
+ * b2r2_core_job_wait()
+ *
+ * @job:
+ *
+ * core->lock _must_ _NOT_ be held when calling this function
+ */
+int b2r2_core_job_wait(struct b2r2_core_job *job)
+{
+	int ret = 0;
+#ifdef CONFIG_B2R2_DEBUG
+	struct b2r2_core *core = (struct b2r2_core *) job->data;
+#endif
+
+	b2r2_log_info(core->dev, "%s (core: %p, job: %p)\n",
+		__func__, core, job);
+	/* Check that we have the job */
+	if (job->job_state == B2R2_CORE_JOB_IDLE) {
+		/* Never or not queued */
+		b2r2_log_info(core->dev, "%s: Job not queued\n", __func__);
+		return -ENOENT;
+	}
+
+	/* Wait for the job to be done */
+	ret = wait_event_interruptible(
+		job->event,
+		is_job_done(job));
+
+	if (ret)
+		b2r2_log_warn(core->dev,
+			"%s: wait_event_interruptible returns %d state is %d",
+			__func__, ret, job->job_state);
+	return ret;
+}
+
+/**
+ * cancel_job() - Cancels a job (removes it from prio list or active jobs) and
+ *                calls the job callback
+ *
+ * @job: Job to cancel
+ *
+ * Returns true if the job was found and cancelled
+ *
+ * core->lock must be held when calling this function
+ */
+static bool cancel_job(struct b2r2_core *core, struct b2r2_core_job *job)
+{
+	bool found_job = false;
+	bool job_was_active = false;
+
+	/* Remove from prio list */
+	if (job->job_state == B2R2_CORE_JOB_QUEUED) {
+		list_del_init(&job->list);
+		found_job = true;
+	}
+
+	/* Remove from active jobs */
+	if (!found_job && core->n_active_jobs > 0) {
+		int i;
+
+		/* Look for timeout:ed jobs and put them in tmp list */
+		for (i = 0; i < ARRAY_SIZE(core->active_jobs); i++) {
+			if (core->active_jobs[i] == job) {
+				stop_queue((enum b2r2_core_queue)i);
+				stop_hw_timer(core, job);
+				core->active_jobs[i] = NULL;
+				core->n_active_jobs--;
+				found_job = true;
+				job_was_active = true;
+			}
+		}
+	}
+
+	/* Handle done list & callback */
+	if (found_job) {
+		/* Job is canceled */
+		job->job_state = B2R2_CORE_JOB_CANCELED;
+
+		queue_work(core->work_queue, &job->work);
+
+		/* Statistics */
+		if (!job_was_active)
+			core->stat_n_jobs_in_prio_list--;
+
+	}
+
+	return found_job;
+}
+
+/* core->lock _must_ _NOT_ be held when calling this function */
+int b2r2_core_job_cancel(struct b2r2_core_job *job)
+{
+	unsigned long flags;
+	int ret = 0;
+	struct b2r2_core *core = (struct b2r2_core *) job->data;
+
+	b2r2_log_info(core->dev, "%s (core: %p, job: %p) (st: %d)\n",
+		__func__, core, job, job->job_state);
+	/* Check that we have the job */
+	if (job->job_state == B2R2_CORE_JOB_IDLE) {
+		/* Never or not queued */
+		b2r2_log_info(core->dev, "%s: Job not queued\n", __func__);
+		return -ENOENT;
+	}
+
+	/* Remove from prio list */
+	spin_lock_irqsave(&core->lock, flags);
+	cancel_job(core, job);
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	return ret;
+}
+
+/* LOCAL FUNCTIONS BELOW */
+
+/**
+ * domain_disable_work_function()
+ *
+ * @core: The b2r2 core entity
+ */
+static void domain_disable_work_function(struct work_struct *work)
+{
+	struct delayed_work *twork = to_delayed_work(work);
+	struct b2r2_core *core = container_of(
+			twork, struct b2r2_core, domain_disable_work);
+
+	if (!mutex_trylock(&core->domain_lock))
+		return;
+
+	if (core->domain_request_count == 0) {
+		core->valid = false;
+		exit_hw(core);
+		clk_disable(core->b2r2_clock);
+		regulator_disable(core->b2r2_reg);
+		core->domain_enabled = false;
+	}
+
+	mutex_unlock(&core->domain_lock);
+}
+
+/**
+ * domain_enable()
+ *
+ * @core: The b2r2 core entity
+ */
+static int domain_enable(struct b2r2_core *core)
+{
+	mutex_lock(&core->domain_lock);
+	core->domain_request_count++;
+
+	if (!core->domain_enabled) {
+		int retry = 0;
+		int ret;
+again:
+		/*
+		 * Since regulator_enable() may sleep we have to handle
+		 * interrupts.
+		 */
+		ret = regulator_enable(core->b2r2_reg);
+		if ((ret == -EAGAIN) &&
+				((retry++) < B2R2_REGULATOR_RETRY_COUNT))
+			goto again;
+		else if (ret < 0)
+			goto regulator_enable_failed;
+
+		ret = clk_enable(core->b2r2_clock);
+		if (ret < 0) {
+			b2r2_log_err(core->dev,
+				"%s: Could not enable clock\n", __func__);
+			goto enable_clk_failed;
+		}
+		if (init_hw(core) < 0)
+			goto init_hw_failed;
+		core->domain_enabled = true;
+		core->valid = true;
+	}
+
+	mutex_unlock(&core->domain_lock);
+
+	return 0;
+
+init_hw_failed:
+	b2r2_log_err(core->dev,
+		"%s: Could not initialize hardware!\n", __func__);
+	clk_disable(core->b2r2_clock);
+
+enable_clk_failed:
+	if (regulator_disable(core->b2r2_reg) < 0)
+		b2r2_log_err(core->dev, "%s: regulator_disable failed!\n",
+				__func__);
+
+regulator_enable_failed:
+	core->domain_request_count--;
+	mutex_unlock(&core->domain_lock);
+
+	return -EFAULT;
+}
+
+/**
+ * domain_disable()
+ *
+ * @core: The b2r2 core entity
+ */
+static void domain_disable(struct b2r2_core *core)
+{
+	mutex_lock(&core->domain_lock);
+
+	if (core->domain_request_count == 0) {
+		b2r2_log_err(core->dev,
+			"%s: Unbalanced domain_disable()\n", __func__);
+	} else {
+		core->domain_request_count--;
+
+		/* Cancel any existing work */
+		cancel_delayed_work_sync(&core->domain_disable_work);
+
+		/* Add a work to disable the power and clock after a delay */
+		queue_delayed_work(core->work_queue, &core->domain_disable_work,
+				B2R2_DOMAIN_DISABLE_TIMEOUT);
+	}
+
+	mutex_unlock(&core->domain_lock);
+}
+
+/**
+ * stop_queue() - Stops the specified queue.
+ */
+static void stop_queue(enum b2r2_core_queue queue)
+{
+	/* TODO: Implement! If this function is not implemented canceled jobs
+	 * will use b2r2 which is a waste of resources. Not stopping jobs will
+	 * also screw up the hardware timing, the job the canceled job
+	 * intrerrupted (if any) will be billed for the time between the point
+	 * where the job is cancelled and when it stops. */
+}
+
+/**
+ * exit_job_list() - Empties a job queue by canceling the jobs
+ *
+ * @core: The b2r2 core entity
+ *
+ * core->lock _must_ be held when calling this function
+ */
+static void exit_job_list(struct b2r2_core *core,
+		struct list_head *job_queue)
+{
+	while (!list_empty(job_queue)) {
+		struct b2r2_core_job *job =
+			list_entry(job_queue->next,
+				struct b2r2_core_job,
+				list);
+		/* Add reference to prevent job from disappearing
+		   in the middle of our work, released below */
+		internal_job_addref(core, job, __func__);
+
+		cancel_job(core, job);
+
+		/* Matching release to addref above */
+		internal_job_release(core, job, __func__);
+
+	}
+}
+
+/**
+ * job_work_function() - Work queue function that calls callback(s) and
+ *                       checks if B2R2 can accept a new job
+ *
+ * @ptr: Pointer to work struct (embedded in struct b2r2_core_job)
+ */
+static void job_work_function(struct work_struct *ptr)
+{
+	unsigned long flags;
+	struct b2r2_core_job *job =
+			container_of(ptr, struct b2r2_core_job, work);
+	struct b2r2_core *core = (struct b2r2_core *) job->data;
+
+	/* Disable B2R2 */
+	domain_disable(core);
+
+	/* Release resources */
+	if (job->release_resources)
+		job->release_resources(job, false);
+
+	spin_lock_irqsave(&core->lock, flags);
+
+	/* Dispatch a new job if possible */
+	check_prio_list(core, false);
+
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	/* Tell the client */
+	if (job->callback)
+		job->callback(job);
+
+	/* Drop our reference, matches the
+	   addref in handle_queue_event or b2r2_core_job_cancel */
+	b2r2_core_job_release(job, __func__);
+}
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+/**
+ * timeout_work_function() - Work queue function that checks for
+ *                           timeout:ed jobs. B2R2 might silently refuse
+ *                           to execute some jobs, i.e. SRC2 fill
+ *
+ * @ptr: Pointer to work struct (embedded in struct b2r2_core)
+ *
+ */
+static void timeout_work_function(struct work_struct *ptr)
+{
+	unsigned long flags;
+	struct list_head job_list;
+	struct delayed_work *twork = to_delayed_work(ptr);
+	struct b2r2_core *core = container_of(twork, struct b2r2_core,
+			timeout_work);
+
+	INIT_LIST_HEAD(&job_list);
+
+	/* Cancel all jobs if too long time since last irq */
+	spin_lock_irqsave(&core->lock, flags);
+	if (core->n_active_jobs > 0) {
+		unsigned long diff =
+			(long) jiffies - (long) core->jiffies_last_irq;
+		if (diff > JOB_TIMEOUT) {
+			/* Active jobs and more than a second since last irq! */
+			int i;
+
+			b2r2_core_print_stats(core);
+
+			/* Look for timeout:ed jobs and put them in tmp list.
+			 * It's important that the application queues are
+			 * killed in order of decreasing priority */
+			for (i = 0; i < ARRAY_SIZE(core->active_jobs); i++) {
+				struct b2r2_core_job *job =
+					core->active_jobs[i];
+
+				if (job) {
+					stop_hw_timer(core, job);
+					core->active_jobs[i] = NULL;
+					core->n_active_jobs--;
+					list_add_tail(&job->list, &job_list);
+				}
+			}
+
+			/* Print the B2R2 register and reset B2R2 */
+			printk_regs(core);
+			hw_reset(core);
+		}
+	}
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	/* Handle timeout:ed jobs */
+	spin_lock_irqsave(&core->lock, flags);
+	while (!list_empty(&job_list)) {
+		struct b2r2_core_job *job =
+			list_entry(job_list.next,
+				struct b2r2_core_job,
+				list);
+
+		b2r2_log_warn(core->dev, "%s: Job timeout\n", __func__);
+
+		list_del_init(&job->list);
+
+		/* Job is cancelled */
+		job->job_state = B2R2_CORE_JOB_CANCELED;
+
+		/* Handle done */
+		wake_up_interruptible(&job->event);
+
+		/* Job callbacks handled via work queue */
+		queue_work(core->work_queue, &job->work);
+	}
+
+	/* Requeue delayed work */
+	if (core->n_active_jobs)
+		queue_delayed_work(
+			core->work_queue,
+			&core->timeout_work, JOB_TIMEOUT);
+
+	spin_unlock_irqrestore(&core->lock, flags);
+}
+#endif
+
+/**
+ * reset_hw_timer() - Resets a job's hardware timer. Must be called before
+ *                    the timer is used.
+ *
+ * @job: Pointer to job struct
+ *
+ * core->lock _must_ be held when calling this function
+ */
+static void reset_hw_timer(struct b2r2_core_job *job)
+{
+	job->nsec_active_in_hw = 0;
+}
+
+/**
+ * start_hw_timer() - Times how long a job spends in hardware (active).
+ *                    Should be called immediatly before starting the
+ *                    hardware.
+ *
+ * @job: Pointer to job struct
+ *
+ * core->lock _must_ be held when calling this function
+ */
+static void start_hw_timer(struct b2r2_core_job *job)
+{
+	job->hw_start_time = b2r2_get_curr_nsec();
+}
+
+/**
+ * stop_hw_timer() - Times how long a job spends in hardware (active).
+ *                   Should be called immediatly after the hardware has
+ *                   finished.
+ *
+ * @core: The b2r2 core entity
+ * @job: Pointer to job struct
+ *
+ * core->lock _must_ be held when calling this function
+ */
+static void stop_hw_timer(struct b2r2_core *core, struct b2r2_core_job *job)
+{
+	/* Assumes only app queues are used, which is the case right now. */
+	/* Not 100% accurate. When a higher prio job interrupts a lower prio job it does
+	so after the current node of the low prio job has finished. Currently we can not
+	sense when the actual switch takes place so the time reported for a job that
+	interrupts a lower prio job will on average contain the time it takes to process
+	half a node in the lower prio job in addition to the time it takes to process the
+	job's own nodes. This could possibly be solved by adding node notifications but
+	that would involve a significant amount of work and consume system resources due
+	to the extra interrupts. */
+	/* If a job takes more than ~2s (absolute time, including idleing in the hardware)
+	the state of the hardware timer will be corrupted and it will not report valid
+	values until b2r2 becomes idle (no active jobs on any queues). The maximum length
+	can possibly be increased by using 64 bit integers. */
+
+	int i;
+
+	u32 stop_time_raw = b2r2_get_curr_nsec();
+	/* We'll add an offset to all positions in time to make the current time equal to
+	0xFFFFFFFF. This way we can compare positions in time to each other without having
+	to wory about wrapping (so long as all positions in time are in the past). */
+	u32 stop_time = 0xFFFFFFFF;
+	u32 time_pos_offset = 0xFFFFFFFF - stop_time_raw;
+	u32 nsec_in_hw = stop_time - (job->hw_start_time + time_pos_offset);
+	job->nsec_active_in_hw += (s32)nsec_in_hw;
+
+	/* Check if we have delayed the start of higher prio jobs. Can happen as queue
+	switching only can be done between nodes. */
+	for (i = (int)job->queue - 1; i >= (int)B2R2_CORE_QUEUE_AQ1; i--) {
+		struct b2r2_core_job *queue_active_job = core->active_jobs[i];
+		if (NULL == queue_active_job)
+			continue;
+
+		queue_active_job->hw_start_time = stop_time_raw;
+	}
+
+	/* Check if the job has stolen time from lower prio jobs */
+	for (i = (int)job->queue + 1; i < B2R2_NUM_APPLICATIONS_QUEUES; i++) {
+		struct b2r2_core_job *queue_active_job = core->active_jobs[i];
+		u32 queue_active_job_hw_start_time;
+
+		if (NULL == queue_active_job)
+			continue;
+
+		queue_active_job_hw_start_time =
+			queue_active_job->hw_start_time +
+			time_pos_offset;
+
+		if (queue_active_job_hw_start_time < stop_time) {
+			u32 queue_active_job_nsec_in_hw = stop_time -
+				queue_active_job_hw_start_time;
+			u32 num_stolen_nsec = min(queue_active_job_nsec_in_hw,
+				nsec_in_hw);
+
+			queue_active_job->nsec_active_in_hw -= (s32)num_stolen_nsec;
+
+			nsec_in_hw -= num_stolen_nsec;
+			stop_time -= num_stolen_nsec;
+		}
+
+		if (0 == nsec_in_hw)
+			break;
+	}
+}
+
+/**
+ * init_job() - Initializes a job structure from filled in client data.
+ *              Reference count will be set to 1
+ *
+ * @job: Job to initialize
+ */
+static void init_job(struct b2r2_core_job *job)
+{
+
+	job->start_sentinel = START_SENTINEL;
+	job->end_sentinel = END_SENTINEL;
+
+	/* Job is idle, never queued */
+	job->job_state = B2R2_CORE_JOB_IDLE;
+
+	/* Initialize internal data */
+	INIT_LIST_HEAD(&job->list);
+	init_waitqueue_head(&job->event);
+	INIT_WORK(&job->work, job_work_function);
+
+	/* Map given prio to B2R2 queues */
+	if (job->prio < B2R2_CORE_LOWEST_PRIO)
+		job->prio = B2R2_CORE_LOWEST_PRIO;
+	else if (job->prio > B2R2_CORE_HIGHEST_PRIO)
+		job->prio = B2R2_CORE_HIGHEST_PRIO;
+
+	if (job->prio > 10) {
+		job->queue = B2R2_CORE_QUEUE_AQ1;
+		job->interrupt_context =
+			(B2R2BLT_ITSAQ1_LNA_Reached);
+		job->control = (B2R2_AQ_Enab | B2R2_AQ_PRIOR_3);
+	} else if (job->prio > 0) {
+		job->queue = B2R2_CORE_QUEUE_AQ2;
+		job->interrupt_context =
+			(B2R2BLT_ITSAQ2_LNA_Reached);
+		job->control = (B2R2_AQ_Enab | B2R2_AQ_PRIOR_2);
+	} else if (job->prio > -10) {
+		job->queue = B2R2_CORE_QUEUE_AQ3;
+		job->interrupt_context =
+			(B2R2BLT_ITSAQ3_LNA_Reached);
+		job->control = (B2R2_AQ_Enab | B2R2_AQ_PRIOR_1);
+	} else {
+		job->queue = B2R2_CORE_QUEUE_AQ4;
+		job->interrupt_context =
+			(B2R2BLT_ITSAQ4_LNA_Reached);
+		job->control = (B2R2_AQ_Enab | B2R2_AQ_PRIOR_0);
+	}
+}
+
+/**
+ * clear_interrupts() - Disables all interrupts
+ *
+ * core->lock _must_ be held
+ */
+static void  clear_interrupts(struct b2r2_core *core)
+{
+	writel(0x0, &core->hw->BLT_ITM0);
+	writel(0x0, &core->hw->BLT_ITM1);
+	writel(0x0, &core->hw->BLT_ITM2);
+	writel(0x0, &core->hw->BLT_ITM3);
+}
+
+/**
+ * insert_into_prio_list() - Inserts the job into the sorted list of jobs.
+ *                           The list is sorted by priority.
+ *
+ * @core: The b2r2 core entity
+ * @job: Job to insert
+ *
+ * core->lock _must_ be held
+ */
+static void insert_into_prio_list(struct b2r2_core *core,
+		struct b2r2_core_job *job)
+{
+	/* Ref count is increased when job put in list,
+	   should be released when job is removed from list */
+	internal_job_addref(core, job, __func__);
+
+	core->stat_n_jobs_in_prio_list++;
+
+	/* Sort in the job */
+	if (list_empty(&core->prio_queue))
+		list_add_tail(&job->list, &core->prio_queue);
+	else {
+		struct b2r2_core_job *first_job = list_entry(
+			core->prio_queue.next,
+				   struct b2r2_core_job, list);
+		struct b2r2_core_job *last_job = list_entry(
+			core->prio_queue.prev,
+				   struct b2r2_core_job, list);
+
+		if (job->prio > first_job->prio)
+			list_add(&job->list, &core->prio_queue);
+		else if (job->prio <= last_job->prio)
+			list_add_tail(&job->list, &core->prio_queue);
+		else {
+			/* We need to find where to put it */
+			struct list_head *ptr;
+
+			list_for_each(ptr, &core->prio_queue) {
+				struct b2r2_core_job *list_job =
+					list_entry(ptr, struct b2r2_core_job,
+						list);
+				if (job->prio > list_job->prio) {
+					list_add_tail(&job->list,
+						&list_job->list);
+					break;
+				}
+			}
+		}
+	}
+	/* The job is now queued */
+	job->job_state = B2R2_CORE_JOB_QUEUED;
+}
+
+/**
+ * check_prio_list() - Checks if the first job(s) in the prio list can
+ *                     be dispatched to B2R2
+ *
+ * @core: The b2r2 core entity
+ * @atomic: true if in atomic context (i.e. interrupt context)
+ *
+ * core->lock _must_ be held
+ */
+static void check_prio_list(struct b2r2_core *core, bool atomic)
+{
+	bool dispatched_job;
+	int n_dispatched = 0;
+	struct b2r2_core_job *job;
+
+	do {
+		dispatched_job = false;
+
+		/* Do we have anything in our prio list? */
+		if (list_empty(&core->prio_queue))
+			break;
+
+		/* The first job waiting */
+		job = list_first_entry(&core->prio_queue,
+				 struct b2r2_core_job, list);
+
+		/* Is the B2R2 queue available? */
+		if (core->active_jobs[job->queue] != NULL)
+			break;
+
+		/* Can we acquire resources? */
+		if (!job->acquire_resources ||
+			job->acquire_resources(job, atomic) == 0) {
+			/* Ok to dispatch job */
+
+			/* Remove from list */
+			list_del_init(&job->list);
+
+			/* The job is now active */
+			core->active_jobs[job->queue] = job;
+			core->n_active_jobs++;
+			job->jiffies = jiffies;
+			core->jiffies_last_active = jiffies;
+
+			/* Kick off B2R2 */
+			trigger_job(core, job);
+			dispatched_job = true;
+			n_dispatched++;
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+			/* Check in one half second if it hangs */
+			queue_delayed_work(core->work_queue,
+				&core->timeout_work, JOB_TIMEOUT);
+#endif
+		} else {
+			/* No resources */
+			if (!atomic && core->n_active_jobs == 0) {
+				b2r2_log_warn(core->dev,
+					"%s: No resource", __func__);
+				cancel_job(core, job);
+			}
+		}
+	} while (dispatched_job);
+
+	core->stat_n_jobs_in_prio_list -= n_dispatched;
+}
+
+/**
+ * find_job_in_list() - Finds job with job_id in list
+ *
+ * @jobid: Job id to find
+ * @list: List to find job id in
+ *
+ * Reference count will be incremented for found job.
+ *
+ * core->lock _must_ be held
+ */
+static struct b2r2_core_job *find_job_in_list(int job_id,
+					      struct list_head *list)
+{
+	struct list_head *ptr;
+
+	list_for_each(ptr, list) {
+		struct b2r2_core_job *job = list_entry(
+				ptr, struct b2r2_core_job, list);
+		if (job->job_id == job_id) {
+			struct b2r2_core *core = (struct b2r2_core *) job->data;
+			/* Increase reference count, should be released by
+			   the caller of b2r2_core_job_find */
+			internal_job_addref(core, job, __func__);
+			return job;
+		}
+	}
+	return NULL;
+}
+
+/**
+ * find_job_in_active_jobs() - Finds job in active job queues
+ *
+ * @core: The b2r2 core entity
+ * @job_id: Job id to find
+ *
+ * Reference count will be incremented for found job.
+ *
+ * core->lock _must_ be held
+ */
+static struct b2r2_core_job *find_job_in_active_jobs(struct b2r2_core *core,
+		int job_id)
+{
+	int i;
+	struct b2r2_core_job *found_job = NULL;
+
+	if (core->n_active_jobs) {
+		for (i = 0; i < ARRAY_SIZE(core->active_jobs); i++) {
+			struct b2r2_core_job *job = core->active_jobs[i];
+
+			if (job && job->job_id == job_id) {
+				internal_job_addref(core, job, __func__);
+				found_job = job;
+				break;
+			}
+		}
+	}
+	return found_job;
+}
+
+/**
+ * find_tag_in_list() - Finds first job with tag in list
+ *
+ * @tag: Tag to find
+ * @list: List to find job id in
+ *
+ * Reference count will be incremented for found job.
+ *
+ * core->lock must be held
+ */
+static struct b2r2_core_job *find_tag_in_list(struct b2r2_core *core,
+		int tag, struct list_head *list)
+{
+	struct list_head *ptr;
+
+	list_for_each(ptr, list) {
+		struct b2r2_core_job *job =
+				list_entry(ptr, struct b2r2_core_job, list);
+		if (job->tag == tag) {
+			/* Increase reference count, should be released by
+			   the caller of b2r2_core_job_find */
+			internal_job_addref(core, job, __func__);
+			return job;
+		}
+	}
+	return NULL;
+}
+
+/**
+ * find_tag_in_active_jobs() - Finds job with tag in active job queues
+ *
+ * @tag: Tag to find
+ *
+ * Reference count will be incremented for found job.
+ *
+ * core->lock must be held
+ */
+static struct b2r2_core_job *find_tag_in_active_jobs(struct b2r2_core *core,
+		int tag)
+{
+	int i;
+	struct b2r2_core_job *found_job = NULL;
+
+	if (core->n_active_jobs) {
+		for (i = 0; i < ARRAY_SIZE(core->active_jobs); i++) {
+			struct b2r2_core_job *job = core->active_jobs[i];
+
+			if (job && job->tag == tag) {
+				internal_job_addref(core, job, __func__);
+				found_job = job;
+				break;
+			}
+		}
+	}
+	return found_job;
+}
+
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+/**
+ * hw_reset() - Resets B2R2 hardware
+ *
+ * core->lock must be held
+ */
+static int hw_reset(struct b2r2_core *core)
+{
+	u32 uTimeOut = B2R2_RESET_TIMEOUT_VALUE;
+
+	/* Tell B2R2 to reset */
+	writel(readl(&core->hw->BLT_CTL) | B2R2BLT_CTLGLOBAL_soft_reset,
+		&core->hw->BLT_CTL);
+	writel(0x00000000, &core->hw->BLT_CTL);
+
+	b2r2_log_info(core->dev, "wait for B2R2 to be idle..\n");
+
+	/** Wait for B2R2 to be idle (on a timeout rather than while loop) */
+	while ((uTimeOut > 0) &&
+	       ((readl(&core->hw->BLT_STA1) &
+		 B2R2BLT_STA1BDISP_IDLE) == 0x0))
+		uTimeOut--;
+
+	if (uTimeOut == 0) {
+		b2r2_log_warn(core->dev,
+			 "error-> after software reset B2R2 is not idle\n");
+		return -EAGAIN;
+	}
+
+	return 0;
+
+}
+#endif
+
+/**
+ * trigger_job() - Put job in B2R2 HW queue
+ *
+ * @job: Job to trigger
+ *
+ * core->lock must be held
+ */
+static void trigger_job(struct b2r2_core *core, struct b2r2_core_job *job)
+{
+	/* Debug prints */
+	b2r2_log_info(core->dev, "queue 0x%x\n", job->queue);
+	b2r2_log_info(core->dev, "BLT TRIG_IP 0x%x (first node)\n",
+		job->first_node_address);
+	b2r2_log_info(core->dev, "BLT LNA_CTL 0x%x (last node)\n",
+		job->last_node_address);
+	b2r2_log_info(core->dev, "BLT TRIG_CTL 0x%x\n", job->control);
+	b2r2_log_info(core->dev, "BLT PACE_CTL 0x%x\n", job->pace_control);
+
+	reset_hw_timer(job);
+	job->job_state = B2R2_CORE_JOB_RUNNING;
+
+	/* Enable interrupt */
+	writel(readl(&core->hw->BLT_ITM0) | job->interrupt_context,
+			&core->hw->BLT_ITM0);
+
+	writel(min_t(u8, max_t(u8, core->op_size, B2R2_PLUG_OPCODE_SIZE_8),
+			B2R2_PLUG_OPCODE_SIZE_64), &core->hw->PLUGS1_OP2);
+	writel(min_t(u8, core->ch_size, B2R2_PLUG_CHUNK_SIZE_128),
+			&core->hw->PLUGS1_CHZ);
+	writel(min_t(u8, core->mg_size, B2R2_PLUG_MESSAGE_SIZE_128) |
+			(core->min_req_time << 16), &core->hw->PLUGS1_MSZ);
+	writel(min_t(u8, core->pg_size, B2R2_PLUG_PAGE_SIZE_256),
+			&core->hw->PLUGS1_PGZ);
+
+	writel(min_t(u8, max_t(u8, core->op_size, B2R2_PLUG_OPCODE_SIZE_8),
+			B2R2_PLUG_OPCODE_SIZE_64), &core->hw->PLUGS2_OP2);
+	writel(min_t(u8, core->ch_size, B2R2_PLUG_CHUNK_SIZE_128),
+			&core->hw->PLUGS2_CHZ);
+	writel(min_t(u8, core->mg_size, B2R2_PLUG_MESSAGE_SIZE_128) |
+			(core->min_req_time << 16), &core->hw->PLUGS2_MSZ);
+	writel(min_t(u8, core->pg_size, B2R2_PLUG_PAGE_SIZE_256),
+			&core->hw->PLUGS2_PGZ);
+
+	writel(min_t(u8, max_t(u8, core->op_size, B2R2_PLUG_OPCODE_SIZE_8),
+			B2R2_PLUG_OPCODE_SIZE_64), &core->hw->PLUGS3_OP2);
+	writel(min_t(u8, core->ch_size, B2R2_PLUG_CHUNK_SIZE_128),
+			&core->hw->PLUGS3_CHZ);
+	writel(min_t(u8, core->mg_size, B2R2_PLUG_MESSAGE_SIZE_128) |
+			(core->min_req_time << 16), &core->hw->PLUGS3_MSZ);
+	writel(min_t(u8, core->pg_size, B2R2_PLUG_PAGE_SIZE_256),
+			&core->hw->PLUGS3_PGZ);
+
+	writel(min_t(u8, max_t(u8, core->op_size, B2R2_PLUG_OPCODE_SIZE_8),
+			B2R2_PLUG_OPCODE_SIZE_64), &core->hw->PLUGT_OP2);
+	writel(min_t(u8, core->ch_size, B2R2_PLUG_CHUNK_SIZE_128),
+			&core->hw->PLUGT_CHZ);
+	writel(min_t(u8, core->mg_size, B2R2_PLUG_MESSAGE_SIZE_128) |
+			(core->min_req_time << 16), &core->hw->PLUGT_MSZ);
+	writel(min_t(u8, core->pg_size, B2R2_PLUG_PAGE_SIZE_256),
+			&core->hw->PLUGT_PGZ);
+
+	/* B2R2 kicks off when LNA is written, LNA write must be last! */
+	switch (job->queue) {
+	case B2R2_CORE_QUEUE_CQ1:
+		writel(job->first_node_address, &core->hw->BLT_CQ1_TRIG_IP);
+		writel(job->control, &core->hw->BLT_CQ1_TRIG_CTL);
+		writel(job->pace_control, &core->hw->BLT_CQ1_PACE_CTL);
+		break;
+
+	case B2R2_CORE_QUEUE_CQ2:
+		writel(job->first_node_address, &core->hw->BLT_CQ2_TRIG_IP);
+		writel(job->control, &core->hw->BLT_CQ2_TRIG_CTL);
+		writel(job->pace_control, &core->hw->BLT_CQ2_PACE_CTL);
+		break;
+
+	case B2R2_CORE_QUEUE_AQ1:
+		writel(job->control, &core->hw->BLT_AQ1_CTL);
+		writel(job->first_node_address, &core->hw->BLT_AQ1_IP);
+		wmb();
+		start_hw_timer(job);
+		writel(job->last_node_address, &core->hw->BLT_AQ1_LNA);
+		break;
+
+	case B2R2_CORE_QUEUE_AQ2:
+		writel(job->control, &core->hw->BLT_AQ2_CTL);
+		writel(job->first_node_address, &core->hw->BLT_AQ2_IP);
+		wmb();
+		start_hw_timer(job);
+		writel(job->last_node_address, &core->hw->BLT_AQ2_LNA);
+		break;
+
+	case B2R2_CORE_QUEUE_AQ3:
+		writel(job->control, &core->hw->BLT_AQ3_CTL);
+		writel(job->first_node_address, &core->hw->BLT_AQ3_IP);
+		wmb();
+		start_hw_timer(job);
+		writel(job->last_node_address, &core->hw->BLT_AQ3_LNA);
+		break;
+
+	case B2R2_CORE_QUEUE_AQ4:
+		writel(job->control, &core->hw->BLT_AQ4_CTL);
+		writel(job->first_node_address, &core->hw->BLT_AQ4_IP);
+		wmb();
+		start_hw_timer(job);
+		writel(job->last_node_address, &core->hw->BLT_AQ4_LNA);
+		break;
+
+		/** Handle the default case */
+	default:
+		break;
+
+	} /* end switch */
+
+}
+
+/**
+ * handle_queue_event() - Handles interrupt event for specified B2R2 queue
+ *
+ * @queue: Queue to handle event for
+ *
+ * core->lock must be held
+ */
+static void handle_queue_event(struct b2r2_core *core,
+		enum b2r2_core_queue queue)
+{
+	struct b2r2_core_job *job;
+
+	job = core->active_jobs[queue];
+	if (job) {
+		if (job->job_state != B2R2_CORE_JOB_RUNNING)
+			/* Should be running
+			   Severe error. TBD */
+			b2r2_log_warn(core->dev,
+				 "%s: Job is not running", __func__);
+
+		stop_hw_timer(core, job);
+
+		/* Remove from queue */
+		BUG_ON(core->n_active_jobs == 0);
+		core->active_jobs[queue] = NULL;
+		core->n_active_jobs--;
+	}
+
+	if (!job) {
+		/* No job, error?  */
+		b2r2_log_warn(core->dev, "%s: No job", __func__);
+		return;
+	}
+
+
+	/* Atomic context release resources, release resources will
+	   be called again later from process context (work queue) */
+	if (job->release_resources)
+		job->release_resources(job, true);
+
+	/* Job is done */
+	job->job_state = B2R2_CORE_JOB_DONE;
+
+	/* Handle done */
+	wake_up_interruptible(&job->event);
+
+	/* Dispatch to work queue to handle callbacks */
+	queue_work(core->work_queue, &job->work);
+}
+
+/**
+ * process_events() - Handles interrupt events
+ *
+ * @status: Contents of the B2R2 ITS register
+ */
+static void process_events(struct b2r2_core *core, u32 status)
+{
+	u32 mask = 0xF;
+	u32 disable_itm_mask = 0;
+
+	b2r2_log_info(core->dev, "Enters process_events\n");
+	b2r2_log_info(core->dev, "status 0x%x\n", status);
+
+	/* Composition queue 1 */
+	if (status & mask) {
+		handle_queue_event(core, B2R2_CORE_QUEUE_CQ1);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 4;
+
+	/* Composition queue 2 */
+	if (status & mask) {
+		handle_queue_event(core, B2R2_CORE_QUEUE_CQ2);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 8;
+
+	/* Application queue 1 */
+	if (status & mask) {
+		handle_queue_event(core, B2R2_CORE_QUEUE_AQ1);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 4;
+
+	/* Application queue 2 */
+	if (status & mask) {
+		handle_queue_event(core, B2R2_CORE_QUEUE_AQ2);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 4;
+
+	/* Application queue 3 */
+	if (status & mask) {
+		handle_queue_event(core, B2R2_CORE_QUEUE_AQ3);
+		disable_itm_mask |= mask;
+	}
+	mask <<= 4;
+
+	/* Application queue 4 */
+	if (status & mask) {
+		handle_queue_event(core, B2R2_CORE_QUEUE_AQ4);
+		disable_itm_mask |= mask;
+	}
+
+	/* Clear received interrupt flags */
+	writel(status, &core->hw->BLT_ITS);
+	/* Disable handled interrupts */
+	writel(readl(&core->hw->BLT_ITM0) & ~disable_itm_mask,
+		 &core->hw->BLT_ITM0);
+
+	b2r2_log_info(core->dev, "Returns process_events\n");
+}
+
+/**
+ * b2r2_irq_handler() - B2R2 interrupt handler
+ *
+ * @irq: Interrupt number (not used)
+ * @dev_id: A pointer to the b2r2 core entity
+ */
+static irqreturn_t b2r2_irq_handler(int irq, void *dev_id)
+{
+	unsigned long flags;
+	struct b2r2_core* core = (struct b2r2_core *) dev_id;
+
+	/* Spin lock is need in irq handler (SMP) */
+	spin_lock_irqsave(&core->lock, flags);
+
+	/* Make a quick exit if this device was not interrupting */
+	if (!core->valid ||
+		((readl(&core->hw->BLT_ITS) & B2R2_ITS_MASK) == 0)) {
+		core->stat_n_irq_skipped++;
+		spin_unlock_irqrestore(&core->lock, flags);
+		return IRQ_NONE;
+	}
+
+	/* Remember time for last irq (for timeout mgmt) */
+	core->jiffies_last_irq = jiffies;
+	core->stat_n_irq++;
+
+	/* Handle the interrupt(s) */
+	process_events(core, readl(&core->hw->BLT_ITS));
+
+	/* Check if we can dispatch new jobs */
+	check_prio_list(core, true);
+
+	core->stat_n_irq_exit++;
+
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+
+#ifdef CONFIG_DEBUG_FS
+/**
+ * struct debugfs_reg - Represents one B2R2 register in debugfs
+ *
+ * @name: Register name
+ * @offset: Byte offset in B2R2 for register
+ */
+struct debugfs_reg {
+	const char name[30];
+	u32        offset;
+};
+
+/**
+ * debugfs_regs - Array of B2R2 debugfs registers
+ */
+static const struct debugfs_reg debugfs_regs[] = {
+	{"BLT_SSBA17", offsetof(struct b2r2_memory_map, BLT_SSBA17)},
+	{"BLT_SSBA18", offsetof(struct b2r2_memory_map, BLT_SSBA18)},
+	{"BLT_SSBA19", offsetof(struct b2r2_memory_map, BLT_SSBA19)},
+	{"BLT_SSBA20", offsetof(struct b2r2_memory_map, BLT_SSBA20)},
+	{"BLT_SSBA21", offsetof(struct b2r2_memory_map, BLT_SSBA21)},
+	{"BLT_SSBA22", offsetof(struct b2r2_memory_map, BLT_SSBA22)},
+	{"BLT_SSBA23", offsetof(struct b2r2_memory_map, BLT_SSBA23)},
+	{"BLT_SSBA24", offsetof(struct b2r2_memory_map, BLT_SSBA24)},
+	{"BLT_STBA5", offsetof(struct b2r2_memory_map, BLT_STBA5)},
+	{"BLT_STBA6", offsetof(struct b2r2_memory_map, BLT_STBA6)},
+	{"BLT_STBA7", offsetof(struct b2r2_memory_map, BLT_STBA7)},
+	{"BLT_STBA8", offsetof(struct b2r2_memory_map, BLT_STBA8)},
+	{"BLT_CTL", offsetof(struct b2r2_memory_map, BLT_CTL)},
+	{"BLT_ITS", offsetof(struct b2r2_memory_map, BLT_ITS)},
+	{"BLT_STA1", offsetof(struct b2r2_memory_map, BLT_STA1)},
+	{"BLT_SSBA1", offsetof(struct b2r2_memory_map, BLT_SSBA1)},
+	{"BLT_SSBA2", offsetof(struct b2r2_memory_map, BLT_SSBA2)},
+	{"BLT_SSBA3", offsetof(struct b2r2_memory_map, BLT_SSBA3)},
+	{"BLT_SSBA4", offsetof(struct b2r2_memory_map, BLT_SSBA4)},
+	{"BLT_SSBA5", offsetof(struct b2r2_memory_map, BLT_SSBA5)},
+	{"BLT_SSBA6", offsetof(struct b2r2_memory_map, BLT_SSBA6)},
+	{"BLT_SSBA7", offsetof(struct b2r2_memory_map, BLT_SSBA7)},
+	{"BLT_SSBA8", offsetof(struct b2r2_memory_map, BLT_SSBA8)},
+	{"BLT_STBA1", offsetof(struct b2r2_memory_map, BLT_STBA1)},
+	{"BLT_STBA2", offsetof(struct b2r2_memory_map, BLT_STBA2)},
+	{"BLT_STBA3", offsetof(struct b2r2_memory_map, BLT_STBA3)},
+	{"BLT_STBA4", offsetof(struct b2r2_memory_map, BLT_STBA4)},
+	{"BLT_CQ1_TRIG_IP", offsetof(struct b2r2_memory_map, BLT_CQ1_TRIG_IP)},
+	{"BLT_CQ1_TRIG_CTL", offsetof(struct b2r2_memory_map,
+				      BLT_CQ1_TRIG_CTL)},
+	{"BLT_CQ1_PACE_CTL", offsetof(struct b2r2_memory_map,
+				      BLT_CQ1_PACE_CTL)},
+	{"BLT_CQ1_IP", offsetof(struct b2r2_memory_map, BLT_CQ1_IP)},
+	{"BLT_CQ2_TRIG_IP", offsetof(struct b2r2_memory_map, BLT_CQ2_TRIG_IP)},
+	{"BLT_CQ2_TRIG_CTL", offsetof(struct b2r2_memory_map,
+				      BLT_CQ2_TRIG_CTL)},
+	{"BLT_CQ2_PACE_CTL", offsetof(struct b2r2_memory_map,
+				      BLT_CQ2_PACE_CTL)},
+	{"BLT_CQ2_IP", offsetof(struct b2r2_memory_map, BLT_CQ2_IP)},
+	{"BLT_AQ1_CTL", offsetof(struct b2r2_memory_map, BLT_AQ1_CTL)},
+	{"BLT_AQ1_IP", offsetof(struct b2r2_memory_map, BLT_AQ1_IP)},
+	{"BLT_AQ1_LNA", offsetof(struct b2r2_memory_map, BLT_AQ1_LNA)},
+	{"BLT_AQ1_STA", offsetof(struct b2r2_memory_map, BLT_AQ1_STA)},
+	{"BLT_AQ2_CTL", offsetof(struct b2r2_memory_map, BLT_AQ2_CTL)},
+	{"BLT_AQ2_IP", offsetof(struct b2r2_memory_map, BLT_AQ2_IP)},
+	{"BLT_AQ2_LNA", offsetof(struct b2r2_memory_map, BLT_AQ2_LNA)},
+	{"BLT_AQ2_STA", offsetof(struct b2r2_memory_map, BLT_AQ2_STA)},
+	{"BLT_AQ3_CTL", offsetof(struct b2r2_memory_map, BLT_AQ3_CTL)},
+	{"BLT_AQ3_IP", offsetof(struct b2r2_memory_map, BLT_AQ3_IP)},
+	{"BLT_AQ3_LNA", offsetof(struct b2r2_memory_map, BLT_AQ3_LNA)},
+	{"BLT_AQ3_STA", offsetof(struct b2r2_memory_map, BLT_AQ3_STA)},
+	{"BLT_AQ4_CTL", offsetof(struct b2r2_memory_map, BLT_AQ4_CTL)},
+	{"BLT_AQ4_IP", offsetof(struct b2r2_memory_map, BLT_AQ4_IP)},
+	{"BLT_AQ4_LNA", offsetof(struct b2r2_memory_map, BLT_AQ4_LNA)},
+	{"BLT_AQ4_STA", offsetof(struct b2r2_memory_map, BLT_AQ4_STA)},
+	{"BLT_SSBA9", offsetof(struct b2r2_memory_map, BLT_SSBA9)},
+	{"BLT_SSBA10", offsetof(struct b2r2_memory_map, BLT_SSBA10)},
+	{"BLT_SSBA11", offsetof(struct b2r2_memory_map, BLT_SSBA11)},
+	{"BLT_SSBA12", offsetof(struct b2r2_memory_map, BLT_SSBA12)},
+	{"BLT_SSBA13", offsetof(struct b2r2_memory_map, BLT_SSBA13)},
+	{"BLT_SSBA14", offsetof(struct b2r2_memory_map, BLT_SSBA14)},
+	{"BLT_SSBA15", offsetof(struct b2r2_memory_map, BLT_SSBA15)},
+	{"BLT_SSBA16", offsetof(struct b2r2_memory_map, BLT_SSBA16)},
+	{"BLT_SGA1", offsetof(struct b2r2_memory_map, BLT_SGA1)},
+	{"BLT_SGA2", offsetof(struct b2r2_memory_map, BLT_SGA2)},
+	{"BLT_ITM0", offsetof(struct b2r2_memory_map, BLT_ITM0)},
+	{"BLT_ITM1", offsetof(struct b2r2_memory_map, BLT_ITM1)},
+	{"BLT_ITM2", offsetof(struct b2r2_memory_map, BLT_ITM2)},
+	{"BLT_ITM3", offsetof(struct b2r2_memory_map, BLT_ITM3)},
+	{"BLT_DFV2", offsetof(struct b2r2_memory_map, BLT_DFV2)},
+	{"BLT_DFV1", offsetof(struct b2r2_memory_map, BLT_DFV1)},
+	{"BLT_PRI", offsetof(struct b2r2_memory_map, BLT_PRI)},
+	{"PLUGS1_OP2", offsetof(struct b2r2_memory_map, PLUGS1_OP2)},
+	{"PLUGS1_CHZ", offsetof(struct b2r2_memory_map, PLUGS1_CHZ)},
+	{"PLUGS1_MSZ", offsetof(struct b2r2_memory_map, PLUGS1_MSZ)},
+	{"PLUGS1_PGZ", offsetof(struct b2r2_memory_map, PLUGS1_PGZ)},
+	{"PLUGS2_OP2", offsetof(struct b2r2_memory_map, PLUGS2_OP2)},
+	{"PLUGS2_CHZ", offsetof(struct b2r2_memory_map, PLUGS2_CHZ)},
+	{"PLUGS2_MSZ", offsetof(struct b2r2_memory_map, PLUGS2_MSZ)},
+	{"PLUGS2_PGZ", offsetof(struct b2r2_memory_map, PLUGS2_PGZ)},
+	{"PLUGS3_OP2", offsetof(struct b2r2_memory_map, PLUGS3_OP2)},
+	{"PLUGS3_CHZ", offsetof(struct b2r2_memory_map, PLUGS3_CHZ)},
+	{"PLUGS3_MSZ", offsetof(struct b2r2_memory_map, PLUGS3_MSZ)},
+	{"PLUGS3_PGZ", offsetof(struct b2r2_memory_map, PLUGS3_PGZ)},
+	{"PLUGT_OP2", offsetof(struct b2r2_memory_map, PLUGT_OP2)},
+	{"PLUGT_CHZ", offsetof(struct b2r2_memory_map, PLUGT_CHZ)},
+	{"PLUGT_MSZ", offsetof(struct b2r2_memory_map, PLUGT_MSZ)},
+	{"PLUGT_PGZ", offsetof(struct b2r2_memory_map, PLUGT_PGZ)},
+	{"BLT_NIP", offsetof(struct b2r2_memory_map, BLT_NIP)},
+	{"BLT_CIC", offsetof(struct b2r2_memory_map, BLT_CIC)},
+	{"BLT_INS", offsetof(struct b2r2_memory_map, BLT_INS)},
+	{"BLT_ACK", offsetof(struct b2r2_memory_map, BLT_ACK)},
+	{"BLT_TBA", offsetof(struct b2r2_memory_map, BLT_TBA)},
+	{"BLT_TTY", offsetof(struct b2r2_memory_map, BLT_TTY)},
+	{"BLT_TXY", offsetof(struct b2r2_memory_map, BLT_TXY)},
+	{"BLT_TSZ", offsetof(struct b2r2_memory_map, BLT_TSZ)},
+	{"BLT_S1CF", offsetof(struct b2r2_memory_map, BLT_S1CF)},
+	{"BLT_S2CF", offsetof(struct b2r2_memory_map, BLT_S2CF)},
+	{"BLT_S1BA", offsetof(struct b2r2_memory_map, BLT_S1BA)},
+	{"BLT_S1TY", offsetof(struct b2r2_memory_map, BLT_S1TY)},
+	{"BLT_S1XY", offsetof(struct b2r2_memory_map, BLT_S1XY)},
+	{"BLT_S2BA", offsetof(struct b2r2_memory_map, BLT_S2BA)},
+	{"BLT_S2TY", offsetof(struct b2r2_memory_map, BLT_S2TY)},
+	{"BLT_S2XY", offsetof(struct b2r2_memory_map, BLT_S2XY)},
+	{"BLT_S2SZ", offsetof(struct b2r2_memory_map, BLT_S2SZ)},
+	{"BLT_S3BA", offsetof(struct b2r2_memory_map, BLT_S3BA)},
+	{"BLT_S3TY", offsetof(struct b2r2_memory_map, BLT_S3TY)},
+	{"BLT_S3XY", offsetof(struct b2r2_memory_map, BLT_S3XY)},
+	{"BLT_S3SZ", offsetof(struct b2r2_memory_map, BLT_S3SZ)},
+	{"BLT_CWO", offsetof(struct b2r2_memory_map, BLT_CWO)},
+	{"BLT_CWS", offsetof(struct b2r2_memory_map, BLT_CWS)},
+	{"BLT_CCO", offsetof(struct b2r2_memory_map, BLT_CCO)},
+	{"BLT_CML", offsetof(struct b2r2_memory_map, BLT_CML)},
+	{"BLT_FCTL", offsetof(struct b2r2_memory_map, BLT_FCTL)},
+	{"BLT_PMK", offsetof(struct b2r2_memory_map, BLT_PMK)},
+	{"BLT_RSF", offsetof(struct b2r2_memory_map, BLT_RSF)},
+	{"BLT_RZI", offsetof(struct b2r2_memory_map, BLT_RZI)},
+	{"BLT_HFP", offsetof(struct b2r2_memory_map, BLT_HFP)},
+	{"BLT_VFP", offsetof(struct b2r2_memory_map, BLT_VFP)},
+	{"BLT_Y_RSF", offsetof(struct b2r2_memory_map, BLT_Y_RSF)},
+	{"BLT_Y_RZI", offsetof(struct b2r2_memory_map, BLT_Y_RZI)},
+	{"BLT_Y_HFP", offsetof(struct b2r2_memory_map, BLT_Y_HFP)},
+	{"BLT_Y_VFP", offsetof(struct b2r2_memory_map, BLT_Y_VFP)},
+	{"BLT_KEY1", offsetof(struct b2r2_memory_map, BLT_KEY1)},
+	{"BLT_KEY2", offsetof(struct b2r2_memory_map, BLT_KEY2)},
+	{"BLT_SAR", offsetof(struct b2r2_memory_map, BLT_SAR)},
+	{"BLT_USR", offsetof(struct b2r2_memory_map, BLT_USR)},
+	{"BLT_IVMX0", offsetof(struct b2r2_memory_map, BLT_IVMX0)},
+	{"BLT_IVMX1", offsetof(struct b2r2_memory_map, BLT_IVMX1)},
+	{"BLT_IVMX2", offsetof(struct b2r2_memory_map, BLT_IVMX2)},
+	{"BLT_IVMX3", offsetof(struct b2r2_memory_map, BLT_IVMX3)},
+	{"BLT_OVMX0", offsetof(struct b2r2_memory_map, BLT_OVMX0)},
+	{"BLT_OVMX1", offsetof(struct b2r2_memory_map, BLT_OVMX1)},
+	{"BLT_OVMX2", offsetof(struct b2r2_memory_map, BLT_OVMX2)},
+	{"BLT_OVMX3", offsetof(struct b2r2_memory_map, BLT_OVMX3)},
+	{"BLT_VC1R", offsetof(struct b2r2_memory_map, BLT_VC1R)},
+	{"BLT_Y_HFC0", offsetof(struct b2r2_memory_map, BLT_Y_HFC0)},
+	{"BLT_Y_HFC1", offsetof(struct b2r2_memory_map, BLT_Y_HFC1)},
+	{"BLT_Y_HFC2", offsetof(struct b2r2_memory_map, BLT_Y_HFC2)},
+	{"BLT_Y_HFC3", offsetof(struct b2r2_memory_map, BLT_Y_HFC3)},
+	{"BLT_Y_HFC4", offsetof(struct b2r2_memory_map, BLT_Y_HFC4)},
+	{"BLT_Y_HFC5", offsetof(struct b2r2_memory_map, BLT_Y_HFC5)},
+	{"BLT_Y_HFC6", offsetof(struct b2r2_memory_map, BLT_Y_HFC6)},
+	{"BLT_Y_HFC7", offsetof(struct b2r2_memory_map, BLT_Y_HFC7)},
+	{"BLT_Y_HFC8", offsetof(struct b2r2_memory_map, BLT_Y_HFC8)},
+	{"BLT_Y_HFC9", offsetof(struct b2r2_memory_map, BLT_Y_HFC9)},
+	{"BLT_Y_HFC10", offsetof(struct b2r2_memory_map, BLT_Y_HFC10)},
+	{"BLT_Y_HFC11", offsetof(struct b2r2_memory_map, BLT_Y_HFC11)},
+	{"BLT_Y_HFC12", offsetof(struct b2r2_memory_map, BLT_Y_HFC12)},
+	{"BLT_Y_HFC13", offsetof(struct b2r2_memory_map, BLT_Y_HFC13)},
+	{"BLT_Y_HFC14", offsetof(struct b2r2_memory_map, BLT_Y_HFC14)},
+	{"BLT_Y_HFC15", offsetof(struct b2r2_memory_map, BLT_Y_HFC15)},
+	{"BLT_Y_VFC0", offsetof(struct b2r2_memory_map, BLT_Y_VFC0)},
+	{"BLT_Y_VFC1", offsetof(struct b2r2_memory_map, BLT_Y_VFC1)},
+	{"BLT_Y_VFC2", offsetof(struct b2r2_memory_map, BLT_Y_VFC2)},
+	{"BLT_Y_VFC3", offsetof(struct b2r2_memory_map, BLT_Y_VFC3)},
+	{"BLT_Y_VFC4", offsetof(struct b2r2_memory_map, BLT_Y_VFC4)},
+	{"BLT_Y_VFC5", offsetof(struct b2r2_memory_map, BLT_Y_VFC5)},
+	{"BLT_Y_VFC6", offsetof(struct b2r2_memory_map, BLT_Y_VFC6)},
+	{"BLT_Y_VFC7", offsetof(struct b2r2_memory_map, BLT_Y_VFC7)},
+	{"BLT_Y_VFC8", offsetof(struct b2r2_memory_map, BLT_Y_VFC8)},
+	{"BLT_Y_VFC9", offsetof(struct b2r2_memory_map, BLT_Y_VFC9)},
+	{"BLT_HFC0", offsetof(struct b2r2_memory_map, BLT_HFC0)},
+	{"BLT_HFC1", offsetof(struct b2r2_memory_map, BLT_HFC1)},
+	{"BLT_HFC2", offsetof(struct b2r2_memory_map, BLT_HFC2)},
+	{"BLT_HFC3", offsetof(struct b2r2_memory_map, BLT_HFC3)},
+	{"BLT_HFC4", offsetof(struct b2r2_memory_map, BLT_HFC4)},
+	{"BLT_HFC5", offsetof(struct b2r2_memory_map, BLT_HFC5)},
+	{"BLT_HFC6", offsetof(struct b2r2_memory_map, BLT_HFC6)},
+	{"BLT_HFC7", offsetof(struct b2r2_memory_map, BLT_HFC7)},
+	{"BLT_HFC8", offsetof(struct b2r2_memory_map, BLT_HFC8)},
+	{"BLT_HFC9", offsetof(struct b2r2_memory_map, BLT_HFC9)},
+	{"BLT_HFC10", offsetof(struct b2r2_memory_map, BLT_HFC10)},
+	{"BLT_HFC11", offsetof(struct b2r2_memory_map, BLT_HFC11)},
+	{"BLT_HFC12", offsetof(struct b2r2_memory_map, BLT_HFC12)},
+	{"BLT_HFC13", offsetof(struct b2r2_memory_map, BLT_HFC13)},
+	{"BLT_HFC14", offsetof(struct b2r2_memory_map, BLT_HFC14)},
+	{"BLT_HFC15", offsetof(struct b2r2_memory_map, BLT_HFC15)},
+	{"BLT_VFC0", offsetof(struct b2r2_memory_map, BLT_VFC0)},
+	{"BLT_VFC1", offsetof(struct b2r2_memory_map, BLT_VFC1)},
+	{"BLT_VFC2", offsetof(struct b2r2_memory_map, BLT_VFC2)},
+	{"BLT_VFC3", offsetof(struct b2r2_memory_map, BLT_VFC3)},
+	{"BLT_VFC4", offsetof(struct b2r2_memory_map, BLT_VFC4)},
+	{"BLT_VFC5", offsetof(struct b2r2_memory_map, BLT_VFC5)},
+	{"BLT_VFC6", offsetof(struct b2r2_memory_map, BLT_VFC6)},
+	{"BLT_VFC7", offsetof(struct b2r2_memory_map, BLT_VFC7)},
+	{"BLT_VFC8", offsetof(struct b2r2_memory_map, BLT_VFC8)},
+	{"BLT_VFC9", offsetof(struct b2r2_memory_map, BLT_VFC9)},
+};
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+/**
+ * printk_regs() - Print B2R2 registers to printk
+ */
+static void printk_regs(struct b2r2_core *core)
+{
+#ifdef CONFIG_B2R2_DEBUG
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(debugfs_regs); i++) {
+		unsigned long value = readl(
+			(unsigned long *) (((u8 *) core->hw) +
+				debugfs_regs[i].offset));
+		b2r2_log_regdump(core->dev, "%s: %08lX\n",
+			debugfs_regs[i].name,
+			value);
+	}
+#endif
+}
+#endif
+
+/**
+ * debugfs_b2r2_reg_read() - Implements debugfs read for B2R2 register
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_reg_read(struct file *filp, char __user *buf,
+				 size_t count, loff_t *f_pos)
+{
+	size_t dev_size;
+	int ret = 0;
+	unsigned long value;
+	char *tmpbuf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+	if (tmpbuf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Read from B2R2 */
+	value = readl((unsigned long *)
+		filp->f_dentry->d_inode->i_private);
+
+	/* Build the string */
+	dev_size = sprintf(tmpbuf, "%8lX\n", value);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	/* Return it to user space */
+	if (copy_to_user(buf, tmpbuf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (tmpbuf != NULL)
+		kfree(tmpbuf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_reg_write() - Implements debugfs write for B2R2 register
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to write
+ * @f_pos: File position
+ *
+ * Returns number of bytes written or negative error code
+ */
+static int debugfs_b2r2_reg_write(struct file *filp, const char __user *buf,
+				  size_t count, loff_t *f_pos)
+{
+	char tmpbuf[80];
+	u32 reg_value;
+	int ret = 0;
+
+	/* Adjust count */
+	if (count >= sizeof(tmpbuf))
+		count = sizeof(tmpbuf) - 1;
+	/* Get it from user space */
+	if (copy_from_user(tmpbuf, buf, count))
+		return -EINVAL;
+	tmpbuf[count] = 0;
+	/* Convert from hex string */
+	if (sscanf(tmpbuf, "%8lX", (unsigned long *) &reg_value) != 1)
+		return -EINVAL;
+
+	writel(reg_value, (u32 *)
+		filp->f_dentry->d_inode->i_private);
+
+	*f_pos += count;
+	ret = count;
+
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_reg_fops() - File operations for B2R2 register debugfs
+ */
+static const struct file_operations debugfs_b2r2_reg_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_reg_read,
+	.write = debugfs_b2r2_reg_write,
+};
+
+/**
+ * debugfs_b2r2_regs_read() - Implements debugfs read for B2R2 register dump
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes written or negative error code
+ */
+static int debugfs_b2r2_regs_read(struct file *filp, char __user *buf,
+				  size_t count, loff_t *f_pos)
+{
+	size_t dev_size = 0;
+	int ret = 0;
+	int i;
+	char *tmpbuf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+
+	if (tmpbuf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Build a giant string containing all registers */
+	for (i = 0; i < ARRAY_SIZE(debugfs_regs); i++) {
+		unsigned long value =
+			readl((u32 *) (((u8 *)
+				filp->f_dentry->d_inode->i_private) +
+				debugfs_regs[i].offset));
+		dev_size += sprintf(tmpbuf + dev_size, "%s: %08lX\n",
+			debugfs_regs[i].name,
+			value);
+	}
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, tmpbuf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (tmpbuf != NULL)
+		kfree(tmpbuf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_regs_fops() - File operations for B2R2 register dump debugfs
+ */
+static const struct file_operations debugfs_b2r2_regs_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_regs_read,
+};
+
+/**
+ * debugfs_b2r2_stat_read() - Implements debugfs read for B2R2 statistics
+ *
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_stat_read(struct file *filp, char __user *buf,
+				  size_t count, loff_t *f_pos)
+{
+	size_t dev_size = 0;
+	int ret = 0;
+	int i = 0;
+	char *tmpbuf = kmalloc(sizeof(char) * 4096, GFP_KERNEL);
+	struct b2r2_core *core = filp->f_dentry->d_inode->i_private;
+
+	if (tmpbuf == NULL) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Build a string containing all statistics */
+	dev_size += sprintf(tmpbuf + dev_size, "Interrupts        : %lu\n",
+			core->stat_n_irq);
+	dev_size += sprintf(tmpbuf + dev_size, "Added jobs        : %lu\n",
+			core->stat_n_jobs_added);
+	dev_size += sprintf(tmpbuf + dev_size, "Removed jobs      : %lu\n",
+			core->stat_n_jobs_removed);
+	dev_size += sprintf(tmpbuf + dev_size, "Jobs in prio list : %lu\n",
+			core->stat_n_jobs_in_prio_list);
+	dev_size += sprintf(tmpbuf + dev_size, "Active jobs       : %lu\n",
+			core->n_active_jobs);
+	for (i = 0; i < ARRAY_SIZE(core->active_jobs); i++)
+		dev_size += sprintf(tmpbuf + dev_size,
+				"   Job in queue %d : 0x%08lx\n",
+				i, (unsigned long) core->active_jobs[i]);
+	dev_size += sprintf(tmpbuf + dev_size, "Clock requests    : %lu\n",
+			core->clock_request_count);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, tmpbuf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	if (tmpbuf != NULL)
+		kfree(tmpbuf);
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_stat_fops() - File operations for B2R2 statistics debugfs
+ */
+static const struct file_operations debugfs_b2r2_stat_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_stat_read,
+};
+
+
+/**
+ * debugfs_b2r2_clock_read() - Implements debugfs read for
+ *                             PMU B2R2 clock register
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_clock_read(struct file *filp, char __user *buf,
+				   size_t count, loff_t *f_pos)
+{
+	/* 10 characters hex number + newline + string terminator; */
+	char tmpbuf[10+2];
+	size_t dev_size;
+	int ret = 0;
+	struct b2r2_core *core = filp->f_dentry->d_inode->i_private;
+
+	unsigned long value = clk_get_rate(core->b2r2_clock);
+
+	dev_size = sprintf(tmpbuf, "%#010lx\n", value);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, tmpbuf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_clock_write() - Implements debugfs write for
+ *                              PMU B2R2 clock register
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to write
+ * @f_pos: File position
+ *
+ * Returns number of bytes written or negative error code
+ */
+static int debugfs_b2r2_clock_write(struct file *filp, const char __user *buf,
+				    size_t count, loff_t *f_pos)
+{
+	char tmpbuf[80];
+	u32 reg_value;
+	int ret = 0;
+
+	if (count >= sizeof(tmpbuf))
+		count = sizeof(tmpbuf) - 1;
+	if (copy_from_user(tmpbuf, buf, count))
+		return -EINVAL;
+	tmpbuf[count] = 0;
+	if (sscanf(tmpbuf, "%8lX", (unsigned long *) &reg_value) != 1)
+		return -EINVAL;
+
+	/*not working yet*/
+	/*clk_set_rate(b2r2_core.b2r2_clock, (unsigned long) reg_value);*/
+
+	*f_pos += count;
+	ret = count;
+
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_clock_fops() - File operations for PMU B2R2 clock debugfs
+ */
+static const struct file_operations debugfs_b2r2_clock_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_clock_read,
+	.write = debugfs_b2r2_clock_write,
+};
+
+/**
+ * debugfs_b2r2_enabled_read() - Implements debugfs read for
+ *                             B2R2 Core Enable/Disable
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to read
+ * @f_pos: File position
+ *
+ * Returns number of bytes read or negative error code
+ */
+static int debugfs_b2r2_enabled_read(struct file *filp, char __user *buf,
+				   size_t count, loff_t *f_pos)
+{
+	/* 4 characters hex number + newline + string terminator; */
+	char tmpbuf[4+2];
+	size_t dev_size;
+	int ret = 0;
+	struct b2r2_core *core = filp->f_dentry->d_inode->i_private;
+
+	dev_size = sprintf(tmpbuf, "%02X\n", core->control->enabled);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, tmpbuf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+out:
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_enabled_write() - Implements debugfs write for
+ *                              B2R2 Core Enable/Disable
+ * @filp: File pointer
+ * @buf: User space buffer
+ * @count: Number of bytes to write
+ * @f_pos: File position
+ *
+ * Returns number of bytes written or negative error code
+ */
+static int debugfs_b2r2_enabled_write(struct file *filp, const char __user *buf,
+				    size_t count, loff_t *f_pos)
+{
+	char tmpbuf[80];
+	unsigned int enable;
+	int ret = 0;
+	struct b2r2_core *core = filp->f_dentry->d_inode->i_private;
+
+	if (count >= sizeof(tmpbuf))
+		count = sizeof(tmpbuf) - 1;
+	if (copy_from_user(tmpbuf, buf, count))
+		return -EINVAL;
+	tmpbuf[count] = 0;
+	if (sscanf(tmpbuf, "%02X", &enable) != 1)
+		return -EINVAL;
+
+	if (enable)
+		core->control->enabled = true;
+	else
+		core->control->enabled = false;
+
+	*f_pos += count;
+	ret = count;
+
+	return ret;
+}
+
+/**
+ * debugfs_b2r2_enabled_fops() - File operations for B2R2 Core Enable/Disable debugfs
+ */
+static const struct file_operations debugfs_b2r2_enabled_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_enabled_read,
+	.write = debugfs_b2r2_enabled_write,
+};
+
+#endif
+
+/**
+ *
+ * init_hw() - B2R2 Hardware reset & initiliaze
+ *
+ * @pdev: B2R2 platform device
+ *
+ * 1)Register interrupt handler
+ *
+ * 2)B2R2 Register map
+ *
+ * 3)For resetting B2R2 hardware,write to B2R2 Control register the
+ * B2R2BLT_CTLGLOBAL_soft_reset and then polling for on
+ * B2R2 status register for B2R2BLT_STA1BDISP_IDLE flag.
+ *
+ * 4)Wait for B2R2 hardware to be idle (on a timeout rather than while loop)
+ *
+ * 5)Driver status reset
+ *
+ * 6)Recover from any error without any leaks.
+ *
+ */
+static int init_hw(struct b2r2_core *core)
+{
+	int result = 0;
+	u32 uTimeOut = B2R2_RESET_TIMEOUT_VALUE;
+
+	/* Put B2R2 into reset */
+	clear_interrupts(core);
+
+	writel(readl(&core->hw->BLT_CTL) | B2R2BLT_CTLGLOBAL_soft_reset,
+		&core->hw->BLT_CTL);
+
+	/* Set up interrupt handler */
+	result = request_irq(core->irq, b2r2_irq_handler, IRQF_SHARED,
+			     "b2r2-interrupt", core);
+	if (result) {
+		b2r2_log_err(core->dev,
+			"%s: failed to register IRQ for B2R2\n", __func__);
+		goto b2r2_init_request_irq_failed;
+	}
+
+	b2r2_log_info(core->dev, "do a global reset..\n");
+
+	/* Release reset */
+	writel(0x00000000, &core->hw->BLT_CTL);
+
+	b2r2_log_info(core->dev, "wait for B2R2 to be idle..\n");
+
+	/** Wait for B2R2 to be idle (on a timeout rather than while loop) */
+	while ((uTimeOut > 0) &&
+	       ((readl(&core->hw->BLT_STA1) &
+		 B2R2BLT_STA1BDISP_IDLE) == 0x0))
+		uTimeOut--;
+	if (uTimeOut == 0) {
+		b2r2_log_err(core->dev,
+			 "%s: B2R2 not idle after SW reset\n", __func__);
+		result = -EAGAIN;
+		goto b2r2_core_init_hw_timeout;
+	}
+
+#ifdef CONFIG_DEBUG_FS
+	/* Register debug fs files for register access */
+	if (!IS_ERR_OR_NULL(core->debugfs_core_root_dir) &&
+			IS_ERR_OR_NULL(core->debugfs_regs_dir)) {
+		core->debugfs_regs_dir = debugfs_create_dir("regs",
+				core->debugfs_core_root_dir);
+	}
+	if (!IS_ERR_OR_NULL(core->debugfs_regs_dir)) {
+		int i;
+		debugfs_create_file("all", 0666, core->debugfs_regs_dir,
+				(void *)core->hw, &debugfs_b2r2_regs_fops);
+		/* Create debugfs entries for all static registers */
+		for (i = 0; i < ARRAY_SIZE(debugfs_regs); i++)
+			debugfs_create_file(debugfs_regs[i].name, 0666,
+					core->debugfs_regs_dir,
+					(void *)(((u8 *) core->hw) +
+							debugfs_regs[i].offset),
+					&debugfs_b2r2_reg_fops);
+	}
+#endif
+
+	b2r2_log_info(core->dev, "%s ended..\n", __func__);
+	return result;
+
+/** Recover from any error without any leaks */
+b2r2_core_init_hw_timeout:
+	/** Free B2R2 interrupt handler */
+	free_irq(core->irq, core);
+
+b2r2_init_request_irq_failed:
+	if (core->hw)
+		iounmap(core->hw);
+	core->hw = NULL;
+
+	return result;
+}
+
+
+/**
+ * exit_hw() - B2R2 Hardware exit
+ *
+ * core->lock _must_ NOT be held
+ */
+static void exit_hw(struct b2r2_core *core)
+{
+	unsigned long flags;
+
+	b2r2_log_info(core->dev, "%s started..\n", __func__);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Unregister our debugfs entries */
+	if (!IS_ERR_OR_NULL(core->debugfs_regs_dir)) {
+		debugfs_remove_recursive(core->debugfs_regs_dir);
+		core->debugfs_regs_dir = NULL;
+	}
+#endif
+	b2r2_log_debug(core->dev, "%s: locking core->lock\n", __func__);
+	spin_lock_irqsave(&core->lock, flags);
+
+	/* Cancel all pending jobs */
+	b2r2_log_debug(core->dev, "%s: canceling pending jobs\n", __func__);
+	exit_job_list(core, &core->prio_queue);
+
+	/* Soft reset B2R2 (Close all DMA,
+	   reset all state to idle, reset regs)*/
+	b2r2_log_debug(core->dev, "%s: putting b2r2 in reset\n", __func__);
+	writel(readl(&core->hw->BLT_CTL) | B2R2BLT_CTLGLOBAL_soft_reset,
+		&core->hw->BLT_CTL);
+
+	b2r2_log_debug(core->dev, "%s: clearing interrupts\n", __func__);
+	clear_interrupts(core);
+
+	/** Free B2R2 interrupt handler */
+	b2r2_log_debug(core->dev, "%s: freeing interrupt handler\n", __func__);
+	free_irq(core->irq, core);
+
+	b2r2_log_debug(core->dev, "%s: unlocking core->lock\n", __func__);
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	b2r2_log_info(core->dev, "%s ended...\n", __func__);
+}
+
+/**
+ * b2r2_probe() - This routine loads the B2R2 core driver
+ *
+ * @pdev: platform device.
+ */
+static int b2r2_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct resource *res = NULL;
+	struct b2r2_core *core = NULL;
+	struct b2r2_control *control = NULL;
+	struct b2r2_platform_data *pdata = NULL;
+	int debug_init = 0;
+
+	BUG_ON(pdev == NULL);
+	BUG_ON(pdev->id < 0 || pdev->id >= B2R2_MAX_NBR_DEVICES);
+
+	pdata = pdev->dev.platform_data;
+
+	core = kzalloc(sizeof(*core), GFP_KERNEL);
+	if (!core) {
+		dev_err(&pdev->dev, "b2r2 core alloc failed\n");
+		ret = -EINVAL;
+		goto error_exit;
+	}
+
+	core->dev = &pdev->dev;
+	dev_set_drvdata(core->dev, core);
+	if (pdev->id)
+		snprintf(core->name, sizeof(core->name), "b2r2_%d", pdev->id);
+	else
+		snprintf(core->name, sizeof(core->name), "b2r2");
+
+	dev_info(&pdev->dev, "init started.\n");
+
+	/* Init spin locks */
+	spin_lock_init(&core->lock);
+
+	/* Init job queues */
+	INIT_LIST_HEAD(&core->prio_queue);
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+	/* Create work queue for callbacks & timeout */
+	INIT_DELAYED_WORK(&core->timeout_work, timeout_work_function);
+#endif
+
+	/* Work queue for callbacks and timeout management */
+	core->work_queue = create_workqueue("B2R2");
+	if (!core->work_queue) {
+		ret = -ENOMEM;
+		goto error_exit;
+	}
+
+	/* Get the clock for B2R2 */
+	core->b2r2_clock = clk_get(core->dev, pdata->clock_id);
+	if (IS_ERR(core->b2r2_clock)) {
+		ret = PTR_ERR(core->b2r2_clock);
+		dev_err(&pdev->dev, "clk_get %s failed\n", pdata->clock_id);
+		goto error_exit;
+	}
+
+	/* Get the B2R2 regulator */
+	core->b2r2_reg = regulator_get(core->dev, pdata->regulator_id);
+	if (IS_ERR(core->b2r2_reg)) {
+		ret = PTR_ERR(core->b2r2_reg);
+		dev_err(&pdev->dev, "regulator_get %s failed "
+			"(dev_name=%s)\n", pdata->regulator_id,
+			dev_name(core->dev));
+		goto error_exit;
+	}
+
+	/* Init power management */
+	mutex_init(&core->domain_lock);
+	INIT_DELAYED_WORK_DEFERRABLE(&core->domain_disable_work,
+			domain_disable_work_function);
+	core->domain_enabled = false;
+	core->valid = false;
+
+	/* Map B2R2 into kernel virtual memory space */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL)
+		goto error_exit;
+
+	/* Hook up irq */
+	core->irq = platform_get_irq(pdev, 0);
+	if (core->irq <= 0) {
+		dev_err(&pdev->dev, "%s: Failed to request irq (irq=%d)\n",
+			__func__, core->irq);
+		goto error_exit;
+	}
+
+	core->hw = (struct b2r2_memory_map *) ioremap(res->start,
+			 res->end - res->start + 1);
+	if (core->hw == NULL) {
+		dev_err(&pdev->dev, "%s: ioremap failed\n", __func__);
+		ret = -ENOMEM;
+		goto error_exit;
+	}
+
+	dev_dbg(core->dev, "b2r2 structure address %p\n", core->hw);
+
+	control = kzalloc(sizeof(*control), GFP_KERNEL);
+	if (!control) {
+		dev_err(&pdev->dev, "b2r2 control alloc failed\n");
+		ret = -EINVAL;
+		goto error_exit;
+	}
+
+	control->data = (void *)core;
+	control->id = pdev->id;
+	control->dev = &pdev->dev; /* Temporary device */
+
+	core->op_size = B2R2_PLUG_OPCODE_SIZE_DEFAULT;
+	core->ch_size = B2R2_PLUG_CHUNK_SIZE_DEFAULT;
+	core->pg_size = B2R2_PLUG_PAGE_SIZE_DEFAULT;
+	core->mg_size = B2R2_PLUG_MESSAGE_SIZE_DEFAULT;
+	core->min_req_time = 0;
+
+#ifdef CONFIG_DEBUG_FS
+	core->debugfs_root_dir = debugfs_create_dir(core->name, NULL);
+	if (!IS_ERR_OR_NULL(core->debugfs_root_dir)) {
+		core->debugfs_core_root_dir = debugfs_create_dir("core",
+				core->debugfs_root_dir);
+		control->debugfs_debug_root_dir = debugfs_create_dir("debug",
+				core->debugfs_root_dir);
+		control->mem_heap.debugfs_root_dir = debugfs_create_dir("mem",
+				core->debugfs_root_dir);
+		control->debugfs_root_dir = debugfs_create_dir("blt",
+				core->debugfs_root_dir);
+	}
+
+	if (!IS_ERR_OR_NULL(core->debugfs_core_root_dir)) {
+		debugfs_create_file("stats", 0666, core->debugfs_core_root_dir,
+				core, &debugfs_b2r2_stat_fops);
+		debugfs_create_file("clock", 0666, core->debugfs_core_root_dir,
+				core, &debugfs_b2r2_clock_fops);
+		debugfs_create_file("enabled", 0666,
+				core->debugfs_core_root_dir,
+				core, &debugfs_b2r2_enabled_fops);
+		debugfs_create_u8("op_size", 0666, core->debugfs_core_root_dir,
+				&core->op_size);
+		debugfs_create_u8("ch_size", 0666, core->debugfs_core_root_dir,
+				&core->ch_size);
+		debugfs_create_u8("pg_size", 0666, core->debugfs_core_root_dir,
+				&core->pg_size);
+		debugfs_create_u8("mg_size", 0666, core->debugfs_core_root_dir,
+				&core->mg_size);
+		debugfs_create_u16("min_req_time", 0666,
+			core->debugfs_core_root_dir, &core->min_req_time);
+	}
+#endif
+
+	ret = b2r2_debug_init(control);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "b2r2_debug_init failed\n");
+		goto error_exit;
+	}
+	debug_init = 1;
+
+	/* Initialize b2r2_control */
+	ret = b2r2_control_init(control);
+	if (ret < 0) {
+		b2r2_log_err(&pdev->dev, "b2r2_control_init failed\n");
+		goto error_exit;
+	}
+	core->control = control;
+
+	/* Add the control to the blitter */
+	kref_init(&control->ref);
+	control->enabled = true;
+	b2r2_blt_add_control(control);
+
+	b2r2_core[pdev->id] = core;
+	dev_info(&pdev->dev, "%s done.\n", __func__);
+
+	return ret;
+
+/** Recover from any error if something fails */
+error_exit:
+	kfree(control);
+
+	if (!IS_ERR_OR_NULL(core->b2r2_reg))
+		regulator_put(core->b2r2_reg);
+
+	if (!IS_ERR_OR_NULL(core->b2r2_clock))
+		clk_put(core->b2r2_clock);
+
+	if (!IS_ERR_OR_NULL(core->work_queue))
+		destroy_workqueue(core->work_queue);
+
+	if (debug_init)
+		b2r2_debug_exit();
+
+#ifdef CONFIG_DEBUG_FS
+	if (!IS_ERR_OR_NULL(core->debugfs_root_dir)) {
+		debugfs_remove_recursive(core->debugfs_root_dir);
+		core->debugfs_root_dir = NULL;
+	}
+#endif
+	kfree(core);
+
+	dev_info(&pdev->dev, "%s done with errors (%d).\n", __func__, ret);
+
+	return ret;
+}
+
+void b2r2_core_release(struct kref *control_ref)
+{
+	struct b2r2_control *control = container_of(
+			control_ref, struct b2r2_control, ref);
+	struct b2r2_core *core = control->data;
+	int id = control->id;
+	unsigned long flags;
+#ifdef CONFIG_B2R2_DEBUG
+	struct device *dev = core->dev;
+#endif
+
+	b2r2_log_info(dev, "%s: enter\n", __func__);
+
+	/* Exit b2r2 control module */
+	b2r2_control_exit(control);
+	kfree(control);
+	b2r2_debug_exit();
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+	cancel_delayed_work(&core->timeout_work);
+#endif
+
+	/* Flush B2R2 work queue (call all callbacks for
+	   cancelled jobs) */
+	flush_workqueue(core->work_queue);
+
+	/* Make sure the power is turned off */
+	cancel_delayed_work_sync(&core->domain_disable_work);
+
+	/** Unmap B2R2 registers */
+	b2r2_log_info(dev, "%s: unmap b2r2 registers..\n", __func__);
+	if (core->hw) {
+		iounmap(core->hw);
+		core->hw = NULL;
+	}
+
+	destroy_workqueue(core->work_queue);
+
+	spin_lock_irqsave(&core->lock, flags);
+	core->work_queue = NULL;
+	spin_unlock_irqrestore(&core->lock, flags);
+
+	/* Return the clock */
+	clk_put(core->b2r2_clock);
+	regulator_put(core->b2r2_reg);
+
+	core->dev = NULL;
+	kfree(core);
+	b2r2_core[id] = NULL;
+
+	b2r2_log_info(dev, "%s: exit\n", __func__);
+}
+
+
+/**
+ * b2r2_remove - This routine unloads b2r2 driver
+ *
+ * @pdev: platform device.
+ */
+static int b2r2_remove(struct platform_device *pdev)
+{
+	struct b2r2_core *core;
+
+	BUG_ON(pdev == NULL);
+
+	core = dev_get_drvdata(&pdev->dev);
+	BUG_ON(core == NULL);
+	b2r2_log_info(&pdev->dev, "%s: Started\n", __func__);
+
+#ifdef CONFIG_DEBUG_FS
+	if (!IS_ERR_OR_NULL(core->debugfs_root_dir)) {
+		debugfs_remove_recursive(core->debugfs_root_dir);
+		core->debugfs_root_dir = NULL;
+	}
+#endif
+
+	/* Flush B2R2 work queue (call all callbacks) */
+	flush_workqueue(core->work_queue);
+
+	/* Remove control from blitter */
+	core->control->enabled = false;
+	b2r2_blt_remove_control(core->control);
+	kref_put(&core->control->ref, b2r2_core_release);
+
+	b2r2_log_info(&pdev->dev, "%s: Ended\n", __func__);
+
+	return 0;
+}
+/**
+ * b2r2_suspend() - This routine puts the B2R2 device in to sustend state.
+ * @pdev: platform device.
+ *
+ * This routine stores the current state of the b2r2 device and puts in to
+ * suspend state.
+ *
+ */
+int b2r2_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct b2r2_core *core;
+
+	BUG_ON(pdev == NULL);
+	core = dev_get_drvdata(&pdev->dev);
+	BUG_ON(core == NULL);
+	b2r2_log_info(core->dev, "%s\n", __func__);
+
+	/* Flush B2R2 work queue (call all callbacks) */
+	flush_workqueue(core->work_queue);
+
+#ifdef HANDLE_TIMEOUTED_JOBS
+	cancel_delayed_work(&core->timeout_work);
+#endif
+
+	/* Flush B2R2 work queue (call all callbacks for
+	   cancelled jobs) */
+	flush_workqueue(core->work_queue);
+
+	/* Make sure power is turned off */
+	cancel_delayed_work_sync(&core->domain_disable_work);
+
+	return 0;
+}
+
+
+/**
+ * b2r2_resume() - This routine resumes the B2R2 device from sustend state.
+ * @pdev: platform device.
+ *
+ * This routine restore back the current state of the b2r2 device resumes.
+ *
+ */
+int b2r2_resume(struct platform_device *pdev)
+{
+	struct b2r2_core *core;
+
+	BUG_ON(pdev == NULL);
+	core = dev_get_drvdata(&pdev->dev);
+	BUG_ON(core == NULL);
+	b2r2_log_info(core->dev, "%s\n", __func__);
+
+	return 0;
+}
+
+void b2r2_core_print_stats(struct b2r2_core *core)
+{
+	b2r2_log_info(core->dev,
+		"%s: n_irq %ld, n_irq_exit %ld, n_irq_skipped %ld,\n"
+		"n_jobs_added %ld, n_active_jobs %ld, "
+		"n_jobs_in_prio_list %ld,\n"
+		"n_jobs_removed %ld\n",
+		__func__,
+		core->stat_n_irq,
+		core->stat_n_irq_exit,
+		core->stat_n_irq_skipped,
+		core->stat_n_jobs_added,
+		core->n_active_jobs,
+		core->stat_n_jobs_in_prio_list,
+		core->stat_n_jobs_removed);
+}
+
+/**
+ * struct platform_b2r2_driver - Platform driver configuration for the
+ * B2R2 core driver
+ */
+static struct platform_driver platform_b2r2_driver = {
+	.remove = b2r2_remove,
+	.driver = {
+		.name	= "b2r2",
+	},
+	/** TODO implement power mgmt functions */
+	.suspend = b2r2_suspend,
+	.resume =  b2r2_resume,
+};
+
+
+/**
+ * b2r2_init() - Module init function for the B2R2 core module
+ */
+static int __init b2r2_init(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	return platform_driver_probe(&platform_b2r2_driver, b2r2_probe);
+}
+module_init(b2r2_init);
+
+/**
+ * b2r2_exit() - Module exit function for the B2R2 core module
+ */
+static void __exit b2r2_exit(void)
+{
+	printk(KERN_INFO "%s\n", __func__);
+	platform_driver_unregister(&platform_b2r2_driver);
+	return;
+}
+module_exit(b2r2_exit);
+
+
+/** Module is having GPL license */
+
+MODULE_LICENSE("GPL");
+
+/** Module author & discription */
+
+MODULE_AUTHOR("Robert Fekete (robert.fekete@stericsson.com)");
+MODULE_DESCRIPTION("B2R2 Core driver");
diff --git a/drivers/video/b2r2/b2r2_core.h b/drivers/video/b2r2/b2r2_core.h
new file mode 100644
index 00000000000..5b9fdcdc2bb
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_core.h
@@ -0,0 +1,300 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 core driver
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __B2R2_CORE_H__
+#define __B2R2_CORE_H__
+
+#include <linux/types.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+
+/**
+ * B2R2_RESET_TIMEOUT_VALUE - The number of times to read the status register
+ *                             waiting for b2r2 to go idle after soft reset.
+ */
+#define B2R2_RESET_TIMEOUT_VALUE (1500)
+
+/**
+ * B2R2_CLK_FLAG - Value to write into clock reg to turn clock on
+ */
+#define B2R2_CLK_FLAG (0x125)
+
+/**
+ * DEBUG_CHECK_ADDREF_RELEASE - Define this to enable addref / release debug
+ */
+#define DEBUG_CHECK_ADDREF_RELEASE 1
+
+#ifdef CONFIG_DEBUG_FS
+/**
+ * HANDLE_TIMEOUTED_JOBS - Define this to check jobs for timeout and cancel them
+ */
+#define HANDLE_TIMEOUTED_JOBS
+#define JOB_TIMEOUT (HZ/2)
+#endif
+
+/**
+ * B2R2_CLOCK_ALWAYS_ON - Define this to disable power save clock turn off
+ */
+/* #define B2R2_CLOCK_ALWAYS_ON 1 */
+
+/**
+ * START_SENTINEL - Watch guard to detect job overwrites
+ */
+#define START_SENTINEL 0xBABEDEEA
+
+/**
+ * STOP_SENTINEL - Watch guard to detect job overwrites
+ */
+#define END_SENTINEL 0xDADBDCDD
+
+/**
+ * B2R2_CORE_LOWEST_PRIO - Lowest prio allowed
+ */
+#define B2R2_CORE_LOWEST_PRIO -19
+/**
+ * B2R2_CORE_HIGHEST_PRIO - Highest prio allowed
+ */
+#define B2R2_CORE_HIGHEST_PRIO 20
+
+/**
+ * B2R2_DOMAIN_DISABLE -
+ */
+#define B2R2_DOMAIN_DISABLE_TIMEOUT (HZ/100)
+
+/**
+ * B2R2_REGULATOR_RETRY_COUNT -
+ */
+#define B2R2_REGULATOR_RETRY_COUNT 10
+
+
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+
+/**
+ * struct addref_release - Represents one addref or release. Used
+ *                         to debug addref / release problems
+ *
+ * @addref: true if this represents an addref else it represents
+ *          a release.
+ * @job: The job that was referenced
+ * @caller: The caller of the addref or release
+ * @ref_count: The job reference count after addref / release
+ */
+struct addref_release {
+	bool addref;
+	struct b2r2_core_job *job;
+	const char *caller;
+	int ref_count;
+};
+
+#endif
+
+/**
+ * struct b2r2_core - Administration data for B2R2 core
+ *
+ * @lock: Spin lock protecting the b2r2_core structure and the B2R2 HW
+ * @hw: B2R2 registers memory mapped
+ * @pmu_b2r2_clock: Control of B2R2 clock
+ * @log_dev: Device used for logging via dev_... functions
+ *
+ * @prio_queue: Queue of jobs sorted in priority order
+ * @active_jobs: Array containing pointer to zero or one job per queue
+ * @n_active_jobs: Number of active jobs
+ * @jiffies_last_active: jiffie value when adding last active job
+ * @jiffies_last_irq: jiffie value when last irq occured
+ * @timeout_work: Work structure for timeout work
+ *
+ * @next_job_id: Contains the job id that will be assigned to the next
+ *               added job.
+ *
+ * @clock_request_count: When non-zero, clock is on
+ * @clock_off_timer: Kernel timer to handle delayed turn off of clock
+ *
+ * @work_queue: Work queue to handle done jobs (callbacks) and timeouts in
+ *              non-interrupt context.
+ *
+ * @stat_n_irq: Number of interrupts (statistics)
+ * @stat_n_jobs_added: Number of jobs added (statistics)
+ * @stat_n_jobs_removed: Number of jobs removed (statistics)
+ * @stat_n_jobs_in_prio_list: Number of jobs in prio list (statistics)
+ *
+ * @debugfs_root_dir: Root directory for B2R2 debugfs
+ *
+ * @ar: Circular array of addref / release debug structs
+ * @ar_write: Where next write will occur
+ * @ar_read: First valid place to read. When ar_read == ar_write then
+ *           the array is empty.
+ */
+struct b2r2_core {
+	spinlock_t       lock;
+
+	struct b2r2_memory_map *hw;
+
+	u8 op_size;
+	u8 ch_size;
+	u8 pg_size;
+	u8 mg_size;
+	u16 min_req_time;
+	int irq;
+
+	char name[16];
+	struct device *dev;
+
+	struct list_head prio_queue;
+
+	struct b2r2_core_job *active_jobs[B2R2_CORE_QUEUE_NO_OF];
+	unsigned long    n_active_jobs;
+
+	unsigned long    jiffies_last_active;
+	unsigned long    jiffies_last_irq;
+#ifdef HANDLE_TIMEOUTED_JOBS
+	struct delayed_work     timeout_work;
+#endif
+	int              next_job_id;
+
+	unsigned long    clock_request_count;
+	struct timer_list clock_off_timer;
+
+	struct workqueue_struct *work_queue;
+
+	/* Statistics */
+	unsigned long    stat_n_irq_exit;
+	unsigned long    stat_n_irq_skipped;
+	unsigned long    stat_n_irq;
+	unsigned long    stat_n_jobs_added;
+	unsigned long    stat_n_jobs_removed;
+
+	unsigned long    stat_n_jobs_in_prio_list;
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *debugfs_root_dir;
+	struct dentry *debugfs_core_root_dir;
+	struct dentry *debugfs_regs_dir;
+#endif
+
+#ifdef DEBUG_CHECK_ADDREF_RELEASE
+	/* Tracking release bug...*/
+	struct addref_release ar[100];
+	int ar_write;
+	int ar_read;
+#endif
+
+	/* Power management variables */
+	struct mutex domain_lock;
+	struct delayed_work domain_disable_work;
+
+	/*
+	 * We need to keep track of both the number of domain_enable/disable()
+	 * calls and whether the power was actually turned off, since the
+	 * power off is done in a delayed job.
+	 */
+	bool domain_enabled;
+	volatile bool valid;
+	int domain_request_count;
+
+	struct clk *b2r2_clock;
+	struct regulator *b2r2_reg;
+
+	struct b2r2_control *control;
+};
+
+/**
+ * b2r2_core_job_add() - Adds a job to B2R2 job queues
+ *
+ * The job reference count will be increased after this function
+ * has been called and b2r2_core_job_release() must be called to
+ * release the reference. The job callback function will be always
+ * be called after the job is done or cancelled.
+ *
+ * @control: The b2r2 control entity
+ * @job: Job to be added
+ *
+ * Returns 0 if OK else negative error code
+ *
+ */
+int b2r2_core_job_add(struct b2r2_control *control,
+		struct b2r2_core_job *job);
+
+/**
+ * b2r2_core_job_wait() - Waits for an added job to be done.
+ *
+ * @job: Job to wait for
+ *
+ * Returns 0 if job done else negative error code
+ *
+ */
+int b2r2_core_job_wait(struct b2r2_core_job *job);
+
+/**
+ * b2r2_core_job_cancel() - Cancel an already added job.
+ *
+ * @job: Job to cancel
+ *
+ * Returns 0 if job cancelled or done else negative error code
+ *
+ */
+int b2r2_core_job_cancel(struct b2r2_core_job *job);
+
+/**
+ * b2r2_core_job_find() - Finds job with given job id
+ *
+ * Reference count will be increased for the found job
+ *
+ * @control: The b2r2 control entity
+ * @job_id: Job id to find
+ *
+ * Returns job if found, else NULL
+ *
+ */
+struct b2r2_core_job *b2r2_core_job_find(struct b2r2_control *control,
+		int job_id);
+
+/**
+ * b2r2_core_job_find_first_with_tag() - Finds first job with given tag
+ *
+ * Reference count will be increased for the found job.
+ * This function can be used to find all jobs for a client, i.e.
+ * when cancelling all jobs for a client.
+ *
+ * @control: The b2r2 control entity
+ * @tag: Tag to find
+ *
+ * Returns job if found, else NULL
+ *
+ */
+struct b2r2_core_job *b2r2_core_job_find_first_with_tag(
+		struct b2r2_control *control, int tag);
+
+/**
+ * b2r2_core_job_addref() - Increase the job reference count.
+ *
+ * @job: Job to increase reference count for.
+ * @caller: The function calling this function (for debug)
+ */
+void b2r2_core_job_addref(struct b2r2_core_job *job, const char *caller);
+
+/**
+ * b2r2_core_job_release() - Decrease the job reference count. The
+ *                           job will be released (the release() function
+ *                           will be called) when the reference count
+ *                           reaches zero.
+ *
+ * @job: Job to decrease reference count for.
+ * @caller: The function calling this function (for debug)
+ */
+void b2r2_core_job_release(struct b2r2_core_job *job, const char *caller);
+
+void b2r2_core_print_stats(struct b2r2_core *core);
+
+void b2r2_core_release(struct kref *control_ref);
+
+#endif /* !defined(__B2R2_CORE_JOB_H__) */
diff --git a/drivers/video/b2r2/b2r2_debug.c b/drivers/video/b2r2/b2r2_debug.c
new file mode 100644
index 00000000000..934ba938ee5
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_debug.c
@@ -0,0 +1,340 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 dynamic debug
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include "b2r2_debug.h"
+#include <linux/debugfs.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+
+int b2r2_log_levels[B2R2_LOG_LEVEL_COUNT];
+static struct dentry *log_lvl_dir;
+static int module_init;
+
+#define CHARS_IN_NODE_DUMP 1544
+#define DUMPED_NODE_SIZE (CHARS_IN_NODE_DUMP * sizeof(char) + 1)
+
+static void dump_node(char *dst, struct b2r2_node *node)
+{
+	dst += sprintf(dst, "node 0x%08x ------------------\n",
+			(unsigned int)node);
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_NIP:", node->node.GROUP0.B2R2_NIP);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CIC:", node->node.GROUP0.B2R2_CIC);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_INS:", node->node.GROUP0.B2R2_INS);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_ACK:", node->node.GROUP0.B2R2_ACK);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_TBA:", node->node.GROUP1.B2R2_TBA);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_TTY:", node->node.GROUP1.B2R2_TTY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_TXY:", node->node.GROUP1.B2R2_TXY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_TSZ:", node->node.GROUP1.B2R2_TSZ);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1CF:", node->node.GROUP2.B2R2_S1CF);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2CF:", node->node.GROUP2.B2R2_S2CF);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1BA:", node->node.GROUP3.B2R2_SBA);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1TY:", node->node.GROUP3.B2R2_STY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1XY:", node->node.GROUP3.B2R2_SXY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S1SZ:", node->node.GROUP3.B2R2_SSZ);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2BA:", node->node.GROUP4.B2R2_SBA);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2TY:", node->node.GROUP4.B2R2_STY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2XY:", node->node.GROUP4.B2R2_SXY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S2SZ:", node->node.GROUP4.B2R2_SSZ);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S3BA:", node->node.GROUP5.B2R2_SBA);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S3TY:", node->node.GROUP5.B2R2_STY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S3XY:", node->node.GROUP5.B2R2_SXY);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_S3SZ:", node->node.GROUP5.B2R2_SSZ);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CWO:", node->node.GROUP6.B2R2_CWO);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CWS:", node->node.GROUP6.B2R2_CWS);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CCO:", node->node.GROUP7.B2R2_CCO);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_CML:", node->node.GROUP7.B2R2_CML);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_PMK:", node->node.GROUP8.B2R2_PMK);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FCTL:", node->node.GROUP8.B2R2_FCTL);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_RSF:", node->node.GROUP9.B2R2_RSF);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_RZI:", node->node.GROUP9.B2R2_RZI);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_HFP:", node->node.GROUP9.B2R2_HFP);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_VFP:", node->node.GROUP9.B2R2_VFP);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_Y_RSF:", node->node.GROUP10.B2R2_RSF);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_Y_RZI:", node->node.GROUP10.B2R2_RZI);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_Y_HFP:", node->node.GROUP10.B2R2_HFP);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_Y_VFP:", node->node.GROUP10.B2R2_VFP);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FF0:", node->node.GROUP11.B2R2_FF0);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FF1:", node->node.GROUP11.B2R2_FF1);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FF2:", node->node.GROUP11.B2R2_FF2);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_FF3:", node->node.GROUP11.B2R2_FF3);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_KEY1:", node->node.GROUP12.B2R2_KEY1);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_KEY2:", node->node.GROUP12.B2R2_KEY2);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_XYL:", node->node.GROUP13.B2R2_XYL);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_XYP:", node->node.GROUP13.B2R2_XYP);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_SAR:", node->node.GROUP14.B2R2_SAR);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_USR:", node->node.GROUP14.B2R2_USR);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_IVMX0:", node->node.GROUP15.B2R2_VMX0);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_IVMX1:", node->node.GROUP15.B2R2_VMX1);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_IVMX2:", node->node.GROUP15.B2R2_VMX2);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_IVMX3:", node->node.GROUP15.B2R2_VMX3);
+	dst += sprintf(dst, "--\n");
+
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_OVMX0:", node->node.GROUP16.B2R2_VMX0);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_OVMX1:", node->node.GROUP16.B2R2_VMX1);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_OVMX2:", node->node.GROUP16.B2R2_VMX2);
+	dst += sprintf(dst, "%s\t0x%08x\n",
+			"B2R2_OVMX3:", node->node.GROUP16.B2R2_VMX3);
+	dst += sprintf(dst, "--\n");
+
+}
+
+void b2r2_debug_job_done(struct b2r2_control *cont,
+		struct b2r2_node *first_node)
+{
+	struct b2r2_node *node = first_node;
+	struct b2r2_node **dst_node;
+	unsigned int node_count = 0;
+
+	while (node != NULL) {
+		node_count++;
+		node = node->next;
+	}
+
+	mutex_lock(&cont->last_job_lock);
+
+	if (cont->last_job) {
+		node = cont->last_job;
+		while (node != NULL) {
+			struct b2r2_node *tmp = node->next;
+			kfree(node);
+			node = tmp;
+		}
+		cont->last_job = NULL;
+	}
+
+	node = first_node;
+	dst_node = &cont->last_job;
+	while (node != NULL) {
+		*dst_node = kzalloc(sizeof(**dst_node), GFP_KERNEL);
+		if (!(*dst_node))
+			goto last_job_alloc_failed;
+
+		memcpy(*dst_node, node, sizeof(**dst_node));
+
+		dst_node = &((*dst_node)->next);
+		node = node->next;
+	}
+
+	mutex_unlock(&cont->last_job_lock);
+
+	return;
+
+last_job_alloc_failed:
+	mutex_unlock(&cont->last_job_lock);
+
+	while (cont->last_job != NULL) {
+		struct b2r2_node *tmp = cont->last_job->next;
+		kfree(cont->last_job);
+		cont->last_job = tmp;
+	}
+
+	return;
+}
+
+static ssize_t last_job_read(struct file *filp, char __user *buf,
+		size_t bytes, loff_t *off)
+{
+	struct b2r2_control *cont = filp->f_dentry->d_inode->i_private;
+	struct b2r2_node *node = cont->last_job;
+	int node_count = 0;
+	int i;
+
+	size_t size;
+	size_t count;
+	loff_t offs = *off;
+
+	for (; node != NULL; node = node->next)
+		node_count++;
+
+	size = node_count * DUMPED_NODE_SIZE;
+
+	if (node_count != cont->prev_node_count) {
+		kfree(cont->last_job_chars);
+
+		cont->last_job_chars = kzalloc(size, GFP_KERNEL);
+		if (!cont->last_job_chars)
+			return 0;
+		cont->prev_node_count = node_count;
+	}
+
+	mutex_lock(&cont->last_job_lock);
+	node = cont->last_job;
+	for (i = 0; i < node_count; i++) {
+		BUG_ON(node == NULL);
+		dump_node(cont->last_job_chars +
+			i * DUMPED_NODE_SIZE/sizeof(char),
+			node);
+		node = node->next;
+	}
+	mutex_unlock(&cont->last_job_lock);
+
+	if (offs > size)
+		return 0;
+
+	if (offs + bytes > size)
+		count = size - offs;
+	else
+		count = bytes;
+
+	if (copy_to_user(buf, cont->last_job_chars + offs, count))
+		return -EFAULT;
+
+	*off = offs + count;
+	return count;
+}
+
+static const struct file_operations last_job_fops = {
+	.read = last_job_read,
+};
+
+int b2r2_debug_init(struct b2r2_control *cont)
+{
+	int i;
+
+	if (!module_init) {
+		for (i = 0; i < B2R2_LOG_LEVEL_COUNT; i++)
+			b2r2_log_levels[i] = 0;
+
+#if !defined(CONFIG_DYNAMIC_DEBUG) && defined(CONFIG_DEBUG_FS)
+		/*
+		 * If dynamic debug is disabled we need some other way to
+		 * control the log prints
+		 */
+		log_lvl_dir = debugfs_create_dir("b2r2_log", NULL);
+
+		/* No need to save the files,
+		 * they will be removed recursively */
+		if (!IS_ERR_OR_NULL(log_lvl_dir)) {
+			(void)debugfs_create_bool("warnings", 0644, log_lvl_dir,
+				&b2r2_log_levels[B2R2_LOG_LEVEL_WARN]);
+			(void)debugfs_create_bool("info", 0644, log_lvl_dir,
+				&b2r2_log_levels[B2R2_LOG_LEVEL_INFO]);
+			(void)debugfs_create_bool("debug", 0644, log_lvl_dir,
+				&b2r2_log_levels[B2R2_LOG_LEVEL_DEBUG]);
+			(void)debugfs_create_bool("regdumps", 0644, log_lvl_dir,
+				&b2r2_log_levels[B2R2_LOG_LEVEL_REGDUMP]);
+		}
+
+#elif defined(CONFIG_DYNAMIC_DEBUG)
+		/* log_lvl_dir is never used */
+		(void)log_lvl_dir;
+#endif
+		module_init++;
+	}
+
+	if (!IS_ERR_OR_NULL(cont->debugfs_debug_root_dir)) {
+		/* No need to save the file,
+		 * it will be removed recursively */
+		(void)debugfs_create_file("last_job", 0444,
+				cont->debugfs_debug_root_dir, cont,
+				&last_job_fops);
+	}
+
+	mutex_init(&cont->last_job_lock);
+
+	return 0;
+}
+
+void b2r2_debug_exit(void)
+{
+#if !defined(CONFIG_DYNAMIC_DEBUG) && defined(CONFIG_DEBUG_FS)
+	module_init--;
+	if (!module_init && !IS_ERR_OR_NULL(log_lvl_dir)) {
+		debugfs_remove_recursive(log_lvl_dir);
+		log_lvl_dir = NULL;
+	}
+#endif
+}
diff --git a/drivers/video/b2r2/b2r2_debug.h b/drivers/video/b2r2/b2r2_debug.h
new file mode 100644
index 00000000000..1b1ac83f6cb
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_debug.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 dynamic debug
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_DEBUG_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_DEBUG_H_
+
+#include <linux/device.h>
+
+#include "b2r2_internal.h"
+
+#ifdef CONFIG_B2R2_DEBUG
+
+/* Log macros */
+enum b2r2_log_levels {
+	B2R2_LOG_LEVEL_WARN,
+	B2R2_LOG_LEVEL_INFO,
+	B2R2_LOG_LEVEL_DEBUG,
+	B2R2_LOG_LEVEL_REGDUMP,
+	B2R2_LOG_LEVEL_COUNT,
+};
+
+/*
+ * Booleans controlling the different log levels. The different log levels are
+ * enabled separately (i.e. you can have info prints without the warn prints).
+ */
+extern int b2r2_log_levels[B2R2_LOG_LEVEL_COUNT];
+
+#define b2r2_log_err(b2r2_log_dev, ...) do { \
+		dev_err(b2r2_log_dev, __VA_ARGS__); \
+	} while (0)
+
+/* If dynamic debug is enabled it should be used instead of loglevels */
+#ifdef CONFIG_DYNAMIC_DEBUG
+#  define b2r2_log_warn(b2r2_log_dev, ...) do { \
+		dev_dbg(b2r2_log_dev, "WARN " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_info(b2r2_log_dev, ...) do { \
+		dev_dbg(b2r2_log_dev, "INFO " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_debug(b2r2_log_dev, ...) do { \
+		dev_dbg(b2r2_log_dev, "DEBUG " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_regdump(b2r2_log_dev, ...) do { \
+		dev_dbg(b2r2_log_dev, "REGD " __VA_ARGS__); \
+	} while (0)
+#else
+#  define b2r2_log_warn(b2r2_log_dev, ...) do { \
+		if (b2r2_log_levels[B2R2_LOG_LEVEL_WARN]) \
+			dev_warn(b2r2_log_dev, "WARN " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_info(b2r2_log_dev, ...) do { \
+		if (b2r2_log_levels[B2R2_LOG_LEVEL_INFO]) \
+			dev_info(b2r2_log_dev, "INFO " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_debug(b2r2_log_dev, ...) do { \
+		if (b2r2_log_levels[B2R2_LOG_LEVEL_DEBUG]) \
+			dev_dbg(b2r2_log_dev, "DEBUG " __VA_ARGS__); \
+	} while (0)
+#  define b2r2_log_regdump(b2r2_log_dev, ...) do { \
+		if (b2r2_log_levels[B2R2_LOG_LEVEL_REGDUMP]) \
+			dev_vdbg(b2r2_log_dev, "REGD " __VA_ARGS__); \
+	} while (0)
+#endif
+
+int b2r2_debug_init(struct b2r2_control *cont);
+void b2r2_debug_exit(void);
+void b2r2_debug_job_done(struct b2r2_control *cont,
+		struct b2r2_node *node);
+
+#else
+
+#define b2r2_log_err(...)
+#define b2r2_log_warn(...)
+#define b2r2_log_info(...)
+#define b2r2_log_debug(...)
+#define b2r2_log_regdump(...)
+
+static inline int b2r2_debug_init(struct b2r2_control *cont)
+{
+	return 0;
+}
+static inline void b2r2_debug_exit(void)
+{
+	return;
+}
+static inline void b2r2_debug_job_done(struct b2r2_control *cont,
+		struct b2r2_node *node)
+{
+	return;
+}
+
+#endif
+
+#endif
diff --git a/drivers/video/b2r2/b2r2_filters.c b/drivers/video/b2r2/b2r2_filters.c
new file mode 100644
index 00000000000..a93eb60d91f
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_filters.c
@@ -0,0 +1,377 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 filters.
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/dma-mapping.h>
+
+#include "b2r2_filters.h"
+#include "b2r2_internal.h"
+#include "b2r2_debug.h"
+
+/**
+ * struct b2r2_filter_spec filters[] - Filter lookup table
+ *
+ * Lookup table for filters for different scale factors. A filter
+ * will be selected according to "min < scale_factor <= max".
+ */
+static struct b2r2_filter_spec filters[] = {
+	{
+		.min = 1024,
+		.max = 1433,
+		.h_coeffs =  {
+			0x00, 0x00, 0xFA, 0x09, 0x34, 0x09, 0x00, 0x00,
+			0x00, 0x00, 0xF9, 0x10, 0x32, 0x06, 0xFF, 0x00,
+			0x00, 0x00, 0xF8, 0x17, 0x2F, 0x02, 0x00, 0x00,
+			0x00, 0x00, 0xF7, 0x20, 0x2A, 0xFF, 0x00, 0x00,
+			0x00, 0x00, 0xF8, 0x27, 0x25, 0xFC, 0x00, 0x00,
+			0x00, 0x00, 0xFA, 0x2D, 0x1D, 0xFC, 0x00, 0x00,
+			0x00, 0x00, 0xFE, 0x31, 0x15, 0xFC, 0x00, 0x00,
+			0x00, 0x00, 0x02, 0x35, 0x0D, 0xFC, 0x00, 0x00
+		},
+		.v_coeffs = {
+			0x00, 0x02, 0x3C, 0x02, 0x00,
+			0x00, 0x08, 0x3B, 0xFD, 0x00,
+			0x00, 0x10, 0x35, 0xFB, 0x00,
+			0x00, 0x18, 0x30, 0xF8, 0x00,
+			0x00, 0x1F, 0x27, 0xFA, 0x00,
+			0x00, 0x27, 0x1E, 0xFB, 0x00,
+			0x00, 0x2E, 0x16, 0xFC, 0x00,
+			0x00, 0x34, 0x0D, 0xFF, 0x00
+		},
+	},
+	{
+		.min = 1433,
+		.max = 1536,
+		.h_coeffs =  {
+			0xfe, 0x06, 0xf8, 0x0b, 0x30, 0x0b, 0xf8, 0x06,
+			0xff, 0x06, 0xf7, 0x12, 0x2d, 0x05, 0xfa, 0x06,
+			0x00, 0x04, 0xf6, 0x18, 0x2c, 0x00, 0xfc, 0x06,
+			0x01, 0x02, 0xf7, 0x1f, 0x27, 0xfd, 0xff, 0x04,
+			0x03, 0x00, 0xf9, 0x24, 0x24, 0xf9, 0x00, 0x03,
+			0x04, 0xff, 0xfd, 0x29, 0x1d, 0xf7, 0x02, 0x01,
+			0x06, 0xfc, 0x00, 0x2d, 0x17, 0xf6, 0x04, 0x00,
+			0x06, 0xfa, 0x05, 0x30, 0x0f, 0xf7, 0x06, 0xff
+		},
+		.v_coeffs = {
+			0xf6, 0x0e, 0x38, 0x0e, 0xf6,
+			0xf5, 0x15, 0x38, 0x06, 0xf8,
+			0xf5, 0x1d, 0x33, 0x00, 0xfb,
+			0xf6, 0x23, 0x2d, 0xfc, 0xfe,
+			0xf9, 0x28, 0x26, 0xf9, 0x00,
+			0xfc, 0x2c, 0x1e, 0xf7, 0x03,
+			0x00, 0x2e, 0x18, 0xf6, 0x04,
+			0x05, 0x2e, 0x11, 0xf7, 0x05
+		},
+	},
+	{
+		.min = 1536,
+		.max = 3072,
+		.h_coeffs =  {
+			0xfc, 0xfd, 0x06, 0x13, 0x18, 0x13, 0x06, 0xfd,
+			0xfc, 0xfe, 0x08, 0x15, 0x17, 0x12, 0x04, 0xfc,
+			0xfb, 0xfe, 0x0a, 0x16, 0x18, 0x10, 0x03, 0xfc,
+			0xfb, 0x00, 0x0b, 0x18, 0x17, 0x0f, 0x01, 0xfb,
+			0xfb, 0x00, 0x0d, 0x19, 0x17, 0x0d, 0x00, 0xfb,
+			0xfb, 0x01, 0x0f, 0x19, 0x16, 0x0b, 0x00, 0xfb,
+			0xfc, 0x03, 0x11, 0x19, 0x15, 0x09, 0xfe, 0xfb,
+			0xfc, 0x04, 0x12, 0x1a, 0x12, 0x08, 0xfe, 0xfc
+		},
+		.v_coeffs = {
+			0x05, 0x10, 0x16, 0x10, 0x05,
+			0x06, 0x11, 0x16, 0x0f, 0x04,
+			0x08, 0x13, 0x15, 0x0e, 0x02,
+			0x09, 0x14, 0x16, 0x0c, 0x01,
+			0x0b, 0x15, 0x15, 0x0b, 0x00,
+			0x0d, 0x16, 0x13, 0x0a, 0x00,
+			0x0f, 0x17, 0x13, 0x08, 0xff,
+			0x11, 0x18, 0x12, 0x07, 0xfe
+		},
+	},
+	{
+		.min = 3072,
+		.max = 4096,
+		.h_coeffs =  {
+			0xfe, 0x02, 0x09, 0x0f, 0x0e, 0x0f, 0x09, 0x02,
+			0xff, 0x02, 0x09, 0x0f, 0x10, 0x0e, 0x08, 0x01,
+			0xff, 0x03, 0x0a, 0x10, 0x10, 0x0d, 0x07, 0x00,
+			0x00, 0x04, 0x0b, 0x10, 0x0f, 0x0c, 0x06, 0x00,
+			0x00, 0x05, 0x0c, 0x10, 0x0e, 0x0c, 0x05, 0x00,
+			0x00, 0x06, 0x0c, 0x11, 0x0e, 0x0b, 0x04, 0x00,
+			0x00, 0x07, 0x0d, 0x11, 0x0f, 0x0a, 0x03, 0xff,
+			0x01, 0x08, 0x0e, 0x11, 0x0e, 0x09, 0x02, 0xff
+		},
+		.v_coeffs = {
+			0x09, 0x0f, 0x10, 0x0f, 0x09,
+			0x09, 0x0f, 0x12, 0x0e, 0x08,
+			0x0a, 0x10, 0x11, 0x0e, 0x07,
+			0x0b, 0x11, 0x11, 0x0d, 0x06,
+			0x0c, 0x11, 0x12, 0x0c, 0x05,
+			0x0d, 0x12, 0x11, 0x0c, 0x04,
+			0x0e, 0x12, 0x11, 0x0b, 0x04,
+			0x0f, 0x13, 0x11, 0x0a, 0x03
+		},
+	},
+	{
+		.min = 4096,
+		.max = 5120,
+		.h_coeffs =  {
+			0x00, 0x04, 0x09, 0x0c, 0x0e, 0x0c, 0x09, 0x04,
+			0x01, 0x05, 0x09, 0x0c, 0x0d, 0x0c, 0x08, 0x04,
+			0x01, 0x05, 0x0a, 0x0c, 0x0e, 0x0b, 0x08, 0x03,
+			0x02, 0x06, 0x0a, 0x0d, 0x0c, 0x0b, 0x07, 0x03,
+			0x02, 0x07, 0x0a, 0x0d, 0x0d, 0x0a, 0x07, 0x02,
+			0x03, 0x07, 0x0b, 0x0d, 0x0c, 0x0a, 0x06, 0x02,
+			0x03, 0x08, 0x0b, 0x0d, 0x0d, 0x0a, 0x05, 0x01,
+			0x04, 0x08, 0x0c, 0x0d, 0x0c, 0x09, 0x05, 0x01
+		},
+		.v_coeffs = {
+			0x0a, 0x0e, 0x10, 0x0e, 0x0a,
+			0x0b, 0x0e, 0x0f, 0x0e, 0x0a,
+			0x0b, 0x0f, 0x10, 0x0d, 0x09,
+			0x0c, 0x0f, 0x10, 0x0d, 0x08,
+			0x0d, 0x0f, 0x0f, 0x0d, 0x08,
+			0x0d, 0x10, 0x10, 0x0c, 0x07,
+			0x0e, 0x10, 0x0f, 0x0c, 0x07,
+			0x0f, 0x10, 0x10, 0x0b, 0x06
+		},
+	},
+};
+static const size_t filters_size = sizeof(filters)/sizeof(filters[0]);
+
+/**
+ * struct b2r2_filter_spec bilinear_filter - A bilinear filter
+ *
+ * The bilinear filter will be used if no custom filters are specified, or
+ * for upscales not matching any filter in the lookup table.
+ */
+static struct b2r2_filter_spec bilinear_filter = {
+	.min = 0,
+	.max = 0xffff,
+	.h_coeffs = {
+		0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+		0x00, 0x00, 0xff, 0x08, 0x3e, 0xfb, 0x00, 0x00,
+		0x00, 0x00, 0xfb, 0x13, 0x3b, 0xf7, 0x00, 0x00,
+		0x00, 0x00, 0xf8, 0x1f, 0x34, 0xf5, 0x00, 0x00,
+		0x00, 0x00, 0xf6, 0x2b, 0x2a, 0xf5, 0x00, 0x00,
+		0x00, 0x00, 0xf6, 0x35, 0x1e, 0xf7, 0x00, 0x00,
+		0x00, 0x00, 0xf9, 0x3c, 0x12, 0xf9, 0x00, 0x00,
+		0x00, 0x00, 0xfd, 0x3f, 0x07, 0xfd, 0x00, 0x00
+	},
+	.v_coeffs = {
+		0x00, 0x00, 0x40, 0x00, 0x00,
+		0x00, 0x09, 0x3d, 0xfa, 0x00,
+		0x00, 0x13, 0x39, 0xf4, 0x00,
+		0x00, 0x1e, 0x31, 0xf1, 0x00,
+		0x00, 0x27, 0x28, 0xf1, 0x00,
+		0x00, 0x31, 0x1d, 0xf2, 0x00,
+		0x00, 0x38, 0x12, 0xf6, 0x00,
+		0x00, 0x3d, 0x07, 0xfc, 0x00
+	},
+};
+
+/**
+ * struct b2r2_filter_spec default_downscale_filter - Default filter for downscale
+ *
+ * The default downscale filter will be used for downscales not matching any
+ * filter in the lookup table.
+ */
+static struct b2r2_filter_spec default_downscale_filter = {
+	.min = 1 << 10,
+	.max = 0xffff,
+	.h_coeffs = {
+		0x03, 0x06, 0x09, 0x0b, 0x09, 0x0b, 0x09, 0x06,
+		0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+		0x03, 0x06, 0x09, 0x0b, 0x0c, 0x0a, 0x08, 0x05,
+		0x04, 0x07, 0x09, 0x0b, 0x0b, 0x0a, 0x08, 0x04,
+		0x04, 0x07, 0x0a, 0x0b, 0x0b, 0x0a, 0x07, 0x04,
+		0x04, 0x08, 0x0a, 0x0b, 0x0b, 0x09, 0x07, 0x04,
+		0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03,
+		0x05, 0x08, 0x0a, 0x0b, 0x0c, 0x09, 0x06, 0x03
+	},
+	.v_coeffs = {
+		0x0b, 0x0e, 0x0e, 0x0e, 0x0b,
+		0x0b, 0x0e, 0x0f, 0x0d, 0x0b,
+		0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+		0x0c, 0x0e, 0x0f, 0x0d, 0x0a,
+		0x0d, 0x0f, 0x0e, 0x0d, 0x09,
+		0x0d, 0x0f, 0x0f, 0x0c, 0x09,
+		0x0e, 0x0f, 0x0e, 0x0c, 0x09,
+		0x0e, 0x0f, 0x0f, 0x0c, 0x08
+	},
+};
+
+/**
+ * struct b2r2_filter_spec blur_filter - Blur filter
+ *
+ * Filter for blurring an image.
+ */
+static struct b2r2_filter_spec blur_filter = {
+	.min = 0,
+	.max = 0xffff,
+	.h_coeffs = {
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08,
+		0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08
+	},
+	.v_coeffs = {
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c,
+		0x0c, 0x0c, 0x10, 0x0c, 0x0c
+	},
+};
+
+/* Private function declarations */
+static int alloc_filter_coeffs(struct device *dev,
+		struct b2r2_filter_spec *filter);
+static void free_filter_coeffs(struct device *dev,
+		struct b2r2_filter_spec *filter);
+
+/* Public functions */
+
+int b2r2_filters_init(struct b2r2_control *cont)
+{
+	int i;
+
+	if (cont->filters_initialized)
+		return 0;
+
+	for (i = 0; i < filters_size; i++) {
+		alloc_filter_coeffs(cont->dev, &filters[i]);
+	}
+
+	alloc_filter_coeffs(cont->dev, &bilinear_filter);
+	alloc_filter_coeffs(cont->dev, &default_downscale_filter);
+	alloc_filter_coeffs(cont->dev, &blur_filter);
+
+	cont->filters_initialized = 1;
+
+	return 0;
+}
+
+void b2r2_filters_exit(struct b2r2_control *cont)
+{
+	int i;
+
+	if (!cont->filters_initialized)
+		return;
+
+	for (i = 0; i < filters_size; i++) {
+		free_filter_coeffs(cont->dev, &filters[i]);
+	}
+
+	free_filter_coeffs(cont->dev, &bilinear_filter);
+	free_filter_coeffs(cont->dev, &default_downscale_filter);
+	free_filter_coeffs(cont->dev, &blur_filter);
+
+	cont->filters_initialized = 0;
+}
+
+struct b2r2_filter_spec *b2r2_filter_find(u16 scale_factor)
+{
+	int i;
+	struct b2r2_filter_spec *filter = NULL;
+
+	for (i = 0; i < filters_size; i++) {
+		if ((filters[i].min < scale_factor) &&
+				(scale_factor <= filters[i].max) &&
+				filters[i].h_coeffs_dma_addr &&
+				filters[i].v_coeffs_dma_addr) {
+			filter = &filters[i];
+			break;
+		}
+	}
+
+	if (filter == NULL) {
+		/*
+		 * No suitable filter has been found. Use default filters,
+		 * bilinear for any upscale.
+		 */
+		if (scale_factor < (1 << 10))
+			filter = &bilinear_filter;
+		else
+			filter = &default_downscale_filter;
+	}
+
+	/*
+	 * Check so that the coefficients were successfully allocated for this
+	 * filter.
+	 */
+	if (!filter->h_coeffs_dma_addr || !filter->v_coeffs_dma_addr)
+		return NULL;
+	else
+		return filter;
+}
+
+struct b2r2_filter_spec *b2r2_filter_blur()
+{
+	return &blur_filter;
+}
+
+/* Private functions */
+static int alloc_filter_coeffs(struct device *dev,
+		struct b2r2_filter_spec *filter)
+{
+	int ret;
+
+	filter->h_coeffs_dma_addr = dma_alloc_coherent(dev,
+			B2R2_HF_TABLE_SIZE, &(filter->h_coeffs_phys_addr),
+			GFP_DMA | GFP_KERNEL);
+	if (filter->h_coeffs_dma_addr == NULL) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	filter->v_coeffs_dma_addr = dma_alloc_coherent(dev,
+			B2R2_VF_TABLE_SIZE, &(filter->v_coeffs_phys_addr),
+			GFP_DMA | GFP_KERNEL);
+	if (filter->v_coeffs_dma_addr == NULL) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	memcpy(filter->h_coeffs_dma_addr, filter->h_coeffs,
+			B2R2_HF_TABLE_SIZE);
+	memcpy(filter->v_coeffs_dma_addr, filter->v_coeffs,
+			B2R2_VF_TABLE_SIZE);
+
+	return 0;
+
+error:
+	free_filter_coeffs(dev, filter);
+	return ret;
+
+}
+
+static void free_filter_coeffs(struct device *dev,
+		struct b2r2_filter_spec *filter)
+{
+	if (filter->h_coeffs_dma_addr != NULL)
+		dma_free_coherent(dev, B2R2_HF_TABLE_SIZE,
+				filter->h_coeffs_dma_addr,
+				filter->h_coeffs_phys_addr);
+	if (filter->v_coeffs_dma_addr != NULL)
+		dma_free_coherent(dev, B2R2_VF_TABLE_SIZE,
+				filter->v_coeffs_dma_addr,
+				filter->v_coeffs_phys_addr);
+
+	filter->h_coeffs_dma_addr = NULL;
+	filter->h_coeffs_phys_addr = 0;
+	filter->v_coeffs_dma_addr = NULL;
+	filter->v_coeffs_phys_addr = 0;
+}
diff --git a/drivers/video/b2r2/b2r2_filters.h b/drivers/video/b2r2/b2r2_filters.h
new file mode 100644
index 00000000000..790c9ec8ee9
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_filters.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 filters.
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_VIDEO_B2R2_FILTERS_H
+#define _LINUX_VIDEO_B2R2_FILTERS_H
+
+#include <linux/kernel.h>
+
+#include "b2r2_internal.h"
+
+#define B2R2_HF_TABLE_SIZE 64
+#define B2R2_VF_TABLE_SIZE 40
+
+/**
+ * @struct b2r2_filter_spec - Filter specification structure
+ *
+ *   @param min - Minimum scale factor for this filter (in 6.10 fixed point)
+ *   @param max - Maximum scale factor for this filter (in 6.10 fixed point)
+ *   @param h_coeffs - Horizontal filter coefficients
+ *   @param v_coeffs - Vertical filter coefficients
+ *   @param h_coeffs_dma_addr - Virtual DMA address for horizontal coefficients
+ *   @param v_coeffs_dma_addr - Virtual DMA address for vertical coefficients
+ *   @param h_coeffs_phys_addr - Physical address for horizontal coefficients
+ *   @param v_coeffs_phys_addr - Physical address for vertical coefficients
+ */
+struct b2r2_filter_spec {
+	const u16 min;
+	const u16 max;
+
+	const u8 h_coeffs[B2R2_HF_TABLE_SIZE];
+	const u8 v_coeffs[B2R2_VF_TABLE_SIZE];
+
+	void *h_coeffs_dma_addr;
+	u32 h_coeffs_phys_addr;
+
+	void *v_coeffs_dma_addr;
+	u32 v_coeffs_phys_addr;
+};
+
+/**
+ * b2r2_filters_init() - Initilizes the B2R2 filters
+ */
+int b2r2_filters_init(struct b2r2_control *control);
+
+/**
+ * b2r2_filters_init() - De-initilizes the B2R2 filters
+ */
+void b2r2_filters_exit(struct b2r2_control *control);
+
+/**
+ * b2r2_filter_find() - Find a filter matching the given scale factor
+ *
+ *   @param scale_factor - Scale factor to find a filter for
+ *
+ * Returns NULL if no filter could be found.
+ */
+struct b2r2_filter_spec *b2r2_filter_find(u16 scale_factor);
+
+/**
+ * b2r2_filter_blur() - Returns the blur filter
+ *
+ * Returns NULL if no blur filter is available.
+ */
+struct b2r2_filter_spec *b2r2_filter_blur(void);
+
+#endif /* _LINUX_VIDEO_B2R2_FILTERS_H */
diff --git a/drivers/video/b2r2/b2r2_generic.c b/drivers/video/b2r2/b2r2_generic.c
new file mode 100644
index 00000000000..4191e497e13
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_generic.c
@@ -0,0 +1,3206 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 generic. Full coverage of user interface but
+ * non optimized implementation. For Fallback purposes.
+ *
+ * Author: Maciej Socha <maciej.socha@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/debugfs.h>
+
+#include "b2r2_generic.h"
+#include "b2r2_internal.h"
+#include "b2r2_global.h"
+#include "b2r2_debug.h"
+#include "b2r2_filters.h"
+#include "b2r2_hw_convert.h"
+
+/*
+ * Debug printing
+ */
+#define B2R2_GENERIC_DEBUG_AREAS 0
+#define B2R2_GENERIC_DEBUG
+
+#define B2R2_GENERIC_WORK_BUF_WIDTH 16
+#define B2R2_GENERIC_WORK_BUF_HEIGHT 16
+#define B2R2_GENERIC_WORK_BUF_PITCH (16 * 4)
+#define B2R2_GENERIC_WORK_BUF_FMT B2R2_NATIVE_ARGB8888
+
+/*
+ * Private functions
+ */
+
+/**
+ * reset_nodes() - clears the node list
+ */
+static void reset_nodes(struct b2r2_control *cont,
+		struct b2r2_node *node)
+{
+	b2r2_log_info(cont->dev, "%s ENTRY\n", __func__);
+
+	while (node != NULL) {
+		memset(&(node->node), 0, sizeof(node->node));
+
+		/* TODO: Implement support for short linked lists */
+		node->node.GROUP0.B2R2_CIC = 0x7fffc;
+
+		if (node->next == NULL)
+			break;
+
+		node->node.GROUP0.B2R2_NIP = node->next->physical_address;
+
+		node = node->next;
+	}
+	b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+}
+
+/**
+ * dump_nodes() - prints the node list
+ */
+static void dump_nodes(struct b2r2_control *cont,
+		struct b2r2_node *first, bool dump_all)
+{
+	struct b2r2_node *node = first;
+	b2r2_log_info(cont->dev, "%s ENTRY\n", __func__);
+	do {
+		b2r2_log_debug(cont->dev, "\nNODE START:\n=============\n");
+		b2r2_log_debug(cont->dev, "B2R2_ACK: \t0x%.8x\n",
+				node->node.GROUP0.B2R2_ACK);
+		b2r2_log_debug(cont->dev, "B2R2_INS: \t0x%.8x\n",
+				node->node.GROUP0.B2R2_INS);
+		b2r2_log_debug(cont->dev, "B2R2_CIC: \t0x%.8x\n",
+				node->node.GROUP0.B2R2_CIC);
+		b2r2_log_debug(cont->dev, "B2R2_NIP: \t0x%.8x\n",
+				node->node.GROUP0.B2R2_NIP);
+
+		b2r2_log_debug(cont->dev, "B2R2_TSZ: \t0x%.8x\n",
+				node->node.GROUP1.B2R2_TSZ);
+		b2r2_log_debug(cont->dev, "B2R2_TXY: \t0x%.8x\n",
+				node->node.GROUP1.B2R2_TXY);
+		b2r2_log_debug(cont->dev, "B2R2_TTY: \t0x%.8x\n",
+				node->node.GROUP1.B2R2_TTY);
+		b2r2_log_debug(cont->dev, "B2R2_TBA: \t0x%.8x\n",
+				node->node.GROUP1.B2R2_TBA);
+
+		b2r2_log_debug(cont->dev, "B2R2_S2CF: \t0x%.8x\n",
+				node->node.GROUP2.B2R2_S2CF);
+		b2r2_log_debug(cont->dev, "B2R2_S1CF: \t0x%.8x\n",
+				node->node.GROUP2.B2R2_S1CF);
+
+		b2r2_log_debug(cont->dev, "B2R2_S1SZ: \t0x%.8x\n",
+				node->node.GROUP3.B2R2_SSZ);
+		b2r2_log_debug(cont->dev, "B2R2_S1XY: \t0x%.8x\n",
+				node->node.GROUP3.B2R2_SXY);
+		b2r2_log_debug(cont->dev, "B2R2_S1TY: \t0x%.8x\n",
+				node->node.GROUP3.B2R2_STY);
+		b2r2_log_debug(cont->dev, "B2R2_S1BA: \t0x%.8x\n",
+				node->node.GROUP3.B2R2_SBA);
+
+		b2r2_log_debug(cont->dev, "B2R2_S2SZ: \t0x%.8x\n",
+				node->node.GROUP4.B2R2_SSZ);
+		b2r2_log_debug(cont->dev, "B2R2_S2XY: \t0x%.8x\n",
+				node->node.GROUP4.B2R2_SXY);
+		b2r2_log_debug(cont->dev, "B2R2_S2TY: \t0x%.8x\n",
+				node->node.GROUP4.B2R2_STY);
+		b2r2_log_debug(cont->dev, "B2R2_S2BA: \t0x%.8x\n",
+				node->node.GROUP4.B2R2_SBA);
+
+		b2r2_log_debug(cont->dev, "B2R2_S3SZ: \t0x%.8x\n",
+				node->node.GROUP5.B2R2_SSZ);
+		b2r2_log_debug(cont->dev, "B2R2_S3XY: \t0x%.8x\n",
+				node->node.GROUP5.B2R2_SXY);
+		b2r2_log_debug(cont->dev, "B2R2_S3TY: \t0x%.8x\n",
+				node->node.GROUP5.B2R2_STY);
+		b2r2_log_debug(cont->dev, "B2R2_S3BA: \t0x%.8x\n",
+				node->node.GROUP5.B2R2_SBA);
+
+		b2r2_log_debug(cont->dev, "B2R2_CWS: \t0x%.8x\n",
+				node->node.GROUP6.B2R2_CWS);
+		b2r2_log_debug(cont->dev, "B2R2_CWO: \t0x%.8x\n",
+				node->node.GROUP6.B2R2_CWO);
+
+		b2r2_log_debug(cont->dev, "B2R2_FCTL: \t0x%.8x\n",
+				node->node.GROUP8.B2R2_FCTL);
+		b2r2_log_debug(cont->dev, "B2R2_RSF: \t0x%.8x\n",
+				node->node.GROUP9.B2R2_RSF);
+		b2r2_log_debug(cont->dev, "B2R2_RZI: \t0x%.8x\n",
+				node->node.GROUP9.B2R2_RZI);
+		b2r2_log_debug(cont->dev, "B2R2_HFP: \t0x%.8x\n",
+				node->node.GROUP9.B2R2_HFP);
+		b2r2_log_debug(cont->dev, "B2R2_VFP: \t0x%.8x\n",
+				node->node.GROUP9.B2R2_VFP);
+		b2r2_log_debug(cont->dev, "B2R2_LUMA_RSF: \t0x%.8x\n",
+				node->node.GROUP10.B2R2_RSF);
+		b2r2_log_debug(cont->dev, "B2R2_LUMA_RZI: \t0x%.8x\n",
+				node->node.GROUP10.B2R2_RZI);
+		b2r2_log_debug(cont->dev, "B2R2_LUMA_HFP: \t0x%.8x\n",
+				node->node.GROUP10.B2R2_HFP);
+		b2r2_log_debug(cont->dev, "B2R2_LUMA_VFP: \t0x%.8x\n",
+				node->node.GROUP10.B2R2_VFP);
+
+
+		b2r2_log_debug(cont->dev, "B2R2_IVMX0: \t0x%.8x\n",
+				node->node.GROUP15.B2R2_VMX0);
+		b2r2_log_debug(cont->dev, "B2R2_IVMX1: \t0x%.8x\n",
+				node->node.GROUP15.B2R2_VMX1);
+		b2r2_log_debug(cont->dev, "B2R2_IVMX2: \t0x%.8x\n",
+				node->node.GROUP15.B2R2_VMX2);
+		b2r2_log_debug(cont->dev, "B2R2_IVMX3: \t0x%.8x\n",
+				node->node.GROUP15.B2R2_VMX3);
+		b2r2_log_debug(cont->dev, "\n=============\nNODE END\n");
+
+		node = node->next;
+	} while (node != NULL && dump_all);
+
+	b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+}
+
+/**
+ * to_native_fmt() - returns the native B2R2 format
+ */
+static inline enum b2r2_native_fmt to_native_fmt(struct b2r2_control *cont,
+		enum b2r2_blt_fmt fmt)
+{
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_UNUSED:
+		return B2R2_NATIVE_RGB565;
+	case B2R2_BLT_FMT_1_BIT_A1:
+		return B2R2_NATIVE_A1;
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return B2R2_NATIVE_A8;
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		return B2R2_NATIVE_RGB565;
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		return B2R2_NATIVE_ARGB4444;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		return B2R2_NATIVE_ARGB1555;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		return B2R2_NATIVE_ARGB8565;
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+		return B2R2_NATIVE_RGB888;
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+		return B2R2_NATIVE_YCBCR888;
+	case B2R2_BLT_FMT_32_BIT_ABGR8888: /* Not actually supported by HW */
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+		return B2R2_NATIVE_ARGB8888;
+	case B2R2_BLT_FMT_32_BIT_VUYA8888: /* fall through */
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return B2R2_NATIVE_AYCBCR8888;
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		return B2R2_NATIVE_YCBCR422R;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+		return B2R2_NATIVE_YCBCR422R;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return B2R2_NATIVE_YCBCR42X_R2B;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return B2R2_NATIVE_YCBCR42X_MBN;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return B2R2_NATIVE_YUV;
+	default:
+		/* Should never ever happen */
+		return B2R2_NATIVE_BYTE;
+	}
+}
+
+/**
+ * get_alpha_range() - returns the alpha range of the given format
+ */
+static inline enum b2r2_ty get_alpha_range(struct b2r2_control *cont,
+		enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		return B2R2_TY_ALPHA_RANGE_255; /* 0 - 255 */
+		break;
+	default:
+		break;
+	}
+
+	return B2R2_TY_ALPHA_RANGE_128; /* 0 - 128 */
+}
+
+static unsigned int get_pitch(struct b2r2_control *cont,
+		enum b2r2_blt_fmt format, u32 width)
+{
+	switch (format) {
+	case B2R2_BLT_FMT_1_BIT_A1: {
+		int pitch = width >> 3;
+		/* Check for remainder */
+		if (width & 7)
+			pitch++;
+		return pitch;
+		break;
+	}
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return width;
+		break;
+	case B2R2_BLT_FMT_16_BIT_RGB565: /* all 16 bits/pixel RGB formats */
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		return width * 2;
+		break;
+	case B2R2_BLT_FMT_24_BIT_RGB888: /* all 24 bits/pixel raster formats */
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+		return width * 3;
+		break;
+	case B2R2_BLT_FMT_32_BIT_ARGB8888: /* all 32 bits/pixel formats */
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return width * 4;
+		break;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		/* width of the buffer must be a multiple of 4 */
+		if (width & 3) {
+			b2r2_log_warn(cont->dev, "%s: Illegal width "
+				"for fmt=%#010x width=%d\n", __func__,
+				format, width);
+			return 0;
+		}
+		return width * 2;
+		break;
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return width;
+		break;
+	/* fall through, same pitch and pointers */
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		/* width of the buffer must be a multiple of 2 */
+		if (width & 1) {
+			b2r2_log_warn(cont->dev, "%s: Illegal width "
+				"for fmt=%#010x width=%d\n", __func__,
+				format, width);
+			return 0;
+		}
+		/*
+		 * return pitch of the Y-buffer.
+		 * U and V pitch can be derived from it.
+		 */
+		return width;
+		break;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		/* width of the buffer must be a multiple of 16. */
+		if (width & 15) {
+			b2r2_log_warn(cont->dev, "%s: Illegal width "
+				"for fmt=%#010x width=%d\n", __func__,
+				format, width);
+			return 0;
+		}
+		/*
+		 * return pitch of the Y-buffer.
+		 * U and V pitch can be derived from it.
+		 */
+		return width;
+		break;
+	default:
+		b2r2_log_warn(cont->dev, "%s: Unable to determine pitch "
+			"for fmt=%#010x width=%d\n", __func__,
+			format, width);
+		return 0;
+	}
+}
+
+static s32 validate_buf(struct b2r2_control *cont,
+		const struct b2r2_blt_img *image,
+		const struct b2r2_resolved_buf *buf)
+{
+	u32 expect_buf_size;
+	u32 pitch;
+
+	if (image->width <= 0 || image->height <= 0) {
+		b2r2_log_warn(cont->dev, "%s: width=%d or height=%d negative"
+				".\n", __func__, image->width, image->height);
+		return -EINVAL;
+	}
+
+	if (image->pitch == 0) {
+		/* autodetect pitch based on format and width */
+		pitch = get_pitch(cont, image->fmt, image->width);
+	} else
+		pitch = image->pitch;
+
+	expect_buf_size = pitch * image->height;
+
+	if (pitch == 0) {
+		b2r2_log_warn(cont->dev, "%s: Unable to detect pitch. "
+			"fmt=%#010x, width=%d\n",
+			__func__,
+			image->fmt, image->width);
+		return -EINVAL;
+	}
+
+	/* format specific adjustments */
+	switch (image->fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+		/*
+		 * Use ceil(height/2) in case buffer height
+		 * is not divisible by 2.
+		 */
+		expect_buf_size +=
+			(pitch >> 1) * ((image->height + 1) >> 1) * 2;
+		break;
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+		expect_buf_size += (pitch >> 1) * image->height * 2;
+		break;
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		expect_buf_size += pitch * image->height * 2;
+		break;
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+		/*
+		 * include space occupied by U and V data.
+		 * U and V interleaved, half resolution, which makes
+		 * the UV pitch equal to luma pitch.
+		 * Use ceil(height/2) in case buffer height
+		 * is not divisible by 2.
+		 */
+		expect_buf_size += pitch * ((image->height + 1) >> 1);
+		break;
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+		/*
+		 * include space occupied by U and V data.
+		 * U and V interleaved, half resolution, which makes
+		 * the UV pitch equal to luma pitch.
+		 */
+		expect_buf_size += pitch * image->height;
+		break;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+		/* Height must be a multiple of 16 for macro-block format.*/
+		if (image->height & 15) {
+			b2r2_log_warn(cont->dev, "%s: Illegal height "
+				"for fmt=%#010x height=%d\n", __func__,
+				image->fmt, image->height);
+			return -EINVAL;
+		}
+		expect_buf_size += pitch * (image->height >> 1);
+		break;
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		/* Height must be a multiple of 16 for macro-block format.*/
+		if (image->height & 15) {
+			b2r2_log_warn(cont->dev, "%s: Illegal height "
+				"for fmt=%#010x height=%d\n", __func__,
+				image->fmt, image->height);
+			return -EINVAL;
+		}
+		expect_buf_size += pitch * image->height;
+		break;
+	default:
+		break;
+	}
+
+	if (buf->file_len < expect_buf_size) {
+		b2r2_log_warn(cont->dev, "%s: Invalid buffer size:\n"
+			"fmt=%#010x w=%d h=%d buf.len=%d expect_buf_size=%d\n",
+			__func__,
+			image->fmt, image->width, image->height, buf->file_len,
+			expect_buf_size);
+		return -EINVAL;
+	}
+
+	if (image->buf.type == B2R2_BLT_PTR_VIRTUAL) {
+		b2r2_log_warn(cont->dev, "%s: Virtual pointers not supported"
+				" yet.\n", __func__);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*
+ * Bit-expand the color from fmt to RGB888 with blue at LSB.
+ * Copy MSBs into missing LSBs.
+ */
+static u32 to_RGB888(struct b2r2_control *cont, u32 color,
+		const enum b2r2_blt_fmt fmt)
+{
+	u32 out_color = 0;
+	u32 r = 0;
+	u32 g = 0;
+	u32 b = 0;
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		r = ((color & 0xf00) << 12) | ((color & 0xf00) << 8);
+		g = ((color & 0xf0) << 8) | ((color & 0xf0) << 4);
+		b = ((color & 0xf) << 4) | (color & 0xf);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		r = ((color & 0x7c00) << 9) | ((color & 0x7000) << 4);
+		g = ((color & 0x3e0) << 6) | ((color & 0x380) << 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		r = ((color & 0xf800) << 8) | ((color & 0xe000) << 3);
+		g = ((color & 0x7e0) << 5) | ((color & 0x600) >> 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+		out_color = color & 0xffffff;
+		break;
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		r = (color & 0xff) << 16;
+		g = color & 0xff00;
+		b = (color & 0xff0000) >> 16;
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		r = ((color & 0xf800) << 8) | ((color & 0xe000) << 3);
+		g = ((color & 0x7e0) << 5) | ((color & 0x600) >> 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	default:
+		break;
+	}
+
+	return out_color;
+}
+
+
+static void setup_fill_input_stage(const struct b2r2_blt_request *req,
+					struct b2r2_node *node,
+					struct b2r2_work_buf *out_buf)
+{
+	enum b2r2_native_fmt fill_fmt = 0;
+	u32 src_color = req->user_req.src_color;
+	const struct b2r2_blt_img *dst_img = &(req->user_req.dst_img);
+	struct b2r2_control *cont = req->instance->control;
+	bool fullrange = (req->user_req.flags &
+		B2R2_BLT_FLAG_FULL_RANGE_YUV) != 0;
+
+	b2r2_log_info(cont->dev, "%s ENTRY\n", __func__);
+
+	/* Determine format in src_color */
+	switch (dst_img->fmt) {
+	/* ARGB formats */
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+		if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_FILL) != 0) {
+			fill_fmt = B2R2_NATIVE_ARGB8888;
+		} else {
+			/* SOURCE_FILL_RAW */
+			fill_fmt = to_native_fmt(cont, dst_img->fmt);
+			if (dst_img->fmt == B2R2_BLT_FMT_32_BIT_ABGR8888) {
+				/*
+				 * Color is read from a register,
+				 * where it is stored in ABGR format.
+				 * Set up IVMX.
+				 */
+				b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_BGR);
+			}
+		}
+		break;
+	/* YUV formats */
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_FILL) != 0) {
+			fill_fmt = B2R2_NATIVE_AYCBCR8888;
+			/*
+			 * Set up IVMX
+			 * The destination format is in fact YUV,
+			 * but the input stage stores the data in
+			 * an intermediate buffer which is RGB.
+			 * Hence the conversion from YUV to RGB.
+			 * Format of the supplied src_color is
+			 * B2R2_BLT_FMT_32_BIT_AYUV8888.
+			 */
+			if (fullrange)
+				b2r2_setup_ivmx(node, B2R2_CC_BLT_YUV888_FULL_TO_RGB);
+			else
+				b2r2_setup_ivmx(node, B2R2_CC_BLT_YUV888_TO_RGB);
+		} else {
+			/* SOURCE_FILL_RAW */
+			bool dst_yuv_planar =
+				B2R2_BLT_FMT_YUV420_PACKED_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV422_PACKED_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YVU420_PACKED_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YVU422_PACKED_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV444_PACKED_PLANAR ==
+					dst_img->fmt;
+
+			bool dst_yuv_semi_planar =
+				B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ==
+					dst_img->fmt ||
+				B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE ==
+					dst_img->fmt;
+
+			if (dst_yuv_planar || dst_yuv_semi_planar) {
+				/*
+				 * SOURCE_FILL_RAW cannot be supported
+				 * with multi-buffer formats.
+				 * Force a legal format to prevent B2R2
+				 * from misbehaving.
+				 */
+				fill_fmt = B2R2_NATIVE_AYCBCR8888;
+			} else {
+				fill_fmt = to_native_fmt(cont, dst_img->fmt);
+			}
+
+			switch (dst_img->fmt) {
+			case B2R2_BLT_FMT_24_BIT_YUV888:
+			case B2R2_BLT_FMT_32_BIT_AYUV8888:
+			case B2R2_BLT_FMT_24_BIT_VUY888:
+			case B2R2_BLT_FMT_32_BIT_VUYA8888:
+				if (fullrange)
+					b2r2_setup_ivmx(node,
+						B2R2_CC_BLT_YUV888_FULL_TO_RGB);
+				else
+					b2r2_setup_ivmx(node,
+						B2R2_CC_BLT_YUV888_TO_RGB);
+				/*
+				 * Re-arrange the color components from
+				 * VUY(A) to (A)YUV
+				 */
+				if (dst_img->fmt ==
+					B2R2_BLT_FMT_24_BIT_VUY888) {
+					u32 Y = src_color & 0xff;
+					u32 U = src_color & 0xff00;
+					u32 V = src_color & 0xff0000;
+					src_color = (Y << 16) | U | (V >> 16);
+				} else if (dst_img->fmt ==
+						B2R2_BLT_FMT_32_BIT_VUYA8888) {
+					u32 A = src_color & 0xff;
+					u32 Y = src_color & 0xff00;
+					u32 U = src_color & 0xff0000;
+					u32 V = src_color & 0xff000000;
+					src_color = (A << 24) |
+							(Y << 8) |
+							(U >> 8) |
+							(V >> 24);
+				}
+				break;
+			case B2R2_BLT_FMT_Y_CB_Y_CR:
+				/*
+				 * Setup input VMX to convert YVU to
+				 * RGB 601 VIDEO
+				 * Chroma components are swapped so
+				 * it is YVU and not YUV.
+				 */
+				if (fullrange)
+					b2r2_setup_ivmx(node,
+						B2R2_CC_YVU_FULL_TO_RGB);
+				else
+					b2r2_setup_ivmx(node, B2R2_CC_YVU_TO_RGB);
+				break;
+			default:
+				/*
+				 * Set up IVMX
+				 * The destination format is in fact YUV,
+				 * but the input stage stores the data in
+				 * an intermediate buffer which is RGB.
+				 * Hence the conversion from YUV to RGB.
+				 */
+				if (fullrange)
+					b2r2_setup_ivmx(node,
+						B2R2_CC_YUV_FULL_TO_RGB);
+				else
+					b2r2_setup_ivmx(node, B2R2_CC_YUV_TO_RGB);
+				break;
+			}
+		}
+		break;
+	default:
+		src_color = 0;
+		fill_fmt = B2R2_NATIVE_ARGB8888;
+		break;
+	}
+
+	node->node.GROUP1.B2R2_TBA = out_buf->phys_addr;
+	node->node.GROUP1.B2R2_TTY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+	/* Set color fill on SRC2 channel */
+	node->node.GROUP4.B2R2_SBA = 0;
+	node->node.GROUP4.B2R2_STY =
+		(0 << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		fill_fmt |
+		get_alpha_range(cont, dst_img->fmt) |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_COLOR_FILL_REGISTER;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_COLOR_FILL;
+	node->node.GROUP2.B2R2_S2CF = src_color;
+
+	node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+	b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+}
+
+static void setup_input_stage(const struct b2r2_blt_request *req,
+			      struct b2r2_node *node,
+			      struct b2r2_work_buf *out_buf)
+{
+	/* Horizontal and vertical scaling factors in 6.10 fixed point format */
+	s32 h_scf = 1 << 10;
+	s32 v_scf = 1 << 10;
+	const struct b2r2_blt_rect *src_rect = &(req->user_req.src_rect);
+	const struct b2r2_blt_rect *dst_rect = &(req->user_req.dst_rect);
+	const struct b2r2_blt_img *src_img = &(req->user_req.src_img);
+	u32 src_pitch = 0;
+	/* horizontal and vertical scan order for out_buf */
+	enum b2r2_ty dst_hso = B2R2_TY_HSO_LEFT_TO_RIGHT;
+	enum b2r2_ty dst_vso = B2R2_TY_VSO_TOP_TO_BOTTOM;
+	u32 endianness = 0;
+	u32 fctl = 0;
+	u32 rsf = 0;
+	u32 rzi = 0;
+	bool yuv_semi_planar =
+		src_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		src_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+
+	bool yuv_planar =
+		src_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		src_img->fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR;
+
+	struct b2r2_filter_spec *hf;
+	struct b2r2_filter_spec *vf;
+
+	bool use_h_filter = false;
+	bool use_v_filter = false;
+
+	struct b2r2_control *cont = req->instance->control;
+	bool fullrange = (req->user_req.flags &
+		B2R2_BLT_FLAG_FULL_RANGE_YUV) != 0;
+
+	b2r2_log_info(cont->dev, "%s ENTRY\n", __func__);
+
+	if (((B2R2_BLT_FLAG_SOURCE_FILL | B2R2_BLT_FLAG_SOURCE_FILL_RAW) &
+			req->user_req.flags) != 0) {
+		setup_fill_input_stage(req, node, out_buf);
+		b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+		return;
+	}
+
+	if (src_img->pitch == 0) {
+		/* Determine pitch based on format and width of the image. */
+		src_pitch = get_pitch(cont, src_img->fmt, src_img->width);
+	} else {
+		src_pitch = src_img->pitch;
+	}
+
+	b2r2_log_info(cont->dev, "%s transform=%#010x\n",
+			__func__, req->user_req.transform);
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		h_scf = (src_rect->width << 10) / dst_rect->height;
+		v_scf = (src_rect->height << 10) / dst_rect->width;
+	} else {
+		h_scf = (src_rect->width << 10) / dst_rect->width;
+		v_scf = (src_rect->height << 10) / dst_rect->height;
+	}
+
+	hf = b2r2_filter_find(h_scf);
+	vf = b2r2_filter_find(v_scf);
+
+	use_h_filter = h_scf != (1 << 10);
+	use_v_filter = v_scf != (1 << 10);
+
+	/* B2R2_BLT_FLAG_BLUR overrides any scaling filter. */
+	if (req->user_req.flags & B2R2_BLT_FLAG_BLUR) {
+		use_h_filter = true;
+		use_v_filter = true;
+		hf = b2r2_filter_blur();
+		vf = b2r2_filter_blur();
+	}
+
+	/* Configure horizontal rescale */
+	if (h_scf != (1 << 10)) {
+		b2r2_log_info(cont->dev, "%s: Scaling horizontally by 0x%.8x"
+			"\ns(%d, %d)->d(%d, %d)\n", __func__,
+			h_scf, src_rect->width, src_rect->height,
+			dst_rect->width, dst_rect->height);
+	}
+	fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER;
+	rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+	rsf |= h_scf << B2R2_RSF_HSRC_INC_SHIFT;
+	rzi |= B2R2_RZI_DEFAULT_HNB_REPEAT;
+
+	/* Configure vertical rescale */
+	if (v_scf != (1 << 10)) {
+		b2r2_log_info(cont->dev, "%s: Scaling vertically by 0x%.8x"
+			"\ns(%d, %d)->d(%d, %d)\n", __func__,
+			v_scf, src_rect->width, src_rect->height,
+			dst_rect->width, dst_rect->height);
+	}
+	fctl |= B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+	rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+	rsf |= v_scf << B2R2_RSF_VSRC_INC_SHIFT;
+	rzi |= 2 << B2R2_RZI_VNB_REPEAT_SHIFT;
+
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_RESCALE2D_ENABLED;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_RESIZE_CHROMA;
+
+	/* Adjustments that depend on the source format */
+	switch (src_img->fmt) {
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_BGR);
+		break;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+		/*
+		 * Setup input VMX to convert YVU to RGB 601 VIDEO
+		 * Chroma components are swapped so
+		 * it is YVU and not YUV.
+		 */
+		if (fullrange)
+			b2r2_setup_ivmx(node, B2R2_CC_YVU_FULL_TO_RGB);
+		else
+			b2r2_setup_ivmx(node, B2R2_CC_YVU_TO_RGB);
+		break;
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		if (fullrange)
+			b2r2_setup_ivmx(node, B2R2_CC_YUV_FULL_TO_RGB);
+		else
+			b2r2_setup_ivmx(node, B2R2_CC_YUV_TO_RGB);
+		break;
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		/*
+		 * Set up IVMX.
+		 * For B2R2_BLT_FMT_32_BIT_YUV888 and
+		 * B2R2_BLT_FMT_32_BIT_AYUV8888
+		 * the color components are laid out in memory as V, U, Y, (A)
+		 * with V at the first byte (due to little endian addressing).
+		 * B2R2 expects them to be as U, Y, V, (A)
+		 * with U at the first byte.
+		 */
+		if (fullrange)
+			b2r2_setup_ivmx(node, B2R2_CC_BLT_YUV888_FULL_TO_RGB);
+		else
+			b2r2_setup_ivmx(node, B2R2_CC_BLT_YUV888_TO_RGB);
+
+		/*
+		 * Re-arrange color components from VUY(A) to (A)YUV
+		 * for input VMX to work on them further.
+		 */
+		if (src_img->fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				src_img->fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			endianness = B2R2_TY_ENDIAN_BIG_NOT_LITTLE;
+		break;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE: {
+		/*
+		 * Luma handled in the same way
+		 * for all YUV multi-buffer formats.
+		 * Set luma rescale registers.
+		 */
+		u32 rsf_luma = 0;
+		u32 rzi_luma = 0;
+
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_RESCALE2D_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_RESIZE_LUMA;
+
+		if (src_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			src_img->fmt ==
+				B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR) {
+			if (fullrange)
+				b2r2_setup_ivmx(node, B2R2_CC_YVU_FULL_TO_RGB);
+			else
+				b2r2_setup_ivmx(node, B2R2_CC_YVU_TO_RGB);
+		} else {
+			if (fullrange)
+				b2r2_setup_ivmx(node, B2R2_CC_YUV_FULL_TO_RGB);
+			else
+				b2r2_setup_ivmx(node, B2R2_CC_YUV_TO_RGB);
+		}
+
+		fctl |= B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_RESIZER |
+			B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_RESIZER;
+
+		if (use_h_filter && hf) {
+			fctl |= B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_FILTER;
+			node->node.GROUP10.B2R2_HFP = hf->h_coeffs_phys_addr;
+		}
+
+		if (use_v_filter && vf) {
+			fctl |= B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_FILTER;
+			node->node.GROUP10.B2R2_VFP = vf->v_coeffs_phys_addr;
+		}
+
+		rsf_luma |= h_scf << B2R2_RSF_HSRC_INC_SHIFT;
+		rzi_luma |= B2R2_RZI_DEFAULT_HNB_REPEAT;
+
+		rsf_luma |= v_scf << B2R2_RSF_VSRC_INC_SHIFT;
+		rzi_luma |= 2 << B2R2_RZI_VNB_REPEAT_SHIFT;
+
+		node->node.GROUP10.B2R2_RSF = rsf_luma;
+		node->node.GROUP10.B2R2_RZI = rzi_luma;
+
+		switch (src_img->fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chrominance is always half the luminance size
+			 * so chrominance resizer is always active.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (h_scf >> 1) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (v_scf >> 1) << B2R2_RSF_VSRC_INC_SHIFT;
+			/* Select suitable filter for chroma */
+			hf = b2r2_filter_find(h_scf >> 1);
+			vf = b2r2_filter_find(v_scf >> 1);
+			use_h_filter = true;
+			use_v_filter = true;
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chrominance is always half the luminance size
+			 * only in horizontal direction.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (h_scf >> 1) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= v_scf << B2R2_RSF_VSRC_INC_SHIFT;
+			/* Select suitable filter for chroma */
+			hf = b2r2_filter_find(h_scf >> 1);
+			use_h_filter = true;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			/* Chrominance is the same size as luminance.*/
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= h_scf << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= v_scf << B2R2_RSF_VSRC_INC_SHIFT;
+			/* Select suitable filter for chroma */
+			hf = b2r2_filter_find(h_scf);
+			vf = b2r2_filter_find(v_scf);
+			use_h_filter = true;
+			use_v_filter = true;
+			break;
+		default:
+			break;
+		}
+		break;
+	}
+	default:
+		break;
+	}
+
+	/*
+	 * Set the filter control and rescale registers.
+	 * GROUP9 registers are used for all single-buffer formats
+	 * or for chroma in case of multi-buffer YUV formats.
+	 * h/v_filter is now appropriately selected for chroma scaling,
+	 * be it YUV multi-buffer, or single-buffer raster format.
+	 * B2R2_BLT_FLAG_BLUR overrides any scaling filter.
+	 */
+	if (req->user_req.flags & B2R2_BLT_FLAG_BLUR) {
+		use_h_filter = true;
+		use_v_filter = true;
+		hf = b2r2_filter_blur();
+		vf = b2r2_filter_blur();
+	}
+
+	if (use_h_filter && hf) {
+		fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER;
+		node->node.GROUP9.B2R2_HFP = hf->h_coeffs_phys_addr;
+	}
+
+	if (use_v_filter && vf) {
+		fctl |= B2R2_FCTL_VF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER;
+		node->node.GROUP9.B2R2_VFP = vf->v_coeffs_phys_addr;
+	}
+
+	node->node.GROUP8.B2R2_FCTL |= fctl;
+	node->node.GROUP9.B2R2_RSF |= rsf;
+	node->node.GROUP9.B2R2_RZI |= rzi;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_FILTER_CONTROL;
+
+	/*
+	 * Flip transform is done before potential rotation.
+	 * This can be achieved with appropriate scan order.
+	 * Transform stage will only do rotation.
+	 */
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_H)
+		dst_hso = B2R2_TY_HSO_RIGHT_TO_LEFT;
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_V)
+		dst_vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+
+	/* Set target buffer */
+	node->node.GROUP1.B2R2_TBA = out_buf->phys_addr;
+	node->node.GROUP1.B2R2_TTY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		dst_hso | dst_vso;
+
+	if (yuv_planar) {
+		/*
+		 * Set up chrominance buffers on source 1 and 2,
+		 * luminance on source 3.
+		 * src_pitch and physical_address apply to luminance,
+		 * corresponding chrominance values have to be derived.
+		 */
+		u32 cb_addr = 0;
+		u32 cr_addr = 0;
+		u32 chroma_pitch = 0;
+		bool swapped_chroma =
+			src_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			src_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR;
+		enum b2r2_native_fmt src_fmt =
+			to_native_fmt(cont, src_img->fmt);
+
+		if (swapped_chroma)
+			cr_addr = req->src_resolved.physical_address +
+				src_pitch * src_img->height;
+		else
+			cb_addr = req->src_resolved.physical_address +
+				src_pitch * src_img->height;
+
+		switch (src_img->fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+			chroma_pitch = src_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					(src_img->height >> 1);
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					(src_img->height >> 1);
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+			chroma_pitch = src_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					src_img->height;
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					src_img->height;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			/* Chrominance has full resolution, same as luminance.*/
+			chroma_pitch = src_pitch;
+			cr_addr =
+				cb_addr + chroma_pitch * src_img->height;
+			break;
+		default:
+			break;
+		}
+
+		node->node.GROUP3.B2R2_SBA = cr_addr;
+		node->node.GROUP3.B2R2_STY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			src_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP4.B2R2_SBA = cb_addr;
+		node->node.GROUP4.B2R2_STY = node->node.GROUP3.B2R2_STY;
+
+		node->node.GROUP5.B2R2_SBA = req->src_resolved.physical_address;
+		node->node.GROUP5.B2R2_STY =
+			(src_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			src_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_1_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_1 |
+			B2R2_CIC_SOURCE_2 |
+			B2R2_CIC_SOURCE_3;
+	} else if (yuv_semi_planar) {
+		/*
+		 * Set up chrominance buffer on source 2, luminance on source 3.
+		 * src_pitch and physical_address apply to luminance,
+		 * corresponding chrominance values have to be derived.
+		 * U and V are interleaved at half the luminance resolution,
+		 * which makes the pitch of the UV plane equal
+		 * to luminance pitch.
+		 */
+		u32 chroma_addr = req->src_resolved.physical_address +
+			src_pitch * src_img->height;
+		u32 chroma_pitch = src_pitch;
+
+		enum b2r2_native_fmt src_fmt =
+			to_native_fmt(cont, src_img->fmt);
+
+		node->node.GROUP4.B2R2_SBA = chroma_addr;
+		node->node.GROUP4.B2R2_STY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			src_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP5.B2R2_SBA = req->src_resolved.physical_address;
+		node->node.GROUP5.B2R2_STY =
+			(src_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			src_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_2 | B2R2_CIC_SOURCE_3;
+	} else {
+		/* single buffer format */
+		node->node.GROUP4.B2R2_SBA = req->src_resolved.physical_address;
+		node->node.GROUP4.B2R2_STY =
+			(src_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			to_native_fmt(cont, src_img->fmt) |
+			get_alpha_range(cont, src_img->fmt) |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			endianness;
+
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+	}
+
+	if ((req->user_req.flags &
+			B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) != 0) {
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_CLUTOP_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_CLUT;
+		node->node.GROUP7.B2R2_CCO = B2R2_CCO_CLUT_COLOR_CORRECTION |
+			B2R2_CCO_CLUT_UPDATE;
+		node->node.GROUP7.B2R2_CML = req->clut_phys_addr;
+	}
+
+	node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+
+	b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+}
+
+static void setup_transform_stage(const struct b2r2_blt_request *req,
+				  struct b2r2_node *node,
+				  struct b2r2_work_buf *out_buf,
+				  struct b2r2_work_buf *in_buf)
+{
+	/* vertical scan order for out_buf */
+	enum b2r2_ty dst_vso = B2R2_TY_VSO_TOP_TO_BOTTOM;
+	enum b2r2_blt_transform transform = req->user_req.transform;
+#ifdef CONFIG_B2R2_DEBUG
+	struct b2r2_control *cont = req->instance->control;
+#endif
+
+	b2r2_log_info(cont->dev, "%s ENTRY\n", __func__);
+
+	if (transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		/*
+		 * Scan order must be flipped otherwise contents will
+		 * be mirrored vertically. Leftmost column of in_buf
+		 * would become top instead of bottom row of out_buf.
+		 */
+		dst_vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_ROTATION_ENABLED;
+	}
+
+	/* Set target buffer */
+	node->node.GROUP1.B2R2_TBA = out_buf->phys_addr;
+	node->node.GROUP1.B2R2_TTY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT | dst_vso;
+
+	/* Set source buffer on SRC2 channel */
+	node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+	node->node.GROUP4.B2R2_STY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+	node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+
+	b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+}
+
+/*
+static void setup_mask_stage(const struct b2r2_blt_request req,
+			     struct b2r2_node *node,
+			     struct b2r2_work_buf *out_buf,
+			     struct b2r2_work_buf *in_buf);
+*/
+
+static void setup_dst_read_stage(const struct b2r2_blt_request *req,
+				 struct b2r2_node *node,
+				 struct b2r2_work_buf *out_buf)
+{
+	const struct b2r2_blt_img *dst_img = &(req->user_req.dst_img);
+	u32 fctl = 0;
+	u32 rsf = 0;
+	u32 endianness = 0;
+	bool yuv_semi_planar =
+		dst_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+
+	bool yuv_planar =
+		dst_img->fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		dst_img->fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR;
+
+	u32 dst_pitch = 0;
+	struct b2r2_control *cont = req->instance->control;
+	bool fullrange = (req->user_req.flags &
+		B2R2_BLT_FLAG_FULL_RANGE_YUV) != 0;
+
+	if (dst_img->pitch == 0) {
+		/* Determine pitch based on format and width of the image. */
+		dst_pitch = get_pitch(cont, dst_img->fmt, dst_img->width);
+	} else {
+		dst_pitch = dst_img->pitch;
+	}
+
+	b2r2_log_info(cont->dev, "%s ENTRY\n", __func__);
+
+	/* Adjustments that depend on the destination format */
+	switch (dst_img->fmt) {
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_BGR);
+		break;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+		/*
+		 * Setup input VMX to convert YVU to RGB 601 VIDEO
+		 * Chroma components are swapped
+		 * so it is YVU and not YUV.
+		 */
+		if (fullrange)
+			b2r2_setup_ivmx(node, B2R2_CC_YVU_FULL_TO_RGB);
+		else
+			b2r2_setup_ivmx(node, B2R2_CC_YVU_TO_RGB);
+		break;
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		if (fullrange)
+			b2r2_setup_ivmx(node, B2R2_CC_YUV_FULL_TO_RGB);
+		else
+			b2r2_setup_ivmx(node, B2R2_CC_YUV_TO_RGB);
+		break;
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		/*
+		 * Set up IVMX.
+		 * For B2R2_BLT_FMT_32_BIT_YUV888 and
+		 * B2R2_BLT_FMT_32_BIT_AYUV8888
+		 * the color components are laid out in memory as V, U, Y, (A)
+		 * with V at the first byte (due to little endian addressing).
+		 * B2R2 expects them to be as U, Y, V, (A)
+		 * with U at the first byte.
+		 */
+		if (fullrange)
+			b2r2_setup_ivmx(node, B2R2_CC_BLT_YUV888_FULL_TO_RGB);
+		else
+			b2r2_setup_ivmx(node, B2R2_CC_BLT_YUV888_TO_RGB);
+
+		/*
+		 * Re-arrange color components from VUY(A) to (A)YUV
+		 * for input VMX to work on them further.
+		 */
+		if (dst_img->fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				dst_img->fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			endianness = B2R2_TY_ENDIAN_BIG_NOT_LITTLE;
+		break;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE: {
+		if (dst_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			dst_img->fmt ==
+				B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR) {
+			if (fullrange)
+				b2r2_setup_ivmx(node, B2R2_CC_YVU_FULL_TO_RGB);
+			else
+				b2r2_setup_ivmx(node, B2R2_CC_YVU_TO_RGB);
+		} else {
+			if (fullrange)
+				b2r2_setup_ivmx(node, B2R2_CC_YUV_FULL_TO_RGB);
+			else
+				b2r2_setup_ivmx(node, B2R2_CC_YUV_TO_RGB);
+		}
+
+		switch (dst_img->fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chrominance is always half the luminance size
+			 * so chrominance resizer is always active.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (1 << 9) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 9) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chrominance is always half the luminance size
+			 * only in horizontal direction.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (1 << 9) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			/* Chrominance is the same size as luminance.*/
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		default:
+			break;
+		}
+		/* Set the filter control and rescale registers for chroma */
+		node->node.GROUP8.B2R2_FCTL |= fctl;
+		node->node.GROUP9.B2R2_RSF |= rsf;
+		node->node.GROUP9.B2R2_RZI =
+			B2R2_RZI_DEFAULT_HNB_REPEAT |
+			(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_RESCALE2D_ENABLED;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_FILTER_CONTROL | B2R2_CIC_RESIZE_CHROMA;
+		break;
+	}
+	default:
+		break;
+	}
+
+	/* Set target buffer */
+	node->node.GROUP1.B2R2_TBA = out_buf->phys_addr;
+	node->node.GROUP1.B2R2_TTY =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	if (yuv_planar) {
+		/*
+		 * Set up chrominance buffers on source 1 and 2,
+		 * luminance on source 3.
+		 * dst_pitch and physical_address apply to luminance,
+		 * corresponding chrominance values have to be derived.
+		 */
+		u32 cb_addr = 0;
+		u32 cr_addr = 0;
+		u32 chroma_pitch = 0;
+		bool swapped_chroma =
+			dst_img->fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			dst_img->fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR;
+		enum b2r2_native_fmt dst_native_fmt =
+				to_native_fmt(cont, dst_img->fmt);
+
+		if (swapped_chroma)
+			cr_addr = req->dst_resolved.physical_address +
+				dst_pitch * dst_img->height;
+		else
+			cb_addr = req->dst_resolved.physical_address +
+				dst_pitch * dst_img->height;
+
+		switch (dst_img->fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+			chroma_pitch = dst_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					(dst_img->height >> 1);
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					(dst_img->height >> 1);
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+			chroma_pitch = dst_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					dst_img->height;
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					dst_img->height;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			chroma_pitch = dst_pitch;
+			cr_addr =
+				cb_addr + chroma_pitch * dst_img->height;
+			break;
+		default:
+			break;
+		}
+
+		node->node.GROUP3.B2R2_SBA = cr_addr;
+		node->node.GROUP3.B2R2_STY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP4.B2R2_SBA = cb_addr;
+		node->node.GROUP4.B2R2_STY = node->node.GROUP3.B2R2_STY;
+
+		node->node.GROUP5.B2R2_SBA = req->dst_resolved.physical_address;
+		node->node.GROUP5.B2R2_STY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_1_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_1 |
+			B2R2_CIC_SOURCE_2 |
+			B2R2_CIC_SOURCE_3;
+	} else if (yuv_semi_planar) {
+		/*
+		 * Set up chrominance buffer on source 2, luminance on source 3.
+		 * dst_pitch and physical_address apply to luminance,
+		 * corresponding chrominance values have to be derived.
+		 * U and V are interleaved at half the luminance resolution,
+		 * which makes the pitch of the UV plane equal
+		 * to luminance pitch.
+		 */
+		u32 chroma_addr = req->dst_resolved.physical_address +
+			dst_pitch * dst_img->height;
+		u32 chroma_pitch = dst_pitch;
+
+		enum b2r2_native_fmt dst_native_fmt =
+				to_native_fmt(cont, dst_img->fmt);
+
+		node->node.GROUP4.B2R2_SBA = chroma_addr;
+		node->node.GROUP4.B2R2_STY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP5.B2R2_SBA = req->dst_resolved.physical_address;
+		node->node.GROUP5.B2R2_STY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_2 | B2R2_CIC_SOURCE_3;
+	} else {
+		/* single buffer format */
+		node->node.GROUP4.B2R2_SBA = req->dst_resolved.physical_address;
+		node->node.GROUP4.B2R2_STY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			to_native_fmt(cont, dst_img->fmt) |
+			get_alpha_range(cont, dst_img->fmt) |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			endianness;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+	}
+
+	node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+
+	b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+}
+
+static void setup_blend_stage(const struct b2r2_blt_request *req,
+			      struct b2r2_node *node,
+			      struct b2r2_work_buf *bg_buf,
+			      struct b2r2_work_buf *fg_buf)
+{
+	u32 global_alpha = req->user_req.global_alpha;
+#ifdef CONFIG_B2R2_DEBUG
+	struct b2r2_control *cont = req->instance->control;
+#endif
+
+	b2r2_log_info(cont->dev, "%s ENTRY\n", __func__);
+
+	node->node.GROUP0.B2R2_ACK = 0;
+
+	if (req->user_req.flags &
+			(B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND |
+			B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND)) {
+		/* Some kind of blending needs to be done. */
+		if (req->user_req.flags & B2R2_BLT_FLAG_SRC_IS_NOT_PREMULT)
+			node->node.GROUP0.B2R2_ACK |=
+				B2R2_ACK_MODE_BLEND_NOT_PREMULT;
+		else
+			node->node.GROUP0.B2R2_ACK |=
+				B2R2_ACK_MODE_BLEND_PREMULT;
+
+		/*
+		 * global_alpha register accepts 0..128 range,
+		 * global_alpha in the request is 0..255, remap needed.
+		 */
+		if (req->user_req.flags & B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND) {
+			if (global_alpha == 255)
+				global_alpha = 128;
+			else
+				global_alpha >>= 1;
+		} else {
+			/*
+			 * Use solid global_alpha
+			 * if global alpha blending is not set.
+			 */
+			global_alpha = 128;
+		}
+
+		node->node.GROUP0.B2R2_ACK |=
+			global_alpha << (B2R2_ACK_GALPHA_ROPID_SHIFT);
+
+		/* Set background on SRC1 channel */
+		node->node.GROUP3.B2R2_SBA = bg_buf->phys_addr;
+		node->node.GROUP3.B2R2_STY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		/* Set foreground on SRC2 channel */
+		node->node.GROUP4.B2R2_SBA = fg_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		/* Set target buffer */
+		node->node.GROUP1.B2R2_TBA = bg_buf->phys_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_1_FETCH_FROM_MEM |
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_1 |
+			B2R2_CIC_SOURCE_2;
+	} else {
+		/*
+		 * No blending, foreground goes on SRC2. No global alpha.
+		 * EMACSOC TODO: The blending stage should be skipped altogether
+		 * if no blending is to be done. Probably could go directly from
+		 * transform to writeback.
+		 */
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+
+		node->node.GROUP4.B2R2_SBA = fg_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+		node->node.GROUP1.B2R2_TBA = bg_buf->phys_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(B2R2_GENERIC_WORK_BUF_PITCH <<
+				B2R2_TY_BITMAP_PITCH_SHIFT) |
+			B2R2_GENERIC_WORK_BUF_FMT |
+			B2R2_TY_ALPHA_RANGE_255 |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM;
+	}
+
+	b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+}
+
+static void setup_writeback_stage(const struct b2r2_blt_request *req,
+				  struct b2r2_node *node,
+				  struct b2r2_work_buf *in_buf)
+{
+	const struct b2r2_blt_img *dst_img = &(req->user_req.dst_img);
+	const enum b2r2_blt_fmt dst_fmt = dst_img->fmt;
+	const bool yuv_planar_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR;
+
+	const bool yuv_semi_planar_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+
+	const u32 group4_b2r2_sty =
+		(B2R2_GENERIC_WORK_BUF_PITCH << B2R2_TY_BITMAP_PITCH_SHIFT) |
+		B2R2_GENERIC_WORK_BUF_FMT |
+		B2R2_TY_ALPHA_RANGE_255 |
+		B2R2_TY_HSO_LEFT_TO_RIGHT |
+		B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	u32 dst_dither = 0;
+	u32 dst_pitch = 0;
+	u32 endianness = 0;
+
+	struct b2r2_control *cont = req->instance->control;
+	bool fullrange = (req->user_req.flags &
+		B2R2_BLT_FLAG_FULL_RANGE_YUV) != 0;
+
+	b2r2_log_info(cont->dev, "%s ENTRY\n", __func__);
+
+	if (dst_img->pitch == 0) {
+		/* Determine pitch based on format and width of the image. */
+		dst_pitch = get_pitch(cont, dst_img->fmt, dst_img->width);
+	} else
+		dst_pitch = dst_img->pitch;
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_DITHER) != 0)
+		dst_dither = B2R2_TTY_RGB_ROUND_DITHER;
+
+	/* Set target buffer(s) */
+	if (yuv_planar_dst) {
+		/*
+		 * three nodes required to write the output.
+		 * Luma, blue chroma and red chroma.
+		 */
+		u32 fctl = 0;
+		u32 rsf = 0;
+		const u32 group0_b2r2_ins =
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_RECT_CLIP_ENABLED;
+		const u32 group0_b2r2_cic =
+			B2R2_CIC_SOURCE_2 |
+			B2R2_CIC_CLIP_WINDOW;
+
+		u32 cb_addr = 0;
+		u32 cr_addr = 0;
+		u32 chroma_pitch = 0;
+		bool swapped_chroma =
+			dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR;
+		enum b2r2_native_fmt dst_native_fmt =
+				to_native_fmt(cont, dst_img->fmt);
+		enum b2r2_ty alpha_range = get_alpha_range(cont, dst_img->fmt);
+
+		if (swapped_chroma)
+			cr_addr = req->dst_resolved.physical_address +
+				dst_pitch * dst_img->height;
+		else
+			cb_addr = req->dst_resolved.physical_address +
+				dst_pitch * dst_img->height;
+
+		switch (dst_fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+			chroma_pitch = dst_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					(dst_img->height >> 1);
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					(dst_img->height >> 1);
+			/*
+			 * Chrominance is always half the luminance size
+			 * so chrominance resizer is always active.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+					B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+			chroma_pitch = dst_pitch >> 1;
+			if (swapped_chroma)
+				cb_addr = cr_addr + chroma_pitch *
+					dst_img->height;
+			else
+				cr_addr = cb_addr + chroma_pitch *
+					dst_img->height;
+			/*
+			 * YUV422 or YVU422
+			 * Chrominance is always half the luminance size
+			 * only in horizontal direction.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			chroma_pitch = dst_pitch;
+			cr_addr =
+				cb_addr + chroma_pitch * dst_img->height;
+			/*
+			 * No scaling required since
+			 * chrominance is not subsampled.
+			 */
+		default:
+			break;
+		}
+
+		/* Luma (Y-component) */
+		node->node.GROUP1.B2R2_TBA = req->dst_resolved.physical_address;
+		node->node.GROUP1.B2R2_TTY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither;
+
+		if (fullrange)
+			b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV_FULL);
+		else
+			b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV);
+
+		 /* bypass ALU, no blending here. Handled in its own stage. */
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS = group0_b2r2_ins;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic;
+
+		/* Set source buffer on SRC2 channel */
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+
+		/* Blue chroma (U-component)*/
+		node = node->next;
+		node->node.GROUP1.B2R2_TBA = cb_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither |
+			B2R2_TTY_CHROMA_NOT_LUMA;
+
+		if (fullrange)
+			b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV_FULL);
+		else
+			b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV);
+
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS = group0_b2r2_ins;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic;
+		if (dst_fmt != B2R2_BLT_FMT_YUV444_PACKED_PLANAR) {
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP0.B2R2_CIC |=
+				B2R2_CIC_FILTER_CONTROL |
+				B2R2_CIC_RESIZE_CHROMA;
+			/* Set the filter control and rescale registers */
+			node->node.GROUP8.B2R2_FCTL = fctl;
+			node->node.GROUP9.B2R2_RSF = rsf;
+			node->node.GROUP9.B2R2_RZI =
+				B2R2_RZI_DEFAULT_HNB_REPEAT |
+				(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+		}
+
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+
+
+		/*
+		 * Red chroma (V-component)
+		 * The flag B2R2_TTY_CB_NOT_CR actually works
+		 * the other way around, i.e. as if it was
+		 * CR_NOT_CB.
+		 */
+		node = node->next;
+		node->node.GROUP1.B2R2_TBA = cr_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TTY_CB_NOT_CR |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither |
+			B2R2_TTY_CHROMA_NOT_LUMA;
+
+		if (fullrange)
+			b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV_FULL);
+		else
+			b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV);
+
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS = group0_b2r2_ins;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic;
+		if (dst_fmt != B2R2_BLT_FMT_YUV444_PACKED_PLANAR) {
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP0.B2R2_CIC |=
+				B2R2_CIC_FILTER_CONTROL |
+				B2R2_CIC_RESIZE_CHROMA;
+			/* Set the filter control and rescale registers */
+			node->node.GROUP8.B2R2_FCTL = fctl;
+			node->node.GROUP9.B2R2_RSF = rsf;
+			node->node.GROUP9.B2R2_RZI =
+				B2R2_RZI_DEFAULT_HNB_REPEAT |
+				(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+		}
+
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+	} else if (yuv_semi_planar_dst) {
+		/*
+		 * two nodes required to write the output.
+		 * One node for luma and one for interleaved chroma
+		 * components.
+		 */
+		u32 fctl = 0;
+		u32 rsf = 0;
+		const u32 group0_b2r2_ins =
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_RECT_CLIP_ENABLED;
+		const u32 group0_b2r2_cic =
+			B2R2_CIC_SOURCE_2 |
+			B2R2_CIC_CLIP_WINDOW;
+
+		u32 chroma_addr = req->dst_resolved.physical_address +
+			dst_pitch * dst_img->height;
+		u32 chroma_pitch = dst_pitch;
+		enum b2r2_native_fmt dst_native_fmt =
+				to_native_fmt(cont, dst_img->fmt);
+		enum b2r2_ty alpha_range = get_alpha_range(cont, dst_img->fmt);
+
+		if (dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+			dst_fmt ==
+				B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+			dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR) {
+			/*
+			 * Chrominance is always half the luminance size
+			 * so chrominance resizer is always active.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+					B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+		} else {
+			/*
+			 * YUV422
+			 * Chrominance is always half the luminance size
+			 * only in horizontal direction.
+			 */
+			fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER;
+
+			rsf &= ~(0xffff << B2R2_RSF_HSRC_INC_SHIFT);
+			rsf |= (2 << 10) << B2R2_RSF_HSRC_INC_SHIFT;
+			rsf &= ~(0xffff << B2R2_RSF_VSRC_INC_SHIFT);
+			rsf |= (1 << 10) << B2R2_RSF_VSRC_INC_SHIFT;
+		}
+
+		/* Luma (Y-component) */
+		node->node.GROUP1.B2R2_TBA = req->dst_resolved.physical_address;
+		node->node.GROUP1.B2R2_TTY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither;
+
+		if (dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR) {
+			if (fullrange)
+				b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YVU_FULL);
+			else
+				b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YVU);
+		} else {
+			if (fullrange)
+				b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV_FULL);
+			else
+				b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV);
+		}
+
+		 /* bypass ALU, no blending here. Handled in its own stage. */
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS = group0_b2r2_ins;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic;
+
+		/* Set source buffer on SRC2 channel */
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+
+		/* Chroma (UV-components)*/
+		node = node->next;
+		node->node.GROUP1.B2R2_TBA = chroma_addr;
+		node->node.GROUP1.B2R2_TTY =
+			(chroma_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			dst_native_fmt | alpha_range |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither |
+			B2R2_TTY_CHROMA_NOT_LUMA;
+
+		if (dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR) {
+			if (fullrange)
+				b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YVU_FULL);
+			else
+				b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YVU);
+		} else {
+			if (fullrange)
+				b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV_FULL);
+			else
+				b2r2_setup_ivmx(node, B2R2_CC_RGB_TO_YUV);
+		}
+
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS =
+			group0_b2r2_ins | B2R2_INS_RESCALE2D_ENABLED;
+		node->node.GROUP0.B2R2_CIC |= group0_b2r2_cic |
+			B2R2_CIC_FILTER_CONTROL |
+			B2R2_CIC_RESIZE_CHROMA;
+
+		/* Set the filter control and rescale registers */
+		node->node.GROUP8.B2R2_FCTL = fctl;
+		node->node.GROUP9.B2R2_RSF = rsf;
+		node->node.GROUP9.B2R2_RZI =
+			B2R2_RZI_DEFAULT_HNB_REPEAT |
+			(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+	} else {
+		/* single buffer target */
+
+		switch (dst_fmt) {
+		case B2R2_BLT_FMT_32_BIT_ABGR8888:
+			b2r2_setup_ovmx(node, B2R2_CC_RGB_TO_BGR);
+			break;
+		case B2R2_BLT_FMT_Y_CB_Y_CR:
+			if (fullrange)
+				b2r2_setup_ovmx(node, B2R2_CC_RGB_TO_YVU_FULL);
+			else
+				b2r2_setup_ovmx(node, B2R2_CC_RGB_TO_YVU);
+			break;
+		case B2R2_BLT_FMT_24_BIT_YUV888: /* fall through */
+		case B2R2_BLT_FMT_32_BIT_AYUV8888: /* fall through */
+		case B2R2_BLT_FMT_24_BIT_VUY888: /* fall through */
+		case B2R2_BLT_FMT_32_BIT_VUYA8888:
+			if (fullrange)
+				b2r2_setup_ovmx(node, B2R2_CC_RGB_TO_BLT_YUV888_FULL);
+			else
+				b2r2_setup_ovmx(node, B2R2_CC_RGB_TO_BLT_YUV888);
+			/*
+			 * Re-arrange color components from (A)YUV to VUY(A)
+			 * when bytes are stored in memory.
+			 */
+			if (dst_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+					dst_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+				endianness = B2R2_TY_ENDIAN_BIG_NOT_LITTLE;
+			break;
+		default:
+			break;
+		}
+
+		node->node.GROUP1.B2R2_TBA = req->dst_resolved.physical_address;
+		node->node.GROUP1.B2R2_TTY =
+			(dst_pitch << B2R2_TY_BITMAP_PITCH_SHIFT) |
+			to_native_fmt(cont, dst_img->fmt) |
+			get_alpha_range(cont, dst_img->fmt) |
+			B2R2_TY_HSO_LEFT_TO_RIGHT |
+			B2R2_TY_VSO_TOP_TO_BOTTOM |
+			dst_dither |
+			endianness;
+
+		node->node.GROUP0.B2R2_ACK = B2R2_ACK_MODE_BYPASS_S2_S3;
+		node->node.GROUP0.B2R2_INS |=
+			B2R2_INS_SOURCE_2_FETCH_FROM_MEM |
+			B2R2_INS_RECT_CLIP_ENABLED;
+		node->node.GROUP0.B2R2_CIC |=
+			B2R2_CIC_SOURCE_2 | B2R2_CIC_CLIP_WINDOW;
+
+		if (req->user_req.flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY) {
+			u32 key_color = 0;
+
+			node->node.GROUP0.B2R2_ACK |=
+				B2R2_ACK_CKEY_SEL_SRC_AFTER_CLUT |
+				B2R2_ACK_CKEY_RED_MATCH_IF_BETWEEN |
+				B2R2_ACK_CKEY_GREEN_MATCH_IF_BETWEEN |
+				B2R2_ACK_CKEY_BLUE_MATCH_IF_BETWEEN;
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_CKEY_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_COLOR_KEY;
+
+			key_color = to_RGB888(cont, req->user_req.src_color,
+				req->user_req.src_img.fmt);
+			node->node.GROUP12.B2R2_KEY1 = key_color;
+			node->node.GROUP12.B2R2_KEY2 = key_color;
+		}
+
+		/* Set source buffer on SRC2 channel */
+		node->node.GROUP4.B2R2_SBA = in_buf->phys_addr;
+		node->node.GROUP4.B2R2_STY = group4_b2r2_sty;
+	}
+	/*
+	 * Writeback is the last stage. Terminate the program chain
+	 * to prevent out-of-control B2R2 execution.
+	 */
+	node->node.GROUP0.B2R2_NIP = 0;
+
+	b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+}
+
+/*
+ * Public functions
+ */
+void b2r2_generic_init(struct b2r2_control *cont)
+{
+
+}
+
+void b2r2_generic_exit(struct b2r2_control *cont)
+{
+
+}
+
+int b2r2_generic_analyze(const struct b2r2_blt_request *req,
+			 s32 *work_buf_width,
+			 s32 *work_buf_height,
+			 u32 *work_buf_count,
+			 u32 *node_count)
+{
+	/*
+	 * Need at least 4 nodes, read or fill input, read dst, blend
+	 * and write back the result */
+	u32 n_nodes = 4;
+	/* Need at least 2 bufs, 1 for blend output and 1 for input */
+	u32 n_work_bufs = 2;
+	/* Horizontal and vertical scaling factors in 6.10 fixed point format */
+	s32 h_scf = 1 << 10;
+	s32 v_scf = 1 << 10;
+	enum b2r2_blt_fmt dst_fmt = 0;
+	bool is_src_fill = false;
+	bool yuv_planar_dst;
+	bool yuv_semi_planar_dst;
+	struct b2r2_blt_rect src_rect;
+	struct b2r2_blt_rect dst_rect;
+	struct b2r2_control *cont = req->instance->control;
+
+	if (req == NULL || work_buf_width == NULL || work_buf_height == NULL ||
+			work_buf_count == NULL || node_count == NULL) {
+		b2r2_log_warn(cont->dev, "%s: Invalid in or out pointers:\n"
+			"req=0x%p\n"
+			"work_buf_width=0x%p work_buf_height=0x%p "
+			"work_buf_count=0x%p\n"
+			"node_count=0x%p.\n",
+			__func__,
+			req,
+			work_buf_width, work_buf_height,
+			work_buf_count,
+			node_count);
+		return -EINVAL;
+	}
+
+	dst_fmt = req->user_req.dst_img.fmt;
+
+	is_src_fill = (req->user_req.flags &
+				(B2R2_BLT_FLAG_SOURCE_FILL |
+				B2R2_BLT_FLAG_SOURCE_FILL_RAW)) != 0;
+
+	yuv_planar_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR;
+	yuv_semi_planar_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+
+	*node_count = 0;
+	*work_buf_width = 0;
+	*work_buf_height = 0;
+	*work_buf_count = 0;
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		n_nodes++;
+		n_work_bufs++;
+	}
+
+	if ((yuv_planar_dst || yuv_semi_planar_dst) &&
+			(req->user_req.flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW)) {
+		b2r2_log_warn(cont->dev,
+			"%s: Invalid combination: source_fill_raw"
+			" and multi-buffer destination.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY) != 0 &&
+			(req->user_req.flags & B2R2_BLT_FLAG_DEST_COLOR_KEY)) {
+		b2r2_log_warn(cont->dev,
+			"%s: Invalid combination: source and "
+			"destination color keying.\n", __func__);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags &
+			(B2R2_BLT_FLAG_SOURCE_FILL |
+			B2R2_BLT_FLAG_SOURCE_FILL_RAW)) &&
+			(req->user_req.flags &
+			(B2R2_BLT_FLAG_SOURCE_COLOR_KEY |
+			B2R2_BLT_FLAG_DEST_COLOR_KEY))) {
+		b2r2_log_warn(cont->dev, "%s: Invalid combination: "
+			"source_fill and color keying.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags &
+			(B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND |
+			B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND)) &&
+			(req->user_req.flags &
+			(B2R2_BLT_FLAG_DEST_COLOR_KEY |
+			B2R2_BLT_FLAG_SOURCE_COLOR_KEY))) {
+		b2r2_log_warn(cont->dev, "%s: Invalid combination: "
+			"blending and color keying.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_MASK) &&
+			(req->user_req.flags &
+			(B2R2_BLT_FLAG_DEST_COLOR_KEY |
+			B2R2_BLT_FLAG_SOURCE_COLOR_KEY))) {
+		b2r2_log_warn(cont->dev, "%s: Invalid combination: source mask"
+				"and color keying.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (req->user_req.flags &
+			(B2R2_BLT_FLAG_DEST_COLOR_KEY |
+			B2R2_BLT_FLAG_SOURCE_MASK)) {
+		b2r2_log_warn(cont->dev, "%s: Unsupported: source mask, "
+			"destination color keying.\n",
+			__func__);
+		return -ENOSYS;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_SOURCE_MASK)) {
+		enum b2r2_blt_fmt src_fmt = req->user_req.src_img.fmt;
+		bool yuv_src =
+			src_fmt == B2R2_BLT_FMT_Y_CB_Y_CR ||
+			src_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			src_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+			src_fmt ==
+				B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+			src_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+		if (yuv_src || src_fmt == B2R2_BLT_FMT_1_BIT_A1 ||
+				src_fmt == B2R2_BLT_FMT_8_BIT_A8) {
+			b2r2_log_warn(cont->dev, "%s: Unsupported: source "
+				"color keying with YUV or pure alpha "
+				"formats.\n", __func__);
+			return -ENOSYS;
+		}
+	}
+
+	/* Check for invalid dimensions that would hinder scale calculations */
+	src_rect = req->user_req.src_rect;
+	dst_rect = req->user_req.dst_rect;
+	/* Check for invalid src_rect unless src_fill is enabled */
+	if (!is_src_fill && (src_rect.x < 0 || src_rect.y < 0 ||
+		src_rect.x + src_rect.width > req->user_req.src_img.width ||
+		src_rect.y + src_rect.height > req->user_req.src_img.height)) {
+		b2r2_log_warn(cont->dev, "%s: src_rect outside src_img:\n"
+			"src(x,y,w,h)=(%d, %d, %d, %d) "
+			"src_img(w,h)=(%d, %d).\n",
+			__func__,
+			src_rect.x, src_rect.y, src_rect.width, src_rect.height,
+			req->user_req.src_img.width,
+			req->user_req.src_img.height);
+		return -EINVAL;
+	}
+
+	if (!is_src_fill && (src_rect.width <= 0 || src_rect.height <= 0)) {
+		b2r2_log_warn(cont->dev, "%s: Invalid source dimensions:\n"
+			"src(w,h)=(%d, %d).\n",
+			__func__,
+			src_rect.width, src_rect.height);
+		return -EINVAL;
+	}
+
+	if (dst_rect.width <= 0 || dst_rect.height <= 0) {
+		b2r2_log_warn(cont->dev, "%s: Invalid dest dimensions:\n"
+			"dst(w,h)=(%d, %d).\n",
+			__func__,
+			dst_rect.width, dst_rect.height);
+		return -EINVAL;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) &&
+			req->user_req.clut == NULL) {
+		b2r2_log_warn(cont->dev, "%s: Invalid request: no table "
+				"specified for CLUT color correction.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	/* Check for invalid image params */
+	if (!is_src_fill && validate_buf(cont, &(req->user_req.src_img),
+			&(req->src_resolved)))
+		return -EINVAL;
+
+	if (validate_buf(cont, &(req->user_req.dst_img), &(req->dst_resolved)))
+		return -EINVAL;
+
+	if (is_src_fill) {
+		/*
+		 * Params correct for a source fill operation.
+		 * No need for further checking.
+		 */
+		if (yuv_planar_dst)
+			n_nodes += 2;
+		else if (yuv_semi_planar_dst)
+			n_nodes++;
+
+		*work_buf_width = B2R2_GENERIC_WORK_BUF_WIDTH;
+		*work_buf_height = B2R2_GENERIC_WORK_BUF_HEIGHT;
+		*work_buf_count = n_work_bufs;
+		*node_count = n_nodes;
+		b2r2_log_info(cont->dev, "%s DONE buf_w=%d buf_h=%d "
+				"buf_count=%d node_count=%d\n", __func__,
+			*work_buf_width, *work_buf_height,
+			*work_buf_count, *node_count);
+		return 0;
+	}
+
+	/*
+	 * Calculate scaling factors, all transform enum values
+	 * that include rotation have the CCW_ROT_90 bit set.
+	 */
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		h_scf = (src_rect.width << 10) / dst_rect.height;
+		v_scf = (src_rect.height << 10) / dst_rect.width;
+	} else {
+		h_scf = (src_rect.width << 10) / dst_rect.width;
+		v_scf = (src_rect.height << 10) / dst_rect.height;
+	}
+
+	/* Check for degenerate/out_of_range scaling factors. */
+	if (h_scf <= 0 || v_scf <= 0 || h_scf > 0x7C00 || v_scf > 0x7C00) {
+		b2r2_log_warn(cont->dev,
+			"%s: Dimensions result in degenerate or "
+			"out of range scaling:\n"
+			"src(w,h)=(%d, %d) "
+			"dst(w,h)=(%d,%d).\n"
+			"h_scf=0x%.8x, v_scf=0x%.8x\n",
+			__func__,
+			src_rect.width, src_rect.height,
+			dst_rect.width, dst_rect.height,
+			h_scf, v_scf);
+		return -EINVAL;
+	}
+
+	if (yuv_planar_dst)
+		n_nodes += 2;
+	else if (yuv_semi_planar_dst)
+		n_nodes++;
+
+	*work_buf_width = B2R2_GENERIC_WORK_BUF_WIDTH;
+	*work_buf_height = B2R2_GENERIC_WORK_BUF_HEIGHT;
+	*work_buf_count = n_work_bufs;
+	*node_count = n_nodes;
+	b2r2_log_info(cont->dev, "%s DONE buf_w=%d buf_h=%d buf_count=%d "
+		"node_count=%d\n", __func__, *work_buf_width,
+		*work_buf_height, *work_buf_count, *node_count);
+	return 0;
+}
+
+/*
+ *
+ */
+int b2r2_generic_configure(const struct b2r2_blt_request *req,
+			   struct b2r2_node *first,
+			   struct b2r2_work_buf *tmp_bufs,
+			   u32 buf_count)
+{
+	struct b2r2_node *node = NULL;
+	struct b2r2_work_buf *in_buf = NULL;
+	struct b2r2_work_buf *out_buf = NULL;
+	struct b2r2_work_buf *empty_buf = NULL;
+	struct b2r2_control *cont = req->instance->control;
+
+#ifdef B2R2_GENERIC_DEBUG
+	u32 needed_bufs = 0;
+	u32 needed_nodes = 0;
+	s32 work_buf_width = 0;
+	s32 work_buf_height = 0;
+	u32 n_nodes = 0;
+	int invalid_req = b2r2_generic_analyze(req, &work_buf_width,
+					       &work_buf_height, &needed_bufs,
+					       &needed_nodes);
+	if (invalid_req < 0) {
+		b2r2_log_warn(cont->dev,
+			"%s: Invalid request supplied, ec=%d\n",
+			__func__, invalid_req);
+		return -EINVAL;
+	}
+
+	node = first;
+
+	while (node != NULL) {
+		n_nodes++;
+		node = node->next;
+	}
+	if (n_nodes < needed_nodes) {
+		b2r2_log_warn(cont->dev, "%s: Not enough nodes %d < %d.\n",
+			      __func__, n_nodes, needed_nodes);
+		return -EINVAL;
+	}
+
+	if (buf_count < needed_bufs) {
+		b2r2_log_warn(cont->dev, "%s: Not enough buffers %d < %d.\n",
+			      __func__, buf_count, needed_bufs);
+		return -EINVAL;
+	}
+
+#endif
+
+	reset_nodes(cont, first);
+	node = first;
+	empty_buf = tmp_bufs;
+	out_buf = empty_buf;
+	empty_buf++;
+	/* Prepare input tile. Color_fill or read from src */
+	setup_input_stage(req, node, out_buf);
+	in_buf = out_buf;
+	out_buf = empty_buf;
+	empty_buf++;
+	node = node->next;
+
+	if ((req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) != 0) {
+		setup_transform_stage(req, node, out_buf, in_buf);
+		node = node->next;
+		in_buf = out_buf;
+		out_buf = empty_buf++;
+	}
+	/* EMACSOC TODO: mask */
+	/*
+	if (req->user_req.flags & B2R2_BLT_FLAG_SOURCE_MASK) {
+		setup_mask_stage(req, node, out_buf, in_buf);
+		node = node->next;
+		in_buf = out_buf;
+		out_buf = empty_buf++;
+	}
+	*/
+	/* Read the part of destination that will be updated */
+	setup_dst_read_stage(req, node, out_buf);
+	node = node->next;
+	setup_blend_stage(req, node, out_buf, in_buf);
+	node = node->next;
+	in_buf = out_buf;
+	setup_writeback_stage(req, node, in_buf);
+	return 0;
+}
+
+void b2r2_generic_set_areas(const struct b2r2_blt_request *req,
+			    struct b2r2_node *first,
+			    struct b2r2_blt_rect *dst_rect_area)
+{
+	/*
+	 * Nodes come in the following order: <input stage>, [transform],
+	 * [src_mask], <dst_read>, <blend>, <writeback>
+	 */
+	struct b2r2_node *node = first;
+	const struct b2r2_blt_rect *dst_rect = &(req->user_req.dst_rect);
+	const struct b2r2_blt_rect *src_rect = &(req->user_req.src_rect);
+	const enum b2r2_blt_fmt src_fmt = req->user_req.src_img.fmt;
+	bool yuv_multi_buffer_src =
+		src_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		src_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		src_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+	const enum b2r2_blt_fmt dst_fmt = req->user_req.dst_img.fmt;
+	const bool yuv_multi_buffer_dst =
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+		dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE ||
+		dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+	s32 h_scf = 1 << 10;
+	s32 v_scf = 1 << 10;
+	s32 src_x = 0;
+	s32 src_y = 0;
+	s32 src_w = 0;
+	s32 src_h = 0;
+	u32 b2r2_rzi = 0;
+	s32 clip_top = 0;
+	s32 clip_left = 0;
+	s32 clip_bottom = req->user_req.dst_img.height - 1;
+	s32 clip_right = req->user_req.dst_img.width - 1;
+	/* Dst coords inside the dst_rect, not the buffer */
+	s32 dst_x = dst_rect_area->x;
+	s32 dst_y = dst_rect_area->y;
+	struct b2r2_control *cont = req->instance->control;
+
+	b2r2_log_info(cont->dev, "%s ENTRY\n", __func__);
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		h_scf = (src_rect->width << 10) / dst_rect->height;
+		v_scf = (src_rect->height << 10) / dst_rect->width;
+	} else {
+		h_scf = (src_rect->width << 10) / dst_rect->width;
+		v_scf = (src_rect->height << 10) / dst_rect->height;
+	}
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		/*
+		 * Normally the inverse transform for 90 degree rotation
+		 * is given by:
+		 * | 0  1|   |x|   | y|
+		 * |     | X | | = |  |
+		 * |-1  0|   |y|   |-x|
+		 * but screen coordinates are flipped in y direction
+		 * (compared to usual Cartesian coordinates), hence the offsets.
+		 */
+		src_x = (dst_rect->height - dst_y - dst_rect_area->height) *
+			h_scf;
+		src_y = dst_x * v_scf;
+		src_w = dst_rect_area->height * h_scf;
+		src_h = dst_rect_area->width * v_scf;
+	} else {
+		src_x = dst_x * h_scf;
+		src_y = dst_y * v_scf;
+		src_w = dst_rect_area->width * h_scf;
+		src_h = dst_rect_area->height * v_scf;
+	}
+
+	b2r2_rzi |= ((src_x & 0x3ff) << B2R2_RZI_HSRC_INIT_SHIFT) |
+		((src_y & 0x3ff) << B2R2_RZI_VSRC_INIT_SHIFT);
+
+	/*
+	 * src_w must contain all the pixels that contribute
+	 * to a particular tile.
+	 * ((x + 0x3ff) >> 10) is equivalent to ceiling(x),
+	 * expressed in 6.10 fixed point format.
+	 * Every destination tile, maps to a certain area in the source
+	 * rectangle. The area in source will most likely not be a rectangle
+	 * with exact integer dimensions whenever arbitrary scaling is involved.
+	 * Consider the following example.
+	 * Suppose, that width of the current destination tile maps
+	 * to 1.7 pixels in source, starting at x == 5.4, as calculated
+	 * using the scaling factor.
+	 * This means that while the destination tile is written,
+	 * the source should be read from x == 5.4 up to x == 5.4 + 1.7 == 7.1
+	 * Consequently, color from 3 pixels (x == 5, 6 and 7)
+	 * needs to be read from source.
+	 * The formula below the comment yields:
+	 * ceil(0.4 + 1.7) == ceil(2.1) == 3
+	 * (src_x & 0x3ff) is the fractional part of src_x,
+	 * which is expressed in 6.10 fixed point format.
+	 * Thus, width of the source area should be 3 pixels wide,
+	 * starting at x == 5.
+	 * However, the reading should not start at x == 5.0
+	 * but a bit inside, namely x == 5.4
+	 * The B2R2_RZI register is used to instruct the HW to do so.
+	 * It contains the fractional part that will be added to
+	 * the first pixel coordinate, before incrementing the current source
+	 * coordinate with the step specified in B2R2_RSF register.
+	 * The same applies to scaling in vertical direction.
+	 */
+	src_w = ((src_x & 0x3ff) + src_w + 0x3ff) >> 10;
+	src_h = ((src_y & 0x3ff) + src_h + 0x3ff) >> 10;
+
+	/*
+	 * EMACSOC TODO: Remove this debug clamp, once tile size
+	 * is taken into account in generic_analyze()
+	 */
+	if (src_w > 128)
+		src_w = 128;
+
+	src_x >>= 10;
+	src_y >>= 10;
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_H)
+		src_x = src_rect->width - src_x - src_w;
+
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_V)
+		src_y = src_rect->height - src_y - src_h;
+
+	/*
+	 * Translate the src/dst_rect coordinates into true
+	 * src/dst_buffer coordinates
+	 */
+	src_x += src_rect->x;
+	src_y += src_rect->y;
+
+	dst_x += dst_rect->x;
+	dst_y += dst_rect->y;
+
+	/*
+	 * Clamp the src coords to buffer dimensions
+	 * to prevent illegal reads.
+	 */
+	if (src_x < 0)
+		src_x = 0;
+
+	if (src_y < 0)
+		src_y = 0;
+
+	if ((src_x + src_w) > req->user_req.src_img.width)
+		src_w = req->user_req.src_img.width - src_x;
+
+	if ((src_y + src_h) > req->user_req.src_img.height)
+		src_h = req->user_req.src_img.height - src_y;
+
+
+	/* The input node */
+	if (yuv_multi_buffer_src) {
+		/* Luma on SRC3 */
+		node->node.GROUP5.B2R2_SXY =
+			((src_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((src_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP5.B2R2_SSZ =
+			((src_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((src_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		/* Clear and set only the SRC_INIT bits */
+		node->node.GROUP10.B2R2_RZI &=
+			~((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+			(0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+		node->node.GROUP10.B2R2_RZI |= b2r2_rzi;
+
+		node->node.GROUP9.B2R2_RZI &=
+			~((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+			(0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+		switch (src_fmt) {
+		case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chroma goes on SRC2 and potentially on SRC1.
+			 * Chroma is half the size of luma. Must round up
+			 * the chroma size to handle cases when luma size is not
+			 * divisible by 2.
+			 * E.g. luma width==7 requires chroma width==4.
+			 * Chroma width==7/2==3 is only enough
+			 * for luma width==6.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				(((src_x & 0xffff) >> 1) << B2R2_XY_X_SHIFT) |
+				(((src_y & 0xffff) >> 1) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((((src_w + 1) & 0xfff) >> 1) <<
+					B2R2_SZ_WIDTH_SHIFT) |
+				((((src_h + 1) & 0xfff) >> 1) <<
+					B2R2_SZ_HEIGHT_SHIFT);
+			if (src_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+					src_fmt ==
+					B2R2_BLT_FMT_YVU420_PACKED_PLANAR) {
+				node->node.GROUP3.B2R2_SXY =
+					node->node.GROUP4.B2R2_SXY;
+				node->node.GROUP3.B2R2_SSZ =
+					node->node.GROUP4.B2R2_SSZ;
+			}
+			node->node.GROUP9.B2R2_RZI |= (b2r2_rzi >> 1) &
+				((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+				 (0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+			break;
+		case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+			/*
+			 * Chroma goes on SRC2 and potentially on SRC1.
+			 * Now chroma is half the size of luma
+			 * only in horizontal direction.
+			 * Same rounding applies as for 420 formats above,
+			 * except it is only done horizontally.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				(((src_x & 0xffff) >> 1) << B2R2_XY_X_SHIFT) |
+				((src_y & 0xffff) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((((src_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+				((src_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+			if (src_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+					src_fmt ==
+					B2R2_BLT_FMT_YVU422_PACKED_PLANAR) {
+				node->node.GROUP3.B2R2_SXY =
+					node->node.GROUP4.B2R2_SXY;
+				node->node.GROUP3.B2R2_SSZ =
+					node->node.GROUP4.B2R2_SSZ;
+			}
+			node->node.GROUP9.B2R2_RZI |=
+				(((src_x & 0x3ff) >> 1) <<
+						B2R2_RZI_HSRC_INIT_SHIFT) |
+				((src_y & 0x3ff) << B2R2_RZI_VSRC_INIT_SHIFT);
+			break;
+		case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+			/*
+			 * Chroma goes on SRC2 and SRC1.
+			 * It is the same size as luma.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				((src_x & 0xffff) << B2R2_XY_X_SHIFT) |
+				((src_y & 0xffff) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((src_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+				((src_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+			node->node.GROUP3.B2R2_SXY = node->node.GROUP4.B2R2_SXY;
+			node->node.GROUP3.B2R2_SSZ = node->node.GROUP4.B2R2_SSZ;
+
+			/* Clear and set only the SRC_INIT bits */
+			node->node.GROUP9.B2R2_RZI &=
+				~((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+				  (0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+			node->node.GROUP9.B2R2_RZI |= b2r2_rzi;
+			break;
+		default:
+			break;
+		}
+	} else {
+		node->node.GROUP4.B2R2_SXY =
+			((src_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((src_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP4.B2R2_SSZ =
+			((src_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((src_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		/* Clear and set only the SRC_INIT bits */
+		node->node.GROUP9.B2R2_RZI &=
+			~((0x3ff << B2R2_RZI_HSRC_INIT_SHIFT) |
+			  (0x3ff << B2R2_RZI_VSRC_INIT_SHIFT));
+		node->node.GROUP9.B2R2_RZI |= b2r2_rzi;
+	}
+
+	node->node.GROUP1.B2R2_TXY = 0;
+	if (req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		/*
+		 * dst_rect_area coordinates are specified
+		 * after potential rotation.
+		 * Input is read before rotation, hence the width and height
+		 * need to be swapped.
+		 * Horizontal and vertical flips are accomplished with
+		 * suitable scanning order while writing
+		 * to the temporary buffer.
+		 */
+		if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_H) {
+			node->node.GROUP1.B2R2_TXY |=
+				((dst_rect_area->height - 1) & 0xffff) <<
+				B2R2_XY_X_SHIFT;
+		}
+
+		if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_V) {
+			node->node.GROUP1.B2R2_TXY |=
+				((dst_rect_area->width - 1) & 0xffff) <<
+				B2R2_XY_Y_SHIFT;
+		}
+
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->height & 0xfff) <<
+						B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->width & 0xfff) <<
+						B2R2_SZ_HEIGHT_SHIFT);
+	} else {
+		if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_H) {
+			node->node.GROUP1.B2R2_TXY |=
+				((dst_rect_area->width - 1) & 0xffff) <<
+				B2R2_XY_X_SHIFT;
+		}
+
+		if (req->user_req.transform & B2R2_BLT_TRANSFORM_FLIP_V) {
+			node->node.GROUP1.B2R2_TXY |=
+				((dst_rect_area->height - 1) & 0xffff) <<
+				B2R2_XY_Y_SHIFT;
+		}
+
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->width & 0xfff) <<
+						B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+						B2R2_SZ_HEIGHT_SHIFT);
+	}
+
+	if (req->user_req.flags &
+		(B2R2_BLT_FLAG_SOURCE_FILL | B2R2_BLT_FLAG_SOURCE_FILL_RAW)) {
+		/*
+		 * Scan order for source fill should always be left-to-right
+		 * and top-to-bottom. Fill the input tile from top left.
+		 */
+		node->node.GROUP1.B2R2_TXY = 0;
+		node->node.GROUP4.B2R2_SSZ = node->node.GROUP1.B2R2_TSZ;
+	}
+
+	if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+			dst_rect_area->y == 0) {
+		dump_nodes(cont, node, false);
+		b2r2_log_debug(cont->dev, "%s Input node done.\n", __func__);
+	}
+
+	/* Transform */
+	if ((req->user_req.transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) != 0) {
+		/*
+		 * Transform node operates on temporary buffers.
+		 * Content always at top left, but scanning order
+		 * has to be flipped during rotation.
+		 * Width and height need to be considered as well, since
+		 * a tile may not necessarily be filled completely.
+		 * dst_rect_area dimensions are specified
+		 * after potential rotation.
+		 * Input is read before rotation, hence the width and height
+		 * need to be swapped on src.
+		 */
+		node = node->next;
+
+		node->node.GROUP4.B2R2_SXY = 0;
+		node->node.GROUP4.B2R2_SSZ =
+			((dst_rect_area->height & 0xfff) <<
+						B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->width & 0xfff) <<
+						B2R2_SZ_HEIGHT_SHIFT);
+		/* Bottom line written first */
+		node->node.GROUP1.B2R2_TXY =
+			((dst_rect_area->height - 1) & 0xffff) <<
+			B2R2_XY_Y_SHIFT;
+
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->width & 0xfff) <<
+						B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+						B2R2_SZ_HEIGHT_SHIFT);
+
+		if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+				dst_rect_area->y == 0) {
+			dump_nodes(cont, node, false);
+			b2r2_log_debug(cont->dev,
+				"%s Tranform node done.\n", __func__);
+		}
+	}
+
+	/* Source mask */
+	if (req->user_req.flags & B2R2_BLT_FLAG_SOURCE_MASK) {
+		node = node->next;
+		/*
+		 * Same coords for mask as for the input stage.
+		 * Should the mask be transformed together with source?
+		 * EMACSOC TODO: Apply mask before any
+		 * transform/scaling is done.
+		 * Otherwise it will be dst_ not src_mask.
+		 */
+		if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+				dst_rect_area->y == 0) {
+			dump_nodes(cont, node, false);
+			b2r2_log_debug(cont->dev,
+				"%s Source mask node done.\n", __func__);
+		}
+	}
+
+	/* dst_read */
+	if (yuv_multi_buffer_dst) {
+		s32 dst_w = dst_rect_area->width;
+		s32 dst_h = dst_rect_area->height;
+		bool yuv420_dst =
+			dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE;
+
+		bool yuv422_dst =
+			dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR ||
+			dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE;
+		node = node->next;
+		/* Luma on SRC3 */
+		node->node.GROUP5.B2R2_SXY =
+			((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP5.B2R2_SSZ =
+			((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		if (yuv420_dst) {
+			/*
+			 * Chroma goes on SRC2 and potentially on SRC1.
+			 * Chroma is half the size of luma. Must round up
+			 * the chroma size to handle cases when luma size is not
+			 * divisible by 2.
+			 * E.g. luma width==7 requires chroma width==4.
+			 * Chroma width==7/2==3 is only enough
+			 * for luma width==6.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				(((dst_x & 0xffff) >> 1) << B2R2_XY_X_SHIFT) |
+				(((dst_y & 0xffff) >> 1) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((((dst_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+				((((dst_h + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+
+			if (dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+					dst_fmt ==
+					B2R2_BLT_FMT_YVU420_PACKED_PLANAR) {
+				node->node.GROUP3.B2R2_SXY =
+					node->node.GROUP4.B2R2_SXY;
+				node->node.GROUP3.B2R2_SSZ =
+					node->node.GROUP4.B2R2_SSZ;
+			}
+		} else if (yuv422_dst) {
+			/*
+			 * Chroma goes on SRC2 and potentially on SRC1.
+			 * Now chroma is half the size of luma
+			 * only in horizontal direction.
+			 * Same rounding applies as for 420 formats above,
+			 * except it is only done horizontally.
+			 */
+			node->node.GROUP4.B2R2_SXY =
+				(((dst_x & 0xffff) >> 1) << B2R2_XY_X_SHIFT) |
+				((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+			node->node.GROUP4.B2R2_SSZ =
+				((((dst_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+				((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+			if (dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+					dst_fmt ==
+					B2R2_BLT_FMT_YVU422_PACKED_PLANAR) {
+				node->node.GROUP3.B2R2_SXY =
+					node->node.GROUP4.B2R2_SXY;
+				node->node.GROUP3.B2R2_SSZ =
+					node->node.GROUP4.B2R2_SSZ;
+			}
+		} else if (dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR) {
+			/*
+			 * Chroma goes on SRC2 and SRC1.
+			 * It is the same size as luma.
+			 */
+			node->node.GROUP4.B2R2_SXY = node->node.GROUP5.B2R2_SXY;
+			node->node.GROUP4.B2R2_SSZ = node->node.GROUP5.B2R2_SSZ;
+			node->node.GROUP3.B2R2_SXY = node->node.GROUP5.B2R2_SXY;
+			node->node.GROUP3.B2R2_SSZ = node->node.GROUP5.B2R2_SSZ;
+		}
+
+		node->node.GROUP1.B2R2_TXY = 0;
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+	} else {
+		node = node->next;
+		node->node.GROUP4.B2R2_SXY =
+			((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP4.B2R2_SSZ =
+			((dst_rect_area->width & 0xfff) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+		node->node.GROUP1.B2R2_TXY = 0;
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->width & 0xfff) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+	}
+
+	if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+			dst_rect_area->y == 0) {
+		dump_nodes(cont, node, false);
+		b2r2_log_debug(cont->dev, "%s dst_read node done.\n", __func__);
+	}
+
+	/* blend */
+	node = node->next;
+	node->node.GROUP3.B2R2_SXY = 0;
+	node->node.GROUP3.B2R2_SSZ =
+		((dst_rect_area->width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+		((dst_rect_area->height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+	/* contents of the foreground temporary buffer always at top left */
+	node->node.GROUP4.B2R2_SXY = 0;
+	node->node.GROUP4.B2R2_SSZ =
+		((dst_rect_area->width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+		((dst_rect_area->height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+	node->node.GROUP1.B2R2_TXY = 0;
+	node->node.GROUP1.B2R2_TSZ =
+		((dst_rect_area->width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+		((dst_rect_area->height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+	if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+			dst_rect_area->y == 0) {
+		dump_nodes(cont, node, false);
+		b2r2_log_debug(cont->dev, "%s Blend node done.\n", __func__);
+	}
+
+	/* writeback */
+	node = node->next;
+	if ((req->user_req.flags & B2R2_BLT_FLAG_DESTINATION_CLIP) != 0) {
+		clip_left = req->user_req.dst_clip_rect.x;
+		clip_top = req->user_req.dst_clip_rect.y;
+		clip_right = clip_left + req->user_req.dst_clip_rect.width - 1;
+		clip_bottom = clip_top + req->user_req.dst_clip_rect.height - 1;
+	}
+	/*
+	 * Clamp the dst clip rectangle to buffer dimensions to prevent
+	 * illegal writes. An illegal clip rectangle, e.g. outside the
+	 * buffer will be ignored, resulting in nothing being clipped.
+	 */
+	if (clip_left < 0 || req->user_req.dst_img.width <= clip_left)
+		clip_left = 0;
+
+	if (clip_top < 0 || req->user_req.dst_img.height <= clip_top)
+		clip_top = 0;
+
+	if (clip_right < 0 || req->user_req.dst_img.width <= clip_right)
+		clip_right = req->user_req.dst_img.width - 1;
+
+	if (clip_bottom < 0 || req->user_req.dst_img.height <= clip_bottom)
+		clip_bottom = req->user_req.dst_img.height - 1;
+
+	/*
+	 * Only allow writing inside the clip rect.
+	 * INTNL bit in B2R2_CWO should be zero.
+	 */
+	node->node.GROUP6.B2R2_CWO =
+		((clip_top & 0x7fff) << B2R2_CWO_Y_SHIFT) |
+		((clip_left & 0x7fff) << B2R2_CWO_X_SHIFT);
+	node->node.GROUP6.B2R2_CWS =
+		((clip_bottom & 0x7fff) << B2R2_CWS_Y_SHIFT) |
+		((clip_right & 0x7fff) << B2R2_CWS_X_SHIFT);
+
+	if (yuv_multi_buffer_dst) {
+		const s32 dst_w = dst_rect_area->width;
+		const s32 dst_h = dst_rect_area->height;
+		int i = 0;
+		/* Number of nodes required to write chroma output */
+		int n_nodes = 1;
+		if (dst_fmt == B2R2_BLT_FMT_YUV420_PACKED_PLANAR ||
+				dst_fmt == B2R2_BLT_FMT_YUV422_PACKED_PLANAR ||
+				dst_fmt == B2R2_BLT_FMT_YVU420_PACKED_PLANAR ||
+				dst_fmt == B2R2_BLT_FMT_YVU422_PACKED_PLANAR ||
+				dst_fmt == B2R2_BLT_FMT_YUV444_PACKED_PLANAR)
+			n_nodes = 2;
+
+		node->node.GROUP4.B2R2_SXY = 0;
+		node->node.GROUP4.B2R2_SSZ =
+			((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		/* Luma (Y-component) */
+		node->node.GROUP1.B2R2_TXY =
+			((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+		node->node.GROUP6.B2R2_CWO =
+			((clip_top & 0x7fff) << B2R2_CWO_Y_SHIFT) |
+			((clip_left & 0x7fff) << B2R2_CWO_X_SHIFT);
+		node->node.GROUP6.B2R2_CWS =
+			((clip_bottom & 0x7fff) << B2R2_CWS_Y_SHIFT) |
+			((clip_right & 0x7fff) << B2R2_CWS_X_SHIFT);
+
+		if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+				dst_rect_area->y == 0) {
+			dump_nodes(cont, node, false);
+			b2r2_log_debug(cont->dev,
+				"%s Writeback luma node done.\n", __func__);
+		}
+
+		node = node->next;
+
+		/*
+		 * Chroma components. 1 or 2 nodes
+		 * for semi-planar or planar buffer respectively.
+		 */
+		for (i = 0; i < n_nodes && node != NULL; ++i) {
+
+			node->node.GROUP4.B2R2_SXY = 0;
+			node->node.GROUP4.B2R2_SSZ =
+				((dst_w & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+				((dst_h & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+
+			switch (dst_fmt) {
+			case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+			case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+			case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+			case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+			case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+				/*
+				 * Chroma is half the size of luma.
+				 * Must round up the chroma size to handle
+				 * cases when luma size is not divisible by 2.
+				 * E.g. luma_width==7 requires chroma_width==4.
+				 * Chroma_width==7/2==3 is only enough
+				 * for luma_width==6.
+				 */
+				node->node.GROUP1.B2R2_TXY =
+					(((dst_x & 0xffff) >> 1) <<
+							B2R2_XY_X_SHIFT) |
+					(((dst_y & 0xffff) >> 1) <<
+							B2R2_XY_Y_SHIFT);
+				node->node.GROUP1.B2R2_TSZ =
+					((((dst_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+					((((dst_h + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+				break;
+			case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+			case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+			case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+			case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+			case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+				/*
+				 * Now chroma is half the size of luma only
+				 * in horizontal direction.
+				 * Same rounding applies as
+				 * for 420 formats above, except it is only
+				 * done horizontally.
+				 */
+				node->node.GROUP1.B2R2_TXY =
+					(((dst_x & 0xffff) >> 1) <<
+							B2R2_XY_X_SHIFT) |
+					((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+				node->node.GROUP1.B2R2_TSZ =
+					((((dst_w + 1) & 0xfff) >> 1) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+					((dst_h & 0xfff) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+				break;
+			case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+				/*
+				 * Chroma has the same resolution as luma.
+				 */
+				node->node.GROUP1.B2R2_TXY =
+					((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+					((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+				node->node.GROUP1.B2R2_TSZ =
+					((dst_w & 0xfff) <<
+							B2R2_SZ_WIDTH_SHIFT) |
+					((dst_h & 0xfff) <<
+							B2R2_SZ_HEIGHT_SHIFT);
+				break;
+			default:
+				break;
+			}
+
+			node->node.GROUP6.B2R2_CWO =
+				((clip_top & 0x7fff) << B2R2_CWO_Y_SHIFT) |
+				((clip_left & 0x7fff) << B2R2_CWO_X_SHIFT);
+			node->node.GROUP6.B2R2_CWS =
+				((clip_bottom & 0x7fff) << B2R2_CWS_Y_SHIFT) |
+				((clip_right & 0x7fff) << B2R2_CWS_X_SHIFT);
+
+			if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+					dst_rect_area->y == 0) {
+				dump_nodes(cont, node, false);
+				b2r2_log_debug(cont->dev, "%s Writeback chroma "
+						"node %d of %d done.\n",
+					__func__, i + 1, n_nodes);
+			}
+
+			node = node->next;
+		}
+	} else {
+		node->node.GROUP4.B2R2_SXY = 0;
+		node->node.GROUP4.B2R2_SSZ =
+			((dst_rect_area->width & 0xfff) <<
+					B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+					B2R2_SZ_HEIGHT_SHIFT);
+		node->node.GROUP1.B2R2_TXY =
+			((dst_x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((dst_y & 0xffff) << B2R2_XY_Y_SHIFT);
+		node->node.GROUP1.B2R2_TSZ =
+			((dst_rect_area->width & 0xfff) <<
+					B2R2_SZ_WIDTH_SHIFT) |
+			((dst_rect_area->height & 0xfff) <<
+					B2R2_SZ_HEIGHT_SHIFT);
+
+		if (B2R2_GENERIC_DEBUG_AREAS && dst_rect_area->x == 0 &&
+				dst_rect_area->y == 0) {
+			dump_nodes(cont, node, false);
+			b2r2_log_debug(cont->dev, "%s Writeback node done.\n",
+					__func__);
+		}
+	}
+
+	b2r2_log_info(cont->dev, "%s DONE\n", __func__);
+}
diff --git a/drivers/video/b2r2/b2r2_generic.h b/drivers/video/b2r2/b2r2_generic.h
new file mode 100644
index 00000000000..3b22f654deb
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_generic.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 generic. Full coverage of user interface but
+ * non optimized implementation. For Fallback purposes.
+ *
+ * Author: Maciej Socha <maciej.socha@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_VIDEO_B2R2_GENERIC_H
+#define _LINUX_VIDEO_B2R2_GENERIC_H
+
+#include <video/b2r2_blt.h>
+
+#include "b2r2_internal.h"
+
+/**
+ * b2r2_generic_init()
+ */
+void b2r2_generic_init(struct b2r2_control *cont);
+
+/**
+ * b2r2_generic_exit()
+ */
+void b2r2_generic_exit(struct b2r2_control *cont);
+
+/**
+ * b2r2_generic_analyze()
+ */
+int b2r2_generic_analyze(const struct b2r2_blt_request *req,
+						 s32 *work_buf_width,
+						 s32 *work_buf_height,
+						 u32 *work_buf_count,
+						 u32 *node_count);
+/**
+ * b2r2_generic_configure()
+ */
+int b2r2_generic_configure(const struct b2r2_blt_request *req,
+				struct b2r2_node *first,
+				struct b2r2_work_buf *tmp_bufs,
+				u32 buf_count);
+/**
+ * b2r2_generic_set_areas()
+ */
+void b2r2_generic_set_areas(const struct b2r2_blt_request *req,
+				struct b2r2_node *first,
+				struct b2r2_blt_rect *dst_rect_area);
+#endif
diff --git a/drivers/video/b2r2/b2r2_global.h b/drivers/video/b2r2/b2r2_global.h
new file mode 100644
index 00000000000..38cf74bb753
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_global.h
@@ -0,0 +1,119 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 global definitions
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __B2R2_GLOBAL_H
+#define __B2R2_GLOBAL_H
+
+/** Sources involved */
+
+struct b2r2_system {
+	unsigned int B2R2_NIP;
+	unsigned int B2R2_CIC;
+	unsigned int B2R2_INS;
+	unsigned int B2R2_ACK;
+};
+
+struct b2r2_target {
+	unsigned int B2R2_TBA;
+	unsigned int B2R2_TTY;
+	unsigned int B2R2_TXY;
+	unsigned int B2R2_TSZ;
+};
+
+struct b2r2_color_fill {
+	unsigned int  B2R2_S1CF;
+	unsigned int  B2R2_S2CF;
+};
+
+struct b2r2_src_config {
+	unsigned int B2R2_SBA;
+	unsigned int B2R2_STY;
+	unsigned int B2R2_SXY;
+	unsigned int B2R2_SSZ;
+};
+
+struct b2r2_clip {
+	 unsigned int B2R2_CWO;
+	 unsigned int B2R2_CWS;
+};
+
+struct b2r2_color_key {
+	 unsigned int B2R2_KEY1;
+	 unsigned int B2R2_KEY2;
+};
+
+struct b2r2_clut {
+	unsigned int B2R2_CCO;
+	unsigned int B2R2_CML;
+};
+
+struct b2r2_rsz_pl_mask {
+	unsigned int B2R2_FCTL;
+	unsigned int B2R2_PMK;
+};
+
+struct b2r2_Cr_luma_rsz {
+	unsigned int B2R2_RSF;
+	unsigned int B2R2_RZI;
+	unsigned int B2R2_HFP;
+	unsigned int B2R2_VFP;
+};
+
+struct b2r2_flikr_filter {
+	unsigned int B2R2_FF0;
+	unsigned int B2R2_FF1;
+	unsigned int B2R2_FF2;
+	unsigned int B2R2_FF3;
+};
+
+struct b2r2_xyl {
+	unsigned int B2R2_XYL;
+	unsigned int B2R2_XYP;
+};
+
+struct b2r2_sau {
+	unsigned int B2R2_SAR;
+	unsigned int B2R2_USR;
+};
+
+struct b2r2_vm {
+	unsigned int B2R2_VMX0;
+	unsigned int B2R2_VMX1;
+	unsigned int B2R2_VMX2;
+	unsigned int B2R2_VMX3;
+};
+
+struct b2r2_link_list {
+
+	struct b2r2_system       GROUP0;
+	struct b2r2_target 	 GROUP1;
+	struct b2r2_color_fill   GROUP2;
+	struct b2r2_src_config	 GROUP3;
+	struct b2r2_src_config   GROUP4;
+	struct b2r2_src_config   GROUP5;
+	struct b2r2_clip         GROUP6;
+	struct b2r2_clut         GROUP7;
+	struct b2r2_rsz_pl_mask	 GROUP8;
+	struct b2r2_Cr_luma_rsz  GROUP9;
+	struct b2r2_Cr_luma_rsz  GROUP10;
+	struct b2r2_flikr_filter GROUP11;
+	struct b2r2_color_key    GROUP12;
+	struct b2r2_xyl  	 GROUP13;
+	struct b2r2_sau		 GROUP14;
+	struct b2r2_vm		 GROUP15;
+	struct b2r2_vm		 GROUP16;
+
+	unsigned int B2R2_RESERVED[2];
+};
+
+
+#endif /* !defined(__B2R2_GLOBAL_H) */
diff --git a/drivers/video/b2r2/b2r2_hw.h b/drivers/video/b2r2/b2r2_hw.h
new file mode 100644
index 00000000000..9739912d78c
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_hw.h
@@ -0,0 +1,458 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 hw definitions
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef B2R2_HW_H__
+#define B2R2_HW_H__
+
+#include <linux/bitops.h>
+
+/* Scaling works in strips 128 pixels wide */
+#define B2R2_RESCALE_MAX_WIDTH 128
+
+/* Rotation works in strips 16 pixels wide */
+#define B2R2_ROTATE_MAX_WIDTH 16
+
+/* B2R2 color formats */
+#define B2R2_COLOR_FORMAT_SHIFT 16
+enum b2r2_native_fmt {
+	/* RGB formats */
+	B2R2_NATIVE_RGB565 = 0x00 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_RGB888 = 0x01 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ARGB8565 = 0x04 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ARGB8888 = 0x05 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ARGB1555 = 0x06 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ARGB4444 = 0x07 << B2R2_COLOR_FORMAT_SHIFT,
+
+	/* YCbCr formats */
+	B2R2_NATIVE_YCBCR888 = 0x10 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YCBCR422R = 0x12 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_AYCBCR8888 = 0x15 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YCBCR42X_MB = 0x14 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YCBCR42X_R2B = 0x16 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YCBCR42X_MBN = 0x0e << B2R2_COLOR_FORMAT_SHIFT,
+
+	/* CLUT formats */
+	B2R2_NATIVE_CLUT2 = 0x09 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_CLUT8 = 0x0b << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ACLUT44 = 0x0c << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_ACLUT88 = 0x0d << B2R2_COLOR_FORMAT_SHIFT,
+
+	/* Misc. formats */
+	B2R2_NATIVE_A1 = 0x18 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_A8 = 0x19 << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_YUV	= 0x1e << B2R2_COLOR_FORMAT_SHIFT,
+	B2R2_NATIVE_BYTE = 0x1f << B2R2_COLOR_FORMAT_SHIFT,
+};
+
+/* B2R2_CIC register values */
+enum b2r2_cic {
+	B2R2_CIC_COLOR_FILL =     BIT(1),/*0x00000002*/
+	B2R2_CIC_SOURCE_1 =       BIT(2),/*0x00000004*/
+	B2R2_CIC_SOURCE_2 =       BIT(3),/*0x00000008*/
+	B2R2_CIC_SOURCE_3 =       BIT(4),/*0x00000010*/
+	B2R2_CIC_CLIP_WINDOW =    BIT(5),/*0x00000020*/
+	B2R2_CIC_CLUT =           BIT(6),/*0x00000040*/
+	B2R2_CIC_FILTER_CONTROL = BIT(7),/*0x00000080*/
+	B2R2_CIC_RESIZE_CHROMA =  BIT(8),/*0x00000100*/
+	B2R2_CIC_RESIZE_LUMA =    BIT(9),/*0x00000200*/
+	B2R2_CIC_FLICKER_COEFF =  BIT(10),/*0x00000400*/
+	B2R2_CIC_COLOR_KEY =      BIT(11),/*0x00000800*/
+	B2R2_CIC_XYL =            BIT(12),/*0x00001000*/
+	B2R2_CIC_SAU =            BIT(13),/*0x00002000*/
+	B2R2_CIC_IVMX =           BIT(14),/*0x00004000*/
+	B2R2_CIC_OVMX =           BIT(15),/*0x00008000*/
+	B2R2_CIC_PACEDOT =        BIT(16),/*0x00010000*/
+	B2R2_CIC_VC1 =            BIT(17)/*0x00020000*/
+};
+
+/* B2R2_INS register values */
+#define B2R2_INS_SOURCE_1_SHIFT 0
+#define B2R2_INS_SOURCE_2_SHIFT 3
+#define B2R2_INS_SOURCE_3_SHIFT 5
+#define B2R2_INS_IVMX_SHIFT 6
+#define B2R2_INS_CLUTOP_SHIFT 7
+#define B2R2_INS_RESCALE2D_SHIFT 8
+#define B2R2_INS_FLICK_FILT_SHIFT 9
+#define B2R2_INS_RECT_CLIP_SHIFT 10
+#define B2R2_INS_CKEY_SHIFT 11
+#define B2R2_INS_OVMX_SHIFT 12
+#define B2R2_INS_DEI_SHIFT 13
+#define B2R2_INS_PLANE_MASK_SHIFT 14
+#define B2R2_INS_XYL_SHIFT 15
+#define B2R2_INS_DOT_SHIFT 16
+#define B2R2_INS_VC1R_SHIFT 17
+#define B2R2_INS_ROTATION_SHIFT 18
+#define B2R2_INS_PACE_DOWN_SHIFT 30
+#define B2R2_INS_BLITCOMPIRQ_SHIFT 31
+enum b2r2_ins {
+	/* Source 1 config */
+	B2R2_INS_SOURCE_1_FETCH_FROM_MEM =      0x1 << B2R2_INS_SOURCE_1_SHIFT,
+	B2R2_INS_SOURCE_1_COLOR_FILL_REGISTER = 0x3 << B2R2_INS_SOURCE_1_SHIFT,
+	B2R2_INS_SOURCE_1_DIRECT_COPY =         0x4 << B2R2_INS_SOURCE_1_SHIFT,
+	B2R2_INS_SOURCE_1_DIRECT_FILL =         0x7 << B2R2_INS_SOURCE_1_SHIFT,
+
+	/* Source 2 config */
+	B2R2_INS_SOURCE_2_FETCH_FROM_MEM =      0x1 << B2R2_INS_SOURCE_2_SHIFT,
+	B2R2_INS_SOURCE_2_COLOR_FILL_REGISTER = 0x3 << B2R2_INS_SOURCE_2_SHIFT,
+
+	/* Source 3 config */
+	B2R2_INS_SOURCE_3_FETCH_FROM_MEM =      0x1 << B2R2_INS_SOURCE_3_SHIFT,
+
+	/* Other configs */
+	B2R2_INS_IVMX_ENABLED =               0x1 << B2R2_INS_IVMX_SHIFT,
+	B2R2_INS_CLUTOP_ENABLED =             0x1 << B2R2_INS_CLUTOP_SHIFT,
+	B2R2_INS_RESCALE2D_ENABLED =          0x1 << B2R2_INS_RESCALE2D_SHIFT,
+	B2R2_INS_FLICK_FILT_ENABLED =         0x1 << B2R2_INS_FLICK_FILT_SHIFT,
+	B2R2_INS_RECT_CLIP_ENABLED =          0x1 << B2R2_INS_RECT_CLIP_SHIFT,
+	B2R2_INS_CKEY_ENABLED =               0x1 << B2R2_INS_CKEY_SHIFT,
+	B2R2_INS_OVMX_ENABLED =               0x1 << B2R2_INS_OVMX_SHIFT,
+	B2R2_INS_DEI_ENABLED =                0x1 << B2R2_INS_DEI_SHIFT,
+	B2R2_INS_PLANE_MASK_ENABLED =         0x1 << B2R2_INS_PLANE_MASK_SHIFT,
+	B2R2_INS_XYL_ENABLED =                0x1 << B2R2_INS_XYL_SHIFT,
+	B2R2_INS_DOT_ENABLED =                0x1 << B2R2_INS_DOT_SHIFT,
+	B2R2_INS_VC1R_ENABLED =               0x1 << B2R2_INS_VC1R_SHIFT,
+	B2R2_INS_ROTATION_ENABLED =           0x1 << B2R2_INS_ROTATION_SHIFT,
+	B2R2_INS_PACE_DOWN_ENABLED =          0x1 << B2R2_INS_PACE_DOWN_SHIFT,
+	B2R2_INS_BLITCOMPIRQ_ENABLED =        0x1 << B2R2_INS_BLITCOMPIRQ_SHIFT,
+
+};
+
+/* B2R2_ACK register values */
+#define B2R2_ACK_MODE_SHIFT 0
+#define B2R2_ACK_SWAP_FG_BG_SHIFT 4
+#define B2R2_ACK_GALPHA_ROPID_SHIFT 8
+#define B2R2_ACK_CKEY_BLUE_SHIFT 16
+#define B2R2_ACK_CKEY_GREEN_SHIFT 18
+#define B2R2_ACK_CKEY_RED_SHIFT 20
+#define B2R2_ACK_CKEY_SEL_SHIFT 22
+enum b2r2_ack {
+	/* ALU operation modes */
+	B2R2_ACK_MODE_LOGICAL_OPERATION =            0x1 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_BLEND_NOT_PREMULT =            0x2 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_BLEND_PREMULT =                0x3 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_LOGICAL_FIRST_PASS =  0x4 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_BLEND =               0x5 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_BYPASS_S2_S3 =                 0x7 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_LOGICAL_SECOND_PASS = 0x8 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_XYL_LOGICAL =         0x9 << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_XYL_BLEND_NOT_PREMULT =
+						     0xa << B2R2_ACK_MODE_SHIFT,
+	B2R2_ACK_MODE_CLIPMASK_XYL_BLEND_PREMULT =   0xb << B2R2_ACK_MODE_SHIFT,
+
+	/* ALU channel selection */
+	B2R2_ACK_SWAP_FG_BG =                  0x1 << B2R2_ACK_SWAP_FG_BG_SHIFT,
+
+	/* Global alpha and ROP IDs */
+	B2R2_ACK_ROP_CLEAR =                 0x0 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_AND =                   0x1 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_AND_REV =               0x2 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_COPY =                  0x3 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_AND_INV =               0x4 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_NOOP =                  0x5 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_XOR =                   0x6 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_OR =                    0x7 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_NOR =                   0x8 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_EQUIV =                 0x9 << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_INVERT =                0xa << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_OR_REV =                0xb << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_COPY_INV =              0xc << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_OR_INV =                0xd << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_NAND =                  0xe << B2R2_ACK_GALPHA_ROPID_SHIFT,
+	B2R2_ACK_ROP_SET =                   0xf << B2R2_ACK_GALPHA_ROPID_SHIFT,
+
+	/* Color key configuration bits */
+	B2R2_ACK_CKEY_BLUE_MATCH_IF_BETWEEN =  0x1 << B2R2_ACK_CKEY_BLUE_SHIFT,
+	B2R2_ACK_CKEY_BLUE_MATCH_IF_LT_OR_GT = 0x2 << B2R2_ACK_CKEY_BLUE_SHIFT,
+	B2R2_ACK_CKEY_RED_MATCH_IF_BETWEEN =   0x1 << B2R2_ACK_CKEY_GREEN_SHIFT,
+	B2R2_ACK_CKEY_RED_MATCH_IF_LT_OR_GT =  0x2 << B2R2_ACK_CKEY_GREEN_SHIFT,
+	B2R2_ACK_CKEY_GREEN_MATCH_IF_BETWEEN = 0x1 << B2R2_ACK_CKEY_RED_SHIFT,
+	B2R2_ACK_CKEY_GREEN_MATCH_IF_LT_OR_GT = 0x2 << B2R2_ACK_CKEY_RED_SHIFT,
+
+	/* Color key input selection */
+	B2R2_ACK_CKEY_SEL_DEST =               0x0 << B2R2_ACK_CKEY_SEL_SHIFT,
+	B2R2_ACK_CKEY_SEL_SRC_BEFORE_CLUT =    0x1 << B2R2_ACK_CKEY_SEL_SHIFT,
+	B2R2_ACK_CKEY_SEL_SRC_AFTER_CLUT =     0x2 << B2R2_ACK_CKEY_SEL_SHIFT,
+	B2R2_ACK_CKEY_SEL_BLANKING_S2_ALPHA =  0x3 << B2R2_ACK_CKEY_SEL_SHIFT,
+};
+
+/* Common <S/T>TY defines */
+#define B2R2_TY_BITMAP_PITCH_SHIFT 0
+#define B2R2_TY_COLOR_FORM_SHIFT 16
+#define B2R2_TY_ALPHA_RANGE_SHIFT 21
+#define B2R2_TY_MB_ACCESS_MODE_SHIFT 23
+#define B2R2_TY_HSO_SHIFT 24
+#define B2R2_TY_VSO_SHIFT 25
+#define B2R2_TY_SUBBYTE_SHIFT 28
+#define B2R2_TY_ENDIAN_SHIFT 30
+#define B2R2_TY_SECURE_SHIFT 31
+
+/* Dummy enum for generalization of <S/T>TY registers */
+enum b2r2_ty {
+	/* Alpha range */
+	B2R2_TY_ALPHA_RANGE_128 = 0x0 << B2R2_TY_ALPHA_RANGE_SHIFT,
+	B2R2_TY_ALPHA_RANGE_255 = 0x1 << B2R2_TY_ALPHA_RANGE_SHIFT,
+
+	/* Access mode in macro-block organized frame buffers */
+	B2R2_TY_MB_ACCESS_MODE_FRAME = 0x0 << B2R2_TY_MB_ACCESS_MODE_SHIFT,
+	B2R2_TY_MB_ACCESS_MODE_FIELD = 0x1 << B2R2_TY_MB_ACCESS_MODE_SHIFT,
+
+	/* Horizontal scan order */
+	B2R2_TY_HSO_LEFT_TO_RIGHT = 0x0 << B2R2_TY_HSO_SHIFT,
+	B2R2_TY_HSO_RIGHT_TO_LEFT = 0x1 << B2R2_TY_HSO_SHIFT,
+
+	/* Vertical scan order */
+	B2R2_TY_VSO_TOP_TO_BOTTOM = 0x0 << B2R2_TY_VSO_SHIFT,
+	B2R2_TY_VSO_BOTTOM_TO_TOP = 0x1 << B2R2_TY_VSO_SHIFT,
+
+	/* Pixel ordering for sub-byte formats (position of right-most pixel) */
+	B2R2_TY_SUBBYTE_MSB = 0x0 << B2R2_TY_SUBBYTE_SHIFT,
+	B2R2_TY_SUBBYTE_LSB = 0x1 << B2R2_TY_SUBBYTE_SHIFT,
+
+	/* Bitmap endianess */
+	B2R2_TY_ENDIAN_BIG_NOT_LITTLE = 0x1 << B2R2_TY_ENDIAN_SHIFT,
+
+	/* Secureness of the target memory region */
+	B2R2_TY_SECURE_UNSECURE = 0x0 << B2R2_TY_SECURE_SHIFT,
+	B2R2_TY_SECURE_SECURE =   0x1 << B2R2_TY_SECURE_SHIFT,
+
+	/* Dummy to make sure the data type is large enough */
+	B2R2_TY_DUMMY = 0xffffffff,
+};
+
+/* B2R2_TTY register values */
+#define B2R2_TTY_CB_NOT_CR_SHIFT 22
+#define B2R2_TTY_RGB_ROUND_SHIFT 26
+#define B2R2_TTY_CHROMA_NOT_LUMA_SHIFT 27
+enum b2r2_tty {
+
+	/* Chroma component selection */
+	B2R2_TTY_CB_NOT_CR = 0x1 << B2R2_TTY_CB_NOT_CR_SHIFT,
+
+	/* RGB rounding mode */
+	B2R2_TTY_RGB_ROUND_NORMAL = 0x0 << B2R2_TTY_RGB_ROUND_SHIFT,
+	B2R2_TTY_RGB_ROUND_DITHER = 0x1 << B2R2_TTY_RGB_ROUND_SHIFT,
+
+	/* Component selection for splitted frame buffer formats */
+	B2R2_TTY_CHROMA_NOT_LUMA = 0x1 << B2R2_TTY_CHROMA_NOT_LUMA_SHIFT,
+};
+
+/* B2R2_S1TY register values */
+#define B2R2_S1TY_A1_SUBST_SHIFT 22
+#define B2R2_S1TY_ROTATION_SHIFT 27
+#define B2R2_S1TY_RGB_EXPANSION_SHIFT 29
+enum b2r2_s1ty {
+
+	/* Alpha bit substitution mode for ARGB1555 */
+	B2R2_S1TY_A1_SUBST_KEY_MODE = 0x1 << B2R2_S1TY_A1_SUBST_SHIFT,
+
+	/* Input rectangle rotation (NOT YET IMPLEMENTED) */
+	B2R2_S1TY_ENABLE_ROTATION = 0x1 << B2R2_S1TY_ROTATION_SHIFT,
+
+	/* RGB expansion mode */
+	B2R2_S1TY_RGB_EXPANSION_MSB_DUP = 0x0  << B2R2_S1TY_RGB_EXPANSION_SHIFT,
+	B2R2_S1TY_RGB_EXPANSION_LSP_ZERO = 0x1 << B2R2_S1TY_RGB_EXPANSION_SHIFT,
+};
+
+/* B2R2_S1TY register values */
+#define B2R2_S2TY_A1_SUBST_SHIFT 22
+#define B2R2_S2TY_CHROMA_LEFT_SHIFT 26
+#define B2R2_S2TY_RGB_EXPANSION_SHIFT 29
+enum b2r2_s2ty {
+
+	/* Alpha bit substitution mode for ARGB1555 */
+	B2R2_S2TY_A1_SUBST_KEY_MODE = 0x1 << B2R2_S2TY_A1_SUBST_SHIFT,
+
+	/* Chroma left extension */
+	B2R2_S2TY_CHROMA_LEFT_EXT_FOLLOWING_PIXEL = 0x0
+						<< B2R2_S2TY_CHROMA_LEFT_SHIFT,
+	B2R2_S2TY_CHROMA_LEFT_EXT_AVERAGE = 0x1 << B2R2_S2TY_CHROMA_LEFT_SHIFT,
+
+	/* RGB expansion mode */
+	B2R2_S2TY_RGB_EXPANSION_MSB_DUP = 0x0  << B2R2_S2TY_RGB_EXPANSION_SHIFT,
+	B2R2_S2TY_RGB_EXPANSION_LSP_ZERO = 0x1 << B2R2_S2TY_RGB_EXPANSION_SHIFT,
+};
+
+/* B2R2_S1TY register values */
+#define B2R2_S3TY_BLANK_ACC_SHIFT 26
+enum b2r2_s3ty {
+	/* Enables "blank" access on this source (nothing will be fetched from
+	   memory) */
+	B2R2_S3TY_ENABLE_BLANK_ACCESS = 0x1 << B2R2_S3TY_BLANK_ACC_SHIFT,
+};
+
+/* B2R2_<S or T>XY register values */
+#define B2R2_XY_X_SHIFT 0
+#define B2R2_XY_Y_SHIFT 16
+
+/* B2R2_<S or T>SZ register values */
+#define B2R2_SZ_WIDTH_SHIFT 0
+#define B2R2_SZ_HEIGHT_SHIFT 16
+
+/* Clip window offset (top left coordinates) */
+#define B2R2_CWO_X_SHIFT 0
+#define B2R2_CWO_Y_SHIFT 16
+
+/* Clip window stop (bottom right coordinates) */
+#define B2R2_CWS_X_SHIFT 0
+#define B2R2_CWS_Y_SHIFT 16
+
+/* Color look-up table */
+enum b2r2_cco {
+	B2R2_CCO_CLUT_COLOR_CORRECTION = (1 << 16),
+	B2R2_CCO_CLUT_UPDATE = (1 << 18),
+	B2R2_CCO_CLUT_ON_S1 = (1 << 15)
+};
+
+/* Filter control (2D resize control) */
+enum b2r2_fctl {
+	/* Horizontal 2D filter mode */
+	B2R2_FCTL_HF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER = BIT(0),
+	B2R2_FCTL_HF2D_MODE_ENABLE_ALPHA_CHANNEL_FILTER = BIT(1),
+	B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER = BIT(2),
+
+	/* Vertical 2D filter mode */
+	B2R2_FCTL_VF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER = BIT(4),
+	B2R2_FCTL_VF2D_MODE_ENABLE_ALPHA_CHANNEL_FILTER = BIT(5),
+	B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER = BIT(6),
+
+	/* Alpha borders */
+	B2R2_FCTL_ENABLE_ALPHA_BORDER_RIGHT = BIT(12),
+	B2R2_FCTL_ENABLE_ALPHA_BORDER_LEFT = BIT(13),
+	B2R2_FCTL_ENABLE_ALPHA_BORDER_BOTTOM = BIT(14),
+	B2R2_FCTL_ENABLE_ALPHA_BORDER_TOP = BIT(15),
+
+	/* Luma path horizontal 2D filter mode */
+	B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_FILTER = BIT(24),
+	B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_RESIZER = BIT(25),
+
+	/* Luma path vertical 2D filter mode */
+	B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_FILTER = BIT(28),
+	B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_RESIZER = BIT(29),
+};
+
+/* Resize scaling factor */
+#define B2R2_RSF_HSRC_INC_SHIFT 0
+#define B2R2_RSF_VSRC_INC_SHIFT 16
+
+/* Resizer initialization */
+#define B2R2_RZI_HSRC_INIT_SHIFT 0
+#define B2R2_RZI_HNB_REPEAT_SHIFT 12
+#define B2R2_RZI_VSRC_INIT_SHIFT 16
+#define B2R2_RZI_VNB_REPEAT_SHIFT 28
+
+/* Default values for the resizer */
+#define B2R2_RZI_DEFAULT_HNB_REPEAT (3 << B2R2_RZI_HNB_REPEAT_SHIFT)
+#define B2R2_RZI_DEFAULT_VNB_REPEAT (3 << B2R2_RZI_VNB_REPEAT_SHIFT)
+
+
+/* Bus plug configuration registers */
+enum b2r2_plug_opcode_size {
+	B2R2_PLUG_OPCODE_SIZE_8 =  0x3,
+	B2R2_PLUG_OPCODE_SIZE_16 = 0x4,
+	B2R2_PLUG_OPCODE_SIZE_32 = 0x5,
+	B2R2_PLUG_OPCODE_SIZE_64 = 0x6,
+};
+
+enum b2r2_plug_chunk_size {
+	B2R2_PLUG_CHUNK_SIZE_1 =   0x0,
+	B2R2_PLUG_CHUNK_SIZE_2 =   0x1,
+	B2R2_PLUG_CHUNK_SIZE_4 =   0x2,
+	B2R2_PLUG_CHUNK_SIZE_8 =   0x3,
+	B2R2_PLUG_CHUNK_SIZE_16 =  0x4,
+	B2R2_PLUG_CHUNK_SIZE_32 =  0x5,
+	B2R2_PLUG_CHUNK_SIZE_64 =  0x6,
+	B2R2_PLUG_CHUNK_SIZE_128 = 0x7,
+};
+
+enum b2r2_plug_message_size {
+	B2R2_PLUG_MESSAGE_SIZE_1 =   0x0,
+	B2R2_PLUG_MESSAGE_SIZE_2 =   0x1,
+	B2R2_PLUG_MESSAGE_SIZE_4 =   0x2,
+	B2R2_PLUG_MESSAGE_SIZE_8 =   0x3,
+	B2R2_PLUG_MESSAGE_SIZE_16 =  0x4,
+	B2R2_PLUG_MESSAGE_SIZE_32 =  0x5,
+	B2R2_PLUG_MESSAGE_SIZE_64 =  0x6,
+	B2R2_PLUG_MESSAGE_SIZE_128 = 0x7,
+};
+
+enum b2r2_plug_page_size {
+	B2R2_PLUG_PAGE_SIZE_64 =  0x0,
+	B2R2_PLUG_PAGE_SIZE_128 = 0x1,
+	B2R2_PLUG_PAGE_SIZE_256 = 0x2,
+};
+
+/* Default opcode size */
+#if defined(CONFIG_B2R2_OPSIZE_8)
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT B2R2_PLUG_OPCODE_SIZE_8
+#elif defined(CONFIG_B2R2_OPSIZE_16)
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT B2R2_PLUG_OPCODE_SIZE_16
+#elif defined(CONFIG_B2R2_OPSIZE_32)
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT B2R2_PLUG_OPCODE_SIZE_32
+#elif defined(CONFIG_B2R2_OPSIZE_64)
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT B2R2_PLUG_OPCODE_SIZE_64
+#else
+#  define B2R2_PLUG_OPCODE_SIZE_DEFAULT 0
+#endif
+
+/* Default chunk size */
+#if defined(CONFIG_B2R2_CHSIZE_1)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_1
+#elif defined(CONFIG_B2R2_CHSIZE_2)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_2
+#elif defined(CONFIG_B2R2_CHSIZE_4)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_4
+#elif defined(CONFIG_B2R2_CHSIZE_8)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_8
+#elif defined(CONFIG_B2R2_CHSIZE_16)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_16
+#elif defined(CONFIG_B2R2_CHSIZE_32)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_32
+#elif defined(CONFIG_B2R2_CHSIZE_64)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_64
+#elif defined(CONFIG_B2R2_CHSIZE_128)
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT B2R2_PLUG_CHUNK_SIZE_128
+#else
+#  define B2R2_PLUG_CHUNK_SIZE_DEFAULT 0
+#endif
+
+/* Default message size */
+#if defined(CONFIG_B2R2_MGSIZE_1)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_1
+#elif defined(CONFIG_B2R2_MGSIZE_2)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_2
+#elif defined(CONFIG_B2R2_MGSIZE_4)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_4
+#elif defined(CONFIG_B2R2_MGSIZE_8)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_8
+#elif defined(CONFIG_B2R2_MGSIZE_16)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_16
+#elif defined(CONFIG_B2R2_MGSIZE_32)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_32
+#elif defined(CONFIG_B2R2_MGSIZE_64)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_64
+#elif defined(CONFIG_B2R2_MGSIZE_128)
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT B2R2_PLUG_MESSAGE_SIZE_128
+#else
+#  define B2R2_PLUG_MESSAGE_SIZE_DEFAULT 0
+#endif
+
+/* Default page size */
+#if defined(CONFIG_B2R2_PGSIZE_64)
+#  define B2R2_PLUG_PAGE_SIZE_DEFAULT B2R2_PLUG_PAGE_SIZE_64
+#elif defined(CONFIG_B2R2_PGSIZE_128)
+#  define B2R2_PLUG_PAGE_SIZE_DEFAULT B2R2_PLUG_PAGE_SIZE_128
+#elif defined(CONFIG_B2R2_PGSIZE_256)
+#  define B2R2_PLUG_PAGE_SIZE_DEFAULT B2R2_PLUG_PAGE_SIZE_256
+#else
+#  define B2R2_PLUG_PAGE_SIZE_DEFAULT 0
+#endif
+
+#endif /* B2R2_HW_H__ */
diff --git a/drivers/video/b2r2/b2r2_hw_convert.c b/drivers/video/b2r2/b2r2_hw_convert.c
new file mode 100644
index 00000000000..d1b44db79c3
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_hw_convert.c
@@ -0,0 +1,747 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2012
+ *
+ * ST-Ericsson B2R2 node splitter
+ *
+ * Author: Jorgen Nilsson <jorgen.nilsson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include "b2r2_hw_convert.h"
+#include "b2r2_internal.h"
+#include "b2r2_utils.h"
+
+/*
+ * Macros and constants
+ */
+
+/*  VMX register values for RGB to YUV color conversion */
+/*  Magic numbers from 27.11 in DB8500_DesignSpecification_v2.5.pdf */
+
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_RGB_TO_YUV_601_FULL_RANGE 0x107e4beb
+#define B2R2_VMX1_RGB_TO_YUV_601_FULL_RANGE 0x0982581d
+#define B2R2_VMX2_RGB_TO_YUV_601_FULL_RANGE 0xfa9ea483
+#define B2R2_VMX3_RGB_TO_YUV_601_FULL_RANGE 0x08000080
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_RGB_TO_YUV_601_STANDARD 0x0e1e8bee
+#define B2R2_VMX1_RGB_TO_YUV_601_STANDARD 0x08420419
+#define B2R2_VMX2_RGB_TO_YUV_601_STANDARD 0xfb5ed471
+#define B2R2_VMX3_RGB_TO_YUV_601_STANDARD 0x08004080
+
+/* 709 Full range conversion matrix */
+#define B2R2_VMX0_RGB_TO_YUV_709_FULL_RANGE 0x107e27f4
+#define B2R2_VMX1_RGB_TO_YUV_709_FULL_RANGE 0x06e2dc13
+#define B2R2_VMX2_RGB_TO_YUV_709_FULL_RANGE 0xfc5e6c83
+#define B2R2_VMX3_RGB_TO_YUV_709_FULL_RANGE 0x08000080
+
+/* 709 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_RGB_TO_YUV_709_STANDARD 0x0e3e6bf5
+#define B2R2_VMX1_RGB_TO_YUV_709_STANDARD 0x05e27410
+#define B2R2_VMX2_RGB_TO_YUV_709_STANDARD 0xfcdea471
+#define B2R2_VMX3_RGB_TO_YUV_709_STANDARD 0x08004080
+
+/* VMX register values for YUV to RGB color conversion */
+
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_YUV_TO_RGB_601_FULL_RANGE 0x2c440000
+#define B2R2_VMX1_YUV_TO_RGB_601_FULL_RANGE 0xe9a403aa
+#define B2R2_VMX2_YUV_TO_RGB_601_FULL_RANGE 0x0004013f
+#define B2R2_VMX3_YUV_TO_RGB_601_FULL_RANGE 0x34f21322
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_YUV_TO_RGB_601_STANDARD 0x3324a800
+#define B2R2_VMX1_YUV_TO_RGB_601_STANDARD 0xe604ab9c
+#define B2R2_VMX2_YUV_TO_RGB_601_STANDARD 0x0004a957
+#define B2R2_VMX3_YUV_TO_RGB_601_STANDARD 0x32121eeb
+
+/* 709 Full range conversion matrix */
+#define B2R2_VMX0_YUV_TO_RGB_709_FULL_RANGE 0x31440000
+#define B2R2_VMX1_YUV_TO_RGB_709_FULL_RANGE 0xf16403d1
+#define B2R2_VMX2_YUV_TO_RGB_709_FULL_RANGE 0x00040145
+#define B2R2_VMX3_YUV_TO_RGB_709_FULL_RANGE 0x33b14b18
+
+/* 709 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_YUV_TO_RGB_709_STANDARD 0x3964a800
+#define B2R2_VMX1_YUV_TO_RGB_709_STANDARD 0xef04abc9
+#define B2R2_VMX2_YUV_TO_RGB_709_STANDARD 0x0004a95f
+#define B2R2_VMX3_YUV_TO_RGB_709_STANDARD 0x307132df
+
+/* VMX register values for RGB to BGR conversion */
+#define B2R2_VMX0_RGB_TO_BGR 0x00000100
+#define B2R2_VMX1_RGB_TO_BGR 0x00040000
+#define B2R2_VMX2_RGB_TO_BGR 0x20000000
+#define B2R2_VMX3_RGB_TO_BGR 0x00000000
+
+/* VMX register values for BGR to YUV color conversion */
+/* Note: All BGR -> YUV values are calculated by multiplying
+ * the RGB -> YUV matrices [A], with [S] to form [A]x[S] where
+ *     |0 0 1|
+ * S = |0 1 0|
+ *     |1 0 0|
+ * Essentially swapping first and third columns in
+ * the matrices (VMX0, VMX1 and VMX2 values).
+ * The offset vector VMX3 remains untouched.
+ * Put another way, the value of bits 0 through 9
+ * is swapped with the value of
+ * bits 20 through 31 in VMX0, VMX1 and VMX2,
+ * taking into consideration the compression
+ * that is used on bits 0 through 9. Bit 0 being LSB.
+ */
+
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_BGR_TO_YUV_601_FULL_RANGE 0xfd7e4883
+#define B2R2_VMX1_BGR_TO_YUV_601_FULL_RANGE 0x03a2584c
+#define B2R2_VMX2_BGR_TO_YUV_601_FULL_RANGE 0x107ea7d4
+#define B2R2_VMX3_BGR_TO_YUV_601_FULL_RANGE 0x08000080
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_BGR_TO_YUV_601_STANDARD 0xfdde8870
+#define B2R2_VMX1_BGR_TO_YUV_601_STANDARD 0x03220442
+#define B2R2_VMX2_BGR_TO_YUV_601_STANDARD 0x0e3ed7da
+#define B2R2_VMX3_BGR_TO_YUV_601_STANDARD 0x08004080
+
+/* 709 Full range conversion matrix */
+#define B2R2_VMX0_BGR_TO_YUV_709_FULL_RANGE 0xfe9e2483
+#define B2R2_VMX1_BGR_TO_YUV_709_FULL_RANGE 0x0262dc37
+#define B2R2_VMX2_BGR_TO_YUV_709_FULL_RANGE 0x107e6fe2
+#define B2R2_VMX3_BGR_TO_YUV_709_FULL_RANGE 0x08000080
+
+/* 709 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_BGR_TO_YUV_709_STANDARD 0xfebe6871
+#define B2R2_VMX1_BGR_TO_YUV_709_STANDARD 0x0202742f
+#define B2R2_VMX2_BGR_TO_YUV_709_STANDARD 0x0e3ea7e6
+#define B2R2_VMX3_BGR_TO_YUV_709_STANDARD 0x08004080
+
+
+/* VMX register values for YUV to BGR conversion */
+/*  Note: All YUV -> BGR values are constructed
+ * from the YUV -> RGB ones, by swapping
+ * first and third rows in the matrix
+ * (VMX0 and VMX2 values). Further, the first and
+ * third values in the offset vector need to be
+ * swapped as well, i.e. bits 0 through 9 are swapped
+ * with bits 20 through 29 in the VMX3 value.
+ * Bit 0 being LSB.
+ */
+
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_YUV_TO_BGR_601_FULL_RANGE (B2R2_VMX2_YUV_TO_RGB_601_FULL_RANGE)
+#define B2R2_VMX1_YUV_TO_BGR_601_FULL_RANGE (B2R2_VMX1_YUV_TO_RGB_601_FULL_RANGE)
+#define B2R2_VMX2_YUV_TO_BGR_601_FULL_RANGE (B2R2_VMX0_YUV_TO_RGB_601_FULL_RANGE)
+#define B2R2_VMX3_YUV_TO_BGR_601_FULL_RANGE 0x3222134f
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_YUV_TO_BGR_601_STANDARD (B2R2_VMX2_YUV_TO_RGB_601_STANDARD)
+#define B2R2_VMX1_YUV_TO_BGR_601_STANDARD (B2R2_VMX1_YUV_TO_RGB_601_STANDARD)
+#define B2R2_VMX2_YUV_TO_BGR_601_STANDARD (B2R2_VMX0_YUV_TO_RGB_601_STANDARD)
+#define B2R2_VMX3_YUV_TO_BGR_601_STANDARD 0x2eb21f21
+
+/* 709 Full range conversion matrix */
+#define B2R2_VMX0_YUV_TO_BGR_709_FULL_RANGE (B2R2_VMX2_YUV_TO_RGB_709_FULL_RANGE)
+#define B2R2_VMX1_YUV_TO_BGR_709_FULL_RANGE (B2R2_VMX1_YUV_TO_RGB_709_FULL_RANGE)
+#define B2R2_VMX2_YUV_TO_BGR_709_FULL_RANGE (B2R2_VMX0_YUV_TO_RGB_709_FULL_RANGE)
+#define B2R2_VMX3_YUV_TO_BGR_709_FULL_RANGE 0x31814b3b
+
+/* 709 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_YUV_TO_BGR_709_STANDARD (B2R2_VMX2_YUV_TO_RGB_709_STANDARD)
+#define B2R2_VMX1_YUV_TO_BGR_709_STANDARD (B2R2_VMX1_YUV_TO_RGB_709_STANDARD)
+#define B2R2_VMX2_YUV_TO_BGR_709_STANDARD (B2R2_VMX0_YUV_TO_RGB_709_STANDARD)
+#define B2R2_VMX3_YUV_TO_BGR_709_STANDARD 0x2df13307
+
+
+/* VMX register values for YVU to RGB conversion */
+
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_YVU_TO_RGB_601_FULL_RANGE 0x00040120
+#define B2R2_VMX1_YVU_TO_RGB_601_FULL_RANGE 0xF544034D
+#define B2R2_VMX2_YVU_TO_RGB_601_FULL_RANGE 0x37840000
+#define B2R2_VMX3_YVU_TO_RGB_601_FULL_RANGE (B2R2_VMX3_YUV_TO_RGB_601_FULL_RANGE)
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_YVU_TO_RGB_601_STANDARD 0x0004A999
+#define B2R2_VMX1_YVU_TO_RGB_601_STANDARD 0xF384AB30
+#define B2R2_VMX2_YVU_TO_RGB_601_STANDARD 0x40A4A800
+#define B2R2_VMX3_YVU_TO_RGB_601_STANDARD (B2R2_VMX3_YUV_TO_RGB_601_STANDARD)
+
+/* VMX register values for RGB to YVU conversion */
+
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_RGB_TO_YVU_601_FULL_RANGE (B2R2_VMX2_RGB_TO_YUV_601_FULL_RANGE)
+#define B2R2_VMX1_RGB_TO_YVU_601_FULL_RANGE (B2R2_VMX1_RGB_TO_YUV_601_FULL_RANGE)
+#define B2R2_VMX2_RGB_TO_YVU_601_FULL_RANGE (B2R2_VMX0_RGB_TO_YUV_601_FULL_RANGE)
+#define B2R2_VMX3_RGB_TO_YVU_601_FULL_RANGE (B2R2_VMX3_RGB_TO_YUV_601_FULL_RANGE)
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_RGB_TO_YVU_601_STANDARD (B2R2_VMX2_RGB_TO_YUV_601_STANDARD)
+#define B2R2_VMX1_RGB_TO_YVU_601_STANDARD (B2R2_VMX1_RGB_TO_YUV_601_STANDARD)
+#define B2R2_VMX2_RGB_TO_YVU_601_STANDARD (B2R2_VMX0_RGB_TO_YUV_601_STANDARD)
+#define B2R2_VMX3_RGB_TO_YVU_601_STANDARD (B2R2_VMX3_RGB_TO_YUV_601_STANDARD)
+
+/* VMX register values for YVU to BGR conversion */
+
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_YVU_TO_BGR_601_FULL_RANGE (B2R2_VMX2_YVU_TO_RGB_601_FULL_RANGE)
+#define B2R2_VMX1_YVU_TO_BGR_601_FULL_RANGE (B2R2_VMX1_YVU_TO_RGB_601_FULL_RANGE)
+#define B2R2_VMX2_YVU_TO_BGR_601_FULL_RANGE (B2R2_VMX0_YVU_TO_RGB_601_FULL_RANGE)
+#define B2R2_VMX3_YVU_TO_BGR_601_FULL_RANGE 0x3222134F
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_YVU_TO_BGR_601_STANDARD (B2R2_VMX2_YVU_TO_RGB_601_STANDARD)
+#define B2R2_VMX1_YVU_TO_BGR_601_STANDARD (B2R2_VMX1_YVU_TO_RGB_601_STANDARD)
+#define B2R2_VMX2_YVU_TO_BGR_601_STANDARD (B2R2_VMX0_YVU_TO_RGB_601_STANDARD)
+#define B2R2_VMX3_YVU_TO_BGR_601_STANDARD 0x3222134F
+
+/* VMX register values for BGR to YVU conversion */
+
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_BGR_TO_YVU_601_FULL_RANGE (B2R2_VMX2_BGR_TO_YUV_601_FULL_RANGE)
+#define B2R2_VMX1_BGR_TO_YVU_601_FULL_RANGE (B2R2_VMX1_BGR_TO_YUV_601_FULL_RANGE)
+#define B2R2_VMX2_BGR_TO_YVU_601_FULL_RANGE (B2R2_VMX0_BGR_TO_YUV_601_FULL_RANGE)
+#define B2R2_VMX3_BGR_TO_YVU_601_FULL_RANGE (B2R2_VMX3_BGR_TO_YUV_601_FULL_RANGE)
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_BGR_TO_YVU_601_STANDARD (B2R2_VMX2_BGR_TO_YUV_601_STANDARD)
+#define B2R2_VMX1_BGR_TO_YVU_601_STANDARD (B2R2_VMX1_BGR_TO_YUV_601_STANDARD)
+#define B2R2_VMX2_BGR_TO_YVU_601_STANDARD (B2R2_VMX0_BGR_TO_YUV_601_STANDARD)
+#define B2R2_VMX3_BGR_TO_YVU_601_STANDARD (B2R2_VMX3_BGR_TO_YUV_601_STANDARD)
+
+/* VMX register values for YVU to YUV conversion */
+
+/* 601 Video Matrix (standard 601 conversion) */
+/* Internally, the components are in fact stored
+ * with luma in the middle, i.e. UYV, which is why
+ * the values are just like for RGB->BGR conversion.
+ */
+#define B2R2_VMX0_YVU_TO_YUV 0x00000100
+#define B2R2_VMX1_YVU_TO_YUV 0x00040000
+#define B2R2_VMX2_YVU_TO_YUV 0x20000000
+#define B2R2_VMX3_YVU_TO_YUV 0x00000000
+
+/* VMX register values for RGB to BLT_YUV888 conversion */
+
+/*
+ * BLT_YUV888 has color components laid out in memory as V, U, Y, (Alpha)
+ * with V at the first byte (due to little endian addressing).
+ * B2R2 expects them to be as U, Y, V, (A)
+ * with U at the first byte.
+ * Note: RGB -> BLT_YUV888 values are calculated by multiplying
+ * the RGB -> YUV matrix [A], with [S] to form [S]x[A] where
+ *     |0 1 0|
+ * S = |0 0 1|
+ *     |1 0 0|
+ * Essentially changing the order of rows in the original
+ * matrix [A].
+ * row1 -> row3
+ * row2 -> row1
+ * row3 -> row2
+ * Values in the offset vector are swapped in the same manner.
+ */
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_RGB_TO_BLT_YUV888_601_FULL_RANGE (B2R2_VMX1_RGB_TO_YUV_601_FULL_RANGE)
+#define B2R2_VMX1_RGB_TO_BLT_YUV888_601_FULL_RANGE (B2R2_VMX2_RGB_TO_YUV_601_FULL_RANGE)
+#define B2R2_VMX2_RGB_TO_BLT_YUV888_601_FULL_RANGE (B2R2_VMX0_RGB_TO_YUV_601_FULL_RANGE)
+#define B2R2_VMX3_RGB_TO_BLT_YUV888_601_FULL_RANGE 0x00020080
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_RGB_TO_BLT_YUV888_601_STANDARD (B2R2_VMX1_RGB_TO_YUV_601_STANDARD)
+#define B2R2_VMX1_RGB_TO_BLT_YUV888_601_STANDARD (B2R2_VMX2_RGB_TO_YUV_601_STANDARD)
+#define B2R2_VMX2_RGB_TO_BLT_YUV888_601_STANDARD (B2R2_VMX0_RGB_TO_YUV_601_STANDARD)
+#define B2R2_VMX3_RGB_TO_BLT_YUV888_601_STANDARD 0x00020080
+
+/* VMX register values for BLT_YUV888 to RGB conversion */
+
+/*
+ * Note: BLT_YUV888 -> RGB values are calculated by multiplying
+ * the YUV -> RGB matrix [A], with [S] to form [A]x[S] where
+ *     |0 0 1|
+ * S = |1 0 0|
+ *     |0 1 0|
+ * Essentially changing the order of columns in the original
+ * matrix [A].
+ * col1 -> col3
+ * col2 -> col1
+ * col3 -> col2
+ * Values in the offset vector remain unchanged.
+ */
+/* 601 Full range conversion matrix */
+#define B2R2_VMX0_BLT_YUV888_TO_RGB_601_FULL_RANGE 0x20000121
+#define B2R2_VMX1_BLT_YUV888_TO_RGB_601_FULL_RANGE 0x201ea74c
+#define B2R2_VMX2_BLT_YUV888_TO_RGB_601_FULL_RANGE 0x2006f000
+#define B2R2_VMX3_BLT_YUV888_TO_RGB_601_FULL_RANGE (B2R2_VMX3_YUV_TO_RGB_601_FULL_RANGE)
+
+/* 601 Standard (clamped) conversion matrix */
+#define B2R2_VMX0_BLT_YUV888_TO_RGB_601_STANDARD 0x25400133
+#define B2R2_VMX1_BLT_YUV888_TO_RGB_601_STANDARD 0x255E7330
+#define B2R2_VMX2_BLT_YUV888_TO_RGB_601_STANDARD 0x25481400
+#define B2R2_VMX3_BLT_YUV888_TO_RGB_601_STANDARD (B2R2_VMX3_YUV_TO_RGB_601_FULL_RANGE)
+
+/* VMX register values for YUV to BLT_YUV888 conversion */
+#define B2R2_VMX0_YUV_TO_BLT_YUV888 0x00040000
+#define B2R2_VMX1_YUV_TO_BLT_YUV888 0x00000100
+#define B2R2_VMX2_YUV_TO_BLT_YUV888 0x20000000
+#define B2R2_VMX3_YUV_TO_BLT_YUV888 0x00000000
+
+/* VMX register values for BLT_YUV888 to YUV conversion */
+#define B2R2_VMX0_BLT_YUV888_TO_YUV 0x00000100
+#define B2R2_VMX1_BLT_YUV888_TO_YUV 0x20000000
+#define B2R2_VMX2_BLT_YUV888_TO_YUV 0x00040000
+#define B2R2_VMX3_BLT_YUV888_TO_YUV 0x00000000
+
+/* VMX register values for YVU to BLT_YUV888 conversion */
+#define B2R2_VMX0_YVU_TO_BLT_YUV888 0x00040000
+#define B2R2_VMX1_YVU_TO_BLT_YUV888 0x20000000
+#define B2R2_VMX2_YVU_TO_BLT_YUV888 0x00000100
+#define B2R2_VMX3_YVU_TO_BLT_YUV888 0x00000000
+
+/* VMX register values for BLT_YUV888 to YVU conversion */
+#define B2R2_VMX0_BLT_YUV888_TO_YVU 0x00040000
+#define B2R2_VMX1_BLT_YUV888_TO_YVU 0x20000000
+#define B2R2_VMX2_BLT_YUV888_TO_YVU 0x00000100
+#define B2R2_VMX3_BLT_YUV888_TO_YVU 0x00000000
+
+/*
+ * Internal types
+ */
+
+/*
+ * Global variables
+ */
+
+ /**
+ * VMx values for color space conversion
+ * (component swap)
+ */
+static const u32 vmx_yuv_to_blt_yuv888[] = {
+	B2R2_VMX0_YUV_TO_BLT_YUV888,
+	B2R2_VMX1_YUV_TO_BLT_YUV888,
+	B2R2_VMX2_YUV_TO_BLT_YUV888,
+	B2R2_VMX3_YUV_TO_BLT_YUV888,
+};
+
+static const u32 vmx_blt_yuv888_to_yuv[] = {
+	B2R2_VMX0_BLT_YUV888_TO_YUV,
+	B2R2_VMX1_BLT_YUV888_TO_YUV,
+	B2R2_VMX2_BLT_YUV888_TO_YUV,
+	B2R2_VMX3_BLT_YUV888_TO_YUV,
+};
+
+static const u32 vmx_yvu_to_blt_yuv888[] = {
+	B2R2_VMX0_YVU_TO_BLT_YUV888,
+	B2R2_VMX1_YVU_TO_BLT_YUV888,
+	B2R2_VMX2_YVU_TO_BLT_YUV888,
+	B2R2_VMX3_YVU_TO_BLT_YUV888,
+};
+
+static const u32 vmx_blt_yuv888_to_yvu[] = {
+	B2R2_VMX0_BLT_YUV888_TO_YVU,
+	B2R2_VMX1_BLT_YUV888_TO_YVU,
+	B2R2_VMX2_BLT_YUV888_TO_YVU,
+	B2R2_VMX3_BLT_YUV888_TO_YVU,
+};
+
+static const u32 vmx_rgb_to_bgr[] = {
+	B2R2_VMX0_RGB_TO_BGR,
+	B2R2_VMX1_RGB_TO_BGR,
+	B2R2_VMX2_RGB_TO_BGR,
+	B2R2_VMX3_RGB_TO_BGR,
+};
+
+static const u32 vmx_yvu_to_yuv[] = {
+	B2R2_VMX0_YVU_TO_YUV,
+	B2R2_VMX1_YVU_TO_YUV,
+	B2R2_VMX2_YVU_TO_YUV,
+	B2R2_VMX3_YVU_TO_YUV,
+};
+
+/**
+ * VMx values for color space conversions
+ * (standard 601 conversions)
+ */
+static const u32 vmx_rgb_to_yuv[] = {
+	B2R2_VMX0_RGB_TO_YUV_601_STANDARD,
+	B2R2_VMX1_RGB_TO_YUV_601_STANDARD,
+	B2R2_VMX2_RGB_TO_YUV_601_STANDARD,
+	B2R2_VMX3_RGB_TO_YUV_601_STANDARD,
+};
+
+static const u32 vmx_yuv_to_rgb[] = {
+	B2R2_VMX0_YUV_TO_RGB_601_STANDARD,
+	B2R2_VMX1_YUV_TO_RGB_601_STANDARD,
+	B2R2_VMX2_YUV_TO_RGB_601_STANDARD,
+	B2R2_VMX3_YUV_TO_RGB_601_STANDARD,
+};
+
+static const u32 vmx_rgb_to_blt_yuv888[] = {
+	B2R2_VMX0_RGB_TO_BLT_YUV888_601_STANDARD,
+	B2R2_VMX1_RGB_TO_BLT_YUV888_601_STANDARD,
+	B2R2_VMX2_RGB_TO_BLT_YUV888_601_STANDARD,
+	B2R2_VMX3_RGB_TO_BLT_YUV888_601_STANDARD,
+};
+
+static const u32 vmx_blt_yuv888_to_rgb[] = {
+	B2R2_VMX0_BLT_YUV888_TO_RGB_601_STANDARD,
+	B2R2_VMX1_BLT_YUV888_TO_RGB_601_STANDARD,
+	B2R2_VMX2_BLT_YUV888_TO_RGB_601_STANDARD,
+	B2R2_VMX3_BLT_YUV888_TO_RGB_601_STANDARD,
+};
+
+static const u32 vmx_rgb_to_yvu[] = {
+	B2R2_VMX0_RGB_TO_YVU_601_STANDARD,
+	B2R2_VMX1_RGB_TO_YVU_601_STANDARD,
+	B2R2_VMX2_RGB_TO_YVU_601_STANDARD,
+	B2R2_VMX3_RGB_TO_YVU_601_STANDARD,
+};
+
+static const u32 vmx_yvu_to_rgb[] = {
+	B2R2_VMX0_YVU_TO_RGB_601_STANDARD,
+	B2R2_VMX1_YVU_TO_RGB_601_STANDARD,
+	B2R2_VMX2_YVU_TO_RGB_601_STANDARD,
+	B2R2_VMX3_YVU_TO_RGB_601_STANDARD,
+};
+
+static const u32 vmx_bgr_to_yuv[] = {
+	B2R2_VMX0_BGR_TO_YUV_601_STANDARD,
+	B2R2_VMX1_BGR_TO_YUV_601_STANDARD,
+	B2R2_VMX2_BGR_TO_YUV_601_STANDARD,
+	B2R2_VMX3_BGR_TO_YUV_601_STANDARD,
+};
+
+static const u32 vmx_yuv_to_bgr[] = {
+	B2R2_VMX0_YUV_TO_BGR_601_STANDARD,
+	B2R2_VMX1_YUV_TO_BGR_601_STANDARD,
+	B2R2_VMX2_YUV_TO_BGR_601_STANDARD,
+	B2R2_VMX3_YUV_TO_BGR_601_STANDARD,
+};
+
+static const u32 vmx_bgr_to_yvu[] = {
+	B2R2_VMX0_BGR_TO_YVU_601_STANDARD,
+	B2R2_VMX1_BGR_TO_YVU_601_STANDARD,
+	B2R2_VMX2_BGR_TO_YVU_601_STANDARD,
+	B2R2_VMX3_BGR_TO_YVU_601_STANDARD,
+};
+
+static const u32 vmx_yvu_to_bgr[] = {
+	B2R2_VMX0_YVU_TO_BGR_601_STANDARD,
+	B2R2_VMX1_YVU_TO_BGR_601_STANDARD,
+	B2R2_VMX2_YVU_TO_BGR_601_STANDARD,
+	B2R2_VMX3_YVU_TO_BGR_601_STANDARD,
+};
+
+/**
+ * VMx values for color space conversions
+ * (full range conversions)
+ */
+
+static const u32 vmx_full_rgb_to_yuv[] = {
+	B2R2_VMX0_RGB_TO_YUV_601_FULL_RANGE,
+	B2R2_VMX1_RGB_TO_YUV_601_FULL_RANGE,
+	B2R2_VMX2_RGB_TO_YUV_601_FULL_RANGE,
+	B2R2_VMX3_RGB_TO_YUV_601_FULL_RANGE,
+};
+
+static const u32 vmx_full_yuv_to_rgb[] = {
+	B2R2_VMX0_YUV_TO_RGB_601_FULL_RANGE,
+	B2R2_VMX1_YUV_TO_RGB_601_FULL_RANGE,
+	B2R2_VMX2_YUV_TO_RGB_601_FULL_RANGE,
+	B2R2_VMX3_YUV_TO_RGB_601_FULL_RANGE,
+};
+
+static const u32 vmx_full_rgb_to_blt_yuv888[] = {
+	B2R2_VMX0_RGB_TO_BLT_YUV888_601_FULL_RANGE,
+	B2R2_VMX1_RGB_TO_BLT_YUV888_601_FULL_RANGE,
+	B2R2_VMX2_RGB_TO_BLT_YUV888_601_FULL_RANGE,
+	B2R2_VMX3_RGB_TO_BLT_YUV888_601_FULL_RANGE,
+};
+
+static const u32 vmx_full_blt_yuv888_to_rgb[] = {
+	B2R2_VMX0_BLT_YUV888_TO_RGB_601_FULL_RANGE,
+	B2R2_VMX1_BLT_YUV888_TO_RGB_601_FULL_RANGE,
+	B2R2_VMX2_BLT_YUV888_TO_RGB_601_FULL_RANGE,
+	B2R2_VMX3_BLT_YUV888_TO_RGB_601_FULL_RANGE,
+};
+
+static const u32 vmx_full_yvu_to_rgb[] = {
+	B2R2_VMX0_YVU_TO_RGB_601_FULL_RANGE,
+	B2R2_VMX1_YVU_TO_RGB_601_FULL_RANGE,
+	B2R2_VMX2_YVU_TO_RGB_601_FULL_RANGE,
+	B2R2_VMX3_YVU_TO_RGB_601_FULL_RANGE,
+};
+
+static const u32 vmx_full_rgb_to_yvu[] = {
+	B2R2_VMX0_RGB_TO_YVU_601_FULL_RANGE,
+	B2R2_VMX1_RGB_TO_YVU_601_FULL_RANGE,
+	B2R2_VMX2_RGB_TO_YVU_601_FULL_RANGE,
+	B2R2_VMX3_RGB_TO_YVU_601_FULL_RANGE,
+};
+
+static const u32 vmx_full_bgr_to_yuv[] = {
+	B2R2_VMX0_BGR_TO_YUV_601_FULL_RANGE,
+	B2R2_VMX1_BGR_TO_YUV_601_FULL_RANGE,
+	B2R2_VMX2_BGR_TO_YUV_601_FULL_RANGE,
+	B2R2_VMX3_BGR_TO_YUV_601_FULL_RANGE,
+};
+
+static const u32 vmx_full_yuv_to_bgr[] = {
+	B2R2_VMX0_YUV_TO_BGR_601_FULL_RANGE,
+	B2R2_VMX1_YUV_TO_BGR_601_FULL_RANGE,
+	B2R2_VMX2_YUV_TO_BGR_601_FULL_RANGE,
+	B2R2_VMX3_YUV_TO_BGR_601_FULL_RANGE,
+};
+
+static const u32 vmx_full_bgr_to_yvu[] = {
+	B2R2_VMX0_BGR_TO_YVU_601_FULL_RANGE,
+	B2R2_VMX1_BGR_TO_YVU_601_FULL_RANGE,
+	B2R2_VMX2_BGR_TO_YVU_601_FULL_RANGE,
+	B2R2_VMX3_BGR_TO_YVU_601_FULL_RANGE,
+};
+
+static const u32 vmx_full_yvu_to_bgr[] = {
+	B2R2_VMX0_YVU_TO_BGR_601_FULL_RANGE,
+	B2R2_VMX1_YVU_TO_BGR_601_FULL_RANGE,
+	B2R2_VMX2_YVU_TO_BGR_601_FULL_RANGE,
+	B2R2_VMX3_YVU_TO_BGR_601_FULL_RANGE,
+};
+
+/*
+ * Forward declaration of private functions
+ */
+
+/*
+ * Public functions
+ */
+
+/**
+ * Setup input versatile matrix for color space conversion
+ */
+int b2r2_setup_ivmx(struct b2r2_node *node, enum b2r2_color_conversion cc)
+{
+	const u32 *vmx = NULL;
+
+	if (b2r2_get_vmx(cc, &vmx) < 0 || vmx == NULL)
+		return -1;
+
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+
+	node->node.GROUP15.B2R2_VMX0 = vmx[0];
+	node->node.GROUP15.B2R2_VMX1 = vmx[1];
+	node->node.GROUP15.B2R2_VMX2 = vmx[2];
+	node->node.GROUP15.B2R2_VMX3 = vmx[3];
+
+	return 0;
+}
+
+/**
+ * Setup output versatile matrix for color space conversion
+ */
+int b2r2_setup_ovmx(struct b2r2_node *node, enum b2r2_color_conversion cc)
+{
+	const u32 *vmx = NULL;
+
+	if (b2r2_get_vmx(cc, &vmx) < 0 || vmx == NULL)
+		return -1;
+
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_OVMX_ENABLED;
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_OVMX;
+
+	node->node.GROUP16.B2R2_VMX0 = vmx[0];
+	node->node.GROUP16.B2R2_VMX1 = vmx[1];
+	node->node.GROUP16.B2R2_VMX2 = vmx[2];
+	node->node.GROUP16.B2R2_VMX3 = vmx[3];
+
+	return 0;
+}
+
+enum b2r2_color_conversion b2r2_get_color_conversion(enum b2r2_blt_fmt src_fmt,
+		enum b2r2_blt_fmt dst_fmt, bool fullrange)
+{
+	if (b2r2_is_rgb_fmt(src_fmt)) {
+		if (b2r2_is_yvu_fmt(dst_fmt))
+			return fullrange ? B2R2_CC_RGB_TO_YVU_FULL :
+				B2R2_CC_RGB_TO_YVU;
+		else if (dst_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+				dst_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			/*
+			 * (A)YUV/VUY(A) formats differ only in component
+			 * order. This is handled by the endianness bit
+			 * in B2R2_STY/TTY registers when src/target are set.
+			 */
+			return fullrange ? B2R2_CC_RGB_TO_BLT_YUV888_FULL :
+				B2R2_CC_RGB_TO_BLT_YUV888;
+		else if (b2r2_is_yuv_fmt(dst_fmt))
+			return fullrange ? B2R2_CC_RGB_TO_YUV_FULL :
+				B2R2_CC_RGB_TO_YUV;
+		else if (b2r2_is_bgr_fmt(dst_fmt))
+			return B2R2_CC_RGB_TO_BGR;
+	} else if (b2r2_is_yvu_fmt(src_fmt)) {
+		if (b2r2_is_rgb_fmt(dst_fmt))
+			return fullrange ? B2R2_CC_YVU_FULL_TO_RGB :
+				B2R2_CC_YVU_TO_RGB;
+		else if (b2r2_is_bgr_fmt(dst_fmt))
+			return fullrange ? B2R2_CC_YVU_FULL_TO_BGR :
+				B2R2_CC_YVU_TO_BGR;
+		else if (dst_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+				dst_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			return B2R2_CC_YVU_TO_BLT_YUV888;
+		else if (b2r2_is_yuv_fmt(dst_fmt) &&
+				!b2r2_is_yvu_fmt(dst_fmt))
+			return B2R2_CC_YVU_TO_YUV;
+	} else if (src_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+			src_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+			src_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+			src_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888) {
+		/*
+		 * (A)YUV/VUY(A) formats differ only in component
+		 * order. This is handled by the endianness bit
+		 * in B2R2_STY/TTY registers when src/target are set.
+		 */
+		if (b2r2_is_rgb_fmt(dst_fmt))
+			return fullrange ? B2R2_CC_BLT_YUV888_FULL_TO_RGB :
+				B2R2_CC_BLT_YUV888_TO_RGB;
+		else if (b2r2_is_yvu_fmt(dst_fmt))
+			return B2R2_CC_BLT_YUV888_TO_YVU;
+		else if (b2r2_is_yuv_fmt(dst_fmt)) {
+			switch (dst_fmt) {
+			case B2R2_BLT_FMT_24_BIT_YUV888:
+			case B2R2_BLT_FMT_32_BIT_AYUV8888:
+			case B2R2_BLT_FMT_24_BIT_VUY888:
+			case B2R2_BLT_FMT_32_BIT_VUYA8888:
+				return B2R2_CC_NOTHING;
+			default:
+				return B2R2_CC_BLT_YUV888_TO_YUV;
+			}
+		}
+	} else if (b2r2_is_yuv_fmt(src_fmt)) {
+		if (b2r2_is_rgb_fmt(dst_fmt))
+			return fullrange ? B2R2_CC_YUV_FULL_TO_RGB :
+				B2R2_CC_YUV_TO_RGB;
+		else if (b2r2_is_bgr_fmt(dst_fmt))
+			return fullrange ? B2R2_CC_YUV_FULL_TO_BGR :
+				B2R2_CC_YUV_TO_BGR;
+		else if (dst_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+				dst_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			return B2R2_CC_YUV_TO_BLT_YUV888;
+		else if (b2r2_is_yvu_fmt(dst_fmt))
+			return B2R2_CC_YVU_TO_YUV;
+	} else if (b2r2_is_bgr_fmt(src_fmt)) {
+		if (b2r2_is_rgb_fmt(dst_fmt))
+			return B2R2_CC_RGB_TO_BGR;
+		else if (b2r2_is_yvu_fmt(dst_fmt))
+			return fullrange ? B2R2_CC_BGR_TO_YVU_FULL :
+				B2R2_CC_BGR_TO_YVU;
+		else if (dst_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+				dst_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			BUG_ON(1);
+		else if (b2r2_is_yuv_fmt(dst_fmt))
+			return fullrange ? B2R2_CC_BGR_TO_YUV_FULL :
+				B2R2_CC_BGR_TO_YUV;
+	}
+
+	return B2R2_CC_NOTHING;
+}
+
+int b2r2_get_vmx(enum b2r2_color_conversion cc, const u32 **vmx)
+{
+	if (vmx == NULL)
+		return -1;
+
+	switch (cc) {
+	case B2R2_CC_RGB_TO_BGR:
+		*vmx = &vmx_rgb_to_bgr[0];
+		break;
+	case B2R2_CC_BLT_YUV888_TO_YVU:
+		*vmx = &vmx_blt_yuv888_to_yvu[0];
+		break;
+	case B2R2_CC_BLT_YUV888_TO_YUV:
+		*vmx = &vmx_blt_yuv888_to_yuv[0];
+		break;
+	case B2R2_CC_YVU_TO_YUV:
+		*vmx = &vmx_yvu_to_yuv[0];
+		break;
+	case B2R2_CC_YVU_TO_BLT_YUV888:
+		*vmx = &vmx_yvu_to_blt_yuv888[0];
+		break;
+	case B2R2_CC_YUV_TO_BLT_YUV888:
+		*vmx = &vmx_yuv_to_blt_yuv888[0];
+		break;
+	case B2R2_CC_RGB_TO_YUV:
+		*vmx = &vmx_rgb_to_yuv[0];
+		break;
+	case B2R2_CC_RGB_TO_YUV_FULL:
+		*vmx = &vmx_full_rgb_to_yuv[0];
+		break;
+	case B2R2_CC_RGB_TO_YVU:
+		*vmx = &vmx_rgb_to_yvu[0];
+		break;
+	case B2R2_CC_RGB_TO_YVU_FULL:
+		*vmx = &vmx_full_rgb_to_yvu[0];
+		break;
+	case B2R2_CC_RGB_TO_BLT_YUV888:
+		*vmx = &vmx_rgb_to_blt_yuv888[0];
+		break;
+	case B2R2_CC_RGB_TO_BLT_YUV888_FULL:
+		*vmx = &vmx_full_rgb_to_blt_yuv888[0];
+		break;
+	case B2R2_CC_BGR_TO_YVU:
+		*vmx = &vmx_bgr_to_yvu[0];
+		break;
+	case B2R2_CC_BGR_TO_YVU_FULL:
+		*vmx = &vmx_full_bgr_to_yvu[0];
+		break;
+	case B2R2_CC_BGR_TO_YUV:
+		*vmx = &vmx_bgr_to_yuv[0];
+		break;
+	case B2R2_CC_BGR_TO_YUV_FULL:
+		*vmx = &vmx_full_bgr_to_yuv[0];
+		break;
+	case B2R2_CC_YUV_TO_RGB:
+		*vmx = &vmx_yuv_to_rgb[0];
+		break;
+	case B2R2_CC_YUV_FULL_TO_RGB:
+		*vmx = &vmx_full_yuv_to_rgb[0];
+		break;
+	case B2R2_CC_YUV_TO_BGR:
+		*vmx = &vmx_yuv_to_bgr[0];
+		break;
+	case B2R2_CC_YUV_FULL_TO_BGR:
+		*vmx = &vmx_full_yuv_to_bgr[0];
+		break;
+	case B2R2_CC_YVU_TO_RGB:
+		*vmx = &vmx_yvu_to_rgb[0];
+		break;
+	case B2R2_CC_YVU_FULL_TO_RGB:
+		*vmx = &vmx_full_yvu_to_rgb[0];
+		break;
+	case B2R2_CC_YVU_TO_BGR:
+		*vmx = &vmx_yvu_to_bgr[0];
+		break;
+	case B2R2_CC_YVU_FULL_TO_BGR:
+		*vmx = &vmx_full_yvu_to_bgr[0];
+		break;
+	case B2R2_CC_BLT_YUV888_TO_RGB:
+		*vmx = &vmx_blt_yuv888_to_rgb[0];
+		break;
+	case B2R2_CC_BLT_YUV888_FULL_TO_RGB:
+		*vmx = &vmx_full_blt_yuv888_to_rgb[0];
+		break;
+	case B2R2_CC_NOTHING:
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+
diff --git a/drivers/video/b2r2/b2r2_hw_convert.h b/drivers/video/b2r2/b2r2_hw_convert.h
new file mode 100644
index 00000000000..e173e898e44
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_hw_convert.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2012
+ *
+ * ST-Ericsson B2R2 hw color conversion definitions
+ *
+ * Author: Jorgen Nilsson <jorgen.nilsson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef B2R2_HW_CONVERT_H__
+#define B2R2_HW_CONVERT_H__
+
+#include "b2r2_internal.h"
+
+enum b2r2_color_conversion {
+	B2R2_CC_NOTHING = 0,
+	B2R2_CC_RGB_TO_BGR,
+	B2R2_CC_BLT_YUV888_TO_YVU,
+	B2R2_CC_BLT_YUV888_TO_YUV,
+	B2R2_CC_YUV_TO_BLT_YUV888,
+	B2R2_CC_YVU_TO_YUV,
+	B2R2_CC_YVU_TO_BLT_YUV888,
+	B2R2_CC_RGB_TO_YUV,
+	B2R2_CC_RGB_TO_YUV_FULL,
+	B2R2_CC_RGB_TO_YVU,
+	B2R2_CC_RGB_TO_YVU_FULL,
+	B2R2_CC_RGB_TO_BLT_YUV888,
+	B2R2_CC_RGB_TO_BLT_YUV888_FULL,
+	B2R2_CC_BGR_TO_YVU,
+	B2R2_CC_BGR_TO_YVU_FULL,
+	B2R2_CC_BGR_TO_YUV,
+	B2R2_CC_BGR_TO_YUV_FULL,
+	B2R2_CC_YUV_TO_RGB,
+	B2R2_CC_YUV_FULL_TO_RGB,
+	B2R2_CC_YUV_TO_BGR,
+	B2R2_CC_YUV_FULL_TO_BGR,
+	B2R2_CC_YVU_TO_RGB,
+	B2R2_CC_YVU_FULL_TO_RGB,
+	B2R2_CC_YVU_TO_BGR,
+	B2R2_CC_YVU_FULL_TO_BGR,
+	B2R2_CC_BLT_YUV888_TO_RGB,
+	B2R2_CC_BLT_YUV888_FULL_TO_RGB,
+};
+
+int b2r2_setup_ivmx(struct b2r2_node *node, enum b2r2_color_conversion cc);
+int b2r2_setup_ovmx(struct b2r2_node *node, enum b2r2_color_conversion cc);
+enum b2r2_color_conversion b2r2_get_color_conversion(enum b2r2_blt_fmt src_fmt,
+		enum b2r2_blt_fmt dst_fmt, bool fullrange);
+int b2r2_get_vmx(enum b2r2_color_conversion cc, const u32 **vmx);
+
+#endif /* B2R2_HW_CONVERT_H__ */
diff --git a/drivers/video/b2r2/b2r2_input_validation.c b/drivers/video/b2r2/b2r2_input_validation.c
new file mode 100644
index 00000000000..c8eb2f7b025
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_input_validation.c
@@ -0,0 +1,496 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson
+ *
+ * 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 "b2r2_internal.h"
+#include "b2r2_input_validation.h"
+#include "b2r2_debug.h"
+#include "b2r2_utils.h"
+
+#include <video/b2r2_blt.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+
+
+static bool is_valid_format(enum b2r2_blt_fmt fmt);
+static bool is_valid_bg_format(enum b2r2_blt_fmt fmt);
+
+static bool is_valid_pitch_for_fmt(struct device *dev,
+		u32 pitch, s32 width, enum b2r2_blt_fmt fmt);
+
+static bool is_aligned_width_for_fmt(s32 width, enum b2r2_blt_fmt fmt);
+static s32 width_2_complete_width(s32 width, enum b2r2_blt_fmt fmt);
+static bool is_complete_width_for_fmt(s32 width, enum b2r2_blt_fmt fmt);
+static bool is_valid_height_for_fmt(s32 height, enum b2r2_blt_fmt fmt);
+
+static bool validate_img(struct device *dev,
+		struct b2r2_blt_img *img);
+static bool validate_rect(struct device *dev,
+		struct b2r2_blt_rect *rect);
+
+
+static bool is_valid_format(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+static bool is_valid_bg_format(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return false;
+	default:
+		return true;
+	}
+}
+
+
+static bool is_valid_pitch_for_fmt(struct device *dev,
+		u32 pitch, s32 width, enum b2r2_blt_fmt fmt)
+{
+	s32 complete_width;
+	u32 pitch_derived_from_width;
+
+	complete_width =  width_2_complete_width(width, fmt);
+
+	pitch_derived_from_width = b2r2_calc_pitch_from_width(dev,
+			complete_width, fmt);
+
+	if (pitch < pitch_derived_from_width)
+		return false;
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		if (!b2r2_is_aligned(pitch, 2))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		if (!b2r2_is_aligned(pitch, 4))
+			return false;
+
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+
+static bool is_aligned_width_for_fmt(s32 width, enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+		if (!b2r2_is_aligned(width, 4))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_1_BIT_A1:
+		if (!b2r2_is_aligned(width, 8))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		if (!b2r2_is_aligned(width, 2))
+			return false;
+
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static s32 width_2_complete_width(s32 width, enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return b2r2_align_up(width, 2);
+
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return b2r2_align_up(width, 16);
+
+	default:
+		return width;
+	}
+}
+
+static bool is_complete_width_for_fmt(s32 width, enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		if (!b2r2_is_aligned(width, 2))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		if (!b2r2_is_aligned(width, 16))
+			return false;
+
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static bool is_valid_height_for_fmt(s32 height, enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		if (!b2r2_is_aligned(height, 2))
+			return false;
+
+		break;
+
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		if (!b2r2_is_aligned(height, 16))
+			return false;
+
+		break;
+
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static bool validate_img(struct device *dev,
+		struct b2r2_blt_img *img)
+{
+	/*
+	 * So that we always can do width * height * bpp without overflowing a
+	 * 32 bit signed integer. isqrt(s32_max / max_bpp) was used to
+	 * calculate the value.
+	 */
+	static const s32 max_img_width_height = 8191;
+
+	s32 img_size;
+
+	if (!is_valid_format(img->fmt)) {
+		b2r2_log_info(dev, "Validation Error: "
+				"!is_valid_format(img->fmt)\n");
+		return false;
+	}
+
+	if (img->width < 0 || img->width > max_img_width_height ||
+		img->height < 0 || img->height > max_img_width_height) {
+		b2r2_log_info(dev, "Validation Error: "
+				"img->width < 0 || "
+				"img->width > max_img_width_height || "
+				"img->height < 0 || "
+				"img->height > max_img_width_height\n");
+		return false;
+	}
+
+	if (b2r2_is_mb_fmt(img->fmt)) {
+		if (!is_complete_width_for_fmt(img->width, img->fmt)) {
+			b2r2_log_info(dev, "Validation Error: "
+					"!is_complete_width_for_fmt(img->width,"
+					" img->fmt)\n");
+			return false;
+		}
+	} else {
+		if (0 == img->pitch &&
+			(!is_aligned_width_for_fmt(img->width, img->fmt) ||
+			!is_complete_width_for_fmt(img->width, img->fmt))) {
+			b2r2_log_info(dev,
+					"Validation Error: "
+					"0 == img->pitch && "
+					"(!is_aligned_width_for_fmt(img->width,"
+					" img->fmt) || "
+					"!is_complete_width_for_fmt(img->width,"
+					" img->fmt))\n");
+			return false;
+		}
+
+		if (img->pitch != 0 &&
+			!is_valid_pitch_for_fmt(dev, img->pitch, img->width,
+				img->fmt)) {
+			b2r2_log_info(dev,
+				"Validation Error: "
+				"img->pitch != 0 && "
+				"!is_valid_pitch_for_fmt(dev, "
+				"img->pitch, img->width, img->fmt)\n");
+			return false;
+		}
+	}
+
+	if (!is_valid_height_for_fmt(img->width, img->fmt)) {
+		b2r2_log_info(dev, "Validation Error: "
+				"!is_valid_height_for_fmt(img->width, img->fmt)\n");
+		return false;
+	}
+
+	img_size = b2r2_get_img_size(dev, img);
+
+	/*
+	 * To keep the entire image inside s32 range.
+	 */
+	if ((B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET == img->buf.type ||
+				B2R2_BLT_PTR_FD_OFFSET == img->buf.type) &&
+			img->buf.offset > (u32)b2r2_s32_max - (u32)img_size) {
+		b2r2_log_info(dev, "Validation Error: "
+				"(B2R2_BLT_PTR_HWMEM_BUF_NAME_OFFSET == "
+				"img->buf.type || B2R2_BLT_PTR_FD_OFFSET == "
+				"img->buf.type) && img->buf.offset > "
+				"(u32)B2R2_MAX_S32 - (u32)img_size\n");
+		return false;
+	}
+
+	return true;
+}
+
+static bool validate_rect(struct device *dev,
+		struct b2r2_blt_rect *rect)
+{
+	if (rect->width < 0 || rect->height < 0) {
+		b2r2_log_info(dev, "Validation Error: "
+				"rect->width < 0 || rect->height < 0\n");
+		return false;
+	}
+
+	return true;
+}
+
+bool b2r2_validate_user_req(struct device *dev,
+		struct b2r2_blt_req *req)
+{
+	bool is_src_img_used;
+	bool is_bg_img_used;
+	bool is_src_mask_used;
+	bool is_dst_clip_rect_used;
+
+	if (req->size != sizeof(struct b2r2_blt_req)) {
+		b2r2_log_err(dev, "Validation Error: "
+				"req->size != sizeof(struct b2r2_blt_req)\n");
+		return false;
+	}
+
+	is_src_img_used = !(req->flags & B2R2_BLT_FLAG_SOURCE_FILL ||
+		req->flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW);
+	is_bg_img_used = (req->flags & B2R2_BLT_FLAG_BG_BLEND);
+	is_src_mask_used = req->flags & B2R2_BLT_FLAG_SOURCE_MASK;
+	is_dst_clip_rect_used = req->flags & B2R2_BLT_FLAG_DESTINATION_CLIP;
+
+	if (is_src_img_used || is_src_mask_used) {
+		if (!validate_rect(dev, &req->src_rect)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!validate_rect(dev, &req->src_rect)\n");
+			return false;
+		}
+	}
+
+	if (!validate_rect(dev, &req->dst_rect)) {
+		b2r2_log_info(dev, "Validation Error: "
+			"!validate_rect(dev, &req->dst_rect)\n");
+		return false;
+	}
+
+	if (is_bg_img_used)	{
+		if (!validate_rect(dev, &req->bg_rect)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!validate_rect(dev, &req->bg_rect)\n");
+			return false;
+		}
+	}
+
+	if (is_dst_clip_rect_used) {
+		if (!validate_rect(dev, &req->dst_clip_rect)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!validate_rect(dev, &req->dst_clip_rect)\n");
+			return false;
+		}
+	}
+
+	if (is_src_img_used) {
+		struct b2r2_blt_rect src_img_bounding_rect;
+
+		if (!validate_img(dev, &req->src_img)) {
+			b2r2_log_info(dev, "Validation Error: "
+					"!validate_img(dev, &req->src_img)\n");
+			return false;
+		}
+
+		b2r2_get_img_bounding_rect(&req->src_img,
+				&src_img_bounding_rect);
+		if (!b2r2_is_rect_inside_rect(&req->src_rect,
+				&src_img_bounding_rect)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!b2r2_is_rect_inside_rect(&req->src_rect, "
+				"&src_img_bounding_rect)\n");
+			return false;
+		}
+	}
+
+	if (is_bg_img_used) {
+		struct b2r2_blt_rect bg_img_bounding_rect;
+
+		if (!validate_img(dev, &req->bg_img)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!validate_img(dev, &req->bg_img)\n");
+			return false;
+		}
+
+		if (!is_valid_bg_format(req->bg_img.fmt)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!is_valid_bg_format(req->bg_img->fmt)\n");
+			return false;
+		}
+
+		b2r2_get_img_bounding_rect(&req->bg_img,
+			&bg_img_bounding_rect);
+		if (!b2r2_is_rect_inside_rect(&req->bg_rect,
+				&bg_img_bounding_rect)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!b2r2_is_rect_inside_rect(&req->bg_rect, "
+				"&bg_img_bounding_rect)\n");
+			return false;
+		}
+	}
+
+	if (is_src_mask_used) {
+		struct b2r2_blt_rect src_mask_bounding_rect;
+
+		if (!validate_img(dev, &req->src_mask)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!validate_img(dev, &req->src_mask)\n");
+			return false;
+		}
+
+		b2r2_get_img_bounding_rect(&req->src_mask,
+			&src_mask_bounding_rect);
+		if (!b2r2_is_rect_inside_rect(&req->src_rect,
+					&src_mask_bounding_rect)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!b2r2_is_rect_inside_rect(&req->src_rect, "
+				"&src_mask_bounding_rect)\n");
+			return false;
+		}
+	}
+
+	if (!validate_img(dev, &req->dst_img)) {
+		b2r2_log_info(dev, "Validation Error: "
+			"!validate_img(dev, &req->dst_img)\n");
+		return false;
+	}
+
+	if (is_bg_img_used)	{
+		if (!b2r2_is_rect_gte_rect(&req->bg_rect, &req->dst_rect)) {
+			b2r2_log_info(dev, "Validation Error: "
+				"!b2r2_is_rect_gte_rect(&req->bg_rect, "
+				"&req->dst_rect)\n");
+			return false;
+		}
+	}
+
+	return true;
+}
diff --git a/drivers/video/b2r2/b2r2_input_validation.h b/drivers/video/b2r2/b2r2_input_validation.h
new file mode 100644
index 00000000000..25f022a45ab
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_input_validation.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson
+ *
+ * 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 _LINUX_DRIVERS_VIDEO_B2R2_INPUT_VALIDATION_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_INPUT_VALIDATION_H_
+
+#include <video/b2r2_blt.h>
+
+#include "b2r2_internal.h"
+
+bool b2r2_validate_user_req(struct device *dev,
+		struct b2r2_blt_req *req);
+
+#endif
diff --git a/drivers/video/b2r2/b2r2_internal.h b/drivers/video/b2r2/b2r2_internal.h
new file mode 100644
index 00000000000..329d644e5e1
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_internal.h
@@ -0,0 +1,610 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 internal definitions
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_INTERNAL_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_INTERNAL_H_
+
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <video/b2r2_blt.h>
+
+#include "b2r2_global.h"
+#include "b2r2_hw.h"
+
+/**
+ * B2R2_MAX_NBR_DEVICES - The maximum number of B2R2s handled
+ */
+#define B2R2_MAX_NBR_DEVICES 2
+
+/* The maximum possible number of temporary buffers needed */
+#define MAX_TMP_BUFS_NEEDED 2
+
+/* Size of the color look-up table */
+#define CLUT_SIZE 1024
+
+/* The defined bits of the Interrupt Status Register */
+#define B2R2_ITS_MASK 0x0FFFF0FF
+
+/**
+ * b2r2_op_type - the type of B2R2 operation to configure
+ */
+enum b2r2_op_type {
+	B2R2_DIRECT_COPY,
+	B2R2_DIRECT_FILL,
+	B2R2_COPY,
+	B2R2_FILL,
+	B2R2_SCALE,
+	B2R2_ROTATE,
+	B2R2_SCALE_AND_ROTATE,
+	B2R2_FLIP,
+};
+
+/**
+ * b2r2_fmt_type - the type of buffer for a given format
+ */
+enum b2r2_fmt_type {
+	B2R2_FMT_TYPE_RASTER,
+	B2R2_FMT_TYPE_SEMI_PLANAR,
+	B2R2_FMT_TYPE_PLANAR,
+};
+
+/**
+ * b2r2_fmt_conv - the type of format conversion to do
+ */
+enum b2r2_fmt_conv {
+	B2R2_FMT_CONV_NONE,
+	B2R2_FMT_CONV_RGB_TO_YUV,
+	B2R2_FMT_CONV_YUV_TO_RGB,
+	B2R2_FMT_CONV_YUV_TO_YUV,
+	B2R2_FMT_CONV_RGB_TO_BGR,
+	B2R2_FMT_CONV_BGR_TO_RGB,
+	B2R2_FMT_CONV_YUV_TO_BGR,
+	B2R2_FMT_CONV_BGR_TO_YUV,
+};
+
+/**
+ * enum b2r2_core_queue - Indicates the B2R2 queue that the job belongs to
+ *
+ * @B2R2_CORE_QUEUE_AQ1: Application queue 1
+ * @B2R2_CORE_QUEUE_AQ2: Application queue 2
+ * @B2R2_CORE_QUEUE_AQ3: Application queue 3
+ * @B2R2_CORE_QUEUE_AQ4: Application queue 4
+ * @B2R2_CORE_QUEUE_CQ1: Composition queue 1
+ * @B2R2_CORE_QUEUE_CQ2: Composition queue 2
+ * @B2R2_CORE_QUEUE_NO_OF: Number of queues
+ */
+enum b2r2_core_queue {
+	B2R2_CORE_QUEUE_AQ1 = 0,
+	B2R2_CORE_QUEUE_AQ2,
+	B2R2_CORE_QUEUE_AQ3,
+	B2R2_CORE_QUEUE_AQ4,
+	B2R2_CORE_QUEUE_CQ1,
+	B2R2_CORE_QUEUE_CQ2,
+	B2R2_CORE_QUEUE_NO_OF,
+};
+
+#define B2R2_NUM_APPLICATIONS_QUEUES 4
+
+/**
+ * enum b2r2_core_job_state - Indicates the current state of the job
+ *
+ * @B2R2_CORE_JOB_IDLE: Never queued
+ * @B2R2_CORE_JOB_QUEUED: In queue but not started yet
+ * @B2R2_CORE_JOB_RUNNING: Running, executed by B2R2
+ * @B2R2_CORE_JOB_DONE: Completed
+ * @B2R2_CORE_JOB_CANCELED: Canceled
+ */
+enum b2r2_core_job_state {
+	B2R2_CORE_JOB_IDLE = 0,
+	B2R2_CORE_JOB_QUEUED,
+	B2R2_CORE_JOB_RUNNING,
+	B2R2_CORE_JOB_DONE,
+	B2R2_CORE_JOB_CANCELED,
+};
+
+/**
+ * b2r2_work_buf - specification for a temporary work buffer
+ *
+ * @size      - the size of the buffer (set by b2r2_node_split)
+ * @phys_addr - the physical address of the buffer (set by b2r2_blt_main)
+ */
+struct b2r2_work_buf {
+	u32 size;
+	u32 phys_addr;
+	void *virt_addr;
+	u32 mem_handle;
+};
+
+struct tmp_buf {
+	struct b2r2_work_buf buf;
+	bool in_use;
+};
+
+/**
+ * struct b2r2_control_instance - Represents the B2R2 instance
+ *                                (one per open and blitter core)
+ *
+ * @lock: Lock to protect the instance
+ * @control_id: The b2r2 core core control identifier
+ * @control: The b2r2 core control entity
+ *
+ * @report_list: Ready requests that should be reported,
+ * @report_list_waitq: Wait queue for report list
+ * @no_of_active_requests: Number of requests added but not reported
+ *                         in callback.
+ * @synching: true if any client is waiting for b2r2_blt_synch(0)
+ * @synch_done_waitq: Wait queue to handle synching on request_id 0
+ */
+struct b2r2_control_instance {
+	struct mutex lock;
+	int control_id;
+	struct b2r2_control *control;
+
+	/* Requests to be reported */
+	struct list_head report_list;
+	wait_queue_head_t report_list_waitq;
+
+	/* Below for synching */
+	u32 no_of_active_requests;
+	bool synching;
+	wait_queue_head_t synch_done_waitq;
+};
+
+/**
+ * struct b2r2_node - Represents a B2R2 node with reqister values, executed
+ *                    by B2R2. Should be allocated non-cached.
+ *
+ * @next: Next node
+ * @physical_address: Physical address to be given to B2R2
+ *                    (physical address of "node" member below)
+ * @node: The B2R2 node with register settings. This is the data
+ *        that B2R2 will use.
+ *
+ */
+struct b2r2_node {
+	struct b2r2_node *next;
+	u32 physical_address;
+
+	int src_tmp_index;
+	int dst_tmp_index;
+
+	int src_index;
+
+	/* B2R2 regs comes here */
+	struct b2r2_link_list node;
+};
+
+/**
+ * struct b2r2_resolved_buf - Contains calculated information about
+ *                            image buffers.
+ *
+ * @physical_address: Physical address of the buffer
+ * @virtual_address: Virtual address of the buffer
+ * @is_pmem: true if buffer is from pmem
+ * @hwmem_session: Hwmem session
+ * @hwmem_alloc: Hwmem alloc
+ * @filep: File pointer of mapped file (like pmem device, frame buffer device)
+ * @file_physical_start: Physical address of file start
+ * @file_virtual_start: Virtual address of file start
+ * @file_len: File len
+ *
+ */
+struct b2r2_resolved_buf {
+	u32                   physical_address;
+	void                 *virtual_address;
+	bool                  is_pmem;
+	struct hwmem_alloc   *hwmem_alloc;
+	/* Data for validation below */
+	struct file          *filep;
+	u32                   file_physical_start;
+	u32                   file_virtual_start;
+	u32                   file_len;
+};
+
+/**
+ * b2r2_node_split_buf - information about a source or destination buffer
+ *
+ * @addr            - the physical base address
+ * @chroma_addr     - the physical address of the chroma plane
+ * @chroma_cr_addr  - the physical address of the Cr chroma plane
+ * @fmt             - the buffer format
+ * @fmt_type        - the buffer format type
+ * @rect            - the rectangle of the buffer to use
+ * @color           - the color value to use is case of a fill operation
+ * @pitch           - the pixmap byte pitch
+ * @height          - the pixmap height
+ * @alpha_range     - the alpha range of the buffer (0-128 or 0-255)
+ * @hso             - the horizontal scan order
+ * @vso             - the vertical scan order
+ * @endian          - the endianess of the buffer
+ * @plane_selection - the plane to write if buffer is planar or semi-planar
+ */
+struct b2r2_node_split_buf {
+	u32 addr;
+	u32 chroma_addr;
+	u32 chroma_cr_addr;
+
+	enum b2r2_blt_fmt fmt;
+	enum b2r2_fmt_type type;
+
+	struct b2r2_blt_rect rect;
+	struct b2r2_blt_rect win;
+
+	s32 dx;
+	s32 dy;
+
+	u32 color;
+	u16 pitch;
+	u16 width;
+	u16 height;
+
+	enum b2r2_ty alpha_range;
+	enum b2r2_ty hso;
+	enum b2r2_ty vso;
+	enum b2r2_ty endian;
+	enum b2r2_tty dither;
+
+	/* Plane selection (used when writing to a multibuffer format) */
+	enum b2r2_tty plane_selection;
+
+	/* Chroma plane selection (used when writing planar formats) */
+	enum b2r2_tty chroma_selection;
+
+	int tmp_buf_index;
+};
+
+/**
+ * b2r2_node_split_job - an instance of a node split job
+ *
+ * @type          - the type of operation
+ * @ivmx          - the ivmx matrix to use for color conversion
+ * @blend         - determines if blending is enabled
+ * @clip          - determines if destination clipping is enabled
+ * @rotation      - determines if rotation is requested
+ * @fullrange     - determines YUV<->RGB conversion matrix (iVMx)
+ * @swap_fg_bg    - determines if FG and BG should be swapped when blending
+ * @flags         - the flags passed in the blt request
+ * @flag_param    - parameter required by certain flags,
+ *                  e.g. color for source color keying.
+ * @transform     - the transforms passed in the blt request
+ * @global_alpha  - the global alpha
+ * @clip_rect     - the clipping rectangle to use
+ * @horiz_rescale - determmines if horizontal rescaling is enabled
+ * @horiz_sf      - the horizontal scale factor
+ * @vert_rescale  - determines if vertical rescale is enabled
+ * @vert_sf       - the vertical scale factor
+ * @src           - the incoming source buffer
+ * @bg            - the incoming background buffer
+ * @dst           - the outgoing destination buffer
+ * @work_bufs     - work buffer specifications
+ * @tmp_bufs      - temporary buffers
+ * @buf_count     - the number of temporary buffers used for the job
+ * @node_count    - the number of nodes used for the job
+ * @max_buf_size  - the maximum size of temporary buffers
+ * @nbr_rows      - the number of tile rows in the blit operation
+ * @nbr_cols      - the number of time columns in the blit operation
+ */
+struct b2r2_node_split_job {
+	enum b2r2_op_type type;
+
+	const u32 *ivmx;
+
+	bool blend;
+	bool clip;
+	bool rotation;
+	bool fullrange;
+
+	bool swap_fg_bg;
+
+	u32 flags;
+	u32 flag_param;
+	u32 transform;
+	u32 global_alpha;
+
+	struct b2r2_blt_rect clip_rect;
+
+	bool h_rescale;
+	u16 h_rsf;
+
+	bool v_rescale;
+	u16 v_rsf;
+
+	struct b2r2_node_split_buf src;
+	struct b2r2_node_split_buf bg;
+	struct b2r2_node_split_buf dst;
+
+	struct b2r2_work_buf work_bufs[MAX_TMP_BUFS_NEEDED];
+	struct b2r2_node_split_buf tmp_bufs[MAX_TMP_BUFS_NEEDED];
+
+	u32 buf_count;
+	u32 node_count;
+	u32 max_buf_size;
+};
+
+/**
+ * struct b2r2_core_job - Represents a B2R2 core job
+ *
+ * @start_sentinel: Memory overwrite guard
+ *
+ * @tag: Client value. Used by b2r2_core_job_find_first_with_tag().
+ * @prio: Job priority, from -19 up to 20. Mapped to the
+ *        B2R2 application queues. Filled in by the client.
+ * @first_node_address: Physical address of the first node. Filled
+ *                      in by the client.
+ * @last_node_address: Physical address of the last node. Filled
+ *                     in by the client.
+ *
+ * @callback: Function that will be called when the job is done.
+ * @acquire_resources: Function that allocates the resources needed
+ *                     to execute the job (i.e. SRAM alloc). Must not
+ *                     sleep if atomic, should fail with negative error code
+ *                     if resources not available.
+ * @release_resources: Function that releases the resources previously
+ *                     allocated by acquire_resources (i.e. SRAM alloc).
+ * @release: Function that will be called when the reference count reaches
+ *           zero.
+ *
+ * @job_id: Unique id for this job, assigned by B2R2 core
+ * @job_state: The current state of the job
+ * @jiffies: Number of jiffies needed for this request
+ *
+ * @list: List entry element for internal list management
+ * @event: Wait queue event to wait for job done
+ * @work: Work queue structure, for callback implementation
+ *
+ * @queue: The queue that this job shall be submitted to
+ * @control: B2R2 Queue control
+ * @pace_control: For composition queue only
+ * @interrupt_context: Context for interrupt
+ * @hw_start_time: The point when the b2r2 HW queue is activated for this job
+ * @nsec_active_in_hw: Time spent on the b2r2 HW queue for this job
+ *
+ * @end_sentinel: Memory overwrite guard
+ */
+struct b2r2_core_job {
+	u32 start_sentinel;
+
+	/* Data to be filled in by client */
+	int tag;
+	int data;
+	int prio;
+	u32 first_node_address;
+	u32 last_node_address;
+	void (*callback)(struct b2r2_core_job *);
+	int (*acquire_resources)(struct b2r2_core_job *,
+		bool atomic);
+	void (*release_resources)(struct b2r2_core_job *,
+		bool atomic);
+	void (*release)(struct b2r2_core_job *);
+
+	/* Output data, do not modify */
+	int  job_id;
+	enum b2r2_core_job_state job_state;
+	unsigned long jiffies;
+
+	/* Data below is internal to b2r2_core, do not modify */
+
+	/* Reference counting */
+	u32 ref_count;
+
+	/* Internal data */
+	struct list_head  list;
+	wait_queue_head_t event;
+	struct work_struct work;
+
+	/* B2R2 HW data */
+	enum b2r2_core_queue queue;
+	u32 control;
+	u32 pace_control;
+	u32 interrupt_context;
+
+	/* Timing data */
+	u32 hw_start_time;
+	s32 nsec_active_in_hw;
+
+	u32 end_sentinel;
+};
+
+/**
+ * struct b2r2_blt_request - Represents one B2R2 blit request
+ *
+ * @instance: Back pointer to the instance structure
+ * @list: List item to keep track of requests per instance
+ * @user_req: The request received from userspace
+ * @job: The administration structure for the B2R2 job,
+ *       consisting of one or more nodes
+ * @node_split_job: The administration structure for the B2R2 node split job
+ * @first_node: Pointer to the first B2R2 node
+ * @request_id: Request id for this job
+ * @core_mask: Bit mask with the cores doing part of the job
+ * @node_split_handle: Handle of the node split
+ * @src_resolved: Calculated info about the source buffer
+ * @src_mask_resolved: Calculated info about the source mask buffer
+ * @bg_resolved: Calculated info about the background buffer
+ * @dst_resolved: Calculated info about the destination buffer
+ * @profile: True if the blit shall be profiled, false otherwise
+ */
+struct b2r2_blt_request {
+	struct b2r2_control_instance   *instance;
+	struct list_head           list;
+	struct b2r2_blt_req        user_req;
+	struct b2r2_core_job       job;
+	struct b2r2_node_split_job node_split_job;
+	struct b2r2_node           *first_node;
+	int                        request_id;
+	u32                        core_mask;
+
+	/* Resolved buffer addresses */
+	struct b2r2_resolved_buf src_resolved;
+	struct b2r2_resolved_buf src_mask_resolved;
+	struct b2r2_resolved_buf bg_resolved;
+	struct b2r2_resolved_buf dst_resolved;
+
+	/* TBD: Info about SRAM usage & needs */
+	struct b2r2_work_buf *bufs;
+	u32 buf_count;
+
+	/* color look-up table */
+	void *clut;
+	u32 clut_phys_addr;
+
+	/* Profiling stuff */
+	bool profile;
+
+	s32 nsec_active_in_cpu;
+
+	u32 start_time_nsec;
+	s32 total_time_nsec;
+};
+
+/**
+ * struct b2r2_mem_heap - The memory heap
+ *
+ * @start_phys_addr: Physical memory start address
+ * @start_virt_ptr: Virtual pointer to start
+ * @size: Memory size
+ * @align: Alignment
+ * @blocks: List of all blocks
+ * @heap_lock: Protection for the heap
+ * @node_size: Size of each B2R2 node
+ * @node_heap: Heap for B2R2 node allocations
+ * @debugfs_root_dir: Debugfs B2R2 mem root dir
+ * @debugfs_heap_stats: Debugfs B2R2 memory status
+ * @debugfs_dir_blocks: Debugfs B2R2 free blocks dir
+ */
+struct b2r2_mem_heap {
+	dma_addr_t       start_phys_addr;
+	void             *start_virt_ptr;
+	u32              size;
+	u32              align;
+	struct list_head blocks;
+	spinlock_t       heap_lock;
+	u32              node_size;
+	struct dma_pool *node_heap;
+#ifdef CONFIG_DEBUG_FS
+	struct dentry   *debugfs_root_dir;
+	struct dentry   *debugfs_heap_stats;
+	struct dentry   *debugfs_dir_blocks;
+#endif
+};
+
+/**
+ *
+ * @dev: The device handle of the b2r2 instance
+ * @id: The id of the b2r2 instance
+ * @name: The name of the b2r2 instance
+ * @data: Used to store a reference to b2r2_core
+ * @tmp_bufs: Temporary buffers needed in the node splitter
+ * @filters_initialized: Indicating of filters has been
+ *                       initialized for this b2r2 instance
+ * @mem_heap: The b2r2 heap, e.g. used to allocate nodes
+ * @debugfs_latest_request: Copy of the latest request issued
+ * @debugfs_root_dir: The debugfs root directory, e.g. /debugfs/b2r2
+ * @debugfs_debug_root_dir: The b2r2 debug root directory,
+ *                          e.g. /debugfs/b2r2/debug
+ * @stat_lock: Spin lock protecting the statistics
+ * @stat_n_jobs_added: Number of jobs added to b2r2_core
+ * @stat_n_jobs_released: Number of jobs released (job_release called)
+ * @stat_n_jobs_in_report_list: Number of jobs currently in the report list
+ * @stat_n_in_blt: Number of client threads currently exec inside b2r2_blt()
+ * @stat_n_in_blt_synch: Number of client threads currently waiting for synch
+ * @stat_n_in_blt_add: Number of client threads currenlty adding in b2r2_blt
+ * @stat_n_in_blt_wait: Number of client threads currently waiting in b2r2_blt
+ * @stat_n_in_synch_0: Number of client threads currently in b2r2_blt_sync
+ *                     waiting for all client jobs to finish
+ * @stat_n_in_synch_job: Number of client threads currently in b2r2_blt_sync
+ *                       waiting specific job to finish
+ * @stat_n_in_query_cap: Number of clients currently in query cap
+ * @stat_n_in_open: Number of clients currently in b2r2_blt_open
+ * @stat_n_in_release: Number of clients currently in b2r2_blt_release
+ * @last_job_lock: Mutex protecting last_job
+ * @last_job: The last running job on this b2r2 instance
+ * @last_job_chars: Temporary buffer used in printing last_job
+ * @prev_node_count: Node cound of last_job
+ */
+struct b2r2_control {
+	struct device                   *dev;
+	void                            *data;
+	int                             id;
+	struct kref                     ref;
+	bool                            enabled;
+	struct tmp_buf                  tmp_bufs[MAX_TMP_BUFS_NEEDED];
+	int                             filters_initialized;
+	struct b2r2_mem_heap            mem_heap;
+#ifdef CONFIG_DEBUG_FS
+	struct b2r2_blt_request         debugfs_latest_request;
+	struct dentry                   *debugfs_root_dir;
+	struct dentry                   *debugfs_debug_root_dir;
+#endif
+	struct mutex                    stat_lock;
+	unsigned long                   stat_n_jobs_added;
+	unsigned long                   stat_n_jobs_released;
+	unsigned long                   stat_n_jobs_in_report_list;
+	unsigned long                   stat_n_in_blt;
+	unsigned long                   stat_n_in_blt_synch;
+	unsigned long                   stat_n_in_blt_add;
+	unsigned long                   stat_n_in_blt_wait;
+	unsigned long                   stat_n_in_synch_0;
+	unsigned long                   stat_n_in_synch_job;
+	unsigned long                   stat_n_in_query_cap;
+	unsigned long                   stat_n_in_open;
+	unsigned long                   stat_n_in_release;
+	struct mutex                    last_job_lock;
+	struct b2r2_node                *last_job;
+	char                            *last_job_chars;
+	int                             prev_node_count;
+};
+
+/* FIXME: The functions below should be removed when we are
+   switching to the new Robert Lind allocator */
+
+/**
+ * b2r2_blt_alloc_nodes() - Allocate nodes
+ *
+ * @node_count: Number of nodes to allocate
+ *
+ * Return:
+ *   Returns a pointer to the first node in the node list.
+ */
+struct b2r2_node *b2r2_blt_alloc_nodes(struct b2r2_control *cont,
+		int node_count);
+
+/**
+ * b2r2_blt_free_nodes() - Release nodes previously allocated via
+ *                         b2r2_generate_nodes
+ *
+ * @first_node: First node in linked list of nodes
+ */
+void b2r2_blt_free_nodes(struct b2r2_control *cont,
+		struct b2r2_node *first_node);
+
+/**
+ * b2r2_blt_module_init() - Initialize the B2R2 blt module
+ */
+int b2r2_blt_module_init(struct b2r2_control *cont);
+
+/**
+ * b2r2_blt_module_exit() - Un-initialize the B2R2 blt module
+ */
+void b2r2_blt_module_exit(struct b2r2_control *cont);
+
+/**
+ * b2r2_blt_add_control() - Add the b2r2 core control
+ */
+void b2r2_blt_add_control(struct b2r2_control *cont);
+
+/**
+ * b2r2_blt_remove_control() - Remove the b2r2 core control
+ */
+void b2r2_blt_remove_control(struct b2r2_control *cont);
+
+#endif
diff --git a/drivers/video/b2r2/b2r2_kernel_if.c b/drivers/video/b2r2/b2r2_kernel_if.c
new file mode 100644
index 00000000000..373311ccca5
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_kernel_if.c
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 kernel interface for beeing a separate module
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/file.h>
+#include <linux/poll.h>
+#include <linux/device.h>
+#include <linux/miscdevice.h>
+#include <linux/list.h>
+#ifdef CONFIG_ANDROID_PMEM
+#include <linux/android_pmem.h>
+#endif
+#include <linux/fb.h>
+#include <linux/sched.h>
+#include <asm/uaccess.h>
+#include <asm/cacheflush.h>
+
+EXPORT_SYMBOL(fget_light);
+EXPORT_SYMBOL(fput_light);
+EXPORT_SYMBOL(flush_cache_range);
+EXPORT_SYMBOL(task_sched_runtime);
+#ifdef CONFIG_ANDROID_PMEM
+EXPORT_SYMBOL(get_pmem_file);
+EXPORT_SYMBOL(put_pmem_file);
+EXPORT_SYMBOL(flush_pmem_file);
+#endif
diff --git a/drivers/video/b2r2/b2r2_mem_alloc.c b/drivers/video/b2r2/b2r2_mem_alloc.c
new file mode 100644
index 00000000000..584b324b8fe
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_mem_alloc.c
@@ -0,0 +1,669 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 internal Memory allocator
+ *
+ * Author: Robert Lind <robert.lind@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/debugfs.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+
+#include "b2r2_internal.h"
+#include "b2r2_mem_alloc.h"
+
+/* Forward declarations */
+static struct b2r2_mem_block *b2r2_mem_block_alloc(
+		struct b2r2_control *cont, u32 offset, u32 size, bool free);
+static void b2r2_mem_block_free(struct b2r2_mem_block *mem_block);
+static int b2r2_mem_heap_status(struct b2r2_mem_heap *mem_heap,
+		struct b2r2_mem_heap_status *mem_heap_status);
+
+/* Align value down to specified alignment */
+static inline u32 align_down(u32 align, u32 value)
+{
+	return value & ~(align - 1);
+}
+
+/* Align value up to specified alignment */
+static inline u32 align_up(u32 align, u32 value)
+{
+	return (value + align - 1) & ~(align - 1);
+}
+
+
+#ifdef CONFIG_DEBUG_FS
+/* About debugfs:
+ *  debugfs is a mountable debug file system.
+ *
+ *  Mount like this:
+ *  mkdir /debug
+ *  mount -t debugfs none /debug
+ *  ls /debug/b2r2/mem
+ *
+ *  ls -al /debug/b2r2/mem/blocks
+ *  cat /debug/b2r2/mem/stats
+ */
+
+
+/* Create string containing memory heap status */
+static char *get_b2r2_mem_stats(struct b2r2_mem_heap *mem_heap, char *buf)
+{
+	struct b2r2_mem_heap_status mem_heap_status;
+
+	if (b2r2_mem_heap_status(mem_heap, &mem_heap_status) != 0) {
+		strcpy(buf, "Error, failed to get status\n");
+		return buf;
+	}
+
+	sprintf(buf,
+			"Handle                 : 0x%lX\n"
+			"Physical start address : 0x%lX\n"
+			"Size                   : %lu\n"
+			"Align                  : %lu\n"
+			"No of blocks allocated : %lu\n"
+			"Allocated size         : %lu\n"
+			"No of free blocks      : %lu\n"
+			"Free size              : %lu\n"
+			"No of locks            : %lu\n"
+			"No of locked           : %lu\n"
+			"No of nodes            : %lu\n",
+			(unsigned long) mem_heap,
+			(unsigned long) mem_heap_status.start_phys_addr,
+			(unsigned long) mem_heap_status.size,
+			(unsigned long) mem_heap_status.align,
+			(unsigned long) mem_heap_status.num_alloc,
+			(unsigned long) mem_heap_status.allocated_size,
+			(unsigned long) mem_heap_status.num_free,
+			(unsigned long) mem_heap_status.free_size,
+			(unsigned long) mem_heap_status.num_locks,
+			(unsigned long) mem_heap_status.num_locked,
+			(unsigned long) mem_heap_status.num_nodes);
+
+	return buf;
+}
+
+/*
+ * Print memory heap status on file
+ * (Use like "cat /debug/b2r2/mem/stats")
+ */
+static int debugfs_b2r2_mem_stats_read(struct file *filp, char __user *buf,
+				       size_t count, loff_t *f_pos)
+{
+	struct b2r2_mem_heap *mem_heap = filp->f_dentry->d_inode->i_private;
+	char Buf[400];
+	size_t dev_size;
+	int ret = 0;
+
+	get_b2r2_mem_stats(mem_heap, Buf);
+	dev_size = strlen(Buf);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	return ret;
+}
+
+/* debugfs file operations for the "stats" file */
+static const struct file_operations debugfs_b2r2_mem_stats_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_mem_stats_read,
+};
+
+/* read function for file in the "blocks" sub directory */
+static int debugfs_b2r2_mem_block_read(struct file *filp, char __user *buf,
+				       size_t count, loff_t *f_pos)
+{
+	struct b2r2_mem_block *mem_block = filp->f_dentry->d_inode->i_private;
+	char Buf[200];
+	size_t dev_size;
+	int ret = 0;
+
+	dev_size = sprintf(Buf, "offset: %08lX %s size: %8d "
+			"lock_count: %2d\n",
+			(unsigned long) mem_block->offset,
+			mem_block->free ? "free" : "allc",
+			mem_block->size,
+			mem_block->lock_count);
+
+	/* No more to read if offset != 0 */
+	if (*f_pos > dev_size)
+		goto out;
+
+	if (*f_pos + count > dev_size)
+		count = dev_size - *f_pos;
+
+	if (copy_to_user(buf, Buf, count))
+		ret = -EINVAL;
+	*f_pos += count;
+	ret = count;
+
+out:
+	return ret;
+}
+
+/* debugfs file operations for files in the "blocks" directory */
+static const struct file_operations debugfs_b2r2_mem_block_fops = {
+	.owner = THIS_MODULE,
+	.read  = debugfs_b2r2_mem_block_read,
+};
+
+/*
+ * Create or update the debugfs directory entry for a file in the
+ * "blocks" directory (a memory allocation)
+ */
+void debugfs_create_mem_block_entry(struct b2r2_mem_block *mem_block,
+				    struct dentry *parent)
+{
+	struct timespec tm = current_kernel_time();
+	struct timespec atime = tm;
+	struct timespec mtime = tm;
+	struct timespec ctime = tm;
+
+	if (!IS_ERR_OR_NULL(mem_block->debugfs_block)) {
+		atime = mem_block->debugfs_block->d_inode->i_atime;
+		ctime = mem_block->debugfs_block->d_inode->i_ctime;
+		debugfs_remove(mem_block->debugfs_block);
+		mem_block->debugfs_block = NULL;
+	}
+
+	/* Add the block in debugfs */
+	if (mem_block->free)
+		sprintf(mem_block->debugfs_fname, "%08lX free",
+				(unsigned long) mem_block->offset);
+	else {
+		sprintf(mem_block->debugfs_fname, "%08lX allc h:%08lX "
+				"lck:%d ",
+				(unsigned long) mem_block->offset,
+				(unsigned long) mem_block,
+				mem_block->lock_count);
+	}
+
+	mem_block->debugfs_block = debugfs_create_file(
+		mem_block->debugfs_fname,
+		0444, parent, mem_block,
+		&debugfs_b2r2_mem_block_fops);
+	if (!IS_ERR_OR_NULL(mem_block->debugfs_block)) {
+		mem_block->debugfs_block->d_inode->i_size = mem_block->size;
+		mem_block->debugfs_block->d_inode->i_atime = atime;
+		mem_block->debugfs_block->d_inode->i_mtime = mtime;
+		mem_block->debugfs_block->d_inode->i_ctime = ctime;
+	}
+}
+#endif   /* CONFIG_DEBUG_FS */
+
+/* Module initialization function */
+int b2r2_mem_init(struct b2r2_control *cont,
+		u32 heap_size, u32 align, u32 node_size)
+{
+	struct b2r2_mem_block *mem_block;
+	u32 aligned_size;
+
+	dev_info(cont->dev, "%s: Creating heap for size %d bytes\n",
+		__func__, (int) heap_size);
+
+	/* Align size */
+	aligned_size = align_down(align, heap_size);
+	if (aligned_size == 0)
+		return -EINVAL;
+
+	cont->mem_heap.start_virt_ptr = dma_alloc_coherent(cont->dev,
+		aligned_size, &(cont->mem_heap.start_phys_addr), GFP_KERNEL);
+	if (!cont->mem_heap.start_phys_addr || !cont->mem_heap.start_virt_ptr) {
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	/* Initialize the heap */
+	cont->mem_heap.size  = aligned_size;
+	cont->mem_heap.align = align;
+
+	INIT_LIST_HEAD(&cont->mem_heap.blocks);
+
+#ifdef CONFIG_DEBUG_FS
+	/* Register debugfs */
+	if (!IS_ERR_OR_NULL(cont->mem_heap.debugfs_root_dir)) {
+		cont->mem_heap.debugfs_heap_stats = debugfs_create_file(
+			"stats", 0444, cont->mem_heap.debugfs_root_dir,
+			&cont->mem_heap, &debugfs_b2r2_mem_stats_fops);
+		cont->mem_heap.debugfs_dir_blocks = debugfs_create_dir(
+			"blocks", cont->mem_heap.debugfs_root_dir);
+	}
+#endif
+
+	/* Create the first _free_ memory block */
+	mem_block = b2r2_mem_block_alloc(cont, 0, aligned_size, true);
+	if (!mem_block) {
+		dma_free_coherent(cont->dev, aligned_size,
+			cont->mem_heap.start_virt_ptr,
+			cont->mem_heap.start_phys_addr);
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	/* Add the free block to the blocks list */
+	list_add(&mem_block->list, &cont->mem_heap.blocks);
+
+	/* Allocate separate heap for B2R2 nodes */
+	cont->mem_heap.node_size = node_size;
+	cont->mem_heap.node_heap = dma_pool_create("b2r2_node_cache",
+		cont->dev, node_size, align, 4096);
+	if (!cont->mem_heap.node_heap) {
+		b2r2_mem_block_free(mem_block);
+		dma_free_coherent(cont->dev, aligned_size,
+			cont->mem_heap.start_virt_ptr,
+			cont->mem_heap.start_phys_addr);
+		printk(KERN_ERR "B2R2_MEM: Failed to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(b2r2_mem_init);
+
+/* Module exit function */
+void b2r2_mem_exit(struct b2r2_control *cont)
+{
+	struct list_head *ptr;
+
+	/* Free B2R2 node heap */
+	dma_pool_destroy(cont->mem_heap.node_heap);
+
+	list_for_each(ptr, &cont->mem_heap.blocks) {
+		struct b2r2_mem_block *mem_block =
+			list_entry(ptr, struct b2r2_mem_block, list);
+
+		b2r2_mem_block_free(mem_block);
+	}
+
+	dma_free_coherent(cont->dev, cont->mem_heap.size,
+		cont->mem_heap.start_virt_ptr,
+		cont->mem_heap.start_phys_addr);
+}
+EXPORT_SYMBOL(b2r2_mem_exit);
+
+/* Return status of the heap */
+static int b2r2_mem_heap_status(struct b2r2_mem_heap *mheap,
+		struct b2r2_mem_heap_status *mem_heap_status)
+{
+	struct list_head *ptr;
+
+	if (!mheap || !mem_heap_status)
+		return -EINVAL;
+	memset(mem_heap_status, 0, sizeof(*mem_heap_status));
+
+	/* Lock the heap */
+	spin_lock(&mheap->heap_lock);
+
+	/* Fill in static info */
+	mem_heap_status->start_phys_addr = mheap->start_phys_addr;
+	mem_heap_status->size = mheap->size;
+	mem_heap_status->align = mheap->align;
+
+	list_for_each(ptr, &mheap->blocks) {
+		struct b2r2_mem_block *mem_block =
+			list_entry(ptr, struct b2r2_mem_block, list);
+
+		if (mem_block->free) {
+			mem_heap_status->num_free++;
+			mem_heap_status->free_size += mem_block->size;
+		} else {
+			if (mem_block->lock_count) {
+				mem_heap_status->num_locked++;
+				mem_heap_status->num_locks +=
+					mem_block->lock_count;
+			}
+			mem_heap_status->num_alloc++;
+			mem_heap_status->allocated_size += mem_block->size;
+		}
+	}
+
+	spin_unlock(&mheap->heap_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(b2r2_mem_heap_status);
+
+/* Internal: Allocate a housekeeping structure
+ * for an allocated or free memory block
+ */
+static struct b2r2_mem_block *b2r2_mem_block_alloc(
+		struct b2r2_control *cont, u32 offset, u32 size, bool free)
+{
+	struct b2r2_mem_block *mem_block = kmalloc(
+		sizeof(struct b2r2_mem_block), GFP_KERNEL);
+
+	if (mem_block) {
+		mem_block->offset     = offset;
+		mem_block->size       = size;
+		mem_block->free       = free;
+		mem_block->lock_count = 0;
+
+		INIT_LIST_HEAD(&mem_block->list);
+
+#ifdef CONFIG_DEBUG_FS
+		mem_block->debugfs_block  = NULL;
+		/* Add the block in debugfs */
+		debugfs_create_mem_block_entry(mem_block,
+			cont->mem_heap.debugfs_dir_blocks);
+#endif
+	}
+
+	return mem_block;
+}
+
+/* Internal: Release housekeeping structure */
+static void b2r2_mem_block_free(struct b2r2_mem_block *mem_block)
+{
+	if (mem_block) {
+#ifdef CONFIG_DEBUG_FS
+		debugfs_remove(mem_block->debugfs_block);
+#endif
+		kfree(mem_block);
+	}
+}
+
+/* Allocate a block from the heap */
+int b2r2_mem_alloc(struct b2r2_control *cont, u32 requested_size,
+		u32 *returned_size, u32 *mem_handle)
+{
+	int ret = 0;
+	struct list_head *ptr;
+	struct b2r2_mem_block *found_mem_block = NULL;
+	u32 aligned_size;
+
+	if (!mem_handle)
+		return -EINVAL;
+
+	printk(KERN_INFO "%s: size=%d\n", __func__, requested_size);
+
+	*mem_handle = 0;
+
+	/* Lock the heap */
+	spin_lock(&cont->mem_heap.heap_lock);
+
+	aligned_size = align_up(cont->mem_heap.align, requested_size);
+	/* Try to find the best matching free block of suitable size */
+	list_for_each(ptr, &cont->mem_heap.blocks) {
+		struct b2r2_mem_block *mem_block =
+			list_entry(ptr, struct b2r2_mem_block, list);
+
+		if (mem_block->free && mem_block->size >= aligned_size &&
+				(!found_mem_block ||
+				mem_block->size < found_mem_block->size)) {
+			found_mem_block = mem_block;
+			if (found_mem_block->size == aligned_size)
+				break;
+		}
+	}
+
+	if (found_mem_block) {
+		struct b2r2_mem_block *new_block
+			= b2r2_mem_block_alloc(cont,
+				found_mem_block->offset,
+				requested_size, false);
+
+		if (new_block) {
+			/* Insert the new block before the found block */
+			list_add_tail(&new_block->list,
+				&found_mem_block->list);
+
+			/* Split the free block */
+			found_mem_block->offset += aligned_size;
+			found_mem_block->size   -= aligned_size;
+
+			if (found_mem_block->size == 0)
+				b2r2_mem_block_free(found_mem_block);
+			else {
+#ifdef CONFIG_DEBUG_FS
+				debugfs_create_mem_block_entry(
+					found_mem_block,
+					cont->mem_heap.debugfs_dir_blocks);
+#endif
+			}
+
+			*mem_handle = (u32) new_block;
+			*returned_size = aligned_size;
+		} else {
+			ret = -ENOMEM;
+		}
+	} else
+		ret = -ENOMEM;
+
+	if (ret != 0) {
+		*returned_size = 0;
+		*mem_handle = (u32) 0;
+	}
+
+	/* Unlock */
+	spin_unlock(&cont->mem_heap.heap_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_mem_alloc);
+
+/* Free the allocated block */
+int b2r2_mem_free(struct b2r2_control *cont, u32 mem_handle)
+{
+	int ret = 0;
+	struct b2r2_mem_block *mem_block = (struct b2r2_mem_block *) mem_handle;
+
+	if (!mem_block)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&cont->mem_heap.heap_lock);
+
+	if (!ret && mem_block->free)
+		ret = -EINVAL;
+
+	if (!ret) {
+		printk(KERN_INFO "%s: freeing block 0x%p\n", __func__, mem_block);
+		/* Release the block */
+
+		mem_block->free = true;
+		mem_block->size = align_up(cont->mem_heap.align,
+					   mem_block->size);
+
+		/* Join with previous block if possible */
+		if (mem_block->list.prev != &cont->mem_heap.blocks) {
+			struct b2r2_mem_block *prev_block =
+				list_entry(mem_block->list.prev,
+					struct b2r2_mem_block, list);
+
+			if (prev_block->free &&
+				(prev_block->offset + prev_block->size) ==
+				mem_block->offset) {
+				mem_block->offset = prev_block->offset;
+				mem_block->size   += prev_block->size;
+
+				b2r2_mem_block_free(prev_block);
+			}
+		}
+
+		/* Join with next block if possible */
+		if (mem_block->list.next != &cont->mem_heap.blocks) {
+			struct b2r2_mem_block *next_block
+				= list_entry(mem_block->list.next,
+						struct b2r2_mem_block,
+						list);
+
+			if (next_block->free &&
+				(mem_block->offset + mem_block->size) ==
+				next_block->offset) {
+				mem_block->size   += next_block->size;
+
+				b2r2_mem_block_free(next_block);
+			}
+		}
+#ifdef CONFIG_DEBUG_FS
+		debugfs_create_mem_block_entry(mem_block,
+				cont->mem_heap.debugfs_dir_blocks);
+#endif
+	}
+
+	/* Unlock */
+	spin_unlock(&cont->mem_heap.heap_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_mem_free);
+
+/* Lock the allocated block in memory */
+int b2r2_mem_lock(struct b2r2_control *cont, u32 mem_handle,
+		u32 *phys_addr, void **virt_ptr, u32 *size)
+{
+	struct b2r2_mem_block *mem_block =
+		(struct b2r2_mem_block *) mem_handle;
+
+	if (!mem_block)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&cont->mem_heap.heap_lock);
+
+	mem_block->lock_count++;
+
+	if (phys_addr)
+		*phys_addr = cont->mem_heap.start_phys_addr + mem_block->offset;
+	if (virt_ptr)
+		*virt_ptr = (char *) cont->mem_heap.start_virt_ptr +
+			mem_block->offset;
+	if (size)
+		*size = align_up(cont->mem_heap.align, mem_block->size);
+#ifdef CONFIG_DEBUG_FS
+	debugfs_create_mem_block_entry(mem_block,
+		cont->mem_heap.debugfs_dir_blocks);
+#endif
+
+	spin_unlock(&cont->mem_heap.heap_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL(b2r2_mem_lock);
+
+/* Unlock the allocated block in memory */
+int b2r2_mem_unlock(struct b2r2_control *cont, u32 mem_handle)
+{
+	struct b2r2_mem_block *mem_block =
+		(struct b2r2_mem_block *) mem_handle;
+
+	if (!mem_block)
+		return -EINVAL;
+
+	/* Lock the heap */
+	spin_lock(&cont->mem_heap.heap_lock);
+
+	mem_block->lock_count--;
+
+	spin_unlock(&cont->mem_heap.heap_lock);
+
+	/* debugfs will be updated in release */
+	return 0;
+/*	return b2r2_mem_free(mem_handle);*/
+}
+EXPORT_SYMBOL(b2r2_mem_unlock);
+
+/* Allocate one or more b2r2 nodes from DMA pool */
+int b2r2_node_alloc(struct b2r2_control *cont, u32 num_nodes,
+		struct b2r2_node **first_node)
+{
+	int i;
+	int ret = 0;
+	u32 physical_address;
+	struct b2r2_node *first_node_ptr;
+	struct b2r2_node *node_ptr;
+
+	/* Check input parameters */
+	if ((num_nodes <= 0) || !first_node) {
+		dev_err(cont->dev,
+			"B2R2_MEM: Invalid parameter for b2r2_node_alloc, "
+			"num_nodes=%d, first_node=%ld\n",
+			(int) num_nodes, (long) first_node);
+		return -EINVAL;
+	}
+
+	/* Allocate the first node */
+	first_node_ptr = dma_pool_alloc(cont->mem_heap.node_heap,
+		GFP_DMA | GFP_KERNEL, &physical_address);
+	if (!first_node_ptr) {
+		dev_err(cont->dev,
+			"B2R2_MEM: Failed to allocate memory for node\n");
+		return -ENOMEM;
+	}
+
+	/* Initialize first node */
+	first_node_ptr->next = NULL;
+	first_node_ptr->physical_address = physical_address +
+			offsetof(struct b2r2_node, node);
+
+	/* Allocate and initialize remaining nodes, */
+	/* and link them into a list                */
+	for (i = 1, node_ptr = first_node_ptr; i < num_nodes; i++) {
+		node_ptr->next = dma_pool_alloc(cont->mem_heap.node_heap,
+				GFP_DMA | GFP_KERNEL, &physical_address);
+		if (node_ptr->next) {
+			node_ptr = node_ptr->next;
+			node_ptr->next = NULL;
+			node_ptr->physical_address = physical_address +
+					offsetof(struct b2r2_node, node);
+		} else {
+			printk(KERN_ERR "B2R2_MEM: Failed to allocate memory for node\n");
+			ret = -ENOMEM;
+			break;
+		}
+	}
+
+	/* If all nodes were allocated successfully, */
+	/* return the first node                     */
+	if (!ret)
+		*first_node = first_node_ptr;
+	else
+		b2r2_node_free(cont, first_node_ptr);
+
+	return ret;
+}
+EXPORT_SYMBOL(b2r2_node_alloc);
+
+/* Free a linked list of b2r2 nodes */
+void b2r2_node_free(struct b2r2_control *cont, struct b2r2_node *first_node)
+{
+	struct b2r2_node *current_node = first_node;
+	struct b2r2_node *next_node = NULL;
+
+	/* Traverse the linked list and free the nodes */
+	while (current_node != NULL) {
+		next_node = current_node->next;
+		dma_pool_free(cont->mem_heap.node_heap, current_node,
+				current_node->physical_address -
+				offsetof(struct b2r2_node, node));
+		current_node = next_node;
+	}
+}
+EXPORT_SYMBOL(b2r2_node_free);
+
+MODULE_AUTHOR("Robert Lind <robert.lind@ericsson.com");
+MODULE_DESCRIPTION("Ericsson AB B2R2 physical memory driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/b2r2/b2r2_mem_alloc.h b/drivers/video/b2r2/b2r2_mem_alloc.h
new file mode 100644
index 00000000000..4fd1e66abca
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_mem_alloc.h
@@ -0,0 +1,161 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 internal Memory allocator
+ *
+ * Author: Robert Lind <robert.lind@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __B2R2_MEM_ALLOC_H
+#define __B2R2_MEM_ALLOC_H
+
+#include "b2r2_internal.h"
+
+
+/**
+ * struct b2r2_mem_heap_status - Information about current state of the heap
+ *
+ * @start_phys_addr: Physical address of the the memory area
+ * @size: Size of the memory area
+ * @align: Alignment of start and allocation sizes (in bytes).
+ * @num_alloc: Number of memory allocations
+ * @allocated_size: Size allocated (sum of requested sizes)
+ * @num_free: Number of free blocks (fragments)
+ * @free_size: Free size available for allocation
+ * @num_locks: Sum of number of number of locks on memory allocations
+ * @num_locked: Number of locked memory allocations
+ * @num_nodes: Number of node allocations
+ *
+ **/
+struct b2r2_mem_heap_status {
+	u32 start_phys_addr;
+	u32 size;
+	u32 align;
+	u32 num_alloc;
+	u32 allocated_size;
+	u32 num_free;
+	u32 free_size;
+	u32 num_locks;
+	u32 num_locked;
+	u32 num_nodes;
+};
+
+/**
+ * struct b2r2_mem_block - Represents one block of b2r2
+ *                         physical memory, free or allocated
+ *
+ * @list: For membership in list
+ * @offset: Offset in b2r2 physical memory area (aligned)
+ * @size: Size of the object (requested size if busy, else actual)
+ * @free: True if the block is free
+ * @lock_count: Lock count
+ * @debugfs_fname: Debugfs file name
+ * @debugfs_block: Debugfs dir entry for the block
+ */
+struct b2r2_mem_block {
+	struct list_head	list;
+	u32					offset;
+	u32					size;
+	bool				free;
+	u32					lock_count;
+#ifdef CONFIG_DEBUG_FS
+	char				debugfs_fname[80];
+	struct dentry		*debugfs_block;
+#endif
+};
+
+
+/* B2R2 memory API (kernel) */
+
+/**
+ * b2r2_mem_init() - Initializes the B2R2 memory manager
+ * @dev: Pointer to device to use for allocating the memory heap
+ * @heap_size: Size of the heap (in bytes)
+ * @align: Alignment to use for memory allocations on heap (in bytes)
+ * @node_size: Size of each B2R2 node (in bytes)
+ *
+ * Returns 0 if success, else negative error code
+ **/
+int b2r2_mem_init(struct b2r2_control *cont,
+		u32 heap_size, u32 align, u32 node_size);
+
+/**
+ * b2r2_mem_exit() - Cleans up the B2R2 memory manager
+ *
+ **/
+void b2r2_mem_exit(struct b2r2_control *cont);
+
+/**
+ * b2r2_mem_alloc() - Allocates memory block from physical memory heap
+ * @requested_size: Requested size
+ * @returned_size: Actual size of memory block. Might be adjusted due to
+ *                 alignment but is always >= requested size if function
+ *                 succeeds
+ * @mem_handle: Returned memory handle
+ *
+ * All memory allocations are movable when not locked.
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_mem_alloc(struct b2r2_control *cont, u32 requested_size,
+		u32 *returned_size, u32 *mem_handle);
+
+/**
+ * b2r2_mem_free() - Frees an allocation
+ * @mem_handle: Memory handle
+ *
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_mem_free(struct b2r2_control *cont, u32 mem_handle);
+
+/**
+ * b2r2_mem_lock() - Lock memory in memory and return physical address
+ * @mem_handle: Memory handle
+ * @phys_addr: Returned physical address to start of memory allocation.
+ *             May be NULL.
+ * @virt_ptr: Returned virtual address pointer to start of memory allocation.
+ *            May be NULL.
+ * @size: Returned size of memory allocation. May be NULL.
+ *
+ * The adress of the memory allocation is locked and the physical address
+ * is returned.
+ * The lock count is incremented by one.
+ * You need to call b2r2_mem_unlock once for each call to
+ * b2r2_mem_lock.
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_mem_lock(struct b2r2_control *cont, u32 mem_handle,
+		u32 *phys_addr, void **virt_ptr, u32 *size);
+
+/**
+ * b2r2_mem_unlock() - Unlock previously locked memory
+ * @mem_handle: Memory handle
+ *
+ * Decrements lock count. When lock count reaches 0 the
+ * memory area is movable again.
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_mem_unlock(struct b2r2_control *cont, u32 mem_handle);
+
+/**
+ * b2r2_node_alloc() - Allocates B2R2 node from physical memory heap
+ * @num_nodes: Number of linked nodes to allocate
+ * @first_node: Returned pointer to first node in linked list
+ *
+ * Returns 0 if OK else negative error value
+ **/
+int b2r2_node_alloc(struct b2r2_control *cont, u32 num_nodes,
+		struct b2r2_node **first_node);
+
+/**
+ * b2r2_node_free() - Frees a linked list of allocated B2R2 nodes
+ * @first_node: Pointer to first node in linked list
+ *
+ * Returns 0 if OK else negative error value
+ **/
+void b2r2_node_free(struct b2r2_control *cont, struct b2r2_node *first_node);
+
+
+#endif /* __B2R2_MEM_ALLOC_H */
diff --git a/drivers/video/b2r2/b2r2_node_gen.c b/drivers/video/b2r2/b2r2_node_gen.c
new file mode 100644
index 00000000000..1f48bac6fe7
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_node_gen.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 node generator
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <asm/dma-mapping.h>
+#include "b2r2_internal.h"
+
+static void free_nodes(struct b2r2_control *cont,
+		struct b2r2_node *first_node)
+{
+	struct b2r2_node *node = first_node;
+	int no_of_nodes = 0;
+
+	while (node) {
+		no_of_nodes++;
+		node = node->next;
+	}
+
+	dma_free_coherent(cont->dev,
+			no_of_nodes * sizeof(struct b2r2_node),
+			first_node,
+			first_node->physical_address -
+			offsetof(struct b2r2_node, node));
+}
+
+struct b2r2_node *b2r2_blt_alloc_nodes(struct b2r2_control *cont,
+		int no_of_nodes)
+{
+	u32 physical_address;
+	struct b2r2_node *nodes;
+	struct b2r2_node *tmpnode;
+
+	if (no_of_nodes <= 0) {
+		dev_err(cont->dev, "%s: Wrong number of nodes (%d)",
+				__func__, no_of_nodes);
+		return NULL;
+	}
+
+	/* Allocate the memory */
+	nodes = (struct b2r2_node *) dma_alloc_coherent(cont->dev,
+			no_of_nodes * sizeof(struct b2r2_node),
+			&physical_address, GFP_DMA | GFP_KERNEL);
+
+	if (nodes == NULL) {
+		dev_err(cont->dev,
+				"%s: Failed to alloc memory for nodes",
+				__func__);
+		return NULL;
+	}
+
+	/* Build the linked list */
+	tmpnode = nodes;
+	physical_address += offsetof(struct b2r2_node, node);
+	while (no_of_nodes--) {
+		tmpnode->physical_address = physical_address;
+		if (no_of_nodes)
+			tmpnode->next = tmpnode + 1;
+		else
+			tmpnode->next = NULL;
+
+		tmpnode++;
+		physical_address += sizeof(struct b2r2_node);
+	}
+
+	return nodes;
+}
+
+void b2r2_blt_free_nodes(struct b2r2_control *cont,
+		struct b2r2_node *first_node)
+{
+	free_nodes(cont, first_node);
+}
+
diff --git a/drivers/video/b2r2/b2r2_node_split.c b/drivers/video/b2r2/b2r2_node_split.c
new file mode 100644
index 00000000000..b2fb07580ca
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_node_split.c
@@ -0,0 +1,3033 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 node splitter
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include "b2r2_debug.h"
+#include "b2r2_node_split.h"
+#include "b2r2_internal.h"
+#include "b2r2_hw_convert.h"
+#include "b2r2_filters.h"
+#include "b2r2_utils.h"
+
+#include <linux/kernel.h>
+
+/*
+ * Macros and constants
+ */
+
+#define INSTANCES_DEFAULT_SIZE 10
+#define INSTANCES_GROW_SIZE 5
+
+/*
+ * Internal types
+ */
+
+
+/*
+ * Global variables
+ */
+
+
+/*
+ * Forward declaration of private functions
+ */
+static int analyze_fmt_conv(struct b2r2_control *cont,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 **vmx, u32 *node_count,
+		bool fullrange);
+static int analyze_color_fill(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count);
+static int analyze_copy(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_scaling(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_rotate(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_transform(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_rot_scale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count);
+static int analyze_scale_factors(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this);
+
+static void configure_src(struct b2r2_control *cont, struct b2r2_node *node,
+		struct b2r2_node_split_buf *src, const u32 *ivmx);
+static void configure_bg(struct b2r2_control *cont, struct b2r2_node *node,
+		struct b2r2_node_split_buf *bg, bool swap_fg_bg);
+static int configure_dst(struct b2r2_control *cont, struct b2r2_node *node,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next);
+static void configure_blend(struct b2r2_control *cont, struct b2r2_node *node,
+		u32 flags, u32 global_alpha);
+static void configure_clip(struct b2r2_control *cont, struct b2r2_node *node,
+		struct b2r2_blt_rect *clip_rect);
+
+static int configure_tile(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this, struct b2r2_node *node,
+		struct b2r2_node **next);
+static void configure_direct_fill(struct b2r2_control *cont,
+		struct b2r2_node *node, u32 color,
+		struct b2r2_node_split_buf *dst,
+		struct b2r2_node **next);
+static int configure_fill(struct b2r2_control *cont,
+		struct b2r2_node *node, u32 color, enum b2r2_blt_fmt fmt,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next);
+static void configure_direct_copy(struct b2r2_control *cont,
+		struct b2r2_node *node, struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, struct b2r2_node **next);
+static int configure_copy(struct b2r2_control *cont,
+		struct b2r2_node *node,	struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next,
+		struct b2r2_node_split_job *this);
+static int configure_rotate(struct b2r2_control *cont,
+		struct b2r2_node *node, struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next,
+		struct b2r2_node_split_job *this);
+static int configure_scale(struct b2r2_control *cont,
+		struct b2r2_node *node, struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst, u16 h_rsf, u16 v_rsf,
+		const u32 *ivmx, struct b2r2_node **next,
+		struct b2r2_node_split_job *this);
+static int configure_rot_scale(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this, struct b2r2_node *node,
+		struct b2r2_node **next);
+
+static int check_rect(struct b2r2_control *cont,
+		const struct b2r2_blt_img *img,
+		const struct b2r2_blt_rect *rect,
+		const struct b2r2_blt_rect *clip);
+static void set_buf(struct b2r2_control *cont,
+		struct b2r2_node_split_buf *buf,
+		u32 addr, const struct b2r2_blt_img *img,
+		const struct b2r2_blt_rect *rect, bool color_fill, u32 color);
+static int setup_tmp_buf(struct b2r2_control *cont,
+		struct b2r2_node_split_buf *this, u32 max_size,
+		enum b2r2_blt_fmt pref_fmt, u32 pref_width, u32 pref_height);
+
+static bool is_transform(const struct b2r2_blt_request *req);
+static s32 rescale(struct b2r2_control *cont, s32 dim, u16 sf);
+static s32 inv_rescale(s32 dim, u16 sf);
+
+static void set_target(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_src(struct b2r2_src_config *src, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_src_1(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_src_2(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_src_3(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf);
+static void set_ivmx(struct b2r2_node *node, const u32 *vmx_values);
+
+static void reset_nodes(struct b2r2_node *node);
+
+static bool bg_format_require_ivmx(enum b2r2_blt_fmt bg_fmt,
+		enum b2r2_blt_fmt dst_fmt);
+
+/*
+ * Public functions
+ */
+
+/**
+ * b2r2_node_split_analyze() - analyzes the request
+ */
+int b2r2_node_split_analyze(const struct b2r2_blt_request *req,
+		u32 max_buf_size, u32 *node_count, struct b2r2_work_buf **bufs,
+		u32 *buf_count, struct b2r2_node_split_job *this)
+{
+	int ret;
+	bool color_fill;
+	struct b2r2_control *cont = req->instance->control;
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	memset(this, 0, sizeof(*this));
+
+	/* Copy parameters */
+	this->flags = req->user_req.flags;
+	this->transform = req->user_req.transform;
+	this->max_buf_size = max_buf_size;
+	this->global_alpha = req->user_req.global_alpha;
+	this->buf_count = 0;
+	this->node_count = 0;
+
+	if (this->flags & B2R2_BLT_FLAG_BLUR) {
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	/* Unsupported formats on src */
+	switch (req->user_req.src_img.fmt) {
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		if (b2r2_is_bgr_fmt(req->user_req.dst_img.fmt)) {
+			ret = -ENOSYS;
+			goto unsupported;
+		}
+		break;
+	default:
+		break;
+	}
+
+	/* Unsupported formats on dst */
+	switch (req->user_req.dst_img.fmt) {
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		if (b2r2_is_bgr_fmt(req->user_req.src_img.fmt)) {
+			ret = -ENOSYS;
+			goto unsupported;
+		}
+		break;
+	default:
+		break;
+	}
+
+	/* Unsupported formats on bg */
+	if (this->flags & B2R2_BLT_FLAG_BG_BLEND)
+		/*
+		 * There are no ivmx on source 1, so check that there is no
+		 * such requirement on the background to destination format
+		 * conversion. This check is sufficient since the node splitter
+		 * currently does not support destination ivmx. That fact also
+		 * removes the source format as a parameter when checking the
+		 * background format.
+		 */
+		if (bg_format_require_ivmx(req->user_req.bg_img.fmt,
+				req->user_req.dst_img.fmt)) {
+			ret = -ENOSYS;
+			goto unsupported;
+		}
+
+	if ((this->flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY) &&
+			(b2r2_is_yuv_fmt(req->user_req.src_img.fmt) ||
+			req->user_req.src_img.fmt == B2R2_BLT_FMT_1_BIT_A1 ||
+			req->user_req.src_img.fmt == B2R2_BLT_FMT_8_BIT_A8)) {
+		b2r2_log_warn(cont->dev, "%s: Unsupported: source color keying "
+			"with YUV or pure alpha formats.\n", __func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	if (this->flags & (B2R2_BLT_FLAG_DEST_COLOR_KEY |
+			B2R2_BLT_FLAG_SOURCE_MASK)) {
+		b2r2_log_warn(cont->dev, "%s: Unsupported: source mask, "
+			"destination color keying.\n", __func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	if ((req->user_req.flags & B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) &&
+			req->user_req.clut == NULL) {
+		b2r2_log_warn(cont->dev, "%s: Invalid request: no table "
+			"specified for CLUT color correction.\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	/* Check for color fill */
+	color_fill = (this->flags & (B2R2_BLT_FLAG_SOURCE_FILL |
+		B2R2_BLT_FLAG_SOURCE_FILL_RAW)) != 0;
+
+	/* Configure the source and destination buffers */
+	set_buf(cont, &this->src, req->src_resolved.physical_address,
+		&req->user_req.src_img, &req->user_req.src_rect,
+		color_fill, req->user_req.src_color);
+
+	if (this->flags & B2R2_BLT_FLAG_BG_BLEND) {
+		set_buf(cont, &this->bg, req->bg_resolved.physical_address,
+			&req->user_req.bg_img, &req->user_req.bg_rect,
+			false, 0);
+	}
+
+	set_buf(cont, &this->dst, req->dst_resolved.physical_address,
+			&req->user_req.dst_img, &req->user_req.dst_rect, false,
+			0);
+
+	b2r2_log_info(cont->dev, "%s:\n"
+		"\t\tsrc.rect=(%4d, %4d, %4d, %4d)\t"
+		"bg.rect=(%4d, %4d, %4d, %4d)\t"
+		"dst.rect=(%4d, %4d, %4d, %4d)\n", __func__, this->src.rect.x,
+		this->src.rect.y, this->src.rect.width, this->src.rect.height,
+		this->bg.rect.x, this->bg.rect.y, this->bg.rect.width,
+		this->bg.rect.height, this->dst.rect.x, this->dst.rect.y,
+		this->dst.rect.width, this->dst.rect.height);
+
+	if (this->flags & B2R2_BLT_FLAG_DITHER)
+		this->dst.dither = B2R2_TTY_RGB_ROUND_DITHER;
+
+	if (this->flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY)
+		this->flag_param = req->user_req.src_color;
+
+	/* Check for blending */
+	if ((this->flags & B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND) &&
+			(this->global_alpha != 255))
+		this->blend = true;
+	else if (this->flags & B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND)
+		this->blend = (color_fill && b2r2_fmt_has_alpha(this->dst.fmt)) ||
+				b2r2_fmt_has_alpha(this->src.fmt);
+	else if (this->flags & B2R2_BLT_FLAG_BG_BLEND)
+		this->blend = true;
+
+	/* Check for full range YUV conversion */
+	if (this->flags & B2R2_BLT_FLAG_FULL_RANGE_YUV)
+		this->fullrange = true;
+
+	if (this->blend && this->src.type == B2R2_FMT_TYPE_PLANAR) {
+		b2r2_log_warn(cont->dev, "%s: Unsupported: blend with planar"
+			" source\n", __func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	/* Check for clipping */
+	this->clip = (this->flags & B2R2_BLT_FLAG_DESTINATION_CLIP) != 0;
+	if (this->clip) {
+		s32 l = req->user_req.dst_clip_rect.x;
+		s32 r = l + req->user_req.dst_clip_rect.width;
+		s32 t = req->user_req.dst_clip_rect.y;
+		s32 b = t + req->user_req.dst_clip_rect.height;
+
+		/* Intersect the clip and buffer rects */
+		if (l < 0)
+			l = 0;
+		if (r > req->user_req.dst_img.width)
+			r = req->user_req.dst_img.width;
+		if (t < 0)
+			t = 0;
+		if (b > req->user_req.dst_img.height)
+			b = req->user_req.dst_img.height;
+
+		this->clip_rect.x = l;
+		this->clip_rect.y = t;
+		this->clip_rect.width = r - l;
+		this->clip_rect.height = b - t;
+	} else {
+		/* Set the clip rectangle to the buffer bounds */
+		this->clip_rect.x = 0;
+		this->clip_rect.y = 0;
+		this->clip_rect.width = req->user_req.dst_img.width;
+		this->clip_rect.height = req->user_req.dst_img.height;
+	}
+
+	/* Validate the destination */
+	ret = check_rect(cont, &req->user_req.dst_img, &req->user_req.dst_rect,
+			&this->clip_rect);
+	if (ret < 0)
+		goto error;
+
+	/* Validate the source (if not color fill) */
+	if (!color_fill) {
+		ret = check_rect(cont, &req->user_req.src_img,
+					&req->user_req.src_rect, NULL);
+		if (ret < 0)
+			goto error;
+	}
+
+	/* Validate the background source */
+	if (this->flags & B2R2_BLT_FLAG_BG_BLEND) {
+		ret = check_rect(cont, &req->user_req.bg_img,
+					&req->user_req.bg_rect, NULL);
+		if (ret < 0)
+			goto error;
+	}
+
+	/* Do the analysis depending on the type of operation */
+	if (color_fill) {
+		ret = analyze_color_fill(this, req, &this->node_count);
+	} else {
+
+		bool upsample;
+		bool downsample;
+
+		/*
+		 * YUV formats that are non-raster, non-yuv444 needs to be
+		 * up (or down) sampled using the resizer.
+		 *
+		 * NOTE: The resizer needs to be enabled for YUV444 as well,
+		 *       even though there is no upsampling. This is most
+		 *       likely a bug in the hardware.
+		 */
+		upsample = this->src.type != B2R2_FMT_TYPE_RASTER &&
+			b2r2_is_yuv_fmt(this->src.fmt);
+		downsample = this->dst.type != B2R2_FMT_TYPE_RASTER &&
+			b2r2_is_yuv_fmt(this->dst.fmt);
+
+		if (is_transform(req) || upsample || downsample)
+			ret = analyze_transform(this, req, &this->node_count,
+						&this->buf_count);
+		else
+			ret = analyze_copy(this, req, &this->node_count,
+						&this->buf_count);
+	}
+
+	if (ret == -ENOSYS) {
+		goto unsupported;
+	} else if (ret < 0) {
+		b2r2_log_warn(cont->dev, "%s: Analysis failed!\n", __func__);
+		goto error;
+	}
+
+	/* Setup the origin and movement of the destination window */
+	if (this->dst.hso == B2R2_TY_HSO_RIGHT_TO_LEFT) {
+		this->dst.dx = -this->dst.win.width;
+		this->dst.win.x = this->dst.rect.x + this->dst.rect.width - 1;
+	} else {
+		this->dst.dx = this->dst.win.width;
+		this->dst.win.x = this->dst.rect.x;
+	}
+	if (this->dst.vso == B2R2_TY_VSO_BOTTOM_TO_TOP) {
+		this->dst.dy = -this->dst.win.height;
+		this->dst.win.y = this->dst.rect.y + this->dst.rect.height - 1;
+	} else {
+		this->dst.dy = this->dst.win.height;
+		this->dst.win.y = this->dst.rect.y;
+	}
+
+	*buf_count = this->buf_count;
+	*node_count = this->node_count;
+
+	if (this->buf_count > 0)
+		*bufs = &this->work_bufs[0];
+
+	b2r2_log_info(cont->dev, "%s: dst.win=(%d, %d, %d, %d), "
+		"dst.dx=%d, dst.dy=%d\n", __func__, this->dst.win.x,
+		this->dst.win.y, this->dst.win.width, this->dst.win.height,
+		this->dst.dx, this->dst.dy);
+	if (this->buf_count > 0)
+		b2r2_log_info(cont->dev, "%s: buf_count=%d, buf_size=%d, "
+			"node_count=%d\n", __func__, *buf_count,
+			bufs[0]->size, *node_count);
+	else
+		b2r2_log_info(cont->dev, "%s: buf_count=%d, node_count=%d\n",
+			__func__, *buf_count, *node_count);
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+unsupported:
+	return ret;
+}
+
+/**
+ * b2r2_node_split_configure() - configures the node list
+ */
+int b2r2_node_split_configure(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this, struct b2r2_node *first)
+{
+	int ret;
+
+	struct b2r2_node_split_buf *dst = &this->dst;
+	struct b2r2_node *node = first;
+
+	u32 x_pixels = 0;
+	u32 y_pixels = 0;
+
+	reset_nodes(node);
+
+	while (y_pixels < dst->rect.height) {
+		s32 dst_x = dst->win.x;
+		s32 dst_w = dst->win.width;
+
+		/* Clamp window height */
+		if (dst->win.height > dst->rect.height - y_pixels)
+			dst->win.height = dst->rect.height - y_pixels;
+
+		while (x_pixels < dst->rect.width) {
+
+			/* Clamp window width */
+			if (dst_w > dst->rect.width - x_pixels)
+				dst->win.width = dst->rect.width - x_pixels;
+
+			ret = configure_tile(cont, this, node, &node);
+			if (ret < 0)
+				goto error;
+
+			dst->win.x += dst->dx;
+			x_pixels += max(dst->dx, -dst->dx);
+			b2r2_log_info(cont->dev, "%s: x_pixels=%d\n",
+				__func__, x_pixels);
+		}
+
+		dst->win.y += dst->dy;
+		y_pixels += max(dst->dy, -dst->dy);
+
+		dst->win.x = dst_x;
+		dst->win.width = dst_w;
+		x_pixels = 0;
+
+		b2r2_log_info(cont->dev, "%s: y_pixels=%d\n",
+			__func__, y_pixels);
+	}
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * b2r2_node_split_assign_buffers() - assigns temporary buffers to the node list
+ */
+int b2r2_node_split_assign_buffers(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this, struct b2r2_node *first,
+		struct b2r2_work_buf *bufs, u32 buf_count)
+{
+	struct b2r2_node *node = first;
+
+	while (node != NULL) {
+		/* The indices are offset by one */
+		if (node->dst_tmp_index) {
+			BUG_ON(node->dst_tmp_index > buf_count);
+
+			b2r2_log_info(cont->dev, "%s: assigning buf %d as "
+				"dst\n", __func__, node->dst_tmp_index);
+
+			node->node.GROUP1.B2R2_TBA =
+				bufs[node->dst_tmp_index - 1].phys_addr;
+		}
+		if (node->src_tmp_index) {
+			u32 addr = bufs[node->src_tmp_index - 1].phys_addr;
+
+			b2r2_log_info(cont->dev, "%s: assigning buf %d as src "
+				"%d ", __func__, node->src_tmp_index,
+				node->src_index);
+
+			BUG_ON(node->src_tmp_index > buf_count);
+
+			switch (node->src_index) {
+			case 1:
+				b2r2_log_info(cont->dev, "1\n");
+				node->node.GROUP3.B2R2_SBA = addr;
+				break;
+			case 2:
+				b2r2_log_info(cont->dev, "2\n");
+				node->node.GROUP4.B2R2_SBA = addr;
+				break;
+			case 3:
+				b2r2_log_info(cont->dev, "3\n");
+				node->node.GROUP5.B2R2_SBA = addr;
+				break;
+			default:
+				BUG_ON(1);
+				break;
+			}
+		}
+
+		b2r2_log_info(cont->dev, "%s: tba=%p\tsba=%p\n", __func__,
+			(void *)node->node.GROUP1.B2R2_TBA,
+			(void *)node->node.GROUP4.B2R2_SBA);
+
+		node = node->next;
+	}
+
+	return 0;
+}
+
+/**
+ * b2r2_node_split_unassign_buffers() - releases temporary buffers
+ */
+void b2r2_node_split_unassign_buffers(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this, struct b2r2_node *first)
+{
+	return;
+}
+
+/**
+ * b2r2_node_split_cancel() - cancels and releases a job instance
+ */
+void b2r2_node_split_cancel(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this)
+{
+	memset(this, 0, sizeof(*this));
+
+	return;
+}
+
+static int check_rect(struct b2r2_control *cont,
+		const struct b2r2_blt_img *img,
+		const struct b2r2_blt_rect *rect,
+		const struct b2r2_blt_rect *clip)
+{
+	int ret;
+
+	s32 l, r, b, t;
+
+	/* Check rectangle dimensions*/
+	if ((rect->width <= 0) || (rect->height <= 0)) {
+		b2r2_log_warn(cont->dev, "%s: Illegal rect (%d, %d, %d, %d)\n",
+				__func__, rect->x, rect->y, rect->width,
+				rect->height);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* If we are using clip we should only look at the intersection of the
+	   rects */
+	if (clip) {
+		l = max(rect->x, clip->x);
+		t = max(rect->y, clip->y);
+		r = min(rect->x + rect->width, clip->x + clip->width);
+		b = min(rect->y + rect->height, clip->y + clip->height);
+	} else {
+		l = rect->x;
+		t = rect->y;
+		r = rect->x + rect->width;
+		b = rect->y + rect->height;
+	}
+
+	/* Check so that the rect isn't outside the buffer */
+	if ((l < 0) || (t < 0) || (l >= img->width) || (t >= img->height)) {
+		b2r2_log_warn(cont->dev, "%s: rect origin outside buffer\n",
+			__func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	if ((r > img->width) || (b > img->height)) {
+		b2r2_log_warn(cont->dev, "%s: rect ends outside buffer\n",
+			__func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* Check so the intersected rectangle isn't empty */
+	if ((l == r) || (t == b)) {
+		b2r2_log_warn(cont->dev,
+			"%s: rect is empty (width or height zero)\n",
+			__func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	return 0;
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * bg_format_require_ivmx()
+ *
+ * Check if there are any color space conversion needed for the
+ * background to the destination format.
+ */
+static bool bg_format_require_ivmx(enum b2r2_blt_fmt bg_fmt,
+				enum b2r2_blt_fmt dst_fmt)
+{
+	if (b2r2_is_rgb_fmt(bg_fmt)) {
+		if (b2r2_is_yvu_fmt(dst_fmt))
+			return true;
+		else if (dst_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+				dst_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			return true;
+		else if (b2r2_is_yuv_fmt(dst_fmt))
+			return true;
+		else if (b2r2_is_bgr_fmt(dst_fmt))
+			return true;
+	} else if (b2r2_is_yvu_fmt(bg_fmt)) {
+		if (b2r2_is_rgb_fmt(dst_fmt))
+			return true;
+		else if (b2r2_is_bgr_fmt(dst_fmt))
+			return true;
+		else if (dst_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+				dst_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			return true;
+		else if (b2r2_is_yuv_fmt(dst_fmt) &&
+				!b2r2_is_yvu_fmt(dst_fmt))
+			return true;
+	} else if (bg_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+			bg_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+			bg_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+			bg_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888) {
+		if (b2r2_is_rgb_fmt(dst_fmt)) {
+			return true;
+		} else if (b2r2_is_yvu_fmt(dst_fmt)) {
+			return true;
+		} else if (b2r2_is_yuv_fmt(dst_fmt)) {
+			switch (dst_fmt) {
+			case B2R2_BLT_FMT_24_BIT_YUV888:
+			case B2R2_BLT_FMT_32_BIT_AYUV8888:
+			case B2R2_BLT_FMT_24_BIT_VUY888:
+			case B2R2_BLT_FMT_32_BIT_VUYA8888:
+				break;
+			default:
+				return true;
+			}
+		}
+	} else if (b2r2_is_yuv_fmt(bg_fmt)) {
+		if (b2r2_is_rgb_fmt(dst_fmt))
+			return true;
+		else if (b2r2_is_bgr_fmt(dst_fmt))
+			return true;
+		else if (dst_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+				dst_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			return true;
+		else if (b2r2_is_yvu_fmt(dst_fmt))
+			return true;
+	} else if (b2r2_is_bgr_fmt(bg_fmt)) {
+		if (b2r2_is_rgb_fmt(dst_fmt))
+			return true;
+		else if (b2r2_is_yvu_fmt(dst_fmt))
+			return true;
+		else if (dst_fmt == B2R2_BLT_FMT_24_BIT_YUV888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_AYUV8888 ||
+				dst_fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+				dst_fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+			return true;
+		else if (b2r2_is_yuv_fmt(dst_fmt))
+			return true;
+	}
+
+	return false;
+}
+
+/**
+ * analyze_fmt_conv() - analyze the format conversions needed for a job
+ */
+static int analyze_fmt_conv(struct b2r2_control *cont,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 **vmx, u32 *node_count, bool fullrange)
+{
+	enum b2r2_color_conversion cc =
+		b2r2_get_color_conversion(src->fmt, dst->fmt, fullrange);
+
+	b2r2_get_vmx(cc, vmx);
+
+	if (dst->type == B2R2_FMT_TYPE_RASTER) {
+		*node_count = 1;
+	} else if (dst->type == B2R2_FMT_TYPE_SEMI_PLANAR) {
+		*node_count = 2;
+	} else if (dst->type == B2R2_FMT_TYPE_PLANAR) {
+		*node_count = 3;
+	} else {
+		/* That's strange... */
+		BUG_ON(1);
+	}
+
+	return 0;
+}
+
+/**
+ * analyze_color_fill() - analyze a color fill operation
+ */
+static int analyze_color_fill(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count)
+{
+	int ret;
+	struct b2r2_control *cont = req->instance->control;
+
+	/* Destination must be raster for raw fill to work */
+	if (this->dst.type != B2R2_FMT_TYPE_RASTER) {
+		b2r2_log_warn(cont->dev,
+			"%s: fill requires raster destination\n",
+			__func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* We will try to fill the entire rectangle in one go */
+	memcpy(&this->dst.win, &this->dst.rect, sizeof(this->dst.win));
+
+	/* Check if this is a direct fill */
+	if ((!this->blend) && ((this->flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW) ||
+			(this->dst.fmt == B2R2_BLT_FMT_32_BIT_ARGB8888) ||
+			(this->dst.fmt == B2R2_BLT_FMT_32_BIT_ABGR8888) ||
+			(this->dst.fmt == B2R2_BLT_FMT_32_BIT_AYUV8888) ||
+			(this->dst.fmt == B2R2_BLT_FMT_32_BIT_VUYA8888))) {
+		this->type = B2R2_DIRECT_FILL;
+
+		/* The color format will be the same as the dst fmt */
+		this->src.fmt = this->dst.fmt;
+
+		/* The entire destination rectangle will be  */
+		memcpy(&this->dst.win, &this->dst.rect,
+			sizeof(this->dst.win));
+		*node_count = 1;
+	} else {
+		this->type = B2R2_FILL;
+
+		/* Determine the fill color format */
+		if (this->flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW) {
+			/* The color format will be the same as the dst fmt */
+			this->src.fmt = this->dst.fmt;
+		} else {
+			/* If the dst fmt is YUV the fill fmt will be as well */
+			if (b2r2_is_yuv_fmt(this->dst.fmt)) {
+				this->src.fmt =	B2R2_BLT_FMT_32_BIT_AYUV8888;
+			} else if (b2r2_is_rgb_fmt(this->dst.fmt)) {
+				this->src.fmt =	B2R2_BLT_FMT_32_BIT_ARGB8888;
+			} else if (b2r2_is_bgr_fmt(this->dst.fmt)) {
+				/* Color will still be ARGB, we will translate
+				   using IVMX (configured later) */
+				this->src.fmt =	B2R2_BLT_FMT_32_BIT_ARGB8888;
+			} else {
+				/* Wait, what? */
+				b2r2_log_warn(cont->dev, "%s: "
+					"Illegal destination format for fill",
+					__func__);
+				ret = -EINVAL;
+				goto error;
+			}
+		}
+
+		/* Also, B2R2 seems to ignore the pixel alpha value */
+		if (((this->flags & B2R2_BLT_FLAG_PER_PIXEL_ALPHA_BLEND)
+					!= 0) &&
+				((this->flags & B2R2_BLT_FLAG_SOURCE_FILL_RAW)
+					== 0) && b2r2_fmt_has_alpha(this->src.fmt)) {
+			u8 pixel_alpha = b2r2_get_alpha(this->src.fmt,
+							this->src.color);
+			u32 new_global = pixel_alpha * this->global_alpha / 255;
+
+			this->global_alpha = (u8)new_global;
+
+			/* Set the pixel alpha to full opaque so we don't get
+			   any nasty surprises */
+			this->src.color = b2r2_set_alpha(this->src.fmt, 0xFF,
+						this->src.color);
+		}
+
+		ret = analyze_fmt_conv(
+			cont, &this->src, &this->dst, &this->ivmx,
+			node_count, this->fullrange);
+		if (ret < 0)
+			goto error;
+	}
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+
+}
+
+/**
+ * analyze_transform() - analyze a transform operation (rescale, rotate, etc.)
+ */
+static int analyze_transform(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+	bool is_scaling;
+#ifdef CONFIG_B2R2_DEBUG
+	struct b2r2_control *cont = req->instance->control;
+#endif
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	/*
+	 * The transform enum is defined so that all rotation transforms are
+	 * masked with the rotation flag
+	 */
+	this->rotation = (this->transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) != 0;
+
+	/* B2R2 cannot do rotations if the destination is not raster, or 422R */
+	if (this->rotation && (this->dst.type != B2R2_FMT_TYPE_RASTER ||
+				this->dst.fmt == B2R2_BLT_FMT_Y_CB_Y_CR ||
+				this->dst.fmt == B2R2_BLT_FMT_CB_Y_CR_Y)) {
+		b2r2_log_warn(cont->dev,
+			"%s: Unsupported operation "
+			"(rot && (!dst_raster || dst==422R))",
+			__func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	/* Flip the image by changing the scan order of the destination */
+	if (this->transform & B2R2_BLT_TRANSFORM_FLIP_H)
+		this->dst.hso = B2R2_TY_HSO_RIGHT_TO_LEFT;
+	if (this->transform & B2R2_BLT_TRANSFORM_FLIP_V)
+		this->dst.vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+
+	/* Check for scaling */
+	if (this->rotation) {
+		is_scaling = (this->src.rect.width != this->dst.rect.height) ||
+			(this->src.rect.height != this->dst.rect.width);
+	} else {
+		is_scaling = (this->src.rect.width != this->dst.rect.width) ||
+			(this->src.rect.height != this->dst.rect.height);
+	}
+
+	/* Plane separated formats must be treated as scaling */
+	is_scaling = is_scaling ||
+			(this->src.type == B2R2_FMT_TYPE_SEMI_PLANAR) ||
+			(this->src.type == B2R2_FMT_TYPE_PLANAR) ||
+			(this->dst.type == B2R2_FMT_TYPE_SEMI_PLANAR) ||
+			(this->dst.type == B2R2_FMT_TYPE_PLANAR);
+
+	if (is_scaling && this->rotation && this->blend) {
+		/* TODO: This is unsupported. Fix it! */
+		b2r2_log_info(cont->dev, "%s: Unsupported operation "
+			"(rot+rescale+blend)\n", __func__);
+		ret = -ENOSYS;
+		goto unsupported;
+	}
+
+	/* Check which type of transform */
+	if (is_scaling && this->rotation) {
+		ret = analyze_rot_scale(this, req, node_count, buf_count);
+		if (ret < 0)
+			goto error;
+	} else if (is_scaling) {
+		ret = analyze_scaling(this, req, node_count, buf_count);
+		if (ret < 0)
+			goto error;
+	} else if (this->rotation) {
+		ret = analyze_rotate(this, req, node_count, buf_count);
+		if (ret < 0)
+			goto error;
+	} else {
+		/* No additional nodes needed for a flip */
+		ret = analyze_copy(this, req, node_count, buf_count);
+		if (ret < 0)
+			goto error;
+		this->type = B2R2_FLIP;
+	}
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: error!\n", __func__);
+unsupported:
+	return ret;
+}
+
+/**
+ * analyze_copy() - analyze a copy operation
+ */
+static int analyze_copy(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+	struct b2r2_control *cont = req->instance->control;
+
+	memcpy(&this->dst.win, &this->dst.rect, sizeof(this->dst.win));
+
+	if (!this->blend &&
+			!(this->flags & B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) &&
+			(this->src.fmt == this->dst.fmt) &&
+			(this->src.type == B2R2_FMT_TYPE_RASTER) &&
+			(this->dst.rect.x >= this->clip_rect.x) &&
+			(this->dst.rect.y >= this->clip_rect.y) &&
+			(this->dst.rect.x + this->dst.rect.width <=
+				this->clip_rect.x + this->clip_rect.width) &&
+			(this->dst.rect.y + this->dst.rect.height <=
+				this->clip_rect.y + this->clip_rect.height)) {
+		this->type = B2R2_DIRECT_COPY;
+		*node_count = 1;
+	} else {
+		u32 copy_count;
+
+		this->type = B2R2_COPY;
+
+		ret = analyze_fmt_conv(cont, &this->src, &this->dst,
+			&this->ivmx, &copy_count, this->fullrange);
+		if (ret < 0)
+			goto error;
+
+		*node_count = copy_count;
+	}
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+}
+
+static int calc_rot_count(u32 width, u32 height)
+{
+	int count;
+
+	count = width / B2R2_ROTATE_MAX_WIDTH;
+	if (width % B2R2_ROTATE_MAX_WIDTH)
+		count++;
+	if (height > B2R2_ROTATE_MAX_WIDTH &&
+			height % B2R2_ROTATE_MAX_WIDTH)
+		count *= 2;
+
+	return count;
+}
+
+static int analyze_rot_scale_downscale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+	struct b2r2_control *cont = req->instance->control;
+	struct b2r2_node_split_buf *src = &this->src;
+	struct b2r2_node_split_buf *dst = &this->dst;
+	struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+
+	u32 num_rows;
+	u32 num_cols;
+	u32 rot_count;
+	u32 rescale_count;
+	u32 nodes_per_rot;
+	u32 nodes_per_rescale;
+	u32 right_width;
+	u32 bottom_height;
+	const u32 *dummy_vmx;
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	/* Calculate the desired tmp buffer size */
+	tmp->win.width = rescale(cont, B2R2_RESCALE_MAX_WIDTH - 1, this->h_rsf);
+	tmp->win.width >>= 10;
+	tmp->win.width = min(tmp->win.width, dst->rect.height);
+	tmp->win.height = dst->rect.width;
+
+	setup_tmp_buf(cont, tmp, this->max_buf_size, dst->fmt, tmp->win.width,
+			tmp->win.height);
+	tmp->tmp_buf_index = 1;
+	this->work_bufs[0].size = tmp->pitch * tmp->height;
+
+	tmp->win.width = tmp->rect.width;
+	tmp->win.height = tmp->rect.height;
+
+	tmp->dither = dst->dither;
+	dst->dither = 0;
+
+	/* Update the dst window with the actual tmp buffer dimensions */
+	dst->win.width = tmp->win.height;
+	dst->win.height = tmp->win.width;
+
+	/* The rotated stripes are written to the destination bottom-up */
+	if (this->dst.vso == B2R2_TY_VSO_TOP_TO_BOTTOM)
+		this->dst.vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+	else
+		this->dst.vso = B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	/*
+	 * Calculate how many nodes are required to copy to and from the tmp
+	 * buffer
+	 */
+	ret = analyze_fmt_conv(cont, src, tmp, &this->ivmx, &nodes_per_rescale,
+			this->fullrange);
+	if (ret < 0)
+		goto error;
+
+	/* We will not do any format conversion in the rotation stage */
+	ret = analyze_fmt_conv(cont, tmp, dst, &dummy_vmx, &nodes_per_rot,
+			this->fullrange);
+	if (ret < 0)
+		goto error;
+
+	/* Calculate node count for the inner tiles */
+	num_cols = dst->rect.width / dst->win.width;
+	num_rows = dst->rect.height / dst->win.height;
+
+	rescale_count = num_cols * num_rows;
+	rot_count = calc_rot_count(dst->win.height, dst->win.width) *
+		num_cols * num_rows;
+
+	right_width = dst->rect.width % dst->win.width;
+	bottom_height = dst->rect.height % dst->win.height;
+
+	/* Calculate node count for the rightmost tiles */
+	if (right_width) {
+		u32 count = calc_rot_count(dst->win.height, right_width);
+
+		rot_count += count * num_rows;
+		rescale_count += num_rows;
+		b2r2_log_info(cont->dev, "%s: rightmost: %d nodes\n", __func__,
+			count*num_rows);
+	}
+
+	/* Calculate node count for the bottom tiles */
+	if (bottom_height) {
+		u32 count = calc_rot_count(bottom_height, dst->win.width);
+
+		rot_count += count * num_cols;
+		rescale_count += num_cols;
+		b2r2_log_info(cont->dev, "%s: bottom: %d nodes\n", __func__,
+			count * num_cols);
+
+	}
+
+	/* And finally for the bottom right corner */
+	if (right_width && bottom_height) {
+		u32 count = calc_rot_count(bottom_height, right_width);
+
+		rot_count += count;
+		rescale_count++;
+		b2r2_log_info(cont->dev, "%s: bottom right: %d nodes\n",
+			__func__, count);
+
+	}
+
+	*node_count = rot_count * nodes_per_rot;
+	*node_count += rescale_count * nodes_per_rescale;
+	*buf_count = 1;
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: error!\n", __func__);
+	return ret;
+}
+
+static int analyze_rot_scale_upscale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	/* TODO: When upscaling we should optimally to the rotation first... */
+	return analyze_rot_scale_downscale(this, req, node_count, buf_count);
+}
+
+/**
+ * analyze_rot_scaling() - analyzes a combined rotation and scaling op
+ */
+static int analyze_rot_scale(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+	bool upscale;
+	struct b2r2_control *cont = req->instance->control;
+
+	ret = analyze_scale_factors(cont, this);
+	if (ret < 0)
+		goto error;
+
+	upscale = (u32)this->h_rsf * (u32)this->v_rsf < (1 << 20);
+
+	if (upscale)
+		ret = analyze_rot_scale_upscale(this, req, node_count,
+			buf_count);
+	else
+		ret = analyze_rot_scale_downscale(this, req, node_count,
+			buf_count);
+
+	if (ret < 0)
+		goto error;
+
+	this->type = B2R2_SCALE_AND_ROTATE;
+
+	return 0;
+
+error:
+	return ret;
+}
+
+/**
+ * analyze_scaling() - analyze a rescale operation
+ */
+static int analyze_scaling(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+	u32 copy_count;
+	u32 nbr_cols;
+	s32 dst_w;
+	struct b2r2_control *cont = req->instance->control;
+
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	ret = analyze_scale_factors(cont, this);
+	if (ret < 0)
+		goto error;
+
+	/* Find out how many nodes a simple copy would require */
+	ret = analyze_fmt_conv(cont, &this->src, &this->dst, &this->ivmx,
+			&copy_count, this->fullrange);
+	if (ret < 0)
+		goto error;
+
+	memcpy(&this->dst.win, &this->dst.rect, sizeof(this->dst.win));
+
+	/*
+	 * We need to subtract from the actual maximum rescale width since the
+	 * start of the stripe will be floored and the end ceiled. This could in
+	 * some cases cause the stripe to be one pixel more than the maximum
+	 * width.
+	 *
+	 * Example:
+	 *   x = 127.8, w = 127.8
+	 *
+	 *   The stripe will touch pixels 127.8 through 255.6, i.e. 129 pixels.
+	 */
+	dst_w = rescale(cont, B2R2_RESCALE_MAX_WIDTH - 1, this->h_rsf);
+	if (dst_w < (1 << 10))
+		dst_w = 1;
+	else
+		dst_w >>= 10;
+
+	b2r2_log_info(cont->dev, "%s: dst_w=%d dst.rect.width=%d\n",
+		__func__, dst_w, this->dst.rect.width);
+
+	this->dst.win.width = min(dst_w, this->dst.rect.width);
+
+	b2r2_log_info(cont->dev, "%s: dst.win.width=%d\n",
+		__func__, this->dst.win.width);
+
+	nbr_cols = this->dst.rect.width / this->dst.win.width;
+	if (this->dst.rect.width % this->dst.win.width)
+		nbr_cols++;
+
+	*node_count = copy_count * nbr_cols;
+
+	this->type = B2R2_SCALE;
+
+	b2r2_log_info(cont->dev, "%s exit\n", __func__);
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+
+}
+
+/**
+ * analyze_rotate() - analyze a rotate operation
+ */
+static int analyze_rotate(struct b2r2_node_split_job *this,
+		const struct b2r2_blt_request *req, u32 *node_count,
+		u32 *buf_count)
+{
+	int ret;
+	u32 nodes_per_tile;
+	struct b2r2_control *cont = req->instance->control;
+
+	/* Find out how many nodes a simple copy would require */
+	ret = analyze_fmt_conv(cont, &this->src, &this->dst, &this->ivmx,
+			&nodes_per_tile, this->fullrange);
+	if (ret < 0)
+		goto error;
+
+	this->type = B2R2_ROTATE;
+
+	/* The rotated stripes are written to the destination bottom-up */
+	if (this->dst.vso == B2R2_TY_VSO_TOP_TO_BOTTOM)
+		this->dst.vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+	else
+		this->dst.vso = B2R2_TY_VSO_TOP_TO_BOTTOM;
+
+	memcpy(&this->dst.win, &this->dst.rect, sizeof(this->dst.win));
+
+	this->dst.win.height = min(this->dst.win.height, B2R2_ROTATE_MAX_WIDTH);
+
+	/*
+	 * B2R2 cannot do rotations on stripes that are not a multiple of 16
+	 * pixels high (if larger than 16 pixels).
+	 */
+	if (this->dst.win.width > 16)
+		this->dst.win.width -= (this->dst.win.width % 16);
+
+	/* Blending cannot be combined with rotation */
+	if (this->blend) {
+		struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+		enum b2r2_blt_fmt tmp_fmt;
+
+		if (b2r2_is_yuv_fmt(this->dst.fmt))
+			tmp_fmt = B2R2_BLT_FMT_32_BIT_AYUV8888;
+		else if (b2r2_is_bgr_fmt(this->dst.fmt))
+			tmp_fmt = B2R2_BLT_FMT_32_BIT_ABGR8888;
+		else
+			tmp_fmt = B2R2_BLT_FMT_32_BIT_ARGB8888;
+
+		setup_tmp_buf(cont, tmp, this->max_buf_size, tmp_fmt,
+				this->dst.win.width, this->dst.win.height);
+
+		tmp->tmp_buf_index = 1;
+
+		tmp->vso = B2R2_TY_VSO_BOTTOM_TO_TOP;
+
+		this->dst.win.width = tmp->rect.width;
+		this->dst.win.height = tmp->rect.height;
+
+		memcpy(&tmp->win, &tmp->rect, sizeof(tmp->win));
+
+		*buf_count = 1;
+		this->work_bufs[0].size = tmp->pitch * tmp->height;
+
+		/*
+		 * One more node per tile is required to rotate to the temp
+		 * buffer.
+		 */
+		nodes_per_tile++;
+	}
+
+	/* Finally, calculate the node count */
+	*node_count = nodes_per_tile *
+		calc_rot_count(this->src.rect.width, this->src.rect.height);
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * analyze_scale_factors() - determines the scale factors for the op
+ */
+static int analyze_scale_factors(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this)
+{
+	int ret;
+
+	u16 hsf;
+	u16 vsf;
+
+	if (this->rotation) {
+		ret = calculate_scale_factor(cont->dev, this->src.rect.width,
+					this->dst.rect.height, &hsf);
+		if (ret < 0)
+			goto error;
+
+		ret = calculate_scale_factor(cont->dev, this->src.rect.height,
+					this->dst.rect.width, &vsf);
+		if (ret < 0)
+			goto error;
+	} else {
+		ret = calculate_scale_factor(cont->dev, this->src.rect.width,
+					this->dst.rect.width, &hsf);
+		if (ret < 0)
+			goto error;
+
+		ret = calculate_scale_factor(cont->dev, this->src.rect.height,
+					this->dst.rect.height, &vsf);
+		if (ret < 0)
+			goto error;
+	}
+
+	this->h_rescale = hsf != (1 << 10);
+	this->v_rescale = vsf != (1 << 10);
+
+	this->h_rsf = hsf;
+	this->v_rsf = vsf;
+
+	b2r2_log_info(cont->dev, "%s: h_rsf=%.4x\n", __func__, this->h_rsf);
+	b2r2_log_info(cont->dev, "%s: v_rsf=%.4x\n", __func__, this->v_rsf);
+
+	return 0;
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_tile() - configures one tile of a blit operation
+ */
+static int configure_tile(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this, struct b2r2_node *node,
+		struct b2r2_node **next)
+{
+	int ret = 0;
+
+	struct b2r2_node *last;
+	struct b2r2_node_split_buf *src = &this->src;
+	struct b2r2_node_split_buf *dst = &this->dst;
+	struct b2r2_node_split_buf *bg = &this->bg;
+
+	struct b2r2_blt_rect dst_norm;
+	struct b2r2_blt_rect src_norm;
+	struct b2r2_blt_rect bg_norm;
+
+	/* Normalize the dest coords to the dest rect coordinate space  */
+	dst_norm.x = dst->win.x - dst->rect.x;
+	dst_norm.y = dst->win.y - dst->rect.y;
+	dst_norm.width = dst->win.width;
+	dst_norm.height = dst->win.height;
+
+	if (dst->vso == B2R2_TY_VSO_BOTTOM_TO_TOP) {
+		/* The y coord should be counted from the bottom */
+		dst_norm.y = dst->rect.height - (dst_norm.y + 1);
+	}
+	if (dst->hso == B2R2_TY_HSO_RIGHT_TO_LEFT) {
+		/* The x coord should be counted from the right */
+		dst_norm.x = dst->rect.width - (dst_norm.x + 1);
+	}
+
+	/* If the destination is rotated we should swap x, y */
+	if (this->rotation) {
+		src_norm.x = dst_norm.y;
+		src_norm.y = dst_norm.x;
+		src_norm.width = dst_norm.height;
+		src_norm.height = dst_norm.width;
+	} else {
+		src_norm.x = dst_norm.x;
+		src_norm.y = dst_norm.y;
+		src_norm.width = dst_norm.width;
+		src_norm.height = dst_norm.height;
+	}
+
+	/* Convert to src coordinate space */
+	src->win.x = src_norm.x + src->rect.x;
+	src->win.y = src_norm.y + src->rect.y;
+	src->win.width = src_norm.width;
+	src->win.height = src_norm.height;
+
+	/* Set bg norm */
+	bg_norm.x = dst->win.x - dst->rect.x;
+	bg_norm.y = dst->win.y - dst->rect.y;
+	bg_norm.width = dst->win.width;
+	bg_norm.height = dst->win.height;
+
+	/* Convert to bg coordinate space */
+	bg->win.x = bg_norm.x + bg->rect.x;
+	bg->win.y = bg_norm.y + bg->rect.y;
+	bg->win.width = bg_norm.width;
+	bg->win.height = bg_norm.height;
+	bg->vso = dst->vso;
+	bg->hso = dst->hso;
+
+	/* Do the configuration depending on operation type */
+	switch (this->type) {
+	case B2R2_DIRECT_FILL:
+		configure_direct_fill(cont, node, this->src.color, dst, &last);
+		break;
+
+	case B2R2_DIRECT_COPY:
+		configure_direct_copy(cont, node, src, dst, &last);
+		break;
+
+	case B2R2_FILL:
+		ret = configure_fill(cont, node, src->color, src->fmt,
+				dst, this->ivmx, &last);
+		break;
+
+	case B2R2_FLIP: /* FLIP is just a copy with different VSO/HSO */
+	case B2R2_COPY:
+		ret = configure_copy(
+			cont, node, src, dst, this->ivmx, &last, this);
+		break;
+
+	case B2R2_ROTATE:
+		{
+			struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+
+			if (this->blend) {
+				b2r2_log_info(cont->dev, "%s: rotation + "
+						"blend\n", __func__);
+
+				tmp->win.x = 0;
+				tmp->win.y = tmp->win.height - 1;
+				tmp->win.width = dst->win.width;
+				tmp->win.height = dst->win.height;
+
+				/* Rotate to the temp buf */
+				ret = configure_rotate(cont, node, src, tmp,
+						this->ivmx, &node, NULL);
+				if (ret < 0)
+					goto error;
+
+				/* Then do a copy to the destination */
+				ret = configure_copy(cont, node, tmp, dst, NULL,
+						&last, this);
+			} else {
+				/* Just do a rotation */
+				ret = configure_rotate(cont, node, src, dst,
+						this->ivmx, &last, this);
+			}
+		}
+		break;
+
+	case B2R2_SCALE:
+		ret = configure_scale(cont, node, src, dst, this->h_rsf,
+				this->v_rsf, this->ivmx, &last, this);
+		break;
+
+	case B2R2_SCALE_AND_ROTATE:
+		ret = configure_rot_scale(cont, this, node, &last);
+		break;
+
+	default:
+		b2r2_log_warn(cont->dev, "%s: Unsupported request\n", __func__);
+		ret = -ENOSYS;
+		goto error;
+		break;
+
+	}
+
+	if (ret < 0)
+		goto error;
+
+	/* Scale and rotate will configure its own blending and clipping */
+	if (this->type != B2R2_SCALE_AND_ROTATE) {
+
+		/* Configure blending and clipping */
+		do {
+			if (node == NULL) {
+				b2r2_log_warn(cont->dev, "%s: "
+					"Internal error! Out of nodes!\n",
+					__func__);
+				ret = -ENOMEM;
+				goto error;
+			}
+
+			if (this->blend) {
+				if (this->flags & B2R2_BLT_FLAG_BG_BLEND)
+					configure_bg(cont, node, bg,
+							this->swap_fg_bg);
+				else
+					configure_bg(cont, node, dst,
+							this->swap_fg_bg);
+				configure_blend(cont, node, this->flags,
+						this->global_alpha);
+			}
+			if (this->clip)
+				configure_clip(cont, node, &this->clip_rect);
+
+			node = node->next;
+
+		} while (node != last);
+	}
+
+	/* Consume the nodes */
+	*next = last;
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: Error!\n", __func__);
+	return ret;
+}
+
+/*
+ * configure_sub_rot() - configure a sub-rotation
+ *
+ * This functions configures a set of nodes for rotation using the destination
+ * window instead of the rectangle for calculating tiles.
+ */
+static int configure_sub_rot(struct b2r2_control *cont,
+		struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 *ivmx, struct b2r2_node **next,
+		struct b2r2_node_split_job *job)
+{
+	int ret;
+
+	struct b2r2_blt_rect src_win;
+	struct b2r2_blt_rect dst_win;
+
+	u32 y_pixels = 0;
+	u32 x_pixels = 0;
+
+	memcpy(&src_win, &src->win, sizeof(src_win));
+	memcpy(&dst_win, &dst->win, sizeof(dst_win));
+
+	b2r2_log_info(cont->dev, "%s: src_win=(%d, %d, %d, %d) "
+			"dst_win=(%d, %d, %d, %d)\n", __func__,
+			src_win.x, src_win.y, src_win.width, src_win.height,
+			dst_win.x, dst_win.y, dst_win.width, dst_win.height);
+
+	dst->win.height = B2R2_ROTATE_MAX_WIDTH;
+	if (dst->win.width % B2R2_ROTATE_MAX_WIDTH)
+		dst->win.width -= dst->win.width % B2R2_ROTATE_MAX_WIDTH;
+
+	while (x_pixels < dst_win.width) {
+		u32 src_x = src->win.x;
+		u32 src_w = src->win.width;
+		u32 dst_y = dst->win.y;
+		u32 dst_h = dst->win.height;
+
+		dst->win.width = min(dst->win.width, dst_win.width -
+			(int)x_pixels);
+		src->win.height = dst->win.width;
+
+		b2r2_log_info(cont->dev, "%s: x_pixels=%d\n",
+			__func__, x_pixels);
+
+		while (y_pixels < dst_win.height) {
+			dst->win.height = min(dst->win.height,
+					dst_win.height - (int)y_pixels);
+			src->win.width = dst->win.height;
+
+			b2r2_log_info(cont->dev, "%s: y_pixels=%d\n",
+				__func__, y_pixels);
+
+			ret = configure_rotate(cont, node, src, dst,
+				ivmx, &node, job);
+			if (ret < 0)
+				goto error;
+
+			src->win.x += (src->hso == B2R2_TY_HSO_LEFT_TO_RIGHT) ?
+					src->win.width : -src->win.width;
+			dst->win.y += (dst->vso == B2R2_TY_VSO_TOP_TO_BOTTOM) ?
+					dst->win.height : -dst->win.height;
+
+			y_pixels += dst->win.height;
+		}
+
+		src->win.x = src_x;
+		src->win.y += (src->vso == B2R2_TY_VSO_TOP_TO_BOTTOM) ?
+				src->win.height : -src->win.height;
+		src->win.width = src_w;
+
+		dst->win.x += (dst->hso == B2R2_TY_HSO_LEFT_TO_RIGHT) ?
+				dst->win.width : -dst->win.width;
+		dst->win.y = dst_y;
+		dst->win.height = dst_h;
+
+		x_pixels += dst->win.width;
+		y_pixels = 0;
+
+	}
+
+	memcpy(&src->win, &src_win, sizeof(src->win));
+	memcpy(&dst->win, &dst_win, sizeof(dst->win));
+
+	*next = node;
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_rot_downscale() - configures a combined rotate and downscale
+ *
+ * When doing a downscale it is better to do the rotation last.
+ */
+static int configure_rot_downscale(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this,
+		struct b2r2_node *node,	struct b2r2_node **next)
+{
+	int ret;
+
+	struct b2r2_node_split_buf *src = &this->src;
+	struct b2r2_node_split_buf *dst = &this->dst;
+	struct b2r2_node_split_buf *tmp = &this->tmp_bufs[0];
+
+	tmp->win.x = 0;
+	tmp->win.y = 0;
+	tmp->win.width = dst->win.height;
+	tmp->win.height = dst->win.width;
+
+	ret = configure_scale(cont, node, src, tmp, this->h_rsf, this->v_rsf,
+			this->ivmx, &node, this);
+	if (ret < 0)
+		goto error;
+
+	ret = configure_sub_rot(cont, node, tmp, dst, NULL, &node, this);
+	if (ret < 0)
+		goto error;
+
+	*next = node;
+
+	return 0;
+
+error:
+	b2r2_log_info(cont->dev, "%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_rot_upscale() - configures a combined rotate and upscale
+ *
+ * When doing an upscale it is better to do the rotation first.
+ */
+static int configure_rot_upscale(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this, struct b2r2_node *node,
+		struct b2r2_node **next)
+{
+	/* TODO: Implement a optimal upscale (rotation first) */
+	return configure_rot_downscale(cont, this, node, next);
+}
+
+/**
+ * configure_rot_scale() - configures a combined rotation and scaling op
+ */
+static int configure_rot_scale(struct b2r2_control *cont,
+		struct b2r2_node_split_job *this, struct b2r2_node *node,
+		struct b2r2_node **next)
+{
+	int ret;
+
+	bool upscale = (u32)this->h_rsf * (u32)this->v_rsf < (1 << 10);
+
+	if (upscale)
+		ret = configure_rot_upscale(cont, this, node, next);
+	else
+		ret = configure_rot_downscale(cont, this, node, next);
+
+	if (ret < 0)
+		goto error;
+
+	return 0;
+
+error:
+	b2r2_log_warn(cont->dev, "%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_direct_fill() - configures the given node for direct fill
+ *
+ * @node  - the node to configure
+ * @color - the fill color
+ * @dst   - the destination buffer
+ * @next  - the next empty node in the node list
+ *
+ * This operation will always consume one node only.
+ */
+static void configure_direct_fill(
+		struct b2r2_control *cont,
+		struct b2r2_node *node,
+		u32 color,
+		struct b2r2_node_split_buf *dst,
+		struct b2r2_node **next)
+{
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_COLOR_FILL | B2R2_CIC_SOURCE_1;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_1_DIRECT_FILL;
+
+	/* Target setup */
+	set_target(node, dst->addr, dst);
+
+	/* Source setup */
+
+	/* It seems B2R2 checks so that source and dest has the same format */
+	node->node.GROUP3.B2R2_STY = b2r2_to_native_fmt(dst->fmt);
+	node->node.GROUP2.B2R2_S1CF = color;
+	node->node.GROUP2.B2R2_S2CF = 0;
+
+	/* Consume the node */
+	*next = node->next;
+}
+
+/**
+ * configure_direct_copy() - configures the node for direct copy
+ *
+ * @node - the node to configure
+ * @src  - the source buffer
+ * @dst  - the destination buffer
+ * @next - the next empty node in the node list
+ *
+ * This operation will always consume one node only.
+ */
+static void configure_direct_copy(
+		struct b2r2_control *cont,
+		struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		struct b2r2_node **next)
+{
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_1;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_1_DIRECT_COPY;
+
+	/* Source setup, use the base function to avoid altering the INS */
+	set_src(&node->node.GROUP3, src->addr, src);
+
+	/* Target setup */
+	set_target(node, dst->addr, dst);
+
+	/* Consume the node */
+	*next = node->next;
+}
+
+/**
+ * configure_fill() - configures the given node for color fill
+ *
+ * @node  - the node to configure
+ * @color - the fill color
+ * @fmt   - the source color format
+ * @dst   - the destination buffer
+ * @next  - the next empty node in the node list
+ *
+ * A normal fill operation can be combined with any other per pixel operations
+ * such as blend.
+ *
+ * This operation will consume as many nodes as are required to write to the
+ * destination format.
+ */
+static int configure_fill(
+		struct b2r2_control *cont,
+		struct b2r2_node *node,
+		u32 color,
+		enum b2r2_blt_fmt fmt,
+		struct b2r2_node_split_buf *dst,
+		const u32 *ivmx,
+		struct b2r2_node **next)
+{
+	int ret;
+	struct b2r2_node *last;
+
+	/* Configure the destination */
+	ret = configure_dst(cont, node, dst, ivmx, &last);
+	if (ret < 0)
+		goto error;
+
+	do {
+		if (node == NULL) {
+			b2r2_log_warn(cont->dev, "%s: "
+			"Internal error! Out of nodes!\n", __func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2 |
+							B2R2_CIC_COLOR_FILL;
+		node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_SOURCE_2_COLOR_FILL_REGISTER;
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+
+		/* B2R2 has a bug that disables color fill from S2. As a
+		   workaround we use S1 for the color. */
+		node->node.GROUP2.B2R2_S1CF = 0;
+		node->node.GROUP2.B2R2_S2CF = color;
+
+		/* TO BE REMOVED: */
+		set_src_2(node, dst->addr, dst);
+		node->node.GROUP4.B2R2_STY = b2r2_to_native_fmt(fmt);
+
+		/* Setup the iVMX for color conversion */
+		if (ivmx != NULL)
+			set_ivmx(node, ivmx);
+
+		if  ((dst->type == B2R2_FMT_TYPE_PLANAR) ||
+				(dst->type == B2R2_FMT_TYPE_SEMI_PLANAR)) {
+
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP8.B2R2_FCTL =
+				B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_RESIZER |
+				B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_RESIZER;
+			node->node.GROUP9.B2R2_RSF =
+				(1 << (B2R2_RSF_HSRC_INC_SHIFT + 10)) |
+				(1 << (B2R2_RSF_VSRC_INC_SHIFT + 10));
+			node->node.GROUP9.B2R2_RZI =
+				B2R2_RZI_DEFAULT_HNB_REPEAT |
+				(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+
+			node->node.GROUP10.B2R2_RSF =
+				(1 << (B2R2_RSF_HSRC_INC_SHIFT + 10)) |
+				(1 << (B2R2_RSF_VSRC_INC_SHIFT + 10));
+			node->node.GROUP10.B2R2_RZI =
+				B2R2_RZI_DEFAULT_HNB_REPEAT |
+				(2 << B2R2_RZI_VNB_REPEAT_SHIFT);
+		}
+
+		node = node->next;
+
+	} while (node != last);
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_copy() - configures the given node for a copy operation
+ *
+ * @node - the node to configure
+ * @src  - the source buffer
+ * @dst  - the destination buffer
+ * @ivmx - the iVMX to use for color conversion
+ * @next - the next empty node in the node list
+ *
+ * This operation will consume as many nodes as are required to write to the
+ * destination format.
+ */
+static int configure_copy(
+		struct b2r2_control *cont,
+		struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 *ivmx,
+		struct b2r2_node **next,
+		struct b2r2_node_split_job *this)
+{
+	int ret;
+
+	struct b2r2_node *last;
+
+	ret = configure_dst(cont, node, dst, ivmx, &last);
+	if (ret < 0)
+		goto error;
+
+	/* Configure the source for each node */
+	do {
+		if (node == NULL) {
+			b2r2_log_warn(cont->dev, "%s: "
+				" Internal error! Out of nodes!\n",
+				__func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BYPASS_S2_S3;
+		if (this != NULL &&
+				(this->flags & B2R2_BLT_FLAG_SOURCE_COLOR_KEY)
+				!= 0) {
+			u32 key_color = 0;
+
+			node->node.GROUP0.B2R2_ACK |=
+				B2R2_ACK_CKEY_SEL_SRC_AFTER_CLUT |
+				B2R2_ACK_CKEY_RED_MATCH_IF_BETWEEN |
+				B2R2_ACK_CKEY_GREEN_MATCH_IF_BETWEEN |
+				B2R2_ACK_CKEY_BLUE_MATCH_IF_BETWEEN;
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_CKEY_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_COLOR_KEY;
+
+			key_color = b2r2_to_RGB888(this->flag_param, src->fmt);
+			node->node.GROUP12.B2R2_KEY1 = key_color;
+			node->node.GROUP12.B2R2_KEY2 = key_color;
+		}
+
+		if (this != NULL &&
+				(this->flags &
+				B2R2_BLT_FLAG_CLUT_COLOR_CORRECTION) != 0) {
+			struct b2r2_blt_request *request =
+				container_of(this, struct b2r2_blt_request,
+						node_split_job);
+			node->node.GROUP0.B2R2_INS |= B2R2_INS_CLUTOP_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_CLUT;
+			node->node.GROUP7.B2R2_CCO =
+				B2R2_CCO_CLUT_COLOR_CORRECTION |
+				B2R2_CCO_CLUT_UPDATE;
+			node->node.GROUP7.B2R2_CML = request->clut_phys_addr;
+		}
+		/* Configure the source(s) */
+		configure_src(cont, node, src, ivmx);
+
+		node = node->next;
+	} while (node != last);
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_rotate() - configures the given node for rotation
+ *
+ * @node - the node to configure
+ * @src  - the source buffer
+ * @dst  - the destination buffer
+ * @ivmx - the iVMX to use for color conversion
+ * @next - the next empty node in the node list
+ *
+ * This operation will consume as many nodes are are required by the combination
+ * of rotating and writing the destination format.
+ */
+static int configure_rotate(
+		struct b2r2_control *cont,
+		struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		const u32 *ivmx,
+		struct b2r2_node **next,
+		struct b2r2_node_split_job *this)
+{
+	int ret;
+
+	struct b2r2_node *last;
+
+	ret = configure_copy(cont, node, src, dst, ivmx, &last, this);
+	if (ret < 0)
+		goto error;
+
+	do {
+		if (node == NULL) {
+			b2r2_log_warn(cont->dev, "%s: "
+				"Internal error! Out of nodes!\n",
+				__func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_ROTATION_ENABLED;
+
+		b2r2_log_debug(cont->dev, "%s:\n"
+				"\tB2R2_TXY:  %.8x\tB2R2_TSZ:  %.8x\n"
+				"\tB2R2_S1XY: %.8x\tB2R2_S1SZ: %.8x\n"
+				"\tB2R2_S2XY: %.8x\tB2R2_S2SZ: %.8x\n"
+				"\tB2R2_S3XY: %.8x\tB2R2_S3SZ: %.8x\n"
+				"-----------------------------------\n",
+				__func__, node->node.GROUP1.B2R2_TXY,
+				node->node.GROUP1.B2R2_TSZ,
+				node->node.GROUP3.B2R2_SXY,
+				node->node.GROUP3.B2R2_SSZ,
+				node->node.GROUP4.B2R2_SXY,
+				node->node.GROUP4.B2R2_SSZ,
+				node->node.GROUP5.B2R2_SXY,
+				node->node.GROUP5.B2R2_SSZ);
+
+		node = node->next;
+
+	} while (node != last);
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn(cont->dev, "%s: error!\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_scale() - configures the given node for scaling
+ *
+ * @node  - the node to configure
+ * @src   - the source buffer
+ * @dst   - the destination buffer
+ * @h_rsf - the horizontal rescale factor
+ * @v_rsf - the vertical rescale factor
+ * @ivmx  - the iVMX to use for color conversion
+ * @next  - the next empty node in the node list
+ */
+static int configure_scale(
+		struct b2r2_control *cont,
+		struct b2r2_node *node,
+		struct b2r2_node_split_buf *src,
+		struct b2r2_node_split_buf *dst,
+		u16 h_rsf, u16 v_rsf,
+		const u32 *ivmx, struct b2r2_node **next,
+		struct b2r2_node_split_job *this)
+{
+	int ret;
+
+	struct b2r2_node *last;
+
+	struct b2r2_filter_spec *hf = NULL;
+	struct b2r2_filter_spec *vf = NULL;
+
+	u32 fctl = 0;
+	u32 rsf = 0;
+	u32 rzi = 0;
+	u32 hsrc_init = 0;
+	u32 vsrc_init = 0;
+	u32 hfp = 0;
+	u32 vfp = 0;
+
+	u16 luma_h_rsf = h_rsf;
+	u16 luma_v_rsf = v_rsf;
+
+	struct b2r2_filter_spec *luma_hf = NULL;
+	struct b2r2_filter_spec *luma_vf = NULL;
+
+	u32 luma_fctl = 0;
+	u32 luma_rsf = 0;
+	u32 luma_rzi = 0;
+	u32 luma_hsrc_init = 0;
+	u32 luma_vsrc_init = 0;
+	u32 luma_hfp = 0;
+	u32 luma_vfp = 0;
+
+	s32 src_x;
+	s32 src_y;
+	s32 src_w;
+	s32 src_h;
+
+	bool upsample;
+	bool downsample;
+
+	struct b2r2_blt_rect tmp_win = src->win;
+	bool src_raster = src->type == B2R2_FMT_TYPE_RASTER;
+	bool dst_raster = dst->type == B2R2_FMT_TYPE_RASTER;
+
+	/* Rescale the normalized source window */
+	src_x = inv_rescale(src->win.x - src->rect.x, luma_h_rsf);
+	src_y = inv_rescale(src->win.y - src->rect.y, luma_v_rsf);
+	src_w = inv_rescale(src->win.width, luma_h_rsf);
+	src_h = inv_rescale(src->win.height, luma_v_rsf);
+
+	/* Convert to src coordinate space */
+	src->win.x = (src_x >> 10) + src->rect.x;
+	src->win.y = (src_y >> 10) + src->rect.y;
+
+	/*
+	 * Since the stripe might start and end on a fractional pixel
+	 * we need to count all the touched pixels in the width.
+	 *
+	 * Example:
+	 *   src_x = 1.8, src_w = 2.8
+	 *
+	 *   The stripe touches pixels 1.8 through 4.6, i.e. 4 pixels
+	 */
+	src->win.width = ((src_x & 0x3ff) + src_w + 0x3ff) >> 10;
+	src->win.height = ((src_y & 0x3ff) + src_h + 0x3ff) >> 10;
+
+	luma_hsrc_init = src_x & 0x3ff;
+	luma_vsrc_init = src_y & 0x3ff;
+
+	/* Check for upsampling of chroma */
+	upsample = !src_raster && !b2r2_is_yuv444_fmt(src->fmt);
+	if (upsample) {
+		h_rsf /= 2;
+
+		if (b2r2_is_yuv420_fmt(src->fmt))
+			v_rsf /= 2;
+	}
+
+	/* Check for downsampling of chroma */
+	downsample = !dst_raster && !b2r2_is_yuv444_fmt(dst->fmt);
+	if (downsample) {
+		h_rsf *= 2;
+
+		if (b2r2_is_yuv420_fmt(dst->fmt))
+			v_rsf *= 2;
+	}
+
+	src_x = inv_rescale(tmp_win.x - src->rect.x, h_rsf);
+	src_y = inv_rescale(tmp_win.y - src->rect.y, v_rsf);
+	hsrc_init = src_x & 0x3ff;
+	vsrc_init = src_y & 0x3ff;
+
+	/* Configure resize and filters */
+	fctl = B2R2_FCTL_HF2D_MODE_ENABLE_RESIZER |
+			B2R2_FCTL_VF2D_MODE_ENABLE_RESIZER;
+	luma_fctl = B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_RESIZER |
+			B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_RESIZER;
+
+	rsf =  (h_rsf << B2R2_RSF_HSRC_INC_SHIFT) |
+			(v_rsf << B2R2_RSF_VSRC_INC_SHIFT);
+	luma_rsf = (luma_h_rsf << B2R2_RSF_HSRC_INC_SHIFT) |
+			(luma_v_rsf << B2R2_RSF_VSRC_INC_SHIFT);
+
+	rzi = B2R2_RZI_DEFAULT_HNB_REPEAT |
+			(2 << B2R2_RZI_VNB_REPEAT_SHIFT) |
+			(hsrc_init << B2R2_RZI_HSRC_INIT_SHIFT) |
+			(vsrc_init << B2R2_RZI_VSRC_INIT_SHIFT);
+	luma_rzi = B2R2_RZI_DEFAULT_HNB_REPEAT |
+			(2 << B2R2_RZI_VNB_REPEAT_SHIFT) |
+			(luma_hsrc_init << B2R2_RZI_HSRC_INIT_SHIFT) |
+			(luma_vsrc_init << B2R2_RZI_VSRC_INIT_SHIFT);
+
+	/*
+	 * We should only filter if there is an actual rescale (i.e. not when
+	 * up or downsampling).
+	 */
+	if (luma_h_rsf != (1 << 10)) {
+		hf = b2r2_filter_find(h_rsf);
+		luma_hf = b2r2_filter_find(luma_h_rsf);
+	}
+	if (luma_v_rsf != (1 << 10)) {
+		vf = b2r2_filter_find(v_rsf);
+		luma_vf = b2r2_filter_find(luma_v_rsf);
+	}
+
+	if (hf) {
+		fctl |= B2R2_FCTL_HF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER;
+		hfp = hf->h_coeffs_phys_addr;
+	}
+
+	if (vf) {
+		fctl |= B2R2_FCTL_VF2D_MODE_ENABLE_COLOR_CHANNEL_FILTER;
+		vfp = vf->v_coeffs_phys_addr;
+	}
+
+	if (luma_hf) {
+		luma_fctl |= B2R2_FCTL_LUMA_HF2D_MODE_ENABLE_FILTER;
+		luma_hfp = luma_hf->h_coeffs_phys_addr;
+	}
+
+	if (luma_vf) {
+		luma_fctl |= B2R2_FCTL_LUMA_VF2D_MODE_ENABLE_FILTER;
+		luma_vfp = luma_vf->v_coeffs_phys_addr;
+	}
+
+	ret = configure_copy(cont, node, src, dst, ivmx, &last, this);
+	if (ret < 0)
+		goto error;
+
+	do {
+		bool chroma_rescale =
+				(h_rsf != (1 << 10)) || (v_rsf != (1 << 10));
+		bool luma_rescale =
+				(luma_h_rsf != (1 << 10)) ||
+				(luma_v_rsf != (1 << 10));
+		bool dst_chroma = node->node.GROUP1.B2R2_TTY &
+				B2R2_TTY_CHROMA_NOT_LUMA;
+		bool dst_luma = !dst_chroma;
+
+		if (node == NULL) {
+			b2r2_log_warn(cont->dev, "%s: Internal error! Out "
+					"of nodes!\n", __func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_FILTER_CONTROL;
+
+		/*
+		 * If the source format is anything other than raster, we
+		 * always have to enable both chroma and luma resizers. This
+		 * could be a bug in the hardware, since it is not mentioned in
+		 * the specification.
+		 *
+		 * Otherwise, we will only enable the chroma resizer when
+		 * writing chroma and the luma resizer when writing luma
+		 * (or both when writing raster). Also, if there is no rescale
+		 * to be done there's no point in using the resizers.
+		 */
+
+		if (!src_raster || (chroma_rescale &&
+				(dst_raster || dst_chroma))) {
+			/* Enable chroma resize */
+			node->node.GROUP0.B2R2_INS |=
+					B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_RESIZE_CHROMA;
+			node->node.GROUP8.B2R2_FCTL |= fctl;
+
+			node->node.GROUP9.B2R2_RSF = rsf;
+			node->node.GROUP9.B2R2_RZI = rzi;
+			node->node.GROUP9.B2R2_HFP = hfp;
+			node->node.GROUP9.B2R2_VFP = vfp;
+		}
+
+		if (!src_raster || (luma_rescale &&
+				(dst_raster || dst_luma))) {
+			/* Enable luma resize */
+			node->node.GROUP0.B2R2_INS |=
+					B2R2_INS_RESCALE2D_ENABLED;
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_RESIZE_LUMA;
+			node->node.GROUP8.B2R2_FCTL |= luma_fctl;
+
+			node->node.GROUP10.B2R2_RSF = luma_rsf;
+			node->node.GROUP10.B2R2_RZI = luma_rzi;
+			node->node.GROUP10.B2R2_HFP = luma_hfp;
+			node->node.GROUP10.B2R2_VFP = luma_vfp;
+			/*
+			 * Scaling operation from raster to a multi-buffer
+			 * format, requires the raster input to be scaled
+			 * before luminance information can be extracted.
+			 * Raster input is scaled by the chroma resizer.
+			 * Luma resizer only handles luminance data which
+			 * exists in a separate buffer in source image,
+			 * as is the case with YUV planar/semi-planar formats.
+			 */
+			if (src_raster) {
+				/* Activate chroma scaling */
+				node->node.GROUP0.B2R2_CIC |=
+					B2R2_CIC_RESIZE_CHROMA;
+				node->node.GROUP8.B2R2_FCTL |= fctl;
+				/*
+				 * Color data must be scaled
+				 * to the same size as luma.
+				 * Use luma scaling parameters.
+				 */
+				node->node.GROUP9.B2R2_RSF = luma_rsf;
+				node->node.GROUP9.B2R2_RZI = luma_rzi;
+				node->node.GROUP9.B2R2_HFP = luma_hfp;
+				node->node.GROUP9.B2R2_VFP = luma_vfp;
+			}
+		}
+
+		b2r2_log_info(cont->dev, "%s:\n"
+				"\tB2R2_TXY:  %.8x\tB2R2_TSZ:  %.8x\n"
+				"\tB2R2_S1XY: %.8x\tB2R2_S1SZ: %.8x\n"
+				"\tB2R2_S2XY: %.8x\tB2R2_S2SZ: %.8x\n"
+				"\tB2R2_S3XY: %.8x\tB2R2_S3SZ: %.8x\n"
+				"----------------------------------\n",
+				__func__, node->node.GROUP1.B2R2_TXY,
+				node->node.GROUP1.B2R2_TSZ,
+				node->node.GROUP3.B2R2_SXY,
+				node->node.GROUP3.B2R2_SSZ,
+				node->node.GROUP4.B2R2_SXY,
+				node->node.GROUP4.B2R2_SSZ,
+				node->node.GROUP5.B2R2_SXY,
+				node->node.GROUP5.B2R2_SSZ);
+
+		node = node->next;
+
+	} while (node != last);
+
+
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+}
+
+/**
+ * configure_src() - configures the source registers and the iVMX
+ *
+ * @node - the node to configure
+ * @src  - the source buffer
+ * @ivmx - the iVMX to use for color conversion
+ *
+ * This operation will not consume any nodes
+ */
+static void configure_src(struct b2r2_control *cont,
+		struct b2r2_node *node,
+		struct b2r2_node_split_buf *src, const u32 *ivmx)
+{
+	struct b2r2_node_split_buf tmp_buf;
+
+	b2r2_log_info(cont->dev,
+			"%s: src.win=(%d, %d, %d, %d)\n", __func__,
+			src->win.x, src->win.y, src->win.width,
+			src->win.height);
+
+	/* Configure S1 - S3 */
+	switch (src->type) {
+	case B2R2_FMT_TYPE_RASTER:
+		set_src_2(node, src->addr, src);
+		break;
+	case B2R2_FMT_TYPE_SEMI_PLANAR:
+		memcpy(&tmp_buf, src, sizeof(tmp_buf));
+
+		/*
+		 * For 420 and 422 the chroma has lower resolution than the
+		 * luma
+		 */
+		if (!b2r2_is_yuv444_fmt(src->fmt)) {
+			tmp_buf.win.x >>= 1;
+			tmp_buf.win.width = (tmp_buf.win.width + 1) / 2;
+
+			if (b2r2_is_yuv420_fmt(src->fmt)) {
+				tmp_buf.win.height =
+						(tmp_buf.win.height + 1) / 2;
+				tmp_buf.win.y >>= 1;
+			}
+		}
+
+		set_src_3(node, src->addr, src);
+		set_src_2(node, tmp_buf.chroma_addr, &tmp_buf);
+		break;
+	case B2R2_FMT_TYPE_PLANAR:
+		memcpy(&tmp_buf, src, sizeof(tmp_buf));
+
+		if (!b2r2_is_yuv444_fmt(src->fmt)) {
+			/*
+			 * Each chroma buffer will have half as many values
+			 * per line as the luma buffer
+			 */
+			tmp_buf.pitch = (tmp_buf.pitch + 1) / 2;
+
+			/* Horizontal resolution is half */
+			tmp_buf.win.x >>= 1;
+			tmp_buf.win.width = (tmp_buf.win.width + 1) / 2;
+
+			/*
+			 * If the buffer is in YUV420 format, the vertical
+			 * resolution is half as well
+			 */
+			if (b2r2_is_yuv420_fmt(src->fmt)) {
+				tmp_buf.win.height =
+						(tmp_buf.win.height + 1) / 2;
+				tmp_buf.win.y >>= 1;
+			}
+		}
+
+		set_src_3(node, src->addr, src);                   /* Y */
+		set_src_2(node, tmp_buf.chroma_addr, &tmp_buf);    /* U */
+		set_src_1(node, tmp_buf.chroma_cr_addr, &tmp_buf); /* V */
+
+		break;
+	default:
+		/* Should never, ever happen */
+		BUG_ON(1);
+		break;
+	}
+
+	/* Configure the iVMX for color space conversions */
+	if (ivmx != NULL)
+		set_ivmx(node, ivmx);
+}
+
+/**
+ * configure_bg() - configures a background for the given node
+ *
+ * @node         - the node to configure
+ * @bg           - the background buffer
+ * @swap_fg_bg   - if true, fg will be on s1 instead of s2
+ *
+ * This operation will not consume any nodes.
+ *
+ * NOTE: This method should be called _AFTER_ the destination has been
+ *       configured.
+ *
+ * WARNING: Take care when using this with semi-planar or planar sources since
+ *          either S1 or S2 will be overwritten!
+ */
+static void configure_bg(struct b2r2_control *cont,
+		struct b2r2_node *node,
+		struct b2r2_node_split_buf *bg, bool swap_fg_bg)
+{
+	b2r2_log_info(cont->dev,
+			"%s: bg.win=(%d, %d, %d, %d)\n", __func__,
+			bg->win.x, bg->win.y, bg->win.width,
+			bg->win.height);
+
+	/* Configure S1 */
+	switch (bg->type) {
+	case B2R2_FMT_TYPE_RASTER:
+		if (swap_fg_bg) {
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+			node->node.GROUP0.B2R2_ACK |= B2R2_ACK_SWAP_FG_BG;
+
+			set_src(&node->node.GROUP4, bg->addr, bg);
+		} else {
+			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_1;
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_SOURCE_1_FETCH_FROM_MEM;
+
+			set_src(&node->node.GROUP3, bg->addr, bg);
+		}
+		break;
+	default:
+		/* Should never, ever happen */
+		BUG_ON(1);
+		break;
+	}
+}
+
+/**
+ * configure_dst() - configures the destination registers of the given node
+ *
+ * @node - the node to configure
+ * @ivmx - the iVMX to use for color conversion
+ * @dst  - the destination buffer
+ *
+ * This operation will consume as many nodes as are required to write the
+ * destination format.
+ */
+static int configure_dst(struct b2r2_control *cont, struct b2r2_node *node,
+		struct b2r2_node_split_buf *dst, const u32 *ivmx,
+		struct b2r2_node **next)
+{
+	int ret;
+	int nbr_planes = 1;
+	int i;
+
+	struct b2r2_node_split_buf dst_planes[3];
+
+	b2r2_log_info(cont->dev,
+		"%s: dst.win=(%d, %d, %d, %d)\n", __func__,
+		dst->win.x, dst->win.y, dst->win.width,
+		dst->win.height);
+
+	memcpy(&dst_planes[0], dst, sizeof(dst_planes[0]));
+
+	if (dst->type != B2R2_FMT_TYPE_RASTER) {
+		/* There will be at least 2 planes */
+		nbr_planes = 2;
+
+		memcpy(&dst_planes[1], dst, sizeof(dst_planes[1]));
+
+		dst_planes[1].addr = dst->chroma_addr;
+		dst_planes[1].plane_selection = B2R2_TTY_CHROMA_NOT_LUMA;
+
+		if (!b2r2_is_yuv444_fmt(dst->fmt)) {
+			/* Horizontal resolution is half */
+			dst_planes[1].win.x /= 2;
+			/*
+			 * Must round up the chroma size to handle cases when
+			 * luma size is not divisible by 2. E.g. luma width==7 r
+			 * equires chroma width==4. Chroma width==7/2==3 is only
+			 * enough for luma width==6.
+			 */
+			dst_planes[1].win.width =
+				(dst_planes[1].win.width + 1) / 2;
+
+			/*
+			 * If the buffer is in YUV420 format, the vertical
+			 * resolution is half as well. Height must be rounded in
+			 * the same way as is done for width.
+			 */
+			if (b2r2_is_yuv420_fmt(dst->fmt)) {
+				dst_planes[1].win.y /= 2;
+				dst_planes[1].win.height =
+					(dst_planes[1].win.height + 1) / 2;
+			}
+		}
+
+		if (dst->type == B2R2_FMT_TYPE_PLANAR) {
+			/* There will be a third plane as well */
+			nbr_planes = 3;
+
+			if (!b2r2_is_yuv444_fmt(dst->fmt)) {
+				/* The chroma planes have half the luma pitch */
+				dst_planes[1].pitch /= 2;
+			}
+
+			memcpy(&dst_planes[2], &dst_planes[1],
+				sizeof(dst_planes[2]));
+			dst_planes[2].addr = dst->chroma_cr_addr;
+
+			/*
+			 * The third plane will be Cr.
+			 * The flag B2R2_TTY_CB_NOT_CR actually works
+			 * the other way around, i.e. as if it was
+			 * B2R2_TTY_CR_NOT_CB.
+			 */
+			dst_planes[2].chroma_selection = B2R2_TTY_CB_NOT_CR;
+		}
+
+	}
+
+	/* Configure one node for each plane */
+	for (i = 0; i < nbr_planes; i++) {
+
+		if (node == NULL) {
+			b2r2_log_warn(cont->dev, "%s: "
+				"Internal error! Out of nodes!\n", __func__);
+			ret = -ENOMEM;
+			goto error;
+		}
+
+		/*
+		 * When writing chroma, there's no need to read the luma and
+		 * vice versa.
+		 */
+		if ((node->node.GROUP3.B2R2_STY & B2R2_NATIVE_YUV) &&
+				(nbr_planes > 1)) {
+			if (i != 0) {
+				node->node.GROUP4.B2R2_STY |=
+					B2R2_S3TY_ENABLE_BLANK_ACCESS;
+			}
+			if (i != 1) {
+				node->node.GROUP0.B2R2_INS &=
+					~B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+				node->node.GROUP0.B2R2_INS |=
+					B2R2_INS_SOURCE_2_COLOR_FILL_REGISTER;
+			}
+			if (i != 2) {
+				node->node.GROUP0.B2R2_INS &=
+					~B2R2_INS_SOURCE_1_FETCH_FROM_MEM;
+				node->node.GROUP0.B2R2_INS |=
+					B2R2_INS_SOURCE_1_COLOR_FILL_REGISTER;
+			}
+		} else if ((node->node.GROUP3.B2R2_STY &
+					(B2R2_NATIVE_YCBCR42X_MBN |
+						B2R2_NATIVE_YCBCR42X_R2B)) &&
+				(nbr_planes > 1)) {
+			if (i != 0) {
+				node->node.GROUP4.B2R2_STY |=
+					B2R2_S3TY_ENABLE_BLANK_ACCESS;
+			}
+		}
+
+		set_target(node, dst_planes[i].addr, &dst_planes[i]);
+
+		node = node->next;
+	}
+
+	/* Consume the nodes */
+	*next = node;
+
+	return 0;
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+
+}
+
+/**
+ * configure_blend() - configures the given node for alpha blending
+ *
+ * @node         - the node to configure
+ * @flags        - the flags passed in the blt_request
+ * @global_alpha - the global alpha to use (if enabled in flags)
+ *
+ * This operation will not consume any nodes.
+ *
+ * NOTE: This method should be called _AFTER_ the destination has been
+ *       configured.
+ *
+ * WARNING: Take care when using this with semi-planar or planar sources since
+ *          either S1 or S2 will be overwritten!
+ */
+static void configure_blend(struct b2r2_control *cont,
+		struct b2r2_node *node, u32 flags, u32 global_alpha)
+{
+	node->node.GROUP0.B2R2_ACK &= ~(B2R2_ACK_MODE_BYPASS_S2_S3);
+
+	/* Check if the foreground is premultiplied */
+	if ((flags & B2R2_BLT_FLAG_SRC_IS_NOT_PREMULT) != 0)
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BLEND_NOT_PREMULT;
+	else
+		node->node.GROUP0.B2R2_ACK |= B2R2_ACK_MODE_BLEND_PREMULT;
+
+	/* Check if global alpha blend should be enabled */
+	if (flags & B2R2_BLT_FLAG_GLOBAL_ALPHA_BLEND) {
+
+		/* B2R2 expects the global alpha to be in 0...128 range */
+		global_alpha = (global_alpha*128)/255;
+
+		node->node.GROUP0.B2R2_ACK |=
+			global_alpha <<	B2R2_ACK_GALPHA_ROPID_SHIFT;
+	} else {
+		node->node.GROUP0.B2R2_ACK |=
+			(128 << B2R2_ACK_GALPHA_ROPID_SHIFT);
+	}
+}
+
+/**
+ * configure_clip() - configures destination clipping for the given node
+ *
+ *   @node      - the node to configure
+ *   @clip_rect - the clip rectangle
+ *
+ *  This operation does not consume any nodes.
+ */
+static void configure_clip(struct b2r2_control *cont, struct b2r2_node *node,
+		struct b2r2_blt_rect *clip_rect)
+{
+	s32 l = clip_rect->x;
+	s32 r = clip_rect->x + clip_rect->width - 1;
+	s32 t = clip_rect->y;
+	s32 b = clip_rect->y + clip_rect->height - 1;
+
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_CLIP_WINDOW;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_RECT_CLIP_ENABLED;
+
+	/* Clip window setup */
+	node->node.GROUP6.B2R2_CWO =
+			((t & 0x7FFF) << B2R2_CWO_Y_SHIFT) |
+			((l & 0x7FFF) << B2R2_CWO_X_SHIFT);
+	node->node.GROUP6.B2R2_CWS =
+			((b & 0x7FFF) << B2R2_CWO_Y_SHIFT) |
+			((r & 0x7FFF) << B2R2_CWO_X_SHIFT);
+}
+
+/**
+ * set_buf() - configures the given buffer with the provided values
+ *
+ * @addr       - the physical base address
+ * @img        - the blt image to base the buffer on
+ * @rect       - the rectangle to use
+ * @color_fill - determines whether the buffer should be used for color fill
+ * @color      - the color to use in case of color fill
+ */
+static void set_buf(struct b2r2_control *cont,
+		struct b2r2_node_split_buf *buf,
+		u32 addr,
+		const struct b2r2_blt_img *img,
+		const struct b2r2_blt_rect *rect,
+		bool color_fill,
+		u32 color)
+{
+	memset(buf, 0, sizeof(*buf));
+
+	buf->fmt = img->fmt;
+	buf->type = b2r2_get_fmt_type(img->fmt);
+
+	if (color_fill) {
+		buf->type = B2R2_FMT_TYPE_RASTER;
+		buf->color = color;
+	} else {
+		buf->addr = addr;
+
+		buf->alpha_range = b2r2_get_alpha_range(img->fmt);
+
+		if (img->pitch == 0)
+			buf->pitch = b2r2_fmt_byte_pitch(img->fmt, img->width);
+		else
+			buf->pitch = img->pitch;
+
+		buf->height = img->height;
+		buf->width = img->width;
+
+		switch (buf->type) {
+		case B2R2_FMT_TYPE_SEMI_PLANAR:
+			buf->chroma_addr = (u32)(((u8 *)addr) +
+					buf->pitch * buf->height);
+			break;
+		case B2R2_FMT_TYPE_PLANAR:
+			if (b2r2_is_yuv422_fmt(buf->fmt) ||
+					b2r2_is_yuv420_fmt(buf->fmt)) {
+				buf->chroma_addr = (u32)(((u8 *)addr) +
+					buf->pitch * buf->height);
+			} else {
+				buf->chroma_cr_addr = (u32)(((u8 *)addr) +
+					buf->pitch * buf->height);
+			}
+			if (b2r2_is_yuv420_fmt(buf->fmt)) {
+				/*
+				 * Use ceil(height/2) in case
+				 * buffer height is not divisible by 2.
+				 */
+				buf->chroma_cr_addr =
+					(u32)(((u8 *)buf->chroma_addr) +
+					(buf->pitch >> 1) *
+					((buf->height + 1) >> 1));
+			} else if (b2r2_is_yuv422_fmt(buf->fmt)) {
+				buf->chroma_cr_addr =
+					(u32)(((u8 *)buf->chroma_addr) +
+					(buf->pitch >> 1) * buf->height);
+			} else if (b2r2_is_yvu420_fmt(buf->fmt)) {
+				buf->chroma_addr =
+					(u32)(((u8 *)buf->chroma_cr_addr) +
+					(buf->pitch >> 1) *
+					((buf->height + 1) >> 1));
+			} else if (b2r2_is_yvu422_fmt(buf->fmt)) {
+				buf->chroma_addr =
+					(u32)(((u8 *)buf->chroma_cr_addr) +
+					(buf->pitch >> 1) * buf->height);
+			}
+			break;
+		default:
+			break;
+		}
+
+		memcpy(&buf->rect, rect, sizeof(buf->rect));
+	}
+}
+
+/**
+ * setup_tmp_buf() - configure a temporary buffer
+ */
+static int setup_tmp_buf(struct b2r2_control *cont,
+		struct b2r2_node_split_buf *tmp,
+		u32 max_size,
+		enum b2r2_blt_fmt pref_fmt,
+		u32 pref_width,
+		u32 pref_height)
+{
+	int ret;
+
+	enum b2r2_blt_fmt fmt;
+
+	u32 width;
+	u32 height;
+	u32 pitch;
+	u32 size;
+
+	/* Determine what format we should use for the tmp buf */
+	if (b2r2_is_rgb_fmt(pref_fmt)) {
+		fmt = B2R2_BLT_FMT_32_BIT_ARGB8888;
+	} else if (b2r2_is_bgr_fmt(pref_fmt)) {
+		fmt = B2R2_BLT_FMT_32_BIT_ABGR8888;
+	} else if (b2r2_is_yvu_fmt(pref_fmt)) {
+		fmt = B2R2_BLT_FMT_CB_Y_CR_Y;
+	} else if (b2r2_is_yuv_fmt(pref_fmt)) {
+		fmt = B2R2_BLT_FMT_32_BIT_AYUV8888;
+	} else {
+		/* Wait, what? */
+		b2r2_log_warn(cont->dev, "%s: "
+			"Cannot create tmp buf from this fmt (%d)\n",
+			__func__, pref_fmt);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* See if we can fit the entire preferred rectangle */
+	width = pref_width;
+	height = pref_height;
+	pitch = b2r2_fmt_byte_pitch(fmt, width);
+	size = pitch * height;
+
+	if (size > max_size) {
+		/* We need to limit the size, so we choose a different width */
+		width = min(width, (u32) B2R2_RESCALE_MAX_WIDTH);
+		pitch = b2r2_fmt_byte_pitch(fmt, width);
+		height = min(height, max_size / pitch);
+		size = pitch * height;
+	}
+
+	/* We should at least have enough room for one scanline */
+	if (height == 0) {
+		b2r2_log_warn(cont->dev, "%s: Not enough tmp mem!\n",
+			__func__);
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	memset(tmp, 0, sizeof(*tmp));
+
+	tmp->fmt = fmt;
+	tmp->type = B2R2_FMT_TYPE_RASTER;
+	tmp->height = height;
+	tmp->width = width;
+	tmp->pitch = pitch;
+
+	tmp->rect.width = width;
+	tmp->rect.height = tmp->height;
+	tmp->alpha_range = B2R2_TY_ALPHA_RANGE_255;
+
+	return 0;
+error:
+	b2r2_log_warn(cont->dev, "%s: Exit...\n", __func__);
+	return ret;
+
+}
+
+/**
+ * is_transform() - returns whether the given request is a transform operation
+ */
+static bool is_transform(const struct b2r2_blt_request *req)
+{
+	return (req->user_req.transform != B2R2_BLT_TRANSFORM_NONE) ||
+			(req->user_req.src_rect.width !=
+				req->user_req.dst_rect.width) ||
+			(req->user_req.src_rect.height !=
+				req->user_req.dst_rect.height);
+}
+
+/**
+ * rescale() - rescales the given dimension
+ *
+ * Returns the rescaled dimension in 22.10 fixed point format.
+ */
+static s32 rescale(struct b2r2_control *cont, s32 dim, u16 sf)
+{
+	b2r2_log_info(cont->dev, "%s\n", __func__);
+
+	if (sf == 0) {
+		b2r2_log_err(cont->dev, "%s: Scale factor is 0!\n", __func__);
+		BUG_ON(1);
+	}
+
+	/*
+	 * This is normally not safe to do, since it drastically decreases the
+	 * precision of the integer part of the dimension. But since the B2R2
+	 * hardware only has 12-bit registers for these values, we are safe.
+	 */
+	return (dim << 20) / sf;
+}
+
+/**
+ * inv_rescale() - does an inverted rescale of the given dimension
+ *
+ * Returns the rescaled dimension in 22.10 fixed point format.
+ */
+static s32 inv_rescale(s32 dim, u16 sf)
+{
+	if (sf == 0)
+		return dim;
+
+	return dim * sf;
+}
+
+/**
+ * set_target() - sets the target registers of the given node
+ */
+static void set_target(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	s32 l;
+	s32 r;
+	s32 t;
+	s32 b;
+
+	if (buf->tmp_buf_index)
+		node->dst_tmp_index = buf->tmp_buf_index;
+
+	node->node.GROUP1.B2R2_TBA = addr;
+	node->node.GROUP1.B2R2_TTY = buf->pitch | b2r2_to_native_fmt(buf->fmt) |
+			buf->alpha_range | buf->chroma_selection | buf->hso |
+			buf->vso | buf->dither | buf->plane_selection;
+
+	if (buf->fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+			buf->fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+		node->node.GROUP1.B2R2_TTY |= B2R2_TY_ENDIAN_BIG_NOT_LITTLE;
+
+	node->node.GROUP1.B2R2_TSZ =
+			((buf->win.width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((buf->win.height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+	node->node.GROUP1.B2R2_TXY =
+			((buf->win.x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((buf->win.y & 0xffff) << B2R2_XY_Y_SHIFT);
+
+	/* Check if the rectangle is outside the buffer */
+	if (buf->vso == B2R2_TY_VSO_BOTTOM_TO_TOP)
+		t = buf->win.y - (buf->win.height - 1);
+	else
+		t = buf->win.y;
+
+	if (buf->hso == B2R2_TY_HSO_RIGHT_TO_LEFT)
+		l = buf->win.x - (buf->win.width - 1);
+	else
+		l = buf->win.x;
+
+	r = l + buf->win.width;
+	b = t + buf->win.height;
+
+	/* Clip to the destination buffer to prevent memory overwrites */
+	if ((l < 0) || (r > buf->width) || (t < 0) || (b > buf->height)) {
+		/* The clip rectangle is including the borders */
+		l = max(l, 0);
+		r = min(r, (s32) buf->width) - 1;
+		t = max(t, 0);
+		b = min(b, (s32) buf->height) - 1;
+
+		node->node.GROUP0.B2R2_CIC |= B2R2_CIC_CLIP_WINDOW;
+		node->node.GROUP0.B2R2_INS |= B2R2_INS_RECT_CLIP_ENABLED;
+		node->node.GROUP6.B2R2_CWO =
+			((l & 0x7FFF) << B2R2_CWS_X_SHIFT) |
+			((t & 0x7FFF) << B2R2_CWS_Y_SHIFT);
+		node->node.GROUP6.B2R2_CWS =
+			((r & 0x7FFF) << B2R2_CWO_X_SHIFT) |
+			((b & 0x7FFF) << B2R2_CWO_Y_SHIFT);
+	}
+
+}
+
+/**
+ * set_src() - configures the given source register with the given values
+ */
+static void set_src(struct b2r2_src_config *src, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	src->B2R2_SBA = addr;
+	src->B2R2_STY = buf->pitch | b2r2_to_native_fmt(buf->fmt) |
+			buf->alpha_range | buf->hso | buf->vso;
+
+	if (buf->fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+			buf->fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+		src->B2R2_STY |= B2R2_TY_ENDIAN_BIG_NOT_LITTLE;
+
+	src->B2R2_SSZ = ((buf->win.width & 0xfff) << B2R2_SZ_WIDTH_SHIFT) |
+			((buf->win.height & 0xfff) << B2R2_SZ_HEIGHT_SHIFT);
+	src->B2R2_SXY = ((buf->win.x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((buf->win.y & 0xffff) << B2R2_XY_Y_SHIFT);
+
+}
+
+/**
+ * set_src_1() - sets the source 1 registers of the given node
+ */
+static void set_src_1(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	if (buf->tmp_buf_index)
+		node->src_tmp_index = buf->tmp_buf_index;
+
+	node->src_index = 1;
+
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_1;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_1_FETCH_FROM_MEM;
+
+	node->node.GROUP3.B2R2_SBA = addr;
+	node->node.GROUP3.B2R2_STY = buf->pitch | b2r2_to_native_fmt(buf->fmt) |
+			buf->alpha_range | buf->hso | buf->vso;
+
+	if (buf->fmt == B2R2_BLT_FMT_24_BIT_VUY888 ||
+			buf->fmt == B2R2_BLT_FMT_32_BIT_VUYA8888)
+		node->node.GROUP3.B2R2_STY |= B2R2_TY_ENDIAN_BIG_NOT_LITTLE;
+
+	node->node.GROUP3.B2R2_SXY =
+			((buf->win.x & 0xffff) << B2R2_XY_X_SHIFT) |
+			((buf->win.y & 0xffff) << B2R2_XY_Y_SHIFT);
+
+	/* Source 1 has no size register */
+}
+
+/**
+ * set_src_2() - sets the source 2 registers of the given node
+ */
+static void set_src_2(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	if (buf->tmp_buf_index)
+		node->src_tmp_index = buf->tmp_buf_index;
+
+	node->src_index = 2;
+
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+
+	set_src(&node->node.GROUP4, addr, buf);
+}
+
+/**
+ * set_src_3() - sets the source 3 registers of the given node
+ */
+static void set_src_3(struct b2r2_node *node, u32 addr,
+		struct b2r2_node_split_buf *buf)
+{
+	if (buf->tmp_buf_index)
+		node->src_tmp_index = buf->tmp_buf_index;
+
+	node->src_index = 3;
+
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_3;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_3_FETCH_FROM_MEM;
+
+	set_src(&node->node.GROUP5, addr, buf);
+}
+
+/**
+ * set_ivmx() - configures the iVMX registers with the given values
+ */
+static void set_ivmx(struct b2r2_node *node, const u32 *vmx_values)
+{
+	node->node.GROUP0.B2R2_CIC |= B2R2_CIC_IVMX;
+	node->node.GROUP0.B2R2_INS |= B2R2_INS_IVMX_ENABLED;
+
+	node->node.GROUP15.B2R2_VMX0 = vmx_values[0];
+	node->node.GROUP15.B2R2_VMX1 = vmx_values[1];
+	node->node.GROUP15.B2R2_VMX2 = vmx_values[2];
+	node->node.GROUP15.B2R2_VMX3 = vmx_values[3];
+}
+
+/**
+ * reset_nodes() - clears the node list
+ */
+static void reset_nodes(struct b2r2_node *node)
+{
+	while (node != NULL) {
+		memset(&node->node, 0, sizeof(node->node));
+
+		node->src_tmp_index = 0;
+		node->dst_tmp_index = 0;
+
+		/* TODO: Implement support for short linked lists */
+		node->node.GROUP0.B2R2_CIC = 0x7ffff;
+
+		if (node->next != NULL)
+			node->node.GROUP0.B2R2_NIP =
+					node->next->physical_address;
+		node = node->next;
+	}
+}
+
+int b2r2_node_split_init(struct b2r2_control *cont)
+{
+	return 0;
+}
+
+void b2r2_node_split_exit(struct b2r2_control *cont)
+{
+
+}
diff --git a/drivers/video/b2r2/b2r2_node_split.h b/drivers/video/b2r2/b2r2_node_split.h
new file mode 100644
index 00000000000..a577241c31b
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_node_split.h
@@ -0,0 +1,124 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 node splitter
+ *
+ * Author: Fredrik Allansson <fredrik.allansson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __B2R2_NODE_SPLIT_H_
+#define __B2R2_NODE_SPLIT_H_
+
+#include "b2r2_internal.h"
+#include "b2r2_hw.h"
+
+/**
+ * b2r2_node_split_analyze() - Analyzes a B2R2 request
+ *
+ * @req          - The request to analyze
+ * @max_buf_size - The largest size allowed for intermediate buffers
+ * @node_count   - Number of nodes required for the job
+ * @buf_count    - Number of intermediate buffers required for the job
+ * @bufs         - An array of buffers needed for intermediate buffers
+ *
+ * Analyzes the request and determines how many nodes and intermediate buffers
+ * are required.
+ *
+ * It is the responsibility of the caller to allocate memory and assign the
+ * physical addresses. After that b2r2_node_split_assign_buffers should be
+ * called to assign the buffers to the right nodes.
+ *
+ * Returns:
+ *   A handle identifing the analyzed request if successful, a negative
+ *   value otherwise.
+ */
+int b2r2_node_split_analyze(const struct b2r2_blt_request *req, u32 max_buf_size,
+		u32 *node_count, struct b2r2_work_buf **bufs, u32* buf_count,
+		struct b2r2_node_split_job *job);
+
+/**
+ * b2r2_node_split_configure() - Performs a node split
+ *
+ * @handle - A handle for the analyzed request
+ * @first  - The first node in the list of nodes to use
+ *
+ * Fills the supplied list of nodes with the parameters acquired by analyzing
+ * the request.
+ *
+ * All pointers to intermediate buffers are represented by integers to be used
+ * in the array returned by b2r2_node_split_analyze.
+ *
+ * Returns:
+ *   A negative value if an error occurred, 0 otherwise.
+ */
+int b2r2_node_split_configure(struct b2r2_control *cont,
+		struct b2r2_node_split_job *job, struct b2r2_node *first);
+
+/**
+ * b2r2_node_split_assign_buffers() - Assignes physical addresses
+ *
+ * @handle    - The handle for the job
+ * @first     - The first node in the node list
+ * @bufs      - Buffers with assigned physical addresses
+ * @buf_count - Number of physical addresses
+ *
+ * Assigns the physical addresses where intermediate buffers are required in
+ * the node list.
+ *
+ * The order of the elements of 'bufs' must be maintained from the call to
+ * b2r2_node_split_analyze.
+ *
+ * Returns:
+ *   A negative value if an error occurred, 0 otherwise.
+ */
+int b2r2_node_split_assign_buffers(struct b2r2_control *cont,
+		struct b2r2_node_split_job *job,
+		struct b2r2_node *first, struct b2r2_work_buf *bufs,
+		u32 buf_count);
+
+/**
+ * b2r2_node_split_unassign_buffers() - Removes all physical addresses
+ *
+ * @handle - The handle associated with the job
+ * @first  - The first node in the node list
+ *
+ * Removes all references to intermediate buffers from the node list.
+ *
+ * This makes it possible to reuse the node list with new buffers by calling
+ * b2r2_node_split_assign_buffers again. Useful for caching node lists.
+ */
+void b2r2_node_split_unassign_buffers(struct b2r2_control *cont,
+		struct b2r2_node_split_job *job,
+		struct b2r2_node *first);
+
+/**
+ * b2r2_node_split_release() - Releases all resources for a job
+ *
+ * @handle - The handle identifying the job. This will be set to 0.
+ *
+ * Releases all resources associated with a job.
+ *
+ * This should always be called once b2r2_node_split_analyze has been called
+ * in order to release any resources allocated while analyzing.
+ */
+void b2r2_node_split_cancel(struct b2r2_control *cont,
+		struct b2r2_node_split_job *job);
+
+/**
+ * b2r2_node_split_init() - Initializes the node split module
+ *
+ * Initializes the node split module and creates debugfs files.
+ */
+int b2r2_node_split_init(struct b2r2_control *cont);
+
+/**
+ * b2r2_node_split_exit() - Deinitializes the node split module
+ *
+ * Releases all resources for the node split module.
+ */
+void b2r2_node_split_exit(struct b2r2_control *cont);
+
+#endif
diff --git a/drivers/video/b2r2/b2r2_profiler/Makefile b/drivers/video/b2r2/b2r2_profiler/Makefile
new file mode 100644
index 00000000000..69a85524fd7
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler/Makefile
@@ -0,0 +1,3 @@
+# Make file for loadable module B2R2 Profiler
+
+obj-$(CONFIG_B2R2_PROFILER) += b2r2_profiler.o
diff --git a/drivers/video/b2r2/b2r2_profiler/b2r2_profiler.c b/drivers/video/b2r2/b2r2_profiler/b2r2_profiler.c
new file mode 100644
index 00000000000..e038941b4e8
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler/b2r2_profiler.c
@@ -0,0 +1,270 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson B2R2 profiler implementation
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/jiffies.h>
+
+#include <video/b2r2_blt.h>
+#include "../b2r2_profiler_api.h"
+
+
+#define S32_MAX 2147483647
+
+
+static int src_format_filter_on = false;
+module_param(src_format_filter_on, bool, S_IRUGO | S_IWUSR);
+static unsigned int src_format_filter;
+module_param(src_format_filter, uint, S_IRUGO | S_IWUSR);
+
+static int print_blts_on = 0;
+module_param(print_blts_on, bool, S_IRUGO | S_IWUSR);
+static int use_mpix_per_second_in_print_blts = 1;
+module_param(use_mpix_per_second_in_print_blts, bool, S_IRUGO | S_IWUSR);
+
+static int profiler_stats_on = 1;
+module_param(profiler_stats_on, bool, S_IRUGO | S_IWUSR);
+
+static const unsigned int profiler_stats_blts_used = 400;
+static struct {
+	unsigned long sampling_start_time_jiffies;
+
+	s32 min_mpix_per_second;
+	struct b2r2_blt_req min_blt_request;
+	struct b2r2_blt_profiling_info min_blt_profiling_info;
+
+	s32 max_mpix_per_second;
+	struct b2r2_blt_req max_blt_request;
+	struct b2r2_blt_profiling_info max_blt_profiling_info;
+
+	s32 accumulated_num_pixels;
+	s32 accumulated_num_usecs;
+
+	u32 num_blts_done;
+} profiler_stats;
+
+
+static s32 nsec_2_usec(const s32 nsec);
+
+static int is_scale_blt(const struct b2r2_blt_req * const request);
+static s32 get_blt_mpix_per_second(const struct b2r2_blt_req * const request,
+	const struct b2r2_blt_profiling_info * const blt_profiling_info);
+static void print_blt(const struct b2r2_blt_req * const request,
+	const struct b2r2_blt_profiling_info * const blt_profiling_info);
+
+static s32 get_num_pixels_in_blt(const struct b2r2_blt_req * const request);
+static s32 get_mpix_per_second(const s32 num_pixels, const s32 num_usecs);
+static void print_profiler_stats(void);
+static void reset_profiler_stats(void);
+static void do_profiler_stats(const struct b2r2_blt_req * const request,
+	const struct b2r2_blt_profiling_info * const blt_profiling_info);
+
+static void blt_done(const struct b2r2_blt_req * const blt,
+	const s32 request_id,
+	const struct b2r2_blt_profiling_info * const blt_profiling_info);
+
+
+static struct b2r2_profiler this = {
+	.blt_done = blt_done,
+};
+
+
+static s32 nsec_2_usec(const s32 nsec)
+{
+	return nsec / 1000;
+}
+
+
+static int is_scale_blt(const struct b2r2_blt_req * const request)
+{
+	if ((request->transform & B2R2_BLT_TRANSFORM_CCW_ROT_90 &&
+			(request->src_rect.width !=
+				request->dst_rect.height ||
+			request->src_rect.height !=
+				request->dst_rect.width)) ||
+		(!(request->transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) &&
+			(request->src_rect.width !=
+				request->dst_rect.width ||
+			request->src_rect.height !=
+				request->dst_rect.height)))
+		return 1;
+	else
+		return 0;
+}
+
+static s32 get_blt_mpix_per_second(const struct b2r2_blt_req * const request,
+	const struct b2r2_blt_profiling_info * const blt_profiling_info)
+{
+	return get_mpix_per_second(get_num_pixels_in_blt(request),
+			nsec_2_usec(blt_profiling_info->nsec_active_in_cpu +
+			blt_profiling_info->nsec_active_in_b2r2));
+}
+
+static void print_blt(const struct b2r2_blt_req * const request,
+	const struct b2r2_blt_profiling_info * const blt_profiling_info)
+{
+	char tmp_str[128];
+	sprintf(tmp_str, "SF: %#10x, DF: %#10x, F: %#10x, T: %#3x, S: %1i, P: %7i",
+		request->src_img.fmt,
+		request->dst_img.fmt,
+		request->flags,
+		request->transform,
+		is_scale_blt(request),
+		get_num_pixels_in_blt(request));
+	if (use_mpix_per_second_in_print_blts)
+		printk(KERN_ALERT "%s, MPix/s: %3i\n", tmp_str,
+			get_blt_mpix_per_second(request, blt_profiling_info));
+	else
+		printk(KERN_ALERT "%s, CPU: %10i, B2R2: %10i, Tot: %10i ns\n",
+			tmp_str, blt_profiling_info->nsec_active_in_cpu,
+			blt_profiling_info->nsec_active_in_b2r2,
+			blt_profiling_info->total_time_nsec);
+}
+
+
+static s32 get_num_pixels_in_blt(const struct b2r2_blt_req * const request)
+{
+	s32 num_pixels_in_src = request->src_rect.width * request->src_rect.height;
+	s32 num_pixels_in_dst = request->dst_rect.width * request->dst_rect.height;
+	if (request->flags & (B2R2_BLT_FLAG_SOURCE_FILL |
+				B2R2_BLT_FLAG_SOURCE_FILL_RAW))
+		return num_pixels_in_dst;
+	else
+		return (num_pixels_in_src + num_pixels_in_dst) / 2;
+}
+
+static s32 get_mpix_per_second(const s32 num_pixels, const s32 num_usecs)
+{
+	s32 num_pixels_scale_factor = num_pixels != 0 ?
+		S32_MAX / num_pixels : S32_MAX;
+	s32 num_usecs_scale_factor = num_usecs != 0 ?
+		S32_MAX / num_usecs : S32_MAX;
+	s32 scale_factor = min(num_pixels_scale_factor, num_usecs_scale_factor);
+
+	s32 num_pixels_scaled = num_pixels * scale_factor;
+	s32 num_usecs_scaled = num_usecs * scale_factor;
+
+	if (num_usecs_scaled < 1000000)
+		return 0;
+
+	return (num_pixels_scaled / 1000000) / (num_usecs_scaled / 1000000);
+}
+
+static void print_profiler_stats(void)
+{
+	printk(KERN_ALERT "Min: %3i, Avg: %3i, Max: %3i MPix/s\n",
+		profiler_stats.min_mpix_per_second,
+		get_mpix_per_second(
+			profiler_stats.accumulated_num_pixels,
+			profiler_stats.accumulated_num_usecs),
+			profiler_stats.max_mpix_per_second);
+	printk(KERN_ALERT "Min blit:\n");
+	print_blt(&profiler_stats.min_blt_request,
+		&profiler_stats.min_blt_profiling_info);
+	printk(KERN_ALERT "Max blit:\n");
+	print_blt(&profiler_stats.max_blt_request,
+		&profiler_stats.max_blt_profiling_info);
+}
+
+static void reset_profiler_stats(void)
+{
+	profiler_stats.sampling_start_time_jiffies = jiffies;
+	profiler_stats.min_mpix_per_second = S32_MAX;
+	profiler_stats.max_mpix_per_second = 0;
+	profiler_stats.accumulated_num_pixels = 0;
+	profiler_stats.accumulated_num_usecs = 0;
+	profiler_stats.num_blts_done = 0;
+}
+
+static void do_profiler_stats(const struct b2r2_blt_req * const request,
+	const struct b2r2_blt_profiling_info * const blt_profiling_info)
+{
+	s32 num_pixels_in_blt;
+	s32 num_usec_blt_took;
+	s32 blt_mpix_per_second;
+
+	if (time_before(jiffies, profiler_stats.sampling_start_time_jiffies))
+		return;
+
+	num_pixels_in_blt = get_num_pixels_in_blt(request);
+	num_usec_blt_took = nsec_2_usec(blt_profiling_info->nsec_active_in_cpu +
+		blt_profiling_info->nsec_active_in_b2r2);
+	blt_mpix_per_second = get_mpix_per_second(num_pixels_in_blt,
+		num_usec_blt_took);
+
+	if (blt_mpix_per_second <=
+			profiler_stats.min_mpix_per_second) {
+		profiler_stats.min_mpix_per_second = blt_mpix_per_second;
+		memcpy(&profiler_stats.min_blt_request,
+			request, sizeof(struct b2r2_blt_req));
+		memcpy(&profiler_stats.min_blt_profiling_info,
+			blt_profiling_info,
+			sizeof(struct b2r2_blt_profiling_info));
+	}
+
+	if (blt_mpix_per_second >= profiler_stats.max_mpix_per_second) {
+		profiler_stats.max_mpix_per_second = blt_mpix_per_second;
+		memcpy(&profiler_stats.max_blt_request, request,
+			sizeof(struct b2r2_blt_req));
+		memcpy(&profiler_stats.max_blt_profiling_info,
+			blt_profiling_info, sizeof(struct b2r2_blt_profiling_info));
+	}
+
+	profiler_stats.accumulated_num_pixels += num_pixels_in_blt;
+	profiler_stats.accumulated_num_usecs += num_usec_blt_took;
+	profiler_stats.num_blts_done++;
+
+	if (profiler_stats.num_blts_done >= profiler_stats_blts_used) {
+		print_profiler_stats();
+		reset_profiler_stats();
+		/* The printouts initiated above can disturb the next measurement
+		so we delay it two seconds to give the printouts a chance to finish. */
+		profiler_stats.sampling_start_time_jiffies = jiffies + (2 * HZ);
+	}
+}
+
+static void blt_done(const struct b2r2_blt_req * const request,
+		const s32 request_id,
+		const struct b2r2_blt_profiling_info * const blt_profiling_info)
+{
+	/* Filters */
+	if (src_format_filter_on && request->src_img.fmt != src_format_filter)
+		return;
+
+	/* Processors */
+	if (print_blts_on)
+		print_blt(request, blt_profiling_info);
+
+	if (profiler_stats_on)
+		do_profiler_stats(request, blt_profiling_info);
+}
+
+
+static int __init b2r2_profiler_init(void)
+{
+	reset_profiler_stats();
+
+	return b2r2_register_profiler(&this);
+}
+module_init(b2r2_profiler_init);
+
+static void __exit b2r2_profiler_exit(void)
+{
+	b2r2_unregister_profiler(&this);
+}
+module_exit(b2r2_profiler_exit);
+
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Johan Mossberg (johan.xx.mossberg@stericsson.com)");
+MODULE_DESCRIPTION("B2R2 Profiler");
diff --git a/drivers/video/b2r2/b2r2_profiler_api.h b/drivers/video/b2r2/b2r2_profiler_api.h
new file mode 100644
index 00000000000..5f1f9abbe49
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler_api.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 profiling API
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#ifndef _LINUX_VIDEO_B2R2_PROFILER_API_H
+#define _LINUX_VIDEO_B2R2_PROFILER_API_H
+
+#include <video/b2r2_blt.h>
+
+/**
+ * struct b2r2_blt_profiling_info - Profiling information for a blit
+ *
+ * @nsec_active_in_cpu: The number of nanoseconds the job was active in the CPU.
+ *                      This is an approximate value, check out the code for more
+ *                      info.
+ * @nsec_active_in_b2r2: The number of nanoseconds the job was active in B2R2. This
+ *                       is an approximate value, check out the code for more info.
+ * @total_time_nsec: The total time the job took in nano seconds. Includes ideling.
+ */
+struct b2r2_blt_profiling_info {
+	s32 nsec_active_in_cpu;
+	s32 nsec_active_in_b2r2;
+	s32 total_time_nsec;
+};
+
+/**
+ * struct b2r2_profiler - B2R2 profiler.
+ *
+ * The callbacks are never run concurrently. No heavy stuff must be done in the
+ * callbacks as this might adversely affect the B2R2 driver. The callbacks must
+ * not call the B2R2 profiler API as this will cause a deadlock. If the callbacks
+ * call into the B2R2 driver care must be taken as deadlock situations can arise.
+ *
+ * @blt_done: Called when a blit has finished, timed out or been canceled.
+ */
+struct b2r2_profiler {
+	void (*blt_done)(const struct b2r2_blt_req * const request, const s32 request_id, const struct b2r2_blt_profiling_info * const blt_profiling_info);
+};
+
+/**
+ * b2r2_register_profiler() - Registers a profiler.
+ *
+ * Currently only one profiler can be registered at any given time.
+ *
+ * @profiler: The profiler
+ *
+ * Returns 0 on success, negative error code on failure
+ */
+int b2r2_register_profiler(const struct b2r2_profiler * const profiler);
+
+/**
+ * b2r2_unregister_profiler() - Unregisters a profiler.
+ *
+ * @profiler: The profiler
+ */
+void b2r2_unregister_profiler(const struct b2r2_profiler * const profiler);
+
+#endif /* #ifdef _LINUX_VIDEO_B2R2_PROFILER_API_H */
diff --git a/drivers/video/b2r2/b2r2_profiler_socket.c b/drivers/video/b2r2/b2r2_profiler_socket.c
new file mode 100644
index 00000000000..cb95af9380e
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler_socket.c
@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 profiler socket communication
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/semaphore.h>
+#include <asm/errno.h>
+
+#include "b2r2_profiler_api.h"
+#include "b2r2_internal.h"
+#include "b2r2_core.h"
+
+/*
+ * TODO: Call the profiler in a seperate thread and have a circular buffer
+ * between the B2R2 driver and that thread. That way the profiler can not slow
+ * down or kill the B2R2 driver. Seems a bit overkill right now as there is
+ * only one B2R2 profiler and we have full control over it but the situation
+ * may be different in the future.
+ */
+
+
+static const struct b2r2_profiler *b2r2_profiler;
+static DEFINE_SEMAPHORE(b2r2_profiler_lock);
+
+
+int b2r2_register_profiler(const struct b2r2_profiler * const profiler)
+{
+	int return_value;
+
+	return_value = down_interruptible(&b2r2_profiler_lock);
+	if (return_value != 0)
+		return return_value;
+
+	if (b2r2_profiler != NULL) {
+		return_value = -EUSERS;
+
+		goto cleanup;
+	}
+
+	b2r2_profiler = profiler;
+
+	return_value = 0;
+
+cleanup:
+	up(&b2r2_profiler_lock);
+
+	return return_value;
+}
+EXPORT_SYMBOL(b2r2_register_profiler);
+
+void b2r2_unregister_profiler(const struct b2r2_profiler * const profiler)
+{
+	down(&b2r2_profiler_lock);
+
+	if (profiler == b2r2_profiler)
+		b2r2_profiler = NULL;
+
+	up(&b2r2_profiler_lock);
+}
+EXPORT_SYMBOL(b2r2_unregister_profiler);
+
+
+bool is_profiler_registered_approx(void)
+{
+	/* No locking by design, to make it fast, hence the approx */
+	if (b2r2_profiler != NULL)
+		return true;
+	else
+		return false;
+}
+
+void b2r2_call_profiler_blt_done(const struct b2r2_blt_request * const request)
+{
+	int return_value;
+	struct b2r2_blt_profiling_info blt_profiling_info;
+	struct b2r2_core *core = (struct b2r2_core *) request->job.data;
+	struct b2r2_control *cont = core->control;
+
+	return_value = down_interruptible(&b2r2_profiler_lock);
+	if (return_value != 0) {
+		dev_err(cont->dev,
+			"%s: Failed to acquire semaphore, ret=%i. "
+			"Lost profiler call!\n", __func__, return_value);
+
+		return;
+	}
+
+	if (NULL == b2r2_profiler)
+		goto cleanup;
+
+	blt_profiling_info.nsec_active_in_cpu = request->nsec_active_in_cpu;
+	blt_profiling_info.nsec_active_in_b2r2 = request->job.nsec_active_in_hw;
+	blt_profiling_info.total_time_nsec = request->total_time_nsec;
+
+	b2r2_profiler->blt_done(&request->user_req, request->request_id, &blt_profiling_info);
+
+cleanup:
+	up(&b2r2_profiler_lock);
+}
diff --git a/drivers/video/b2r2/b2r2_profiler_socket.h b/drivers/video/b2r2/b2r2_profiler_socket.h
new file mode 100644
index 00000000000..80b2c20293f
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_profiler_socket.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 profiler socket communication
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_VIDEO_B2R2_PROFILER_SOCKET_H
+#define _LINUX_VIDEO_B2R2_PROFILER_SOCKET_H
+
+#include "b2r2_internal.h"
+
+/* Will give a correct result most of the time but can be wrong */
+bool is_profiler_registered_approx(void);
+
+void b2r2_call_profiler_blt_done(const struct b2r2_blt_request * const request);
+
+#endif /* _LINUX_VIDEO_B2R2_PROFILER_SOCKET_H */
diff --git a/drivers/video/b2r2/b2r2_structures.h b/drivers/video/b2r2/b2r2_structures.h
new file mode 100644
index 00000000000..99fa7f047d3
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_structures.h
@@ -0,0 +1,226 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 register struct
+ *
+ * Author: Robert Fekete <robert.fekete@stericsson.com>
+ * Author: Paul Wannback
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+
+#ifndef __B2R2_STRUCTURES_H
+#define __B2R2_STRUCTURES_H
+
+/* C struct view */
+struct b2r2_memory_map {
+	unsigned char fill0[2304];
+	unsigned int BLT_SSBA17; /* @2304 */
+	unsigned int BLT_SSBA18; /* @2308 */
+	unsigned int BLT_SSBA19; /* @2312 */
+	unsigned int BLT_SSBA20; /* @2316 */
+	unsigned int BLT_SSBA21; /* @2320 */
+	unsigned int BLT_SSBA22; /* @2324 */
+	unsigned int BLT_SSBA23; /* @2328 */
+	unsigned int BLT_SSBA24; /* @2332 */
+	unsigned char fill1[32];
+	unsigned int BLT_STBA5; /* @2368 */
+	unsigned int BLT_STBA6; /* @2372 */
+	unsigned int BLT_STBA7; /* @2376 */
+	unsigned int BLT_STBA8; /* @2380 */
+	unsigned char fill2[176];
+	unsigned int BLT_CTL; /* @2560 */
+	unsigned int BLT_ITS; /* @2564 */
+	unsigned int BLT_STA1; /* @2568 */
+	unsigned char fill3[4];
+	unsigned int BLT_SSBA1; /* @2576 */
+	unsigned int BLT_SSBA2; /* @2580 */
+	unsigned int BLT_SSBA3; /* @2584 */
+	unsigned int BLT_SSBA4; /* @2588 */
+	unsigned int BLT_SSBA5; /* @2592 */
+	unsigned int BLT_SSBA6; /* @2596 */
+	unsigned int BLT_SSBA7; /* @2600 */
+	unsigned int BLT_SSBA8; /* @2604 */
+	unsigned int BLT_STBA1; /* @2608 */
+	unsigned int BLT_STBA2; /* @2612 */
+	unsigned int BLT_STBA3; /* @2616 */
+	unsigned int BLT_STBA4; /* @2620 */
+	unsigned int BLT_CQ1_TRIG_IP; /* @2624 */
+	unsigned int BLT_CQ1_TRIG_CTL; /* @2628 */
+	unsigned int BLT_CQ1_PACE_CTL; /* @2632 */
+	unsigned int BLT_CQ1_IP; /* @2636 */
+	unsigned int BLT_CQ2_TRIG_IP; /* @2640 */
+	unsigned int BLT_CQ2_TRIG_CTL; /* @2644 */
+	unsigned int BLT_CQ2_PACE_CTL; /* @2648 */
+	unsigned int BLT_CQ2_IP; /* @2652 */
+	unsigned int BLT_AQ1_CTL; /* @2656 */
+	unsigned int BLT_AQ1_IP; /* @2660 */
+	unsigned int BLT_AQ1_LNA; /* @2664 */
+	unsigned int BLT_AQ1_STA; /* @2668 */
+	unsigned int BLT_AQ2_CTL; /* @2672 */
+	unsigned int BLT_AQ2_IP; /* @2676 */
+	unsigned int BLT_AQ2_LNA; /* @2680 */
+	unsigned int BLT_AQ2_STA; /* @2684 */
+	unsigned int BLT_AQ3_CTL; /* @2688 */
+	unsigned int BLT_AQ3_IP; /* @2692 */
+	unsigned int BLT_AQ3_LNA; /* @2696 */
+	unsigned int BLT_AQ3_STA; /* @2700 */
+	unsigned int BLT_AQ4_CTL; /* @2704 */
+	unsigned int BLT_AQ4_IP; /* @2708 */
+	unsigned int BLT_AQ4_LNA; /* @2712 */
+	unsigned int BLT_AQ4_STA; /* @2716 */
+	unsigned int BLT_SSBA9; /* @2720 */
+	unsigned int BLT_SSBA10; /* @2724 */
+	unsigned int BLT_SSBA11; /* @2728 */
+	unsigned int BLT_SSBA12; /* @2732 */
+	unsigned int BLT_SSBA13; /* @2736 */
+	unsigned int BLT_SSBA14; /* @2740 */
+	unsigned int BLT_SSBA15; /* @2744 */
+	unsigned int BLT_SSBA16; /* @2748 */
+	unsigned int BLT_SGA1; /* @2752 */
+	unsigned int BLT_SGA2; /* @2756 */
+	unsigned char fill4[8];
+	unsigned int BLT_ITM0; /* @2768 */
+	unsigned int BLT_ITM1; /* @2772 */
+	unsigned int BLT_ITM2; /* @2776 */
+	unsigned int BLT_ITM3; /* @2780 */
+	unsigned char fill5[16];
+	unsigned int BLT_DFV2; /* @2800 */
+	unsigned int BLT_DFV1; /* @2804 */
+	unsigned int BLT_PRI; /* @2808 */
+	unsigned char fill6[8];
+	unsigned int PLUGS1_OP2; /* @2820 */
+	unsigned int PLUGS1_CHZ; /* @2824 */
+	unsigned int PLUGS1_MSZ; /* @2828 */
+	unsigned int PLUGS1_PGZ; /* @2832 */
+	unsigned char fill7[16];
+	unsigned int PLUGS2_OP2; /* @2852 */
+	unsigned int PLUGS2_CHZ; /* @2856 */
+	unsigned int PLUGS2_MSZ; /* @2860 */
+	unsigned int PLUGS2_PGZ; /* @2864 */
+	unsigned char fill8[16];
+	unsigned int PLUGS3_OP2; /* @2884 */
+	unsigned int PLUGS3_CHZ; /* @2888 */
+	unsigned int PLUGS3_MSZ; /* @2892 */
+	unsigned int PLUGS3_PGZ; /* @2896 */
+	unsigned char fill9[48];
+	unsigned int PLUGT_OP2; /* @2948 */
+	unsigned int PLUGT_CHZ; /* @2952 */
+	unsigned int PLUGT_MSZ; /* @2956 */
+	unsigned int PLUGT_PGZ; /* @2960 */
+	unsigned char fill10[108];
+	unsigned int BLT_NIP; /* @3072 */
+	unsigned int BLT_CIC; /* @3076 */
+	unsigned int BLT_INS; /* @3080 */
+	unsigned int BLT_ACK; /* @3084 */
+	unsigned int BLT_TBA; /* @3088 */
+	unsigned int BLT_TTY; /* @3092 */
+	unsigned int BLT_TXY; /* @3096 */
+	unsigned int BLT_TSZ; /* @3100 */
+	unsigned int BLT_S1CF; /* @3104 */
+	unsigned int BLT_S2CF; /* @3108 */
+	unsigned int BLT_S1BA; /* @3112 */
+	unsigned int BLT_S1TY; /* @3116 */
+	unsigned int BLT_S1XY; /* @3120 */
+	unsigned char fill11[4];
+	unsigned int BLT_S2BA; /* @3128 */
+	unsigned int BLT_S2TY; /* @3132 */
+	unsigned int BLT_S2XY; /* @3136 */
+	unsigned int BLT_S2SZ; /* @3140 */
+	unsigned int BLT_S3BA; /* @3144 */
+	unsigned int BLT_S3TY; /* @3148 */
+	unsigned int BLT_S3XY; /* @3152 */
+	unsigned int BLT_S3SZ; /* @3156 */
+	unsigned int BLT_CWO; /* @3160 */
+	unsigned int BLT_CWS; /* @3164 */
+	unsigned int BLT_CCO; /* @3168 */
+	unsigned int BLT_CML; /* @3172 */
+	unsigned int BLT_FCTL; /* @3176 */
+	unsigned int BLT_PMK; /* @3180 */
+	unsigned int BLT_RSF; /* @3184 */
+	unsigned int BLT_RZI; /* @3188 */
+	unsigned int BLT_HFP; /* @3192 */
+	unsigned int BLT_VFP; /* @3196 */
+	unsigned int BLT_Y_RSF; /* @3200 */
+	unsigned int BLT_Y_RZI; /* @3204 */
+	unsigned int BLT_Y_HFP; /* @3208 */
+	unsigned int BLT_Y_VFP; /* @3212 */
+	unsigned char fill12[16];
+	unsigned int BLT_KEY1; /* @3232 */
+	unsigned int BLT_KEY2; /* @3236 */
+	unsigned char fill13[8];
+	unsigned int BLT_SAR; /* @3248 */
+	unsigned int BLT_USR; /* @3252 */
+	unsigned char fill14[8];
+	unsigned int BLT_IVMX0; /* @3264 */
+	unsigned int BLT_IVMX1; /* @3268 */
+	unsigned int BLT_IVMX2; /* @3272 */
+	unsigned int BLT_IVMX3; /* @3276 */
+	unsigned int BLT_OVMX0; /* @3280 */
+	unsigned int BLT_OVMX1; /* @3284 */
+	unsigned int BLT_OVMX2; /* @3288 */
+	unsigned int BLT_OVMX3; /* @3292 */
+	unsigned char fill15[8];
+	unsigned int BLT_VC1R; /* @3304 */
+	unsigned char fill16[20];
+	unsigned int BLT_Y_HFC0; /* @3328 */
+	unsigned int BLT_Y_HFC1; /* @3332 */
+	unsigned int BLT_Y_HFC2; /* @3336 */
+	unsigned int BLT_Y_HFC3; /* @3340 */
+	unsigned int BLT_Y_HFC4; /* @3344 */
+	unsigned int BLT_Y_HFC5; /* @3348 */
+	unsigned int BLT_Y_HFC6; /* @3352 */
+	unsigned int BLT_Y_HFC7; /* @3356 */
+	unsigned int BLT_Y_HFC8; /* @3360 */
+	unsigned int BLT_Y_HFC9; /* @3364 */
+	unsigned int BLT_Y_HFC10; /* @3368 */
+	unsigned int BLT_Y_HFC11; /* @3372 */
+	unsigned int BLT_Y_HFC12; /* @3376 */
+	unsigned int BLT_Y_HFC13; /* @3380 */
+	unsigned int BLT_Y_HFC14; /* @3384 */
+	unsigned int BLT_Y_HFC15; /* @3388 */
+	unsigned char fill17[80];
+	unsigned int BLT_Y_VFC0; /* @3472 */
+	unsigned int BLT_Y_VFC1; /* @3476 */
+	unsigned int BLT_Y_VFC2; /* @3480 */
+	unsigned int BLT_Y_VFC3; /* @3484 */
+	unsigned int BLT_Y_VFC4; /* @3488 */
+	unsigned int BLT_Y_VFC5; /* @3492 */
+	unsigned int BLT_Y_VFC6; /* @3496 */
+	unsigned int BLT_Y_VFC7; /* @3500 */
+	unsigned int BLT_Y_VFC8; /* @3504 */
+	unsigned int BLT_Y_VFC9; /* @3508 */
+	unsigned char fill18[72];
+	unsigned int BLT_HFC0; /* @3584 */
+	unsigned int BLT_HFC1; /* @3588 */
+	unsigned int BLT_HFC2; /* @3592 */
+	unsigned int BLT_HFC3; /* @3596 */
+	unsigned int BLT_HFC4; /* @3600 */
+	unsigned int BLT_HFC5; /* @3604 */
+	unsigned int BLT_HFC6; /* @3608 */
+	unsigned int BLT_HFC7; /* @3612 */
+	unsigned int BLT_HFC8; /* @3616 */
+	unsigned int BLT_HFC9; /* @3620 */
+	unsigned int BLT_HFC10; /* @3624 */
+	unsigned int BLT_HFC11; /* @3628 */
+	unsigned int BLT_HFC12; /* @3632 */
+	unsigned int BLT_HFC13; /* @3636 */
+	unsigned int BLT_HFC14; /* @3640 */
+	unsigned int BLT_HFC15; /* @3644 */
+	unsigned char fill19[80];
+	unsigned int BLT_VFC0; /* @3728 */
+	unsigned int BLT_VFC1; /* @3732 */
+	unsigned int BLT_VFC2; /* @3736 */
+	unsigned int BLT_VFC3; /* @3740 */
+	unsigned int BLT_VFC4; /* @3744 */
+	unsigned int BLT_VFC5; /* @3748 */
+	unsigned int BLT_VFC6; /* @3752 */
+	unsigned int BLT_VFC7; /* @3756 */
+	unsigned int BLT_VFC8; /* @3760 */
+	unsigned int BLT_VFC9; /* @3764 */
+};
+
+#endif /* !defined(__B2R2_STRUCTURES_H) */
+
diff --git a/drivers/video/b2r2/b2r2_timing.c b/drivers/video/b2r2/b2r2_timing.c
new file mode 100644
index 00000000000..4f3e2b8b042
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_timing.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 timing
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/time.h>
+
+
+u32 b2r2_get_curr_nsec(void)
+{
+	struct timespec ts;
+
+	getrawmonotonic(&ts);
+
+	return (u32)timespec_to_ns(&ts);
+}
diff --git a/drivers/video/b2r2/b2r2_timing.h b/drivers/video/b2r2/b2r2_timing.h
new file mode 100644
index 00000000000..e87113c0ec9
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_timing.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 timing
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_TIMING_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_TIMING_H_
+
+/**
+ * b2r2_get_curr_nsec() - Return the current nanosecond. Notice that the value
+ *                        wraps when the u32 limit is reached.
+ *
+ */
+u32 b2r2_get_curr_nsec(void);
+
+#endif /* _LINUX_DRIVERS_VIDEO_B2R2_TIMING_H_ */
diff --git a/drivers/video/b2r2/b2r2_utils.c b/drivers/video/b2r2/b2r2_utils.c
new file mode 100644
index 00000000000..44c738b5aab
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_utils.c
@@ -0,0 +1,1324 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 utils
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+
+#include <video/b2r2_blt.h>
+
+#include "b2r2_utils.h"
+#include "b2r2_debug.h"
+#include "b2r2_internal.h"
+
+const s32 b2r2_s32_max = 2147483647;
+
+
+/**
+ * calculate_scale_factor() - calculates the scale factor between the given
+ *                            values
+ */
+int calculate_scale_factor(struct device *dev,
+			u32 from, u32 to, u16 *sf_out)
+{
+	int ret;
+	u32 sf;
+
+	b2r2_log_info(dev, "%s\n", __func__);
+
+	if (to == from) {
+		*sf_out = 1 << 10;
+		return 0;
+	} else if (to == 0) {
+		b2r2_log_err(dev, "%s: To is 0!\n", __func__);
+		BUG_ON(1);
+	}
+
+	sf = (from << 10) / to;
+
+	if ((sf & 0xffff0000) != 0) {
+		/* Overflow error */
+		b2r2_log_warn(dev, "%s: "
+			"Scale factor too large\n", __func__);
+		ret = -EINVAL;
+		goto error;
+	} else if (sf == 0) {
+		b2r2_log_warn(dev, "%s: "
+			"Scale factor too small\n", __func__);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	*sf_out = (u16)sf;
+
+	b2r2_log_info(dev, "%s exit\n", __func__);
+
+	return 0;
+
+error:
+	b2r2_log_warn(dev, "%s: Exit...\n", __func__);
+	return ret;
+}
+
+void b2r2_get_img_bounding_rect(struct b2r2_blt_img *img,
+					struct b2r2_blt_rect *bounding_rect)
+{
+	bounding_rect->x = 0;
+	bounding_rect->y = 0;
+	bounding_rect->width = img->width;
+	bounding_rect->height = img->height;
+}
+
+
+bool b2r2_is_zero_area_rect(struct b2r2_blt_rect *rect)
+{
+	return rect->width == 0 || rect->height == 0;
+}
+
+bool b2r2_is_rect_inside_rect(struct b2r2_blt_rect *rect1,
+						struct b2r2_blt_rect *rect2)
+{
+	return rect1->x >= rect2->x &&
+		rect1->y >= rect2->y &&
+		rect1->x + rect1->width <= rect2->x + rect2->width &&
+		rect1->y + rect1->height <= rect2->y + rect2->height;
+}
+
+bool b2r2_is_rect_gte_rect(struct b2r2_blt_rect *rect1,
+		struct b2r2_blt_rect *rect2)
+{
+	return rect1->width >= rect2->width &&
+			rect1->height >= rect2->height;
+}
+
+void b2r2_intersect_rects(struct b2r2_blt_rect *rect1,
+	struct b2r2_blt_rect *rect2, struct b2r2_blt_rect *intersection)
+{
+	struct b2r2_blt_rect tmp_rect;
+
+	tmp_rect.x = max(rect1->x, rect2->x);
+	tmp_rect.y = max(rect1->y, rect2->y);
+	tmp_rect.width = min(rect1->x + rect1->width, rect2->x + rect2->width)
+				- tmp_rect.x;
+	if (tmp_rect.width < 0)
+		tmp_rect.width = 0;
+	tmp_rect.height =
+		min(rect1->y + rect1->height, rect2->y + rect2->height) -
+				tmp_rect.y;
+	if (tmp_rect.height < 0)
+		tmp_rect.height = 0;
+
+	*intersection = tmp_rect;
+}
+
+/*
+ * Calculate new rectangles for the supplied
+ * request, so that clipping to destination imaage
+ * can be avoided.
+ * Essentially, the new destination rectangle is
+ * defined inside the old one. Given the transform
+ * and scaling, one has to calculate which part of
+ * the old source rectangle corresponds to
+ * to the new part of old destination rectangle.
+ */
+void b2r2_trim_rects(struct device *dev,
+			const struct b2r2_blt_req *req,
+			struct b2r2_blt_rect *new_bg_rect,
+			struct b2r2_blt_rect *new_dst_rect,
+			struct b2r2_blt_rect *new_src_rect)
+{
+	enum b2r2_blt_transform transform = req->transform;
+	struct b2r2_blt_rect *old_src_rect =
+		(struct b2r2_blt_rect *) &req->src_rect;
+	struct b2r2_blt_rect *old_dst_rect =
+		(struct b2r2_blt_rect *) &req->dst_rect;
+	struct b2r2_blt_rect *old_bg_rect =
+		(struct b2r2_blt_rect *) &req->bg_rect;
+	struct b2r2_blt_rect dst_img_bounds;
+	s32 src_x = 0;
+	s32 src_y = 0;
+	s32 src_w = 0;
+	s32 src_h = 0;
+	s32 dx = 0;
+	s32 dy = 0;
+	s16 hsf;
+	s16 vsf;
+
+	b2r2_log_info(dev,
+		"%s\nold_dst_rect(x,y,w,h)=(%d, %d, %d, %d)\n", __func__,
+		old_dst_rect->x, old_dst_rect->y,
+		old_dst_rect->width, old_dst_rect->height);
+	b2r2_log_info(dev,
+		"%s\nold_src_rect(x,y,w,h)=(%d, %d, %d, %d)\n", __func__,
+		old_src_rect->x, old_src_rect->y,
+		old_src_rect->width, old_src_rect->height);
+
+	b2r2_get_img_bounding_rect((struct b2r2_blt_img *) &req->dst_img,
+		&dst_img_bounds);
+
+	/* dst_rect inside dst_img, no clipping necessary */
+	if (b2r2_is_rect_inside_rect(old_dst_rect, &dst_img_bounds))
+		goto keep_rects;
+
+	b2r2_intersect_rects(old_dst_rect, &dst_img_bounds, new_dst_rect);
+	b2r2_log_info(dev,
+		"%s\nnew_dst_rect(x,y,w,h)=(%d, %d, %d, %d)\n", __func__,
+		new_dst_rect->x, new_dst_rect->y,
+		new_dst_rect->width, new_dst_rect->height);
+
+	/* dst_rect completely outside, leave it to validation */
+	if (new_dst_rect->width == 0 || new_dst_rect->height == 0)
+		goto keep_rects;
+
+	dx = new_dst_rect->x - old_dst_rect->x;
+	dy = new_dst_rect->y - old_dst_rect->y;
+
+	if (transform & B2R2_BLT_TRANSFORM_CCW_ROT_90) {
+		int res = 0;
+		res = calculate_scale_factor(dev, old_src_rect->width,
+			old_dst_rect->height, &hsf);
+		/* invalid dimensions, leave them to validation */
+		if (res < 0)
+			goto keep_rects;
+
+		res = calculate_scale_factor(dev, old_src_rect->height,
+			old_dst_rect->width, &vsf);
+		if (res < 0)
+			goto keep_rects;
+
+		/*
+		 * After applying the inverse transform
+		 * for 90 degree rotation, the top-left corner
+		 * becomes top-right.
+		 * src_rect origin is defined as top-left,
+		 * so a translation between dst and src
+		 * coordinate spaces is necessary.
+		 */
+		src_x = (old_src_rect->width << 10) -
+			hsf * (dy + new_dst_rect->height);
+		src_y = dx * vsf;
+		src_w = new_dst_rect->height * hsf;
+		src_h = new_dst_rect->width * vsf;
+	} else {
+		int res = 0;
+		res = calculate_scale_factor(dev, old_src_rect->width,
+			old_dst_rect->width, &hsf);
+		if (res < 0)
+			goto keep_rects;
+
+		res = calculate_scale_factor(dev, old_src_rect->height,
+			old_dst_rect->height, &vsf);
+		if (res < 0)
+			goto keep_rects;
+
+		src_x = dx * hsf;
+		src_y = dy * vsf;
+		src_w = new_dst_rect->width * hsf;
+		src_h = new_dst_rect->height * vsf;
+	}
+
+	/*
+	 * src_w must contain all the pixels that contribute
+	 * to a particular destination rectangle.
+	 * ((x + 0x3ff) >> 10) is equivalent to ceiling(x),
+	 * expressed in 6.10 fixed point format.
+	 * Every destination rectangle, maps to a certain area in the source
+	 * rectangle. The area in source will most likely not be a rectangle
+	 * with exact integer dimensions whenever arbitrary scaling is involved.
+	 * Consider the following example.
+	 * Suppose, that width of the current destination rectangle maps
+	 * to 1.7 pixels in source, starting at x == 5.4, as calculated
+	 * using the scaling factor.
+	 * This means that while the destination rectangle is written,
+	 * the source should be read from x == 5.4 up to x == 5.4 + 1.7 == 7.1
+	 * Consequently, color from 3 pixels (x == 5, 6 and 7)
+	 * needs to be read from source.
+	 * The formula below the comment yields:
+	 * ceil(0.4 + 1.7) == ceil(2.1) == 3
+	 * (src_x & 0x3ff) is the fractional part of src_x,
+	 * which is expressed in 6.10 fixed point format.
+	 * Thus, width of the source area should be 3 pixels wide,
+	 * starting at x == 5.
+	 */
+	src_w = ((src_x & 0x3ff) + src_w + 0x3ff) >> 10;
+	src_h = ((src_y & 0x3ff) + src_h + 0x3ff) >> 10;
+
+	src_x >>= 10;
+	src_y >>= 10;
+
+	if (transform & B2R2_BLT_TRANSFORM_FLIP_H)
+		src_x = old_src_rect->width - src_x - src_w;
+
+	if (transform & B2R2_BLT_TRANSFORM_FLIP_V)
+		src_y = old_src_rect->height - src_y - src_h;
+
+	/*
+	 * Translate the src_rect coordinates into true
+	 * src_buffer coordinates.
+	 */
+	src_x += old_src_rect->x;
+	src_y += old_src_rect->y;
+
+	new_src_rect->x = src_x;
+	new_src_rect->y = src_y;
+	new_src_rect->width = src_w;
+	new_src_rect->height = src_h;
+
+	b2r2_log_info(dev,
+		"%s\nnew_src_rect(x,y,w,h)=(%d, %d, %d, %d)\n", __func__,
+		new_src_rect->x, new_src_rect->y,
+		new_src_rect->width, new_src_rect->height);
+
+	if (req->flags & B2R2_BLT_FLAG_BG_BLEND) {
+		/* Modify bg_rect in the same way as dst_rect */
+		s32 dw = new_dst_rect->width - old_dst_rect->width;
+		s32 dh = new_dst_rect->height - old_dst_rect->height;
+		b2r2_log_info(dev,
+			"%s\nold bg_rect(x,y,w,h)=(%d, %d, %d, %d)\n",
+			__func__, old_bg_rect->x, old_bg_rect->y,
+			old_bg_rect->width, old_bg_rect->height);
+		new_bg_rect->x = old_bg_rect->x + dx;
+		new_bg_rect->y = old_bg_rect->y + dy;
+		new_bg_rect->width = old_bg_rect->width + dw;
+		new_bg_rect->height = old_bg_rect->height + dh;
+		b2r2_log_info(dev,
+			"%s\nnew bg_rect(x,y,w,h)=(%d, %d, %d, %d)\n",
+			__func__, new_bg_rect->x, new_bg_rect->y,
+			new_bg_rect->width, new_bg_rect->height);
+	}
+	return;
+keep_rects:
+	/*
+	 * Recalculation was not possible, or not necessary.
+	 * Do not change anything, leave it to validation.
+	 */
+	*new_src_rect = *old_src_rect;
+	*new_dst_rect = *old_dst_rect;
+	*new_bg_rect = *old_bg_rect;
+	b2r2_log_info(dev, "%s original rectangles preserved.\n", __func__);
+	return;
+}
+
+int b2r2_get_fmt_bpp(struct device *dev, enum b2r2_blt_fmt fmt)
+{
+	/*
+	 * Currently this function is not used that often but if that changes a
+	 * lookup table could make it a lot faster.
+	 */
+	switch (fmt) {
+	case B2R2_BLT_FMT_1_BIT_A1:
+		return 1;
+
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return 8;
+
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+		return 12;
+
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return 16;
+
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return 24;
+
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		return 32;
+
+	default:
+		b2r2_log_err(dev,
+			"%s: Internal error! Format %#x not recognized.\n",
+			__func__, fmt);
+		return 32;
+	}
+}
+
+int b2r2_get_fmt_y_bpp(struct device *dev, enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		return 8;
+
+	default:
+		b2r2_log_err(dev,
+			"%s: Internal error! Non YCbCr format supplied.\n",
+			__func__);
+		return 8;
+	}
+}
+
+
+bool b2r2_is_single_plane_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_independent_pixel_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcri_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcrsp_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcrp_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcr420_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcr422_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_ycbcr444_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_mb_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+u32 b2r2_calc_pitch_from_width(struct device *dev,
+		s32 width, enum b2r2_blt_fmt fmt)
+{
+	if (b2r2_is_single_plane_fmt(fmt)) {
+		return (u32)b2r2_div_round_up(width *
+			b2r2_get_fmt_bpp(dev, fmt), 8);
+	} else if (b2r2_is_ycbcrsp_fmt(fmt) || b2r2_is_ycbcrp_fmt(fmt)) {
+		return (u32)b2r2_div_round_up(width *
+			b2r2_get_fmt_y_bpp(dev, fmt), 8);
+	} else {
+		b2r2_log_err(dev, "%s: Internal error! "
+			"Pitchless format supplied.\n",
+			__func__);
+		return 0;
+	}
+}
+
+u32 b2r2_get_img_pitch(struct device *dev, struct b2r2_blt_img *img)
+{
+	if (img->pitch != 0)
+		return img->pitch;
+	else
+		return b2r2_calc_pitch_from_width(dev, img->width, img->fmt);
+}
+
+s32 b2r2_get_img_size(struct device *dev, struct b2r2_blt_img *img)
+{
+	if (b2r2_is_single_plane_fmt(img->fmt)) {
+		return (s32)b2r2_get_img_pitch(dev, img) * img->height;
+	} else if (b2r2_is_ycbcrsp_fmt(img->fmt) ||
+			b2r2_is_ycbcrp_fmt(img->fmt)) {
+		s32 y_plane_size;
+
+		y_plane_size = (s32)b2r2_get_img_pitch(dev, img) * img->height;
+
+		if (b2r2_is_ycbcr420_fmt(img->fmt)) {
+			return y_plane_size + y_plane_size / 2;
+		} else if (b2r2_is_ycbcr422_fmt(img->fmt)) {
+			return y_plane_size * 2;
+		} else if (b2r2_is_ycbcr444_fmt(img->fmt)) {
+			return y_plane_size * 3;
+		} else {
+			b2r2_log_err(dev,	"%s: Internal error!"
+				" Format %#x not recognized.\n",
+				__func__, img->fmt);
+			return 0;
+		}
+	} else if (b2r2_is_mb_fmt(img->fmt)) {
+		return (img->width * img->height *
+			b2r2_get_fmt_bpp(dev, img->fmt)) / 8;
+	} else {
+		b2r2_log_err(dev, "%s: Internal error! "
+			"Format %#x not recognized.\n",
+			__func__, img->fmt);
+		return 0;
+	}
+}
+
+
+s32 b2r2_div_round_up(s32 dividend, s32 divisor)
+{
+	s32 quotient = dividend / divisor;
+	if (dividend % divisor != 0)
+		quotient++;
+
+	return quotient;
+}
+
+bool b2r2_is_aligned(s32 value, s32 alignment)
+{
+	return value % alignment == 0;
+}
+
+s32 b2r2_align_up(s32 value, s32 alignment)
+{
+	s32 remainder = abs(value) % abs(alignment);
+	s32 value_to_add;
+
+	if (remainder > 0) {
+		if (value >= 0)
+			value_to_add = alignment - remainder;
+		else
+			value_to_add = remainder;
+	} else {
+		value_to_add = 0;
+	}
+
+	return value + value_to_add;
+}
+
+/**
+ * b2r2_get_alpha_range() - returns the alpha range of the given format
+ */
+enum b2r2_ty b2r2_get_alpha_range(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_8_BIT_A8:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		return B2R2_TY_ALPHA_RANGE_255; /* 0 - 255 */
+	default:
+		return B2R2_TY_ALPHA_RANGE_128; /* 0 - 128 */
+	}
+}
+
+/**
+ * b2r2_get_alpha() - returns the pixel alpha in 0...255 range
+ */
+u8 b2r2_get_alpha(enum b2r2_blt_fmt fmt, u32 pixel)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		return (pixel >> 24) & 0xff;
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		return pixel & 0xff;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		return (pixel & 0xfff) >> 16;
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		return (((pixel >> 12) & 0xf) * 255) / 15;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		return (pixel >> 15) * 255;
+	case B2R2_BLT_FMT_1_BIT_A1:
+		return pixel * 255;
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return pixel;
+	default:
+		return 255;
+	}
+}
+
+/**
+ * b2r2_set_alpha() - returns a color value with the alpha component set
+ */
+u32 b2r2_set_alpha(enum b2r2_blt_fmt fmt, u8 alpha, u32 color)
+{
+	u32 alpha_mask;
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+		color &= 0x00ffffff;
+		alpha_mask = alpha << 24;
+		break;
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		color &= 0xffffff00;
+		alpha_mask = alpha;
+		break;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		color &= 0x00ffff;
+		alpha_mask = alpha << 16;
+		break;
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		color &= 0x0fff;
+		alpha_mask = (alpha << 8) & 0xF000;
+		break;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		color &= 0x7fff;
+		alpha_mask = (alpha / 255) << 15 ;
+		break;
+	case B2R2_BLT_FMT_1_BIT_A1:
+		color = 0;
+		alpha_mask = (alpha / 255);
+		break;
+	case B2R2_BLT_FMT_8_BIT_A8:
+		color = 0;
+		alpha_mask = alpha;
+		break;
+	default:
+		alpha_mask = 0;
+	}
+
+	return color | alpha_mask;
+}
+
+/**
+ * b2r2_fmt_has_alpha() - returns whether the given format carries an alpha value
+ */
+bool b2r2_fmt_has_alpha(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * b2r2_is_rgb_fmt() - returns whether the given format is a rgb format
+ */
+bool b2r2_is_rgb_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * b2r2_is_bgr_fmt() - returns whether the given format is a bgr format
+ */
+bool b2r2_is_bgr_fmt(enum b2r2_blt_fmt fmt)
+{
+	return (fmt == B2R2_BLT_FMT_32_BIT_ABGR8888);
+}
+
+/**
+ * b2r2_is_yuv_fmt() - returns whether the given format is a yuv format
+ */
+bool b2r2_is_yuv_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * b2r2_is_yvu_fmt() - returns whether the given format is a yvu format
+ */
+bool b2r2_is_yvu_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * b2r2_is_yuv420_fmt() - returns whether the given format is a yuv420 format
+ */
+bool b2r2_is_yuv420_fmt(enum b2r2_blt_fmt fmt)
+{
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_yuv422_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * b2r2_is_yvu420_fmt() - returns whether the given format is a yvu420 format
+ */
+bool b2r2_is_yvu420_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+bool b2r2_is_yvu422_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+
+/**
+ * b2r2_is_yuv444_fmt() - returns whether the given format is a yuv444 format
+ */
+bool b2r2_is_yuv444_fmt(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * b2r2_fmt_byte_pitch() - returns the pitch of a pixmap with the given width
+ */
+int b2r2_fmt_byte_pitch(enum b2r2_blt_fmt fmt, u32 width)
+{
+	int pitch;
+
+	switch (fmt) {
+
+	case B2R2_BLT_FMT_1_BIT_A1:
+		pitch = width >> 3; /* Shift is faster than division */
+		if ((width & 0x3) != 0) /* Check for remainder */
+			pitch++;
+		return pitch;
+
+	case B2R2_BLT_FMT_8_BIT_A8:                        /* Fall through */
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:            /* Fall through */
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:            /* Fall through */
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE: /* Fall through */
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:       /* Fall through */
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:       /* Fall through */
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:            /* Fall through */
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:            /* Fall through */
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE: /* Fall through */
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:       /* Fall through */
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:       /* Fall through */
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return width;
+
+	case B2R2_BLT_FMT_16_BIT_ARGB4444: /* Fall through */
+	case B2R2_BLT_FMT_16_BIT_ARGB1555: /* Fall through */
+	case B2R2_BLT_FMT_16_BIT_RGB565:   /* Fall through */
+	case B2R2_BLT_FMT_Y_CB_Y_CR:       /* Fall through */
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		return width << 1;
+
+	case B2R2_BLT_FMT_24_BIT_RGB888:   /* Fall through */
+	case B2R2_BLT_FMT_24_BIT_ARGB8565: /* Fall through */
+	case B2R2_BLT_FMT_24_BIT_YUV888:   /* Fall through */
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+		return width * 3;
+
+	case B2R2_BLT_FMT_32_BIT_ARGB8888: /* Fall through */
+	case B2R2_BLT_FMT_32_BIT_ABGR8888: /* Fall through */
+	case B2R2_BLT_FMT_32_BIT_AYUV8888: /* Fall through */
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+		return width << 2;
+
+	default:
+		/* Should never, ever happen */
+		BUG_ON(1);
+		return 0;
+	}
+}
+
+/**
+ * b2r2_to_native_fmt() - returns the native B2R2 format
+ */
+enum b2r2_native_fmt b2r2_to_native_fmt(enum b2r2_blt_fmt fmt)
+{
+
+	switch (fmt) {
+	case B2R2_BLT_FMT_UNUSED:
+		return B2R2_NATIVE_RGB565;
+	case B2R2_BLT_FMT_1_BIT_A1:
+		return B2R2_NATIVE_A1;
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return B2R2_NATIVE_A8;
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		return B2R2_NATIVE_RGB565;
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		return B2R2_NATIVE_ARGB4444;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		return B2R2_NATIVE_ARGB1555;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		return B2R2_NATIVE_ARGB8565;
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+		return B2R2_NATIVE_RGB888;
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_24_BIT_VUY888: /* Not actually supported by HW */
+		return B2R2_NATIVE_YCBCR888;
+	case B2R2_BLT_FMT_32_BIT_ABGR8888: /* Not actually supported by HW */
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+		return B2R2_NATIVE_ARGB8888;
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888: /* Not actually supported by HW */
+		return B2R2_NATIVE_AYCBCR8888;
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+		return B2R2_NATIVE_YCBCR422R;
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+		return B2R2_NATIVE_YCBCR422R;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+		return B2R2_NATIVE_YCBCR42X_R2B;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return B2R2_NATIVE_YCBCR42X_MBN;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return B2R2_NATIVE_YUV;
+	default:
+		/* Should never ever happen */
+		return B2R2_NATIVE_BYTE;
+	}
+}
+
+/**
+ * Bit-expand the color from fmt to RGB888 with blue at LSB.
+ * Copy MSBs into missing LSBs.
+ */
+u32 b2r2_to_RGB888(u32 color, const enum b2r2_blt_fmt fmt)
+{
+	u32 out_color = 0;
+	u32 r = 0;
+	u32 g = 0;
+	u32 b = 0;
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+		r = ((color & 0xf00) << 12) | ((color & 0xf00) << 8);
+		g = ((color & 0xf0) << 8) | ((color & 0xf0) << 4);
+		b = ((color & 0xf) << 4) | (color & 0xf);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+		r = ((color & 0x7c00) << 9) | ((color & 0x7000) << 4);
+		g = ((color & 0x3e0) << 6) | ((color & 0x380) << 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+		r = ((color & 0xf800) << 8) | ((color & 0xe000) << 3);
+		g = ((color & 0x7e0) << 5) | ((color & 0x600) >> 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+		out_color = color & 0xffffff;
+		break;
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+		r = (color & 0xff) << 16;
+		g = color & 0xff00;
+		b = (color & 0xff0000) >> 16;
+		out_color = r | g | b;
+		break;
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+		r = ((color & 0xf800) << 8) | ((color & 0xe000) << 3);
+		g = ((color & 0x7e0) << 5) | ((color & 0x600) >> 1);
+		b = ((color & 0x1f) << 3) | ((color & 0x1c) >> 2);
+		out_color = r | g | b;
+		break;
+	default:
+		break;
+	}
+
+	return out_color;
+}
+
+/**
+ * b2r2_get_fmt_type() - returns the type of the given format (raster, planar, etc.)
+ */
+enum b2r2_fmt_type b2r2_get_fmt_type(enum b2r2_blt_fmt fmt)
+{
+	switch (fmt) {
+	case B2R2_BLT_FMT_16_BIT_ARGB4444:
+	case B2R2_BLT_FMT_16_BIT_ARGB1555:
+	case B2R2_BLT_FMT_16_BIT_RGB565:
+	case B2R2_BLT_FMT_24_BIT_RGB888:
+	case B2R2_BLT_FMT_32_BIT_ARGB8888:
+	case B2R2_BLT_FMT_Y_CB_Y_CR:
+	case B2R2_BLT_FMT_CB_Y_CR_Y:
+	case B2R2_BLT_FMT_32_BIT_ABGR8888:
+	case B2R2_BLT_FMT_24_BIT_ARGB8565:
+	case B2R2_BLT_FMT_24_BIT_YUV888:
+	case B2R2_BLT_FMT_32_BIT_AYUV8888:
+	case B2R2_BLT_FMT_24_BIT_VUY888:
+	case B2R2_BLT_FMT_32_BIT_VUYA8888:
+	case B2R2_BLT_FMT_1_BIT_A1:
+	case B2R2_BLT_FMT_8_BIT_A8:
+		return B2R2_FMT_TYPE_RASTER;
+	case B2R2_BLT_FMT_YUV420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_PLANAR:
+	case B2R2_BLT_FMT_YUV444_PACKED_PLANAR:
+		return B2R2_FMT_TYPE_PLANAR;
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU420_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YVU422_PACKED_SEMI_PLANAR:
+	case B2R2_BLT_FMT_YUV420_PACKED_SEMIPLANAR_MB_STE:
+	case B2R2_BLT_FMT_YUV422_PACKED_SEMIPLANAR_MB_STE:
+		return B2R2_FMT_TYPE_SEMI_PLANAR;
+	default:
+		return B2R2_FMT_TYPE_RASTER;
+	}
+}
+
+#ifdef CONFIG_DEBUG_FS
+/**
+ * sprintf_req() - Builds a string representing the request, for debug
+ *
+ * @request:Request that should be encoded into a string
+ * @buf: Receiving buffer
+ * @size: Size of receiving buffer
+ *
+ * Returns number of characters in string, excluding null terminator
+ */
+int sprintf_req(struct b2r2_blt_request *request, char *buf, int size)
+{
+	size_t dev_size = 0;
+
+	/* generic request info */
+	dev_size += sprintf(buf + dev_size,
+		"instance      : 0x%08lX\n",
+		(unsigned long) request->instance);
+	dev_size += sprintf(buf + dev_size,
+		"size          : %d bytes\n", request->user_req.size);
+	dev_size += sprintf(buf + dev_size,
+		"flags         : 0x%08lX\n",
+		(unsigned long) request->user_req.flags);
+	dev_size += sprintf(buf + dev_size,
+		"transform     : %d\n",
+		(int) request->user_req.transform);
+	dev_size += sprintf(buf + dev_size,
+		"prio          : %d\n", request->user_req.transform);
+	dev_size += sprintf(buf + dev_size,
+		"global_alpha  : %d\n",
+		(int) request->user_req.global_alpha);
+	dev_size += sprintf(buf + dev_size,
+		"report1       : 0x%08lX\n",
+		(unsigned long) request->user_req.report1);
+	dev_size += sprintf(buf + dev_size,
+		"report2       : 0x%08lX\n",
+		(unsigned long) request->user_req.report2);
+	dev_size += sprintf(buf + dev_size,
+		"request_id    : 0x%08lX\n\n",
+		(unsigned long) request->request_id);
+
+	/* src info */
+	dev_size += sprintf(buf + dev_size,
+		"src_img.fmt   : %#010x\n",
+		request->user_req.src_img.fmt);
+	dev_size += sprintf(buf + dev_size,
+		"src_img.buf   : {type=%d, hwmem_buf_name=%d, fd=%d, "
+		"offset=%d, len=%d}\n",
+		request->user_req.src_img.buf.type,
+		request->user_req.src_img.buf.hwmem_buf_name,
+		request->user_req.src_img.buf.fd,
+		request->user_req.src_img.buf.offset,
+		request->user_req.src_img.buf.len);
+	dev_size += sprintf(buf + dev_size,
+		"src_img       : {width=%d, height=%d, pitch=%d}\n",
+		request->user_req.src_img.width,
+		request->user_req.src_img.height,
+		request->user_req.src_img.pitch);
+	dev_size += sprintf(buf + dev_size,
+		"src_mask.fmt  : %#010x\n",
+		request->user_req.src_mask.fmt);
+	dev_size += sprintf(buf + dev_size,
+		"src_mask.buf  : {type=%d, hwmem_buf_name=%d, fd=%d,"
+		" offset=%d, len=%d}\n",
+		request->user_req.src_mask.buf.type,
+		request->user_req.src_mask.buf.hwmem_buf_name,
+		request->user_req.src_mask.buf.fd,
+		request->user_req.src_mask.buf.offset,
+		request->user_req.src_mask.buf.len);
+	dev_size += sprintf(buf + dev_size,
+		"src_mask      : {width=%d, height=%d, pitch=%d}\n",
+		request->user_req.src_mask.width,
+		request->user_req.src_mask.height,
+		request->user_req.src_mask.pitch);
+	dev_size += sprintf(buf + dev_size,
+		"src_rect      : {x=%d, y=%d, width=%d, height=%d}\n",
+		request->user_req.src_rect.x,
+		request->user_req.src_rect.y,
+		request->user_req.src_rect.width,
+		request->user_req.src_rect.height);
+	dev_size += sprintf(buf + dev_size,
+		"src_color     : 0x%08lX\n\n",
+		(unsigned long) request->user_req.src_color);
+
+	/* bg info */
+	dev_size += sprintf(buf + dev_size,
+		"bg_img.fmt    : %#010x\n",
+		request->user_req.bg_img.fmt);
+	dev_size += sprintf(buf + dev_size,
+		"bg_img.buf    : {type=%d, hwmem_buf_name=%d, fd=%d,"
+		" offset=%d, len=%d}\n",
+		request->user_req.bg_img.buf.type,
+		request->user_req.bg_img.buf.hwmem_buf_name,
+		request->user_req.bg_img.buf.fd,
+		request->user_req.bg_img.buf.offset,
+		request->user_req.bg_img.buf.len);
+	dev_size += sprintf(buf + dev_size,
+		"bg_img        : {width=%d, height=%d, pitch=%d}\n",
+		request->user_req.bg_img.width,
+		request->user_req.bg_img.height,
+		request->user_req.bg_img.pitch);
+	dev_size += sprintf(buf + dev_size,
+		"bg_rect       : {x=%d, y=%d, width=%d, height=%d}\n\n",
+		request->user_req.bg_rect.x,
+		request->user_req.bg_rect.y,
+		request->user_req.bg_rect.width,
+		request->user_req.bg_rect.height);
+
+	/* dst info */
+	dev_size += sprintf(buf + dev_size,
+		"dst_img.fmt   : %#010x\n",
+		request->user_req.dst_img.fmt);
+	dev_size += sprintf(buf + dev_size,
+		"dst_img.buf   : {type=%d, hwmem_buf_name=%d, fd=%d,"
+		" offset=%d, len=%d}\n",
+		request->user_req.dst_img.buf.type,
+		request->user_req.dst_img.buf.hwmem_buf_name,
+		request->user_req.dst_img.buf.fd,
+		request->user_req.dst_img.buf.offset,
+		request->user_req.dst_img.buf.len);
+	dev_size += sprintf(buf + dev_size,
+		"dst_img       : {width=%d, height=%d, pitch=%d}\n",
+		request->user_req.dst_img.width,
+		request->user_req.dst_img.height,
+		request->user_req.dst_img.pitch);
+	dev_size += sprintf(buf + dev_size,
+		"dst_rect      : {x=%d, y=%d, width=%d, height=%d}\n",
+		request->user_req.dst_rect.x,
+		request->user_req.dst_rect.y,
+		request->user_req.dst_rect.width,
+		request->user_req.dst_rect.height);
+	dev_size += sprintf(buf + dev_size,
+		"dst_clip_rect : {x=%d, y=%d, width=%d, height=%d}\n",
+		request->user_req.dst_clip_rect.x,
+		request->user_req.dst_clip_rect.y,
+		request->user_req.dst_clip_rect.width,
+		request->user_req.dst_clip_rect.height);
+	dev_size += sprintf(buf + dev_size,
+		"dst_color     : 0x%08lX\n\n",
+		(unsigned long) request->user_req.dst_color);
+
+	dev_size += sprintf(buf + dev_size,
+		"src_resolved.physical                  : 0x%08lX\n",
+		(unsigned long) request->src_resolved.
+		physical_address);
+	dev_size += sprintf(buf + dev_size,
+		"src_resolved.virtual                   : 0x%08lX\n",
+		(unsigned long) request->src_resolved.virtual_address);
+	dev_size += sprintf(buf + dev_size,
+		"src_resolved.filep                     : 0x%08lX\n",
+		(unsigned long) request->src_resolved.filep);
+	dev_size += sprintf(buf + dev_size,
+		"src_resolved.filep_physical_start      : 0x%08lX\n",
+		(unsigned long) request->src_resolved.
+		file_physical_start);
+	dev_size += sprintf(buf + dev_size,
+		"src_resolved.filep_virtual_start       : 0x%08lX\n",
+		(unsigned long) request->src_resolved.file_virtual_start);
+	dev_size += sprintf(buf + dev_size,
+		"src_resolved.file_len                  : %d\n\n",
+		request->src_resolved.file_len);
+
+	dev_size += sprintf(buf + dev_size,
+		"src_mask_resolved.physical             : 0x%08lX\n",
+		(unsigned long) request->src_mask_resolved.
+		physical_address);
+	dev_size += sprintf(buf + dev_size,
+		"src_mask_resolved.virtual              : 0x%08lX\n",
+		(unsigned long) request->src_mask_resolved.virtual_address);
+	dev_size += sprintf(buf + dev_size,
+		"src_mask_resolved.filep                : 0x%08lX\n",
+		(unsigned long) request->src_mask_resolved.filep);
+	dev_size += sprintf(buf + dev_size,
+		"src_mask_resolved.filep_physical_start : 0x%08lX\n",
+		(unsigned long) request->src_mask_resolved.
+		file_physical_start);
+	dev_size += sprintf(buf + dev_size,
+		"src_mask_resolved.filep_virtual_start  : 0x%08lX\n",
+		(unsigned long) request->src_mask_resolved.
+		file_virtual_start);
+	dev_size += sprintf(buf + dev_size,
+		"src_mask_resolved.file_len             : %d\n\n",
+		request->src_mask_resolved.file_len);
+
+	dev_size += sprintf(buf + dev_size,
+		"dst_resolved.physical                  : 0x%08lX\n",
+		(unsigned long) request->dst_resolved.
+		physical_address);
+	dev_size += sprintf(buf + dev_size,
+		"dst_resolved.virtual                   : 0x%08lX\n",
+		(unsigned long) request->dst_resolved.virtual_address);
+	dev_size += sprintf(buf + dev_size,
+		"dst_resolved.filep                     : 0x%08lX\n",
+		(unsigned long) request->dst_resolved.filep);
+	dev_size += sprintf(buf + dev_size,
+		"dst_resolved.filep_physical_start      : 0x%08lX\n",
+		(unsigned long) request->dst_resolved.
+		file_physical_start);
+	dev_size += sprintf(buf + dev_size,
+		"dst_resolved.filep_virtual_start       : 0x%08lX\n",
+		(unsigned long) request->dst_resolved.file_virtual_start);
+	dev_size += sprintf(buf + dev_size,
+		"dst_resolved.file_len                  : %d\n\n",
+		request->dst_resolved.file_len);
+
+	return dev_size;
+}
+#endif
+
+void b2r2_recalculate_rects(struct device *dev,
+		struct b2r2_blt_req *req)
+{
+	struct b2r2_blt_rect new_dst_rect;
+	struct b2r2_blt_rect new_src_rect;
+	struct b2r2_blt_rect new_bg_rect;
+
+	b2r2_trim_rects(dev,
+		req, &new_bg_rect, &new_dst_rect, &new_src_rect);
+
+	req->dst_rect = new_dst_rect;
+	req->src_rect = new_src_rect;
+	if (req->flags & B2R2_BLT_FLAG_BG_BLEND)
+		req->bg_rect = new_bg_rect;
+}
diff --git a/drivers/video/b2r2/b2r2_utils.h b/drivers/video/b2r2/b2r2_utils.h
new file mode 100644
index 00000000000..081ac1f4848
--- /dev/null
+++ b/drivers/video/b2r2/b2r2_utils.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson B2R2 utils
+ *
+ * Author: Johan Mossberg <johan.xx.mossberg@stericsson.com> for ST-Ericsson
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef _LINUX_DRIVERS_VIDEO_B2R2_UTILS_H_
+#define _LINUX_DRIVERS_VIDEO_B2R2_UTILS_H_
+
+#include <video/b2r2_blt.h>
+
+#include "b2r2_internal.h"
+
+extern const s32 b2r2_s32_max;
+
+int calculate_scale_factor(struct device *dev,
+		u32 from, u32 to, u16 *sf_out);
+void b2r2_get_img_bounding_rect(struct b2r2_blt_img *img,
+		struct b2r2_blt_rect *bounding_rect);
+
+bool b2r2_is_zero_area_rect(struct b2r2_blt_rect *rect);
+bool b2r2_is_rect_inside_rect(struct b2r2_blt_rect *rect1,
+		struct b2r2_blt_rect *rect2);
+bool b2r2_is_rect_gte_rect(struct b2r2_blt_rect *rect1,
+		struct b2r2_blt_rect *rect2);
+void b2r2_intersect_rects(struct b2r2_blt_rect *rect1,
+		struct b2r2_blt_rect *rect2,
+		struct b2r2_blt_rect *intersection);
+void b2r2_trim_rects(struct device *dev,
+			const struct b2r2_blt_req *req,
+			struct b2r2_blt_rect *new_bg_rect,
+			struct b2r2_blt_rect *new_dst_rect,
+			struct b2r2_blt_rect *new_src_rect);
+
+int b2r2_get_fmt_bpp(struct device *dev, enum b2r2_blt_fmt fmt);
+int b2r2_get_fmt_y_bpp(struct device *dev, enum b2r2_blt_fmt fmt);
+
+bool b2r2_is_single_plane_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_independent_pixel_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcri_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcrsp_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcrp_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcr420_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcr422_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_ycbcr444_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_mb_fmt(enum b2r2_blt_fmt fmt);
+
+/*
+ * Rounds up if an invalid width causes the pitch to be non byte aligned.
+ */
+u32 b2r2_calc_pitch_from_width(struct device *dev,
+		s32 width, enum b2r2_blt_fmt fmt);
+u32 b2r2_get_img_pitch(struct device *dev,
+		struct b2r2_blt_img *img);
+s32 b2r2_get_img_size(struct device *dev,
+		struct b2r2_blt_img *img);
+
+s32 b2r2_div_round_up(s32 dividend, s32 divisor);
+bool b2r2_is_aligned(s32 value, s32 alignment);
+s32 b2r2_align_up(s32 value, s32 alignment);
+
+enum b2r2_ty b2r2_get_alpha_range(enum b2r2_blt_fmt fmt);
+u8 b2r2_get_alpha(enum b2r2_blt_fmt fmt, u32 pixel);
+u32 b2r2_set_alpha(enum b2r2_blt_fmt fmt, u8 alpha, u32 color);
+bool b2r2_fmt_has_alpha(enum b2r2_blt_fmt fmt);
+bool b2r2_is_rgb_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_bgr_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_yuv_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_yvu_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_yuv420_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_yuv422_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_yvu420_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_yvu422_fmt(enum b2r2_blt_fmt fmt);
+bool b2r2_is_yuv444_fmt(enum b2r2_blt_fmt fmt);
+int b2r2_fmt_byte_pitch(enum b2r2_blt_fmt fmt, u32 width);
+enum b2r2_native_fmt b2r2_to_native_fmt(enum b2r2_blt_fmt fmt);
+u32 b2r2_to_RGB888(u32 color, const enum b2r2_blt_fmt fmt);
+enum b2r2_fmt_type b2r2_get_fmt_type(enum b2r2_blt_fmt fmt);
+#ifdef CONFIG_DEBUG_FS
+int sprintf_req(struct b2r2_blt_request *request, char *buf, int size);
+#endif
+void b2r2_recalculate_rects(struct device *dev,
+		struct b2r2_blt_req *req);
+
+#endif
diff --git a/drivers/video/mcde/Kconfig b/drivers/video/mcde/Kconfig
new file mode 100644
index 00000000000..cb88a66d370
--- /dev/null
+++ b/drivers/video/mcde/Kconfig
@@ -0,0 +1,96 @@
+config FB_MCDE
+	tristate "MCDE support"
+	depends on FB
+	select FB_SYS_FILLRECT
+	select FB_SYS_COPYAREA
+	select FB_SYS_IMAGEBLIT
+	select FB_SYS_FOPS
+	select HWMEM
+	---help---
+	  This enables support for MCDE based frame buffer driver.
+
+	  Please read the file <file:Documentation/fb/mcde.txt>
+
+config FB_MCDE_DEBUG
+	bool "MCDE debug messages"
+	depends on FB_MCDE
+	---help---
+	  Say Y here if you want the MCDE driver to output debug messages
+
+config FB_MCDE_VDEBUG
+	bool "MCDE verbose debug messages"
+	depends on FB_MCDE_DEBUG
+	---help---
+	  Say Y here if you want the MCDE driver to output more debug messages
+
+config MCDE_FB_AVOID_REALLOC
+	bool "MCDE early allocate framebuffer"
+	default n
+	depends on FB_MCDE
+	---help---
+	  If you say Y here maximum frame buffer size is allocated and
+	  used for all resolutions. If you say N here, the frame buffer is
+	  reallocated when resolution is changed. This reallocation might
+	  fail because of fragmented memory. Note that this memory will
+	  never be deallocated, while the MCDE framebuffer is used.
+
+config MCDE_DISPLAY_DSI
+	bool "Support for DSI displays within MCDE"
+	depends on FB_MCDE
+	default y
+
+menu "MCDE DSI displays"
+	depends on MCDE_DISPLAY_DSI
+
+config MCDE_DISPLAY_GENERIC_DSI
+	tristate "Generic DSI display driver"
+
+config MCDE_DISPLAY_SAMSUNG_S6D16D0
+	bool "Samsung S6D16D0 DSI display driver"
+	---help---
+	  Say Y if you have a TPO Taal or Blackpearl display panel.
+
+config MCDE_DISPLAY_SONY_ACX424AKP_DSI
+	tristate "Sony acx424akp DSI display driver"
+
+config MCDE_DISPLAY_AV8100
+	tristate "AV8100 HDMI/CVBS display driver"
+	select AV8100
+
+config MCDE_DISPLAY_HDMI_FB_AUTO_CREATE
+	bool "HDMI_FB_AUTO_CREATE"
+	default y
+	depends on MCDE_DISPLAY_AV8100
+	---help---
+	  Say Y if you want the HDMI frame buffer to be created on start
+	  Say N if you want the HDMI frame buffer to be created when HDMI
+	  cable is plugged (needs user space HDMIservice)
+
+endmenu
+
+config MCDE_DISPLAY_DPI
+	bool "Support for DPI displays within MCDE"
+	depends on FB_MCDE
+	default n
+	---help---
+	  Add this option to choose which DPI display driver for MCDE to include
+
+	  DPI (Display Pixel Interface) is a MIPI Alliance standard used for
+	  active-matrix LCDs. The DPI uses parallel data lines.
+
+menu "MCDE DPI displays"
+	depends on MCDE_DISPLAY_DPI
+
+config MCDE_DISPLAY_VUIB500_DPI
+	tristate "DPI display driver for the VUIB500 board"
+	---help---
+	  The VUIB500 is an ST-Ericsson user interface board.
+
+endmenu
+
+config MCDE_DISPLAY_AB8500_DENC
+	tristate "AB8500 CVBS display driver"
+	depends on FB_MCDE
+	select AB8500_DENC
+
+
diff --git a/drivers/video/mcde/Makefile b/drivers/video/mcde/Makefile
new file mode 100644
index 00000000000..82a78c2542a
--- /dev/null
+++ b/drivers/video/mcde/Makefile
@@ -0,0 +1,23 @@
+mcde-objs		+= mcde_mod.o
+mcde-objs		+= mcde_hw.o
+mcde-objs		+= mcde_dss.o
+mcde-objs		+= mcde_display.o
+mcde-objs		+= mcde_bus.o
+mcde-objs		+= mcde_fb.o
+mcde-objs		+= mcde_debugfs.o
+obj-$(CONFIG_FB_MCDE)	+= mcde.o
+
+obj-$(CONFIG_MCDE_DISPLAY_GENERIC_DSI)		+= display-generic_dsi.o
+obj-$(CONFIG_MCDE_DISPLAY_SAMSUNG_S6D16D0)	+= display-samsung_s6d16d0.o
+obj-$(CONFIG_MCDE_DISPLAY_SONY_ACX424AKP_DSI)   += display-sony_acx424akp_dsi.o
+obj-$(CONFIG_MCDE_DISPLAY_VUIB500_DPI)		+= display-vuib500-dpi.o
+obj-$(CONFIG_MCDE_DISPLAY_AB8500_DENC)		+= display-ab8500.o
+obj-$(CONFIG_MCDE_DISPLAY_AV8100)		+= display-av8100.o
+obj-$(CONFIG_DISPLAY_FICTIVE)			+= display-fictive.o
+
+ifdef CONFIG_FB_MCDE_DEBUG
+EXTRA_CFLAGS += -DDEBUG
+endif
+ifdef CONFIG_FB_MCDE_VDEBUG
+EXTRA_CFLAGS += -DVERBOSE_DEBUG
+endif
diff --git a/drivers/video/mcde/display-ab8500.c b/drivers/video/mcde/display-ab8500.c
new file mode 100644
index 00000000000..a761a5eec80
--- /dev/null
+++ b/drivers/video/mcde/display-ab8500.c
@@ -0,0 +1,494 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * AB8500 display driver
+ *
+ * Author: Marcel Tunnissen <marcel.tuennissen@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/mfd/ab8500/denc.h>
+#include <video/mcde_display.h>
+#include <video/mcde_display-ab8500.h>
+
+#define AB8500_DISP_TRACE	dev_dbg(&ddev->dev, "%s\n", __func__)
+
+#define SDTV_PIXCLOCK		37037
+
+/*
+ * PAL:
+ * Total nr of active lines:   576
+ * Total nr of blanking lines:  49
+ *                      total: 625
+ */
+#define PAL_HBP			132
+#define PAL_HFP			12
+#define PAL_VBP_FIELD_1		22
+#define PAL_VBP_FIELD_2		23
+#define PAL_VFP_FIELD_1		2
+#define PAL_VFP_FIELD_2		2
+
+/*
+ * NTSC (ITU-R BT.470-5):
+ * Total nr of active lines:   486
+ * Total nr of blanking lines:  39
+ *                      total: 525
+ */
+#define NTSC_ORG_HBP		122
+#define NTSC_ORG_HFP		16
+#define NTSC_ORG_VBP_FIELD_1	16
+#define NTSC_ORG_VBP_FIELD_2	17
+#define NTSC_ORG_VFP_FIELD_1	3
+#define NTSC_ORG_VFP_FIELD_2	3
+
+/*
+ * NTSC (DV variant):
+ * Total nr of active lines:   480
+ * Total nr of blanking lines:  45
+ *                      total: 525
+ */
+#define NTSC_HBP		122
+#define NTSC_HFP		16
+#define NTSC_VBP_FIELD_1	19
+#define NTSC_VBP_FIELD_2	20
+#define NTSC_VFP_FIELD_1	3
+#define NTSC_VFP_FIELD_2	3
+
+struct display_driver_data {
+	struct ab8500_denc_conf denc_conf;
+	struct platform_device *denc_dev;
+	int nr_regulators;
+	struct regulator **regulator;
+};
+
+static int try_video_mode(struct mcde_display_device *ddev,
+				struct mcde_video_mode *video_mode);
+static int set_video_mode(struct mcde_display_device *ddev,
+				struct mcde_video_mode *video_mode);
+static int set_power_mode(struct mcde_display_device *ddev,
+				enum mcde_display_power_mode power_mode);
+static int on_first_update(struct mcde_display_device *ddev);
+static int display_update(struct mcde_display_device *ddev,
+							bool tripple_buffer);
+
+static int __devinit ab8500_probe(struct mcde_display_device *ddev)
+{
+	int ret = 0;
+	int i;
+	struct ab8500_display_platform_data *pdata = ddev->dev.platform_data;
+	struct display_driver_data *driver_data;
+
+	AB8500_DISP_TRACE;
+
+	if (pdata == NULL) {
+		dev_err(&ddev->dev, "%s:Platform data missing\n", __func__);
+		return -EINVAL;
+	}
+	if (ddev->port->type != MCDE_PORTTYPE_DPI) {
+		dev_err(&ddev->dev, "%s:Invalid port type %d\n", __func__,
+							ddev->port->type);
+		return -EINVAL;
+	}
+
+	driver_data = (struct display_driver_data *)
+		kzalloc(sizeof(struct display_driver_data), GFP_KERNEL);
+	if (!driver_data) {
+		dev_err(&ddev->dev, "Failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+	driver_data->denc_dev = ab8500_denc_get_device();
+	if (!driver_data->denc_dev) {
+		dev_err(&ddev->dev, "Failed to get DENC device\n");
+		ret = -ENODEV;
+		goto dev_get_failed;
+	}
+
+	driver_data->regulator = kzalloc(pdata->nr_regulators *
+					sizeof(struct regulator *), GFP_KERNEL);
+	if (!driver_data->regulator) {
+		dev_err(&ddev->dev, "Failed to allocate regulator list\n");
+		ret = -ENOMEM;
+		goto reg_alloc_failed;
+	}
+	for (i = 0; i < pdata->nr_regulators; i++) {
+		driver_data->regulator[i] = regulator_get(&ddev->dev,
+						pdata->regulator_id[i]);
+		if (IS_ERR(driver_data->regulator[i])) {
+			ret = PTR_ERR(driver_data->regulator[i]);
+			dev_warn(&ddev->dev, "%s:Failed to get regulator %s\n",
+					__func__, pdata->regulator_id[i]);
+			goto regulator_get_failed;
+		}
+	}
+	driver_data->nr_regulators = pdata->nr_regulators;
+
+	dev_set_drvdata(&ddev->dev, driver_data);
+
+	ddev->try_video_mode = try_video_mode;
+	ddev->set_video_mode = set_video_mode;
+	ddev->set_power_mode = set_power_mode;
+	ddev->on_first_update = on_first_update;
+	ddev->update = display_update;
+
+	return 0;
+
+regulator_get_failed:
+	for (i--; i >= 0; i--)
+		regulator_put(driver_data->regulator[i]);
+	kfree(driver_data->regulator);
+	driver_data->regulator = NULL;
+reg_alloc_failed:
+	ab8500_denc_put_device(driver_data->denc_dev);
+dev_get_failed:
+	kfree(driver_data);
+	return ret;
+}
+
+static int __devexit ab8500_remove(struct mcde_display_device *ddev)
+{
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+	AB8500_DISP_TRACE;
+
+	ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+
+	if (driver_data->regulator) {
+		int i;
+		for (i = driver_data->nr_regulators - 1; i >= 0; i--)
+			regulator_put(driver_data->regulator[i]);
+		kfree(driver_data->regulator);
+		driver_data->regulator = NULL;
+		driver_data->nr_regulators = 0;
+	}
+	ab8500_denc_put_device(driver_data->denc_dev);
+	kfree(driver_data);
+	return 0;
+}
+
+static int ab8500_resume(struct mcde_display_device *ddev)
+{
+	int ret = 0;
+	AB8500_DISP_TRACE;
+
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s: Failed to resume display\n",
+								__func__);
+
+	return ret;
+}
+
+static int ab8500_suspend(struct mcde_display_device *ddev, pm_message_t state)
+{
+	int ret = 0;
+	AB8500_DISP_TRACE;
+
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s: Failed to suspend display\n",
+								__func__);
+
+	return ret;
+}
+
+
+static struct mcde_display_driver ab8500_driver = {
+	.probe	= ab8500_probe,
+	.remove = ab8500_remove,
+	.suspend = ab8500_suspend,
+	.resume = ab8500_resume,
+	.driver = {
+		.name	= "mcde_tv_ab8500",
+	},
+};
+
+static void print_vmode(struct mcde_video_mode *vmode)
+{
+	pr_debug("resolution: %dx%d\n", vmode->xres, vmode->yres);
+	pr_debug("  pixclock: %d\n",    vmode->pixclock);
+	pr_debug("       hbp: %d\n",    vmode->hbp);
+	pr_debug("       hfp: %d\n",    vmode->hfp);
+	pr_debug("       vbp: %d\n",    vmode->vbp);
+	pr_debug("       vfp: %d\n",    vmode->vfp);
+	pr_debug("interlaced: %s\n", vmode->interlaced ? "true" : "false");
+}
+
+static int try_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	AB8500_DISP_TRACE;
+
+	if (ddev == NULL || video_mode == NULL) {
+		dev_warn(&ddev->dev, "%s:ddev = NULL or video_mode = NULL\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	if (video_mode->xres != 720) {
+		dev_warn(&ddev->dev,
+			"%s:Failed to find video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+
+	/* TODO: move this part to MCDE: mcde_dss_try_video_mode? */
+	/* check for PAL */
+	switch (video_mode->yres) {
+	case 576:
+		/* set including SAV/EAV: */
+		video_mode->hbp = PAL_HBP;
+		video_mode->hfp = PAL_HFP;
+		video_mode->vbp = PAL_VBP_FIELD_1 + PAL_VBP_FIELD_2;
+		video_mode->vfp = PAL_VFP_FIELD_1 + PAL_VFP_FIELD_2;
+		video_mode->interlaced = true;
+		video_mode->pixclock = SDTV_PIXCLOCK;
+		break;
+	case 480:
+		/* set including SAV/EAV */
+		video_mode->hbp = NTSC_HBP;
+		video_mode->hfp = NTSC_HFP;
+		video_mode->vbp = NTSC_VBP_FIELD_1 + NTSC_VBP_FIELD_2;
+		video_mode->vfp = NTSC_VFP_FIELD_1 + NTSC_VFP_FIELD_2;
+		video_mode->interlaced = true;
+		video_mode->pixclock = SDTV_PIXCLOCK;
+		break;
+	case 486:
+		/* set including SAV/EAV */
+		video_mode->hbp = NTSC_ORG_HBP;
+		video_mode->hfp = NTSC_ORG_HFP;
+		video_mode->vbp = NTSC_ORG_VBP_FIELD_1 + NTSC_ORG_VBP_FIELD_2;
+		video_mode->vfp = NTSC_ORG_VFP_FIELD_1 + NTSC_ORG_VFP_FIELD_2;
+		video_mode->interlaced = true;
+		video_mode->pixclock = SDTV_PIXCLOCK;
+		break;
+	default:
+		dev_warn(&ddev->dev,
+			"%s:Failed to find video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+
+	print_vmode(video_mode);
+
+	return 0;
+
+}
+
+static int set_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	int res;
+	struct ab8500_display_platform_data *pdata = ddev->dev.platform_data;
+	struct display_driver_data *driver_data =
+		(struct display_driver_data *)dev_get_drvdata(&ddev->dev);
+	AB8500_DISP_TRACE;
+
+	if (ddev == NULL || video_mode == NULL) {
+		dev_warn(&ddev->dev, "%s:ddev = NULL or video_mode = NULL\n",
+			__func__);
+		return -EINVAL;
+	}
+	ddev->video_mode = *video_mode;
+
+	if (video_mode->xres != 720) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+
+	/* check for PAL BDGHI and N */
+	switch (video_mode->yres) {
+	case 576:
+		driver_data->denc_conf.TV_std = TV_STD_PAL_BDGHI;
+		/* TODO: how to choose LOW DEF FILTER */
+		driver_data->denc_conf.cr_filter = TV_CR_PAL_HIGH_DEF_FILTER;
+		/* TODO: PAL N (e.g. uses a setup of 7.5 IRE) */
+		driver_data->denc_conf.black_level_setup = false;
+		break;
+	case 480: /* NTSC, PAL M DV variant */
+	case 486: /* NTSC, PAL M original   */
+		/* TODO: PAL M */
+		driver_data->denc_conf.TV_std = TV_STD_NTSC_M;
+		/* TODO: how to choose LOW DEF FILTER */
+		driver_data->denc_conf.cr_filter = TV_CR_NTSC_HIGH_DEF_FILTER;
+		driver_data->denc_conf.black_level_setup = true;
+		break;
+	default:
+		dev_warn(&ddev->dev, "%s:Failed to set video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+
+
+	driver_data->denc_conf.progressive	= !video_mode->interlaced;
+	driver_data->denc_conf.act_output	= true;
+	driver_data->denc_conf.test_pattern	= false;
+	driver_data->denc_conf.partial_blanking	= true;
+	driver_data->denc_conf.blank_all	= false;
+	driver_data->denc_conf.suppress_col	= false;
+	driver_data->denc_conf.phase_reset_mode	= TV_PHASE_RST_MOD_DISABLE;
+	driver_data->denc_conf.dac_enable	= false;
+	driver_data->denc_conf.act_dc_output	= true;
+
+	set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (pdata->rgb_2_yCbCr_transform)
+		mcde_chnl_set_col_convert(ddev->chnl_state,
+						pdata->rgb_2_yCbCr_transform,
+						MCDE_CONVERT_RGB_2_YCBCR);
+	mcde_chnl_stop_flow(ddev->chnl_state);
+	res = mcde_chnl_set_video_mode(ddev->chnl_state, &ddev->video_mode);
+	if (res < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode on channel\n",
+			__func__);
+
+		return res;
+	}
+	ddev->update_flags |= UPDATE_FLAG_VIDEO_MODE;
+
+	return 0;
+}
+
+static int set_power_mode(struct mcde_display_device *ddev,
+	enum mcde_display_power_mode power_mode)
+{
+	int ret = 0;
+	int i;
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+	AB8500_DISP_TRACE;
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+					power_mode != MCDE_DISPLAY_PM_OFF) {
+		dev_dbg(&ddev->dev, "off -> standby\n");
+		if (ddev->platform_enable) {
+			ret = ddev->platform_enable(ddev);
+			if (ret)
+				goto error;
+		}
+		if (driver_data->regulator) {
+			for (i = 0; i < driver_data->nr_regulators; i++) {
+				ret = regulator_enable(
+						driver_data->regulator[i]);
+				if (ret)
+					goto off_to_standby_failed;
+				dev_dbg(&ddev->dev, "regulator %d on\n", i);
+			}
+		}
+		ab8500_denc_power_up(driver_data->denc_dev);
+		ab8500_denc_reset(driver_data->denc_dev, true);
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+	/* STANDBY -> ON */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+					power_mode == MCDE_DISPLAY_PM_ON) {
+		dev_dbg(&ddev->dev, "standby -> on\n");
+		ddev->power_mode = MCDE_DISPLAY_PM_ON;
+	}
+	/* ON -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+					power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+		dev_dbg(&ddev->dev, "on -> standby\n");
+		ab8500_denc_reset(driver_data->denc_dev, false);
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+	/* STANDBY -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+					power_mode == MCDE_DISPLAY_PM_OFF) {
+		bool error = false;
+		dev_dbg(&ddev->dev, "standby -> off\n");
+		if (driver_data->regulator) {
+			for (i = 0; i < driver_data->nr_regulators; i++) {
+				ret = regulator_disable(
+						driver_data->regulator[i]);
+				/* continue in case of an error */
+				error |= (ret != 0);
+				dev_dbg(&ddev->dev, "regulator %d off\n", i);
+			}
+		}
+		if (ddev->platform_disable) {
+			ret = ddev->platform_disable(ddev);
+			error |= (ret != 0);
+		}
+		if (error) {
+			/* the latest error code is returned */
+			goto error;
+		}
+		memset(&(ddev->video_mode), 0, sizeof(struct mcde_video_mode));
+		ab8500_denc_power_down(driver_data->denc_dev);
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+	return 0;
+
+	/* In case of an error, try to leave in off-state */
+off_to_standby_failed:
+	for (i--; i >= 0; i--)
+		regulator_disable(driver_data->regulator[i]);
+	ddev->platform_disable(ddev);
+
+error:
+	dev_err(&ddev->dev, "Failed to set power mode");
+	return ret;
+}
+
+static int on_first_update(struct mcde_display_device *ddev)
+{
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+
+	ab8500_denc_conf(driver_data->denc_dev, &driver_data->denc_conf);
+	ab8500_denc_conf_plug_detect(driver_data->denc_dev, true, false,
+							TV_PLUG_TIME_2S);
+	ab8500_denc_mask_int_plug_det(driver_data->denc_dev, false, false);
+	ddev->first_update = false;
+	return 0;
+}
+
+static int display_update(struct mcde_display_device *ddev, bool tripple_buffer)
+{
+	int ret;
+
+	if (ddev->first_update)
+		on_first_update(ddev);
+	if (ddev->power_mode != MCDE_DISPLAY_PM_ON && ddev->set_power_mode) {
+		ret = set_power_mode(ddev, MCDE_DISPLAY_PM_ON);
+		if (ret < 0)
+			goto error;
+	}
+	ret = mcde_chnl_update(ddev->chnl_state, &ddev->update_area,
+								tripple_buffer);
+	if (ret < 0)
+		goto error;
+out:
+	return ret;
+error:
+	dev_warn(&ddev->dev, "%s:Failed to set power mode to on\n", __func__);
+	goto out;
+}
+
+/* Module init */
+static int __init mcde_display_tvout_ab8500_init(void)
+{
+	pr_debug("%s\n", __func__);
+
+	return mcde_display_driver_register(&ab8500_driver);
+}
+late_initcall(mcde_display_tvout_ab8500_init);
+
+static void __exit mcde_display_tvout_ab8500_exit(void)
+{
+	pr_debug("%s\n", __func__);
+
+	mcde_display_driver_unregister(&ab8500_driver);
+}
+module_exit(mcde_display_tvout_ab8500_exit);
+
+MODULE_AUTHOR("Marcel Tunnissen <marcel.tuennissen@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE TVout through AB8500 display driver");
diff --git a/drivers/video/mcde/display-av8100.c b/drivers/video/mcde/display-av8100.c
new file mode 100644
index 00000000000..70750998824
--- /dev/null
+++ b/drivers/video/mcde/display-av8100.c
@@ -0,0 +1,1656 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson HDMI display driver
+ *
+ * Author: Per Persson <per-xb-persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/compdev.h>
+#include <linux/clonedev.h>
+
+#include <video/mcde_fb.h>
+#include <video/mcde_display.h>
+#include <video/mcde_display-av8100.h>
+#include <video/av8100.h>
+#include <video/hdmi.h>
+
+#define SWITCH_HELPSTR ", 0=HDMI, 1=SDTV, 2=DVI\n"
+
+/* AVI Infoframe */
+#define AVI_INFOFRAME_DATA_SIZE	13
+#define AVI_INFOFRAME_TYPE	0x82
+#define AVI_INFOFRAME_VERSION	0x02
+#define AVI_INFOFRAME_DB1	0x10	/* Active Information present */
+#define AVI_INFOFRAME_DB2	0x08	/* Active Portion Aspect ratio */
+
+#ifdef CONFIG_DISPLAY_AV8100_TRIPPLE_BUFFER
+#define NUM_FB_BUFFERS 3
+#else
+#define NUM_FB_BUFFERS 2
+#endif
+
+#define DSI_HS_FREQ_HZ 840320000
+#define DSI_LP_FREQ_HZ 19200000
+
+struct cea_vesa_video_mode {
+	u32 cea;
+	u32 vesa_cea_nr;
+	struct mcde_video_mode *video_mode;
+};
+
+static int hdmi_try_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode);
+static int hdmi_set_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode);
+static int hdmi_set_pixel_format(
+	struct mcde_display_device *ddev, enum mcde_ovly_pix_fmt format);
+static struct mcde_video_mode *video_mode_get(struct mcde_display_device *ddev,
+		u8 cea, u8 vesa_cea_nr);
+static int ceanr_convert(struct mcde_display_device *ddev,
+					u8 cea, u8 vesa_cea_nr, u16 *w, u16 *h);
+
+static ssize_t show_hdmisdtvswitch(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_hdmisdtvswitch(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_input_pixel_format(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_input_pixel_format(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_disponoff(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_disponoff(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t show_vesacea(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t show_timing(struct device *dev,
+		struct device_attribute *attr, char *buf);
+static ssize_t store_timing(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static ssize_t store_stayalive(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count);
+static DEVICE_ATTR(disponoff, S_IRUGO | S_IWUSR, show_disponoff,
+		store_disponoff);
+static DEVICE_ATTR(vesacea, S_IRUGO, show_vesacea, NULL);
+static DEVICE_ATTR(timing, S_IRUGO | S_IWUSR, show_timing, store_timing);
+static DEVICE_ATTR(stayalive, S_IWUSR, NULL, store_stayalive);
+
+static DEVICE_ATTR(hdmisdtvswitch, S_IRUGO | S_IWUSR, show_hdmisdtvswitch,
+			store_hdmisdtvswitch);
+static DEVICE_ATTR(input_pixel_format, S_IRUGO | S_IWUSR,
+			show_input_pixel_format, store_input_pixel_format);
+
+static ssize_t show_hdmisdtvswitch(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcde_display_device *mdev = to_mcde_display_device(dev);
+	int index;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	sprintf(buf, "%1x%s", mdev->port->hdmi_sdtv_switch, SWITCH_HELPSTR);
+	index = 1 + strlen(SWITCH_HELPSTR) + 1;
+
+	return index;
+}
+
+static ssize_t store_hdmisdtvswitch(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *mdev = to_mcde_display_device(dev);
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (count > 0) {
+		if ((*buf == 0) || (*buf == '0')) {
+			dev_dbg(dev, "hdmi/sdtv switch = hdmi\n");
+			mdev->port->hdmi_sdtv_switch = HDMI_SWITCH;
+			mdev->native_x_res = NATIVE_XRES_HDMI;
+			mdev->native_y_res = NATIVE_YRES_HDMI;
+		} else if ((*buf == 1) || (*buf == '1')) {
+			dev_dbg(dev, "hdmi/sdtv switch = sdtv\n");
+			mdev->port->hdmi_sdtv_switch = SDTV_SWITCH;
+			mdev->native_x_res = NATIVE_XRES_SDTV;
+			mdev->native_y_res = NATIVE_YRES_SDTV;
+		} else if ((*buf == 2) || (*buf == '2')) {
+			dev_dbg(dev, "hdmi/sdtv switch = dvi\n");
+			mdev->port->hdmi_sdtv_switch = DVI_SWITCH;
+			mdev->native_x_res = NATIVE_XRES_HDMI;
+			mdev->native_y_res = NATIVE_YRES_HDMI;
+		}
+		/* implicitely read by a memcmp in dss */
+		mdev->video_mode.force_update = true;
+	}
+
+	return count;
+}
+
+static ssize_t show_input_pixel_format(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+
+	return sprintf(buf, "%d\n", ddev->port->pixel_format);
+}
+
+static ssize_t store_input_pixel_format(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+
+	dev_dbg(dev, "%s\n", __func__);
+	if (count > 0) {
+		unsigned long input;
+		if (strict_strtoul(buf, 10, &input) != 0)
+			return -EINVAL;
+		switch (input) {
+		/* intentional fall through */
+		case MCDE_PORTPIXFMT_DSI_16BPP:
+		case MCDE_PORTPIXFMT_DSI_18BPP:
+		case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		case MCDE_PORTPIXFMT_DSI_24BPP:
+		case MCDE_PORTPIXFMT_DSI_YCBCR422:
+			ddev->port->pixel_format = input;
+			break;
+		default:
+			dev_warn(&ddev->dev, "invalid format (%ld)\n",
+								input);
+			return -EINVAL;
+			break;
+		}
+		/* implicitely read by a memcmp in dss */
+		ddev->video_mode.force_update = true;
+		driver_data->update_port_pixel_format = true;
+	}
+
+	return count;
+}
+
+static ssize_t show_disponoff(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (ddev->fbi && driver_data->fbdevname) {
+		dev_dbg(dev, "name:%s\n", driver_data->fbdevname);
+		strcpy(buf, driver_data->fbdevname);
+		return strlen(driver_data->fbdevname) + 1;
+	}
+	return 0;
+}
+
+static ssize_t store_disponoff(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *mdev = to_mcde_display_device(dev);
+	bool enable = false;
+	u8 cea = 0;
+	u8 vesa_cea_nr = 0;
+#ifdef CONFIG_COMPDEV
+	struct mcde_fb *mfb;
+#endif
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if ((count != DISPONOFF_SIZE) && (count != DISPONOFF_SIZE + 1))
+		return -EINVAL;
+
+	if ((*buf == '0') && (*(buf + 1) == '1'))
+		enable = true;
+	cea = (hex_to_bin(buf[2]) << 4) + hex_to_bin(buf[3]);
+	vesa_cea_nr = (hex_to_bin(buf[4]) << 4) + hex_to_bin(buf[5]);
+	dev_dbg(dev, "enable:%d cea:%d nr:%d\n", enable, cea, vesa_cea_nr);
+
+	if (enable && !mdev->fbi) {
+		struct display_driver_data *driver_data = dev_get_drvdata(dev);
+		u16 w = mdev->native_x_res;
+		u16 h = mdev->native_y_res, vh;
+		int buffering = NUM_FB_BUFFERS;
+		struct fb_info *fbi;
+
+		ceanr_convert(mdev, cea, vesa_cea_nr, &w, &h);
+		vh = h * buffering;
+		fbi = mcde_fb_create(mdev, w, h, w, vh,
+				mdev->default_pixel_format, FB_ROTATE_UR);
+		if (IS_ERR(fbi))
+			dev_warn(dev, "fb create failed\n");
+		else
+			driver_data->fbdevname = dev_name(fbi->dev);
+
+#ifdef CONFIG_COMPDEV
+		/* TODO need another way for compdev to get actual size */
+		mdev->native_x_res = w;
+		mdev->native_y_res = h;
+
+		mfb = to_mcde_fb(fbi);
+		/* Create a compdev overlay for this display */
+		if (compdev_create(mdev, mfb->ovlys[0], false) < 0) {
+			dev_warn(&mdev->dev,
+				"Failed to create compdev for display %s\n",
+						mdev->name);
+		} else {
+			dev_dbg(&mdev->dev, "compdev created for (%s)\n",
+						mdev->name);
+		}
+#ifdef CONFIG_CLONEDEV
+		if (clonedev_create()) {
+			dev_warn(&mdev->dev,
+				"Failed to create clonedev for display %s\n",
+						mdev->name);
+		} else {
+			dev_dbg(&mdev->dev, "clonedev created for (%s)\n",
+						mdev->name);
+		}
+#endif
+#endif
+	} else if (!enable && mdev->fbi) {
+#ifdef CONFIG_CLONEDEV
+		clonedev_destroy();
+#endif
+#ifdef CONFIG_COMPDEV
+		compdev_destroy(mdev);
+#endif
+		mcde_fb_destroy(mdev);
+	}
+
+	return count;
+}
+
+static ssize_t show_timing(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+	struct mcde_video_mode *video_mode;
+	int index;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	index = 0;
+	if (driver_data->video_mode) {
+		video_mode = driver_data->video_mode;
+		memcpy(buf + index, &video_mode->xres, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->yres, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->pixclock, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->hbp, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->hfp, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->vbp, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->vfp, sizeof(u32));
+		index += sizeof(u32);
+		memcpy(buf + index, &video_mode->interlaced, sizeof(u32));
+		index += sizeof(u32);
+	}
+	return index;
+}
+
+static ssize_t store_timing(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (count != TIMING_SIZE)
+		return -EINVAL;
+
+	driver_data->video_mode = video_mode_get(ddev, *buf, *(buf + 1));
+
+	return count;
+}
+
+static ssize_t store_stayalive(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+
+	if (count != STAYALIVE_SIZE)
+		return -EINVAL;
+
+	if ((*buf == 1) || (*buf == '1'))
+		ddev->stay_alive = true;
+	else
+		ddev->stay_alive = false;
+
+	dev_dbg(dev, "%s %d\n", __func__, ddev->stay_alive);
+
+	return count;
+}
+
+static int ceanr_convert(struct mcde_display_device *ddev,
+			u8 cea, u8 vesa_cea_nr, u16 *w, u16 *h)
+{
+	struct mcde_video_mode *video_mode;
+
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+	video_mode = video_mode_get(ddev, cea, vesa_cea_nr);
+	if (video_mode) {
+		*w = video_mode->xres;
+		*h = video_mode->yres;
+		dev_dbg(&ddev->dev, "cea:%d nr:%d found\n",
+				cea, vesa_cea_nr);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+/* Supported HDMI modes */
+static struct mcde_video_mode video_modes_supp_hdmi[] = {
+	/* 0 CEA #1 640_480_60_P */
+	{
+		.xres = 640,	.yres = 480,
+		.pixclock = 39682,
+		.hbp = 112,	.hfp = 48,
+		.vbp = 33,	.vfp = 12
+	},
+	/* 1 720_480_60_P */
+	{
+		.xres = 720,	.yres = 480,
+		.pixclock = 37000,
+		.hbp = 104,	.hfp = 34,
+		.vbp = 30,	.vfp = 15
+	},
+	/* 2 720_576_50_P */
+	{
+		.xres = 720,	.yres = 576,
+		.pixclock = 37037,
+		.hbp = 132,	.hfp = 12,
+		.vbp = 44,	.vfp = 5
+	},
+	/* 3 1280_720_60_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 13468,
+		.hbp = 256,	.hfp = 114,
+		.vbp = 20,	.vfp = 10
+	},
+	/* 4 1280_720_50_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 13468,
+		.hbp = 260,	.hfp = 440,
+		.vbp = 25,	.vfp = 5
+	},
+	/* 5 1280_720_30_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 13468,
+		.hbp = 260,	.hfp = 1760,
+		.vbp = 20,	.vfp = 10
+	},
+	/* 6 1280_720_24_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 16835,
+		.hbp = 260,	.hfp = 1760,
+		.vbp = 20,	.vfp = 10
+	},
+	/* 7 1280_720_25_P */
+	{
+		.xres = 1280,	.yres = 720,
+		.pixclock = 13468,
+		.hbp = 260,	.hfp = 2420,
+		.vbp = 20,	.vfp = 10
+	},
+	/* 8 1920_1080_30_P */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 189,	.hfp = 91,
+		.vbp = 36,	.vfp = 9
+	},
+	/* 9 1920_1080_24_P */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 170,	.hfp = 660,
+		.vbp = 36,	.vfp = 9
+	},
+	/* 10 1920_1080_25_P */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 192,	.hfp = 528,
+		.vbp = 36,	.vfp = 9
+	},
+	/* 11 720_480_60_I */
+	{
+		.xres = 720,	.yres = 480,
+		.pixclock = 74074,
+		.hbp = 126,	.hfp = 12,
+		.vbp = 44,	.vfp = 1,
+		.interlaced = true,
+	},
+	/* 12 720_576_50_I */
+	{
+		.xres = 720,	.yres = 576,
+		.pixclock = 74074,
+		.hbp = 132,	.hfp = 12,
+		.vbp = 44,	.vfp = 5,
+		.interlaced = true,
+	},
+	/* 13 1920_1080_50_I */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 192,	.hfp = 528,
+		.vbp = 20,	.vfp = 25,
+		.interlaced = true,
+	},
+	/* 14 1920_1080_60_I */
+	{
+		.xres = 1920,	.yres = 1080,
+		.pixclock = 13468,
+		.hbp = 192,	.hfp = 88,
+		.vbp = 20,	.vfp = 25,
+		.interlaced = true,
+	},
+	/* 15 VESA #9 800_600_60_P */
+	{
+		.xres = 800,	.yres = 600,
+		.pixclock = 25000,
+		.hbp = 168,	.hfp = 88,
+		.vbp = 23,	.vfp = 5,
+		.interlaced = false,
+	},
+	/* 16 VESA #14 848_480_60_P */
+	{
+		.xres = 848,	.yres = 480,
+		.pixclock = 29630,
+		.hbp = 128,	.hfp = 112,
+		.vbp = 23,	.vfp = 14,
+		.interlaced = false,
+	},
+	/* 17 VESA #16 1024_768_60_P */
+	{
+		.xres = 1024,	.yres = 768,
+		.pixclock = 15385,
+		.hbp = 160,	.hfp = 160,
+		.vbp = 29,	.vfp = 9,
+		.interlaced = false,
+	},
+	/* 18 VESA #22 1280_768_60_P */
+	{
+		.xres = 1280,	.yres = 768,
+		.pixclock = 14652,
+		.hbp = 80,	.hfp = 80,
+		.vbp = 12,	.vfp = 10,
+		.interlaced = false,
+	},
+	/* 19 VESA #23 1280_768_60_P */
+	{
+		.xres = 1280,	.yres = 768,
+		.pixclock = 12579,
+		.hbp = 192,	.hfp = 192,
+		.vbp = 20,	.vfp = 10,
+		.interlaced = false,
+	},
+	/* 20 VESA #27 1280_800_60_P */
+	{
+		.xres = 1280,	.yres = 800,
+		.pixclock = 14085,
+		.hbp = 80,	.hfp = 80,
+		.vbp = 14,	.vfp = 9,
+		.interlaced = false,
+	},
+	/* 21 VESA #28 1280_800_60_P */
+	{
+		.xres = 1280,	.yres = 800,
+		.pixclock = 11976,
+		.hbp = 200,	.hfp = 200,
+		.vbp = 22,	.vfp = 9,
+		.interlaced = false,
+	},
+	/* 22 VESA #39 1360_768_60_P */
+	{
+		.xres = 1360,	.yres = 768,
+		.pixclock = 11696,
+		.hbp = 176,	.hfp = 256,
+		.vbp = 18,	.vfp = 9,
+		.interlaced = false,
+	},
+	/* 23 VESA #81 1366_768_60_P */
+	{
+		.xres = 1366,	.yres = 768,
+		.pixclock = 11662,
+		.hbp = 213,	.hfp = 213,
+		.vbp = 24,	.vfp = 6,
+		.interlaced = false,
+	},
+};
+
+/* Supported TVout modes */
+static struct mcde_video_mode video_modes_supp_sdtv[] = {
+	/* 720_480_60_I) */
+	{
+		.xres = 720,	.yres = 480,
+		.pixclock = 74074,
+		.hbp = 126,	.hfp = 12,
+		.vbp = 44,	.vfp = 1,
+		.interlaced = true,
+	},
+	/* 720_576_50_I) */
+	{
+		.xres = 720,	.yres = 576,
+		.pixclock = 74074,
+		.hbp = 132,	.hfp = 12,
+		.vbp = 44,	.vfp = 5,
+		.interlaced = true,
+	},
+};
+
+static struct cea_vesa_video_mode cea_vesa_video_mode[] = {
+		/* 640_480_60_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 1,
+			.video_mode = &video_modes_supp_hdmi[0],
+		},
+		/* 720_480_60_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 2,
+			.video_mode = &video_modes_supp_hdmi[1],
+		},
+		/* 720_480_60_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 3,
+			.video_mode = &video_modes_supp_hdmi[1],
+		},
+		/* 720_576_50_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 17,
+			.video_mode = &video_modes_supp_hdmi[2],
+		},
+		/* 720_576_50_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 18,
+			.video_mode = &video_modes_supp_hdmi[2],
+		},
+		/* 1280_720_60_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 4,
+			.video_mode = &video_modes_supp_hdmi[3],
+		},
+		/* 1280_720_50_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 19,
+			.video_mode = &video_modes_supp_hdmi[4],
+		},
+		/* 1280_720_30_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 62,
+			.video_mode = &video_modes_supp_hdmi[5],
+		},
+		/* 1280_720_24_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 60,
+			.video_mode = &video_modes_supp_hdmi[6],
+		},
+		/* 1280_720_25_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 61,
+			.video_mode = &video_modes_supp_hdmi[7],
+		},
+		/* 1920_1080_30_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 34,
+			.video_mode = &video_modes_supp_hdmi[8],
+		},
+		/* 1920_1080_24_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 32,
+			.video_mode = &video_modes_supp_hdmi[9],
+		},
+		/* 1920_1080_25_P */
+		{
+			.cea = 1,	.vesa_cea_nr = 33,
+			.video_mode = &video_modes_supp_hdmi[10],
+		},
+		/* 720_480_60_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 6,
+			.video_mode = &video_modes_supp_hdmi[11],
+		},
+		/* 720_480_60_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 7,
+			.video_mode = &video_modes_supp_hdmi[11],
+		},
+		/* 720_576_50_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 21,
+			.video_mode = &video_modes_supp_hdmi[12],
+		},
+		/* 720_576_50_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 22,
+			.video_mode = &video_modes_supp_hdmi[12],
+		},
+		/* 1920_1080_50_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 20,
+			.video_mode = &video_modes_supp_hdmi[13],
+		},
+		/* 1920_1080_60_I) */
+		{
+			.cea = 1,	.vesa_cea_nr = 5,
+			.video_mode = &video_modes_supp_hdmi[14],
+		},
+		/* VESA #4 640_480_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 4,
+			.video_mode = &video_modes_supp_hdmi[0],
+		},
+		/* VESA #9 800_600_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 9,
+			.video_mode = &video_modes_supp_hdmi[15],
+		},
+		/* VESA #14 848_480_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 14,
+			.video_mode = &video_modes_supp_hdmi[16],
+		},
+		/* VESA #16 1024_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 16,
+			.video_mode = &video_modes_supp_hdmi[17],
+		},
+		/* VESA #22 1280_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 22,
+			.video_mode = &video_modes_supp_hdmi[18],
+		},
+		/* VESA #23 1280_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 23,
+			.video_mode = &video_modes_supp_hdmi[19],
+		},
+		/* VESA #27 1280_800_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 27,
+			.video_mode = &video_modes_supp_hdmi[20],
+		},
+		/* VESA #28 1280_800_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 28,
+			.video_mode = &video_modes_supp_hdmi[21],
+		},
+		/* VESA #39 1360_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 39,
+			.video_mode = &video_modes_supp_hdmi[22],
+		},
+		/* VESA #81 1366_768_60_P) */
+		{
+			.cea = 0,	.vesa_cea_nr = 81,
+			.video_mode = &video_modes_supp_hdmi[23],
+		},
+};
+
+static ssize_t show_vesacea(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	int findex;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	for (findex = 0; findex < ARRAY_SIZE(cea_vesa_video_mode); findex++) {
+		*(buf + findex * 2) = cea_vesa_video_mode[findex].cea;
+		*(buf + findex * 2 + 1) =
+				cea_vesa_video_mode[findex].vesa_cea_nr;
+	}
+	*(buf + findex * 2) = '\0';
+
+	return findex * 2 + 1;
+}
+
+static struct mcde_video_mode *video_mode_get(struct mcde_display_device *ddev,
+		u8 cea, u8 vesa_cea_nr)
+{
+	int findex;
+
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+
+	for (findex = 0; findex < ARRAY_SIZE(cea_vesa_video_mode); findex++)
+		if ((cea == cea_vesa_video_mode[findex].cea) &&
+				(vesa_cea_nr ==
+				cea_vesa_video_mode[findex].vesa_cea_nr)) {
+			dev_dbg(&ddev->dev, "cea:%d nr:%d\n", cea, vesa_cea_nr);
+			return cea_vesa_video_mode[findex].video_mode;
+		}
+
+	return NULL;
+}
+
+static u8 ceanr_get(struct mcde_display_device *ddev)
+{
+	int cnt;
+	int cea;
+	int vesa_cea_nr;
+	struct mcde_video_mode *vmode = &ddev->video_mode;
+	struct mcde_video_mode *vmode_try;
+
+	if (!vmode)
+		return 0;
+
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+
+	for (cnt = 0; cnt < ARRAY_SIZE(cea_vesa_video_mode); cnt++) {
+		vmode_try = cea_vesa_video_mode[cnt].video_mode;
+		cea = cea_vesa_video_mode[cnt].cea;
+		vesa_cea_nr = cea_vesa_video_mode[cnt].vesa_cea_nr;
+
+		if (cea && vmode_try->xres == vmode->xres &&
+				vmode_try->yres == vmode->yres &&
+				vmode_try->pixclock == vmode->pixclock &&
+				vmode_try->hbp == vmode->hbp &&
+				vmode_try->hfp == vmode->hfp &&
+				vmode_try->vbp == vmode->vbp &&
+				vmode_try->vfp == vmode->vfp &&
+				vmode_try->interlaced == vmode->interlaced) {
+			dev_dbg(&ddev->dev, "ceanr:%d\n", vesa_cea_nr);
+			return vesa_cea_nr;
+		}
+	}
+
+	return 0;
+}
+
+#define AV8100_MAX_LEVEL 255
+
+static int hdmi_try_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	int index = 0;
+	int match_level = AV8100_MAX_LEVEL;
+	int found_index = -1;
+	struct mcde_video_mode *video_modes_supp;
+	int array_size;
+
+	if (ddev == NULL || video_mode == NULL) {
+		pr_warning("%s:ddev = NULL or video_mode = NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	dev_vdbg(&ddev->dev, "%s\n", __func__);
+
+	if (ddev->port->hdmi_sdtv_switch == SDTV_SWITCH) {
+		video_mode->interlaced = true;
+		video_modes_supp = video_modes_supp_sdtv;
+		array_size = ARRAY_SIZE(video_modes_supp_sdtv);
+	} else {
+		video_modes_supp = video_modes_supp_hdmi;
+		array_size = ARRAY_SIZE(video_modes_supp_hdmi);
+	}
+
+	while (index < array_size) {
+		/* 1. Check if all parameters match */
+		if ((video_mode->xres == video_modes_supp[index].xres) &&
+			(video_mode->yres == video_modes_supp[index].yres) &&
+			((video_mode->xres + video_mode->hbp +
+				video_mode->hfp) ==
+			(video_modes_supp[index].xres +
+				video_modes_supp[index].hbp +
+				video_modes_supp[index].hfp)) &&
+			((video_mode->yres + video_mode->vbp + video_mode->vfp)
+				==
+			(video_modes_supp[index].yres +
+				video_modes_supp[index].vbp +
+				video_modes_supp[index].vfp)) &&
+			(video_mode->pixclock ==
+				video_modes_supp[index].pixclock) &&
+			(video_mode->interlaced ==
+				video_modes_supp[index].interlaced)) {
+					match_level = 1;
+					found_index = index;
+					break;
+		}
+
+		/* 2. Check if xres,yres,htot,vtot,interlaced match */
+		if ((match_level > 2) &&
+			(video_mode->xres == video_modes_supp[index].xres) &&
+			(video_mode->yres == video_modes_supp[index].yres) &&
+			((video_mode->xres + video_mode->hbp +
+				video_mode->hfp) ==
+			(video_modes_supp[index].xres +
+				video_modes_supp[index].hbp +
+				video_modes_supp[index].hfp)) &&
+			((video_mode->yres + video_mode->vbp + video_mode->vfp)
+				==
+			(video_modes_supp[index].yres +
+				video_modes_supp[index].vbp +
+				video_modes_supp[index].vfp)) &&
+			(video_mode->interlaced ==
+				video_modes_supp[index].interlaced)) {
+			match_level = 2;
+			found_index = index;
+		}
+
+		/* 3. Check if xres,yres,pixelclock,interlaced match */
+		if ((match_level > 3) &&
+			(video_mode->xres == video_modes_supp[index].xres) &&
+			(video_mode->yres == video_modes_supp[index].yres) &&
+			(video_mode->interlaced ==
+				video_modes_supp[index].interlaced) &&
+			(video_mode->pixclock ==
+				video_modes_supp[index].pixclock)) {
+			match_level = 3;
+			found_index = index;
+		}
+
+		/* 4. Check if xres,yres,interlaced match */
+		if ((match_level > 4) &&
+			(video_mode->xres == video_modes_supp[index].xres) &&
+			(video_mode->yres == video_modes_supp[index].yres) &&
+			(video_mode->interlaced ==
+				video_modes_supp[index].interlaced)) {
+			match_level = 4;
+			found_index = index;
+		}
+
+		index++;
+	}
+
+	if (found_index == -1) {
+		dev_dbg(&ddev->dev, "video_mode not accepted\n");
+		dev_dbg(&ddev->dev, "xres:%d yres:%d pixclock:%d hbp:%d hfp:%d "
+			"vfp:%d vbp:%d intlcd:%d\n",
+			video_mode->xres, video_mode->yres,
+			video_mode->pixclock,
+			video_mode->hbp, video_mode->hfp,
+			video_mode->vfp, video_mode->vbp,
+			video_mode->interlaced);
+		return -EINVAL;
+	}
+
+	memset(video_mode, 0, sizeof(struct mcde_video_mode));
+	memcpy(video_mode, &video_modes_supp[found_index],
+		sizeof(struct mcde_video_mode));
+
+	dev_dbg(&ddev->dev, "%s:HDMI video_mode %d chosen. Level:%d\n",
+		__func__, found_index, match_level);
+
+	return 0;
+}
+
+static int hdmi_set_video_mode(
+	struct mcde_display_device *dev, struct mcde_video_mode *video_mode)
+{
+	int ret;
+	union av8100_configuration av8100_config;
+	struct mcde_display_hdmi_platform_data *pdata;
+	struct display_driver_data *driver_data;
+	struct av8100_status status;
+
+	/* TODO check video_mode_params */
+	if (dev == NULL || video_mode == NULL) {
+		pr_warning("%s:ddev = NULL or video_mode = NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	pdata = dev->dev.platform_data;
+	driver_data = dev_get_drvdata(&dev->dev);
+
+	dev_dbg(&dev->dev, "%s:\n", __func__);
+	dev_vdbg(&dev->dev, "%s:xres:%d yres:%d hbp:%d hfp:%d vbp:%d vfp:%d "
+		"interlaced:%d\n", __func__,
+		video_mode->xres,
+		video_mode->yres,
+		video_mode->hbp,
+		video_mode->hfp,
+		video_mode->vbp,
+		video_mode->vfp,
+		video_mode->interlaced);
+
+	if (driver_data->update_port_pixel_format) {
+		hdmi_set_pixel_format(dev, dev->pixel_format);
+		driver_data->update_port_pixel_format = false;
+	}
+
+	memset(&(dev->video_mode), 0, sizeof(struct mcde_video_mode));
+	memcpy(&(dev->video_mode), video_mode, sizeof(struct mcde_video_mode));
+
+	if (dev->port->pixel_format == MCDE_PORTPIXFMT_DSI_YCBCR422 &&
+						pdata->rgb_2_yCbCr_transform)
+		mcde_chnl_set_col_convert(dev->chnl_state,
+						pdata->rgb_2_yCbCr_transform,
+						MCDE_CONVERT_RGB_2_YCBCR);
+	mcde_chnl_stop_flow(dev->chnl_state);
+
+	ret = mcde_chnl_set_video_mode(dev->chnl_state, &dev->video_mode);
+	if (ret < 0) {
+		dev_warn(&dev->dev, "Failed to set video mode\n");
+		return ret;
+	}
+
+	status = av8100_status_get();
+	if (status.av8100_state == AV8100_OPMODE_UNDEFINED)
+		return -EINVAL;
+
+	if (av8100_ver_get() == AV8100_CHIPVER_1) {
+		if (status.av8100_state >= AV8100_OPMODE_STANDBY) {
+			/* Disable interrupts */
+			ret = av8100_disable_interrupt();
+			if (ret) {
+				dev_err(&dev->dev,
+					"%s:av8100_disable_interrupt failed\n",
+					__func__);
+				return ret;
+			}
+
+			ret = av8100_powerdown();
+			if (ret) {
+				dev_err(&dev->dev,
+						"av8100_powerdown failed\n");
+				return ret;
+			}
+
+			msleep(10);
+		}
+	}
+
+	/* Set to powerup with interrupts disabled */
+	status = av8100_status_get();
+	if (status.av8100_state < AV8100_OPMODE_STANDBY) {
+		ret = av8100_powerup();
+		if (ret) {
+			dev_err(&dev->dev, "av8100_powerup failed\n");
+			return ret;
+		}
+	}
+
+	if (status.av8100_state <= AV8100_OPMODE_IDLE) {
+		ret = av8100_download_firmware(I2C_INTERFACE);
+		if (ret) {
+			dev_err(&dev->dev, "av8100_download_firmware failed\n");
+			return ret;
+		}
+	}
+
+	if (av8100_disable_interrupt())
+		return -EFAULT;
+
+	/*
+	 * Don't look at dev->port->hdmi_sdtv_switch; it states only which
+	 * one should be started, not which one is currently working
+	 */
+	if (av8100_conf_get(AV8100_COMMAND_HDMI, &av8100_config))
+		return -EFAULT;
+	if (av8100_config.hdmi_format.hdmi_mode == AV8100_HDMI_ON) {
+		/* Set HDMI mode to OFF */
+		av8100_config.hdmi_format.hdmi_mode = AV8100_HDMI_OFF;
+		av8100_config.hdmi_format.dvi_format = AV8100_DVI_CTRL_CTL0;
+		av8100_config.hdmi_format.hdmi_format = AV8100_HDMI;
+		if (av8100_conf_prep(AV8100_COMMAND_HDMI, &av8100_config))
+			return -EFAULT;
+
+		if (av8100_conf_w(AV8100_COMMAND_HDMI, NULL, NULL,
+								I2C_INTERFACE))
+			return -EFAULT;
+	}
+	if (av8100_conf_get(AV8100_COMMAND_DENC, &av8100_config))
+		return -EFAULT;
+	if (av8100_config.denc_format.enable) {
+		/* Turn off DENC */
+		av8100_config.denc_format.enable = 0;
+		if (av8100_conf_prep(AV8100_COMMAND_DENC, &av8100_config))
+			return -EFAULT;
+		if (av8100_conf_w(AV8100_COMMAND_DENC, NULL, NULL,
+								I2C_INTERFACE))
+			return -EFAULT;
+	}
+
+	/* Get current av8100 video output format */
+	ret = av8100_conf_get(AV8100_COMMAND_VIDEO_OUTPUT_FORMAT,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_get "
+			"AV8100_COMMAND_VIDEO_OUTPUT_FORMAT failed\n",
+			__func__);
+		return ret;
+	}
+
+	if (dev->port->hdmi_sdtv_switch == SDTV_SWITCH)
+		av8100_config.video_output_format.video_output_cea_vesa =
+			dev->video_mode.yres == NATIVE_YRES_SDTV ?
+				AV8100_CEA21_22_576I_PAL_50HZ :
+				AV8100_CEA6_7_NTSC_60HZ;
+	else
+		av8100_config.video_output_format.video_output_cea_vesa =
+			av8100_video_output_format_get(
+				dev->video_mode.xres,
+				dev->video_mode.yres,
+				dev->video_mode.xres +
+				dev->video_mode.hbp + dev->video_mode.hfp,
+				dev->video_mode.yres +
+				dev->video_mode.vbp + dev->video_mode.vfp,
+				dev->video_mode.pixclock,
+				dev->video_mode.interlaced);
+
+	if (AV8100_VIDEO_OUTPUT_CEA_VESA_MAX ==
+		av8100_config.video_output_format.video_output_cea_vesa) {
+		dev_err(&dev->dev, "%s:video output format not found "
+			"\n", __func__);
+		return ret;
+	}
+
+	ret = av8100_conf_prep(AV8100_COMMAND_VIDEO_OUTPUT_FORMAT,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_prep "
+			"AV8100_COMMAND_VIDEO_OUTPUT_FORMAT failed\n",
+			__func__);
+		return ret;
+	}
+
+	/* Get current av8100 video input format */
+	ret = av8100_conf_get(AV8100_COMMAND_VIDEO_INPUT_FORMAT,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_get "
+			"AV8100_COMMAND_VIDEO_INPUT_FORMAT failed\n",
+			__func__);
+		return ret;
+	}
+
+	/* Set correct av8100 video input pixel format */
+	switch (dev->port->pixel_format) {
+	case MCDE_PORTPIXFMT_DSI_16BPP:
+	default:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_RGB565;
+		break;
+	case MCDE_PORTPIXFMT_DSI_18BPP:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_RGB666;
+		break;
+	case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_RGB666P;
+		break;
+	case MCDE_PORTPIXFMT_DSI_24BPP:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_RGB888;
+		break;
+	case MCDE_PORTPIXFMT_DSI_YCBCR422:
+		av8100_config.video_input_format.input_pixel_format =
+			AV8100_INPUT_PIX_YCBCR422;
+		break;
+	}
+
+	/*  Set ui_x4 */
+	av8100_config.video_input_format.ui_x4 = dev->port->phy.dsi.ui;
+
+	/* Set TE_config */
+	switch (dev->port->sync_src) {
+	case MCDE_SYNCSRC_TE0:
+		av8100_config.video_input_format.TE_config = AV8100_TE_IT_LINE;
+		break;
+	case MCDE_SYNCSRC_TE1:
+		av8100_config.video_input_format.TE_config = AV8100_TE_GPIO_IT;
+		break;
+	case MCDE_SYNCSRC_TE_POLLING:
+		av8100_config.video_input_format.TE_config =
+			AV8100_TE_DSI_LANE; /* Only on DSI, no interrupts */
+		break;
+	case MCDE_SYNCSRC_OFF:
+	default:
+		av8100_config.video_input_format.TE_config = AV8100_TE_OFF;
+		break;
+	}
+
+	ret = av8100_conf_prep(AV8100_COMMAND_VIDEO_INPUT_FORMAT,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_prep "
+				"AV8100_COMMAND_VIDEO_INPUT_FORMAT failed\n",
+				__func__);
+		return ret;
+	}
+
+	ret = av8100_conf_w(AV8100_COMMAND_VIDEO_INPUT_FORMAT,
+		NULL, NULL, I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_w "
+				"AV8100_COMMAND_VIDEO_INPUT_FORMAT failed\n",
+				__func__);
+		return ret;
+	}
+
+	if (dev->port->hdmi_sdtv_switch == SDTV_SWITCH) {
+		if (dev->port->pixel_format != MCDE_PORTPIXFMT_DSI_YCBCR422)
+			av8100_config.color_transform =
+					AV8100_COLOR_TRANSFORM_RGB_TO_DENC;
+		else
+			av8100_config.color_transform =
+					AV8100_COLOR_TRANSFORM_YUV_TO_DENC;
+	} else if (dev->port->pixel_format == MCDE_PORTPIXFMT_DSI_YCBCR422) {
+		av8100_config.color_transform =
+					AV8100_COLOR_TRANSFORM_YUV_TO_RGB;
+	} else {
+		av8100_config.color_transform =
+					AV8100_COLOR_TRANSFORM_INDENTITY;
+	}
+
+	ret = av8100_conf_prep(
+		AV8100_COMMAND_COLORSPACECONVERSION,
+		&av8100_config);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_configuration_prepare "
+			"AV8100_COMMAND_COLORSPACECONVERSION failed\n",
+			__func__);
+		return ret;
+	}
+
+	ret = av8100_conf_w(
+			AV8100_COMMAND_COLORSPACECONVERSION,
+			NULL, NULL, I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_w "
+			"AV8100_COMMAND_COLORSPACECONVERSION failed\n",
+			__func__);
+		return ret;
+	}
+
+	/* Set video output format */
+	ret = av8100_conf_w(AV8100_COMMAND_VIDEO_OUTPUT_FORMAT,
+		NULL, NULL, I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "av8100_conf_w failed\n");
+		return ret;
+	}
+
+	/* Set audio input format */
+	ret = av8100_conf_w(AV8100_COMMAND_AUDIO_INPUT_FORMAT,
+		NULL, NULL, I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_w "
+				"AV8100_COMMAND_AUDIO_INPUT_FORMAT failed\n",
+			__func__);
+		return ret;
+	}
+
+	dev->update_flags |= UPDATE_FLAG_VIDEO_MODE;
+	dev->first_update = true;
+
+	return 0;
+}
+
+static u16 rotate_byte_left(u8 c, int nr)
+{
+	return (0xff & (c << nr)) | (0xff & (c >> (8 - nr)));
+}
+
+static u16 map_yv(u8 in)
+{
+	return rotate_byte_left(in, 3) << 4;
+}
+
+static u16 map_u(u8 in)
+{
+	return rotate_byte_left(in, 5) << 4;
+}
+
+static int hdmi_set_pixel_format(
+	struct mcde_display_device *ddev, enum mcde_ovly_pix_fmt format)
+{
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+	ddev->pixel_format = format;
+
+	return 0;
+}
+
+static int hdmi_set_port_pixel_format(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	dev_dbg(&ddev->dev, "%s\n", __func__);
+	mcde_chnl_stop_flow(ddev->chnl_state);
+	ret = mcde_chnl_set_pixel_format(ddev->chnl_state,
+						ddev->port->pixel_format);
+
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s: Failed to set pixel format = %d\n",
+					__func__, ddev->port->pixel_format);
+		return ret;
+	}
+
+	if (ddev->port->pixel_format == MCDE_PORTPIXFMT_DSI_YCBCR422 &&
+						av8100_ver_get() == 2) {
+		/* The V2 version has an error for unpacking YUV422 */
+		struct mcde_palette_table palette = {
+			.map_col_ch0 = *map_yv,
+			.map_col_ch1 = *map_u,
+			.map_col_ch2 = *map_yv,
+		};
+		ret = mcde_chnl_set_palette(ddev->chnl_state, &palette);
+	} else {
+		ret = mcde_chnl_set_palette(ddev->chnl_state, NULL);
+	}
+
+	return 0;
+}
+
+static int hdmi_apply_config(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	if (!ddev->update_flags)
+		return 0;
+
+	ret = mcde_chnl_apply(ddev->chnl_state);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to apply to channel\n",
+							__func__);
+		return ret;
+	}
+	ddev->update_flags = 0;
+
+	return 0;
+}
+
+static int hdmi_on_first_update(struct mcde_display_device *dev)
+{
+	int ret;
+	union av8100_configuration av8100_config;
+	u8 *infofr_data;
+	int infofr_crc;
+	int cnt;
+
+	dev->first_update = false;
+
+	/*
+	 * Prepare HDMI configuration
+	 * Avoid simultaneous output of DENC and HDMI/DVI.
+	 * Only one of them should be enabled.
+	 * Note HDMI/DVI and DENC are always turned off in set_video_mode.
+	 */
+	switch (dev->port->hdmi_sdtv_switch) {
+	case SDTV_SWITCH:
+		if (av8100_conf_get(AV8100_COMMAND_DENC, &av8100_config))
+			return -EFAULT;
+		av8100_config.denc_format.enable = 1;
+		if (dev->video_mode.yres == NATIVE_YRES_SDTV) {
+			av8100_config.denc_format.standard_selection =
+					AV8100_PAL_BDGHI;
+			av8100_config.denc_format.cvbs_video_format  =
+					AV8100_CVBS_625;
+		} else {
+			av8100_config.denc_format.standard_selection =
+					AV8100_NTSC_M;
+			av8100_config.denc_format.cvbs_video_format  =
+					AV8100_CVBS_525;
+		}
+		ret = av8100_conf_prep(AV8100_COMMAND_DENC, &av8100_config);
+		break;
+	case DVI_SWITCH:
+		av8100_config.hdmi_format.hdmi_mode = AV8100_HDMI_ON;
+		av8100_config.hdmi_format.hdmi_format = AV8100_DVI;
+		av8100_config.hdmi_format.dvi_format = AV8100_DVI_CTRL_CTL0;
+		ret = av8100_conf_prep(AV8100_COMMAND_HDMI, &av8100_config);
+		break;
+	case HDMI_SWITCH:
+	default:
+		av8100_config.hdmi_format.hdmi_mode = AV8100_HDMI_ON;
+		av8100_config.hdmi_format.hdmi_format = AV8100_HDMI;
+		av8100_config.hdmi_format.dvi_format = AV8100_DVI_CTRL_CTL0;
+		ret = av8100_conf_prep(AV8100_COMMAND_HDMI, &av8100_config);
+		break;
+	}
+
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_prep "
+			"AV8100_COMMAND_HDMI/DENC failed\n", __func__);
+		return ret;
+	}
+
+	/* Enable interrupts */
+	ret = av8100_enable_interrupt();
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_enable_interrupt failed\n",
+			__func__);
+		return ret;
+	}
+
+	if (dev->port->hdmi_sdtv_switch == SDTV_SWITCH)
+		ret = av8100_conf_w(AV8100_COMMAND_DENC, NULL, NULL,
+								I2C_INTERFACE);
+	else
+		ret = av8100_conf_w(AV8100_COMMAND_HDMI, NULL, NULL,
+								I2C_INTERFACE);
+	if (ret) {
+		dev_err(&dev->dev, "%s:av8100_conf_w "
+			"AV8100_COMMAND_HDMI/DENC failed\n", __func__);
+		return ret;
+	}
+
+	/* AVI Infoframe only if HDMI */
+	if (dev->port->hdmi_sdtv_switch != HDMI_SWITCH)
+		goto hdmi_on_first_update_end;
+
+	/* Create AVI Infoframe */
+	av8100_config.infoframes_format.type = AVI_INFOFRAME_TYPE;
+	av8100_config.infoframes_format.version = AVI_INFOFRAME_VERSION;
+	av8100_config.infoframes_format.length = AVI_INFOFRAME_DATA_SIZE;
+
+	/* AVI Infoframe data */
+	infofr_data = &av8100_config.infoframes_format.data[0];
+	memset(infofr_data, 0, AVI_INFOFRAME_DATA_SIZE);
+	infofr_data[0] = AVI_INFOFRAME_DB1;
+	infofr_data[1] = AVI_INFOFRAME_DB2;
+	infofr_data[3] = ceanr_get(dev);
+
+	/* Calculate AVI Infoframe checksum */
+	infofr_crc = av8100_config.infoframes_format.type +
+			av8100_config.infoframes_format.version +
+			av8100_config.infoframes_format.length;
+	for (cnt = 0; cnt < AVI_INFOFRAME_DATA_SIZE; cnt++)
+		infofr_crc += infofr_data[cnt];
+	infofr_crc &= 0xFF;
+	av8100_config.infoframes_format.crc = 0x100 - infofr_crc;
+
+	/* Send AVI Infoframe */
+	if (av8100_conf_prep(AV8100_COMMAND_INFOFRAMES,
+		&av8100_config) != 0) {
+		dev_err(&dev->dev, "av8100_conf_prep FAIL\n");
+		return -EINVAL;
+	}
+
+	if (av8100_conf_w(AV8100_COMMAND_INFOFRAMES,
+		NULL, NULL, I2C_INTERFACE) != 0) {
+		dev_err(&dev->dev, "av8100_conf_w FAIL\n");
+		return -EINVAL;
+	}
+
+hdmi_on_first_update_end:
+	return ret;
+}
+
+static int hdmi_set_power_mode(struct mcde_display_device *ddev,
+	enum mcde_display_power_mode power_mode)
+{
+	struct display_driver_data *driver_data = dev_get_drvdata(&ddev->dev);
+	int ret = 0;
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+					power_mode != MCDE_DISPLAY_PM_OFF) {
+		if (ddev->platform_enable) {
+			ret = ddev->platform_enable(ddev);
+			if (ret)
+				return ret;
+		}
+
+		/*
+		 * the regulator for analog TV out is only enabled here,
+		 * this means that one needs to switch to the OFF state
+		 * to be able to switch from HDMI to CVBS.
+		 */
+		if (ddev->port->hdmi_sdtv_switch == SDTV_SWITCH) {
+			ret = regulator_enable(driver_data->cvbs_regulator);
+			if (ret)
+				return ret;
+			driver_data->cvbs_regulator_enabled = true;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+
+		hdmi_set_port_pixel_format(ddev);
+	}
+	/* STANDBY -> ON */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+				power_mode == MCDE_DISPLAY_PM_ON) {
+
+		ddev->power_mode = MCDE_DISPLAY_PM_ON;
+		goto set_power_and_exit;
+	}
+	/* ON -> STANDBY */
+	else if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+				power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* STANDBY -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+				power_mode == MCDE_DISPLAY_PM_OFF) {
+		memset(&(ddev->video_mode), 0, sizeof(struct mcde_video_mode));
+		ret = av8100_powerscan();
+		if (ret)
+			dev_err(&ddev->dev, "%s:av8100_powerscan failed\n"
+								, __func__);
+		if (ddev->platform_disable) {
+			ret = ddev->platform_disable(ddev);
+			if (ret)
+				return ret;
+		}
+		if (driver_data->cvbs_regulator_enabled) {
+			ret = regulator_disable(driver_data->cvbs_regulator);
+			if (ret)
+				return ret;
+			driver_data->cvbs_regulator_enabled = false;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+set_power_and_exit:
+	mcde_chnl_set_power_mode(ddev->chnl_state, ddev->power_mode);
+
+	return ret;
+}
+
+static int hdmi_set_rotation(struct mcde_display_device *ddev,
+				enum mcde_display_rotation rotation)
+{
+	/* Not possible to rotate HDMI */
+	return 0;
+}
+
+static int __devinit hdmi_probe(struct mcde_display_device *dev)
+{
+	int ret = 0;
+	struct mcde_port *port;
+	struct display_driver_data *driver_data;
+	struct mcde_display_hdmi_platform_data *pdata =
+		dev->dev.platform_data;
+
+	if (pdata == NULL) {
+		dev_err(&dev->dev, "%s:Platform data missing\n", __func__);
+		return -EINVAL;
+	}
+
+	if (dev->port->type != MCDE_PORTTYPE_DSI) {
+		dev_err(&dev->dev, "%s:Invalid port type %d\n",
+			__func__, dev->port->type);
+		return -EINVAL;
+	}
+
+	driver_data = (struct display_driver_data *)
+		kzalloc(sizeof(struct display_driver_data), GFP_KERNEL);
+	if (!driver_data) {
+		dev_err(&dev->dev, "Failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	/* DSI use clock continous mode if AV8100_CHIPVER_1 > 1 */
+	if (av8100_ver_get() > AV8100_CHIPVER_1)
+		dev->port->phy.dsi.clk_cont = true;
+
+	dev->on_first_update = hdmi_on_first_update;
+	dev->try_video_mode = hdmi_try_video_mode;
+	dev->set_video_mode = hdmi_set_video_mode;
+	dev->apply_config = hdmi_apply_config;
+	dev->set_pixel_format = hdmi_set_pixel_format;
+	dev->set_power_mode = hdmi_set_power_mode;
+	dev->set_rotation = hdmi_set_rotation;
+
+	port = dev->port;
+
+	port->phy.dsi.host_eot_gen = true;
+	port->phy.dsi.num_data_lanes = 2;
+	port->phy.dsi.hs_freq = DSI_HS_FREQ_HZ;
+	port->phy.dsi.lp_freq = DSI_LP_FREQ_HZ;
+
+	/* Create sysfs files */
+	if (device_create_file(&dev->dev, &dev_attr_hdmisdtvswitch))
+		dev_info(&dev->dev,
+			"Unable to create hdmisdtvswitch attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_input_pixel_format))
+		dev_info(&dev->dev,
+			"Unable to create input_pixel_format attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_disponoff))
+		dev_info(&dev->dev,
+			"Unable to create disponoff attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_vesacea))
+		dev_info(&dev->dev,
+			"Unable to create ceavesa attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_timing))
+		dev_info(&dev->dev,
+			"Unable to create timing attr\n");
+	if (device_create_file(&dev->dev, &dev_attr_stayalive))
+		dev_info(&dev->dev,
+			"Unable to create stayalive attr\n");
+
+	if (pdata->cvbs_regulator_id) {
+		driver_data->cvbs_regulator = regulator_get(&dev->dev,
+						pdata->cvbs_regulator_id);
+		if (IS_ERR(driver_data->cvbs_regulator)) {
+			ret = PTR_ERR(driver_data->cvbs_regulator);
+			dev_warn(&dev->dev, "%s:Failed to get regulator %s\n",
+				__func__, pdata->cvbs_regulator_id);
+			driver_data->cvbs_regulator = NULL;
+			goto av_regulator_get_failed;
+		}
+	}
+
+	dev_set_drvdata(&dev->dev, driver_data);
+	dev_info(&dev->dev, "HDMI display probed\n");
+
+	return 0;
+
+av_regulator_get_failed:
+	kfree(driver_data);
+	return ret;
+}
+
+static int __devexit hdmi_remove(struct mcde_display_device *dev)
+{
+	struct display_driver_data *driver_data = dev_get_drvdata(&dev->dev);
+	struct mcde_display_hdmi_platform_data *pdata =
+		dev->dev.platform_data;
+
+	/* Remove sysfs files */
+	device_remove_file(&dev->dev, &dev_attr_input_pixel_format);
+	device_remove_file(&dev->dev, &dev_attr_hdmisdtvswitch);
+	device_remove_file(&dev->dev, &dev_attr_disponoff);
+	device_remove_file(&dev->dev, &dev_attr_vesacea);
+	device_remove_file(&dev->dev, &dev_attr_timing);
+	device_remove_file(&dev->dev, &dev_attr_stayalive);
+
+	dev->set_power_mode(dev, MCDE_DISPLAY_PM_OFF);
+
+	if (driver_data->cvbs_regulator)
+		regulator_put(driver_data->cvbs_regulator);
+	kfree(driver_data);
+	if (pdata->hdmi_platform_enable) {
+		if (pdata->regulator)
+			regulator_put(pdata->regulator);
+		if (pdata->reset_gpio) {
+			gpio_direction_input(pdata->reset_gpio);
+			gpio_free(pdata->reset_gpio);
+		}
+	}
+
+	return 0;
+}
+
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+static int hdmi_resume(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	if (ddev->chnl_state == NULL)
+		return 0;
+
+	/* set_power_mode will handle call platform_enable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to resume display\n"
+			, __func__);
+
+	return ret;
+}
+
+static int hdmi_suspend(struct mcde_display_device *ddev, pm_message_t state)
+{
+	int ret;
+
+	if (ddev->chnl_state == NULL)
+		return 0;
+
+	/* set_power_mode will handle call platform_disable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to suspend display\n"
+			, __func__);
+
+	return ret;
+}
+#endif
+
+static struct mcde_display_driver hdmi_driver = {
+	.probe	= hdmi_probe,
+	.remove = hdmi_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+	.suspend = hdmi_suspend,
+	.resume = hdmi_resume,
+#else
+	.suspend = NULL,
+	.resume = NULL,
+#endif
+	.driver = {
+		.name	= "av8100_hdmi",
+	},
+};
+
+/* Module init */
+static int __init mcde_display_hdmi_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&hdmi_driver);
+
+}
+late_initcall(mcde_display_hdmi_init);
+
+static void __exit mcde_display_hdmi_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&hdmi_driver);
+}
+module_exit(mcde_display_hdmi_exit);
+
+MODULE_AUTHOR("Per Persson <per.xb.persson@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson hdmi display driver");
diff --git a/drivers/video/mcde/display-fictive.c b/drivers/video/mcde/display-fictive.c
new file mode 100644
index 00000000000..c7ea1429b9f
--- /dev/null
+++ b/drivers/video/mcde/display-fictive.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * ST-Ericsson MCDE fictive display driver
+ *
+ * Author: Per Persson <per.xb.persson@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+
+#include <video/mcde_display.h>
+
+static int __devinit fictive_probe(struct mcde_display_device *dev)
+{
+	dev->platform_enable = NULL,
+	dev->platform_disable = NULL,
+	dev->set_power_mode = NULL;
+
+	dev_info(&dev->dev, "Fictive display probed\n");
+
+	return 0;
+}
+
+static int __devexit fictive_remove(struct mcde_display_device *dev)
+{
+	return 0;
+}
+
+static struct mcde_display_driver fictive_driver = {
+	.probe	= fictive_probe,
+	.remove = fictive_remove,
+	.driver = {
+		.name	= "mcde_disp_fictive",
+	},
+};
+
+/* Module init */
+static int __init mcde_display_fictive_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&fictive_driver);
+}
+module_init(mcde_display_fictive_init);
+
+static void __exit mcde_display_fictive_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&fictive_driver);
+}
+module_exit(mcde_display_fictive_exit);
+
+MODULE_AUTHOR("Per Persson <per.xb.persson@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE fictive display driver");
diff --git a/drivers/video/mcde/display-generic_dsi.c b/drivers/video/mcde/display-generic_dsi.c
new file mode 100644
index 00000000000..dfbaa4a8765
--- /dev/null
+++ b/drivers/video/mcde/display-generic_dsi.c
@@ -0,0 +1,307 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE generic DCS display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+
+#include <video/mcde_display.h>
+#include <video/mcde_display-generic_dsi.h>
+
+static int generic_platform_enable(struct mcde_display_device *dev)
+{
+	struct mcde_display_generic_platform_data *pdata =
+		dev->dev.platform_data;
+
+	dev_dbg(&dev->dev, "%s: Reset & power on generic display\n", __func__);
+
+	if (pdata->regulator) {
+		if (regulator_enable(pdata->regulator) < 0) {
+			dev_err(&dev->dev, "%s:Failed to enable regulator\n"
+				, __func__);
+			return -EINVAL;
+		}
+	}
+	if (pdata->reset_gpio)
+		gpio_set_value_cansleep(pdata->reset_gpio, pdata->reset_high);
+	mdelay(pdata->reset_delay);
+	if (pdata->reset_gpio)
+		gpio_set_value_cansleep(pdata->reset_gpio, !pdata->reset_high);
+
+	return 0;
+}
+
+static int generic_platform_disable(struct mcde_display_device *dev)
+{
+	struct mcde_display_generic_platform_data *pdata =
+		dev->dev.platform_data;
+
+	dev_dbg(&dev->dev, "%s:Reset & power off generic display\n", __func__);
+
+	if (pdata->regulator) {
+		if (regulator_disable(pdata->regulator) < 0) {
+			dev_err(&dev->dev, "%s:Failed to disable regulator\n"
+				, __func__);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static int generic_set_power_mode(struct mcde_display_device *ddev,
+	enum mcde_display_power_mode power_mode)
+{
+	int ret = 0;
+	struct mcde_display_generic_platform_data *pdata =
+		ddev->dev.platform_data;
+
+	dev_dbg(&ddev->dev, "%s:Set Power mode\n", __func__);
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+		power_mode != MCDE_DISPLAY_PM_OFF) {
+
+		if (ddev->platform_enable) {
+			ret = ddev->platform_enable(ddev);
+			if (ret)
+				return ret;
+		}
+
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* STANDBY -> ON */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+		power_mode == MCDE_DISPLAY_PM_ON) {
+
+		ret = mcde_dsi_dcs_write(ddev->chnl_state,
+		DCS_CMD_EXIT_SLEEP_MODE, NULL, 0);
+		if (ret)
+			return ret;
+
+		msleep(pdata->sleep_out_delay);
+
+		ret = mcde_dsi_dcs_write(ddev->chnl_state,
+			DCS_CMD_SET_DISPLAY_ON, NULL, 0);
+		if (ret)
+			return ret;
+
+		ddev->power_mode = MCDE_DISPLAY_PM_ON;
+		goto set_power_and_exit;
+	}
+	/* ON -> STANDBY */
+	else if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+		power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+		ret = mcde_dsi_dcs_write(ddev->chnl_state,
+			DCS_CMD_SET_DISPLAY_OFF, NULL, 0);
+		if (ret)
+			return ret;
+
+		ret = mcde_dsi_dcs_write(ddev->chnl_state,
+			DCS_CMD_ENTER_SLEEP_MODE, NULL, 0);
+		if (ret)
+			return ret;
+
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* SLEEP -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+		power_mode == MCDE_DISPLAY_PM_OFF) {
+		if (ddev->platform_disable) {
+			ret = ddev->platform_disable(ddev);
+			if (ret)
+				return ret;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+set_power_and_exit:
+	mcde_chnl_set_power_mode(ddev->chnl_state, ddev->power_mode);
+
+	return ret;
+}
+
+static int __devinit generic_probe(struct mcde_display_device *dev)
+{
+	int ret = 0;
+	struct mcde_display_generic_platform_data *pdata =
+		dev->dev.platform_data;
+
+	if (pdata == NULL) {
+		dev_err(&dev->dev, "%s:Platform data missing\n", __func__);
+		return -EINVAL;
+	}
+
+	if (dev->port->type != MCDE_PORTTYPE_DSI) {
+		dev_err(&dev->dev,
+			"%s:Invalid port type %d\n",
+			__func__, dev->port->type);
+		return -EINVAL;
+	}
+
+	if (!dev->platform_enable && !dev->platform_disable) {
+		pdata->generic_platform_enable = true;
+		if (pdata->reset_gpio) {
+			ret = gpio_request(pdata->reset_gpio, NULL);
+			if (ret) {
+				dev_warn(&dev->dev,
+					"%s:Failed to request gpio %d\n",
+					__func__, pdata->reset_gpio);
+				goto gpio_request_failed;
+			}
+			gpio_direction_output(pdata->reset_gpio,
+				!pdata->reset_high);
+		}
+		if (pdata->regulator_id) {
+			pdata->regulator = regulator_get(&dev->dev,
+				pdata->regulator_id);
+			if (IS_ERR(pdata->regulator)) {
+				ret = PTR_ERR(pdata->regulator);
+				dev_warn(&dev->dev,
+					"%s:Failed to get regulator '%s'\n",
+					__func__, pdata->regulator_id);
+				pdata->regulator = NULL;
+				goto regulator_get_failed;
+			}
+
+			if (regulator_set_voltage(pdata->regulator,
+					pdata->min_supply_voltage,
+					pdata->max_supply_voltage) < 0) {
+				int volt;
+
+				dev_warn(&dev->dev,
+					"%s:Failed to set voltage '%s'\n",
+					__func__, pdata->regulator_id);
+				volt = regulator_get_voltage(pdata->regulator);
+				dev_warn(&dev->dev,
+					"Voltage:%d\n", volt);
+			}
+
+			/*
+			* When u-boot has display a startup screen.
+			* U-boot has turned on display power however the
+			* regulator framework does not know about that
+			* This is the case here, the display driver has to
+			* enable the regulator for the display.
+			*/
+			if (dev->power_mode == MCDE_DISPLAY_PM_STANDBY) {
+				ret = regulator_enable(pdata->regulator);
+				if (ret < 0) {
+					dev_err(&dev->dev,
+					"%s:Failed to enable regulator\n"
+					, __func__);
+					goto regulator_enable_failed;
+				}
+			}
+		}
+	}
+
+	dev->platform_enable = generic_platform_enable,
+	dev->platform_disable = generic_platform_disable,
+	dev->set_power_mode = generic_set_power_mode;
+
+	dev_info(&dev->dev, "Generic display probed\n");
+
+	goto out;
+regulator_enable_failed:
+regulator_get_failed:
+	if (pdata->generic_platform_enable && pdata->reset_gpio)
+		gpio_free(pdata->reset_gpio);
+gpio_request_failed:
+out:
+	return ret;
+}
+
+static int __devexit generic_remove(struct mcde_display_device *dev)
+{
+	struct mcde_display_generic_platform_data *pdata =
+		dev->dev.platform_data;
+
+	dev->set_power_mode(dev, MCDE_DISPLAY_PM_OFF);
+
+	if (!pdata->generic_platform_enable)
+		return 0;
+
+	if (pdata->regulator)
+		regulator_put(pdata->regulator);
+	if (pdata->reset_gpio) {
+		gpio_direction_input(pdata->reset_gpio);
+		gpio_free(pdata->reset_gpio);
+	}
+
+	return 0;
+}
+
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+static int generic_resume(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	/* set_power_mode will handle call platform_enable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to resume display\n"
+			, __func__);
+	return ret;
+}
+
+static int generic_suspend(struct mcde_display_device *ddev, pm_message_t state)
+{
+	int ret;
+
+	/* set_power_mode will handle call platform_disable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to suspend display\n"
+			, __func__);
+	return ret;
+}
+#endif
+
+static struct mcde_display_driver generic_driver = {
+	.probe	= generic_probe,
+	.remove = generic_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+	.suspend = generic_suspend,
+	.resume = generic_resume,
+#else
+	.suspend = NULL,
+	.resume = NULL,
+#endif
+	.driver = {
+		.name	= "mcde_disp_generic",
+	},
+};
+
+/* Module init */
+static int __init mcde_display_generic_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&generic_driver);
+}
+module_init(mcde_display_generic_init);
+
+static void __exit mcde_display_generic_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&generic_driver);
+}
+module_exit(mcde_display_generic_exit);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE generic DCS display driver");
diff --git a/drivers/video/mcde/display-samsung_s6d16d0.c b/drivers/video/mcde/display-samsung_s6d16d0.c
new file mode 100644
index 00000000000..900c3f70aa6
--- /dev/null
+++ b/drivers/video/mcde/display-samsung_s6d16d0.c
@@ -0,0 +1,232 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE Samsung S6D16D0 display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+
+#include <video/mcde_display.h>
+
+#define RESET_DURATION_US	10
+#define RESET_DELAY_MS		120
+#define SLEEP_OUT_DELAY_MS	120
+#define IO_REGU			"vdd1"
+#define IO_REGU_MIN		1650000
+#define IO_REGU_MAX		3300000
+
+#define DSI_HS_FREQ_HZ		420160000
+#define DSI_LP_FREQ_HZ		19200000
+
+struct device_info {
+	int reset_gpio;
+	struct mcde_port port;
+	struct regulator *regulator;
+};
+
+static inline struct device_info *get_drvdata(struct mcde_display_device *ddev)
+{
+	return (struct device_info *)dev_get_drvdata(&ddev->dev);
+}
+
+static int power_on(struct mcde_display_device *ddev)
+{
+	struct device_info *di = get_drvdata(ddev);
+
+	dev_dbg(&ddev->dev, "Reset & power on s6d16d0 display\n");
+
+	regulator_enable(di->regulator);
+	gpio_set_value_cansleep(di->reset_gpio, 0);
+	udelay(RESET_DURATION_US);
+	gpio_set_value_cansleep(di->reset_gpio, 1);
+	msleep(RESET_DELAY_MS);
+
+	return 0;
+}
+
+static int power_off(struct mcde_display_device *ddev)
+{
+	struct device_info *di = get_drvdata(ddev);
+
+	dev_dbg(&ddev->dev, "Power off s6d16d0 display\n");
+
+	regulator_disable(di->regulator);
+
+	return 0;
+}
+
+static int display_on(struct mcde_display_device *ddev)
+{
+	int ret;
+	u8 val = 0;
+
+	dev_dbg(&ddev->dev, "Display on s6d16d0\n");
+
+	ret = mcde_dsi_dcs_write(ddev->chnl_state,
+						DCS_CMD_SET_TEAR_ON, &val, 1);
+	if (ret)
+		dev_warn(&ddev->dev,
+			"%s:Failed to enable synchronized update\n", __func__);
+
+	ret = mcde_dsi_dcs_write(ddev->chnl_state, DCS_CMD_EXIT_SLEEP_MODE,
+								NULL, 0);
+	if (ret)
+		return ret;
+	msleep(SLEEP_OUT_DELAY_MS);
+	return mcde_dsi_dcs_write(ddev->chnl_state, DCS_CMD_SET_DISPLAY_ON,
+								NULL, 0);
+}
+
+static int display_off(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	dev_dbg(&ddev->dev, "Display off s6d16d0\n");
+
+	ret = mcde_dsi_dcs_write(ddev->chnl_state, DCS_CMD_SET_DISPLAY_OFF,
+								NULL, 0);
+	if (ret)
+		return ret;
+
+	return mcde_dsi_dcs_write(ddev->chnl_state, DCS_CMD_ENTER_SLEEP_MODE,
+								NULL, 0);
+}
+
+static int set_power_mode(struct mcde_display_device *ddev,
+					enum mcde_display_power_mode power_mode)
+{
+	int ret = 0;
+
+	dev_dbg(&ddev->dev, "Set power mode %d\n", power_mode);
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+					power_mode != MCDE_DISPLAY_PM_OFF) {
+		ret = power_on(ddev);
+		if (ret)
+			return ret;
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* STANDBY -> ON */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+					power_mode == MCDE_DISPLAY_PM_ON) {
+
+		ret = display_on(ddev);
+		if (ret)
+			return ret;
+		ddev->power_mode = MCDE_DISPLAY_PM_ON;
+	}
+	/* ON -> STANDBY */
+	else if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+					power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+
+		ret = display_off(ddev);
+		if (ret)
+			return ret;
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* STANDBY -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+					power_mode == MCDE_DISPLAY_PM_OFF) {
+		ret = power_off(ddev);
+		if (ret)
+			return ret;
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+	return mcde_chnl_set_power_mode(ddev->chnl_state, ddev->power_mode);
+}
+
+static int __devinit samsung_s6d16d0_probe(struct mcde_display_device *ddev)
+{
+	int ret = 0;
+	struct mcde_display_dsi_platform_data *pdata = ddev->dev.platform_data;
+	struct device_info *di;
+
+	if (pdata == NULL || !pdata->reset_gpio) {
+		dev_err(&ddev->dev, "Invalid platform data\n");
+		return -EINVAL;
+	}
+
+	di = kzalloc(sizeof(*di), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+	di->reset_gpio = pdata->reset_gpio;
+	di->port.link = pdata->link;
+	di->port.type = MCDE_PORTTYPE_DSI;
+	di->port.mode = MCDE_PORTMODE_CMD;
+	di->port.pixel_format = MCDE_PORTPIXFMT_DSI_24BPP;
+	di->port.sync_src = ddev->port->sync_src;
+	di->port.frame_trig = ddev->port->frame_trig;
+	di->port.phy.dsi.num_data_lanes = 2;
+	di->port.phy.dsi.host_eot_gen = true;
+	/* TODO: Move UI to mcde_hw.c when clk_get_rate(dsi) is done */
+	di->port.phy.dsi.ui = 9;
+	di->port.phy.dsi.hs_freq = DSI_HS_FREQ_HZ;
+	di->port.phy.dsi.lp_freq = DSI_LP_FREQ_HZ;
+
+	ret = gpio_request(di->reset_gpio, NULL);
+	if (ret)
+		goto gpio_request_failed;
+	gpio_direction_output(di->reset_gpio, 1);
+	di->regulator = regulator_get(&ddev->dev, IO_REGU);
+	if (IS_ERR(di->regulator)) {
+		di->regulator = NULL;
+		goto regulator_get_failed;
+	}
+	ret = regulator_set_voltage(di->regulator, IO_REGU_MIN, IO_REGU_MAX);
+	if (WARN_ON(ret))
+		goto regulator_voltage_failed;
+
+	/* Get in sync with u-boot */
+	if (ddev->power_mode != MCDE_DISPLAY_PM_OFF)
+		(void)regulator_enable(di->regulator);
+
+	ddev->set_power_mode = set_power_mode;
+	ddev->port = &di->port;
+	ddev->native_x_res = 864;
+	ddev->native_y_res = 480;
+	dev_set_drvdata(&ddev->dev, di);
+
+	dev_info(&ddev->dev, "Samsung s6d16d0 display probed\n");
+
+	return 0;
+regulator_voltage_failed:
+	regulator_put(di->regulator);
+regulator_get_failed:
+	gpio_free(di->reset_gpio);
+gpio_request_failed:
+	kfree(di);
+	return ret;
+}
+
+static struct mcde_display_driver samsung_s6d16d0_driver = {
+	.probe	= samsung_s6d16d0_probe,
+	.driver = {
+		.name	= "samsung_s6d16d0",
+	},
+};
+
+static int __init samsung_s6d16d0_init(void)
+{
+	return mcde_display_driver_register(&samsung_s6d16d0_driver);
+}
+module_init(samsung_s6d16d0_init);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE Samsung S6D16D0 display driver");
diff --git a/drivers/video/mcde/display-sony_acx424akp_dsi.c b/drivers/video/mcde/display-sony_acx424akp_dsi.c
new file mode 100644
index 00000000000..867bbb37375
--- /dev/null
+++ b/drivers/video/mcde/display-sony_acx424akp_dsi.c
@@ -0,0 +1,422 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE Sony acx424akp DCS display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+
+#include <linux/regulator/consumer.h>
+
+#include <video/mcde_display.h>
+#include <video/mcde_display-sony_acx424akp_dsi.h>
+
+#define RESET_DELAY_MS		11
+#define RESET_LOW_DELAY_US	20
+#define SLEEP_OUT_DELAY_MS	140
+#define SLEEP_IN_DELAY_MS	85	/* Assume 60 Hz 5 frames */
+#define IO_REGU			"vddi"
+#define IO_REGU_MIN		1600000
+#define IO_REGU_MAX		3300000
+
+#define DSI_HS_FREQ_HZ		420160000
+#define DSI_LP_FREQ_HZ		19200000
+
+struct device_info {
+	int reset_gpio;
+	struct mcde_port port;
+	struct regulator *regulator;
+};
+
+static inline struct device_info *get_drvdata(struct mcde_display_device *ddev)
+{
+	return (struct device_info *)dev_get_drvdata(&ddev->dev);
+}
+
+static int display_read_deviceid(struct mcde_display_device *dev, u16 *id)
+{
+	struct mcde_chnl_state *chnl;
+
+	u8  id1, id2, id3;
+	int len = 1;
+	int ret = 0;
+	int readret = 0;
+
+	dev_dbg(&dev->dev, "%s: Read device id of the display\n", __func__);
+
+	/* Acquire MCDE resources */
+	chnl = mcde_chnl_get(dev->chnl_id, dev->fifo, dev->port);
+	if (IS_ERR(chnl)) {
+		ret = PTR_ERR(chnl);
+		dev_warn(&dev->dev, "Failed to acquire MCDE channel\n");
+		goto out;
+	}
+
+	/* plugnplay: use registers DA, DBh and DCh to detect display */
+	readret = mcde_dsi_dcs_read(chnl, 0xDA, (u32 *)&id1, &len);
+	if (!readret)
+		readret = mcde_dsi_dcs_read(chnl, 0xDB, (u32 *)&id2, &len);
+	if (!readret)
+		readret = mcde_dsi_dcs_read(chnl, 0xDC, (u32 *)&id3, &len);
+
+	if (readret) {
+		dev_info(&dev->dev,
+			"mcde_dsi_dcs_read failed to read display ID\n");
+		goto read_fail;
+	}
+
+	*id = (id3 << 8) | id2;
+read_fail:
+	/* close  MCDE channel */
+	mcde_chnl_put(chnl);
+out:
+	return 0;
+}
+
+static int power_on(struct mcde_display_device *dev)
+{
+	struct device_info *di = get_drvdata(dev);
+
+	dev_dbg(&dev->dev, "%s: Reset & power on sony display\n", __func__);
+
+	regulator_enable(di->regulator);
+	gpio_set_value_cansleep(di->reset_gpio, 1);
+	msleep(RESET_DELAY_MS);
+	gpio_set_value_cansleep(di->reset_gpio, 0);
+	udelay(RESET_LOW_DELAY_US);
+	gpio_set_value_cansleep(di->reset_gpio, 1);
+	msleep(RESET_DELAY_MS);
+
+	return 0;
+}
+
+static int power_off(struct mcde_display_device *dev)
+{
+	struct device_info *di = get_drvdata(dev);
+
+	dev_dbg(&dev->dev, "%s:Reset & power off sony display\n", __func__);
+
+	gpio_set_value_cansleep(di->reset_gpio, 0);
+	msleep(RESET_DELAY_MS);
+	regulator_disable(di->regulator);
+
+	return 0;
+}
+
+static int display_on(struct mcde_display_device *ddev)
+{
+	int ret;
+	u8 val = 0;
+
+	dev_dbg(&ddev->dev, "Display on sony display\n");
+
+	ret = mcde_dsi_dcs_write(ddev->chnl_state,
+						DCS_CMD_SET_TEAR_ON, &val, 1);
+	if (ret)
+		dev_warn(&ddev->dev,
+			"%s:Failed to enable synchronized update\n", __func__);
+
+	ret = mcde_dsi_dcs_write(ddev->chnl_state, DCS_CMD_EXIT_SLEEP_MODE,
+								NULL, 0);
+	if (ret)
+		return ret;
+	msleep(SLEEP_OUT_DELAY_MS);
+	return mcde_dsi_dcs_write(ddev->chnl_state, DCS_CMD_SET_DISPLAY_ON,
+								NULL, 0);
+}
+
+static int display_off(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	dev_dbg(&ddev->dev, "Display off sony display\n");
+
+	ret = mcde_dsi_dcs_write(ddev->chnl_state, DCS_CMD_SET_DISPLAY_OFF,
+								NULL, 0);
+	if (ret)
+		return ret;
+
+	ret = mcde_dsi_dcs_write(ddev->chnl_state, DCS_CMD_ENTER_SLEEP_MODE,
+								NULL, 0);
+	/* Wait for 4 frames or more */
+	msleep(SLEEP_IN_DELAY_MS);
+
+	return ret;
+}
+
+static int sony_acx424akp_set_scan_mode(struct mcde_display_device *ddev,
+		enum mcde_display_power_mode power_mode)
+{
+	int ret = 0;
+	u8 param[MCDE_MAX_DSI_DIRECT_CMD_WRITE];
+
+	dev_dbg(&ddev->dev, "%s:Set Power mode\n", __func__);
+
+	/* 180 rotation for SONY ACX424AKP display */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY) {
+		param[0] = 0xAA;
+		ret = mcde_dsi_dcs_write(ddev->chnl_state, 0xf3, param, 1);
+		if (ret)
+			return ret;
+
+		param[0] = 0x00;
+		param[1] = 0x00;
+		ret = mcde_dsi_generic_write(ddev->chnl_state, param, 3);
+		if (ret)
+			return ret;
+
+		param[0] = 0xC9;
+		param[1] = 0x01;
+		ret = mcde_dsi_generic_write(ddev->chnl_state, param, 3);
+		if (ret)
+			return ret;
+
+		param[0] = 0xA2;
+		param[1] = 0x00;
+		ret = mcde_dsi_generic_write(ddev->chnl_state, param, 3);
+		if (ret)
+			return ret;
+
+		param[0] = 0xFF;
+		param[1] = 0xAA;
+		ret = mcde_dsi_generic_write(ddev->chnl_state, param, 3);
+		if (ret)
+			return ret;
+	}
+	return ret;
+}
+
+static int sony_acx424akp_set_power_mode(struct mcde_display_device *ddev,
+	enum mcde_display_power_mode power_mode)
+{
+	int ret = 0;
+
+	dev_dbg(&ddev->dev, "%s:Set Power mode\n", __func__);
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+					power_mode != MCDE_DISPLAY_PM_OFF) {
+		ret = power_on(ddev);
+		if (ret)
+			return ret;
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* STANDBY -> ON */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+					power_mode == MCDE_DISPLAY_PM_ON) {
+
+		ret = display_on(ddev);
+		if (ret)
+			return ret;
+		ddev->power_mode = MCDE_DISPLAY_PM_ON;
+	}
+	/* ON -> STANDBY */
+	else if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+					power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+
+		ret = display_off(ddev);
+		if (ret)
+			return ret;
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	/* STANDBY -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+					power_mode == MCDE_DISPLAY_PM_OFF) {
+		ret = power_off(ddev);
+		if (ret)
+			return ret;
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+	return mcde_chnl_set_power_mode(ddev->chnl_state, ddev->power_mode);
+}
+
+static int __devinit sony_acx424akp_probe(struct mcde_display_device *dev)
+{
+	int ret = 0;
+	u16 id = 0;
+	struct device_info *di;
+	struct mcde_port *port;
+	struct mcde_display_sony_acx424akp_platform_data *pdata =
+		dev->dev.platform_data;
+
+	if (pdata == NULL || !pdata->reset_gpio) {
+		dev_err(&dev->dev, "Invalid platform data\n");
+		return -EINVAL;
+	}
+
+	di = kzalloc(sizeof(*di), GFP_KERNEL);
+	if (!di)
+		return -ENOMEM;
+
+	port = dev->port;
+	di->reset_gpio = pdata->reset_gpio;
+	di->port.type = MCDE_PORTTYPE_DSI;
+	di->port.mode = MCDE_PORTMODE_CMD;
+	di->port.pixel_format = MCDE_PORTPIXFMT_DSI_24BPP;
+	di->port.sync_src = dev->port->sync_src;
+	di->port.frame_trig = dev->port->frame_trig;
+	di->port.phy.dsi.num_data_lanes = 2;
+	di->port.link = port->link;
+	di->port.phy.dsi.host_eot_gen = true;
+	/* TODO: Move UI to mcde_hw.c when clk_get_rate(dsi) is done */
+	di->port.phy.dsi.ui = 9;
+	di->port.phy.dsi.hs_freq = DSI_HS_FREQ_HZ;
+	di->port.phy.dsi.lp_freq = DSI_LP_FREQ_HZ;
+
+	ret = gpio_request(di->reset_gpio, NULL);
+	if (WARN_ON(ret))
+		goto gpio_request_failed;
+
+	gpio_direction_output(di->reset_gpio, 1);
+	di->regulator = regulator_get(&dev->dev, IO_REGU);
+	if (IS_ERR(di->regulator)) {
+		ret = PTR_ERR(di->regulator);
+		di->regulator = NULL;
+		goto regulator_get_failed;
+	}
+	ret = regulator_set_voltage(di->regulator, IO_REGU_MIN, IO_REGU_MAX);
+	if (WARN_ON(ret))
+		goto regulator_voltage_failed;
+
+	dev->set_power_mode = sony_acx424akp_set_power_mode;
+
+	dev->port = &di->port;
+	dev->native_x_res = 480;
+	dev->native_y_res = 854;
+	dev_set_drvdata(&dev->dev, di);
+
+	/*
+	* When u-boot has display a startup screen.
+	* U-boot has turned on display power however the
+	* regulator framework does not know about that
+	* This is the case here, the display driver has to
+	* enable the regulator for the display.
+	*/
+	if (dev->power_mode != MCDE_DISPLAY_PM_OFF) {
+		(void) regulator_enable(di->regulator);
+	} else {
+		power_on(dev);
+		dev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+	}
+
+	ret = display_read_deviceid(dev, &id);
+	if (ret)
+		goto read_id_failed;
+
+	switch (id) {
+	case DISPLAY_SONY_ACX424AKP:
+	case DISPLAY_SONY_ACX424AKP_ID2:
+		pdata->disp_panel = id;
+		dev_info(&dev->dev,
+			"Sony ACX424AKP display (ID 0x%.4X) probed\n", id);
+		break;
+	default:
+		pdata->disp_panel = DISPLAY_NONE;
+		dev_info(&dev->dev,
+			"Display not recognized (ID 0x%.4X) probed\n", id);
+		goto read_id_failed;
+	}
+
+	return 0;
+
+read_id_failed:
+regulator_voltage_failed:
+	regulator_put(di->regulator);
+regulator_get_failed:
+	gpio_free(di->reset_gpio);
+gpio_request_failed:
+	kfree(di);
+	return ret;
+}
+
+static int __devexit sony_acx424akp_remove(struct mcde_display_device *dev)
+{
+	struct device_info *di = get_drvdata(dev);
+
+	dev->set_power_mode(dev, MCDE_DISPLAY_PM_OFF);
+
+	regulator_put(di->regulator);
+	gpio_direction_input(di->reset_gpio);
+	gpio_free(di->reset_gpio);
+
+	kfree(di);
+
+	return 0;
+}
+
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+static int sony_acx424akp_resume(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	/* set_power_mode will handle call platform_enable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to resume display\n"
+			, __func__);
+	return ret;
+}
+
+static int sony_acx424akp_suspend(struct mcde_display_device *ddev, \
+							pm_message_t state)
+{
+	int ret;
+
+	/* set_power_mode will handle call platform_disable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to suspend display\n"
+			, __func__);
+	return ret;
+}
+#endif
+
+static struct mcde_display_driver sony_acx424akp_driver = {
+	.probe	= sony_acx424akp_probe,
+	.remove = sony_acx424akp_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+	.suspend = sony_acx424akp_suspend,
+	.resume = sony_acx424akp_resume,
+#else
+	.suspend = NULL,
+	.resume = NULL,
+#endif
+	.driver = {
+		.name	= "mcde_disp_sony_acx424akp",
+	},
+};
+
+/* Module init */
+static int __init mcde_display_sony_acx424akp_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&sony_acx424akp_driver);
+}
+module_init(mcde_display_sony_acx424akp_init);
+
+static void __exit mcde_display_sony_acx424akp_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&sony_acx424akp_driver);
+}
+module_exit(mcde_display_sony_acx424akp_exit);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE Sony ACX424AKP DCS display driver");
diff --git a/drivers/video/mcde/display-vuib500-dpi.c b/drivers/video/mcde/display-vuib500-dpi.c
new file mode 100644
index 00000000000..2bd5b990608
--- /dev/null
+++ b/drivers/video/mcde/display-vuib500-dpi.c
@@ -0,0 +1,215 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE DPI display driver
+ * The VUIB500 is an user interface board the can be attached to an HREF. It
+ * supports the DPI pixel interface and converts this to an analog VGA signal,
+ * which can be connected to a monitor using a DSUB connector. The VUIB board
+ * uses an external power supply of 5V.
+ *
+ * Author: Marcel Tunnissen <marcel.tuennissen@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+
+#include <video/mcde_display.h>
+#include <video/mcde_display-vuib500-dpi.h>
+
+#define DPI_DISP_TRACE	dev_dbg(&ddev->dev, "%s\n", __func__)
+
+static int try_video_mode(struct mcde_display_device *ddev,
+				struct mcde_video_mode *video_mode);
+static int set_video_mode(struct mcde_display_device *ddev,
+				struct mcde_video_mode *video_mode);
+
+static int __devinit dpi_display_probe(struct mcde_display_device *ddev)
+{
+	int ret = 0;
+	struct mcde_display_dpi_platform_data *pdata = ddev->dev.platform_data;
+	DPI_DISP_TRACE;
+
+	if (pdata == NULL) {
+		dev_err(&ddev->dev, "%s:Platform data missing\n", __func__);
+		ret = -EINVAL;
+		goto no_pdata;
+	}
+
+	if (ddev->port->type != MCDE_PORTTYPE_DPI) {
+		dev_err(&ddev->dev,
+			"%s:Invalid port type %d\n",
+			__func__, ddev->port->type);
+		ret = -EINVAL;
+		goto invalid_port_type;
+	}
+
+	ddev->try_video_mode = try_video_mode;
+	ddev->set_video_mode = set_video_mode;
+	dev_info(&ddev->dev, "DPI display probed\n");
+
+	goto out;
+invalid_port_type:
+no_pdata:
+out:
+	return ret;
+}
+
+static int __devexit dpi_display_remove(struct mcde_display_device *ddev)
+{
+	DPI_DISP_TRACE;
+
+	ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+
+	return 0;
+}
+
+static int dpi_display_resume(struct mcde_display_device *ddev)
+{
+	int ret;
+	DPI_DISP_TRACE;
+
+	/* set_power_mode will handle call platform_enable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to resume display\n"
+			, __func__);
+	return ret;
+}
+
+static int dpi_display_suspend(struct mcde_display_device *ddev,
+							pm_message_t state)
+{
+	int ret;
+	DPI_DISP_TRACE;
+
+	/* set_power_mode will handle call platform_disable */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+	if (ret < 0)
+		dev_warn(&ddev->dev, "%s:Failed to suspend display\n"
+			, __func__);
+	return ret;
+}
+
+static void print_vmode(struct mcde_video_mode *vmode)
+{
+	pr_debug("resolution: %dx%d\n", vmode->xres, vmode->yres);
+	pr_debug("  pixclock: %d\n",    vmode->pixclock);
+	pr_debug("       hbp: %d\n",    vmode->hbp);
+	pr_debug("       hfp: %d\n",    vmode->hfp);
+	pr_debug("       hsw: %d\n",    vmode->hsw);
+	pr_debug("       vbp: %d\n",    vmode->vbp);
+	pr_debug("       vfp: %d\n",    vmode->vfp);
+	pr_debug("       vsw: %d\n",    vmode->vsw);
+	pr_debug("interlaced: %s\n", vmode->interlaced ? "true" : "false");
+}
+
+/* Taken from the programmed value of the LCD clock in PRCMU */
+#define PIX_CLK_FREQ		25000000
+#define VMODE_XRES		640
+#define VMODE_YRES		480
+
+static int try_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	int res = -EINVAL;
+	DPI_DISP_TRACE;
+
+	if (ddev == NULL || video_mode == NULL) {
+		dev_warn(&ddev->dev, "%s:ddev = NULL or video_mode = NULL\n",
+			__func__);
+		return res;
+	}
+
+	if (video_mode->xres == VMODE_XRES && video_mode->yres == VMODE_YRES) {
+		video_mode->hbp = 40;
+		video_mode->hfp = 8;
+		video_mode->hsw = 96;
+		video_mode->vbp = 25;
+		video_mode->vfp = 2;
+		video_mode->vsw = 2;
+		/*
+		 * The pixclock setting is not used within MCDE. The clock is
+		 * setup elsewhere. But the pixclock value is visible in user
+		 * space.
+		 */
+		video_mode->pixclock =	(int) (1e+12 * (1.0 / PIX_CLK_FREQ));
+		res = 0;
+	} /* TODO: add more supported resolutions here */
+	video_mode->interlaced = false;
+
+	if (res == 0)
+		print_vmode(video_mode);
+	else
+		dev_warn(&ddev->dev,
+			"%s:Failed to find video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+
+	return res;
+
+}
+
+static int set_video_mode(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	int res;
+	DPI_DISP_TRACE;
+
+	if (ddev == NULL || video_mode == NULL) {
+		dev_warn(&ddev->dev, "%s:ddev = NULL or video_mode = NULL\n",
+			__func__);
+		return -EINVAL;
+	}
+	if (video_mode->xres != VMODE_XRES || video_mode->yres != VMODE_YRES) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode x=%d, y=%d\n",
+			__func__, video_mode->xres, video_mode->yres);
+		return -EINVAL;
+	}
+	ddev->video_mode = *video_mode;
+	print_vmode(video_mode);
+
+	res = mcde_chnl_set_video_mode(ddev->chnl_state, &ddev->video_mode);
+	if (res < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode on channel\n",
+			__func__);
+
+	}
+	/* notify mcde display driver about updated video mode */
+	ddev->update_flags |= UPDATE_FLAG_VIDEO_MODE;
+	return res;
+}
+
+static struct mcde_display_driver dpi_display_driver = {
+	.probe	= dpi_display_probe,
+	.remove = dpi_display_remove,
+	.suspend = dpi_display_suspend,
+	.resume = dpi_display_resume,
+	.driver = {
+		.name	= "mcde_display_dpi",
+	},
+};
+
+/* Module init */
+static int __init mcde_dpi_display_init(void)
+{
+	pr_info("%s\n", __func__);
+
+	return mcde_display_driver_register(&dpi_display_driver);
+}
+module_init(mcde_dpi_display_init);
+
+static void __exit mcde_dpi_display_exit(void)
+{
+	pr_info("%s\n", __func__);
+
+	mcde_display_driver_unregister(&dpi_display_driver);
+}
+module_exit(mcde_dpi_display_exit);
+
+MODULE_AUTHOR("Marcel Tunnissen <marcel.tuennissen@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE DPI display driver fro VUIB500 display");
diff --git a/drivers/video/mcde/dsilink_regs.h b/drivers/video/mcde/dsilink_regs.h
new file mode 100644
index 00000000000..29fa75a1752
--- /dev/null
+++ b/drivers/video/mcde/dsilink_regs.h
@@ -0,0 +1,2037 @@
+
+#define DSI_VAL2REG(__reg, __fld, __val) \
+	(((__val) << __reg##_##__fld##_SHIFT) & __reg##_##__fld##_MASK)
+#define DSI_REG2VAL(__reg, __fld, __val) \
+	(((__val) & __reg##_##__fld##_MASK) >> __reg##_##__fld##_SHIFT)
+
+#define DSI_MCTL_INTEGRATION_MODE 0x00000000
+#define DSI_MCTL_INTEGRATION_MODE_INT_MODE_EN_SHIFT 0
+#define DSI_MCTL_INTEGRATION_MODE_INT_MODE_EN_MASK 0x00000001
+#define DSI_MCTL_INTEGRATION_MODE_INT_MODE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_INTEGRATION_MODE, INT_MODE_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL 0x00000004
+#define DSI_MCTL_MAIN_DATA_CTL_LINK_EN_SHIFT 0
+#define DSI_MCTL_MAIN_DATA_CTL_LINK_EN_MASK 0x00000001
+#define DSI_MCTL_MAIN_DATA_CTL_LINK_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, LINK_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_SHIFT 1
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_MASK 0x00000002
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_CMD 0
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_VID 1
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, IF1_MODE, \
+	DSI_MCTL_MAIN_DATA_CTL_IF1_MODE_##__x)
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_MODE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, IF1_MODE, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_VID_EN_SHIFT 2
+#define DSI_MCTL_MAIN_DATA_CTL_VID_EN_MASK 0x00000004
+#define DSI_MCTL_MAIN_DATA_CTL_VID_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, VID_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_TVG_SEL_SHIFT 3
+#define DSI_MCTL_MAIN_DATA_CTL_TVG_SEL_MASK 0x00000008
+#define DSI_MCTL_MAIN_DATA_CTL_TVG_SEL(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, TVG_SEL, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_TBG_SEL_SHIFT 4
+#define DSI_MCTL_MAIN_DATA_CTL_TBG_SEL_MASK 0x00000010
+#define DSI_MCTL_MAIN_DATA_CTL_TBG_SEL(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, TBG_SEL, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_TE_EN_SHIFT 5
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_TE_EN_MASK 0x00000020
+#define DSI_MCTL_MAIN_DATA_CTL_IF1_TE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, IF1_TE_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_IF2_TE_EN_SHIFT 6
+#define DSI_MCTL_MAIN_DATA_CTL_IF2_TE_EN_MASK 0x00000040
+#define DSI_MCTL_MAIN_DATA_CTL_IF2_TE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, IF2_TE_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_REG_TE_EN_SHIFT 7
+#define DSI_MCTL_MAIN_DATA_CTL_REG_TE_EN_MASK 0x00000080
+#define DSI_MCTL_MAIN_DATA_CTL_REG_TE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_READ_EN_SHIFT 8
+#define DSI_MCTL_MAIN_DATA_CTL_READ_EN_MASK 0x00000100
+#define DSI_MCTL_MAIN_DATA_CTL_READ_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, READ_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_BTA_EN_SHIFT 9
+#define DSI_MCTL_MAIN_DATA_CTL_BTA_EN_MASK 0x00000200
+#define DSI_MCTL_MAIN_DATA_CTL_BTA_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, BTA_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_ECC_SHIFT 10
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_ECC_MASK 0x00000400
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_ECC(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DISP_GEN_ECC, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_CHECKSUM_SHIFT 11
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_CHECKSUM_MASK 0x00000800
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_GEN_CHECKSUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DISP_GEN_CHECKSUM, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_HOST_EOT_GEN_SHIFT 12
+#define DSI_MCTL_MAIN_DATA_CTL_HOST_EOT_GEN_MASK 0x00001000
+#define DSI_MCTL_MAIN_DATA_CTL_HOST_EOT_GEN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, HOST_EOT_GEN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_EOT_GEN_SHIFT 13
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_EOT_GEN_MASK 0x00002000
+#define DSI_MCTL_MAIN_DATA_CTL_DISP_EOT_GEN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DISP_EOT_GEN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_DLX_REMAP_EN_SHIFT 14
+#define DSI_MCTL_MAIN_DATA_CTL_DLX_REMAP_EN_MASK 0x00004000
+#define DSI_MCTL_MAIN_DATA_CTL_DLX_REMAP_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, DLX_REMAP_EN, __x)
+#define DSI_MCTL_MAIN_DATA_CTL_TE_POLLING_EN_SHIFT 15
+#define DSI_MCTL_MAIN_DATA_CTL_TE_POLLING_EN_MASK 0x00008000
+#define DSI_MCTL_MAIN_DATA_CTL_TE_POLLING_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_DATA_CTL, TE_POLLING_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL 0x00000008
+#define DSI_MCTL_MAIN_PHY_CTL_LANE2_EN_SHIFT 0
+#define DSI_MCTL_MAIN_PHY_CTL_LANE2_EN_MASK 0x00000001
+#define DSI_MCTL_MAIN_PHY_CTL_LANE2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, LANE2_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_FORCE_STOP_MODE_SHIFT 1
+#define DSI_MCTL_MAIN_PHY_CTL_FORCE_STOP_MODE_MASK 0x00000002
+#define DSI_MCTL_MAIN_PHY_CTL_FORCE_STOP_MODE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, FORCE_STOP_MODE, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS_SHIFT 2
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS_MASK 0x00000004
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, CLK_CONTINUOUS, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_ULPM_EN_SHIFT 3
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_ULPM_EN_MASK 0x00000008
+#define DSI_MCTL_MAIN_PHY_CTL_CLK_ULPM_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, CLK_ULPM_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_DAT1_ULPM_EN_SHIFT 4
+#define DSI_MCTL_MAIN_PHY_CTL_DAT1_ULPM_EN_MASK 0x00000010
+#define DSI_MCTL_MAIN_PHY_CTL_DAT1_ULPM_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, DAT1_ULPM_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_DAT2_ULPM_EN_SHIFT 5
+#define DSI_MCTL_MAIN_PHY_CTL_DAT2_ULPM_EN_MASK 0x00000020
+#define DSI_MCTL_MAIN_PHY_CTL_DAT2_ULPM_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, DAT2_ULPM_EN, __x)
+#define DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME_SHIFT 6
+#define DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME_MASK 0x000003C0
+#define DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_PHY_CTL, WAIT_BURST_TIME, __x)
+#define DSI_MCTL_PLL_CTL 0x0000000C
+#define DSI_MCTL_PLL_CTL_PLL_MULT_SHIFT 0
+#define DSI_MCTL_PLL_CTL_PLL_MULT_MASK 0x000000FF
+#define DSI_MCTL_PLL_CTL_PLL_MULT(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_MULT, __x)
+#define DSI_MCTL_PLL_CTL_PLL_OUT_DIV_SHIFT 8
+#define DSI_MCTL_PLL_CTL_PLL_OUT_DIV_MASK 0x00003F00
+#define DSI_MCTL_PLL_CTL_PLL_OUT_DIV(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_OUT_DIV, __x)
+#define DSI_MCTL_PLL_CTL_PLL_IN_DIV_SHIFT 14
+#define DSI_MCTL_PLL_CTL_PLL_IN_DIV_MASK 0x0001C000
+#define DSI_MCTL_PLL_CTL_PLL_IN_DIV(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_IN_DIV, __x)
+#define DSI_MCTL_PLL_CTL_PLL_SEL_DIV2_SHIFT 17
+#define DSI_MCTL_PLL_CTL_PLL_SEL_DIV2_MASK 0x00020000
+#define DSI_MCTL_PLL_CTL_PLL_SEL_DIV2(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_SEL_DIV2, __x)
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_SHIFT 18
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_MASK 0x00040000
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_INT_PLL 0
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_SYS_PLL 1
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_OUT_SEL, \
+	DSI_MCTL_PLL_CTL_PLL_OUT_SEL_##__x)
+#define DSI_MCTL_PLL_CTL_PLL_OUT_SEL(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_OUT_SEL, __x)
+#define DSI_MCTL_PLL_CTL_PLL_MASTER_SHIFT 31
+#define DSI_MCTL_PLL_CTL_PLL_MASTER_MASK 0x80000000
+#define DSI_MCTL_PLL_CTL_PLL_MASTER(__x) \
+	DSI_VAL2REG(DSI_MCTL_PLL_CTL, PLL_MASTER, __x)
+#define DSI_MCTL_LANE_STS 0x00000010
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_SHIFT 0
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_MASK 0x00000003
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_START 0
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_IDLE 1
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_HS 2
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_ULPM 3
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, CLKLANE_STATE, \
+	DSI_MCTL_LANE_STS_CLKLANE_STATE_##__x)
+#define DSI_MCTL_LANE_STS_CLKLANE_STATE(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, CLKLANE_STATE, __x)
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_SHIFT 2
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_MASK 0x0000001C
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_START 0
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_IDLE 1
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_WRITE 2
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_ULPM 3
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_READ 4
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, DATLANE1_STATE, \
+	DSI_MCTL_LANE_STS_DATLANE1_STATE_##__x)
+#define DSI_MCTL_LANE_STS_DATLANE1_STATE(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, DATLANE1_STATE, __x)
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_SHIFT 5
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_MASK 0x00000060
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_START 0
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_IDLE 1
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_WRITE 2
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_ULPM 3
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE_ENUM(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, DATLANE2_STATE, \
+	DSI_MCTL_LANE_STS_DATLANE2_STATE_##__x)
+#define DSI_MCTL_LANE_STS_DATLANE2_STATE(__x) \
+	DSI_VAL2REG(DSI_MCTL_LANE_STS, DATLANE2_STATE, __x)
+#define DSI_MCTL_DPHY_TIMEOUT 0x00000014
+#define DSI_MCTL_DPHY_TIMEOUT_CLK_DIV_SHIFT 0
+#define DSI_MCTL_DPHY_TIMEOUT_CLK_DIV_MASK 0x0000000F
+#define DSI_MCTL_DPHY_TIMEOUT_CLK_DIV(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_TIMEOUT, CLK_DIV, __x)
+#define DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL_SHIFT 4
+#define DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL_MASK 0x0003FFF0
+#define DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_TIMEOUT, HSTX_TO_VAL, __x)
+#define DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL_SHIFT 18
+#define DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL_MASK 0xFFFC0000
+#define DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_TIMEOUT, LPRX_TO_VAL, __x)
+#define DSI_MCTL_ULPOUT_TIME 0x00000018
+#define DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME_SHIFT 0
+#define DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME_MASK 0x000001FF
+#define DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME(__x) \
+	DSI_VAL2REG(DSI_MCTL_ULPOUT_TIME, CKLANE_ULPOUT_TIME, __x)
+#define DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME_SHIFT 9
+#define DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME_MASK 0x0003FE00
+#define DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME(__x) \
+	DSI_VAL2REG(DSI_MCTL_ULPOUT_TIME, DATA_ULPOUT_TIME, __x)
+#define DSI_MCTL_DPHY_STATIC 0x0000001C
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_CLK_SHIFT 0
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_CLK_MASK 0x00000001
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_CLK(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, SWAP_PINS_CLK, __x)
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_CLK_SHIFT 1
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_CLK_MASK 0x00000002
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_CLK(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, HS_INVERT_CLK, __x)
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT1_SHIFT 2
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT1_MASK 0x00000004
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, SWAP_PINS_DAT1, __x)
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT1_SHIFT 3
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT1_MASK 0x00000008
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, HS_INVERT_DAT1, __x)
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT2_SHIFT 4
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT2_MASK 0x00000010
+#define DSI_MCTL_DPHY_STATIC_SWAP_PINS_DAT2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, SWAP_PINS_DAT2, __x)
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT2_SHIFT 5
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT2_MASK 0x00000020
+#define DSI_MCTL_DPHY_STATIC_HS_INVERT_DAT2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, HS_INVERT_DAT2, __x)
+#define DSI_MCTL_DPHY_STATIC_UI_X4_SHIFT 6
+#define DSI_MCTL_DPHY_STATIC_UI_X4_MASK 0x00000FC0
+#define DSI_MCTL_DPHY_STATIC_UI_X4(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_STATIC, UI_X4, __x)
+#define DSI_MCTL_MAIN_EN 0x00000020
+#define DSI_MCTL_MAIN_EN_PLL_START_SHIFT 0
+#define DSI_MCTL_MAIN_EN_PLL_START_MASK 0x00000001
+#define DSI_MCTL_MAIN_EN_PLL_START(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, PLL_START, __x)
+#define DSI_MCTL_MAIN_EN_CKLANE_EN_SHIFT 3
+#define DSI_MCTL_MAIN_EN_CKLANE_EN_MASK 0x00000008
+#define DSI_MCTL_MAIN_EN_CKLANE_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, CKLANE_EN, __x)
+#define DSI_MCTL_MAIN_EN_DAT1_EN_SHIFT 4
+#define DSI_MCTL_MAIN_EN_DAT1_EN_MASK 0x00000010
+#define DSI_MCTL_MAIN_EN_DAT1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, DAT1_EN, __x)
+#define DSI_MCTL_MAIN_EN_DAT2_EN_SHIFT 5
+#define DSI_MCTL_MAIN_EN_DAT2_EN_MASK 0x00000020
+#define DSI_MCTL_MAIN_EN_DAT2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, DAT2_EN, __x)
+#define DSI_MCTL_MAIN_EN_CLKLANE_ULPM_REQ_SHIFT 6
+#define DSI_MCTL_MAIN_EN_CLKLANE_ULPM_REQ_MASK 0x00000040
+#define DSI_MCTL_MAIN_EN_CLKLANE_ULPM_REQ(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, CLKLANE_ULPM_REQ, __x)
+#define DSI_MCTL_MAIN_EN_DAT1_ULPM_REQ_SHIFT 7
+#define DSI_MCTL_MAIN_EN_DAT1_ULPM_REQ_MASK 0x00000080
+#define DSI_MCTL_MAIN_EN_DAT1_ULPM_REQ(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, DAT1_ULPM_REQ, __x)
+#define DSI_MCTL_MAIN_EN_DAT2_ULPM_REQ_SHIFT 8
+#define DSI_MCTL_MAIN_EN_DAT2_ULPM_REQ_MASK 0x00000100
+#define DSI_MCTL_MAIN_EN_DAT2_ULPM_REQ(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, DAT2_ULPM_REQ, __x)
+#define DSI_MCTL_MAIN_EN_IF1_EN_SHIFT 9
+#define DSI_MCTL_MAIN_EN_IF1_EN_MASK 0x00000200
+#define DSI_MCTL_MAIN_EN_IF1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, IF1_EN, __x)
+#define DSI_MCTL_MAIN_EN_IF2_EN_SHIFT 10
+#define DSI_MCTL_MAIN_EN_IF2_EN_MASK 0x00000400
+#define DSI_MCTL_MAIN_EN_IF2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_EN, IF2_EN, __x)
+#define DSI_MCTL_MAIN_STS 0x00000024
+#define DSI_MCTL_MAIN_STS_PLL_LOCK_SHIFT 0
+#define DSI_MCTL_MAIN_STS_PLL_LOCK_MASK 0x00000001
+#define DSI_MCTL_MAIN_STS_PLL_LOCK(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, PLL_LOCK, __x)
+#define DSI_MCTL_MAIN_STS_CLKLANE_READY_SHIFT 1
+#define DSI_MCTL_MAIN_STS_CLKLANE_READY_MASK 0x00000002
+#define DSI_MCTL_MAIN_STS_CLKLANE_READY(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, CLKLANE_READY, __x)
+#define DSI_MCTL_MAIN_STS_DAT1_READY_SHIFT 2
+#define DSI_MCTL_MAIN_STS_DAT1_READY_MASK 0x00000004
+#define DSI_MCTL_MAIN_STS_DAT1_READY(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, DAT1_READY, __x)
+#define DSI_MCTL_MAIN_STS_DAT2_READY_SHIFT 3
+#define DSI_MCTL_MAIN_STS_DAT2_READY_MASK 0x00000008
+#define DSI_MCTL_MAIN_STS_DAT2_READY(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, DAT2_READY, __x)
+#define DSI_MCTL_MAIN_STS_HSTX_TO_ERR_SHIFT 4
+#define DSI_MCTL_MAIN_STS_HSTX_TO_ERR_MASK 0x00000010
+#define DSI_MCTL_MAIN_STS_HSTX_TO_ERR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, HSTX_TO_ERR, __x)
+#define DSI_MCTL_MAIN_STS_LPRX_TO_ERR_SHIFT 5
+#define DSI_MCTL_MAIN_STS_LPRX_TO_ERR_MASK 0x00000020
+#define DSI_MCTL_MAIN_STS_LPRX_TO_ERR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, LPRX_TO_ERR, __x)
+#define DSI_MCTL_MAIN_STS_CRS_UNTERM_PCK_SHIFT 6
+#define DSI_MCTL_MAIN_STS_CRS_UNTERM_PCK_MASK 0x00000040
+#define DSI_MCTL_MAIN_STS_CRS_UNTERM_PCK(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, CRS_UNTERM_PCK, __x)
+#define DSI_MCTL_MAIN_STS_VRS_UNTERM_PCK_SHIFT 7
+#define DSI_MCTL_MAIN_STS_VRS_UNTERM_PCK_MASK 0x00000080
+#define DSI_MCTL_MAIN_STS_VRS_UNTERM_PCK(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS, VRS_UNTERM_PCK, __x)
+#define DSI_MCTL_DPHY_ERR 0x00000028
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_1_SHIFT 6
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_1_MASK 0x00000040
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_ESC_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_2_SHIFT 7
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_2_MASK 0x00000080
+#define DSI_MCTL_DPHY_ERR_ERR_ESC_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_ESC_2, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_1_SHIFT 8
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_1_MASK 0x00000100
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_SYNCESC_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_2_SHIFT 9
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_2_MASK 0x00000200
+#define DSI_MCTL_DPHY_ERR_ERR_SYNCESC_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_SYNCESC_2, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_1_SHIFT 10
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_1_MASK 0x00000400
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONTROL_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_2_SHIFT 11
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_2_MASK 0x00000800
+#define DSI_MCTL_DPHY_ERR_ERR_CONTROL_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONTROL_2, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_1_SHIFT 12
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_1_MASK 0x00001000
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONT_LP0_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_2_SHIFT 13
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_2_MASK 0x00002000
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP0_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONT_LP0_2, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_1_SHIFT 14
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_1_MASK 0x00004000
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_1(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONT_LP1_1, __x)
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_2_SHIFT 15
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_2_MASK 0x00008000
+#define DSI_MCTL_DPHY_ERR_ERR_CONT_LP1_2(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR, ERR_CONT_LP1_2, __x)
+#define DSI_INT_VID_RDDATA 0x00000030
+#define DSI_INT_VID_RDDATA_IF_DATA_SHIFT 0
+#define DSI_INT_VID_RDDATA_IF_DATA_MASK 0x0000FFFF
+#define DSI_INT_VID_RDDATA_IF_DATA(__x) \
+	DSI_VAL2REG(DSI_INT_VID_RDDATA, IF_DATA, __x)
+#define DSI_INT_VID_RDDATA_IF_VALID_SHIFT 16
+#define DSI_INT_VID_RDDATA_IF_VALID_MASK 0x00010000
+#define DSI_INT_VID_RDDATA_IF_VALID(__x) \
+	DSI_VAL2REG(DSI_INT_VID_RDDATA, IF_VALID, __x)
+#define DSI_INT_VID_RDDATA_IF_START_SHIFT 17
+#define DSI_INT_VID_RDDATA_IF_START_MASK 0x00020000
+#define DSI_INT_VID_RDDATA_IF_START(__x) \
+	DSI_VAL2REG(DSI_INT_VID_RDDATA, IF_START, __x)
+#define DSI_INT_VID_RDDATA_IF_FRAME_SYNC_SHIFT 18
+#define DSI_INT_VID_RDDATA_IF_FRAME_SYNC_MASK 0x00040000
+#define DSI_INT_VID_RDDATA_IF_FRAME_SYNC(__x) \
+	DSI_VAL2REG(DSI_INT_VID_RDDATA, IF_FRAME_SYNC, __x)
+#define DSI_INT_VID_GNT 0x00000034
+#define DSI_INT_VID_GNT_IF_STALL_SHIFT 0
+#define DSI_INT_VID_GNT_IF_STALL_MASK 0x00000001
+#define DSI_INT_VID_GNT_IF_STALL(__x) \
+	DSI_VAL2REG(DSI_INT_VID_GNT, IF_STALL, __x)
+#define DSI_INT_CMD_RDDATA 0x00000038
+#define DSI_INT_CMD_RDDATA_IF_DATA_SHIFT 0
+#define DSI_INT_CMD_RDDATA_IF_DATA_MASK 0x0000FFFF
+#define DSI_INT_CMD_RDDATA_IF_DATA(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_RDDATA, IF_DATA, __x)
+#define DSI_INT_CMD_RDDATA_IF_VALID_SHIFT 16
+#define DSI_INT_CMD_RDDATA_IF_VALID_MASK 0x00010000
+#define DSI_INT_CMD_RDDATA_IF_VALID(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_RDDATA, IF_VALID, __x)
+#define DSI_INT_CMD_RDDATA_IF_START_SHIFT 17
+#define DSI_INT_CMD_RDDATA_IF_START_MASK 0x00020000
+#define DSI_INT_CMD_RDDATA_IF_START(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_RDDATA, IF_START, __x)
+#define DSI_INT_CMD_RDDATA_IF_FRAME_SYNC_SHIFT 18
+#define DSI_INT_CMD_RDDATA_IF_FRAME_SYNC_MASK 0x00040000
+#define DSI_INT_CMD_RDDATA_IF_FRAME_SYNC(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_RDDATA, IF_FRAME_SYNC, __x)
+#define DSI_INT_CMD_GNT 0x0000003C
+#define DSI_INT_CMD_GNT_IF_STALL_SHIFT 0
+#define DSI_INT_CMD_GNT_IF_STALL_MASK 0x00000001
+#define DSI_INT_CMD_GNT_IF_STALL(__x) \
+	DSI_VAL2REG(DSI_INT_CMD_GNT, IF_STALL, __x)
+#define DSI_INT_INTERRUPT_CTL 0x00000040
+#define DSI_INT_INTERRUPT_CTL_INT_VAL_SHIFT 0
+#define DSI_INT_INTERRUPT_CTL_INT_VAL_MASK 0x00000001
+#define DSI_INT_INTERRUPT_CTL_INT_VAL(__x) \
+	DSI_VAL2REG(DSI_INT_INTERRUPT_CTL, INT_VAL, __x)
+#define DSI_CMD_MODE_CTL 0x00000050
+#define DSI_CMD_MODE_CTL_IF1_ID_SHIFT 0
+#define DSI_CMD_MODE_CTL_IF1_ID_MASK 0x00000003
+#define DSI_CMD_MODE_CTL_IF1_ID(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, IF1_ID, __x)
+#define DSI_CMD_MODE_CTL_IF2_ID_SHIFT 2
+#define DSI_CMD_MODE_CTL_IF2_ID_MASK 0x0000000C
+#define DSI_CMD_MODE_CTL_IF2_ID(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, IF2_ID, __x)
+#define DSI_CMD_MODE_CTL_IF1_LP_EN_SHIFT 4
+#define DSI_CMD_MODE_CTL_IF1_LP_EN_MASK 0x00000010
+#define DSI_CMD_MODE_CTL_IF1_LP_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, IF1_LP_EN, __x)
+#define DSI_CMD_MODE_CTL_IF2_LP_EN_SHIFT 5
+#define DSI_CMD_MODE_CTL_IF2_LP_EN_MASK 0x00000020
+#define DSI_CMD_MODE_CTL_IF2_LP_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, IF2_LP_EN, __x)
+#define DSI_CMD_MODE_CTL_ARB_MODE_SHIFT 6
+#define DSI_CMD_MODE_CTL_ARB_MODE_MASK 0x00000040
+#define DSI_CMD_MODE_CTL_ARB_MODE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, ARB_MODE, __x)
+#define DSI_CMD_MODE_CTL_ARB_PRI_SHIFT 7
+#define DSI_CMD_MODE_CTL_ARB_PRI_MASK 0x00000080
+#define DSI_CMD_MODE_CTL_ARB_PRI(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, ARB_PRI, __x)
+#define DSI_CMD_MODE_CTL_FIL_VALUE_SHIFT 8
+#define DSI_CMD_MODE_CTL_FIL_VALUE_MASK 0x0000FF00
+#define DSI_CMD_MODE_CTL_FIL_VALUE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, FIL_VALUE, __x)
+#define DSI_CMD_MODE_CTL_TE_TIMEOUT_SHIFT 16
+#define DSI_CMD_MODE_CTL_TE_TIMEOUT_MASK 0x03FF0000
+#define DSI_CMD_MODE_CTL_TE_TIMEOUT(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_CTL, TE_TIMEOUT, __x)
+#define DSI_CMD_MODE_STS 0x00000054
+#define DSI_CMD_MODE_STS_ERR_NO_TE_SHIFT 0
+#define DSI_CMD_MODE_STS_ERR_NO_TE_MASK 0x00000001
+#define DSI_CMD_MODE_STS_ERR_NO_TE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_NO_TE, __x)
+#define DSI_CMD_MODE_STS_ERR_TE_MISS_SHIFT 1
+#define DSI_CMD_MODE_STS_ERR_TE_MISS_MASK 0x00000002
+#define DSI_CMD_MODE_STS_ERR_TE_MISS(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_TE_MISS, __x)
+#define DSI_CMD_MODE_STS_ERR_SDI1_UNDERRUN_SHIFT 2
+#define DSI_CMD_MODE_STS_ERR_SDI1_UNDERRUN_MASK 0x00000004
+#define DSI_CMD_MODE_STS_ERR_SDI1_UNDERRUN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_SDI1_UNDERRUN, __x)
+#define DSI_CMD_MODE_STS_ERR_SDI2_UNDERRUN_SHIFT 3
+#define DSI_CMD_MODE_STS_ERR_SDI2_UNDERRUN_MASK 0x00000008
+#define DSI_CMD_MODE_STS_ERR_SDI2_UNDERRUN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_SDI2_UNDERRUN, __x)
+#define DSI_CMD_MODE_STS_ERR_UNWANTED_RD_SHIFT 4
+#define DSI_CMD_MODE_STS_ERR_UNWANTED_RD_MASK 0x00000010
+#define DSI_CMD_MODE_STS_ERR_UNWANTED_RD(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, ERR_UNWANTED_RD, __x)
+#define DSI_CMD_MODE_STS_CSM_RUNNING_SHIFT 5
+#define DSI_CMD_MODE_STS_CSM_RUNNING_MASK 0x00000020
+#define DSI_CMD_MODE_STS_CSM_RUNNING(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS, CSM_RUNNING, __x)
+#define DSI_DIRECT_CMD_SEND 0x00000060
+#define DSI_DIRECT_CMD_SEND_START_SHIFT 0
+#define DSI_DIRECT_CMD_SEND_START_MASK 0xFFFFFFFF
+#define DSI_DIRECT_CMD_SEND_START(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_SEND, START, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS 0x00000064
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_SHIFT 0
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_MASK 0x00000007
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_WRITE 0
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_READ 1
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_TE_REQ 4
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_TRIG_REQ 5
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_BTA_REQ 6
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_NAT, \
+	DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_##__x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_NAT, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT_SHIFT 3
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT_MASK 0x00000008
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_LONGNOTSHORT, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_SHIFT 8
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_MASK 0x00003F00
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_GENERIC_SHORT_WRITE_0 3
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_GENERIC_SHORT_WRITE_1 19
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_GENERIC_SHORT_WRITE_2 35
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_GENERIC_LONG_WRITE 41
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_DCS_SHORT_WRITE_0 5
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_DCS_SHORT_WRITE_1 21
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_DCS_LONG_WRITE 57
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_DCS_READ 6
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_SET_MAX_PKT_SIZE 55
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_HEAD, \
+	DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_##__x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_HEAD, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID_SHIFT 14
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID_MASK 0x0000C000
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_ID, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE_SHIFT 16
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE_MASK 0x001F0000
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_SIZE, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN_SHIFT 21
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN_MASK 0x00200000
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, CMD_LP_EN, __x)
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_TRIGGER_VAL_SHIFT 24
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_TRIGGER_VAL_MASK 0x0F000000
+#define DSI_DIRECT_CMD_MAIN_SETTINGS_TRIGGER_VAL(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_MAIN_SETTINGS, TRIGGER_VAL, __x)
+#define DSI_DIRECT_CMD_STS 0x00000068
+#define DSI_DIRECT_CMD_STS_CMD_TRANSMISSION_SHIFT 0
+#define DSI_DIRECT_CMD_STS_CMD_TRANSMISSION_MASK 0x00000001
+#define DSI_DIRECT_CMD_STS_CMD_TRANSMISSION(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, CMD_TRANSMISSION, __x)
+#define DSI_DIRECT_CMD_STS_WRITE_COMPLETED_SHIFT 1
+#define DSI_DIRECT_CMD_STS_WRITE_COMPLETED_MASK 0x00000002
+#define DSI_DIRECT_CMD_STS_WRITE_COMPLETED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, WRITE_COMPLETED, __x)
+#define DSI_DIRECT_CMD_STS_TRIGGER_COMPLETED_SHIFT 2
+#define DSI_DIRECT_CMD_STS_TRIGGER_COMPLETED_MASK 0x00000004
+#define DSI_DIRECT_CMD_STS_TRIGGER_COMPLETED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, TRIGGER_COMPLETED, __x)
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_SHIFT 3
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_MASK 0x00000008
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, READ_COMPLETED, __x)
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_RECEIVED_SHIFT 4
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_RECEIVED_MASK 0x00000010
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_RECEIVED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, ACKNOWLEDGE_RECEIVED, __x)
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_WITH_ERR_RECEIVED_SHIFT 5
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_WITH_ERR_RECEIVED_MASK 0x00000020
+#define DSI_DIRECT_CMD_STS_ACKNOWLEDGE_WITH_ERR_RECEIVED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, ACKNOWLEDGE_WITH_ERR_RECEIVED, __x)
+#define DSI_DIRECT_CMD_STS_TRIGGER_RECEIVED_SHIFT 6
+#define DSI_DIRECT_CMD_STS_TRIGGER_RECEIVED_MASK 0x00000040
+#define DSI_DIRECT_CMD_STS_TRIGGER_RECEIVED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, TRIGGER_RECEIVED, __x)
+#define DSI_DIRECT_CMD_STS_TE_RECEIVED_SHIFT 7
+#define DSI_DIRECT_CMD_STS_TE_RECEIVED_MASK 0x00000080
+#define DSI_DIRECT_CMD_STS_TE_RECEIVED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, TE_RECEIVED, __x)
+#define DSI_DIRECT_CMD_STS_BTA_COMPLETED_SHIFT 8
+#define DSI_DIRECT_CMD_STS_BTA_COMPLETED_MASK 0x00000100
+#define DSI_DIRECT_CMD_STS_BTA_COMPLETED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, BTA_COMPLETED, __x)
+#define DSI_DIRECT_CMD_STS_BTA_FINISHED_SHIFT 9
+#define DSI_DIRECT_CMD_STS_BTA_FINISHED_MASK 0x00000200
+#define DSI_DIRECT_CMD_STS_BTA_FINISHED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, BTA_FINISHED, __x)
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_WITH_ERR_SHIFT 10
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_WITH_ERR_MASK 0x00000400
+#define DSI_DIRECT_CMD_STS_READ_COMPLETED_WITH_ERR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, READ_COMPLETED_WITH_ERR, __x)
+#define DSI_DIRECT_CMD_STS_TRIGGER_VAL_SHIFT 11
+#define DSI_DIRECT_CMD_STS_TRIGGER_VAL_MASK 0x00007800
+#define DSI_DIRECT_CMD_STS_TRIGGER_VAL(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, TRIGGER_VAL, __x)
+#define DSI_DIRECT_CMD_STS_ACK_VAL_SHIFT 16
+#define DSI_DIRECT_CMD_STS_ACK_VAL_MASK 0xFFFF0000
+#define DSI_DIRECT_CMD_STS_ACK_VAL(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS, ACK_VAL, __x)
+#define DSI_DIRECT_CMD_RD_INIT 0x0000006C
+#define DSI_DIRECT_CMD_RD_INIT_RESET_SHIFT 0
+#define DSI_DIRECT_CMD_RD_INIT_RESET_MASK 0xFFFFFFFF
+#define DSI_DIRECT_CMD_RD_INIT_RESET(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_INIT, RESET, __x)
+#define DSI_DIRECT_CMD_WRDAT0 0x00000070
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT0_SHIFT 0
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT0_MASK 0x000000FF
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT0(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT0, WRDAT0, __x)
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT1_SHIFT 8
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT1_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT1(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT0, WRDAT1, __x)
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT2_SHIFT 16
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT2_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT2(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT0, WRDAT2, __x)
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT3_SHIFT 24
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT3_MASK 0xFF000000
+#define DSI_DIRECT_CMD_WRDAT0_WRDAT3(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT0, WRDAT3, __x)
+#define DSI_DIRECT_CMD_WRDAT1 0x00000074
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT4_SHIFT 0
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT4_MASK 0x000000FF
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT4(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT1, WRDAT4, __x)
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT5_SHIFT 8
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT5_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT5(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT1, WRDAT5, __x)
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT6_SHIFT 16
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT6_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT6(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT1, WRDAT6, __x)
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT7_SHIFT 24
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT7_MASK 0xFF000000
+#define DSI_DIRECT_CMD_WRDAT1_WRDAT7(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT1, WRDAT7, __x)
+#define DSI_DIRECT_CMD_WRDAT2 0x00000078
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT8_SHIFT 0
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT8_MASK 0x000000FF
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT8(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT2, WRDAT8, __x)
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT9_SHIFT 8
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT9_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT9(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT2, WRDAT9, __x)
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT10_SHIFT 16
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT10_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT10(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT2, WRDAT10, __x)
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT11_SHIFT 24
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT11_MASK 0xFF000000
+#define DSI_DIRECT_CMD_WRDAT2_WRDAT11(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT2, WRDAT11, __x)
+#define DSI_DIRECT_CMD_WRDAT3 0x0000007C
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT12_SHIFT 0
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT12_MASK 0x000000FF
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT12(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT3, WRDAT12, __x)
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT13_SHIFT 8
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT13_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT13(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT3, WRDAT13, __x)
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT14_SHIFT 16
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT14_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT14(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT3, WRDAT14, __x)
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT15_SHIFT 24
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT15_MASK 0xFF000000
+#define DSI_DIRECT_CMD_WRDAT3_WRDAT15(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_WRDAT3, WRDAT15, __x)
+#define DSI_DIRECT_CMD_RDDAT 0x00000080
+#define DSI_DIRECT_CMD_RDDAT_RDDAT0_SHIFT 0
+#define DSI_DIRECT_CMD_RDDAT_RDDAT0_MASK 0x000000FF
+#define DSI_DIRECT_CMD_RDDAT_RDDAT0(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RDDAT, RDDAT0, __x)
+#define DSI_DIRECT_CMD_RDDAT_RDDAT1_SHIFT 8
+#define DSI_DIRECT_CMD_RDDAT_RDDAT1_MASK 0x0000FF00
+#define DSI_DIRECT_CMD_RDDAT_RDDAT1(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RDDAT, RDDAT1, __x)
+#define DSI_DIRECT_CMD_RDDAT_RDDAT2_SHIFT 16
+#define DSI_DIRECT_CMD_RDDAT_RDDAT2_MASK 0x00FF0000
+#define DSI_DIRECT_CMD_RDDAT_RDDAT2(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RDDAT, RDDAT2, __x)
+#define DSI_DIRECT_CMD_RDDAT_RDDAT3_SHIFT 24
+#define DSI_DIRECT_CMD_RDDAT_RDDAT3_MASK 0xFF000000
+#define DSI_DIRECT_CMD_RDDAT_RDDAT3(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RDDAT, RDDAT3, __x)
+#define DSI_DIRECT_CMD_RD_PROPERTY 0x00000084
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_SIZE_SHIFT 0
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_SIZE_MASK 0x0000FFFF
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_SIZE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_PROPERTY, RD_SIZE, __x)
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_ID_SHIFT 16
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_ID_MASK 0x00030000
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_ID(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_PROPERTY, RD_ID, __x)
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_DCSNOTGENERIC_SHIFT 18
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_DCSNOTGENERIC_MASK 0x00040000
+#define DSI_DIRECT_CMD_RD_PROPERTY_RD_DCSNOTGENERIC(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_PROPERTY, RD_DCSNOTGENERIC, __x)
+#define DSI_DIRECT_CMD_RD_STS 0x00000088
+#define DSI_DIRECT_CMD_RD_STS_ERR_FIXED_SHIFT 0
+#define DSI_DIRECT_CMD_RD_STS_ERR_FIXED_MASK 0x00000001
+#define DSI_DIRECT_CMD_RD_STS_ERR_FIXED(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_FIXED, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNCORRECTABLE_SHIFT 1
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNCORRECTABLE_MASK 0x00000002
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNCORRECTABLE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_UNCORRECTABLE, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_CHECKSUM_SHIFT 2
+#define DSI_DIRECT_CMD_RD_STS_ERR_CHECKSUM_MASK 0x00000004
+#define DSI_DIRECT_CMD_RD_STS_ERR_CHECKSUM(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_CHECKSUM, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNDECODABLE_SHIFT 3
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNDECODABLE_MASK 0x00000008
+#define DSI_DIRECT_CMD_RD_STS_ERR_UNDECODABLE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_UNDECODABLE, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_RECEIVE_SHIFT 4
+#define DSI_DIRECT_CMD_RD_STS_ERR_RECEIVE_MASK 0x00000010
+#define DSI_DIRECT_CMD_RD_STS_ERR_RECEIVE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_RECEIVE, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_OVERSIZE_SHIFT 5
+#define DSI_DIRECT_CMD_RD_STS_ERR_OVERSIZE_MASK 0x00000020
+#define DSI_DIRECT_CMD_RD_STS_ERR_OVERSIZE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_OVERSIZE, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_WRONG_LENGTH_SHIFT 6
+#define DSI_DIRECT_CMD_RD_STS_ERR_WRONG_LENGTH_MASK 0x00000040
+#define DSI_DIRECT_CMD_RD_STS_ERR_WRONG_LENGTH(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_WRONG_LENGTH, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_MISSING_EOT_SHIFT 7
+#define DSI_DIRECT_CMD_RD_STS_ERR_MISSING_EOT_MASK 0x00000080
+#define DSI_DIRECT_CMD_RD_STS_ERR_MISSING_EOT(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_MISSING_EOT, __x)
+#define DSI_DIRECT_CMD_RD_STS_ERR_EOT_WITH_ERR_SHIFT 8
+#define DSI_DIRECT_CMD_RD_STS_ERR_EOT_WITH_ERR_MASK 0x00000100
+#define DSI_DIRECT_CMD_RD_STS_ERR_EOT_WITH_ERR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS, ERR_EOT_WITH_ERR, __x)
+#define DSI_VID_MAIN_CTL 0x00000090
+#define DSI_VID_MAIN_CTL_START_MODE_SHIFT 0
+#define DSI_VID_MAIN_CTL_START_MODE_MASK 0x00000003
+#define DSI_VID_MAIN_CTL_START_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, START_MODE, __x)
+#define DSI_VID_MAIN_CTL_STOP_MODE_SHIFT 2
+#define DSI_VID_MAIN_CTL_STOP_MODE_MASK 0x0000000C
+#define DSI_VID_MAIN_CTL_STOP_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, STOP_MODE, __x)
+#define DSI_VID_MAIN_CTL_VID_ID_SHIFT 4
+#define DSI_VID_MAIN_CTL_VID_ID_MASK 0x00000030
+#define DSI_VID_MAIN_CTL_VID_ID(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, VID_ID, __x)
+#define DSI_VID_MAIN_CTL_HEADER_SHIFT 6
+#define DSI_VID_MAIN_CTL_HEADER_MASK 0x00000FC0
+#define DSI_VID_MAIN_CTL_HEADER(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, HEADER, __x)
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_SHIFT 12
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_MASK 0x00003000
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_16BITS 0
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_18BITS 1
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_18BITS_LOOSE 2
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_24BITS 3
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, VID_PIXEL_MODE, \
+	DSI_VID_MAIN_CTL_VID_PIXEL_MODE_##__x)
+#define DSI_VID_MAIN_CTL_VID_PIXEL_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, VID_PIXEL_MODE, __x)
+#define DSI_VID_MAIN_CTL_BURST_MODE_SHIFT 14
+#define DSI_VID_MAIN_CTL_BURST_MODE_MASK 0x00004000
+#define DSI_VID_MAIN_CTL_BURST_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, BURST_MODE, __x)
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_ACTIVE_SHIFT 15
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_ACTIVE_MASK 0x00008000
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_ACTIVE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, SYNC_PULSE_ACTIVE, __x)
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_HORIZONTAL_SHIFT 16
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_HORIZONTAL_MASK 0x00010000
+#define DSI_VID_MAIN_CTL_SYNC_PULSE_HORIZONTAL(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, SYNC_PULSE_HORIZONTAL, __x)
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_SHIFT 17
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_MASK 0x00060000
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_NULL 0
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_BLANKING 1
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_LP_0 2
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_LP_1 3
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, REG_BLKLINE_MODE, \
+	DSI_VID_MAIN_CTL_REG_BLKLINE_MODE_##__x)
+#define DSI_VID_MAIN_CTL_REG_BLKLINE_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, REG_BLKLINE_MODE, __x)
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_SHIFT 19
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_MASK 0x00180000
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_NULL 0
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_BLANKING 1
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_LP_0 2
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_LP_1 3
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, REG_BLKEOL_MODE, \
+	DSI_VID_MAIN_CTL_REG_BLKEOL_MODE_##__x)
+#define DSI_VID_MAIN_CTL_REG_BLKEOL_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, REG_BLKEOL_MODE, __x)
+#define DSI_VID_MAIN_CTL_RECOVERY_MODE_SHIFT 21
+#define DSI_VID_MAIN_CTL_RECOVERY_MODE_MASK 0x00600000
+#define DSI_VID_MAIN_CTL_RECOVERY_MODE(__x) \
+	DSI_VAL2REG(DSI_VID_MAIN_CTL, RECOVERY_MODE, __x)
+#define DSI_VID_VSIZE 0x00000094
+#define DSI_VID_VSIZE_VSA_LENGTH_SHIFT 0
+#define DSI_VID_VSIZE_VSA_LENGTH_MASK 0x0000003F
+#define DSI_VID_VSIZE_VSA_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_VSIZE, VSA_LENGTH, __x)
+#define DSI_VID_VSIZE_VBP_LENGTH_SHIFT 6
+#define DSI_VID_VSIZE_VBP_LENGTH_MASK 0x00000FC0
+#define DSI_VID_VSIZE_VBP_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_VSIZE, VBP_LENGTH, __x)
+#define DSI_VID_VSIZE_VFP_LENGTH_SHIFT 12
+#define DSI_VID_VSIZE_VFP_LENGTH_MASK 0x000FF000
+#define DSI_VID_VSIZE_VFP_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_VSIZE, VFP_LENGTH, __x)
+#define DSI_VID_VSIZE_VACT_LENGTH_SHIFT 20
+#define DSI_VID_VSIZE_VACT_LENGTH_MASK 0x7FF00000
+#define DSI_VID_VSIZE_VACT_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_VSIZE, VACT_LENGTH, __x)
+#define DSI_VID_HSIZE1 0x00000098
+#define DSI_VID_HSIZE1_HSA_LENGTH_SHIFT 0
+#define DSI_VID_HSIZE1_HSA_LENGTH_MASK 0x000003FF
+#define DSI_VID_HSIZE1_HSA_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_HSIZE1, HSA_LENGTH, __x)
+#define DSI_VID_HSIZE1_HBP_LENGTH_SHIFT 10
+#define DSI_VID_HSIZE1_HBP_LENGTH_MASK 0x000FFC00
+#define DSI_VID_HSIZE1_HBP_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_HSIZE1, HBP_LENGTH, __x)
+#define DSI_VID_HSIZE1_HFP_LENGTH_SHIFT 20
+#define DSI_VID_HSIZE1_HFP_LENGTH_MASK 0x7FF00000
+#define DSI_VID_HSIZE1_HFP_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_HSIZE1, HFP_LENGTH, __x)
+#define DSI_VID_HSIZE2 0x0000009C
+#define DSI_VID_HSIZE2_RGB_SIZE_SHIFT 0
+#define DSI_VID_HSIZE2_RGB_SIZE_MASK 0x00001FFF
+#define DSI_VID_HSIZE2_RGB_SIZE(__x) \
+	DSI_VAL2REG(DSI_VID_HSIZE2, RGB_SIZE, __x)
+#define DSI_VID_BLKSIZE1 0x000000A0
+#define DSI_VID_BLKSIZE1_BLKLINE_EVENT_PCK_SHIFT 0
+#define DSI_VID_BLKSIZE1_BLKLINE_EVENT_PCK_MASK 0x00001FFF
+#define DSI_VID_BLKSIZE1_BLKLINE_EVENT_PCK(__x) \
+	DSI_VAL2REG(DSI_VID_BLKSIZE1, BLKLINE_EVENT_PCK, __x)
+#define DSI_VID_BLKSIZE1_BLKEOL_PCK_SHIFT 13
+#define DSI_VID_BLKSIZE1_BLKEOL_PCK_MASK 0x03FFE000
+#define DSI_VID_BLKSIZE1_BLKEOL_PCK(__x) \
+	DSI_VAL2REG(DSI_VID_BLKSIZE1, BLKEOL_PCK, __x)
+#define DSI_VID_BLKSIZE2 0x000000A4
+#define DSI_VID_BLKSIZE2_BLKLINE_PULSE_PCK_SHIFT 0
+#define DSI_VID_BLKSIZE2_BLKLINE_PULSE_PCK_MASK 0x00001FFF
+#define DSI_VID_BLKSIZE2_BLKLINE_PULSE_PCK(__x) \
+	DSI_VAL2REG(DSI_VID_BLKSIZE2, BLKLINE_PULSE_PCK, __x)
+#define DSI_VID_PCK_TIME 0x000000A8
+#define DSI_VID_PCK_TIME_BLKEOL_DURATION_SHIFT 0
+#define DSI_VID_PCK_TIME_BLKEOL_DURATION_MASK 0x00001FFF
+#define DSI_VID_PCK_TIME_BLKEOL_DURATION(__x) \
+	DSI_VAL2REG(DSI_VID_PCK_TIME, BLKEOL_DURATION, __x)
+#define DSI_VID_DPHY_TIME 0x000000AC
+#define DSI_VID_DPHY_TIME_REG_LINE_DURATION_SHIFT 0
+#define DSI_VID_DPHY_TIME_REG_LINE_DURATION_MASK 0x00001FFF
+#define DSI_VID_DPHY_TIME_REG_LINE_DURATION(__x) \
+	DSI_VAL2REG(DSI_VID_DPHY_TIME, REG_LINE_DURATION, __x)
+#define DSI_VID_DPHY_TIME_REG_WAKEUP_TIME_SHIFT 13
+#define DSI_VID_DPHY_TIME_REG_WAKEUP_TIME_MASK 0x00FFE000
+#define DSI_VID_DPHY_TIME_REG_WAKEUP_TIME(__x) \
+	DSI_VAL2REG(DSI_VID_DPHY_TIME, REG_WAKEUP_TIME, __x)
+#define DSI_VID_ERR_COLOR 0x000000B0
+#define DSI_VID_ERR_COLOR_COL_RED_SHIFT 0
+#define DSI_VID_ERR_COLOR_COL_RED_MASK 0x000000FF
+#define DSI_VID_ERR_COLOR_COL_RED(__x) \
+	DSI_VAL2REG(DSI_VID_ERR_COLOR, COL_RED, __x)
+#define DSI_VID_ERR_COLOR_COL_GREEN_SHIFT 8
+#define DSI_VID_ERR_COLOR_COL_GREEN_MASK 0x0000FF00
+#define DSI_VID_ERR_COLOR_COL_GREEN(__x) \
+	DSI_VAL2REG(DSI_VID_ERR_COLOR, COL_GREEN, __x)
+#define DSI_VID_ERR_COLOR_COL_BLUE_SHIFT 16
+#define DSI_VID_ERR_COLOR_COL_BLUE_MASK 0x00FF0000
+#define DSI_VID_ERR_COLOR_COL_BLUE(__x) \
+	DSI_VAL2REG(DSI_VID_ERR_COLOR, COL_BLUE, __x)
+#define DSI_VID_ERR_COLOR_PAD_VAL_SHIFT 24
+#define DSI_VID_ERR_COLOR_PAD_VAL_MASK 0xFF000000
+#define DSI_VID_ERR_COLOR_PAD_VAL(__x) \
+	DSI_VAL2REG(DSI_VID_ERR_COLOR, PAD_VAL, __x)
+#define DSI_VID_VPOS 0x000000B4
+#define DSI_VID_VPOS_LINE_POS_SHIFT 0
+#define DSI_VID_VPOS_LINE_POS_MASK 0x00000003
+#define DSI_VID_VPOS_LINE_POS(__x) \
+	DSI_VAL2REG(DSI_VID_VPOS, LINE_POS, __x)
+#define DSI_VID_VPOS_LINE_VAL_SHIFT 2
+#define DSI_VID_VPOS_LINE_VAL_MASK 0x00001FFC
+#define DSI_VID_VPOS_LINE_VAL(__x) \
+	DSI_VAL2REG(DSI_VID_VPOS, LINE_VAL, __x)
+#define DSI_VID_HPOS 0x000000B8
+#define DSI_VID_HPOS_HORIZONTAL_POS_SHIFT 0
+#define DSI_VID_HPOS_HORIZONTAL_POS_MASK 0x00000007
+#define DSI_VID_HPOS_HORIZONTAL_POS(__x) \
+	DSI_VAL2REG(DSI_VID_HPOS, HORIZONTAL_POS, __x)
+#define DSI_VID_HPOS_HORIZONTAL_VAL_SHIFT 3
+#define DSI_VID_HPOS_HORIZONTAL_VAL_MASK 0x0000FFF8
+#define DSI_VID_HPOS_HORIZONTAL_VAL(__x) \
+	DSI_VAL2REG(DSI_VID_HPOS, HORIZONTAL_VAL, __x)
+#define DSI_VID_MODE_STS 0x000000BC
+#define DSI_VID_MODE_STS_VSG_RUNNING_SHIFT 0
+#define DSI_VID_MODE_STS_VSG_RUNNING_MASK 0x00000001
+#define DSI_VID_MODE_STS_VSG_RUNNING(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, VSG_RUNNING, __x)
+#define DSI_VID_MODE_STS_ERR_MISSING_DATA_SHIFT 1
+#define DSI_VID_MODE_STS_ERR_MISSING_DATA_MASK 0x00000002
+#define DSI_VID_MODE_STS_ERR_MISSING_DATA(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_MISSING_DATA, __x)
+#define DSI_VID_MODE_STS_ERR_MISSING_HSYNC_SHIFT 2
+#define DSI_VID_MODE_STS_ERR_MISSING_HSYNC_MASK 0x00000004
+#define DSI_VID_MODE_STS_ERR_MISSING_HSYNC(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_MISSING_HSYNC, __x)
+#define DSI_VID_MODE_STS_ERR_MISSING_VSYNC_SHIFT 3
+#define DSI_VID_MODE_STS_ERR_MISSING_VSYNC_MASK 0x00000008
+#define DSI_VID_MODE_STS_ERR_MISSING_VSYNC(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_MISSING_VSYNC, __x)
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_LENGTH_SHIFT 4
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_LENGTH_MASK 0x00000010
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, REG_ERR_SMALL_LENGTH, __x)
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_HEIGHT_SHIFT 5
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_HEIGHT_MASK 0x00000020
+#define DSI_VID_MODE_STS_REG_ERR_SMALL_HEIGHT(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, REG_ERR_SMALL_HEIGHT, __x)
+#define DSI_VID_MODE_STS_ERR_BURSTWRITE_SHIFT 6
+#define DSI_VID_MODE_STS_ERR_BURSTWRITE_MASK 0x00000040
+#define DSI_VID_MODE_STS_ERR_BURSTWRITE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_BURSTWRITE, __x)
+#define DSI_VID_MODE_STS_ERR_LONGWRITE_SHIFT 7
+#define DSI_VID_MODE_STS_ERR_LONGWRITE_MASK 0x00000080
+#define DSI_VID_MODE_STS_ERR_LONGWRITE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_LONGWRITE, __x)
+#define DSI_VID_MODE_STS_ERR_LONGREAD_SHIFT 8
+#define DSI_VID_MODE_STS_ERR_LONGREAD_MASK 0x00000100
+#define DSI_VID_MODE_STS_ERR_LONGREAD(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_LONGREAD, __x)
+#define DSI_VID_MODE_STS_ERR_VRS_WRONG_LENGTH_SHIFT 9
+#define DSI_VID_MODE_STS_ERR_VRS_WRONG_LENGTH_MASK 0x00000200
+#define DSI_VID_MODE_STS_ERR_VRS_WRONG_LENGTH(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, ERR_VRS_WRONG_LENGTH, __x)
+#define DSI_VID_MODE_STS_VSG_RECOVERY_SHIFT 10
+#define DSI_VID_MODE_STS_VSG_RECOVERY_MASK 0x00000400
+#define DSI_VID_MODE_STS_VSG_RECOVERY(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS, VSG_RECOVERY, __x)
+#define DSI_VID_VCA_SETTING1 0x000000C0
+#define DSI_VID_VCA_SETTING1_MAX_BURST_LIMIT_SHIFT 0
+#define DSI_VID_VCA_SETTING1_MAX_BURST_LIMIT_MASK 0x0000FFFF
+#define DSI_VID_VCA_SETTING1_MAX_BURST_LIMIT(__x) \
+	DSI_VAL2REG(DSI_VID_VCA_SETTING1, MAX_BURST_LIMIT, __x)
+#define DSI_VID_VCA_SETTING1_BURST_LP_SHIFT 16
+#define DSI_VID_VCA_SETTING1_BURST_LP_MASK 0x00010000
+#define DSI_VID_VCA_SETTING1_BURST_LP(__x) \
+	DSI_VAL2REG(DSI_VID_VCA_SETTING1, BURST_LP, __x)
+#define DSI_VID_VCA_SETTING2 0x000000C4
+#define DSI_VID_VCA_SETTING2_EXACT_BURST_LIMIT_SHIFT 0
+#define DSI_VID_VCA_SETTING2_EXACT_BURST_LIMIT_MASK 0x0000FFFF
+#define DSI_VID_VCA_SETTING2_EXACT_BURST_LIMIT(__x) \
+	DSI_VAL2REG(DSI_VID_VCA_SETTING2, EXACT_BURST_LIMIT, __x)
+#define DSI_VID_VCA_SETTING2_MAX_LINE_LIMIT_SHIFT 16
+#define DSI_VID_VCA_SETTING2_MAX_LINE_LIMIT_MASK 0xFFFF0000
+#define DSI_VID_VCA_SETTING2_MAX_LINE_LIMIT(__x) \
+	DSI_VAL2REG(DSI_VID_VCA_SETTING2, MAX_LINE_LIMIT, __x)
+#define DSI_TVG_CTL 0x000000C8
+#define DSI_TVG_CTL_TVG_RUN_SHIFT 0
+#define DSI_TVG_CTL_TVG_RUN_MASK 0x00000001
+#define DSI_TVG_CTL_TVG_RUN(__x) \
+	DSI_VAL2REG(DSI_TVG_CTL, TVG_RUN, __x)
+#define DSI_TVG_CTL_TVG_STOPMODE_SHIFT 1
+#define DSI_TVG_CTL_TVG_STOPMODE_MASK 0x00000006
+#define DSI_TVG_CTL_TVG_STOPMODE(__x) \
+	DSI_VAL2REG(DSI_TVG_CTL, TVG_STOPMODE, __x)
+#define DSI_TVG_CTL_TVG_MODE_SHIFT 3
+#define DSI_TVG_CTL_TVG_MODE_MASK 0x00000018
+#define DSI_TVG_CTL_TVG_MODE(__x) \
+	DSI_VAL2REG(DSI_TVG_CTL, TVG_MODE, __x)
+#define DSI_TVG_CTL_TVG_STRIPE_SIZE_SHIFT 5
+#define DSI_TVG_CTL_TVG_STRIPE_SIZE_MASK 0x000000E0
+#define DSI_TVG_CTL_TVG_STRIPE_SIZE(__x) \
+	DSI_VAL2REG(DSI_TVG_CTL, TVG_STRIPE_SIZE, __x)
+#define DSI_TVG_IMG_SIZE 0x000000CC
+#define DSI_TVG_IMG_SIZE_TVG_LINE_SIZE_SHIFT 0
+#define DSI_TVG_IMG_SIZE_TVG_LINE_SIZE_MASK 0x00001FFF
+#define DSI_TVG_IMG_SIZE_TVG_LINE_SIZE(__x) \
+	DSI_VAL2REG(DSI_TVG_IMG_SIZE, TVG_LINE_SIZE, __x)
+#define DSI_TVG_IMG_SIZE_TVG_NBLINE_SHIFT 16
+#define DSI_TVG_IMG_SIZE_TVG_NBLINE_MASK 0x07FF0000
+#define DSI_TVG_IMG_SIZE_TVG_NBLINE(__x) \
+	DSI_VAL2REG(DSI_TVG_IMG_SIZE, TVG_NBLINE, __x)
+#define DSI_TVG_COLOR1 0x000000D0
+#define DSI_TVG_COLOR1_COL1_RED_SHIFT 0
+#define DSI_TVG_COLOR1_COL1_RED_MASK 0x000000FF
+#define DSI_TVG_COLOR1_COL1_RED(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR1, COL1_RED, __x)
+#define DSI_TVG_COLOR1_COL1_GREEN_SHIFT 8
+#define DSI_TVG_COLOR1_COL1_GREEN_MASK 0x0000FF00
+#define DSI_TVG_COLOR1_COL1_GREEN(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR1, COL1_GREEN, __x)
+#define DSI_TVG_COLOR1_COL1_BLUE_SHIFT 16
+#define DSI_TVG_COLOR1_COL1_BLUE_MASK 0x00FF0000
+#define DSI_TVG_COLOR1_COL1_BLUE(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR1, COL1_BLUE, __x)
+#define DSI_TVG_COLOR2 0x000000D4
+#define DSI_TVG_COLOR2_COL2_RED_SHIFT 0
+#define DSI_TVG_COLOR2_COL2_RED_MASK 0x000000FF
+#define DSI_TVG_COLOR2_COL2_RED(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR2, COL2_RED, __x)
+#define DSI_TVG_COLOR2_COL2_GREEN_SHIFT 8
+#define DSI_TVG_COLOR2_COL2_GREEN_MASK 0x0000FF00
+#define DSI_TVG_COLOR2_COL2_GREEN(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR2, COL2_GREEN, __x)
+#define DSI_TVG_COLOR2_COL2_BLUE_SHIFT 16
+#define DSI_TVG_COLOR2_COL2_BLUE_MASK 0x00FF0000
+#define DSI_TVG_COLOR2_COL2_BLUE(__x) \
+	DSI_VAL2REG(DSI_TVG_COLOR2, COL2_BLUE, __x)
+#define DSI_TVG_STS 0x000000D8
+#define DSI_TVG_STS_TVG_RUNNING_SHIFT 0
+#define DSI_TVG_STS_TVG_RUNNING_MASK 0x00000001
+#define DSI_TVG_STS_TVG_RUNNING(__x) \
+	DSI_VAL2REG(DSI_TVG_STS, TVG_RUNNING, __x)
+#define DSI_TBG_CTL 0x000000E0
+#define DSI_TBG_CTL_TBG_START_SHIFT 0
+#define DSI_TBG_CTL_TBG_START_MASK 0x00000001
+#define DSI_TBG_CTL_TBG_START(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_START, __x)
+#define DSI_TBG_CTL_TBG_HS_REQ_SHIFT 1
+#define DSI_TBG_CTL_TBG_HS_REQ_MASK 0x00000002
+#define DSI_TBG_CTL_TBG_HS_REQ(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_HS_REQ, __x)
+#define DSI_TBG_CTL_TBG_DATA_SEL_SHIFT 2
+#define DSI_TBG_CTL_TBG_DATA_SEL_MASK 0x00000004
+#define DSI_TBG_CTL_TBG_DATA_SEL(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_DATA_SEL, __x)
+#define DSI_TBG_CTL_TBG_MODE_SHIFT 3
+#define DSI_TBG_CTL_TBG_MODE_MASK 0x00000018
+#define DSI_TBG_CTL_TBG_MODE_1BYTE 0
+#define DSI_TBG_CTL_TBG_MODE_2BYTE 1
+#define DSI_TBG_CTL_TBG_MODE_BURST_COUNTER 2
+#define DSI_TBG_CTL_TBG_MODE_BURST 3
+#define DSI_TBG_CTL_TBG_MODE_ENUM(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_MODE, DSI_TBG_CTL_TBG_MODE_##__x)
+#define DSI_TBG_CTL_TBG_MODE(__x) \
+	DSI_VAL2REG(DSI_TBG_CTL, TBG_MODE, __x)
+#define DSI_TBG_SETTING 0x000000E4
+#define DSI_TBG_SETTING_TBG_DATA_SHIFT 0
+#define DSI_TBG_SETTING_TBG_DATA_MASK 0x0000FFFF
+#define DSI_TBG_SETTING_TBG_DATA(__x) \
+	DSI_VAL2REG(DSI_TBG_SETTING, TBG_DATA, __x)
+#define DSI_TBG_SETTING_TBG_CPT_SHIFT 16
+#define DSI_TBG_SETTING_TBG_CPT_MASK 0x0FFF0000
+#define DSI_TBG_SETTING_TBG_CPT(__x) \
+	DSI_VAL2REG(DSI_TBG_SETTING, TBG_CPT, __x)
+#define DSI_TBG_STS 0x000000E8
+#define DSI_TBG_STS_TBG_STATUS_SHIFT 0
+#define DSI_TBG_STS_TBG_STATUS_MASK 0x00000001
+#define DSI_TBG_STS_TBG_STATUS(__x) \
+	DSI_VAL2REG(DSI_TBG_STS, TBG_STATUS, __x)
+#define DSI_MCTL_MAIN_STS_CTL 0x000000F0
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EN_SHIFT 0
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EN_MASK 0x00000001
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, PLL_LOCK_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EN_SHIFT 1
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EN_MASK 0x00000002
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, CLKLANE_READY_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EN_SHIFT 2
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EN_MASK 0x00000004
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, DAT1_READY_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EN_SHIFT 3
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EN_MASK 0x00000008
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, DAT2_READY_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EN_SHIFT 4
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EN_MASK 0x00000010
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, HSTX_TO_ERR_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EN_SHIFT 5
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EN_MASK 0x00000020
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, LPRX_TO_ERR_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EN_SHIFT 6
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EN_MASK 0x00000040
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, CRS_UNTERM_PCK_ERR_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EN_SHIFT 7
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EN_MASK 0x00000080
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, VRS_UNTERM_PCK_ERR_EN, __x)
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EDGE_SHIFT 16
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EDGE_MASK 0x00010000
+#define DSI_MCTL_MAIN_STS_CTL_PLL_LOCK_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, PLL_LOCK_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EDGE_SHIFT 17
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EDGE_MASK 0x00020000
+#define DSI_MCTL_MAIN_STS_CTL_CLKLANE_READY_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, CLKLANE_READY_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EDGE_SHIFT 18
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EDGE_MASK 0x00040000
+#define DSI_MCTL_MAIN_STS_CTL_DAT1_READY_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, DAT1_READY_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EDGE_SHIFT 19
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EDGE_MASK 0x00080000
+#define DSI_MCTL_MAIN_STS_CTL_DAT2_READY_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, DAT2_READY_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EDGE_SHIFT 20
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EDGE_MASK 0x00100000
+#define DSI_MCTL_MAIN_STS_CTL_HSTX_TO_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, HSTX_TO_ERR_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EDGE_SHIFT 21
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EDGE_MASK 0x00200000
+#define DSI_MCTL_MAIN_STS_CTL_LPRX_TO_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, LPRX_TO_ERR_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EDGE_SHIFT 22
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EDGE_MASK 0x00400000
+#define DSI_MCTL_MAIN_STS_CTL_CRS_UNTERM_PCK_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, CRS_UNTERM_PCK_ERR_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EDGE_SHIFT 23
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EDGE_MASK 0x00800000
+#define DSI_MCTL_MAIN_STS_CTL_VRS_UNTERM_PCK_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CTL, VRS_UNTERM_PCK_ERR_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL 0x000000F4
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EN_SHIFT 0
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EN_MASK 0x00000001
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_NO_TE_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EN_SHIFT 1
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EN_MASK 0x00000002
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_TE_MISS_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EN_SHIFT 2
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EN_MASK 0x00000004
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_SDI1_UNDERRUN_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EN_SHIFT 3
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EN_MASK 0x00000008
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_SDI2_UNDERRUN_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EN_SHIFT 4
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EN_MASK 0x00000010
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_UNWANTED_RD_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EN_SHIFT 5
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EN_MASK 0x00000020
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EN(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, CSM_RUNNING_EN, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EDGE_SHIFT 16
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EDGE_MASK 0x00010000
+#define DSI_CMD_MODE_STS_CTL_ERR_NO_TE_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_NO_TE_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EDGE_SHIFT 17
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EDGE_MASK 0x00020000
+#define DSI_CMD_MODE_STS_CTL_ERR_TE_MISS_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_TE_MISS_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EDGE_SHIFT 18
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EDGE_MASK 0x00040000
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI1_UNDERRUN_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_SDI1_UNDERRUN_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EDGE_SHIFT 19
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EDGE_MASK 0x00080000
+#define DSI_CMD_MODE_STS_CTL_ERR_SDI2_UNDERRUN_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_SDI2_UNDERRUN_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EDGE_SHIFT 20
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EDGE_MASK 0x00100000
+#define DSI_CMD_MODE_STS_CTL_ERR_UNWANTED_RD_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, ERR_UNWANTED_RD_EDGE, __x)
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EDGE_SHIFT 21
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EDGE_MASK 0x00200000
+#define DSI_CMD_MODE_STS_CTL_CSM_RUNNING_EDGE(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CTL, CSM_RUNNING_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL 0x000000F8
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EN_SHIFT 0
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EN_MASK 0x00000001
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, CMD_TRANSMISSION_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EN_SHIFT 1
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EN_MASK 0x00000002
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, WRITE_COMPLETED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EN_SHIFT 2
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EN_MASK 0x00000004
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TRIGGER_COMPLETED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EN_SHIFT 3
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EN_MASK 0x00000008
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, READ_COMPLETED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EN_SHIFT 4
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EN_MASK 0x00000010
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, ACKNOWLEDGE_RECEIVED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EN_SHIFT 5
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EN_MASK 0x00000020
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, ACKNOWLEDGE_WITH_ERR_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EN_SHIFT 6
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EN_MASK 0x00000040
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TRIGGER_RECEIVED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EN_SHIFT 7
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EN_MASK 0x00000080
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TE_RECEIVED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EN_SHIFT 8
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EN_MASK 0x00000100
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, BTA_COMPLETED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EN_SHIFT 9
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EN_MASK 0x00000200
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, BTA_FINISHED_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EN_SHIFT 10
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EN_MASK 0x00000400
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, READ_COMPLETED_WITH_ERR_EN, __x)
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EDGE_SHIFT 16
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EDGE_MASK 0x00010000
+#define DSI_DIRECT_CMD_STS_CTL_CMD_TRANSMISSION_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, CMD_TRANSMISSION_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EDGE_SHIFT 17
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EDGE_MASK 0x00020000
+#define DSI_DIRECT_CMD_STS_CTL_WRITE_COMPLETED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, WRITE_COMPLETED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EDGE_SHIFT 18
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EDGE_MASK 0x00040000
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_COMPLETED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TRIGGER_COMPLETED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EDGE_SHIFT 19
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EDGE_MASK 0x00080000
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, READ_COMPLETED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EDGE_SHIFT 20
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EDGE_MASK 0x00100000
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_RECEIVED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, ACKNOWLEDGE_RECEIVED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EDGE_SHIFT 21
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EDGE_MASK 0x00200000
+#define DSI_DIRECT_CMD_STS_CTL_ACKNOWLEDGE_WITH_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, ACKNOWLEDGE_WITH_ERR_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EDGE_SHIFT 22
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EDGE_MASK 0x00400000
+#define DSI_DIRECT_CMD_STS_CTL_TRIGGER_RECEIVED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TRIGGER_RECEIVED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EDGE_SHIFT 23
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EDGE_MASK 0x00800000
+#define DSI_DIRECT_CMD_STS_CTL_TE_RECEIVED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, TE_RECEIVED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EDGE_SHIFT 24
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EDGE_MASK 0x01000000
+#define DSI_DIRECT_CMD_STS_CTL_BTA_COMPLETED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, BTA_COMPLETED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EDGE_SHIFT 25
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EDGE_MASK 0x02000000
+#define DSI_DIRECT_CMD_STS_CTL_BTA_FINISHED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, BTA_FINISHED_EDGE, __x)
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EDGE_SHIFT 26
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EDGE_MASK 0x04000000
+#define DSI_DIRECT_CMD_STS_CTL_READ_COMPLETED_WITH_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CTL, READ_COMPLETED_WITH_ERR_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL 0x000000FC
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EN_SHIFT 0
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EN_MASK 0x00000001
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_FIXED_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EN_SHIFT 1
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EN_MASK 0x00000002
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_UNCORRECTABLE_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EN_SHIFT 2
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EN_MASK 0x00000004
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_CHECKSUM_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EN_SHIFT 3
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EN_MASK 0x00000008
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_UNDECODABLE_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EN_SHIFT 4
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EN_MASK 0x00000010
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_RECEIVE_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EN_SHIFT 5
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EN_MASK 0x00000020
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_OVERSIZE_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EN_SHIFT 6
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EN_MASK 0x00000040
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_WRONG_LENGTH_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EN_SHIFT 7
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EN_MASK 0x00000080
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_MISSING_EOT_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EN_SHIFT 8
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EN_MASK 0x00000100
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EN(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_EOT_WITH_ERR_EN, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EDGE_SHIFT 16
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EDGE_MASK 0x00010000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_FIXED_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_FIXED_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EDGE_SHIFT 17
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EDGE_MASK 0x00020000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNCORRECTABLE_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_UNCORRECTABLE_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EDGE_SHIFT 18
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EDGE_MASK 0x00040000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_CHECKSUM_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_CHECKSUM_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EDGE_SHIFT 19
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EDGE_MASK 0x00080000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_UNDECODABLE_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_UNDECODABLE_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EDGE_SHIFT 20
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EDGE_MASK 0x00100000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_RECEIVE_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_RECEIVE_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EDGE_SHIFT 21
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EDGE_MASK 0x00200000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_OVERSIZE_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_OVERSIZE_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EDGE_SHIFT 22
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EDGE_MASK 0x00400000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_WRONG_LENGTH_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_WRONG_LENGTH_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EDGE_SHIFT 23
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EDGE_MASK 0x00800000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_MISSING_EOT_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_MISSING_EOT_EDGE, __x)
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EDGE_SHIFT 24
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EDGE_MASK 0x01000000
+#define DSI_DIRECT_CMD_RD_STS_CTL_ERR_EOT_WITH_ERR_EDGE(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CTL, ERR_EOT_WITH_ERR_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL 0x00000100
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EN_SHIFT 0
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EN_MASK 0x00000001
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, VSG_RUNNING_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EN_SHIFT 1
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EN_MASK 0x00000002
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_DATA_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EN_SHIFT 2
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EN_MASK 0x00000004
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_HSYNC_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EN_SHIFT 3
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EN_MASK 0x00000008
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_VSYNC_EN, __x)
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EN_SHIFT 4
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EN_MASK 0x00000010
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, REG_ERR_SMALL_LENGTH_EN, __x)
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EN_SHIFT 5
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EN_MASK 0x00000020
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, REG_ERR_SMALL_HEIGHT_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EN_SHIFT 6
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EN_MASK 0x00000040
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_BURSTWRITE_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EN_SHIFT 7
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EN_MASK 0x00000080
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_LONGWRITE_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EN_SHIFT 8
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EN_MASK 0x00000100
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_LONGREAD_EN, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EN_SHIFT 9
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EN_MASK 0x00000200
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EN(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_VRS_WRONG_LENGTH_EN, __x)
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EDGE_SHIFT 16
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EDGE_MASK 0x00010000
+#define DSI_VID_MODE_STS_CTL_VSG_RUNNING_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, VSG_RUNNING_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EDGE_SHIFT 17
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EDGE_MASK 0x00020000
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_DATA_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_DATA_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EDGE_SHIFT 18
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EDGE_MASK 0x00040000
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_HSYNC_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_HSYNC_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EDGE_SHIFT 19
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EDGE_MASK 0x00080000
+#define DSI_VID_MODE_STS_CTL_ERR_MISSING_VSYNC_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_MISSING_VSYNC_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EDGE_SHIFT 20
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EDGE_MASK 0x00100000
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_LENGTH_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, REG_ERR_SMALL_LENGTH_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EDGE_SHIFT 21
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EDGE_MASK 0x00200000
+#define DSI_VID_MODE_STS_CTL_REG_ERR_SMALL_HEIGHT_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, REG_ERR_SMALL_HEIGHT_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EDGE_SHIFT 22
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EDGE_MASK 0x00400000
+#define DSI_VID_MODE_STS_CTL_ERR_BURSTWRITE_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_BURSTWRITE_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EDGE_SHIFT 23
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EDGE_MASK 0x00800000
+#define DSI_VID_MODE_STS_CTL_ERR_LONGWRITE_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_LONGWRITE_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EDGE_SHIFT 24
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EDGE_MASK 0x01000000
+#define DSI_VID_MODE_STS_CTL_ERR_LONGREAD_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_LONGREAD_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EDGE_SHIFT 25
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EDGE_MASK 0x02000000
+#define DSI_VID_MODE_STS_CTL_ERR_VRS_WRONG_LENGTH_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, ERR_VRS_WRONG_LENGTH_EDGE, __x)
+#define DSI_VID_MODE_STS_CTL_VSG_RECOVERY_EDGE_SHIFT 26
+#define DSI_VID_MODE_STS_CTL_VSG_RECOVERY_EDGE_MASK 0x04000000
+#define DSI_VID_MODE_STS_CTL_VSG_RECOVERY_EDGE(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CTL, VSG_RECOVERY_EDGE, __x)
+#define DSI_TG_STS_CTL 0x00000104
+#define DSI_TG_STS_CTL_TVG_STS_EN_SHIFT 0
+#define DSI_TG_STS_CTL_TVG_STS_EN_MASK 0x00000001
+#define DSI_TG_STS_CTL_TVG_STS_EN(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CTL, TVG_STS_EN, __x)
+#define DSI_TG_STS_CTL_TBG_STS_EN_SHIFT 1
+#define DSI_TG_STS_CTL_TBG_STS_EN_MASK 0x00000002
+#define DSI_TG_STS_CTL_TBG_STS_EN(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CTL, TBG_STS_EN, __x)
+#define DSI_TG_STS_CTL_TVG_STS_EDGE_SHIFT 16
+#define DSI_TG_STS_CTL_TVG_STS_EDGE_MASK 0x00010000
+#define DSI_TG_STS_CTL_TVG_STS_EDGE(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CTL, TVG_STS_EDGE, __x)
+#define DSI_TG_STS_CTL_TBG_STS_EDGE_SHIFT 17
+#define DSI_TG_STS_CTL_TBG_STS_EDGE_MASK 0x00020000
+#define DSI_TG_STS_CTL_TBG_STS_EDGE(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CTL, TBG_STS_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL 0x00000108
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EN_SHIFT 6
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EN_MASK 0x00000040
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_ESC_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EN_SHIFT 7
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EN_MASK 0x00000080
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_ESC_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EN_SHIFT 8
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EN_MASK 0x00000100
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_SYNCESC_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EN_SHIFT 9
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EN_MASK 0x00000200
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_SYNCESC_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EN_SHIFT 10
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EN_MASK 0x00000400
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONTROL_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EN_SHIFT 11
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EN_MASK 0x00000800
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONTROL_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EN_SHIFT 12
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EN_MASK 0x00001000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP0_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EN_SHIFT 13
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EN_MASK 0x00002000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP0_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EN_SHIFT 14
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EN_MASK 0x00004000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP1_1_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EN_SHIFT 15
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EN_MASK 0x00008000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EN(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP1_2_EN, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EDGE_SHIFT 22
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EDGE_MASK 0x00400000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_ESC_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EDGE_SHIFT 23
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EDGE_MASK 0x00800000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_ESC_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_ESC_2_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EDGE_SHIFT 24
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EDGE_MASK 0x01000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_SYNCESC_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EDGE_SHIFT 25
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EDGE_MASK 0x02000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_SYNCESC_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_SYNCESC_2_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EDGE_SHIFT 26
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EDGE_MASK 0x04000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONTROL_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EDGE_SHIFT 27
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EDGE_MASK 0x08000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONTROL_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONTROL_2_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EDGE_SHIFT 28
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EDGE_MASK 0x10000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP0_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EDGE_SHIFT 29
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EDGE_MASK 0x20000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP0_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP0_2_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EDGE_SHIFT 30
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EDGE_MASK 0x40000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_1_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP1_1_EDGE, __x)
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EDGE_SHIFT 31
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EDGE_MASK 0x80000000
+#define DSI_MCTL_DHPY_ERR_CTL_ERR_CONT_LP1_2_EDGE(__x) \
+	DSI_VAL2REG(DSI_MCTL_DHPY_ERR_CTL, ERR_CONT_LP1_2_EDGE, __x)
+#define DSI_MCTL_MAIN_STS_CLR 0x00000110
+#define DSI_MCTL_MAIN_STS_CLR_PLL_LOCK_CLR_SHIFT 0
+#define DSI_MCTL_MAIN_STS_CLR_PLL_LOCK_CLR_MASK 0x00000001
+#define DSI_MCTL_MAIN_STS_CLR_PLL_LOCK_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, PLL_LOCK_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_CLKLANE_READY_CLR_SHIFT 1
+#define DSI_MCTL_MAIN_STS_CLR_CLKLANE_READY_CLR_MASK 0x00000002
+#define DSI_MCTL_MAIN_STS_CLR_CLKLANE_READY_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, CLKLANE_READY_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_DAT1_READY_CLR_SHIFT 2
+#define DSI_MCTL_MAIN_STS_CLR_DAT1_READY_CLR_MASK 0x00000004
+#define DSI_MCTL_MAIN_STS_CLR_DAT1_READY_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, DAT1_READY_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_DAT2_READY_CLR_SHIFT 3
+#define DSI_MCTL_MAIN_STS_CLR_DAT2_READY_CLR_MASK 0x00000008
+#define DSI_MCTL_MAIN_STS_CLR_DAT2_READY_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, DAT2_READY_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_HSTX_TO_ERR_CLR_SHIFT 4
+#define DSI_MCTL_MAIN_STS_CLR_HSTX_TO_ERR_CLR_MASK 0x00000010
+#define DSI_MCTL_MAIN_STS_CLR_HSTX_TO_ERR_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, HSTX_TO_ERR_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_LPRX_TO_ERR_CLR_SHIFT 5
+#define DSI_MCTL_MAIN_STS_CLR_LPRX_TO_ERR_CLR_MASK 0x00000020
+#define DSI_MCTL_MAIN_STS_CLR_LPRX_TO_ERR_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, LPRX_TO_ERR_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_CRS_UNTERM_PCK_CLR_SHIFT 6
+#define DSI_MCTL_MAIN_STS_CLR_CRS_UNTERM_PCK_CLR_MASK 0x00000040
+#define DSI_MCTL_MAIN_STS_CLR_CRS_UNTERM_PCK_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, CRS_UNTERM_PCK_CLR, __x)
+#define DSI_MCTL_MAIN_STS_CLR_VRS_UNTERM_PCK_CLR_SHIFT 7
+#define DSI_MCTL_MAIN_STS_CLR_VRS_UNTERM_PCK_CLR_MASK 0x00000080
+#define DSI_MCTL_MAIN_STS_CLR_VRS_UNTERM_PCK_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_CLR, VRS_UNTERM_PCK_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR 0x00000114
+#define DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR_SHIFT 0
+#define DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR_MASK 0x00000001
+#define DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_NO_TE_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_ERR_TE_MISS_CLR_SHIFT 1
+#define DSI_CMD_MODE_STS_CLR_ERR_TE_MISS_CLR_MASK 0x00000002
+#define DSI_CMD_MODE_STS_CLR_ERR_TE_MISS_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_TE_MISS_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI1_UNDERRUN_CLR_SHIFT 2
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI1_UNDERRUN_CLR_MASK 0x00000004
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI1_UNDERRUN_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_SDI1_UNDERRUN_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI2_UNDERRUN_CLR_SHIFT 3
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI2_UNDERRUN_CLR_MASK 0x00000008
+#define DSI_CMD_MODE_STS_CLR_ERR_SDI2_UNDERRUN_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_SDI2_UNDERRUN_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_ERR_UNWANTED_RD_CLR_SHIFT 4
+#define DSI_CMD_MODE_STS_CLR_ERR_UNWANTED_RD_CLR_MASK 0x00000010
+#define DSI_CMD_MODE_STS_CLR_ERR_UNWANTED_RD_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, ERR_UNWANTED_RD_CLR, __x)
+#define DSI_CMD_MODE_STS_CLR_CSM_RUNNING_CLR_SHIFT 5
+#define DSI_CMD_MODE_STS_CLR_CSM_RUNNING_CLR_MASK 0x00000020
+#define DSI_CMD_MODE_STS_CLR_CSM_RUNNING_CLR(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_CLR, CSM_RUNNING_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR 0x00000118
+#define DSI_DIRECT_CMD_STS_CLR_CMD_TRANSMISSION_CLR_SHIFT 0
+#define DSI_DIRECT_CMD_STS_CLR_CMD_TRANSMISSION_CLR_MASK 0x00000001
+#define DSI_DIRECT_CMD_STS_CLR_CMD_TRANSMISSION_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, CMD_TRANSMISSION_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_WRITE_COMPLETED_CLR_SHIFT 1
+#define DSI_DIRECT_CMD_STS_CLR_WRITE_COMPLETED_CLR_MASK 0x00000002
+#define DSI_DIRECT_CMD_STS_CLR_WRITE_COMPLETED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, WRITE_COMPLETED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_COMPLETED_CLR_SHIFT 2
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_COMPLETED_CLR_MASK 0x00000004
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_COMPLETED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, TRIGGER_COMPLETED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_CLR_SHIFT 3
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_CLR_MASK 0x00000008
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, READ_COMPLETED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_RECEIVED_CLR_SHIFT 4
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_RECEIVED_CLR_MASK 0x00000010
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_RECEIVED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, ACKNOWLEDGE_RECEIVED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_WITH_ERR_RECEIVED_CLR_SHIFT 5
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_WITH_ERR_RECEIVED_CLR_MASK 0x00000020
+#define DSI_DIRECT_CMD_STS_CLR_ACKNOWLEDGE_WITH_ERR_RECEIVED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, ACKNOWLEDGE_WITH_ERR_RECEIVED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_RECEIVED_CLR_SHIFT 6
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_RECEIVED_CLR_MASK 0x00000040
+#define DSI_DIRECT_CMD_STS_CLR_TRIGGER_RECEIVED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, TRIGGER_RECEIVED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR_SHIFT 7
+#define DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR_MASK 0x00000080
+#define DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, TE_RECEIVED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_BTA_COMPLETED_CLR_SHIFT 8
+#define DSI_DIRECT_CMD_STS_CLR_BTA_COMPLETED_CLR_MASK 0x00000100
+#define DSI_DIRECT_CMD_STS_CLR_BTA_COMPLETED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, BTA_COMPLETED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_BTA_FINISHED_CLR_SHIFT 9
+#define DSI_DIRECT_CMD_STS_CLR_BTA_FINISHED_CLR_MASK 0x00000200
+#define DSI_DIRECT_CMD_STS_CLR_BTA_FINISHED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, BTA_FINISHED_CLR, __x)
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_WITH_ERR_CLR_SHIFT 10
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_WITH_ERR_CLR_MASK 0x00000400
+#define DSI_DIRECT_CMD_STS_CLR_READ_COMPLETED_WITH_ERR_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_CLR, READ_COMPLETED_WITH_ERR_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR 0x0000011C
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_FIXED_CLR_SHIFT 0
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_FIXED_CLR_MASK 0x00000001
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_FIXED_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_FIXED_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNCORRECTABLE_CLR_SHIFT 1
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNCORRECTABLE_CLR_MASK 0x00000002
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNCORRECTABLE_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_UNCORRECTABLE_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_CHECKSUM_CLR_SHIFT 2
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_CHECKSUM_CLR_MASK 0x00000004
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_CHECKSUM_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_CHECKSUM_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNDECODABLE_CLR_SHIFT 3
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNDECODABLE_CLR_MASK 0x00000008
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_UNDECODABLE_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_UNDECODABLE_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_RECEIVE_CLR_SHIFT 4
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_RECEIVE_CLR_MASK 0x00000010
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_RECEIVE_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_RECEIVE_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_OVERSIZE_CLR_SHIFT 5
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_OVERSIZE_CLR_MASK 0x00000020
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_OVERSIZE_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_OVERSIZE_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_WRONG_LENGTH_CLR_SHIFT 6
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_WRONG_LENGTH_CLR_MASK 0x00000040
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_WRONG_LENGTH_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_WRONG_LENGTH_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_MISSING_EOT_CLR_SHIFT 7
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_MISSING_EOT_CLR_MASK 0x00000080
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_MISSING_EOT_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_MISSING_EOT_CLR, __x)
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_EOT_WITH_ERR_CLR_SHIFT 8
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_EOT_WITH_ERR_CLR_MASK 0x00000100
+#define DSI_DIRECT_CMD_RD_STS_CLR_ERR_EOT_WITH_ERR_CLR(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_CLR, ERR_EOT_WITH_ERR_CLR, __x)
+#define DSI_VID_MODE_STS_CLR 0x00000120
+#define DSI_VID_MODE_STS_CLR_VSG_STS_CLR_SHIFT 0
+#define DSI_VID_MODE_STS_CLR_VSG_STS_CLR_MASK 0x00000001
+#define DSI_VID_MODE_STS_CLR_VSG_STS_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, VSG_STS_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_DATA_CLR_SHIFT 1
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_DATA_CLR_MASK 0x00000002
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_DATA_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_MISSING_DATA_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_HSYNC_CLR_SHIFT 2
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_HSYNC_CLR_MASK 0x00000004
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_HSYNC_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_MISSING_HSYNC_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_VSYNC_CLR_SHIFT 3
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_VSYNC_CLR_MASK 0x00000008
+#define DSI_VID_MODE_STS_CLR_ERR_MISSING_VSYNC_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_MISSING_VSYNC_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_LENGTH_CLR_SHIFT 4
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_LENGTH_CLR_MASK 0x00000010
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_LENGTH_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, REG_ERR_SMALL_LENGTH_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_HEIGHT_CLR_SHIFT 5
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_HEIGHT_CLR_MASK 0x00000020
+#define DSI_VID_MODE_STS_CLR_REG_ERR_SMALL_HEIGHT_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, REG_ERR_SMALL_HEIGHT_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_BURSTWRITE_CLR_SHIFT 6
+#define DSI_VID_MODE_STS_CLR_ERR_BURSTWRITE_CLR_MASK 0x00000040
+#define DSI_VID_MODE_STS_CLR_ERR_BURSTWRITE_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_BURSTWRITE_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_LONGWRITE_CLR_SHIFT 7
+#define DSI_VID_MODE_STS_CLR_ERR_LONGWRITE_CLR_MASK 0x00000080
+#define DSI_VID_MODE_STS_CLR_ERR_LONGWRITE_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_LONGWRITE_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_LONGREAD_CLR_SHIFT 8
+#define DSI_VID_MODE_STS_CLR_ERR_LONGREAD_CLR_MASK 0x00000100
+#define DSI_VID_MODE_STS_CLR_ERR_LONGREAD_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_LONGREAD_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_ERR_VRS_WRONG_LENGTH_CLR_SHIFT 9
+#define DSI_VID_MODE_STS_CLR_ERR_VRS_WRONG_LENGTH_CLR_MASK 0x00000200
+#define DSI_VID_MODE_STS_CLR_ERR_VRS_WRONG_LENGTH_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, ERR_VRS_WRONG_LENGTH_CLR, __x)
+#define DSI_VID_MODE_STS_CLR_VSG_RECOVERY_CLR_SHIFT 10
+#define DSI_VID_MODE_STS_CLR_VSG_RECOVERY_CLR_MASK 0x00000400
+#define DSI_VID_MODE_STS_CLR_VSG_RECOVERY_CLR(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_CLR, VSG_RECOVERY_CLR, __x)
+#define DSI_TG_STS_CLR 0x00000124
+#define DSI_TG_STS_CLR_TVG_STS_CLR_SHIFT 0
+#define DSI_TG_STS_CLR_TVG_STS_CLR_MASK 0x00000001
+#define DSI_TG_STS_CLR_TVG_STS_CLR(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CLR, TVG_STS_CLR, __x)
+#define DSI_TG_STS_CLR_TBG_STS_CLR_SHIFT 1
+#define DSI_TG_STS_CLR_TBG_STS_CLR_MASK 0x00000002
+#define DSI_TG_STS_CLR_TBG_STS_CLR(__x) \
+	DSI_VAL2REG(DSI_TG_STS_CLR, TBG_STS_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR 0x00000128
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_1_CLR_SHIFT 6
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_1_CLR_MASK 0x00000040
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_ESC_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_2_CLR_SHIFT 7
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_2_CLR_MASK 0x00000080
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_ESC_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_ESC_2_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_1_CLR_SHIFT 8
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_1_CLR_MASK 0x00000100
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_SYNCESC_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_2_CLR_SHIFT 9
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_2_CLR_MASK 0x00000200
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_SYNCESC_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_SYNCESC_2_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_1_CLR_SHIFT 10
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_1_CLR_MASK 0x00000400
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONTROL_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_2_CLR_SHIFT 11
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_2_CLR_MASK 0x00000800
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONTROL_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONTROL_2_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_1_CLR_SHIFT 12
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_1_CLR_MASK 0x00001000
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONT_LP0_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_2_CLR_SHIFT 13
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_2_CLR_MASK 0x00002000
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP0_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONT_LP0_2_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_1_CLR_SHIFT 14
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_1_CLR_MASK 0x00004000
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_1_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONT_LP1_1_CLR, __x)
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_2_CLR_SHIFT 15
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_2_CLR_MASK 0x00008000
+#define DSI_MCTL_DPHY_ERR_CLR_ERR_CONT_LP1_2_CLR(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_CLR, ERR_CONT_LP1_2_CLR, __x)
+#define DSI_MCTL_MAIN_STS_FLAG 0x00000130
+#define DSI_MCTL_MAIN_STS_FLAG_PLL_LOCK_FLAG_SHIFT 0
+#define DSI_MCTL_MAIN_STS_FLAG_PLL_LOCK_FLAG_MASK 0x00000001
+#define DSI_MCTL_MAIN_STS_FLAG_PLL_LOCK_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, PLL_LOCK_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_CLKLANE_READY_FLAG_SHIFT 1
+#define DSI_MCTL_MAIN_STS_FLAG_CLKLANE_READY_FLAG_MASK 0x00000002
+#define DSI_MCTL_MAIN_STS_FLAG_CLKLANE_READY_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, CLKLANE_READY_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_DAT1_READY_FLAG_SHIFT 2
+#define DSI_MCTL_MAIN_STS_FLAG_DAT1_READY_FLAG_MASK 0x00000004
+#define DSI_MCTL_MAIN_STS_FLAG_DAT1_READY_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, DAT1_READY_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_DAT2_READY_FLAG_SHIFT 3
+#define DSI_MCTL_MAIN_STS_FLAG_DAT2_READY_FLAG_MASK 0x00000008
+#define DSI_MCTL_MAIN_STS_FLAG_DAT2_READY_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, DAT2_READY_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_HSTX_TO_ERR_FLAG_SHIFT 4
+#define DSI_MCTL_MAIN_STS_FLAG_HSTX_TO_ERR_FLAG_MASK 0x00000010
+#define DSI_MCTL_MAIN_STS_FLAG_HSTX_TO_ERR_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, HSTX_TO_ERR_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_LPRX_TO_ERR_FLAG_SHIFT 5
+#define DSI_MCTL_MAIN_STS_FLAG_LPRX_TO_ERR_FLAG_MASK 0x00000020
+#define DSI_MCTL_MAIN_STS_FLAG_LPRX_TO_ERR_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, LPRX_TO_ERR_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_CRS_UNTERM_PCK_FLAG_SHIFT 6
+#define DSI_MCTL_MAIN_STS_FLAG_CRS_UNTERM_PCK_FLAG_MASK 0x00000040
+#define DSI_MCTL_MAIN_STS_FLAG_CRS_UNTERM_PCK_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, CRS_UNTERM_PCK_FLAG, __x)
+#define DSI_MCTL_MAIN_STS_FLAG_VRS_UNTERM_PCK_FLAG_SHIFT 7
+#define DSI_MCTL_MAIN_STS_FLAG_VRS_UNTERM_PCK_FLAG_MASK 0x00000080
+#define DSI_MCTL_MAIN_STS_FLAG_VRS_UNTERM_PCK_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_MAIN_STS_FLAG, VRS_UNTERM_PCK_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG 0x00000134
+#define DSI_CMD_MODE_STS_FLAG_ERR_NO_TE_FLAG_SHIFT 0
+#define DSI_CMD_MODE_STS_FLAG_ERR_NO_TE_FLAG_MASK 0x00000001
+#define DSI_CMD_MODE_STS_FLAG_ERR_NO_TE_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_NO_TE_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_ERR_TE_MISS_FLAG_SHIFT 1
+#define DSI_CMD_MODE_STS_FLAG_ERR_TE_MISS_FLAG_MASK 0x00000002
+#define DSI_CMD_MODE_STS_FLAG_ERR_TE_MISS_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_TE_MISS_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI1_UNDERRUN_FLAG_SHIFT 2
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI1_UNDERRUN_FLAG_MASK 0x00000004
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI1_UNDERRUN_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_SDI1_UNDERRUN_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI2_UNDERRUN_FLAG_SHIFT 3
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI2_UNDERRUN_FLAG_MASK 0x00000008
+#define DSI_CMD_MODE_STS_FLAG_ERR_SDI2_UNDERRUN_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_SDI2_UNDERRUN_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_ERR_UNWANTED_RD_FLAG_SHIFT 4
+#define DSI_CMD_MODE_STS_FLAG_ERR_UNWANTED_RD_FLAG_MASK 0x00000010
+#define DSI_CMD_MODE_STS_FLAG_ERR_UNWANTED_RD_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, ERR_UNWANTED_RD_FLAG, __x)
+#define DSI_CMD_MODE_STS_FLAG_CSM_RUNNING_FLAG_SHIFT 5
+#define DSI_CMD_MODE_STS_FLAG_CSM_RUNNING_FLAG_MASK 0x00000020
+#define DSI_CMD_MODE_STS_FLAG_CSM_RUNNING_FLAG(__x) \
+	DSI_VAL2REG(DSI_CMD_MODE_STS_FLAG, CSM_RUNNING_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG 0x00000138
+#define DSI_DIRECT_CMD_STS_FLAG_CMD_TRANSMISSION_FLAG_SHIFT 0
+#define DSI_DIRECT_CMD_STS_FLAG_CMD_TRANSMISSION_FLAG_MASK 0x00000001
+#define DSI_DIRECT_CMD_STS_FLAG_CMD_TRANSMISSION_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, CMD_TRANSMISSION_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_WRITE_COMPLETED_FLAG_SHIFT 1
+#define DSI_DIRECT_CMD_STS_FLAG_WRITE_COMPLETED_FLAG_MASK 0x00000002
+#define DSI_DIRECT_CMD_STS_FLAG_WRITE_COMPLETED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, WRITE_COMPLETED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_COMPLETED_FLAG_SHIFT 2
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_COMPLETED_FLAG_MASK 0x00000004
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_COMPLETED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, TRIGGER_COMPLETED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_FLAG_SHIFT 3
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_FLAG_MASK 0x00000008
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, READ_COMPLETED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_RECEIVED_FLAG_SHIFT 4
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_RECEIVED_FLAG_MASK 0x00000010
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_RECEIVED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, ACKNOWLEDGE_RECEIVED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_WITH_ERR_RECEIVED_FLAG_SHIFT 5
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_WITH_ERR_RECEIVED_FLAG_MASK 0x00000020
+#define DSI_DIRECT_CMD_STS_FLAG_ACKNOWLEDGE_WITH_ERR_RECEIVED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, ACKNOWLEDGE_WITH_ERR_RECEIVED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_RECEIVED_FLAG_SHIFT 6
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_RECEIVED_FLAG_MASK 0x00000040
+#define DSI_DIRECT_CMD_STS_FLAG_TRIGGER_RECEIVED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, TRIGGER_RECEIVED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_TE_RECEIVED_FLAG_SHIFT 7
+#define DSI_DIRECT_CMD_STS_FLAG_TE_RECEIVED_FLAG_MASK 0x00000080
+#define DSI_DIRECT_CMD_STS_FLAG_TE_RECEIVED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, TE_RECEIVED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_COMPLETED_FLAG_SHIFT 8
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_COMPLETED_FLAG_MASK 0x00000100
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_COMPLETED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, BTA_COMPLETED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_FINISHED_FLAG_SHIFT 9
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_FINISHED_FLAG_MASK 0x00000200
+#define DSI_DIRECT_CMD_STS_FLAG_BTA_FINISHED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, BTA_FINISHED_FLAG, __x)
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_WITH_ERR_FLAG_SHIFT 10
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_WITH_ERR_FLAG_MASK 0x00000400
+#define DSI_DIRECT_CMD_STS_FLAG_READ_COMPLETED_WITH_ERR_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_STS_FLAG, READ_COMPLETED_WITH_ERR_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG 0x0000013C
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_FIXED_FLAG_SHIFT 0
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_FIXED_FLAG_MASK 0x00000001
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_FIXED_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_FIXED_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNCORRECTABLE_FLAG_SHIFT 1
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNCORRECTABLE_FLAG_MASK 0x00000002
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNCORRECTABLE_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_UNCORRECTABLE_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_CHECKSUM_FLAG_SHIFT 2
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_CHECKSUM_FLAG_MASK 0x00000004
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_CHECKSUM_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_CHECKSUM_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNDECODABLE_FLAG_SHIFT 3
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNDECODABLE_FLAG_MASK 0x00000008
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_UNDECODABLE_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_UNDECODABLE_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_RECEIVE_FLAG_SHIFT 4
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_RECEIVE_FLAG_MASK 0x00000010
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_RECEIVE_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_RECEIVE_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_OVERSIZE_FLAG_SHIFT 5
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_OVERSIZE_FLAG_MASK 0x00000020
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_OVERSIZE_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_OVERSIZE_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_WRONG_LENGTH_FLAG_SHIFT 6
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_WRONG_LENGTH_FLAG_MASK 0x00000040
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_WRONG_LENGTH_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_WRONG_LENGTH_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_MISSING_EOT_FLAG_SHIFT 7
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_MISSING_EOT_FLAG_MASK 0x00000080
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_MISSING_EOT_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_MISSING_EOT_FLAG, __x)
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_EOT_WITH_ERR_FLAG_SHIFT 8
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_EOT_WITH_ERR_FLAG_MASK 0x00000100
+#define DSI_DIRECT_CMD_RD_STS_FLAG_ERR_EOT_WITH_ERR_FLAG(__x) \
+	DSI_VAL2REG(DSI_DIRECT_CMD_RD_STS_FLAG, ERR_EOT_WITH_ERR_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG 0x00000140
+#define DSI_VID_MODE_STS_FLAG_VSG_STS_FLAG_SHIFT 0
+#define DSI_VID_MODE_STS_FLAG_VSG_STS_FLAG_MASK 0x00000001
+#define DSI_VID_MODE_STS_FLAG_VSG_STS_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, VSG_STS_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_DATA_FLAG_SHIFT 1
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_DATA_FLAG_MASK 0x00000002
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_DATA_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_MISSING_DATA_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_HSYNC_FLAG_SHIFT 2
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_HSYNC_FLAG_MASK 0x00000004
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_HSYNC_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_MISSING_HSYNC_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_VSYNC_FLAG_SHIFT 3
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_VSYNC_FLAG_MASK 0x00000008
+#define DSI_VID_MODE_STS_FLAG_ERR_MISSING_VSYNC_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_MISSING_VSYNC_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_LENGTH_FLAG_SHIFT 4
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_LENGTH_FLAG_MASK 0x00000010
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_LENGTH_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, REG_ERR_SMALL_LENGTH_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_HEIGHT_FLAG_SHIFT 5
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_HEIGHT_FLAG_MASK 0x00000020
+#define DSI_VID_MODE_STS_FLAG_REG_ERR_SMALL_HEIGHT_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, REG_ERR_SMALL_HEIGHT_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_BURSTWRITE_FLAG_SHIFT 6
+#define DSI_VID_MODE_STS_FLAG_ERR_BURSTWRITE_FLAG_MASK 0x00000040
+#define DSI_VID_MODE_STS_FLAG_ERR_BURSTWRITE_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_BURSTWRITE_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGWRITE_FLAG_SHIFT 7
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGWRITE_FLAG_MASK 0x00000080
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGWRITE_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_LONGWRITE_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGREAD_FLAG_SHIFT 8
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGREAD_FLAG_MASK 0x00000100
+#define DSI_VID_MODE_STS_FLAG_ERR_LONGREAD_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_LONGREAD_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_ERR_VRS_WRONG_LENGTH_FLAG_SHIFT 9
+#define DSI_VID_MODE_STS_FLAG_ERR_VRS_WRONG_LENGTH_FLAG_MASK 0x00000200
+#define DSI_VID_MODE_STS_FLAG_ERR_VRS_WRONG_LENGTH_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, ERR_VRS_WRONG_LENGTH_FLAG, __x)
+#define DSI_VID_MODE_STS_FLAG_VSG_RECOVERY_FLAG_SHIFT 10
+#define DSI_VID_MODE_STS_FLAG_VSG_RECOVERY_FLAG_MASK 0x00000400
+#define DSI_VID_MODE_STS_FLAG_VSG_RECOVERY_FLAG(__x) \
+	DSI_VAL2REG(DSI_VID_MODE_STS_FLAG, VSG_RECOVERY_FLAG, __x)
+#define DSI_TG_STS_FLAG 0x00000144
+#define DSI_TG_STS_FLAG_TVG_STS_FLAG_SHIFT 0
+#define DSI_TG_STS_FLAG_TVG_STS_FLAG_MASK 0x00000001
+#define DSI_TG_STS_FLAG_TVG_STS_FLAG(__x) \
+	DSI_VAL2REG(DSI_TG_STS_FLAG, TVG_STS_FLAG, __x)
+#define DSI_TG_STS_FLAG_TBG_STS_FLAG_SHIFT 1
+#define DSI_TG_STS_FLAG_TBG_STS_FLAG_MASK 0x00000002
+#define DSI_TG_STS_FLAG_TBG_STS_FLAG(__x) \
+	DSI_VAL2REG(DSI_TG_STS_FLAG, TBG_STS_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG 0x00000148
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_1_FLAG_SHIFT 6
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_1_FLAG_MASK 0x00000040
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_ESC_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_2_FLAG_SHIFT 7
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_2_FLAG_MASK 0x00000080
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_ESC_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_ESC_2_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_1_FLAG_SHIFT 8
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_1_FLAG_MASK 0x00000100
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_SYNCESC_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_2_FLAG_SHIFT 9
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_2_FLAG_MASK 0x00000200
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_SYNCESC_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_SYNCESC_2_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_1_FLAG_SHIFT 10
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_1_FLAG_MASK 0x00000400
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONTROL_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_2_FLAG_SHIFT 11
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_2_FLAG_MASK 0x00000800
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONTROL_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONTROL_2_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_1_FLAG_SHIFT 12
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_1_FLAG_MASK 0x00001000
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONT_LP0_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_2_FLAG_SHIFT 13
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_2_FLAG_MASK 0x00002000
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP0_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONT_LP0_2_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_1_FLAG_SHIFT 14
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_1_FLAG_MASK 0x00004000
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_1_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONT_LP1_1_FLAG, __x)
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_2_FLAG_SHIFT 15
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_2_FLAG_MASK 0x00008000
+#define DSI_MCTL_DPHY_ERR_FLAG_ERR_CONT_LP1_2_FLAG(__x) \
+	DSI_VAL2REG(DSI_MCTL_DPHY_ERR_FLAG, ERR_CONT_LP1_2_FLAG, __x)
+#define DSI_DPHY_LANES_TRIM 0x00000150
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT1_SHIFT 0
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT1_MASK 0x00000003
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SKEW_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_CD_OFF_DAT1_SHIFT 2
+#define DSI_DPHY_LANES_TRIM_DPHY_CD_OFF_DAT1_MASK 0x00000004
+#define DSI_DPHY_LANES_TRIM_DPHY_CD_OFF_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_CD_OFF_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT1_SHIFT 3
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT1_MASK 0x00000008
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_UP_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT1_SHIFT 4
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT1_MASK 0x00000010
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_DOWN_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT1_SHIFT 5
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT1_MASK 0x00000020
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT1(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_TEST_RESERVED_1_DAT1, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_CLK_SHIFT 6
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_CLK_MASK 0x000000C0
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SKEW_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_RX_VIL_CLK_SHIFT 8
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_RX_VIL_CLK_MASK 0x00000300
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_RX_VIL_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_LP_RX_VIL_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_TX_SLEWRATE_CLK_SHIFT 10
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_TX_SLEWRATE_CLK_MASK 0x00000C00
+#define DSI_DPHY_LANES_TRIM_DPHY_LP_TX_SLEWRATE_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_LP_TX_SLEWRATE_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_SHIFT 12
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_MASK 0x00001000
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_0_81 0
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_0_90 1
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_ENUM(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SPECS_90_81B, \
+	DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_##__x)
+#define DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SPECS_90_81B, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_CLK_SHIFT 13
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_CLK_MASK 0x00002000
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_UP_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_CLK_SHIFT 14
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_CLK_MASK 0x00004000
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_DOWN_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_CLK_SHIFT 15
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_CLK_MASK 0x00008000
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_CLK(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_TEST_RESERVED_1_CLK, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT2_SHIFT 16
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT2_MASK 0x00030000
+#define DSI_DPHY_LANES_TRIM_DPHY_SKEW_DAT2(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_SKEW_DAT2, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT2_SHIFT 18
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT2_MASK 0x00040000
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_UP_DAT2(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_UP_DAT2, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT2_SHIFT 19
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT2_MASK 0x00080000
+#define DSI_DPHY_LANES_TRIM_DPHY_HSTX_SLEWRATE_DOWN_DAT2(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_HSTX_SLEWRATE_DOWN_DAT2, __x)
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT2_SHIFT 20
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT2_MASK 0x00100000
+#define DSI_DPHY_LANES_TRIM_DPHY_TEST_RESERVED_1_DAT2(__x) \
+	DSI_VAL2REG(DSI_DPHY_LANES_TRIM, DPHY_TEST_RESERVED_1_DAT2, __x)
+#define DSI_ID_REG 0x00000FF0
+#define DSI_ID_REG_Y_SHIFT 0
+#define DSI_ID_REG_Y_MASK 0x0000000F
+#define DSI_ID_REG_Y(__x) \
+	DSI_VAL2REG(DSI_ID_REG, Y, __x)
+#define DSI_ID_REG_X_SHIFT 4
+#define DSI_ID_REG_X_MASK 0x000000F0
+#define DSI_ID_REG_X(__x) \
+	DSI_VAL2REG(DSI_ID_REG, X, __x)
+#define DSI_ID_REG_H_SHIFT 8
+#define DSI_ID_REG_H_MASK 0x00000300
+#define DSI_ID_REG_H(__x) \
+	DSI_VAL2REG(DSI_ID_REG, H, __x)
+#define DSI_ID_REG_PRODUCT_ID_SHIFT 10
+#define DSI_ID_REG_PRODUCT_ID_MASK 0x0003FC00
+#define DSI_ID_REG_PRODUCT_ID(__x) \
+	DSI_VAL2REG(DSI_ID_REG, PRODUCT_ID, __x)
+#define DSI_ID_REG_VENDOR_ID_SHIFT 18
+#define DSI_ID_REG_VENDOR_ID_MASK 0xFFFC0000
+#define DSI_ID_REG_VENDOR_ID(__x) \
+	DSI_VAL2REG(DSI_ID_REG, VENDOR_ID, __x)
+#define DSI_IP_CONF 0x00000FF4
+#define DSI_IP_CONF_FIFO_SIZE_SHIFT 0
+#define DSI_IP_CONF_FIFO_SIZE_MASK 0x0000003F
+#define DSI_IP_CONF_FIFO_SIZE(__x) \
+	DSI_VAL2REG(DSI_IP_CONF, FIFO_SIZE, __x)
diff --git a/drivers/video/mcde/mcde_bus.c b/drivers/video/mcde/mcde_bus.c
new file mode 100644
index 00000000000..bdcf65b0fb9
--- /dev/null
+++ b/drivers/video/mcde/mcde_bus.c
@@ -0,0 +1,274 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson MCDE display bus driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/export.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/notifier.h>
+
+#include <video/mcde_display.h>
+#include <video/mcde_dss.h>
+
+#define to_mcde_display_driver(__drv) \
+	container_of((__drv), struct mcde_display_driver, driver)
+
+static BLOCKING_NOTIFIER_HEAD(bus_notifier_list);
+
+static int mcde_drv_suspend(struct device *_dev, pm_message_t state);
+static int mcde_drv_resume(struct device *_dev);
+struct bus_type mcde_bus_type;
+
+static int mcde_suspend_device(struct device *dev, void *data)
+{
+	pm_message_t* state = (pm_message_t *) data;
+	if (dev->driver && dev->driver->suspend)
+		return dev->driver->suspend(dev, *state);
+	return 0;
+}
+
+static int mcde_resume_device(struct device *dev, void *data)
+{
+	if (dev->driver && dev->driver->resume)
+		return dev->driver->resume(dev);
+	return 0;
+}
+
+/* Bus driver */
+
+static int mcde_bus_match(struct device *_dev, struct device_driver *driver)
+{
+	pr_debug("Matching device %s with driver %s\n",
+		dev_name(_dev), driver->name);
+
+	return strncmp(dev_name(_dev), driver->name, strlen(driver->name)) == 0;
+}
+
+static int mcde_bus_suspend(struct device *_dev, pm_message_t state)
+{
+	int ret;
+	ret = bus_for_each_dev(&mcde_bus_type, NULL, &state,
+				mcde_suspend_device);
+	if (ret) {
+		/* TODO Resume all suspended devices */
+		/* mcde_bus_resume(dev); */
+		return ret;
+	}
+	return 0;
+}
+
+static int mcde_bus_resume(struct device *_dev)
+{
+	return bus_for_each_dev(&mcde_bus_type, NULL, NULL, mcde_resume_device);
+}
+
+struct bus_type mcde_bus_type = {
+	.name = "mcde_bus",
+	.match = mcde_bus_match,
+	.suspend = mcde_bus_suspend,
+	.resume = mcde_bus_resume,
+};
+
+static int mcde_drv_probe(struct device *_dev)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	return drv->probe(dev);
+}
+
+static int mcde_drv_remove(struct device *_dev)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	return drv->remove(dev);
+}
+
+static void mcde_drv_shutdown(struct device *_dev)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	drv->shutdown(dev);
+}
+
+static int mcde_drv_suspend(struct device *_dev, pm_message_t state)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	if (drv->suspend)
+		return drv->suspend(dev, state);
+	else
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+		return dev->set_power_mode(dev, MCDE_DISPLAY_PM_OFF);
+#else
+		return 0;
+#endif
+}
+
+static int mcde_drv_resume(struct device *_dev)
+{
+	struct mcde_display_driver *drv = to_mcde_display_driver(_dev->driver);
+	struct mcde_display_device *dev = to_mcde_display_device(_dev);
+
+	if (drv->resume)
+		return drv->resume(dev);
+	else
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+		return dev->set_power_mode(dev, MCDE_DISPLAY_PM_STANDBY);
+#else
+		return 0;
+#endif
+}
+
+/* Bus device */
+
+static void mcde_bus_release(struct device *dev)
+{
+}
+
+struct device mcde_bus = {
+	.init_name = "mcde_bus",
+	.release  = mcde_bus_release
+};
+
+/* Public bus API */
+
+int mcde_display_driver_register(struct mcde_display_driver *drv)
+{
+	drv->driver.bus = &mcde_bus_type;
+	if (drv->probe)
+		drv->driver.probe = mcde_drv_probe;
+	if (drv->remove)
+		drv->driver.remove = mcde_drv_remove;
+	if (drv->shutdown)
+		drv->driver.shutdown = mcde_drv_shutdown;
+	drv->driver.suspend = mcde_drv_suspend;
+	drv->driver.resume = mcde_drv_resume;
+
+	return driver_register(&drv->driver);
+}
+EXPORT_SYMBOL(mcde_display_driver_register);
+
+void mcde_display_driver_unregister(struct mcde_display_driver *drv)
+{
+	driver_unregister(&drv->driver);
+}
+EXPORT_SYMBOL(mcde_display_driver_unregister);
+
+static void mcde_display_dev_release(struct device *dev)
+{
+	/* Do nothing */
+}
+
+int mcde_display_device_register(struct mcde_display_device *dev)
+{
+	/* Setup device */
+	if (!dev)
+		return -EINVAL;
+	dev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
+	dev->dev.bus = &mcde_bus_type;
+	if (dev->dev.parent != NULL)
+		dev->dev.parent = &mcde_bus;
+	dev->dev.release = mcde_display_dev_release;
+	if (dev->id != -1)
+		dev_set_name(&dev->dev, "%s.%d", dev->name,  dev->id);
+	else
+		dev_set_name(&dev->dev, dev->name);
+
+	mcde_display_init_device(dev);
+
+	return device_register(&dev->dev);
+}
+EXPORT_SYMBOL(mcde_display_device_register);
+
+void mcde_display_device_unregister(struct mcde_display_device *dev)
+{
+	device_unregister(&dev->dev);
+}
+EXPORT_SYMBOL(mcde_display_device_unregister);
+
+/* Notifications */
+int mcde_dss_register_notifier(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_register(&bus_notifier_list, nb);
+}
+EXPORT_SYMBOL(mcde_dss_register_notifier);
+
+int mcde_dss_unregister_notifier(struct notifier_block *nb)
+{
+	return blocking_notifier_chain_unregister(&bus_notifier_list, nb);
+}
+EXPORT_SYMBOL(mcde_dss_unregister_notifier);
+
+static int bus_notify_callback(struct notifier_block *nb,
+	unsigned long event, void *dev)
+{
+	struct mcde_display_device *ddev = to_mcde_display_device(dev);
+
+	if (event == BUS_NOTIFY_BOUND_DRIVER) {
+		ddev->initialized = true;
+		blocking_notifier_call_chain(&bus_notifier_list,
+			MCDE_DSS_EVENT_DISPLAY_REGISTERED, ddev);
+	} else if (event == BUS_NOTIFY_UNBIND_DRIVER) {
+		ddev->initialized = false;
+		blocking_notifier_call_chain(&bus_notifier_list,
+			MCDE_DSS_EVENT_DISPLAY_UNREGISTERED, ddev);
+	}
+	return 0;
+}
+
+struct notifier_block bus_nb = {
+	.notifier_call = bus_notify_callback,
+};
+
+/* Driver init/exit */
+
+int __init mcde_display_init(void)
+{
+	int ret;
+
+	ret = bus_register(&mcde_bus_type);
+	if (ret) {
+		pr_warning("Unable to register bus type\n");
+		goto no_bus_registration;
+	}
+	ret = device_register(&mcde_bus);
+	if (ret) {
+		pr_warning("Unable to register bus device\n");
+		goto no_device_registration;
+	}
+	ret = bus_register_notifier(&mcde_bus_type, &bus_nb);
+	if (ret) {
+		pr_warning("Unable to register bus notifier\n");
+		goto no_bus_notifier;
+	}
+
+	goto out;
+
+no_bus_notifier:
+	device_unregister(&mcde_bus);
+no_device_registration:
+	bus_unregister(&mcde_bus_type);
+no_bus_registration:
+out:
+	return ret;
+}
+
+void mcde_display_exit(void)
+{
+	bus_unregister_notifier(&mcde_bus_type, &bus_nb);
+	device_unregister(&mcde_bus);
+	bus_unregister(&mcde_bus_type);
+}
diff --git a/drivers/video/mcde/mcde_debugfs.c b/drivers/video/mcde/mcde_debugfs.c
new file mode 100644
index 00000000000..586b1787d00
--- /dev/null
+++ b/drivers/video/mcde/mcde_debugfs.c
@@ -0,0 +1,207 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE base driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/stat.h>
+#include <linux/time.h>
+#include <linux/debugfs.h>
+#include <linux/slab.h>
+#include <asm/page.h>
+
+#include "mcde_debugfs.h"
+
+#define MAX_NUM_OVERLAYS 2
+#define MAX_NUM_CHANNELS 4
+#define DEFAULT_DMESG_FPS_LOG_INTERVAL 100
+
+struct fps_info {
+	u32 enable_dmesg;
+	u32 interval_ms;
+	struct timespec timestamp_last;
+	u32 frame_counter_last;
+	u32 frame_counter;
+	u32 fpks;
+};
+
+struct overlay_info {
+	u8 id;
+	struct dentry *dentry;
+	struct fps_info fps;
+};
+
+struct channel_info {
+	u8 id;
+	struct dentry *dentry;
+	struct mcde_chnl_state *chnl;
+	struct fps_info fps;
+	struct overlay_info overlays[MAX_NUM_OVERLAYS];
+};
+
+static struct mcde_info {
+	struct device *dev;
+	struct dentry *dentry;
+	struct channel_info channels[MAX_NUM_CHANNELS];
+} mcde;
+
+/* Requires: lhs > rhs */
+static inline u32 timespec_ms_diff(struct timespec lhs, struct timespec rhs)
+{
+	struct timespec tmp_ts = timespec_sub(lhs, rhs);
+	u64 tmp_ns = (u64)timespec_to_ns(&tmp_ts);
+	do_div(tmp_ns, NSEC_PER_MSEC);
+	return (u32)tmp_ns;
+}
+
+/* Returns "frames per 1000 secs", divide by 1000 to get fps with 3 decimals */
+static u32 update_fps(struct fps_info *fps)
+{
+	struct timespec now;
+	u32 fpks = 0, ms_since_last, num_frames;
+
+	getrawmonotonic(&now);
+	fps->frame_counter++;
+
+	ms_since_last = timespec_ms_diff(now, fps->timestamp_last);
+	num_frames = fps->frame_counter - fps->frame_counter_last;
+	if (num_frames > 1 && ms_since_last >= fps->interval_ms) {
+		fpks = (num_frames * 1000000) / ms_since_last;
+		fps->timestamp_last = now;
+		fps->frame_counter_last = fps->frame_counter;
+		fps->fpks = fpks;
+	}
+
+	return fpks;
+}
+
+static void update_chnl_fps(struct channel_info *ci)
+{
+	u32 fpks = update_fps(&ci->fps);
+	if (fpks && ci->fps.enable_dmesg)
+		dev_info(mcde.dev, "FPS: chnl=%d fps=%d.%.3d\n", ci->id,
+						fpks / 1000, fpks % 1000);
+}
+
+static void update_ovly_fps(struct channel_info *ci, struct overlay_info *oi)
+{
+	u32 fpks = update_fps(&oi->fps);
+	if (fpks && oi->fps.enable_dmesg)
+		dev_info(mcde.dev, "FPS: ovly=%d.%d fps=%d.%.3d\n", ci->id,
+					oi->id, fpks / 1000, fpks % 1000);
+}
+
+int mcde_debugfs_create(struct device *dev)
+{
+	if (mcde.dev)
+		return -EBUSY;
+
+	mcde.dentry = debugfs_create_dir("mcde", NULL);
+	if (!mcde.dentry)
+		return -ENOMEM;
+	mcde.dev = dev;
+
+	return 0;
+}
+
+static struct channel_info *find_chnl(u8 chnl_id)
+{
+	if (chnl_id > MAX_NUM_CHANNELS)
+		return NULL;
+	return &mcde.channels[chnl_id];
+}
+
+static struct overlay_info *find_ovly(struct channel_info *ci, u8 ovly_id)
+{
+	if (!ci || ovly_id >= MAX_NUM_OVERLAYS)
+		return NULL;
+	return &ci->overlays[ovly_id];
+}
+
+static void create_fps_files(struct dentry *dentry, struct fps_info *fps)
+{
+	debugfs_create_u32("frame_counter", S_IRUGO, dentry,
+							&fps->frame_counter);
+	debugfs_create_u32("frames_per_ksecs", S_IRUGO, dentry, &fps->fpks);
+	debugfs_create_u32("interval_ms", S_IRUGO|S_IWUGO, dentry,
+							&fps->interval_ms);
+	debugfs_create_u32("dmesg", S_IRUGO|S_IWUGO, dentry,
+							&fps->enable_dmesg);
+}
+
+int mcde_debugfs_channel_create(u8 chnl_id, struct mcde_chnl_state *chnl)
+{
+	struct channel_info *ci = find_chnl(chnl_id);
+	char name[10];
+
+	if (!chnl || !ci)
+		return -EINVAL;
+	if (ci->chnl)
+		return -EBUSY;
+
+	snprintf(name, sizeof(name), "chnl%d", chnl_id);
+	ci->dentry = debugfs_create_dir(name, mcde.dentry);
+	if (!ci->dentry)
+		return -ENOMEM;
+
+	create_fps_files(ci->dentry, &ci->fps);
+
+	ci->fps.interval_ms = DEFAULT_DMESG_FPS_LOG_INTERVAL;
+	ci->id = chnl_id;
+	ci->chnl = chnl;
+
+	return 0;
+}
+
+int mcde_debugfs_overlay_create(u8 chnl_id, u8 ovly_id)
+{
+	struct channel_info *ci = find_chnl(chnl_id);
+	struct overlay_info *oi = find_ovly(ci, ovly_id);
+	char name[10];
+
+	if (!oi || !ci || ovly_id >= MAX_NUM_OVERLAYS)
+		return -EINVAL;
+	if (oi->dentry)
+		return -EBUSY;
+
+	snprintf(name, sizeof(name), "ovly%d", ovly_id);
+	oi->dentry = debugfs_create_dir(name, ci->dentry);
+	if (!oi->dentry)
+		return -ENOMEM;
+
+	create_fps_files(oi->dentry, &oi->fps);
+
+	oi->fps.interval_ms = DEFAULT_DMESG_FPS_LOG_INTERVAL;
+	oi->id = ovly_id;
+
+	return 0;
+}
+
+void mcde_debugfs_channel_update(u8 chnl_id)
+{
+	struct channel_info *ci = find_chnl(chnl_id);
+
+	if (!ci || !ci->chnl)
+		return;
+
+	update_chnl_fps(ci);
+}
+
+void mcde_debugfs_overlay_update(u8 chnl_id, u8 ovly_id)
+{
+	struct channel_info *ci = find_chnl(chnl_id);
+	struct overlay_info *oi = find_ovly(ci, ovly_id);
+
+	if (!oi || !oi->dentry)
+		return;
+
+	update_ovly_fps(ci, oi);
+}
+
diff --git a/drivers/video/mcde/mcde_debugfs.h b/drivers/video/mcde/mcde_debugfs.h
new file mode 100644
index 00000000000..9f1e7f18ea5
--- /dev/null
+++ b/drivers/video/mcde/mcde_debugfs.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE base driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#ifndef __MCDE_DEBUGFS__H__
+#define __MCDE_DEBUGFS__H__
+
+#include <video/mcde.h>
+
+int mcde_debugfs_create(struct device *dev);
+int mcde_debugfs_channel_create(u8 chnl_id, struct mcde_chnl_state *chnl);
+int mcde_debugfs_overlay_create(u8 chnl_id, u8 ovly_id);
+
+void mcde_debugfs_channel_update(u8 chnl_id);
+void mcde_debugfs_overlay_update(u8 chnl_id, u8 ovly_id);
+
+#endif /* __MCDE_DEBUGFS__H__ */
+
diff --git a/drivers/video/mcde/mcde_display.c b/drivers/video/mcde/mcde_display.c
new file mode 100644
index 00000000000..9e9eb78516e
--- /dev/null
+++ b/drivers/video/mcde/mcde_display.c
@@ -0,0 +1,366 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE display driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+
+#include <video/mcde_display.h>
+
+/*temp*/
+#include <linux/delay.h>
+
+static void mcde_display_get_native_resolution_default(
+	struct mcde_display_device *ddev, u16 *x_res, u16 *y_res)
+{
+	if (x_res)
+		*x_res = ddev->native_x_res;
+	if (y_res)
+		*y_res = ddev->native_y_res;
+}
+
+static enum mcde_ovly_pix_fmt mcde_display_get_default_pixel_format_default(
+	struct mcde_display_device *ddev)
+{
+	return ddev->default_pixel_format;
+}
+
+static void mcde_display_get_physical_size_default(
+	struct mcde_display_device *ddev, u16 *width, u16 *height)
+{
+	if (width)
+		*width = ddev->physical_width;
+	if (height)
+		*height = ddev->physical_height;
+}
+
+static int mcde_display_set_power_mode_default(struct mcde_display_device *ddev,
+	enum mcde_display_power_mode power_mode)
+{
+	int ret = 0;
+
+	/* OFF -> STANDBY */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_OFF &&
+		power_mode != MCDE_DISPLAY_PM_OFF) {
+		if (ddev->platform_enable) {
+			ret = ddev->platform_enable(ddev);
+			if (ret)
+				return ret;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+		/* force register settings */
+		if (ddev->port->type == MCDE_PORTTYPE_DPI)
+			ddev->update_flags = UPDATE_FLAG_VIDEO_MODE | UPDATE_FLAG_PIXEL_FORMAT;
+	}
+
+	if (ddev->port->type == MCDE_PORTTYPE_DSI) {
+		/* STANDBY -> ON */
+		if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+			power_mode == MCDE_DISPLAY_PM_ON) {
+			ret = mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_EXIT_SLEEP_MODE, NULL, 0);
+			if (ret)
+				return ret;
+
+			ret = mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_SET_DISPLAY_ON, NULL, 0);
+			if (ret)
+				return ret;
+
+			ddev->power_mode = MCDE_DISPLAY_PM_ON;
+		} else if (ddev->power_mode == MCDE_DISPLAY_PM_ON &&
+			power_mode <= MCDE_DISPLAY_PM_STANDBY) {
+			/* ON -> STANDBY */
+			ret = mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_SET_DISPLAY_OFF, NULL, 0);
+			if (ret)
+				return ret;
+
+			ret = mcde_dsi_dcs_write(ddev->chnl_state,
+				DCS_CMD_ENTER_SLEEP_MODE, NULL, 0);
+			if (ret)
+				return ret;
+
+			ddev->power_mode = MCDE_DISPLAY_PM_STANDBY;
+		}
+	} else if (ddev->port->type == MCDE_PORTTYPE_DPI) {
+		ddev->power_mode = power_mode;
+	} else if (ddev->power_mode != power_mode) {
+		return -EINVAL;
+	}
+
+	/* SLEEP -> OFF */
+	if (ddev->power_mode == MCDE_DISPLAY_PM_STANDBY &&
+		power_mode == MCDE_DISPLAY_PM_OFF) {
+		if (ddev->platform_disable) {
+			ret = ddev->platform_disable(ddev);
+			if (ret)
+				return ret;
+		}
+		ddev->power_mode = MCDE_DISPLAY_PM_OFF;
+	}
+
+	mcde_chnl_set_power_mode(ddev->chnl_state, ddev->power_mode);
+
+	return ret;
+}
+
+static inline enum mcde_display_power_mode mcde_display_get_power_mode_default(
+	struct mcde_display_device *ddev)
+{
+	return ddev->power_mode;
+}
+
+static inline int mcde_display_try_video_mode_default(
+	struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode)
+{
+	/*
+	 * DSI video mode:
+	 * This function is intended for configuring supported video mode(s).
+	 * Overload it into the panel driver file and set up blanking
+	 * intervals and pixel clock according to below recommendations.
+	 *
+	 * vertical blanking parameters vbp, vfp, vsw are given in lines
+	 * horizontal blanking parameters hbp, hfp, hsw are given in pixels
+	 *
+	 * video_mode->pixclock is the time between two pixels (in picoseconds)
+	 * The source of the pixel clock is DSI PLL and it shall be set to
+	 * meet the requirement
+	 *
+	 * non-burst mode:
+	 * pixel clock (Hz) = (VACT+VBP+VFP+VSA) * (HACT+HBP+HFP+HSA) *
+	 *                    framerate * bpp / num_data_lanes
+	 *
+	 * burst mode:
+	 * pixel clock (Hz) > (VACT+VBP+VFP+VSA) * (HACT+HBP+HFP+HSA) *
+	 *                    framerate * bpp / num_data_lanes * 1.1
+	 * (1.1 is a 10% margin needed for burst mode calculations)
+	 */
+	return 0;
+}
+
+static int mcde_display_set_video_mode_default(struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode)
+{
+	int ret;
+	struct mcde_video_mode channel_video_mode;
+
+	if (!video_mode)
+		return -EINVAL;
+
+	ddev->video_mode = *video_mode;
+	channel_video_mode = ddev->video_mode;
+	/* Dependant on if display should rotate or MCDE should rotate */
+	if (ddev->rotation == MCDE_DISPLAY_ROT_90_CCW ||
+				ddev->rotation == MCDE_DISPLAY_ROT_90_CW) {
+		channel_video_mode.xres = ddev->native_x_res;
+		channel_video_mode.yres = ddev->native_y_res;
+	}
+	ret = mcde_chnl_set_video_mode(ddev->chnl_state, &channel_video_mode);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to set video mode\n", __func__);
+		return ret;
+	}
+
+	ddev->update_flags |= UPDATE_FLAG_VIDEO_MODE;
+
+	return 0;
+}
+
+static inline void mcde_display_get_video_mode_default(
+	struct mcde_display_device *ddev, struct mcde_video_mode *video_mode)
+{
+	if (video_mode)
+		*video_mode = ddev->video_mode;
+}
+
+static int mcde_display_set_pixel_format_default(
+	struct mcde_display_device *ddev, enum mcde_ovly_pix_fmt format)
+{
+	int ret;
+
+	ddev->pixel_format = format;
+	ret = mcde_chnl_set_pixel_format(ddev->chnl_state,
+						ddev->port->pixel_format);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to set pixel format = %d\n",
+							__func__, format);
+		return ret;
+	}
+
+	return 0;
+}
+
+static inline enum mcde_ovly_pix_fmt mcde_display_get_pixel_format_default(
+	struct mcde_display_device *ddev)
+{
+	return ddev->pixel_format;
+}
+
+
+static int mcde_display_set_rotation_default(struct mcde_display_device *ddev,
+	enum mcde_display_rotation rotation)
+{
+	int ret;
+	u8 param = 0;
+	enum mcde_display_rotation final;
+
+	final = (360 + rotation - ddev->orientation) % 360;
+	ret = mcde_chnl_set_rotation(ddev->chnl_state, final);
+	if (WARN_ON(ret))
+		return ret;
+
+	if (final == MCDE_DISPLAY_ROT_180_CW)
+		param = 0x40; /* Horizontal flip */
+	(void)mcde_dsi_dcs_write(ddev->chnl_state, DCS_CMD_SET_ADDRESS_MODE,
+								&param, 1);
+
+	ddev->rotation = rotation;
+	ddev->update_flags |= UPDATE_FLAG_ROTATION;
+
+	return 0;
+}
+
+static inline enum mcde_display_rotation mcde_display_get_rotation_default(
+	struct mcde_display_device *ddev)
+{
+	return ddev->rotation;
+}
+
+static int mcde_display_apply_config_default(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	if (!ddev->update_flags)
+		return 0;
+
+	if (ddev->update_flags & UPDATE_FLAG_VIDEO_MODE)
+		mcde_chnl_stop_flow(ddev->chnl_state);
+
+	ret = mcde_chnl_apply(ddev->chnl_state);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to apply to channel\n",
+							__func__);
+		return ret;
+	}
+	ddev->update_flags = 0;
+	ddev->first_update = true;
+
+	return 0;
+}
+
+static int mcde_display_invalidate_area_default(
+					struct mcde_display_device *ddev,
+					struct mcde_rectangle *area)
+{
+	dev_vdbg(&ddev->dev, "%s\n", __func__);
+	if (area) {
+		/* take union of rects */
+		u16 t;
+		t = min(ddev->update_area.x, area->x);
+		/* note should be > 0 */
+		ddev->update_area.w = max(ddev->update_area.x +
+							ddev->update_area.w,
+							area->x + area->w) - t;
+		ddev->update_area.x = t;
+		t = min(ddev->update_area.y, area->y);
+		ddev->update_area.h = max(ddev->update_area.y +
+							ddev->update_area.h,
+							area->y + area->h) - t;
+		ddev->update_area.y = t;
+		/* TODO: Implement real clipping when partial refresh is
+		activated.*/
+		ddev->update_area.w = min((u16) ddev->video_mode.xres,
+					(u16) ddev->update_area.w);
+		ddev->update_area.h = min((u16) ddev->video_mode.yres,
+					(u16) ddev->update_area.h);
+	} else {
+		ddev->update_area.x = 0;
+		ddev->update_area.y = 0;
+		ddev->update_area.w = ddev->video_mode.xres;
+		ddev->update_area.h = ddev->video_mode.yres;
+		/* Invalidate_area(ddev, NULL) means reset area to empty
+		 * rectangle really. After that the rectangle should grow by
+		 * taking an union (above). This means that the code should
+		 * really look like below, however the code above is a temp fix
+		 * for rotation.
+		 * TODO: fix
+		 * ddev->update_area.x = ddev->video_mode.xres;
+		 * ddev->update_area.y = ddev->video_mode.yres;
+		 * ddev->update_area.w = 0;
+		 * ddev->update_area.h = 0;
+		 */
+	}
+
+	return 0;
+}
+
+static int mcde_display_update_default(struct mcde_display_device *ddev,
+							bool tripple_buffer)
+{
+	int ret = 0;
+
+	ret = mcde_chnl_update(ddev->chnl_state, &ddev->update_area,
+							tripple_buffer);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "%s:Failed to update channel\n", __func__);
+		return ret;
+	}
+	if (ddev->first_update && ddev->on_first_update)
+		ddev->on_first_update(ddev);
+
+	if (ddev->power_mode != MCDE_DISPLAY_PM_ON && ddev->set_power_mode) {
+		ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_ON);
+		if (ret < 0) {
+			dev_warn(&ddev->dev,
+				"%s:Failed to set power mode to on\n",
+				__func__);
+			return ret;
+		}
+	}
+
+	dev_vdbg(&ddev->dev, "Overlay updated, chnl=%d\n", ddev->chnl_id);
+
+	return 0;
+}
+
+static inline int mcde_display_on_first_update_default(
+					struct mcde_display_device *ddev)
+{
+	ddev->first_update = false;
+	return 0;
+}
+
+void mcde_display_init_device(struct mcde_display_device *ddev)
+{
+	/* Setup default callbacks */
+	ddev->get_native_resolution =
+				mcde_display_get_native_resolution_default;
+	ddev->get_default_pixel_format =
+				mcde_display_get_default_pixel_format_default;
+	ddev->get_physical_size = mcde_display_get_physical_size_default;
+	ddev->set_power_mode = mcde_display_set_power_mode_default;
+	ddev->get_power_mode = mcde_display_get_power_mode_default;
+	ddev->try_video_mode = mcde_display_try_video_mode_default;
+	ddev->set_video_mode = mcde_display_set_video_mode_default;
+	ddev->get_video_mode = mcde_display_get_video_mode_default;
+	ddev->set_pixel_format = mcde_display_set_pixel_format_default;
+	ddev->get_pixel_format = mcde_display_get_pixel_format_default;
+	ddev->set_rotation = mcde_display_set_rotation_default;
+	ddev->get_rotation = mcde_display_get_rotation_default;
+	ddev->apply_config = mcde_display_apply_config_default;
+	ddev->invalidate_area = mcde_display_invalidate_area_default;
+	ddev->update = mcde_display_update_default;
+	ddev->on_first_update = mcde_display_on_first_update_default;
+
+	mutex_init(&ddev->display_lock);
+}
+
diff --git a/drivers/video/mcde/mcde_dss.c b/drivers/video/mcde/mcde_dss.c
new file mode 100644
index 00000000000..35a6cbe9540
--- /dev/null
+++ b/drivers/video/mcde/mcde_dss.c
@@ -0,0 +1,445 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE display sub system driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/export.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+
+#include <video/mcde_dss.h>
+
+#define to_overlay(x) container_of(x, struct mcde_overlay, kobj)
+
+void overlay_release(struct kobject *kobj)
+{
+	struct mcde_overlay *ovly = to_overlay(kobj);
+
+	kfree(ovly);
+}
+
+struct kobj_type ovly_type = {
+	.release = overlay_release,
+};
+
+static int apply_overlay(struct mcde_overlay *ovly,
+				struct mcde_overlay_info *info, bool force)
+{
+	int ret = 0;
+	if (ovly->ddev->invalidate_area) {
+		/* TODO: transform ovly coord to screen coords (vmode):
+		 * add offset
+		 */
+		struct mcde_rectangle dirty = info->dirty;
+		mutex_lock(&ovly->ddev->display_lock);
+		ret = ovly->ddev->invalidate_area(ovly->ddev, &dirty);
+		mutex_unlock(&ovly->ddev->display_lock);
+	}
+
+	if (ovly->info.paddr != info->paddr || force)
+		mcde_ovly_set_source_buf(ovly->state, info->paddr);
+
+	if (ovly->info.stride != info->stride || ovly->info.fmt != info->fmt ||
+									force)
+		mcde_ovly_set_source_info(ovly->state, info->stride, info->fmt);
+	if (ovly->info.src_x != info->src_x ||
+					ovly->info.src_y != info->src_y ||
+					ovly->info.w != info->w ||
+					ovly->info.h != info->h || force)
+		mcde_ovly_set_source_area(ovly->state,
+				info->src_x, info->src_y, info->w, info->h);
+	if (ovly->info.dst_x != info->dst_x || ovly->info.dst_y != info->dst_y
+					|| ovly->info.dst_z != info->dst_z ||
+					force)
+		mcde_ovly_set_dest_pos(ovly->state,
+					info->dst_x, info->dst_y, info->dst_z);
+
+	mcde_ovly_apply(ovly->state);
+	ovly->info = *info;
+
+	return ret;
+}
+
+/* MCDE DSS operations */
+
+int mcde_dss_open_channel(struct mcde_display_device *ddev)
+{
+	int ret = 0;
+	struct mcde_chnl_state *chnl;
+
+	mutex_lock(&ddev->display_lock);
+	/* Acquire MCDE resources */
+	chnl = mcde_chnl_get(ddev->chnl_id, ddev->fifo, ddev->port);
+	if (IS_ERR(chnl)) {
+		ret = PTR_ERR(chnl);
+		dev_warn(&ddev->dev, "Failed to acquire MCDE channel\n");
+		goto chnl_get_failed;
+	}
+	ddev->chnl_state = chnl;
+chnl_get_failed:
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_open_channel);
+
+void mcde_dss_close_channel(struct mcde_display_device *ddev)
+{
+	mutex_lock(&ddev->display_lock);
+	mcde_chnl_put(ddev->chnl_state);
+	ddev->chnl_state = NULL;
+	mutex_unlock(&ddev->display_lock);
+}
+EXPORT_SYMBOL(mcde_dss_close_channel);
+
+int mcde_dss_enable_display(struct mcde_display_device *ddev)
+{
+	int ret;
+
+	if (ddev->enabled)
+		return 0;
+
+	mutex_lock(&ddev->display_lock);
+	mcde_chnl_enable(ddev->chnl_state);
+
+	/* Initiate display communication */
+	ret = ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_STANDBY);
+	if (ret < 0) {
+		dev_warn(&ddev->dev, "Failed to initialize display\n");
+		goto display_failed;
+	}
+
+	ret = ddev->set_rotation(ddev, ddev->get_rotation(ddev));
+	if (ret < 0)
+		dev_warn(&ddev->dev, "Failed to set rotation\n");
+
+	dev_dbg(&ddev->dev, "Display enabled, chnl=%d\n",
+					ddev->chnl_id);
+	ddev->enabled = true;
+	mutex_unlock(&ddev->display_lock);
+
+	return 0;
+
+display_failed:
+	mcde_chnl_disable(ddev->chnl_state);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_enable_display);
+
+void mcde_dss_disable_display(struct mcde_display_device *ddev)
+{
+	if (!ddev->enabled)
+		return;
+
+	/* TODO: Disable overlays */
+	mutex_lock(&ddev->display_lock);
+
+	mcde_chnl_stop_flow(ddev->chnl_state);
+
+	(void)ddev->set_power_mode(ddev, MCDE_DISPLAY_PM_OFF);
+
+	mcde_chnl_disable(ddev->chnl_state);
+
+	ddev->enabled = false;
+	mutex_unlock(&ddev->display_lock);
+
+	dev_dbg(&ddev->dev, "Display disabled, chnl=%d\n", ddev->chnl_id);
+}
+EXPORT_SYMBOL(mcde_dss_disable_display);
+
+int mcde_dss_apply_channel(struct mcde_display_device *ddev)
+{
+	int ret;
+	if (!ddev->apply_config)
+		return -EINVAL;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->apply_config(ddev);
+	mutex_unlock(&ddev->display_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_apply_channel);
+
+struct mcde_overlay *mcde_dss_create_overlay(struct mcde_display_device *ddev,
+	struct mcde_overlay_info *info)
+{
+	struct mcde_overlay *ovly;
+
+	ovly = kzalloc(sizeof(struct mcde_overlay), GFP_KERNEL);
+	if (!ovly)
+		return NULL;
+
+	kobject_init(&ovly->kobj, &ovly_type); /* Local ref */
+	kobject_get(&ovly->kobj); /* Creator ref */
+	INIT_LIST_HEAD(&ovly->list);
+	mutex_lock(&ddev->display_lock);
+	list_add(&ddev->ovlys, &ovly->list);
+	mutex_unlock(&ddev->display_lock);
+	ovly->info = *info;
+	ovly->ddev = ddev;
+
+	return ovly;
+}
+EXPORT_SYMBOL(mcde_dss_create_overlay);
+
+void mcde_dss_destroy_overlay(struct mcde_overlay *ovly)
+{
+	list_del(&ovly->list);
+	if (ovly->state)
+		mcde_dss_disable_overlay(ovly);
+	kobject_put(&ovly->kobj);
+}
+EXPORT_SYMBOL(mcde_dss_destroy_overlay);
+
+int mcde_dss_enable_overlay(struct mcde_overlay *ovly)
+{
+	int ret;
+
+	if (!ovly->ddev->chnl_state)
+		return -EINVAL;
+
+	if (!ovly->state) {
+		struct mcde_ovly_state *state;
+		state = mcde_ovly_get(ovly->ddev->chnl_state);
+		if (IS_ERR(state)) {
+			ret = PTR_ERR(state);
+			dev_warn(&ovly->ddev->dev,
+				"Failed to acquire overlay\n");
+			return ret;
+		}
+		ovly->state = state;
+	}
+
+	apply_overlay(ovly, &ovly->info, true);
+
+	dev_vdbg(&ovly->ddev->dev, "Overlay enabled, chnl=%d\n",
+							ovly->ddev->chnl_id);
+	return 0;
+}
+EXPORT_SYMBOL(mcde_dss_enable_overlay);
+
+int mcde_dss_apply_overlay(struct mcde_overlay *ovly,
+						struct mcde_overlay_info *info)
+{
+	if (info == NULL)
+		info = &ovly->info;
+	return apply_overlay(ovly, info, false);
+}
+EXPORT_SYMBOL(mcde_dss_apply_overlay);
+
+void mcde_dss_disable_overlay(struct mcde_overlay *ovly)
+{
+	if (!ovly->state)
+		return;
+
+	mcde_ovly_put(ovly->state);
+
+	dev_dbg(&ovly->ddev->dev, "Overlay disabled, chnl=%d\n",
+							ovly->ddev->chnl_id);
+
+	ovly->state = NULL;
+}
+EXPORT_SYMBOL(mcde_dss_disable_overlay);
+
+int mcde_dss_update_overlay(struct mcde_overlay *ovly, bool tripple_buffer)
+{
+	int ret;
+	dev_vdbg(&ovly->ddev->dev, "Overlay update, chnl=%d\n",
+							ovly->ddev->chnl_id);
+
+	if (!ovly->state || !ovly->ddev->update || !ovly->ddev->invalidate_area)
+		return -EINVAL;
+
+	mutex_lock(&ovly->ddev->display_lock);
+	/* Do not perform an update if power mode is off */
+	if (ovly->ddev->get_power_mode(ovly->ddev) == MCDE_DISPLAY_PM_OFF) {
+		ret = 0;
+		goto power_mode_off;
+	}
+
+	ret = ovly->ddev->update(ovly->ddev, tripple_buffer);
+	if (ret)
+		goto update_failed;
+
+	ret = ovly->ddev->invalidate_area(ovly->ddev, NULL);
+
+power_mode_off:
+update_failed:
+	mutex_unlock(&ovly->ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_update_overlay);
+
+void mcde_dss_get_overlay_info(struct mcde_overlay *ovly,
+				struct mcde_overlay_info *info) {
+	if (info)
+		*info = ovly->info;
+}
+EXPORT_SYMBOL(mcde_dss_get_overlay_info);
+
+void mcde_dss_get_native_resolution(struct mcde_display_device *ddev,
+	u16 *x_res, u16 *y_res)
+{
+	u16 x_tmp, y_tmp;
+	mutex_lock(&ddev->display_lock);
+	ddev->get_native_resolution(ddev, &x_tmp, &y_tmp);
+	if (ddev->orientation == MCDE_DISPLAY_ROT_90_CW ||
+				ddev->orientation == MCDE_DISPLAY_ROT_90_CCW) {
+		*x_res = y_tmp;
+		*y_res = x_tmp;
+	} else {
+		*x_res = x_tmp;
+		*y_res = y_tmp;
+	}
+	mutex_unlock(&ddev->display_lock);
+}
+EXPORT_SYMBOL(mcde_dss_get_native_resolution);
+
+enum mcde_ovly_pix_fmt mcde_dss_get_default_pixel_format(
+	struct mcde_display_device *ddev)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->get_default_pixel_format(ddev);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_get_default_pixel_format);
+
+void mcde_dss_get_physical_size(struct mcde_display_device *ddev,
+	u16 *physical_width, u16 *physical_height)
+{
+	mutex_lock(&ddev->display_lock);
+	ddev->get_physical_size(ddev, physical_width, physical_height);
+	mutex_unlock(&ddev->display_lock);
+}
+EXPORT_SYMBOL(mcde_dss_get_physical_size);
+
+int mcde_dss_try_video_mode(struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->try_video_mode(ddev, video_mode);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_try_video_mode);
+
+int mcde_dss_set_video_mode(struct mcde_display_device *ddev,
+	struct mcde_video_mode *vmode)
+{
+	int ret = 0;
+	struct mcde_video_mode old_vmode;
+
+	mutex_lock(&ddev->display_lock);
+	/* Do not perform set_video_mode if power mode is off */
+	if (ddev->get_power_mode(ddev) == MCDE_DISPLAY_PM_OFF)
+		goto power_mode_off;
+
+	ddev->get_video_mode(ddev, &old_vmode);
+	if (memcmp(vmode, &old_vmode, sizeof(old_vmode)) == 0)
+		goto same_video_mode;
+
+	ret = ddev->set_video_mode(ddev, vmode);
+	if (ret)
+		goto set_video_mode_failed;
+
+	if (ddev->invalidate_area)
+		ret = ddev->invalidate_area(ddev, NULL);
+power_mode_off:
+same_video_mode:
+set_video_mode_failed:
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_set_video_mode);
+
+void mcde_dss_get_video_mode(struct mcde_display_device *ddev,
+	struct mcde_video_mode *video_mode)
+{
+	mutex_lock(&ddev->display_lock);
+	ddev->get_video_mode(ddev, video_mode);
+	mutex_unlock(&ddev->display_lock);
+}
+EXPORT_SYMBOL(mcde_dss_get_video_mode);
+
+int mcde_dss_set_pixel_format(struct mcde_display_device *ddev,
+	enum mcde_ovly_pix_fmt pix_fmt)
+{
+	enum mcde_ovly_pix_fmt old_pix_fmt;
+	int ret;
+
+	mutex_lock(&ddev->display_lock);
+	old_pix_fmt = ddev->get_pixel_format(ddev);
+	if (old_pix_fmt == pix_fmt) {
+		ret = 0;
+		goto same_pixel_format;
+	}
+
+	ret = ddev->set_pixel_format(ddev, pix_fmt);
+
+same_pixel_format:
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_set_pixel_format);
+
+int mcde_dss_get_pixel_format(struct mcde_display_device *ddev)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->get_pixel_format(ddev);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_get_pixel_format);
+
+int mcde_dss_set_rotation(struct mcde_display_device *ddev,
+	enum mcde_display_rotation rotation)
+{
+	int ret;
+	enum mcde_display_rotation old_rotation;
+
+	mutex_lock(&ddev->display_lock);
+	old_rotation = ddev->get_rotation(ddev);
+	if (old_rotation == rotation) {
+		ret = 0;
+		goto same_rotation;
+	}
+
+	ret = ddev->set_rotation(ddev, rotation);
+same_rotation:
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_set_rotation);
+
+enum mcde_display_rotation mcde_dss_get_rotation(
+	struct mcde_display_device *ddev)
+{
+	int ret;
+	mutex_lock(&ddev->display_lock);
+	ret = ddev->get_rotation(ddev);
+	mutex_unlock(&ddev->display_lock);
+	return ret;
+}
+EXPORT_SYMBOL(mcde_dss_get_rotation);
+
+int __init mcde_dss_init(void)
+{
+	return 0;
+}
+
+void mcde_dss_exit(void)
+{
+}
+
diff --git a/drivers/video/mcde/mcde_fb.c b/drivers/video/mcde/mcde_fb.c
new file mode 100644
index 00000000000..7d877ec3e4f
--- /dev/null
+++ b/drivers/video/mcde/mcde_fb.c
@@ -0,0 +1,898 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson MCDE frame buffer driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/fb.h>
+#include <linux/mm.h>
+#include <linux/dma-mapping.h>
+
+#include <linux/hwmem.h>
+#include <linux/io.h>
+
+#include <linux/console.h>
+
+#include <video/mcde_fb.h>
+
+#define MCDE_FB_BPP_MAX		16
+#define MCDE_FB_VXRES_MAX	1920
+#define MCDE_FB_VYRES_MAX	2160
+
+static struct fb_ops fb_ops;
+
+struct pix_fmt_info {
+	enum mcde_ovly_pix_fmt pix_fmt;
+
+	u32		   bpp;
+	struct fb_bitfield r;
+	struct fb_bitfield g;
+	struct fb_bitfield b;
+	struct fb_bitfield a;
+	u32                nonstd;
+};
+
+struct pix_fmt_info pix_fmt_map[] = {
+	{
+		.pix_fmt = MCDE_OVLYPIXFMT_RGB565,
+		.bpp = 16,
+		.r = { .offset = 11, .length = 5 },
+		.g = { .offset =  5, .length = 6 },
+		.b = { .offset =  0, .length = 5 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGBA5551,
+		.bpp = 16,
+		.r = { .offset = 11, .length = 5 },
+		.g = { .offset =  6, .length = 5 },
+		.b = { .offset =  1, .length = 5 },
+		.a = { .offset =  0, .length = 1 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGBA4444,
+		.bpp = 16,
+		.r = { .offset = 12, .length = 4 },
+		.g = { .offset =  8, .length = 4 },
+		.b = { .offset =  4, .length = 4 },
+		.a = { .offset =  0, .length = 4 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_YCbCr422,
+		.bpp = 16,
+		.nonstd = MCDE_OVLYPIXFMT_YCbCr422,
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGB888,
+		.bpp = 24,
+		.r = { .offset = 16, .length = 8 },
+		.g = { .offset =  8, .length = 8 },
+		.b = { .offset =  0, .length = 8 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGBA8888,
+		.bpp = 32,
+		.r = { .offset = 16, .length = 8 },
+		.g = { .offset =  8, .length = 8 },
+		.b = { .offset =  0, .length = 8 },
+		.a = { .offset = 24, .length = 8 },
+	}, {
+		.pix_fmt = MCDE_OVLYPIXFMT_RGBX8888,
+		.bpp = 32,
+		.r = { .offset = 16, .length = 8 },
+		.g = { .offset =  8, .length = 8 },
+		.b = { .offset =  0, .length = 8 },
+	}
+
+};
+
+static struct platform_device mcde_fb_device = {
+	.name = "mcde_fb",
+	.id = -1,
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void early_suspend(struct early_suspend *data)
+{
+	int i;
+	struct mcde_fb *mfb =
+		container_of(data, struct mcde_fb, early_suspend);
+
+	console_lock();
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		if (mfb->ovlys[i] && mfb->ovlys[i]->ddev &&
+				(mfb->ovlys[i]->ddev->stay_alive == false))
+			mcde_dss_disable_display(mfb->ovlys[i]->ddev);
+	}
+	console_unlock();
+}
+
+static void late_resume(struct early_suspend *data)
+{
+	int i;
+	struct mcde_fb *mfb =
+		container_of(data, struct mcde_fb, early_suspend);
+
+	console_lock();
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		if (mfb->ovlys[i]) {
+			struct mcde_overlay *ovly = mfb->ovlys[i];
+			(void) mcde_dss_enable_display(ovly->ddev);
+		}
+	}
+	console_unlock();
+}
+#endif
+
+/* Helpers */
+
+static struct pix_fmt_info *find_pix_fmt_info(enum mcde_ovly_pix_fmt pix_fmt)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pix_fmt_map); i++) {
+		if (pix_fmt_map[i].pix_fmt == pix_fmt)
+			return &pix_fmt_map[i];
+	}
+	return NULL;
+}
+
+static bool bitfield_cmp(struct fb_bitfield *bf1, struct fb_bitfield *bf2)
+{
+	return bf1->offset == bf2->offset &&
+		bf1->length == bf2->length &&
+		bf1->msb_right == bf2->msb_right;
+}
+
+static struct pix_fmt_info *var_to_pix_fmt_info(struct fb_var_screeninfo *var)
+{
+	int i;
+	struct pix_fmt_info *info;
+
+	if (var->nonstd)
+		return find_pix_fmt_info(var->nonstd);
+
+	for (i = 0; i < ARRAY_SIZE(pix_fmt_map); i++) {
+		info = &pix_fmt_map[i];
+		if (info->bpp == var->bits_per_pixel &&
+					bitfield_cmp(&info->r, &var->red) &&
+					bitfield_cmp(&info->g, &var->green) &&
+					bitfield_cmp(&info->b, &var->blue) &&
+					bitfield_cmp(&info->a, &var->transp))
+			return info;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(pix_fmt_map); i++) {
+		info = &pix_fmt_map[i];
+		if (var->bits_per_pixel == info->bpp)
+			return info;
+	}
+
+	return NULL;
+}
+
+static void pix_fmt_info_to_var(struct pix_fmt_info *pix_fmt_info,
+	struct fb_var_screeninfo *var)
+{
+	var->bits_per_pixel = pix_fmt_info->bpp;
+	var->nonstd = pix_fmt_info->nonstd;
+	var->red = pix_fmt_info->r;
+	var->green = pix_fmt_info->g;
+	var->blue = pix_fmt_info->b;
+	var->transp = pix_fmt_info->a;
+}
+
+static int init_var_fmt(struct fb_var_screeninfo *var,
+	u16 w, u16 h, u16 vw, u16 vh, enum mcde_ovly_pix_fmt pix_fmt,
+	u32 rotate)
+{
+	struct pix_fmt_info *info;
+
+	info = find_pix_fmt_info(pix_fmt);
+	if (!info)
+		return -EINVAL;
+
+	var->bits_per_pixel = info->bpp;
+	var->nonstd         = info->nonstd;
+	var->red            = info->r;
+	var->green          = info->g;
+	var->blue           = info->b;
+	var->transp         = info->a;
+	var->grayscale      = false;
+
+	var->xres = w;
+	var->yres = h;
+	var->xres_virtual = vw;
+	var->yres_virtual = vh;
+	var->xoffset = 0;
+	var->yoffset = 0;
+	var->activate = FB_ACTIVATE_NOW;
+	var->rotate = rotate;
+
+	return 0;
+};
+
+static int reallocate_fb_mem(struct fb_info *fbi, u32 size)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+	void *vaddr;
+	struct hwmem_alloc *alloc;
+	struct hwmem_mem_chunk mem_chunk;
+	size_t num_mem_chunks = 1;
+	int name;
+
+	size = PAGE_ALIGN(size);
+
+	if (size == fbi->screen_size)
+		return 0;
+
+/* TODO: Remove once hwmem has support for defragmentation */
+#ifdef CONFIG_MCDE_FB_AVOID_REALLOC
+	if (!mfb->alloc) {
+		u32 old_size = size;
+
+		size = MCDE_FB_BPP_MAX / 8 * MCDE_FB_VXRES_MAX *
+			MCDE_FB_VYRES_MAX;
+#endif
+
+		alloc = hwmem_alloc(size, HWMEM_ALLOC_HINT_WRITE_COMBINE |
+				HWMEM_ALLOC_HINT_UNCACHED,
+				(HWMEM_ACCESS_READ  | HWMEM_ACCESS_WRITE |
+				HWMEM_ACCESS_IMPORT),
+				HWMEM_MEM_CONTIGUOUS_SYS);
+		if (IS_ERR(alloc))
+			return PTR_ERR(alloc);
+
+		name = hwmem_get_name(alloc);
+		if (name < 0) {
+			hwmem_release(alloc);
+			return name;
+		}
+
+		if (mfb->alloc) {
+			hwmem_kunmap(mfb->alloc);
+			hwmem_unpin(mfb->alloc);
+			hwmem_release(mfb->alloc);
+		}
+
+		(void)hwmem_pin(alloc, &mem_chunk, &num_mem_chunks);
+
+		vaddr = hwmem_kmap(alloc);
+		if (vaddr == NULL) {
+			hwmem_unpin(alloc);
+			hwmem_release(alloc);
+			return -ENOMEM;
+		}
+
+		mfb->alloc = alloc;
+		mfb->alloc_name = name;
+
+		fbi->screen_base = vaddr;
+		fbi->fix.smem_start = mem_chunk.paddr;
+
+#ifdef CONFIG_MCDE_FB_AVOID_REALLOC
+		size = old_size;
+	}
+#endif
+
+	fbi->screen_size = size;
+	fbi->fix.smem_len = size;
+
+	return 0;
+}
+
+static void free_fb_mem(struct fb_info *fbi)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	if (mfb->alloc) {
+		hwmem_kunmap(mfb->alloc);
+		hwmem_unpin(mfb->alloc);
+		hwmem_release(mfb->alloc);
+		mfb->alloc = NULL;
+		mfb->alloc_name = 0;
+
+		fbi->fix.smem_start = 0;
+		fbi->fix.smem_len = 0;
+		fbi->screen_base = 0;
+		fbi->screen_size = 0;
+	}
+}
+
+static void init_fb(struct fb_info *fbi)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	strlcpy(fbi->fix.id, "mcde_fb", sizeof(fbi->fix.id));
+	fbi->fix.type = FB_TYPE_PACKED_PIXELS;
+	fbi->fix.visual = FB_VISUAL_DIRECTCOLOR;
+	fbi->fix.xpanstep = 1;
+	fbi->fix.ypanstep = 1;
+	fbi->flags = FBINFO_HWACCEL_DISABLED;
+	fbi->fbops = &fb_ops;
+	fbi->pseudo_palette = &mfb->pseudo_palette[0];
+}
+
+static void get_ovly_info(struct fb_info *fbi, struct mcde_overlay *ovly,
+	struct mcde_overlay_info *info)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	memset(info, 0, sizeof(*info));
+	info->paddr = fbi->fix.smem_start +
+		fbi->fix.line_length * fbi->var.yoffset;
+	info->vaddr = (u32 *)(fbi->screen_base +
+		fbi->fix.line_length * fbi->var.yoffset);
+	/* TODO: move mem check to check_var/pan_display */
+	if (info->paddr + fbi->fix.line_length * fbi->var.yres >
+		fbi->fix.smem_start + fbi->fix.smem_len) {
+		info->paddr = fbi->fix.smem_start;
+		info->vaddr = (u32 *)fbi->screen_base;
+	}
+	info->fmt = mfb->pix_fmt;
+	info->stride = fbi->fix.line_length;
+	if (ovly) {
+		info->src_x = ovly->info.src_x;
+		info->src_y = ovly->info.src_y;
+		info->dst_x = ovly->info.dst_x;
+		info->dst_y = ovly->info.dst_y;
+		info->dst_z = 1;
+	} else {
+		info->src_x = 0;
+		info->src_y = 0;
+		info->dst_x = 0;
+		info->dst_y = 0;
+		info->dst_z = 1;
+	}
+	info->w = fbi->var.xres;
+	info->h = fbi->var.yres;
+	info->dirty.x = 0;
+	info->dirty.y = 0;
+	info->dirty.w = fbi->var.xres;
+	info->dirty.h = fbi->var.yres;
+}
+
+void vmode_to_var(struct mcde_video_mode *video_mode,
+	struct fb_var_screeninfo *var)
+{
+	/* TODO: use only 1 vbp and 1 vfp */
+	var->xres           = video_mode->xres;
+	var->yres           = video_mode->yres;
+	var->pixclock       = video_mode->pixclock;
+	var->upper_margin   = video_mode->vbp;
+	var->lower_margin   = video_mode->vfp;
+	var->vsync_len      = video_mode->vsw;
+	var->left_margin    = video_mode->hbp;
+	var->right_margin   = video_mode->hfp;
+	var->hsync_len      = video_mode->hsw;
+	var->vmode         &= ~FB_VMODE_INTERLACED;
+	var->vmode         |= video_mode->interlaced ?
+				FB_VMODE_INTERLACED : FB_VMODE_NONINTERLACED;
+}
+
+void var_to_vmode(struct fb_var_screeninfo *var,
+	struct mcde_video_mode *video_mode)
+{
+	video_mode->xres       = var->xres;
+	video_mode->yres       = var->yres;
+	video_mode->pixclock   = var->pixclock;
+	video_mode->vbp        = var->upper_margin;
+	video_mode->vfp        = var->lower_margin;
+	video_mode->vsw        = var->vsync_len;
+	video_mode->hbp        = var->left_margin;
+	video_mode->hfp        = var->right_margin;
+	video_mode->hsw        = var->hsync_len;
+	video_mode->interlaced = (var->vmode & FB_VMODE_INTERLACED) ==
+							FB_VMODE_INTERLACED;
+}
+
+enum mcde_display_rotation var_to_rotation(struct fb_var_screeninfo *var)
+{
+	enum mcde_display_rotation rot;
+
+	switch (var->rotate) {
+	case FB_ROTATE_UR:
+		rot = MCDE_DISPLAY_ROT_0;
+		break;
+	case FB_ROTATE_CW:
+		rot = MCDE_DISPLAY_ROT_90_CW;
+		break;
+	case FB_ROTATE_UD:
+		rot = MCDE_DISPLAY_ROT_180_CW;
+		break;
+	case FB_ROTATE_CCW:
+		rot = MCDE_DISPLAY_ROT_90_CCW;
+		break;
+	default:
+		rot = MCDE_DISPLAY_ROT_0;
+		break;
+	}
+	dev_vdbg(&mcde_fb_device.dev, "var_rot: %d -> mcde_rot: %d\n",
+							var->rotate, rot);
+	return rot;
+}
+
+static struct mcde_display_device *fb_to_display(struct fb_info *fbi)
+{
+	int i;
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		if (mfb->ovlys[i])
+			return mfb->ovlys[i]->ddev;
+	}
+	return NULL;
+}
+
+static int check_var(struct fb_var_screeninfo *var, struct fb_info *fbi,
+	struct mcde_display_device *ddev)
+{
+	int ret;
+	u16 w = -1, h = -1;
+	struct mcde_video_mode vmode;
+	struct pix_fmt_info *fmtinfo;
+
+	/* TODO: check sizes/offsets/memory validity */
+
+	/* Device physical size */
+	mcde_dss_get_physical_size(ddev, &w, &h);
+	var->width  = w;
+	var->height = h;
+
+	/* Rotation */
+	if (var->rotate > 3) {
+		dev_info(&(ddev->dev), "check_var failed var->rotate\n");
+		return -EINVAL;
+	}
+
+	/* Video mode */
+	var_to_vmode(var, &vmode);
+	ret = mcde_dss_try_video_mode(ddev, &vmode);
+	if (ret < 0) {
+		dev_vdbg(&(ddev->dev), "check_var failed "
+			"mcde_dss_try_video_mode with size = %x\n", ret);
+		return ret;
+	}
+	vmode_to_var(&vmode, var);
+
+	/* Pixel format */
+	fmtinfo = var_to_pix_fmt_info(var);
+	if (!fmtinfo) {
+		dev_vdbg(&(ddev->dev), "check_var failed fmtinfo\n");
+		return -EINVAL;
+	}
+	pix_fmt_info_to_var(fmtinfo, var);
+
+	/* Not used */
+	var->grayscale = 0;
+	var->sync = 0;
+
+	return 0;
+}
+
+static int apply_var(struct fb_info *fbi, struct mcde_display_device *ddev)
+{
+	int ret, i;
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+	struct fb_var_screeninfo *var;
+	struct mcde_video_mode vmode;
+	struct pix_fmt_info *fmt;
+	u32 line_len, size;
+
+	if (!ddev)
+		return -ENODEV;
+
+	dev_vdbg(&(ddev->dev), "%s\n", __func__);
+
+	var = &fbi->var;
+
+	ddev->check_transparency = 60;
+
+	/* Reallocate memory */
+	line_len = (fbi->var.bits_per_pixel * var->xres_virtual) / 8;
+	line_len = ALIGN(line_len, MCDE_BUF_LINE_ALIGMENT);
+	size = line_len * var->yres_virtual;
+	ret = reallocate_fb_mem(fbi, size);
+	if (ret) {
+		dev_vdbg(&(ddev->dev), "apply_var failed with"
+				"reallocate mem with size = %d\n", size);
+		return ret;
+	}
+	fbi->fix.line_length = line_len;
+
+	if (ddev->fictive)
+		goto apply_var_end;
+
+	/* Apply pixel format */
+	fmt = var_to_pix_fmt_info(var);
+	mfb->pix_fmt = fmt->pix_fmt;
+
+	/* Apply rotation */
+	mcde_dss_set_rotation(ddev, var_to_rotation(var));
+	/* Apply video mode */
+	memset(&vmode, 0, sizeof(struct mcde_video_mode));
+	var_to_vmode(var, &vmode);
+	ret = mcde_dss_set_video_mode(ddev, &vmode);
+	if (ret)
+		return ret;
+
+	mcde_dss_apply_channel(ddev);
+
+	/* Apply overlay info */
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		struct mcde_overlay *ovly = mfb->ovlys[i];
+		struct mcde_overlay_info info;
+		int num_buffers;
+
+		get_ovly_info(fbi, ovly, &info);
+		(void) mcde_dss_apply_overlay(ovly, &info);
+
+		num_buffers = var->yres_virtual / var->yres;
+		mcde_dss_update_overlay(ovly, num_buffers == 3);
+	}
+
+apply_var_end:
+	return 0;
+}
+
+/* FB ops */
+
+static int mcde_fb_open(struct fb_info *fbi, int user)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int mcde_fb_release(struct fb_info *fbi, int user)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+	return 0;
+}
+
+static int mcde_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *fbi)
+{
+	struct mcde_display_device *ddev = fb_to_display(fbi);
+
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	if (!ddev) {
+		printk(KERN_ERR "mcde_fb_check_var failed !ddev\n");
+		return -ENODEV;
+	}
+
+	return check_var(var, fbi, ddev);
+}
+
+static int mcde_fb_set_par(struct fb_info *fbi)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+	struct mcde_display_device *ddev = fb_to_display(fbi);
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	if (mfb->ovlys[0]->state == NULL &&
+		ddev->fictive == false) {
+		printk(KERN_INFO "%s() - Enable fb %p\n",
+			__func__,
+			mfb->ovlys[0]);
+		mcde_dss_enable_overlay(mfb->ovlys[0]);
+	}
+
+	return apply_var(fbi, ddev);
+}
+
+static int mcde_fb_blank(int blank, struct fb_info *fbi)
+{
+	int ret = 0;
+	struct mcde_display_device *ddev = fb_to_display(fbi);
+
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	if (ddev->fictive)
+		goto mcde_fb_blank_end;
+
+	switch (blank) {
+	case FB_BLANK_NORMAL:
+	break;
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_POWERDOWN:
+		mcde_dss_disable_display(ddev);
+	break;
+	case FB_BLANK_UNBLANK:
+		ret = mcde_dss_enable_display(ddev);
+	break;
+	default:
+		ret = -EINVAL;
+	}
+
+mcde_fb_blank_end:
+	return ret;
+}
+
+static int mcde_fb_pan_display(struct fb_var_screeninfo *var,
+	struct fb_info *fbi)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	if (var->xoffset == fbi->var.xoffset &&
+					var->yoffset == fbi->var.yoffset)
+		return 0;
+
+	fbi->var.xoffset = var->xoffset;
+	fbi->var.yoffset = var->yoffset;
+	return apply_var(fbi, fb_to_display(fbi));
+}
+
+static void mcde_fb_rotate(struct fb_info *fbi, int rotate)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+}
+
+static int mcde_fb_ioctl(struct fb_info *fbi, unsigned int cmd,
+							 unsigned long arg)
+{
+	struct mcde_fb *mfb = to_mcde_fb(fbi);
+
+	if (cmd == MCDE_GET_BUFFER_NAME_IOC)
+		return mfb->alloc_name;
+
+	return -EINVAL;
+}
+
+static int mcde_fb_setcolreg(unsigned int regno, unsigned int red,
+		unsigned int green, unsigned int blue, unsigned int transp,
+		struct fb_info *fbi)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	if (regno >= 256)
+		return 1;
+
+	if (regno < 17) {
+		u32 pseudo_val;
+		u32 r, g, b;
+
+		if (fbi->var.bits_per_pixel > 16) {
+			r = red >> 8;
+			g = green >> 8;
+			b = blue >> 8;
+		} else if (fbi->var.bits_per_pixel == 16) {
+			r = red >> (3 + 8);
+			g = green >> (2 + 8);
+			b = blue >> (3 + 8);
+		} else if (fbi->var.bits_per_pixel == 15) {
+			r = red >> (3 + 8);
+			g = green >> (3 + 8);
+			b = blue >> (3 + 8);
+		} else
+			r = b = g = (regno & 15);
+		pseudo_val  = r << fbi->var.red.offset;
+		pseudo_val |= g << fbi->var.green.offset;
+		pseudo_val |= b << fbi->var.blue.offset;
+
+		((u32 *)fbi->pseudo_palette)[regno] = pseudo_val;
+	}
+
+	return 0;
+}
+
+static int mcde_fb_setcmap(struct fb_cmap *cmap, struct fb_info *fbi)
+{
+	dev_vdbg(fbi->dev, "%s\n", __func__);
+
+	/*Nothing to see here, move along*/
+	return 0;
+}
+
+static struct fb_ops fb_ops = {
+	/* creg, cmap */
+	.owner          = THIS_MODULE,
+	.fb_open        = mcde_fb_open,
+	.fb_release     = mcde_fb_release,
+	.fb_read        = fb_sys_read,
+	.fb_write       = fb_sys_write,
+	.fb_fillrect    = sys_fillrect,
+	.fb_copyarea    = sys_copyarea,
+	.fb_imageblit   = sys_imageblit,
+	.fb_check_var   = mcde_fb_check_var,
+	.fb_set_par     = mcde_fb_set_par,
+	.fb_blank       = mcde_fb_blank,
+	.fb_pan_display = mcde_fb_pan_display,
+	.fb_rotate      = mcde_fb_rotate,
+	.fb_ioctl       = mcde_fb_ioctl,
+	.fb_setcolreg   = mcde_fb_setcolreg,
+	.fb_setcmap     = mcde_fb_setcmap,
+};
+
+/* FB driver */
+
+struct fb_info *mcde_fb_create(struct mcde_display_device *ddev,
+	u16 w, u16 h, u16 vw, u16 vh, enum mcde_ovly_pix_fmt pix_fmt,
+	u32 rotate)
+{
+	int ret = 0;
+	struct fb_info *fbi;
+	struct mcde_fb *mfb;
+	struct mcde_overlay *ovly = NULL;
+	struct mcde_overlay_info ovly_info;
+
+	dev_vdbg(&ddev->dev, "%s\n", __func__);
+	if (!ddev->initialized) {
+		dev_warn(&ddev->dev, "%s: Device not initialized\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	/* Init fb */
+	fbi = framebuffer_alloc(sizeof(struct mcde_fb), &mcde_fb_device.dev);
+	if (fbi == NULL) {
+		ret = -ENOMEM;
+		goto fb_alloc_failed;
+	}
+	init_fb(fbi);
+	mfb = to_mcde_fb(fbi);
+
+	if (ddev->fictive == false) {
+		ret = mcde_dss_open_channel(ddev);
+		if (ret)
+			goto channel_open_failed;
+
+		ret = mcde_dss_enable_display(ddev);
+		if (ret)
+			goto display_enable_failed;
+	}
+
+	/* Prepare var and allocate frame buffer memory */
+	init_var_fmt(&fbi->var, w, h, vw, vh, pix_fmt, rotate);
+	check_var(&fbi->var, fbi, ddev);
+	ret = apply_var(fbi, ddev);
+	if (ret)
+		goto apply_var_failed;
+
+	if (ddev->fictive == false)
+		mcde_dss_set_pixel_format(ddev, ddev->port->pixel_format);
+
+	/* Setup overlay */
+	get_ovly_info(fbi, NULL, &ovly_info);
+	ovly = mcde_dss_create_overlay(ddev, &ovly_info);
+	if (!ovly) {
+		ret = PTR_ERR(ovly);
+		goto ovly_alloc_failed;
+	}
+	mfb->ovlys[0] = ovly;
+	mfb->num_ovlys = 1;
+
+	if (ddev->fictive == false) {
+		ret = mcde_dss_enable_overlay(ovly);
+		if (ret)
+			goto ovly_enable_failed;
+	}
+
+	mfb->id = ddev->id;
+
+	/* Register framebuffer */
+	ret = register_framebuffer(fbi);
+	if (ret)
+		goto fb_register_failed;
+
+	ret = fb_alloc_cmap(&fbi->cmap, 256, 0);
+	if (ret)
+		dev_warn(&ddev->dev, "%s: Allocate color map memory failed!\n",
+				__func__);
+
+	ddev->fbi = fbi;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	if (ddev->fictive == false) {
+		mfb->early_suspend.level =
+				EARLY_SUSPEND_LEVEL_DISABLE_FB;
+		mfb->early_suspend.suspend = early_suspend;
+		mfb->early_suspend.resume = late_resume;
+		register_early_suspend(&mfb->early_suspend);
+	}
+#endif
+
+	goto out;
+fb_register_failed:
+	mcde_dss_disable_overlay(ovly);
+ovly_enable_failed:
+	mcde_dss_destroy_overlay(ovly);
+ovly_alloc_failed:
+	free_fb_mem(fbi);
+apply_var_failed:
+	mcde_dss_disable_display(ddev);
+display_enable_failed:
+	mcde_dss_close_channel(ddev);
+channel_open_failed:
+	framebuffer_release(fbi);
+	fbi = NULL;
+fb_alloc_failed:
+out:
+	return ret ? ERR_PTR(ret) : fbi;
+}
+EXPORT_SYMBOL(mcde_fb_create);
+
+int mcde_fb_attach_overlay(struct fb_info *fb_info, struct mcde_overlay *ovl)
+{
+	/* TODO: Attach extra overlay targets */
+	return -EINVAL;
+}
+
+void mcde_fb_destroy(struct mcde_display_device *dev)
+{
+	struct mcde_fb *mfb;
+	int i;
+
+	dev_vdbg(&dev->dev, "%s\n", __func__);
+
+	if (dev->fictive == false) {
+		mcde_dss_disable_display(dev);
+		mcde_dss_close_channel(dev);
+	}
+
+	mfb = to_mcde_fb(dev->fbi);
+	for (i = 0; i < mfb->num_ovlys; i++) {
+		if (mfb->ovlys[i])
+			mcde_dss_destroy_overlay(mfb->ovlys[i]);
+	}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	if (dev->fictive == false)
+		unregister_early_suspend(&mfb->early_suspend);
+#endif
+	fb_dealloc_cmap(&dev->fbi->cmap);
+
+	unregister_framebuffer(dev->fbi);
+	free_fb_mem(dev->fbi);
+	framebuffer_release(dev->fbi);
+	dev->fbi = NULL;
+}
+
+/* Overlay fbs' platform device */
+static int mcde_fb_probe(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static int mcde_fb_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver mcde_fb_driver = {
+	.probe  = mcde_fb_probe,
+	.remove = mcde_fb_remove,
+	.driver = {
+		.name  = "mcde_fb",
+		.owner = THIS_MODULE,
+	},
+};
+
+/* MCDE fb init */
+
+int __init mcde_fb_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&mcde_fb_driver);
+	if (ret)
+		goto fb_driver_failed;
+	ret = platform_device_register(&mcde_fb_device);
+	if (ret)
+		goto fb_device_failed;
+
+	goto out;
+fb_device_failed:
+	platform_driver_unregister(&mcde_fb_driver);
+fb_driver_failed:
+out:
+	return ret;
+}
+
+void mcde_fb_exit(void)
+{
+	platform_device_unregister(&mcde_fb_device);
+	platform_driver_unregister(&mcde_fb_driver);
+}
diff --git a/drivers/video/mcde/mcde_hw.c b/drivers/video/mcde/mcde_hw.c
new file mode 100644
index 00000000000..92cdb1ef7a9
--- /dev/null
+++ b/drivers/video/mcde/mcde_hw.c
@@ -0,0 +1,3834 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * ST-Ericsson MCDE base driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/export.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/err.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/regulator/consumer.h>
+#include <linux/clk.h>
+#include <linux/slab.h>
+#include <linux/jiffies.h>
+#include <linux/workqueue.h>
+
+#include <linux/mfd/dbx500-prcmu.h>
+
+#include <video/mcde.h>
+#include "dsilink_regs.h"
+#include "mcde_regs.h"
+#include "mcde_debugfs.h"
+
+
+/* MCDE channel states
+ *
+ * Allowed state transitions:
+ *   IDLE <-> SUSPEND
+ *   IDLE <-> DSI_READ
+ *   IDLE <-> DSI_WRITE
+ *   IDLE -> SETUP -> (WAIT_TE ->) RUNNING -> STOPPING1 -> STOPPING2 -> IDLE
+ *   WAIT_TE -> STOPPED (for missing TE to allow re-enable)
+ */
+enum chnl_state {
+	CHNLSTATE_SUSPEND,   /* HW in suspended mode, initial state */
+	CHNLSTATE_IDLE,      /* Channel aquired, but not running, FLOEN==0 */
+	CHNLSTATE_DSI_READ,  /* Executing DSI read */
+	CHNLSTATE_DSI_WRITE, /* Executing DSI write */
+	CHNLSTATE_SETUP,     /* Channel register setup to prepare for running */
+	CHNLSTATE_WAIT_TE,   /* Waiting for BTA or external TE */
+	CHNLSTATE_RUNNING,   /* Update started, FLOEN=1, FLOEN==1 */
+	CHNLSTATE_STOPPING,  /* Stopping, FLOEN=0, FLOEN==1, awaiting VCMP */
+	CHNLSTATE_STOPPED,   /* Stopped, after VCMP, FLOEN==0|1 */
+};
+
+enum dsi_lane_status {
+	DSI_LANE_STATE_START	= 0x00,
+	DSI_LANE_STATE_IDLE	= 0x01,
+	DSI_LANE_STATE_WRITE	= 0x02,
+	DSI_LANE_STATE_ULPM	= 0x03,
+};
+
+static int set_channel_state_atomic(struct mcde_chnl_state *chnl,
+							enum chnl_state state);
+static int set_channel_state_sync(struct mcde_chnl_state *chnl,
+							enum chnl_state state);
+static void stop_channel(struct mcde_chnl_state *chnl);
+static int _mcde_chnl_enable(struct mcde_chnl_state *chnl);
+static int _mcde_chnl_apply(struct mcde_chnl_state *chnl);
+static void disable_flow(struct mcde_chnl_state *chnl);
+static void enable_flow(struct mcde_chnl_state *chnl);
+static void do_softwaretrig(struct mcde_chnl_state *chnl);
+static void dsi_te_poll_req(struct mcde_chnl_state *chnl);
+static void dsi_te_poll_set_timer(struct mcde_chnl_state *chnl,
+		unsigned int timeout);
+static void dsi_te_timer_function(unsigned long value);
+static int wait_for_vcmp(struct mcde_chnl_state *chnl);
+static int probe_hw(struct platform_device *pdev);
+static void wait_for_flow_disabled(struct mcde_chnl_state *chnl);
+
+#define OVLY_TIMEOUT 100
+#define CHNL_TIMEOUT 100
+#define FLOW_STOP_TIMEOUT 20
+#define SCREEN_PPL_HIGH 1280
+#define SCREEN_PPL_CEA2 720
+#define SCREEN_LPF_CEA2 480
+#define DSI_DELAY0_CEA2_ADD 10
+
+#define MCDE_SLEEP_WATCHDOG 500
+#define DSI_TE_NO_ANSWER_TIMEOUT_INIT 2500
+#define DSI_TE_NO_ANSWER_TIMEOUT 250
+#define DSI_WAIT_FOR_ULPM_STATE_MS 1
+#define DSI_ULPM_STATE_NBR_OF_RETRIES 10
+#define DSI_READ_TIMEOUT 200
+#define DSI_WRITE_CMD_TIMEOUT 1000
+#define DSI_READ_DELAY 5
+#define DSI_READ_NBR_OF_RETRIES 2
+#define MCDE_FLOWEN_MAX_TRIAL 60
+
+#define MCDE_VERSION_4_1_3 0x04010300
+#define MCDE_VERSION_4_0_4 0x04000400
+#define MCDE_VERSION_3_0_8 0x03000800
+#define MCDE_VERSION_3_0_5 0x03000500
+#define MCDE_VERSION_1_0_4 0x01000400
+
+#define CLK_MCDE	"mcde"
+#define CLK_DPI		"lcd"
+
+static u8 *mcdeio;
+static u8 **dsiio;
+static struct platform_device *mcde_dev;
+static u8 num_dsilinks;
+static u8 num_channels;
+static u8 num_overlays;
+static int mcde_irq;
+static u32 input_fifo_size;
+static u32 output_fifo_ab_size;
+static u32 output_fifo_c0c1_size;
+
+static struct regulator *regulator_vana;
+static struct regulator *regulator_mcde_epod;
+static struct regulator *regulator_esram_epod;
+static struct clk *clock_mcde;
+
+/* TODO remove when all platforms support dsilp and dsihs clocks */
+static struct clk *clock_dsi;
+static struct clk *clock_dsi_lp;
+
+static u8 mcde_is_enabled;
+static struct delayed_work hw_timeout_work;
+static u8 dsi_pll_is_enabled;
+static u8 dsi_ifc_is_supported;
+static u8 dsi_use_clk_framework;
+static u32 mcde_clk_rate; /* In Hz */
+
+static struct mutex mcde_hw_lock;
+static inline void mcde_lock(const char *func, int line)
+{
+	mutex_lock(&mcde_hw_lock);
+	dev_vdbg(&mcde_dev->dev, "Enter MCDE: %s:%d\n", func, line);
+}
+
+static inline void mcde_unlock(const char *func, int line)
+{
+	dev_vdbg(&mcde_dev->dev, "Exit MCDE: %s:%d\n", func, line);
+	mutex_unlock(&mcde_hw_lock);
+}
+
+static inline bool mcde_trylock(const char *func, int line)
+{
+	bool locked = mutex_trylock(&mcde_hw_lock) == 1;
+	if (locked)
+		dev_vdbg(&mcde_dev->dev, "Enter MCDE: %s:%d\n", func, line);
+	return locked;
+}
+
+static u8 mcde_dynamic_power_management = true;
+
+static inline u32 dsi_rreg(int i, u32 reg)
+{
+	return readl(dsiio[i] + reg);
+}
+static inline void dsi_wreg(int i, u32 reg, u32 val)
+{
+	writel(val, dsiio[i] + reg);
+}
+
+#define dsi_rfld(__i, __reg, __fld) \
+({ \
+	const u32 mask = __reg##_##__fld##_MASK; \
+	const u32 shift = __reg##_##__fld##_SHIFT; \
+	((dsi_rreg(__i, __reg) & mask) >> shift); \
+})
+
+#define dsi_wfld(__i, __reg, __fld, __val) \
+({ \
+	const u32 mask = __reg##_##__fld##_MASK; \
+	const u32 shift = __reg##_##__fld##_SHIFT; \
+	const u32 oldval = dsi_rreg(__i, __reg); \
+	const u32 newval = ((__val) << shift); \
+	dsi_wreg(__i, __reg, (oldval & ~mask) | (newval & mask)); \
+})
+
+static inline u32 mcde_rreg(u32 reg)
+{
+	return readl(mcdeio + reg);
+}
+static inline void mcde_wreg(u32 reg, u32 val)
+{
+	writel(val, mcdeio + reg);
+}
+
+
+#define mcde_rfld(__reg, __fld) \
+({ \
+	const u32 mask = __reg##_##__fld##_MASK; \
+	const u32 shift = __reg##_##__fld##_SHIFT; \
+	((mcde_rreg(__reg) & mask) >> shift); \
+})
+
+#define mcde_wfld(__reg, __fld, __val) \
+({ \
+	const u32 mask = __reg##_##__fld##_MASK; \
+	const u32 shift = __reg##_##__fld##_SHIFT; \
+	const u32 oldval = mcde_rreg(__reg); \
+	const u32 newval = ((__val) << shift); \
+	mcde_wreg(__reg, (oldval & ~mask) | (newval & mask)); \
+})
+
+struct ovly_regs {
+	bool enabled;
+	bool dirty;
+	bool dirty_buf;
+
+	u8   ch_id;
+	u32  baseaddress0;
+	u32  baseaddress1;
+	u8   bits_per_pixel;
+	u8   bpp;
+	bool bgr;
+	bool bebo;
+	bool opq;
+	u8   col_conv;
+	u8   alpha_source;
+	u8   alpha_value;
+	u8   pixoff;
+	u16  ppl;
+	u16  lpf;
+	u16  cropx;
+	u16  cropy;
+	u16  xpos;
+	u16  ypos;
+	u8   z;
+};
+
+struct mcde_ovly_state {
+	bool inuse;
+	u8 idx; /* MCDE overlay index */
+	struct mcde_chnl_state *chnl; /* Owner channel */
+	bool dirty;
+	bool dirty_buf;
+
+	/* Staged settings */
+	u32 paddr;
+	u16 stride;
+	enum mcde_ovly_pix_fmt pix_fmt;
+
+	u16 src_x;
+	u16 src_y;
+	u16 dst_x;
+	u16 dst_y;
+	u16 dst_z;
+	u16 w;
+	u16 h;
+
+	u8 alpha_source;
+	u8 alpha_value;
+
+	/* Applied settings */
+	struct ovly_regs regs;
+};
+
+static struct mcde_ovly_state *overlays;
+
+struct chnl_regs {
+	bool dirty;
+
+	bool floen;
+	u16  x;
+	u16  y;
+	u16  ppl;
+	u16  lpf;
+	u8   bpp;
+	bool internal_clk; /* CLKTYPE field */
+	u16  pcd;
+	u8   clksel;
+	u8   cdwin;
+	u16 (*map_r)(u8);
+	u16 (*map_g)(u8);
+	u16 (*map_b)(u8);
+	bool palette_enable;
+	bool bcd;
+	bool roten;
+	u8   rotdir;
+	u32  rotbuf1;
+	u32  rotbuf2;
+	u32  rotbufsize;
+
+	/* Blending */
+	u8 blend_ctrl;
+	bool blend_en;
+	u8 alpha_blend;
+
+	/* DSI */
+	u8 dsipacking;
+};
+
+struct col_regs {
+	bool dirty;
+
+	u16 y_red;
+	u16 y_green;
+	u16 y_blue;
+	u16 cb_red;
+	u16 cb_green;
+	u16 cb_blue;
+	u16 cr_red;
+	u16 cr_green;
+	u16 cr_blue;
+	u16 off_y;
+	u16 off_cb;
+	u16 off_cr;
+};
+
+struct tv_regs {
+	bool dirty;
+
+	u16 dho; /* TV mode: left border width; destination horizontal offset */
+		 /* LCD MODE: horizontal back porch */
+	u16 alw; /* TV mode: right border width */
+		 /* LCD mode: horizontal front porch */
+	u16 hsw; /* horizontal synch width */
+	u16 dvo; /* TV mode: top border width; destination horizontal offset */
+		 /* LCD MODE: vertical back porch */
+	u16 bsl; /* TV mode: bottom border width; blanking start line */
+		 /* LCD MODE: vertical front porch */
+	/* field 1 */
+	u16 bel1; /* TV mode: field total vertical blanking lines */
+		 /* LCD mode: vertical sync width */
+	u16 fsl1; /* field vbp */
+	/* field 2 */
+	u16 bel2;
+	u16 fsl2;
+	u8 tv_mode;
+	bool sel_mode_tv;
+	bool inv_clk;
+	bool interlaced_en;
+	u32 lcdtim1;
+};
+
+struct mcde_chnl_state {
+	bool enabled;
+	bool reserved;
+	enum mcde_chnl id;
+	enum mcde_fifo fifo;
+	struct mcde_port port;
+	struct mcde_ovly_state *ovly0;
+	struct mcde_ovly_state *ovly1;
+	enum chnl_state state;
+	wait_queue_head_t state_waitq;
+	wait_queue_head_t vcmp_waitq;
+	atomic_t vcmp_cnt;
+	struct timer_list dsi_te_timer;
+	struct clk *clk_dsi_lp;
+	struct clk *clk_dsi_hs;
+	struct clk *clk_dpi;
+
+	enum mcde_display_power_mode power_mode;
+
+	/* Staged settings */
+	u16 (*map_r)(u8);
+	u16 (*map_g)(u8);
+	u16 (*map_b)(u8);
+	bool palette_enable;
+	struct mcde_video_mode vmode;
+	enum mcde_display_rotation rotation;
+	u32 rotbuf1;
+	u32 rotbuf2;
+	u32 rotbufsize;
+
+	struct mcde_col_transform rgb_2_ycbcr;
+	struct mcde_col_transform ycbcr_2_rgb;
+	struct mcde_col_transform *transform;
+
+	/* Blending */
+	u8 blend_ctrl;
+	bool blend_en;
+	u8 alpha_blend;
+
+	/* Applied settings */
+	struct chnl_regs regs;
+	struct col_regs  col_regs;
+	struct tv_regs   tv_regs;
+
+	/* an interlaced digital TV signal generates a VCMP per field */
+	bool vcmp_per_field;
+	bool even_vcmp;
+
+	bool formatter_updated;
+	bool esram_is_enabled;
+};
+
+static struct mcde_chnl_state *channels;
+/*
+ * Wait for CSM_RUNNING, all data sent for display
+ */
+static inline void wait_while_dsi_running(int lnk)
+{
+	u8 counter = DSI_READ_TIMEOUT;
+	while (dsi_rfld(lnk, DSI_CMD_MODE_STS, CSM_RUNNING) && --counter) {
+		dev_vdbg(&mcde_dev->dev,
+			"%s: DSI link %u read running state retry %u times\n"
+			, __func__, lnk, (DSI_READ_TIMEOUT - counter));
+		udelay(DSI_READ_DELAY);
+	}
+	WARN_ON(!counter);
+	if (!counter)
+		dev_warn(&mcde_dev->dev,
+			"%s: DSI link %u read timeout!\n", __func__, lnk);
+}
+
+static void enable_clocks_and_power(struct platform_device *pdev)
+{
+	struct mcde_platform_data *pdata = pdev->dev.platform_data;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	/* VANA should be enabled before a DSS hard reset */
+	if (regulator_vana)
+		WARN_ON_ONCE(regulator_enable(regulator_vana));
+
+	WARN_ON_ONCE(regulator_enable(regulator_mcde_epod));
+
+	if (!dsi_use_clk_framework)
+		pdata->platform_set_clocks();
+
+	WARN_ON_ONCE(clk_enable(clock_mcde));
+}
+
+static void disable_clocks_and_power(struct platform_device *pdev)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	clk_disable(clock_mcde);
+
+	WARN_ON_ONCE(regulator_disable(regulator_mcde_epod));
+
+	if (regulator_vana)
+		WARN_ON_ONCE(regulator_disable(regulator_vana));
+}
+
+static void update_mcde_registers(void)
+{
+	struct mcde_platform_data *pdata = mcde_dev->dev.platform_data;
+
+	/* Setup output muxing */
+	mcde_wreg(MCDE_CONF0,
+		MCDE_CONF0_IFIFOCTRLWTRMRKLVL(7) |
+		MCDE_CONF0_OUTMUX0(pdata->outmux[0]) |
+		MCDE_CONF0_OUTMUX1(pdata->outmux[1]) |
+		MCDE_CONF0_OUTMUX2(pdata->outmux[2]) |
+		MCDE_CONF0_OUTMUX3(pdata->outmux[3]) |
+		MCDE_CONF0_OUTMUX4(pdata->outmux[4]) |
+		pdata->syncmux);
+
+	mcde_wfld(MCDE_RISPP, VCMPARIS, 1);
+	mcde_wfld(MCDE_RISPP, VCMPBRIS, 1);
+	mcde_wfld(MCDE_RISPP, VCMPC0RIS, 1);
+	mcde_wfld(MCDE_RISPP, VCMPC1RIS, 1);
+
+	/* Enable channel VCMP interrupts */
+	mcde_wreg(MCDE_IMSCPP,
+		MCDE_IMSCPP_VCMPAIM(true) |
+		MCDE_IMSCPP_VCMPBIM(true) |
+		MCDE_IMSCPP_VCMPC0IM(true) |
+		MCDE_IMSCPP_VCMPC1IM(true));
+
+	mcde_wreg(MCDE_IMSCCHNL, MCDE_IMSCCHNL_CHNLAIM(0xf));
+	mcde_wreg(MCDE_IMSCERR, 0xFFFF01FF);
+
+	/* Setup sync pulse length
+	 * Setting VSPMAX=0 disables the filter and VSYNC
+	 * is generated after VSPMIN mcde cycles
+	 */
+	mcde_wreg(MCDE_VSCRC0,
+		MCDE_VSCRC0_VSPMIN(0) |
+		MCDE_VSCRC0_VSPMAX(0));
+	mcde_wreg(MCDE_VSCRC1,
+		MCDE_VSCRC1_VSPMIN(1) |
+		MCDE_VSCRC1_VSPMAX(0xff));
+}
+
+static void dsi_link_handle_reset(u8 link, bool release)
+{
+	u32 value;
+
+	value = prcmu_read(DB8500_PRCM_DSI_SW_RESET);
+	if (release) {
+		switch (link) {
+		case 0:
+			value |= DB8500_PRCM_DSI_SW_RESET_DSI0_SW_RESETN;
+			break;
+		case 1:
+			value |= DB8500_PRCM_DSI_SW_RESET_DSI1_SW_RESETN;
+			break;
+		case 2:
+			value |= DB8500_PRCM_DSI_SW_RESET_DSI2_SW_RESETN;
+			break;
+		default:
+			break;
+		}
+	} else {
+		switch (link) {
+		case 0:
+			value &= ~DB8500_PRCM_DSI_SW_RESET_DSI0_SW_RESETN;
+			break;
+		case 1:
+			value &= ~DB8500_PRCM_DSI_SW_RESET_DSI1_SW_RESETN;
+			break;
+		case 2:
+			value &= ~DB8500_PRCM_DSI_SW_RESET_DSI2_SW_RESETN;
+			break;
+		default:
+			break;
+		}
+	}
+	prcmu_write(DB8500_PRCM_DSI_SW_RESET, value);
+}
+
+static void dsi_link_switch_byte_clk(u8 link, bool to_system_clock)
+{
+	u32 value;
+
+	value = prcmu_read(DB8500_PRCM_DSI_GLITCHFREE_EN);
+	if (to_system_clock) {
+		switch (link) {
+		case 0:
+			value |= DB8500_PRCM_DSI_GLITCHFREE_EN_DSI0_BYTE_CLK;
+			break;
+		case 1:
+			value |= DB8500_PRCM_DSI_GLITCHFREE_EN_DSI1_BYTE_CLK;
+			break;
+		case 2:
+			value |= DB8500_PRCM_DSI_GLITCHFREE_EN_DSI2_BYTE_CLK;
+			break;
+		default:
+			break;
+		}
+	} else {
+		switch (link) {
+		case 0:
+			value &= ~DB8500_PRCM_DSI_GLITCHFREE_EN_DSI0_BYTE_CLK;
+			break;
+		case 1:
+			value &= ~DB8500_PRCM_DSI_GLITCHFREE_EN_DSI1_BYTE_CLK;
+			break;
+		case 2:
+			value &= ~DB8500_PRCM_DSI_GLITCHFREE_EN_DSI2_BYTE_CLK;
+			break;
+		default:
+			break;
+		}
+
+	}
+	prcmu_write(DB8500_PRCM_DSI_GLITCHFREE_EN, value);
+	dsi_wfld(link, DSI_MCTL_PLL_CTL, PLL_OUT_SEL, to_system_clock);
+}
+
+static void dsi_link_handle_ulpm(struct mcde_port *port, bool enter_ulpm)
+{
+	u8 link = port->link;
+	u8 num_data_lanes = port->phy.dsi.num_data_lanes;
+	u8 nbr_of_retries = 0;
+	u8 lane_state;
+
+	/*
+	 * The D-PHY protocol specifies the time to leave the ULP mode
+	 * in ms. It will at least take 1 ms to exit ULPM.
+	 * The ULPOUT time value is using number of system clock ticks
+	 * divided by 1000. The system clock for the DSI link is the MCDE
+	 * clock.
+	 */
+	dsi_wreg(link, DSI_MCTL_ULPOUT_TIME,
+			DSI_MCTL_ULPOUT_TIME_CKLANE_ULPOUT_TIME(0x1FF) |
+			DSI_MCTL_ULPOUT_TIME_DATA_ULPOUT_TIME(0x1FF));
+
+	if (enter_ulpm) {
+		lane_state = DSI_LANE_STATE_ULPM;
+		dsi_link_switch_byte_clk(link, true);
+	}
+
+	dsi_wfld(link, DSI_MCTL_MAIN_EN, DAT1_ULPM_REQ, enter_ulpm);
+	dsi_wfld(link, DSI_MCTL_MAIN_EN, DAT2_ULPM_REQ,
+					enter_ulpm && num_data_lanes == 2);
+	dsi_wfld(link, DSI_MCTL_MAIN_EN, CLKLANE_ULPM_REQ, enter_ulpm);
+
+	if (!enter_ulpm) {
+		lane_state = DSI_LANE_STATE_IDLE;
+		dsi_link_switch_byte_clk(link, false);
+	}
+
+	/* Wait for data lanes to enter ULPM */
+	while (dsi_rfld(link, DSI_MCTL_LANE_STS, DATLANE1_STATE)
+						!= lane_state ||
+		(dsi_rfld(link, DSI_MCTL_LANE_STS, DATLANE2_STATE)
+						!= lane_state &&
+							num_data_lanes > 1)) {
+		mdelay(DSI_WAIT_FOR_ULPM_STATE_MS);
+		if (nbr_of_retries++ == DSI_ULPM_STATE_NBR_OF_RETRIES) {
+			dev_dbg(&mcde_dev->dev,
+				"Could not enter correct state=%d (link=%d)!\n",
+							lane_state, link);
+			break;
+		}
+	}
+
+	nbr_of_retries = 0;
+	/* Wait for clock lane to enter ULPM */
+	while (dsi_rfld(link, DSI_MCTL_LANE_STS, CLKLANE_STATE)
+						!= lane_state) {
+		mdelay(DSI_WAIT_FOR_ULPM_STATE_MS);
+		if (nbr_of_retries++ == DSI_ULPM_STATE_NBR_OF_RETRIES) {
+			dev_dbg(&mcde_dev->dev,
+				"Could not enter correct state=%d (link=%d)!\n",
+							lane_state, link);
+			break;
+		}
+	}
+}
+
+static int dsi_link_enable(struct mcde_chnl_state *chnl)
+{
+	int ret = 0;
+	u8 link = chnl->port.link;
+
+	if (dsi_use_clk_framework) {
+		WARN_ON_ONCE(clk_enable(chnl->clk_dsi_lp));
+		WARN_ON_ONCE(clk_enable(chnl->clk_dsi_hs));
+		dsi_link_handle_reset(link, true);
+	} else {
+		WARN_ON_ONCE(clk_enable(clock_dsi));
+		WARN_ON_ONCE(clk_enable(clock_dsi_lp));
+
+		if (!dsi_pll_is_enabled) {
+			struct mcde_platform_data *pdata =
+					mcde_dev->dev.platform_data;
+			ret = pdata->platform_enable_dsipll();
+			if (ret < 0) {
+				dev_warn(&mcde_dev->dev, "%s: "
+					"enable_dsipll failed ret = %d\n",
+								__func__, ret);
+				goto enable_dsipll_err;
+			}
+			dev_dbg(&mcde_dev->dev, "%s enable dsipll\n",
+								__func__);
+		}
+		dsi_pll_is_enabled++;
+	}
+
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, LINK_EN, true);
+
+	dev_dbg(&mcde_dev->dev, "DSI%d LINK_EN\n", link);
+
+	return 0;
+
+enable_dsipll_err:
+	clk_disable(clock_dsi_lp);
+	clk_disable(clock_dsi);
+	return ret;
+}
+
+static void dsi_link_disable(struct mcde_chnl_state *chnl, bool suspend)
+{
+	wait_while_dsi_running(chnl->port.link);
+	dsi_link_handle_ulpm(&chnl->port, true);
+	if (dsi_use_clk_framework) {
+		clk_disable(chnl->clk_dsi_lp);
+		clk_disable(chnl->clk_dsi_hs);
+	} else {
+		if (dsi_pll_is_enabled && (--dsi_pll_is_enabled == 0)) {
+			struct mcde_platform_data *pdata =
+				    mcde_dev->dev.platform_data;
+			dev_dbg(&mcde_dev->dev, "%s disable dsipll\n",
+								__func__);
+			pdata->platform_disable_dsipll();
+		}
+		clk_disable(clock_dsi);
+		clk_disable(clock_dsi_lp);
+	}
+}
+
+static void disable_mcde_hw(bool force_disable, bool suspend)
+{
+	int i;
+	bool mcde_up = false;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!mcde_is_enabled)
+		return;
+
+	for (i = 0; i < num_channels; i++) {
+		struct mcde_chnl_state *chnl = &channels[i];
+		if (force_disable || (chnl->enabled &&
+					chnl->state != CHNLSTATE_RUNNING)) {
+			stop_channel(chnl);
+			set_channel_state_sync(chnl, CHNLSTATE_SUSPEND);
+
+			if (chnl->formatter_updated) {
+				if (chnl->port.type == MCDE_PORTTYPE_DSI)
+					dsi_link_disable(chnl, suspend);
+				else if (chnl->port.type == MCDE_PORTTYPE_DPI)
+					clk_disable(chnl->clk_dpi);
+				chnl->formatter_updated = false;
+			}
+			if (chnl->esram_is_enabled) {
+				WARN_ON_ONCE(regulator_disable(
+							regulator_esram_epod));
+				chnl->esram_is_enabled = false;
+			}
+		} else if (chnl->enabled && chnl->state == CHNLSTATE_RUNNING) {
+			mcde_up = true;
+		}
+	}
+
+	if (mcde_up)
+		return;
+
+	free_irq(mcde_irq, &mcde_dev->dev);
+
+	disable_clocks_and_power(mcde_dev);
+
+	mcde_is_enabled = false;
+}
+
+static void dpi_video_mode_apply(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	chnl->tv_regs.interlaced_en = chnl->vmode.interlaced;
+
+	chnl->tv_regs.sel_mode_tv = chnl->port.phy.dpi.tv_mode;
+	if (chnl->tv_regs.sel_mode_tv) {
+		/* TV mode */
+		u32 bel;
+		/* -4 since hsw is excluding SAV/EAV, 2 bytes each */
+		chnl->tv_regs.hsw  = chnl->vmode.hbp + chnl->vmode.hfp - 4;
+		/* vbp_field2 = vbp_field1 + 1 */
+		chnl->tv_regs.fsl1 = chnl->vmode.vbp / 2;
+		chnl->tv_regs.fsl2 = chnl->vmode.vbp - chnl->tv_regs.fsl1;
+		/* +1 since vbp_field2 = vbp_field1 + 1 */
+		bel = chnl->vmode.vbp + chnl->vmode.vfp;
+		/* in TV mode: bel2 = bel1 + 1 */
+		chnl->tv_regs.bel1 = bel / 2;
+		chnl->tv_regs.bel2 = bel - chnl->tv_regs.bel1;
+		if (chnl->port.phy.dpi.bus_width == 4)
+			chnl->tv_regs.tv_mode = MCDE_TVCRA_TVMODE_SDTV_656P_BE;
+		else
+			chnl->tv_regs.tv_mode = MCDE_TVCRA_TVMODE_SDTV_656P;
+		chnl->tv_regs.inv_clk = true;
+	} else {
+		/* LCD mode */
+		u32 polarity;
+		chnl->tv_regs.hsw  = chnl->vmode.hsw;
+		chnl->tv_regs.dho  = chnl->vmode.hbp;
+		chnl->tv_regs.alw  = chnl->vmode.hfp;
+		chnl->tv_regs.bel1 = chnl->vmode.vsw;
+		chnl->tv_regs.bel2 = chnl->tv_regs.bel1;
+		chnl->tv_regs.dvo  = chnl->vmode.vbp;
+		chnl->tv_regs.bsl  = chnl->vmode.vfp;
+		chnl->tv_regs.fsl1 = 0;
+		chnl->tv_regs.fsl2 = 0;
+		polarity = chnl->port.phy.dpi.polarity;
+		chnl->tv_regs.lcdtim1 = MCDE_LCDTIM1A_IHS(
+				(polarity & DPI_ACT_LOW_HSYNC) != 0);
+		chnl->tv_regs.lcdtim1 |= MCDE_LCDTIM1A_IVS(
+				(polarity & DPI_ACT_LOW_VSYNC) != 0);
+		chnl->tv_regs.lcdtim1 |= MCDE_LCDTIM1A_IOE(
+				(polarity & DPI_ACT_LOW_DATA_ENABLE) != 0);
+		chnl->tv_regs.lcdtim1 |= MCDE_LCDTIM1A_IPC(
+				(polarity & DPI_ACT_ON_FALLING_EDGE) != 0);
+	}
+	chnl->tv_regs.dirty = true;
+}
+
+static void update_dpi_registers(enum mcde_chnl chnl_id, struct tv_regs *regs)
+{
+	u8 idx = chnl_id;
+
+	dev_dbg(&mcde_dev->dev, "%s\n", __func__);
+	mcde_wreg(MCDE_TVCRA + idx * MCDE_TVCRA_GROUPOFFSET,
+			MCDE_TVCRA_SEL_MOD(regs->sel_mode_tv)             |
+			MCDE_TVCRA_INTEREN(regs->interlaced_en)           |
+			MCDE_TVCRA_IFIELD(0)                              |
+			MCDE_TVCRA_TVMODE(regs->tv_mode)                  |
+			MCDE_TVCRA_SDTVMODE(MCDE_TVCRA_SDTVMODE_Y0CBY1CR) |
+			MCDE_TVCRA_CKINV(regs->inv_clk)                   |
+			MCDE_TVCRA_AVRGEN(0));
+	mcde_wreg(MCDE_TVBLUA + idx * MCDE_TVBLUA_GROUPOFFSET,
+		MCDE_TVBLUA_TVBLU(MCDE_CONFIG_TVOUT_BACKGROUND_LUMINANCE) |
+		MCDE_TVBLUA_TVBCB(MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CB)|
+		MCDE_TVBLUA_TVBCR(MCDE_CONFIG_TVOUT_BACKGROUND_CHROMINANCE_CR));
+
+	/* Vertical timing registers */
+	mcde_wreg(MCDE_TVDVOA + idx * MCDE_TVDVOA_GROUPOFFSET,
+					MCDE_TVDVOA_DVO1(regs->dvo) |
+					MCDE_TVDVOA_DVO2(regs->dvo));
+	mcde_wreg(MCDE_TVBL1A + idx * MCDE_TVBL1A_GROUPOFFSET,
+					MCDE_TVBL1A_BEL1(regs->bel1) |
+					MCDE_TVBL1A_BSL1(regs->bsl));
+	mcde_wreg(MCDE_TVBL2A + idx * MCDE_TVBL1A_GROUPOFFSET,
+					MCDE_TVBL2A_BEL2(regs->bel2) |
+					MCDE_TVBL2A_BSL2(regs->bsl));
+	mcde_wreg(MCDE_TVISLA + idx * MCDE_TVISLA_GROUPOFFSET,
+					MCDE_TVISLA_FSL1(regs->fsl1) |
+					MCDE_TVISLA_FSL2(regs->fsl2));
+
+	/* Horizontal timing registers */
+	mcde_wreg(MCDE_TVLBALWA + idx * MCDE_TVLBALWA_GROUPOFFSET,
+				MCDE_TVLBALWA_LBW(regs->hsw) |
+				MCDE_TVLBALWA_ALW(regs->alw));
+	mcde_wreg(MCDE_TVTIM1A + idx * MCDE_TVTIM1A_GROUPOFFSET,
+				MCDE_TVTIM1A_DHO(regs->dho));
+	if (!regs->sel_mode_tv)
+		mcde_wreg(MCDE_LCDTIM1A + idx * MCDE_LCDTIM1A_GROUPOFFSET,
+								regs->lcdtim1);
+	regs->dirty = false;
+}
+
+static void update_col_registers(enum mcde_chnl chnl_id, struct col_regs *regs)
+{
+	u8 idx = chnl_id;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	mcde_wreg(MCDE_RGBCONV1A + idx * MCDE_RGBCONV1A_GROUPOFFSET,
+				MCDE_RGBCONV1A_YR_RED(regs->y_red) |
+				MCDE_RGBCONV1A_YR_GREEN(regs->y_green));
+	mcde_wreg(MCDE_RGBCONV2A + idx * MCDE_RGBCONV2A_GROUPOFFSET,
+				MCDE_RGBCONV2A_YR_BLUE(regs->y_blue) |
+				MCDE_RGBCONV2A_CR_RED(regs->cr_red));
+	mcde_wreg(MCDE_RGBCONV3A + idx * MCDE_RGBCONV3A_GROUPOFFSET,
+				MCDE_RGBCONV3A_CR_GREEN(regs->cr_green) |
+				MCDE_RGBCONV3A_CR_BLUE(regs->cr_blue));
+	mcde_wreg(MCDE_RGBCONV4A + idx * MCDE_RGBCONV4A_GROUPOFFSET,
+				MCDE_RGBCONV4A_CB_RED(regs->cb_red) |
+				MCDE_RGBCONV4A_CB_GREEN(regs->cb_green));
+	mcde_wreg(MCDE_RGBCONV5A + idx * MCDE_RGBCONV5A_GROUPOFFSET,
+				MCDE_RGBCONV5A_CB_BLUE(regs->cb_blue) |
+				MCDE_RGBCONV5A_OFF_RED(regs->off_cr));
+	mcde_wreg(MCDE_RGBCONV6A + idx * MCDE_RGBCONV6A_GROUPOFFSET,
+				MCDE_RGBCONV6A_OFF_GREEN(regs->off_y) |
+				MCDE_RGBCONV6A_OFF_BLUE(regs->off_cb));
+	regs->dirty = false;
+}
+
+/* MCDE internal helpers */
+static u8 portfmt2dsipacking(enum mcde_port_pix_fmt pix_fmt)
+{
+	switch (pix_fmt) {
+	case MCDE_PORTPIXFMT_DSI_16BPP:
+		return MCDE_DSIVID0CONF0_PACKING_RGB565;
+	case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		return MCDE_DSIVID0CONF0_PACKING_RGB666;
+	case MCDE_PORTPIXFMT_DSI_18BPP:
+	case MCDE_PORTPIXFMT_DSI_24BPP:
+	default:
+		return MCDE_DSIVID0CONF0_PACKING_RGB888;
+	case MCDE_PORTPIXFMT_DSI_YCBCR422:
+		return MCDE_DSIVID0CONF0_PACKING_HDTV;
+	}
+}
+
+static u8 portfmt2bpp(enum mcde_port_pix_fmt pix_fmt)
+{
+	/* TODO: Check DPI spec *//* REVIEW: Remove or check */
+	switch (pix_fmt) {
+	case MCDE_PORTPIXFMT_DPI_16BPP_C1:
+	case MCDE_PORTPIXFMT_DPI_16BPP_C2:
+	case MCDE_PORTPIXFMT_DPI_16BPP_C3:
+	case MCDE_PORTPIXFMT_DSI_16BPP:
+	case MCDE_PORTPIXFMT_DSI_YCBCR422:
+		return 16;
+	case MCDE_PORTPIXFMT_DPI_18BPP_C1:
+	case MCDE_PORTPIXFMT_DPI_18BPP_C2:
+	case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		return 18;
+	case MCDE_PORTPIXFMT_DSI_18BPP:
+	case MCDE_PORTPIXFMT_DPI_24BPP:
+	case MCDE_PORTPIXFMT_DSI_24BPP:
+		return 24;
+	default:
+		return 1;
+	}
+}
+
+static u8 bpp2outbpp(u8 bpp)
+{
+	switch (bpp) {
+	case 16:
+		return MCDE_CRA1_OUTBPP_16BPP;
+	case 18:
+		return MCDE_CRA1_OUTBPP_18BPP;
+	case 24:
+		return MCDE_CRA1_OUTBPP_24BPP;
+	default:
+		return 0;
+	}
+}
+
+static u8 portfmt2cdwin(enum mcde_port_pix_fmt pix_fmt)
+{
+	switch (pix_fmt) {
+	case MCDE_PORTPIXFMT_DPI_16BPP_C1:
+		return MCDE_CRA1_CDWIN_16BPP_C1;
+	case MCDE_PORTPIXFMT_DPI_16BPP_C2:
+		return MCDE_CRA1_CDWIN_16BPP_C2;
+	case MCDE_PORTPIXFMT_DPI_16BPP_C3:
+		return MCDE_CRA1_CDWIN_16BPP_C3;
+	case MCDE_PORTPIXFMT_DPI_18BPP_C1:
+		return MCDE_CRA1_CDWIN_18BPP_C1;
+	case MCDE_PORTPIXFMT_DPI_18BPP_C2:
+		return MCDE_CRA1_CDWIN_18BPP_C2;
+	case MCDE_PORTPIXFMT_DPI_24BPP:
+		return MCDE_CRA1_CDWIN_24BPP;
+	default:
+		/* only DPI formats are relevant */
+		return 0;
+	}
+}
+
+static u32 get_output_fifo_size(enum mcde_fifo fifo)
+{
+	u32 ret = 1; /* Avoid div by zero */
+
+	switch (fifo) {
+	case MCDE_FIFO_A:
+	case MCDE_FIFO_B:
+		ret = output_fifo_ab_size;
+		break;
+	case MCDE_FIFO_C0:
+	case MCDE_FIFO_C1:
+		ret = output_fifo_c0c1_size;
+		break;
+	default:
+		dev_warn(&mcde_dev->dev, "Unsupported fifo");
+		break;
+	}
+	return ret;
+}
+
+static inline u8 get_dsi_formatter_id(const struct mcde_port *port)
+{
+	if (dsi_ifc_is_supported)
+		return 2 * port->link + port->ifc;
+	else
+		return port->link;
+}
+
+static struct mcde_chnl_state *find_channel_by_dsilink(int link)
+{
+	struct mcde_chnl_state *chnl = &channels[0];
+	for (; chnl < &channels[num_channels]; chnl++)
+		if (chnl->enabled && chnl->port.link == link &&
+					chnl->port.type == MCDE_PORTTYPE_DSI)
+			return chnl;
+	return NULL;
+}
+
+static inline void mcde_handle_vcmp(struct mcde_chnl_state *chnl)
+{
+	if (!chnl->vcmp_per_field ||
+				(chnl->vcmp_per_field && chnl->even_vcmp)) {
+		atomic_inc(&chnl->vcmp_cnt);
+		if (chnl->state == CHNLSTATE_STOPPING)
+			set_channel_state_atomic(chnl, CHNLSTATE_STOPPED);
+		wake_up_all(&chnl->vcmp_waitq);
+	}
+	chnl->even_vcmp = !chnl->even_vcmp;
+}
+
+static void handle_dsi_irq(struct mcde_chnl_state *chnl, int i)
+{
+	u32 irq_status = dsi_rfld(i, DSI_DIRECT_CMD_STS_FLAG, TE_RECEIVED_FLAG);
+	if (irq_status) {
+		dsi_wreg(i, DSI_DIRECT_CMD_STS_CLR,
+				DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR(true));
+		dev_vdbg(&mcde_dev->dev, "BTA TE DSI%d\n", i);
+		if (chnl->port.frame_trig == MCDE_TRIG_SW)
+			do_softwaretrig(chnl);
+	}
+
+	irq_status = dsi_rfld(i, DSI_CMD_MODE_STS_FLAG, ERR_NO_TE_FLAG);
+	if (irq_status) {
+		dsi_wreg(i, DSI_CMD_MODE_STS_CLR,
+			DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR(true));
+		dev_warn(&mcde_dev->dev, "NO_TE DSI%d\n", i);
+		set_channel_state_atomic(chnl, CHNLSTATE_STOPPED);
+	}
+
+	irq_status = dsi_rfld(i, DSI_DIRECT_CMD_STS, TRIGGER_RECEIVED);
+	if (irq_status) {
+		/* DSI TE polling answer received */
+		dsi_wreg(i, DSI_DIRECT_CMD_STS_CLR,
+			DSI_DIRECT_CMD_STS_CLR_TRIGGER_RECEIVED_CLR(true));
+
+		/* Reset TE watchdog timer */
+		if (chnl->port.sync_src == MCDE_SYNCSRC_TE_POLLING)
+			dsi_te_poll_set_timer(chnl, DSI_TE_NO_ANSWER_TIMEOUT);
+	}
+}
+
+static irqreturn_t mcde_irq_handler(int irq, void *dev)
+{
+	int i;
+	u32 irq_status;
+
+	irq_status = mcde_rreg(MCDE_MISCHNL);
+	if (irq_status) {
+		dev_err(&mcde_dev->dev, "chnl error=%.8x\n", irq_status);
+		mcde_wreg(MCDE_RISCHNL, irq_status);
+	}
+	irq_status = mcde_rreg(MCDE_MISERR);
+	if (irq_status) {
+		dev_err(&mcde_dev->dev, "error=%.8x\n", irq_status);
+		mcde_wreg(MCDE_RISERR, irq_status);
+	}
+
+	/* Handle channel irqs */
+	irq_status = mcde_rreg(MCDE_RISPP);
+	if (irq_status & MCDE_RISPP_VCMPARIS_MASK)
+		mcde_handle_vcmp(&channels[MCDE_CHNL_A]);
+	if (irq_status & MCDE_RISPP_VCMPBRIS_MASK)
+		mcde_handle_vcmp(&channels[MCDE_CHNL_B]);
+	if (irq_status & MCDE_RISPP_VCMPC0RIS_MASK)
+		mcde_handle_vcmp(&channels[MCDE_CHNL_C0]);
+	if (irq_status & MCDE_RISPP_VCMPC1RIS_MASK)
+		mcde_handle_vcmp(&channels[MCDE_CHNL_C1]);
+	mcde_wreg(MCDE_RISPP, irq_status);
+
+	for (i = 0; i < num_dsilinks; i++) {
+		struct mcde_chnl_state *chnl_from_dsi;
+
+		chnl_from_dsi = find_channel_by_dsilink(i);
+
+		if (chnl_from_dsi == NULL)
+			continue;
+
+		handle_dsi_irq(chnl_from_dsi, i);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/* Transitions allowed: WAIT_TE -> UPDATE -> STOPPING */
+static int set_channel_state_atomic(struct mcde_chnl_state *chnl,
+							enum chnl_state state)
+{
+	enum chnl_state chnl_state = chnl->state;
+
+	dev_dbg(&mcde_dev->dev, "Channel state change"
+		" (chnl=%d, old=%d, new=%d)\n", chnl->id, chnl_state, state);
+
+	if ((chnl_state == CHNLSTATE_SETUP && state == CHNLSTATE_WAIT_TE) ||
+	    (chnl_state == CHNLSTATE_SETUP && state == CHNLSTATE_RUNNING) ||
+	    (chnl_state == CHNLSTATE_WAIT_TE && state == CHNLSTATE_RUNNING) ||
+	    (chnl_state == CHNLSTATE_RUNNING && state == CHNLSTATE_STOPPING)) {
+		/* Set wait TE, running, or stopping state */
+		chnl->state = state;
+		return 0;
+	} else if ((chnl_state == CHNLSTATE_STOPPING &&
+						state == CHNLSTATE_STOPPED) ||
+		   (chnl_state == CHNLSTATE_WAIT_TE &&
+						state == CHNLSTATE_STOPPED)) {
+		/* Set stopped state */
+		chnl->state = state;
+		wake_up_all(&chnl->state_waitq);
+		return 0;
+	} else if (state == CHNLSTATE_IDLE) {
+		/* Set idle state */
+		WARN_ON_ONCE(chnl_state != CHNLSTATE_DSI_READ &&
+			     chnl_state != CHNLSTATE_DSI_WRITE &&
+			     chnl_state != CHNLSTATE_SUSPEND);
+		chnl->state = state;
+		wake_up_all(&chnl->state_waitq);
+		return 0;
+	} else {
+		/* Invalid atomic state transition */
+		dev_warn(&mcde_dev->dev, "Channel state change error (chnl=%d,"
+			" old=%d, new=%d)\n", chnl->id, chnl_state, state);
+		WARN_ON_ONCE(true);
+		return -EINVAL;
+	}
+}
+
+/* LOCKING: mcde_hw_lock */
+static int set_channel_state_sync(struct mcde_chnl_state *chnl,
+							enum chnl_state state)
+{
+	int ret = 0;
+	enum chnl_state chnl_state = chnl->state;
+
+	dev_dbg(&mcde_dev->dev, "Channel state change"
+		" (chnl=%d, old=%d, new=%d)\n", chnl->id, chnl->state, state);
+
+	/* No change */
+	if (chnl_state == state)
+		return 0;
+
+	/* Wait for IDLE before changing state */
+	if (chnl_state != CHNLSTATE_IDLE) {
+		ret = wait_event_timeout(chnl->state_waitq,
+			/* STOPPED -> IDLE is manual, so wait for both */
+			chnl->state == CHNLSTATE_STOPPED ||
+			chnl->state == CHNLSTATE_IDLE,
+						msecs_to_jiffies(CHNL_TIMEOUT));
+		if (WARN_ON_ONCE(!ret))
+			dev_warn(&mcde_dev->dev, "Wait for channel timeout "
+						"(chnl=%d, curr=%d, new=%d)\n",
+						chnl->id, chnl->state, state);
+		chnl_state = chnl->state;
+	}
+
+	/* Do manual transition from STOPPED to IDLE */
+	if (chnl_state == CHNLSTATE_STOPPED)
+		wait_for_flow_disabled(chnl);
+
+	/* State is IDLE, do transition to new state */
+	chnl->state = state;
+
+	return ret;
+}
+
+static int wait_for_vcmp(struct mcde_chnl_state *chnl)
+{
+	u64 vcmp = atomic_read(&chnl->vcmp_cnt) + 1;
+	int ret = wait_event_timeout(chnl->vcmp_waitq,
+					atomic_read(&chnl->vcmp_cnt) >= vcmp,
+						msecs_to_jiffies(CHNL_TIMEOUT));
+	return ret;
+}
+
+static void get_vid_operating_mode(const struct mcde_port *port,
+		bool *burst_mode, bool *sync_is_pulse, bool *tvg_enable)
+{
+	switch (port->phy.dsi.vid_mode) {
+	case NON_BURST_MODE_WITH_SYNC_EVENT:
+		*burst_mode = false;
+		*sync_is_pulse = false;
+		*tvg_enable = false;
+		break;
+	case NON_BURST_MODE_WITH_SYNC_EVENT_TVG_ENABLED:
+		*burst_mode = false;
+		*sync_is_pulse = false;
+		*tvg_enable = true;
+		break;
+	case BURST_MODE_WITH_SYNC_EVENT:
+		*burst_mode = true;
+		*sync_is_pulse = false;
+		*tvg_enable = false;
+		break;
+	case BURST_MODE_WITH_SYNC_PULSE:
+		*burst_mode = true;
+		*sync_is_pulse = true;
+		*tvg_enable = false;
+		break;
+	default:
+		dev_err(&mcde_dev->dev, "Unsupported video mode");
+		break;
+	}
+}
+
+static void update_vid_static_registers(const struct mcde_port *port)
+{
+	u8 link = port->link;
+	bool burst_mode, sync_is_pulse, tvg_enable;
+
+	get_vid_operating_mode(port, &burst_mode, &sync_is_pulse, &tvg_enable);
+
+	/* burst mode or non-burst mode */
+	dsi_wfld(link, DSI_VID_MAIN_CTL, BURST_MODE, burst_mode);
+
+	/* sync is pulse or event */
+	dsi_wfld(link, DSI_VID_MAIN_CTL, SYNC_PULSE_ACTIVE, sync_is_pulse);
+	dsi_wfld(link, DSI_VID_MAIN_CTL, SYNC_PULSE_HORIZONTAL, sync_is_pulse);
+
+	/* disable video stream when using TVG */
+	if (tvg_enable) {
+		dsi_wfld(link, DSI_MCTL_MAIN_EN, IF1_EN, false);
+		dsi_wfld(link, DSI_MCTL_MAIN_EN, IF2_EN, false);
+	}
+
+	/*
+	 * behavior during blanking time
+	 * 00: NULL packet 1x:LP 01:blanking-packet
+	 */
+	dsi_wfld(link, DSI_VID_MAIN_CTL, REG_BLKLINE_MODE, 1);
+
+	/*
+	 * behavior during eol
+	 * 00: NULL packet 1x:LP 01:blanking-packet
+	 */
+	dsi_wfld(link, DSI_VID_MAIN_CTL, REG_BLKEOL_MODE, 2);
+
+	/* time to perform LP->HS on D-PHY */
+	dsi_wfld(link, DSI_VID_DPHY_TIME, REG_WAKEUP_TIME,
+						port->phy.dsi.vid_wakeup_time);
+
+	/*
+	 * video stream starts on VSYNC packet
+	 * and stops at the end of a frame
+	 */
+	dsi_wfld(link, DSI_VID_MAIN_CTL, VID_ID, port->phy.dsi.virt_id);
+	dsi_wfld(link, DSI_VID_MAIN_CTL, START_MODE, 0);
+	dsi_wfld(link, DSI_VID_MAIN_CTL, STOP_MODE, 0);
+
+	/* 1: if1 in video mode, 0: if1 in command mode */
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, IF1_MODE, 1);
+
+	/* 1: enables the link, 0: disables the link */
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, VID_EN, 1);
+}
+
+static int update_channel_static_registers(struct mcde_chnl_state *chnl)
+{
+	const struct mcde_port *port = &chnl->port;
+
+	switch (chnl->fifo) {
+	case MCDE_FIFO_A:
+		mcde_wreg(MCDE_CHNL0MUXING + chnl->id *
+			MCDE_CHNL0MUXING_GROUPOFFSET,
+			MCDE_CHNL0MUXING_FIFO_ID_ENUM(FIFO_A));
+		if (port->type == MCDE_PORTTYPE_DPI) {
+			mcde_wfld(MCDE_CTRLA, FORMTYPE,
+					MCDE_CTRLA_FORMTYPE_DPITV);
+			mcde_wfld(MCDE_CTRLA, FORMID, port->link);
+		} else if (port->type == MCDE_PORTTYPE_DSI) {
+			mcde_wfld(MCDE_CTRLA, FORMTYPE,
+					MCDE_CTRLA_FORMTYPE_DSI);
+			mcde_wfld(MCDE_CTRLA, FORMID,
+						get_dsi_formatter_id(port));
+		}
+		break;
+	case MCDE_FIFO_B:
+		mcde_wreg(MCDE_CHNL0MUXING + chnl->id *
+			MCDE_CHNL0MUXING_GROUPOFFSET,
+			MCDE_CHNL0MUXING_FIFO_ID_ENUM(FIFO_B));
+		if (port->type == MCDE_PORTTYPE_DPI) {
+			mcde_wfld(MCDE_CTRLB, FORMTYPE,
+					MCDE_CTRLB_FORMTYPE_DPITV);
+			mcde_wfld(MCDE_CTRLB, FORMID, port->link);
+		} else if (port->type == MCDE_PORTTYPE_DSI) {
+			mcde_wfld(MCDE_CTRLB, FORMTYPE,
+					MCDE_CTRLB_FORMTYPE_DSI);
+			mcde_wfld(MCDE_CTRLB, FORMID,
+						get_dsi_formatter_id(port));
+		}
+
+		break;
+	case MCDE_FIFO_C0:
+		mcde_wreg(MCDE_CHNL0MUXING + chnl->id *
+			MCDE_CHNL0MUXING_GROUPOFFSET,
+			MCDE_CHNL0MUXING_FIFO_ID_ENUM(FIFO_C0));
+		if (port->type == MCDE_PORTTYPE_DPI)
+			return -EINVAL;
+		mcde_wfld(MCDE_CTRLC0, FORMTYPE,
+					MCDE_CTRLC0_FORMTYPE_DSI);
+		mcde_wfld(MCDE_CTRLC0, FORMID, get_dsi_formatter_id(port));
+		break;
+	case MCDE_FIFO_C1:
+		mcde_wreg(MCDE_CHNL0MUXING + chnl->id *
+			MCDE_CHNL0MUXING_GROUPOFFSET,
+			MCDE_CHNL0MUXING_FIFO_ID_ENUM(FIFO_C1));
+		if (port->type == MCDE_PORTTYPE_DPI)
+			return -EINVAL;
+		mcde_wfld(MCDE_CTRLC1, FORMTYPE,
+					MCDE_CTRLC1_FORMTYPE_DSI);
+		mcde_wfld(MCDE_CTRLC1, FORMID, get_dsi_formatter_id(port));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Formatter */
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		int i = 0;
+		u8 idx;
+		u8 lnk = port->link;
+
+		idx = get_dsi_formatter_id(port);
+
+		if (dsi_link_enable(chnl))
+			goto failed_to_enable_link;
+
+		if (port->sync_src == MCDE_SYNCSRC_TE_POLLING) {
+			/* Enable DSI TE polling */
+			dsi_te_poll_req(chnl);
+
+			/* Set timer to detect non TE answer */
+			dsi_te_poll_set_timer(chnl,
+					DSI_TE_NO_ANSWER_TIMEOUT_INIT);
+		} else {
+			dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, true);
+			dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, READ_EN, true);
+			dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, true);
+		}
+
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, HOST_EOT_GEN,
+						port->phy.dsi.host_eot_gen);
+
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, DLX_REMAP_EN,
+					port->phy.dsi.data_lanes_swap);
+
+		dsi_wreg(lnk, DSI_MCTL_DPHY_STATIC,
+			DSI_MCTL_DPHY_STATIC_UI_X4(port->phy.dsi.ui));
+		dsi_wreg(lnk, DSI_DPHY_LANES_TRIM,
+			DSI_DPHY_LANES_TRIM_DPHY_SPECS_90_81B_ENUM(0_90));
+		dsi_wreg(lnk, DSI_MCTL_DPHY_TIMEOUT,
+			DSI_MCTL_DPHY_TIMEOUT_CLK_DIV(0xf) |
+			DSI_MCTL_DPHY_TIMEOUT_HSTX_TO_VAL(0x3fff) |
+			DSI_MCTL_DPHY_TIMEOUT_LPRX_TO_VAL(0x3fff));
+		dsi_wreg(lnk, DSI_MCTL_MAIN_PHY_CTL,
+			DSI_MCTL_MAIN_PHY_CTL_WAIT_BURST_TIME(0xf) |
+			DSI_MCTL_MAIN_PHY_CTL_CLK_ULPM_EN(true) |
+			DSI_MCTL_MAIN_PHY_CTL_DAT1_ULPM_EN(true) |
+			DSI_MCTL_MAIN_PHY_CTL_DAT2_ULPM_EN(true) |
+			DSI_MCTL_MAIN_PHY_CTL_LANE2_EN(
+				port->phy.dsi.num_data_lanes >= 2) |
+			DSI_MCTL_MAIN_PHY_CTL_CLK_CONTINUOUS(
+				port->phy.dsi.clk_cont));
+		/* TODO: make enum */
+		dsi_wfld(lnk, DSI_CMD_MODE_CTL, ARB_MODE, false);
+		/* TODO: make enum */
+		dsi_wfld(lnk, DSI_CMD_MODE_CTL, ARB_PRI, port->ifc == 1);
+		dsi_wreg(lnk, DSI_MCTL_MAIN_EN,
+			DSI_MCTL_MAIN_EN_PLL_START(true) |
+			DSI_MCTL_MAIN_EN_CKLANE_EN(true) |
+			DSI_MCTL_MAIN_EN_DAT1_EN(true) |
+			DSI_MCTL_MAIN_EN_DAT2_EN(port->phy.dsi.num_data_lanes
+				== 2) |
+			DSI_MCTL_MAIN_EN_IF1_EN(port->ifc == 0) |
+			DSI_MCTL_MAIN_EN_IF2_EN(port->ifc == 1));
+		while (dsi_rfld(lnk, DSI_MCTL_MAIN_STS, CLKLANE_READY) == 0 ||
+			dsi_rfld(lnk, DSI_MCTL_MAIN_STS, DAT1_READY) == 0 ||
+			(dsi_rfld(lnk, DSI_MCTL_MAIN_STS, DAT2_READY) == 0 &&
+					port->phy.dsi.num_data_lanes > 1)) {
+			mdelay(1);
+			if (i++ == 10) {
+				dev_warn(&mcde_dev->dev,
+					"DSI lane not ready (link=%d)!\n", lnk);
+				goto dsi_link_error;
+			}
+		}
+
+		dsi_link_handle_ulpm(&chnl->port, false);
+		mcde_wreg(MCDE_DSIVID0CONF0 +
+			idx * MCDE_DSIVID0CONF0_GROUPOFFSET,
+			MCDE_DSIVID0CONF0_BLANKING(0) |
+			MCDE_DSIVID0CONF0_VID_MODE(
+				port->mode == MCDE_PORTMODE_VID) |
+			MCDE_DSIVID0CONF0_CMD8(true) |
+			MCDE_DSIVID0CONF0_BIT_SWAP(false) |
+			MCDE_DSIVID0CONF0_BYTE_SWAP(false) |
+			MCDE_DSIVID0CONF0_DCSVID_NOTGEN(true));
+
+		if (port->mode == MCDE_PORTMODE_VID) {
+			update_vid_static_registers(port);
+		} else {
+			if (port->ifc == 0)
+				dsi_wfld(port->link, DSI_CMD_MODE_CTL, IF1_ID,
+					port->phy.dsi.virt_id);
+			else if (port->ifc == 1)
+				dsi_wfld(port->link, DSI_CMD_MODE_CTL, IF2_ID,
+					port->phy.dsi.virt_id);
+		}
+	}
+
+	if (port->type == MCDE_PORTTYPE_DPI) {
+		if (port->phy.dpi.lcd_freq != clk_round_rate(chnl->clk_dpi,
+							port->phy.dpi.lcd_freq))
+			dev_warn(&mcde_dev->dev, "Could not set lcd freq"
+					" to %d\n", port->phy.dpi.lcd_freq);
+		WARN_ON_ONCE(clk_set_rate(chnl->clk_dpi,
+						port->phy.dpi.lcd_freq));
+		WARN_ON_ONCE(clk_enable(chnl->clk_dpi));
+	}
+
+	mcde_wfld(MCDE_CR, MCDEEN, true);
+	chnl->formatter_updated = true;
+
+	dev_vdbg(&mcde_dev->dev, "Static registers setup, chnl=%d\n", chnl->id);
+
+	return 0;
+dsi_link_error:
+	dsi_link_disable(chnl, true);
+failed_to_enable_link:
+	return -EINVAL;
+}
+
+void mcde_chnl_col_convert_apply(struct mcde_chnl_state *chnl,
+					struct mcde_col_transform *transform)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (chnl->transform != transform) {
+
+		chnl->col_regs.y_red     = transform->matrix[0][0];
+		chnl->col_regs.y_green   = transform->matrix[0][1];
+		chnl->col_regs.y_blue    = transform->matrix[0][2];
+		chnl->col_regs.cb_red    = transform->matrix[1][0];
+		chnl->col_regs.cb_green  = transform->matrix[1][1];
+		chnl->col_regs.cb_blue   = transform->matrix[1][2];
+		chnl->col_regs.cr_red    = transform->matrix[2][0];
+		chnl->col_regs.cr_green  = transform->matrix[2][1];
+		chnl->col_regs.cr_blue   = transform->matrix[2][2];
+		chnl->col_regs.off_y     = transform->offset[0];
+		chnl->col_regs.off_cb    = transform->offset[1];
+		chnl->col_regs.off_cr    = transform->offset[2];
+		chnl->col_regs.dirty = true;
+
+		chnl->transform = transform;
+	}
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+static void chnl_ovly_pixel_format_apply(struct mcde_chnl_state *chnl,
+						struct mcde_ovly_state *ovly)
+{
+	struct mcde_port *port = &chnl->port;
+	struct ovly_regs *regs = &ovly->regs;
+
+	/* Note: YUV -> YUV: blending YUV overlays will not make sense. */
+	static struct mcde_col_transform crycb_2_ycbcr = {
+		/* Note that in MCDE YUV 422 pixels come as VYU pixels */
+		.matrix = {
+			{0x0000, 0x0100, 0x0000},
+			{0x0000, 0x0000, 0x0100},
+			{0x0100, 0x0000, 0x0000},
+		},
+		.offset = {0, 0, 0},
+	};
+
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		if (port->pixel_format != MCDE_PORTPIXFMT_DSI_YCBCR422) {
+			if (ovly->pix_fmt != MCDE_OVLYPIXFMT_YCbCr422) {
+				/* standard case: DSI: RGB -> RGB */
+				regs->col_conv = MCDE_OVL0CR_COLCCTRL_DISABLED;
+			} else {
+				/* DSI: YUV -> RGB */
+				/* TODO change matrix */
+				regs->col_conv =
+					MCDE_OVL0CR_COLCCTRL_ENABLED_SAT;
+				mcde_chnl_col_convert_apply(chnl,
+							&chnl->ycbcr_2_rgb);
+			}
+		} else {
+			if (ovly->pix_fmt != MCDE_OVLYPIXFMT_YCbCr422)
+				/* DSI: RGB -> YUV */
+				mcde_chnl_col_convert_apply(chnl,
+							&chnl->rgb_2_ycbcr);
+			else
+				/* DSI: YUV -> YUV */
+				mcde_chnl_col_convert_apply(chnl,
+							&crycb_2_ycbcr);
+			regs->col_conv = MCDE_OVL0CR_COLCCTRL_ENABLED_NO_SAT;
+		}
+	} else if (port->type == MCDE_PORTTYPE_DPI && port->phy.dpi.tv_mode) {
+		regs->col_conv = MCDE_OVL0CR_COLCCTRL_ENABLED_NO_SAT;
+		if (ovly->pix_fmt != MCDE_OVLYPIXFMT_YCbCr422)
+			mcde_chnl_col_convert_apply(chnl, &chnl->rgb_2_ycbcr);
+		else
+			mcde_chnl_col_convert_apply(chnl, &crycb_2_ycbcr);
+	} else if (port->type == MCDE_PORTTYPE_DPI) {
+		/* Note: YUV is not support port pixel format for DPI */
+		if (ovly->pix_fmt != MCDE_OVLYPIXFMT_YCbCr422) {
+			/* standard case: DPI: RGB -> RGB */
+			regs->col_conv = MCDE_OVL0CR_COLCCTRL_DISABLED;
+		} else {
+			/* DPI: YUV -> RGB */
+			regs->col_conv =
+				MCDE_OVL0CR_COLCCTRL_ENABLED_SAT;
+			mcde_chnl_col_convert_apply(chnl,
+						&chnl->ycbcr_2_rgb);
+		}
+	}
+}
+
+/* REVIEW: Make update_* an mcde_rectangle? */
+static void update_overlay_registers(u8 idx, struct ovly_regs *regs,
+			struct mcde_port *port, enum mcde_fifo fifo,
+			u16 update_x, u16 update_y, u16 update_w,
+			u16 update_h, s16 stride, bool interlaced,
+			enum mcde_display_rotation rotation)
+{
+	/* TODO: fix clipping for small overlay */
+	u32 lmrgn = (regs->cropx + update_x) * regs->bits_per_pixel;
+	u32 tmrgn = (regs->cropy + update_y) * stride;
+	u32 ppl = regs->ppl - update_x;
+	u32 lpf = regs->lpf - update_y;
+	s32 ljinc = stride;
+	u32 pixelfetchwtrmrklevel;
+	u8  nr_of_bufs = 1;
+	u32 sel_mod = MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL;
+
+	if (rotation == MCDE_DISPLAY_ROT_180_CCW) {
+		ljinc = -ljinc;
+		tmrgn += stride * (regs->lpf - 1) / 8;
+	}
+
+	/*
+	 * Preferably most of this is done in some apply function instead of for
+	 * every update. However lpf has a dependency on update_y.
+	 */
+	if (interlaced && port->type == MCDE_PORTTYPE_DSI) {
+		nr_of_bufs = 2;
+		lpf = lpf / 2;
+		ljinc *= 2;
+	}
+
+	if ((fifo == MCDE_FIFO_A || fifo == MCDE_FIFO_B) &&
+			regs->ppl >= SCREEN_PPL_HIGH)
+		pixelfetchwtrmrklevel = input_fifo_size * 2;
+	else
+		pixelfetchwtrmrklevel = input_fifo_size / 2;
+
+	if (port->update_auto_trig && port->type == MCDE_PORTTYPE_DSI) {
+		switch (port->sync_src) {
+		case MCDE_SYNCSRC_OFF:
+			sel_mod = MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL;
+			break;
+		case MCDE_SYNCSRC_TE0:
+		case MCDE_SYNCSRC_TE1:
+		case MCDE_SYNCSRC_TE_POLLING:
+		default:
+			sel_mod = MCDE_EXTSRC0CR_SEL_MOD_AUTO_TOGGLE;
+			break;
+		}
+	} else if (port->type == MCDE_PORTTYPE_DPI)
+		sel_mod = MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL;
+
+	mcde_wreg(MCDE_EXTSRC0CONF + idx * MCDE_EXTSRC0CONF_GROUPOFFSET,
+		MCDE_EXTSRC0CONF_BUF_ID(0) |
+		MCDE_EXTSRC0CONF_BUF_NB(nr_of_bufs) |
+		MCDE_EXTSRC0CONF_PRI_OVLID(idx) |
+		MCDE_EXTSRC0CONF_BPP(regs->bpp) |
+		MCDE_EXTSRC0CONF_BGR(regs->bgr) |
+		MCDE_EXTSRC0CONF_BEBO(regs->bebo) |
+		MCDE_EXTSRC0CONF_BEPO(false));
+	mcde_wreg(MCDE_EXTSRC0CR + idx * MCDE_EXTSRC0CR_GROUPOFFSET,
+		MCDE_EXTSRC0CR_SEL_MOD(sel_mod) |
+		MCDE_EXTSRC0CR_MULTIOVL_CTRL_ENUM(PRIMARY) |
+		MCDE_EXTSRC0CR_FS_DIV_DISABLE(false) |
+		MCDE_EXTSRC0CR_FORCE_FS_DIV(false));
+	mcde_wreg(MCDE_OVL0CR + idx * MCDE_OVL0CR_GROUPOFFSET,
+		MCDE_OVL0CR_OVLEN(regs->enabled) |
+	MCDE_OVL0CR_COLCCTRL(regs->col_conv) |
+		MCDE_OVL0CR_CKEYGEN(false) |
+		MCDE_OVL0CR_ALPHAPMEN(false) |
+		MCDE_OVL0CR_OVLF(false) |
+		MCDE_OVL0CR_OVLR(false) |
+		MCDE_OVL0CR_OVLB(false) |
+		MCDE_OVL0CR_FETCH_ROPC(0) |
+		MCDE_OVL0CR_STBPRIO(0) |
+		MCDE_OVL0CR_BURSTSIZE_ENUM(HW_8W) |
+		/* TODO: enum, get from ovly */
+		MCDE_OVL0CR_MAXOUTSTANDING_ENUM(8_REQ) |
+		/* TODO: _HW_8W, calculate? */
+		MCDE_OVL0CR_ROTBURSTSIZE_ENUM(HW_8W));
+	mcde_wreg(MCDE_OVL0CONF + idx * MCDE_OVL0CONF_GROUPOFFSET,
+		MCDE_OVL0CONF_PPL(ppl) |
+		MCDE_OVL0CONF_EXTSRC_ID(idx) |
+		MCDE_OVL0CONF_LPF(lpf));
+	mcde_wreg(MCDE_OVL0CONF2 + idx * MCDE_OVL0CONF2_GROUPOFFSET,
+		MCDE_OVL0CONF2_BP(regs->alpha_source) |
+		MCDE_OVL0CONF2_ALPHAVALUE(regs->alpha_value) |
+		MCDE_OVL0CONF2_OPQ(regs->opq) |
+		MCDE_OVL0CONF2_PIXOFF(lmrgn & 63) |
+		MCDE_OVL0CONF2_PIXELFETCHERWATERMARKLEVEL(
+			pixelfetchwtrmrklevel));
+	mcde_wreg(MCDE_OVL0LJINC + idx * MCDE_OVL0LJINC_GROUPOFFSET,
+		ljinc);
+	mcde_wreg(MCDE_OVL0CROP + idx * MCDE_OVL0CROP_GROUPOFFSET,
+		MCDE_OVL0CROP_TMRGN(tmrgn) |
+		MCDE_OVL0CROP_LMRGN(lmrgn >> 6));
+	regs->dirty = false;
+
+	dev_vdbg(&mcde_dev->dev, "Overlay registers setup, idx=%d\n", idx);
+}
+
+static void update_overlay_registers_on_the_fly(u8 idx, struct ovly_regs *regs)
+{
+	mcde_wreg(MCDE_OVL0COMP + idx * MCDE_OVL0COMP_GROUPOFFSET,
+		MCDE_OVL0COMP_XPOS(regs->xpos) |
+		MCDE_OVL0COMP_CH_ID(regs->ch_id) |
+		MCDE_OVL0COMP_YPOS(regs->ypos) |
+		MCDE_OVL0COMP_Z(regs->z));
+
+	mcde_wreg(MCDE_EXTSRC0A0 + idx * MCDE_EXTSRC0A0_GROUPOFFSET,
+		regs->baseaddress0);
+	mcde_wreg(MCDE_EXTSRC0A1 + idx * MCDE_EXTSRC0A1_GROUPOFFSET,
+		regs->baseaddress1);
+	regs->dirty_buf = false;
+}
+
+static void do_softwaretrig(struct mcde_chnl_state *chnl)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+
+	enable_flow(chnl);
+	mcde_wreg(MCDE_CHNL0SYNCHSW +
+		chnl->id * MCDE_CHNL0SYNCHSW_GROUPOFFSET,
+		MCDE_CHNL0SYNCHSW_SW_TRIG(true));
+	disable_flow(chnl);
+
+	local_irq_restore(flags);
+
+	dev_vdbg(&mcde_dev->dev, "Software TRIG on channel %d\n", chnl->id);
+}
+
+static void disable_flow(struct mcde_chnl_state *chnl)
+{
+	unsigned long flags;
+
+	if (WARN_ON_ONCE(chnl->state != CHNLSTATE_RUNNING))
+		return;
+
+	local_irq_save(flags);
+
+	switch (chnl->id) {
+	case MCDE_CHNL_A:
+		mcde_wfld(MCDE_CRA0, FLOEN, false);
+		break;
+	case MCDE_CHNL_B:
+		mcde_wfld(MCDE_CRB0, FLOEN, false);
+		break;
+	case MCDE_CHNL_C0:
+		mcde_wfld(MCDE_CRC, C1EN, false);
+		break;
+	case MCDE_CHNL_C1:
+		mcde_wfld(MCDE_CRC, C2EN, false);
+		break;
+	}
+
+	set_channel_state_atomic(chnl, CHNLSTATE_STOPPING);
+
+	local_irq_restore(flags);
+}
+
+static void stop_channel(struct mcde_chnl_state *chnl)
+{
+	const struct mcde_port *port = &chnl->port;
+	bool dpi_lcd_mode;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (chnl->state != CHNLSTATE_RUNNING)
+		return;
+
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		dsi_wfld(port->link, DSI_MCTL_MAIN_PHY_CTL, CLK_CONTINUOUS,
+			false);
+		if (port->sync_src == MCDE_SYNCSRC_TE_POLLING)
+			del_timer(&chnl->dsi_te_timer);
+	}
+
+	disable_flow(chnl);
+	/*
+	 * Needs to manually trigger VCOMP after the channel is
+	 * disabled. For all channels using video mode
+	 * except for dpi lcd.
+	*/
+	dpi_lcd_mode = (port->type == MCDE_PORTTYPE_DPI &&
+				!chnl->port.phy.dpi.tv_mode);
+
+	if (chnl->port.update_auto_trig && !dpi_lcd_mode)
+		mcde_wreg(MCDE_SISPP, 1 << chnl->id);
+}
+
+static void wait_for_flow_disabled(struct mcde_chnl_state *chnl)
+{
+	int i = 0;
+
+	switch (chnl->id) {
+	case MCDE_CHNL_A:
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			if (!mcde_rfld(MCDE_CRA0, FLOEN)) {
+				dev_vdbg(&mcde_dev->dev,
+					"Flow (A) disable after >= %d ms\n", i);
+				break;
+			}
+			msleep(1);
+		}
+		break;
+	case MCDE_CHNL_B:
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			if (!mcde_rfld(MCDE_CRB0, FLOEN)) {
+				dev_vdbg(&mcde_dev->dev,
+				"Flow (B) disable after >= %d ms\n", i);
+				break;
+			}
+			msleep(1);
+		}
+		break;
+	case MCDE_CHNL_C0:
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			if (!mcde_rfld(MCDE_CRC, C1EN)) {
+				dev_vdbg(&mcde_dev->dev,
+				"Flow (C1) disable after >= %d ms\n", i);
+				break;
+			}
+			msleep(1);
+		}
+		break;
+	case MCDE_CHNL_C1:
+		for (i = 0; i < MCDE_FLOWEN_MAX_TRIAL; i++) {
+			if (!mcde_rfld(MCDE_CRC, C2EN)) {
+				dev_vdbg(&mcde_dev->dev,
+				"Flow (C2) disable after >= %d ms\n", i);
+				break;
+			}
+			msleep(1);
+		}
+		break;
+	}
+	if (i == MCDE_FLOWEN_MAX_TRIAL)
+		dev_err(&mcde_dev->dev, "%s: channel %d timeout\n",
+							__func__, chnl->id);
+}
+
+static void enable_flow(struct mcde_chnl_state *chnl)
+{
+	const struct mcde_port *port = &chnl->port;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (port->type == MCDE_PORTTYPE_DSI)
+		dsi_wfld(port->link, DSI_MCTL_MAIN_PHY_CTL, CLK_CONTINUOUS,
+				port->phy.dsi.clk_cont);
+
+	/*
+	 * When ROTEN is set, the FLOEN bit will also be set but
+	 * the flow has to be started anyway.
+	 */
+	switch (chnl->id) {
+	case MCDE_CHNL_A:
+		WARN_ON_ONCE(mcde_rfld(MCDE_CRA0, FLOEN));
+		mcde_wfld(MCDE_CRA0, ROTEN, chnl->regs.roten);
+		mcde_wfld(MCDE_CRA0, FLOEN, true);
+		break;
+	case MCDE_CHNL_B:
+		WARN_ON_ONCE(mcde_rfld(MCDE_CRB0, FLOEN));
+		mcde_wfld(MCDE_CRB0, ROTEN, chnl->regs.roten);
+		mcde_wfld(MCDE_CRB0, FLOEN, true);
+		break;
+	case MCDE_CHNL_C0:
+		WARN_ON_ONCE(mcde_rfld(MCDE_CRC, C1EN));
+		mcde_wfld(MCDE_CRC, C1EN, true);
+		break;
+	case MCDE_CHNL_C1:
+		WARN_ON_ONCE(mcde_rfld(MCDE_CRC, C2EN));
+		mcde_wfld(MCDE_CRC, C2EN, true);
+		break;
+	}
+
+	set_channel_state_atomic(chnl, CHNLSTATE_RUNNING);
+}
+
+static void work_sleep_function(struct work_struct *ptr)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	if (mcde_trylock(__func__, __LINE__)) {
+		if (mcde_dynamic_power_management)
+			disable_mcde_hw(false, false);
+		mcde_unlock(__func__, __LINE__);
+	}
+}
+
+/* TODO get from register */
+#define MCDE_CLK_FREQ_MHZ 160
+static u32 get_pkt_div(u32 disp_ppl,
+		struct mcde_port *port,
+		enum mcde_fifo fifo)
+{
+	/*
+	 * The lines can be split in several packets only on DSI CMD mode.
+	 * In DSI VIDEO mode, 1 line = 1 packet.
+	 * DPI is like DSI VIDEO (watermark = 1 line).
+	 * DPI waits for fifo ready only for the first line of the first frame.
+	 * If line is wider than fifo size, one can set watermark
+	 * at fifo size, or set it to line size as watermark will be
+	 * saturated at fifo size inside MCDE.
+	 */
+	switch (port->type) {
+	case MCDE_PORTTYPE_DSI:
+		if (port->mode == MCDE_PORTMODE_CMD)
+			/* Equivalent of ceil(disp_ppl/fifo_size) */
+			return (disp_ppl - 1) / get_output_fifo_size(fifo) + 1;
+		else
+			return 1;
+		break;
+	case MCDE_PORTTYPE_DPI:
+		return 1;
+		break;
+	default:
+		break;
+	}
+	return 1;
+}
+
+static void update_vid_horizontal_blanking(struct mcde_port *port,
+		struct mcde_video_mode *vmode, bool sync_is_pulse, u8 bpp)
+{
+	int hfp, hbp, hsa;
+	u8 link = port->link;
+
+	/*
+	 * vmode->hfp, vmode->hbp and vmode->hsw are given in pixels
+	 * and must be re-calculated into bytes
+	 *
+	 * 6 + 2 is HFP header + checksum
+	 */
+	hfp = vmode->hfp * bpp - 6 - 2;
+	if (sync_is_pulse) {
+		/*
+		 * 6 is HBP header + checksum
+		 * 4 is RGB header + checksum
+		 */
+		hbp = vmode->hbp * bpp - 4 - 6;
+		/*
+		 * 6 is HBP header + checksum
+		 * 4 is HSW packet bytes
+		 * 4 is RGB header + checksum
+		 */
+		hsa = vmode->hsw * bpp - 4 - 4 - 6;
+	} else {
+		/*
+		 * 6 is HBP header + checksum
+		 * 4 is HSW packet bytes
+		 * 4 is RGB header + checksum
+		 */
+		hbp = (vmode->hbp + vmode->hsw) * bpp - 4 - 4 - 6;
+		/* HSA is not considered in this mode and set to 0 */
+		hsa = 0;
+	}
+	if (hfp < 0) {
+		hfp = 0;
+		dev_warn(&mcde_dev->dev,
+			"%s: negative calc for hfp, set to 0\n", __func__);
+	}
+	if (hbp < 0) {
+		hbp = 0;
+		dev_warn(&mcde_dev->dev,
+			"%s: negative calc for hbp, set to 0\n", __func__);
+	}
+	if (hsa < 0) {
+		hsa = 0;
+		dev_warn(&mcde_dev->dev,
+			"%s: negative calc for hsa, set to 0\n", __func__);
+	}
+
+	dsi_wfld(link, DSI_VID_HSIZE1, HFP_LENGTH, hfp);
+	dsi_wfld(link, DSI_VID_HSIZE1, HBP_LENGTH, hbp);
+	dsi_wfld(link, DSI_VID_HSIZE1, HSA_LENGTH, hsa);
+}
+
+static void update_vid_frame_parameters(struct mcde_port *port,
+				struct mcde_video_mode *vmode, u8 bpp)
+{
+	u8 link = port->link;
+	bool burst_mode, sync_is_pulse, tvg_enable;
+	u32 hs_byte_clk, pck_len, blkline_pck, line_duration;
+	u32 blkeol_pck, blkeol_duration;
+	u8 pixel_mode;
+	u8 rgb_header;
+
+	get_vid_operating_mode(port, &burst_mode, &sync_is_pulse, &tvg_enable);
+
+	dsi_wfld(link, DSI_VID_VSIZE, VFP_LENGTH, vmode->vfp);
+	dsi_wfld(link, DSI_VID_VSIZE, VBP_LENGTH, vmode->vbp);
+	dsi_wfld(link, DSI_VID_VSIZE, VSA_LENGTH, vmode->vsw);
+	update_vid_horizontal_blanking(port, vmode, sync_is_pulse, bpp);
+
+	dsi_wfld(link, DSI_VID_VSIZE, VACT_LENGTH, vmode->yres);
+	dsi_wfld(link, DSI_VID_HSIZE2, RGB_SIZE, vmode->xres * bpp);
+
+	/*
+	 * The rgb_header identifies the pixel stream format,
+	 * as described in the MIPI DSI Specification:
+	 *
+	 * 0x0E: Packed pixel stream, 16-bit RGB, 565 format
+	 * 0x1E: Packed pixel stream, 18-bit RGB, 666 format
+	 * 0x2E: Loosely Packed pixel stream, 18-bit RGB, 666 format
+	 * 0x3E: Packed pixel stream, 24-bit RGB, 888 format
+	 */
+	switch (port->pixel_format) {
+	case MCDE_PORTPIXFMT_DSI_16BPP:
+		pixel_mode = 0;
+		rgb_header = 0x0E;
+		break;
+	case MCDE_PORTPIXFMT_DSI_18BPP:
+		pixel_mode = 2;
+		rgb_header = 0x2E;
+		break;
+	case MCDE_PORTPIXFMT_DSI_18BPP_PACKED:
+		pixel_mode = 1;
+		rgb_header = 0x1E;
+		break;
+	case MCDE_PORTPIXFMT_DSI_24BPP:
+		pixel_mode = 3;
+		rgb_header = 0x3E;
+		break;
+	default:
+		pixel_mode = 3;
+		rgb_header = 0x3E;
+		dev_warn(&mcde_dev->dev,
+			"%s: invalid pixel format %d\n",
+			__func__, port->pixel_format);
+		break;
+	}
+
+	dsi_wfld(link, DSI_VID_MAIN_CTL, VID_PIXEL_MODE, pixel_mode);
+	dsi_wfld(link, DSI_VID_MAIN_CTL, HEADER, rgb_header);
+
+	if (tvg_enable) {
+		/*
+		 * with these settings, expect to see 64 pixels wide
+		 * red and green vertical stripes on the screen when
+		 * tvg_enable = 1
+		 */
+		dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, TVG_SEL, 1);
+
+		dsi_wfld(link, DSI_TVG_CTL, TVG_STRIPE_SIZE, 6);
+		dsi_wfld(link, DSI_TVG_CTL, TVG_MODE, 2);
+		dsi_wfld(link, DSI_TVG_CTL, TVG_STOPMODE, 2);
+		dsi_wfld(link, DSI_TVG_CTL, TVG_RUN, 1);
+
+		dsi_wfld(link, DSI_TVG_IMG_SIZE, TVG_NBLINE, vmode->yres);
+		dsi_wfld(link, DSI_TVG_IMG_SIZE, TVG_LINE_SIZE,
+							vmode->xres * bpp);
+
+		dsi_wfld(link, DSI_TVG_COLOR1, COL1_BLUE, 0);
+		dsi_wfld(link, DSI_TVG_COLOR1, COL1_GREEN, 0);
+		dsi_wfld(link, DSI_TVG_COLOR1, COL1_RED, 0xFF);
+
+		dsi_wfld(link, DSI_TVG_COLOR2, COL2_BLUE, 0);
+		dsi_wfld(link, DSI_TVG_COLOR2, COL2_GREEN, 0xFF);
+		dsi_wfld(link, DSI_TVG_COLOR2, COL2_RED, 0);
+	}
+
+	/*
+	 * vid->pixclock is the time between two pixels (in picoseconds)
+	 *
+	 * hs_byte_clk is the amount of transferred bytes per lane and
+	 * second (in MHz)
+	 */
+	hs_byte_clk = 1000000 / vmode->pixclock / 8;
+	pck_len = 1000000 * hs_byte_clk / port->refresh_rate /
+			(vmode->vsw + vmode->vbp + vmode->yres + vmode->vfp) *
+						port->phy.dsi.num_data_lanes;
+
+	/*
+	 * 6 is header + checksum, header = 4 bytes, checksum = 2 bytes
+	 * 4 is short packet for vsync/hsync
+	 */
+	if (sync_is_pulse)
+		blkline_pck = pck_len - vmode->hsw - 6;
+	else
+		blkline_pck = pck_len - 4 - 6;
+
+	line_duration = (blkline_pck + 6) / port->phy.dsi.num_data_lanes;
+	blkeol_pck = pck_len -
+		(vmode->hsw + vmode->hbp + vmode->xres + vmode->hfp) * bpp - 6;
+	blkeol_duration = (blkeol_pck + 6) / port->phy.dsi.num_data_lanes;
+
+	if (sync_is_pulse)
+		dsi_wfld(link, DSI_VID_BLKSIZE2, BLKLINE_PULSE_PCK,
+								blkline_pck);
+	else
+		dsi_wfld(link, DSI_VID_BLKSIZE1, BLKLINE_EVENT_PCK,
+								blkline_pck);
+	dsi_wfld(link, DSI_VID_DPHY_TIME, REG_LINE_DURATION, line_duration);
+	if (burst_mode) {
+		dsi_wfld(link, DSI_VID_BLKSIZE1, BLKEOL_PCK, blkeol_pck);
+		dsi_wfld(link, DSI_VID_PCK_TIME, BLKEOL_DURATION,
+							blkeol_duration);
+		dsi_wfld(link, DSI_VID_VCA_SETTING1, MAX_BURST_LIMIT,
+							blkeol_pck - 6);
+		dsi_wfld(link, DSI_VID_VCA_SETTING2, EXACT_BURST_LIMIT,
+								blkeol_pck);
+	}
+	if (sync_is_pulse)
+		dsi_wfld(link, DSI_VID_VCA_SETTING2, MAX_LINE_LIMIT,
+							blkline_pck - 6);
+}
+
+static void set_vsync_method(u8 idx, struct mcde_port *port)
+{
+	u32 out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER;
+	u32 src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_HARDWARE;
+
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		switch (port->frame_trig) {
+		case MCDE_TRIG_HW:
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_HARDWARE;
+			break;
+		case MCDE_TRIG_SW:
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE;
+			break;
+		default:
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_HARDWARE;
+			break;
+		}
+
+		switch (port->sync_src) {
+		case MCDE_SYNCSRC_OFF:
+			out_synch_src =
+				MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER;
+			break;
+		case MCDE_SYNCSRC_TE0:
+			out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_TE0;
+			if (src_synch ==
+				MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE) {
+				dev_dbg(&mcde_dev->dev, "%s: badly configured "
+						"frame sync, TE0 defaulting "
+						"to hw frame trig\n", __func__);
+				src_synch =
+					MCDE_CHNL0SYNCHMOD_SRC_SYNCH_HARDWARE;
+			}
+			break;
+		case MCDE_SYNCSRC_TE1:
+			out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_TE1;
+			if (src_synch ==
+				MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE) {
+				dev_dbg(&mcde_dev->dev, "%s: badly configured "
+						"frame sync, TE1 defaulting "
+						"to hw frame trig\n", __func__);
+				src_synch =
+					MCDE_CHNL0SYNCHMOD_SRC_SYNCH_HARDWARE;
+			}
+			break;
+		case MCDE_SYNCSRC_BTA:
+			out_synch_src =
+				MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER;
+			break;
+		case MCDE_SYNCSRC_TE_POLLING:
+			out_synch_src =
+				MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER;
+			if (src_synch ==
+				MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE) {
+				dev_dbg(&mcde_dev->dev, "%s: badly configured "
+					"frame sync, TE_POLLING defaulting "
+						"to hw frame trig\n", __func__);
+				src_synch =
+					MCDE_CHNL0SYNCHMOD_SRC_SYNCH_HARDWARE;
+			}
+			break;
+		default:
+			out_synch_src =
+				MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER;
+			src_synch = MCDE_CHNL0SYNCHMOD_SRC_SYNCH_HARDWARE;
+			dev_dbg(&mcde_dev->dev, "%s: no sync src selected, "
+						"defaulting to DSI BTA with "
+						"hw frame trig\n", __func__);
+			break;
+		}
+	} else if (port->type == MCDE_PORTTYPE_DPI) {
+		out_synch_src = MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER;
+		src_synch = port->update_auto_trig ?
+					MCDE_CHNL0SYNCHMOD_SRC_SYNCH_HARDWARE :
+					MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE;
+	}
+
+	mcde_wreg(MCDE_CHNL0SYNCHMOD +
+		idx * MCDE_CHNL0SYNCHMOD_GROUPOFFSET,
+		MCDE_CHNL0SYNCHMOD_SRC_SYNCH(src_synch) |
+		MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC(out_synch_src));
+}
+
+void update_channel_registers(enum mcde_chnl chnl_id, struct chnl_regs *regs,
+				struct mcde_port *port, enum mcde_fifo fifo,
+				struct mcde_video_mode *video_mode)
+{
+	u8 idx = chnl_id;
+	u32 fifo_wtrmrk = 0;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	/*
+	 * Select appropriate fifo watermark.
+	 * Watermark will be saturated at fifo size inside MCDE.
+	 */
+	fifo_wtrmrk = video_mode->xres /
+		get_pkt_div(video_mode->xres, port, fifo);
+
+	dev_vdbg(&mcde_dev->dev, "%s fifo_watermark=%d for chnl_id=%d\n",
+		__func__, fifo_wtrmrk, chnl_id);
+
+	switch (chnl_id) {
+	case MCDE_CHNL_A:
+		mcde_wfld(MCDE_CTRLA, FIFOWTRMRK, fifo_wtrmrk);
+		break;
+	case MCDE_CHNL_B:
+		mcde_wfld(MCDE_CTRLB, FIFOWTRMRK, fifo_wtrmrk);
+		break;
+	case MCDE_CHNL_C0:
+		mcde_wfld(MCDE_CTRLC0, FIFOWTRMRK, fifo_wtrmrk);
+		break;
+	case MCDE_CHNL_C1:
+		mcde_wfld(MCDE_CTRLC1, FIFOWTRMRK, fifo_wtrmrk);
+		break;
+	default:
+		break;
+	}
+
+	set_vsync_method(idx, port);
+
+	mcde_wreg(MCDE_CHNL0CONF + idx * MCDE_CHNL0CONF_GROUPOFFSET,
+		MCDE_CHNL0CONF_PPL(regs->ppl-1) |
+		MCDE_CHNL0CONF_LPF(regs->lpf-1));
+	mcde_wreg(MCDE_CHNL0STAT + idx * MCDE_CHNL0STAT_GROUPOFFSET,
+		MCDE_CHNL0STAT_CHNLBLBCKGND_EN(false) |
+		MCDE_CHNL0STAT_CHNLRD(true));
+	mcde_wreg(MCDE_CHNL0BCKGNDCOL + idx * MCDE_CHNL0BCKGNDCOL_GROUPOFFSET,
+		MCDE_CHNL0BCKGNDCOL_B(0) |
+		MCDE_CHNL0BCKGNDCOL_G(0) |
+		MCDE_CHNL0BCKGNDCOL_R(0));
+
+	if (chnl_id == MCDE_CHNL_A || chnl_id == MCDE_CHNL_B) {
+		u32 mcde_crx1;
+		u32 mcde_pal0x;
+		u32 mcde_pal1x;
+		if (chnl_id == MCDE_CHNL_A) {
+			mcde_crx1 = MCDE_CRA1;
+			mcde_pal0x = MCDE_PAL0A;
+			mcde_pal1x = MCDE_PAL1A;
+			mcde_wfld(MCDE_CRA0, PALEN, regs->palette_enable);
+		} else {
+			mcde_crx1 = MCDE_CRB1;
+			mcde_pal0x = MCDE_PAL0B;
+			mcde_pal1x = MCDE_PAL1B;
+			mcde_wfld(MCDE_CRB0, PALEN, regs->palette_enable);
+		}
+		mcde_wreg(mcde_crx1,
+			MCDE_CRA1_PCD(regs->pcd) |
+			MCDE_CRA1_CLKSEL(regs->clksel) |
+			MCDE_CRA1_CDWIN(regs->cdwin) |
+			MCDE_CRA1_OUTBPP(bpp2outbpp(regs->bpp)) |
+			MCDE_CRA1_BCD(regs->bcd) |
+			MCDE_CRA1_CLKTYPE(regs->internal_clk));
+		if (regs->palette_enable) {
+			int i;
+			for (i = 0; i < 256; i++) {
+				mcde_wreg(mcde_pal0x,
+					MCDE_PAL0A_GREEN(regs->map_g(i)) |
+					MCDE_PAL0A_BLUE(regs->map_b(i)));
+				mcde_wreg(mcde_pal1x,
+					MCDE_PAL1A_RED(regs->map_r(i)));
+			}
+		}
+	}
+
+	/* Formatter */
+	if (port->type == MCDE_PORTTYPE_DSI) {
+		u8 fidx;
+		u32 temp, packet;
+		/* pkt_div is used to avoid underflow in output fifo for
+		 * large packets */
+		u32 pkt_div = 1;
+		u32 dsi_delay0 = 0;
+		u32 screen_ppl, screen_lpf;
+
+		fidx = get_dsi_formatter_id(port);
+
+		screen_ppl = video_mode->xres;
+		screen_lpf = video_mode->yres;
+
+		pkt_div = get_pkt_div(screen_ppl, port, fifo);
+
+		if (video_mode->interlaced)
+			screen_lpf /= 2;
+
+		/* pkt_delay_progressive = pixelclock * htot /
+		 * (1E12 / 160E6) / pkt_div */
+		dsi_delay0 = (video_mode->pixclock) *
+			(video_mode->xres + video_mode->hbp +
+				video_mode->hfp) /
+			(100000000 / ((mcde_clk_rate / 10000))) / pkt_div;
+
+		if ((screen_ppl == SCREEN_PPL_CEA2) &&
+				(screen_lpf == SCREEN_LPF_CEA2))
+			dsi_delay0 += DSI_DELAY0_CEA2_ADD;
+
+		temp = mcde_rreg(MCDE_DSIVID0CONF0 +
+			fidx * MCDE_DSIVID0CONF0_GROUPOFFSET);
+		mcde_wreg(MCDE_DSIVID0CONF0 +
+			fidx * MCDE_DSIVID0CONF0_GROUPOFFSET,
+			(temp & ~MCDE_DSIVID0CONF0_PACKING_MASK) |
+			MCDE_DSIVID0CONF0_PACKING(regs->dsipacking));
+		/* no extra command byte in video mode */
+		if (port->mode == MCDE_PORTMODE_CMD)
+			packet = ((screen_ppl / pkt_div * regs->bpp) >> 3) + 1;
+		else
+			packet = ((screen_ppl / pkt_div * regs->bpp) >> 3);
+		mcde_wreg(MCDE_DSIVID0FRAME +
+			fidx * MCDE_DSIVID0FRAME_GROUPOFFSET,
+			MCDE_DSIVID0FRAME_FRAME(packet * pkt_div * screen_lpf));
+		mcde_wreg(MCDE_DSIVID0PKT + fidx * MCDE_DSIVID0PKT_GROUPOFFSET,
+			MCDE_DSIVID0PKT_PACKET(packet));
+		mcde_wreg(MCDE_DSIVID0SYNC +
+			fidx * MCDE_DSIVID0SYNC_GROUPOFFSET,
+			MCDE_DSIVID0SYNC_SW(0) |
+			MCDE_DSIVID0SYNC_DMA(0));
+		mcde_wreg(MCDE_DSIVID0CMDW +
+			fidx * MCDE_DSIVID0CMDW_GROUPOFFSET,
+			MCDE_DSIVID0CMDW_CMDW_START(DCS_CMD_WRITE_START) |
+			MCDE_DSIVID0CMDW_CMDW_CONTINUE(DCS_CMD_WRITE_CONTINUE));
+		mcde_wreg(MCDE_DSIVID0DELAY0 +
+			fidx * MCDE_DSIVID0DELAY0_GROUPOFFSET,
+			MCDE_DSIVID0DELAY0_INTPKTDEL(dsi_delay0));
+		mcde_wreg(MCDE_DSIVID0DELAY1 +
+			fidx * MCDE_DSIVID0DELAY1_GROUPOFFSET,
+			MCDE_DSIVID0DELAY1_TEREQDEL(0) |
+			MCDE_DSIVID0DELAY1_FRAMESTARTDEL(0));
+
+		if (port->mode == MCDE_PORTMODE_VID)
+			update_vid_frame_parameters(port, video_mode,
+								regs->bpp / 8);
+	} else if (port->type == MCDE_PORTTYPE_DPI &&
+						!port->phy.dpi.tv_mode) {
+		/* DPI LCD Mode */
+		if (chnl_id == MCDE_CHNL_A) {
+			mcde_wreg(MCDE_SYNCHCONFA,
+				MCDE_SYNCHCONFA_HWREQVEVENT_ENUM(
+							ACTIVE_VIDEO) |
+				MCDE_SYNCHCONFA_HWREQVCNT(
+							video_mode->yres - 1) |
+				MCDE_SYNCHCONFA_SWINTVEVENT_ENUM(
+							ACTIVE_VIDEO) |
+				MCDE_SYNCHCONFA_SWINTVCNT(
+							video_mode->yres - 1));
+		} else if (chnl_id == MCDE_CHNL_B) {
+			mcde_wreg(MCDE_SYNCHCONFB,
+				MCDE_SYNCHCONFB_HWREQVEVENT_ENUM(
+							ACTIVE_VIDEO) |
+				MCDE_SYNCHCONFB_HWREQVCNT(
+							video_mode->yres - 1) |
+				MCDE_SYNCHCONFB_SWINTVEVENT_ENUM(
+							ACTIVE_VIDEO) |
+				MCDE_SYNCHCONFB_SWINTVCNT(
+							video_mode->yres - 1));
+		}
+	}
+
+	if (regs->roten) {
+		u32 stripwidth;
+		u32 stripwidth_val;
+
+		/* calc strip width, 32 bits used internally */
+		stripwidth = regs->rotbufsize / (video_mode->yres * 4);
+		if (stripwidth >= 32)
+			stripwidth_val = MCDE_ROTACONF_STRIP_WIDTH_32PIX;
+		else if (stripwidth >= 16)
+			stripwidth_val = MCDE_ROTACONF_STRIP_WIDTH_16PIX;
+		else if (stripwidth >= 8)
+			stripwidth_val = MCDE_ROTACONF_STRIP_WIDTH_8PIX;
+		else if (stripwidth >= 4)
+			stripwidth_val = MCDE_ROTACONF_STRIP_WIDTH_4PIX;
+		else
+			stripwidth_val = MCDE_ROTACONF_STRIP_WIDTH_2PIX;
+		dev_vdbg(&mcde_dev->dev, "%s stripwidth=%d\n", __func__,
+						1 << (stripwidth_val + 1));
+		mcde_wreg(MCDE_ROTADD0A + chnl_id * MCDE_ROTADD0A_GROUPOFFSET,
+			regs->rotbuf1);
+		mcde_wreg(MCDE_ROTADD1A + chnl_id * MCDE_ROTADD1A_GROUPOFFSET,
+			regs->rotbuf2);
+		mcde_wreg(MCDE_ROTACONF + chnl_id * MCDE_ROTACONF_GROUPOFFSET,
+			MCDE_ROTACONF_ROTBURSTSIZE_ENUM(HW_8W) |
+			MCDE_ROTACONF_ROTDIR(regs->rotdir) |
+			MCDE_ROTACONF_STRIP_WIDTH(stripwidth_val) |
+			MCDE_ROTACONF_RD_MAXOUT_ENUM(4_REQ) |
+			MCDE_ROTACONF_WR_MAXOUT_ENUM(8_REQ));
+	}
+
+	/* Blending */
+	if (chnl_id == MCDE_CHNL_A) {
+		mcde_wfld(MCDE_CRA0, BLENDEN, regs->blend_en);
+		mcde_wfld(MCDE_CRA0, BLENDCTRL, regs->blend_ctrl);
+		mcde_wfld(MCDE_CRA0, ALPHABLEND, regs->alpha_blend);
+	} else if (chnl_id == MCDE_CHNL_B) {
+		mcde_wfld(MCDE_CRB0, BLENDEN, regs->blend_en);
+		mcde_wfld(MCDE_CRB0, BLENDCTRL, regs->blend_ctrl);
+		mcde_wfld(MCDE_CRB0, ALPHABLEND, regs->alpha_blend);
+	}
+
+	dev_vdbg(&mcde_dev->dev, "Channel registers setup, chnl=%d\n", chnl_id);
+	regs->dirty = false;
+}
+
+static int enable_mcde_hw(void)
+{
+	int ret;
+	int i;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	cancel_delayed_work(&hw_timeout_work);
+	schedule_delayed_work(&hw_timeout_work,
+					msecs_to_jiffies(MCDE_SLEEP_WATCHDOG));
+
+	for (i = 0; i < num_channels; i++) {
+		struct mcde_chnl_state *chnl = &channels[i];
+		if (chnl->state == CHNLSTATE_SUSPEND) {
+			/* Mark all registers as dirty */
+			set_channel_state_atomic(chnl, CHNLSTATE_IDLE);
+			chnl->ovly0->regs.dirty = true;
+			chnl->ovly0->regs.dirty_buf = true;
+			if (chnl->ovly1) {
+				chnl->ovly1->regs.dirty = true;
+				chnl->ovly1->regs.dirty_buf = true;
+			}
+			chnl->regs.dirty = true;
+			chnl->col_regs.dirty = true;
+			chnl->tv_regs.dirty = true;
+			atomic_set(&chnl->vcmp_cnt, 0);
+		}
+	}
+
+	if (mcde_is_enabled) {
+		dev_vdbg(&mcde_dev->dev, "%s - already enabled\n", __func__);
+		return 0;
+	}
+
+	enable_clocks_and_power(mcde_dev);
+
+	ret = request_irq(mcde_irq, mcde_irq_handler, 0, "mcde",
+							&mcde_dev->dev);
+	if (ret) {
+		dev_dbg(&mcde_dev->dev, "Failed to request irq (irq=%d)\n",
+								mcde_irq);
+		cancel_delayed_work(&hw_timeout_work);
+		return -EINVAL;
+	}
+
+	update_mcde_registers();
+
+	dev_vdbg(&mcde_dev->dev, "%s - enable done\n", __func__);
+
+	mcde_is_enabled = true;
+	return 0;
+}
+
+/* DSI */
+static int mcde_dsi_direct_cmd_write(struct mcde_chnl_state *chnl,
+			bool dcs, u8 cmd, u8 *data, int len)
+{
+	int i, ret = 0;
+	u32 wrdat[4] = { 0, 0, 0, 0 };
+	u32 settings;
+	u8 link = chnl->port.link;
+	u8 virt_id = chnl->port.phy.dsi.virt_id;
+	u32 counter = DSI_WRITE_CMD_TIMEOUT;
+
+	if (len > MCDE_MAX_DSI_DIRECT_CMD_WRITE ||
+			chnl->port.type != MCDE_PORTTYPE_DSI)
+		return -EINVAL;
+
+	mcde_lock(__func__, __LINE__);
+
+	_mcde_chnl_enable(chnl);
+	if (enable_mcde_hw()) {
+		mcde_unlock(__func__, __LINE__);
+		return -EINVAL;
+	}
+	if (!chnl->formatter_updated)
+		(void)update_channel_static_registers(chnl);
+
+	set_channel_state_sync(chnl, CHNLSTATE_DSI_WRITE);
+
+	if (dcs) {
+		wrdat[0] = cmd;
+		for (i = 1; i <= len; i++)
+			wrdat[i>>2] |= ((u32)data[i-1] << ((i & 3) * 8));
+	} else {
+		/* no explicit cmd byte for generic_write, only params */
+		for (i = 0; i < len; i++)
+			wrdat[i>>2] |= ((u32)data[i] << ((i & 3) * 8));
+	}
+
+	settings = DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(WRITE) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(len > 1) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(virt_id) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(len+1) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(true);
+	if (dcs) {
+		if (len == 0)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				DCS_SHORT_WRITE_0);
+		else if (len == 1)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				DCS_SHORT_WRITE_1);
+		else
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				DCS_LONG_WRITE);
+	} else {
+		if (len == 0)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				GENERIC_SHORT_WRITE_0);
+		else if (len == 1)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				GENERIC_SHORT_WRITE_1);
+		else if (len == 2)
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				GENERIC_SHORT_WRITE_2);
+		else
+			settings |= DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+				GENERIC_LONG_WRITE);
+	}
+
+	dsi_wreg(link, DSI_DIRECT_CMD_MAIN_SETTINGS, settings);
+	dsi_wreg(link, DSI_DIRECT_CMD_WRDAT0, wrdat[0]);
+	if (len >  3)
+		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT1, wrdat[1]);
+	if (len >  7)
+		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT2, wrdat[2]);
+	if (len > 11)
+		dsi_wreg(link, DSI_DIRECT_CMD_WRDAT3, wrdat[3]);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
+	dsi_wreg(link, DSI_CMD_MODE_STS_CLR, ~0);
+	dsi_wreg(link, DSI_DIRECT_CMD_SEND, true);
+
+	/* loop will normally run zero or one time until WRITE_COMPLETED */
+	while (!dsi_rfld(link, DSI_DIRECT_CMD_STS, WRITE_COMPLETED)
+			&& --counter)
+		cpu_relax();
+
+	if (!counter) {
+		dev_err(&mcde_dev->dev,
+			"%s: DSI write cmd 0x%x timeout on DSI link %u!\n",
+			__func__, cmd, link);
+		ret = -ETIME;
+	} else {
+		/* inform if >100 loops before command completion */
+		if (counter < (DSI_WRITE_CMD_TIMEOUT-DSI_WRITE_CMD_TIMEOUT/10))
+			dev_vdbg(&mcde_dev->dev,
+				"%s: %u loops for DSI command %x completion\n",
+				__func__, (DSI_WRITE_CMD_TIMEOUT - counter),
+				cmd);
+
+		dev_vdbg(&mcde_dev->dev, "DSI Write ok %x error %x\n",
+			dsi_rreg(link, DSI_DIRECT_CMD_STS_FLAG),
+			dsi_rreg(link, DSI_CMD_MODE_STS_FLAG));
+	}
+
+	set_channel_state_atomic(chnl, CHNLSTATE_IDLE);
+
+	mcde_unlock(__func__, __LINE__);
+
+	return ret;
+}
+
+int mcde_dsi_generic_write(struct mcde_chnl_state *chnl, u8* para, int len)
+{
+	return mcde_dsi_direct_cmd_write(chnl, false, 0, para, len);
+}
+
+int mcde_dsi_dcs_write(struct mcde_chnl_state *chnl, u8 cmd, u8* data, int len)
+{
+	return mcde_dsi_direct_cmd_write(chnl, true, cmd, data, len);
+}
+
+int mcde_dsi_dcs_read(struct mcde_chnl_state *chnl,
+			u8 cmd, u32 *data, int *len)
+{
+	int ret = 0;
+	u8 link = chnl->port.link;
+	u8 virt_id = chnl->port.phy.dsi.virt_id;
+	u32 settings;
+	bool ok = false;
+	bool error, ack_with_err;
+	u8 nbr_of_retries = DSI_READ_NBR_OF_RETRIES;
+
+	if (*len > MCDE_MAX_DCS_READ || chnl->port.type != MCDE_PORTTYPE_DSI)
+		return -EINVAL;
+
+	mcde_lock(__func__, __LINE__);
+
+	_mcde_chnl_enable(chnl);
+	if (enable_mcde_hw()) {
+		mcde_unlock(__func__, __LINE__);
+		return -EINVAL;
+	}
+	if (!chnl->formatter_updated)
+		(void)update_channel_static_registers(chnl);
+
+	set_channel_state_sync(chnl, CHNLSTATE_DSI_READ);
+
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, true);
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, READ_EN, true);
+	settings = DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(READ) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(false) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(virt_id) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(1) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(true) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(DCS_READ);
+	dsi_wreg(link, DSI_DIRECT_CMD_MAIN_SETTINGS, settings);
+	dsi_wreg(link, DSI_DIRECT_CMD_WRDAT0, cmd);
+
+	do {
+		u8 wait  = DSI_READ_TIMEOUT;
+		dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
+		dsi_wreg(link, DSI_DIRECT_CMD_RD_STS_CLR, ~0);
+		dsi_wreg(link, DSI_DIRECT_CMD_SEND, true);
+
+		while (wait-- && !(error = dsi_rfld(link, DSI_DIRECT_CMD_STS,
+					READ_COMPLETED_WITH_ERR)) &&
+				!(ok = dsi_rfld(link, DSI_DIRECT_CMD_STS,
+							READ_COMPLETED)))
+			udelay(DSI_READ_DELAY);
+
+		ack_with_err = dsi_rfld(link, DSI_DIRECT_CMD_STS,
+						ACKNOWLEDGE_WITH_ERR_RECEIVED);
+		if (ack_with_err)
+			dev_warn(&mcde_dev->dev,
+					"DCS Acknowledge Error Report %.4X\n",
+				dsi_rfld(link, DSI_DIRECT_CMD_STS, ACK_VAL));
+	} while (--nbr_of_retries && ack_with_err);
+
+	if (ok) {
+		int rdsize;
+		u32 rddat;
+
+		rdsize = dsi_rfld(link, DSI_DIRECT_CMD_RD_PROPERTY, RD_SIZE);
+		rddat = dsi_rreg(link, DSI_DIRECT_CMD_RDDAT);
+		if (rdsize < *len)
+			dev_warn(&mcde_dev->dev, "DCS incomplete read %d<%d"
+					" (%.8X)\n", rdsize, *len, rddat);
+		*len = min(*len, rdsize);
+		memcpy(data, &rddat, *len);
+	} else {
+		dev_err(&mcde_dev->dev, "DCS read failed, err=%d, sts=%X\n",
+				error, dsi_rreg(link, DSI_DIRECT_CMD_STS));
+		ret = -EIO;
+	}
+
+	dsi_wreg(link, DSI_CMD_MODE_STS_CLR, ~0);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR, ~0);
+
+	set_channel_state_atomic(chnl, CHNLSTATE_IDLE);
+
+	mcde_unlock(__func__, __LINE__);
+
+	return ret;
+}
+
+/*
+ * Set Maximum Return Packet size is a command that specifies the
+ * maximum size of the payload transmitted from peripheral back to
+ * the host processor.
+ *
+ * During power-on or reset sequence, the Maximum Return Packet Size
+ * is set to a default value of one. In order to be able to use
+ * mcde_dsi_dcs_read for reading more than 1 byte at a time, this
+ * parameter should be set by the host processor to the desired value
+ * in the initialization routine before commencing normal operation.
+ */
+int mcde_dsi_set_max_pkt_size(struct mcde_chnl_state *chnl)
+{
+	u32 settings;
+	u8 link = chnl->port.link;
+	u8 virt_id = chnl->port.phy.dsi.virt_id;
+
+	if (chnl->port.type != MCDE_PORTTYPE_DSI)
+		return -EINVAL;
+
+	mcde_lock(__func__, __LINE__);
+
+	if (enable_mcde_hw()) {
+		mcde_unlock(__func__, __LINE__);
+		return -EIO;
+	}
+
+	set_channel_state_sync(chnl, CHNLSTATE_DSI_WRITE);
+
+	/*
+	 * Set Maximum Return Packet Size is a two-byte command packet
+	 * that specifies the maximum size of the payload as u16 value.
+	 * The order of bytes is: MaxSize LSB, MaxSize MSB
+	 */
+	settings = DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(WRITE) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(false) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(virt_id) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(2) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(true) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(
+							SET_MAX_PKT_SIZE);
+	dsi_wreg(link, DSI_DIRECT_CMD_MAIN_SETTINGS, settings);
+	dsi_wreg(link, DSI_DIRECT_CMD_WRDAT0, MCDE_MAX_DCS_READ);
+	dsi_wreg(link, DSI_DIRECT_CMD_SEND, true);
+
+	set_channel_state_atomic(chnl, CHNLSTATE_IDLE);
+
+	mcde_unlock(__func__, __LINE__);
+
+	return 0;
+}
+
+static void dsi_te_poll_req(struct mcde_chnl_state *chnl)
+{
+	u8 lnk = chnl->port.link;
+	const struct mcde_port *port = &chnl->port;
+
+	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, false);
+	if (port->ifc == 0)
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, IF1_TE_EN, true);
+	if (port->ifc == 1)
+		dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, IF2_TE_EN, true);
+	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, true);
+	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, READ_EN, true);
+	dsi_wfld(lnk, DSI_CMD_MODE_CTL, TE_TIMEOUT, 0x3FF);
+	dsi_wfld(lnk, DSI_MCTL_MAIN_DATA_CTL, TE_POLLING_EN, true);
+}
+
+static void dsi_te_poll_set_timer(struct mcde_chnl_state *chnl,
+		unsigned int timeout)
+{
+	mod_timer(&chnl->dsi_te_timer,
+			jiffies +
+			msecs_to_jiffies(timeout));
+}
+
+static void dsi_te_timer_function(unsigned long arg)
+{
+	struct mcde_chnl_state *chnl;
+	u8 lnk;
+
+	if (arg >= num_channels) {
+		dev_err(&mcde_dev->dev, "%s invalid arg:%ld\n", __func__, arg);
+		return;
+	}
+
+	chnl = &channels[arg];
+
+	if (mcde_is_enabled && chnl->enabled && chnl->formatter_updated) {
+		lnk = chnl->port.link;
+		/* No TE answer; force stop */
+		dsi_wfld(lnk, DSI_MCTL_MAIN_PHY_CTL, FORCE_STOP_MODE, true);
+		udelay(20);
+		dsi_wfld(lnk, DSI_MCTL_MAIN_PHY_CTL, FORCE_STOP_MODE, false);
+		dev_info(&mcde_dev->dev, "DSI%d force stop\n", lnk);
+		dsi_te_poll_set_timer(chnl, DSI_TE_NO_ANSWER_TIMEOUT);
+	} else {
+		dev_info(&mcde_dev->dev, "1:DSI force stop\n");
+	}
+}
+
+static void dsi_te_request(struct mcde_chnl_state *chnl)
+{
+	u8 link = chnl->port.link;
+	u8 virt_id = chnl->port.phy.dsi.virt_id;
+	u32 settings;
+
+	dev_vdbg(&mcde_dev->dev, "Request BTA TE, chnl=%d\n",
+		chnl->id);
+
+	set_channel_state_atomic(chnl, CHNLSTATE_WAIT_TE);
+
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, BTA_EN, true);
+	dsi_wfld(link, DSI_MCTL_MAIN_DATA_CTL, REG_TE_EN, true);
+	dsi_wfld(link, DSI_CMD_MODE_CTL, TE_TIMEOUT, 0x3FF);
+	settings = DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_NAT_ENUM(TE_REQ) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LONGNOTSHORT(false) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_ID(virt_id) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_SIZE(2) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_LP_EN(true) |
+		DSI_DIRECT_CMD_MAIN_SETTINGS_CMD_HEAD_ENUM(DCS_SHORT_WRITE_1);
+	dsi_wreg(link, DSI_DIRECT_CMD_MAIN_SETTINGS, settings);
+	dsi_wreg(link, DSI_DIRECT_CMD_WRDAT0, DCS_CMD_SET_TEAR_ON);
+	dsi_wreg(link, DSI_DIRECT_CMD_STS_CLR,
+		DSI_DIRECT_CMD_STS_CLR_TE_RECEIVED_CLR(true));
+	dsi_wfld(link, DSI_DIRECT_CMD_STS_CTL, TE_RECEIVED_EN, true);
+	dsi_wreg(link, DSI_CMD_MODE_STS_CLR,
+		DSI_CMD_MODE_STS_CLR_ERR_NO_TE_CLR(true));
+	dsi_wfld(link, DSI_CMD_MODE_STS_CTL, ERR_NO_TE_EN, true);
+	dsi_wreg(link, DSI_DIRECT_CMD_SEND, true);
+}
+
+/* MCDE channels */
+static struct mcde_chnl_state *_mcde_chnl_get(enum mcde_chnl chnl_id,
+	enum mcde_fifo fifo, const struct mcde_port *port)
+{
+	int i;
+	struct mcde_chnl_state *chnl = NULL;
+	struct mcde_platform_data *pdata = mcde_dev->dev.platform_data;
+
+	static struct mcde_col_transform ycbcr_2_rgb = {
+		/* Note that in MCDE YUV 422 pixels come as VYU pixels */
+		.matrix = {
+			{0xff30, 0x012a, 0xff9c},
+			{0x0000, 0x012a, 0x0204},
+			{0x0199, 0x012a, 0x0000},
+		},
+		.offset = {0x0088, 0xfeeb, 0xff21},
+	};
+
+	static struct mcde_col_transform rgb_2_ycbcr = {
+		.matrix = {
+			{0x0042, 0x0081, 0x0019},
+			{0xffda, 0xffb6, 0x0070},
+			{0x0070, 0xffa2, 0xffee},
+		},
+		.offset = {0x0010, 0x0080, 0x0080},
+	};
+
+	/* Allocate channel */
+	for (i = 0; i < num_channels; i++) {
+		if (chnl_id == channels[i].id)
+			chnl = &channels[i];
+	}
+	if (!chnl) {
+		dev_dbg(&mcde_dev->dev, "Invalid channel, chnl=%d\n", chnl_id);
+		return ERR_PTR(-EINVAL);
+	}
+	if (chnl->reserved) {
+		dev_dbg(&mcde_dev->dev, "Channel in use, chnl=%d\n", chnl_id);
+		return ERR_PTR(-EBUSY);
+	}
+
+	chnl->port = *port;
+	chnl->fifo = fifo;
+	chnl->formatter_updated = false;
+	chnl->ycbcr_2_rgb = ycbcr_2_rgb;
+	chnl->rgb_2_ycbcr = rgb_2_ycbcr;
+
+	chnl->blend_en = true;
+	chnl->blend_ctrl = MCDE_CRA0_BLENDCTRL_SOURCE;
+	chnl->alpha_blend = 0xFF;
+	chnl->rotbuf1 = pdata->rotbuf1;
+	chnl->rotbuf2 = pdata->rotbuf2;
+	chnl->rotbufsize = pdata->rotbufsize;
+
+	_mcde_chnl_apply(chnl);
+	chnl->reserved = true;
+
+	if (chnl->port.type == MCDE_PORTTYPE_DPI) {
+		chnl->clk_dpi = clk_get(&mcde_dev->dev, CLK_DPI);
+		if (chnl->port.phy.dpi.tv_mode)
+			chnl->vcmp_per_field = true;
+	} else if (chnl->port.type == MCDE_PORTTYPE_DSI &&
+							dsi_use_clk_framework) {
+		char dsihs_name[10];
+		char dsilp_name[10];
+
+		sprintf(dsihs_name, "dsihs%d", port->link);
+		sprintf(dsilp_name, "dsilp%d", port->link);
+
+		chnl->clk_dsi_lp = clk_get(&mcde_dev->dev, dsilp_name);
+		chnl->clk_dsi_hs = clk_get(&mcde_dev->dev, dsihs_name);
+		if (port->phy.dsi.lp_freq != clk_round_rate(chnl->clk_dsi_lp,
+							port->phy.dsi.lp_freq))
+			dev_warn(&mcde_dev->dev, "Could not set dsi lp freq"
+					" to %d\n", port->phy.dsi.lp_freq);
+		WARN_ON_ONCE(clk_set_rate(chnl->clk_dsi_lp,
+							port->phy.dsi.lp_freq));
+		if (port->phy.dsi.hs_freq != clk_round_rate(chnl->clk_dsi_hs,
+							port->phy.dsi.hs_freq))
+			dev_warn(&mcde_dev->dev, "Could not set dsi hs freq"
+					" to %d\n", port->phy.dsi.hs_freq);
+		WARN_ON_ONCE(clk_set_rate(chnl->clk_dsi_hs,
+							port->phy.dsi.hs_freq));
+	}
+	return chnl;
+}
+
+static int _mcde_chnl_apply(struct mcde_chnl_state *chnl)
+{
+	bool roten = false;
+	u8 rotdir = 0;
+
+	if (chnl->rotation == MCDE_DISPLAY_ROT_90_CCW) {
+		roten = true;
+		rotdir = MCDE_ROTACONF_ROTDIR_CCW;
+	} else if (chnl->rotation == MCDE_DISPLAY_ROT_90_CW) {
+		roten = true;
+		rotdir = MCDE_ROTACONF_ROTDIR_CW;
+	}
+	/* REVIEW: 180 deg? */
+
+	chnl->regs.bpp = portfmt2bpp(chnl->port.pixel_format);
+	chnl->regs.roten = roten;
+	chnl->regs.rotdir = rotdir;
+	chnl->regs.rotbuf1 = chnl->rotbuf1;
+	chnl->regs.rotbuf2 = chnl->rotbuf2;
+	chnl->regs.rotbufsize = chnl->rotbufsize;
+	chnl->regs.palette_enable = chnl->palette_enable;
+	chnl->regs.map_r = chnl->map_r;
+	chnl->regs.map_g = chnl->map_g;
+	chnl->regs.map_b = chnl->map_b;
+	if (chnl->port.type == MCDE_PORTTYPE_DSI) {
+		chnl->regs.clksel = MCDE_CRA1_CLKSEL_MCDECLK;
+		chnl->regs.dsipacking =
+				portfmt2dsipacking(chnl->port.pixel_format);
+	} else if (chnl->port.type == MCDE_PORTTYPE_DPI) {
+		if (chnl->port.phy.dpi.tv_mode) {
+			chnl->regs.internal_clk = false;
+			chnl->regs.bcd = true;
+			if (chnl->id == MCDE_CHNL_A)
+				chnl->regs.clksel = MCDE_CRA1_CLKSEL_TV1CLK;
+			else
+				chnl->regs.clksel = MCDE_CRA1_CLKSEL_TV2CLK;
+		} else {
+			chnl->regs.internal_clk = true;
+			chnl->regs.clksel = MCDE_CRA1_CLKSEL_CLKPLL72;
+			chnl->regs.cdwin =
+					portfmt2cdwin(chnl->port.pixel_format);
+			chnl->regs.bcd = (chnl->port.phy.dpi.clock_div < 2);
+			if (!chnl->regs.bcd)
+				chnl->regs.pcd =
+					chnl->port.phy.dpi.clock_div - 2;
+		}
+		dpi_video_mode_apply(chnl);
+	}
+
+	chnl->regs.blend_ctrl = chnl->blend_ctrl;
+	chnl->regs.blend_en = chnl->blend_en;
+	chnl->regs.alpha_blend = chnl->alpha_blend;
+
+	chnl->regs.dirty = true;
+
+	dev_vdbg(&mcde_dev->dev, "Channel applied, chnl=%d\n", chnl->id);
+	return 0;
+}
+
+static void setup_channel(struct mcde_chnl_state *chnl)
+{
+	set_channel_state_sync(chnl, CHNLSTATE_SETUP);
+
+	if (chnl->port.type == MCDE_PORTTYPE_DPI && chnl->tv_regs.dirty)
+		update_dpi_registers(chnl->id, &chnl->tv_regs);
+	if ((chnl->id == MCDE_CHNL_A || chnl->id == MCDE_CHNL_B) &&
+							chnl->col_regs.dirty)
+		update_col_registers(chnl->id, &chnl->col_regs);
+	if (chnl->regs.dirty)
+		update_channel_registers(chnl->id, &chnl->regs, &chnl->port,
+						chnl->fifo, &chnl->vmode);
+}
+
+static void chnl_update_continous(struct mcde_chnl_state *chnl,
+						bool tripple_buffer)
+{
+	if (chnl->state == CHNLSTATE_RUNNING) {
+		if (!tripple_buffer)
+			wait_for_vcmp(chnl);
+		return;
+	}
+
+	setup_channel(chnl);
+	if (chnl->port.sync_src == MCDE_SYNCSRC_TE0) {
+		mcde_wfld(MCDE_CRC, SYCEN0, true);
+	} else if (chnl->port.sync_src == MCDE_SYNCSRC_TE1) {
+		mcde_wfld(MCDE_VSCRC1, VSSEL, 1);
+		mcde_wfld(MCDE_CRC, SYCEN1, true);
+	}
+
+	enable_flow(chnl);
+}
+
+static void chnl_update_non_continous(struct mcde_chnl_state *chnl)
+{
+	/* Commit settings to registers */
+	setup_channel(chnl);
+
+	if (chnl->port.type == MCDE_PORTTYPE_DSI) {
+		if (chnl->port.sync_src == MCDE_SYNCSRC_OFF) {
+			if (chnl->port.frame_trig == MCDE_TRIG_SW) {
+				do_softwaretrig(chnl);
+			} else {
+				enable_flow(chnl);
+				disable_flow(chnl);
+			}
+			dev_vdbg(&mcde_dev->dev, "Channel update (no sync), "
+							"chnl=%d\n", chnl->id);
+		} else if (chnl->port.sync_src == MCDE_SYNCSRC_BTA) {
+			if (chnl->power_mode == MCDE_DISPLAY_PM_ON) {
+				dsi_te_request(chnl);
+			} else {
+				if (chnl->port.frame_trig == MCDE_TRIG_SW)
+					do_softwaretrig(chnl);
+			}
+			if (chnl->port.frame_trig == MCDE_TRIG_HW) {
+				/*
+				 * During BTA TE the MCDE block will be stalled,
+				 * once the TE is received the DMA trig will
+				 * happen
+				 */
+				enable_flow(chnl);
+				disable_flow(chnl);
+			}
+		}
+	}
+}
+
+static void chnl_update_overlay(struct mcde_chnl_state *chnl,
+						struct mcde_ovly_state *ovly)
+{
+	if (!ovly)
+		return;
+
+	if (ovly->regs.dirty_buf) {
+		if (!chnl->port.update_auto_trig)
+			set_channel_state_sync(chnl, CHNLSTATE_SETUP);
+		update_overlay_registers_on_the_fly(ovly->idx, &ovly->regs);
+		mcde_debugfs_overlay_update(chnl->id, ovly != chnl->ovly0);
+	}
+	if (ovly->regs.dirty) {
+		if (!chnl->port.update_auto_trig)
+			set_channel_state_sync(chnl, CHNLSTATE_SETUP);
+		chnl_ovly_pixel_format_apply(chnl, ovly);
+		update_overlay_registers(ovly->idx, &ovly->regs, &chnl->port,
+			chnl->fifo, chnl->regs.x, chnl->regs.y,
+			chnl->regs.ppl, chnl->regs.lpf, ovly->stride,
+			chnl->vmode.interlaced, chnl->rotation);
+		if (chnl->id == MCDE_CHNL_A || chnl->id == MCDE_CHNL_B)
+			update_col_registers(chnl->id, &chnl->col_regs);
+	}
+}
+
+static int _mcde_chnl_update(struct mcde_chnl_state *chnl,
+					struct mcde_rectangle *update_area,
+					bool tripple_buffer)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	/* TODO: lock & make wait->trig async */
+	if (!chnl->enabled || !update_area
+			|| (update_area->w == 0 && update_area->h == 0)) {
+		return -EINVAL;
+	}
+
+	if (chnl->port.update_auto_trig && tripple_buffer)
+		wait_for_vcmp(chnl);
+
+	chnl->regs.x   = update_area->x;
+	chnl->regs.y   = update_area->y;
+	/* TODO Crop against video_mode.xres and video_mode.yres */
+	chnl->regs.ppl = update_area->w;
+	chnl->regs.lpf = update_area->h;
+	if (chnl->port.type == MCDE_PORTTYPE_DPI &&
+						chnl->port.phy.dpi.tv_mode) {
+		/* subtract border */
+		chnl->regs.ppl -= chnl->tv_regs.dho + chnl->tv_regs.alw;
+		/* subtract double borders, ie. for both fields */
+		chnl->regs.lpf -= 2 * (chnl->tv_regs.dvo + chnl->tv_regs.bsl);
+	} else if (chnl->port.type == MCDE_PORTTYPE_DSI &&
+			chnl->vmode.interlaced)
+		chnl->regs.lpf /= 2;
+
+	chnl_update_overlay(chnl, chnl->ovly0);
+	chnl_update_overlay(chnl, chnl->ovly1);
+
+	if (chnl->port.update_auto_trig)
+		chnl_update_continous(chnl, tripple_buffer);
+	else
+		chnl_update_non_continous(chnl);
+
+	dev_vdbg(&mcde_dev->dev, "Channel updated, chnl=%d\n", chnl->id);
+	mcde_debugfs_channel_update(chnl->id);
+	return 0;
+}
+
+static int _mcde_chnl_enable(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	chnl->enabled = true;
+	return 0;
+}
+
+/* API entry points */
+/* MCDE channels */
+struct mcde_chnl_state *mcde_chnl_get(enum mcde_chnl chnl_id,
+			enum mcde_fifo fifo, const struct mcde_port *port)
+{
+	struct mcde_chnl_state *chnl;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+	chnl = _mcde_chnl_get(chnl_id, fifo, port);
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return chnl;
+}
+
+int mcde_chnl_set_pixel_format(struct mcde_chnl_state *chnl,
+					enum mcde_port_pix_fmt pix_fmt)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+	chnl->port.pixel_format = pix_fmt;
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return 0;
+}
+
+int mcde_chnl_set_palette(struct mcde_chnl_state *chnl,
+					struct mcde_palette_table *palette)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+	if (palette != NULL) {
+		chnl->map_r = palette->map_col_ch0;
+		chnl->map_g = palette->map_col_ch1;
+		chnl->map_b = palette->map_col_ch2;
+		chnl->palette_enable = true;
+	} else {
+		chnl->map_r = NULL;
+		chnl->map_g = NULL;
+		chnl->map_b = NULL;
+		chnl->palette_enable = false;
+	}
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+	return 0;
+}
+
+void mcde_chnl_set_col_convert(struct mcde_chnl_state *chnl,
+					struct mcde_col_transform *transform,
+					enum   mcde_col_convert    convert)
+{
+	switch (convert) {
+	case MCDE_CONVERT_RGB_2_YCBCR:
+		memcpy(&chnl->rgb_2_ycbcr, transform,
+				sizeof(struct mcde_col_transform));
+		/* force update: */
+		if (chnl->transform == &chnl->rgb_2_ycbcr) {
+			chnl->transform = NULL;
+			chnl->ovly0->dirty = true;
+			chnl->ovly1->dirty = true;
+		}
+		break;
+	case MCDE_CONVERT_YCBCR_2_RGB:
+		memcpy(&chnl->ycbcr_2_rgb, transform,
+				sizeof(struct mcde_col_transform));
+		/* force update: */
+		if (chnl->transform == &chnl->ycbcr_2_rgb) {
+			chnl->transform = NULL;
+			chnl->ovly0->dirty = true;
+			chnl->ovly1->dirty = true;
+		}
+		break;
+	default:
+		/* Trivial transforms are handled internally */
+		dev_warn(&mcde_dev->dev,
+			"%s: unsupported col convert\n", __func__);
+		break;
+	}
+}
+
+int mcde_chnl_set_video_mode(struct mcde_chnl_state *chnl,
+					struct mcde_video_mode *vmode)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (chnl == NULL || vmode == NULL)
+		return -EINVAL;
+
+	chnl->vmode = *vmode;
+
+	chnl->ovly0->dirty = true;
+	if (chnl->ovly1)
+		chnl->ovly1->dirty = true;
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return 0;
+}
+EXPORT_SYMBOL(mcde_chnl_set_video_mode);
+
+int mcde_chnl_set_rotation(struct mcde_chnl_state *chnl,
+					enum mcde_display_rotation rotation)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+
+	if ((rotation == MCDE_DISPLAY_ROT_90_CW ||
+			rotation == MCDE_DISPLAY_ROT_90_CCW) &&
+			(chnl->id != MCDE_CHNL_A && chnl->id != MCDE_CHNL_B))
+		return -EINVAL;
+
+	chnl->rotation = rotation;
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return 0;
+}
+
+int mcde_chnl_set_power_mode(struct mcde_chnl_state *chnl,
+				enum mcde_display_power_mode power_mode)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+
+	chnl->power_mode = power_mode;
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+
+	return 0;
+}
+
+int mcde_chnl_apply(struct mcde_chnl_state *chnl)
+{
+	int ret ;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+
+	mcde_lock(__func__, __LINE__);
+	ret = _mcde_chnl_apply(chnl);
+	mcde_unlock(__func__, __LINE__);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit with ret %d\n", __func__, ret);
+
+	return ret;
+}
+
+int mcde_chnl_update(struct mcde_chnl_state *chnl,
+					struct mcde_rectangle *update_area,
+					bool tripple_buffer)
+{
+	int ret;
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return -EINVAL;
+
+	mcde_lock(__func__, __LINE__);
+	enable_mcde_hw();
+	if (!chnl->formatter_updated)
+		(void)update_channel_static_registers(chnl);
+
+	if (chnl->regs.roten && !chnl->esram_is_enabled) {
+		WARN_ON_ONCE(regulator_enable(regulator_esram_epod));
+		chnl->esram_is_enabled = true;
+	} else if (!chnl->regs.roten && chnl->esram_is_enabled) {
+		WARN_ON_ONCE(regulator_disable(regulator_esram_epod));
+		chnl->esram_is_enabled = false;
+	}
+
+	ret = _mcde_chnl_update(chnl, update_area, tripple_buffer);
+
+	mcde_unlock(__func__, __LINE__);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit with ret %d\n", __func__, ret);
+
+	return ret;
+}
+
+void mcde_chnl_put(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (chnl->enabled) {
+		stop_channel(chnl);
+		cancel_delayed_work(&hw_timeout_work);
+		disable_mcde_hw(false, true);
+		chnl->enabled = false;
+	}
+
+	chnl->reserved = false;
+	if (chnl->port.type == MCDE_PORTTYPE_DPI) {
+		clk_put(chnl->clk_dpi);
+		if (chnl->port.phy.dpi.tv_mode) {
+			chnl->vcmp_per_field = false;
+			chnl->even_vcmp = false;
+		}
+	} else if (chnl->port.type == MCDE_PORTTYPE_DSI) {
+		if (dsi_use_clk_framework) {
+			clk_put(chnl->clk_dsi_lp);
+			clk_put(chnl->clk_dsi_hs);
+		}
+	}
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+void mcde_chnl_stop_flow(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mcde_lock(__func__, __LINE__);
+	if (mcde_is_enabled && chnl->enabled)
+		stop_channel(chnl);
+	mcde_unlock(__func__, __LINE__);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+void mcde_chnl_enable(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mcde_lock(__func__, __LINE__);
+	_mcde_chnl_enable(chnl);
+	mcde_unlock(__func__, __LINE__);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+void mcde_chnl_disable(struct mcde_chnl_state *chnl)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mcde_lock(__func__, __LINE__);
+	cancel_delayed_work(&hw_timeout_work);
+	/* The channel must be stopped before it is disabled */
+	WARN_ON_ONCE(chnl->state == CHNLSTATE_RUNNING);
+	disable_mcde_hw(false, true);
+	chnl->enabled = false;
+	mcde_unlock(__func__, __LINE__);
+
+	dev_vdbg(&mcde_dev->dev, "%s exit\n", __func__);
+}
+
+/* MCDE overlays */
+struct mcde_ovly_state *mcde_ovly_get(struct mcde_chnl_state *chnl)
+{
+	struct mcde_ovly_state *ovly;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!chnl->reserved)
+		return ERR_PTR(-EINVAL);
+
+	if (!chnl->ovly0->inuse)
+		ovly = chnl->ovly0;
+	else if (chnl->ovly1 && !chnl->ovly1->inuse)
+		ovly = chnl->ovly1;
+	else
+		ovly = ERR_PTR(-EBUSY);
+
+	if (!IS_ERR(ovly)) {
+		ovly->inuse = true;
+		ovly->paddr = 0;
+		ovly->stride = 0;
+		ovly->pix_fmt = MCDE_OVLYPIXFMT_RGB565;
+		ovly->src_x = 0;
+		ovly->src_y = 0;
+		ovly->dst_x = 0;
+		ovly->dst_y = 0;
+		ovly->dst_z = 0;
+		ovly->w = 0;
+		ovly->h = 0;
+		ovly->alpha_value = 0xFF;
+		ovly->alpha_source = MCDE_OVL1CONF2_BP_PER_PIXEL_ALPHA;
+		ovly->dirty = true;
+		mcde_ovly_apply(ovly);
+	}
+
+	return ovly;
+}
+
+void mcde_ovly_put(struct mcde_ovly_state *ovly)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	if (!ovly->inuse)
+		return;
+	if (ovly->regs.enabled) {
+		ovly->paddr = 0;
+		ovly->dirty = true;
+		mcde_ovly_apply(ovly);/* REVIEW: API call calling API call! */
+	}
+	ovly->inuse = false;
+}
+
+void mcde_ovly_set_source_buf(struct mcde_ovly_state *ovly, u32 paddr)
+{
+	if (!ovly->inuse)
+		return;
+
+	ovly->dirty = paddr == 0 || ovly->paddr == 0;
+	ovly->dirty_buf = true;
+
+	ovly->paddr = paddr;
+}
+
+void mcde_ovly_set_source_info(struct mcde_ovly_state *ovly,
+	u32 stride, enum mcde_ovly_pix_fmt pix_fmt)
+{
+	if (!ovly->inuse)
+		return;
+
+	ovly->stride = stride;
+	ovly->pix_fmt = pix_fmt;
+	ovly->dirty = true;
+}
+
+void mcde_ovly_set_source_area(struct mcde_ovly_state *ovly,
+	u16 x, u16 y, u16 w, u16 h)
+{
+	if (!ovly->inuse)
+		return;
+
+	ovly->src_x = x;
+	ovly->src_y = y;
+	ovly->w = w;
+	ovly->h = h;
+	ovly->dirty = true;
+}
+
+void mcde_ovly_set_dest_pos(struct mcde_ovly_state *ovly, u16 x, u16 y, u8 z)
+{
+	if (!ovly->inuse)
+		return;
+
+	ovly->dst_x = x;
+	ovly->dst_y = y;
+	ovly->dst_z = z;
+	ovly->dirty = true;
+}
+
+void mcde_ovly_apply(struct mcde_ovly_state *ovly)
+{
+	if (!ovly->inuse)
+		return;
+
+	mcde_lock(__func__, __LINE__);
+
+	if (ovly->dirty || ovly->dirty_buf) {
+		ovly->regs.ch_id = ovly->chnl->id;
+		ovly->regs.enabled = ovly->paddr != 0;
+		ovly->regs.baseaddress0 = ovly->paddr;
+		ovly->regs.baseaddress1 =
+					ovly->regs.baseaddress0 + ovly->stride;
+		ovly->regs.dirty_buf = true;
+		ovly->dirty_buf = false;
+	}
+	if (!ovly->dirty) {
+		mcde_unlock(__func__, __LINE__);
+		return;
+	}
+
+	switch (ovly->pix_fmt) {/* REVIEW: Extract to table */
+	case MCDE_OVLYPIXFMT_RGB565:
+		ovly->regs.bits_per_pixel = 16;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_RGB565;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = true;
+		break;
+	case MCDE_OVLYPIXFMT_RGBA5551:
+		ovly->regs.bits_per_pixel = 16;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_IRGB1555;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = false;
+		break;
+	case MCDE_OVLYPIXFMT_RGBA4444:
+		ovly->regs.bits_per_pixel = 16;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_ARGB4444;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = false;
+		break;
+	case MCDE_OVLYPIXFMT_RGB888:
+		ovly->regs.bits_per_pixel = 24;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_RGB888;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = true;
+		break;
+	case MCDE_OVLYPIXFMT_RGBX8888:
+		ovly->regs.bits_per_pixel = 32;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_XRGB8888;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = true;
+		ovly->regs.opq = true;
+		break;
+	case MCDE_OVLYPIXFMT_RGBA8888:
+		ovly->regs.bits_per_pixel = 32;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_ARGB8888;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = false;
+		break;
+	case MCDE_OVLYPIXFMT_YCbCr422:
+		ovly->regs.bits_per_pixel = 16;
+		ovly->regs.bpp = MCDE_EXTSRC0CONF_BPP_YCBCR422;
+		ovly->regs.bgr = false;
+		ovly->regs.bebo = false;
+		ovly->regs.opq = true;
+		break;
+	default:
+		break;
+	}
+
+	ovly->regs.ppl = ovly->w;
+	ovly->regs.lpf = ovly->h;
+	ovly->regs.cropx = ovly->src_x;
+	ovly->regs.cropy = ovly->src_y;
+	ovly->regs.xpos = ovly->dst_x;
+	ovly->regs.ypos = ovly->dst_y;
+	ovly->regs.z = ovly->dst_z > 0; /* 0 or 1 */
+	ovly->regs.col_conv = MCDE_OVL0CR_COLCCTRL_DISABLED;
+	ovly->regs.alpha_source = ovly->alpha_source;
+	ovly->regs.alpha_value = ovly->alpha_value;
+
+	ovly->regs.dirty = true;
+	ovly->dirty = false;
+
+	mcde_unlock(__func__, __LINE__);
+
+	dev_vdbg(&mcde_dev->dev, "Overlay applied, idx=%d chnl=%d\n",
+						ovly->idx, ovly->chnl->id);
+}
+
+static int init_clocks_and_power(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct mcde_platform_data *pdata = pdev->dev.platform_data;
+
+	if (pdata->regulator_mcde_epod_id) {
+		regulator_mcde_epod = regulator_get(&pdev->dev,
+				pdata->regulator_mcde_epod_id);
+		if (IS_ERR(regulator_mcde_epod)) {
+			ret = PTR_ERR(regulator_mcde_epod);
+			dev_warn(&pdev->dev,
+				"%s: Failed to get regulator '%s'\n",
+				__func__, pdata->regulator_mcde_epod_id);
+			regulator_mcde_epod = NULL;
+			return ret;
+		}
+	} else {
+		dev_warn(&pdev->dev, "%s: No mcde regulator id supplied\n",
+								__func__);
+		return -EINVAL;
+	}
+
+	if (pdata->regulator_esram_epod_id) {
+		regulator_esram_epod = regulator_get(&pdev->dev,
+				pdata->regulator_esram_epod_id);
+		if (IS_ERR(regulator_esram_epod)) {
+			ret = PTR_ERR(regulator_esram_epod);
+			dev_warn(&pdev->dev,
+				"%s: Failed to get regulator '%s'\n",
+				__func__, pdata->regulator_esram_epod_id);
+			regulator_esram_epod = NULL;
+			goto regulator_esram_err;
+		}
+	} else {
+		dev_warn(&pdev->dev, "%s: No esram regulator id supplied\n",
+								__func__);
+	}
+
+	if (pdata->regulator_vana_id) {
+		regulator_vana = regulator_get(&pdev->dev,
+				pdata->regulator_vana_id);
+		if (IS_ERR(regulator_vana)) {
+			ret = PTR_ERR(regulator_vana);
+			dev_warn(&pdev->dev,
+				"%s: Failed to get regulator '%s'\n",
+				__func__, pdata->regulator_vana_id);
+			regulator_vana = NULL;
+			goto regulator_vana_err;
+		}
+	} else {
+		dev_dbg(&pdev->dev, "%s: No vana regulator id supplied\n",
+								__func__);
+	}
+
+	if (!dsi_use_clk_framework) {
+		clock_dsi = clk_get(&pdev->dev, pdata->clock_dsi_id);
+		if (IS_ERR(clock_dsi))
+			dev_dbg(&pdev->dev, "%s: Failed to get clock '%s'\n",
+						__func__, pdata->clock_dsi_id);
+
+		clock_dsi_lp = clk_get(&pdev->dev, pdata->clock_dsi_lp_id);
+		if (IS_ERR(clock_dsi_lp))
+			dev_dbg(&pdev->dev, "%s: Failed to get clock '%s'\n",
+					__func__, pdata->clock_dsi_lp_id);
+	}
+
+	clock_mcde = clk_get(&pdev->dev, CLK_MCDE);
+	if (IS_ERR(clock_mcde)) {
+		ret = PTR_ERR(clock_mcde);
+		dev_warn(&pdev->dev, "%s: Failed to get mcde_clk\n", __func__);
+		goto clk_mcde_err;
+	}
+
+	return ret;
+
+clk_mcde_err:
+	if (!dsi_use_clk_framework) {
+		clk_put(clock_dsi_lp);
+		clk_put(clock_dsi);
+	}
+
+	if (regulator_vana)
+		regulator_put(regulator_vana);
+regulator_vana_err:
+	if (regulator_esram_epod)
+		regulator_put(regulator_esram_epod);
+regulator_esram_err:
+	regulator_put(regulator_mcde_epod);
+	return ret;
+}
+
+static void remove_clocks_and_power(struct platform_device *pdev)
+{
+	/* REVIEW: Release only if exist */
+	/* REVIEW: Remove make sure MCDE is done */
+	if (!dsi_use_clk_framework) {
+		clk_put(clock_dsi_lp);
+		clk_put(clock_dsi);
+	}
+	clk_put(clock_mcde);
+	if (regulator_vana)
+		regulator_put(regulator_vana);
+	regulator_put(regulator_mcde_epod);
+	regulator_put(regulator_esram_epod);
+}
+
+static int probe_hw(struct platform_device *pdev)
+{
+	int i;
+	int ret;
+	u32 pid;
+	struct resource *res;
+
+	dev_info(&mcde_dev->dev, "Probe HW\n");
+
+	/* Get MCDE HW version */
+	regulator_enable(regulator_mcde_epod);
+	clk_enable(clock_mcde);
+	pid = mcde_rreg(MCDE_PID);
+
+	dev_info(&mcde_dev->dev, "MCDE HW revision 0x%.8X\n", pid);
+
+	clk_disable(clock_mcde);
+	regulator_disable(regulator_mcde_epod);
+
+	switch (pid) {
+	case MCDE_VERSION_3_0_8:
+		num_dsilinks = 3;
+		num_channels = 4;
+		num_overlays = 6;
+		dsi_ifc_is_supported = true;
+		input_fifo_size = 128;
+		output_fifo_ab_size = 640;
+		output_fifo_c0c1_size = 160;
+		dsi_use_clk_framework = false;
+		dev_info(&mcde_dev->dev, "db8500 V2 HW\n");
+		break;
+	case MCDE_VERSION_4_0_4:
+		num_dsilinks = 2;
+		num_channels = 2;
+		num_overlays = 3;
+		input_fifo_size = 80;
+		output_fifo_ab_size = 320;
+		dsi_ifc_is_supported = false;
+		dsi_use_clk_framework = false;
+		dev_info(&mcde_dev->dev, "db5500 V2 HW\n");
+		break;
+	case MCDE_VERSION_4_1_3:
+		num_dsilinks = 3;
+		num_channels = 4;
+		num_overlays = 6;
+		dsi_ifc_is_supported = true;
+		input_fifo_size = 192;
+		output_fifo_ab_size = 640;
+		output_fifo_c0c1_size = 160;
+		dsi_use_clk_framework = false;
+		dev_info(&mcde_dev->dev, "db9540 V1 HW\n");
+		break;
+	case MCDE_VERSION_3_0_5:
+		/* Intentional */
+	case MCDE_VERSION_1_0_4:
+		/* Intentional */
+	default:
+		dev_err(&mcde_dev->dev, "Unsupported HW version\n");
+		ret = -ENOTSUPP;
+		goto unsupported_hw;
+		break;
+	}
+
+	channels = kzalloc(num_channels * sizeof(struct mcde_chnl_state),
+								GFP_KERNEL);
+	if (!channels) {
+		ret = -ENOMEM;
+		goto failed_channels_alloc;
+	}
+
+	overlays = kzalloc(num_overlays * sizeof(struct mcde_ovly_state),
+								GFP_KERNEL);
+	if (!overlays) {
+		ret = -ENOMEM;
+		goto failed_overlays_alloc;
+	}
+
+	dsiio = kzalloc(num_dsilinks * sizeof(*dsiio), GFP_KERNEL);
+	if (!dsiio) {
+		ret = -ENOMEM;
+		goto failed_dsi_alloc;
+	}
+
+	for (i = 0; i < num_dsilinks; i++) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 1+i);
+		if (!res) {
+			dev_dbg(&pdev->dev, "No DSI%d io defined\n", i);
+			ret = -EINVAL;
+			goto failed_get_dsi_io;
+		}
+		dsiio[i] = ioremap(res->start, res->end - res->start + 1);
+		if (!dsiio[i]) {
+			dev_dbg(&pdev->dev, "MCDE DSI%d iomap failed\n", i);
+			ret = -EINVAL;
+			goto failed_map_dsi_io;
+		}
+		dev_info(&pdev->dev, "MCDE DSI%d iomap: 0x%.8X->0x%.8X\n",
+			i, (u32)res->start, (u32)dsiio[i]);
+	}
+
+	/* Init MCDE */
+	for (i = 0; i < num_overlays; i++)
+		overlays[i].idx = i;
+
+	channels[0].ovly0 = &overlays[0];
+	channels[0].ovly1 = &overlays[1];
+	channels[1].ovly0 = &overlays[2];
+
+	if (pid == MCDE_VERSION_3_0_8) {
+		channels[1].ovly1 = &overlays[3];
+		channels[2].ovly0 = &overlays[4];
+		channels[3].ovly0 = &overlays[5];
+	}
+
+	mcde_debugfs_create(&mcde_dev->dev);
+	for (i = 0; i < num_channels; i++) {
+		channels[i].id = i;
+
+		channels[i].ovly0->chnl = &channels[i];
+		if (channels[i].ovly1)
+			channels[i].ovly1->chnl = &channels[i];
+
+		init_waitqueue_head(&channels[i].state_waitq);
+		init_waitqueue_head(&channels[i].vcmp_waitq);
+		init_timer(&channels[i].dsi_te_timer);
+		channels[i].dsi_te_timer.function =
+					dsi_te_timer_function;
+		channels[i].dsi_te_timer.data = i;
+
+		mcde_debugfs_channel_create(i, &channels[i]);
+		mcde_debugfs_overlay_create(i, 0);
+		if (channels[i].ovly1)
+			mcde_debugfs_overlay_create(i, 1);
+	}
+        (void) prcmu_qos_add_requirement(PRCMU_QOS_APE_OPP, "mcde", 100);
+        mcde_clk_rate = clk_get_rate(clock_mcde);
+        dev_info(&mcde_dev->dev, "MCDE_CLK is %d MHz\n", mcde_clk_rate);
+        prcmu_qos_remove_requirement(PRCMU_QOS_APE_OPP, "mcde");
+
+	return 0;
+
+failed_map_dsi_io:
+	for (i = 0; i < num_dsilinks; i++) {
+		if (dsiio[i])
+			iounmap(dsiio[i]);
+	}
+failed_get_dsi_io:
+	kfree(dsiio);
+	dsiio = NULL;
+failed_dsi_alloc:
+	kfree(overlays);
+	overlays = NULL;
+failed_overlays_alloc:
+	kfree(channels);
+	channels = NULL;
+unsupported_hw:
+failed_channels_alloc:
+	num_dsilinks = 0;
+	num_channels = 0;
+	num_overlays = 0;
+	return ret;
+}
+
+static int __devinit mcde_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	struct resource *res;
+	struct mcde_platform_data *pdata = pdev->dev.platform_data;
+
+	if (!pdata) {
+		dev_dbg(&pdev->dev, "No platform data\n");
+		return -EINVAL;
+	}
+
+	mcde_dev = pdev;
+
+	/* Hook up irq */
+	mcde_irq = platform_get_irq(pdev, 0);
+	if (mcde_irq <= 0) {
+		dev_dbg(&pdev->dev, "No irq defined\n");
+		ret = -EINVAL;
+		goto failed_irq_get;
+	}
+
+	/* Map I/O */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_dbg(&pdev->dev, "No MCDE io defined\n");
+		ret = -EINVAL;
+		goto failed_get_mcde_io;
+	}
+	mcdeio = ioremap(res->start, res->end - res->start + 1);
+	if (!mcdeio) {
+		dev_dbg(&pdev->dev, "MCDE iomap failed\n");
+		ret = -EINVAL;
+		goto failed_map_mcde_io;
+	}
+	dev_info(&pdev->dev, "MCDE iomap: 0x%.8X->0x%.8X\n",
+		(u32)res->start, (u32)mcdeio);
+
+	ret = init_clocks_and_power(pdev);
+	if (ret < 0) {
+		dev_warn(&pdev->dev, "%s: init_clocks_and_power failed\n"
+					, __func__);
+		goto failed_init_clocks;
+	}
+
+	INIT_DELAYED_WORK_DEFERRABLE(&hw_timeout_work, work_sleep_function);
+
+	ret = probe_hw(pdev);
+	if (ret)
+		goto failed_probe_hw;
+
+	ret = enable_mcde_hw();
+	if (ret)
+		goto failed_mcde_enable;
+
+	return 0;
+
+failed_mcde_enable:
+failed_probe_hw:
+	remove_clocks_and_power(pdev);
+failed_init_clocks:
+	iounmap(mcdeio);
+failed_map_mcde_io:
+failed_get_mcde_io:
+failed_irq_get:
+	return ret;
+}
+
+static int __devexit mcde_remove(struct platform_device *pdev)
+{
+	struct mcde_chnl_state *chnl = &channels[0];
+
+	for (; chnl < &channels[num_channels]; chnl++) {
+		if (del_timer(&chnl->dsi_te_timer))
+			dev_vdbg(&mcde_dev->dev,
+				"%s dsi timer could not be stopped\n"
+				, __func__);
+	}
+
+	remove_clocks_and_power(pdev);
+	return 0;
+}
+
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+static int mcde_resume(struct platform_device *pdev)
+{
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mcde_lock(__func__, __LINE__);
+
+	if (enable_mcde_hw()) {
+		mcde_unlock(__func__, __LINE__);
+		return -EINVAL;
+	}
+
+	mcde_unlock(__func__, __LINE__);
+
+	return 0;
+}
+
+static int mcde_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	int ret;
+
+	dev_vdbg(&mcde_dev->dev, "%s\n", __func__);
+
+	mcde_lock(__func__, __LINE__);
+
+	cancel_delayed_work(&hw_timeout_work);
+
+	if (!mcde_is_enabled) {
+		mcde_unlock(__func__, __LINE__);
+		return 0;
+	}
+	disable_mcde_hw(true, true);
+
+	mcde_unlock(__func__, __LINE__);
+
+	return ret;
+}
+#endif
+
+static struct platform_driver mcde_driver = {
+	.probe = mcde_probe,
+	.remove = mcde_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_PM)
+	.suspend = mcde_suspend,
+	.resume = mcde_resume,
+#else
+	.suspend = NULL,
+	.resume = NULL,
+#endif
+	.driver = {
+		.name	= "mcde",
+	},
+};
+
+int __init mcde_init(void)
+{
+	mutex_init(&mcde_hw_lock);
+	return platform_driver_register(&mcde_driver);
+}
+
+void mcde_exit(void)
+{
+	/* REVIEW: shutdown MCDE? */
+	platform_driver_unregister(&mcde_driver);
+}
diff --git a/drivers/video/mcde/mcde_mod.c b/drivers/video/mcde/mcde_mod.c
new file mode 100644
index 00000000000..60df0d4965f
--- /dev/null
+++ b/drivers/video/mcde/mcde_mod.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) ST-Ericsson AB 2010
+ *
+ * ST-Ericsson MCDE driver
+ *
+ * Author: Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>
+ * for ST-Ericsson.
+ *
+ * License terms: GNU General Public License (GPL), version 2.
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+
+#include <video/mcde.h>
+#include <video/mcde_fb.h>
+#include <video/mcde_dss.h>
+#include <video/mcde_display.h>
+
+/* Module init */
+
+static int __init mcde_subsystem_init(void)
+{
+	int ret;
+	pr_info("MCDE subsystem init begin\n");
+
+	/* MCDE module init sequence */
+	ret = mcde_init();
+	if (ret)
+		goto mcde_failed;
+	ret = mcde_display_init();
+	if (ret)
+		goto mcde_display_failed;
+	ret = mcde_dss_init();
+	if (ret)
+		goto mcde_dss_failed;
+	ret = mcde_fb_init();
+	if (ret)
+		goto mcde_fb_failed;
+	pr_info("MCDE subsystem init done\n");
+
+	goto done;
+mcde_fb_failed:
+	mcde_dss_exit();
+mcde_dss_failed:
+	mcde_display_exit();
+mcde_display_failed:
+	mcde_exit();
+mcde_failed:
+done:
+	return ret;
+}
+#ifdef MODULE
+module_init(mcde_subsystem_init);
+#else
+fs_initcall(mcde_subsystem_init);
+#endif
+
+static void __exit mcde_module_exit(void)
+{
+	mcde_exit();
+	mcde_display_exit();
+	mcde_dss_exit();
+}
+module_exit(mcde_module_exit);
+
+MODULE_AUTHOR("Marcus Lorentzon <marcus.xm.lorentzon@stericsson.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ST-Ericsson MCDE driver");
+
diff --git a/drivers/video/mcde/mcde_regs.h b/drivers/video/mcde/mcde_regs.h
new file mode 100644
index 00000000000..0eece3faea2
--- /dev/null
+++ b/drivers/video/mcde/mcde_regs.h
@@ -0,0 +1,5096 @@
+
+#define MCDE_VAL2REG(__reg, __fld, __val) \
+	(((__val) << __reg##_##__fld##_SHIFT) & __reg##_##__fld##_MASK)
+#define MCDE_REG2VAL(__reg, __fld, __val) \
+	(((__val) & __reg##_##__fld##_MASK) >> __reg##_##__fld##_SHIFT)
+
+#define MCDE_CR 0x00000000
+#define MCDE_CR_IFIFOCTRLEN_SHIFT 15
+#define MCDE_CR_IFIFOCTRLEN_MASK 0x00008000
+#define MCDE_CR_IFIFOCTRLEN(__x) \
+	MCDE_VAL2REG(MCDE_CR, IFIFOCTRLEN, __x)
+#define MCDE_CR_AUTOCLKG_EN_SHIFT 30
+#define MCDE_CR_AUTOCLKG_EN_MASK 0x40000000
+#define MCDE_CR_AUTOCLKG_EN(__x) \
+	MCDE_VAL2REG(MCDE_CR, AUTOCLKG_EN, __x)
+#define MCDE_CR_MCDEEN_SHIFT 31
+#define MCDE_CR_MCDEEN_MASK 0x80000000
+#define MCDE_CR_MCDEEN(__x) \
+	MCDE_VAL2REG(MCDE_CR, MCDEEN, __x)
+#define MCDE_CONF0 0x00000004
+#define MCDE_CONF0_SYNCMUX0_SHIFT 0
+#define MCDE_CONF0_SYNCMUX0_MASK 0x00000001
+#define MCDE_CONF0_SYNCMUX0(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX0, __x)
+#define MCDE_CONF0_SYNCMUX1_SHIFT 1
+#define MCDE_CONF0_SYNCMUX1_MASK 0x00000002
+#define MCDE_CONF0_SYNCMUX1(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX1, __x)
+#define MCDE_CONF0_SYNCMUX2_SHIFT 2
+#define MCDE_CONF0_SYNCMUX2_MASK 0x00000004
+#define MCDE_CONF0_SYNCMUX2(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX2, __x)
+#define MCDE_CONF0_SYNCMUX3_SHIFT 3
+#define MCDE_CONF0_SYNCMUX3_MASK 0x00000008
+#define MCDE_CONF0_SYNCMUX3(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX3, __x)
+#define MCDE_CONF0_SYNCMUX4_SHIFT 4
+#define MCDE_CONF0_SYNCMUX4_MASK 0x00000010
+#define MCDE_CONF0_SYNCMUX4(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX4, __x)
+#define MCDE_CONF0_SYNCMUX5_SHIFT 5
+#define MCDE_CONF0_SYNCMUX5_MASK 0x00000020
+#define MCDE_CONF0_SYNCMUX5(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX5, __x)
+#define MCDE_CONF0_SYNCMUX6_SHIFT 6
+#define MCDE_CONF0_SYNCMUX6_MASK 0x00000040
+#define MCDE_CONF0_SYNCMUX6(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX6, __x)
+#define MCDE_CONF0_SYNCMUX7_SHIFT 7
+#define MCDE_CONF0_SYNCMUX7_MASK 0x00000080
+#define MCDE_CONF0_SYNCMUX7(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, SYNCMUX7, __x)
+#define MCDE_CONF0_IFIFOCTRLWTRMRKLVL_SHIFT 12
+#define MCDE_CONF0_IFIFOCTRLWTRMRKLVL_MASK 0x00007000
+#define MCDE_CONF0_IFIFOCTRLWTRMRKLVL(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, IFIFOCTRLWTRMRKLVL, __x)
+#define MCDE_CONF0_OUTMUX0_SHIFT 16
+#define MCDE_CONF0_OUTMUX0_MASK 0x00070000
+#define MCDE_CONF0_OUTMUX0(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX0, __x)
+#define MCDE_CONF0_OUTMUX1_SHIFT 19
+#define MCDE_CONF0_OUTMUX1_MASK 0x00380000
+#define MCDE_CONF0_OUTMUX1(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX1, __x)
+#define MCDE_CONF0_OUTMUX2_SHIFT 22
+#define MCDE_CONF0_OUTMUX2_MASK 0x01C00000
+#define MCDE_CONF0_OUTMUX2(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX2, __x)
+#define MCDE_CONF0_OUTMUX3_SHIFT 25
+#define MCDE_CONF0_OUTMUX3_MASK 0x0E000000
+#define MCDE_CONF0_OUTMUX3(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX3, __x)
+#define MCDE_CONF0_OUTMUX4_SHIFT 28
+#define MCDE_CONF0_OUTMUX4_MASK 0x70000000
+#define MCDE_CONF0_OUTMUX4(__x) \
+	MCDE_VAL2REG(MCDE_CONF0, OUTMUX4, __x)
+#define MCDE_SSP 0x00000008
+#define MCDE_SSP_SSPDATA_SHIFT 0
+#define MCDE_SSP_SSPDATA_MASK 0x000000FF
+#define MCDE_SSP_SSPDATA(__x) \
+	MCDE_VAL2REG(MCDE_SSP, SSPDATA, __x)
+#define MCDE_SSP_SSPCMD_SHIFT 8
+#define MCDE_SSP_SSPCMD_MASK 0x00000100
+#define MCDE_SSP_SSPCMD_DATA 0
+#define MCDE_SSP_SSPCMD_COMMAND 1
+#define MCDE_SSP_SSPCMD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SSP, SSPCMD, MCDE_SSP_SSPCMD_##__x)
+#define MCDE_SSP_SSPCMD(__x) \
+	MCDE_VAL2REG(MCDE_SSP, SSPCMD, __x)
+#define MCDE_SSP_SSPEN_SHIFT 16
+#define MCDE_SSP_SSPEN_MASK 0x00010000
+#define MCDE_SSP_SSPEN(__x) \
+	MCDE_VAL2REG(MCDE_SSP, SSPEN, __x)
+#define MCDE_AIS 0x00000100
+#define MCDE_AIS_MCDEPPI_SHIFT 0
+#define MCDE_AIS_MCDEPPI_MASK 0x00000001
+#define MCDE_AIS_MCDEPPI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, MCDEPPI, __x)
+#define MCDE_AIS_MCDEOVLI_SHIFT 1
+#define MCDE_AIS_MCDEOVLI_MASK 0x00000002
+#define MCDE_AIS_MCDEOVLI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, MCDEOVLI, __x)
+#define MCDE_AIS_MCDECHNLI_SHIFT 2
+#define MCDE_AIS_MCDECHNLI_MASK 0x00000004
+#define MCDE_AIS_MCDECHNLI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, MCDECHNLI, __x)
+#define MCDE_AIS_MCDEERRI_SHIFT 3
+#define MCDE_AIS_MCDEERRI_MASK 0x00000008
+#define MCDE_AIS_MCDEERRI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, MCDEERRI, __x)
+#define MCDE_AIS_DSI0AI_SHIFT 4
+#define MCDE_AIS_DSI0AI_MASK 0x00000010
+#define MCDE_AIS_DSI0AI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, DSI0AI, __x)
+#define MCDE_AIS_DSI1AI_SHIFT 5
+#define MCDE_AIS_DSI1AI_MASK 0x00000020
+#define MCDE_AIS_DSI1AI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, DSI1AI, __x)
+#define MCDE_AIS_DSI2AI_SHIFT 6
+#define MCDE_AIS_DSI2AI_MASK 0x00000040
+#define MCDE_AIS_DSI2AI(__x) \
+	MCDE_VAL2REG(MCDE_AIS, DSI2AI, __x)
+#define MCDE_IMSCPP 0x00000104
+#define MCDE_IMSCPP_VCMPAIM_SHIFT 0
+#define MCDE_IMSCPP_VCMPAIM_MASK 0x00000001
+#define MCDE_IMSCPP_VCMPAIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VCMPAIM, __x)
+#define MCDE_IMSCPP_VCMPBIM_SHIFT 1
+#define MCDE_IMSCPP_VCMPBIM_MASK 0x00000002
+#define MCDE_IMSCPP_VCMPBIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VCMPBIM, __x)
+#define MCDE_IMSCPP_VSCC0IM_SHIFT 2
+#define MCDE_IMSCPP_VSCC0IM_MASK 0x00000004
+#define MCDE_IMSCPP_VSCC0IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VSCC0IM, __x)
+#define MCDE_IMSCPP_VSCC1IM_SHIFT 3
+#define MCDE_IMSCPP_VSCC1IM_MASK 0x00000008
+#define MCDE_IMSCPP_VSCC1IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VSCC1IM, __x)
+#define MCDE_IMSCPP_VCMPC0IM_SHIFT 4
+#define MCDE_IMSCPP_VCMPC0IM_MASK 0x00000010
+#define MCDE_IMSCPP_VCMPC0IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VCMPC0IM, __x)
+#define MCDE_IMSCPP_VCMPC1IM_SHIFT 5
+#define MCDE_IMSCPP_VCMPC1IM_MASK 0x00000020
+#define MCDE_IMSCPP_VCMPC1IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, VCMPC1IM, __x)
+#define MCDE_IMSCPP_ROTFDIM_B_SHIFT 6
+#define MCDE_IMSCPP_ROTFDIM_B_MASK 0x00000040
+#define MCDE_IMSCPP_ROTFDIM_B(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, ROTFDIM_B, __x)
+#define MCDE_IMSCPP_ROTFDIM_A_SHIFT 7
+#define MCDE_IMSCPP_ROTFDIM_A_MASK 0x00000080
+#define MCDE_IMSCPP_ROTFDIM_A(__x) \
+	MCDE_VAL2REG(MCDE_IMSCPP, ROTFDIM_A, __x)
+#define MCDE_IMSCOVL 0x00000108
+#define MCDE_IMSCOVL_OVLRDIM_SHIFT 0
+#define MCDE_IMSCOVL_OVLRDIM_MASK 0x0000FFFF
+#define MCDE_IMSCOVL_OVLRDIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCOVL, OVLRDIM, __x)
+#define MCDE_IMSCOVL_OVLFDIM_SHIFT 16
+#define MCDE_IMSCOVL_OVLFDIM_MASK 0xFFFF0000
+#define MCDE_IMSCOVL_OVLFDIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCOVL, OVLFDIM, __x)
+#define MCDE_IMSCCHNL 0x0000010C
+#define MCDE_IMSCCHNL_CHNLRDIM_SHIFT 0
+#define MCDE_IMSCCHNL_CHNLRDIM_MASK 0x0000FFFF
+#define MCDE_IMSCCHNL_CHNLRDIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCCHNL, CHNLRDIM, __x)
+#define MCDE_IMSCCHNL_CHNLAIM_SHIFT 16
+#define MCDE_IMSCCHNL_CHNLAIM_MASK 0xFFFF0000
+#define MCDE_IMSCCHNL_CHNLAIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCCHNL, CHNLAIM, __x)
+#define MCDE_IMSCERR 0x00000110
+#define MCDE_IMSCERR_FUAIM_SHIFT 0
+#define MCDE_IMSCERR_FUAIM_MASK 0x00000001
+#define MCDE_IMSCERR_FUAIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, FUAIM, __x)
+#define MCDE_IMSCERR_FUBIM_SHIFT 1
+#define MCDE_IMSCERR_FUBIM_MASK 0x00000002
+#define MCDE_IMSCERR_FUBIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, FUBIM, __x)
+#define MCDE_IMSCERR_SCHBLCKDIM_SHIFT 2
+#define MCDE_IMSCERR_SCHBLCKDIM_MASK 0x00000004
+#define MCDE_IMSCERR_SCHBLCKDIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, SCHBLCKDIM, __x)
+#define MCDE_IMSCERR_ROTAFEIM_WRITE_SHIFT 3
+#define MCDE_IMSCERR_ROTAFEIM_WRITE_MASK 0x00000008
+#define MCDE_IMSCERR_ROTAFEIM_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, ROTAFEIM_WRITE, __x)
+#define MCDE_IMSCERR_ROTAFEIM_READ_SHIFT 4
+#define MCDE_IMSCERR_ROTAFEIM_READ_MASK 0x00000010
+#define MCDE_IMSCERR_ROTAFEIM_READ(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, ROTAFEIM_READ, __x)
+#define MCDE_IMSCERR_ROTBFEIM_WRITE_SHIFT 5
+#define MCDE_IMSCERR_ROTBFEIM_WRITE_MASK 0x00000020
+#define MCDE_IMSCERR_ROTBFEIM_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, ROTBFEIM_WRITE, __x)
+#define MCDE_IMSCERR_ROTBFEIM_READ_SHIFT 6
+#define MCDE_IMSCERR_ROTBFEIM_READ_MASK 0x00000040
+#define MCDE_IMSCERR_ROTBFEIM_READ(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, ROTBFEIM_READ, __x)
+#define MCDE_IMSCERR_FUC0IM_SHIFT 7
+#define MCDE_IMSCERR_FUC0IM_MASK 0x00000080
+#define MCDE_IMSCERR_FUC0IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, FUC0IM, __x)
+#define MCDE_IMSCERR_FUC1IM_SHIFT 8
+#define MCDE_IMSCERR_FUC1IM_MASK 0x00000100
+#define MCDE_IMSCERR_FUC1IM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, FUC1IM, __x)
+#define MCDE_IMSCERR_OVLFERRIM_SHIFT 16
+#define MCDE_IMSCERR_OVLFERRIM_MASK 0xFFFF0000
+#define MCDE_IMSCERR_OVLFERRIM(__x) \
+	MCDE_VAL2REG(MCDE_IMSCERR, OVLFERRIM, __x)
+#define MCDE_RISPP 0x00000114
+#define MCDE_RISPP_VCMPARIS_SHIFT 0
+#define MCDE_RISPP_VCMPARIS_MASK 0x00000001
+#define MCDE_RISPP_VCMPARIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VCMPARIS, __x)
+#define MCDE_RISPP_VCMPBRIS_SHIFT 1
+#define MCDE_RISPP_VCMPBRIS_MASK 0x00000002
+#define MCDE_RISPP_VCMPBRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VCMPBRIS, __x)
+#define MCDE_RISPP_VSCC0RIS_SHIFT 2
+#define MCDE_RISPP_VSCC0RIS_MASK 0x00000004
+#define MCDE_RISPP_VSCC0RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VSCC0RIS, __x)
+#define MCDE_RISPP_VSCC1RIS_SHIFT 3
+#define MCDE_RISPP_VSCC1RIS_MASK 0x00000008
+#define MCDE_RISPP_VSCC1RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VSCC1RIS, __x)
+#define MCDE_RISPP_VCMPC0RIS_SHIFT 4
+#define MCDE_RISPP_VCMPC0RIS_MASK 0x00000010
+#define MCDE_RISPP_VCMPC0RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VCMPC0RIS, __x)
+#define MCDE_RISPP_VCMPC1RIS_SHIFT 5
+#define MCDE_RISPP_VCMPC1RIS_MASK 0x00000020
+#define MCDE_RISPP_VCMPC1RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, VCMPC1RIS, __x)
+#define MCDE_RISPP_ROTFDRIS_B_SHIFT 6
+#define MCDE_RISPP_ROTFDRIS_B_MASK 0x00000040
+#define MCDE_RISPP_ROTFDRIS_B(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, ROTFDRIS_B, __x)
+#define MCDE_RISPP_ROTFDRIS_A_SHIFT 7
+#define MCDE_RISPP_ROTFDRIS_A_MASK 0x00000080
+#define MCDE_RISPP_ROTFDRIS_A(__x) \
+	MCDE_VAL2REG(MCDE_RISPP, ROTFDRIS_A, __x)
+#define MCDE_RISOVL 0x00000118
+#define MCDE_RISOVL_OVLRDRIS_SHIFT 0
+#define MCDE_RISOVL_OVLRDRIS_MASK 0x0000FFFF
+#define MCDE_RISOVL_OVLRDRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISOVL, OVLRDRIS, __x)
+#define MCDE_RISOVL_OVLFDRIS_SHIFT 16
+#define MCDE_RISOVL_OVLFDRIS_MASK 0xFFFF0000
+#define MCDE_RISOVL_OVLFDRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISOVL, OVLFDRIS, __x)
+#define MCDE_RISCHNL 0x0000011C
+#define MCDE_RISCHNL_CHNLRDRIS_SHIFT 0
+#define MCDE_RISCHNL_CHNLRDRIS_MASK 0x0000FFFF
+#define MCDE_RISCHNL_CHNLRDRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISCHNL, CHNLRDRIS, __x)
+#define MCDE_RISCHNL_CHNLARIS_SHIFT 16
+#define MCDE_RISCHNL_CHNLARIS_MASK 0xFFFF0000
+#define MCDE_RISCHNL_CHNLARIS(__x) \
+	MCDE_VAL2REG(MCDE_RISCHNL, CHNLARIS, __x)
+#define MCDE_RISERR 0x00000120
+#define MCDE_RISERR_FUARIS_SHIFT 0
+#define MCDE_RISERR_FUARIS_MASK 0x00000001
+#define MCDE_RISERR_FUARIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, FUARIS, __x)
+#define MCDE_RISERR_FUBRIS_SHIFT 1
+#define MCDE_RISERR_FUBRIS_MASK 0x00000002
+#define MCDE_RISERR_FUBRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, FUBRIS, __x)
+#define MCDE_RISERR_SCHBLCKDRIS_SHIFT 2
+#define MCDE_RISERR_SCHBLCKDRIS_MASK 0x00000004
+#define MCDE_RISERR_SCHBLCKDRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, SCHBLCKDRIS, __x)
+#define MCDE_RISERR_ROTAFERIS_WRITE_SHIFT 3
+#define MCDE_RISERR_ROTAFERIS_WRITE_MASK 0x00000008
+#define MCDE_RISERR_ROTAFERIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, ROTAFERIS_WRITE, __x)
+#define MCDE_RISERR_ROTAFERIS_READ_SHIFT 4
+#define MCDE_RISERR_ROTAFERIS_READ_MASK 0x00000010
+#define MCDE_RISERR_ROTAFERIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, ROTAFERIS_READ, __x)
+#define MCDE_RISERR_ROTBFERIS_WRITE_SHIFT 5
+#define MCDE_RISERR_ROTBFERIS_WRITE_MASK 0x00000020
+#define MCDE_RISERR_ROTBFERIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, ROTBFERIS_WRITE, __x)
+#define MCDE_RISERR_ROTBFERIS_READ_SHIFT 6
+#define MCDE_RISERR_ROTBFERIS_READ_MASK 0x00000040
+#define MCDE_RISERR_ROTBFERIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, ROTBFERIS_READ, __x)
+#define MCDE_RISERR_FUC0RIS_SHIFT 7
+#define MCDE_RISERR_FUC0RIS_MASK 0x00000080
+#define MCDE_RISERR_FUC0RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, FUC0RIS, __x)
+#define MCDE_RISERR_FUC1RIS_SHIFT 8
+#define MCDE_RISERR_FUC1RIS_MASK 0x00000100
+#define MCDE_RISERR_FUC1RIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, FUC1RIS, __x)
+#define MCDE_RISERR_OVLFERRRIS_SHIFT 16
+#define MCDE_RISERR_OVLFERRRIS_MASK 0xFFFF0000
+#define MCDE_RISERR_OVLFERRRIS(__x) \
+	MCDE_VAL2REG(MCDE_RISERR, OVLFERRRIS, __x)
+#define MCDE_MISPP 0x00000124
+#define MCDE_MISPP_VCMPAMIS_SHIFT 0
+#define MCDE_MISPP_VCMPAMIS_MASK 0x00000001
+#define MCDE_MISPP_VCMPAMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VCMPAMIS, __x)
+#define MCDE_MISPP_VCMPBMIS_SHIFT 1
+#define MCDE_MISPP_VCMPBMIS_MASK 0x00000002
+#define MCDE_MISPP_VCMPBMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VCMPBMIS, __x)
+#define MCDE_MISPP_VSCC0MIS_SHIFT 2
+#define MCDE_MISPP_VSCC0MIS_MASK 0x00000004
+#define MCDE_MISPP_VSCC0MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VSCC0MIS, __x)
+#define MCDE_MISPP_VSCC1MIS_SHIFT 3
+#define MCDE_MISPP_VSCC1MIS_MASK 0x00000008
+#define MCDE_MISPP_VSCC1MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VSCC1MIS, __x)
+#define MCDE_MISPP_VCMPC0MIS_SHIFT 4
+#define MCDE_MISPP_VCMPC0MIS_MASK 0x00000010
+#define MCDE_MISPP_VCMPC0MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VCMPC0MIS, __x)
+#define MCDE_MISPP_VCMPC1MIS_SHIFT 5
+#define MCDE_MISPP_VCMPC1MIS_MASK 0x00000020
+#define MCDE_MISPP_VCMPC1MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, VCMPC1MIS, __x)
+#define MCDE_MISPP_ROTFDMIS_A_SHIFT 6
+#define MCDE_MISPP_ROTFDMIS_A_MASK 0x00000040
+#define MCDE_MISPP_ROTFDMIS_A(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, ROTFDMIS_A, __x)
+#define MCDE_MISPP_ROTFDMIS_B_SHIFT 7
+#define MCDE_MISPP_ROTFDMIS_B_MASK 0x00000080
+#define MCDE_MISPP_ROTFDMIS_B(__x) \
+	MCDE_VAL2REG(MCDE_MISPP, ROTFDMIS_B, __x)
+#define MCDE_MISOVL 0x00000128
+#define MCDE_MISOVL_OVLRDMIS_SHIFT 0
+#define MCDE_MISOVL_OVLRDMIS_MASK 0x0000FFFF
+#define MCDE_MISOVL_OVLRDMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISOVL, OVLRDMIS, __x)
+#define MCDE_MISOVL_OVLFDMIS_SHIFT 16
+#define MCDE_MISOVL_OVLFDMIS_MASK 0xFFFF0000
+#define MCDE_MISOVL_OVLFDMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISOVL, OVLFDMIS, __x)
+#define MCDE_MISCHNL 0x0000012C
+#define MCDE_MISCHNL_CHNLRDMIS_SHIFT 0
+#define MCDE_MISCHNL_CHNLRDMIS_MASK 0x0000FFFF
+#define MCDE_MISCHNL_CHNLRDMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISCHNL, CHNLRDMIS, __x)
+#define MCDE_MISCHNL_CHNLAMIS_SHIFT 16
+#define MCDE_MISCHNL_CHNLAMIS_MASK 0xFFFF0000
+#define MCDE_MISCHNL_CHNLAMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISCHNL, CHNLAMIS, __x)
+#define MCDE_MISERR 0x00000130
+#define MCDE_MISERR_FUAMIS_SHIFT 0
+#define MCDE_MISERR_FUAMIS_MASK 0x00000001
+#define MCDE_MISERR_FUAMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, FUAMIS, __x)
+#define MCDE_MISERR_FUBMIS_SHIFT 1
+#define MCDE_MISERR_FUBMIS_MASK 0x00000002
+#define MCDE_MISERR_FUBMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, FUBMIS, __x)
+#define MCDE_MISERR_SCHBLCKDMIS_SHIFT 2
+#define MCDE_MISERR_SCHBLCKDMIS_MASK 0x00000004
+#define MCDE_MISERR_SCHBLCKDMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, SCHBLCKDMIS, __x)
+#define MCDE_MISERR_ROTAFEMIS_WRITE_SHIFT 3
+#define MCDE_MISERR_ROTAFEMIS_WRITE_MASK 0x00000008
+#define MCDE_MISERR_ROTAFEMIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, ROTAFEMIS_WRITE, __x)
+#define MCDE_MISERR_ROTAFEMIS_READ_SHIFT 4
+#define MCDE_MISERR_ROTAFEMIS_READ_MASK 0x00000010
+#define MCDE_MISERR_ROTAFEMIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, ROTAFEMIS_READ, __x)
+#define MCDE_MISERR_ROTBFEMIS_WRITE_SHIFT 5
+#define MCDE_MISERR_ROTBFEMIS_WRITE_MASK 0x00000020
+#define MCDE_MISERR_ROTBFEMIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, ROTBFEMIS_WRITE, __x)
+#define MCDE_MISERR_ROTBFEMIS_READ_SHIFT 6
+#define MCDE_MISERR_ROTBFEMIS_READ_MASK 0x00000040
+#define MCDE_MISERR_ROTBFEMIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, ROTBFEMIS_READ, __x)
+#define MCDE_MISERR_FUC0MIS_SHIFT 7
+#define MCDE_MISERR_FUC0MIS_MASK 0x00000080
+#define MCDE_MISERR_FUC0MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, FUC0MIS, __x)
+#define MCDE_MISERR_FUC1MIS_SHIFT 8
+#define MCDE_MISERR_FUC1MIS_MASK 0x00000100
+#define MCDE_MISERR_FUC1MIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, FUC1MIS, __x)
+#define MCDE_MISERR_OVLFERMIS_SHIFT 16
+#define MCDE_MISERR_OVLFERMIS_MASK 0xFFFF0000
+#define MCDE_MISERR_OVLFERMIS(__x) \
+	MCDE_VAL2REG(MCDE_MISERR, OVLFERMIS, __x)
+#define MCDE_SISPP 0x00000134
+#define MCDE_SISPP_VCMPASIS_SHIFT 0
+#define MCDE_SISPP_VCMPASIS_MASK 0x00000001
+#define MCDE_SISPP_VCMPASIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VCMPASIS, __x)
+#define MCDE_SISPP_VCMPBSIS_SHIFT 1
+#define MCDE_SISPP_VCMPBSIS_MASK 0x00000002
+#define MCDE_SISPP_VCMPBSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VCMPBSIS, __x)
+#define MCDE_SISPP_VSCC0SIS_SHIFT 2
+#define MCDE_SISPP_VSCC0SIS_MASK 0x00000004
+#define MCDE_SISPP_VSCC0SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VSCC0SIS, __x)
+#define MCDE_SISPP_VSCC1SIS_SHIFT 3
+#define MCDE_SISPP_VSCC1SIS_MASK 0x00000008
+#define MCDE_SISPP_VSCC1SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VSCC1SIS, __x)
+#define MCDE_SISPP_VCMPC0SIS_SHIFT 4
+#define MCDE_SISPP_VCMPC0SIS_MASK 0x00000010
+#define MCDE_SISPP_VCMPC0SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VCMPC0SIS, __x)
+#define MCDE_SISPP_VCMPC1SIS_SHIFT 5
+#define MCDE_SISPP_VCMPC1SIS_MASK 0x00000020
+#define MCDE_SISPP_VCMPC1SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, VCMPC1SIS, __x)
+#define MCDE_SISPP_ROTFDSIS_A_SHIFT 6
+#define MCDE_SISPP_ROTFDSIS_A_MASK 0x00000040
+#define MCDE_SISPP_ROTFDSIS_A(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, ROTFDSIS_A, __x)
+#define MCDE_SISPP_ROTFDSIS_B_SHIFT 7
+#define MCDE_SISPP_ROTFDSIS_B_MASK 0x00000080
+#define MCDE_SISPP_ROTFDSIS_B(__x) \
+	MCDE_VAL2REG(MCDE_SISPP, ROTFDSIS_B, __x)
+#define MCDE_SISOVL 0x00000138
+#define MCDE_SISOVL_OVLRDSIS_SHIFT 0
+#define MCDE_SISOVL_OVLRDSIS_MASK 0x0000FFFF
+#define MCDE_SISOVL_OVLRDSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISOVL, OVLRDSIS, __x)
+#define MCDE_SISOVL_OVLFDSIS_SHIFT 16
+#define MCDE_SISOVL_OVLFDSIS_MASK 0xFFFF0000
+#define MCDE_SISOVL_OVLFDSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISOVL, OVLFDSIS, __x)
+#define MCDE_SISCHNL 0x0000013C
+#define MCDE_SISCHNL_CHNLRDSIS_SHIFT 0
+#define MCDE_SISCHNL_CHNLRDSIS_MASK 0x0000FFFF
+#define MCDE_SISCHNL_CHNLRDSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISCHNL, CHNLRDSIS, __x)
+#define MCDE_SISCHNL_CHNLASIS_SHIFT 16
+#define MCDE_SISCHNL_CHNLASIS_MASK 0xFFFF0000
+#define MCDE_SISCHNL_CHNLASIS(__x) \
+	MCDE_VAL2REG(MCDE_SISCHNL, CHNLASIS, __x)
+#define MCDE_SISERR 0x00000140
+#define MCDE_SISERR_FUASIS_SHIFT 0
+#define MCDE_SISERR_FUASIS_MASK 0x00000001
+#define MCDE_SISERR_FUASIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, FUASIS, __x)
+#define MCDE_SISERR_FUBSIS_SHIFT 1
+#define MCDE_SISERR_FUBSIS_MASK 0x00000002
+#define MCDE_SISERR_FUBSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, FUBSIS, __x)
+#define MCDE_SISERR_SCHBLCKDSIS_SHIFT 2
+#define MCDE_SISERR_SCHBLCKDSIS_MASK 0x00000004
+#define MCDE_SISERR_SCHBLCKDSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, SCHBLCKDSIS, __x)
+#define MCDE_SISERR_ROTAFESIS_WRITE_SHIFT 3
+#define MCDE_SISERR_ROTAFESIS_WRITE_MASK 0x00000008
+#define MCDE_SISERR_ROTAFESIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, ROTAFESIS_WRITE, __x)
+#define MCDE_SISERR_ROTAFESIS_READ_SHIFT 4
+#define MCDE_SISERR_ROTAFESIS_READ_MASK 0x00000010
+#define MCDE_SISERR_ROTAFESIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, ROTAFESIS_READ, __x)
+#define MCDE_SISERR_ROTBFESIS_WRITE_SHIFT 5
+#define MCDE_SISERR_ROTBFESIS_WRITE_MASK 0x00000020
+#define MCDE_SISERR_ROTBFESIS_WRITE(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, ROTBFESIS_WRITE, __x)
+#define MCDE_SISERR_ROTBFESIS_READ_SHIFT 6
+#define MCDE_SISERR_ROTBFESIS_READ_MASK 0x00000040
+#define MCDE_SISERR_ROTBFESIS_READ(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, ROTBFESIS_READ, __x)
+#define MCDE_SISERR_FUC0SIS_SHIFT 7
+#define MCDE_SISERR_FUC0SIS_MASK 0x00000080
+#define MCDE_SISERR_FUC0SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, FUC0SIS, __x)
+#define MCDE_SISERR_FUC1SIS_SHIFT 8
+#define MCDE_SISERR_FUC1SIS_MASK 0x00000100
+#define MCDE_SISERR_FUC1SIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, FUC1SIS, __x)
+#define MCDE_SISERR_OVLFERSIS_SHIFT 16
+#define MCDE_SISERR_OVLFERSIS_MASK 0xFFFF0000
+#define MCDE_SISERR_OVLFERSIS(__x) \
+	MCDE_VAL2REG(MCDE_SISERR, OVLFERSIS, __x)
+#define MCDE_PID 0x000001FC
+#define MCDE_PID_METALFIX_VERSION_SHIFT 0
+#define MCDE_PID_METALFIX_VERSION_MASK 0x000000FF
+#define MCDE_PID_METALFIX_VERSION(__x) \
+	MCDE_VAL2REG(MCDE_PID, METALFIX_VERSION, __x)
+#define MCDE_PID_DEVELOPMENT_VERSION_SHIFT 8
+#define MCDE_PID_DEVELOPMENT_VERSION_MASK 0x0000FF00
+#define MCDE_PID_DEVELOPMENT_VERSION(__x) \
+	MCDE_VAL2REG(MCDE_PID, DEVELOPMENT_VERSION, __x)
+#define MCDE_PID_MINOR_VERSION_SHIFT 16
+#define MCDE_PID_MINOR_VERSION_MASK 0x00FF0000
+#define MCDE_PID_MINOR_VERSION(__x) \
+	MCDE_VAL2REG(MCDE_PID, MINOR_VERSION, __x)
+#define MCDE_PID_MAJOR_VERSION_SHIFT 24
+#define MCDE_PID_MAJOR_VERSION_MASK 0xFF000000
+#define MCDE_PID_MAJOR_VERSION(__x) \
+	MCDE_VAL2REG(MCDE_PID, MAJOR_VERSION, __x)
+#define MCDE_EXTSRC0A0 0x00000200
+#define MCDE_EXTSRC0A0_GROUPOFFSET 0x20
+#define MCDE_EXTSRC0A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC0A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC0A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC1A0 0x00000220
+#define MCDE_EXTSRC1A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC1A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC1A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC2A0 0x00000240
+#define MCDE_EXTSRC2A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC2A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC2A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC3A0 0x00000260
+#define MCDE_EXTSRC3A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC3A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC3A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC4A0 0x00000280
+#define MCDE_EXTSRC4A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC4A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC4A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC5A0 0x000002A0
+#define MCDE_EXTSRC5A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC5A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC5A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC6A0 0x000002C0
+#define MCDE_EXTSRC6A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC6A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC6A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC7A0 0x000002E0
+#define MCDE_EXTSRC7A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC7A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC7A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC8A0 0x00000300
+#define MCDE_EXTSRC8A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC8A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC8A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC9A0 0x00000320
+#define MCDE_EXTSRC9A0_BASEADDRESS0_SHIFT 3
+#define MCDE_EXTSRC9A0_BASEADDRESS0_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC9A0_BASEADDRESS0(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9A0, BASEADDRESS0, __x)
+#define MCDE_EXTSRC0A1 0x00000204
+#define MCDE_EXTSRC0A1_GROUPOFFSET 0x20
+#define MCDE_EXTSRC0A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC0A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC0A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC1A1 0x00000224
+#define MCDE_EXTSRC1A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC1A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC1A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC2A1 0x00000244
+#define MCDE_EXTSRC2A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC2A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC2A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC3A1 0x00000264
+#define MCDE_EXTSRC3A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC3A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC3A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC4A1 0x00000284
+#define MCDE_EXTSRC4A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC4A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC4A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC5A1 0x000002A4
+#define MCDE_EXTSRC5A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC5A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC5A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC6A1 0x000002C4
+#define MCDE_EXTSRC6A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC6A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC6A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC7A1 0x000002E4
+#define MCDE_EXTSRC7A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC7A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC7A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC8A1 0x00000304
+#define MCDE_EXTSRC8A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC8A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC8A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC9A1 0x00000324
+#define MCDE_EXTSRC9A1_BASEADDRESS1_SHIFT 3
+#define MCDE_EXTSRC9A1_BASEADDRESS1_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC9A1_BASEADDRESS1(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9A1, BASEADDRESS1, __x)
+#define MCDE_EXTSRC6A2 0x000002C8
+#define MCDE_EXTSRC6A2_BASEADDRESS2_SHIFT 3
+#define MCDE_EXTSRC6A2_BASEADDRESS2_MASK 0xFFFFFFF8
+#define MCDE_EXTSRC6A2_BASEADDRESS2(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6A2, BASEADDRESS2, __x)
+#define MCDE_EXTSRC0CONF 0x0000020C
+#define MCDE_EXTSRC0CONF_GROUPOFFSET 0x20
+#define MCDE_EXTSRC0CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC0CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC0CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BUF_ID, __x)
+#define MCDE_EXTSRC0CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC0CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC0CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BUF_NB, __x)
+#define MCDE_EXTSRC0CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC0CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC0CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC0CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC0CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC0CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC0CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC0CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC0CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC0CONF_BPP_RGB444 4
+#define MCDE_EXTSRC0CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC0CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC0CONF_BPP_RGB565 7
+#define MCDE_EXTSRC0CONF_BPP_RGB888 8
+#define MCDE_EXTSRC0CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC0CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC0CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC0CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BPP, MCDE_EXTSRC0CONF_BPP_##__x)
+#define MCDE_EXTSRC0CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BPP, __x)
+#define MCDE_EXTSRC0CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC0CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC0CONF_BGR_RGB 0
+#define MCDE_EXTSRC0CONF_BGR_BGR 1
+#define MCDE_EXTSRC0CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BGR, MCDE_EXTSRC0CONF_BGR_##__x)
+#define MCDE_EXTSRC0CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BGR, __x)
+#define MCDE_EXTSRC0CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC0CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC0CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC0CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC0CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BEBO, MCDE_EXTSRC0CONF_BEBO_##__x)
+#define MCDE_EXTSRC0CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BEBO, __x)
+#define MCDE_EXTSRC0CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC0CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC0CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC0CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC0CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BEPO, MCDE_EXTSRC0CONF_BEPO_##__x)
+#define MCDE_EXTSRC0CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CONF, BEPO, __x)
+#define MCDE_EXTSRC1CONF 0x0000022C
+#define MCDE_EXTSRC1CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC1CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC1CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BUF_ID, __x)
+#define MCDE_EXTSRC1CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC1CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC1CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BUF_NB, __x)
+#define MCDE_EXTSRC1CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC1CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC1CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC1CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC1CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC1CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC1CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC1CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC1CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC1CONF_BPP_RGB444 4
+#define MCDE_EXTSRC1CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC1CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC1CONF_BPP_RGB565 7
+#define MCDE_EXTSRC1CONF_BPP_RGB888 8
+#define MCDE_EXTSRC1CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC1CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC1CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC1CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BPP, MCDE_EXTSRC1CONF_BPP_##__x)
+#define MCDE_EXTSRC1CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BPP, __x)
+#define MCDE_EXTSRC1CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC1CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC1CONF_BGR_RGB 0
+#define MCDE_EXTSRC1CONF_BGR_BGR 1
+#define MCDE_EXTSRC1CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BGR, MCDE_EXTSRC1CONF_BGR_##__x)
+#define MCDE_EXTSRC1CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BGR, __x)
+#define MCDE_EXTSRC1CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC1CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC1CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC1CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC1CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BEBO, MCDE_EXTSRC1CONF_BEBO_##__x)
+#define MCDE_EXTSRC1CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BEBO, __x)
+#define MCDE_EXTSRC1CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC1CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC1CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC1CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC1CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BEPO, MCDE_EXTSRC1CONF_BEPO_##__x)
+#define MCDE_EXTSRC1CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CONF, BEPO, __x)
+#define MCDE_EXTSRC2CONF 0x0000024C
+#define MCDE_EXTSRC2CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC2CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC2CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BUF_ID, __x)
+#define MCDE_EXTSRC2CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC2CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC2CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BUF_NB, __x)
+#define MCDE_EXTSRC2CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC2CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC2CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC2CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC2CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC2CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC2CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC2CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC2CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC2CONF_BPP_RGB444 4
+#define MCDE_EXTSRC2CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC2CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC2CONF_BPP_RGB565 7
+#define MCDE_EXTSRC2CONF_BPP_RGB888 8
+#define MCDE_EXTSRC2CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC2CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC2CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC2CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BPP, MCDE_EXTSRC2CONF_BPP_##__x)
+#define MCDE_EXTSRC2CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BPP, __x)
+#define MCDE_EXTSRC2CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC2CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC2CONF_BGR_RGB 0
+#define MCDE_EXTSRC2CONF_BGR_BGR 1
+#define MCDE_EXTSRC2CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BGR, MCDE_EXTSRC2CONF_BGR_##__x)
+#define MCDE_EXTSRC2CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BGR, __x)
+#define MCDE_EXTSRC2CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC2CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC2CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC2CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC2CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BEBO, MCDE_EXTSRC2CONF_BEBO_##__x)
+#define MCDE_EXTSRC2CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BEBO, __x)
+#define MCDE_EXTSRC2CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC2CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC2CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC2CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC2CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BEPO, MCDE_EXTSRC2CONF_BEPO_##__x)
+#define MCDE_EXTSRC2CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CONF, BEPO, __x)
+#define MCDE_EXTSRC3CONF 0x0000026C
+#define MCDE_EXTSRC3CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC3CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC3CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BUF_ID, __x)
+#define MCDE_EXTSRC3CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC3CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC3CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BUF_NB, __x)
+#define MCDE_EXTSRC3CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC3CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC3CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC3CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC3CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC3CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC3CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC3CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC3CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC3CONF_BPP_RGB444 4
+#define MCDE_EXTSRC3CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC3CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC3CONF_BPP_RGB565 7
+#define MCDE_EXTSRC3CONF_BPP_RGB888 8
+#define MCDE_EXTSRC3CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC3CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC3CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC3CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BPP, MCDE_EXTSRC3CONF_BPP_##__x)
+#define MCDE_EXTSRC3CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BPP, __x)
+#define MCDE_EXTSRC3CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC3CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC3CONF_BGR_RGB 0
+#define MCDE_EXTSRC3CONF_BGR_BGR 1
+#define MCDE_EXTSRC3CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BGR, MCDE_EXTSRC3CONF_BGR_##__x)
+#define MCDE_EXTSRC3CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BGR, __x)
+#define MCDE_EXTSRC3CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC3CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC3CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC3CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC3CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BEBO, MCDE_EXTSRC3CONF_BEBO_##__x)
+#define MCDE_EXTSRC3CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BEBO, __x)
+#define MCDE_EXTSRC3CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC3CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC3CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC3CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC3CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BEPO, MCDE_EXTSRC3CONF_BEPO_##__x)
+#define MCDE_EXTSRC3CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CONF, BEPO, __x)
+#define MCDE_EXTSRC4CONF 0x0000028C
+#define MCDE_EXTSRC4CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC4CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC4CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BUF_ID, __x)
+#define MCDE_EXTSRC4CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC4CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC4CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BUF_NB, __x)
+#define MCDE_EXTSRC4CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC4CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC4CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC4CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC4CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC4CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC4CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC4CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC4CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC4CONF_BPP_RGB444 4
+#define MCDE_EXTSRC4CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC4CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC4CONF_BPP_RGB565 7
+#define MCDE_EXTSRC4CONF_BPP_RGB888 8
+#define MCDE_EXTSRC4CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC4CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC4CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC4CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BPP, MCDE_EXTSRC4CONF_BPP_##__x)
+#define MCDE_EXTSRC4CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BPP, __x)
+#define MCDE_EXTSRC4CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC4CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC4CONF_BGR_RGB 0
+#define MCDE_EXTSRC4CONF_BGR_BGR 1
+#define MCDE_EXTSRC4CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BGR, MCDE_EXTSRC4CONF_BGR_##__x)
+#define MCDE_EXTSRC4CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BGR, __x)
+#define MCDE_EXTSRC4CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC4CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC4CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC4CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC4CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BEBO, MCDE_EXTSRC4CONF_BEBO_##__x)
+#define MCDE_EXTSRC4CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BEBO, __x)
+#define MCDE_EXTSRC4CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC4CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC4CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC4CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC4CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BEPO, MCDE_EXTSRC4CONF_BEPO_##__x)
+#define MCDE_EXTSRC4CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CONF, BEPO, __x)
+#define MCDE_EXTSRC5CONF 0x000002AC
+#define MCDE_EXTSRC5CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC5CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC5CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BUF_ID, __x)
+#define MCDE_EXTSRC5CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC5CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC5CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BUF_NB, __x)
+#define MCDE_EXTSRC5CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC5CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC5CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC5CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC5CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC5CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC5CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC5CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC5CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC5CONF_BPP_RGB444 4
+#define MCDE_EXTSRC5CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC5CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC5CONF_BPP_RGB565 7
+#define MCDE_EXTSRC5CONF_BPP_RGB888 8
+#define MCDE_EXTSRC5CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC5CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC5CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC5CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BPP, MCDE_EXTSRC5CONF_BPP_##__x)
+#define MCDE_EXTSRC5CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BPP, __x)
+#define MCDE_EXTSRC5CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC5CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC5CONF_BGR_RGB 0
+#define MCDE_EXTSRC5CONF_BGR_BGR 1
+#define MCDE_EXTSRC5CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BGR, MCDE_EXTSRC5CONF_BGR_##__x)
+#define MCDE_EXTSRC5CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BGR, __x)
+#define MCDE_EXTSRC5CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC5CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC5CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC5CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC5CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BEBO, MCDE_EXTSRC5CONF_BEBO_##__x)
+#define MCDE_EXTSRC5CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BEBO, __x)
+#define MCDE_EXTSRC5CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC5CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC5CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC5CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC5CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BEPO, MCDE_EXTSRC5CONF_BEPO_##__x)
+#define MCDE_EXTSRC5CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CONF, BEPO, __x)
+#define MCDE_EXTSRC6CONF 0x000002CC
+#define MCDE_EXTSRC6CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC6CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC6CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BUF_ID, __x)
+#define MCDE_EXTSRC6CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC6CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC6CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BUF_NB, __x)
+#define MCDE_EXTSRC6CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC6CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC6CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC6CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC6CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC6CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC6CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC6CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC6CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC6CONF_BPP_RGB444 4
+#define MCDE_EXTSRC6CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC6CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC6CONF_BPP_RGB565 7
+#define MCDE_EXTSRC6CONF_BPP_RGB888 8
+#define MCDE_EXTSRC6CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC6CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC6CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC6CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BPP, MCDE_EXTSRC6CONF_BPP_##__x)
+#define MCDE_EXTSRC6CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BPP, __x)
+#define MCDE_EXTSRC6CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC6CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC6CONF_BGR_RGB 0
+#define MCDE_EXTSRC6CONF_BGR_BGR 1
+#define MCDE_EXTSRC6CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BGR, MCDE_EXTSRC6CONF_BGR_##__x)
+#define MCDE_EXTSRC6CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BGR, __x)
+#define MCDE_EXTSRC6CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC6CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC6CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC6CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC6CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BEBO, MCDE_EXTSRC6CONF_BEBO_##__x)
+#define MCDE_EXTSRC6CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BEBO, __x)
+#define MCDE_EXTSRC6CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC6CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC6CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC6CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC6CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BEPO, MCDE_EXTSRC6CONF_BEPO_##__x)
+#define MCDE_EXTSRC6CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CONF, BEPO, __x)
+#define MCDE_EXTSRC7CONF 0x000002EC
+#define MCDE_EXTSRC7CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC7CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC7CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BUF_ID, __x)
+#define MCDE_EXTSRC7CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC7CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC7CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BUF_NB, __x)
+#define MCDE_EXTSRC7CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC7CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC7CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC7CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC7CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC7CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC7CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC7CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC7CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC7CONF_BPP_RGB444 4
+#define MCDE_EXTSRC7CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC7CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC7CONF_BPP_RGB565 7
+#define MCDE_EXTSRC7CONF_BPP_RGB888 8
+#define MCDE_EXTSRC7CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC7CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC7CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC7CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BPP, MCDE_EXTSRC7CONF_BPP_##__x)
+#define MCDE_EXTSRC7CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BPP, __x)
+#define MCDE_EXTSRC7CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC7CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC7CONF_BGR_RGB 0
+#define MCDE_EXTSRC7CONF_BGR_BGR 1
+#define MCDE_EXTSRC7CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BGR, MCDE_EXTSRC7CONF_BGR_##__x)
+#define MCDE_EXTSRC7CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BGR, __x)
+#define MCDE_EXTSRC7CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC7CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC7CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC7CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC7CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BEBO, MCDE_EXTSRC7CONF_BEBO_##__x)
+#define MCDE_EXTSRC7CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BEBO, __x)
+#define MCDE_EXTSRC7CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC7CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC7CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC7CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC7CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BEPO, MCDE_EXTSRC7CONF_BEPO_##__x)
+#define MCDE_EXTSRC7CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CONF, BEPO, __x)
+#define MCDE_EXTSRC8CONF 0x0000030C
+#define MCDE_EXTSRC8CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC8CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC8CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BUF_ID, __x)
+#define MCDE_EXTSRC8CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC8CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC8CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BUF_NB, __x)
+#define MCDE_EXTSRC8CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC8CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC8CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC8CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC8CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC8CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC8CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC8CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC8CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC8CONF_BPP_RGB444 4
+#define MCDE_EXTSRC8CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC8CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC8CONF_BPP_RGB565 7
+#define MCDE_EXTSRC8CONF_BPP_RGB888 8
+#define MCDE_EXTSRC8CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC8CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC8CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC8CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BPP, MCDE_EXTSRC8CONF_BPP_##__x)
+#define MCDE_EXTSRC8CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BPP, __x)
+#define MCDE_EXTSRC8CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC8CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC8CONF_BGR_RGB 0
+#define MCDE_EXTSRC8CONF_BGR_BGR 1
+#define MCDE_EXTSRC8CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BGR, MCDE_EXTSRC8CONF_BGR_##__x)
+#define MCDE_EXTSRC8CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BGR, __x)
+#define MCDE_EXTSRC8CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC8CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC8CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC8CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC8CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BEBO, MCDE_EXTSRC8CONF_BEBO_##__x)
+#define MCDE_EXTSRC8CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BEBO, __x)
+#define MCDE_EXTSRC8CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC8CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC8CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC8CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC8CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BEPO, MCDE_EXTSRC8CONF_BEPO_##__x)
+#define MCDE_EXTSRC8CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CONF, BEPO, __x)
+#define MCDE_EXTSRC9CONF 0x0000032C
+#define MCDE_EXTSRC9CONF_BUF_ID_SHIFT 0
+#define MCDE_EXTSRC9CONF_BUF_ID_MASK 0x00000003
+#define MCDE_EXTSRC9CONF_BUF_ID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BUF_ID, __x)
+#define MCDE_EXTSRC9CONF_BUF_NB_SHIFT 2
+#define MCDE_EXTSRC9CONF_BUF_NB_MASK 0x0000000C
+#define MCDE_EXTSRC9CONF_BUF_NB(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BUF_NB, __x)
+#define MCDE_EXTSRC9CONF_PRI_OVLID_SHIFT 4
+#define MCDE_EXTSRC9CONF_PRI_OVLID_MASK 0x000000F0
+#define MCDE_EXTSRC9CONF_PRI_OVLID(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, PRI_OVLID, __x)
+#define MCDE_EXTSRC9CONF_BPP_SHIFT 8
+#define MCDE_EXTSRC9CONF_BPP_MASK 0x00000F00
+#define MCDE_EXTSRC9CONF_BPP_1BPP_PAL 0
+#define MCDE_EXTSRC9CONF_BPP_2BPP_PAL 1
+#define MCDE_EXTSRC9CONF_BPP_4BPP_PAL 2
+#define MCDE_EXTSRC9CONF_BPP_8BPP_PAL 3
+#define MCDE_EXTSRC9CONF_BPP_RGB444 4
+#define MCDE_EXTSRC9CONF_BPP_ARGB4444 5
+#define MCDE_EXTSRC9CONF_BPP_IRGB1555 6
+#define MCDE_EXTSRC9CONF_BPP_RGB565 7
+#define MCDE_EXTSRC9CONF_BPP_RGB888 8
+#define MCDE_EXTSRC9CONF_BPP_XRGB8888 9
+#define MCDE_EXTSRC9CONF_BPP_ARGB8888 10
+#define MCDE_EXTSRC9CONF_BPP_YCBCR422 11
+#define MCDE_EXTSRC9CONF_BPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BPP, MCDE_EXTSRC9CONF_BPP_##__x)
+#define MCDE_EXTSRC9CONF_BPP(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BPP, __x)
+#define MCDE_EXTSRC9CONF_BGR_SHIFT 12
+#define MCDE_EXTSRC9CONF_BGR_MASK 0x00001000
+#define MCDE_EXTSRC9CONF_BGR_RGB 0
+#define MCDE_EXTSRC9CONF_BGR_BGR 1
+#define MCDE_EXTSRC9CONF_BGR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BGR, MCDE_EXTSRC9CONF_BGR_##__x)
+#define MCDE_EXTSRC9CONF_BGR(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BGR, __x)
+#define MCDE_EXTSRC9CONF_BEBO_SHIFT 13
+#define MCDE_EXTSRC9CONF_BEBO_MASK 0x00002000
+#define MCDE_EXTSRC9CONF_BEBO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC9CONF_BEBO_BIG_ENDIAN 1
+#define MCDE_EXTSRC9CONF_BEBO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BEBO, MCDE_EXTSRC9CONF_BEBO_##__x)
+#define MCDE_EXTSRC9CONF_BEBO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BEBO, __x)
+#define MCDE_EXTSRC9CONF_BEPO_SHIFT 14
+#define MCDE_EXTSRC9CONF_BEPO_MASK 0x00004000
+#define MCDE_EXTSRC9CONF_BEPO_LITTLE_ENDIAN 0
+#define MCDE_EXTSRC9CONF_BEPO_BIG_ENDIAN 1
+#define MCDE_EXTSRC9CONF_BEPO_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BEPO, MCDE_EXTSRC9CONF_BEPO_##__x)
+#define MCDE_EXTSRC9CONF_BEPO(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CONF, BEPO, __x)
+#define MCDE_EXTSRC0CR 0x00000210
+#define MCDE_EXTSRC0CR_GROUPOFFSET 0x20
+#define MCDE_EXTSRC0CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC0CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC0CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC0CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC0CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC0CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, SEL_MOD, MCDE_EXTSRC0CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC0CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, SEL_MOD, __x)
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC0CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC0CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC0CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC0CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC0CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC0CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC0CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC0CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC0CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC1CR 0x00000230
+#define MCDE_EXTSRC1CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC1CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC1CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC1CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC1CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC1CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, SEL_MOD, MCDE_EXTSRC1CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC1CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, SEL_MOD, __x)
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC1CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC1CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC1CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC1CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC1CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC1CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC1CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC1CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC1CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC2CR 0x00000250
+#define MCDE_EXTSRC2CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC2CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC2CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC2CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC2CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC2CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, SEL_MOD, MCDE_EXTSRC2CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC2CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, SEL_MOD, __x)
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC2CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC2CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC2CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC2CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC2CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC2CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC2CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC2CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC2CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC3CR 0x00000270
+#define MCDE_EXTSRC3CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC3CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC3CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC3CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC3CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC3CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, SEL_MOD, MCDE_EXTSRC3CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC3CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, SEL_MOD, __x)
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC3CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC3CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC3CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC3CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC3CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC3CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC3CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC3CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC3CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC4CR 0x00000290
+#define MCDE_EXTSRC4CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC4CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC4CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC4CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC4CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC4CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, SEL_MOD, MCDE_EXTSRC4CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC4CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, SEL_MOD, __x)
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC4CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC4CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC4CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC4CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC4CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC4CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC4CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC4CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC4CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC5CR 0x000002B0
+#define MCDE_EXTSRC5CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC5CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC5CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC5CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC5CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC5CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, SEL_MOD, MCDE_EXTSRC5CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC5CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, SEL_MOD, __x)
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC5CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC5CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC5CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC5CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC5CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC5CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC5CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC5CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC5CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC6CR 0x000002D0
+#define MCDE_EXTSRC6CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC6CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC6CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC6CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC6CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC6CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, SEL_MOD, MCDE_EXTSRC6CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC6CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, SEL_MOD, __x)
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC6CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC6CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC6CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC6CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC6CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC6CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC6CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC6CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC6CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC7CR 0x000002F0
+#define MCDE_EXTSRC7CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC7CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC7CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC7CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC7CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC7CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, SEL_MOD, MCDE_EXTSRC7CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC7CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, SEL_MOD, __x)
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC7CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC7CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC7CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC7CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC7CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC7CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC7CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC7CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC7CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC8CR 0x00000310
+#define MCDE_EXTSRC8CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC8CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC8CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC8CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC8CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC8CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, SEL_MOD, MCDE_EXTSRC8CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC8CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, SEL_MOD, __x)
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC8CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC8CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC8CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC8CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC8CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC8CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC8CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC8CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC8CR, FORCE_FS_DIV, __x)
+#define MCDE_EXTSRC9CR 0x00000330
+#define MCDE_EXTSRC9CR_SEL_MOD_SHIFT 0
+#define MCDE_EXTSRC9CR_SEL_MOD_MASK 0x00000003
+#define MCDE_EXTSRC9CR_SEL_MOD_EXTERNAL_SEL 0
+#define MCDE_EXTSRC9CR_SEL_MOD_AUTO_TOGGLE 1
+#define MCDE_EXTSRC9CR_SEL_MOD_SOFTWARE_SEL 2
+#define MCDE_EXTSRC9CR_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, SEL_MOD, MCDE_EXTSRC9CR_SEL_MOD_##__x)
+#define MCDE_EXTSRC9CR_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, SEL_MOD, __x)
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_SHIFT 2
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_MASK 0x00000004
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_ALL 0
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_PRIMARY 1
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, MULTIOVL_CTRL, \
+	MCDE_EXTSRC9CR_MULTIOVL_CTRL_##__x)
+#define MCDE_EXTSRC9CR_MULTIOVL_CTRL(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, MULTIOVL_CTRL, __x)
+#define MCDE_EXTSRC9CR_FS_DIV_DISABLE_SHIFT 3
+#define MCDE_EXTSRC9CR_FS_DIV_DISABLE_MASK 0x00000008
+#define MCDE_EXTSRC9CR_FS_DIV_DISABLE(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, FS_DIV_DISABLE, __x)
+#define MCDE_EXTSRC9CR_FORCE_FS_DIV_SHIFT 4
+#define MCDE_EXTSRC9CR_FORCE_FS_DIV_MASK 0x00000010
+#define MCDE_EXTSRC9CR_FORCE_FS_DIV(__x) \
+	MCDE_VAL2REG(MCDE_EXTSRC9CR, FORCE_FS_DIV, __x)
+#define MCDE_OVL0CR 0x00000400
+#define MCDE_OVL0CR_GROUPOFFSET 0x20
+#define MCDE_OVL0CR_OVLEN_SHIFT 0
+#define MCDE_OVL0CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL0CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, OVLEN, __x)
+#define MCDE_OVL0CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL0CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL0CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL0CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL0CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL0CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, COLCCTRL, MCDE_OVL0CR_COLCCTRL_##__x)
+#define MCDE_OVL0CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, COLCCTRL, __x)
+#define MCDE_OVL0CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL0CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL0CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, CKEYGEN, __x)
+#define MCDE_OVL0CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL0CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL0CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, ALPHAPMEN, __x)
+#define MCDE_OVL0CR_OVLF_SHIFT 5
+#define MCDE_OVL0CR_OVLF_MASK 0x00000020
+#define MCDE_OVL0CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, OVLF, __x)
+#define MCDE_OVL0CR_OVLR_SHIFT 6
+#define MCDE_OVL0CR_OVLR_MASK 0x00000040
+#define MCDE_OVL0CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, OVLR, __x)
+#define MCDE_OVL0CR_OVLB_SHIFT 7
+#define MCDE_OVL0CR_OVLB_MASK 0x00000080
+#define MCDE_OVL0CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, OVLB, __x)
+#define MCDE_OVL0CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL0CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL0CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, FETCH_ROPC, __x)
+#define MCDE_OVL0CR_STBPRIO_SHIFT 16
+#define MCDE_OVL0CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL0CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, STBPRIO, __x)
+#define MCDE_OVL0CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL0CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL0CR_BURSTSIZE_1W 0
+#define MCDE_OVL0CR_BURSTSIZE_2W 1
+#define MCDE_OVL0CR_BURSTSIZE_4W 2
+#define MCDE_OVL0CR_BURSTSIZE_8W 3
+#define MCDE_OVL0CR_BURSTSIZE_16W 4
+#define MCDE_OVL0CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL0CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL0CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL0CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL0CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL0CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, BURSTSIZE, MCDE_OVL0CR_BURSTSIZE_##__x)
+#define MCDE_OVL0CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, BURSTSIZE, __x)
+#define MCDE_OVL0CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL0CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL0CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL0CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL0CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL0CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL0CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL0CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, MAXOUTSTANDING, \
+	MCDE_OVL0CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL0CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL0CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL0CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL0CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL0CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL0CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL0CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL0CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL0CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL0CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, ROTBURSTSIZE, MCDE_OVL0CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL0CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL1CR 0x00000420
+#define MCDE_OVL1CR_OVLEN_SHIFT 0
+#define MCDE_OVL1CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL1CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, OVLEN, __x)
+#define MCDE_OVL1CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL1CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL1CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL1CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL1CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL1CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, COLCCTRL, MCDE_OVL1CR_COLCCTRL_##__x)
+#define MCDE_OVL1CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, COLCCTRL, __x)
+#define MCDE_OVL1CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL1CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL1CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, CKEYGEN, __x)
+#define MCDE_OVL1CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL1CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL1CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, ALPHAPMEN, __x)
+#define MCDE_OVL1CR_OVLF_SHIFT 5
+#define MCDE_OVL1CR_OVLF_MASK 0x00000020
+#define MCDE_OVL1CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, OVLF, __x)
+#define MCDE_OVL1CR_OVLR_SHIFT 6
+#define MCDE_OVL1CR_OVLR_MASK 0x00000040
+#define MCDE_OVL1CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, OVLR, __x)
+#define MCDE_OVL1CR_OVLB_SHIFT 7
+#define MCDE_OVL1CR_OVLB_MASK 0x00000080
+#define MCDE_OVL1CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, OVLB, __x)
+#define MCDE_OVL1CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL1CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL1CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, FETCH_ROPC, __x)
+#define MCDE_OVL1CR_STBPRIO_SHIFT 16
+#define MCDE_OVL1CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL1CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, STBPRIO, __x)
+#define MCDE_OVL1CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL1CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL1CR_BURSTSIZE_1W 0
+#define MCDE_OVL1CR_BURSTSIZE_2W 1
+#define MCDE_OVL1CR_BURSTSIZE_4W 2
+#define MCDE_OVL1CR_BURSTSIZE_8W 3
+#define MCDE_OVL1CR_BURSTSIZE_16W 4
+#define MCDE_OVL1CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL1CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL1CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL1CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL1CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL1CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, BURSTSIZE, MCDE_OVL1CR_BURSTSIZE_##__x)
+#define MCDE_OVL1CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, BURSTSIZE, __x)
+#define MCDE_OVL1CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL1CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL1CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL1CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL1CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL1CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL1CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL1CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, MAXOUTSTANDING, \
+	MCDE_OVL1CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL1CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL1CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL1CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL1CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL1CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL1CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL1CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL1CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL1CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL1CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, ROTBURSTSIZE, MCDE_OVL1CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL1CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL2CR 0x00000440
+#define MCDE_OVL2CR_OVLEN_SHIFT 0
+#define MCDE_OVL2CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL2CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, OVLEN, __x)
+#define MCDE_OVL2CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL2CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL2CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL2CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL2CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL2CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, COLCCTRL, MCDE_OVL2CR_COLCCTRL_##__x)
+#define MCDE_OVL2CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, COLCCTRL, __x)
+#define MCDE_OVL2CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL2CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL2CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, CKEYGEN, __x)
+#define MCDE_OVL2CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL2CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL2CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, ALPHAPMEN, __x)
+#define MCDE_OVL2CR_OVLF_SHIFT 5
+#define MCDE_OVL2CR_OVLF_MASK 0x00000020
+#define MCDE_OVL2CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, OVLF, __x)
+#define MCDE_OVL2CR_OVLR_SHIFT 6
+#define MCDE_OVL2CR_OVLR_MASK 0x00000040
+#define MCDE_OVL2CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, OVLR, __x)
+#define MCDE_OVL2CR_OVLB_SHIFT 7
+#define MCDE_OVL2CR_OVLB_MASK 0x00000080
+#define MCDE_OVL2CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, OVLB, __x)
+#define MCDE_OVL2CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL2CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL2CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, FETCH_ROPC, __x)
+#define MCDE_OVL2CR_STBPRIO_SHIFT 16
+#define MCDE_OVL2CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL2CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, STBPRIO, __x)
+#define MCDE_OVL2CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL2CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL2CR_BURSTSIZE_1W 0
+#define MCDE_OVL2CR_BURSTSIZE_2W 1
+#define MCDE_OVL2CR_BURSTSIZE_4W 2
+#define MCDE_OVL2CR_BURSTSIZE_8W 3
+#define MCDE_OVL2CR_BURSTSIZE_16W 4
+#define MCDE_OVL2CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL2CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL2CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL2CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL2CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL2CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, BURSTSIZE, MCDE_OVL2CR_BURSTSIZE_##__x)
+#define MCDE_OVL2CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, BURSTSIZE, __x)
+#define MCDE_OVL2CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL2CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL2CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL2CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL2CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL2CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL2CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL2CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, MAXOUTSTANDING, \
+	MCDE_OVL2CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL2CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL2CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL2CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL2CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL2CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL2CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL2CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL2CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL2CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL2CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, ROTBURSTSIZE, MCDE_OVL2CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL2CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL3CR 0x00000460
+#define MCDE_OVL3CR_OVLEN_SHIFT 0
+#define MCDE_OVL3CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL3CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, OVLEN, __x)
+#define MCDE_OVL3CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL3CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL3CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL3CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL3CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL3CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, COLCCTRL, MCDE_OVL3CR_COLCCTRL_##__x)
+#define MCDE_OVL3CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, COLCCTRL, __x)
+#define MCDE_OVL3CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL3CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL3CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, CKEYGEN, __x)
+#define MCDE_OVL3CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL3CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL3CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, ALPHAPMEN, __x)
+#define MCDE_OVL3CR_OVLF_SHIFT 5
+#define MCDE_OVL3CR_OVLF_MASK 0x00000020
+#define MCDE_OVL3CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, OVLF, __x)
+#define MCDE_OVL3CR_OVLR_SHIFT 6
+#define MCDE_OVL3CR_OVLR_MASK 0x00000040
+#define MCDE_OVL3CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, OVLR, __x)
+#define MCDE_OVL3CR_OVLB_SHIFT 7
+#define MCDE_OVL3CR_OVLB_MASK 0x00000080
+#define MCDE_OVL3CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, OVLB, __x)
+#define MCDE_OVL3CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL3CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL3CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, FETCH_ROPC, __x)
+#define MCDE_OVL3CR_STBPRIO_SHIFT 16
+#define MCDE_OVL3CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL3CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, STBPRIO, __x)
+#define MCDE_OVL3CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL3CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL3CR_BURSTSIZE_1W 0
+#define MCDE_OVL3CR_BURSTSIZE_2W 1
+#define MCDE_OVL3CR_BURSTSIZE_4W 2
+#define MCDE_OVL3CR_BURSTSIZE_8W 3
+#define MCDE_OVL3CR_BURSTSIZE_16W 4
+#define MCDE_OVL3CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL3CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL3CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL3CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL3CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL3CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, BURSTSIZE, MCDE_OVL3CR_BURSTSIZE_##__x)
+#define MCDE_OVL3CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, BURSTSIZE, __x)
+#define MCDE_OVL3CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL3CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL3CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL3CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL3CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL3CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL3CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL3CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, MAXOUTSTANDING, \
+	MCDE_OVL3CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL3CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL3CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL3CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL3CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL3CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL3CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL3CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL3CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL3CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL3CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, ROTBURSTSIZE, MCDE_OVL3CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL3CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL4CR 0x00000480
+#define MCDE_OVL4CR_OVLEN_SHIFT 0
+#define MCDE_OVL4CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL4CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, OVLEN, __x)
+#define MCDE_OVL4CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL4CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL4CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL4CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL4CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL4CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, COLCCTRL, MCDE_OVL4CR_COLCCTRL_##__x)
+#define MCDE_OVL4CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, COLCCTRL, __x)
+#define MCDE_OVL4CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL4CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL4CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, CKEYGEN, __x)
+#define MCDE_OVL4CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL4CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL4CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, ALPHAPMEN, __x)
+#define MCDE_OVL4CR_OVLF_SHIFT 5
+#define MCDE_OVL4CR_OVLF_MASK 0x00000020
+#define MCDE_OVL4CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, OVLF, __x)
+#define MCDE_OVL4CR_OVLR_SHIFT 6
+#define MCDE_OVL4CR_OVLR_MASK 0x00000040
+#define MCDE_OVL4CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, OVLR, __x)
+#define MCDE_OVL4CR_OVLB_SHIFT 7
+#define MCDE_OVL4CR_OVLB_MASK 0x00000080
+#define MCDE_OVL4CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, OVLB, __x)
+#define MCDE_OVL4CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL4CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL4CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, FETCH_ROPC, __x)
+#define MCDE_OVL4CR_STBPRIO_SHIFT 16
+#define MCDE_OVL4CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL4CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, STBPRIO, __x)
+#define MCDE_OVL4CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL4CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL4CR_BURSTSIZE_1W 0
+#define MCDE_OVL4CR_BURSTSIZE_2W 1
+#define MCDE_OVL4CR_BURSTSIZE_4W 2
+#define MCDE_OVL4CR_BURSTSIZE_8W 3
+#define MCDE_OVL4CR_BURSTSIZE_16W 4
+#define MCDE_OVL4CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL4CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL4CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL4CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL4CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL4CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, BURSTSIZE, MCDE_OVL4CR_BURSTSIZE_##__x)
+#define MCDE_OVL4CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, BURSTSIZE, __x)
+#define MCDE_OVL4CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL4CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL4CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL4CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL4CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL4CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL4CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL4CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, MAXOUTSTANDING, \
+	MCDE_OVL4CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL4CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL4CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL4CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL4CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL4CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL4CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL4CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL4CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL4CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL4CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, ROTBURSTSIZE, MCDE_OVL4CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL4CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL5CR 0x000004A0
+#define MCDE_OVL5CR_OVLEN_SHIFT 0
+#define MCDE_OVL5CR_OVLEN_MASK 0x00000001
+#define MCDE_OVL5CR_OVLEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, OVLEN, __x)
+#define MCDE_OVL5CR_COLCCTRL_SHIFT 1
+#define MCDE_OVL5CR_COLCCTRL_MASK 0x00000006
+#define MCDE_OVL5CR_COLCCTRL_DISABLED 0
+#define MCDE_OVL5CR_COLCCTRL_ENABLED_NO_SAT 1
+#define MCDE_OVL5CR_COLCCTRL_ENABLED_SAT 2
+#define MCDE_OVL5CR_COLCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, COLCCTRL, MCDE_OVL5CR_COLCCTRL_##__x)
+#define MCDE_OVL5CR_COLCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, COLCCTRL, __x)
+#define MCDE_OVL5CR_CKEYGEN_SHIFT 3
+#define MCDE_OVL5CR_CKEYGEN_MASK 0x00000008
+#define MCDE_OVL5CR_CKEYGEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, CKEYGEN, __x)
+#define MCDE_OVL5CR_ALPHAPMEN_SHIFT 4
+#define MCDE_OVL5CR_ALPHAPMEN_MASK 0x00000010
+#define MCDE_OVL5CR_ALPHAPMEN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, ALPHAPMEN, __x)
+#define MCDE_OVL5CR_OVLF_SHIFT 5
+#define MCDE_OVL5CR_OVLF_MASK 0x00000020
+#define MCDE_OVL5CR_OVLF(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, OVLF, __x)
+#define MCDE_OVL5CR_OVLR_SHIFT 6
+#define MCDE_OVL5CR_OVLR_MASK 0x00000040
+#define MCDE_OVL5CR_OVLR(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, OVLR, __x)
+#define MCDE_OVL5CR_OVLB_SHIFT 7
+#define MCDE_OVL5CR_OVLB_MASK 0x00000080
+#define MCDE_OVL5CR_OVLB(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, OVLB, __x)
+#define MCDE_OVL5CR_FETCH_ROPC_SHIFT 8
+#define MCDE_OVL5CR_FETCH_ROPC_MASK 0x0000FF00
+#define MCDE_OVL5CR_FETCH_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, FETCH_ROPC, __x)
+#define MCDE_OVL5CR_STBPRIO_SHIFT 16
+#define MCDE_OVL5CR_STBPRIO_MASK 0x000F0000
+#define MCDE_OVL5CR_STBPRIO(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, STBPRIO, __x)
+#define MCDE_OVL5CR_BURSTSIZE_SHIFT 20
+#define MCDE_OVL5CR_BURSTSIZE_MASK 0x00F00000
+#define MCDE_OVL5CR_BURSTSIZE_1W 0
+#define MCDE_OVL5CR_BURSTSIZE_2W 1
+#define MCDE_OVL5CR_BURSTSIZE_4W 2
+#define MCDE_OVL5CR_BURSTSIZE_8W 3
+#define MCDE_OVL5CR_BURSTSIZE_16W 4
+#define MCDE_OVL5CR_BURSTSIZE_HW_1W 8
+#define MCDE_OVL5CR_BURSTSIZE_HW_2W 9
+#define MCDE_OVL5CR_BURSTSIZE_HW_4W 10
+#define MCDE_OVL5CR_BURSTSIZE_HW_8W 11
+#define MCDE_OVL5CR_BURSTSIZE_HW_16W 12
+#define MCDE_OVL5CR_BURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, BURSTSIZE, MCDE_OVL5CR_BURSTSIZE_##__x)
+#define MCDE_OVL5CR_BURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, BURSTSIZE, __x)
+#define MCDE_OVL5CR_MAXOUTSTANDING_SHIFT 24
+#define MCDE_OVL5CR_MAXOUTSTANDING_MASK 0x0F000000
+#define MCDE_OVL5CR_MAXOUTSTANDING_1_REQ 0
+#define MCDE_OVL5CR_MAXOUTSTANDING_2_REQ 1
+#define MCDE_OVL5CR_MAXOUTSTANDING_4_REQ 2
+#define MCDE_OVL5CR_MAXOUTSTANDING_8_REQ 3
+#define MCDE_OVL5CR_MAXOUTSTANDING_16_REQ 4
+#define MCDE_OVL5CR_MAXOUTSTANDING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, MAXOUTSTANDING, \
+	MCDE_OVL5CR_MAXOUTSTANDING_##__x)
+#define MCDE_OVL5CR_MAXOUTSTANDING(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, MAXOUTSTANDING, __x)
+#define MCDE_OVL5CR_ROTBURSTSIZE_SHIFT 28
+#define MCDE_OVL5CR_ROTBURSTSIZE_MASK 0xF0000000
+#define MCDE_OVL5CR_ROTBURSTSIZE_1W 0
+#define MCDE_OVL5CR_ROTBURSTSIZE_2W 1
+#define MCDE_OVL5CR_ROTBURSTSIZE_4W 2
+#define MCDE_OVL5CR_ROTBURSTSIZE_8W 3
+#define MCDE_OVL5CR_ROTBURSTSIZE_16W 4
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_1W 8
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_2W 9
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_4W 10
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_8W 11
+#define MCDE_OVL5CR_ROTBURSTSIZE_HW_16W 12
+#define MCDE_OVL5CR_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, ROTBURSTSIZE, MCDE_OVL5CR_ROTBURSTSIZE_##__x)
+#define MCDE_OVL5CR_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CR, ROTBURSTSIZE, __x)
+#define MCDE_OVL0CONF 0x00000404
+#define MCDE_OVL0CONF_GROUPOFFSET 0x20
+#define MCDE_OVL0CONF_PPL_SHIFT 0
+#define MCDE_OVL0CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL0CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF, PPL, __x)
+#define MCDE_OVL0CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL0CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL0CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF, EXTSRC_ID, __x)
+#define MCDE_OVL0CONF_LPF_SHIFT 16
+#define MCDE_OVL0CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL0CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF, LPF, __x)
+#define MCDE_OVL1CONF 0x00000424
+#define MCDE_OVL1CONF_PPL_SHIFT 0
+#define MCDE_OVL1CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL1CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF, PPL, __x)
+#define MCDE_OVL1CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL1CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL1CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF, EXTSRC_ID, __x)
+#define MCDE_OVL1CONF_LPF_SHIFT 16
+#define MCDE_OVL1CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL1CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF, LPF, __x)
+#define MCDE_OVL2CONF 0x00000444
+#define MCDE_OVL2CONF_PPL_SHIFT 0
+#define MCDE_OVL2CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL2CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF, PPL, __x)
+#define MCDE_OVL2CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL2CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL2CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF, EXTSRC_ID, __x)
+#define MCDE_OVL2CONF_LPF_SHIFT 16
+#define MCDE_OVL2CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL2CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF, LPF, __x)
+#define MCDE_OVL3CONF 0x00000464
+#define MCDE_OVL3CONF_PPL_SHIFT 0
+#define MCDE_OVL3CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL3CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF, PPL, __x)
+#define MCDE_OVL3CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL3CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL3CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF, EXTSRC_ID, __x)
+#define MCDE_OVL3CONF_LPF_SHIFT 16
+#define MCDE_OVL3CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL3CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF, LPF, __x)
+#define MCDE_OVL4CONF 0x00000484
+#define MCDE_OVL4CONF_PPL_SHIFT 0
+#define MCDE_OVL4CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL4CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF, PPL, __x)
+#define MCDE_OVL4CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL4CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL4CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF, EXTSRC_ID, __x)
+#define MCDE_OVL4CONF_LPF_SHIFT 16
+#define MCDE_OVL4CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL4CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF, LPF, __x)
+#define MCDE_OVL5CONF 0x000004A4
+#define MCDE_OVL5CONF_PPL_SHIFT 0
+#define MCDE_OVL5CONF_PPL_MASK 0x000007FF
+#define MCDE_OVL5CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF, PPL, __x)
+#define MCDE_OVL5CONF_EXTSRC_ID_SHIFT 11
+#define MCDE_OVL5CONF_EXTSRC_ID_MASK 0x00007800
+#define MCDE_OVL5CONF_EXTSRC_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF, EXTSRC_ID, __x)
+#define MCDE_OVL5CONF_LPF_SHIFT 16
+#define MCDE_OVL5CONF_LPF_MASK 0x07FF0000
+#define MCDE_OVL5CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF, LPF, __x)
+#define MCDE_OVL0CONF2 0x00000408
+#define MCDE_OVL0CONF2_GROUPOFFSET 0x20
+#define MCDE_OVL0CONF2_BP_SHIFT 0
+#define MCDE_OVL0CONF2_BP_MASK 0x00000001
+#define MCDE_OVL0CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL0CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL0CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, BP, MCDE_OVL0CONF2_BP_##__x)
+#define MCDE_OVL0CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, BP, __x)
+#define MCDE_OVL0CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL0CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL0CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL0CONF2_OPQ_SHIFT 9
+#define MCDE_OVL0CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL0CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, OPQ, __x)
+#define MCDE_OVL0CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL0CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL0CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, PIXOFF, __x)
+#define MCDE_OVL0CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL0CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL0CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL1CONF2 0x00000428
+#define MCDE_OVL1CONF2_BP_SHIFT 0
+#define MCDE_OVL1CONF2_BP_MASK 0x00000001
+#define MCDE_OVL1CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL1CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL1CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, BP, MCDE_OVL1CONF2_BP_##__x)
+#define MCDE_OVL1CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, BP, __x)
+#define MCDE_OVL1CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL1CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL1CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL1CONF2_OPQ_SHIFT 9
+#define MCDE_OVL1CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL1CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, OPQ, __x)
+#define MCDE_OVL1CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL1CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL1CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, PIXOFF, __x)
+#define MCDE_OVL1CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL1CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL1CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL2CONF2 0x00000448
+#define MCDE_OVL2CONF2_BP_SHIFT 0
+#define MCDE_OVL2CONF2_BP_MASK 0x00000001
+#define MCDE_OVL2CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL2CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL2CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, BP, MCDE_OVL2CONF2_BP_##__x)
+#define MCDE_OVL2CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, BP, __x)
+#define MCDE_OVL2CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL2CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL2CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL2CONF2_OPQ_SHIFT 9
+#define MCDE_OVL2CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL2CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, OPQ, __x)
+#define MCDE_OVL2CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL2CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL2CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, PIXOFF, __x)
+#define MCDE_OVL2CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL2CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL2CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL3CONF2 0x00000468
+#define MCDE_OVL3CONF2_BP_SHIFT 0
+#define MCDE_OVL3CONF2_BP_MASK 0x00000001
+#define MCDE_OVL3CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL3CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL3CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, BP, MCDE_OVL3CONF2_BP_##__x)
+#define MCDE_OVL3CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, BP, __x)
+#define MCDE_OVL3CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL3CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL3CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL3CONF2_OPQ_SHIFT 9
+#define MCDE_OVL3CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL3CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, OPQ, __x)
+#define MCDE_OVL3CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL3CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL3CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, PIXOFF, __x)
+#define MCDE_OVL3CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL3CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL3CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL4CONF2 0x00000488
+#define MCDE_OVL4CONF2_BP_SHIFT 0
+#define MCDE_OVL4CONF2_BP_MASK 0x00000001
+#define MCDE_OVL4CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL4CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL4CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, BP, MCDE_OVL4CONF2_BP_##__x)
+#define MCDE_OVL4CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, BP, __x)
+#define MCDE_OVL4CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL4CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL4CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL4CONF2_OPQ_SHIFT 9
+#define MCDE_OVL4CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL4CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, OPQ, __x)
+#define MCDE_OVL4CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL4CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL4CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, PIXOFF, __x)
+#define MCDE_OVL4CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL4CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL4CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL5CONF2 0x000004A8
+#define MCDE_OVL5CONF2_BP_SHIFT 0
+#define MCDE_OVL5CONF2_BP_MASK 0x00000001
+#define MCDE_OVL5CONF2_BP_PER_PIXEL_ALPHA 0
+#define MCDE_OVL5CONF2_BP_CONSTANT_ALPHA 1
+#define MCDE_OVL5CONF2_BP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, BP, MCDE_OVL5CONF2_BP_##__x)
+#define MCDE_OVL5CONF2_BP(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, BP, __x)
+#define MCDE_OVL5CONF2_ALPHAVALUE_SHIFT 1
+#define MCDE_OVL5CONF2_ALPHAVALUE_MASK 0x000001FE
+#define MCDE_OVL5CONF2_ALPHAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, ALPHAVALUE, __x)
+#define MCDE_OVL5CONF2_OPQ_SHIFT 9
+#define MCDE_OVL5CONF2_OPQ_MASK 0x00000200
+#define MCDE_OVL5CONF2_OPQ(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, OPQ, __x)
+#define MCDE_OVL5CONF2_PIXOFF_SHIFT 10
+#define MCDE_OVL5CONF2_PIXOFF_MASK 0x0000FC00
+#define MCDE_OVL5CONF2_PIXOFF(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, PIXOFF, __x)
+#define MCDE_OVL5CONF2_PIXELFETCHERWATERMARKLEVEL_SHIFT 16
+#define MCDE_OVL5CONF2_PIXELFETCHERWATERMARKLEVEL_MASK 0x1FFF0000
+#define MCDE_OVL5CONF2_PIXELFETCHERWATERMARKLEVEL(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CONF2, PIXELFETCHERWATERMARKLEVEL, __x)
+#define MCDE_OVL0LJINC 0x0000040C
+#define MCDE_OVL0LJINC_GROUPOFFSET 0x20
+#define MCDE_OVL0LJINC_LJINC_SHIFT 0
+#define MCDE_OVL0LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL0LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL0LJINC, LJINC, __x)
+#define MCDE_OVL1LJINC 0x0000042C
+#define MCDE_OVL1LJINC_LJINC_SHIFT 0
+#define MCDE_OVL1LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL1LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL1LJINC, LJINC, __x)
+#define MCDE_OVL2LJINC 0x0000044C
+#define MCDE_OVL2LJINC_LJINC_SHIFT 0
+#define MCDE_OVL2LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL2LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL2LJINC, LJINC, __x)
+#define MCDE_OVL3LJINC 0x0000046C
+#define MCDE_OVL3LJINC_LJINC_SHIFT 0
+#define MCDE_OVL3LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL3LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL3LJINC, LJINC, __x)
+#define MCDE_OVL4LJINC 0x0000048C
+#define MCDE_OVL4LJINC_LJINC_SHIFT 0
+#define MCDE_OVL4LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL4LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL4LJINC, LJINC, __x)
+#define MCDE_OVL5LJINC 0x000004AC
+#define MCDE_OVL5LJINC_LJINC_SHIFT 0
+#define MCDE_OVL5LJINC_LJINC_MASK 0xFFFFFFFF
+#define MCDE_OVL5LJINC_LJINC(__x) \
+	MCDE_VAL2REG(MCDE_OVL5LJINC, LJINC, __x)
+#define MCDE_OVL0CROP 0x00000410
+#define MCDE_OVL0CROP_GROUPOFFSET 0x20
+#define MCDE_OVL0CROP_TMRGN_SHIFT 0
+#define MCDE_OVL0CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL0CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CROP, TMRGN, __x)
+#define MCDE_OVL0CROP_LMRGN_SHIFT 22
+#define MCDE_OVL0CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL0CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL0CROP, LMRGN, __x)
+#define MCDE_OVL1CROP 0x00000430
+#define MCDE_OVL1CROP_TMRGN_SHIFT 0
+#define MCDE_OVL1CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL1CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CROP, TMRGN, __x)
+#define MCDE_OVL1CROP_LMRGN_SHIFT 22
+#define MCDE_OVL1CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL1CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL1CROP, LMRGN, __x)
+#define MCDE_OVL2CROP 0x00000450
+#define MCDE_OVL2CROP_TMRGN_SHIFT 0
+#define MCDE_OVL2CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL2CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CROP, TMRGN, __x)
+#define MCDE_OVL2CROP_LMRGN_SHIFT 22
+#define MCDE_OVL2CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL2CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL2CROP, LMRGN, __x)
+#define MCDE_OVL3CROP 0x00000470
+#define MCDE_OVL3CROP_TMRGN_SHIFT 0
+#define MCDE_OVL3CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL3CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CROP, TMRGN, __x)
+#define MCDE_OVL3CROP_LMRGN_SHIFT 22
+#define MCDE_OVL3CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL3CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL3CROP, LMRGN, __x)
+#define MCDE_OVL4CROP 0x00000490
+#define MCDE_OVL4CROP_TMRGN_SHIFT 0
+#define MCDE_OVL4CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL4CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CROP, TMRGN, __x)
+#define MCDE_OVL4CROP_LMRGN_SHIFT 22
+#define MCDE_OVL4CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL4CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL4CROP, LMRGN, __x)
+#define MCDE_OVL5CROP 0x000004B0
+#define MCDE_OVL5CROP_TMRGN_SHIFT 0
+#define MCDE_OVL5CROP_TMRGN_MASK 0x003FFFFF
+#define MCDE_OVL5CROP_TMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CROP, TMRGN, __x)
+#define MCDE_OVL5CROP_LMRGN_SHIFT 22
+#define MCDE_OVL5CROP_LMRGN_MASK 0xFFC00000
+#define MCDE_OVL5CROP_LMRGN(__x) \
+	MCDE_VAL2REG(MCDE_OVL5CROP, LMRGN, __x)
+#define MCDE_OVL0COMP 0x00000414
+#define MCDE_OVL0COMP_GROUPOFFSET 0x20
+#define MCDE_OVL0COMP_XPOS_SHIFT 0
+#define MCDE_OVL0COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL0COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL0COMP, XPOS, __x)
+#define MCDE_OVL0COMP_CH_ID_SHIFT 11
+#define MCDE_OVL0COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL0COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL0COMP, CH_ID, __x)
+#define MCDE_OVL0COMP_YPOS_SHIFT 16
+#define MCDE_OVL0COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL0COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL0COMP, YPOS, __x)
+#define MCDE_OVL0COMP_Z_SHIFT 27
+#define MCDE_OVL0COMP_Z_MASK 0x78000000
+#define MCDE_OVL0COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL0COMP, Z, __x)
+#define MCDE_OVL1COMP 0x00000434
+#define MCDE_OVL1COMP_XPOS_SHIFT 0
+#define MCDE_OVL1COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL1COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL1COMP, XPOS, __x)
+#define MCDE_OVL1COMP_CH_ID_SHIFT 11
+#define MCDE_OVL1COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL1COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL1COMP, CH_ID, __x)
+#define MCDE_OVL1COMP_YPOS_SHIFT 16
+#define MCDE_OVL1COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL1COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL1COMP, YPOS, __x)
+#define MCDE_OVL1COMP_Z_SHIFT 27
+#define MCDE_OVL1COMP_Z_MASK 0x78000000
+#define MCDE_OVL1COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL1COMP, Z, __x)
+#define MCDE_OVL2COMP 0x00000454
+#define MCDE_OVL2COMP_XPOS_SHIFT 0
+#define MCDE_OVL2COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL2COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL2COMP, XPOS, __x)
+#define MCDE_OVL2COMP_CH_ID_SHIFT 11
+#define MCDE_OVL2COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL2COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL2COMP, CH_ID, __x)
+#define MCDE_OVL2COMP_YPOS_SHIFT 16
+#define MCDE_OVL2COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL2COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL2COMP, YPOS, __x)
+#define MCDE_OVL2COMP_Z_SHIFT 27
+#define MCDE_OVL2COMP_Z_MASK 0x78000000
+#define MCDE_OVL2COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL2COMP, Z, __x)
+#define MCDE_OVL3COMP 0x00000474
+#define MCDE_OVL3COMP_XPOS_SHIFT 0
+#define MCDE_OVL3COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL3COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL3COMP, XPOS, __x)
+#define MCDE_OVL3COMP_CH_ID_SHIFT 11
+#define MCDE_OVL3COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL3COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL3COMP, CH_ID, __x)
+#define MCDE_OVL3COMP_YPOS_SHIFT 16
+#define MCDE_OVL3COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL3COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL3COMP, YPOS, __x)
+#define MCDE_OVL3COMP_Z_SHIFT 27
+#define MCDE_OVL3COMP_Z_MASK 0x78000000
+#define MCDE_OVL3COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL3COMP, Z, __x)
+#define MCDE_OVL4COMP 0x00000494
+#define MCDE_OVL4COMP_XPOS_SHIFT 0
+#define MCDE_OVL4COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL4COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL4COMP, XPOS, __x)
+#define MCDE_OVL4COMP_CH_ID_SHIFT 11
+#define MCDE_OVL4COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL4COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL4COMP, CH_ID, __x)
+#define MCDE_OVL4COMP_YPOS_SHIFT 16
+#define MCDE_OVL4COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL4COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL4COMP, YPOS, __x)
+#define MCDE_OVL4COMP_Z_SHIFT 27
+#define MCDE_OVL4COMP_Z_MASK 0x78000000
+#define MCDE_OVL4COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL4COMP, Z, __x)
+#define MCDE_OVL5COMP 0x000004B4
+#define MCDE_OVL5COMP_XPOS_SHIFT 0
+#define MCDE_OVL5COMP_XPOS_MASK 0x000007FF
+#define MCDE_OVL5COMP_XPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL5COMP, XPOS, __x)
+#define MCDE_OVL5COMP_CH_ID_SHIFT 11
+#define MCDE_OVL5COMP_CH_ID_MASK 0x00007800
+#define MCDE_OVL5COMP_CH_ID(__x) \
+	MCDE_VAL2REG(MCDE_OVL5COMP, CH_ID, __x)
+#define MCDE_OVL5COMP_YPOS_SHIFT 16
+#define MCDE_OVL5COMP_YPOS_MASK 0x07FF0000
+#define MCDE_OVL5COMP_YPOS(__x) \
+	MCDE_VAL2REG(MCDE_OVL5COMP, YPOS, __x)
+#define MCDE_OVL5COMP_Z_SHIFT 27
+#define MCDE_OVL5COMP_Z_MASK 0x78000000
+#define MCDE_OVL5COMP_Z(__x) \
+	MCDE_VAL2REG(MCDE_OVL5COMP, Z, __x)
+#define MCDE_CHNL0CONF 0x00000600
+#define MCDE_CHNL0CONF_GROUPOFFSET 0x20
+#define MCDE_CHNL0CONF_PPL_SHIFT 0
+#define MCDE_CHNL0CONF_PPL_MASK 0x000007FF
+#define MCDE_CHNL0CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0CONF, PPL, __x)
+#define MCDE_CHNL0CONF_LPF_SHIFT 16
+#define MCDE_CHNL0CONF_LPF_MASK 0x07FF0000
+#define MCDE_CHNL0CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0CONF, LPF, __x)
+#define MCDE_CHNL1CONF 0x00000620
+#define MCDE_CHNL1CONF_PPL_SHIFT 0
+#define MCDE_CHNL1CONF_PPL_MASK 0x000007FF
+#define MCDE_CHNL1CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1CONF, PPL, __x)
+#define MCDE_CHNL1CONF_LPF_SHIFT 16
+#define MCDE_CHNL1CONF_LPF_MASK 0x07FF0000
+#define MCDE_CHNL1CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1CONF, LPF, __x)
+#define MCDE_CHNL2CONF 0x00000640
+#define MCDE_CHNL2CONF_PPL_SHIFT 0
+#define MCDE_CHNL2CONF_PPL_MASK 0x000007FF
+#define MCDE_CHNL2CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2CONF, PPL, __x)
+#define MCDE_CHNL2CONF_LPF_SHIFT 16
+#define MCDE_CHNL2CONF_LPF_MASK 0x07FF0000
+#define MCDE_CHNL2CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2CONF, LPF, __x)
+#define MCDE_CHNL3CONF 0x00000660
+#define MCDE_CHNL3CONF_PPL_SHIFT 0
+#define MCDE_CHNL3CONF_PPL_MASK 0x000007FF
+#define MCDE_CHNL3CONF_PPL(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3CONF, PPL, __x)
+#define MCDE_CHNL3CONF_LPF_SHIFT 16
+#define MCDE_CHNL3CONF_LPF_MASK 0x07FF0000
+#define MCDE_CHNL3CONF_LPF(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3CONF, LPF, __x)
+#define MCDE_CHNL0STAT 0x00000604
+#define MCDE_CHNL0STAT_GROUPOFFSET 0x20
+#define MCDE_CHNL0STAT_CHNLRD_SHIFT 0
+#define MCDE_CHNL0STAT_CHNLRD_MASK 0x00000001
+#define MCDE_CHNL0STAT_CHNLRD(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0STAT, CHNLRD, __x)
+#define MCDE_CHNL0STAT_CHNLA_SHIFT 1
+#define MCDE_CHNL0STAT_CHNLA_MASK 0x00000002
+#define MCDE_CHNL0STAT_CHNLA(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0STAT, CHNLA, __x)
+#define MCDE_CHNL0STAT_CHNLBLBCKGND_EN_SHIFT 16
+#define MCDE_CHNL0STAT_CHNLBLBCKGND_EN_MASK 0x00010000
+#define MCDE_CHNL0STAT_CHNLBLBCKGND_EN(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0STAT, CHNLBLBCKGND_EN, __x)
+#define MCDE_CHNL1STAT 0x00000624
+#define MCDE_CHNL1STAT_CHNLRD_SHIFT 0
+#define MCDE_CHNL1STAT_CHNLRD_MASK 0x00000001
+#define MCDE_CHNL1STAT_CHNLRD(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1STAT, CHNLRD, __x)
+#define MCDE_CHNL1STAT_CHNLA_SHIFT 1
+#define MCDE_CHNL1STAT_CHNLA_MASK 0x00000002
+#define MCDE_CHNL1STAT_CHNLA(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1STAT, CHNLA, __x)
+#define MCDE_CHNL1STAT_CHNLBLBCKGND_EN_SHIFT 16
+#define MCDE_CHNL1STAT_CHNLBLBCKGND_EN_MASK 0x00010000
+#define MCDE_CHNL1STAT_CHNLBLBCKGND_EN(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1STAT, CHNLBLBCKGND_EN, __x)
+#define MCDE_CHNL2STAT 0x00000644
+#define MCDE_CHNL2STAT_CHNLRD_SHIFT 0
+#define MCDE_CHNL2STAT_CHNLRD_MASK 0x00000001
+#define MCDE_CHNL2STAT_CHNLRD(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2STAT, CHNLRD, __x)
+#define MCDE_CHNL2STAT_CHNLA_SHIFT 1
+#define MCDE_CHNL2STAT_CHNLA_MASK 0x00000002
+#define MCDE_CHNL2STAT_CHNLA(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2STAT, CHNLA, __x)
+#define MCDE_CHNL2STAT_CHNLBLBCKGND_EN_SHIFT 16
+#define MCDE_CHNL2STAT_CHNLBLBCKGND_EN_MASK 0x00010000
+#define MCDE_CHNL2STAT_CHNLBLBCKGND_EN(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2STAT, CHNLBLBCKGND_EN, __x)
+#define MCDE_CHNL3STAT 0x00000664
+#define MCDE_CHNL3STAT_CHNLRD_SHIFT 0
+#define MCDE_CHNL3STAT_CHNLRD_MASK 0x00000001
+#define MCDE_CHNL3STAT_CHNLRD(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3STAT, CHNLRD, __x)
+#define MCDE_CHNL3STAT_CHNLA_SHIFT 1
+#define MCDE_CHNL3STAT_CHNLA_MASK 0x00000002
+#define MCDE_CHNL3STAT_CHNLA(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3STAT, CHNLA, __x)
+#define MCDE_CHNL3STAT_CHNLBLBCKGND_EN_SHIFT 16
+#define MCDE_CHNL3STAT_CHNLBLBCKGND_EN_MASK 0x00010000
+#define MCDE_CHNL3STAT_CHNLBLBCKGND_EN(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3STAT, CHNLBLBCKGND_EN, __x)
+#define MCDE_CHNL0SYNCHMOD 0x00000608
+#define MCDE_CHNL0SYNCHMOD_GROUPOFFSET 0x20
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SHIFT 0
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_MASK 0x00000003
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_HARDWARE 0
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_NO_SYNCH 1
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_SOFTWARE 2
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHMOD, SRC_SYNCH, \
+	MCDE_CHNL0SYNCHMOD_SRC_SYNCH_##__x)
+#define MCDE_CHNL0SYNCHMOD_SRC_SYNCH(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHMOD, SRC_SYNCH, __x)
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_SHIFT 2
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_MASK 0x0000001C
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_FORMATTER 0
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_TE0 1
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_TE1 2
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHMOD, OUT_SYNCH_SRC, \
+	MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC_##__x)
+#define MCDE_CHNL0SYNCHMOD_OUT_SYNCH_SRC(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHMOD, OUT_SYNCH_SRC, __x)
+#define MCDE_CHNL1SYNCHMOD 0x00000628
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_SHIFT 0
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_MASK 0x00000003
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_HARDWARE 0
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_NO_SYNCH 1
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_SOFTWARE 2
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHMOD, SRC_SYNCH, \
+	MCDE_CHNL1SYNCHMOD_SRC_SYNCH_##__x)
+#define MCDE_CHNL1SYNCHMOD_SRC_SYNCH(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHMOD, SRC_SYNCH, __x)
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_SHIFT 2
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_MASK 0x0000001C
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_FORMATTER 0
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_TE0 1
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_TE1 2
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHMOD, OUT_SYNCH_SRC, \
+	MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC_##__x)
+#define MCDE_CHNL1SYNCHMOD_OUT_SYNCH_SRC(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHMOD, OUT_SYNCH_SRC, __x)
+#define MCDE_CHNL2SYNCHMOD 0x00000648
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_SHIFT 0
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_MASK 0x00000003
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_HARDWARE 0
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_NO_SYNCH 1
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_SOFTWARE 2
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHMOD, SRC_SYNCH, \
+	MCDE_CHNL2SYNCHMOD_SRC_SYNCH_##__x)
+#define MCDE_CHNL2SYNCHMOD_SRC_SYNCH(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHMOD, SRC_SYNCH, __x)
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_SHIFT 2
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_MASK 0x0000001C
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_FORMATTER 0
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_TE0 1
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_TE1 2
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHMOD, OUT_SYNCH_SRC, \
+	MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC_##__x)
+#define MCDE_CHNL2SYNCHMOD_OUT_SYNCH_SRC(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHMOD, OUT_SYNCH_SRC, __x)
+#define MCDE_CHNL3SYNCHMOD 0x00000668
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_SHIFT 0
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_MASK 0x00000003
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_HARDWARE 0
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_NO_SYNCH 1
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_SOFTWARE 2
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHMOD, SRC_SYNCH, \
+	MCDE_CHNL3SYNCHMOD_SRC_SYNCH_##__x)
+#define MCDE_CHNL3SYNCHMOD_SRC_SYNCH(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHMOD, SRC_SYNCH, __x)
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_SHIFT 2
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_MASK 0x0000001C
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_FORMATTER 0
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_TE0 1
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_TE1 2
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHMOD, OUT_SYNCH_SRC, \
+	MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC_##__x)
+#define MCDE_CHNL3SYNCHMOD_OUT_SYNCH_SRC(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHMOD, OUT_SYNCH_SRC, __x)
+#define MCDE_CHNL0SYNCHSW 0x0000060C
+#define MCDE_CHNL0SYNCHSW_GROUPOFFSET 0x20
+#define MCDE_CHNL0SYNCHSW_SW_TRIG_SHIFT 0
+#define MCDE_CHNL0SYNCHSW_SW_TRIG_MASK 0x00000001
+#define MCDE_CHNL0SYNCHSW_SW_TRIG(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0SYNCHSW, SW_TRIG, __x)
+#define MCDE_CHNL1SYNCHSW 0x0000062C
+#define MCDE_CHNL1SYNCHSW_SW_TRIG_SHIFT 0
+#define MCDE_CHNL1SYNCHSW_SW_TRIG_MASK 0x00000001
+#define MCDE_CHNL1SYNCHSW_SW_TRIG(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1SYNCHSW, SW_TRIG, __x)
+#define MCDE_CHNL2SYNCHSW 0x0000064C
+#define MCDE_CHNL2SYNCHSW_SW_TRIG_SHIFT 0
+#define MCDE_CHNL2SYNCHSW_SW_TRIG_MASK 0x00000001
+#define MCDE_CHNL2SYNCHSW_SW_TRIG(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2SYNCHSW, SW_TRIG, __x)
+#define MCDE_CHNL3SYNCHSW 0x0000066C
+#define MCDE_CHNL3SYNCHSW_SW_TRIG_SHIFT 0
+#define MCDE_CHNL3SYNCHSW_SW_TRIG_MASK 0x00000001
+#define MCDE_CHNL3SYNCHSW_SW_TRIG(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3SYNCHSW, SW_TRIG, __x)
+#define MCDE_CHNL0BCKGNDCOL 0x00000610
+#define MCDE_CHNL0BCKGNDCOL_GROUPOFFSET 0x20
+#define MCDE_CHNL0BCKGNDCOL_B_SHIFT 0
+#define MCDE_CHNL0BCKGNDCOL_B_MASK 0x000000FF
+#define MCDE_CHNL0BCKGNDCOL_B(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0BCKGNDCOL, B, __x)
+#define MCDE_CHNL0BCKGNDCOL_G_SHIFT 8
+#define MCDE_CHNL0BCKGNDCOL_G_MASK 0x0000FF00
+#define MCDE_CHNL0BCKGNDCOL_G(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0BCKGNDCOL, G, __x)
+#define MCDE_CHNL0BCKGNDCOL_R_SHIFT 16
+#define MCDE_CHNL0BCKGNDCOL_R_MASK 0x00FF0000
+#define MCDE_CHNL0BCKGNDCOL_R(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0BCKGNDCOL, R, __x)
+#define MCDE_CHNL1BCKGNDCOL 0x00000630
+#define MCDE_CHNL1BCKGNDCOL_B_SHIFT 0
+#define MCDE_CHNL1BCKGNDCOL_B_MASK 0x000000FF
+#define MCDE_CHNL1BCKGNDCOL_B(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1BCKGNDCOL, B, __x)
+#define MCDE_CHNL1BCKGNDCOL_G_SHIFT 8
+#define MCDE_CHNL1BCKGNDCOL_G_MASK 0x0000FF00
+#define MCDE_CHNL1BCKGNDCOL_G(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1BCKGNDCOL, G, __x)
+#define MCDE_CHNL1BCKGNDCOL_R_SHIFT 16
+#define MCDE_CHNL1BCKGNDCOL_R_MASK 0x00FF0000
+#define MCDE_CHNL1BCKGNDCOL_R(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1BCKGNDCOL, R, __x)
+#define MCDE_CHNL2BCKGNDCOL 0x00000650
+#define MCDE_CHNL2BCKGNDCOL_B_SHIFT 0
+#define MCDE_CHNL2BCKGNDCOL_B_MASK 0x000000FF
+#define MCDE_CHNL2BCKGNDCOL_B(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2BCKGNDCOL, B, __x)
+#define MCDE_CHNL2BCKGNDCOL_G_SHIFT 8
+#define MCDE_CHNL2BCKGNDCOL_G_MASK 0x0000FF00
+#define MCDE_CHNL2BCKGNDCOL_G(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2BCKGNDCOL, G, __x)
+#define MCDE_CHNL2BCKGNDCOL_R_SHIFT 16
+#define MCDE_CHNL2BCKGNDCOL_R_MASK 0x00FF0000
+#define MCDE_CHNL2BCKGNDCOL_R(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2BCKGNDCOL, R, __x)
+#define MCDE_CHNL3BCKGNDCOL 0x00000670
+#define MCDE_CHNL3BCKGNDCOL_B_SHIFT 0
+#define MCDE_CHNL3BCKGNDCOL_B_MASK 0x000000FF
+#define MCDE_CHNL3BCKGNDCOL_B(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3BCKGNDCOL, B, __x)
+#define MCDE_CHNL3BCKGNDCOL_G_SHIFT 8
+#define MCDE_CHNL3BCKGNDCOL_G_MASK 0x0000FF00
+#define MCDE_CHNL3BCKGNDCOL_G(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3BCKGNDCOL, G, __x)
+#define MCDE_CHNL3BCKGNDCOL_R_SHIFT 16
+#define MCDE_CHNL3BCKGNDCOL_R_MASK 0x00FF0000
+#define MCDE_CHNL3BCKGNDCOL_R(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3BCKGNDCOL, R, __x)
+#define MCDE_CHNL0MUXING 0x00000614
+#define MCDE_CHNL0MUXING_GROUPOFFSET 0x20
+#define MCDE_CHNL0MUXING_FIFO_ID_SHIFT 0
+#define MCDE_CHNL0MUXING_FIFO_ID_MASK 0x00000007
+#define MCDE_CHNL0MUXING_FIFO_ID_FIFO_A 0
+#define MCDE_CHNL0MUXING_FIFO_ID_FIFO_B 1
+#define MCDE_CHNL0MUXING_FIFO_ID_FIFO_C0 2
+#define MCDE_CHNL0MUXING_FIFO_ID_FIFO_C1 3
+#define MCDE_CHNL0MUXING_FIFO_ID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0MUXING, FIFO_ID, MCDE_CHNL0MUXING_FIFO_ID_##__x)
+#define MCDE_CHNL0MUXING_FIFO_ID(__x) \
+	MCDE_VAL2REG(MCDE_CHNL0MUXING, FIFO_ID, __x)
+#define MCDE_CHNL1MUXING 0x00000634
+#define MCDE_CHNL1MUXING_FIFO_ID_SHIFT 0
+#define MCDE_CHNL1MUXING_FIFO_ID_MASK 0x00000007
+#define MCDE_CHNL1MUXING_FIFO_ID_FIFO_A 0
+#define MCDE_CHNL1MUXING_FIFO_ID_FIFO_B 1
+#define MCDE_CHNL1MUXING_FIFO_ID_FIFO_C0 2
+#define MCDE_CHNL1MUXING_FIFO_ID_FIFO_C1 3
+#define MCDE_CHNL1MUXING_FIFO_ID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1MUXING, FIFO_ID, MCDE_CHNL1MUXING_FIFO_ID_##__x)
+#define MCDE_CHNL1MUXING_FIFO_ID(__x) \
+	MCDE_VAL2REG(MCDE_CHNL1MUXING, FIFO_ID, __x)
+#define MCDE_CHNL2MUXING 0x00000654
+#define MCDE_CHNL2MUXING_FIFO_ID_SHIFT 0
+#define MCDE_CHNL2MUXING_FIFO_ID_MASK 0x00000007
+#define MCDE_CHNL2MUXING_FIFO_ID_FIFO_A 0
+#define MCDE_CHNL2MUXING_FIFO_ID_FIFO_B 1
+#define MCDE_CHNL2MUXING_FIFO_ID_FIFO_C0 2
+#define MCDE_CHNL2MUXING_FIFO_ID_FIFO_C1 3
+#define MCDE_CHNL2MUXING_FIFO_ID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2MUXING, FIFO_ID, MCDE_CHNL2MUXING_FIFO_ID_##__x)
+#define MCDE_CHNL2MUXING_FIFO_ID(__x) \
+	MCDE_VAL2REG(MCDE_CHNL2MUXING, FIFO_ID, __x)
+#define MCDE_CHNL3MUXING 0x00000674
+#define MCDE_CHNL3MUXING_FIFO_ID_SHIFT 0
+#define MCDE_CHNL3MUXING_FIFO_ID_MASK 0x00000007
+#define MCDE_CHNL3MUXING_FIFO_ID_FIFO_A 0
+#define MCDE_CHNL3MUXING_FIFO_ID_FIFO_B 1
+#define MCDE_CHNL3MUXING_FIFO_ID_FIFO_C0 2
+#define MCDE_CHNL3MUXING_FIFO_ID_FIFO_C1 3
+#define MCDE_CHNL3MUXING_FIFO_ID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3MUXING, FIFO_ID, MCDE_CHNL3MUXING_FIFO_ID_##__x)
+#define MCDE_CHNL3MUXING_FIFO_ID(__x) \
+	MCDE_VAL2REG(MCDE_CHNL3MUXING, FIFO_ID, __x)
+#define MCDE_CRA0 0x00000800
+#define MCDE_CRA0_GROUPOFFSET 0x200
+#define MCDE_CRA0_FLOEN_SHIFT 0
+#define MCDE_CRA0_FLOEN_MASK 0x00000001
+#define MCDE_CRA0_FLOEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLOEN, __x)
+#define MCDE_CRA0_BLENDEN_SHIFT 2
+#define MCDE_CRA0_BLENDEN_MASK 0x00000004
+#define MCDE_CRA0_BLENDEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, BLENDEN, __x)
+#define MCDE_CRA0_AFLICKEN_SHIFT 3
+#define MCDE_CRA0_AFLICKEN_MASK 0x00000008
+#define MCDE_CRA0_AFLICKEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, AFLICKEN, __x)
+#define MCDE_CRA0_PALEN_SHIFT 4
+#define MCDE_CRA0_PALEN_MASK 0x00000010
+#define MCDE_CRA0_PALEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, PALEN, __x)
+#define MCDE_CRA0_DITHEN_SHIFT 5
+#define MCDE_CRA0_DITHEN_MASK 0x00000020
+#define MCDE_CRA0_DITHEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, DITHEN, __x)
+#define MCDE_CRA0_GAMEN_SHIFT 6
+#define MCDE_CRA0_GAMEN_MASK 0x00000040
+#define MCDE_CRA0_GAMEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, GAMEN, __x)
+#define MCDE_CRA0_KEYCTRL_SHIFT 7
+#define MCDE_CRA0_KEYCTRL_MASK 0x00000380
+#define MCDE_CRA0_KEYCTRL_OFF 0
+#define MCDE_CRA0_KEYCTRL_ALPHA_RGB 1
+#define MCDE_CRA0_KEYCTRL_RGB 2
+#define MCDE_CRA0_KEYCTRL_FALPHA_FRGB 4
+#define MCDE_CRA0_KEYCTRL_FRGB 5
+#define MCDE_CRA0_KEYCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, KEYCTRL, MCDE_CRA0_KEYCTRL_##__x)
+#define MCDE_CRA0_KEYCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, KEYCTRL, __x)
+#define MCDE_CRA0_BLENDCTRL_SHIFT 10
+#define MCDE_CRA0_BLENDCTRL_MASK 0x00000400
+#define MCDE_CRA0_BLENDCTRL_SOURCE 0
+#define MCDE_CRA0_BLENDCTRL_CONSTANT 1
+#define MCDE_CRA0_BLENDCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, BLENDCTRL, MCDE_CRA0_BLENDCTRL_##__x)
+#define MCDE_CRA0_BLENDCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, BLENDCTRL, __x)
+#define MCDE_CRA0_FLICKMODE_SHIFT 11
+#define MCDE_CRA0_FLICKMODE_MASK 0x00001800
+#define MCDE_CRA0_FLICKMODE_FORCE_FILTER_0 0
+#define MCDE_CRA0_FLICKMODE_ADAPTIVE 1
+#define MCDE_CRA0_FLICKMODE_TEST_MODE 2
+#define MCDE_CRA0_FLICKMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLICKMODE, MCDE_CRA0_FLICKMODE_##__x)
+#define MCDE_CRA0_FLICKMODE(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLICKMODE, __x)
+#define MCDE_CRA0_FLOCKFORMAT_SHIFT 13
+#define MCDE_CRA0_FLOCKFORMAT_MASK 0x00002000
+#define MCDE_CRA0_FLOCKFORMAT_YCBCR 0
+#define MCDE_CRA0_FLOCKFORMAT_RGB 1
+#define MCDE_CRA0_FLOCKFORMAT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLOCKFORMAT, MCDE_CRA0_FLOCKFORMAT_##__x)
+#define MCDE_CRA0_FLOCKFORMAT(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, FLOCKFORMAT, __x)
+#define MCDE_CRA0_PALMODE_SHIFT 14
+#define MCDE_CRA0_PALMODE_MASK 0x00004000
+#define MCDE_CRA0_PALMODE_PALETTE 0
+#define MCDE_CRA0_PALMODE_GAMMA 1
+#define MCDE_CRA0_PALMODE(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, PALMODE, __x)
+#define MCDE_CRA0_OLEDEN_SHIFT 15
+#define MCDE_CRA0_OLEDEN_MASK 0x00008000
+#define MCDE_CRA0_OLEDEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, OLEDEN, __x)
+#define MCDE_CRA0_ALPHABLEND_SHIFT 16
+#define MCDE_CRA0_ALPHABLEND_MASK 0x00FF0000
+#define MCDE_CRA0_ALPHABLEND(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, ALPHABLEND, __x)
+#define MCDE_CRA0_ROTEN_SHIFT 24
+#define MCDE_CRA0_ROTEN_MASK 0x01000000
+#define MCDE_CRA0_ROTEN(__x) \
+	MCDE_VAL2REG(MCDE_CRA0, ROTEN, __x)
+#define MCDE_CRB0 0x00000A00
+#define MCDE_CRB0_FLOEN_SHIFT 0
+#define MCDE_CRB0_FLOEN_MASK 0x00000001
+#define MCDE_CRB0_FLOEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLOEN, __x)
+#define MCDE_CRB0_BLENDEN_SHIFT 2
+#define MCDE_CRB0_BLENDEN_MASK 0x00000004
+#define MCDE_CRB0_BLENDEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, BLENDEN, __x)
+#define MCDE_CRB0_AFLICKEN_SHIFT 3
+#define MCDE_CRB0_AFLICKEN_MASK 0x00000008
+#define MCDE_CRB0_AFLICKEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, AFLICKEN, __x)
+#define MCDE_CRB0_PALEN_SHIFT 4
+#define MCDE_CRB0_PALEN_MASK 0x00000010
+#define MCDE_CRB0_PALEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, PALEN, __x)
+#define MCDE_CRB0_DITHEN_SHIFT 5
+#define MCDE_CRB0_DITHEN_MASK 0x00000020
+#define MCDE_CRB0_DITHEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, DITHEN, __x)
+#define MCDE_CRB0_GAMEN_SHIFT 6
+#define MCDE_CRB0_GAMEN_MASK 0x00000040
+#define MCDE_CRB0_GAMEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, GAMEN, __x)
+#define MCDE_CRB0_KEYCTRL_SHIFT 7
+#define MCDE_CRB0_KEYCTRL_MASK 0x00000380
+#define MCDE_CRB0_KEYCTRL_OFF 0
+#define MCDE_CRB0_KEYCTRL_ALPHA_RGB 1
+#define MCDE_CRB0_KEYCTRL_RGB 2
+#define MCDE_CRB0_KEYCTRL_FALPHA_FRGB 4
+#define MCDE_CRB0_KEYCTRL_FRGB 5
+#define MCDE_CRB0_KEYCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, KEYCTRL, MCDE_CRB0_KEYCTRL_##__x)
+#define MCDE_CRB0_KEYCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, KEYCTRL, __x)
+#define MCDE_CRB0_BLENDCTRL_SHIFT 10
+#define MCDE_CRB0_BLENDCTRL_MASK 0x00000400
+#define MCDE_CRB0_BLENDCTRL_SOURCE 0
+#define MCDE_CRB0_BLENDCTRL_CONSTANT 1
+#define MCDE_CRB0_BLENDCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, BLENDCTRL, MCDE_CRB0_BLENDCTRL_##__x)
+#define MCDE_CRB0_BLENDCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, BLENDCTRL, __x)
+#define MCDE_CRB0_FLICKMODE_SHIFT 11
+#define MCDE_CRB0_FLICKMODE_MASK 0x00001800
+#define MCDE_CRB0_FLICKMODE_FORCE_FILTER_0 0
+#define MCDE_CRB0_FLICKMODE_ADAPTIVE 1
+#define MCDE_CRB0_FLICKMODE_TEST_MODE 2
+#define MCDE_CRB0_FLICKMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLICKMODE, MCDE_CRB0_FLICKMODE_##__x)
+#define MCDE_CRB0_FLICKMODE(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLICKMODE, __x)
+#define MCDE_CRB0_FLOCKFORMAT_SHIFT 13
+#define MCDE_CRB0_FLOCKFORMAT_MASK 0x00002000
+#define MCDE_CRB0_FLOCKFORMAT_YCBCR 0
+#define MCDE_CRB0_FLOCKFORMAT_RGB 1
+#define MCDE_CRB0_FLOCKFORMAT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLOCKFORMAT, MCDE_CRB0_FLOCKFORMAT_##__x)
+#define MCDE_CRB0_FLOCKFORMAT(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, FLOCKFORMAT, __x)
+#define MCDE_CRB0_PALMODE_SHIFT 14
+#define MCDE_CRB0_PALMODE_MASK 0x00004000
+#define MCDE_CRB0_PALMODE_PALETTE 0
+#define MCDE_CRB0_PALMODE_GAMMA 1
+#define MCDE_CRB0_PALMODE(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, PALMODE, __x)
+#define MCDE_CRB0_OLEDEN_SHIFT 15
+#define MCDE_CRB0_OLEDEN_MASK 0x00008000
+#define MCDE_CRB0_OLEDEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, OLEDEN, __x)
+#define MCDE_CRB0_ALPHABLEND_SHIFT 16
+#define MCDE_CRB0_ALPHABLEND_MASK 0x00FF0000
+#define MCDE_CRB0_ALPHABLEND(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, ALPHABLEND, __x)
+#define MCDE_CRB0_ROTEN_SHIFT 24
+#define MCDE_CRB0_ROTEN_MASK 0x01000000
+#define MCDE_CRB0_ROTEN(__x) \
+	MCDE_VAL2REG(MCDE_CRB0, ROTEN, __x)
+#define MCDE_CRA1 0x00000804
+#define MCDE_CRA1_GROUPOFFSET 0x200
+#define MCDE_CRA1_PCD_SHIFT 0
+#define MCDE_CRA1_PCD_MASK 0x000003FF
+#define MCDE_CRA1_PCD(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, PCD, __x)
+#define MCDE_CRA1_CLKSEL_SHIFT 10
+#define MCDE_CRA1_CLKSEL_MASK 0x00001C00
+#define MCDE_CRA1_CLKSEL_CLKPLL72 0
+#define MCDE_CRA1_CLKSEL_CLKPLL27 2
+#define MCDE_CRA1_CLKSEL_TV1CLK 3
+#define MCDE_CRA1_CLKSEL_TV2CLK 4
+#define MCDE_CRA1_CLKSEL_MCDECLK 5
+#define MCDE_CRA1_CLKSEL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CLKSEL, MCDE_CRA1_CLKSEL_##__x)
+#define MCDE_CRA1_CLKSEL(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CLKSEL, __x)
+#define MCDE_CRA1_CDWIN_SHIFT 13
+#define MCDE_CRA1_CDWIN_MASK 0x0001E000
+#define MCDE_CRA1_CDWIN_8BPP_C1 0
+#define MCDE_CRA1_CDWIN_12BPP_C1 1
+#define MCDE_CRA1_CDWIN_12BPP_C2 2
+#define MCDE_CRA1_CDWIN_16BPP_C1 3
+#define MCDE_CRA1_CDWIN_16BPP_C2 4
+#define MCDE_CRA1_CDWIN_16BPP_C3 5
+#define MCDE_CRA1_CDWIN_18BPP_C1 6
+#define MCDE_CRA1_CDWIN_18BPP_C2 7
+#define MCDE_CRA1_CDWIN_24BPP 8
+#define MCDE_CRA1_CDWIN_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CDWIN, MCDE_CRA1_CDWIN_##__x)
+#define MCDE_CRA1_CDWIN(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CDWIN, __x)
+#define MCDE_CRA1_OUTBPP_SHIFT 25
+#define MCDE_CRA1_OUTBPP_MASK 0x1E000000
+#define MCDE_CRA1_OUTBPP_MONO1 0
+#define MCDE_CRA1_OUTBPP_MONO2 1
+#define MCDE_CRA1_OUTBPP_MONO4 2
+#define MCDE_CRA1_OUTBPP_MONO8 3
+#define MCDE_CRA1_OUTBPP_8BPP 4
+#define MCDE_CRA1_OUTBPP_12BPP 5
+#define MCDE_CRA1_OUTBPP_15BPP 6
+#define MCDE_CRA1_OUTBPP_16BPP 7
+#define MCDE_CRA1_OUTBPP_18BPP 8
+#define MCDE_CRA1_OUTBPP_24BPP 9
+#define MCDE_CRA1_OUTBPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, OUTBPP, MCDE_CRA1_OUTBPP_##__x)
+#define MCDE_CRA1_OUTBPP(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, OUTBPP, __x)
+#define MCDE_CRA1_BCD_SHIFT 29
+#define MCDE_CRA1_BCD_MASK 0x20000000
+#define MCDE_CRA1_BCD(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, BCD, __x)
+#define MCDE_CRA1_CLKTYPE_SHIFT 30
+#define MCDE_CRA1_CLKTYPE_MASK 0x40000000
+#define MCDE_CRA1_CLKTYPE_TVXCLKSEL0 0
+#define MCDE_CRA1_CLKTYPE_TVXCLKSEL1 1
+#define MCDE_CRA1_CLKTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CLKTYPE, MCDE_CRA1_CLKTYPE_##__x)
+#define MCDE_CRA1_CLKTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CRA1, CLKTYPE, __x)
+#define MCDE_CRB1 0x00000A04
+#define MCDE_CRB1_PCD_SHIFT 0
+#define MCDE_CRB1_PCD_MASK 0x000003FF
+#define MCDE_CRB1_PCD(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, PCD, __x)
+#define MCDE_CRB1_CLKSEL_SHIFT 10
+#define MCDE_CRB1_CLKSEL_MASK 0x00001C00
+#define MCDE_CRB1_CLKSEL_CLKPLL72 0
+#define MCDE_CRB1_CLKSEL_CLKPLL27 2
+#define MCDE_CRB1_CLKSEL_TV1CLK 3
+#define MCDE_CRB1_CLKSEL_TV2CLK 4
+#define MCDE_CRB1_CLKSEL_MCDECLK 5
+#define MCDE_CRB1_CLKSEL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CLKSEL, MCDE_CRB1_CLKSEL_##__x)
+#define MCDE_CRB1_CLKSEL(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CLKSEL, __x)
+#define MCDE_CRB1_CDWIN_SHIFT 13
+#define MCDE_CRB1_CDWIN_MASK 0x0001E000
+#define MCDE_CRB1_CDWIN_8BPP_C1 0
+#define MCDE_CRB1_CDWIN_12BPP_C1 1
+#define MCDE_CRB1_CDWIN_12BPP_C2 2
+#define MCDE_CRB1_CDWIN_16BPP_C1 3
+#define MCDE_CRB1_CDWIN_16BPP_C2 4
+#define MCDE_CRB1_CDWIN_16BPP_C3 5
+#define MCDE_CRB1_CDWIN_18BPP_C1 6
+#define MCDE_CRB1_CDWIN_18BPP_C2 7
+#define MCDE_CRB1_CDWIN_24BPP 8
+#define MCDE_CRB1_CDWIN_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CDWIN, MCDE_CRB1_CDWIN_##__x)
+#define MCDE_CRB1_CDWIN(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CDWIN, __x)
+#define MCDE_CRB1_OUTBPP_SHIFT 25
+#define MCDE_CRB1_OUTBPP_MASK 0x1E000000
+#define MCDE_CRB1_OUTBPP_MONO1 0
+#define MCDE_CRB1_OUTBPP_MONO2 1
+#define MCDE_CRB1_OUTBPP_MONO4 2
+#define MCDE_CRB1_OUTBPP_MONO8 3
+#define MCDE_CRB1_OUTBPP_8BPP 4
+#define MCDE_CRB1_OUTBPP_12BPP 5
+#define MCDE_CRB1_OUTBPP_15BPP 6
+#define MCDE_CRB1_OUTBPP_16BPP 7
+#define MCDE_CRB1_OUTBPP_18BPP 8
+#define MCDE_CRB1_OUTBPP_24BPP 9
+#define MCDE_CRB1_OUTBPP_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, OUTBPP, MCDE_CRB1_OUTBPP_##__x)
+#define MCDE_CRB1_OUTBPP(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, OUTBPP, __x)
+#define MCDE_CRB1_BCD_SHIFT 29
+#define MCDE_CRB1_BCD_MASK 0x20000000
+#define MCDE_CRB1_BCD(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, BCD, __x)
+#define MCDE_CRB1_CLKTYPE_SHIFT 30
+#define MCDE_CRB1_CLKTYPE_MASK 0x40000000
+#define MCDE_CRB1_CLKTYPE_TVXCLKSEL0 0
+#define MCDE_CRB1_CLKTYPE_TVXCLKSEL1 1
+#define MCDE_CRB1_CLKTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CLKTYPE, MCDE_CRB1_CLKTYPE_##__x)
+#define MCDE_CRB1_CLKTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CRB1, CLKTYPE, __x)
+#define MCDE_COLKEYA 0x00000808
+#define MCDE_COLKEYA_GROUPOFFSET 0x200
+#define MCDE_COLKEYA_KEYB_SHIFT 0
+#define MCDE_COLKEYA_KEYB_MASK 0x000000FF
+#define MCDE_COLKEYA_KEYB(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYA, KEYB, __x)
+#define MCDE_COLKEYA_KEYG_SHIFT 8
+#define MCDE_COLKEYA_KEYG_MASK 0x0000FF00
+#define MCDE_COLKEYA_KEYG(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYA, KEYG, __x)
+#define MCDE_COLKEYA_KEYR_SHIFT 16
+#define MCDE_COLKEYA_KEYR_MASK 0x00FF0000
+#define MCDE_COLKEYA_KEYR(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYA, KEYR, __x)
+#define MCDE_COLKEYA_KEYA_SHIFT 24
+#define MCDE_COLKEYA_KEYA_MASK 0xFF000000
+#define MCDE_COLKEYA_KEYA(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYA, KEYA, __x)
+#define MCDE_COLKEYB 0x00000A08
+#define MCDE_COLKEYB_KEYB_SHIFT 0
+#define MCDE_COLKEYB_KEYB_MASK 0x000000FF
+#define MCDE_COLKEYB_KEYB(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYB, KEYB, __x)
+#define MCDE_COLKEYB_KEYG_SHIFT 8
+#define MCDE_COLKEYB_KEYG_MASK 0x0000FF00
+#define MCDE_COLKEYB_KEYG(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYB, KEYG, __x)
+#define MCDE_COLKEYB_KEYR_SHIFT 16
+#define MCDE_COLKEYB_KEYR_MASK 0x00FF0000
+#define MCDE_COLKEYB_KEYR(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYB, KEYR, __x)
+#define MCDE_COLKEYB_KEYA_SHIFT 24
+#define MCDE_COLKEYB_KEYA_MASK 0xFF000000
+#define MCDE_COLKEYB_KEYA(__x) \
+	MCDE_VAL2REG(MCDE_COLKEYB, KEYA, __x)
+#define MCDE_FCOLKEYA 0x0000080C
+#define MCDE_FCOLKEYA_GROUPOFFSET 0x200
+#define MCDE_FCOLKEYA_FKEYB_SHIFT 0
+#define MCDE_FCOLKEYA_FKEYB_MASK 0x000000FF
+#define MCDE_FCOLKEYA_FKEYB(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYA, FKEYB, __x)
+#define MCDE_FCOLKEYA_FKEYG_SHIFT 8
+#define MCDE_FCOLKEYA_FKEYG_MASK 0x0000FF00
+#define MCDE_FCOLKEYA_FKEYG(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYA, FKEYG, __x)
+#define MCDE_FCOLKEYA_FKEYR_SHIFT 16
+#define MCDE_FCOLKEYA_FKEYR_MASK 0x00FF0000
+#define MCDE_FCOLKEYA_FKEYR(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYA, FKEYR, __x)
+#define MCDE_FCOLKEYA_FKEYA_SHIFT 24
+#define MCDE_FCOLKEYA_FKEYA_MASK 0xFF000000
+#define MCDE_FCOLKEYA_FKEYA(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYA, FKEYA, __x)
+#define MCDE_FCOLKEYB 0x00000A0C
+#define MCDE_FCOLKEYB_FKEYB_SHIFT 0
+#define MCDE_FCOLKEYB_FKEYB_MASK 0x000000FF
+#define MCDE_FCOLKEYB_FKEYB(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYB, FKEYB, __x)
+#define MCDE_FCOLKEYB_FKEYG_SHIFT 8
+#define MCDE_FCOLKEYB_FKEYG_MASK 0x0000FF00
+#define MCDE_FCOLKEYB_FKEYG(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYB, FKEYG, __x)
+#define MCDE_FCOLKEYB_FKEYR_SHIFT 16
+#define MCDE_FCOLKEYB_FKEYR_MASK 0x00FF0000
+#define MCDE_FCOLKEYB_FKEYR(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYB, FKEYR, __x)
+#define MCDE_FCOLKEYB_FKEYA_SHIFT 24
+#define MCDE_FCOLKEYB_FKEYA_MASK 0xFF000000
+#define MCDE_FCOLKEYB_FKEYA(__x) \
+	MCDE_VAL2REG(MCDE_FCOLKEYB, FKEYA, __x)
+#define MCDE_RGBCONV1A 0x00000810
+#define MCDE_RGBCONV1A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV1A_YR_GREEN_SHIFT 0
+#define MCDE_RGBCONV1A_YR_GREEN_MASK 0x000007FF
+#define MCDE_RGBCONV1A_YR_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV1A, YR_GREEN, __x)
+#define MCDE_RGBCONV1A_YR_RED_SHIFT 16
+#define MCDE_RGBCONV1A_YR_RED_MASK 0x07FF0000
+#define MCDE_RGBCONV1A_YR_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV1A, YR_RED, __x)
+#define MCDE_RGBCONV1B 0x00000A10
+#define MCDE_RGBCONV1B_YR_GREEN_SHIFT 0
+#define MCDE_RGBCONV1B_YR_GREEN_MASK 0x000007FF
+#define MCDE_RGBCONV1B_YR_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV1B, YR_GREEN, __x)
+#define MCDE_RGBCONV1B_YR_RED_SHIFT 16
+#define MCDE_RGBCONV1B_YR_RED_MASK 0x07FF0000
+#define MCDE_RGBCONV1B_YR_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV1B, YR_RED, __x)
+#define MCDE_RGBCONV2A 0x00000814
+#define MCDE_RGBCONV2A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV2A_CR_RED_SHIFT 0
+#define MCDE_RGBCONV2A_CR_RED_MASK 0x000007FF
+#define MCDE_RGBCONV2A_CR_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV2A, CR_RED, __x)
+#define MCDE_RGBCONV2A_YR_BLUE_SHIFT 16
+#define MCDE_RGBCONV2A_YR_BLUE_MASK 0x07FF0000
+#define MCDE_RGBCONV2A_YR_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV2A, YR_BLUE, __x)
+#define MCDE_RGBCONV2B 0x00000A14
+#define MCDE_RGBCONV2B_CR_RED_SHIFT 0
+#define MCDE_RGBCONV2B_CR_RED_MASK 0x000007FF
+#define MCDE_RGBCONV2B_CR_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV2B, CR_RED, __x)
+#define MCDE_RGBCONV2B_YR_BLUE_SHIFT 16
+#define MCDE_RGBCONV2B_YR_BLUE_MASK 0x07FF0000
+#define MCDE_RGBCONV2B_YR_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV2B, YR_BLUE, __x)
+#define MCDE_RGBCONV3A 0x00000818
+#define MCDE_RGBCONV3A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV3A_CR_BLUE_SHIFT 0
+#define MCDE_RGBCONV3A_CR_BLUE_MASK 0x000007FF
+#define MCDE_RGBCONV3A_CR_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV3A, CR_BLUE, __x)
+#define MCDE_RGBCONV3A_CR_GREEN_SHIFT 16
+#define MCDE_RGBCONV3A_CR_GREEN_MASK 0x07FF0000
+#define MCDE_RGBCONV3A_CR_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV3A, CR_GREEN, __x)
+#define MCDE_RGBCONV3B 0x00000A18
+#define MCDE_RGBCONV3B_CR_BLUE_SHIFT 0
+#define MCDE_RGBCONV3B_CR_BLUE_MASK 0x000007FF
+#define MCDE_RGBCONV3B_CR_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV3B, CR_BLUE, __x)
+#define MCDE_RGBCONV3B_CR_GREEN_SHIFT 16
+#define MCDE_RGBCONV3B_CR_GREEN_MASK 0x07FF0000
+#define MCDE_RGBCONV3B_CR_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV3B, CR_GREEN, __x)
+#define MCDE_RGBCONV4A 0x0000081C
+#define MCDE_RGBCONV4A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV4A_CB_GREEN_SHIFT 0
+#define MCDE_RGBCONV4A_CB_GREEN_MASK 0x000007FF
+#define MCDE_RGBCONV4A_CB_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV4A, CB_GREEN, __x)
+#define MCDE_RGBCONV4A_CB_RED_SHIFT 16
+#define MCDE_RGBCONV4A_CB_RED_MASK 0x07FF0000
+#define MCDE_RGBCONV4A_CB_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV4A, CB_RED, __x)
+#define MCDE_RGBCONV4B 0x00000A1C
+#define MCDE_RGBCONV4B_CB_GREEN_SHIFT 0
+#define MCDE_RGBCONV4B_CB_GREEN_MASK 0x000007FF
+#define MCDE_RGBCONV4B_CB_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV4B, CB_GREEN, __x)
+#define MCDE_RGBCONV4B_CB_RED_SHIFT 16
+#define MCDE_RGBCONV4B_CB_RED_MASK 0x07FF0000
+#define MCDE_RGBCONV4B_CB_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV4B, CB_RED, __x)
+#define MCDE_RGBCONV5A 0x00000820
+#define MCDE_RGBCONV5A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV5A_OFF_RED_SHIFT 0
+#define MCDE_RGBCONV5A_OFF_RED_MASK 0x000007FF
+#define MCDE_RGBCONV5A_OFF_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV5A, OFF_RED, __x)
+#define MCDE_RGBCONV5A_CB_BLUE_SHIFT 16
+#define MCDE_RGBCONV5A_CB_BLUE_MASK 0x07FF0000
+#define MCDE_RGBCONV5A_CB_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV5A, CB_BLUE, __x)
+#define MCDE_RGBCONV5B 0x00000A20
+#define MCDE_RGBCONV5B_OFF_RED_SHIFT 0
+#define MCDE_RGBCONV5B_OFF_RED_MASK 0x000007FF
+#define MCDE_RGBCONV5B_OFF_RED(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV5B, OFF_RED, __x)
+#define MCDE_RGBCONV5B_CB_BLUE_SHIFT 16
+#define MCDE_RGBCONV5B_CB_BLUE_MASK 0x07FF0000
+#define MCDE_RGBCONV5B_CB_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV5B, CB_BLUE, __x)
+#define MCDE_RGBCONV6A 0x00000824
+#define MCDE_RGBCONV6A_GROUPOFFSET 0x200
+#define MCDE_RGBCONV6A_OFF_BLUE_SHIFT 0
+#define MCDE_RGBCONV6A_OFF_BLUE_MASK 0x000007FF
+#define MCDE_RGBCONV6A_OFF_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV6A, OFF_BLUE, __x)
+#define MCDE_RGBCONV6A_OFF_GREEN_SHIFT 16
+#define MCDE_RGBCONV6A_OFF_GREEN_MASK 0x07FF0000
+#define MCDE_RGBCONV6A_OFF_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV6A, OFF_GREEN, __x)
+#define MCDE_RGBCONV6B 0x00000A24
+#define MCDE_RGBCONV6B_OFF_BLUE_SHIFT 0
+#define MCDE_RGBCONV6B_OFF_BLUE_MASK 0x000007FF
+#define MCDE_RGBCONV6B_OFF_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV6B, OFF_BLUE, __x)
+#define MCDE_RGBCONV6B_OFF_GREEN_SHIFT 16
+#define MCDE_RGBCONV6B_OFF_GREEN_MASK 0x07FF0000
+#define MCDE_RGBCONV6B_OFF_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_RGBCONV6B, OFF_GREEN, __x)
+#define MCDE_FFCOEF0 0x00000828
+#define MCDE_FFCOEF0_COEFF0_N1_SHIFT 0
+#define MCDE_FFCOEF0_COEFF0_N1_MASK 0x000000FF
+#define MCDE_FFCOEF0_COEFF0_N1(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF0, COEFF0_N1, __x)
+#define MCDE_FFCOEF0_COEFF0_N2_SHIFT 8
+#define MCDE_FFCOEF0_COEFF0_N2_MASK 0x0000FF00
+#define MCDE_FFCOEF0_COEFF0_N2(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF0, COEFF0_N2, __x)
+#define MCDE_FFCOEF0_COEFF0_N3_SHIFT 16
+#define MCDE_FFCOEF0_COEFF0_N3_MASK 0x00FF0000
+#define MCDE_FFCOEF0_COEFF0_N3(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF0, COEFF0_N3, __x)
+#define MCDE_FFCOEF0_T0_SHIFT 24
+#define MCDE_FFCOEF0_T0_MASK 0x0F000000
+#define MCDE_FFCOEF0_T0(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF0, T0, __x)
+#define MCDE_FFCOEF1 0x0000082C
+#define MCDE_FFCOEF1_COEFF1_N1_SHIFT 0
+#define MCDE_FFCOEF1_COEFF1_N1_MASK 0x000000FF
+#define MCDE_FFCOEF1_COEFF1_N1(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF1, COEFF1_N1, __x)
+#define MCDE_FFCOEF1_COEFF1_N2_SHIFT 8
+#define MCDE_FFCOEF1_COEFF1_N2_MASK 0x0000FF00
+#define MCDE_FFCOEF1_COEFF1_N2(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF1, COEFF1_N2, __x)
+#define MCDE_FFCOEF1_COEFF1_N3_SHIFT 16
+#define MCDE_FFCOEF1_COEFF1_N3_MASK 0x00FF0000
+#define MCDE_FFCOEF1_COEFF1_N3(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF1, COEFF1_N3, __x)
+#define MCDE_FFCOEF1_T1_SHIFT 24
+#define MCDE_FFCOEF1_T1_MASK 0x0F000000
+#define MCDE_FFCOEF1_T1(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF1, T1, __x)
+#define MCDE_FFCOEF2 0x00000830
+#define MCDE_FFCOEF2_COEFF2_N1_SHIFT 0
+#define MCDE_FFCOEF2_COEFF2_N1_MASK 0x000000FF
+#define MCDE_FFCOEF2_COEFF2_N1(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF2, COEFF2_N1, __x)
+#define MCDE_FFCOEF2_COEFF2_N2_SHIFT 8
+#define MCDE_FFCOEF2_COEFF2_N2_MASK 0x0000FF00
+#define MCDE_FFCOEF2_COEFF2_N2(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF2, COEFF2_N2, __x)
+#define MCDE_FFCOEF2_COEFF2_N3_SHIFT 16
+#define MCDE_FFCOEF2_COEFF2_N3_MASK 0x00FF0000
+#define MCDE_FFCOEF2_COEFF2_N3(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF2, COEFF2_N3, __x)
+#define MCDE_FFCOEF2_T2_SHIFT 24
+#define MCDE_FFCOEF2_T2_MASK 0x0F000000
+#define MCDE_FFCOEF2_T2(__x) \
+	MCDE_VAL2REG(MCDE_FFCOEF2, T2, __x)
+#define MCDE_MCDE_WDATAA 0x00000834
+#define MCDE_MCDE_WDATAA_GROUPOFFSET 0x200
+#define MCDE_MCDE_WDATAA_DC_SHIFT 24
+#define MCDE_MCDE_WDATAA_DC_MASK 0x01000000
+#define MCDE_MCDE_WDATAA_DC(__x) \
+	MCDE_VAL2REG(MCDE_MCDE_WDATAA, DC, __x)
+#define MCDE_MCDE_WDATAA_DATAVALUE_SHIFT 0
+#define MCDE_MCDE_WDATAA_DATAVALUE_MASK 0x00FFFFFF
+#define MCDE_MCDE_WDATAA_DATAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_MCDE_WDATAA, DATAVALUE, __x)
+#define MCDE_MCDE_WDATAB 0x00000A34
+#define MCDE_MCDE_WDATAB_DC_SHIFT 24
+#define MCDE_MCDE_WDATAB_DC_MASK 0x01000000
+#define MCDE_MCDE_WDATAB_DC(__x) \
+	MCDE_VAL2REG(MCDE_MCDE_WDATAB, DC, __x)
+#define MCDE_MCDE_WDATAB_DATAVALUE_SHIFT 0
+#define MCDE_MCDE_WDATAB_DATAVALUE_MASK 0x00FFFFFF
+#define MCDE_MCDE_WDATAB_DATAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_MCDE_WDATAB, DATAVALUE, __x)
+#define MCDE_TVCRA 0x00000838
+#define MCDE_TVCRA_GROUPOFFSET 0x200
+#define MCDE_TVCRA_SEL_MOD_SHIFT 0
+#define MCDE_TVCRA_SEL_MOD_MASK 0x00000001
+#define MCDE_TVCRA_SEL_MOD_LCD 0
+#define MCDE_TVCRA_SEL_MOD_TV 1
+#define MCDE_TVCRA_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, SEL_MOD, MCDE_TVCRA_SEL_MOD_##__x)
+#define MCDE_TVCRA_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, SEL_MOD, __x)
+#define MCDE_TVCRA_INTEREN_SHIFT 1
+#define MCDE_TVCRA_INTEREN_MASK 0x00000002
+#define MCDE_TVCRA_INTEREN(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, INTEREN, __x)
+#define MCDE_TVCRA_IFIELD_SHIFT 2
+#define MCDE_TVCRA_IFIELD_MASK 0x00000004
+#define MCDE_TVCRA_IFIELD(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, IFIELD, __x)
+#define MCDE_TVCRA_TVMODE_SHIFT 3
+#define MCDE_TVCRA_TVMODE_MASK 0x00000038
+#define MCDE_TVCRA_TVMODE_SDTV_656P 0
+#define MCDE_TVCRA_TVMODE_SDTV_656P_LE 3
+#define MCDE_TVCRA_TVMODE_SDTV_656P_BE 4
+#define MCDE_TVCRA_TVMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, TVMODE, MCDE_TVCRA_TVMODE_##__x)
+#define MCDE_TVCRA_TVMODE(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, TVMODE, __x)
+#define MCDE_TVCRA_SDTVMODE_SHIFT 6
+#define MCDE_TVCRA_SDTVMODE_MASK 0x000000C0
+#define MCDE_TVCRA_SDTVMODE_Y0CBY1CR 0
+#define MCDE_TVCRA_SDTVMODE_CBY0CRY1 1
+#define MCDE_TVCRA_SDTVMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, SDTVMODE, MCDE_TVCRA_SDTVMODE_##__x)
+#define MCDE_TVCRA_SDTVMODE(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, SDTVMODE, __x)
+#define MCDE_TVCRA_AVRGEN_SHIFT 8
+#define MCDE_TVCRA_AVRGEN_MASK 0x00000100
+#define MCDE_TVCRA_AVRGEN(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, AVRGEN, __x)
+#define MCDE_TVCRA_CKINV_SHIFT 9
+#define MCDE_TVCRA_CKINV_MASK 0x00000200
+#define MCDE_TVCRA_CKINV(__x) \
+	MCDE_VAL2REG(MCDE_TVCRA, CKINV, __x)
+#define MCDE_TVCRB 0x00000A38
+#define MCDE_TVCRB_SEL_MOD_SHIFT 0
+#define MCDE_TVCRB_SEL_MOD_MASK 0x00000001
+#define MCDE_TVCRB_SEL_MOD_LCD 0
+#define MCDE_TVCRB_SEL_MOD_TV 1
+#define MCDE_TVCRB_SEL_MOD_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, SEL_MOD, MCDE_TVCRB_SEL_MOD_##__x)
+#define MCDE_TVCRB_SEL_MOD(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, SEL_MOD, __x)
+#define MCDE_TVCRB_INTEREN_SHIFT 1
+#define MCDE_TVCRB_INTEREN_MASK 0x00000002
+#define MCDE_TVCRB_INTEREN(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, INTEREN, __x)
+#define MCDE_TVCRB_IFIELD_SHIFT 2
+#define MCDE_TVCRB_IFIELD_MASK 0x00000004
+#define MCDE_TVCRB_IFIELD(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, IFIELD, __x)
+#define MCDE_TVCRB_TVMODE_SHIFT 3
+#define MCDE_TVCRB_TVMODE_MASK 0x00000038
+#define MCDE_TVCRB_TVMODE_SDTV_656P 0
+#define MCDE_TVCRB_TVMODE_SDTV_656P_LE 3
+#define MCDE_TVCRB_TVMODE_SDTV_656P_BE 4
+#define MCDE_TVCRB_TVMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, TVMODE, MCDE_TVCRB_TVMODE_##__x)
+#define MCDE_TVCRB_TVMODE(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, TVMODE, __x)
+#define MCDE_TVCRB_SDTVMODE_SHIFT 6
+#define MCDE_TVCRB_SDTVMODE_MASK 0x000000C0
+#define MCDE_TVCRB_SDTVMODE_Y0CBY1CR 0
+#define MCDE_TVCRB_SDTVMODE_CBY0CRY1 1
+#define MCDE_TVCRB_SDTVMODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, SDTVMODE, MCDE_TVCRB_SDTVMODE_##__x)
+#define MCDE_TVCRB_SDTVMODE(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, SDTVMODE, __x)
+#define MCDE_TVCRB_AVRGEN_SHIFT 8
+#define MCDE_TVCRB_AVRGEN_MASK 0x00000100
+#define MCDE_TVCRB_AVRGEN(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, AVRGEN, __x)
+#define MCDE_TVCRB_CKINV_SHIFT 9
+#define MCDE_TVCRB_CKINV_MASK 0x00000200
+#define MCDE_TVCRB_CKINV(__x) \
+	MCDE_VAL2REG(MCDE_TVCRB, CKINV, __x)
+#define MCDE_TVBL1A 0x0000083C
+#define MCDE_TVBL1A_GROUPOFFSET 0x200
+#define MCDE_TVBL1A_BEL1_SHIFT 0
+#define MCDE_TVBL1A_BEL1_MASK 0x000007FF
+#define MCDE_TVBL1A_BEL1(__x) \
+	MCDE_VAL2REG(MCDE_TVBL1A, BEL1, __x)
+#define MCDE_TVBL1A_BSL1_SHIFT 16
+#define MCDE_TVBL1A_BSL1_MASK 0x07FF0000
+#define MCDE_TVBL1A_BSL1(__x) \
+	MCDE_VAL2REG(MCDE_TVBL1A, BSL1, __x)
+#define MCDE_TVBL1B 0x00000A3C
+#define MCDE_TVBL1B_BEL1_SHIFT 0
+#define MCDE_TVBL1B_BEL1_MASK 0x000007FF
+#define MCDE_TVBL1B_BEL1(__x) \
+	MCDE_VAL2REG(MCDE_TVBL1B, BEL1, __x)
+#define MCDE_TVBL1B_BSL1_SHIFT 16
+#define MCDE_TVBL1B_BSL1_MASK 0x07FF0000
+#define MCDE_TVBL1B_BSL1(__x) \
+	MCDE_VAL2REG(MCDE_TVBL1B, BSL1, __x)
+#define MCDE_TVISLA 0x00000840
+#define MCDE_TVISLA_GROUPOFFSET 0x200
+#define MCDE_TVISLA_FSL1_SHIFT 0
+#define MCDE_TVISLA_FSL1_MASK 0x000007FF
+#define MCDE_TVISLA_FSL1(__x) \
+	MCDE_VAL2REG(MCDE_TVISLA, FSL1, __x)
+#define MCDE_TVISLA_FSL2_SHIFT 16
+#define MCDE_TVISLA_FSL2_MASK 0x07FF0000
+#define MCDE_TVISLA_FSL2(__x) \
+	MCDE_VAL2REG(MCDE_TVISLA, FSL2, __x)
+#define MCDE_TVISLB 0x00000A40
+#define MCDE_TVISLB_FSL1_SHIFT 0
+#define MCDE_TVISLB_FSL1_MASK 0x000007FF
+#define MCDE_TVISLB_FSL1(__x) \
+	MCDE_VAL2REG(MCDE_TVISLB, FSL1, __x)
+#define MCDE_TVISLB_FSL2_SHIFT 16
+#define MCDE_TVISLB_FSL2_MASK 0x07FF0000
+#define MCDE_TVISLB_FSL2(__x) \
+	MCDE_VAL2REG(MCDE_TVISLB, FSL2, __x)
+#define MCDE_TVDVOA 0x00000844
+#define MCDE_TVDVOA_GROUPOFFSET 0x200
+#define MCDE_TVDVOA_DVO1_SHIFT 0
+#define MCDE_TVDVOA_DVO1_MASK 0x000007FF
+#define MCDE_TVDVOA_DVO1(__x) \
+	MCDE_VAL2REG(MCDE_TVDVOA, DVO1, __x)
+#define MCDE_TVDVOA_DVO2_SHIFT 16
+#define MCDE_TVDVOA_DVO2_MASK 0x07FF0000
+#define MCDE_TVDVOA_DVO2(__x) \
+	MCDE_VAL2REG(MCDE_TVDVOA, DVO2, __x)
+#define MCDE_TVDVOB 0x00000A44
+#define MCDE_TVDVOB_DVO1_SHIFT 0
+#define MCDE_TVDVOB_DVO1_MASK 0x000007FF
+#define MCDE_TVDVOB_DVO1(__x) \
+	MCDE_VAL2REG(MCDE_TVDVOB, DVO1, __x)
+#define MCDE_TVDVOB_DVO2_SHIFT 16
+#define MCDE_TVDVOB_DVO2_MASK 0x07FF0000
+#define MCDE_TVDVOB_DVO2(__x) \
+	MCDE_VAL2REG(MCDE_TVDVOB, DVO2, __x)
+#define MCDE_TVTIM1A 0x0000084C
+#define MCDE_TVTIM1A_GROUPOFFSET 0x200
+#define MCDE_TVTIM1A_DHO_SHIFT 0
+#define MCDE_TVTIM1A_DHO_MASK 0x000007FF
+#define MCDE_TVTIM1A_DHO(__x) \
+	MCDE_VAL2REG(MCDE_TVTIM1A, DHO, __x)
+#define MCDE_TVTIM1B 0x00000A4C
+#define MCDE_TVTIM1B_DHO_SHIFT 0
+#define MCDE_TVTIM1B_DHO_MASK 0x000007FF
+#define MCDE_TVTIM1B_DHO(__x) \
+	MCDE_VAL2REG(MCDE_TVTIM1B, DHO, __x)
+#define MCDE_TVLBALWA 0x00000850
+#define MCDE_TVLBALWA_GROUPOFFSET 0x200
+#define MCDE_TVLBALWA_LBW_SHIFT 0
+#define MCDE_TVLBALWA_LBW_MASK 0x000007FF
+#define MCDE_TVLBALWA_LBW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWA, LBW, __x)
+#define MCDE_TVLBALWA_ALW_SHIFT 16
+#define MCDE_TVLBALWA_ALW_MASK 0x07FF0000
+#define MCDE_TVLBALWA_ALW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWA, ALW, __x)
+#define MCDE_TVLBALWB 0x00000A50
+#define MCDE_TVLBALWB_LBW_SHIFT 0
+#define MCDE_TVLBALWB_LBW_MASK 0x000007FF
+#define MCDE_TVLBALWB_LBW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWB, LBW, __x)
+#define MCDE_TVLBALWB_ALW_SHIFT 16
+#define MCDE_TVLBALWB_ALW_MASK 0x07FF0000
+#define MCDE_TVLBALWB_ALW(__x) \
+	MCDE_VAL2REG(MCDE_TVLBALWB, ALW, __x)
+#define MCDE_TVBL2A 0x00000854
+#define MCDE_TVBL2A_GROUPOFFSET 0x200
+#define MCDE_TVBL2A_BEL2_SHIFT 0
+#define MCDE_TVBL2A_BEL2_MASK 0x000007FF
+#define MCDE_TVBL2A_BEL2(__x) \
+	MCDE_VAL2REG(MCDE_TVBL2A, BEL2, __x)
+#define MCDE_TVBL2A_BSL2_SHIFT 16
+#define MCDE_TVBL2A_BSL2_MASK 0x07FF0000
+#define MCDE_TVBL2A_BSL2(__x) \
+	MCDE_VAL2REG(MCDE_TVBL2A, BSL2, __x)
+#define MCDE_TVBL2B 0x00000A54
+#define MCDE_TVBL2B_BEL2_SHIFT 0
+#define MCDE_TVBL2B_BEL2_MASK 0x000007FF
+#define MCDE_TVBL2B_BEL2(__x) \
+	MCDE_VAL2REG(MCDE_TVBL2B, BEL2, __x)
+#define MCDE_TVBL2B_BSL2_SHIFT 16
+#define MCDE_TVBL2B_BSL2_MASK 0x07FF0000
+#define MCDE_TVBL2B_BSL2(__x) \
+	MCDE_VAL2REG(MCDE_TVBL2B, BSL2, __x)
+#define MCDE_TVBLUA 0x00000858
+#define MCDE_TVBLUA_GROUPOFFSET 0x200
+#define MCDE_TVBLUA_TVBLU_SHIFT 0
+#define MCDE_TVBLUA_TVBLU_MASK 0x000000FF
+#define MCDE_TVBLUA_TVBLU(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUA, TVBLU, __x)
+#define MCDE_TVBLUA_TVBCB_SHIFT 8
+#define MCDE_TVBLUA_TVBCB_MASK 0x0000FF00
+#define MCDE_TVBLUA_TVBCB(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUA, TVBCB, __x)
+#define MCDE_TVBLUA_TVBCR_SHIFT 16
+#define MCDE_TVBLUA_TVBCR_MASK 0x00FF0000
+#define MCDE_TVBLUA_TVBCR(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUA, TVBCR, __x)
+#define MCDE_TVBLUB 0x00000A58
+#define MCDE_TVBLUB_TVBLU_SHIFT 0
+#define MCDE_TVBLUB_TVBLU_MASK 0x000000FF
+#define MCDE_TVBLUB_TVBLU(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUB, TVBLU, __x)
+#define MCDE_TVBLUB_TVBCB_SHIFT 8
+#define MCDE_TVBLUB_TVBCB_MASK 0x0000FF00
+#define MCDE_TVBLUB_TVBCB(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUB, TVBCB, __x)
+#define MCDE_TVBLUB_TVBCR_SHIFT 16
+#define MCDE_TVBLUB_TVBCR_MASK 0x00FF0000
+#define MCDE_TVBLUB_TVBCR(__x) \
+	MCDE_VAL2REG(MCDE_TVBLUB, TVBCR, __x)
+#define MCDE_LCDTIM1A 0x00000860
+#define MCDE_LCDTIM1A_GROUPOFFSET 0x200
+#define MCDE_LCDTIM1A_IVP_SHIFT 19
+#define MCDE_LCDTIM1A_IVP_MASK 0x00080000
+#define MCDE_LCDTIM1A_IVP(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IVP, __x)
+#define MCDE_LCDTIM1A_IVS_SHIFT 20
+#define MCDE_LCDTIM1A_IVS_MASK 0x00100000
+#define MCDE_LCDTIM1A_IVS(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IVS, __x)
+#define MCDE_LCDTIM1A_IHS_SHIFT 21
+#define MCDE_LCDTIM1A_IHS_MASK 0x00200000
+#define MCDE_LCDTIM1A_IHS(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IHS, __x)
+#define MCDE_LCDTIM1A_IPC_SHIFT 22
+#define MCDE_LCDTIM1A_IPC_MASK 0x00400000
+#define MCDE_LCDTIM1A_IPC(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IPC, __x)
+#define MCDE_LCDTIM1A_IOE_SHIFT 23
+#define MCDE_LCDTIM1A_IOE_MASK 0x00800000
+#define MCDE_LCDTIM1A_IOE(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1A, IOE, __x)
+#define MCDE_LCDTIM1B 0x00000A60
+#define MCDE_LCDTIM1B_IVP_SHIFT 19
+#define MCDE_LCDTIM1B_IVP_MASK 0x00080000
+#define MCDE_LCDTIM1B_IVP(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IVP, __x)
+#define MCDE_LCDTIM1B_IVS_SHIFT 20
+#define MCDE_LCDTIM1B_IVS_MASK 0x00100000
+#define MCDE_LCDTIM1B_IVS(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IVS, __x)
+#define MCDE_LCDTIM1B_IHS_SHIFT 21
+#define MCDE_LCDTIM1B_IHS_MASK 0x00200000
+#define MCDE_LCDTIM1B_IHS(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IHS, __x)
+#define MCDE_LCDTIM1B_IPC_SHIFT 22
+#define MCDE_LCDTIM1B_IPC_MASK 0x00400000
+#define MCDE_LCDTIM1B_IPC(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IPC, __x)
+#define MCDE_LCDTIM1B_IOE_SHIFT 23
+#define MCDE_LCDTIM1B_IOE_MASK 0x00800000
+#define MCDE_LCDTIM1B_IOE(__x) \
+	MCDE_VAL2REG(MCDE_LCDTIM1B, IOE, __x)
+#define MCDE_DITCTRLA 0x00000864
+#define MCDE_DITCTRLA_GROUPOFFSET 0x200
+#define MCDE_DITCTRLA_TEMP_SHIFT 0
+#define MCDE_DITCTRLA_TEMP_MASK 0x00000001
+#define MCDE_DITCTRLA_TEMP(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, TEMP, __x)
+#define MCDE_DITCTRLA_COMP_SHIFT 1
+#define MCDE_DITCTRLA_COMP_MASK 0x00000002
+#define MCDE_DITCTRLA_COMP(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, COMP, __x)
+#define MCDE_DITCTRLA_MODE_SHIFT 2
+#define MCDE_DITCTRLA_MODE_MASK 0x0000000C
+#define MCDE_DITCTRLA_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, MODE, __x)
+#define MCDE_DITCTRLA_MASK_SHIFT 4
+#define MCDE_DITCTRLA_MASK_MASK 0x00000010
+#define MCDE_DITCTRLA_MASK(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, MASK, __x)
+#define MCDE_DITCTRLA_FOFFX_SHIFT 5
+#define MCDE_DITCTRLA_FOFFX_MASK 0x000003E0
+#define MCDE_DITCTRLA_FOFFX(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, FOFFX, __x)
+#define MCDE_DITCTRLA_FOFFY_SHIFT 10
+#define MCDE_DITCTRLA_FOFFY_MASK 0x00007C00
+#define MCDE_DITCTRLA_FOFFY(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLA, FOFFY, __x)
+#define MCDE_DITCTRLB 0x00000A64
+#define MCDE_DITCTRLB_TEMP_SHIFT 0
+#define MCDE_DITCTRLB_TEMP_MASK 0x00000001
+#define MCDE_DITCTRLB_TEMP(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, TEMP, __x)
+#define MCDE_DITCTRLB_COMP_SHIFT 1
+#define MCDE_DITCTRLB_COMP_MASK 0x00000002
+#define MCDE_DITCTRLB_COMP(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, COMP, __x)
+#define MCDE_DITCTRLB_MODE_SHIFT 2
+#define MCDE_DITCTRLB_MODE_MASK 0x0000000C
+#define MCDE_DITCTRLB_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, MODE, __x)
+#define MCDE_DITCTRLB_MASK_SHIFT 4
+#define MCDE_DITCTRLB_MASK_MASK 0x00000010
+#define MCDE_DITCTRLB_MASK(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, MASK, __x)
+#define MCDE_DITCTRLB_FOFFX_SHIFT 5
+#define MCDE_DITCTRLB_FOFFX_MASK 0x000003E0
+#define MCDE_DITCTRLB_FOFFX(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, FOFFX, __x)
+#define MCDE_DITCTRLB_FOFFY_SHIFT 10
+#define MCDE_DITCTRLB_FOFFY_MASK 0x00007C00
+#define MCDE_DITCTRLB_FOFFY(__x) \
+	MCDE_VAL2REG(MCDE_DITCTRLB, FOFFY, __x)
+#define MCDE_DITOFFA 0x00000868
+#define MCDE_DITOFFA_GROUPOFFSET 0x200
+#define MCDE_DITOFFA_XG_SHIFT 0
+#define MCDE_DITOFFA_XG_MASK 0x0000001F
+#define MCDE_DITOFFA_XG(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFA, XG, __x)
+#define MCDE_DITOFFA_YG_SHIFT 8
+#define MCDE_DITOFFA_YG_MASK 0x00001F00
+#define MCDE_DITOFFA_YG(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFA, YG, __x)
+#define MCDE_DITOFFA_XB_SHIFT 16
+#define MCDE_DITOFFA_XB_MASK 0x001F0000
+#define MCDE_DITOFFA_XB(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFA, XB, __x)
+#define MCDE_DITOFFA_YB_SHIFT 24
+#define MCDE_DITOFFA_YB_MASK 0x1F000000
+#define MCDE_DITOFFA_YB(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFA, YB, __x)
+#define MCDE_DITOFFB 0x00000A68
+#define MCDE_DITOFFB_XG_SHIFT 0
+#define MCDE_DITOFFB_XG_MASK 0x0000001F
+#define MCDE_DITOFFB_XG(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFB, XG, __x)
+#define MCDE_DITOFFB_YG_SHIFT 8
+#define MCDE_DITOFFB_YG_MASK 0x00001F00
+#define MCDE_DITOFFB_YG(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFB, YG, __x)
+#define MCDE_DITOFFB_XB_SHIFT 16
+#define MCDE_DITOFFB_XB_MASK 0x001F0000
+#define MCDE_DITOFFB_XB(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFB, XB, __x)
+#define MCDE_DITOFFB_YB_SHIFT 24
+#define MCDE_DITOFFB_YB_MASK 0x1F000000
+#define MCDE_DITOFFB_YB(__x) \
+	MCDE_VAL2REG(MCDE_DITOFFB, YB, __x)
+#define MCDE_PAL0A 0x0000086C
+#define MCDE_PAL0A_GROUPOFFSET 0x200
+#define MCDE_PAL0A_BLUE_SHIFT 0
+#define MCDE_PAL0A_BLUE_MASK 0x00000FFF
+#define MCDE_PAL0A_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_PAL0A, BLUE, __x)
+#define MCDE_PAL0A_GREEN_SHIFT 16
+#define MCDE_PAL0A_GREEN_MASK 0x0FFF0000
+#define MCDE_PAL0A_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_PAL0A, GREEN, __x)
+#define MCDE_PAL0B 0x00000A6C
+#define MCDE_PAL0B_BLUE_SHIFT 0
+#define MCDE_PAL0B_BLUE_MASK 0x00000FFF
+#define MCDE_PAL0B_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_PAL0B, BLUE, __x)
+#define MCDE_PAL0B_GREEN_SHIFT 16
+#define MCDE_PAL0B_GREEN_MASK 0x0FFF0000
+#define MCDE_PAL0B_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_PAL0B, GREEN, __x)
+#define MCDE_PAL1A 0x00000870
+#define MCDE_PAL1A_GROUPOFFSET 0x200
+#define MCDE_PAL1A_RED_SHIFT 0
+#define MCDE_PAL1A_RED_MASK 0x00000FFF
+#define MCDE_PAL1A_RED(__x) \
+	MCDE_VAL2REG(MCDE_PAL1A, RED, __x)
+#define MCDE_PAL1B 0x00000A70
+#define MCDE_PAL1B_RED_SHIFT 0
+#define MCDE_PAL1B_RED_MASK 0x00000FFF
+#define MCDE_PAL1B_RED(__x) \
+	MCDE_VAL2REG(MCDE_PAL1B, RED, __x)
+#define MCDE_ROTADD0A 0x00000874
+#define MCDE_ROTADD0A_GROUPOFFSET 0x200
+#define MCDE_ROTADD0A_ROTADD0_SHIFT 3
+#define MCDE_ROTADD0A_ROTADD0_MASK 0xFFFFFFF8
+#define MCDE_ROTADD0A_ROTADD0(__x) \
+	MCDE_VAL2REG(MCDE_ROTADD0A, ROTADD0, __x)
+#define MCDE_ROTADD0B 0x00000A74
+#define MCDE_ROTADD0B_ROTADD0_SHIFT 3
+#define MCDE_ROTADD0B_ROTADD0_MASK 0xFFFFFFF8
+#define MCDE_ROTADD0B_ROTADD0(__x) \
+	MCDE_VAL2REG(MCDE_ROTADD0B, ROTADD0, __x)
+#define MCDE_ROTADD1A 0x00000878
+#define MCDE_ROTADD1A_GROUPOFFSET 0x200
+#define MCDE_ROTADD1A_ROTADD1_SHIFT 3
+#define MCDE_ROTADD1A_ROTADD1_MASK 0xFFFFFFF8
+#define MCDE_ROTADD1A_ROTADD1(__x) \
+	MCDE_VAL2REG(MCDE_ROTADD1A, ROTADD1, __x)
+#define MCDE_ROTADD1B 0x00000A78
+#define MCDE_ROTADD1B_ROTADD1_SHIFT 3
+#define MCDE_ROTADD1B_ROTADD1_MASK 0xFFFFFFF8
+#define MCDE_ROTADD1B_ROTADD1(__x) \
+	MCDE_VAL2REG(MCDE_ROTADD1B, ROTADD1, __x)
+#define MCDE_ROTACONF 0x0000087C
+#define MCDE_ROTACONF_GROUPOFFSET 0x200
+#define MCDE_ROTACONF_ROTBURSTSIZE_SHIFT 0
+#define MCDE_ROTACONF_ROTBURSTSIZE_MASK 0x00000007
+#define MCDE_ROTACONF_ROTBURSTSIZE_1W 0
+#define MCDE_ROTACONF_ROTBURSTSIZE_2W 1
+#define MCDE_ROTACONF_ROTBURSTSIZE_4W 2
+#define MCDE_ROTACONF_ROTBURSTSIZE_8W 3
+#define MCDE_ROTACONF_ROTBURSTSIZE_HW_1W 4
+#define MCDE_ROTACONF_ROTBURSTSIZE_HW_2W 5
+#define MCDE_ROTACONF_ROTBURSTSIZE_HW_4W 6
+#define MCDE_ROTACONF_ROTBURSTSIZE_HW_8W 7
+#define MCDE_ROTACONF_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, ROTBURSTSIZE, \
+	MCDE_ROTACONF_ROTBURSTSIZE_##__x)
+#define MCDE_ROTACONF_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, ROTBURSTSIZE, __x)
+#define MCDE_ROTACONF_ROTDIR_SHIFT 3
+#define MCDE_ROTACONF_ROTDIR_MASK 0x00000008
+#define MCDE_ROTACONF_ROTDIR_CCW 0
+#define MCDE_ROTACONF_ROTDIR_CW 1
+#define MCDE_ROTACONF_ROTDIR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, ROTDIR, MCDE_ROTACONF_ROTDIR_##__x)
+#define MCDE_ROTACONF_ROTDIR(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, ROTDIR, __x)
+#define MCDE_ROTACONF_WR_MAXOUT_SHIFT 4
+#define MCDE_ROTACONF_WR_MAXOUT_MASK 0x00000030
+#define MCDE_ROTACONF_WR_MAXOUT_1_REQ 0
+#define MCDE_ROTACONF_WR_MAXOUT_2_REQ 1
+#define MCDE_ROTACONF_WR_MAXOUT_4_REQ 2
+#define MCDE_ROTACONF_WR_MAXOUT_8_REQ 3
+#define MCDE_ROTACONF_WR_MAXOUT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, WR_MAXOUT, MCDE_ROTACONF_WR_MAXOUT_##__x)
+#define MCDE_ROTACONF_WR_MAXOUT(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, WR_MAXOUT, __x)
+#define MCDE_ROTACONF_RD_MAXOUT_SHIFT 6
+#define MCDE_ROTACONF_RD_MAXOUT_MASK 0x000000C0
+#define MCDE_ROTACONF_RD_MAXOUT_1_REQ 0
+#define MCDE_ROTACONF_RD_MAXOUT_2_REQ 1
+#define MCDE_ROTACONF_RD_MAXOUT_4_REQ 2
+#define MCDE_ROTACONF_RD_MAXOUT_8_REQ 3
+#define MCDE_ROTACONF_RD_MAXOUT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, RD_MAXOUT, MCDE_ROTACONF_RD_MAXOUT_##__x)
+#define MCDE_ROTACONF_RD_MAXOUT(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, RD_MAXOUT, __x)
+#define MCDE_ROTACONF_STRIP_WIDTH_SHIFT 8
+#define MCDE_ROTACONF_STRIP_WIDTH_MASK 0x00007F00
+#define MCDE_ROTACONF_STRIP_WIDTH_2PIX 0
+#define MCDE_ROTACONF_STRIP_WIDTH_4PIX 1
+#define MCDE_ROTACONF_STRIP_WIDTH_8PIX 2
+#define MCDE_ROTACONF_STRIP_WIDTH_16PIX 3
+#define MCDE_ROTACONF_STRIP_WIDTH_32PIX 4
+#define MCDE_ROTACONF_STRIP_WIDTH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, STRIP_WIDTH, \
+	MCDE_ROTACONF_STRIP_WIDTH_##__x)
+#define MCDE_ROTACONF_STRIP_WIDTH(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, STRIP_WIDTH, __x)
+#define MCDE_ROTACONF_SINGLE_BUF_SHIFT 15
+#define MCDE_ROTACONF_SINGLE_BUF_MASK 0x00008000
+#define MCDE_ROTACONF_SINGLE_BUF(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, SINGLE_BUF, __x)
+#define MCDE_ROTACONF_WR_ROPC_SHIFT 16
+#define MCDE_ROTACONF_WR_ROPC_MASK 0x00FF0000
+#define MCDE_ROTACONF_WR_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, WR_ROPC, __x)
+#define MCDE_ROTACONF_RD_ROPC_SHIFT 24
+#define MCDE_ROTACONF_RD_ROPC_MASK 0xFF000000
+#define MCDE_ROTACONF_RD_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_ROTACONF, RD_ROPC, __x)
+#define MCDE_ROTBCONF 0x00000A7C
+#define MCDE_ROTBCONF_ROTBURSTSIZE_SHIFT 0
+#define MCDE_ROTBCONF_ROTBURSTSIZE_MASK 0x00000007
+#define MCDE_ROTBCONF_ROTBURSTSIZE_1W 0
+#define MCDE_ROTBCONF_ROTBURSTSIZE_2W 1
+#define MCDE_ROTBCONF_ROTBURSTSIZE_4W 2
+#define MCDE_ROTBCONF_ROTBURSTSIZE_8W 3
+#define MCDE_ROTBCONF_ROTBURSTSIZE_HW_1W 4
+#define MCDE_ROTBCONF_ROTBURSTSIZE_HW_2W 5
+#define MCDE_ROTBCONF_ROTBURSTSIZE_HW_4W 6
+#define MCDE_ROTBCONF_ROTBURSTSIZE_HW_8W 7
+#define MCDE_ROTBCONF_ROTBURSTSIZE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, ROTBURSTSIZE, \
+	MCDE_ROTBCONF_ROTBURSTSIZE_##__x)
+#define MCDE_ROTBCONF_ROTBURSTSIZE(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, ROTBURSTSIZE, __x)
+#define MCDE_ROTBCONF_ROTDIR_SHIFT 3
+#define MCDE_ROTBCONF_ROTDIR_MASK 0x00000008
+#define MCDE_ROTBCONF_ROTDIR_CCW 0
+#define MCDE_ROTBCONF_ROTDIR_CW 1
+#define MCDE_ROTBCONF_ROTDIR_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, ROTDIR, MCDE_ROTBCONF_ROTDIR_##__x)
+#define MCDE_ROTBCONF_ROTDIR(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, ROTDIR, __x)
+#define MCDE_ROTBCONF_WR_MAXOUT_SHIFT 4
+#define MCDE_ROTBCONF_WR_MAXOUT_MASK 0x00000030
+#define MCDE_ROTBCONF_WR_MAXOUT_1_REQ 0
+#define MCDE_ROTBCONF_WR_MAXOUT_2_REQ 1
+#define MCDE_ROTBCONF_WR_MAXOUT_4_REQ 2
+#define MCDE_ROTBCONF_WR_MAXOUT_8_REQ 3
+#define MCDE_ROTBCONF_WR_MAXOUT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, WR_MAXOUT, MCDE_ROTBCONF_WR_MAXOUT_##__x)
+#define MCDE_ROTBCONF_WR_MAXOUT(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, WR_MAXOUT, __x)
+#define MCDE_ROTBCONF_RD_MAXOUT_SHIFT 6
+#define MCDE_ROTBCONF_RD_MAXOUT_MASK 0x000000C0
+#define MCDE_ROTBCONF_RD_MAXOUT_1_REQ 0
+#define MCDE_ROTBCONF_RD_MAXOUT_2_REQ 1
+#define MCDE_ROTBCONF_RD_MAXOUT_4_REQ 2
+#define MCDE_ROTBCONF_RD_MAXOUT_8_REQ 3
+#define MCDE_ROTBCONF_RD_MAXOUT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, RD_MAXOUT, MCDE_ROTBCONF_RD_MAXOUT_##__x)
+#define MCDE_ROTBCONF_RD_MAXOUT(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, RD_MAXOUT, __x)
+#define MCDE_ROTBCONF_STRIP_WIDTH_SHIFT 8
+#define MCDE_ROTBCONF_STRIP_WIDTH_MASK 0x00007F00
+#define MCDE_ROTBCONF_STRIP_WIDTH_2PIX 0
+#define MCDE_ROTBCONF_STRIP_WIDTH_4PIX 1
+#define MCDE_ROTBCONF_STRIP_WIDTH_8PIX 2
+#define MCDE_ROTBCONF_STRIP_WIDTH_16PIX 3
+#define MCDE_ROTBCONF_STRIP_WIDTH_32PIX 4
+#define MCDE_ROTBCONF_STRIP_WIDTH_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, STRIP_WIDTH, \
+	MCDE_ROTBCONF_STRIP_WIDTH_##__x)
+#define MCDE_ROTBCONF_STRIP_WIDTH(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, STRIP_WIDTH, __x)
+#define MCDE_ROTBCONF_SINGLE_BUF_SHIFT 15
+#define MCDE_ROTBCONF_SINGLE_BUF_MASK 0x00008000
+#define MCDE_ROTBCONF_SINGLE_BUF(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, SINGLE_BUF, __x)
+#define MCDE_ROTBCONF_WR_ROPC_SHIFT 16
+#define MCDE_ROTBCONF_WR_ROPC_MASK 0x00FF0000
+#define MCDE_ROTBCONF_WR_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, WR_ROPC, __x)
+#define MCDE_ROTBCONF_RD_ROPC_SHIFT 24
+#define MCDE_ROTBCONF_RD_ROPC_MASK 0xFF000000
+#define MCDE_ROTBCONF_RD_ROPC(__x) \
+	MCDE_VAL2REG(MCDE_ROTBCONF, RD_ROPC, __x)
+#define MCDE_SYNCHCONFA 0x00000880
+#define MCDE_SYNCHCONFA_GROUPOFFSET 0x200
+#define MCDE_SYNCHCONFA_HWREQVEVENT_SHIFT 0
+#define MCDE_SYNCHCONFA_HWREQVEVENT_MASK 0x00000003
+#define MCDE_SYNCHCONFA_HWREQVEVENT_VSYNC 0
+#define MCDE_SYNCHCONFA_HWREQVEVENT_BACK_PORCH 1
+#define MCDE_SYNCHCONFA_HWREQVEVENT_ACTIVE_VIDEO 2
+#define MCDE_SYNCHCONFA_HWREQVEVENT_FRONT_PORCH 3
+#define MCDE_SYNCHCONFA_HWREQVEVENT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, HWREQVEVENT, \
+	MCDE_SYNCHCONFA_HWREQVEVENT_##__x)
+#define MCDE_SYNCHCONFA_HWREQVEVENT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, HWREQVEVENT, __x)
+#define MCDE_SYNCHCONFA_HWREQVCNT_SHIFT 2
+#define MCDE_SYNCHCONFA_HWREQVCNT_MASK 0x0000FFFC
+#define MCDE_SYNCHCONFA_HWREQVCNT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, HWREQVCNT, __x)
+#define MCDE_SYNCHCONFA_SWINTVEVENT_SHIFT 16
+#define MCDE_SYNCHCONFA_SWINTVEVENT_MASK 0x00030000
+#define MCDE_SYNCHCONFA_SWINTVEVENT_VSYNC 0
+#define MCDE_SYNCHCONFA_SWINTVEVENT_BACK_PORCH 1
+#define MCDE_SYNCHCONFA_SWINTVEVENT_ACTIVE_VIDEO 2
+#define MCDE_SYNCHCONFA_SWINTVEVENT_FRONT_PORCH 3
+#define MCDE_SYNCHCONFA_SWINTVEVENT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, SWINTVEVENT, \
+	MCDE_SYNCHCONFA_SWINTVEVENT_##__x)
+#define MCDE_SYNCHCONFA_SWINTVEVENT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, SWINTVEVENT, __x)
+#define MCDE_SYNCHCONFA_SWINTVCNT_SHIFT 18
+#define MCDE_SYNCHCONFA_SWINTVCNT_MASK 0xFFFC0000
+#define MCDE_SYNCHCONFA_SWINTVCNT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFA, SWINTVCNT, __x)
+#define MCDE_SYNCHCONFB 0x00000A80
+#define MCDE_SYNCHCONFB_HWREQVEVENT_SHIFT 0
+#define MCDE_SYNCHCONFB_HWREQVEVENT_MASK 0x00000003
+#define MCDE_SYNCHCONFB_HWREQVEVENT_VSYNC 0
+#define MCDE_SYNCHCONFB_HWREQVEVENT_BACK_PORCH 1
+#define MCDE_SYNCHCONFB_HWREQVEVENT_ACTIVE_VIDEO 2
+#define MCDE_SYNCHCONFB_HWREQVEVENT_FRONT_PORCH 3
+#define MCDE_SYNCHCONFB_HWREQVEVENT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, HWREQVEVENT, \
+	MCDE_SYNCHCONFB_HWREQVEVENT_##__x)
+#define MCDE_SYNCHCONFB_HWREQVEVENT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, HWREQVEVENT, __x)
+#define MCDE_SYNCHCONFB_HWREQVCNT_SHIFT 2
+#define MCDE_SYNCHCONFB_HWREQVCNT_MASK 0x0000FFFC
+#define MCDE_SYNCHCONFB_HWREQVCNT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, HWREQVCNT, __x)
+#define MCDE_SYNCHCONFB_SWINTVEVENT_SHIFT 16
+#define MCDE_SYNCHCONFB_SWINTVEVENT_MASK 0x00030000
+#define MCDE_SYNCHCONFB_SWINTVEVENT_VSYNC 0
+#define MCDE_SYNCHCONFB_SWINTVEVENT_BACK_PORCH 1
+#define MCDE_SYNCHCONFB_SWINTVEVENT_ACTIVE_VIDEO 2
+#define MCDE_SYNCHCONFB_SWINTVEVENT_FRONT_PORCH 3
+#define MCDE_SYNCHCONFB_SWINTVEVENT_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, SWINTVEVENT, \
+	MCDE_SYNCHCONFB_SWINTVEVENT_##__x)
+#define MCDE_SYNCHCONFB_SWINTVEVENT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, SWINTVEVENT, __x)
+#define MCDE_SYNCHCONFB_SWINTVCNT_SHIFT 18
+#define MCDE_SYNCHCONFB_SWINTVCNT_MASK 0xFFFC0000
+#define MCDE_SYNCHCONFB_SWINTVCNT(__x) \
+	MCDE_VAL2REG(MCDE_SYNCHCONFB, SWINTVCNT, __x)
+#define MCDE_CTRLA 0x00000884
+#define MCDE_CTRLA_GROUPOFFSET 0x200
+#define MCDE_CTRLA_FIFOWTRMRK_SHIFT 0
+#define MCDE_CTRLA_FIFOWTRMRK_MASK 0x000003FF
+#define MCDE_CTRLA_FIFOWTRMRK(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FIFOWTRMRK, __x)
+#define MCDE_CTRLA_FIFOEMPTY_SHIFT 12
+#define MCDE_CTRLA_FIFOEMPTY_MASK 0x00001000
+#define MCDE_CTRLA_FIFOEMPTY(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FIFOEMPTY, __x)
+#define MCDE_CTRLA_FIFOFULL_SHIFT 13
+#define MCDE_CTRLA_FIFOFULL_MASK 0x00002000
+#define MCDE_CTRLA_FIFOFULL(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FIFOFULL, __x)
+#define MCDE_CTRLA_FORMID_SHIFT 16
+#define MCDE_CTRLA_FORMID_MASK 0x00070000
+#define MCDE_CTRLA_FORMID_DSI0VID 0
+#define MCDE_CTRLA_FORMID_DSI0CMD 1
+#define MCDE_CTRLA_FORMID_DSI1VID 2
+#define MCDE_CTRLA_FORMID_DSI1CMD 3
+#define MCDE_CTRLA_FORMID_DSI2VID 4
+#define MCDE_CTRLA_FORMID_DSI2CMD 5
+#define MCDE_CTRLA_FORMID_DPIA 0
+#define MCDE_CTRLA_FORMID_DPIB 1
+#define MCDE_CTRLA_FORMID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FORMID, MCDE_CTRLA_FORMID_##__x)
+#define MCDE_CTRLA_FORMID(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FORMID, __x)
+#define MCDE_CTRLA_FORMTYPE_SHIFT 20
+#define MCDE_CTRLA_FORMTYPE_MASK 0x00700000
+#define MCDE_CTRLA_FORMTYPE_DPITV 0
+#define MCDE_CTRLA_FORMTYPE_DBI 1
+#define MCDE_CTRLA_FORMTYPE_DSI 2
+#define MCDE_CTRLA_FORMTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FORMTYPE, MCDE_CTRLA_FORMTYPE_##__x)
+#define MCDE_CTRLA_FORMTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CTRLA, FORMTYPE, __x)
+#define MCDE_CTRLB 0x00000A84
+#define MCDE_CTRLB_FIFOWTRMRK_SHIFT 0
+#define MCDE_CTRLB_FIFOWTRMRK_MASK 0x000003FF
+#define MCDE_CTRLB_FIFOWTRMRK(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FIFOWTRMRK, __x)
+#define MCDE_CTRLB_FIFOEMPTY_SHIFT 12
+#define MCDE_CTRLB_FIFOEMPTY_MASK 0x00001000
+#define MCDE_CTRLB_FIFOEMPTY(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FIFOEMPTY, __x)
+#define MCDE_CTRLB_FIFOFULL_SHIFT 13
+#define MCDE_CTRLB_FIFOFULL_MASK 0x00002000
+#define MCDE_CTRLB_FIFOFULL(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FIFOFULL, __x)
+#define MCDE_CTRLB_FORMID_SHIFT 16
+#define MCDE_CTRLB_FORMID_MASK 0x00070000
+#define MCDE_CTRLB_FORMID_DSI0VID 0
+#define MCDE_CTRLB_FORMID_DSI0CMD 1
+#define MCDE_CTRLB_FORMID_DSI1VID 2
+#define MCDE_CTRLB_FORMID_DSI1CMD 3
+#define MCDE_CTRLB_FORMID_DSI2VID 4
+#define MCDE_CTRLB_FORMID_DSI2CMD 5
+#define MCDE_CTRLB_FORMID_DPIA 0
+#define MCDE_CTRLB_FORMID_DPIB 1
+#define MCDE_CTRLB_FORMID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FORMID, MCDE_CTRLB_FORMID_##__x)
+#define MCDE_CTRLB_FORMID(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FORMID, __x)
+#define MCDE_CTRLB_FORMTYPE_SHIFT 20
+#define MCDE_CTRLB_FORMTYPE_MASK 0x00700000
+#define MCDE_CTRLB_FORMTYPE_DPITV 0
+#define MCDE_CTRLB_FORMTYPE_DBI 1
+#define MCDE_CTRLB_FORMTYPE_DSI 2
+#define MCDE_CTRLB_FORMTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FORMTYPE, MCDE_CTRLB_FORMTYPE_##__x)
+#define MCDE_CTRLB_FORMTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CTRLB, FORMTYPE, __x)
+#define MCDE_GAM0A 0x00000888
+#define MCDE_GAM0A_GROUPOFFSET 0x200
+#define MCDE_GAM0A_BLUE_SHIFT 0
+#define MCDE_GAM0A_BLUE_MASK 0x00FFFFFF
+#define MCDE_GAM0A_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_GAM0A, BLUE, __x)
+#define MCDE_GAM0B 0x00000A88
+#define MCDE_GAM0B_BLUE_SHIFT 0
+#define MCDE_GAM0B_BLUE_MASK 0x00FFFFFF
+#define MCDE_GAM0B_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_GAM0B, BLUE, __x)
+#define MCDE_GAM1A 0x0000088C
+#define MCDE_GAM1A_GROUPOFFSET 0x200
+#define MCDE_GAM1A_GREEN_SHIFT 0
+#define MCDE_GAM1A_GREEN_MASK 0x00FFFFFF
+#define MCDE_GAM1A_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_GAM1A, GREEN, __x)
+#define MCDE_GAM1B 0x00000A8C
+#define MCDE_GAM1B_GREEN_SHIFT 0
+#define MCDE_GAM1B_GREEN_MASK 0x00FFFFFF
+#define MCDE_GAM1B_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_GAM1B, GREEN, __x)
+#define MCDE_GAM2A 0x00000890
+#define MCDE_GAM2A_GROUPOFFSET 0x200
+#define MCDE_GAM2A_RED_SHIFT 0
+#define MCDE_GAM2A_RED_MASK 0x00FFFFFF
+#define MCDE_GAM2A_RED(__x) \
+	MCDE_VAL2REG(MCDE_GAM2A, RED, __x)
+#define MCDE_GAM2B 0x00000A90
+#define MCDE_GAM2B_RED_SHIFT 0
+#define MCDE_GAM2B_RED_MASK 0x00FFFFFF
+#define MCDE_GAM2B_RED(__x) \
+	MCDE_VAL2REG(MCDE_GAM2B, RED, __x)
+#define MCDE_OLEDCONV1A 0x00000894
+#define MCDE_OLEDCONV1A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV1A_ALPHA_RED_SHIFT 0
+#define MCDE_OLEDCONV1A_ALPHA_RED_MASK 0x00003FFF
+#define MCDE_OLEDCONV1A_ALPHA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV1A, ALPHA_RED, __x)
+#define MCDE_OLEDCONV1A_ALPHA_GREEN_SHIFT 16
+#define MCDE_OLEDCONV1A_ALPHA_GREEN_MASK 0x3FFF0000
+#define MCDE_OLEDCONV1A_ALPHA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV1A, ALPHA_GREEN, __x)
+#define MCDE_OLEDCONV1B 0x00000A94
+#define MCDE_OLEDCONV1B_ALPHA_RED_SHIFT 0
+#define MCDE_OLEDCONV1B_ALPHA_RED_MASK 0x00003FFF
+#define MCDE_OLEDCONV1B_ALPHA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV1B, ALPHA_RED, __x)
+#define MCDE_OLEDCONV1B_ALPHA_GREEN_SHIFT 16
+#define MCDE_OLEDCONV1B_ALPHA_GREEN_MASK 0x3FFF0000
+#define MCDE_OLEDCONV1B_ALPHA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV1B, ALPHA_GREEN, __x)
+#define MCDE_OLEDCONV2A 0x00000898
+#define MCDE_OLEDCONV2A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV2A_ALPHA_BLUE_SHIFT 0
+#define MCDE_OLEDCONV2A_ALPHA_BLUE_MASK 0x00003FFF
+#define MCDE_OLEDCONV2A_ALPHA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV2A, ALPHA_BLUE, __x)
+#define MCDE_OLEDCONV2A_BETA_RED_SHIFT 16
+#define MCDE_OLEDCONV2A_BETA_RED_MASK 0x3FFF0000
+#define MCDE_OLEDCONV2A_BETA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV2A, BETA_RED, __x)
+#define MCDE_OLEDCONV2B 0x00000A98
+#define MCDE_OLEDCONV2B_ALPHA_BLUE_SHIFT 0
+#define MCDE_OLEDCONV2B_ALPHA_BLUE_MASK 0x00003FFF
+#define MCDE_OLEDCONV2B_ALPHA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV2B, ALPHA_BLUE, __x)
+#define MCDE_OLEDCONV2B_BETA_RED_SHIFT 16
+#define MCDE_OLEDCONV2B_BETA_RED_MASK 0x3FFF0000
+#define MCDE_OLEDCONV2B_BETA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV2B, BETA_RED, __x)
+#define MCDE_OLEDCONV3A 0x0000089C
+#define MCDE_OLEDCONV3A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV3A_BETA_GREEN_SHIFT 0
+#define MCDE_OLEDCONV3A_BETA_GREEN_MASK 0x00003FFF
+#define MCDE_OLEDCONV3A_BETA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV3A, BETA_GREEN, __x)
+#define MCDE_OLEDCONV3A_BETA_BLUE_SHIFT 16
+#define MCDE_OLEDCONV3A_BETA_BLUE_MASK 0x3FFF0000
+#define MCDE_OLEDCONV3A_BETA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV3A, BETA_BLUE, __x)
+#define MCDE_OLEDCONV3B 0x00000A9C
+#define MCDE_OLEDCONV3B_BETA_GREEN_SHIFT 0
+#define MCDE_OLEDCONV3B_BETA_GREEN_MASK 0x00003FFF
+#define MCDE_OLEDCONV3B_BETA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV3B, BETA_GREEN, __x)
+#define MCDE_OLEDCONV3B_BETA_BLUE_SHIFT 16
+#define MCDE_OLEDCONV3B_BETA_BLUE_MASK 0x3FFF0000
+#define MCDE_OLEDCONV3B_BETA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV3B, BETA_BLUE, __x)
+#define MCDE_OLEDCONV4A 0x000008A0
+#define MCDE_OLEDCONV4A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV4A_GAMMA_RED_SHIFT 0
+#define MCDE_OLEDCONV4A_GAMMA_RED_MASK 0x00003FFF
+#define MCDE_OLEDCONV4A_GAMMA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV4A, GAMMA_RED, __x)
+#define MCDE_OLEDCONV4A_GAMMA_GREEN_SHIFT 16
+#define MCDE_OLEDCONV4A_GAMMA_GREEN_MASK 0x3FFF0000
+#define MCDE_OLEDCONV4A_GAMMA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV4A, GAMMA_GREEN, __x)
+#define MCDE_OLEDCONV4B 0x00000AA0
+#define MCDE_OLEDCONV4B_GAMMA_RED_SHIFT 0
+#define MCDE_OLEDCONV4B_GAMMA_RED_MASK 0x00003FFF
+#define MCDE_OLEDCONV4B_GAMMA_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV4B, GAMMA_RED, __x)
+#define MCDE_OLEDCONV4B_GAMMA_GREEN_SHIFT 16
+#define MCDE_OLEDCONV4B_GAMMA_GREEN_MASK 0x3FFF0000
+#define MCDE_OLEDCONV4B_GAMMA_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV4B, GAMMA_GREEN, __x)
+#define MCDE_OLEDCONV5A 0x000008A4
+#define MCDE_OLEDCONV5A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV5A_GAMMA_BLUE_SHIFT 0
+#define MCDE_OLEDCONV5A_GAMMA_BLUE_MASK 0x00003FFF
+#define MCDE_OLEDCONV5A_GAMMA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV5A, GAMMA_BLUE, __x)
+#define MCDE_OLEDCONV5A_OFF_RED_SHIFT 16
+#define MCDE_OLEDCONV5A_OFF_RED_MASK 0x3FFF0000
+#define MCDE_OLEDCONV5A_OFF_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV5A, OFF_RED, __x)
+#define MCDE_OLEDCONV5B 0x00000AA4
+#define MCDE_OLEDCONV5B_GAMMA_BLUE_SHIFT 0
+#define MCDE_OLEDCONV5B_GAMMA_BLUE_MASK 0x00003FFF
+#define MCDE_OLEDCONV5B_GAMMA_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV5B, GAMMA_BLUE, __x)
+#define MCDE_OLEDCONV5B_OFF_RED_SHIFT 16
+#define MCDE_OLEDCONV5B_OFF_RED_MASK 0x3FFF0000
+#define MCDE_OLEDCONV5B_OFF_RED(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV5B, OFF_RED, __x)
+#define MCDE_OLEDCONV6A 0x000008A8
+#define MCDE_OLEDCONV6A_GROUPOFFSET 0x200
+#define MCDE_OLEDCONV6A_OFF_GREEN_SHIFT 0
+#define MCDE_OLEDCONV6A_OFF_GREEN_MASK 0x00003FFF
+#define MCDE_OLEDCONV6A_OFF_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV6A, OFF_GREEN, __x)
+#define MCDE_OLEDCONV6A_OFF_BLUE_SHIFT 16
+#define MCDE_OLEDCONV6A_OFF_BLUE_MASK 0x3FFF0000
+#define MCDE_OLEDCONV6A_OFF_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV6A, OFF_BLUE, __x)
+#define MCDE_OLEDCONV6B 0x00000AA8
+#define MCDE_OLEDCONV6B_OFF_GREEN_SHIFT 0
+#define MCDE_OLEDCONV6B_OFF_GREEN_MASK 0x00003FFF
+#define MCDE_OLEDCONV6B_OFF_GREEN(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV6B, OFF_GREEN, __x)
+#define MCDE_OLEDCONV6B_OFF_BLUE_SHIFT 16
+#define MCDE_OLEDCONV6B_OFF_BLUE_MASK 0x3FFF0000
+#define MCDE_OLEDCONV6B_OFF_BLUE(__x) \
+	MCDE_VAL2REG(MCDE_OLEDCONV6B, OFF_BLUE, __x)
+#define MCDE_CRC 0x00000C00
+#define MCDE_CRC_C1EN_SHIFT 2
+#define MCDE_CRC_C1EN_MASK 0x00000004
+#define MCDE_CRC_C1EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, C1EN, __x)
+#define MCDE_CRC_C2EN_SHIFT 3
+#define MCDE_CRC_C2EN_MASK 0x00000008
+#define MCDE_CRC_C2EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, C2EN, __x)
+#define MCDE_CRC_SYCEN0_SHIFT 7
+#define MCDE_CRC_SYCEN0_MASK 0x00000080
+#define MCDE_CRC_SYCEN0(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SYCEN0, __x)
+#define MCDE_CRC_SYCEN1_SHIFT 8
+#define MCDE_CRC_SYCEN1_MASK 0x00000100
+#define MCDE_CRC_SYCEN1(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SYCEN1, __x)
+#define MCDE_CRC_SIZE1_SHIFT 9
+#define MCDE_CRC_SIZE1_MASK 0x00000200
+#define MCDE_CRC_SIZE1(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SIZE1, __x)
+#define MCDE_CRC_SIZE2_SHIFT 10
+#define MCDE_CRC_SIZE2_MASK 0x00000400
+#define MCDE_CRC_SIZE2(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SIZE2, __x)
+#define MCDE_CRC_YUVCONVC1EN_SHIFT 15
+#define MCDE_CRC_YUVCONVC1EN_MASK 0x00008000
+#define MCDE_CRC_YUVCONVC1EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, YUVCONVC1EN, __x)
+#define MCDE_CRC_CS1EN_SHIFT 16
+#define MCDE_CRC_CS1EN_MASK 0x00010000
+#define MCDE_CRC_CS1EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CS1EN, __x)
+#define MCDE_CRC_CS2EN_SHIFT 17
+#define MCDE_CRC_CS2EN_MASK 0x00020000
+#define MCDE_CRC_CS2EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CS2EN, __x)
+#define MCDE_CRC_CS1POL_SHIFT 19
+#define MCDE_CRC_CS1POL_MASK 0x00080000
+#define MCDE_CRC_CS1POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CS1POL, __x)
+#define MCDE_CRC_CS2POL_SHIFT 20
+#define MCDE_CRC_CS2POL_MASK 0x00100000
+#define MCDE_CRC_CS2POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CS2POL, __x)
+#define MCDE_CRC_CD1POL_SHIFT 21
+#define MCDE_CRC_CD1POL_MASK 0x00200000
+#define MCDE_CRC_CD1POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CD1POL, __x)
+#define MCDE_CRC_CD2POL_SHIFT 22
+#define MCDE_CRC_CD2POL_MASK 0x00400000
+#define MCDE_CRC_CD2POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CD2POL, __x)
+#define MCDE_CRC_WR1POL_SHIFT 23
+#define MCDE_CRC_WR1POL_MASK 0x00800000
+#define MCDE_CRC_WR1POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, WR1POL, __x)
+#define MCDE_CRC_WR2POL_SHIFT 24
+#define MCDE_CRC_WR2POL_MASK 0x01000000
+#define MCDE_CRC_WR2POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, WR2POL, __x)
+#define MCDE_CRC_RD1POL_SHIFT 25
+#define MCDE_CRC_RD1POL_MASK 0x02000000
+#define MCDE_CRC_RD1POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, RD1POL, __x)
+#define MCDE_CRC_RD2POL_SHIFT 26
+#define MCDE_CRC_RD2POL_MASK 0x04000000
+#define MCDE_CRC_RD2POL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, RD2POL, __x)
+#define MCDE_CRC_SYNCCTRL_SHIFT 29
+#define MCDE_CRC_SYNCCTRL_MASK 0x60000000
+#define MCDE_CRC_SYNCCTRL_NO_SYNC 0
+#define MCDE_CRC_SYNCCTRL_DBI0 1
+#define MCDE_CRC_SYNCCTRL_DBI1 2
+#define MCDE_CRC_SYNCCTRL_PING_PONG 3
+#define MCDE_CRC_SYNCCTRL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SYNCCTRL, MCDE_CRC_SYNCCTRL_##__x)
+#define MCDE_CRC_SYNCCTRL(__x) \
+	MCDE_VAL2REG(MCDE_CRC, SYNCCTRL, __x)
+#define MCDE_CRC_CLAMPC1EN_SHIFT 31
+#define MCDE_CRC_CLAMPC1EN_MASK 0x80000000
+#define MCDE_CRC_CLAMPC1EN(__x) \
+	MCDE_VAL2REG(MCDE_CRC, CLAMPC1EN, __x)
+#define MCDE_PBCCRC0 0x00000C04
+#define MCDE_PBCCRC0_GROUPOFFSET 0x4
+#define MCDE_PBCCRC0_BSCM_SHIFT 0
+#define MCDE_PBCCRC0_BSCM_MASK 0x00000007
+#define MCDE_PBCCRC0_BSCM_1_8BIT 0
+#define MCDE_PBCCRC0_BSCM_2_8BIT 1
+#define MCDE_PBCCRC0_BSCM_3_8BIT 2
+#define MCDE_PBCCRC0_BSCM_1_16BIT 3
+#define MCDE_PBCCRC0_BSCM_2_16BIT 4
+#define MCDE_PBCCRC0_BSCM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BSCM, MCDE_PBCCRC0_BSCM_##__x)
+#define MCDE_PBCCRC0_BSCM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BSCM, __x)
+#define MCDE_PBCCRC0_BSDM_SHIFT 3
+#define MCDE_PBCCRC0_BSDM_MASK 0x00000038
+#define MCDE_PBCCRC0_BSDM_1_8BIT 0
+#define MCDE_PBCCRC0_BSDM_2_8BIT 1
+#define MCDE_PBCCRC0_BSDM_3_8BIT 2
+#define MCDE_PBCCRC0_BSDM_1_16BIT 3
+#define MCDE_PBCCRC0_BSDM_2_16BIT 4
+#define MCDE_PBCCRC0_BSDM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BSDM, MCDE_PBCCRC0_BSDM_##__x)
+#define MCDE_PBCCRC0_BSDM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BSDM, __x)
+#define MCDE_PBCCRC0_PDM_SHIFT 6
+#define MCDE_PBCCRC0_PDM_MASK 0x000000C0
+#define MCDE_PBCCRC0_PDM_NORMAL 0
+#define MCDE_PBCCRC0_PDM_16_TO_32 1
+#define MCDE_PBCCRC0_PDM_24_TO_32_RIGHT 2
+#define MCDE_PBCCRC0_PDM_24_TO_32_LEFT 3
+#define MCDE_PBCCRC0_PDM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, PDM, MCDE_PBCCRC0_PDM_##__x)
+#define MCDE_PBCCRC0_PDM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, PDM, __x)
+#define MCDE_PBCCRC0_PDCTRL_SHIFT 12
+#define MCDE_PBCCRC0_PDCTRL_MASK 0x00001000
+#define MCDE_PBCCRC0_PDCTRL(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, PDCTRL, __x)
+#define MCDE_PBCCRC0_BPP_SHIFT 13
+#define MCDE_PBCCRC0_BPP_MASK 0x0000E000
+#define MCDE_PBCCRC0_BPP_8BPP 0
+#define MCDE_PBCCRC0_BPP_12BPP 1
+#define MCDE_PBCCRC0_BPP_15BPP 2
+#define MCDE_PBCCRC0_BPP_16BPP 3
+#define MCDE_PBCCRC0_BPP_18BPP 4
+#define MCDE_PBCCRC0_BPP_24BPP 5
+#define MCDE_PBCCRC0_BPP(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC0, BPP, __x)
+#define MCDE_PBCCRC1 0x00000C08
+#define MCDE_PBCCRC1_BSCM_SHIFT 0
+#define MCDE_PBCCRC1_BSCM_MASK 0x00000007
+#define MCDE_PBCCRC1_BSCM_1_8BIT 0
+#define MCDE_PBCCRC1_BSCM_2_8BIT 1
+#define MCDE_PBCCRC1_BSCM_3_8BIT 2
+#define MCDE_PBCCRC1_BSCM_1_16BIT 3
+#define MCDE_PBCCRC1_BSCM_2_16BIT 4
+#define MCDE_PBCCRC1_BSCM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BSCM, MCDE_PBCCRC1_BSCM_##__x)
+#define MCDE_PBCCRC1_BSCM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BSCM, __x)
+#define MCDE_PBCCRC1_BSDM_SHIFT 3
+#define MCDE_PBCCRC1_BSDM_MASK 0x00000038
+#define MCDE_PBCCRC1_BSDM_1_8BIT 0
+#define MCDE_PBCCRC1_BSDM_2_8BIT 1
+#define MCDE_PBCCRC1_BSDM_3_8BIT 2
+#define MCDE_PBCCRC1_BSDM_1_16BIT 3
+#define MCDE_PBCCRC1_BSDM_2_16BIT 4
+#define MCDE_PBCCRC1_BSDM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BSDM, MCDE_PBCCRC1_BSDM_##__x)
+#define MCDE_PBCCRC1_BSDM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BSDM, __x)
+#define MCDE_PBCCRC1_PDM_SHIFT 6
+#define MCDE_PBCCRC1_PDM_MASK 0x000000C0
+#define MCDE_PBCCRC1_PDM_NORMAL 0
+#define MCDE_PBCCRC1_PDM_16_TO_32 1
+#define MCDE_PBCCRC1_PDM_24_TO_32_RIGHT 2
+#define MCDE_PBCCRC1_PDM_24_TO_32_LEFT 3
+#define MCDE_PBCCRC1_PDM_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, PDM, MCDE_PBCCRC1_PDM_##__x)
+#define MCDE_PBCCRC1_PDM(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, PDM, __x)
+#define MCDE_PBCCRC1_PDCTRL_SHIFT 12
+#define MCDE_PBCCRC1_PDCTRL_MASK 0x00001000
+#define MCDE_PBCCRC1_PDCTRL(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, PDCTRL, __x)
+#define MCDE_PBCCRC1_BPP_SHIFT 13
+#define MCDE_PBCCRC1_BPP_MASK 0x0000E000
+#define MCDE_PBCCRC1_BPP_8BPP 0
+#define MCDE_PBCCRC1_BPP_12BPP 1
+#define MCDE_PBCCRC1_BPP_15BPP 2
+#define MCDE_PBCCRC1_BPP_16BPP 3
+#define MCDE_PBCCRC1_BPP_18BPP 4
+#define MCDE_PBCCRC1_BPP_24BPP 5
+#define MCDE_PBCCRC1_BPP(__x) \
+	MCDE_VAL2REG(MCDE_PBCCRC1, BPP, __x)
+#define MCDE_PBCBMRC00 0x00000C0C
+#define MCDE_PBCBMRC00_GROUPOFFSET 0x4
+#define MCDE_PBCBMRC00_MUXI_SHIFT 0
+#define MCDE_PBCBMRC00_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC00_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC00, MUXI, __x)
+#define MCDE_PBCBMRC01 0x00000C10
+#define MCDE_PBCBMRC01_MUXI_SHIFT 0
+#define MCDE_PBCBMRC01_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC01_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC01, MUXI, __x)
+#define MCDE_PBCBMRC02 0x00000C14
+#define MCDE_PBCBMRC02_MUXI_SHIFT 0
+#define MCDE_PBCBMRC02_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC02_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC02, MUXI, __x)
+#define MCDE_PBCBMRC03 0x00000C18
+#define MCDE_PBCBMRC03_MUXI_SHIFT 0
+#define MCDE_PBCBMRC03_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC03_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC03, MUXI, __x)
+#define MCDE_PBCBMRC04 0x00000C1C
+#define MCDE_PBCBMRC04_MUXI_SHIFT 0
+#define MCDE_PBCBMRC04_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC04_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC04, MUXI, __x)
+#define MCDE_PBCBMRC10 0x00000C20
+#define MCDE_PBCBMRC10_MUXI_SHIFT 0
+#define MCDE_PBCBMRC10_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC10_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC10, MUXI, __x)
+#define MCDE_PBCBMRC11 0x00000C24
+#define MCDE_PBCBMRC11_MUXI_SHIFT 0
+#define MCDE_PBCBMRC11_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC11_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC11, MUXI, __x)
+#define MCDE_PBCBMRC12 0x00000C28
+#define MCDE_PBCBMRC12_MUXI_SHIFT 0
+#define MCDE_PBCBMRC12_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC12_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC12, MUXI, __x)
+#define MCDE_PBCBMRC13 0x00000C2C
+#define MCDE_PBCBMRC13_MUXI_SHIFT 0
+#define MCDE_PBCBMRC13_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC13_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC13, MUXI, __x)
+#define MCDE_PBCBMRC14 0x00000C30
+#define MCDE_PBCBMRC14_MUXI_SHIFT 0
+#define MCDE_PBCBMRC14_MUXI_MASK 0xFFFFFFFF
+#define MCDE_PBCBMRC14_MUXI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBMRC14, MUXI, __x)
+#define MCDE_PBCBCRC00 0x00000C34
+#define MCDE_PBCBCRC00_GROUPOFFSET 0x4
+#define MCDE_PBCBCRC00_CTLI_SHIFT 0
+#define MCDE_PBCBCRC00_CTLI_MASK 0xFFFFFFFF
+#define MCDE_PBCBCRC00_CTLI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBCRC00, CTLI, __x)
+#define MCDE_PBCBCRC10 0x00000C38
+#define MCDE_PBCBCRC10_CTLI_SHIFT 0
+#define MCDE_PBCBCRC10_CTLI_MASK 0xFFFFFFFF
+#define MCDE_PBCBCRC10_CTLI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBCRC10, CTLI, __x)
+#define MCDE_PBCBCRC01 0x00000C48
+#define MCDE_PBCBCRC01_GROUPOFFSET 0x4
+#define MCDE_PBCBCRC01_CTLI_SHIFT 0
+#define MCDE_PBCBCRC01_CTLI_MASK 0xFFFFFFFF
+#define MCDE_PBCBCRC01_CTLI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBCRC01, CTLI, __x)
+#define MCDE_PBCBCRC11 0x00000C4C
+#define MCDE_PBCBCRC11_CTLI_SHIFT 0
+#define MCDE_PBCBCRC11_CTLI_MASK 0xFFFFFFFF
+#define MCDE_PBCBCRC11_CTLI(__x) \
+	MCDE_VAL2REG(MCDE_PBCBCRC11, CTLI, __x)
+#define MCDE_VSCRC0 0x00000C5C
+#define MCDE_VSCRC0_GROUPOFFSET 0x4
+#define MCDE_VSCRC0_VSPMIN_SHIFT 0
+#define MCDE_VSCRC0_VSPMIN_MASK 0x00000FFF
+#define MCDE_VSCRC0_VSPMIN(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPMIN, __x)
+#define MCDE_VSCRC0_VSPMAX_SHIFT 12
+#define MCDE_VSCRC0_VSPMAX_MASK 0x00FFF000
+#define MCDE_VSCRC0_VSPMAX(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPMAX, __x)
+#define MCDE_VSCRC0_VSPDIV_SHIFT 24
+#define MCDE_VSCRC0_VSPDIV_MASK 0x07000000
+#define MCDE_VSCRC0_VSPDIV_MCDECLK_DIV_1 0
+#define MCDE_VSCRC0_VSPDIV_MCDECLK_DIV_2 1
+#define MCDE_VSCRC0_VSPDIV_MCDECLK_DIV_4 2
+#define MCDE_VSCRC0_VSPDIV_MCDECLK_DIV_8 3
+#define MCDE_VSCRC0_VSPDIV_MCDECLK_DIV_16 4
+#define MCDE_VSCRC0_VSPDIV_MCDECLK_DIV_32 5
+#define MCDE_VSCRC0_VSPDIV_MCDECLK_DIV_64 6
+#define MCDE_VSCRC0_VSPDIV_MCDECLK_DIV_128 7
+#define MCDE_VSCRC0_VSPDIV_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPDIV, MCDE_VSCRC0_VSPDIV_##__x)
+#define MCDE_VSCRC0_VSPDIV(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPDIV, __x)
+#define MCDE_VSCRC0_VSPOL_SHIFT 27
+#define MCDE_VSCRC0_VSPOL_MASK 0x08000000
+#define MCDE_VSCRC0_VSPOL_ACTIVE_HIGH 0
+#define MCDE_VSCRC0_VSPOL_ACTIVE_LOW 1
+#define MCDE_VSCRC0_VSPOL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPOL, MCDE_VSCRC0_VSPOL_##__x)
+#define MCDE_VSCRC0_VSPOL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSPOL, __x)
+#define MCDE_VSCRC0_VSSEL_SHIFT 28
+#define MCDE_VSCRC0_VSSEL_MASK 0x10000000
+#define MCDE_VSCRC0_VSSEL_VSYNC0 0
+#define MCDE_VSCRC0_VSSEL_VSYNC1 1
+#define MCDE_VSCRC0_VSSEL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSSEL, MCDE_VSCRC0_VSSEL_##__x)
+#define MCDE_VSCRC0_VSSEL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSSEL, __x)
+#define MCDE_VSCRC0_VSDBL_SHIFT 29
+#define MCDE_VSCRC0_VSDBL_MASK 0xE0000000
+#define MCDE_VSCRC0_VSDBL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC0, VSDBL, __x)
+#define MCDE_VSCRC1 0x00000C60
+#define MCDE_VSCRC1_VSPMIN_SHIFT 0
+#define MCDE_VSCRC1_VSPMIN_MASK 0x00000FFF
+#define MCDE_VSCRC1_VSPMIN(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPMIN, __x)
+#define MCDE_VSCRC1_VSPMAX_SHIFT 12
+#define MCDE_VSCRC1_VSPMAX_MASK 0x00FFF000
+#define MCDE_VSCRC1_VSPMAX(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPMAX, __x)
+#define MCDE_VSCRC1_VSPDIV_SHIFT 24
+#define MCDE_VSCRC1_VSPDIV_MASK 0x07000000
+#define MCDE_VSCRC1_VSPDIV_MCDECLK_DIV_1 0
+#define MCDE_VSCRC1_VSPDIV_MCDECLK_DIV_2 1
+#define MCDE_VSCRC1_VSPDIV_MCDECLK_DIV_4 2
+#define MCDE_VSCRC1_VSPDIV_MCDECLK_DIV_8 3
+#define MCDE_VSCRC1_VSPDIV_MCDECLK_DIV_16 4
+#define MCDE_VSCRC1_VSPDIV_MCDECLK_DIV_32 5
+#define MCDE_VSCRC1_VSPDIV_MCDECLK_DIV_64 6
+#define MCDE_VSCRC1_VSPDIV_MCDECLK_DIV_128 7
+#define MCDE_VSCRC1_VSPDIV_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPDIV, MCDE_VSCRC1_VSPDIV_##__x)
+#define MCDE_VSCRC1_VSPDIV(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPDIV, __x)
+#define MCDE_VSCRC1_VSPOL_SHIFT 27
+#define MCDE_VSCRC1_VSPOL_MASK 0x08000000
+#define MCDE_VSCRC1_VSPOL_ACTIVE_HIGH 0
+#define MCDE_VSCRC1_VSPOL_ACTIVE_LOW 1
+#define MCDE_VSCRC1_VSPOL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPOL, MCDE_VSCRC1_VSPOL_##__x)
+#define MCDE_VSCRC1_VSPOL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSPOL, __x)
+#define MCDE_VSCRC1_VSSEL_SHIFT 28
+#define MCDE_VSCRC1_VSSEL_MASK 0x10000000
+#define MCDE_VSCRC1_VSSEL_VSYNC0 0
+#define MCDE_VSCRC1_VSSEL_VSYNC1 1
+#define MCDE_VSCRC1_VSSEL_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSSEL, MCDE_VSCRC1_VSSEL_##__x)
+#define MCDE_VSCRC1_VSSEL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSSEL, __x)
+#define MCDE_VSCRC1_VSDBL_SHIFT 29
+#define MCDE_VSCRC1_VSDBL_MASK 0xE0000000
+#define MCDE_VSCRC1_VSDBL(__x) \
+	MCDE_VAL2REG(MCDE_VSCRC1, VSDBL, __x)
+#define MCDE_SCTRC 0x00000C64
+#define MCDE_SCTRC_SYNCDELC0_SHIFT 0
+#define MCDE_SCTRC_SYNCDELC0_MASK 0x000000FF
+#define MCDE_SCTRC_SYNCDELC0(__x) \
+	MCDE_VAL2REG(MCDE_SCTRC, SYNCDELC0, __x)
+#define MCDE_SCTRC_SYNCDELC1_SHIFT 8
+#define MCDE_SCTRC_SYNCDELC1_MASK 0x0000FF00
+#define MCDE_SCTRC_SYNCDELC1(__x) \
+	MCDE_VAL2REG(MCDE_SCTRC, SYNCDELC1, __x)
+#define MCDE_SCTRC_TRDELC_SHIFT 16
+#define MCDE_SCTRC_TRDELC_MASK 0x0FFF0000
+#define MCDE_SCTRC_TRDELC(__x) \
+	MCDE_VAL2REG(MCDE_SCTRC, TRDELC, __x)
+#define MCDE_SCSRC 0x00000C68
+#define MCDE_SCSRC_VSTAC0_SHIFT 0
+#define MCDE_SCSRC_VSTAC0_MASK 0x00000001
+#define MCDE_SCSRC_VSTAC0(__x) \
+	MCDE_VAL2REG(MCDE_SCSRC, VSTAC0, __x)
+#define MCDE_SCSRC_VSTAC1_SHIFT 1
+#define MCDE_SCSRC_VSTAC1_MASK 0x00000002
+#define MCDE_SCSRC_VSTAC1(__x) \
+	MCDE_VAL2REG(MCDE_SCSRC, VSTAC1, __x)
+#define MCDE_BCNR0 0x00000C6C
+#define MCDE_BCNR0_GROUPOFFSET 0x4
+#define MCDE_BCNR0_BCN_SHIFT 0
+#define MCDE_BCNR0_BCN_MASK 0x000000FF
+#define MCDE_BCNR0_BCN(__x) \
+	MCDE_VAL2REG(MCDE_BCNR0, BCN, __x)
+#define MCDE_BCNR1 0x00000C70
+#define MCDE_BCNR1_BCN_SHIFT 0
+#define MCDE_BCNR1_BCN_MASK 0x000000FF
+#define MCDE_BCNR1_BCN(__x) \
+	MCDE_VAL2REG(MCDE_BCNR1, BCN, __x)
+#define MCDE_CSCDTR0 0x00000C74
+#define MCDE_CSCDTR0_GROUPOFFSET 0x4
+#define MCDE_CSCDTR0_CSACT_SHIFT 0
+#define MCDE_CSCDTR0_CSACT_MASK 0x000000FF
+#define MCDE_CSCDTR0_CSACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR0, CSACT, __x)
+#define MCDE_CSCDTR0_CSDEACT_SHIFT 8
+#define MCDE_CSCDTR0_CSDEACT_MASK 0x0000FF00
+#define MCDE_CSCDTR0_CSDEACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR0, CSDEACT, __x)
+#define MCDE_CSCDTR0_CDACT_SHIFT 16
+#define MCDE_CSCDTR0_CDACT_MASK 0x00FF0000
+#define MCDE_CSCDTR0_CDACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR0, CDACT, __x)
+#define MCDE_CSCDTR0_CDDEACT_SHIFT 24
+#define MCDE_CSCDTR0_CDDEACT_MASK 0xFF000000
+#define MCDE_CSCDTR0_CDDEACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR0, CDDEACT, __x)
+#define MCDE_CSCDTR1 0x00000C78
+#define MCDE_CSCDTR1_CSACT_SHIFT 0
+#define MCDE_CSCDTR1_CSACT_MASK 0x000000FF
+#define MCDE_CSCDTR1_CSACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR1, CSACT, __x)
+#define MCDE_CSCDTR1_CSDEACT_SHIFT 8
+#define MCDE_CSCDTR1_CSDEACT_MASK 0x0000FF00
+#define MCDE_CSCDTR1_CSDEACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR1, CSDEACT, __x)
+#define MCDE_CSCDTR1_CDACT_SHIFT 16
+#define MCDE_CSCDTR1_CDACT_MASK 0x00FF0000
+#define MCDE_CSCDTR1_CDACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR1, CDACT, __x)
+#define MCDE_CSCDTR1_CDDEACT_SHIFT 24
+#define MCDE_CSCDTR1_CDDEACT_MASK 0xFF000000
+#define MCDE_CSCDTR1_CDDEACT(__x) \
+	MCDE_VAL2REG(MCDE_CSCDTR1, CDDEACT, __x)
+#define MCDE_RDWRTR0 0x00000C7C
+#define MCDE_RDWRTR0_GROUPOFFSET 0x4
+#define MCDE_RDWRTR0_RWACT_SHIFT 0
+#define MCDE_RDWRTR0_RWACT_MASK 0x000000FF
+#define MCDE_RDWRTR0_RWACT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR0, RWACT, __x)
+#define MCDE_RDWRTR0_RWDEACT_SHIFT 8
+#define MCDE_RDWRTR0_RWDEACT_MASK 0x0000FF00
+#define MCDE_RDWRTR0_RWDEACT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR0, RWDEACT, __x)
+#define MCDE_RDWRTR0_MOTINT_SHIFT 16
+#define MCDE_RDWRTR0_MOTINT_MASK 0x00010000
+#define MCDE_RDWRTR0_MOTINT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR0, MOTINT, __x)
+#define MCDE_RDWRTR1 0x00000C80
+#define MCDE_RDWRTR1_RWACT_SHIFT 0
+#define MCDE_RDWRTR1_RWACT_MASK 0x000000FF
+#define MCDE_RDWRTR1_RWACT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR1, RWACT, __x)
+#define MCDE_RDWRTR1_RWDEACT_SHIFT 8
+#define MCDE_RDWRTR1_RWDEACT_MASK 0x0000FF00
+#define MCDE_RDWRTR1_RWDEACT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR1, RWDEACT, __x)
+#define MCDE_RDWRTR1_MOTINT_SHIFT 16
+#define MCDE_RDWRTR1_MOTINT_MASK 0x00010000
+#define MCDE_RDWRTR1_MOTINT(__x) \
+	MCDE_VAL2REG(MCDE_RDWRTR1, MOTINT, __x)
+#define MCDE_DOTR0 0x00000C84
+#define MCDE_DOTR0_GROUPOFFSET 0x4
+#define MCDE_DOTR0_DOACT_SHIFT 0
+#define MCDE_DOTR0_DOACT_MASK 0x000000FF
+#define MCDE_DOTR0_DOACT(__x) \
+	MCDE_VAL2REG(MCDE_DOTR0, DOACT, __x)
+#define MCDE_DOTR0_DODEACT_SHIFT 8
+#define MCDE_DOTR0_DODEACT_MASK 0x0000FF00
+#define MCDE_DOTR0_DODEACT(__x) \
+	MCDE_VAL2REG(MCDE_DOTR0, DODEACT, __x)
+#define MCDE_DOTR1 0x00000C88
+#define MCDE_DOTR1_DOACT_SHIFT 0
+#define MCDE_DOTR1_DOACT_MASK 0x000000FF
+#define MCDE_DOTR1_DOACT(__x) \
+	MCDE_VAL2REG(MCDE_DOTR1, DOACT, __x)
+#define MCDE_DOTR1_DODEACT_SHIFT 8
+#define MCDE_DOTR1_DODEACT_MASK 0x0000FF00
+#define MCDE_DOTR1_DODEACT(__x) \
+	MCDE_VAL2REG(MCDE_DOTR1, DODEACT, __x)
+#define MCDE_WDATADC0 0x00000C94
+#define MCDE_WDATADC0_GROUPOFFSET 0x4
+#define MCDE_WDATADC0_DATAVALUE_SHIFT 0
+#define MCDE_WDATADC0_DATAVALUE_MASK 0x00FFFFFF
+#define MCDE_WDATADC0_DATAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_WDATADC0, DATAVALUE, __x)
+#define MCDE_WDATADC0_DC_SHIFT 24
+#define MCDE_WDATADC0_DC_MASK 0x01000000
+#define MCDE_WDATADC0_DC(__x) \
+	MCDE_VAL2REG(MCDE_WDATADC0, DC, __x)
+#define MCDE_WDATADC1 0x00000C98
+#define MCDE_WDATADC1_DATAVALUE_SHIFT 0
+#define MCDE_WDATADC1_DATAVALUE_MASK 0x00FFFFFF
+#define MCDE_WDATADC1_DATAVALUE(__x) \
+	MCDE_VAL2REG(MCDE_WDATADC1, DATAVALUE, __x)
+#define MCDE_WDATADC1_DC_SHIFT 24
+#define MCDE_WDATADC1_DC_MASK 0x01000000
+#define MCDE_WDATADC1_DC(__x) \
+	MCDE_VAL2REG(MCDE_WDATADC1, DC, __x)
+#define MCDE_RDATADC0 0x00000C9C
+#define MCDE_RDATADC0_GROUPOFFSET 0x4
+#define MCDE_RDATADC0_DATAREADFROMDISPLAYMODULE_SHIFT 0
+#define MCDE_RDATADC0_DATAREADFROMDISPLAYMODULE_MASK 0x0000FFFF
+#define MCDE_RDATADC0_DATAREADFROMDISPLAYMODULE(__x) \
+	MCDE_VAL2REG(MCDE_RDATADC0, DATAREADFROMDISPLAYMODULE, __x)
+#define MCDE_RDATADC0_STARTREAD_SHIFT 16
+#define MCDE_RDATADC0_STARTREAD_MASK 0x00010000
+#define MCDE_RDATADC0_STARTREAD(__x) \
+	MCDE_VAL2REG(MCDE_RDATADC0, STARTREAD, __x)
+#define MCDE_RDATADC1 0x00000CA0
+#define MCDE_RDATADC1_DATAREADFROMDISPLAYMODULE_SHIFT 0
+#define MCDE_RDATADC1_DATAREADFROMDISPLAYMODULE_MASK 0x0000FFFF
+#define MCDE_RDATADC1_DATAREADFROMDISPLAYMODULE(__x) \
+	MCDE_VAL2REG(MCDE_RDATADC1, DATAREADFROMDISPLAYMODULE, __x)
+#define MCDE_RDATADC1_STARTREAD_SHIFT 16
+#define MCDE_RDATADC1_STARTREAD_MASK 0x00010000
+#define MCDE_RDATADC1_STARTREAD(__x) \
+	MCDE_VAL2REG(MCDE_RDATADC1, STARTREAD, __x)
+#define MCDE_STATC 0x00000CA4
+#define MCDE_STATC_STATBUSY0_SHIFT 0
+#define MCDE_STATC_STATBUSY0_MASK 0x00000001
+#define MCDE_STATC_STATBUSY0(__x) \
+	MCDE_VAL2REG(MCDE_STATC, STATBUSY0, __x)
+#define MCDE_STATC_STATBUSY1_SHIFT 5
+#define MCDE_STATC_STATBUSY1_MASK 0x00000020
+#define MCDE_STATC_STATBUSY1(__x) \
+	MCDE_VAL2REG(MCDE_STATC, STATBUSY1, __x)
+#define MCDE_CTRLC0 0x00000CA8
+#define MCDE_CTRLC0_GROUPOFFSET 0x4
+#define MCDE_CTRLC0_FIFOWTRMRK_SHIFT 0
+#define MCDE_CTRLC0_FIFOWTRMRK_MASK 0x000000FF
+#define MCDE_CTRLC0_FIFOWTRMRK(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FIFOWTRMRK, __x)
+#define MCDE_CTRLC0_FIFOEMPTY_SHIFT 12
+#define MCDE_CTRLC0_FIFOEMPTY_MASK 0x00001000
+#define MCDE_CTRLC0_FIFOEMPTY(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FIFOEMPTY, __x)
+#define MCDE_CTRLC0_FIFOFULL_SHIFT 13
+#define MCDE_CTRLC0_FIFOFULL_MASK 0x00002000
+#define MCDE_CTRLC0_FIFOFULL(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FIFOFULL, __x)
+#define MCDE_CTRLC0_FORMID_SHIFT 16
+#define MCDE_CTRLC0_FORMID_MASK 0x00070000
+#define MCDE_CTRLC0_FORMID_DSI0VID 0
+#define MCDE_CTRLC0_FORMID_DSI0CMD 1
+#define MCDE_CTRLC0_FORMID_DSI1VID 2
+#define MCDE_CTRLC0_FORMID_DSI1CMD 3
+#define MCDE_CTRLC0_FORMID_DSI2VID 4
+#define MCDE_CTRLC0_FORMID_DSI2CMD 5
+#define MCDE_CTRLC0_FORMID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FORMID, MCDE_CTRLC0_FORMID_##__x)
+#define MCDE_CTRLC0_FORMID(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FORMID, __x)
+#define MCDE_CTRLC0_FORMTYPE_SHIFT 20
+#define MCDE_CTRLC0_FORMTYPE_MASK 0x00700000
+#define MCDE_CTRLC0_FORMTYPE_DPITV 0
+#define MCDE_CTRLC0_FORMTYPE_DBI 1
+#define MCDE_CTRLC0_FORMTYPE_DSI 2
+#define MCDE_CTRLC0_FORMTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FORMTYPE, MCDE_CTRLC0_FORMTYPE_##__x)
+#define MCDE_CTRLC0_FORMTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC0, FORMTYPE, __x)
+#define MCDE_CTRLC1 0x00000CAC
+#define MCDE_CTRLC1_FIFOWTRMRK_SHIFT 0
+#define MCDE_CTRLC1_FIFOWTRMRK_MASK 0x000000FF
+#define MCDE_CTRLC1_FIFOWTRMRK(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FIFOWTRMRK, __x)
+#define MCDE_CTRLC1_FIFOEMPTY_SHIFT 12
+#define MCDE_CTRLC1_FIFOEMPTY_MASK 0x00001000
+#define MCDE_CTRLC1_FIFOEMPTY(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FIFOEMPTY, __x)
+#define MCDE_CTRLC1_FIFOFULL_SHIFT 13
+#define MCDE_CTRLC1_FIFOFULL_MASK 0x00002000
+#define MCDE_CTRLC1_FIFOFULL(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FIFOFULL, __x)
+#define MCDE_CTRLC1_FORMID_SHIFT 16
+#define MCDE_CTRLC1_FORMID_MASK 0x00070000
+#define MCDE_CTRLC1_FORMID_DSI0VID 0
+#define MCDE_CTRLC1_FORMID_DSI0CMD 1
+#define MCDE_CTRLC1_FORMID_DSI1VID 2
+#define MCDE_CTRLC1_FORMID_DSI1CMD 3
+#define MCDE_CTRLC1_FORMID_DSI2VID 4
+#define MCDE_CTRLC1_FORMID_DSI2CMD 5
+#define MCDE_CTRLC1_FORMID_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FORMID, MCDE_CTRLC1_FORMID_##__x)
+#define MCDE_CTRLC1_FORMID(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FORMID, __x)
+#define MCDE_CTRLC1_FORMTYPE_SHIFT 20
+#define MCDE_CTRLC1_FORMTYPE_MASK 0x00700000
+#define MCDE_CTRLC1_FORMTYPE_DPITV 0
+#define MCDE_CTRLC1_FORMTYPE_DBI 1
+#define MCDE_CTRLC1_FORMTYPE_DSI 2
+#define MCDE_CTRLC1_FORMTYPE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FORMTYPE, MCDE_CTRLC1_FORMTYPE_##__x)
+#define MCDE_CTRLC1_FORMTYPE(__x) \
+	MCDE_VAL2REG(MCDE_CTRLC1, FORMTYPE, __x)
+#define MCDE_DSIVID0CONF0 0x00000E00
+#define MCDE_DSIVID0CONF0_GROUPOFFSET 0x20
+#define MCDE_DSIVID0CONF0_BLANKING_SHIFT 0
+#define MCDE_DSIVID0CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSIVID0CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, BLANKING, __x)
+#define MCDE_DSIVID0CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSIVID0CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSIVID0CONF0_VID_MODE_CMD 0
+#define MCDE_DSIVID0CONF0_VID_MODE_VID 1
+#define MCDE_DSIVID0CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, VID_MODE, \
+	MCDE_DSIVID0CONF0_VID_MODE_##__x)
+#define MCDE_DSIVID0CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, VID_MODE, __x)
+#define MCDE_DSIVID0CONF0_CMD8_SHIFT 13
+#define MCDE_DSIVID0CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSIVID0CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, CMD8, __x)
+#define MCDE_DSIVID0CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSIVID0CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSIVID0CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, BIT_SWAP, __x)
+#define MCDE_DSIVID0CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSIVID0CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSIVID0CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, BYTE_SWAP, __x)
+#define MCDE_DSIVID0CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSIVID0CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSIVID0CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSIVID0CONF0_PACKING_SHIFT 20
+#define MCDE_DSIVID0CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSIVID0CONF0_PACKING_RGB565 0
+#define MCDE_DSIVID0CONF0_PACKING_RGB666 1
+#define MCDE_DSIVID0CONF0_PACKING_RGB888 2
+#define MCDE_DSIVID0CONF0_PACKING_BGR888 3
+#define MCDE_DSIVID0CONF0_PACKING_HDTV 4
+#define MCDE_DSIVID0CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, PACKING, \
+	MCDE_DSIVID0CONF0_PACKING_##__x)
+#define MCDE_DSIVID0CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CONF0, PACKING, __x)
+#define MCDE_DSICMD0CONF0 0x00000E20
+#define MCDE_DSICMD0CONF0_BLANKING_SHIFT 0
+#define MCDE_DSICMD0CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSICMD0CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, BLANKING, __x)
+#define MCDE_DSICMD0CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSICMD0CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSICMD0CONF0_VID_MODE_CMD 0
+#define MCDE_DSICMD0CONF0_VID_MODE_VID 1
+#define MCDE_DSICMD0CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, VID_MODE, \
+	MCDE_DSICMD0CONF0_VID_MODE_##__x)
+#define MCDE_DSICMD0CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, VID_MODE, __x)
+#define MCDE_DSICMD0CONF0_CMD8_SHIFT 13
+#define MCDE_DSICMD0CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSICMD0CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, CMD8, __x)
+#define MCDE_DSICMD0CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSICMD0CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSICMD0CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, BIT_SWAP, __x)
+#define MCDE_DSICMD0CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSICMD0CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSICMD0CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, BYTE_SWAP, __x)
+#define MCDE_DSICMD0CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSICMD0CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSICMD0CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSICMD0CONF0_PACKING_SHIFT 20
+#define MCDE_DSICMD0CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSICMD0CONF0_PACKING_RGB565 0
+#define MCDE_DSICMD0CONF0_PACKING_RGB666 1
+#define MCDE_DSICMD0CONF0_PACKING_RGB888 2
+#define MCDE_DSICMD0CONF0_PACKING_BGR888 3
+#define MCDE_DSICMD0CONF0_PACKING_HDTV 4
+#define MCDE_DSICMD0CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, PACKING, \
+	MCDE_DSICMD0CONF0_PACKING_##__x)
+#define MCDE_DSICMD0CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CONF0, PACKING, __x)
+#define MCDE_DSIVID1CONF0 0x00000E40
+#define MCDE_DSIVID1CONF0_BLANKING_SHIFT 0
+#define MCDE_DSIVID1CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSIVID1CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, BLANKING, __x)
+#define MCDE_DSIVID1CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSIVID1CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSIVID1CONF0_VID_MODE_CMD 0
+#define MCDE_DSIVID1CONF0_VID_MODE_VID 1
+#define MCDE_DSIVID1CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, VID_MODE, \
+	MCDE_DSIVID1CONF0_VID_MODE_##__x)
+#define MCDE_DSIVID1CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, VID_MODE, __x)
+#define MCDE_DSIVID1CONF0_CMD8_SHIFT 13
+#define MCDE_DSIVID1CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSIVID1CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, CMD8, __x)
+#define MCDE_DSIVID1CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSIVID1CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSIVID1CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, BIT_SWAP, __x)
+#define MCDE_DSIVID1CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSIVID1CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSIVID1CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, BYTE_SWAP, __x)
+#define MCDE_DSIVID1CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSIVID1CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSIVID1CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSIVID1CONF0_PACKING_SHIFT 20
+#define MCDE_DSIVID1CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSIVID1CONF0_PACKING_RGB565 0
+#define MCDE_DSIVID1CONF0_PACKING_RGB666 1
+#define MCDE_DSIVID1CONF0_PACKING_RGB888 2
+#define MCDE_DSIVID1CONF0_PACKING_BGR888 3
+#define MCDE_DSIVID1CONF0_PACKING_HDTV 4
+#define MCDE_DSIVID1CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, PACKING, \
+	MCDE_DSIVID1CONF0_PACKING_##__x)
+#define MCDE_DSIVID1CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CONF0, PACKING, __x)
+#define MCDE_DSICMD1CONF0 0x00000E60
+#define MCDE_DSICMD1CONF0_BLANKING_SHIFT 0
+#define MCDE_DSICMD1CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSICMD1CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, BLANKING, __x)
+#define MCDE_DSICMD1CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSICMD1CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSICMD1CONF0_VID_MODE_CMD 0
+#define MCDE_DSICMD1CONF0_VID_MODE_VID 1
+#define MCDE_DSICMD1CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, VID_MODE, \
+	MCDE_DSICMD1CONF0_VID_MODE_##__x)
+#define MCDE_DSICMD1CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, VID_MODE, __x)
+#define MCDE_DSICMD1CONF0_CMD8_SHIFT 13
+#define MCDE_DSICMD1CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSICMD1CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, CMD8, __x)
+#define MCDE_DSICMD1CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSICMD1CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSICMD1CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, BIT_SWAP, __x)
+#define MCDE_DSICMD1CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSICMD1CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSICMD1CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, BYTE_SWAP, __x)
+#define MCDE_DSICMD1CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSICMD1CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSICMD1CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSICMD1CONF0_PACKING_SHIFT 20
+#define MCDE_DSICMD1CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSICMD1CONF0_PACKING_RGB565 0
+#define MCDE_DSICMD1CONF0_PACKING_RGB666 1
+#define MCDE_DSICMD1CONF0_PACKING_RGB888 2
+#define MCDE_DSICMD1CONF0_PACKING_BGR888 3
+#define MCDE_DSICMD1CONF0_PACKING_HDTV 4
+#define MCDE_DSICMD1CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, PACKING, \
+	MCDE_DSICMD1CONF0_PACKING_##__x)
+#define MCDE_DSICMD1CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CONF0, PACKING, __x)
+#define MCDE_DSIVID2CONF0 0x00000E80
+#define MCDE_DSIVID2CONF0_BLANKING_SHIFT 0
+#define MCDE_DSIVID2CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSIVID2CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, BLANKING, __x)
+#define MCDE_DSIVID2CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSIVID2CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSIVID2CONF0_VID_MODE_CMD 0
+#define MCDE_DSIVID2CONF0_VID_MODE_VID 1
+#define MCDE_DSIVID2CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, VID_MODE, \
+	MCDE_DSIVID2CONF0_VID_MODE_##__x)
+#define MCDE_DSIVID2CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, VID_MODE, __x)
+#define MCDE_DSIVID2CONF0_CMD8_SHIFT 13
+#define MCDE_DSIVID2CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSIVID2CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, CMD8, __x)
+#define MCDE_DSIVID2CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSIVID2CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSIVID2CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, BIT_SWAP, __x)
+#define MCDE_DSIVID2CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSIVID2CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSIVID2CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, BYTE_SWAP, __x)
+#define MCDE_DSIVID2CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSIVID2CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSIVID2CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSIVID2CONF0_PACKING_SHIFT 20
+#define MCDE_DSIVID2CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSIVID2CONF0_PACKING_RGB565 0
+#define MCDE_DSIVID2CONF0_PACKING_RGB666 1
+#define MCDE_DSIVID2CONF0_PACKING_RGB888 2
+#define MCDE_DSIVID2CONF0_PACKING_BGR888 3
+#define MCDE_DSIVID2CONF0_PACKING_HDTV 4
+#define MCDE_DSIVID2CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, PACKING, \
+	MCDE_DSIVID2CONF0_PACKING_##__x)
+#define MCDE_DSIVID2CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CONF0, PACKING, __x)
+#define MCDE_DSICMD2CONF0 0x00000EA0
+#define MCDE_DSICMD2CONF0_BLANKING_SHIFT 0
+#define MCDE_DSICMD2CONF0_BLANKING_MASK 0x000000FF
+#define MCDE_DSICMD2CONF0_BLANKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, BLANKING, __x)
+#define MCDE_DSICMD2CONF0_VID_MODE_SHIFT 12
+#define MCDE_DSICMD2CONF0_VID_MODE_MASK 0x00001000
+#define MCDE_DSICMD2CONF0_VID_MODE_CMD 0
+#define MCDE_DSICMD2CONF0_VID_MODE_VID 1
+#define MCDE_DSICMD2CONF0_VID_MODE_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, VID_MODE, \
+	MCDE_DSICMD2CONF0_VID_MODE_##__x)
+#define MCDE_DSICMD2CONF0_VID_MODE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, VID_MODE, __x)
+#define MCDE_DSICMD2CONF0_CMD8_SHIFT 13
+#define MCDE_DSICMD2CONF0_CMD8_MASK 0x00002000
+#define MCDE_DSICMD2CONF0_CMD8(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, CMD8, __x)
+#define MCDE_DSICMD2CONF0_BIT_SWAP_SHIFT 16
+#define MCDE_DSICMD2CONF0_BIT_SWAP_MASK 0x00010000
+#define MCDE_DSICMD2CONF0_BIT_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, BIT_SWAP, __x)
+#define MCDE_DSICMD2CONF0_BYTE_SWAP_SHIFT 17
+#define MCDE_DSICMD2CONF0_BYTE_SWAP_MASK 0x00020000
+#define MCDE_DSICMD2CONF0_BYTE_SWAP(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, BYTE_SWAP, __x)
+#define MCDE_DSICMD2CONF0_DCSVID_NOTGEN_SHIFT 18
+#define MCDE_DSICMD2CONF0_DCSVID_NOTGEN_MASK 0x00040000
+#define MCDE_DSICMD2CONF0_DCSVID_NOTGEN(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, DCSVID_NOTGEN, __x)
+#define MCDE_DSICMD2CONF0_PACKING_SHIFT 20
+#define MCDE_DSICMD2CONF0_PACKING_MASK 0x00700000
+#define MCDE_DSICMD2CONF0_PACKING_RGB565 0
+#define MCDE_DSICMD2CONF0_PACKING_RGB666 1
+#define MCDE_DSICMD2CONF0_PACKING_RGB888 2
+#define MCDE_DSICMD2CONF0_PACKING_BGR888 3
+#define MCDE_DSICMD2CONF0_PACKING_HDTV 4
+#define MCDE_DSICMD2CONF0_PACKING_ENUM(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, PACKING, \
+	MCDE_DSICMD2CONF0_PACKING_##__x)
+#define MCDE_DSICMD2CONF0_PACKING(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CONF0, PACKING, __x)
+#define MCDE_DSIVID0FRAME 0x00000E04
+#define MCDE_DSIVID0FRAME_GROUPOFFSET 0x20
+#define MCDE_DSIVID0FRAME_FRAME_SHIFT 0
+#define MCDE_DSIVID0FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSIVID0FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0FRAME, FRAME, __x)
+#define MCDE_DSICMD0FRAME 0x00000E24
+#define MCDE_DSICMD0FRAME_FRAME_SHIFT 0
+#define MCDE_DSICMD0FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSICMD0FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0FRAME, FRAME, __x)
+#define MCDE_DSIVID1FRAME 0x00000E44
+#define MCDE_DSIVID1FRAME_FRAME_SHIFT 0
+#define MCDE_DSIVID1FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSIVID1FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1FRAME, FRAME, __x)
+#define MCDE_DSICMD1FRAME 0x00000E64
+#define MCDE_DSICMD1FRAME_FRAME_SHIFT 0
+#define MCDE_DSICMD1FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSICMD1FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1FRAME, FRAME, __x)
+#define MCDE_DSIVID2FRAME 0x00000E84
+#define MCDE_DSIVID2FRAME_FRAME_SHIFT 0
+#define MCDE_DSIVID2FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSIVID2FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2FRAME, FRAME, __x)
+#define MCDE_DSICMD2FRAME 0x00000EA4
+#define MCDE_DSICMD2FRAME_FRAME_SHIFT 0
+#define MCDE_DSICMD2FRAME_FRAME_MASK 0x00FFFFFF
+#define MCDE_DSICMD2FRAME_FRAME(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2FRAME, FRAME, __x)
+#define MCDE_DSIVID0PKT 0x00000E08
+#define MCDE_DSIVID0PKT_GROUPOFFSET 0x20
+#define MCDE_DSIVID0PKT_PACKET_SHIFT 0
+#define MCDE_DSIVID0PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSIVID0PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0PKT, PACKET, __x)
+#define MCDE_DSICMD0PKT 0x00000E28
+#define MCDE_DSICMD0PKT_PACKET_SHIFT 0
+#define MCDE_DSICMD0PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSICMD0PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0PKT, PACKET, __x)
+#define MCDE_DSIVID1PKT 0x00000E48
+#define MCDE_DSIVID1PKT_PACKET_SHIFT 0
+#define MCDE_DSIVID1PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSIVID1PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1PKT, PACKET, __x)
+#define MCDE_DSICMD1PKT 0x00000E68
+#define MCDE_DSICMD1PKT_PACKET_SHIFT 0
+#define MCDE_DSICMD1PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSICMD1PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1PKT, PACKET, __x)
+#define MCDE_DSIVID2PKT 0x00000E88
+#define MCDE_DSIVID2PKT_PACKET_SHIFT 0
+#define MCDE_DSIVID2PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSIVID2PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2PKT, PACKET, __x)
+#define MCDE_DSICMD2PKT 0x00000EA8
+#define MCDE_DSICMD2PKT_PACKET_SHIFT 0
+#define MCDE_DSICMD2PKT_PACKET_MASK 0x0000FFFF
+#define MCDE_DSICMD2PKT_PACKET(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2PKT, PACKET, __x)
+#define MCDE_DSIVID0SYNC 0x00000E0C
+#define MCDE_DSIVID0SYNC_GROUPOFFSET 0x20
+#define MCDE_DSIVID0SYNC_DMA_SHIFT 0
+#define MCDE_DSIVID0SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSIVID0SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0SYNC, DMA, __x)
+#define MCDE_DSIVID0SYNC_SW_SHIFT 16
+#define MCDE_DSIVID0SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSIVID0SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0SYNC, SW, __x)
+#define MCDE_DSICMD0SYNC 0x00000E2C
+#define MCDE_DSICMD0SYNC_DMA_SHIFT 0
+#define MCDE_DSICMD0SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSICMD0SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0SYNC, DMA, __x)
+#define MCDE_DSICMD0SYNC_SW_SHIFT 16
+#define MCDE_DSICMD0SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSICMD0SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0SYNC, SW, __x)
+#define MCDE_DSIVID1SYNC 0x00000E4C
+#define MCDE_DSIVID1SYNC_DMA_SHIFT 0
+#define MCDE_DSIVID1SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSIVID1SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1SYNC, DMA, __x)
+#define MCDE_DSIVID1SYNC_SW_SHIFT 16
+#define MCDE_DSIVID1SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSIVID1SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1SYNC, SW, __x)
+#define MCDE_DSICMD1SYNC 0x00000E6C
+#define MCDE_DSICMD1SYNC_DMA_SHIFT 0
+#define MCDE_DSICMD1SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSICMD1SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1SYNC, DMA, __x)
+#define MCDE_DSICMD1SYNC_SW_SHIFT 16
+#define MCDE_DSICMD1SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSICMD1SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1SYNC, SW, __x)
+#define MCDE_DSIVID2SYNC 0x00000E8C
+#define MCDE_DSIVID2SYNC_DMA_SHIFT 0
+#define MCDE_DSIVID2SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSIVID2SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2SYNC, DMA, __x)
+#define MCDE_DSIVID2SYNC_SW_SHIFT 16
+#define MCDE_DSIVID2SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSIVID2SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2SYNC, SW, __x)
+#define MCDE_DSICMD2SYNC 0x00000EAC
+#define MCDE_DSICMD2SYNC_DMA_SHIFT 0
+#define MCDE_DSICMD2SYNC_DMA_MASK 0x00000FFF
+#define MCDE_DSICMD2SYNC_DMA(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2SYNC, DMA, __x)
+#define MCDE_DSICMD2SYNC_SW_SHIFT 16
+#define MCDE_DSICMD2SYNC_SW_MASK 0x0FFF0000
+#define MCDE_DSICMD2SYNC_SW(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2SYNC, SW, __x)
+#define MCDE_DSIVID0CMDW 0x00000E10
+#define MCDE_DSIVID0CMDW_GROUPOFFSET 0x20
+#define MCDE_DSIVID0CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSIVID0CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSIVID0CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSIVID0CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSIVID0CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSIVID0CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0CMDW, CMDW_START, __x)
+#define MCDE_DSICMD0CMDW 0x00000E30
+#define MCDE_DSICMD0CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSICMD0CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSICMD0CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSICMD0CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSICMD0CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSICMD0CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0CMDW, CMDW_START, __x)
+#define MCDE_DSIVID1CMDW 0x00000E50
+#define MCDE_DSIVID1CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSIVID1CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSIVID1CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSIVID1CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSIVID1CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSIVID1CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1CMDW, CMDW_START, __x)
+#define MCDE_DSICMD1CMDW 0x00000E70
+#define MCDE_DSICMD1CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSICMD1CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSICMD1CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSICMD1CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSICMD1CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSICMD1CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1CMDW, CMDW_START, __x)
+#define MCDE_DSIVID2CMDW 0x00000E90
+#define MCDE_DSIVID2CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSIVID2CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSIVID2CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSIVID2CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSIVID2CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSIVID2CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2CMDW, CMDW_START, __x)
+#define MCDE_DSICMD2CMDW 0x00000EB0
+#define MCDE_DSICMD2CMDW_CMDW_CONTINUE_SHIFT 0
+#define MCDE_DSICMD2CMDW_CMDW_CONTINUE_MASK 0x0000FFFF
+#define MCDE_DSICMD2CMDW_CMDW_CONTINUE(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CMDW, CMDW_CONTINUE, __x)
+#define MCDE_DSICMD2CMDW_CMDW_START_SHIFT 16
+#define MCDE_DSICMD2CMDW_CMDW_START_MASK 0xFFFF0000
+#define MCDE_DSICMD2CMDW_CMDW_START(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2CMDW, CMDW_START, __x)
+#define MCDE_DSIVID0DELAY0 0x00000E14
+#define MCDE_DSIVID0DELAY0_GROUPOFFSET 0x20
+#define MCDE_DSIVID0DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSIVID0DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSIVID0DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0DELAY0, INTPKTDEL, __x)
+#define MCDE_DSICMD0DELAY0 0x00000E34
+#define MCDE_DSICMD0DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSICMD0DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSICMD0DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0DELAY0, INTPKTDEL, __x)
+#define MCDE_DSIVID1DELAY0 0x00000E54
+#define MCDE_DSIVID1DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSIVID1DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSIVID1DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1DELAY0, INTPKTDEL, __x)
+#define MCDE_DSICMD1DELAY0 0x00000E74
+#define MCDE_DSICMD1DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSICMD1DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSICMD1DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1DELAY0, INTPKTDEL, __x)
+#define MCDE_DSIVID2DELAY0 0x00000E94
+#define MCDE_DSIVID2DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSIVID2DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSIVID2DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2DELAY0, INTPKTDEL, __x)
+#define MCDE_DSICMD2DELAY0 0x00000EB4
+#define MCDE_DSICMD2DELAY0_INTPKTDEL_SHIFT 0
+#define MCDE_DSICMD2DELAY0_INTPKTDEL_MASK 0x0000FFFF
+#define MCDE_DSICMD2DELAY0_INTPKTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2DELAY0, INTPKTDEL, __x)
+#define MCDE_DSIVID0DELAY1 0x00000E18
+#define MCDE_DSIVID0DELAY1_GROUPOFFSET 0x20
+#define MCDE_DSIVID0DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSIVID0DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSIVID0DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0DELAY1, TEREQDEL, __x)
+#define MCDE_DSIVID0DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSIVID0DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSIVID0DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID0DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSICMD0DELAY1 0x00000E38
+#define MCDE_DSICMD0DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSICMD0DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSICMD0DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0DELAY1, TEREQDEL, __x)
+#define MCDE_DSICMD0DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSICMD0DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSICMD0DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD0DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSIVID1DELAY1 0x00000E58
+#define MCDE_DSIVID1DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSIVID1DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSIVID1DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1DELAY1, TEREQDEL, __x)
+#define MCDE_DSIVID1DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSIVID1DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSIVID1DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID1DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSICMD1DELAY1 0x00000E78
+#define MCDE_DSICMD1DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSICMD1DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSICMD1DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1DELAY1, TEREQDEL, __x)
+#define MCDE_DSICMD1DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSICMD1DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSICMD1DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD1DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSIVID2DELAY1 0x00000E98
+#define MCDE_DSIVID2DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSIVID2DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSIVID2DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2DELAY1, TEREQDEL, __x)
+#define MCDE_DSIVID2DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSIVID2DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSIVID2DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSIVID2DELAY1, FRAMESTARTDEL, __x)
+#define MCDE_DSICMD2DELAY1 0x00000EB8
+#define MCDE_DSICMD2DELAY1_TEREQDEL_SHIFT 0
+#define MCDE_DSICMD2DELAY1_TEREQDEL_MASK 0x00000FFF
+#define MCDE_DSICMD2DELAY1_TEREQDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2DELAY1, TEREQDEL, __x)
+#define MCDE_DSICMD2DELAY1_FRAMESTARTDEL_SHIFT 16
+#define MCDE_DSICMD2DELAY1_FRAMESTARTDEL_MASK 0x00FF0000
+#define MCDE_DSICMD2DELAY1_FRAMESTARTDEL(__x) \
+	MCDE_VAL2REG(MCDE_DSICMD2DELAY1, FRAMESTARTDEL, __x)
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 37096246c93..14e568be223 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -349,6 +349,22 @@ config IMX2_WDT
 	  To compile this driver as a module, choose M here: the
 	  module will be called imx2_wdt.
 
+config UX500_WATCHDOG
+	bool "ST-Ericsson Ux500 watchdog"
+	depends on UX500_SOC_DB8500 || UX500_SOC_DB5500
+	default y
+	help
+	  Say Y here to include Watchdog timer support for the
+	  watchdog existing in the prcmu of ST-Ericsson Ux500 series platforms.
+	  This watchdog is used to reset the system and thus cannot be
+	  compiled as a module.
+
+config UX500_WATCHDOG_DEBUG
+	bool "ST-Ericsson Ux500 watchdog DEBUG"
+	depends on (UX500_SOC_DB8500 || UX500_SOC_DB5500) && DEBUG_FS
+	help
+	  Say Y here to add various debugfs entries in wdog/
+
 # AVR32 Architecture
 
 config AT32AP700X_WDT
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index e8f479a1640..738a0f3ad21 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -53,6 +53,7 @@ obj-$(CONFIG_STMP3XXX_WATCHDOG) += stmp3xxx_wdt.o
 obj-$(CONFIG_NUC900_WATCHDOG) += nuc900_wdt.o
 obj-$(CONFIG_TS72XX_WATCHDOG) += ts72xx_wdt.o
 obj-$(CONFIG_IMX2_WDT) += imx2_wdt.o
+obj-$(CONFIG_UX500_WATCHDOG) += ux500_wdt.o
 
 # AVR32 Architecture
 obj-$(CONFIG_AT32AP700X_WDT) += at32ap700x_wdt.o
diff --git a/drivers/watchdog/mpcore_wdt.c b/drivers/watchdog/mpcore_wdt.c
index 7c741dc987b..96ae2465c25 100644
--- a/drivers/watchdog/mpcore_wdt.c
+++ b/drivers/watchdog/mpcore_wdt.c
@@ -35,11 +35,13 @@
 #include <linux/uaccess.h>
 #include <linux/slab.h>
 #include <linux/io.h>
+#include <linux/cpufreq.h>
+#include <linux/kexec.h>
 
 #include <asm/smp_twd.h>
 
 struct mpcore_wdt {
-	unsigned long	timer_alive;
+	cpumask_t	timer_alive;
 	struct device	*dev;
 	void __iomem	*base;
 	int		irq;
@@ -50,6 +52,8 @@ struct mpcore_wdt {
 static struct platform_device *mpcore_wdt_pdev;
 static DEFINE_SPINLOCK(wdt_lock);
 
+static DEFINE_PER_CPU(unsigned long, mpcore_wdt_rate);
+
 #define TIMER_MARGIN	60
 static int mpcore_margin = TIMER_MARGIN;
 module_param(mpcore_margin, int, 0);
@@ -70,6 +74,8 @@ MODULE_PARM_DESC(mpcore_noboot, "MPcore watchdog action, "
 	"set to 1 to ignore reboots, 0 to reboot (default="
 					__MODULE_STRING(ONLY_TESTING) ")");
 
+#define MPCORE_WDT_PERIPHCLK_PRESCALER 2
+
 /*
  *	This is the interrupt handler.  Note that we only use this
  *	in testing mode, so don't actually do a reboot here.
@@ -102,9 +108,8 @@ static void mpcore_wdt_keepalive(struct mpcore_wdt *wdt)
 
 	spin_lock(&wdt_lock);
 	/* Assume prescale is set to 256 */
-	count =  __raw_readl(wdt->base + TWD_WDOG_COUNTER);
-	count = (0xFFFFFFFFU - count) * (HZ / 5);
-	count = (count / 256) * mpcore_margin;
+	count = per_cpu(mpcore_wdt_rate, smp_processor_id()) / 256;
+	count = count*mpcore_margin;
 
 	/* Reload the counter */
 	writel(count + wdt->perturb, wdt->base + TWD_WDOG_LOAD);
@@ -112,6 +117,56 @@ static void mpcore_wdt_keepalive(struct mpcore_wdt *wdt)
 	spin_unlock(&wdt_lock);
 }
 
+static void mpcore_wdt_set_rate(unsigned long new_rate)
+{
+	unsigned long count;
+	unsigned long long rate_tmp;
+	unsigned long old_rate;
+
+	spin_lock(&wdt_lock);
+	old_rate = per_cpu(mpcore_wdt_rate, smp_processor_id());
+	per_cpu(mpcore_wdt_rate, smp_processor_id()) = new_rate;
+
+	if (mpcore_wdt_dev) {
+		struct mpcore_wdt *wdt = platform_get_drvdata(mpcore_wdt_dev);
+		count = readl(wdt->base + TWD_WDOG_COUNTER);
+		/* The goal: count = count * (new_rate/old_rate); */
+		rate_tmp = (unsigned long long)count * new_rate;
+		do_div(rate_tmp, old_rate);
+		count = rate_tmp;
+		writel(count + wdt->perturb, wdt->base + TWD_WDOG_LOAD);
+		wdt->perturb = wdt->perturb ? 0 : 1;
+	}
+	spin_unlock(&wdt_lock);
+}
+
+static void mpcore_wdt_update_cpu_frequency_on_cpu(void *data)
+{
+	struct cpufreq_freqs *freq = data;
+	mpcore_wdt_set_rate((freq->new * 1000) /
+			    MPCORE_WDT_PERIPHCLK_PRESCALER);
+}
+
+static int mpcore_wdt_cpufreq_notifier(struct notifier_block *nb,
+				       unsigned long event, void *data)
+{
+	struct cpufreq_freqs *freq = data;
+
+	if (event == CPUFREQ_RESUMECHANGE ||
+	    (event == CPUFREQ_PRECHANGE && freq->new > freq->old) ||
+	    (event == CPUFREQ_POSTCHANGE && freq->new < freq->old))
+		smp_call_function_single(freq->cpu,
+					 mpcore_wdt_update_cpu_frequency_on_cpu,
+					 freq, 1);
+
+	return 0;
+}
+
+static struct notifier_block mpcore_wdt_cpufreq_notifier_block = {
+	.notifier_call = mpcore_wdt_cpufreq_notifier,
+};
+
+
 static void mpcore_wdt_stop(struct mpcore_wdt *wdt)
 {
 	spin_lock(&wdt_lock);
@@ -146,6 +201,20 @@ static int mpcore_wdt_set_heartbeat(int t)
 	return 0;
 }
 
+static int mpcore_wdt_stop_notifier(struct notifier_block *this,
+				unsigned long event, void *ptr)
+{
+	struct mpcore_wdt *wdt = platform_get_drvdata(mpcore_wdt_dev);
+	printk(KERN_INFO "Stopping watchdog on non-crashing core %u\n",
+	       smp_processor_id());
+	mpcore_wdt_stop(wdt);
+	return NOTIFY_STOP;
+}
+
+static struct notifier_block mpcore_wdt_stop_block = {
+	.notifier_call = mpcore_wdt_stop_notifier,
+};
+
 /*
  *	/dev/watchdog handling
  */
@@ -153,7 +222,7 @@ static int mpcore_wdt_open(struct inode *inode, struct file *file)
 {
 	struct mpcore_wdt *wdt = platform_get_drvdata(mpcore_wdt_pdev);
 
-	if (test_and_set_bit(0, &wdt->timer_alive))
+	if (cpumask_test_and_set_cpu(smp_processor_id(), &wdt->timer_alive))
 		return -EBUSY;
 
 	if (nowayout)
@@ -161,6 +230,9 @@ static int mpcore_wdt_open(struct inode *inode, struct file *file)
 
 	file->private_data = wdt;
 
+	atomic_notifier_chain_register(&crash_percpu_notifier_list,
+				       &mpcore_wdt_stop_block);
+
 	/*
 	 *	Activate timer
 	 */
@@ -184,7 +256,7 @@ static int mpcore_wdt_release(struct inode *inode, struct file *file)
 				"unexpected close, not stopping watchdog!\n");
 		mpcore_wdt_keepalive(wdt);
 	}
-	clear_bit(0, &wdt->timer_alive);
+	cpumask_clear_cpu(smp_processor_id(), &wdt->timer_alive);
 	wdt->expect_close = 0;
 	return 0;
 }
@@ -427,6 +499,8 @@ static struct platform_driver mpcore_wdt_driver = {
 
 static int __init mpcore_wdt_init(void)
 {
+	int i;
+
 	/*
 	 * Check that the margin value is within it's range;
 	 * if not reset to the default
@@ -437,6 +511,18 @@ static int __init mpcore_wdt_init(void)
 			TIMER_MARGIN);
 	}
 
+	cpufreq_register_notifier(&mpcore_wdt_cpufreq_notifier_block,
+				  CPUFREQ_TRANSITION_NOTIFIER);
+
+	for_each_online_cpu(i)
+		per_cpu(mpcore_wdt_rate, i) =
+		(cpufreq_get(i) * 1000) / MPCORE_WDT_PERIPHCLK_PRESCALER;
+
+	for_each_online_cpu(i)
+		pr_info("mpcore_wdt: rate for core %d is %lu.%02luMHz.\n", i,
+		       per_cpu(mpcore_wdt_rate, i) / 1000000,
+		       (per_cpu(mpcore_wdt_rate, i) / 10000) % 100);
+
 	pr_info("MPcore Watchdog Timer: 0.1. mpcore_noboot=%d mpcore_margin=%d sec (nowayout= %d)\n",
 		mpcore_noboot, mpcore_margin, nowayout);
 
diff --git a/drivers/watchdog/ux500_wdt.c b/drivers/watchdog/ux500_wdt.c
new file mode 100644
index 00000000000..a1e8c2dbf10
--- /dev/null
+++ b/drivers/watchdog/ux500_wdt.c
@@ -0,0 +1,454 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2011
+ *
+ * License Terms: GNU General Public License v2
+ *
+ * Author: Jonas Aaberg <jonas.aberg@stericsson.com> for ST-Ericsson
+ *
+ * Heavily based upon geodewdt.c
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/moduleparam.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/err.h>
+#include <linux/uaccess.h>
+#include <linux/miscdevice.h>
+#include <linux/watchdog.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/dbx500-prcmu.h>
+
+#define WATCHDOG_TIMEOUT 600 /* 10 minutes */
+
+#define WDT_FLAGS_OPEN 1
+#define WDT_FLAGS_ORPHAN 2
+
+static unsigned long wdt_flags;
+
+static int timeout = WATCHDOG_TIMEOUT;
+module_param(timeout, int, 0);
+MODULE_PARM_DESC(timeout,
+	"Watchdog timeout in seconds. default="
+				__MODULE_STRING(WATCHDOG_TIMEOUT) ".");
+
+static int nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, int, 0);
+MODULE_PARM_DESC(nowayout,
+	"Watchdog cannot be stopped once started (default="
+				__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+static u8 wdog_id;
+static bool wdt_en;
+static bool wdt_auto_off = false;
+static bool safe_close;
+
+static int ux500_wdt_open(struct inode *inode, struct file *file)
+{
+	if (!timeout)
+		return -ENODEV;
+
+	if (test_and_set_bit(WDT_FLAGS_OPEN, &wdt_flags))
+		return -EBUSY;
+
+	if (!test_and_clear_bit(WDT_FLAGS_ORPHAN, &wdt_flags))
+		__module_get(THIS_MODULE);
+
+	prcmu_enable_a9wdog(wdog_id);
+	wdt_en = true;
+
+	return nonseekable_open(inode, file);
+}
+
+static int ux500_wdt_release(struct inode *inode, struct file *file)
+{
+	if (safe_close) {
+		prcmu_disable_a9wdog(wdog_id);
+		module_put(THIS_MODULE);
+	} else {
+		pr_crit("Unexpected close - watchdog is not stopping.\n");
+		prcmu_kick_a9wdog(wdog_id);
+
+		set_bit(WDT_FLAGS_ORPHAN, &wdt_flags);
+	}
+
+	clear_bit(WDT_FLAGS_OPEN, &wdt_flags);
+	safe_close = false;
+	return 0;
+}
+
+static ssize_t ux500_wdt_write(struct file *file, const char __user *data,
+			       size_t len, loff_t *ppos)
+{
+	if (!len)
+		return len;
+
+	if (!nowayout) {
+		size_t i;
+		safe_close = false;
+
+		for (i = 0; i != len; i++) {
+			char c;
+
+			if (get_user(c, data + i))
+				return -EFAULT;
+
+			if (c == 'V')
+				safe_close = true;
+		}
+	}
+
+	prcmu_kick_a9wdog(wdog_id);
+
+	return len;
+}
+
+static long ux500_wdt_ioctl(struct file *file, unsigned int cmd,
+			   unsigned long arg)
+{
+	void __user *argp = (void __user *)arg;
+	int __user *p = argp;
+	int interval;
+
+	static const struct watchdog_info ident = {
+		.options =	WDIOF_SETTIMEOUT |
+				WDIOF_KEEPALIVEPING |
+				WDIOF_MAGICCLOSE,
+		.firmware_version =     1,
+		.identity	= "Ux500 WDT",
+	};
+
+	switch (cmd) {
+	case WDIOC_GETSUPPORT:
+		return copy_to_user(argp, &ident,
+				    sizeof(ident)) ? -EFAULT : 0;
+
+	case WDIOC_GETSTATUS:
+	case WDIOC_GETBOOTSTATUS:
+		return put_user(0, p);
+
+	case WDIOC_SETOPTIONS:
+	{
+		int options;
+		int ret = -EINVAL;
+
+		if (get_user(options, p))
+			return -EFAULT;
+
+		if (options & WDIOS_DISABLECARD) {
+			prcmu_disable_a9wdog(wdog_id);
+			wdt_en = false;
+			ret = 0;
+		}
+
+		if (options & WDIOS_ENABLECARD) {
+			prcmu_enable_a9wdog(wdog_id);
+			wdt_en = true;
+			ret = 0;
+		}
+
+		return ret;
+	}
+	case WDIOC_KEEPALIVE:
+		return prcmu_kick_a9wdog(wdog_id);
+
+	case WDIOC_SETTIMEOUT:
+		if (get_user(interval, p))
+			return -EFAULT;
+
+		if (cpu_is_u8500()) {
+			/* 28 bit resolution in ms, becomes 268435.455 s */
+			if (interval > 268435 || interval < 0)
+				return -EINVAL;
+		} else if (cpu_is_u5500()) {
+			/* 32 bit resolution in ms, becomes 4294967.295 s */
+			if (interval > 4294967 || interval < 0)
+				return -EINVAL;
+		} else
+			return -EINVAL;
+
+		timeout = interval;
+		prcmu_disable_a9wdog(wdog_id);
+		prcmu_load_a9wdog(wdog_id, timeout * 1000);
+		prcmu_enable_a9wdog(wdog_id);
+
+	/* Fall through */
+	case WDIOC_GETTIMEOUT:
+		return put_user(timeout, p);
+
+	default:
+		return -ENOTTY;
+	}
+
+	return 0;
+}
+
+static const struct file_operations ux500_wdt_fops = {
+	.owner		= THIS_MODULE,
+	.llseek		= no_llseek,
+	.write		= ux500_wdt_write,
+	.unlocked_ioctl = ux500_wdt_ioctl,
+	.open		= ux500_wdt_open,
+	.release	= ux500_wdt_release,
+};
+
+static struct miscdevice ux500_wdt_miscdev = {
+	.minor = WATCHDOG_MINOR,
+	.name = "watchdog",
+	.fops = &ux500_wdt_fops,
+};
+
+#ifdef CONFIG_UX500_WATCHDOG_DEBUG
+enum wdog_dbg {
+	WDOG_DBG_CONFIG,
+	WDOG_DBG_LOAD,
+	WDOG_DBG_KICK,
+	WDOG_DBG_EN,
+	WDOG_DBG_DIS,
+};
+
+static ssize_t wdog_dbg_write(struct file *file,
+			      const char __user *user_buf,
+			      size_t count, loff_t *ppos)
+{
+	unsigned long val;
+	int err;
+	enum wdog_dbg v = (enum wdog_dbg)((struct seq_file *)
+					  (file->private_data))->private;
+
+	switch(v) {
+	case WDOG_DBG_CONFIG:
+		err = kstrtoul_from_user(user_buf, count, 0, &val);
+
+		if (!err) {
+			wdt_auto_off = val != 0;
+			(void) prcmu_config_a9wdog(1,
+						 wdt_auto_off);
+		}
+		else {
+			pr_err("ux500_wdt:dbg: unknown value\n");
+		}
+		break;
+	case WDOG_DBG_LOAD:
+		err = kstrtoul_from_user(user_buf, count, 0, &val);
+
+		if (!err) {
+			timeout = val;
+			/* Convert seconds to ms */
+			prcmu_disable_a9wdog(wdog_id);
+			prcmu_load_a9wdog(wdog_id, timeout * 1000);
+			prcmu_enable_a9wdog(wdog_id);
+		}
+		else {
+			pr_err("ux500_wdt:dbg: unknown value\n");
+		}
+		break;
+	case WDOG_DBG_KICK:
+		(void) prcmu_kick_a9wdog(wdog_id);
+		break;
+	case WDOG_DBG_EN:
+		wdt_en = true;
+		(void) prcmu_enable_a9wdog(wdog_id);
+		break;
+	case WDOG_DBG_DIS:
+		wdt_en = false;
+		(void) prcmu_disable_a9wdog(wdog_id);
+		break;
+	}
+
+	return count;
+}
+
+static int wdog_dbg_read(struct seq_file *s, void *p)
+{
+	enum wdog_dbg v = (enum wdog_dbg)s->private;
+
+	switch(v) {
+	case WDOG_DBG_CONFIG:
+		seq_printf(s,"wdog is on id %d, auto off on sleep: %s\n",
+			   (int)wdog_id,
+			   wdt_auto_off ? "enabled": "disabled");
+		break;
+	case WDOG_DBG_LOAD:
+		/* In 1s */
+		seq_printf(s, "wdog load is: %d s\n",
+			   timeout);
+		break;
+	case WDOG_DBG_KICK:
+		break;
+	case WDOG_DBG_EN:
+	case WDOG_DBG_DIS:
+		seq_printf(s, "wdog is %sabled\n",
+			       wdt_en ? "en" : "dis");
+		break;
+	}
+	return 0;
+}
+
+static int wdog_dbg_open(struct inode *inode,
+			struct file *file)
+{
+	return single_open(file, wdog_dbg_read, inode->i_private);
+}
+
+static const struct file_operations wdog_dbg_fops = {
+	.open		= wdog_dbg_open,
+	.write		= wdog_dbg_write,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+	.owner		= THIS_MODULE,
+};
+
+static int __init wdog_dbg_init(void)
+{
+	struct dentry *wdog_dir;
+
+	wdog_dir = debugfs_create_dir("wdog", NULL);
+	if (IS_ERR_OR_NULL(wdog_dir))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_u8("id",
+					     S_IWUGO | S_IRUGO, wdog_dir,
+					     &wdog_id)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("config",
+					       S_IWUGO | S_IRUGO, wdog_dir,
+					       (void *)WDOG_DBG_CONFIG,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("load",
+					       S_IWUGO | S_IRUGO, wdog_dir,
+					       (void *)WDOG_DBG_LOAD,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("kick",
+					       S_IWUGO, wdog_dir,
+					       (void *)WDOG_DBG_KICK,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("enable",
+					       S_IWUGO | S_IRUGO, wdog_dir,
+					       (void *)WDOG_DBG_EN,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	if (IS_ERR_OR_NULL(debugfs_create_file("disable",
+					       S_IWUGO | S_IRUGO, wdog_dir,
+					       (void *)WDOG_DBG_DIS,
+					       &wdog_dbg_fops)))
+		goto fail;
+
+	return 0;
+fail:
+	pr_err("ux500:wdog: Failed to initialize wdog dbg\n");
+	debugfs_remove_recursive(wdog_dir);
+
+	return -EFAULT;
+}
+
+#else
+static inline int __init wdog_dbg_init(void)
+{
+	return 0;
+}
+#endif
+
+static int __init ux500_wdt_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	/* Number of watch dogs */
+	prcmu_config_a9wdog(1, wdt_auto_off);
+	/* convert to ms */
+	prcmu_load_a9wdog(wdog_id, timeout * 1000);
+
+	ret = misc_register(&ux500_wdt_miscdev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to register misc\n");
+		return ret;
+	}
+
+	ret = wdog_dbg_init();
+	if (ret < 0)
+		goto fail;
+
+	dev_info(&pdev->dev, "initialized\n");
+
+	return 0;
+fail:
+	misc_deregister(&ux500_wdt_miscdev);
+	return ret;
+}
+
+static int __exit ux500_wdt_remove(struct platform_device *dev)
+{
+	prcmu_disable_a9wdog(wdog_id);
+	wdt_en = false;
+	misc_deregister(&ux500_wdt_miscdev);
+	return 0;
+}
+#ifdef CONFIG_PM
+static int ux500_wdt_suspend(struct platform_device *pdev,
+			     pm_message_t state)
+{
+	if (wdt_en && cpu_is_u5500()) {
+		prcmu_disable_a9wdog(wdog_id);
+		return 0;
+	}
+
+	if (wdt_en && !wdt_auto_off) {
+		prcmu_disable_a9wdog(wdog_id);
+		prcmu_config_a9wdog(1, true);
+
+		prcmu_load_a9wdog(wdog_id, timeout * 1000);
+		prcmu_enable_a9wdog(wdog_id);
+	}
+	return 0;
+}
+
+static int ux500_wdt_resume(struct platform_device *pdev)
+{
+	if (wdt_en && cpu_is_u5500()) {
+		prcmu_load_a9wdog(wdog_id, timeout * 1000);
+		prcmu_enable_a9wdog(wdog_id);
+		return 0;
+	}
+
+	if (wdt_en && !wdt_auto_off) {
+		prcmu_disable_a9wdog(wdog_id);
+		prcmu_config_a9wdog(1, wdt_auto_off);
+
+		prcmu_load_a9wdog(wdog_id, timeout * 1000);
+		prcmu_enable_a9wdog(wdog_id);
+	}
+	return 0;
+}
+
+#else
+#define ux500_wdt_suspend NULL
+#define ux500_wdt_resume NULL
+#endif
+static struct platform_driver ux500_wdt_driver = {
+	.remove		= __exit_p(ux500_wdt_remove),
+	.driver		= {
+		.owner	= THIS_MODULE,
+		.name	= "ux500_wdt",
+	},
+	.suspend	= ux500_wdt_suspend,
+	.resume		= ux500_wdt_resume,
+};
+
+static int __init ux500_wdt_init(void)
+{
+	return platform_driver_probe(&ux500_wdt_driver, ux500_wdt_probe);
+}
+module_init(ux500_wdt_init);
+
+MODULE_AUTHOR("Jonas Aaberg <jonas.aberg@stericsson.com>");
+MODULE_DESCRIPTION("Ux500 Watchdog Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_MISCDEV(WATCHDOG_MINOR);