ENGR00117389 Port 5.0.0 release to 2.6.31

This is i.MX BSP 5.0.0 release ported to 2.6.31

Signed-off-by: Rob Herring <r.herring@freescale.com>
Signed-off-by: Alan Tull <r80115@freescale.com>
Signed-off-by: Xinyu Chen <xinyu.chen@freescale.com>

64 files changed, 35048 insertions, 0 deletions

diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index 3315cac875e5..14b1af783e1a 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -406,4 +406,6 @@ config RADIO_TEA5764_XTAL
 	  Say Y here if TEA5764 have a 32768 Hz crystal in circuit, say N
 	  here if TEA5764 reference frequency is connected in FREQIN.
 
+source "drivers/media/radio/stfm1000/Kconfig"
+
 endif # RADIO_ADAPTERS
diff --git a/drivers/media/radio/Makefile b/drivers/media/radio/Makefile
index 0f2b35b3e560..d8f720f16782 100644
--- a/drivers/media/radio/Makefile
+++ b/drivers/media/radio/Makefile
@@ -21,4 +21,6 @@ obj-$(CONFIG_USB_SI470X) += radio-si470x.o
 obj-$(CONFIG_USB_MR800) += radio-mr800.o
 obj-$(CONFIG_RADIO_TEA5764) += radio-tea5764.o
 
+obj-$(CONFIG_RADIO_STFM1000) += stfm1000/
+
 EXTRA_CFLAGS += -Isound
diff --git a/drivers/media/radio/stfm1000/Kconfig b/drivers/media/radio/stfm1000/Kconfig
new file mode 100644
index 000000000000..ef30bf87de0b
--- /dev/null
+++ b/drivers/media/radio/stfm1000/Kconfig
@@ -0,0 +1,26 @@
+config RADIO_STFM1000
+	tristate "STFM1000 support"
+	depends on I2C && VIDEO_V4L2 && ARCH_STMP3XXX
+	select I2C_ALGOBIT
+	---help---
+	  Choose Y here if you have this FM radio card, and then fill in the
+	  port address below.
+
+	  In order to control your radio card, you will need to use programs
+	  that are compatible with the Video For Linux API.  Information on
+	  this API and pointers to "v4l" programs may be found at
+	  <file:Documentation/video4linux/API.html>.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called stfm1000.
+
+config RADIO_STFM1000_ALSA
+	tristate "STFM1000 audio support"
+	depends on RADIO_STFM1000 && SND
+	select SND_PCM
+	---help---
+	  This is a video4linux driver for direct (DMA) audio in
+	  STFM1000 using ALSA
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called stfm1000-alsa.
diff --git a/drivers/media/radio/stfm1000/Makefile b/drivers/media/radio/stfm1000/Makefile
new file mode 100644
index 000000000000..01f354001a64
--- /dev/null
+++ b/drivers/media/radio/stfm1000/Makefile
@@ -0,0 +1,14 @@
+stfm1000-objs	:= stfm1000-core.o stfm1000-i2c.o stfm1000-precalc.o stfm1000-filter.o stfm1000-rds.o
+
+clean-files += stfm1000-precalc.o
+
+obj-$(CONFIG_RADIO_STFM1000) += stfm1000.o
+obj-$(CONFIG_RADIO_STFM1000_ALSA) += stfm1000-alsa.o
+
+stfm1000-core.o: $(obj)/stfm1000-precalc.h
+
+hostprogs-$(CONFIG_RADIO_STFM1000) := gen-precalc
+$(obj)/stfm1000-precalc.c: $(obj)/gen-precalc $(src)/stfm1000-regs.h
+	$(obj)/gen-precalc >$@
+
+EXTRA_CFLAGS += -Idrivers/media/radio
diff --git a/drivers/media/radio/stfm1000/gen-precalc.c b/drivers/media/radio/stfm1000/gen-precalc.c
new file mode 100644
index 000000000000..d3797dbef815
--- /dev/null
+++ b/drivers/media/radio/stfm1000/gen-precalc.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+/* generate precalculated tables */
+#include <stddef.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "stfm1000-regs.h"
+
+static void generate_tune1(void)
+{
+	int start, end;
+	int ndiv;           // N Divider in PLL
+	int incr;           // Increment in PLL
+	int cicosr;         // CIC oversampling ratio
+	int sdnominal;      // value to serve pilot/interpolator loop in SD
+	int i, temp;        // used in tuning table construction
+
+	start = STFM1000_FREQUENCY_100KHZ_MIN;
+	end = start + STFM1000_FREQUENCY_100KHZ_RANGE;
+
+	printf("const struct stfm1000_tune1\n"
+		"stfm1000_tune1_table[STFM1000_FREQUENCY_100KHZ_RANGE] = {\n");
+
+	for (i = start; i < end; i++) {
+
+		ndiv = (int)((i+14)/15) - 48;
+		incr = i - (int)(i/15)*15;
+		cicosr = (int)(i*2/3.0/16.0 + 0.5);
+		sdnominal = (int)(i*100.0e3/1.5/(double)cicosr/2.0/2.0*2.0*8.0*256.0/228.0e3*65536);
+
+		temp = 0x00000000;                                    // clear
+		temp = temp | ((cicosr<<9) & STFM1000_TUNE1_CICOSR);  // bits[14:9] 0x00007E00
+		temp = temp | ((ndiv<<4)   & STFM1000_TUNE1_PLL_DIV); // bits[8:4]  0x000001F0
+		temp = temp | ((incr)      & STFM1000_TUNE1_PLL_DIV); // bits[3:0]  0x0000000F
+
+		printf("\t[%d - STFM1000_FREQUENCY_100KHZ_MIN] = "
+			"{ .tune1 = 0x%08x, .sdnom = 0x%08x },\n",
+			i, temp, sdnominal);
+	}
+	printf("};\n");
+
+}
+
+int main(int argc, char *argv[])
+{
+	printf("#include \"stfm1000-regs.h\"\n\n");
+
+	generate_tune1();
+
+	return 0;
+}
diff --git a/drivers/media/radio/stfm1000/stfm1000-alsa.c b/drivers/media/radio/stfm1000/stfm1000-alsa.c
new file mode 100644
index 000000000000..d1da4475bc07
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-alsa.c
@@ -0,0 +1,660 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/math64.h>
+
+#include <sound/control.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <linux/version.h>      /* for KERNEL_VERSION MACRO     */
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <mach/regs-dri.h>
+#include <mach/regs-apbx.h>
+#include <mach/regs-clkctrl.h>
+
+#include "stfm1000.h"
+
+#define STFM1000_PERIODS	16
+
+static int stfm1000_snd_volume_info(struct snd_kcontrol *kcontrol,
+				   struct snd_ctl_elem_info *uinfo)
+{
+	uinfo->type = SNDRV_CTL_ELEM_TYPE_INTEGER;
+	uinfo->count = 2;			/* two channels */
+	uinfo->value.integer.min = 0;
+	uinfo->value.integer.max = 20;
+	return 0;
+}
+
+static int stfm1000_snd_volume_get(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *ucontrol)
+{
+	struct stfm1000 *stfm1000 = snd_kcontrol_chip(kcontrol);
+
+	(void)stfm1000;
+	ucontrol->value.integer.value[0] = 0;	/* left */
+	ucontrol->value.integer.value[1] = 0;	/* right */
+	return 0;
+}
+
+static int stfm1000_snd_volume_put(struct snd_kcontrol *kcontrol,
+				  struct snd_ctl_elem_value *ucontrol)
+{
+	struct stfm1000 *stfm1000 = snd_kcontrol_chip(kcontrol);
+	int change;
+	int left, right;
+
+	(void)stfm1000;
+
+	left = ucontrol->value.integer.value[0];
+	if (left < 0)
+		left = 0;
+	if (left > 20)
+		left = 20;
+	right = ucontrol->value.integer.value[1];
+	if (right < 0)
+		right = 0;
+	if (right > 20)
+		right = 20;
+
+	change = 1;
+	return change;
+}
+
+static struct snd_kcontrol_new stfm1000_snd_controls[] = {
+	{
+		.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+		.name = "Radio Volume",
+		.index = 0,
+		.info = stfm1000_snd_volume_info,
+		.get = stfm1000_snd_volume_get,
+		.put = stfm1000_snd_volume_put,
+		.private_value = 0,
+	},
+};
+
+static struct snd_pcm_hardware stfm1000_snd_capture = {
+
+	.info =                 SNDRV_PCM_INFO_MMAP |
+				SNDRV_PCM_INFO_MMAP_VALID |
+				SNDRV_PCM_INFO_INTERLEAVED |
+				SNDRV_PCM_INFO_BLOCK_TRANSFER,
+	.formats =		SNDRV_PCM_FMTBIT_S16_LE,
+	.rates =		SNDRV_PCM_RATE_44100 | SNDRV_PCM_RATE_48000,
+	.rate_min =		44100,
+	.rate_max =		48000,
+	.channels_min =		2,
+	.channels_max =		2,
+	.buffer_bytes_max =	SZ_256K,
+	.period_bytes_min =	SZ_4K,
+	.period_bytes_max =	SZ_4K,
+	.periods_min =		STFM1000_PERIODS,
+	.periods_max =		STFM1000_PERIODS,
+};
+
+static int stfm1000_snd_capture_open(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+	int err;
+
+	/* should never happen, just a sanity check */
+	BUG_ON(stfm1000 == NULL);
+
+	mutex_lock(&stfm1000->deffered_work_lock);
+	stfm1000->read_count  = 0;
+	stfm1000->read_offset = 0;
+
+	stfm1000->substream = substream;
+	runtime->private_data = stfm1000;
+	runtime->hw = stfm1000_snd_capture;
+
+	mutex_unlock(&stfm1000->deffered_work_lock);
+
+	err = snd_pcm_hw_constraint_integer(runtime,
+		SNDRV_PCM_HW_PARAM_PERIODS);
+	if (err < 0) {
+		printk(KERN_ERR "%s: snd_pcm_hw_constraint_integer "
+			"SNDRV_PCM_HW_PARAM_PERIODS failed\n", __func__);
+		return err;
+	}
+
+	err = snd_pcm_hw_constraint_step(runtime, 0,
+		SNDRV_PCM_HW_PARAM_PERIODS, 2);
+	if (err < 0) {
+		printk(KERN_ERR "%s: snd_pcm_hw_constraint_integer "
+			"SNDRV_PCM_HW_PARAM_PERIODS failed\n", __func__);
+		return err;
+	}
+
+	return 0;
+}
+
+static int stfm1000_snd_capture_close(struct snd_pcm_substream *substream)
+{
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+
+	(void)stfm1000;	/* nothing */
+	return 0;
+}
+
+static int stfm1000_snd_hw_params(struct snd_pcm_substream *substream,
+	struct snd_pcm_hw_params *hw_params)
+{
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+	unsigned int period_size, periods;
+	int ret;
+
+	periods = params_periods(hw_params);
+	period_size = params_period_bytes(hw_params);
+
+	if (period_size < 0x100 || period_size > 0x10000)
+		return -EINVAL;
+	if (periods < STFM1000_PERIODS)
+		return -EINVAL;
+	if (period_size * periods > 1024 * 1024)
+		return -EINVAL;
+
+	stfm1000->blocks  = periods;
+	stfm1000->blksize = period_size;
+	stfm1000->bufsize = params_buffer_bytes(hw_params);
+
+	ret = snd_pcm_lib_malloc_pages(substream, stfm1000->bufsize);
+	if (ret < 0) {	/* 0 & 1 are valid returns */
+		printk(KERN_ERR "%s: snd_pcm_lib_malloc_pages() failed\n",
+			__func__);
+		return ret;
+	}
+
+	/* the dri buffer is twice as large as the audio buffer */
+	stfm1000->dri_bufsz = (stfm1000->bufsize / 4) *
+		sizeof(struct stfm1000_dri_sample);
+	stfm1000->dri_buf = dma_alloc_coherent(&stfm1000->radio.dev,
+			stfm1000->dri_bufsz, &stfm1000->dri_phys, GFP_KERNEL);
+	if (stfm1000->dri_buf == NULL) {
+		printk(KERN_ERR "%s: dma_alloc_coherent() failed\n", __func__);
+		snd_pcm_lib_free_pages(substream);
+		return -ENOMEM;
+	}
+
+	return ret;
+}
+
+static int stfm1000_snd_hw_free(struct snd_pcm_substream *substream)
+{
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+
+	if (stfm1000->dri_buf) {
+		dma_free_coherent(&stfm1000->radio.dev,
+			(stfm1000->bufsize / 4) *
+			sizeof(struct stfm1000_dri_sample),
+			stfm1000->dri_buf, stfm1000->dri_phys);
+		stfm1000->dri_buf = NULL;
+		stfm1000->dri_phys = 0;
+	}
+	snd_pcm_lib_free_pages(substream);
+	return 0;
+}
+
+
+static int stfm1000_snd_capture_prepare(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct stfm1000 *stfm1000 = snd_pcm_substream_chip(substream);
+
+	stfm1000->substream = substream;
+
+	if (snd_pcm_format_width(runtime->format) != 16 ||
+		!snd_pcm_format_signed(runtime->format) ||
+			snd_pcm_format_big_endian(runtime->format)) {
+		printk(KERN_INFO "STFM1000: ALSA capture_prepare illegal format\n");
+		return -EINVAL;
+	}
+
+	/* really shouldn't happen */
+	BUG_ON(stfm1000->blocks > stfm1000->desc_num);
+
+	mutex_lock(&stfm1000->deffered_work_lock);
+
+	if (stfm1000->now_recording != 0) {
+		printk(KERN_INFO "STFM1000: ALSA capture_prepare still running\n");
+		mutex_unlock(&stfm1000->deffered_work_lock);
+		return -EBUSY;
+	}
+	stfm1000->now_recording = 1;
+
+	mutex_unlock(&stfm1000->deffered_work_lock);
+
+	return 0;
+
+}
+
+static void stfm1000_snd_capture_trigger_start(struct work_struct *work)
+{
+	struct stfm1000 *stfm1000;
+
+	stfm1000 = container_of(work, struct stfm1000,
+		snd_capture_start_work.work);
+
+	mutex_lock(&stfm1000->deffered_work_lock);
+
+	BUG_ON(stfm1000->now_recording != 1);
+
+	stfm1000_bring_up(stfm1000);
+
+	mutex_unlock(&stfm1000->deffered_work_lock);
+}
+
+static void stfm1000_snd_capture_trigger_stop(struct work_struct *work)
+{
+	struct stfm1000 *stfm1000;
+
+	stfm1000 = container_of(work, struct stfm1000,
+		snd_capture_stop_work.work);
+
+	mutex_lock(&stfm1000->deffered_work_lock);
+
+	stfm1000->stopping_recording = 1;
+
+	stfm1000_take_down(stfm1000);
+
+	BUG_ON(stfm1000->now_recording != 1);
+	stfm1000->now_recording = 0;
+
+	stfm1000->stopping_recording = 0;
+
+	mutex_unlock(&stfm1000->deffered_work_lock);
+}
+
+static int execute_non_atomic(work_func_t fn, struct execute_work *ew)
+{
+	if (!in_atomic() && !in_interrupt()) {
+		fn(&ew->work);
+		return 0;
+	}
+
+	INIT_WORK(&ew->work, fn);
+	schedule_work(&ew->work);
+
+	return 1;
+}
+
+static int stfm1000_snd_capture_trigger(struct snd_pcm_substream *substream,
+	int cmd)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct stfm1000 *stfm1000 = runtime->private_data;
+	int err = 0;
+
+	(void)stfm1000;
+
+	switch (cmd) {
+
+	case SNDRV_PCM_TRIGGER_START:
+		execute_non_atomic(stfm1000_snd_capture_trigger_start,
+			&stfm1000->snd_capture_start_work);
+		break;
+
+	case SNDRV_PCM_TRIGGER_STOP:
+		execute_non_atomic(stfm1000_snd_capture_trigger_stop,
+			&stfm1000->snd_capture_stop_work);
+		break;
+
+	case SNDRV_PCM_TRIGGER_RESUME:
+	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+		stmp3xxx_dma_unfreeze(stfm1000->dma_ch);
+		break;
+
+	case SNDRV_PCM_TRIGGER_SUSPEND:
+	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+		stmp3xxx_dma_freeze(stfm1000->dma_ch);
+		break;
+
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	return err;
+}
+
+static snd_pcm_uframes_t
+stfm1000_snd_capture_pointer(struct snd_pcm_substream *substream)
+{
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct stfm1000 *stfm1000 = runtime->private_data;
+
+	if (stfm1000->read_count) {
+		stfm1000->read_count  -= snd_pcm_lib_period_bytes(substream);
+		stfm1000->read_offset += snd_pcm_lib_period_bytes(substream);
+		if (stfm1000->read_offset == substream->runtime->dma_bytes)
+			stfm1000->read_offset = 0;
+	}
+
+	return bytes_to_frames(runtime, stfm1000->read_offset);
+}
+
+static struct snd_pcm_ops stfm1000_snd_capture_ops = {
+	.open = stfm1000_snd_capture_open,
+	.close = stfm1000_snd_capture_close,
+	.ioctl = snd_pcm_lib_ioctl,
+	.hw_params = stfm1000_snd_hw_params,
+	.hw_free = stfm1000_snd_hw_free,
+	.prepare = stfm1000_snd_capture_prepare,
+	.trigger = stfm1000_snd_capture_trigger,
+	.pointer = stfm1000_snd_capture_pointer,
+};
+
+static void stfm1000_snd_free(struct snd_card *card)
+{
+	struct stfm1000 *stfm1000 = card->private_data;
+
+	free_irq(IRQ_DRI_ATTENTION, stfm1000);
+	free_irq(IRQ_DRI_DMA, stfm1000);
+}
+
+static int stfm1000_alsa_instance_init(struct stfm1000 *stfm1000)
+{
+	int ret, i;
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+	struct snd_kcontrol *ctl;
+
+	mutex_init(&stfm1000->deffered_work_lock);
+
+	/* request dma channel */
+	stfm1000->desc_num = STFM1000_PERIODS;
+	stfm1000->dma_ch = STMP3xxx_DMA(5, STMP3XXX_BUS_APBX);
+	ret = stmp3xxx_dma_request(stfm1000->dma_ch, &stfm1000->radio.dev,
+			"stmp3xxx dri");
+	if (ret != 0) {
+		printk(KERN_ERR "%s: stmp3xxx_dma_request failed\n", __func__);
+		goto err;
+	}
+
+	stfm1000->dma = kzalloc(sizeof(*stfm1000->dma) * stfm1000->desc_num,
+			GFP_KERNEL);
+	if (stfm1000->dma == NULL) {
+		printk(KERN_ERR "%s: stmp3xxx_dma_request failed\n", __func__);
+		ret = -ENOMEM;
+		goto err_rel_dma;
+	}
+
+	for (i = 0; i < stfm1000->desc_num; i++) {
+		ret = stmp3xxx_dma_allocate_command(stfm1000->dma_ch,
+				&stfm1000->dma[i]);
+		if (ret != 0) {
+			printk(KERN_ERR "%s: stmp3xxx_dma_allocate_command "
+				"failed\n", __func__);
+			goto err_free_dma;
+		}
+	}
+
+	/* allocate ALSA card structure (we only need an extra pointer
+	 * back to stfm1000) */
+	card = snd_card_new(-1, NULL, THIS_MODULE, 0);
+	if (card == NULL) {
+		ret = -ENOMEM;
+		printk(KERN_ERR "%s: snd_card_new failed\n", __func__);
+		goto err_free_dma;
+	}
+	stfm1000->card = card;
+	card->private_data = stfm1000;	/* point back */
+
+	/* mixer controls */
+	strcpy(card->driver, "stfm1000");
+	card->private_free = stfm1000_snd_free;
+
+	strcpy(card->mixername, "stfm1000 mixer");
+	for (i = 0; i < ARRAY_SIZE(stfm1000_snd_controls); i++) {
+		ctl = snd_ctl_new1(&stfm1000_snd_controls[i], stfm1000);
+		if (ctl == NULL) {
+			printk(KERN_ERR "%s: snd_ctl_new1 failed\n", __func__);
+			goto err_free_controls;
+		}
+		ret = snd_ctl_add(card, ctl);
+		if (ret != 0) {
+			printk(KERN_ERR "%s: snd_ctl_add failed\n", __func__);
+			goto err_free_controls;
+		}
+	}
+
+	/* PCM */
+	ret = snd_pcm_new(card, "STFM1000 PCM", 0, 0, 1, &pcm);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: snd_ctl_add failed\n", __func__);
+		goto err_free_controls;
+	}
+	stfm1000->pcm = pcm;
+	pcm->private_data = stfm1000;	/* point back */
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE,
+		&stfm1000_snd_capture_ops);
+	pcm->info_flags = 0;
+	strcpy(pcm->name, "STFM1000 PCM");
+
+	snd_card_set_dev(card, &stfm1000->radio.dev);
+	strcpy(card->shortname, "STFM1000");
+
+	ret = snd_pcm_lib_preallocate_pages_for_all(stfm1000->pcm,
+		SNDRV_DMA_TYPE_CONTINUOUS, card->dev, SZ_256K, SZ_256K);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: snd_pcm_lib_preallocate_pages_for_all "
+			"failed\n", __func__);
+		goto err_free_pcm;
+	}
+
+	ret = request_irq(IRQ_DRI_DMA, stfm1000_dri_dma_irq, 0, "stfm1000",
+		stfm1000);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: request_irq failed\n", __func__);
+		goto err_free_prealloc;
+	}
+
+	ret = request_irq(IRQ_DRI_ATTENTION, stfm1000_dri_attn_irq, 0,
+		"stfm1000", stfm1000);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: request_irq failed\n", __func__);
+		goto err_rel_irq;
+	}
+
+	ret = snd_card_register(stfm1000->card);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: snd_card_register failed\n", __func__);
+		goto err_rel_irq2;
+	}
+
+	/* Enable completion interrupt */
+	stmp3xxx_dma_clear_interrupt(stfm1000->dma_ch);
+	stmp3xxx_dma_enable_interrupt(stfm1000->dma_ch);
+
+	printk(KERN_INFO "%s/alsa: %s registered\n", "STFM1000",
+		card->longname);
+
+	return 0;
+
+err_rel_irq2:
+	free_irq(IRQ_DRI_ATTENTION, stfm1000);
+
+err_rel_irq:
+	free_irq(IRQ_DRI_DMA, stfm1000);
+
+err_free_prealloc:
+	snd_pcm_lib_preallocate_free_for_all(stfm1000->pcm);
+
+err_free_pcm:
+	/* XXX TODO */
+
+err_free_controls:
+	/* XXX TODO */
+
+/* err_free_card: */
+	snd_card_free(stfm1000->card);
+
+err_free_dma:
+	for (i = stfm1000->desc_num - 1; i >= 0; i--) {
+		if (stfm1000->dma[i].command != NULL)
+			stmp3xxx_dma_free_command(stfm1000->dma_ch,
+				&stfm1000->dma[i]);
+	}
+
+err_rel_dma:
+	stmp3xxx_dma_release(stfm1000->dma_ch);
+err:
+	return ret;
+}
+
+static void stfm1000_alsa_instance_release(struct stfm1000 *stfm1000)
+{
+	int i;
+
+	stmp3xxx_dma_clear_interrupt(stfm1000->dma_ch);
+	stmp3xxx_arch_dma_reset_channel(stfm1000->dma_ch);
+
+	snd_card_free(stfm1000->card);
+
+	for (i = stfm1000->desc_num - 1; i >= 0; i--)
+		stmp3xxx_dma_free_command(stfm1000->dma_ch, &stfm1000->dma[i]);
+
+	kfree(stfm1000->dma);
+
+	stmp3xxx_dma_release(stfm1000->dma_ch);
+}
+
+static void stfm1000_alsa_dma_irq(struct stfm1000 *stfm1000)
+{
+	struct snd_pcm_runtime *runtime;
+	int desc;
+	s16 *src, *dst;
+
+	if (stfm1000->stopping_recording)
+		return;
+
+	if (stfm1000->read_count >= stfm1000->blksize *
+			(stfm1000->blocks - 2)) {
+		printk(KERN_ERR "irq: overrun %d - Blocks in %d\n",
+			stfm1000->read_count, stfm1000->blocks);
+		return;
+	}
+
+	/* someone has brutally killed user-space */
+	if (stfm1000->substream == NULL ||
+			stfm1000->substream->runtime == NULL)
+		return;
+
+	BUG_ON(stfm1000->substream == NULL);
+	BUG_ON(stfm1000->substream->runtime == NULL);
+
+	desc = stfm1000->read_offset / stfm1000->blksize;
+	runtime = stfm1000->substream->runtime;
+
+	if (runtime->dma_area == NULL)
+		printk(KERN_INFO "runtime->dma_area = NULL\n");
+	BUG_ON(runtime->dma_area == NULL);
+	if (stfm1000->dri_buf == NULL)
+		printk(KERN_INFO "stfm1000->dri_buf = NULL\n");
+	BUG_ON(stfm1000->dri_buf == NULL);
+
+	if (desc >= stfm1000->blocks) {
+		printk(KERN_INFO "desc=%d ->blocks=%d\n",
+				desc, stfm1000->blocks);
+		printk(KERN_INFO "->read_offset=%x ->blksize=%x\n",
+				stfm1000->read_offset, stfm1000->blksize);
+	}
+	BUG_ON(desc >= stfm1000->blocks);
+
+	src = stfm1000->dri_buf + desc * (stfm1000->blksize * 2);
+	dst = (void *)runtime->dma_area + desc * stfm1000->blksize;
+
+	/* perform filtering */
+	stfm1000_decode_block(stfm1000, src, dst, stfm1000->blksize / 4);
+
+	stfm1000->read_count += stfm1000->blksize;
+
+	if (stfm1000->read_count >=
+		snd_pcm_lib_period_bytes(stfm1000->substream))
+		snd_pcm_period_elapsed(stfm1000->substream);
+}
+
+static void stfm1000_alsa_attn_irq(struct stfm1000 *stfm1000)
+{
+	/* nothing */
+}
+
+struct stfm1000_alsa_ops stfm1000_default_alsa_ops = {
+	.init = stfm1000_alsa_instance_init,
+	.release = stfm1000_alsa_instance_release,
+	.dma_irq = stfm1000_alsa_dma_irq,
+	.attn_irq = stfm1000_alsa_attn_irq,
+};
+
+static int stfm1000_alsa_init(void)
+{
+	struct stfm1000 *stfm1000 = NULL;
+	struct list_head *list;
+	int ret;
+
+	stfm1000_alsa_ops = &stfm1000_default_alsa_ops;
+
+	list_for_each(list, &stfm1000_devlist) {
+		stfm1000 = list_entry(list, struct stfm1000, devlist);
+		ret = (*stfm1000_alsa_ops->init)(stfm1000);
+		if (ret != 0) {
+			printk(KERN_ERR "stfm1000 ALSA driver for DMA sound "
+				"failed init.\n");
+			return ret;
+		}
+		stfm1000->alsa_initialized = 1;
+	}
+
+	printk(KERN_INFO "stfm1000 ALSA driver for DMA sound loaded\n");
+
+	return 0;
+}
+
+static void stfm1000_alsa_exit(void)
+{
+	struct stfm1000 *stfm1000 = NULL;
+	struct list_head *list;
+
+	list_for_each(list, &stfm1000_devlist) {
+		stfm1000 = list_entry(list, struct stfm1000, devlist);
+
+		if (!stfm1000->alsa_initialized)
+			continue;
+
+		stfm1000_take_down(stfm1000);
+		(*stfm1000_alsa_ops->release)(stfm1000);
+		stfm1000->alsa_initialized = 0;
+	}
+
+	printk(KERN_INFO "stfm1000 ALSA driver for DMA sound unloaded\n");
+}
+
+/* We initialize this late, to make sure the sound system is up and running */
+late_initcall(stfm1000_alsa_init);
+module_exit(stfm1000_alsa_exit);
+
+MODULE_AUTHOR("Pantelis Antoniou");
+MODULE_DESCRIPTION("An ALSA PCM driver for the STFM1000 chip.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/radio/stfm1000/stfm1000-core.c b/drivers/media/radio/stfm1000/stfm1000-core.c
new file mode 100644
index 000000000000..5086100bb480
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-core.c
@@ -0,0 +1,2459 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/math64.h>
+
+#include <sound/control.h>
+#include <sound/pcm.h>
+#include <sound/pcm_params.h>
+#include <linux/version.h>	/* for KERNEL_VERSION MACRO */
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include <linux/device.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+#include <mach/regs-dri.h>
+#include <mach/regs-apbx.h>
+#include <mach/regs-clkctrl.h>
+
+#include "stfm1000.h"
+
+static DEFINE_MUTEX(devlist_lock);
+static unsigned int stfm1000_devcount;
+
+LIST_HEAD(stfm1000_devlist);
+EXPORT_SYMBOL(stfm1000_devlist);
+
+/* alsa interface */
+struct stfm1000_alsa_ops *stfm1000_alsa_ops;
+EXPORT_SYMBOL(stfm1000_alsa_ops);
+
+/* region, 0=US, 1=europe */
+static int georegion = 1;	/* default is europe */
+static int rds_enable = 1;	/* default is enabled */
+
+static int sw_tune(struct stfm1000 *stfm1000, u32 freq);
+
+static const const char *stfm1000_get_rev_txt(u32 id)
+{
+	switch (id) {
+	case 0x01: return "TA1";
+	case 0x02: return "TA2";
+	case 0x11: return "TB1";
+	case 0x12: return "TB2";
+	}
+	return NULL;
+}
+
+static const struct stfm1000_reg stfm1000_tb2_powerup[] = {
+	STFM1000_REG(REF, 0x00200000),
+	STFM1000_DELAY(20),
+	STFM1000_REG(DATAPATH, 0x00010210),
+	STFM1000_REG(TUNE1, 0x0004CF01),
+	STFM1000_REG(SDNOMINAL, 0x1C5EBCF0),
+	STFM1000_REG(PILOTTRACKING, 0x000001B6),
+	STFM1000_REG(INITIALIZATION1, 0x9fb80008),
+	STFM1000_REG(INITIALIZATION2, 0x8516e444 | STFM1000_DEEMPH_50_75B),
+	STFM1000_REG(INITIALIZATION3, 0x1402190b),
+	STFM1000_REG(INITIALIZATION4, 0x525bf052),
+	STFM1000_REG(INITIALIZATION5, 0x1000d106),
+	STFM1000_REG(INITIALIZATION6, 0x000062cb),
+	STFM1000_REG(AGC_CONTROL1, 0x1BCB2202),
+	STFM1000_REG(AGC_CONTROL2, 0x000020F0),
+	STFM1000_REG(CLK1, 0x10000000),
+	STFM1000_REG(CLK1, 0x20000000),
+	STFM1000_REG(CLK1, 0x00000000),
+	STFM1000_REG(CLK2, 0x7f000000),
+	STFM1000_REG(REF, 0x00B8222D),
+	STFM1000_REG(CLK1, 0x30000000),
+	STFM1000_REG(CLK1, 0x30002000),
+	STFM1000_REG(CLK1, 0x10002000),
+	STFM1000_REG(LNA, 0x0D080009),
+	STFM1000_DELAY(10),
+	STFM1000_REG(MIXFILT, 0x00008000),
+	STFM1000_REG(MIXFILT, 0x00000000),
+	STFM1000_REG(MIXFILT, 0x00007205),
+	STFM1000_REG(ADC, 0x001B3282),
+	STFM1000_REG(ATTENTION, 0x0000003F),
+	STFM1000_END,
+};
+
+static const struct stfm1000_reg stfm1000_ta2_powerup[] = {
+	STFM1000_REG(REF, 0x00200000),
+	STFM1000_DELAY(20),
+	STFM1000_REG(DATAPATH, 0x00010210),
+	STFM1000_REG(TUNE1, 0x00044F01),
+	STFM1000_REG(SDNOMINAL, 0x1C5EBCF0),
+	STFM1000_REG(PILOTTRACKING, 0x000001B6),
+	STFM1000_REG(INITIALIZATION1, 0x9fb80008),
+	STFM1000_REG(INITIALIZATION2, 0x8506e444),
+	STFM1000_REG(INITIALIZATION3, 0x1402190b),
+	STFM1000_REG(INITIALIZATION4, 0x525bf052),
+	STFM1000_REG(INITIALIZATION5, 0x7000d106),
+	STFM1000_REG(INITIALIZATION6, 0x0000c2cb),
+	STFM1000_REG(AGC_CONTROL1, 0x002c8402),
+	STFM1000_REG(AGC_CONTROL2, 0x00140050),
+	STFM1000_REG(CLK1, 0x10000000),
+	STFM1000_REG(CLK1, 0x20000000),
+	STFM1000_REG(CLK1, 0x00000000),
+	STFM1000_REG(CLK2, 0x7f000000),
+	STFM1000_REG(REF, 0x0030222D),
+	STFM1000_REG(CLK1, 0x30000000),
+	STFM1000_REG(CLK1, 0x30002000),
+	STFM1000_REG(CLK1, 0x10002000),
+	STFM1000_REG(LNA, 0x05080009),
+	STFM1000_REG(MIXFILT, 0x00008000),
+	STFM1000_REG(MIXFILT, 0x00000000),
+	STFM1000_REG(MIXFILT, 0x00007200),
+	STFM1000_REG(ADC, 0x00033000),
+	STFM1000_REG(ATTENTION, 0x0000003F),
+	STFM1000_END,
+};
+
+static const struct stfm1000_reg stfm1000_powerdown[] = {
+	STFM1000_REG(DATAPATH, 0x00010210),
+	STFM1000_REG(REF, 0),
+	STFM1000_REG(LNA, 0),
+	STFM1000_REG(MIXFILT, 0),
+	STFM1000_REG(CLK1, 0x20000000),
+	STFM1000_REG(CLK1, 0),
+	STFM1000_REG(CLK2, 0),
+	STFM1000_REG(ADC, 0),
+	STFM1000_REG(TUNE1, 0),
+	STFM1000_REG(SDNOMINAL, 0),
+	STFM1000_REG(PILOTTRACKING, 0),
+	STFM1000_REG(INITIALIZATION1, 0),
+	STFM1000_REG(INITIALIZATION2, 0),
+	STFM1000_REG(INITIALIZATION3, 0),
+	STFM1000_REG(INITIALIZATION4, 0),
+	STFM1000_REG(INITIALIZATION5, 0),
+	STFM1000_REG(INITIALIZATION6, 0x00007E00),
+	STFM1000_REG(AGC_CONTROL1, 0),
+	STFM1000_REG(AGC_CONTROL2, 0),
+	STFM1000_REG(DATAPATH, 0x00000200),
+};
+
+struct stfm1000_tuner_pmi {
+	u32 min;
+	u32 max;
+	u32 freq;
+	u32 pll_xtal;	/* 1 = pll, 0 = xtal */
+};
+
+#define PLL 1
+#define XTAL 0
+
+static const struct stfm1000_tuner_pmi stfm1000_pmi_lookup[] = {
+	{ .min =  76100, .max =  76500, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  79700, .max =  79900, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  80800, .max =  81200, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  82100, .max =  82600, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  86800, .max =  87200, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  88100, .max =  88600, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  89800, .max =  90500, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  91400, .max =  91900, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  92800, .max =  93300, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  97400, .max =  97900, .freq = 19200, .pll_xtal = PLL },
+	{ .min =  98800, .max =  99200, .freq = 19200, .pll_xtal = PLL },
+	{ .min = 100200, .max = 100400, .freq = 19200, .pll_xtal = PLL },
+	{ .min = 103500, .max = 103900, .freq = 19200, .pll_xtal = PLL },
+	{ .min = 104800, .max = 105200, .freq = 19200, .pll_xtal = PLL },
+	{ .min = 106100, .max = 106500, .freq = 19200, .pll_xtal = PLL },
+
+	{ .min =  76600, .max =  77000, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  77800, .max =  78300, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  79200, .max =  79600, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  80600, .max =  80700, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  83900, .max =  84400, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  85300, .max =  85800, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  94200, .max =  94700, .freq = 20000, .pll_xtal = PLL },
+	{ .min =  95600, .max =  96100, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 100500, .max = 100800, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 101800, .max = 102200, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 103100, .max = 103400, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 106600, .max = 106900, .freq = 20000, .pll_xtal = PLL },
+	{ .min = 107800, .max = 108000, .freq = 20000, .pll_xtal = PLL },
+
+	{ .min =      0, .max =      0, .freq = 24000, .pll_xtal = XTAL }
+};
+
+int stfm1000_power_up(struct stfm1000 *stfm1000)
+{
+	struct stfm1000_reg *reg, *pwrup_reg;
+	const struct stfm1000_reg *orig_reg, *treg;
+	int ret, size;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	/* Enable DRI clock for 24Mhz. */
+	HW_CLKCTRL_XTAL_CLR(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE);
+
+	orig_reg = stfm1000->revid == STFM1000_CHIP_REV_TA2 ?
+			stfm1000_ta2_powerup : stfm1000_tb2_powerup;
+
+	/* find size of the set */
+	for (treg = orig_reg; treg->regno != STFM1000_REG_END; treg++)
+		;
+	size = (treg + 1 - orig_reg) * sizeof(*treg);
+
+	/* allocate copy */
+	pwrup_reg = kmalloc(size, GFP_KERNEL);
+	if (pwrup_reg == NULL) {
+		printk(KERN_ERR "%s: out of memory\n", __func__);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* copy it */
+	memcpy(pwrup_reg, orig_reg, size);
+
+	/* fixup region of INITILIZATION2 */
+	for (reg = pwrup_reg; reg->regno != STFM1000_REG_END; reg++) {
+
+		/* we only care for INITIALIZATION2 register */
+		if (reg->regno != STFM1000_INITIALIZATION2)
+			continue;
+
+		/* geographic region select */
+		if (stfm1000->georegion == 0)	/* USA */
+			reg->value &= ~STFM1000_DEEMPH_50_75B;
+		else			/* Europe */
+			reg->value |=  STFM1000_DEEMPH_50_75B;
+
+		/* RDS enabled */
+		if (stfm1000->revid == STFM1000_CHIP_REV_TB2) {
+			if (stfm1000->rds_enable)
+				reg->value |=  STFM1000_RDS_ENABLE;
+			else
+				reg->value &= ~STFM1000_RDS_ENABLE;
+		}
+	}
+
+	ret = stfm1000_write_regs(stfm1000, pwrup_reg);
+
+	kfree(pwrup_reg);
+out:
+	mutex_unlock(&stfm1000->state_lock);
+
+	return ret;
+}
+
+int stfm1000_power_down(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	/* Disable DRI clock for 24Mhz. */
+	HW_CLKCTRL_XTAL_CLR(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE);
+
+	ret = stfm1000_write_regs(stfm1000, stfm1000_powerdown);
+
+	/* Disable DRI clock for 24Mhz. */
+	/* XXX bug warning, disabling the DRI clock is bad news */
+	/* doing so causes noise to be received from the DRI */
+	/* interface. Leave it on for now */
+	/* HW_CLKCTRL_XTAL_CLR(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE); */
+
+	mutex_unlock(&stfm1000->state_lock);
+
+	return ret;
+}
+
+int stfm1000_dcdc_update(struct stfm1000 *stfm1000, u32 freq)
+{
+	const struct stfm1000_tuner_pmi *pmi;
+	int i;
+
+	/* search for DCDC frequency */
+	pmi = stfm1000_pmi_lookup;
+	for (i = 0;  i < ARRAY_SIZE(stfm1000_pmi_lookup); i++, pmi++) {
+		if (freq >= pmi->min && freq <= pmi->max)
+			break;
+	}
+	if (i >= ARRAY_SIZE(stfm1000_pmi_lookup))
+		return -1;
+
+	/* adjust DCDC frequency so that it is out of Tuner PLL range */
+	/* XXX there is no adjustment API (os_pmi_SetDcdcFreq)*/
+	return 0;
+}
+
+static void Mute_Audio(struct stfm1000 *stfm1000)
+{
+	stfm1000->mute = 1;
+}
+
+static void Unmute_Audio(struct stfm1000 *stfm1000)
+{
+	stfm1000->mute = 0;
+}
+
+static const struct stfm1000_reg sd_dp_on_regs[] = {
+	STFM1000_REG_SETBITS(DATAPATH, STFM1000_DP_EN),
+	STFM1000_DELAY(3),
+	STFM1000_REG_SETBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_REG_CLRBITS(AGC_CONTROL1, STFM1000_B2_BYPASS_AGC_CTL),
+	STFM1000_REG_CLRBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_END,
+};
+
+static int SD_DP_On(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = stfm1000_write_regs(stfm1000, sd_dp_on_regs);
+	if (ret != 0)
+		return ret;
+
+	return 0;
+}
+
+static const struct stfm1000_reg sd_dp_off_regs[] = {
+	STFM1000_REG_SETBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_REG_CLRBITS(DATAPATH, STFM1000_DP_EN),
+	STFM1000_REG_SETBITS(AGC_CONTROL1, STFM1000_B2_BYPASS_AGC_CTL),
+	STFM1000_REG_CLRBITS(PILOTTRACKING, STFM1000_B2_PILOTTRACKING_EN),
+	STFM1000_REG_CLRBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_END,
+};
+
+static int SD_DP_Off(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = stfm1000_write_regs(stfm1000, sd_dp_off_regs);
+	if (ret != 0)
+		return ret;
+
+	return 0;
+}
+
+static int DRI_Start_Stream(struct stfm1000 *stfm1000)
+{
+	dma_addr_t dma_buffer_phys;
+	int i, next;
+	u32 cmd;
+
+	/* we must not be gated */
+	BUG_ON(HW_CLKCTRL_XTAL_RD() & BM_CLKCTRL_XTAL_DRI_CLK24M_GATE);
+
+	/* hw_dri_SetReset */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_SFTRST | BM_DRI_CTRL_CLKGATE);
+	HW_DRI_CTRL_SET(BM_DRI_CTRL_SFTRST);
+	while ((HW_DRI_CTRL_RD() & BM_DRI_CTRL_CLKGATE) == 0)
+		cpu_relax();
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_SFTRST | BM_DRI_CTRL_CLKGATE);
+
+	/* DRI enable/config */
+	HW_DRI_TIMING_WR(BF_DRI_TIMING_GAP_DETECTION_INTERVAL(0x10) |
+			 BF_DRI_TIMING_PILOT_REP_RATE(0x08));
+
+	/* XXX SDK bug */
+	/* While the SDK enables the gate here, everytime the stream */
+	/* is started, doing so, causes the DRI to input audio noise */
+	/* at any subsequent starts */
+	/* Enable DRI clock for 24Mhz. */
+	/* HW_CLKCTRL_XTAL_CLR(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE); */
+
+	stmp3xxx_arch_dma_reset_channel(stfm1000->dma_ch);
+
+	dma_buffer_phys = stfm1000->dri_phys;
+
+	for (i = 0; i < stfm1000->blocks; i++) {
+		next = (i + 1) % stfm1000->blocks;
+
+		/* link */
+		stfm1000->dma[i].command->next = stfm1000->dma[next].handle;
+		stfm1000->dma[i].next_descr = &stfm1000->dma[next];
+
+		/* receive DRI is 8 bytes per 4 samples */
+		cmd = BF_APBX_CHn_CMD_XFER_COUNT(stfm1000->blksize * 2) |
+		      BM_APBX_CHn_CMD_IRQONCMPLT |
+		      BM_APBX_CHn_CMD_CHAIN |
+		      BF_APBX_CHn_CMD_COMMAND(
+			BV_APBX_CHn_CMD_COMMAND__DMA_WRITE);
+
+		stfm1000->dma[i].command->cmd = cmd;
+		stfm1000->dma[i].command->buf_ptr = dma_buffer_phys;
+		stfm1000->dma[i].command->pio_words[0] =
+			BM_DRI_CTRL_OVERFLOW_IRQ_EN |
+			BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ_EN |
+			BM_DRI_CTRL_ATTENTION_IRQ_EN |
+			/* BM_DRI_CTRL_STOP_ON_OFLOW_ERROR | */
+			/* BM_DRI_CTRL_STOP_ON_PILOT_ERROR | */
+			BM_DRI_CTRL_ENABLE_INPUTS;
+
+		dma_buffer_phys += stfm1000->blksize * 2;
+
+	}
+
+	/* Enable completion interrupt */
+	stmp3xxx_dma_clear_interrupt(stfm1000->dma_ch);
+	stmp3xxx_dma_enable_interrupt(stfm1000->dma_ch);
+
+	/* clear DRI interrupts pending */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_OVERFLOW_IRQ |
+			BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ |
+			BM_DRI_CTRL_ATTENTION_IRQ);
+
+	/* Stop DRI on error */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_STOP_ON_OFLOW_ERROR |
+			BM_DRI_CTRL_STOP_ON_PILOT_ERROR);
+
+	/* Reacquire data stream */
+	HW_DRI_CTRL_SET(BM_DRI_CTRL_REACQUIRE_PHASE |
+		BM_DRI_CTRL_OVERFLOW_IRQ_EN |
+		BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ_EN |
+		BM_DRI_CTRL_ATTENTION_IRQ_EN |
+		BM_DRI_CTRL_ENABLE_INPUTS);
+
+	stmp3xxx_dma_go(stfm1000->dma_ch, stfm1000->dma, 1);
+
+	/* Turn on DRI hardware (don't forget to leave RUN bit ON) */
+	HW_DRI_CTRL_SET(BM_DRI_CTRL_RUN);
+
+	return 0;
+}
+
+static int DRI_Stop_Stream(struct stfm1000 *stfm1000)
+{
+	int desc;
+
+	/* disable interrupts */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_OVERFLOW_IRQ_EN |
+			BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ_EN |
+			BM_DRI_CTRL_ATTENTION_IRQ_EN);
+
+	/* Freeze DMA channel for a moment */
+	stmp3xxx_dma_freeze(stfm1000->dma_ch);
+
+	/* all descriptors, set sema bit */
+	for (desc = 0; desc < stfm1000->blocks; desc++)
+		stfm1000->dma[desc].command->cmd |= BM_APBX_CHn_CMD_SEMAPHORE;
+
+	/* Let the current DMA transaction finish */
+	stmp3xxx_dma_unfreeze(stfm1000->dma_ch);
+	msleep(5);
+
+	/* dma shutdown */
+	stmp3xxx_arch_dma_reset_channel(stfm1000->dma_ch);
+
+	/* Turn OFF data lines and stop controller */
+	HW_DRI_CTRL_CLR(BM_DRI_CTRL_ENABLE_INPUTS | BM_DRI_CTRL_RUN);
+
+	/* hw_dri_SetReset */
+	HW_DRI_CTRL_SET(BM_DRI_CTRL_SFTRST | BM_DRI_CTRL_CLKGATE);
+
+	/* XXX SDK bug */
+	/* While the SDK enables the gate here, everytime the stream */
+	/* is started, doing so, causes the DRI to input audio noise */
+	/* at any subsequent starts */
+	/* Enable DRI clock for 24Mhz. */
+	/* Disable DRI clock for 24Mhz. */
+	/* HW_CLKCTRL_XTAL_SET(BM_CLKCTRL_XTAL_DRI_CLK24M_GATE); */
+
+	return 0;
+}
+
+static int DRI_On(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	if (stfm1000->active)
+		DRI_Start_Stream(stfm1000);
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_SAI_EN);
+	return ret;
+}
+
+static int DRI_Off(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	if (stfm1000->active)
+		DRI_Stop_Stream(stfm1000);
+
+	ret = stfm1000_clear_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_SAI_EN);
+
+	return 0;
+}
+
+static int SD_Set_Channel_Filter(struct stfm1000 *stfm1000)
+{
+	int bypass_setting;
+	int sig_qual;
+	u32 tmp;
+	int ret;
+
+	/*
+	 * set channel filter
+	 *
+	 * B2_NEAR_CHAN_MIX_REG_MASK values from T-Spec
+	 * 000 : 0 kHz mix.
+	 * 001 : +100 kHz mix.
+	 * 010 : +200 kHz mix.
+	 * 011 : +300 kHz mix.
+	 * 100 : -400 kHz mix.
+	 * 101 : -300 kHz mix.
+	 * 110 : -200 kHz mix.
+	 * 111 : -100 kHz mix.
+	 */
+
+	/* get near channel amplitude */
+	ret = stfm1000_write_masked(stfm1000, STFM1000_INITIALIZATION3,
+		STFM1000_B2_NEAR_CHAN_MIX(0x01),
+		STFM1000_B2_NEAR_CHAN_MIX_MASK);
+	if (ret != 0)
+		return ret;
+
+	msleep(10);	/* wait for the signal quality to settle */
+
+	ret = stfm1000_read(stfm1000, STFM1000_SIGNALQUALITY, &tmp);
+	if (ret != 0)
+		return ret;
+
+	sig_qual = (tmp & STFM1000_NEAR_CHAN_AMPLITUDE_MASK) >>
+		STFM1000_NEAR_CHAN_AMPLITUDE_SHIFT;
+
+	bypass_setting = 0;
+
+	/* check near channel amplitude vs threshold */
+	if (sig_qual < stfm1000->adj_chan_th) {
+		/* get near channel amplitude again */
+		ret = stfm1000_write_masked(stfm1000, STFM1000_INITIALIZATION3,
+			STFM1000_B2_NEAR_CHAN_MIX(0x05),
+			STFM1000_B2_NEAR_CHAN_MIX_MASK);
+		if (ret != 0)
+			return ret;
+
+		msleep(10);	/* wait for the signal quality to settle */
+
+		ret = stfm1000_read(stfm1000, STFM1000_SIGNALQUALITY, &tmp);
+		if (ret != 0)
+			return ret;
+
+		sig_qual = (tmp & STFM1000_NEAR_CHAN_AMPLITUDE_MASK) >>
+			STFM1000_NEAR_CHAN_AMPLITUDE_SHIFT;
+
+		if (sig_qual < stfm1000->adj_chan_th)
+			bypass_setting = 2;
+	}
+
+	/* set filter settings */
+	ret = stfm1000_write_masked(stfm1000, STFM1000_INITIALIZATION1,
+		STFM1000_B2_BYPASS_FILT(bypass_setting),
+		STFM1000_B2_BYPASS_FILT_MASK);
+	if (ret != 0)
+		return ret;
+
+	return 0;
+}
+
+static int SD_Look_For_Pilot_TA2(struct stfm1000 *stfm1000)
+{
+	int i;
+	u32 pilot;
+	int ret;
+
+	/* assume pilot */
+	stfm1000->pilot_present = 1;
+
+	for (i = 0; i < 3; i++) {
+
+		ret = stfm1000_read(stfm1000, STFM1000_PILOTCORRECTION,
+			&pilot);
+		if (ret != 0)
+			return ret;
+
+		pilot &= STFM1000_PILOTEST_TA2_MASK;
+		pilot >>= STFM1000_PILOTEST_TA2_SHIFT;
+
+		/* out of range? */
+		if (pilot < 0xe2 || pilot >= 0xb5) {
+			stfm1000->pilot_present = 0;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+
+static int SD_Look_For_Pilot_TB2(struct stfm1000 *stfm1000)
+{
+	int i;
+	u32 pilot;
+	int ret;
+
+	/* assume pilot */
+	stfm1000->pilot_present = 1;
+
+	for (i = 0; i < 3; i++) {
+
+		ret = stfm1000_read(stfm1000, STFM1000_PILOTCORRECTION,
+			&pilot);
+		if (ret != 0)
+			return ret;
+
+		pilot &= STFM1000_PILOTEST_TB2_MASK;
+		pilot >>= STFM1000_PILOTEST_TB2_SHIFT;
+
+		/* out of range? */
+		if (pilot < 0x1e || pilot >= 0x7f) {
+			stfm1000->pilot_present = 0;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int SD_Look_For_Pilot(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	if (stfm1000->revid == STFM1000_CHIP_REV_TA2)
+		ret = SD_Look_For_Pilot_TA2(stfm1000);
+	else
+		ret = SD_Look_For_Pilot_TB2(stfm1000);
+
+	if (ret != 0)
+		return ret;
+
+	if (!stfm1000->pilot_present) {
+		ret = stfm1000_clear_bits(stfm1000, STFM1000_PILOTTRACKING,
+			STFM1000_B2_PILOTTRACKING_EN);
+		if (ret != 0)
+			return ret;
+
+		/* set force mono parameters for the filter */
+		stfm1000->filter_parms.pCoefForcedMono = 1;
+
+		/* yeah, I know, it's stupid */
+		stfm1000->rds_state.demod.pCoefForcedMono =
+			stfm1000->filter_parms.pCoefForcedMono;
+	}
+
+	return 0;
+}
+
+static int SD_Gear_Shift_Pilot_Tracking(struct stfm1000 *stfm1000)
+{
+	static const struct {
+		int delay;
+		u32 value;
+	} track_table[] = {
+		{ .delay = 10, .value = 0x81b6 },
+		{ .delay =  6, .value = 0x82a5 },
+		{ .delay =  6, .value = 0x8395 },
+		{ .delay =  8, .value = 0x8474 },
+		{ .delay = 20, .value = 0x8535 },
+		{ .delay = 50, .value = 0x8632 },
+		{ .delay =  0, .value = 0x8810 },
+	};
+	int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(track_table); i++) {
+		ret = stfm1000_write(stfm1000, STFM1000_PILOTTRACKING,
+			track_table[i].value);
+		if (ret != 0)
+			return ret;
+
+		if (i < ARRAY_SIZE(track_table) - 1)	/* last one no delay */
+			msleep(track_table[i].delay);
+	}
+
+	return 0;
+}
+
+static int SD_Optimize_Channel(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		return ret;
+
+	ret = stfm1000_write(stfm1000, STFM1000_PILOTTRACKING,
+		STFM1000_B2_PILOTTRACKING_EN |
+		STFM1000_B2_PILOTLPF_TIMECONSTANT(0x01) |
+		STFM1000_B2_PFDSCALE(0x0B) |
+		STFM1000_B2_PFDFILTER_SPEEDUP(0x06));	/* 0x000081B6 */
+	if (ret != 0)
+		return ret;
+
+	ret = SD_Set_Channel_Filter(stfm1000);
+	if (ret != 0)
+		return ret;
+
+	ret = SD_Look_For_Pilot(stfm1000);
+	if (ret != 0)
+		return ret;
+
+	if (stfm1000->pilot_present) {
+		ret = SD_Gear_Shift_Pilot_Tracking(stfm1000);
+		if (ret != 0)
+			return ret;
+	}
+
+	ret = stfm1000_clear_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		return ret;
+
+	return 0;
+}
+
+static int Monitor_STFM_Quality(struct stfm1000 *stfm1000)
+{
+	u32 tmp, rssi_dc_est, tone_data;
+	u32 lna_rms, bias, agc_out, lna_th, lna, ref;
+	u16 rssi_mantissa, rssi_exponent, rssi_decoded;
+	u16 prssi;
+	s16 mpx_dc;
+	int rssi_log;
+	int bypass_filter;
+	int ret;
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		return ret;
+
+	/* Get Rssi register readings from STFM1000 */
+	stfm1000_read(stfm1000, STFM1000_RSSI_TONE, &tmp);
+	rssi_dc_est = tmp & 0xffff;
+	tone_data = (tmp >> 16) & 0x0fff;
+
+	rssi_mantissa = (rssi_dc_est & 0xffe0) >> 5;	/* 11Msb */
+	rssi_exponent = rssi_dc_est & 0x001f;	/* 5 lsb */
+	rssi_decoded = (u32)rssi_mantissa << rssi_exponent;
+
+	/* Convert Rsst to 10log(Rssi) */
+	for (prssi = 20; prssi > 0; prssi--)
+		if (rssi_decoded >= (1 << prssi))
+			break;
+
+	rssi_log = (3 * rssi_decoded >> prssi) + (3 * prssi - 3);
+	/* clamp to positive */
+	if (rssi_log < 0)
+		rssi_log = 0;
+	/* Compensate for errors in truncation/approximation by adding 1 */
+	rssi_log++;
+
+	stfm1000->rssi_dc_est_log = rssi_log;
+	stfm1000->signal_strength = stfm1000->rssi_dc_est_log;
+
+	/* determine absolute value */
+	if (tmp & 0x0800)
+		mpx_dc = ((tmp >> 16) & 0x0fff) | 0xf000;
+	else
+		mpx_dc = (tmp >> 16) & 0x0fff;
+	stfm1000->mpx_dc = mpx_dc;
+	mpx_dc = mpx_dc < 0 ? -mpx_dc : mpx_dc;
+
+	if (stfm1000->tuning_grid_50KHz)
+		stfm1000->is_station = rssi_log > stfm1000->tune_rssi_th;
+	else
+		stfm1000->is_station = rssi_log > stfm1000->tune_rssi_th &&
+			mpx_dc > stfm1000->tune_mpx_dc_th;
+
+	/* weak signal? */
+	if (stfm1000->rssi_dc_est_log <
+		(stfm1000->filter_parms.pCoefLmrGaTh - 20)) {
+
+		if (stfm1000->pilot_present)
+			bypass_filter = 1;	/* Filter settings #2 */
+		else
+			bypass_filter = 0;
+
+		/* configure filter for narrow band */
+		ret = stfm1000_write_masked(stfm1000, STFM1000_AGC_CONTROL1,
+				STFM1000_B2_BYPASS_FILT(bypass_filter),
+				STFM1000_B2_BYPASS_FILT_MASK);
+		if (ret != 0)
+			return ret;
+
+		/* Turn off pilot tracking */
+		ret = stfm1000_clear_bits(stfm1000, STFM1000_PILOTTRACKING,
+				STFM1000_B2_PILOTTRACKING_EN);
+		if (ret != 0)
+			return ret;
+
+		/* enable "forced mono" in black box */
+		stfm1000->filter_parms.pCoefForcedMono = 1;
+
+		/* yeah, I know, it's stupid */
+		stfm1000->rds_state.demod.pCoefForcedMono =
+			stfm1000->filter_parms.pCoefForcedMono;
+
+		/* Set weak signal flag */
+		stfm1000->weak_signal = 1;
+
+		if (stfm1000->revid == STFM1000_CHIP_REV_TA2) {
+
+			/* read AGC_STAT register */
+			ret = stfm1000_read(stfm1000, STFM1000_AGC_STAT, &tmp);
+			if (ret != 0)
+				return ret;
+
+			lna_rms = (tmp & STFM1000_LNA_RMS_MASK) >>
+				STFM1000_LNA_RMS_SHIFT;
+
+			/* Check  the energy level from LNA Power Meter A/D */
+			if (lna_rms == 0)
+				bias = STFM1000_IBIAS2_DN | STFM1000_IBIAS1_UP;
+			else
+				bias = STFM1000_IBIAS2_UP | STFM1000_IBIAS1_DN;
+
+			if (lna_rms == 0 || lna_rms > 2) {
+				ret = stfm1000_write_masked(stfm1000,
+					STFM1000_LNA, bias,
+					STFM1000_IBIAS2_UP |
+					STFM1000_IBIAS2_DN |
+					STFM1000_IBIAS1_UP |
+					STFM1000_IBIAS1_DN);
+				if (ret != 0)
+					return ret;
+			}
+
+		} else {
+
+			/* Set LNA bias */
+
+			/* read AGC_STAT register */
+			ret = stfm1000_read(stfm1000, STFM1000_AGC_STAT, &tmp);
+			if (ret != 0)
+				return ret;
+
+			agc_out = (tmp & STFM1000_AGCOUT_STAT_MASK) >>
+					STFM1000_AGCOUT_STAT_SHIFT;
+
+			/* read LNA register (this is a cached register) */
+			ret = stfm1000_read(stfm1000, STFM1000_LNA, &lna);
+			if (ret != 0)
+				return ret;
+
+			/* read REF register (this is a cached register) */
+			ret = stfm1000_read(stfm1000, STFM1000_REF, &ref);
+			if (ret != 0)
+				return ret;
+
+/* work around the 80 line width problem */
+#undef LNADEF
+#define LNADEF STFM1000_LNA_AMP1_IMPROVE_DISTORTION
+			if (agc_out == 31) {
+				if (rssi_log <= 16) {
+					if (lna & STFM1000_IBIAS1_DN)
+						lna &= ~STFM1000_IBIAS1_DN;
+					else {
+						lna |=  STFM1000_IBIAS1_UP;
+						ref &= ~LNADEF;
+					}
+				}
+				if (rssi_log >= 26) {
+					if (lna & STFM1000_IBIAS1_UP) {
+						lna &= ~STFM1000_IBIAS1_UP;
+						ref |=  LNADEF;
+					} else
+						lna |=  STFM1000_IBIAS1_DN;
+				}
+			} else {
+				lna &= ~STFM1000_IBIAS1_UP;
+				lna |=  STFM1000_IBIAS1_DN;
+				ref |=  LNADEF;
+			}
+#undef LNADEF
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+				lna, STFM1000_IBIAS1_UP | STFM1000_IBIAS1_DN);
+			if (ret != 0)
+				return ret;
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_REF,
+				ref, STFM1000_LNA_AMP1_IMPROVE_DISTORTION);
+			if (ret != 0)
+				return ret;
+		}
+
+	} else if (stfm1000->rssi_dc_est_log >
+		(stfm1000->filter_parms.pCoefLmrGaTh - 17)) {
+
+			bias = STFM1000_IBIAS2_UP | STFM1000_IBIAS1_DN;
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+				bias, STFM1000_IBIAS2_UP | STFM1000_IBIAS2_DN |
+				STFM1000_IBIAS1_UP | STFM1000_IBIAS1_DN);
+			if (ret != 0)
+				return ret;
+
+			ret = SD_Set_Channel_Filter(stfm1000);
+			if (ret != 0)
+				return ret;
+
+			ret = SD_Look_For_Pilot(stfm1000);
+			if (ret != 0)
+				return ret;
+
+			if (stfm1000->pilot_present) {
+				if (stfm1000->prev_pilot_present ||
+					stfm1000->weak_signal) {
+
+					/* gear shift pilot tracking */
+					ret = SD_Gear_Shift_Pilot_Tracking(
+						stfm1000);
+					if (ret != 0)
+						return ret;
+
+					/* set force mono parameters for the
+					 * filter */
+					stfm1000->filter_parms.
+						pCoefForcedMono = stfm1000->
+							force_mono;
+
+					/* yeah, I know, it's stupid */
+					stfm1000->rds_state.demod.
+						pCoefForcedMono = stfm1000->
+							filter_parms.
+							pCoefForcedMono;
+				}
+			} else {
+				ret = stfm1000_clear_bits(stfm1000,
+					STFM1000_PILOTTRACKING,
+					STFM1000_B2_PILOTTRACKING_EN);
+				if (ret != 0)
+					return ret;
+
+				/* set force mono parameters for the filter */
+				stfm1000->filter_parms.pCoefForcedMono = 1;
+
+				/* yeah, I know, it's stupid */
+				stfm1000->rds_state.demod.pCoefForcedMono =
+					stfm1000->filter_parms.pCoefForcedMono;
+			}
+
+			/* Reset weak signal flag */
+			stfm1000->weak_signal = 0;
+			stfm1000->prev_pilot_present = stfm1000->pilot_present;
+
+	} else {
+
+		ret = SD_Look_For_Pilot(stfm1000);
+		if (ret != 0)
+			return ret;
+
+		if (!stfm1000->pilot_present) {
+			ret = stfm1000_clear_bits(stfm1000,
+				STFM1000_PILOTTRACKING,
+				STFM1000_B2_PILOTTRACKING_EN);
+			if (ret != 0)
+				return ret;
+
+			/* set force mono parameters for the filter */
+			stfm1000->filter_parms.pCoefForcedMono = 1;
+
+			/* yeah, I know, it's stupid */
+			stfm1000->rds_state.demod.pCoefForcedMono =
+				stfm1000->filter_parms.pCoefForcedMono;
+
+			/* Reset weak signal flag */
+			stfm1000->weak_signal = 0;
+			stfm1000->prev_pilot_present = stfm1000->pilot_present;
+		}
+
+	}
+
+	if (stfm1000->revid == STFM1000_CHIP_REV_TA2) {
+
+		/* read AGC_STAT register */
+		ret = stfm1000_read(stfm1000, STFM1000_AGC_STAT, &tmp);
+		if (ret != 0)
+			return ret;
+
+		agc_out = (tmp & STFM1000_AGCOUT_STAT_MASK) >>
+			STFM1000_AGCOUT_STAT_SHIFT;
+		lna_rms = (tmp & STFM1000_LNA_RMS_MASK) >>
+			STFM1000_LNA_RMS_SHIFT;
+
+		ret = stfm1000_read(stfm1000, STFM1000_AGC_CONTROL1, &tmp);
+		if (ret != 0)
+			return ret;
+
+		/* extract LNATH */
+		lna_th = (tmp & STFM1000_B2_LNATH_MASK) >>
+			STFM1000_B2_LNATH_SHIFT;
+
+		if (lna_rms > lna_th && agc_out <= 1) {
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+				STFM1000_USEATTEN(1), STFM1000_USEATTEN_MASK);
+			if (ret != 0)
+				return ret;
+
+		} else if (agc_out > 15) {
+
+			ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+				STFM1000_USEATTEN(0), STFM1000_USEATTEN_MASK);
+			if (ret != 0)
+				return ret;
+		}
+	}
+
+	/* disable buffered writes */
+	ret = stfm1000_clear_bits(stfm1000, STFM1000_DATAPATH,
+		STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		return ret;
+
+	return ret;
+}
+
+static int Is_Station(struct stfm1000 *stfm1000)
+{
+	u32 tmp, rssi_dc_est, tone_data;
+	u16 rssi_mantissa, rssi_exponent, rssi_decoded;
+	u16 prssi;
+	s16 mpx_dc;
+	int rssi_log;
+
+	/* Get Rssi register readings from STFM1000 */
+	stfm1000_read(stfm1000, STFM1000_RSSI_TONE, &tmp);
+	rssi_dc_est = tmp & 0xffff;
+	tone_data = (tmp >> 16) & 0x0fff;
+
+	rssi_mantissa = (rssi_dc_est & 0xffe0) >> 5;	/* 11Msb */
+	rssi_exponent = rssi_dc_est & 0x001f;	/* 5 lsb */
+	rssi_decoded = (u32)rssi_mantissa << rssi_exponent;
+
+	/* Convert Rsst to 10log(Rssi) */
+	for (prssi = 20; prssi > 0; prssi--)
+		if (rssi_decoded >= (1 << prssi))
+			break;
+
+	rssi_log = (3 * rssi_decoded >> prssi) + (3 * prssi - 3);
+	/* clamp to positive */
+	if (rssi_log < 0)
+		rssi_log = 0;
+	/* Compensate for errors in truncation/approximation by adding 1 */
+	rssi_log++;
+
+	stfm1000->rssi_dc_est_log = rssi_log;
+	stfm1000->signal_strength = stfm1000->rssi_dc_est_log;
+
+	/* determine absolute value */
+	if (tmp & 0x0800)
+		mpx_dc = ((tmp >> 16) & 0x0fff) | 0xf000;
+	else
+		mpx_dc = (tmp >> 16) & 0x0fff;
+	stfm1000->mpx_dc = mpx_dc;
+	mpx_dc = mpx_dc < 0 ? -mpx_dc : mpx_dc;
+
+	if (stfm1000->tuning_grid_50KHz)
+		stfm1000->is_station = rssi_log > stfm1000->tune_rssi_th;
+	else
+		stfm1000->is_station = rssi_log > stfm1000->tune_rssi_th &&
+			mpx_dc > stfm1000->tune_mpx_dc_th;
+
+	return 0;
+}
+
+int Monitor_STFM_AGC(struct stfm1000 *stfm1000)
+{
+	/* we don't do any AGC for now */
+	return 0;
+}
+
+static int Take_Down(struct stfm1000 *stfm1000)
+{
+	Mute_Audio(stfm1000);
+
+	DRI_Off(stfm1000);
+
+	SD_DP_Off(stfm1000);
+
+	return 0;
+}
+
+static int Bring_Up(struct stfm1000 *stfm1000)
+{
+	SD_DP_On(stfm1000);
+
+	SD_Optimize_Channel(stfm1000);
+
+	DRI_On(stfm1000);
+
+	Unmute_Audio(stfm1000);
+
+	if (stfm1000->rds_enable)
+		stfm1000_rds_reset(&stfm1000->rds_state);
+
+	stfm1000->rds_sync = stfm1000->rds_enable; /* force sync (if RDS) */
+	stfm1000->rds_demod_running = 0;
+	stfm1000->rssi_dc_est_log = 0;
+	stfm1000->signal_strength = 0;
+
+	stfm1000->next_quality_monitor = jiffies + msecs_to_jiffies(
+		stfm1000->quality_monitor_period);
+	stfm1000->next_agc_monitor = jiffies + msecs_to_jiffies(
+		stfm1000->agc_monitor_period);
+	stfm1000->rds_pkt_bad = 0;
+	stfm1000->rds_pkt_good = 0;
+	stfm1000->rds_pkt_recovered = 0;
+	stfm1000->rds_pkt_lost_sync = 0;
+	stfm1000->rds_bit_overruns = 0;
+
+	return 0;
+}
+
+/* These are not used yet */
+
+static int Lock_Station(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = SD_Optimize_Channel(stfm1000);
+	if (ret != 0)
+		return ret;
+
+	/* AGC monitor start? */
+
+	return ret;
+}
+
+static const struct stfm1000_reg sd_unlock_regs[] = {
+	STFM1000_REG_SETBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_REG_CLRBITS(PILOTTRACKING, STFM1000_B2_PILOTTRACKING_EN),
+	STFM1000_REG_CLRBITS(DATAPATH, STFM1000_DB_ACCEPT),
+	STFM1000_END,
+};
+
+static int Unlock_Station(struct stfm1000 *stfm1000)
+{
+	int ret;
+
+	ret = stfm1000_write_regs(stfm1000, sd_unlock_regs);
+	return ret;
+}
+
+irqreturn_t stfm1000_dri_dma_irq(int irq, void *dev_id)
+{
+	struct stfm1000 *stfm1000 = dev_id;
+	u32 err_mask, irq_mask;
+	u32 ctrl;
+	int handled = 0;
+
+#ifdef CONFIG_ARCH_STMP37XX
+	err_mask = 1 << (16 + stfm1000->dma_ch);
+#endif
+#ifdef CONFIG_ARCH_STMP378X
+	err_mask = 1 << stfm1000->dma_ch;
+#endif
+	irq_mask = 1 << stfm1000->dma_ch;
+
+#ifdef CONFIG_ARCH_STMP37XX
+	ctrl = HW_APBX_CTRL1_RD();
+#endif
+#ifdef CONFIG_ARCH_STMP378X
+	ctrl = HW_APBX_CTRL2_RD();
+#endif
+
+	if (ctrl & err_mask) {
+		handled = 1;
+		printk(KERN_WARNING "%s: DMA audio channel %d error\n",
+			__func__, stfm1000->dma_ch);
+#ifdef CONFIG_ARCH_STMP37XX
+		HW_APBX_CTRL1_CLR(err_mask);
+#endif
+#ifdef CONFIG_ARCH_STMP378X
+		HW_APBX_CTRL2_CLR(err_mask);
+#endif
+	}
+
+	if (HW_APBX_CTRL1_RD() & irq_mask) {
+		handled = 1;
+		stmp3xxx_dma_clear_interrupt(stfm1000->dma_ch);
+
+		if (stfm1000->alsa_initialized) {
+			BUG_ON(stfm1000_alsa_ops->dma_irq == NULL);
+			(*stfm1000_alsa_ops->dma_irq)(stfm1000);
+		}
+	}
+
+	return handled ? IRQ_HANDLED : IRQ_NONE;
+}
+EXPORT_SYMBOL(stfm1000_dri_dma_irq);
+
+irqreturn_t stfm1000_dri_attn_irq(int irq, void *dev_id)
+{
+	struct stfm1000 *stfm1000 = dev_id;
+	int handled = 1;
+	u32 mask;
+
+	(void)stfm1000;
+	mask = HW_DRI_CTRL_RD();
+	mask &= BM_DRI_CTRL_OVERFLOW_IRQ | BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ |
+		BM_DRI_CTRL_ATTENTION_IRQ;
+
+	HW_DRI_CTRL_CLR(mask);
+
+	printk(KERN_INFO "DRI_ATTN:%s%s%s\n",
+			(mask & BM_DRI_CTRL_OVERFLOW_IRQ) ? " OV" : "",
+			(mask & BM_DRI_CTRL_PILOT_SYNC_LOSS_IRQ) ? " SL" : "",
+			(mask & BM_DRI_CTRL_ATTENTION_IRQ) ? " AT" : "");
+
+	if (stfm1000->alsa_initialized) {
+		BUG_ON(stfm1000_alsa_ops->attn_irq == NULL);
+		(*stfm1000_alsa_ops->attn_irq)(stfm1000);
+	}
+
+	return handled ? IRQ_HANDLED : IRQ_NONE;
+}
+EXPORT_SYMBOL(stfm1000_dri_attn_irq);
+
+void stfm1000_decode_block(struct stfm1000 *stfm1000, const s16 *src, s16 *dst,
+	int count)
+{
+	int i;
+
+	if (stfm1000->mute) {
+		memset(dst, 0, count * sizeof(s16) * 2);
+		return;
+
+	}
+
+	for (i = 0; i < count; i++, dst += 2, src += 4) {
+
+		stfm1000_filter_decode(&stfm1000->filter_parms,
+			src[0],	src[1], src[2]);
+
+		dst[0] = stfm1000_filter_value_left(&stfm1000->filter_parms);
+		dst[1] = stfm1000_filter_value_right(&stfm1000->filter_parms);
+	}
+
+	stfm1000->rssi = stfm1000->filter_parms.RssiDecoded;
+	stfm1000->stereo = stfm1000->pilot_present &&
+		!stfm1000->filter_parms.pCoefForcedMono;
+
+	/* RDS processing */
+	if (stfm1000->rds_demod_running) {
+		/* rewind */
+		src -= count * 4;
+		stfm1000_rds_demod(&stfm1000->rds_state, src, count);
+	}
+
+}
+EXPORT_SYMBOL(stfm1000_decode_block);
+
+void stfm1000_take_down(struct stfm1000 *stfm1000)
+{
+	mutex_lock(&stfm1000->state_lock);
+	stfm1000->active = 0;
+	Take_Down(stfm1000);
+	mutex_unlock(&stfm1000->state_lock);
+}
+EXPORT_SYMBOL(stfm1000_take_down);
+
+void stfm1000_bring_up(struct stfm1000 *stfm1000)
+{
+	mutex_lock(&stfm1000->state_lock);
+
+	stfm1000->active = 1;
+
+	stfm1000_filter_reset(&stfm1000->filter_parms);
+
+	Bring_Up(stfm1000);
+
+	mutex_unlock(&stfm1000->state_lock);
+}
+EXPORT_SYMBOL(stfm1000_bring_up);
+
+void stfm1000_tune_current(struct stfm1000 *stfm1000)
+{
+	mutex_lock(&stfm1000->state_lock);
+	sw_tune(stfm1000, stfm1000->freq);
+	mutex_unlock(&stfm1000->state_lock);
+}
+EXPORT_SYMBOL(stfm1000_tune_current);
+
+/* Alternate ZIF Tunings to avoid EMI */
+const struct stfm1000_tune1
+stfm1000_board_emi_tuneups[STFM1000_FREQUENCY_100KHZ_RANGE] = {
+#undef TUNE_ENTRY
+#define TUNE_ENTRY(f, t1, sd) \
+	[(f) - STFM1000_FREQUENCY_100KHZ_MIN] = \
+		{ .tune1 = (t1), .sdnom = (sd) }
+	TUNE_ENTRY(765, 0x84030, 0x1BF5E50D),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(780, 0x84240, 0x1BA5162F),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(795, 0x84250, 0x1C2D2F39),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(810, 0x84460, 0x1BDD207E),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(825, 0x84470, 0x1C6138CD),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(839, 0xC4680, 0x1C11F704),	/* 061215 Jon, IF +100kHz */
+	TUNE_ENTRY(840, 0x84680, 0x1c11f704),
+	TUNE_ENTRY(855, 0x84890, 0x1BC71C71),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(870, 0x848A0, 0x1C43DE10),	/* 061215 Jon, IF +0kHz */
+	TUNE_ENTRY(885, 0x84AB0, 0x1BF9B021),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(899, 0xC4CC0, 0x1BB369A9),	/* 061025 Arthur, IF +100kHz */
+	TUNE_ENTRY(900, 0x84CC0, 0x1BB369A9),	/* 061025 Arthur, IF 0kHz */
+	TUNE_ENTRY(915, 0x84CD0, 0x1C299A5B),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(930, 0x84ee0, 0x1be3e6aa),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(945, 0x84ef0, 0x1c570f8b),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(959, 0xC5100, 0x1c11f704),
+	TUNE_ENTRY(960, 0x85100, 0x1c11f704),
+	TUNE_ENTRY(975, 0x85310, 0x1bd03d57),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(990, 0x85320, 0x1c3dc822),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1005, 0x85530, 0x1bfc93ff),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1019, 0xC5740, 0x1BBE683C),	/* 061025 Arthur, IF +100kHz */
+	TUNE_ENTRY(1020, 0x85740, 0x1bbe683c),	/* 061025 Arthur, IF +0kHz */
+	TUNE_ENTRY(1035, 0x85750, 0x1c26dab6),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1050, 0x85960, 0x1be922b4),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1065, 0x85970, 0x1c4f357c),	/* 061101 Arthur, IF +0kHz */
+	TUNE_ENTRY(1079, 0xC5B80, 0x1c11f704),
+	TUNE_ENTRY(1080, 0x85B80, 0x1c11f704),
+#undef TUNE_ENTRY
+};
+
+static const struct stfm1000_tune1 *stfm1000_board_emi_tune(int freq100)
+{
+	const struct stfm1000_tune1 *tune1;
+
+	if ((unsigned int)(freq100 - STFM1000_FREQUENCY_100KHZ_MIN) >=
+			STFM1000_FREQUENCY_100KHZ_RANGE)
+		return NULL;
+
+	tune1 = &stfm1000_board_emi_tuneups[freq100 -
+		STFM1000_FREQUENCY_100KHZ_MIN];
+	if (tune1->tune1 == 0 && tune1->sdnom == 0)
+		return NULL;
+	return tune1;
+}
+
+/* freq in kHz */
+static int sw_tune(struct stfm1000 *stfm1000, u32 freq)
+{
+	u32 freq100 = freq / 100;
+	int tune_cap;
+	int i2s_clock;
+	int mix_reg;
+	int if_freq, fe_freq;
+	u32 tune1, sdnom, agc1;
+	const struct stfm1000_tune1 *tp;
+	int ret;
+
+	if_freq = 0;
+	mix_reg = 1;
+	switch (mix_reg) {
+	case 0: if_freq = -2; break;
+	case 1: if_freq = -1; break;
+	case 2: if_freq =  0; break;
+	case 3: if_freq =  1; break;
+	case 4: if_freq =  2; break;
+	}
+
+	/* handle board specific EMI tuning */
+	tp = stfm1000_board_emi_tune(freq100);
+	if (tp != NULL) {
+		tune1 = tp->tune1;
+		sdnom = tp->sdnom;
+	} else {
+		fe_freq = freq100 + if_freq;
+
+		/* clamp into range */
+		if (fe_freq < STFM1000_FREQUENCY_100KHZ_MIN)
+			fe_freq = STFM1000_FREQUENCY_100KHZ_MIN;
+		else if (fe_freq > STFM1000_FREQUENCY_100KHZ_MAX)
+			fe_freq = STFM1000_FREQUENCY_100KHZ_MAX;
+
+		tp = &stfm1000_tune1_table[fe_freq -
+			STFM1000_FREQUENCY_100KHZ_MIN];
+
+		/* bits [14:0], [20:18] */
+		tune1 = (tp->tune1 & 0x7fff) | (mix_reg << 18);
+		sdnom = tp->sdnom;
+	}
+
+	agc1 = stfm1000->revid == STFM1000_CHIP_REV_TA2 ? 0x0400 : 0x2200;
+
+	ret = stfm1000_write_masked(stfm1000, STFM1000_AGC_CONTROL1,
+			agc1, 0x3f00);
+	if (ret != 0)
+		goto err;
+
+	ret = stfm1000_write_masked(stfm1000, STFM1000_TUNE1, tune1,
+		0xFFFF7FFF);	/* do not set bit-15 */
+	if (ret != 0)
+		goto err;
+
+	/* keep this around */
+	stfm1000->sdnominal_pivot = sdnom;
+
+	ret = stfm1000_write(stfm1000, STFM1000_SDNOMINAL, sdnom);
+	if (ret != 0)
+		goto err;
+
+	/* fix for seek-not-stopping on alternate tunings */
+	ret = stfm1000_set_bits(stfm1000, STFM1000_DATAPATH,
+			STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		goto err;
+
+	ret = stfm1000_clear_bits(stfm1000, STFM1000_DATAPATH,
+			STFM1000_DB_ACCEPT);
+	if (ret != 0)
+		goto err;
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_DRI_CLK_EN);
+	if (ret != 0)
+		goto err;
+
+	/* 6MHz spur fix */
+	if ((freq100 >=  778 && freq100 <=  782) ||
+	    (freq100 >=  838 && freq100 <=  842) ||
+	    (freq100 >=  898 && freq100 <=  902) ||
+	    (freq100 >=  958 && freq100 <=  962) ||
+	    (freq100 >= 1018 && freq100 <= 1022) ||
+	    (freq100 >= 1078 && freq100 <= 1080))
+		i2s_clock = 5;  /* 4.8MHz */
+	else
+		i2s_clock = 4;
+
+	ret = stfm1000_write_masked(stfm1000, STFM1000_DATAPATH,
+		STFM1000_SAI_CLK_DIV(i2s_clock), STFM1000_SAI_CLK_DIV_MASK);
+	if (ret != 0)
+		goto err;
+
+	ret = stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+		STFM1000_DRI_CLK_EN);
+	if (ret != 0)
+		goto err;
+
+	if (tune1 & 0xf)
+		ret = stfm1000_set_bits(stfm1000, STFM1000_CLK1,
+			STFM1000_ENABLE_TAPDELAYFIX);
+	else
+		ret = stfm1000_clear_bits(stfm1000, STFM1000_CLK1,
+			STFM1000_ENABLE_TAPDELAYFIX);
+
+	if (ret != 0)
+		goto err;
+
+	tune_cap = (int)(stfm1000->tune_cap_a_f -
+		stfm1000->tune_cap_b_f * freq100);
+	if (tune_cap < 4)
+		tune_cap = 4;
+	ret = stfm1000_write_masked(stfm1000, STFM1000_LNA,
+		STFM1000_ANTENNA_TUNECAP(tune_cap),
+		STFM1000_ANTENNA_TUNECAP_MASK);
+	if (ret != 0)
+		goto err;
+
+	/* set signal strenth to 0 */
+	/* stfm1000_dcdc_update(); */
+
+	/* cmp_rds_setRdsStatus(0) */
+	/* cmp_rds_ResetGroupCallbacks(); */
+	stfm1000->freq = freq;
+
+	return 0;
+err:
+	return -1;
+}
+
+static const struct v4l2_queryctrl radio_qctrl[] = {
+	{
+		.id		= V4L2_CID_AUDIO_MUTE,
+		.name		= "Mute",
+		.minimum	= 0,
+		.maximum	= 1,
+		.default_value	= 1,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+	},
+};
+
+static int vidioc_querycap(struct file *file, void *priv,
+				struct v4l2_capability *v)
+{
+	strlcpy(v->driver, "radio-stfm1000", sizeof(v->driver));
+	strlcpy(v->card, "STFM1000 Radio", sizeof(v->card));
+	sprintf(v->bus_info, "i2c");
+	v->version = KERNEL_VERSION(0, 0, 1);
+	v->capabilities = V4L2_CAP_TUNER;
+	return 0;
+}
+
+static int vidioc_g_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *v)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+	u32 tmp, rssi_dc_est, tone_data;
+	u16 rssi_mantissa, rssi_exponent, rssi_decoded;
+	u16 prssi;
+	s16 mpx_dc;
+	int rssi_log;
+	int ret;
+
+	if (v->index > 0)
+		return -EINVAL;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	strcpy(v->name, "FM");
+	v->type = V4L2_TUNER_RADIO;
+	v->rangelow = (u32)(87.5 * 16000);
+	v->rangehigh = (u32)(108 * 16000);
+	v->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO;
+	v->capability = V4L2_TUNER_CAP_LOW;
+	v->audmode = V4L2_TUNER_MODE_STEREO;
+	v->signal = 0; /* tr_getsigstr(); */
+
+	msleep(50);
+
+	ret = stfm1000_read(stfm1000, STFM1000_RSSI_TONE, &tmp);
+	if (ret != 0)
+		goto out;
+
+	rssi_dc_est = tmp & 0xffff;
+	tone_data = (tmp >> 16) & 0x0fff;
+
+	rssi_mantissa = (rssi_dc_est & 0xffe0) >> 5;	/* 11Msb */
+	rssi_exponent = rssi_dc_est & 0x001f;	/* 5 lsb */
+	rssi_decoded = (u32)rssi_mantissa << rssi_exponent;
+
+	/* Convert Rsst to 10log(Rssi) */
+	for (prssi = 20; prssi > 0; prssi--)
+		if (rssi_decoded >= (1 << prssi))
+			break;
+
+	rssi_log = (3 * rssi_decoded >> prssi) + (3 * prssi - 3);
+	/* clamp to positive */
+	if (rssi_log < 0)
+		rssi_log = 0;
+	/* Compensate for errors in truncation/approximation by adding 1 */
+	rssi_log++;
+
+	stfm1000->rssi_dc_est_log = rssi_log;
+	stfm1000->signal_strength = stfm1000->rssi_dc_est_log;
+
+	/* determine absolute value */
+	if (tmp & 0x0800)
+		mpx_dc = ((tmp >> 16) & 0x0fff) | 0xf000;
+	else
+		mpx_dc = (tmp >> 16) & 0x0fff;
+	stfm1000->mpx_dc = mpx_dc;
+	mpx_dc = mpx_dc < 0 ? -mpx_dc : mpx_dc;
+
+	v->signal = rssi_decoded & 0xffff;
+
+out:
+	mutex_unlock(&stfm1000->state_lock);
+
+	return ret;
+}
+
+static int vidioc_s_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *v)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	(void)stfm1000;
+
+	if (v->index > 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int vidioc_s_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	mutex_lock(&stfm1000->state_lock);
+
+	/* convert from the crazy linux value to our decimal based values */
+	stfm1000->freq = (u32)div_u64((u64)(125 * (u64)f->frequency), 2000);
+
+	if (stfm1000->active)
+		Take_Down(stfm1000);
+
+	sw_tune(stfm1000, stfm1000->freq);
+
+	if (stfm1000->active)
+		Bring_Up(stfm1000);
+
+	mutex_unlock(&stfm1000->state_lock);
+
+	return 0;
+}
+
+static int vidioc_g_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	f->type = V4L2_TUNER_RADIO;
+	f->frequency = stfm1000->freq * 16;
+
+	return 0;
+}
+
+static int vidioc_queryctrl(struct file *file, void *priv,
+				struct v4l2_queryctrl *qc)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+	int i;
+
+	(void)stfm1000;
+
+	for (i = 0; i < ARRAY_SIZE(radio_qctrl); i++) {
+		if (qc->id && qc->id == radio_qctrl[i].id) {
+			memcpy(qc, &radio_qctrl[i], sizeof(*qc));
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static int vidioc_g_ctrl(struct file *file, void *priv,
+				struct v4l2_control *ctrl)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	switch (ctrl->id) {
+
+	case V4L2_CID_AUDIO_MUTE:
+		ctrl->value = stfm1000->mute;
+		return 0;
+
+	}
+	return -EINVAL;
+}
+
+static int vidioc_s_ctrl(struct file *file, void *priv,
+				struct v4l2_control *ctrl)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+	int ret;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	ret = -EINVAL;
+
+	switch (ctrl->id) {
+
+	case V4L2_CID_AUDIO_MUTE:
+		stfm1000->mute = ctrl->value;
+		ret = 0;
+		break;
+	}
+
+	mutex_unlock(&stfm1000->state_lock);
+
+	return ret;
+}
+
+static int vidioc_g_audio(struct file *file, void *priv,
+				struct v4l2_audio *a)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	(void)stfm1000;
+
+	if (a->index > 1)
+		return -EINVAL;
+
+	strcpy(a->name, "Radio");
+	a->capability = V4L2_AUDCAP_STEREO;
+	return 0;
+}
+
+static int vidioc_s_audio(struct file *file, void *priv,
+					struct v4l2_audio *a)
+{
+	if (a->index > 1)
+		return -EINVAL;
+	return 0;
+}
+
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	(void)stfm1000;
+
+	*i = 0;
+
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	(void)stfm1000;
+
+	if (i != 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+const struct v4l2_ioctl_ops stfm_ioctl_ops = {
+	.vidioc_querycap	= vidioc_querycap,
+	.vidioc_g_tuner		= vidioc_g_tuner,
+	.vidioc_s_tuner		= vidioc_s_tuner,
+	.vidioc_g_frequency	= vidioc_g_frequency,
+	.vidioc_s_frequency	= vidioc_s_frequency,
+	.vidioc_queryctrl	= vidioc_queryctrl,
+	.vidioc_g_ctrl		= vidioc_g_ctrl,
+	.vidioc_s_ctrl		= vidioc_s_ctrl,
+	.vidioc_g_audio		= vidioc_g_audio,
+	.vidioc_s_audio		= vidioc_s_audio,
+	.vidioc_g_input		= vidioc_g_input,
+	.vidioc_s_input		= vidioc_s_input,
+};
+
+static int stfm1000_open(struct inode *inode, struct file *file)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	mutex_lock(&stfm1000->state_lock);
+	stfm1000->users = 1;
+	mutex_unlock(&stfm1000->state_lock);
+
+	return 0;
+}
+static int stfm1000_close(struct inode *inode, struct file *file)
+{
+	struct stfm1000 *stfm1000 = stfm1000_from_file(file);
+
+	if (!stfm1000)
+		return -ENODEV;
+
+	stfm1000->users = 0;
+	if (stfm1000->removed)
+		kfree(stfm1000);
+	return 0;
+}
+
+static const struct file_operations stfm1000_fops = {
+	.owner		= THIS_MODULE,
+	.open		= stfm1000_open,
+	.release	= stfm1000_close,
+	.ioctl		= video_ioctl2,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl	= v4l_compat_ioctl32,
+#endif
+	.llseek		= no_llseek,
+};
+
+/* sysfs */
+
+#define STFM1000_RO_ATTR(var) \
+static ssize_t stfm1000_show_ ## var(struct device *d, \
+	struct device_attribute *attr, char *buf) \
+{ \
+	struct i2c_client *client = to_i2c_client(d); \
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client); \
+	return sprintf(buf, "%d\n", stfm1000->var); \
+} \
+static DEVICE_ATTR(var, 0444, stfm1000_show_ ##var, NULL)
+
+#define STFM1000_RW_ATTR(var) \
+static ssize_t stfm1000_show_ ## var(struct device *d, \
+	struct device_attribute *attr, char *buf) \
+{ \
+	struct i2c_client *client = to_i2c_client(d); \
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client); \
+	return sprintf(buf, "%u\n", stfm1000->var); \
+} \
+static ssize_t stfm1000_store_ ## var(struct device *d, \
+	struct device_attribute *attr, const char *buf, size_t size) \
+{ \
+	struct i2c_client *client = to_i2c_client(d); \
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client); \
+	unsigned long v; \
+	\
+	strict_strtoul(buf, 0, &v); \
+	stfm1000_commit_ ## var(stfm1000, v); \
+	return size; \
+} \
+static DEVICE_ATTR(var, 0644, stfm1000_show_ ##var, stfm1000_store_ ##var)
+
+#define STFM1000_RW_ATTR_SIMPLE(var) \
+static void stfm1000_commit_ ## var(struct stfm1000 *stfm1000, \
+	unsigned long value) \
+{ \
+	stfm1000->var = value; \
+} \
+STFM1000_RW_ATTR(var)
+
+STFM1000_RO_ATTR(weak_signal);
+STFM1000_RO_ATTR(pilot_present);
+STFM1000_RO_ATTR(stereo);
+STFM1000_RO_ATTR(rssi);
+STFM1000_RO_ATTR(mpx_dc);
+STFM1000_RO_ATTR(signal_strength);
+STFM1000_RW_ATTR_SIMPLE(rds_signal_th);
+STFM1000_RO_ATTR(rds_present);
+STFM1000_RO_ATTR(is_station);
+
+static void stfm1000_commit_georegion(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	/* don't do anything for illegal region */
+	if (value != 0 && value != 1)
+		return;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	stfm1000->georegion = value;
+	if (stfm1000->georegion == 0)
+		stfm1000_clear_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_DEEMPH_50_75B);
+	else
+		stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_DEEMPH_50_75B);
+
+	mutex_unlock(&stfm1000->state_lock);
+}
+STFM1000_RW_ATTR(georegion);
+
+static void stfm1000_commit_freq(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	mutex_lock(&stfm1000->state_lock);
+
+	/* clamp */
+	if (value < STFM1000_FREQUENCY_100KHZ_MIN * 100)
+		value = STFM1000_FREQUENCY_100KHZ_MIN * 100;
+	else if (value > STFM1000_FREQUENCY_100KHZ_MAX * 100)
+		value = STFM1000_FREQUENCY_100KHZ_MAX * 100;
+
+	stfm1000->freq = value;
+
+	if (stfm1000->active)
+		Take_Down(stfm1000);
+
+	sw_tune(stfm1000, stfm1000->freq);
+
+	if (stfm1000->active)
+		Bring_Up(stfm1000);
+
+	mutex_unlock(&stfm1000->state_lock);
+}
+STFM1000_RW_ATTR(freq);
+
+static void stfm1000_commit_mute(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->mute = !!value;
+}
+STFM1000_RW_ATTR(mute);
+
+static void stfm1000_commit_force_mono(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->force_mono = !!value;
+	/* set force mono parameters for the filter */
+	stfm1000->filter_parms.pCoefForcedMono = stfm1000->force_mono;
+
+	/* yeah, I know, it's stupid */
+	stfm1000->rds_state.demod.pCoefForcedMono =
+		stfm1000->filter_parms.pCoefForcedMono;
+}
+STFM1000_RW_ATTR(force_mono);
+
+STFM1000_RW_ATTR_SIMPLE(monitor_period);
+STFM1000_RW_ATTR_SIMPLE(quality_monitor);
+STFM1000_RW_ATTR_SIMPLE(quality_monitor_period);
+STFM1000_RW_ATTR_SIMPLE(agc_monitor_period);
+STFM1000_RW_ATTR_SIMPLE(tune_rssi_th);
+STFM1000_RW_ATTR_SIMPLE(tune_mpx_dc_th);
+
+static void stfm1000_commit_rds_enable(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	/* don't do anything for illegal values (or for not TB2) */
+	if ((value != 0 && value != 1) ||
+		stfm1000->revid == STFM1000_CHIP_REV_TA2)
+		return;
+
+	mutex_lock(&stfm1000->state_lock);
+
+	stfm1000->rds_enable = value;
+	if (stfm1000->rds_enable == 0)
+		stfm1000_clear_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_RDS_ENABLE);
+	else
+		stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+			STFM1000_RDS_ENABLE);
+
+	mutex_unlock(&stfm1000->state_lock);
+}
+STFM1000_RW_ATTR(rds_enable);
+
+static void stfm1000_commit_rds_sync(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->rds_sync = stfm1000->rds_enable && !!value;
+}
+STFM1000_RW_ATTR(rds_sync);
+
+STFM1000_RW_ATTR_SIMPLE(rds_pkt_good);
+STFM1000_RW_ATTR_SIMPLE(rds_pkt_bad);
+STFM1000_RW_ATTR_SIMPLE(rds_pkt_recovered);
+STFM1000_RW_ATTR_SIMPLE(rds_pkt_lost_sync);
+STFM1000_RW_ATTR_SIMPLE(rds_bit_overruns);
+STFM1000_RW_ATTR_SIMPLE(rds_info);
+
+static void stfm1000_commit_rds_sdnominal_adapt(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->rds_sdnominal_adapt = !!value;
+	stfm1000->rds_state.demod.sdnom_adapt = stfm1000->rds_sdnominal_adapt;
+}
+STFM1000_RW_ATTR(rds_sdnominal_adapt);
+
+static void stfm1000_commit_rds_phase_pop(struct stfm1000 *stfm1000,
+	unsigned long value)
+{
+	stfm1000->rds_phase_pop = !!value;
+	stfm1000->rds_state.demod.PhasePoppingEnabled =
+		stfm1000->rds_phase_pop;
+}
+STFM1000_RW_ATTR(rds_phase_pop);
+
+STFM1000_RW_ATTR_SIMPLE(tuning_grid_50KHz);
+
+static ssize_t stfm1000_show_rds_ps(struct device *d,
+	struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(d);
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client);
+	char ps[9];
+
+	if (stfm1000_rds_get_ps(&stfm1000->rds_state, ps, sizeof(ps)) <= 0)
+		ps[0] = '\0';
+
+	return sprintf(buf, "%s\n", ps);
+}
+static DEVICE_ATTR(rds_ps, 0444, stfm1000_show_rds_ps, NULL);
+
+static ssize_t stfm1000_show_rds_text(struct device *d,
+	struct device_attribute *attr, char *buf)
+{
+	struct i2c_client *client = to_i2c_client(d);
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client);
+	char text[65];
+
+	if (stfm1000_rds_get_text(&stfm1000->rds_state, text,
+		sizeof(text)) <= 0)
+		text[0] = '\0';
+
+	return sprintf(buf, "%s\n", text);
+}
+static DEVICE_ATTR(rds_text, 0444, stfm1000_show_rds_text, NULL);
+
+static struct device_attribute *stfm1000_attrs[] = {
+	&dev_attr_agc_monitor_period,
+	&dev_attr_force_mono,
+	&dev_attr_freq,
+	&dev_attr_georegion,
+	&dev_attr_is_station,
+	&dev_attr_monitor_period,
+	&dev_attr_mpx_dc,
+	&dev_attr_mute,
+	&dev_attr_pilot_present,
+	&dev_attr_quality_monitor,
+	&dev_attr_quality_monitor_period,
+	&dev_attr_rds_bit_overruns,
+	&dev_attr_rds_enable,
+	&dev_attr_rds_info,
+	&dev_attr_rds_phase_pop,
+	&dev_attr_rds_pkt_bad,
+	&dev_attr_rds_pkt_good,
+	&dev_attr_rds_pkt_lost_sync,
+	&dev_attr_rds_pkt_recovered,
+	&dev_attr_rds_present,
+	&dev_attr_rds_ps,
+	&dev_attr_rds_sdnominal_adapt,
+	&dev_attr_rds_signal_th,
+	&dev_attr_rds_sync,
+	&dev_attr_rds_text,
+	&dev_attr_rssi,
+	&dev_attr_signal_strength,
+	&dev_attr_stereo,
+	&dev_attr_tune_mpx_dc_th,
+	&dev_attr_tune_rssi_th,
+	&dev_attr_tuning_grid_50KHz,
+	&dev_attr_weak_signal,
+	NULL,
+};
+
+/* monitor thread */
+
+static void rds_process(struct stfm1000 *stfm1000)
+{
+	int count, bit;
+	int mix_reg, sdnominal_reg;
+	u32 sdnom, sdnom_new, limit;
+	u8 buf[8];
+
+	if (!stfm1000->rds_enable)
+		return;
+
+	if (stfm1000->rds_sync &&
+		stfm1000->rssi_dc_est_log > stfm1000->rds_signal_th) {
+		if (stfm1000->rds_info)
+			printk(KERN_INFO "RDS: sync\n");
+		stfm1000_rds_reset(&stfm1000->rds_state);
+		stfm1000->rds_demod_running = 1;
+		stfm1000->rds_sync = 0;
+	}
+
+	if (!stfm1000->rds_demod_running)
+		return;
+
+	/* process mix reg requests */
+	spin_lock_irq(&stfm1000->rds_lock);
+	mix_reg = stfm1000_rds_mix_msg_get(&stfm1000->rds_state);
+	spin_unlock_irq(&stfm1000->rds_lock);
+
+	if (mix_reg != -1) {
+
+		if (stfm1000->rds_info)
+			printk(KERN_INFO "RDS: new RDS_MIXOFFSET %d\n",
+				mix_reg & 1);
+
+		/* update register */
+		if (mix_reg & 1)
+			stfm1000_set_bits(stfm1000, STFM1000_INITIALIZATION2,
+				STFM1000_RDS_MIXOFFSET);
+		else
+			stfm1000_clear_bits(stfm1000, STFM1000_INITIALIZATION2,
+				STFM1000_RDS_MIXOFFSET);
+
+		/* signal it's processed */
+		spin_lock_irq(&stfm1000->rds_lock);
+		stfm1000_rds_mix_msg_processed(&stfm1000->rds_state, mix_reg);
+		spin_unlock_irq(&stfm1000->rds_lock);
+	}
+
+	/* process sdnominal reg requests */
+	spin_lock_irq(&stfm1000->rds_lock);
+	sdnominal_reg = stfm1000_rds_sdnominal_msg_get(&stfm1000->rds_state);
+	spin_unlock_irq(&stfm1000->rds_lock);
+
+	/* any change? */
+	if (sdnominal_reg != 0) {
+
+		stfm1000_read(stfm1000, STFM1000_SDNOMINAL, &sdnom);
+
+		sdnom_new = sdnom + sdnominal_reg;
+
+		/* Limit SDNOMINAL to within 244 ppm of its ideal value */
+		limit = stfm1000->sdnominal_pivot +
+			(stfm1000->sdnominal_pivot >> 12);
+		if (sdnom_new > limit)
+			sdnom_new = limit;
+
+		limit = stfm1000->sdnominal_pivot -
+			(stfm1000->sdnominal_pivot >> 12);
+		if (sdnom_new < limit)
+			sdnom_new = limit;
+
+		/* write the register */
+		stfm1000_write(stfm1000, STFM1000_SDNOMINAL, sdnom_new);
+
+		/* signal it's processed */
+		spin_lock_irq(&stfm1000->rds_lock);
+		stfm1000_rds_sdnominal_msg_processed(&stfm1000->rds_state,
+			sdnominal_reg);
+		spin_unlock_irq(&stfm1000->rds_lock);
+	}
+
+	/* pump bits out & pass them to the process function */
+	spin_lock_irq(&stfm1000->rds_lock);
+	while (stfm1000_rds_bits_available(&stfm1000->rds_state) > 128) {
+		count = 0;
+		while (count++ < 128 &&
+			(bit = stmf1000_rds_get_bit(
+				&stfm1000->rds_state)) >= 0) {
+			spin_unlock_irq(&stfm1000->rds_lock);
+
+			/* push bit for packet processing */
+			stfm1000_rds_packet_bit(&stfm1000->rds_state, bit);
+
+			spin_lock_irq(&stfm1000->rds_lock);
+		}
+	}
+	spin_unlock_irq(&stfm1000->rds_lock);
+
+	/* now we're free to process non-interrupt related work */
+	while (stfm1000_rds_packet_dequeue(&stfm1000->rds_state, buf) == 0) {
+
+		if (stfm1000->rds_info)
+			printk(KERN_INFO "RDS-PKT: %02x %02x %02x %02x "
+					"%02x %02x %02x %02x\n",
+				buf[0], buf[1], buf[2], buf[3],
+				buf[4], buf[5], buf[6], buf[7]);
+
+		stfm1000_rds_process_packet(&stfm1000->rds_state, buf);
+	}
+
+	/* update our own counters */
+	stfm1000->rds_pkt_good += stfm1000->rds_state.pkt.good_packets;
+	stfm1000->rds_pkt_bad += stfm1000->rds_state.pkt.bad_packets;
+	stfm1000->rds_pkt_recovered +=
+		stfm1000->rds_state.pkt.recovered_packets;
+	stfm1000->rds_pkt_lost_sync +=
+		stfm1000->rds_state.pkt.sync_lost_packets;
+	stfm1000->rds_bit_overruns +=
+		stfm1000->rds_state.demod.RdsDemodSkippedBitCnt;
+
+	/* zero them now */
+	stfm1000->rds_state.pkt.good_packets = 0;
+	stfm1000->rds_state.pkt.bad_packets = 0;
+	stfm1000->rds_state.pkt.recovered_packets = 0;
+	stfm1000->rds_state.pkt.sync_lost_packets = 0;
+	stfm1000->rds_state.demod.RdsDemodSkippedBitCnt = 0;
+
+	/* reset requested from RDS handler? */
+	if (stfm1000_rds_get_reset_req(&stfm1000->rds_state)) {
+		if (stfm1000->rds_info)
+			printk(KERN_INFO "RDS: reset requested\n");
+		stfm1000_rds_reset(&stfm1000->rds_state);
+
+		stfm1000->rds_sync = stfm1000->rds_enable; /* force sync (if RDS) */
+		stfm1000->rds_demod_running = 0;
+		stfm1000->rssi_dc_est_log = 0;
+		stfm1000->signal_strength = 0;
+	}
+}
+
+void stfm1000_monitor_signal(struct stfm1000 *stfm1000, int bit)
+{
+	set_bit(bit, &stfm1000->thread_events);
+	return wake_up_interruptible(&stfm1000->thread_wait);
+}
+
+static int stfm1000_monitor_thread(void *data)
+{
+	struct stfm1000 *stfm1000 = data;
+	int ret;
+
+	printk(KERN_INFO "stfm1000: monitor thread started\n");
+
+	set_freezable();
+
+	/* Hmm, linux becomes *very* unhappy without this ... */
+	while (!kthread_should_stop()) {
+
+		ret = wait_event_interruptible_timeout(stfm1000->thread_wait,
+				stfm1000->thread_events == 0,
+				msecs_to_jiffies(stfm1000->monitor_period));
+
+		stfm1000->thread_events = 0;
+
+		if (kthread_should_stop())
+			break;
+
+		try_to_freeze();
+
+		mutex_lock(&stfm1000->state_lock);
+
+		/* we must be active */
+		if (!stfm1000->active)
+			goto next;
+
+		if (stfm1000->rds_enable)
+			rds_process(stfm1000);
+
+		/* perform quality monitor */
+		if (time_after_eq(jiffies, stfm1000->next_quality_monitor)) {
+
+			/* full quality monitor? */
+			if (stfm1000->quality_monitor)
+				Monitor_STFM_Quality(stfm1000);
+			else	/* simple */
+				Is_Station(stfm1000);
+
+			while (time_after_eq(jiffies,
+					stfm1000->next_quality_monitor))
+				stfm1000->next_quality_monitor +=
+					msecs_to_jiffies(
+					stfm1000->quality_monitor_period);
+		}
+
+		/* perform AGC monitor (if enabled) */
+		if (stfm1000->agc_monitor && time_after_eq(jiffies,
+				stfm1000->next_agc_monitor)) {
+			Monitor_STFM_AGC(stfm1000);
+			while (time_after_eq(jiffies,
+					stfm1000->next_agc_monitor))
+				stfm1000->next_agc_monitor +=
+					msecs_to_jiffies(
+						stfm1000->agc_monitor_period);
+		}
+next:
+		mutex_unlock(&stfm1000->state_lock);
+	}
+
+	printk(KERN_INFO "stfm1000: monitor thread stopped\n");
+
+	return 0;
+}
+
+static u64 stfm1000_dma_mask = DMA_32BIT_MASK;
+
+static int stfm1000_probe(struct i2c_client *client,
+			 const struct i2c_device_id *did)
+{
+	struct device *dev;
+	struct stfm1000 *stfm1000;
+	struct video_device *vd;
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	int ret;
+	u32 id;
+	const char *idtxt;
+	int i;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA)) {
+		dev_warn(&adapter->dev,
+			"I2C doesn't support I2C_FUNC_SMBUS_BYTE_DATA\n");
+		return -EIO;
+	}
+
+	/* make sure the dma masks are set correctly */
+	dev = &client->dev;
+	if (!dev->dma_mask)
+		dev->dma_mask = &stfm1000_dma_mask;
+	if (!dev->coherent_dma_mask)
+		dev->coherent_dma_mask = DMA_32BIT_MASK;
+
+	stfm1000 = kzalloc(sizeof(*stfm1000), GFP_KERNEL);
+	if (!stfm1000)
+		return -ENOMEM;
+
+	stfm1000->client = client;
+	i2c_set_clientdata(client, stfm1000);
+
+	mutex_init(&stfm1000->xfer_lock);
+	mutex_init(&stfm1000->state_lock);
+
+	vd = &stfm1000->radio;
+
+	strcpy(vd->name, "stfm1000");
+	vd->vfl_type		= VID_TYPE_TUNER;
+	vd->fops		= &stfm1000_fops;
+	vd->ioctl_ops		= &stfm_ioctl_ops;
+
+	/* vd->debug = V4L2_DEBUG_IOCTL | V4L2_DEBUG_IOCTL_ARG; */
+
+	vd->parent = &client->dev;
+
+	ret = video_register_device(vd, VFL_TYPE_RADIO, -1);
+	if (ret != 0) {
+		dev_warn(&adapter->dev,
+			 "Cannot register radio device\n");
+		goto out;
+	}
+
+	spin_lock_init(&stfm1000->rds_lock);
+
+	stfm1000_setup_reg_set(stfm1000);
+
+	/* stfm1000->dbgflg |= STFM1000_DBGFLG_I2C; */
+
+	ret = stfm1000_read(stfm1000, STFM1000_CHIPID, &id);
+	if (ret < 0) {
+		dev_warn(&adapter->dev,
+			 "Cannot read ID register\n");
+		goto out;
+	}
+	stfm1000->revid = id & 0xff;
+
+	/* NOTE: the tables are precalculated */
+	stfm1000->tune_rssi_th = 28;
+	stfm1000->tune_mpx_dc_th = 300;
+	stfm1000->adj_chan_th = 100;
+	stfm1000->pilot_est_th = 25;
+	stfm1000->agc_monitor = 0;	/* AGC monitor disabled */
+	stfm1000->quality_monitor = 1;
+	stfm1000->weak_signal = 0;
+	stfm1000->prev_pilot_present = 0;
+	stfm1000->tune_cap_a_f = (u32)(72.4 * 65536);
+	stfm1000->tune_cap_b_f = (u32)(0.07 * 65536);
+
+	/* only TB2 supports RDS */
+	stfm1000->rds_enable = stfm1000->revid == STFM1000_CHIP_REV_TB2 &&
+		rds_enable;
+	stfm1000->rds_present = 0;
+	stfm1000->rds_signal_th = 33;
+
+	stfm1000->freq = 92600;
+
+	stfm1000->georegion = georegion;
+	stfm1000->rssi = 0;
+	stfm1000->stereo = 0;
+	stfm1000->force_mono = 0;
+	stfm1000->monitor_period = 100;
+	stfm1000->quality_monitor_period = 1000;
+	stfm1000->agc_monitor_period = 200;
+
+	stfm1000->rds_sdnominal_adapt = 0;
+	stfm1000->rds_phase_pop = 1;
+
+	/* enable info about RDS */
+	stfm1000->rds_info = 0;
+
+	ret = stfm1000_power_up(stfm1000);
+	if (ret != 0) {
+		printk(KERN_ERR "%s: stfm1000_power_up failed\n",
+				__func__);
+		goto out;
+	}
+
+	if (stfm1000_alsa_ops && stfm1000_alsa_ops->init) {
+		ret = (*stfm1000_alsa_ops->init)(stfm1000);
+		if (ret != 0)
+			goto out;
+		stfm1000->alsa_initialized = 1;
+	}
+
+	ret = 0;
+	for (i = 0; stfm1000_attrs[i]; i++) {
+		ret = device_create_file(dev, stfm1000_attrs[i]);
+		if (ret)
+			break;
+	}
+	if (ret) {
+		while (--i >= 0)
+			device_remove_file(dev, stfm1000_attrs[i]);
+		goto out;
+	}
+
+	/* add it to the list */
+	mutex_lock(&devlist_lock);
+	stfm1000->idx = stfm1000_devcount++;
+	list_add_tail(&stfm1000->devlist, &stfm1000_devlist);
+	mutex_unlock(&devlist_lock);
+
+	init_waitqueue_head(&stfm1000->thread_wait);
+	stfm1000->thread = kthread_run(stfm1000_monitor_thread, stfm1000,
+			"stfm1000-%d", stfm1000->idx);
+	if (stfm1000->thread == NULL) {
+		printk(KERN_ERR "stfm1000: kthread_run failed\n");
+		goto out;
+	}
+
+	idtxt = stfm1000_get_rev_txt(stfm1000->revid);
+	if (idtxt == NULL)
+		printk(KERN_INFO "STFM1000: Loaded for unknown revision id "
+			"0x%02x\n", stfm1000->revid);
+	else
+		printk(KERN_INFO "STFM1000: Loaded for revision %s\n", idtxt);
+
+	return 0;
+
+out:
+	kfree(stfm1000);
+	return ret;
+}
+
+static int stfm1000_remove(struct i2c_client *client)
+{
+	struct stfm1000 *stfm1000 = i2c_get_clientdata(client);
+	struct device *dev = &client->dev;
+	int i;
+
+	kthread_stop(stfm1000->thread);
+
+	for (i = 0; stfm1000_attrs[i]; i++)
+		device_remove_file(dev, stfm1000_attrs[i]);
+
+	if (stfm1000->alsa_initialized) {
+		BUG_ON(stfm1000_alsa_ops->release == NULL);
+		(*stfm1000_alsa_ops->release)(stfm1000);
+		stfm1000->alsa_initialized = 0;
+	}
+
+	stfm1000_power_down(stfm1000);
+
+	video_unregister_device(&stfm1000->radio);
+
+	mutex_lock(&devlist_lock);
+	list_del(&stfm1000->devlist);
+	mutex_unlock(&devlist_lock);
+
+	kfree(stfm1000);
+	return 0;
+}
+
+static const struct i2c_device_id stfm1000_id[] = {
+	{ "stfm1000", 0xC0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, stfm1000_id);
+
+static struct i2c_driver stfm1000_i2c_driver = {
+	.driver = {
+		.name = "stfm1000",
+	},
+	.probe		= stfm1000_probe,
+	.remove		= stfm1000_remove,
+	.id_table	= stfm1000_id,
+};
+
+static int __init
+stfm1000_init(void)
+{
+	/* pull those in */
+	(void)Lock_Station;
+	(void)Unlock_Station;
+	return i2c_add_driver(&stfm1000_i2c_driver);
+}
+
+static void __exit
+stfm1000_exit(void)
+{
+	i2c_del_driver(&stfm1000_i2c_driver);
+
+	stfm1000_alsa_ops = NULL;
+}
+
+module_init(stfm1000_init);
+module_exit(stfm1000_exit);
+
+MODULE_AUTHOR("Pantelis Antoniou");
+MODULE_DESCRIPTION("A driver for the STFM1000 chip.");
+MODULE_LICENSE("GPL");
+
+module_param(georegion, int, 0400);
+module_param(rds_enable, int, 0400);
diff --git a/drivers/media/radio/stfm1000/stfm1000-filter.c b/drivers/media/radio/stfm1000/stfm1000-filter.c
new file mode 100644
index 000000000000..df42524a5da7
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-filter.c
@@ -0,0 +1,860 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/init.h>
+
+#include "stfm1000.h"
+
+void stfm1000_filter_reset(struct stfm1000_filter_parms *sdf)
+{
+	sdf->Left = 0;
+	sdf->Right = 0;
+	sdf->RssiDecoded = 0;
+	sdf->RssiMant = 0;
+	sdf->RssiExp = 0;
+	sdf->RssiLb = 0;
+	sdf->TrueRssi = 0;
+	sdf->Prssi = 0;
+	sdf->RssiLog = 0;
+	sdf->ScaledTrueRssi = 0;
+	sdf->FilteredRssi = 0;
+	sdf->PrevFilteredRssi = 0;
+	sdf->DecRssi = 0;
+	sdf->ScaledRssiDecoded = 0;
+	sdf->ScaledRssiDecodedZ = 0;
+	sdf->ScaledRssiDecodedZz = 0;
+	sdf->Echo = 0;
+	sdf->EchoLb = 0;
+	sdf->TrueEcho = 0;
+	sdf->FilteredEchoLpr = 0;
+	sdf->PrevFilteredEchoLpr = 0;
+	sdf->FilteredEchoLmr = 0;
+	sdf->PrevFilteredEchoLmr = 0;
+	sdf->GatedEcho = 0;
+	sdf->ControlLpr = 0;
+	sdf->ControlLmr = 0;
+	sdf->LprBw = 0;
+	sdf->LmrBw = 0;
+
+	sdf->LprXz = 0;
+	sdf->LprXzz = 0;
+	sdf->LprYz = 0;
+	sdf->LprYzz = 0;
+	sdf->LmrXz = 0;
+	sdf->LmrXzz = 0;
+	sdf->LmrYz = 0;
+	sdf->LmrYzz = 0;
+	sdf->FilteredLpr = 0;
+	sdf->FilteredLmr = 0;
+
+	sdf->B0B = 0;
+	sdf->B0S = 0;
+	sdf->B0M = 0;
+	sdf->B1over2B = 0;
+	sdf->B1over2S = 0;
+	sdf->B1over2M = 0;
+	sdf->A1over2B = 0;
+	sdf->A1over2S = 0;
+	sdf->A1over2M = 0;
+	sdf->A2B = 0;
+	sdf->A2S = 0;
+	sdf->A2M = 0;
+
+	sdf->AdjBw = 0;
+
+	sdf->pCoefLprBwThLo = 20 << 8;
+	sdf->pCoefLprBwThHi = 30 << 8;
+	sdf->pCoefLmrBwThLo = 40 << 8;
+	sdf->pCoefLmrBwThHi = 50 << 8;
+	sdf->pCoefLprBwSlSc = 4800;	/* SDK-2287 */
+	sdf->pCoefLprBwSlSh = 10;	/* SDK-2287 */
+	sdf->pCoefLmrBwSlSc = 4800;	/* SDK-2287 */
+	sdf->pCoefLmrBwSlSh = 10;	/* SDK-2287 */
+	sdf->pCoefLprGaSlSc = 0;
+	sdf->pCoefLprGaSlSh = 0;
+
+	sdf->ScaledControlLmr = 0;
+
+	sdf->LprGa = 32767;
+	sdf->LmrGa = 32767;
+
+	sdf->pCoefLprGaTh = 20;		/* 25 */
+	sdf->pCoefLmrGaTh = 55;		/* 60 50 */
+
+	sdf->MuteAudio = 0;
+	sdf->PrevMuteAudio = 0;
+	sdf->MuteActionFlag = 0;
+	sdf->ScaleAudio = 0;
+
+	/* *** Programmable initial setup for stereo path filters */
+	sdf->LprB0 = 18806;		/* -3dB cutoff = 17 kHz */
+	sdf->LprB1over2 = 18812;	/* -3dB cutoff = 17 kHz */
+	sdf->LprA1over2 = -16079;	/* -3dB cutoff = 17 kHz */
+	sdf->LprA2 = -11125;		/* -3dB cutoff = 17 kHz */
+	sdf->LmrB0 = 18806;		/* -3dB cutoff = 17 kHz */
+	sdf->LmrB1over2 = 18812;	/* -3dB cutoff = 17 kHz */
+	sdf->LmrA1over2 = -16079;	/* -3dB cutoff = 17 kHz */
+	sdf->LmrA2 = -11125;		/* -3dB cutoff = 17 kHz */
+
+	sdf->pCoefForceLockLmrBw = 0;	/* Force Lock LMR BW = LPR BW
+					 * XXX BUG WARNING -
+					 * This control doesn't work! */
+
+	sdf->pCoefForcedMono = 0;	/* Do not set this =
+					 * Quality Monitor will overwrite it */
+	sdf->pCoefBypassBlend = 0;	/* BUG WARNING -
+					 * This control doesn't work! */
+	sdf->pCoefBypassSoftmute = 0;	/* BUG WARNING -
+					 * This control doesn't work! */
+	sdf->pCoefBypassBwCtl = 0;	/* BUG WARNING -
+					 * This control doesn't work! */
+
+	/* There's a bug or something in the attack/decay section b/c
+	 * setting these coef's to anything */
+	/* higher than 100ms or so causes the RSSI to be artificially low -
+	 * Needs investigation! 15DEC06 */
+	sdf->pCoefRssiAttack = 65386;		/* changed to 100ms to avoid
+						 * stereo crackling
+						 * 60764 corresponds to 3 */
+	sdf->pCoefRssiDecay = 65386;		/* changed to 100ms to avoid
+						 * stereo crackling
+						 * 65530 corresponds to 10 */
+	sdf->pCoefEchoLprAttack = 52239;	/* corresponds to 1 */
+	sdf->pCoefEchoLprDecay = 64796;		/* corresponds to 20 */
+	sdf->pCoefEchoLmrAttack = 52239;	/* corresponds to 1 */
+	sdf->pCoefEchoLmrDecay = 65520;		/* corresponds to 20 */
+	sdf->pCoefEchoTh = 100;
+	sdf->pCoefEchoScLpr = (u16) (0.9999 * 32767.0);
+	sdf->pCoefEchoScLmr = (u16) (0.9999 * 32767.0);
+}
+
+void stfm1000_filter_decode(struct stfm1000_filter_parms *sdf, s16 Lpr,
+	s16 Lmr, u16 Rssi)
+{
+	s16 temp1_reg;		/* mimics 16 bit register */
+	s16 temp2_reg;		/* mimics 16 bit register */
+	s16 temp3_reg;		/* mimics 16 bit register */
+	s16 temp4_reg;		/* mimics 16 bit register */
+#ifndef _TUNER_STFM_MUTE
+	s16 temp5_reg;		/* mimics 16 bit register */
+#endif
+	s32 temp2_reg_32;	/*eI 108 27th Feb 06 temp variables. */
+
+	/* **************************************************************** */
+	/* *** Stereo Processing ****************************************** */
+	/* **************************************************************** */
+	/* *** This block operates at Fs = 44.1kHz */
+	/* ******** */
+	/* *** LPR path filter (2nd order IIR) */
+
+	sdf->Acc_signed = sdf->LprB0 * Lpr + 2 * (sdf->LprB1over2 * sdf->LprXz)
+	    + sdf->LprB0 * sdf->LprXzz + 2 * (sdf->LprA1over2 * sdf->LprYz)
+	    + sdf->LprA2 * sdf->LprYzz;
+
+	sdf->FilteredLpr = sdf->Acc_signed >> 15;
+
+	sdf->LprXzz = sdf->LprXz;	/* update taps */
+	sdf->LprXz = Lpr;
+	sdf->LprYzz = sdf->LprYz;
+	sdf->LprYz = sdf->FilteredLpr;
+
+	/* *** LMR path filter (2nd order IIR) */
+	sdf->Acc_signed = sdf->LmrB0 * Lmr + 2 * (sdf->LmrB1over2 * sdf->LmrXz)
+	    + sdf->LmrB0 * sdf->LmrXzz + 2 * (sdf->LmrA1over2 * sdf->LmrYz)
+	    + sdf->LmrA2 * sdf->LmrYzz;
+
+	sdf->FilteredLmr = sdf->Acc_signed >> 15;
+
+	sdf->LmrXzz = sdf->LmrXz;	/* update taps */
+	sdf->LmrXz = Lmr;
+	sdf->LmrYzz = sdf->LmrYz;
+	sdf->LmrYz = sdf->FilteredLmr;
+
+	/* *** Stereo Matrix */
+	if (0 == sdf->pCoefBypassBlend)
+		temp1_reg = sdf->LmrGa * sdf->FilteredLmr >> 15; /* Blend */
+	else
+		temp1_reg = sdf->FilteredLmr;
+
+	if (sdf->pCoefForcedMono)	/* Forced Mono */
+		temp1_reg = 0;
+
+	if (0 == sdf->pCoefBypassSoftmute) {
+		temp2_reg = sdf->LprGa * sdf->FilteredLpr >> 15; /* LPR */
+		temp3_reg = sdf->LprGa * temp1_reg >> 15;	 /* LMR */
+	} else {
+		temp2_reg = sdf->FilteredLpr;
+		temp3_reg = temp1_reg;
+	}
+
+	temp4_reg = (temp2_reg + temp3_reg) / 2;	/* Matrix */
+
+#ifndef _TUNER_STFM_MUTE
+	temp5_reg = (temp2_reg - temp3_reg) / 2;
+#endif
+
+#if 0
+	/* *** DC Cut Filter (leaky bucket estimate) */
+	if (0 == sdf->pCoefBypassDcCut) {
+		sdf->LeftLb_i32 =
+		    sdf->LeftLb_i32 + temp4_reg - (sdf->LeftLb_i32 >> 8);
+		temp2_reg = temp4_reg - (sdf->LeftLb_i32 >> 8);	/* signal -
+								dc_estimate */
+
+		sdf->RightLb_i32 =
+		    sdf->RightLb_i32 + temp5_reg - (sdf->RightLb_i32 >> 8);
+		temp3_reg = temp5_reg - (sdf->RightLb_i32 >> 8); /* signal -
+								dc_estimate */
+	} else {
+		temp2_reg = temp4_reg;
+		temp3_reg = temp5_reg;
+	}
+#endif
+#ifdef _TUNER_STFM_MUTE
+	/* *** Mute Audio */
+	if (sdf->MuteAudio != sdf->PrevMuteAudio)	/* Mute transition */
+		sdf->MuteActionFlag = 1;	/* set flag */
+	sdf->PrevMuteAudio = sdf->MuteAudio;	/* update history */
+
+	if (sdf->MuteActionFlag) {
+		if (0 == sdf->MuteAudio) {	/* Mute to zero */
+			/* gradual mute down */
+			sdf->ScaleAudio = sdf->ScaleAudio - sdf->pCoefMuteStep;
+
+			/* eI-117:Oct28:as per C++ code */
+			/* if (0 < sdf->ScaleAudio) */
+			if (0 > sdf->ScaleAudio) {
+				sdf->ScaleAudio = 0;	 /* Minimum scale
+							  * factor */
+				sdf->MuteActionFlag = 0; /* End Mute Action */
+			}
+		} else {	/* Un-Mute to one */
+				/* gradual mute up */
+			sdf->ScaleAudio = sdf->ScaleAudio + sdf->pCoefMuteStep;
+			if (0 > sdf->ScaleAudio) {	/* look for rollover
+							 * beyong 32767 */
+				sdf->ScaleAudio = 32767; /* Maximum scale
+							  * factor */
+				sdf->MuteActionFlag = 0; /* End Mute Action */
+			}
+		}		/* end else */
+	}			/* end if (sdf->MuteActionFlag) */
+
+/*! Output Processed Sample */
+
+	sdf->Left = (temp2_reg * sdf->ScaleAudio) >> 15;	/* Scale */
+	sdf->Right = (temp3_reg * sdf->ScaleAudio) >> 15;	/* Scale */
+
+#else /* !_TUNER_STFM_MUTE */
+
+	sdf->Left = temp4_reg;
+	sdf->Right = temp5_reg;
+
+#endif /* !_TUNER_STFM_MUTE */
+
+	/* *** End Stereo Processing ************************************** */
+	/* **************************************************************** */
+
+	/* **************************************************************** */
+	/* *** Signal Quality Indicators ********************************** */
+	/* **************************************************************** */
+	/* *** This block operates at Fs = 44.1kHz */
+	/* ******** */
+	/* *** RSSI */
+	/* ******** */
+	/* Decode Floating Point RSSI data */
+	/*! Input RSSI sample */
+	sdf->RssiMant = (Rssi & 0xFFE0) >> 5;	/* 11 msb's */
+	sdf->RssiExp = Rssi & 0x001F;	/* 5 lsb's */
+	sdf->RssiDecoded = sdf->RssiMant << sdf->RssiExp;
+
+	/* *** Convert RSSI to 10*Log10(RSSI) */
+	/* This is easily accomplished in DSP code using the CLZ instruction */
+	/* rather than using all these comparisons. */
+	/* The basic idea is this: */
+	/* if x >= 2^P */
+	/*   f(x) = 3*x>>P + (3*P-3) */
+	/* Approx. is valid over the range of sdf->RssiDecoded in [0, 2^21] */
+	/* *** */
+	if (sdf->RssiDecoded >= 1048576)
+		sdf->Prssi = 20;
+	else if (sdf->RssiDecoded >= 524288)
+		sdf->Prssi = 19;
+	else if (sdf->RssiDecoded >= 262144)
+		sdf->Prssi = 18;
+	else if (sdf->RssiDecoded >= 131072)
+		sdf->Prssi = 17;
+	else if (sdf->RssiDecoded >= 65536)
+		sdf->Prssi = 16;
+	else if (sdf->RssiDecoded >= 32768)
+		sdf->Prssi = 15;
+	else if (sdf->RssiDecoded >= 16384)
+		sdf->Prssi = 14;
+	else if (sdf->RssiDecoded >= 8192)
+		sdf->Prssi = 13;
+	else if (sdf->RssiDecoded >= 4096)
+		sdf->Prssi = 12;
+	else if (sdf->RssiDecoded >= 2048)
+		sdf->Prssi = 11;
+	else if (sdf->RssiDecoded >= 1024)
+		sdf->Prssi = 10;
+	else if (sdf->RssiDecoded >= 512)
+		sdf->Prssi = 9;
+	else if (sdf->RssiDecoded >= 256)
+		sdf->Prssi = 8;
+	else if (sdf->RssiDecoded >= 128)
+		sdf->Prssi = 7;
+	else if (sdf->RssiDecoded >= 64)
+		sdf->Prssi = 6;
+	else if (sdf->RssiDecoded >= 32)
+		sdf->Prssi = 5;
+	else if (sdf->RssiDecoded >= 16)
+		sdf->Prssi = 4;
+	else if (sdf->RssiDecoded >= 8)
+		sdf->Prssi = 3;
+	else if (sdf->RssiDecoded >= 4)
+		sdf->Prssi = 2;
+	else if (sdf->RssiDecoded >= 2)
+		sdf->Prssi = 1;
+	else
+		sdf->Prssi = 0;
+	sdf->RssiLog =
+	    (3 * sdf->RssiDecoded >> sdf->Prssi) + (3 * sdf->Prssi - 3);
+
+	if (0 > sdf->RssiLog)	/* Clamp to positive */
+		sdf->RssiLog = 0;
+
+	/* Compensate for errors in truncation/approximation by adding 1 */
+	sdf->RssiLog = sdf->RssiLog + 1;
+
+	/* Leaky Bucket Filter DC estimate of RSSI */
+	sdf->RssiLb = sdf->RssiLb + sdf->RssiLog - (sdf->RssiLb >> 3);
+	sdf->TrueRssi = sdf->RssiLb >> 3;
+
+	/* Scale up so we have some room for precision */
+	sdf->ScaledTrueRssi = sdf->TrueRssi << 8;
+	/* ************ */
+	/* *** end RSSI */
+	/* ************ */
+
+	/* ******** */
+	/* *** Echo */
+	/* ******** */
+	/* *** Isolate Echo information as higher frequency info */
+	/* using [1 -2 1] highpass FIR */
+	sdf->ScaledRssiDecoded = sdf->RssiDecoded >> 4;
+	sdf->Echo =
+	    (s16) ((sdf->ScaledRssiDecoded -
+		    2 * sdf->ScaledRssiDecodedZ + sdf->ScaledRssiDecodedZz));
+	sdf->ScaledRssiDecodedZz = sdf->ScaledRssiDecodedZ;
+	sdf->ScaledRssiDecodedZ = sdf->ScaledRssiDecoded;
+	/* ************ */
+	/* *** end Echo */
+	/* ************ */
+	/* *** End Signal Quality Indicators ******************************* */
+	/* ***************************************************************** */
+
+	/* ***************************************************************** */
+	/* *** Weak Signal Processing ************************************** */
+	/* ***************************************************************** */
+	/* *** This block operates at Fs = 44.1/16 = 2.75 Khz
+	 * *eI 108 28th Feb 06 WSP and SM executes at 2.75Khz */
+	/* decimate by 16                   STFM_FILTER_BLOCK_MULTIPLE is 16 */
+	if (0 == sdf->DecRssi) {
+		/* *** Filter RSSI via attack/decay structure */
+		if (sdf->ScaledTrueRssi > sdf->PrevFilteredRssi)
+			sdf->Acc =
+			    sdf->pCoefRssiAttack *
+			    sdf->PrevFilteredRssi + (65535 -
+						     sdf->pCoefRssiAttack)
+			    * sdf->ScaledTrueRssi;
+		else
+			sdf->Acc =
+			    sdf->pCoefRssiDecay *
+			    sdf->PrevFilteredRssi + (65535 -
+						     sdf->pCoefRssiDecay)
+			    * sdf->ScaledTrueRssi;
+
+		sdf->FilteredRssi = sdf->Acc >> 16;
+		sdf->PrevFilteredRssi = sdf->FilteredRssi;
+
+		/* *** Form Echo "energy" representation */
+		if (0 > sdf->Echo)
+			sdf->Echo = -sdf->Echo;	/* ABS() */
+
+		/* Threshold compare */
+		sdf->GatedEcho = (s16) (sdf->Echo - sdf->pCoefEchoTh);
+		if (0 > sdf->GatedEcho)	/* Clamp to (+)ve */
+			sdf->GatedEcho = 0;
+
+		/* *** Leaky bucket DC estimate of Echo energy */
+		sdf->EchoLb = sdf->EchoLb + sdf->GatedEcho -
+				(sdf->EchoLb >> 3);
+		sdf->TrueEcho = sdf->EchoLb >> 3;
+
+		/* *** Filter Echo via attack/decay structure for LPR */
+		if (sdf->TrueEcho > sdf->PrevFilteredEchoLpr)
+			sdf->Acc =
+			    sdf->pCoefEchoLprAttack *
+			    sdf->PrevFilteredEchoLpr +
+				(65535 - sdf->pCoefEchoLprAttack) *
+					sdf->TrueEcho;
+		else
+			sdf->Acc =
+			    sdf->pCoefEchoLprDecay *
+			    sdf->PrevFilteredEchoLpr +
+				(65535 - sdf->pCoefEchoLprDecay) *
+					sdf->TrueEcho;
+
+		sdf->FilteredEchoLpr = sdf->Acc >> 16;
+		sdf->PrevFilteredEchoLpr = sdf->FilteredEchoLpr;
+
+		/* *** Filter Echo via attack/decay structure for LMR */
+		if (sdf->TrueEcho > sdf->PrevFilteredEchoLmr)
+			sdf->Acc = sdf->pCoefEchoLmrAttack *
+				sdf->PrevFilteredEchoLmr +
+					(65535 - sdf->pCoefEchoLmrAttack)
+						* sdf->TrueEcho;
+		else
+			sdf->Acc =
+			    sdf->pCoefEchoLmrDecay *
+			    sdf->PrevFilteredEchoLmr + (65535 -
+							sdf->pCoefEchoLmrDecay)
+			    * sdf->TrueEcho;
+
+		sdf->FilteredEchoLmr = sdf->Acc >> 16;
+		sdf->PrevFilteredEchoLmr = sdf->FilteredEchoLmr;
+
+		/* *** Form control variables */
+		/* Generically speaking, ctl = f(RSSI, Echo) =
+		 * RSSI - (a*Echo)<<b, where a,b are programmable */
+		sdf->ControlLpr = sdf->FilteredRssi -
+		    ((sdf->pCoefEchoScLpr *
+		      sdf->FilteredEchoLpr << sdf->pCoefEchoShLpr) >> 15);
+		if (0 > sdf->ControlLpr)
+			sdf->ControlLpr = 0;	/* Clamp to positive */
+
+		sdf->ControlLmr = sdf->FilteredRssi -
+		    ((sdf->pCoefEchoScLmr *
+		      sdf->FilteredEchoLmr << sdf->pCoefEchoShLmr) >> 15);
+		if (0 > sdf->ControlLmr)
+			sdf->ControlLmr = 0;	/* Clamp to positive */
+
+		/* *** Define LPR_BW = f(control LPR) */
+		/* Assume that 5 kHz and 17 kHz are limits of LPR_BW control */
+		if (sdf->ControlLpr <= sdf->pCoefLprBwThLo)
+			sdf->LprBw = 5000;	/* lower limit is 5 kHz */
+		else if (sdf->ControlLpr >= sdf->pCoefLprBwThHi)
+			sdf->LprBw = 17000;	/* upper limit is 17 kHz */
+		else
+			sdf->LprBw = 17000 -
+			    ((sdf->pCoefLprBwSlSc *
+			      (sdf->pCoefLprBwThHi -
+			       sdf->ControlLpr)) >> sdf->pCoefLprBwSlSh);
+
+		/* *** Define LMR_BW = f(control LMR) */
+		/* Assume that 5 kHz and 17 kHz are limits of LPR_BW control */
+		if (0 == sdf->pCoefForceLockLmrBw) {	/* only do these calc's
+							 * if LMR BW not
+							 * ForceLocked */
+			if (sdf->ControlLmr <= sdf->pCoefLmrBwThLo)
+				sdf->LmrBw = 5000;	/* lower limit is
+							 * 5 kHz */
+			else if (sdf->ControlLmr >= sdf->pCoefLmrBwThHi)
+				sdf->LmrBw = 17000;	/* upper limit is
+							 * 17 kHz */
+			else
+				sdf->LmrBw = 17000 -
+				    ((sdf->pCoefLmrBwSlSc *
+				      (sdf->pCoefLmrBwThHi -
+				       sdf->ControlLmr)) >>
+				     sdf->pCoefLmrBwSlSh);
+		}
+		/* *** Define LMR_Gain = f(control LMR)
+		 * Assume that Blending occurs across 20 dB range of
+		 * control LMR. For sake of listenability, approximate
+		 * antilog blending curve
+		 * To simplify antilog approx, scale control LMR back into
+		 * "RSSI in dB range" [0,60] */
+		sdf->ScaledControlLmr = sdf->ControlLmr >> 8;
+
+		/* how far below blend threshold are we? */
+		temp1_reg = sdf->pCoefLmrGaTh - sdf->ScaledControlLmr;
+		if (0 > temp1_reg)	/* We're not below threshold,
+					 * so no blending needed */
+			temp1_reg = 0;
+		temp2_reg = 20 - temp1_reg;	/* Blend range = 20 dB */
+		if (0 > temp2_reg)
+			temp2_reg = 0;	/* if beyond that range,
+					 * then clamp to 0 */
+
+		/* We want stereo separation (n dB) to rolloff linearly over
+		 * the 20 dB wide blend region.
+		 * this necessitates a particular rolloff for the blend
+		 * parameter, which is not obvious.
+		 * See sw_audio/log_approx.m for calculation of this rolloff,
+		 * implemented below...
+		 * Note that stereo_separation (in dB) = 20*log10((1+a)/(1-a)),
+		 * where a = blend scaler
+		 * appropriately scaled for 2^15.  This relationship sits at
+		 * the heart of why this curve is needed. */
+		if (15 <= temp2_reg)
+			temp3_reg = 264 * temp2_reg + 27487;
+		else if (10 <= temp2_reg)
+			temp3_reg = 650 * temp2_reg + 21692;
+		else if (5 <= temp2_reg)
+			temp3_reg = 1903 * temp2_reg + 9166;
+		else
+			temp3_reg = 3736 * temp2_reg;
+
+		sdf->LmrGa = temp3_reg;
+
+		if (32767 < sdf->LmrGa)
+			sdf->LmrGa = 32767;	/* Clamp to '1' */
+
+		/* *** Define LPR_Gain = f(control LPR)
+		 * Assume that SoftMuting occurs across 20 dB range of
+		 * control LPR
+		 * For sake of listenability, approximate antilog softmute
+		 * curve To simplify antilog approx, scale control LPR back
+		 * into "RSSI in dB range" [0,60] */
+		sdf->ScaledControlLpr = sdf->ControlLpr >> 8;
+		/* how far below softmute threshold are we? */
+		temp1_reg = sdf->pCoefLprGaTh - sdf->ScaledControlLpr;
+		if (0 > temp1_reg)	/* We're not below threshold,
+					 * so no softmute needed */
+			temp1_reg = 0;
+		temp2_reg = 20 - temp1_reg;	/* SoftmMute range = 20 dB */
+		if (0 > temp2_reg)
+			temp2_reg = 0;	/* if beyond that range,
+					 * then clamp to 0 */
+		/* Form 100*10^((temp2_reg-20)/20) approximation (antilog)
+		 * over range [0,20] dB
+		 * approximation in range [0,100], but we only want to
+		 * softmute down to -20 dB, no further */
+		if (16 < temp2_reg)
+			temp3_reg = 9 * temp2_reg - 80;
+		else if (12 < temp2_reg)
+			temp3_reg = 6 * temp2_reg - 33;
+		else if (8 < temp2_reg)
+			temp3_reg = 4 * temp2_reg - 8;
+		else
+			temp3_reg = 2 * temp2_reg + 9;
+
+		sdf->LprGa = 328 * temp3_reg;	/* close to 32767*(1/100) */
+
+		if (32767 < sdf->LprGa)
+			sdf->LprGa = 32767;	/* Clamp to '1' */
+
+		if (3277 > sdf->LprGa)
+			sdf->LprGa = 3277;	/* Clamp to 0.1*32767 =
+						 * -20 dB min gain */
+
+		/* *************** Bandwidth Sweep Algorithm ************ */
+		/* *** Calculate 2nd order filter coefficients as function
+		 * of desired BW. We do this by constructing piece-wise
+		 * linear filter coef's as f(BW), which is why we break the
+		 * calc's into different BW regions below.
+		 * coef(BW) = S*(M*BW + B)
+		 * For more info, see sw_audio/ws_filter.m checked into CVS */
+		if (0 == sdf->pCoefBypassBwCtl)	{ /* if ==1, then we just go
+						   * with default coef set */
+			/* determine if we run thru loop once or twice... */
+			if (1 == sdf->pCoefForceLockLmrBw)
+				temp4_reg = 1;	/* run thru once only to calc.
+						 * LPR coef's */
+			else
+				temp4_reg = 2;	/* run thru twice to calc.
+						 * LPR and LMR coef's */
+
+			/* Here's the big coef. calc. loop */
+			for (temp1_reg = 0; temp1_reg < temp4_reg;
+				temp1_reg++) {
+
+				if (0 == temp1_reg)
+					temp2_reg = (s16) sdf->LprBw;
+				else
+					temp2_reg = (s16) sdf->LmrBw;
+
+
+				if (6000 > temp2_reg) {
+					/* interval = [4.4kHz, 6.0kHz) */
+					sdf->B0M = 22102;
+					sdf->B0B = -2209;
+					sdf->B0S = 1;
+
+					sdf->B1over2M = 22089;
+					sdf->B1over2B = -2205;
+					sdf->B1over2S = 1;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = -24664;
+					sdf->A2B = 11698;
+					sdf->A2S = 2;
+				} else if (8000 > temp2_reg) {
+					/* interval = [6.0kHz, 8.0kHz) */
+					sdf->B0M = 22102;
+					sdf->B0B = -2209;
+					sdf->B0S = 1;
+
+					sdf->B1over2M = 22089;
+					sdf->B1over2B = -2205;
+					sdf->B1over2S = 1;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = -31231;
+					sdf->A2B = 18468;
+					sdf->A2S = 1;
+				} else if (10000 > temp2_reg) {
+					/* interval = [8.0kHz, 10.0kHz) */
+					sdf->B0M = 28433;
+					sdf->B0B = -4506;
+					sdf->B0S = 1;
+
+					sdf->B1over2M = 28462;
+					sdf->B1over2B = -4584;
+					sdf->B1over2S = 1;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = -14811;
+					sdf->A2B = 12511;
+					sdf->A2S = 1;
+				} else if (12000 > temp2_reg) {
+					/* interval = [10.0kHz, 12.0kHz) */
+					sdf->B0M = 28433;
+					sdf->B0B = -4506;
+					sdf->B0S = 1;
+
+					sdf->B1over2M = 28462;
+					sdf->B1over2B = -4584;
+					sdf->B1over2S = 1;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = -181;
+					sdf->A2B = 5875;
+					sdf->A2S = 1;
+				} else if (14000 > temp2_reg) {
+					/* interval = [12.0kHz, 14.0kHz) */
+					sdf->B0M = 18291;
+					sdf->B0B = -4470;
+					sdf->B0S = 2;
+
+					sdf->B1over2M = 18461;
+					sdf->B1over2B = -4597;
+					sdf->B1over2S = 2;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = 14379;
+					sdf->A2B = -2068;
+					sdf->A2S = 1;
+				} else if (16000 > temp2_reg) {
+					/* interval = [14.0kHz, 16.0kHz) */
+					sdf->B0M = 18291;
+					sdf->B0B = -4470;
+					sdf->B0S = 2;
+
+					sdf->B1over2M = 18461;
+					sdf->B1over2B = -4597;
+					sdf->B1over2S = 2;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = 30815;
+					sdf->A2B = -12481;
+					sdf->A2S = 1;
+				} else if (18000 > temp2_reg) {
+					/* interval = [16.0kHz, 18.0kHz) */
+					sdf->B0M = 24740;
+					sdf->B0B = -9152;
+					sdf->B0S = 2;
+
+					sdf->B1over2M = 24730;
+					sdf->B1over2B = -9142;
+					sdf->B1over2S = 2;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = 25631;
+					sdf->A2B = -13661;
+					sdf->A2S = 2;
+				} else {
+					/* interval = [18.0kHz, 19.845kHz) */
+					sdf->B0M = 24740;
+					sdf->B0B = -9152;
+					sdf->B0S = 2;
+
+					sdf->B1over2M = 24730;
+					sdf->B1over2B = -9142;
+					sdf->B1over2S = 2;
+
+					sdf->A1over2M = 31646;
+					sdf->A1over2B = -15695;
+					sdf->A1over2S = 2;
+
+					sdf->A2M = 19382;
+					sdf->A2B = -12183;
+					sdf->A2S = 4;
+				}
+
+				if (0 == temp1_reg) {
+					/* The piece-wise linear eq's are
+					 * based on a scaled version
+					 * (32768/22050) of BW */
+
+					/* Note 32768/22050 <-> 2*(16384/22050)
+					 * <-> 2*((16384/22050)*32768)>>15 */
+					sdf->AdjBw = ((temp2_reg << 1) *
+						24348) >> 15;
+
+					/* temp = mx */
+					temp3_reg = (sdf->B0M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LprB0 = sdf->B0S *
+						(temp3_reg + sdf->B0B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->B1over2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LprB1over2 = sdf->B1over2S *
+						(temp3_reg + sdf->B1over2B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->A1over2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LprA1over2 = -sdf->A1over2S *
+						(temp3_reg + sdf->A1over2B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->A2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LprA2 = -sdf->A2S *
+						(temp3_reg + sdf->A2B);
+					/* *** end LPR channel --
+					 * LPR coefficients now ready for
+					 * Stereo Path next time */
+				} else {
+					/* The piece-wise linear eq's are
+					 * based on a scaled version
+					 * (32768/22050) of BW */
+
+					/* Note 32768/22050 <-> 2*(16384/22050)
+					 * <-> 2*((16384/22050)*32768)>>15 */
+					sdf->AdjBw = ((temp2_reg << 1) *
+						24348) >> 15;
+
+					/* temp = mx */
+					temp3_reg = (sdf->B0M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LmrB0 = sdf->B0S *
+						(temp3_reg + sdf->B0B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->B1over2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LmrB1over2 = sdf->B1over2S *
+						(temp3_reg + sdf->B1over2B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->A1over2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LmrA1over2 = -sdf->A1over2S *
+						(temp3_reg + sdf->A1over2B);
+
+					/* temp = mx */
+					temp3_reg = (sdf->A2M *
+						sdf->AdjBw) >> 15;
+
+					/* y = S*(mx + b) */
+					sdf->LmrA2 = -sdf->A2S *
+						(temp3_reg + sdf->A2B);
+					/* *** end LMR channel -- LMR
+					 * coefficients now ready for Stereo
+					 * Path next time */
+				}
+			}	/* end for (temp1_reg=0... */
+			if (1 == sdf->pCoefForceLockLmrBw) {
+				/* if Force Lock LMR BW = LPR BW */
+				/* then set LMR coef's = LPR coef's */
+				sdf->LmrB0 = sdf->LprB0;
+				sdf->LmrB1over2 = sdf->LprB1over2;
+				sdf->LmrA1over2 = sdf->LprA1over2;
+				sdf->LmrA2 = sdf->LprA2;
+			}
+
+		}		/* end if (0 == sdf->pCoef_BypassBwCtl) */
+		/* eI 108 24th Feb 06 Streo Matrix part moved after
+		 * weak signal processing. */
+		if (0 == sdf->pCoefBypassBlend)
+			temp1_reg = sdf->LmrGa;	/* Blend */
+		else
+			temp1_reg = 1;
+
+		if (sdf->pCoefForcedMono)	/* Forced Mono */
+			temp1_reg = 0;
+
+		if (0 == sdf->pCoefBypassSoftmute) {
+
+			/* SoftMute applied to LPR */
+			sdf->temp2_reg_sm = sdf->LprGa;
+
+			temp2_reg_32 = sdf->LprGa * temp1_reg;
+
+			/* SoftMute applied to LMR */
+			sdf->temp3_reg_sm = (temp2_reg_32) >> 15;
+		} else {
+			sdf->temp2_reg_sm = 1;	/* eI 108 24th Feb 06 update
+						 * global variable for IIR
+						 * filter. */
+			sdf->temp3_reg_sm = temp1_reg;
+		}
+
+	}			/* end if (0 == sdf->DecRssi) */
+
+	sdf->DecRssi = ((sdf->DecRssi + 1) % 16);	/* end decimation
+							 * by 16 */
+
+	/* *** End Weak Signal Processing ********************************** */
+	/* ***************************************************************** */
+}
diff --git a/drivers/media/radio/stfm1000/stfm1000-filter.h b/drivers/media/radio/stfm1000/stfm1000-filter.h
new file mode 100644
index 000000000000..d24e3e9244b8
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-filter.h
@@ -0,0 +1,185 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef STFM1000_FILTER_H
+#define STFM1000_FILTER_H
+
+/* STFM1000 Black Box Filter parameters */
+struct stfm1000_filter_parms {
+	s16 LprXzz;		/* LPR x(n-2) stereo filter */
+	s16 LmrXzz;		/* LMR x(n-2) stereo filter */
+	s16 LprYzz;		/* LPR y(n-2) stereo filter */
+	s16 LmrYzz;		/* LMR y(n-2) stereo filter */
+
+	s16 LprXz;		/* LPR x(n-1) stereo filter */
+	s16 LmrXz;		/* LMR x(n-1) stereo filter */
+	s16 FilteredLpr;	/* LPR filter output */
+	s16 FilteredLmr;	/* LMR filter output */
+
+	s16 LprB0;		/* LPR stereo filter coef */
+	s16 LprB1over2;		/* LPR stereo filter coef */
+	s16 LprA1over2;		/* LPR stereo filter coef */
+	s16 LprA2;		/* LPR stereo filter coef */
+
+	s16 LmrB0;		/* LMR stereo filter coef */
+	s16 LmrB1over2;		/* LMR stereo filter coef */
+	s16 LmrA1over2;		/* LMR stereo filter coef */
+	s16 LmrA2;		/* LMR stereo filter coef */
+
+	s16 LprYz;		/* LPR y(n-1) stereo filter */
+	s16 LmrYz;		/* LMR y(n-1) stereo filter */
+
+	s16 Left;		/* left channel audio out */
+	s16 Right;		/* right channel audio out */
+	s32 LeftLb;		/* left channel dc estimate */
+	s32 RightLb;		/* right channel dc estimate */
+
+	u32 RssiDecoded;	/* integer decoded RSSI */
+
+	u16 RssiMant;		/* mantissa of float-coded RSSI */
+	s16 RssiLog;		/* 10log10(decoded RSSI) */
+
+	u16 RssiExp;		/* exponent of float-coded RSSI */
+	u16 RssiLb;		/* leaky bucket dc of rssi */
+
+	u16 Prssi;		/* power of 2 for RSSI */
+	u16 TrueRssi;		/* DC estimate of log RSSI */
+
+	u16 ScaledRssiDecoded;	/* scaled log RSSI */
+	s16 Echo;		/* Echo info from HiPass(RSSI) */
+	u16 ScaledRssiDecodedZ;	/* history buffer for above */
+	u16 ScaledRssiDecodedZz;/* ditto */
+
+	u16 ScaledTrueRssi;	/* scaled version for precision */
+	u16 FilteredRssi;	/* Attack/Decay filtered RSSI */
+	u16 PrevFilteredRssi;	/* previous version of above */
+
+	u16 EchoLb;		/* DC estimate of Echo energy */
+	u16 TrueEcho;		/* scaled version of above */
+	u16 FilteredEchoLpr;	/* Attack/Decay filt. Echo */
+	u16 PrevFilteredEchoLpr;/* previous version of above */
+	u16 FilteredEchoLmr;	/* Attack/Decay filt. Echo */
+	u16 PrevFilteredEchoLmr;/* previous version of above */
+	s16 GatedEcho;		/* Echo gated by threshold */
+
+	s16 ControlLpr;		/* master control for LPR */
+	s16 ControlLmr;		/* master control for LMR */
+	u16 LprBw;		/* LPR Bandwidth desired */
+	u16 LmrBw;		/* LMR Bandwidth desired */
+	u16 LprGa;		/* LPR Gain (SoftMute) desired */
+	u16 LmrGa;		/* LMR Gain (Blend) desired */
+	u16 ScaledControlLmr;	/* Scaled down version Ctl LMR */
+	u16 ScaledControlLpr;	/* Scaled down version Ctl LPR */
+
+	s16 B0M;		/* BW ctl B0 coef slope */
+	s16 B0B;		/* BW ctl B0 coef y-intercept */
+
+	u16 B0S;		/* BW ctl B0 coef scale */
+	s16 B1over2M;		/* BW ctl B1/2 coef slope */
+
+	s16 B1over2B;		/* BW ctl B1/2 coef y-intercept */
+	s16 A1over2B;		/* BW ctl A1/2 coef y-intercept */
+
+	u16 B1over2S;		/* BW ctl B1/2 coef scale */
+	u16 A1over2S;		/* BW ctl A1/2 coef scale */
+
+	s16 A1over2M;		/* BW ctl A1/2 coef slope */
+	u16 A2S;		/* BW ctl A2 coef scale */
+
+	s16 A2M;		/* BW ctl A2 coef slope */
+	s16 A2B;		/* BW ctl A2 coef y-intercept */
+
+	u16 AdjBw;		/* Desired Filter BW scaled into range */
+
+	u16 DecRssi;		/*! Decimation modulo counter */
+
+	s16 ScaleAudio;		/*! Scale factor for Audio Mute */
+	u8 MuteAudio;		/*! Control for muting audio */
+	u8 PrevMuteAudio;	/*! History of control for muting audio */
+	u8 MuteActionFlag;	/*! Indicator of when mute ramping occurs */
+
+	u32 Acc;		/* mimics H/W accumulator */
+	s32 Acc_signed;
+	s16 temp1_reg;		/* mimics 16 bit register */
+	s16 temp2_reg;		/* mimics 16 bit register */
+	s16 temp3_reg;		/* mimics 16 bit register */
+	s16 temp4_reg;		/* mimics 16 bit register */
+	s16 temp5_reg;		/* mimics 16 bit register */
+
+	/* *** Programmable Coefficients */
+	u16 pCoefRssiAttack;	/* prog coef RSSI attack */
+	u16 pCoefRssiDecay;	/* prog coef RSSI decay */
+	u16 pCoefEchoLprAttack;	/* prog coef Echo LPR attack */
+	u16 pCoefEchoLprDecay;	/* prog coef Echo LPR decay */
+	u16 pCoefEchoLmrAttack;	/* prog coef Echo LMR attack */
+	u16 pCoefEchoLmrDecay;	/* prog coef Echo LMR decay */
+
+	u16 pCoefEchoTh;	/* prog coef Echo threshold */
+
+	u16 pCoefEchoScLpr;	/* prog coef scale Echo LPR infl. */
+	u16 pCoefEchoScLmr;	/* prog coef scale Echo LMR infl. */
+	u16 pCoefEchoShLpr;	/* prog coef shift Echo LPR infl. */
+	u16 pCoefEchoShLmr;	/* prog coef shift Echo LMR infl. */
+
+	u16 pCoefLprBwThLo;	/* prog coef Low Th LPR BW */
+	u16 pCoefLprBwThHi;	/* prog coef High Th LPR BW */
+	u16 pCoefLmrBwThLo;	/* prog coef Low Th LMR BW */
+	u16 pCoefLmrBwThHi;	/* prog coef High Th LMR BW */
+
+	u16 pCoefLprGaTh;	/* prog coef Th LPR Gain (SoftMute) */
+	u16 pCoefLmrGaTh;	/* prog coef Th LMR Gain (Blend) */
+
+	u16 pCoefLprBwSlSc;	/* prog coef Slope scale LPR BW */
+	u16 pCoefLprBwSlSh;	/* prog coef Slope shift LPR BW */
+	u16 pCoefLmrBwSlSc;	/* prog coef Slope scale LMR BW */
+	u16 pCoefLmrBwSlSh;	/* prog coef Slope shift LMR BW */
+	u16 pCoefLprGaSlSc;	/* prog coef Slope scale LPR Gain */
+	u16 pCoefLprGaSlSh;	/* prog coef Slope shift LPR Gain */
+
+	u8 pCoefForcedMono;	/* Forced Mono control bit */
+	u8 pCoefBypassBlend;	/* Forced bypass of stereo blend */
+	u8 pCoefBypassSoftmute;	/* Forced bypass of softmute */
+	u8 pCoefBypassDcCut;	/* Forced bypass of audio DC Cut filter */
+
+	u8 pCoefBypassBwCtl;	/* Forced bypass of bandwidth control */
+	u8 pCoefForceLockLmrBw;	/* prog flag to force LMR BW=LPR BW */
+
+	/* XXX added here, they were global */
+	s16 temp2_reg_sm;
+	s16 temp3_reg_sm;
+
+};
+
+/* STFM1000 Black Box Filter Function prototypes */
+void stfm1000_filter_reset(struct stfm1000_filter_parms *sdf);
+void stfm1000_filter_decode(struct stfm1000_filter_parms *sdf, s16 Lpr,
+	s16 Lmr, u16 Rssi);
+
+static inline s16
+stfm1000_filter_value_left(struct stfm1000_filter_parms *sdf)
+{
+	return sdf->Left;
+}
+
+static inline s16
+stfm1000_filter_value_right(struct stfm1000_filter_parms *sdf)
+{
+	return sdf->Right;
+}
+
+static inline u32
+stfm1000_filter_value_rssi(struct stfm1000_filter_parms *sdf)
+{
+	return sdf->RssiDecoded;
+}
+
+#endif
diff --git a/drivers/media/radio/stfm1000/stfm1000-i2c.c b/drivers/media/radio/stfm1000/stfm1000-i2c.c
new file mode 100644
index 000000000000..5ddeb3a5fd3a
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-i2c.c
@@ -0,0 +1,452 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/io.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+
+#include <linux/version.h>      /* for KERNEL_VERSION MACRO     */
+
+#include "stfm1000.h"
+
+#define stfm1000_i2c_debug(p, fmt, arg...) \
+	do { \
+		if ((p)->dbgflg & STFM1000_DBGFLG_I2C) \
+			printk(KERN_INFO "stfm1000: " fmt, ##arg); \
+	} while (0)
+
+static const char *reg_names[STFM1000_NUM_REGS] = {
+#undef REGNAME
+#define REGNAME(x) \
+	[STFM1000_ ## x / 4] = #x ""
+
+	REGNAME(TUNE1),
+	REGNAME(SDNOMINAL),
+	REGNAME(PILOTTRACKING),
+	REGNAME(INITIALIZATION1),
+	REGNAME(INITIALIZATION2),
+	REGNAME(INITIALIZATION3),
+	REGNAME(INITIALIZATION4),
+	REGNAME(INITIALIZATION5),
+	REGNAME(INITIALIZATION6),
+	REGNAME(REF),
+	REGNAME(LNA),
+	REGNAME(MIXFILT),
+	REGNAME(CLK1),
+	REGNAME(CLK2),
+	REGNAME(ADC),
+	REGNAME(AGC_CONTROL1),
+	REGNAME(AGC_CONTROL2),
+	REGNAME(DATAPATH),
+	REGNAME(RMS),
+	REGNAME(AGC_STAT),
+	REGNAME(SIGNALQUALITY),
+	REGNAME(DCEST),
+	REGNAME(RSSI_TONE),
+	REGNAME(PILOTCORRECTION),
+	REGNAME(ATTENTION),
+	REGNAME(CLK3),
+	REGNAME(CHIPID),
+#undef REGNAME
+};
+
+static const int stfm1000_rw_regs[] = {
+	STFM1000_TUNE1,
+	STFM1000_SDNOMINAL,
+	STFM1000_PILOTTRACKING,
+	STFM1000_INITIALIZATION1,
+	STFM1000_INITIALIZATION2,
+	STFM1000_INITIALIZATION3,
+	STFM1000_INITIALIZATION4,
+	STFM1000_INITIALIZATION5,
+	STFM1000_INITIALIZATION6,
+	STFM1000_REF,
+	STFM1000_LNA,
+	STFM1000_MIXFILT,
+	STFM1000_CLK1,
+	STFM1000_CLK2,
+	STFM1000_ADC,
+	STFM1000_AGC_CONTROL1,
+	STFM1000_AGC_CONTROL2,
+	STFM1000_DATAPATH,
+	STFM1000_ATTENTION,	/* it's both WR/RD */
+};
+
+static const int stfm1000_ra_regs[] = {
+	STFM1000_RMS,
+	STFM1000_AGC_STAT,
+	STFM1000_SIGNALQUALITY,
+	STFM1000_DCEST,
+	STFM1000_RSSI_TONE,
+	STFM1000_PILOTCORRECTION,
+	STFM1000_ATTENTION,	/* it's both WR/RD - always read */
+	STFM1000_CLK3,
+	STFM1000_CHIPID
+};
+
+static int verify_writes;
+
+void stfm1000_setup_reg_set(struct stfm1000 *stfm1000)
+{
+	int i, reg;
+
+	/* set up register sets (read/write) */
+	for (i = 0; i < ARRAY_SIZE(stfm1000_rw_regs); i++) {
+		reg = stfm1000_rw_regs[i] / 4;
+		stfm1000->reg_rw_set[reg / 32] |= 1U << (reg & 31);
+		/* printk(KERN_INFO "STFM1000: rw <= %d\n", reg); */
+	}
+
+	/* for (i = 0; i < ARRAY_SIZE(stfm1000->reg_rw_set); i++)
+		printk("RW[%d] = 0x%08x\n", i, stfm1000->reg_rw_set[i]); */
+
+	/* set up register sets (read only) */
+	for (i = 0; i < ARRAY_SIZE(stfm1000_ra_regs); i++) {
+		reg = stfm1000_ra_regs[i] / 4;
+		stfm1000->reg_ra_set[reg / 32] |= 1U << (reg & 31);
+		/* printk(KERN_INFO "STFM1000: rw <= %d\n", reg); */
+	}
+	/* for (i = 0; i < ARRAY_SIZE(stfm1000->reg_ra_set); i++)
+		printk("RO[%d] = 0x%08x\n", i, stfm1000->reg_ra_set[i]); */
+
+	/* clear dirty */
+	memset(stfm1000->reg_dirty_set, 0, sizeof(stfm1000->reg_dirty_set));
+}
+
+static int stfm1000_reg_is_rw(struct stfm1000 *stfm1000, int reg)
+{
+	reg >>= 2;
+	return !!(stfm1000->reg_rw_set[reg / 32] & (1 << (reg & 31)));
+}
+
+static int stfm1000_reg_is_ra(struct stfm1000 *stfm1000, int reg)
+{
+	reg >>= 2;
+	return !!(stfm1000->reg_ra_set[reg / 32] & (1 << (reg & 31)));
+}
+
+static int stfm1000_reg_is_dirty(struct stfm1000 *stfm1000, int reg)
+{
+	reg >>= 2;
+	return !!(stfm1000->reg_dirty_set[reg / 32] & (1 << (reg & 31)));
+}
+
+static void stfm1000_reg_set_dirty(struct stfm1000 *stfm1000, int reg)
+{
+	reg >>= 2;
+	stfm1000->reg_dirty_set[reg / 32] |= 1 << (reg & 31);
+}
+
+static inline int stfm1000_reg_is_writeable(struct stfm1000 *stfm1000, int reg)
+{
+	return stfm1000_reg_is_rw(stfm1000, reg);
+}
+
+static inline int stfm1000_reg_is_readable(struct stfm1000 *stfm1000, int reg)
+{
+	return stfm1000_reg_is_rw(stfm1000, reg) ||
+	       stfm1000_reg_is_ra(stfm1000, reg);
+}
+
+/********************************************************/
+
+static int write_reg_internal(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	u8 values[5];
+	int ret;
+
+	stfm1000_i2c_debug(stfm1000, "%s(%s - 0x%02x, 0x%08x)\n", __func__,
+		reg_names[reg / 4], reg, value);
+
+	values[0] = (u8)reg;
+	values[1] = (u8)value;
+	values[2] = (u8)(value >> 8);
+	values[3] = (u8)(value >> 16);
+	values[4] = (u8)(value >> 24);
+	ret = i2c_master_send(stfm1000->client, values, 5);
+	if (ret < 0)
+		return ret;
+	return 0;
+}
+
+static int read_reg_internal(struct stfm1000 *stfm1000, int reg, u32 *value)
+{
+	u8 regb = reg;
+	u8 values[4];
+	int ret;
+
+	ret = i2c_master_send(stfm1000->client, &regb, 1);
+	if (ret < 0)
+		goto out;
+	ret = i2c_master_recv(stfm1000->client, values, 4);
+	if (ret < 0)
+		goto out;
+	*value = (u32)values[0] | ((u32)values[1] << 8) |
+		 ((u32)values[2] << 16) | ((u32)values[3] << 24);
+	ret = 0;
+
+	stfm1000_i2c_debug(stfm1000, "%s(%s - 0x%02x, 0x%08x)\n", __func__,
+		reg_names[reg / 4], reg, *value);
+out:
+	return ret;
+}
+
+int stfm1000_raw_write(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	int ret;
+
+	mutex_lock(&stfm1000->xfer_lock);
+	ret = write_reg_internal(stfm1000, reg, value);
+	mutex_unlock(&stfm1000->xfer_lock);
+
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	return ret;
+}
+
+int stfm1000_raw_read(struct stfm1000 *stfm1000, int reg, u32 *value)
+{
+	int ret;
+
+	mutex_lock(&stfm1000->xfer_lock);
+	ret = read_reg_internal(stfm1000, reg, value);
+	mutex_unlock(&stfm1000->xfer_lock);
+
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	return ret;
+}
+
+static inline void stfm1000_set_shadow_reg(struct stfm1000 *stfm1000,
+	int reg, u32 val)
+{
+	stfm1000->shadow_regs[reg / 4] = val;
+}
+
+static inline u32 stfm1000_get_shadow_reg(struct stfm1000 *stfm1000, int reg)
+{
+	return stfm1000->shadow_regs[reg / 4];
+}
+
+int stfm1000_write(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	int ret;
+
+	if (!stfm1000_reg_is_writeable(stfm1000, reg)) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	mutex_lock(&stfm1000->xfer_lock);
+
+	/* same value as last one written? */
+	if (stfm1000_reg_is_dirty(stfm1000, reg) &&
+		stfm1000_get_shadow_reg(stfm1000, reg) == value) {
+		ret = 0;
+
+		stfm1000_i2c_debug(stfm1000, "%s - HIT "
+			"(%s - 0x%02x, 0x%08x)\n", __func__,
+			reg_names[reg / 4], reg, value);
+
+		goto out_unlock;
+	}
+
+	/* actually write the register */
+	ret = write_reg_internal(stfm1000, reg, value);
+	if (ret < 0)
+		goto out_unlock;
+
+	/* update shadow register & mark it as dirty */
+	/* only if register is not read always */
+	if (!stfm1000_reg_is_ra(stfm1000, reg)) {
+		stfm1000_set_shadow_reg(stfm1000, reg, value);
+		stfm1000_reg_set_dirty(stfm1000, reg);
+	}
+
+out_unlock:
+	mutex_unlock(&stfm1000->xfer_lock);
+
+out:
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	if (verify_writes) {
+		u32 value2 = ~0;
+
+		stfm1000_raw_read(stfm1000, reg, &value2);
+
+		stfm1000_i2c_debug(stfm1000, "%s - VER "
+			"(%s - 0x%02x, W=0x%08x V=0x%08x) %s\n", __func__,
+			reg_names[reg / 4], reg, value, value2,
+			value == value2 ? "OK" : "** differs **");
+	}
+
+	return ret;
+}
+
+int stfm1000_read(struct stfm1000 *stfm1000, int reg, u32 *value)
+{
+	int ret = 0;
+
+	if (!stfm1000_reg_is_readable(stfm1000, reg)) {
+		ret = -EINVAL;
+		printk(KERN_INFO "%s: !readable(%d)\n", __func__, reg);
+		goto out;
+	}
+
+	mutex_lock(&stfm1000->xfer_lock);
+
+	/* if the register can be written & is dirty, use the shadow */
+	if (stfm1000_reg_is_writeable(stfm1000, reg) &&
+		stfm1000_reg_is_dirty(stfm1000, reg)) {
+
+		*value = stfm1000_get_shadow_reg(stfm1000, reg);
+		ret = 0;
+
+		stfm1000_i2c_debug(stfm1000, "%s - HIT "
+			"(%s - 0x%02x, 0x%08x)\n", __func__,
+			reg_names[reg / 4], reg, *value);
+
+		goto out_unlock;
+	}
+
+	/* register must be read */
+	ret = read_reg_internal(stfm1000, reg, value);
+	if (ret < 0)
+		goto out;
+
+	/* if the register is writeable, update shadow */
+	if (stfm1000_reg_is_writeable(stfm1000, reg)) {
+		stfm1000_set_shadow_reg(stfm1000, reg, *value);
+		stfm1000_reg_set_dirty(stfm1000, reg);
+	}
+
+out_unlock:
+	mutex_unlock(&stfm1000->xfer_lock);
+
+out:
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	return ret;
+}
+
+int stfm1000_write_masked(struct stfm1000 *stfm1000, int reg, u32 value,
+	u32 mask)
+{
+	int ret = 0;
+	u32 old_value;
+
+	if (!stfm1000_reg_is_writeable(stfm1000, reg)) {
+		ret = -EINVAL;
+		printk(KERN_ERR "%s: !writeable(%d)\n", __func__, reg);
+		goto out;
+	}
+
+	mutex_lock(&stfm1000->xfer_lock);
+
+	/* if the register wasn't written before, read it */
+	if (!stfm1000_reg_is_dirty(stfm1000, reg)) {
+		ret = read_reg_internal(stfm1000, reg, &old_value);
+		if (ret != 0)
+			goto out_unlock;
+	} else	/* register was written, use the last value */
+		old_value = stfm1000_get_shadow_reg(stfm1000, reg);
+
+	/* perform masking */
+	value = (old_value & ~mask) | (value & mask);
+
+	/* if we write the same value, don't bother */
+	if (stfm1000_reg_is_dirty(stfm1000, reg) && value == old_value) {
+		ret = 0;
+
+		stfm1000_i2c_debug(stfm1000, "%s - HIT "
+			"(%s - 0x%02x, 0x%08x)\n", __func__,
+			reg_names[reg / 4], reg, value);
+
+		goto out_unlock;
+	}
+
+	/* actually write the register to the chip */
+	ret = write_reg_internal(stfm1000, reg, value);
+	if (ret < 0)
+		goto out_unlock;
+
+	/* if no error, update the shadow register and mark it as dirty */
+	stfm1000_set_shadow_reg(stfm1000, reg, value);
+	stfm1000_reg_set_dirty(stfm1000, reg);
+
+out_unlock:
+	mutex_unlock(&stfm1000->xfer_lock);
+
+out:
+	if (ret < 0)
+		dev_err(&stfm1000->client->dev, "%s: failed", __func__);
+
+	if (verify_writes) {
+		u32 value2 = ~0;
+
+		stfm1000_raw_read(stfm1000, reg, &value2);
+
+		stfm1000_i2c_debug(stfm1000, "%s - VER "
+			"(%s - 0x%02x, W=0x%08x V=0x%08x) %s\n", __func__,
+			reg_names[reg / 4], reg, value, value2,
+			value == value2 ? "OK" : "** differs **");
+	}
+
+	return ret;
+}
+
+int stfm1000_set_bits(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	return stfm1000_write_masked(stfm1000, reg, value, value);
+}
+
+int stfm1000_clear_bits(struct stfm1000 *stfm1000, int reg, u32 value)
+{
+	return stfm1000_write_masked(stfm1000, reg, ~value, value);
+}
+
+int stfm1000_write_regs(struct stfm1000 *stfm1000,
+	const struct stfm1000_reg *reg)
+{
+	int ret;
+
+	for (; reg && reg->regno != STFM1000_REG_END; reg++) {
+
+		if (reg->regno == STFM1000_REG_DELAY) {
+			msleep(reg->value);
+			continue;
+		}
+
+		if (reg->regno & STFM1000_REG_SET_BITS_MASK)
+			ret = stfm1000_set_bits(stfm1000, reg->regno & 0xff,
+				reg->value);
+		else if (reg->regno & STFM1000_REG_CLEAR_BITS_MASK)
+			ret = stfm1000_clear_bits(stfm1000, reg->regno & 0xff,
+				reg->value);
+		else
+			ret = stfm1000_write(stfm1000, reg->regno, reg->value);
+
+		if (ret != 0) {
+			printk(KERN_ERR "%s: failed to write reg 0x%x "
+				"with 0x%08x\n",
+				__func__, reg->regno, reg->value);
+			return ret;
+		}
+	}
+	return 0;
+}
diff --git a/drivers/media/radio/stfm1000/stfm1000-rds.c b/drivers/media/radio/stfm1000/stfm1000-rds.c
new file mode 100644
index 000000000000..5f63052d00c2
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-rds.c
@@ -0,0 +1,1529 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/init.h>
+
+#include "stfm1000.h"
+
+#include "stfm1000-rds.h"
+
+#define bitstream_to_rds_state(b)	\
+	container_of(b, struct stfm1000_rds_state, bitstream)
+#define demod_to_rds_state(d)		\
+	container_of(d, struct stfm1000_rds_state, demod)
+#define pkt_to_rds_state(p)		\
+	container_of(p, struct stfm1000_rds_state, pkt)
+#define text_to_rds_state(t)		\
+	container_of(t, struct stfm1000_rds_state, text)
+#define rds_state_to_stfm1000(r)	\
+	container_of(r, struct stfm1000, rds_state)
+
+#define TADJSH 8 /*Shifts used in bitslice loop filter */
+
+/* Reverse of Matlab's Fquant (see MatchedFilterDecomposition.m), so that */
+/* mixandsum code is easy;   Used by rds_bitstream_stfmdemod.arm */
+const s16 u16_rds_basis[2*RDS_BASISLENGTH+8] = {
+	14, 24, 34, 43, 50, 56, 60, 62, 62,
+	60, 55, 49, 41, 32, 22, 11, 14, 24,
+	34, 43, 50, 56, 60, 62, 62, 60, 55,
+	49, 41, 32, 22, 11, 14, 24, 34, 43,
+	50, 56, 60, 62
+};
+
+static int bits_free(struct stfm1000_rds_bitstream *rdsb)
+{
+	/* Do not show the last one word free. */
+	int FreeSpace = rdsb->TailBitCount - rdsb->HeadBitCount - 32;
+
+	if (FreeSpace < 0)
+		FreeSpace = (RDS_BITBUFSIZE * 32) + FreeSpace;
+	return FreeSpace;
+}
+
+static void put1bit(struct stfm1000_rds_bitstream *rdsb, int bit)
+{
+	int index = (rdsb->HeadBitCount >> 5);
+	u32 CurBit = (rdsb->HeadBitCount & 0x1f);
+	u32 CurWord = rdsb->buf[index];
+
+	if (CurBit == 0)
+		CurWord = 0;
+
+	CurWord = CurWord | (((u32)bit & 1) << CurBit);
+	rdsb->buf[index] = CurWord;
+	rdsb->HeadBitCount++;
+	if (rdsb->HeadBitCount >= RDS_BITBUFSIZE * 32)
+		rdsb->HeadBitCount = 0;
+}
+
+static int get1bit(struct stfm1000_rds_bitstream *rdsb)
+{
+	int Bit = 0;
+	int index = (rdsb->TailBitCount >> 5);
+	int CurBit = (rdsb->TailBitCount & 0x1f);
+	u32 CurWord = rdsb->buf[index];
+
+	Bit = (CurWord >> CurBit) & 1;
+	rdsb->TailBitCount++;
+	if (rdsb->TailBitCount == RDS_BITBUFSIZE*32)
+		rdsb->TailBitCount = 0;
+
+	return Bit;
+}
+
+static int bits_filled(struct stfm1000_rds_bitstream *rdsb)
+{
+	int FilledSpace = rdsb->HeadBitCount - rdsb->TailBitCount;
+
+	if (FilledSpace < 0)
+		FilledSpace = (RDS_BITBUFSIZE * 32) + FilledSpace;
+	return FilledSpace;
+}
+
+static void rds_mix_msg(struct stfm1000_rds_demod *rdsd, u8 MixSetting)
+{
+	if (rdsd->mix_msg_pending)
+		rdsd->mix_msg_overrun++;
+	rdsd->mix_msg = MixSetting;
+	rdsd->mix_msg_pending = 1;
+
+	/* signal monitor thread */
+	stfm1000_monitor_signal(
+		rds_state_to_stfm1000(demod_to_rds_state(rdsd)),
+		EVENT_RDS_MIXFILT);
+}
+
+/* call with interrupts disabled please */
+int stfm1000_rds_mix_msg_get(struct stfm1000_rds_state *rds)
+{
+	struct stfm1000_rds_demod *rdsd = &rds->demod;
+
+	if (!rdsd->mix_msg_pending)
+		return -1;
+
+	return rdsd->mix_msg;
+}
+
+/* call with interrupts disabled please */
+int stfm1000_rds_mix_msg_processed(struct stfm1000_rds_state *rds, int mix_msg)
+{
+	struct stfm1000_rds_demod *rdsd = &rds->demod;
+
+	if (!rdsd->mix_msg_pending)
+		return -1;
+
+	rdsd->mix_msg_pending = 0;
+
+	/* update the completion indication bit */
+	if ((mix_msg & 0x8) == 0)
+		rdsd->MixPopDone = 1;
+
+	/* this is reflected off the hardware register */
+	rdsd->rds_mix_offset = mix_msg & 1;
+
+	if (rdsd->mix_msg != mix_msg) {
+		rdsd->mix_msg_processed_changed++;
+		return -1;
+	}
+	return 0;
+}
+
+static void rds_sdnominal_msg(struct stfm1000_rds_demod *rdsd, int sdnominal)
+{
+	if (rdsd->sdnominal_msg_pending)
+		rdsd->sdnominal_msg_overrun++;
+	rdsd->sdnominal_msg = sdnominal;
+	rdsd->sdnominal_msg_pending = 1;
+
+	/* signal monitor thread */
+	stfm1000_monitor_signal(
+		rds_state_to_stfm1000(demod_to_rds_state(rdsd)),
+		EVENT_RDS_SDNOMINAL);
+}
+
+/* call with interrupts disabled please */
+int stfm1000_rds_sdnominal_msg_get(struct stfm1000_rds_state *rds)
+{
+	struct stfm1000_rds_demod *rdsd = &rds->demod;
+
+	if (!rdsd->sdnominal_msg_pending)
+		return 0;
+
+	return rdsd->sdnominal_msg;
+}
+
+/* call with interrupts disabled please */
+int stfm1000_rds_sdnominal_msg_processed(struct stfm1000_rds_state *rds,
+	int sdnominal_msg)
+{
+	struct stfm1000_rds_demod *rdsd = &rds->demod;
+
+	if (!rdsd->sdnominal_msg_pending)
+		return -1;
+
+	rdsd->sdnominal_msg_pending = 0;
+	return 0;
+}
+
+void demod_loop(struct stfm1000_rds_bitstream *rdsb,
+	struct stfm1000_rds_demod *rdsd)
+{
+	s32 filter_out;
+	u32 freeSpace;
+	s32 decomp_hist_pp;
+	u8 phase;
+
+	/* Check if we're at a half-basis point */
+	if ((rdsd->i & (RDS_BASISLENGTH/2 - 1)) != 0)
+		return;	/* Nope, return */
+
+	/* Yes, time to do our work */
+	/* Rotate the length 3 history buffer */
+	decomp_hist_pp = rdsd->decomp_hist_p;
+	rdsd->decomp_hist_p = rdsd->decomp_hist;
+	if ((rdsd->i & (RDS_BASISLENGTH-1)) == 0) {
+		rdsd->decomp_hist = rdsd->mixandsum1>>9; /* Grab output of
+							  * mixandsum1/512 */
+		rdsd->mixandsum1 = 0;	 /* Reset mixandsum #1 */
+	} else {
+		rdsd->decomp_hist = rdsd->mixandsum2>>9; /*Grab output of
+							  * mixandsum2/512 */
+		rdsd->mixandsum2 = 0;	/* Reset mixandsum #2 */
+	}
+
+	/* Form correlator/decimator output by convolving with the
+	 * decomposition coefficients, DecompQuant from Matlab work. */
+	filter_out = (-58*rdsd->decomp_hist + 59*decomp_hist_pp)>>7;
+
+	/*Figure out which half-basis we are in (out of a bit-length cycle) */
+	phase = rdsd->i*2/RDS_BASISLENGTH;
+	/*Now what we do depends on the phase variable */
+	/*Phase 0:      Bitslice and do timing alignment */
+	/*others (1-3): Keep value for timing alignment */
+
+	if (phase == 0) {   /*Main processing (bitslice) */
+		u32 Ph;
+		u8 OldBit = rdsd->sliced_data;	/* Save the previous value */
+
+		rdsd->return_num = 1;
+		if (filter_out >= 0) { /*This bit is "1" */
+			/*return value is XOR of previous bit (still in
+			 * sliced_data) w/ this */
+			/*  bit (1), which equals (NOT of the previous bit) */
+			rdsd->return_rdsdemod = !OldBit;
+			rdsd->sliced_data = 1;	/*Newest bit value is 1 */
+		} else { /*This bit is "0" */
+			/*return value is XOR of previous bit (still in
+			 * sliced_data) w/ this */
+			/*  bit (0), which equals the previous bit */
+			rdsd->return_rdsdemod = OldBit;
+			rdsd->sliced_data = 0;	/*Newest bit value is 0 */
+		}
+
+		freeSpace = bits_free(rdsb);
+
+		if (freeSpace > 0)
+			put1bit(rdsb, rdsd->return_rdsdemod);
+		else
+			rdsd->RdsDemodSkippedBitCnt++;
+
+		/*Increment bits received counter */
+		rdsd->BitAlignmentCounter++;
+		/*If mixer phase determination hasn't been done, start it */
+		if ((rdsd->MixPhaseState == 0) && (!rdsd->MixPhaseDetInProg)) {
+			rdsd->MixPhaseDetInProg = 1;
+			/*Go to first mixer setting (0) */
+			rds_mix_msg(rdsd, 0);
+		}
+
+		/* Do bit-slicing time adaption after the mixer phase
+		 * determination */
+		if (!(rdsd->MixPhaseDetInProg) && !(rdsd->Synchronous)) {
+
+			/* Bitslice Timing Adjust Code (runs after
+			 * MixPhaseDetInProg and if RDS is not synchronous to
+			 * the FM pilot. */
+
+			u8 BigPh2;	/* Expecting a large value in
+					 * PhaseValue[2] */
+			u32 MaxRMS = 0;	/*Largest phase RMS */
+			s8 MaxPh = 0;	/*Index of largest phase RMS */
+			s32 zerocross;
+
+			/* Locate the largest phase RMS
+			 * (should be at phase zero) */
+			for (Ph = 0; Ph < 4; Ph++)
+				if (rdsd->Ph_RMS[Ph] > MaxRMS) {
+					MaxRMS = rdsd->Ph_RMS[Ph];
+					MaxPh = Ph;
+				}
+
+			/* During each bit time we expect the four phases to
+			 * take one of the following patterns, where 1
+			 * corresponds to maximum modulation:
+			 *   1,    0,  -1,    0			Case I
+			 *  -1,    0,   1,    0			Case II
+			 *   1,  1/2,   0, -1/2			Case III
+			 *  -1, -1/2,   0,  1/2			Case IV
+			 * We need to distinguish between cases in order to do
+			 * the timing adjustment. Below we compare the
+			 * correlation of the samples with Case I and Case III
+			 * to see which has a bigger abs(correlation).  Thus
+			 * BigPh2, if set, means that we decided on Case I or
+			 * Case II; if BigPh2 clear, we decided Case III or IV.
+			 */
+			BigPh2 = abs(rdsd->PhaseValue[0]-rdsd->PhaseValue[2]) >
+				 abs(rdsd->PhaseValue[0] +
+				 ((rdsd->PhaseValue[1]-
+					rdsd->PhaseValue[3])>>1));
+			/* If BigPh2, use the difference between phase 1 value
+			 * (downgoing for Case I, upgoing for Case II) and
+			 * phase 3 value (upgoing for Case I, downgoing for
+			 * Case II, thus the subtraction) to indicate timing
+			 * error.  If not BigPh2, use the sum of the phase 1
+			 * value (downgoing for Case III, upgoing for Case IV)
+			 * and phase 3 value (downgoing for Case III, upgoing
+			 * for Case IV, thus the addition) to indicate timing
+			 * error.  If BigPh2, the slopes at phase 1 & phase 3
+			 * are approximately double that if not BigPh2.
+			 * Since we are trying to measure timing, scale
+			 * by 1/2 in the BigPh2 case. */
+			if (BigPh2)
+				zerocross = (rdsd->PhaseValue[1]-
+					rdsd->PhaseValue[3])>>1;
+			else
+				zerocross = rdsd->PhaseValue[1]+
+					rdsd->PhaseValue[3];
+			/* Now if the prev bit was a "1", then the first zero
+			 * crossing (phase 1 if BigPh2, phase 2 if !BigPh2)
+			 * was a falling one, and if we were late then
+			 * zerocross should be negative. If the prev bit was a
+			 * "0", then the first zero crossing was a rising one,
+			 * and if we were late then zerocross would be
+			 * positive. If we are "late" it means that we need to
+			 * do a shorter cycle of, say, 15 samples instead of
+			 * 16, to "catch up" so that in the future we will be
+			 * sampling earlier.  We shorten the cycle by adding
+			 * to i, so "late" is going to mean "increment i".
+			 * Therefore "late" should be positive, which is done
+			 * here by inverting zerocross if the previous bit was
+			 * 1.  You could say that this step reflects cases I
+			 * and III into II and IV, respectively. */
+			if (OldBit)
+				zerocross = -zerocross;
+			if (!rdsd->DisablePushing) {
+				/*The algorithm so far has a stable operating
+				 * point 17 phases away from the correct one.
+				 * The following code is experimental and may
+				 * be deleterious in low SNR conditions, but is
+				 * an attempt to move off of the incorrect
+				 * operating point. */
+
+				if (MaxPh != 0) {
+					/* If it isn't the same MaxPh as the
+					 * last non-zero one, clear the counter
+					 */
+					if (MaxPh != rdsd->PushLastMaxPh) {
+						/*Reset the counter */
+						rdsd->PushCounter = 0;
+						/*Record which phase we're now
+						 * counting */
+						rdsd->PushLastMaxPh = MaxPh;
+					}
+					/* If the Max RMS is on the same
+					 * non-zero phase, count up */
+					rdsd->PushCounter++;
+				}
+				/* Once every 128 bits, check and then reset
+				 * PushCounter */
+				if (!(rdsd->BitAlignmentCounter & 0x0FF)) {
+					/*If 90% of the time the max phase has
+					 * been from the same non-zero phase,
+					 * decide that we are latched onto a 0
+					 * lock point. Do a large push of the
+					 * timing. */
+					if (rdsd->PushCounter > 230) {
+						s32 pshiph;
+						/*Convert from phase number to
+						 * the number of filter
+						 *  output samples that we need
+						 *  to shift */
+						if (rdsd->PushLastMaxPh >= 2)
+							pshiph =
+								4 - (s8)rdsd->
+								PushLastMaxPh;
+						else
+							pshiph =
+								-(s8)rdsd->
+								PushLastMaxPh;
+						/* Scale by the number of i-
+						 * phases per output sample */
+						pshiph <<=
+							RDS_BASISSHIFTS-1;
+						/* Perform big pop to get near
+						 * correct timing */
+						rdsd->i += (RDS_BASISLENGTH<<1)
+							+ pshiph;
+						/* Set status indicating big
+						 * pop was needed.  Reset all
+						 * leaky-bucket and summation
+						 * variables because the big
+						 * timing shift has invalidated
+						 * them. Ph_RMS values don't
+						 * need to be reset because
+						 * they will shift over to
+						 * reasonable values again
+						 * before their erroneous
+						 * values could have effect. */
+						rdsd->rds_big_timeshift = 1;
+						/*rdsd->Ph_RMS[0] = 0; */
+						/*rdsd->Ph_RMS[1] = 0; */
+						/*rdsd->Ph_RMS[2] = 0; */
+						/*rdsd->Ph_RMS[3] = 0; */
+						rdsd->mixandsum1 = 0;
+						rdsd->mixandsum2 = 0;
+						rdsd->SkipsAccum +=
+							pshiph;
+
+						/* Make adjustments in other
+						 * values because of the push
+						 * (they wouldn't otherwise be
+						 * able to use the information
+						 * that a push was needed in
+						 * their future control
+						 * decisions). */
+						if (rdsd->PushLastMaxPh != 2) {
+							/* If we weren't
+							 * pushing from phase
+							 * two, accumulate (for
+							 * use in adapting
+							 * SDNOMINAL) the
+							 * phases moved by
+							 * pushing. Phase two
+							 * pushes are not used;
+							 * the push direction
+							 * is arbitrary since
+							 * Phase 2 is 180
+							 * degrees out.  Also,
+							 * phase 2 pushes don't
+							 * result from
+							 * reasonable slippage.
+							 * */
+
+							if (rdsd->sdnom_adapt)
+								rdsd->SdnomSk
+								+= pshiph;
+
+							/* Modify timing_adj to
+							 * account for half of
+							 * the DC response that
+							 * would have occurred
+							 * in timing_adj if
+							 * that control loop
+							 * had seen the push
+							 * happen. (Why half?
+							 * Because the loop has
+							 * already seen a
+							 * history of zerocross
+							 * values that heads it
+							 * in the same
+							 * direction as this
+							 * adjustment, but may
+							 * have seen as few as
+							 * half of what it
+							 * should have.) */
+							rdsd->timing_adj +=
+								pshiph <<
+								(TADJSH+1);
+						}
+						/*Set countdown timer that will
+						 * prevent any mixer popping
+						 * until the Ph_RMS variables
+						 * have had enough time to
+						 * stabilize */
+
+						/* 2.5 time constants */
+						rdsd->PushSafetyZone = 5;
+					}
+					/*Reset the push counter */
+					rdsd->PushCounter = 0;
+				}  /*end once every 128 bits */
+			}  /*end if !DisablePushing */
+
+			/* Further possible additions:
+			 *
+			 * 1. Pushes modify timing_adj to decrease convergence
+			 *    time.
+			 * 2. Separate timing_adj into pilottracking and non-pt
+			 *    cases (avoids convergence time after stereo/mono
+			 *    transitions)
+			 *
+			 * Old loop filter was a leaky bucket integrator, and
+			 * it always lagged behind if the FM station had RDS
+			 * asynchronous to the pilot, because the control loop
+			 * needs another integrator to converge on a frequency
+			 * error.
+			 * New loop filter = 1/(1-1/z) * (a-1/z) * k,
+			 * where a = 1+1/256 and k = 1/1024.
+			 * You can narrow the loop bandwidth by making "a"
+			 * twice as close to 1 and halving k, e.g. a = 1+1/512
+			 * and k = 1/2048.
+			 * (The way implemented, that narrowing loop BW by
+			 * a factor of 2 can be done by incrementing TADJSH.)
+			 *
+			 * TGR 8/31/2007 */
+
+			/*Integrator, 1/(1-1/z) */
+			rdsd->timing_adj += zerocross;
+			/*Limit to 1 phase every 8 samples */
+			if (rdsd->SkipSafetyZone) {
+				rdsd->SkipSafetyZone--;
+				rdsd->sampskip = 0;
+			} else {
+				/*sampskip of non-zero is allowed,
+				 * calculate what it really is */
+
+				/*Saturate timing_adj to 2's comp
+				 * (2*TADJSH+4)-bit range. */
+				if (rdsd->timing_adj > (1<<(2*TADJSH+3))-1)
+					rdsd->timing_adj = (1<<(2*TADJSH+3))-1;
+				if (rdsd->timing_adj < -(1<<(2*TADJSH+3)))
+					rdsd->timing_adj = -(1<<(2*TADJSH+3));
+
+				/* Zero, implemented after the integrator
+				 * output.
+				 * (a-1/z) = (1+1/256) - 1/z = (1-1/z) + 1/256.
+				 * But (1 - 1/z) is timing_adj-
+				 * prev_timing_adj = zerocross. */
+				rdsd->sampskip = zerocross	/* 1 - 1/z */
+					/* 1/256 (with rounding) */
+					+ ((rdsd->timing_adj
+						+ (1<<(TADJSH-1)))>>TADJSH);
+				/*Round and apply k */
+				rdsd->sampskip += (1<<(TADJSH+1));
+				rdsd->sampskip >>= (TADJSH+2);
+				/*Limit to [-1,+1] inclusive */
+				if (rdsd->sampskip > 1)
+					rdsd->sampskip = 1;
+				if (rdsd->sampskip < -1)
+					rdsd->sampskip = -1;
+				/* If non-zero, start the skip safety zone,
+				 * which excludes more sample skipping for a
+				 * while.  Note that the safety zone only
+				 * applies to the skips -- pushes can still
+				 * happen inside a SkipSafetyZone. */
+				if (rdsd->sampskip)
+					rdsd->SkipSafetyZone = 8-1;
+			}
+			/**********************************************
+			* End Timing Adjust Code
+			**********************************************/
+
+			/**********************************************
+			* Begin Phase Popper Code
+			**********************************************/
+			/* If Phase Popping is enabled and 1/2 of a
+			 * time constant has gone by... */
+			if (rdsd->PhasePoppingEnabled &&
+				!(rdsd->BitAlignmentCounter &
+					((1<<(RMSALPHASHIFTS-1))-1))) {
+
+				u8 ForcePop = 0; /* Used to force a pop */
+
+				/*Record the maximum of the envelope */
+				if (MaxRMS > rdsd->PhasePopMaxRMS)
+					rdsd->PhasePopMaxRMS = MaxRMS;
+				/* Also track MaxRMS into MixPhase0/1Mag, so
+				 * that we can see what the largest RMS on each
+				 * of those phases is.  On synchronous stations
+				 * (meaning the RDS carrier and bit rate are
+				 * synchronized with the pilot), the right mix
+				 * phase will always be big and the wrong phase
+				 * small. On asynchronous stations (and
+				 * stations without RDS), both phases will at
+				 * some time or other have about the
+				 * same amplitude on each of the phases. */
+				if (rdsd->rds_mix_offset) {
+					if (MaxRMS > rdsd->MixPhase1Mag)
+						rdsd->MixPhase1Mag = MaxRMS;
+				} else {
+					if (MaxRMS > rdsd->MixPhase0Mag)
+						rdsd->MixPhase0Mag = MaxRMS;
+				}
+				/* Update PopSafetyZone and PushSafetyZone
+				 * counters.  With RMSALPHASHIFTS = 5, each
+				 * tick is 16/1187.5 =~ 13.5 ms. */
+				if (rdsd->PopSafetyZone) {
+					rdsd->PopSafetyZone--;
+					/* If safety zone just ended and this
+					 * mix phase is giving smaller RMS than
+					 * before the pop, then the pop was a
+					 * mistake.  Go back to previous mixer
+					 * phase */
+					if (!(rdsd->PopSafetyZone)
+						&& (rdsd->PhasePopMaxRMS <
+							rdsd->PrePopRMS))
+						ForcePop = 1;
+				}
+				/* If there is no recent push, and Phase 0 has
+				 * the maximum RMS, and at least 1/7th of a
+				 * second has passed since the last phase pop,
+				 * and ((the RMS is less than 1/2 of
+				 * PhasePopMaxRMS) or (the RMS is less than
+				 * 100)), then try a phase pop. */
+				if (/* (rdsd->Ph_RMS[0] == MaxRMS) &&
+						* Phase 0 has maximum RMS  */
+					!(rdsd->PopSafetyZone))	{
+					 /* and Long enough since last
+					  * phase pop */
+
+					/* Eligible for a pop, see if one of
+					 * the pop conditions is met */
+					if ((MaxRMS<<1) <
+							rdsd->PhasePopMaxRMS) {
+						/*RMS decline from its peak */
+						ForcePop = 1;
+					} else if ((MaxRMS>>RMSALPHASHIFTS)
+						< 50) {
+						/*RMS too small to receive,
+						 * either there's no RDS or
+						 * this is the wrong phase */
+						ForcePop = 1;
+					}
+				}
+				if (ForcePop) {
+
+					/*Pop to opposite setting */
+					rds_mix_msg(rdsd, 0x8 |
+						!rdsd->rds_mix_offset);
+
+					/*Save the pre-pop RMS so that later we
+					 * can see if the pop was actually
+					 * effective */
+					rdsd->PrePopRMS = MaxRMS;
+					/*Reset the PhasePopMaxRMS.  We rely on
+					 * the PopSafetyZone to give time to
+					 * get a new valid max RMS before we're
+					 * eligible for the next phase pop.  If
+					 * there were no reset we'd be forever
+					 * incrementing PhasePopMaxRMS due
+					 * to just happenstance large-noise
+					 * samples and it might eventually get
+					 * some freakish large value causing
+					 * frequent erroneous pops. */
+					rdsd->PhasePopMaxRMS = 0;
+					/* Pop Safety zone length is decided by
+					 * how much of an asynchronous
+					 * frequency can be supported. Allowing
+					* 50 ppm of transmitter error (error
+					* between their own pilot, that we
+					* should be locked to, and their RDS
+					* carrier (which by RDS spec should be
+					* locked to their pilot, but we've
+					* recently found frequently isn't).
+					* 50ppm * 57kHz = 2.85Hz.
+					* (2.85 cycles/sec)(4 pops/cycle)
+					* = 11.4 pops/second.
+					* Safety zone = (1/11.4) seconds =~ 104
+					* bits, round down to 96 bits which
+					* yields 6 ticks if RMSALPHASHIFTS = 5.
+					* */
+					rdsd->PopSafetyZone = 96>>
+						(RMSALPHASHIFTS-1);
+				}
+			}
+			/******************************************************
+			* End Phase Popper Code
+			******************************************************/
+
+			/* SDNOMINAL adaption */
+			if (rdsd->sdnom_adapt) {
+				rdsd->SdnomSk += rdsd->sampskip;
+				if (rdsd->pCoefForcedMono &&
+					(rdsd->BitAlignmentCounter & 0xFFF) ==
+						0x800) {
+
+					rds_sdnominal_msg(rdsd,
+						-(rdsd->SdnomSk<<9));
+
+					/*Reset skips counter */
+					rdsd->SdnomSk = 0;
+				}
+			}
+
+			rdsd->SkipsAccum += rdsd->sampskip;
+			/* Once per 3.45 seconds, print out signal strength,
+			 * skips and pops. Then reset the variables totalling
+			 * those occurrences */
+			if (!(rdsd->BitAlignmentCounter & 0xFFF)) {
+				/* During very noisy input (or if no RDS, or no
+				 * station present), timing_adj can go crazy,
+				 * since it is the integral of noise.  Although
+				 * it is a saturated value (earlier, in the
+				 * timing adjust code), the level at which we
+				 * can saturate still leaves room for
+				 * timing_adj to get too big.  A large value of
+				 * timing_adj is a persistent pathology because
+				 * the phase is shifting so quickly that the
+				 * push detector (which relies on stable
+				 * phase-RMS values) never triggers, thus there
+				 * is no implemented rescue besides this
+				 * clearing that restores proper function. */
+				if (abs(rdsd->SkipsAccum) > 300)
+					rdsd->timing_adj = 0;
+				/*Reset the accumulations. */
+				rdsd->SkipsAccum = 0;
+			}
+		} /*End of bit timing adaption */
+
+		/* If mixer phase determination in progress,
+		 * perform actions at certain times */
+		if (rdsd->MixPhaseDetInProg) {
+			/*~10ms settling time after mixer phase change */
+			#define MIXPHASE_STARTMEAS 12
+			/*~20ms measurement window */
+			#define MIXPHASE_ENDMEAS (MIXPHASE_STARTMEAS+24)
+			if (rdsd->BitAlignmentCounter == MIXPHASE_STARTMEAS) {
+				/*Reset the RMS variables */
+				rdsd->Ph_RMS[0] = 0;
+				rdsd->Ph_RMS[1] = 0;
+				rdsd->Ph_RMS[2] = 0;
+				rdsd->Ph_RMS[3] = 0;
+				/* Don't reset mixandsum values because at
+				 * least they have filtered continuously.  All
+				 * we really need for the mixer phase decision
+				 * is a constant measurement window. */
+			} else if (rdsd->BitAlignmentCounter ==
+					MIXPHASE_ENDMEAS) {
+				/*Measurement = mean of RMS values */
+				u32 Ndx, MeasVal = 0;
+				for (Ndx = 0; Ndx < 4;
+					MeasVal += rdsd->Ph_RMS[Ndx++]>>2);
+				/*Store measurement in correct place */
+				if (rdsd->MixPhaseState == 1) {
+					rdsd->MixPhase0Mag = MeasVal;
+					/*Go to next mixer setting */
+					rds_mix_msg(rdsd, 1);
+				} else if (rdsd->MixPhaseState == 2) {
+					u8 NextMixSetting;
+					rdsd->MixPhase1Mag = MeasVal;
+					/* Both measurements done now, see what
+					 * mixer setting we need to use.
+					 * 0 if MixPhase0Mag > MixPhase1Mag,
+					 * 1 otherwise. */
+					NextMixSetting = (rdsd->MixPhase0Mag
+						<= rdsd->MixPhase1Mag);
+					/* If the mixer setting needed is 1,
+					 * that is already the current setting.
+					 * Terminate mixer phase determination.
+					 * Otherwise send message to switch the
+					 * mixer phase setting. */
+					if (NextMixSetting) {
+						rdsd->MixPhaseState = 3;
+						rdsd->MixPhaseDetInProg = 0;
+					} else
+						rds_mix_msg(rdsd, 0);
+				}
+			}
+			/* Reset BitAlignmentCounter if the Mixer just popped
+			 * Change state, if required.  States are:
+			 * 0: Initial state, send msg causing RDS_MIXOFFSET=>0
+			 * 1: Measure with RDS_MIXOFFSET = 0.
+			 * 	Lasts just over 30 ms.
+			 * 2: Measure with RDS_MIXOFFSET = 1.
+			 * 	Lasts just over 30 ms.
+			 * 3: At final RDS_MIXOFFSET value.
+			 * 	Lasts as long as RDS continues. */
+			if (rdsd->MixPopDone) {
+				rdsd->MixPopDone = 0;
+				rdsd->BitAlignmentCounter = 0;
+				rdsd->MixPhaseState++;	/*Go to next state */
+				/* If we got to state 3, turn off mixer phase
+				 * determination code */
+				if (rdsd->MixPhaseState == 3)
+					rdsd->MixPhaseDetInProg = 0;
+			}
+		}
+
+		/* Update status variables */
+		rdsd->RDS_BIT_AMP_STAT_REG9 = rdsd->Ph_RMS[0]>>RMSALPHASHIFTS;
+		/*Saturate */
+		if (rdsd->RDS_BIT_AMP_STAT_REG9 > 511)
+			rdsd->RDS_BIT_AMP_STAT_REG9 = 511;
+	}  /*End phase 0 code */
+
+	/***************************************************
+	* Actions common to all phases
+	***************************************************/
+
+	/* Save the output of each phase for possible
+	 * calculations during phase 0 */
+	rdsd->PhaseValue[phase] = filter_out;
+
+	/*So that we can measure signal amplitude and/or determine what (if */
+	/*  any) big jump is needed, maintain the RMS of each phase.  Phase */
+	/*  0 RMS is already in Ph_RMS[0] (see bitslicing code, earlier). */
+	rdsd->Ph_RMS[phase] += abs(filter_out) -
+		(rdsd->Ph_RMS[phase]>>RMSALPHASHIFTS);
+}
+
+#if defined(CONFIG_ARM)
+
+/* assembly version for ARM */
+#define RDS_MAC(_acc, _x, _y) \
+	__asm__ __volatile__ (			 \
+	"smlabb	%0, %1, %2, %0\n"		 \
+	: "=&r" (_acc)				 \
+	: "r" (_x), "r" (_y)			 \
+	: "cc")
+
+#else
+
+/* all others, use standard C */
+#define RDS_MAC(_acc, _x, _y) 			 \
+	do {					 \
+		(_acc) += (s16)(_x) * (s16)(_y); \
+	} while (0)
+
+#endif
+
+static void rds_demod(const u16 *data, struct stfm1000_rds_demod *rdsd,
+	struct stfm1000_rds_bitstream *rbit, int total)
+{
+	register const s16 *basis0;
+	register const s16 *basis1;
+	register s16 val;
+	register int i;
+	register int sampskip;
+	register s32 acc1;
+	register s32 acc2;
+
+	/* point to the table */
+	basis0 = u16_rds_basis;
+	basis1 = basis0 + 8;
+
+	rdsd->return_num = 0;
+
+	/* restore state */
+	i = rdsd->i;
+	acc1 = rdsd->mixandsum1;
+	acc2 = rdsd->mixandsum2;	/* 64 bit */
+	sampskip = rdsd->sampskip;
+
+	while (total-- > 0) {
+
+		val = data[3];	/* load RDS data */
+		data += 4;
+		if (val == 0x7fff)	/* illegal RDS sample */
+			continue;
+
+		RDS_MAC(acc1, val, basis0[i]);
+		RDS_MAC(acc2, val, basis1[i]);
+
+		if (i == 4) {
+			i += sampskip;
+			sampskip = 0;
+		}
+
+		if ((i & (RDS_BASISLENGTH / 2 - 1)) == 0) {
+
+			/* save state */
+			rdsd->mixandsum1 = acc1;
+			rdsd->mixandsum2 = acc2;
+			rdsd->i = i;
+			rdsd->sampskip = sampskip;
+
+			demod_loop(rbit, rdsd);
+
+			/* restore state */
+			acc1 = rdsd->mixandsum1;
+			acc2 = rdsd->mixandsum2;
+			i = rdsd->i;
+			sampskip = rdsd->sampskip;
+		}
+		i = (i + 1) & 31;
+	}
+
+	/* save state */
+	rdsd->mixandsum1 = acc1;
+	rdsd->mixandsum2 = acc2;
+	rdsd->i = i;
+	rdsd->sampskip = sampskip;
+}
+
+void stfm1000_rds_demod(struct stfm1000_rds_state *rds, const u16 *dri_data,
+	int total)
+{
+	rds_demod(dri_data, &rds->demod, &rds->bitstream, total);
+
+	/* signal only when we have enough */
+	if (bits_filled(&rds->bitstream) > 128)
+		stfm1000_monitor_signal(rds_state_to_stfm1000(rds),
+				EVENT_RDS_BITS);
+}
+
+static void bitstream_reset(struct stfm1000_rds_bitstream *rdsb)
+{
+	memset(rdsb, 0, sizeof(*rdsb));
+}
+
+static void demod_reset(struct stfm1000_rds_demod *rdsd)
+{
+	memset(rdsd, 0, sizeof(*rdsd));
+	rdsd->sdnom_adapt = 0;	/* XXX this doesn't really work right */
+					/* it causes underruns at ALSA */
+	rdsd->PhasePoppingEnabled = 1;	/* does this? */
+}
+
+static void packet_reset(struct stfm1000_rds_pkt *rdsp)
+{
+	memset(rdsp, 0, sizeof(*rdsp));
+	rdsp->state = SYNC_OFFSET_A;
+}
+
+static void text_reset(struct stfm1000_rds_text *rdst)
+{
+	memset(rdst, 0, sizeof(*rdst));
+}
+
+void stfm1000_rds_reset(struct stfm1000_rds_state *rds)
+{
+	bitstream_reset(&rds->bitstream);
+	demod_reset(&rds->demod);
+	packet_reset(&rds->pkt);
+	text_reset(&rds->text);
+	rds->reset_req = 0;
+}
+
+int stfm1000_rds_bits_available(struct stfm1000_rds_state *rds)
+{
+	return bits_filled(&rds->bitstream);
+}
+
+int stmf1000_rds_get_bit(struct stfm1000_rds_state *rds)
+{
+	if (bits_filled(&rds->bitstream) == 0)
+		return -1;
+	return get1bit(&rds->bitstream);
+}
+
+int stmf1000_rds_avail_bits(struct stfm1000_rds_state *rds)
+{
+	return bits_filled(&rds->bitstream);
+}
+
+static const u32 rds_ParityCheck[] = {
+	0x31B, 0x38F, 0x2A7, 0x0F7, 0x1EE,
+	0x3DC, 0x201, 0x1BB, 0x376, 0x355,
+	0x313, 0x39F, 0x287, 0x0B7, 0x16E,
+	0x2DC, 0x001, 0x002, 0x004, 0x008,
+	0x010, 0x020, 0x040, 0x080, 0x100,
+	0x200
+};
+
+static int calc_syndrome(u32 rdscrc)
+{
+	int i;
+	u32 syndrome = 0;
+	int word = 0x1;
+
+	for (i = 0; i < 26; i++) {
+		if (rdscrc & word)
+			syndrome ^= rds_ParityCheck[i];
+		word <<= 1;
+	}
+	return syndrome;
+}
+
+static u32 ecc_table[1024];
+static int ecc_table_generated;
+
+static void generate_ecc_table(void)
+{
+	int i, j, size;
+	u32 syndrome, word;
+
+	for (i = 0; i < ECC_TBL_SIZE; i++)
+		ecc_table[i] = 0xFFFFFFFF;
+	ecc_table[0] = 0x0;
+
+	for (j = 0; j < 5; j++) {
+		word = (1 << (j + 1)) - 1;	/* 0x01 0x03 0x07 0x0f 0x1f */
+		size = 26 - j; 			/* 26, 25, 24, 23, 22 */
+		syndrome = 0;
+		for (i = 0; i < size; i++) {
+			syndrome = calc_syndrome(word);
+			ecc_table[syndrome] = word;
+			word <<= 1;
+		}
+	}
+}
+
+static u32 ecc_correct(u32 rdsBits, int *recovered)
+{
+	u32 syndrome;
+	u32 errorBits;
+
+	if (recovered)
+		*recovered = 0;
+
+	/* Calculate Syndrome on Received Packet */
+	syndrome = calc_syndrome(rdsBits);
+
+	if (syndrome == 0)
+		return rdsBits; /* block is clean */
+
+	/* generate table first time we get here */
+	if (!ecc_table_generated) {
+		generate_ecc_table();
+		ecc_table_generated = 1;
+	}
+
+	/* Attempt to recover block */
+	errorBits = ecc_table[syndrome];
+	if (errorBits == UNRECOVERABLE_RDS_BLOCK)
+		return UNRECOVERABLE_RDS_BLOCK;	/* Block can not be recovered.
+						 * it is bad packet */
+
+	rdsBits = rdsBits ^ errorBits;
+	if (recovered)
+		(*recovered)++;
+	return rdsBits;                 /* ECC correct */
+}
+
+/* The following table lists the RDS and RBDS Program Type codes
+ * and their meanings:
+ * PTY code RDS Program type RBDS Program type */
+static const struct stfm1000_rds_pty stc_tss_pty_tab[] = {
+	{  0, "No program type",    "No program type"},
+	{  1, "News",               "News"},
+	{  2, "Current affairs",    "Information"},
+	{  3, "Information",        "Sports"},
+	{  4, "Sports",             "Talk"},
+	{  5, "Education",          "Rock"},
+	{  6, "Drama",              "Classic Rock"},
+	{  7, "Culture",            "Adult Hits"},
+	{  8, "Science",            "Soft Rock"},
+	{  9, "Varied",             "Top 40"},
+	{ 10, "Pop",                "Music Country"},
+	{ 11, "Rock",               "Music Oldies"},
+	{ 12, "M.O.R.",             "Music Soft"},
+	{ 13, "Light classical",    "Nostalgia"},
+	{ 14, "Serious",            "Classical Jazz"},
+	{ 15, "Other Music",        "Classical"},
+	{ 16, "Weather",            "Rhythm and Blues"},
+	{ 17, "Finance",            "Soft Rhythm and Blues"},
+	{ 18, "Children's programs", "Language"},
+	{ 19, "Social Affairs",     "Religious Music"},
+	{ 20, "Religion",           "Religious Talk"},
+	{ 21, "Phone In",           "Personality"},
+	{ 22, "Travel",             "Public"},
+	{ 23, "Leisure",            "College"},
+	{ 24, "Jazz Music",         "Unassigned"},
+	{ 25, "Country Music",      "Unassigned"},
+	{ 26, "National Music",     "Unassigned"},
+	{ 27, "Oldies Music",       "Unassigned"},
+	{ 28, "Folk Music",         "Unassigned"},
+	{ 29, "Documentary",        "Weather"},
+	{ 30, "Alarm Test",         "Emergency Test"},
+	{ 31, "Alarm",              "Emergency"},
+};
+
+#if 0
+static const char *rds_group_txt[] = {
+	[RDS_GROUP_TYPE_0A] = "Basic tuning and switching information (0A)",
+	[RDS_GROUP_TYPE_0B] = "Basic tuning and switching information (0B)",
+	[RDS_GROUP_TYPE_1A] = "Program item number and slow labeling codes",
+	[RDS_GROUP_TYPE_1B] = "Program item number",
+	[RDS_GROUP_TYPE_2A] = "Radio Text (2A)",
+	[RDS_GROUP_TYPE_2B] = "Radio Text (2B)",
+	[RDS_GROUP_TYPE_3A] = "Application identification for ODA only",
+	[RDS_GROUP_TYPE_3B] = "Open data applications",
+	[RDS_GROUP_TYPE_4A] = "Clock-time and date",
+	[RDS_GROUP_TYPE_4B] = "Open data applications",
+	[RDS_GROUP_TYPE_5A] = "Transparent Data Channels (32 ch.) or ODA (5A)",
+	[RDS_GROUP_TYPE_5B] = "Transparent Data Channels (32 ch.) or ODA (5B)",
+	[RDS_GROUP_TYPE_6A] = "In House Applications or ODA (6A)",
+	[RDS_GROUP_TYPE_6B] = "In House Applications or ODA (6B)",
+	[RDS_GROUP_TYPE_7A] = "Radio Paging or ODA",
+	[RDS_GROUP_TYPE_7B] = "Open Data Applications",
+	[RDS_GROUP_TYPE_8A] = "Traffic Message Channel or ODA",
+	[RDS_GROUP_TYPE_8B] = "Open Data Applications",
+	[RDS_GROUP_TYPE_9A] = "Emergency warning system or ODA",
+	[RDS_GROUP_TYPE_9B] = "Open Data Applications",
+	[RDS_GROUP_TYPE_10A] = "Program Type Name",
+	[RDS_GROUP_TYPE_10B] = "Open Data Applications (10B)",
+	[RDS_GROUP_TYPE_11A] = "Open Data Applications (11A)",
+	[RDS_GROUP_TYPE_11B] = "Open Data Applications (11B)",
+	[RDS_GROUP_TYPE_12A] = "Open Data Applications (12A)",
+	[RDS_GROUP_TYPE_12B] = "Open Data Applications (12B)",
+	[RDS_GROUP_TYPE_13A] = "Enhanced Radio Paging or ODA",
+	[RDS_GROUP_TYPE_13B] = "Open Data Applications",
+	[RDS_GROUP_TYPE_14A] = "Enhanced Other Networks information (14A)",
+	[RDS_GROUP_TYPE_14B] = "Enhanced Other Networks information (14B)",
+	[RDS_GROUP_TYPE_15A] = "Defined in RBDS",
+	[RDS_GROUP_TYPE_15B] = "Fast switching information",
+};
+#endif
+
+static void dump_rds_packet(u8 *buf)
+{
+	u16 pi, offb;
+
+	pi   = (u16)(buf[0] << 8) | buf[1];
+	offb = (u16)(buf[1] << 8) | buf[2];
+
+	printk(KERN_INFO "GRP: "
+		"PI=0x%04x "
+		"GT=%2d VER=%d TP=%d PTY=%2d "
+		"PS_SEG=%2d RT_AB=%2d RT_SEG=%2d\n", pi,
+		RDS_GROUP_TYPE(offb), RDS_VERSION(offb), RDS_TP(offb),
+		RDS_PTY(offb),
+		RDS_PS_SEG(offb), RDS_RT_AB(offb), RDS_RT_SEG(offb));
+}
+
+void stfm1000_rds_process_packet(struct stfm1000_rds_state *rds, u8 *buffer)
+{
+	struct stfm1000_rds_text *rdst = &rds->text;
+	/* char tempCallLetters[5] = {0}; */
+	struct rds_group_data grp;
+	int grpno;
+	u32 offset;
+	char tps[9];
+	int i, seg, idx;
+
+	grp.piCode  = ((u16)buffer[0] << 8) | buffer[1];
+	grp.offsetB = ((u16)buffer[2] << 8) | buffer[3];
+	grp.offsetC = ((u16)buffer[4] << 8) | buffer[5];
+	grp.offsetD = ((u16)buffer[6] << 8) | buffer[7];
+
+	grpno = (grp.offsetB >> (8 + 3)) & 0x1f;
+
+	if (rds_state_to_stfm1000(rds)->rds_info)
+		dump_rds_packet(buffer);
+
+	/* Is this the first time through? */
+	if (!rdst->bRds_detected) {
+		rdst->pi            = grp.piCode;
+		rdst->tp            = RDS_TP(grp.offsetB);
+		rdst->version       = RDS_VERSION(grp.offsetB);
+		rdst->pty.id        = RDS_PTY(grp.offsetB);
+		rdst->pty.pRds      = stc_tss_pty_tab[rdst->pty.id].pRds;
+		rdst->pty.pRdbs     = stc_tss_pty_tab[rdst->pty.id].pRdbs;
+		rdst->bRds_detected = 1;
+	}
+
+	/* Have we process too many PI errors? */
+	if (grp.piCode != rdst->pi) {
+		if (rdst->mismatch++ > 10) {
+
+			/* requested reset of RDS */
+			rds->reset_req = 1;
+
+			/* signal monitor thread */
+			stfm1000_monitor_signal(rds_state_to_stfm1000(rds),
+				EVENT_RDS_RESET);
+
+			if (rds_state_to_stfm1000(rds)->rds_info)
+				printk(KERN_INFO "RDS: RESET!!!\n");
+
+			text_reset(rdst);
+		}
+		rdst->consecutiveGood = 0;
+		return;
+	}
+
+	if (rdst->consecutiveGood++ > 10)
+		rdst->mismatch = 0;            /* reset bad count */
+
+	if (rdst->consecutiveGood > rdst->consecutiveGoodMax)
+		rdst->consecutiveGoodMax = rdst->consecutiveGood;
+
+	switch (grpno) {
+	case RDS_GROUP_TYPE_0A:
+	case RDS_GROUP_TYPE_0B:
+		/* Extract Service Name information */
+		offset = RDS_PS_SEG(grp.offsetB) * 2;
+		rdst->wk_ps[offset]     = buffer[6];	/* better */
+		rdst->wk_ps[offset + 1] = buffer[7];
+		rdst->wk_ps_mask |= 1 << RDS_PS_SEG(grp.offsetB);
+
+		if (rds_state_to_stfm1000(rds)->rds_info) {
+			for (i = 0; i < 8; i++) {
+				if (rdst->wk_ps_mask & (1 << i)) {
+					tps[i * 2] =
+						rdst->wk_ps[i * 2];
+					tps[i * 2 + 1] =
+						rdst->wk_ps[i * 2 + 1];
+				} else {
+					tps[i * 2] = '_';
+					tps[i * 2 + 1] = '_';
+				}
+			}
+			tps[ARRAY_SIZE(tps) - 1] = '\0';
+			if (rds_state_to_stfm1000(rds)->rds_info)
+				printk(KERN_INFO "RDS-PS (curr): %s\n", tps);
+		}
+
+		if (rdst->wk_ps_mask != ALL_SEGMENT_BITS)
+			break;
+
+		if (rdst->ps_valid) {
+			if (memcmp(rdst->ps, rdst->wk_ps, 8) != 0) {
+				memset(rdst->cp_ps, 0, 8);
+				memset(rdst->wk_ps, 0, 8);
+				rdst->wk_ps_mask = 0;
+			}
+
+			memset(rdst->ps, 0, 8);
+			rdst->ps_valid = 0;
+			break;
+		}
+
+		/* does working buffer == compare buffer */
+		if (memcmp(rdst->cp_ps, rdst->wk_ps, 8) != 0) {
+			/* just copy from working to compare buffer */
+			memcpy(rdst->cp_ps, rdst->wk_ps, 8);
+			rdst->wk_ps_mask = 0;
+			break;
+		}
+
+		/* working buffer matches compare buffer, send to UI */
+		memcpy(rdst->ps, rdst->cp_ps, 8);
+		rdst->ps_valid = 1;
+
+		if (rds_state_to_stfm1000(rds)->rds_info)
+			printk(KERN_INFO "RDS: PS '%s'\n", rdst->ps);
+
+		/* clear working mask-only */
+		rdst->wk_ps_mask = 0;
+		break;
+
+	case RDS_GROUP_TYPE_2A:
+
+		/* Clear buffer */
+		if (rdst->textAB_flag != RDS_RT_AB(grp.offsetB)) {
+			memset(rdst->wk_text, 0, 64);
+			rdst->wk_text_mask = 0;
+			rdst->textAB_flag  = RDS_RT_AB(grp.offsetB);
+		}
+
+		/* Extract Text */
+		seg = RDS_RT_SEG(grp.offsetB);
+		idx = seg * 4;
+
+		#define CNVT_EOT(x) ((x) != RDS_EOT ? (x) : 0)
+		rdst->wk_text[idx++] = CNVT_EOT(buffer[4]);
+		rdst->wk_text[idx++] = CNVT_EOT(buffer[5]);
+		rdst->wk_text[idx++] = CNVT_EOT(buffer[6]);
+		rdst->wk_text[idx++] = CNVT_EOT(buffer[7]);
+
+		rdst->wk_text_mask |= 1 << seg;
+		/* scan msg data for EOT.  If found set all higher
+		 * mask bits */
+		for (idx = 0; idx < 4; idx++) {
+			if (rdst->text[idx] == RDS_EOT)
+				break;
+		}
+		if (idx < 4) {
+			/* set current and all higher bits  */
+			for (idx = RDS_RT_SEG(grp.offsetB); idx < 16;
+				idx++)
+				rdst->wk_text_mask |= 1 << idx;
+		}
+
+		/* Process buffer when filled */
+		if (rdst->wk_text_mask != ALL_TEXT_BITS)
+			break;
+
+		if (!rdst->text_valid)
+			rdst->text_valid = 1;
+		else if (memcmp(rdst->text, rdst->wk_text, 64) == 0)
+			break;
+
+		memcpy(rdst->text, rdst->wk_text, 64);
+
+		if (rds_state_to_stfm1000(rds)->rds_info)
+			printk(KERN_INFO "RDS: TEXT '%s'\n", rdst->text);
+
+		memset(rdst->wk_text, 0, 64);
+		rdst->wk_text_mask = 0;
+		break;
+
+	default:
+		break;
+	}
+}
+
+int stfm1000_rds_packet_dequeue(struct stfm1000_rds_state *rds, u8 *buf)
+{
+	struct stfm1000_rds_pkt *pkt = &rds->pkt;
+
+	if (pkt->buf_cnt == 0)
+		return -1;
+
+	memcpy(buf, &pkt->buf_queue[pkt->buf_tail][0], 8);
+	if (++pkt->buf_tail >= RDS_PKT_QUEUE)
+		pkt->buf_tail = 0;
+	pkt->buf_cnt--;
+
+	return 0;
+}
+
+void stfm1000_rds_packet_bit(struct stfm1000_rds_state *rds, int bit)
+{
+	struct stfm1000_rds_pkt *pkt = &rds->pkt;
+	u32 rdsdata, rdscrc, rdscrc_c, rdscrc_cp;
+	int correct, correct2, recovered, recovered2;
+	int RetVal;
+
+	/* Stick into shift register */
+	pkt->rdsstream = ((pkt->rdsstream << 1) | bit) & 0x03ffffff;
+	pkt->bitsinfifo++;
+	pkt->bitcount++;
+
+	/* wait for 26 bits of block */
+	if (pkt->bitsinfifo < 26)
+		return;
+
+	rdsdata = pkt->rdsstream & 0x03fffc00;	/* 16 bits of Info. word */
+	rdscrc = pkt->rdsstream & 0x3ff;	/* 10 bits of Checkword */
+
+	switch (pkt->state) {
+	case SYNC_OFFSET_A:
+
+		RetVal = calc_syndrome(pkt->rdsstream);
+
+		switch (RetVal) {
+		case RDS_SYNDROME_OFFSETA:
+			pkt->state = OFFSET_B;
+			break;
+		case RDS_SYNDROME_OFFSETB:
+			pkt->state = OFFSET_C_CP;
+			break;
+		case RDS_SYNDROME_OFFSETC:
+			pkt->state = OFFSET_D;
+			break;
+		case RDS_SYNDROME_OFFSETCP:
+			pkt->state = OFFSET_D;
+			break;
+		case RDS_SYNDROME_OFFSETD:
+			pkt->state = OFFSET_A;
+			break;
+		default:
+			pkt->state = SYNC_OFFSET_A;
+			break;
+		}
+		if (pkt->state == SYNC_OFFSET_A) {
+			pkt->sync_lost_packets++;
+			/* XXX send info? */
+			break;
+		}
+
+		pkt->good_packets++;
+
+		rdsdata = pkt->rdsstream & 0x03fffc00;
+
+		/* Save type A packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[0] = (rdsdata >> 8);
+		pkt->buffer[1] = (rdsdata & 0xff);
+		pkt->bitsinfifo = 0;
+
+		/* We found a block with zero errors, but it is not at the
+		 * start of the group. */
+		if (pkt->state == OFFSET_B)
+			pkt->discardpacket = 0;
+		else
+			pkt->discardpacket = 1;
+		break;
+
+	case OFFSET_A:		/* Type A: we are in sync now */
+		rdscrc ^= RDS_OFFSETA;
+		correct = ecc_correct(rdsdata | rdscrc, &recovered);
+		if (correct == UNRECOVERABLE_RDS_BLOCK) {
+			pkt->bad_packets++;
+			pkt->discardpacket++;
+			pkt->state++;
+			pkt->bitsinfifo = 0;
+			break;
+		}
+
+		if (recovered)
+			pkt->recovered_packets++;
+		pkt->good_packets++;
+
+		/* Attempt to see, if we can get the entire group.
+		 * Don't discard. */
+		pkt->discardpacket = 0;
+		rdsdata = correct & 0x03fffc00;
+
+		/* Save type A packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[0] = (rdsdata >> 8);
+		pkt->buffer[1] = (rdsdata & 0xff);
+		pkt->bitsinfifo = 0;
+		pkt->state++;
+		break;
+
+	case OFFSET_B:		/* Waiting for type B */
+		rdscrc ^= RDS_OFFSETB;
+		correct = ecc_correct(rdsdata | rdscrc, &recovered);
+		if (correct == UNRECOVERABLE_RDS_BLOCK) {
+			pkt->bad_packets++;
+			pkt->discardpacket++;
+			pkt->state++;
+			pkt->bitsinfifo = 0;
+			break;
+		}
+		if (recovered)
+			pkt->recovered_packets++;
+		pkt->good_packets++;
+
+		rdsdata = correct & 0x03fffc00;
+
+		/* Save type B packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[2] = (rdsdata >> 8);
+		pkt->buffer[3] = (rdsdata & 0xff);
+		pkt->bitsinfifo = 0;
+		pkt->state++;
+		break;
+
+	case OFFSET_C_CP:	/* Waiting for type C or C' */
+		rdscrc_c = rdscrc ^ RDS_OFFSETC;
+		rdscrc_cp = rdscrc ^ RDS_OFFSETCP;
+		correct = ecc_correct(rdsdata | rdscrc_c, &recovered);
+		correct2 = ecc_correct(rdsdata | rdscrc_cp, &recovered2);
+		if (correct == UNRECOVERABLE_RDS_BLOCK
+		    && correct2 == UNRECOVERABLE_RDS_BLOCK) {
+			pkt->bad_packets++;
+			pkt->discardpacket++;
+			pkt->state++;
+			pkt->bitsinfifo = 0;
+			break;
+		}
+
+		if (recovered || recovered2)
+			pkt->recovered_packets++;
+		pkt->good_packets++;
+
+		if (correct == UNRECOVERABLE_RDS_BLOCK)
+			correct = correct2;
+
+		rdsdata = correct & 0x03fffc00;
+
+		/* Save type C packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[4] = (rdsdata >> 8);
+		pkt->buffer[5] = (rdsdata & 0xff);
+		pkt->bitsinfifo = 0;
+		pkt->state++;
+		break;
+
+	case OFFSET_D:		/* Waiting for type D */
+		rdscrc ^= RDS_OFFSETD;
+		correct = ecc_correct(rdsdata | rdscrc, &recovered);
+		if (correct == UNRECOVERABLE_RDS_BLOCK) {
+			pkt->bad_packets++;
+			pkt->discardpacket++;
+			pkt->state = OFFSET_A;
+			pkt->bitsinfifo = 0;
+			break;
+		}
+
+		if (recovered)
+			pkt->recovered_packets++;
+		pkt->good_packets++;
+
+		rdsdata = correct & 0x03fffc00;
+
+		/* Save type D packet in buffer */
+		rdsdata >>= 10;
+		pkt->buffer[6] = (rdsdata >> 8);
+		pkt->buffer[7] = (rdsdata & 0xff);
+
+		/* buffer it if all segments were ok */
+		if (pkt->discardpacket) {
+			/* We're still in sync, so back to state 1 */
+			pkt->state = OFFSET_A;
+			pkt->bitsinfifo = 0;
+			pkt->discardpacket = 0;
+			break;
+		}
+
+		pkt->state++;
+		/* fall-through */
+
+	case PACKET_OUT:
+		pkt->GroupDropOnce = 1;
+
+		/* queue packet */
+		if (pkt->buf_cnt < RDS_PKT_QUEUE) {
+			memcpy(&pkt->buf_queue[pkt->buf_head][0],
+				pkt->buffer, 8);
+			if (++pkt->buf_head >= RDS_PKT_QUEUE)
+				pkt->buf_head = 0;
+			pkt->buf_cnt++;
+		} else
+			pkt->buf_overruns++;
+
+		/* We're still in sync, so back to state 1 */
+		pkt->state = OFFSET_A;
+		pkt->bitsinfifo = 0;
+		pkt->discardpacket = 0;
+		break;
+
+	}
+
+	/* Lots of errors? If so, go back to resync mode */
+	if (pkt->discardpacket >= 10) {
+		pkt->state = SYNC_OFFSET_A;	/* reset sync state */
+		pkt->bitsinfifo = 26;	/* resync a bit faster */
+	}
+}
+
+/* GROUP_TYPE 0A-0B (buffer must have enough space for 9 bytes) */
+int stfm1000_rds_get_ps(struct stfm1000_rds_state *rds, u8 *buffer,
+	int bufsize)
+{
+	struct stfm1000_rds_text *rdst = &rds->text;
+
+	if (bufsize < 9)
+		return -1;
+
+	if (!rdst->ps_valid)
+		return -1;
+
+	memcpy(buffer, rdst->ps, 8);
+	buffer[8] = '\0';
+
+	return 8;
+}
+
+/* GROUP_TYPE 2A (buffer must have enough space for 65 bytes) */
+int stfm1000_rds_get_text(struct stfm1000_rds_state *rds, u8 *buffer,
+	int bufsize)
+{
+	struct stfm1000_rds_text *rdst = &rds->text;
+
+	if (bufsize < 9)
+		return -1;
+
+	if (!rdst->text_valid)
+		return -1;
+
+	memcpy(buffer, rdst->text, 64);
+	buffer[64] = '\0';
+
+	return 64;
+}
diff --git a/drivers/media/radio/stfm1000/stfm1000-rds.h b/drivers/media/radio/stfm1000/stfm1000-rds.h
new file mode 100644
index 000000000000..44b9a610f86e
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-rds.h
@@ -0,0 +1,364 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef STFM1000_RDS_H
+#define STFM1000_RDS_H
+
+#include <linux/types.h>
+
+/* log2(number of samples in a filter basis) */
+#define RDS_BASISSHIFTS		4
+
+/* number of samples in a filter basis */
+#define RDS_BASISLENGTH		(1 << RDS_BASISSHIFTS)
+
+#define TIME_ADAPT_OVER		100
+
+/* 2^(-this) is the RMS leaky bucket time constant */
+#define RMSALPHASHIFTS		5
+
+#define PROCESS_RDS_BITS	128
+
+#define RDS_BITBUFSIZE		1024	/* was 128 */
+struct stfm1000_rds_bitstream {
+	u32 buf[RDS_BITBUFSIZE];	/* bit buffer */
+	int HeadBitCount;		/* bit buffer head counter */
+	int TailBitCount;		/* bit buffer tail counter */
+};
+
+struct stfm1000_rds_demod {
+	u32 mixandsum1;			/* Accumulator for first
+					 * basis filter */
+	u32 mixandsum2;			/* Accumulator for 2nd
+					 * basis filter */
+	u32 i;				/* Phase Index, 32 phases per
+					 * RDS bit */
+	u32 return_num;			/* Set if there is a new RDS bit */
+	u32 BitAlignmentCounter;	/* Counts bits for timing purposes */
+	int sampskip;			/* Requested timing shift (on i) */
+
+	int DisablePushing;		/* Disables phase push algorithm
+					 * (phase push happens when Ph_RMS[x],
+					 * x != 0, is consistently the maximum
+					 * Ph_RMS) */
+	int MixPopDone;			/* Last mixer phase set request is
+					 * done */
+	u8 rds_big_timeshift;		/* If set, indicates a push or large
+					 * timing shift occurred */
+	int return_rdsdemod;		/* Output, (most recent bit) XOR
+					 * (prev bit) */
+	u32 RDS_BIT_AMP_STAT_REG9;	/* Size of bit (RMS of RDS signal at
+					 * bitslicing instant, typically 220
+					 * to 270) */
+	s32 decomp_hist;		/* Most recent basis filter output */
+	s32 decomp_hist_p;		/* Previous basis filter output */
+	s32 PhaseValue[4];		/* Half-basis phase samples over the
+					 * most recent bit */
+	u32 Ph_RMS[4];			/* RMS of the four half-basis phases */
+	s32 timing_adj;			/* Timing loop leaky-bucket
+					 * accumulator */
+	u32 MixPhase0Mag;		/* Magnitude of RDS signal with RDS
+					 * mixer phase 0 (from mixer phase
+					 * determination) */
+	u32 MixPhase1Mag;		/* Magnitude of RDS signal with RDS
+					 * mixer phase 1 (from mixer phase
+					 * determination) */
+	u32 PhasePopMaxRMS;		/* Maximum RMS observed since last
+					 * phase pop */
+	u32 PrePopRMS;			/* Max of Ph_RMS array right before the
+					 * most recent phase pop */
+	u8 MixPhaseState;		/* State of RDS mixer phase
+					 * determination state machine */
+	int MixPhaseDetInProg;		/* Set if RDS mix phase determination
+					 * is in progress */
+	int sliced_data;		/* The most recent bit decision */
+	u8 PopSafetyZone;		/* Countdown timer, holds off next
+					 * phase pop after a recent one */
+	u8 PushSafetyZone;		/* Countdown timer, holds off next
+					 * phase pop after a timing push (b/c
+					 * timing push resets Ph_RMS vars) */
+	u8 SkipSafetyZone;		/* Countdown timer, holds off next
+					 * phase skip (small timing adj) */
+	int Synchronous;		/* RDS has been determined to be
+					 * synchronous to pilot */
+	u8 PushLastMaxPh;		/* The index at which Ph_RMS is
+					 * maximum ("x" in the above two
+					 * comments) */
+	s32 PushCounter;		/* Counts instances of Ph_RMS[x], x!=0,
+					 * being the maximum Ph_RMS */
+	s32 SkipsAccum;			/* Accumulation of all timing skips
+					 * since RDS demod started */
+	s32 SdnomSk;			/* Skips counter used for SDNOMINAL
+					 * adaption */
+
+	/* update this everytime it's changed & put it here */
+	unsigned int rds_mix_offset : 1;
+
+	unsigned int sdnom_adapt : 1;
+	unsigned int pCoefForcedMono : 1;	/* copy of filter parameter */
+	unsigned int PhasePoppingEnabled : 1;
+
+	unsigned int mix_msg_pending : 1;
+	u8 mix_msg;
+	unsigned int mix_msg_overrun;
+	unsigned int mix_msg_processed_changed;
+
+	unsigned int sdnominal_msg_pending : 1;
+	int sdnominal_msg;
+	unsigned int sdnominal_msg_overrun;
+
+	u32 RdsDemodSkippedBitCnt;	/* bit skipped by RDS demodulator due
+					 * to unavailable space in buf[]
+					 * (bit buffer) */
+};
+
+#define RDS_OFFSETA	0x0fc
+#define RDS_OFFSETB	0x198
+#define RDS_OFFSETC	0x168
+#define RDS_OFFSETCP	0x350
+#define RDS_OFFSETD	0x1b4
+
+#define RDS_SYNDROME_OFFSETA    0x3d8
+#define RDS_SYNDROME_OFFSETB    0x3d4
+#define RDS_SYNDROME_OFFSETC    0x25c
+#define RDS_SYNDROME_OFFSETCP   0x3cc
+#define RDS_SYNDROME_OFFSETD    0x258
+
+#define SYNC_OFFSET_A 0 /* default state */
+#define OFFSET_A      1
+#define OFFSET_B      2
+#define OFFSET_C_CP   3
+#define OFFSET_D      4
+#define PACKET_OUT    5
+
+#define ECC_TBL_SIZE   		    1024
+#define UNRECOVERABLE_RDS_BLOCK 0xffffffff
+
+#define RDS_PKT_QUEUE	16
+
+struct stfm1000_rds_pkt {
+	int state;		/* Current state */
+	u32 rdsstream;		/* Current RDS data */
+	u8 buffer[8];		/* temporary storage of RDS data */
+	int discardpacket;	/* discard packet count */
+	int sync_lost_packets;	/* sync lost */
+	int good_packets;	/* good packet */
+	int bad_packets;	/* bad packet */
+	int recovered_packets;	/* recovered packet */
+	int bitsinfifo;		/* bits count */
+	int GroupDropOnce;	/* Send Group Drop Message once */
+	int bitcount;		/* Counter for Number of Bits read */
+
+	/* queue the packets here */
+	int buf_overruns;
+	int buf_head;
+	int buf_tail;
+	int buf_cnt;
+	int buf_queue[RDS_PKT_QUEUE][8];
+};
+
+#define AUDIT            0
+#define ALL_SEGMENT_BITS 0xF
+#define ALL_TEXT_BITS    0xFFFF
+
+struct stfm1000_rds_pty {
+	u8  id;         /* Program Type ID */
+	u8 *pRds;       /* RDS description */
+	u8 *pRdbs;      /* RDBS description */
+};
+
+struct stfm1000_rds_text {
+	u8 bRds_detected;		/* Has the first packet come in yet? */
+	u16 pi;				/* Program Identification Code (PI) */
+	struct stfm1000_rds_pty pty;	/* Program Type (PTY)) */
+	u8 tp;				/* Traffic Program (TP) identification
+					 * code */
+	u8 ps[9];			/* Program Service Name Sent to UI */
+	u8 altFreq[2];			/* Alternate frequency (AF) */
+	u8 callLetters[5];		/* For US, stations call letters */
+
+	u8 text[65];			/* Radio Text A */
+
+	unsigned int version : 1;	/* Is station broadcasting version
+					 * A or B (B0) */
+	unsigned int ps_valid : 1;	/* station name is valid */
+	unsigned int text_valid : 1;	/* Text is valid */
+	unsigned int textAB_flag : 1;	/* Current flag setting, reset if flag
+					 * changes */
+
+	/*------------------Working area--------------------------- */
+	u8 cp_ps[8];			/* Compare buffer for PS */
+	u8 wk_ps[8];			/* Program Service buffer */
+	u8 wk_ps_mask;			/* lower 4 bits must be set
+					 * before copy */
+	u8 wk_text[64];			/* Radio Text buffer */
+	u16 wk_text_mask;		/* all bits must be set before copy */
+
+	/*-------------------Counters------------------------------ */
+	u32 messages;			/* total number of messages recieved */
+	u32 unsupported;		/* call to unsupported group type */
+	u32 mismatch;			/* Mismatched values */
+	u32 consecutiveGood;		/* Consecutive good will clear bad  */
+	u32 consecutiveGoodMax;		/* Max counter for paramaters */
+};
+
+/* Maximum number of RDS groups described in the U.S. RBDS Standard. */
+#define MAX_RDS_GROUPS_SUPPORTED 32
+
+/* Common Constants */
+#define RDS_LINE_FEED   0xA
+#define RDS_EOT         0xD
+
+/* Offsets into OFFSETB  */
+#define RDS_GROUP_TYPE(x)         (((x) >> 12) & 0xF)
+#define RDS_VERSION(x)            (((x) >> 11) & 0x1)
+#define RDS_TP(x)                 (((x) >> 10) & 0x1)
+#define RDS_PTY(x)                (((x) >>  5) & 0x1F)
+#define RDS_PS_SEG(x)              ((x) & 0x3)
+#define RDS_RT_AB(x)              (((x) >>  4) & 0x1)
+#define RDS_RT_SEG(x)              ((x) & 0xF)
+
+/* This values corresond to the Group Types defined */
+/* In the U.S. RBDS standard. */
+#define RDS_GROUP_TYPE_0A	 0 /* Basic tuning and switching information */
+#define RDS_GROUP_TYPE_0B	 1 /* Basic tuning and switching information */
+#define RDS_GROUP_TYPE_1A	 2 /* Program item number and slow labeling
+				    * codes */
+#define RDS_GROUP_TYPE_1B	 3 /* Program item number */
+#define RDS_GROUP_TYPE_2A	 4 /* Radio Text */
+#define RDS_GROUP_TYPE_2B	 5 /* Radio Text */
+#define RDS_GROUP_TYPE_3A	 6 /* Application identification for ODA
+				    * only */
+#define RDS_GROUP_TYPE_3B	 7 /* Open data applications */
+#define RDS_GROUP_TYPE_4A	 8 /* Clock-time and date */
+#define RDS_GROUP_TYPE_4B	 9 /* Open data applications */
+#define RDS_GROUP_TYPE_5A	10 /* Transparent Data Channels (32 channels)
+				    * or ODA */
+#define RDS_GROUP_TYPE_5B	11 /* Transparent Data Channels (32 channels)
+				    * or ODA */
+#define RDS_GROUP_TYPE_6A	12 /* In House Applications or ODA */
+#define RDS_GROUP_TYPE_6B	13 /* In House Applications or ODA */
+#define RDS_GROUP_TYPE_7A	14 /* Radio Paging or ODA */
+#define RDS_GROUP_TYPE_7B	15 /* Open Data Applications */
+#define RDS_GROUP_TYPE_8A	16 /* Traffic Message Channel or ODA */
+#define RDS_GROUP_TYPE_8B	17 /* Open Data Applications */
+#define RDS_GROUP_TYPE_9A	18 /* Emergency warning system or ODA */
+#define RDS_GROUP_TYPE_9B	19 /* Open Data Applications */
+#define RDS_GROUP_TYPE_10A	20 /* Program Type Name */
+#define RDS_GROUP_TYPE_10B	21 /* Open Data Applications */
+#define RDS_GROUP_TYPE_11A	22 /* Open Data Applications */
+#define RDS_GROUP_TYPE_11B	23 /* Open Data Applications */
+#define RDS_GROUP_TYPE_12A	24 /* Open Data Applications */
+#define RDS_GROUP_TYPE_12B	25 /* Open Data Applications */
+#define RDS_GROUP_TYPE_13A	26 /* Enhanced Radio Paging or ODA */
+#define RDS_GROUP_TYPE_13B	27 /* Open Data Applications */
+#define RDS_GROUP_TYPE_14A	28 /* Enhanced Other Networks information */
+#define RDS_GROUP_TYPE_14B	29 /* Enhanced Other Networks information */
+#define RDS_GROUP_TYPE_15A	30 /* Defined in RBDS */
+#define RDS_GROUP_TYPE_15B	31 /* Fast switching information */
+#define NUM_DEFINED_RDS_GROUPS	32 /* Number of groups defined in RBDS
+				    * standard */
+
+/* Structure representing Generic packet of 64 bits. */
+struct rds_group_data {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u16 offsetC;	/* subject to group type */
+	u16 offsetD;	/* subject to group type */
+};
+
+/* Structure representing Group 0A (Service Name) */
+struct rds_group0A {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u8 freq[2];	/* alt frequency 0=1 */
+	u8 text[2];	/* Name segment */
+};
+
+/* Structure representing Group 0B (Service Name) */
+struct rds_group0B {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u16 piCode_dup;	/* Duplicate PI Code */
+	u8 text[2];	/* station text */
+};
+
+/* Structure representing Group 2A (Radio Text) (64 char) */
+struct rds_group2A {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u8 text[4];
+};
+
+/* Structure representing Group 2B (Radio Text) (32 char) */
+struct rds_group2B {
+	u16 piCode;	/* * Program ID */
+	u16 offsetB;	/* subject to group type */
+	u16 piCode_dup;	/* Duplicate PI Code */
+	u8 text[2];
+};
+
+/* Structure representing all groups */
+union rds_msg {
+	struct rds_group2B gt2B;
+	struct rds_group2A gt2A;
+	struct rds_group0B gt0B;
+	struct rds_group0A gt0A;
+	struct rds_group_data gt00;
+};
+
+struct stfm1000_rds_state {
+	struct stfm1000_rds_bitstream bitstream;
+	struct stfm1000_rds_demod demod;
+	struct stfm1000_rds_pkt pkt;
+	struct stfm1000_rds_text text;
+	unsigned int reset_req : 1;
+};
+
+/* callback from rds etc. */
+void stfm1000_rds_reset(struct stfm1000_rds_state *rds);
+void stfm1000_rds_start(struct stfm1000_rds_state *rds);
+void stfm1000_rds_stop(struct stfm1000_rds_state *rds);
+
+/* call these from the monitor thread, but with interrupts disabled */
+int stfm1000_rds_mix_msg_get(struct stfm1000_rds_state *rds);
+int stfm1000_rds_mix_msg_processed(struct stfm1000_rds_state *rds,
+	int mix_msg);
+int stfm1000_rds_sdnominal_msg_get(struct stfm1000_rds_state *rds);
+int stfm1000_rds_sdnominal_msg_processed(struct stfm1000_rds_state *rds,
+	int sdnominal_msg);
+int stfm1000_rds_bits_available(struct stfm1000_rds_state *rds);
+int stmf1000_rds_get_bit(struct stfm1000_rds_state *rds);
+
+/* called from audio handler (interrupt) */
+void stfm1000_rds_demod(struct stfm1000_rds_state *rds, const u16 *dri_data,
+	int total);
+
+/* call these from monitor thread, interrupts enabled */
+void stfm1000_rds_packet_bit(struct stfm1000_rds_state *rds, int bit);
+int stfm1000_rds_packet_dequeue(struct stfm1000_rds_state *rds, u8 *buf);
+void stfm1000_rds_process_packet(struct stfm1000_rds_state *rds, u8 *buffer);
+
+static inline int stfm1000_rds_get_reset_req(struct stfm1000_rds_state *rds)
+{
+	return rds->reset_req;
+}
+
+/* GROUP_TYPE 0A-0B */
+int stfm1000_rds_get_ps(struct stfm1000_rds_state *rds, u8 *buffer,
+	int bufsize);
+
+/* GROUP_TYPE 2A */
+int stfm1000_rds_get_text(struct stfm1000_rds_state *rds, u8 *buffer,
+	int bufsize);
+
+#endif
diff --git a/drivers/media/radio/stfm1000/stfm1000-regs.h b/drivers/media/radio/stfm1000/stfm1000-regs.h
new file mode 100644
index 000000000000..c9476b7d67e1
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000-regs.h
@@ -0,0 +1,165 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef STFM1000_REGS_H
+#define STFM1000_REGS_H
+
+/* registers */
+#define STFM1000_TUNE1			0x00
+#define STFM1000_SDNOMINAL		0x04
+#define STFM1000_PILOTTRACKING		0x08
+#define STFM1000_INITIALIZATION1	0x10
+#define STFM1000_INITIALIZATION2	0x14
+#define STFM1000_INITIALIZATION3	0x18
+#define STFM1000_INITIALIZATION4	0x1C
+#define STFM1000_INITIALIZATION5	0x20
+#define STFM1000_INITIALIZATION6	0x24
+#define STFM1000_REF			0x28
+#define STFM1000_LNA			0x2C
+#define STFM1000_MIXFILT		0x30
+#define STFM1000_CLK1			0x34
+#define STFM1000_CLK2			0x38
+#define STFM1000_ADC			0x3C
+#define STFM1000_AGC_CONTROL1		0x44
+#define STFM1000_AGC_CONTROL2		0x48
+#define STFM1000_DATAPATH		0x5C
+#define STFM1000_RMS			0x60
+#define STFM1000_AGC_STAT		0x64
+#define STFM1000_SIGNALQUALITY		0x68
+#define STFM1000_DCEST			0x6C
+#define STFM1000_RSSI_TONE		0x70
+#define STFM1000_PILOTCORRECTION	0x74
+#define STFM1000_ATTENTION		0x78
+#define STFM1000_CLK3			0x7C
+#define STFM1000_CHIPID			0x80
+
+/* number of registers */
+#define STFM1000_NUM_REGS		((0x80 + 4) / 4)
+
+#define STFM1000_FREQUENCY_100KHZ_MIN	758
+#define STFM1000_FREQUENCY_100KHZ_RANGE	325
+#define STFM1000_FREQUENCY_100KHZ_MAX	(STFM1000_FREQUENCY_100KHZ_MIN + \
+					STFM1000_FREQUENCY_100KHZ_RANGE)
+
+#define STFM1000_TUNE1_B2_MIX		0x001C0000
+#define STFM1000_TUNE1_CICOSR		0x00007E00
+#define STFM1000_TUNE1_PLL_DIV		0x000001FF
+
+#define STFM1000_CHIP_REV_TA1		0x00000001
+#define STFM1000_CHIP_REV_TA2		0x00000002
+#define STFM1000_CHIP_REV_TB1		0x00000011
+#define STFM1000_CHIP_REV_TB2		0x00000012
+
+/* DATAPATH bits we use */
+#define STFM1000_DP_EN			0x01000000
+#define STFM1000_DB_ACCEPT		0x00010000
+#define STFM1000_SAI_CLK_DIV_MASK	0x7c
+#define STFM1000_SAI_CLK_DIV_SHIFT	2
+#define STFM1000_SAI_CLK_DIV(x) \
+	(((x) << STFM1000_SAI_CLK_DIV_SHIFT) & STFM1000_SAI_CLK_DIV_MASK)
+#define STFM1000_SAI_EN			0x00000001
+
+/* AGC_CONTROL1 bits we use */
+#define STFM1000_B2_BYPASS_AGC_CTL	0x00004000
+#define STFM1000_B2_BYPASS_FILT_MASK	0x0000000C
+#define STFM1000_B2_BYPASS_FILT_SHIFT	2
+#define STFM1000_B2_BYPASS_FILT(x) \
+	(((x) << STFM1000_B2_BYPASS_FILT_SHIFT) & STFM1000_B2_BYPASS_FILT_MASK)
+#define STFM1000_B2_LNATH_MASK		0x001F0000
+#define STFM1000_B2_LNATH_SHIFT		16
+#define STFM1000_B2_LNATH(x) \
+	(((x) << STFM1000_B2_LNATH_SHIFT) & STFM1000_B2_LNATH_MASK)
+
+/* AGC_STAT bits we use */
+#define STFM1000_AGCOUT_STAT_MASK	0x1F000000
+#define STFM1000_AGCOUT_STAT_SHIFT	24
+#define STFM1000_LNA_RMS_MASK		0x00001F00
+#define STFM1000_LNA_RMS_SHIFT		8
+
+/* PILOTTRACKING bits we use */
+#define STFM1000_B2_PILOTTRACKING_EN		0x00008000
+#define STFM1000_B2_PILOTLPF_TIMECONSTANT_MASK	0x00000f00
+#define STFM1000_B2_PILOTLPF_TIMECONSTANT_SHIFT	8
+#define STFM1000_B2_PILOTLPF_TIMECONSTANT(x)	\
+	(((x) << STFM1000_B2_PILOTLPF_TIMECONSTANT_SHIFT) & \
+		STFM1000_B2_PILOTLPF_TIMECONSTANT_MASK)
+#define STFM1000_B2_PFDSCALE_MASK		0x000000f0
+#define STFM1000_B2_PFDSCALE_SHIFT		4
+#define STFM1000_B2_PFDSCALE(x)	\
+	(((x) << STFM1000_B2_PFDSCALE_SHIFT) & STFM1000_B2_PFDSCALE_MASK)
+#define STFM1000_B2_PFDFILTER_SPEEDUP_MASK	0x0000000f
+#define STFM1000_B2_PFDFILTER_SPEEDUP_SHIFT	0
+#define STFM1000_B2_PFDFILTER_SPEEDUP(x)	\
+	(((x) << STFM1000_B2_PFDFILTER_SPEEDUP_SHIFT) & \
+		STFM1000_B2_PFDFILTER_SPEEDUP_MASK)
+
+/* PILOTCORRECTION bits we use */
+#define STFM1000_PILOTEST_TA2_MASK	0xff000000
+#define STFM1000_PILOTEST_TA2_SHIFT	24
+#define STFM1000_PILOTEST_TB2_MASK	0xfe000000
+#define STFM1000_PILOTEST_TB2_SHIFT	25
+
+/* INITIALIZATION1 bits we use */
+#define STFM1000_B2_BYPASS_FILT_MASK	0x0000000C
+#define STFM1000_B2_BYPASS_FILT_SHIFT	2
+#define STFM1000_B2_BYPASS_FILT(x)	\
+	(((x) << STFM1000_B2_BYPASS_FILT_SHIFT) & STFM1000_B2_BYPASS_FILT_MASK)
+
+/* INITIALIZATION2 bits we use */
+#define STFM1000_DRI_CLK_EN		0x80000000
+#define STFM1000_DEEMPH_50_75B		0x00000100
+#define STFM1000_RDS_ENABLE		0x00100000
+#define STFM1000_RDS_MIXOFFSET		0x00200000
+
+/* INITIALIZATION3 bits we use */
+#define STFM1000_B2_NEAR_CHAN_MIX_MASK	0x1c000000
+#define STFM1000_B2_NEAR_CHAN_MIX_SHIFT	26
+#define STFM1000_B2_NEAR_CHAN_MIX(x)	\
+	(((x) << STFM1000_B2_NEAR_CHAN_MIX_SHIFT) & \
+		STFM1000_B2_NEAR_CHAN_MIX_MASK)
+
+/* CLK1 bits we use */
+#define STFM1000_ENABLE_TAPDELAYFIX	0x00000020
+
+/* REF bits we use */
+#define STFM1000_LNA_AMP1_IMPROVE_DISTORTION 0x08000000
+
+/* LNA bits we use */
+#define STFM1000_AMP2_IMPROVE_DISTORTION 0x08000000
+#define STFM1000_ANTENNA_TUNECAP_MASK	0x001F0000
+#define STFM1000_ANTENNA_TUNECAP_SHIFT	16
+#define STFM1000_ANTENNA_TUNECAP(x)	\
+	(((x) << STFM1000_ANTENNA_TUNECAP_SHIFT) & \
+		STFM1000_ANTENNA_TUNECAP_MASK)
+#define STFM1000_IBIAS2_UP		0x00000008
+#define STFM1000_IBIAS2_DN		0x00000004
+#define STFM1000_IBIAS1_UP		0x00000002
+#define STFM1000_IBIAS1_DN		0x00000001
+#define STFM1000_USEATTEN_MASK		0x00600000
+#define STFM1000_USEATTEN_SHIFT		21
+#define STFM1000_USEATTEN(x)	\
+	(((x) << STFM1000_USEATTEN_SHIFT) & STFM1000_USEATTEN_MASK)
+
+/* SIGNALQUALITY bits we use */
+#define STFM1000_NEAR_CHAN_AMPLITUDE_MASK	0x0000007F
+#define STFM1000_NEAR_CHAN_AMPLITUDE_SHIFT	0
+#define STFM1000_NEAR_CHAN_AMPLITUDE(x)	\
+	(((x) << STFM1000_NEAR_CHAN_AMPLITUDE_SHIFT) & \
+		STFM1000_NEAR_CHAN_AMPLITUDE_MASK)
+
+/* precalc tables elements */
+struct stfm1000_tune1 {
+	unsigned int tune1;	/* at least 32 bit */
+	unsigned int sdnom;
+};
+
+#endif
diff --git a/drivers/media/radio/stfm1000/stfm1000.h b/drivers/media/radio/stfm1000/stfm1000.h
new file mode 100644
index 000000000000..5ae6f38db3b0
--- /dev/null
+++ b/drivers/media/radio/stfm1000/stfm1000.h
@@ -0,0 +1,254 @@
+/*
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#ifndef STFM1000_H
+#define STFM1000_H
+
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <linux/i2c.h>
+#include <linux/list.h>
+#include <linux/wait.h>
+#include <linux/irq.h>
+#include <media/videobuf-dma-sg.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <mach/dma.h>
+
+#include "stfm1000-regs.h"
+
+#include "stfm1000-filter.h"
+#include "stfm1000-rds.h"
+
+struct stfm1000 {
+	struct list_head devlist;
+	int idx;
+
+	struct i2c_client *client;
+	struct video_device radio;
+
+	/* alsa */
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+	struct snd_pcm_substream *substream;
+	struct stmp3xxx_dma_descriptor *dma;
+	int desc_num;
+	int dma_ch;
+	int dma_irq;
+	int attn_irq;
+
+	struct mutex state_lock;
+	int read_count;
+	int read_offset;
+	int blocks;
+	int blksize;
+	int bufsize;
+
+	struct mutex deffered_work_lock;
+	struct execute_work snd_capture_start_work;
+	struct execute_work snd_capture_stop_work;
+
+	int now_recording;
+	int alsa_initialized;
+	int stopping_recording;
+
+	/* actual DRI buffer */
+	dma_addr_t dri_phys;
+	void *dri_buf;
+	int dri_bufsz;
+
+	/* various */
+	u16 curvol;
+	int users;
+	int removed;
+	struct mutex xfer_lock;
+	u8 revid;
+
+	unsigned int dbgflg;
+
+	/* shadow registers */
+	u32 shadow_regs[STFM1000_NUM_REGS];
+	u32 reg_rw_set[(STFM1000_NUM_REGS + 31) / 32];
+	u32 reg_ra_set[(STFM1000_NUM_REGS + 31) / 32];
+	u32 reg_dirty_set[(STFM1000_NUM_REGS + 31) / 32];
+
+	/* tuning parameters (not everything is used for now) */
+	u16 tune_rssi_th;		/* sd_ctl_TuneRssiTh_u16 */
+	u16 tune_mpx_dc_th;		/* sd_ctl_TuneMpxDcTh_u16 */
+	u16 adj_chan_th;		/* sd_ctl_AdjChanTh_u16 */
+	u16 pilot_est_th;		/* sd_ctl_PilotEstTh_u16 */
+	u16 coef_lna_turn_off_th;	/* sd_ctl_pCoefLnaTurnOffTh_u16 */
+	u16 coef_lna_turn_on_th;	/* sd_ctl_pCoefLnaTurnOnTh_u16 */
+	u16 reg_agc_ref_lna_off;	/* sd_ctl_pRegAgcRefLnaOff_u16 */
+	u16 reg_agc_ref_lna_on;		/* sd_ctl_pRegAgcRefLnaOn_u16 */
+
+	u32 sdnominal_pivot;		/* sd_ctl_SdnominalData_u32 */
+
+	/* jiffies of the next monitor cycle */
+	unsigned long next_quality_monitor;
+	unsigned long next_agc_monitor;
+
+	unsigned int mute : 1;			/* XXX */
+	unsigned int lna_driving : 1;		/* sd_ctl_LnaDriving_u1 */
+	unsigned int weak_signal : 1;		/* sd_ctl_WeakSignal_u1 */
+	unsigned int is_station : 1;		/* XXX */
+	unsigned int force_mono : 1;		/* XXX */
+	unsigned int signal_indicator : 1;	/* XXX */
+	unsigned int stereo_indicator : 1;	/* XXX */
+	unsigned int agc_monitor : 1;		/* XXX */
+	unsigned int quality_monitor : 1;	/* XXX */
+	unsigned int pilot_present : 1;		/* sd_ctl_PilotPresent_u1 */
+	unsigned int prev_pilot_present : 1;	/* XXX */
+	unsigned int stereo : 1;
+	unsigned int active : 1;		/* set when audio enabled */
+	unsigned int rds_enable : 1;		/* set when rds is enabled */
+	unsigned int rds_present : 1;		/* RDS info present */
+	unsigned int rds_sync : 1;		/* RDS force sync */
+	unsigned int rds_demod_running : 1;	/* RDS demod is running ATM */
+	unsigned int rds_sdnominal_adapt : 1;	/* adapt for better recept. */
+	unsigned int rds_phase_pop : 1;		/* enable phase pop */
+	unsigned int rds_info : 1;		/* print debugging info RDS */
+	unsigned int tuning_grid_50KHz : 1;	/* tuning grid of 50Khz */
+	u32 rssi;				/* rssi last decoded frame */
+	u16 rssi_dc_est_log;
+	u16 signal_strength;			/* is rssi_dc_est_log */
+	u16 rds_signal_th;			/* RDS threshold */
+	s16 mpx_dc;				/* sd_ctl_ShadowToneData_i16 */
+
+	u32 tune_cap_a_f;			/* float! sd_ctl_TuneCapA_f */
+	u32 tune_cap_b_f;			/* float! sd_ctl_TuneCapB_f */
+
+	int monitor_period;			/* period of the monitor */
+	int quality_monitor_period;		/* update period in ms */
+	int agc_monitor_period;			/* update period in ms */
+
+	int georegion;				/* current graphical region */
+
+	/* last tuned frequency */
+	int freq;				/* 88.0 = 8800 */
+
+	/* weak signal processing filter state */
+	struct stfm1000_filter_parms filter_parms;
+
+	/* state of rds */
+	spinlock_t rds_lock;
+	struct stfm1000_rds_state rds_state;
+	unsigned int rds_pkt_bad;
+	unsigned int rds_pkt_good;
+	unsigned int rds_pkt_recovered;
+	unsigned int rds_pkt_lost_sync;
+	unsigned int rds_bit_overruns;
+
+	/* monitor thread */
+	wait_queue_head_t thread_wait;
+	unsigned long thread_events;
+	struct task_struct *thread;
+};
+
+#define EVENT_RDS_BITS		0
+#define EVENT_RDS_MIXFILT	1
+#define EVENT_RDS_SDNOMINAL	2
+#define EVENT_RDS_RESET		3
+
+#define STFM1000_DBGFLG_I2C	(1 << 0)
+
+static inline struct stfm1000 *stfm1000_from_file(struct file *file)
+{
+	return container_of(video_devdata(file), struct stfm1000, radio);
+}
+
+/* in stfm1000-i2c.c */
+
+/* setup reg set */
+void stfm1000_setup_reg_set(struct stfm1000 *stfm1000);
+
+/* direct access to registers bypassing the shadow register set */
+int stfm1000_raw_read(struct stfm1000 *stfm1000, int reg, u32 *value);
+int stfm1000_raw_write(struct stfm1000 *stfm1000, int reg, u32 value);
+
+/* access using the shadow register set */
+int stfm1000_write(struct stfm1000 *stfm1000, int reg, u32 value);
+int stfm1000_read(struct stfm1000 *stfm1000, int reg, u32 *value);
+int stfm1000_write_masked(struct stfm1000 *stfm1000, int reg, u32 value,
+		u32 mask);
+int stfm1000_set_bits(struct stfm1000 *stfm1000, int reg, u32 value);
+int stfm1000_clear_bits(struct stfm1000 *stfm1000, int reg, u32 value);
+
+struct stfm1000_reg {
+	unsigned int regno;
+	u32 value;
+};
+
+#define STFM1000_REG_END	-1
+#define STFM1000_REG_DELAY	-2
+
+#define STFM1000_REG_SET_BITS_MASK	0x1000
+#define STFM1000_REG_CLEAR_BITS_MASK	0x2000
+
+#define STFM1000_REG(r, v) \
+	{ .regno = STFM1000_ ## r , .value = (v) }
+
+#define STFM1000_END \
+	{ .regno = STFM1000_REG_END }
+
+#define STFM1000_DELAY(x) \
+	{ .regno = STFM1000_REG_DELAY, .value = (x) }
+
+#define STFM1000_REG_SETBITS(r, v) \
+	{ .regno = STFM1000_ ## r | STFM1000_REG_SET_BITS_MASK, \
+		.value = (v) }
+
+#define STFM1000_REG_CLRBITS(r, v) \
+	{ .regno = STFM1000_ ## r | STFM1000_REG_CLEAR_BITS_MASK, \
+		.value = (v) }
+
+int stfm1000_write_regs(struct stfm1000 *stfm1000,
+		const struct stfm1000_reg *reg);
+
+/* in stfm1000-precalc.c */
+extern const struct stfm1000_tune1
+stfm1000_tune1_table[STFM1000_FREQUENCY_100KHZ_RANGE];
+
+/* exported for use by alsa driver */
+
+struct stfm1000_dri_sample {
+	/* L+R */
+	u16 l_plus_r;
+	/* L-R */
+	u16 l_minus_r;
+	/* Rx signal strength channel */
+	u16 rssi;
+	/* Radio data service channel */
+	u16 rds;
+};
+
+struct stfm1000_alsa_ops {
+	int  (*init)(struct stfm1000 *stfm1000);
+	void  (*release)(struct stfm1000 *stfm1000);
+	void (*dma_irq)(struct stfm1000 *stfm1000);
+	void (*attn_irq)(struct stfm1000 *stfm1000);
+};
+
+extern struct list_head stfm1000_devlist;
+extern struct stfm1000_alsa_ops *stfm1000_alsa_ops;
+
+/* needed for setting the interrupt handlers from alsa */
+irqreturn_t stfm1000_dri_attn_irq(int irq, void *dev_id);
+irqreturn_t stfm1000_dri_dma_irq(int irq, void *dev_id);
+void stfm1000_decode_block(struct stfm1000 *stfm1000, const s16 *src, s16 *dst, int count);
+void stfm1000_take_down(struct stfm1000 *stfm1000);
+void stfm1000_bring_up(struct stfm1000 *stfm1000);
+void stfm1000_tune_current(struct stfm1000 *stfm1000);
+
+void stfm1000_monitor_signal(struct stfm1000 *stfm1000, int bit);
+
+#endif
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index dcf9fa9264bb..7210c4f3c355 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -550,6 +550,45 @@ config VIDEO_W9966
 	  Check out <file:Documentation/video4linux/w9966.txt> for more
 	  information.
 
+config VIDEO_MXC_CAMERA
+	tristate "MXC Video For Linux Camera"
+	depends on VIDEO_DEV && ARCH_MXC
+	default y
+	---help---
+	  This is the video4linux2 capture driver based on MXC IPU/eMMA module.
+
+source "drivers/media/video/mxc/capture/Kconfig"
+
+config VIDEO_MXC_OUTPUT
+	tristate "MXC Video For Linux Video Output"
+	depends on VIDEO_DEV && ARCH_MXC
+	default y
+	---help---
+	  This is the video4linux2 output driver based on MXC IPU/eMMA module.
+
+source "drivers/media/video/mxc/output/Kconfig"
+
+config VIDEO_PXP
+	tristate "STMP3xxx PxP"
+	depends on VIDEO_DEV && VIDEO_V4L2 && ARCH_STMP3XXX
+	select VIDEOBUF_DMA_CONTIG
+	---help---
+	  This is a video4linux driver for the Freescale PxP
+	  (Pixel Pipeline). This module supports output overlay of
+	  the STMP3xxx framebuffer on a video stream.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called pxp.
+
+config VIDEO_MXC_OPL
+	tristate
+	depends on VIDEO_DEV && ARCH_MXC
+	default n
+	---help---
+	  This is the ARM9-optimized OPL (Open Primitives Library) software
+	  rotation/mirroring implementation. It may be used by eMMA video
+	  capture or output device.
+
 config VIDEO_CPIA
 	tristate "CPiA Video For Linux"
 	depends on VIDEO_V4L1
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index 9f2e3214a482..f361f073e356 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -88,6 +88,14 @@ obj-$(CONFIG_VIDEO_W9966) += w9966.o
 obj-$(CONFIG_VIDEO_PMS) += pms.o
 obj-$(CONFIG_VIDEO_VINO) += vino.o
 obj-$(CONFIG_VIDEO_STRADIS) += stradis.o
+obj-$(CONFIG_VIDEO_MXC_IPU_CAMERA) += mxc/capture/
+obj-$(CONFIG_VIDEO_MXC_EMMA_CAMERA) += mxc/capture/
+obj-$(CONFIG_VIDEO_MXC_CSI_CAMERA) += mxc/capture/
+obj-$(CONFIG_VIDEO_MXC_IPU_OUTPUT) += mxc/output/
+obj-$(CONFIG_VIDEO_MXC_IPUV1_WVGA_OUTPUT) += mxc/output/
+obj-$(CONFIG_VIDEO_MXC_EMMA_OUTPUT) += mxc/output/
+obj-$(CONFIG_VIDEO_MXC_OPL) += mxc/opl/
+obj-$(CONFIG_VIDEO_PXP)	+= pxp.o
 obj-$(CONFIG_VIDEO_CPIA) += cpia.o
 obj-$(CONFIG_VIDEO_CPIA_PP) += cpia_pp.o
 obj-$(CONFIG_VIDEO_CPIA_USB) += cpia_usb.o
diff --git a/drivers/media/video/mxc/capture/Kconfig b/drivers/media/video/mxc/capture/Kconfig
new file mode 100644
index 000000000000..adab0a886f36
--- /dev/null
+++ b/drivers/media/video/mxc/capture/Kconfig
@@ -0,0 +1,123 @@
+if VIDEO_MXC_CAMERA
+
+menu "MXC Camera/V4L2 PRP Features support"
+config VIDEO_MXC_IPU_CAMERA
+	bool
+	depends on VIDEO_MXC_CAMERA && MXC_IPU
+	default y
+
+config VIDEO_MXC_EMMA_CAMERA
+	tristate "MX27 eMMA support"
+	depends on VIDEO_MXC_CAMERA && MXC_EMMA && FB_MXC_SYNC_PANEL
+	select VIDEO_MXC_OPL
+	default y
+
+config VIDEO_MXC_CSI_CAMERA
+	tristate "MX25 CSI camera support"
+	depends on !VIDEO_MXC_EMMA_CAMERA
+
+config VIDEO_MXC_CSI_DMA
+	bool "CSI-DMA Still Image Capture support"
+	depends on VIDEO_MXC_EMMA_CAMERA
+	default n
+	---help---
+	  Use CSI-DMA method instead of CSI-PrP link to capture still image. This allows
+	  to use less physical contiguous memory to capture big resolution still image. But
+	  with this method the CSC (Color Space Conversion) and resize are not supported.
+	  If unsure, say N.
+
+choice
+	prompt "Select Camera/TV Decoder"
+	default MXC_CAMERA_OV3640
+	depends on VIDEO_MXC_CAMERA
+
+config MXC_CAMERA_MC521DA
+	tristate "Magnachip mc521da camera support"
+	select I2C_MXC
+	depends on VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the mc521da Camera with your MXC system, say Y here.
+
+config MXC_EMMA_CAMERA_MICRON111
+	tristate "Micron mt9v111 camera support with eMMA"
+	select I2C_MXC
+	depends on VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the mt9v111 Camera with your MXC system, say Y here.
+
+config MXC_CAMERA_OV2640_EMMA
+	tristate "OmniVision ov2640 camera support with eMMA"
+	depends on VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the ov2640 Camera with your MXC system, say Y here.
+
+config MXC_CAMERA_MICRON111
+	tristate "Micron mt9v111 camera support"
+	select I2C_MXC
+	depends on ! VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the mt9v111 Camera with your MXC system, say Y here.
+
+config MXC_CAMERA_OV2640
+	tristate "OmniVision ov2640 camera support"
+	depends on !VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the ov2640 Camera with your MXC system, say Y here.
+
+config MXC_CAMERA_OV3640
+	tristate "OmniVision ov3640 camera support"
+	depends on !VIDEO_MXC_EMMA_CAMERA
+	---help---
+	  If you plan to use the ov3640 Camera with your MXC system, say Y here.
+
+config MXC_TVIN_ADV7180
+	tristate "Analog Device adv7180 TV Decoder Input support"
+	depends on MACH_MX35_3DS
+	---help---
+	  If you plan to use the adv7180 video decoder with your MXC system, say Y here.
+
+endchoice
+
+config MXC_IPU_PRP_VF_SDC
+	tristate "Pre-Processor VF SDC library"
+	depends on VIDEO_MXC_IPU_CAMERA && FB_MXC_SYNC_PANEL
+	default y
+	---help---
+	  Use case PRP_VF_SDC:
+		Preprocessing image from smart sensor for viewfinder and
+		displaying it on synchronous display with SDC use case.
+		If SDC BG is selected, Rotation will not be supported.
+		CSI -> IC (PRP VF) -> MEM
+		MEM -> IC (ROT) -> MEM
+		MEM -> SDC (FG/BG)
+
+config MXC_IPU_PRP_VF_ADC
+	tristate "Pre-Processor VF ADC library"
+	depends on VIDEO_MXC_IPU_CAMERA && FB_MXC_ASYNC_PANEL
+	default y
+	---help---
+	  Use case PRP_VF_ADC:
+		Preprocessing image from smart sensor for viewfinder and
+		displaying it on asynchronous display.
+		CSI -> IC (PRP VF) -> ADC2
+
+config MXC_IPU_PRP_ENC
+	tristate "Pre-processor Encoder library"
+	depends on VIDEO_MXC_IPU_CAMERA
+	default y
+	---help---
+	  Use case PRP_ENC:
+		Preprocessing image from smart sensor for encoder.
+		CSI -> IC (PRP ENC) -> MEM
+
+config MXC_IPU_CSI_ENC
+	tristate "IPU CSI Encoder library"
+	depends on VIDEO_MXC_IPU_CAMERA
+	default y
+	---help---
+	  Use case IPU_CSI_ENC:
+		Get raw image with CSI from smart sensor for encoder.
+		CSI -> MEM
+endmenu
+
+endif
diff --git a/drivers/media/video/mxc/capture/Makefile b/drivers/media/video/mxc/capture/Makefile
new file mode 100644
index 000000000000..112923c8fc8f
--- /dev/null
+++ b/drivers/media/video/mxc/capture/Makefile
@@ -0,0 +1,39 @@
+ifeq ($(CONFIG_VIDEO_MXC_IPU_CAMERA),y)
+	obj-$(CONFIG_VIDEO_MXC_CAMERA) += mxc_v4l2_capture.o
+	obj-$(CONFIG_MXC_IPU_PRP_VF_ADC) += ipu_prp_vf_adc.o
+	obj-$(CONFIG_MXC_IPU_PRP_VF_SDC) += ipu_prp_vf_sdc.o ipu_prp_vf_sdc_bg.o
+	obj-$(CONFIG_MXC_IPU_PRP_ENC) += ipu_prp_enc.o ipu_still.o
+	obj-$(CONFIG_MXC_IPU_CSI_ENC) += ipu_csi_enc.o ipu_still.o
+endif
+
+obj-$(CONFIG_VIDEO_MXC_CSI_CAMERA) += fsl_csi.o csi_v4l2_capture.o
+
+mx27_capture-objs := mx27_prphw.o mx27_prpsw.o emma_v4l2_capture.o
+obj-$(CONFIG_VIDEO_MXC_EMMA_CAMERA) += mx27_csi.o mx27_capture.o
+
+mc521da_camera-objs := mc521da.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_MC521DA) += mc521da_camera.o
+
+emma_mt9v111_camera-objs := emma_mt9v111.o sensor_clock.o
+obj-$(CONFIG_MXC_EMMA_CAMERA_MICRON111) += emma_mt9v111_camera.o
+
+mt9v111_camera-objs := mt9v111.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_MICRON111) += mt9v111_camera.o
+
+hv7161_camera-objs := hv7161.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_HV7161) += hv7161_camera.o
+
+s5k3aaex_camera-objs := s5k3aaex.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_S5K3AAEX) += s5k3aaex_camera.o
+
+emma_ov2640_camera-objs := emma_ov2640.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_OV2640_EMMA) += emma_ov2640_camera.o
+
+ov2640_camera-objs := ov2640.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_OV2640) += ov2640_camera.o
+
+ov3640_camera-objs := ov3640.o sensor_clock.o
+obj-$(CONFIG_MXC_CAMERA_OV3640) += ov3640_camera.o
+
+adv7180_tvin-objs := adv7180.o sensor_clock.o
+obj-$(CONFIG_MXC_TVIN_ADV7180) += adv7180_tvin.o
diff --git a/drivers/media/video/mxc/capture/adv7180.c b/drivers/media/video/mxc/capture/adv7180.c
new file mode 100644
index 000000000000..07a68ecaa0a5
--- /dev/null
+++ b/drivers/media/video/mxc/capture/adv7180.c
@@ -0,0 +1,981 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file adv7180.c
+ *
+ * @brief Analog Device ADV7180 video decoder functions
+ *
+ * @ingroup Camera
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/wait.h>
+#include <linux/videodev2.h>
+#include <linux/workqueue.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+
+static struct regulator *dvddio_regulator;
+static struct regulator *dvdd_regulator;
+static struct regulator *avdd_regulator;
+static struct regulator *pvdd_regulator;
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+static int adv7180_probe(struct i2c_client *adapter,
+			 const struct i2c_device_id *id);
+static int adv7180_detach(struct i2c_client *client);
+
+static const struct i2c_device_id adv7180_id[] = {
+	{"adv7180", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, adv7180_id);
+
+static struct i2c_driver adv7180_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "adv7180",
+		   },
+	.probe = adv7180_probe,
+	.remove = adv7180_detach,
+	.id_table = adv7180_id,
+};
+
+/*!
+ * Maintains the information on the current state of the sesor.
+ */
+struct sensor {
+	struct v4l2_int_device *v4l2_int_device;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	struct v4l2_captureparm streamcap;
+	bool on;
+
+	/* control settings */
+	int brightness;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+	v4l2_std_id std_id;
+} adv7180_data;
+
+/*! List of input video formats supported. The video formats is corresponding
+ * with v4l2 id in video_fmt_t
+ */
+typedef enum {
+	ADV7180_NTSC = 0,	/*!< Locked on (M) NTSC video signal. */
+	ADV7180_PAL,		/*!< (B, G, H, I, N)PAL video signal. */
+	ADV7180_NOT_LOCKED,	/*!< Not locked on a signal. */
+} video_fmt_idx;
+
+/*! Number of video standards supported (including 'not locked' signal). */
+#define ADV7180_STD_MAX		(ADV7180_PAL + 1)
+
+/*! Video format structure. */
+typedef struct {
+	int v4l2_id;		/*!< Video for linux ID. */
+	char name[16];		/*!< Name (e.g., "NTSC", "PAL", etc.) */
+	u16 raw_width;		/*!< Raw width. */
+	u16 raw_height;		/*!< Raw height. */
+	u16 active_width;	/*!< Active width. */
+	u16 active_height;	/*!< Active height. */
+} video_fmt_t;
+
+/*! Description of video formats supported.
+ *
+ *  PAL: raw=720x625, active=720x576.
+ *  NTSC: raw=720x525, active=720x480.
+ */
+static video_fmt_t video_fmts[] = {
+	{			/*! NTSC */
+	 .v4l2_id = V4L2_STD_NTSC,
+	 .name = "NTSC",
+	 .raw_width = 720 - 1,	/* SENS_FRM_WIDTH */
+	 .raw_height = 288 - 1,	/* SENS_FRM_HEIGHT */
+	 .active_width = 720,	/* ACT_FRM_WIDTH plus 1 */
+	 .active_height = (480 / 2),	/* ACT_FRM_WIDTH plus 1 */
+	 },
+	{			/*! (B, G, H, I, N) PAL */
+	 .v4l2_id = V4L2_STD_PAL,
+	 .name = "PAL",
+	 .raw_width = 720 - 1,
+	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .active_width = 720,
+	 .active_height = (576 / 2),
+	 },
+	{			/*! Unlocked standard */
+	 .v4l2_id = V4L2_STD_ALL,
+	 .name = "Autodetect",
+	 .raw_width = 720 - 1,
+	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .active_width = 720,
+	 .active_height = (576 / 2),
+	 },
+};
+
+/*!* Standard index of ADV7180. */
+static video_fmt_idx video_idx = ADV7180_PAL;
+
+/*! @brief This mutex is used to provide mutual exclusion.
+ *
+ *  Create a mutex that can be used to provide mutually exclusive
+ *  read/write access to the globally accessible data structures
+ *  and variables that were defined above.
+ */
+static DECLARE_MUTEX(mutex);
+
+#define IF_NAME                    "adv7180"
+#define ADV7180_INPUT_CTL              0x00	/* Input Control */
+#define ADV7180_STATUS_1               0x10	/* Status #1 */
+#define ADV7180_BRIGHTNESS             0x0a	/* Brightness */
+#define ADV7180_IDENT                  0x11	/* IDENT */
+#define ADV7180_VSYNC_FIELD_CTL_1      0x31	/* VSYNC Field Control #1 */
+#define ADV7180_MANUAL_WIN_CTL         0x3d	/* Manual Window Control */
+#define ADV7180_SD_SATURATION_CB       0xe3	/* SD Saturation Cb */
+#define ADV7180_SD_SATURATION_CR       0xe4	/* SD Saturation Cr */
+#define ADV7180_PWR_MNG                0x0f     /* Power Management */
+
+/* supported controls */
+/* This hasn't been fully implemented yet.
+ * This is how it should work, though. */
+static struct v4l2_queryctrl adv7180_qctrl[] = {
+	{
+	.id = V4L2_CID_BRIGHTNESS,
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.name = "Brightness",
+	.minimum = 0,		/* check this value */
+	.maximum = 255,		/* check this value */
+	.step = 1,		/* check this value */
+	.default_value = 127,	/* check this value */
+	.flags = 0,
+	}, {
+	.id = V4L2_CID_SATURATION,
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.name = "Saturation",
+	.minimum = 0,		/* check this value */
+	.maximum = 255,		/* check this value */
+	.step = 0x1,		/* check this value */
+	.default_value = 127,	/* check this value */
+	.flags = 0,
+	}
+};
+
+/***********************************************************************
+ * I2C transfert.
+ ***********************************************************************/
+
+/*! Read one register from a ADV7180 i2c slave device.
+ *
+ *  @param *reg		register in the device we wish to access.
+ *
+ *  @return		       0 if success, an error code otherwise.
+ */
+static inline int adv7180_read(u8 reg)
+{
+	int val;
+	val = i2c_smbus_read_byte_data(adv7180_data.i2c_client, reg);
+	if (val < 0) {
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"%s:read reg error: reg=%2x \n", __func__, reg);
+		return -1;
+	}
+	return val;
+}
+
+/*! Write one register of a ADV7180 i2c slave device.
+ *
+ *  @param *reg		register in the device we wish to access.
+ *
+ *  @return		       0 if success, an error code otherwise.
+ */
+static int adv7180_write_reg(u8 reg, u8 val)
+{
+	if (i2c_smbus_write_byte_data(adv7180_data.i2c_client, reg, val) < 0) {
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"%s:write reg error:reg=%2x,val=%2x\n", __func__,
+			reg, val);
+		return -1;
+	}
+	return 0;
+}
+
+/***********************************************************************
+ * mxc_v4l2_capture interface.
+ ***********************************************************************/
+
+/*!
+ * Return attributes of current video standard.
+ * Since this device autodetects the current standard, this function also
+ * sets the values that need to be changed if the standard changes.
+ * There is no set std equivalent function.
+ *
+ *  @return		None.
+ */
+static void adv7180_get_std(v4l2_std_id *std)
+{
+	int tmp;
+	int idx;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_get_std\n");
+
+	/* Read the AD_RESULT to get the detect output video standard */
+	tmp = adv7180_read(ADV7180_STATUS_1) & 0x70;
+
+	down(&mutex);
+	if (tmp == 0x40) {
+		/* PAL */
+		*std = V4L2_STD_PAL;
+		idx = ADV7180_PAL;
+	} else if (tmp == 0) {
+		/*NTSC*/
+		*std = V4L2_STD_NTSC;
+		idx = ADV7180_NTSC;
+	} else {
+		*std = V4L2_STD_ALL;
+		idx = ADV7180_NOT_LOCKED;
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"Got invalid video standard! \n");
+	}
+	up(&mutex);
+
+	/* This assumes autodetect which this device uses. */
+	if (*std != adv7180_data.std_id) {
+		video_idx = idx;
+		adv7180_data.std_id = *std;
+		adv7180_data.pix.width = video_fmts[video_idx].raw_width;
+		adv7180_data.pix.height = video_fmts[video_idx].raw_height;
+	}
+}
+
+/***********************************************************************
+ * IOCTL Functions from v4l2_int_ioctl_desc.
+ ***********************************************************************/
+
+/*!
+ * ioctl_g_ifparm - V4L2 sensor interface handler for vidioc_int_g_ifparm_num
+ * s: pointer to standard V4L2 device structure
+ * p: pointer to standard V4L2 vidioc_int_g_ifparm_num ioctl structure
+ *
+ * Gets slave interface parameters.
+ * Calculates the required xclk value to support the requested
+ * clock parameters in p.  This value is returned in the p
+ * parameter.
+ *
+ * vidioc_int_g_ifparm returns platform-specific information about the
+ * interface settings used by the sensor.
+ *
+ * Called on open.
+ */
+static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_ifparm\n");
+
+	if (s == NULL) {
+		pr_err("   ERROR!! no slave device set!\n");
+		return -1;
+	}
+
+	/* Initialize structure to 0s then set any non-0 values. */
+	memset(p, 0, sizeof(*p));
+	p->if_type = V4L2_IF_TYPE_BT656; /* This is the only possibility. */
+	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.nobt_hs_inv = 1;
+
+	/* ADV7180 has a dedicated clock so no clock settings needed. */
+
+	return 0;
+}
+
+/*!
+ * Sets the camera power.
+ *
+ * s  pointer to the camera device
+ * on if 1, power is to be turned on.  0 means power is to be turned off
+ *
+ * ioctl_s_power - V4L2 sensor interface handler for vidioc_int_s_power_num
+ * @s: pointer to standard V4L2 device structure
+ * @on: power state to which device is to be set
+ *
+ * Sets devices power state to requrested state, if possible.
+ * This is called on open, close, suspend and resume.
+ */
+static int ioctl_s_power(struct v4l2_int_device *s, int on)
+{
+	struct sensor *sensor = s->priv;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_s_power\n");
+
+	if (on && !sensor->on) {
+		gpio_sensor_active();
+		if (adv7180_write_reg(ADV7180_PWR_MNG, 0) != 0)
+			return -EIO;
+	} else if (!on && sensor->on) {
+		if (adv7180_write_reg(ADV7180_PWR_MNG, 0x24) != 0)
+			return -EIO;
+		gpio_sensor_inactive();
+	}
+
+	sensor->on = on;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_parm - V4L2 sensor interface handler for VIDIOC_G_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the sensor's video CAPTURE parameters.
+ */
+static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_parm\n");
+
+	switch (a->type) {
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = sensor->streamcap.capability;
+		cparm->timeperframe = sensor->streamcap.timeperframe;
+		cparm->capturemode = sensor->streamcap.capturemode;
+		break;
+
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		break;
+
+	default:
+		pr_debug("ioctl_g_parm:type is unknown %d\n", a->type);
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ *
+ * This driver cannot change these settings.
+ */
+static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_s_parm\n");
+
+	switch (a->type) {
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		break;
+
+	default:
+		pr_debug("   type is unknown - %d\n", a->type);
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_fmt_cap - V4L2 sensor interface handler for ioctl_g_fmt_cap
+ * @s: pointer to standard V4L2 device structure
+ * @f: pointer to standard V4L2 v4l2_format structure
+ *
+ * Returns the sensor's current pixel format in the v4l2_format
+ * parameter.
+ */
+static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_fmt_cap\n");
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   Returning size of %dx%d\n",
+			 sensor->pix.width, sensor->pix.height);
+		f->fmt.pix = sensor->pix;
+		break;
+
+	case V4L2_BUF_TYPE_PRIVATE: {
+		v4l2_std_id std;
+		adv7180_get_std(&std);
+		f->fmt.pix.pixelformat = (u32)std;
+		}
+		break;
+
+	default:
+		f->fmt.pix = sensor->pix;
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_queryctrl - V4L2 sensor interface handler for VIDIOC_QUERYCTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @qc: standard V4L2 VIDIOC_QUERYCTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control information
+ * from the video_control[] array.  Otherwise, returns -EINVAL if the
+ * control is not supported.
+ */
+static int ioctl_queryctrl(struct v4l2_int_device *s,
+			   struct v4l2_queryctrl *qc)
+{
+	int i;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_queryctrl\n");
+
+	for (i = 0; i < ARRAY_SIZE(adv7180_qctrl); i++)
+		if (qc->id && qc->id == adv7180_qctrl[i].id) {
+			memcpy(qc, &(adv7180_qctrl[i]),
+				sizeof(*qc));
+			return (0);
+		}
+
+	return -EINVAL;
+}
+
+/*!
+ * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_G_CTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control's current
+ * value from the video_control[] array.  Otherwise, returns -EINVAL
+ * if the control is not supported.
+ */
+static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int ret = 0;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_ctrl\n");
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_BRIGHTNESS\n");
+		adv7180_data.brightness = adv7180_read(ADV7180_BRIGHTNESS);
+		vc->value = adv7180_data.brightness;
+		break;
+	case V4L2_CID_CONTRAST:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_CONTRAST\n");
+		vc->value = adv7180_data.contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_SATURATION\n");
+		adv7180_data.saturation = adv7180_read(ADV7180_SD_SATURATION_CB);
+		vc->value = adv7180_data.saturation;
+		break;
+	case V4L2_CID_HUE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_HUE\n");
+		vc->value = adv7180_data.hue;
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_RED_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_RED_BALANCE\n");
+		vc->value = adv7180_data.red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_BLUE_BALANCE\n");
+		vc->value = adv7180_data.blue;
+		break;
+	case V4L2_CID_GAMMA:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_GAMMA\n");
+		break;
+	case V4L2_CID_EXPOSURE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_EXPOSURE\n");
+		vc->value = adv7180_data.ae_mode;
+		break;
+	case V4L2_CID_AUTOGAIN:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_AUTOGAIN\n");
+		break;
+	case V4L2_CID_GAIN:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_GAIN\n");
+		break;
+	case V4L2_CID_HFLIP:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_HFLIP\n");
+		break;
+	case V4L2_CID_VFLIP:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_VFLIP\n");
+		break;
+	default:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   Default case\n");
+		vc->value = 0;
+		ret = -EPERM;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_ctrl - V4L2 sensor interface handler for VIDIOC_S_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_S_CTRL ioctl structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW (and updates the video_control[] array).  Otherwise,
+ * returns -EINVAL if the control is not supported.
+ */
+static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int retval = 0;
+	u8 tmp;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_s_ctrl\n");
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_BRIGHTNESS\n");
+		tmp = vc->value;
+		adv7180_write_reg(ADV7180_BRIGHTNESS, tmp);
+		adv7180_data.brightness = vc->value;
+		break;
+	case V4L2_CID_CONTRAST:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_CONTRAST\n");
+		break;
+	case V4L2_CID_SATURATION:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_SATURATION\n");
+		tmp = vc->value;
+		adv7180_write_reg(ADV7180_SD_SATURATION_CB, tmp);
+		adv7180_write_reg(ADV7180_SD_SATURATION_CR, tmp);
+		adv7180_data.saturation = vc->value;
+		break;
+	case V4L2_CID_HUE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_HUE\n");
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_RED_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_RED_BALANCE\n");
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_BLUE_BALANCE\n");
+		break;
+	case V4L2_CID_GAMMA:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_GAMMA\n");
+		break;
+	case V4L2_CID_EXPOSURE:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_EXPOSURE\n");
+		break;
+	case V4L2_CID_AUTOGAIN:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_AUTOGAIN\n");
+		break;
+	case V4L2_CID_GAIN:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_GAIN\n");
+		break;
+	case V4L2_CID_HFLIP:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_HFLIP\n");
+		break;
+	case V4L2_CID_VFLIP:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   V4L2_CID_VFLIP\n");
+		break;
+	default:
+		dev_dbg(&adv7180_data.i2c_client->dev,
+			"   Default case\n");
+		retval = -EPERM;
+		break;
+	}
+
+	return retval;
+}
+
+/*!
+ * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
+ * @s: pointer to standard V4L2 device structure
+ */
+static int ioctl_init(struct v4l2_int_device *s)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_init\n");
+	return 0;
+}
+
+/*!
+ * ioctl_dev_init - V4L2 sensor interface handler for vidioc_int_dev_init_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Initialise the device when slave attaches to the master.
+ */
+static int ioctl_dev_init(struct v4l2_int_device *s)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_dev_init\n");
+	return 0;
+}
+
+/*!
+ * This structure defines all the ioctls for this module.
+ */
+static struct v4l2_int_ioctl_desc adv7180_ioctl_desc[] = {
+
+	{vidioc_int_dev_init_num, (v4l2_int_ioctl_func *)ioctl_dev_init},
+
+	/*!
+	 * Delinitialise the dev. at slave detach.
+	 * The complement of ioctl_dev_init.
+	 */
+/*	{vidioc_int_dev_exit_num, (v4l2_int_ioctl_func *)ioctl_dev_exit}, */
+
+	{vidioc_int_s_power_num, (v4l2_int_ioctl_func *)ioctl_s_power},
+	{vidioc_int_g_ifparm_num, (v4l2_int_ioctl_func *)ioctl_g_ifparm},
+/*	{vidioc_int_g_needs_reset_num,
+				(v4l2_int_ioctl_func *)ioctl_g_needs_reset}, */
+/*	{vidioc_int_reset_num, (v4l2_int_ioctl_func *)ioctl_reset}, */
+	{vidioc_int_init_num, (v4l2_int_ioctl_func *)ioctl_init},
+
+	/*!
+	 * VIDIOC_ENUM_FMT ioctl for the CAPTURE buffer type.
+	 */
+/*	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_enum_fmt_cap}, */
+
+	/*!
+	 * VIDIOC_TRY_FMT ioctl for the CAPTURE buffer type.
+	 * This ioctl is used to negotiate the image capture size and
+	 * pixel format without actually making it take effect.
+	 */
+/*	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_try_fmt_cap}, */
+
+	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_g_fmt_cap},
+
+	/*!
+	 * If the requested format is supported, configures the HW to use that
+	 * format, returns error code if format not supported or HW can't be
+	 * correctly configured.
+	 */
+/*	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap}, */
+
+	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *)ioctl_g_parm},
+	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *)ioctl_s_parm},
+	{vidioc_int_queryctrl_num, (v4l2_int_ioctl_func *)ioctl_queryctrl},
+	{vidioc_int_g_ctrl_num, (v4l2_int_ioctl_func *)ioctl_g_ctrl},
+	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *)ioctl_s_ctrl},
+};
+
+static struct v4l2_int_slave adv7180_slave = {
+	.ioctls = adv7180_ioctl_desc,
+	.num_ioctls = ARRAY_SIZE(adv7180_ioctl_desc),
+};
+
+static struct v4l2_int_device adv7180_int_device = {
+	.module = THIS_MODULE,
+	.name = "adv7180",
+	.type = v4l2_int_type_slave,
+	.u = {
+		.slave = &adv7180_slave,
+	},
+};
+
+
+/***********************************************************************
+ * I2C client and driver.
+ ***********************************************************************/
+
+/*! ADV7180 Reset function.
+ *
+ *  @return		None.
+ */
+static void adv7180_hard_reset(void)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev,
+		"In adv7180:adv7180_hard_reset\n");
+
+	/*! Driver works fine without explicit register
+	 * initialization. Furthermore, initializations takes about 2 seconds
+	 * at startup...
+	 */
+
+	/*! Set YPbPr input on AIN1,4,5 and normal
+	 * operations(autodection of all stds).
+	 */
+	adv7180_write_reg(ADV7180_INPUT_CTL, 0x09);
+
+	/*! Datasheet recommends: */
+	adv7180_write_reg(ADV7180_VSYNC_FIELD_CTL_1, 0x02);
+	adv7180_write_reg(ADV7180_MANUAL_WIN_CTL, 0xa2);
+}
+
+/*! ADV7180 I2C attach function.
+ *
+ *  @param *adapter	struct i2c_adapter *.
+ *
+ *  @return		Error code indicating success or failure.
+ */
+
+/*!
+ * ADV7180 I2C probe function.
+ * Function set in i2c_driver struct.
+ * Called by insmod.
+ *
+ *  @param *adapter	I2C adapter descriptor.
+ *
+ *  @return		Error code indicating success or failure.
+ */
+static int adv7180_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	int rev_id;
+	int ret = 0;
+	struct mxc_tvin_platform_data *plat_data = client->dev.platform_data;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_probe\n");
+
+	if (plat_data->dvddio_reg) {
+		dvddio_regulator =
+		    regulator_get(&client->dev, plat_data->dvddio_reg);
+		if (!IS_ERR_VALUE((unsigned long)dvddio_regulator)) {
+			regulator_set_voltage(dvddio_regulator, 3300000, 3300000);
+			if (regulator_enable(dvddio_regulator) != 0)
+				return -ENODEV;
+		}
+	}
+
+	if (plat_data->dvdd_reg) {
+		dvdd_regulator =
+		    regulator_get(&client->dev, plat_data->dvdd_reg);
+		if (!IS_ERR_VALUE((unsigned long)dvdd_regulator)) {
+			regulator_set_voltage(dvdd_regulator, 1800000, 1800000);
+			if (regulator_enable(dvdd_regulator) != 0)
+				return -ENODEV;
+		}
+	}
+
+	if (plat_data->avdd_reg) {
+		avdd_regulator =
+		    regulator_get(&client->dev, plat_data->avdd_reg);
+		if (!IS_ERR_VALUE((unsigned long)avdd_regulator)) {
+			regulator_set_voltage(avdd_regulator, 1800000, 1800000);
+			if (regulator_enable(avdd_regulator) != 0)
+				return -ENODEV;
+		}
+	}
+
+	if (plat_data->pvdd_reg) {
+		pvdd_regulator =
+		    regulator_get(&client->dev, plat_data->pvdd_reg);
+		if (!IS_ERR_VALUE((unsigned long)pvdd_regulator)) {
+			regulator_set_voltage(pvdd_regulator, 1800000, 1800000);
+			if (regulator_enable(pvdd_regulator) != 0)
+				return -ENODEV;
+		}
+	}
+
+	if (plat_data->reset)
+		plat_data->reset();
+
+	if (plat_data->pwdn)
+		plat_data->pwdn(1);
+
+	msleep(1);
+
+	/* Set initial values for the sensor struct. */
+	memset(&adv7180_data, 0, sizeof(adv7180_data));
+	adv7180_data.i2c_client = client;
+	adv7180_data.streamcap.timeperframe.denominator = 30;
+	adv7180_data.streamcap.timeperframe.numerator = 1;
+	adv7180_data.std_id = V4L2_STD_ALL;
+	video_idx = ADV7180_NOT_LOCKED;
+	adv7180_data.pix.width = video_fmts[video_idx].raw_width;
+	adv7180_data.pix.height = video_fmts[video_idx].raw_height;
+	adv7180_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;  /* YUV422 */
+	adv7180_data.pix.priv = 1;  /* 1 is used to indicate TV in */
+	adv7180_data.on = true;
+
+	gpio_sensor_active();
+
+	dev_dbg(&adv7180_data.i2c_client->dev,
+		"%s:adv7180 probe i2c address is 0x%02X \n",
+		__func__, adv7180_data.i2c_client->addr);
+
+	/*! Read the revision ID of the tvin chip */
+	rev_id = adv7180_read(ADV7180_IDENT);
+	dev_dbg(&adv7180_data.i2c_client->dev,
+		"%s:Analog Device adv7%2X0 detected! \n", __func__,
+		rev_id);
+
+	/*! ADV7180 initialization. */
+	adv7180_hard_reset();
+
+	pr_debug("   type is %d (expect %d)\n",
+		 adv7180_int_device.type, v4l2_int_type_slave);
+	pr_debug("   num ioctls is %d\n",
+		 adv7180_int_device.u.slave->num_ioctls);
+
+	/* This function attaches this structure to the /dev/video0 device.
+	 * The pointer in priv points to the mt9v111_data structure here.*/
+	adv7180_int_device.priv = &adv7180_data;
+	ret = v4l2_int_device_register(&adv7180_int_device);
+
+	return ret;
+}
+
+/*!
+ * ADV7180 I2C detach function.
+ * Called on rmmod.
+ *
+ *  @param *client	struct i2c_client*.
+ *
+ *  @return		Error code indicating success or failure.
+ */
+static int adv7180_detach(struct i2c_client *client)
+{
+	struct mxc_tvin_platform_data *plat_data = client->dev.platform_data;
+
+	dev_dbg(&adv7180_data.i2c_client->dev,
+		"%s:Removing %s video decoder @ 0x%02X from adapter %s \n",
+		__func__, IF_NAME, client->addr << 1, client->adapter->name);
+
+	if (plat_data->pwdn)
+		plat_data->pwdn(0);
+
+	if (dvddio_regulator) {
+		regulator_disable(dvddio_regulator);
+		regulator_put(dvddio_regulator);
+	}
+
+	if (dvdd_regulator) {
+		regulator_disable(dvdd_regulator);
+		regulator_put(dvdd_regulator);
+	}
+
+	if (avdd_regulator) {
+		regulator_disable(avdd_regulator);
+		regulator_put(avdd_regulator);
+	}
+
+	if (pvdd_regulator) {
+		regulator_disable(pvdd_regulator);
+		regulator_put(pvdd_regulator);
+	}
+
+	v4l2_int_device_unregister(&adv7180_int_device);
+
+	return 0;
+}
+
+/*!
+ * ADV7180 init function.
+ * Called on insmod.
+ *
+ * @return    Error code indicating success or failure.
+ */
+static __init int adv7180_init(void)
+{
+	u8 err = 0;
+
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_init\n");
+
+	/* Tells the i2c driver what functions to call for this driver. */
+	err = i2c_add_driver(&adv7180_i2c_driver);
+	if (err != 0)
+		pr_err("%s:driver registration failed, error=%d \n",
+			__func__, err);
+
+	return err;
+}
+
+/*!
+ * ADV7180 cleanup function.
+ * Called on rmmod.
+ *
+ * @return   Error code indicating success or failure.
+ */
+static void __exit adv7180_clean(void)
+{
+	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_clean\n");
+	i2c_del_driver(&adv7180_i2c_driver);
+	gpio_sensor_inactive();
+}
+
+module_init(adv7180_init);
+module_exit(adv7180_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor");
+MODULE_DESCRIPTION("Anolog Device ADV7180 video decoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/csi_v4l2_capture.c b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
new file mode 100644
index 000000000000..3266d2500081
--- /dev/null
+++ b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
@@ -0,0 +1,1024 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/media/video/mxc/capture/csi_v4l2_capture.c
+ * This file is derived from mxc_v4l2_capture.c
+ *
+ * @brief MX25 Video For Linux 2 driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/io.h>
+#include <linux/semaphore.h>
+#include <linux/pagemap.h>
+#include <linux/vmalloc.h>
+#include <linux/types.h>
+#include <linux/fb.h>
+#include <linux/dma-mapping.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-int-device.h>
+#include <linux/mxcfb.h>
+#include "mxc_v4l2_capture.h"
+#include "fsl_csi.h"
+
+static int video_nr = -1;
+static cam_data *g_cam;
+
+static int csi_v4l2_master_attach(struct v4l2_int_device *slave);
+static void csi_v4l2_master_detach(struct v4l2_int_device *slave);
+static u8 camera_power(cam_data *cam, bool cameraOn);
+
+/*! Information about this driver. */
+static struct v4l2_int_master csi_v4l2_master = {
+	.attach = csi_v4l2_master_attach,
+	.detach = csi_v4l2_master_detach,
+};
+
+static struct v4l2_int_device csi_v4l2_int_device = {
+	.module = THIS_MODULE,
+	.name = "csi_v4l2_cap",
+	.type = v4l2_int_type_master,
+	.u = {
+	      .master = &csi_v4l2_master,
+	      },
+};
+
+/*!
+ * Indicates whether the palette is supported.
+ *
+ * @param palette V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_UYVY or V4L2_PIX_FMT_YUV420
+ *
+ * @return 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return (palette == V4L2_PIX_FMT_RGB565) ||
+	    (palette == V4L2_PIX_FMT_UYVY) || (palette == V4L2_PIX_FMT_YUV420);
+}
+
+/*!
+ * start the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int start_preview(cam_data *cam)
+{
+	unsigned long fb_addr = (unsigned long)cam->v4l2_fb.base;
+
+	__raw_writel(fb_addr, CSI_CSIDMASA_FB1);
+	__raw_writel(fb_addr, CSI_CSIDMASA_FB2);
+	__raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+
+	csi_enable_int(0);
+
+	return 0;
+}
+
+/*!
+ * shut down the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int stop_preview(cam_data *cam)
+{
+	csi_disable_int();
+
+	/* set CSI_CSIDMASA_FB1 and CSI_CSIDMASA_FB2 to default value */
+	__raw_writel(0, CSI_CSIDMASA_FB1);
+	__raw_writel(0, CSI_CSIDMASA_FB2);
+	__raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+
+	return 0;
+}
+
+/***************************************************************************
+ * VIDIOC Functions.
+ **************************************************************************/
+
+/*!
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int csi_v4l2_g_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		f->fmt.pix = cam->v2f.fmt.pix;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
+		f->fmt.win = cam->win;
+		break;
+	default:
+		pr_debug("   type is invalid\n");
+		retval = -EINVAL;
+	}
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__, cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+
+	return retval;
+}
+
+/*!
+ * V4L2 - csi_v4l2_s_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int csi_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+	int size = 0;
+	int bytesperline = 0;
+	int *width, *height;
+
+	pr_debug("In MVC: %s\n", __func__);
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		if (!valid_mode(f->fmt.pix.pixelformat)) {
+			pr_err("ERROR: v4l2 capture: %s: format "
+			       "not supported\n", __func__);
+			return -EINVAL;
+		}
+
+		/* Handle case where size requested is larger than cuurent
+		 * camera setting. */
+		if ((f->fmt.pix.width > cam->crop_bounds.width)
+		    || (f->fmt.pix.height > cam->crop_bounds.height)) {
+			/* Need the logic here, calling vidioc_s_param if
+			 * camera can change. */
+			pr_debug("csi_v4l2_s_fmt size changed\n");
+		}
+		if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+			height = &f->fmt.pix.width;
+			width = &f->fmt.pix.height;
+		} else {
+			width = &f->fmt.pix.width;
+			height = &f->fmt.pix.height;
+		}
+
+		if ((cam->crop_bounds.width / *width > 8) ||
+		    ((cam->crop_bounds.width / *width == 8) &&
+		     (cam->crop_bounds.width % *width))) {
+			*width = cam->crop_bounds.width / 8;
+			if (*width % 8)
+				*width += 8 - *width % 8;
+			pr_err("ERROR: v4l2 capture: width exceeds limit "
+			       "resize to %d.\n", *width);
+		}
+
+		if ((cam->crop_bounds.height / *height > 8) ||
+		    ((cam->crop_bounds.height / *height == 8) &&
+		     (cam->crop_bounds.height % *height))) {
+			*height = cam->crop_bounds.height / 8;
+			if (*height % 8)
+				*height += 8 - *height % 8;
+			pr_err("ERROR: v4l2 capture: height exceeds limit "
+			       "resize to %d.\n", *height);
+		}
+
+		switch (f->fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_RGB565:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			csi_set_16bit_imagpara(f->fmt.pix.width,
+					       f->fmt.pix.height);
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_UYVY:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			csi_set_16bit_imagpara(f->fmt.pix.width,
+					       f->fmt.pix.height);
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_YUV420:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3 / 2;
+			csi_set_12bit_imagpara(f->fmt.pix.width,
+					       f->fmt.pix.height);
+			bytesperline = f->fmt.pix.width;
+			break;
+		case V4L2_PIX_FMT_YUV422P:
+		case V4L2_PIX_FMT_RGB24:
+		case V4L2_PIX_FMT_BGR24:
+		case V4L2_PIX_FMT_BGR32:
+		case V4L2_PIX_FMT_RGB32:
+		case V4L2_PIX_FMT_NV12:
+		default:
+			pr_debug("   case not supported\n");
+			break;
+		}
+
+		if (f->fmt.pix.bytesperline < bytesperline)
+			f->fmt.pix.bytesperline = bytesperline;
+		else
+			bytesperline = f->fmt.pix.bytesperline;
+
+		if (f->fmt.pix.sizeimage < size)
+			f->fmt.pix.sizeimage = size;
+		else
+			size = f->fmt.pix.sizeimage;
+
+		cam->v2f.fmt.pix = f->fmt.pix;
+
+		if (cam->v2f.fmt.pix.priv != 0) {
+			if (copy_from_user(&cam->offset,
+					   (void *)cam->v2f.fmt.pix.priv,
+					   sizeof(cam->offset))) {
+				retval = -EFAULT;
+				break;
+			}
+		}
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
+		cam->win = f->fmt.win;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__, cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+
+	return retval;
+}
+
+/*!
+ * V4L2 - csi_v4l2_s_param function
+ * Allows setting of capturemode and frame rate.
+ *
+ * @param cam         structure cam_data *
+ * @param parm        structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int csi_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
+{
+	struct v4l2_ifparm ifparm;
+	struct v4l2_format cam_fmt;
+	struct v4l2_streamparm currentparm;
+	int err = 0;
+
+	pr_debug("In %s\n", __func__);
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pr_err(KERN_ERR "%s invalid type\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+
+	currentparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* First check that this device can support the changes requested. */
+	err = vidioc_int_g_parm(cam->sensor, &currentparm);
+	if (err) {
+		pr_err("%s: vidioc_int_g_parm returned an error %d\n",
+		       __func__, err);
+		goto exit;
+	}
+
+	pr_debug("   Current capabilities are %x\n",
+		 currentparm.parm.capture.capability);
+	pr_debug("   Current capturemode is %d  change to %d\n",
+		 currentparm.parm.capture.capturemode,
+		 parm->parm.capture.capturemode);
+	pr_debug("   Current framerate is %d  change to %d\n",
+		 currentparm.parm.capture.timeperframe.denominator,
+		 parm->parm.capture.timeperframe.denominator);
+
+	err = vidioc_int_s_parm(cam->sensor, parm);
+	if (err) {
+		pr_err("%s: vidioc_int_s_parm returned an error %d\n",
+		       __func__, err);
+		goto exit;
+	}
+
+	vidioc_int_g_ifparm(cam->sensor, &ifparm);
+	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	pr_debug("   g_fmt_cap returns widthxheight of input as %d x %d\n",
+		 cam_fmt.fmt.pix.width, cam_fmt.fmt.pix.height);
+
+exit:
+	return err;
+}
+
+/*!
+ * V4L interface - open function
+ *
+ * @param inode        structure inode *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int csi_v4l_open(struct inode *inode, struct file *file)
+{
+	struct v4l2_ifparm ifparm;
+	struct v4l2_format cam_fmt;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int err = 0;
+
+	pr_debug("   device name is %s\n", dev->name);
+
+	if (!cam) {
+		pr_err("ERROR: v4l2 capture: Internal error, "
+		       "cam_data not found!\n");
+		return -EBADF;
+	}
+
+	down(&cam->busy_lock);
+	err = 0;
+	if (signal_pending(current))
+		goto oops;
+
+	if (cam->open_count++ == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+
+		vidioc_int_g_ifparm(cam->sensor, &ifparm);
+
+		cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		vidioc_int_init(cam->sensor);
+	}
+
+	file->private_data = dev;
+
+oops:
+	up(&cam->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L interface - close function
+ *
+ * @param inode    struct inode *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int csi_v4l_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	int err = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("In MVC:%s\n", __func__);
+
+	if (!cam) {
+		pr_err("ERROR: v4l2 capture: Internal error, "
+		       "cam_data not found!\n");
+		return -EBADF;
+	}
+
+	/* for the case somebody hit the ctrl C */
+	if (cam->overlay_pid == current->pid) {
+		err = stop_preview(cam);
+		cam->overlay_on = false;
+	}
+
+	if (--cam->open_count == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+		file->private_data = NULL;
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+	}
+
+	return err;
+}
+
+/*
+ * V4L interface - read function
+ *
+ * @param file       struct file *
+ * @param read buf   char *
+ * @param count      size_t
+ * @param ppos       structure loff_t *
+ *
+ * @return           bytes read
+ */
+static ssize_t csi_v4l_read(struct file *file, char *buf, size_t count,
+			    loff_t *ppos)
+{
+	int err = 0;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+
+	if (cam->still_buf_vaddr == NULL) {
+		cam->still_buf_vaddr = dma_alloc_coherent(0,
+							  PAGE_ALIGN
+							  (cam->v2f.fmt.
+							   pix.sizeimage),
+							  &cam->
+							  still_buf,
+							  GFP_DMA | GFP_KERNEL);
+		if (cam->still_buf_vaddr == NULL) {
+			pr_err("alloc dma memory failed\n");
+			return -ENOMEM;
+		}
+		cam->still_counter = 0;
+		__raw_writel(cam->still_buf, CSI_CSIDMASA_FB2);
+		__raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF,
+			     CSI_CSICR3);
+		__raw_writel(__raw_readl(CSI_CSISR), CSI_CSISR);
+		__raw_writel(__raw_readl(CSI_CSICR3) | BIT_FRMCNT_RST,
+			     CSI_CSICR3);
+		csi_enable_int(1);
+	}
+
+	wait_event_interruptible(cam->still_queue, cam->still_counter);
+	csi_disable_int();
+	err = copy_to_user(buf, cam->still_buf_vaddr,
+			   cam->v2f.fmt.pix.sizeimage);
+
+	if (cam->still_buf_vaddr != NULL) {
+		dma_free_coherent(0, PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				  cam->still_buf_vaddr, cam->still_buf);
+		cam->still_buf = 0;
+		cam->still_buf_vaddr = NULL;
+	}
+
+	if (cam->overlay_on == true)
+		start_preview(cam);
+
+	up(&cam->busy_lock);
+	if (err < 0)
+		return err;
+
+	return cam->v2f.fmt.pix.sizeimage - err;
+}
+
+/*!
+ * V4L interface - ioctl function
+ *
+ * @param inode      struct inode*
+ *
+ * @param file       struct file*
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void*
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int csi_v4l_do_ioctl(struct inode *inode, struct file *file,
+			    unsigned int ioctlnr, void *arg)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int retval = 0;
+
+	pr_debug("In MVC: %s, %x\n", __func__, ioctlnr);
+	wait_event_interruptible(cam->power_queue, cam->low_power == false);
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+		/*!
+		 * V4l2 VIDIOC_G_FMT ioctl
+		 */
+	case VIDIOC_G_FMT:{
+			struct v4l2_format *gf = arg;
+			pr_debug("   case VIDIOC_G_FMT\n");
+			retval = csi_v4l2_g_fmt(cam, gf);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_FMT ioctl
+		 */
+	case VIDIOC_S_FMT:{
+			struct v4l2_format *sf = arg;
+			pr_debug("   case VIDIOC_S_FMT\n");
+			retval = csi_v4l2_s_fmt(cam, sf);
+			vidioc_int_s_fmt_cap(cam->sensor, sf);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_OVERLAY ioctl
+		 */
+	case VIDIOC_OVERLAY:{
+			int *on = arg;
+			pr_debug("   case VIDIOC_OVERLAY\n");
+			if (*on) {
+				cam->overlay_on = true;
+				cam->overlay_pid = current->pid;
+				start_preview(cam);
+			}
+			if (!*on) {
+				stop_preview(cam);
+				cam->overlay_on = false;
+			}
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_FBUF ioctl
+		 */
+	case VIDIOC_G_FBUF:{
+			struct v4l2_framebuffer *fb = arg;
+			*fb = cam->v4l2_fb;
+			fb->capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_FBUF ioctl
+		 */
+	case VIDIOC_S_FBUF:{
+			struct v4l2_framebuffer *fb = arg;
+			cam->v4l2_fb = *fb;
+			break;
+		}
+
+	case VIDIOC_G_PARM:{
+			struct v4l2_streamparm *parm = arg;
+			pr_debug("   case VIDIOC_G_PARM\n");
+			vidioc_int_g_parm(cam->sensor, parm);
+			break;
+		}
+
+	case VIDIOC_S_PARM:{
+			struct v4l2_streamparm *parm = arg;
+			pr_debug("   case VIDIOC_S_PARM\n");
+			retval = csi_v4l2_s_param(cam, parm);
+			break;
+		}
+
+	case VIDIOC_QUERYCAP:{
+			struct v4l2_capability *cap = arg;
+			pr_debug("   case VIDIOC_QUERYCAP\n");
+			strcpy(cap->driver, "csi_v4l2");
+			cap->version = KERNEL_VERSION(0, 1, 11);
+			cap->capabilities = V4L2_CAP_VIDEO_OVERLAY |
+			    V4L2_CAP_VIDEO_OUTPUT_OVERLAY | V4L2_CAP_READWRITE;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			break;
+		}
+
+	case VIDIOC_S_CROP:
+		pr_debug("   case not supported\n");
+		break;
+
+	case VIDIOC_S_CTRL:
+	case VIDIOC_G_STD:
+	case VIDIOC_QBUF:
+	case VIDIOC_QUERYBUF:
+	case VIDIOC_REQBUFS:
+	case VIDIOC_DQBUF:
+	case VIDIOC_G_OUTPUT:
+	case VIDIOC_S_OUTPUT:
+	case VIDIOC_ENUMSTD:
+	case VIDIOC_G_CROP:
+	case VIDIOC_CROPCAP:
+	case VIDIOC_S_STD:
+	case VIDIOC_G_CTRL:
+	case VIDIOC_STREAMOFF:
+	case VIDIOC_STREAMON:
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_ENUMINPUT:
+	case VIDIOC_G_INPUT:
+	case VIDIOC_S_INPUT:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	case VIDIOC_ENUMOUTPUT:
+	default:
+		pr_debug("   case not supported\n");
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&cam->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L interface - ioctl function
+ *
+ * @return  None
+ */
+static int csi_v4l_ioctl(struct inode *inode, struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, csi_v4l_do_ioctl);
+}
+
+/*!
+ * V4L interface - mmap function
+ *
+ * @param file        structure file *
+ *
+ * @param vma         structure vm_area_struct *
+ *
+ * @return status     0 Success, EINTR busy lock error, ENOBUFS remap_page error
+ */
+static int csi_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = video_devdata(file);
+	unsigned long size;
+	int res = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("%s\n", __func__);
+	pr_debug("\npgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		 vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	size = vma->vm_end - vma->vm_start;
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		pr_err("ERROR: v4l2 capture: %s : "
+		       "remap_pfn_range failed\n", __func__);
+		res = -ENOBUFS;
+		goto csi_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+csi_mmap_exit:
+	up(&cam->busy_lock);
+	return res;
+}
+
+/*!
+ * This structure defines the functions to be called in this driver.
+ */
+static struct file_operations csi_v4l_fops = {
+	.owner = THIS_MODULE,
+	.open = csi_v4l_open,
+	.release = csi_v4l_close,
+	.read = csi_v4l_read,
+	.ioctl = csi_v4l_ioctl,
+	.mmap = csi_mmap,
+};
+
+static struct video_device csi_v4l_template = {
+	.name = "Mx25 Camera",
+	.vfl_type = VID_TYPE_CAPTURE,
+	.fops = &csi_v4l_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * This function can be used to release any platform data on closing.
+ */
+static void camera_platform_release(struct device *device)
+{
+}
+
+/*! Device Definition for csi v4l2 device */
+static struct platform_device csi_v4l2_devices = {
+	.name = "csi_v4l2",
+	.dev = {
+		.release = camera_platform_release,
+		},
+	.id = 0,
+};
+
+/*!
+ * initialize cam_data structure
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static void init_camera_struct(cam_data *cam)
+{
+	pr_debug("In MVC: %s\n", __func__);
+
+	/* Default everything to 0 */
+	memset(cam, 0, sizeof(cam_data));
+
+	init_MUTEX(&cam->param_lock);
+	init_MUTEX(&cam->busy_lock);
+
+	cam->video_dev = video_device_alloc();
+	if (cam->video_dev == NULL)
+		return;
+
+	*(cam->video_dev) = csi_v4l_template;
+
+	video_set_drvdata(cam->video_dev, cam);
+	dev_set_drvdata(&csi_v4l2_devices.dev, (void *)cam);
+	cam->video_dev->minor = -1;
+
+	init_waitqueue_head(&cam->still_queue);
+
+	cam->streamparm.parm.capture.capturemode = 0;
+
+	cam->standard.index = 0;
+	cam->standard.id = V4L2_STD_UNKNOWN;
+	cam->standard.frameperiod.denominator = 30;
+	cam->standard.frameperiod.numerator = 1;
+	cam->standard.framelines = 480;
+	cam->standard_autodetect = true;
+	cam->streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	cam->streamparm.parm.capture.timeperframe = cam->standard.frameperiod;
+	cam->streamparm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	cam->overlay_on = false;
+	cam->capture_on = false;
+	cam->skip_frame = 0;
+	cam->v4l2_fb.flags = V4L2_FBUF_FLAG_OVERLAY;
+
+	cam->v2f.fmt.pix.sizeimage = 480 * 640 * 2;
+	cam->v2f.fmt.pix.bytesperline = 640 * 2;
+	cam->v2f.fmt.pix.width = 640;
+	cam->v2f.fmt.pix.height = 480;
+	cam->v2f.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	cam->win.w.width = 160;
+	cam->win.w.height = 160;
+	cam->win.w.left = 0;
+	cam->win.w.top = 0;
+	cam->still_counter = 0;
+
+	csi_start_callback(cam);
+	init_waitqueue_head(&cam->power_queue);
+	spin_lock_init(&cam->int_lock);
+}
+
+/*!
+ * camera_power function
+ *    Turns Sensor power On/Off
+ *
+ * @param       cam           cam data struct
+ * @param       cameraOn      true to turn camera on, false to turn off power.
+ *
+ * @return status
+ */
+static u8 camera_power(cam_data *cam, bool cameraOn)
+{
+	pr_debug("In MVC: %s on=%d\n", __func__, cameraOn);
+
+	if (cameraOn == true) {
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		vidioc_int_s_power(cam->sensor, 1);
+	} else {
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+		vidioc_int_s_power(cam->sensor, 0);
+	}
+	return 0;
+}
+
+/*!
+ * This function is called to put the sensor in a low power state.
+ * Refer to the document driver-model/driver.txt in the kernel source tree
+ * for more information.
+ *
+ * @param   pdev  the device structure used to give information on which I2C
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure.
+ */
+static int csi_v4l2_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	pr_debug("In MVC: %s\n", __func__);
+
+	if (cam == NULL)
+		return -1;
+
+	cam->low_power = true;
+
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+
+	camera_power(cam, false);
+
+	return 0;
+}
+
+/*!
+ * This function is called to bring the sensor back from a low power state.
+ * Refer to the document driver-model/driver.txt in the kernel source tree
+ * for more information.
+ *
+ * @param   pdev   the device structure
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int csi_v4l2_resume(struct platform_device *pdev)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	pr_debug("In MVC: %s\n", __func__);
+
+	if (cam == NULL)
+		return -1;
+
+	cam->low_power = false;
+	wake_up_interruptible(&cam->power_queue);
+	camera_power(cam, true);
+
+	if (cam->overlay_on == true)
+		start_preview(cam);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver csi_v4l2_driver = {
+	.driver = {
+		   .name = "csi_v4l2",
+		   },
+	.probe = NULL,
+	.remove = NULL,
+#ifdef CONFIG_PM
+	.suspend = csi_v4l2_suspend,
+	.resume = csi_v4l2_resume,
+#endif
+	.shutdown = NULL,
+};
+
+/*!
+ * Initializes the camera driver.
+ */
+static int csi_v4l2_master_attach(struct v4l2_int_device *slave)
+{
+	cam_data *cam = slave->u.slave->master->priv;
+	struct v4l2_format cam_fmt;
+
+	pr_debug("In MVC: %s\n", __func__);
+	pr_debug("   slave.name = %s\n", slave->name);
+	pr_debug("   master.name = %s\n", slave->u.slave->master->name);
+
+	cam->sensor = slave;
+	if (slave == NULL) {
+		pr_err("ERROR: v4l2 capture: slave parameter not valid.\n");
+		return -1;
+	}
+
+	csi_enable_mclk(CSI_MCLK_I2C, true, true);
+	vidioc_int_dev_init(slave);
+	csi_enable_mclk(CSI_MCLK_I2C, false, false);
+	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* Used to detect TV in (type 1) vs. camera (type 0) */
+	cam->device_type = cam_fmt.fmt.pix.priv;
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__, cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+
+	return 0;
+}
+
+/*!
+ * Disconnects the camera driver.
+ */
+static void csi_v4l2_master_detach(struct v4l2_int_device *slave)
+{
+	pr_debug("In MVC: %s\n", __func__);
+
+	vidioc_int_dev_exit(slave);
+}
+
+/*!
+ * Entry point for the V4L2
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int camera_init(void)
+{
+	u8 err = 0;
+
+	/* Register the device driver structure. */
+	err = platform_driver_register(&csi_v4l2_driver);
+	if (err != 0) {
+		pr_err("ERROR: v4l2 capture:camera_init: "
+		       "platform_driver_register failed.\n");
+		return err;
+	}
+
+	/* Create g_cam and initialize it. */
+	g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL);
+	if (g_cam == NULL) {
+		pr_err("ERROR: v4l2 capture: failed to register camera\n");
+		platform_driver_unregister(&csi_v4l2_driver);
+		return -1;
+	}
+	init_camera_struct(g_cam);
+
+	/* Set up the v4l2 device and register it */
+	csi_v4l2_int_device.priv = g_cam;
+	/* This function contains a bug that won't let this be rmmod'd. */
+	v4l2_int_device_register(&csi_v4l2_int_device);
+
+	/* Register the platform device */
+	err = platform_device_register(&csi_v4l2_devices);
+	if (err != 0) {
+		pr_err("ERROR: v4l2 capture: camera_init: "
+		       "platform_device_register failed.\n");
+		platform_driver_unregister(&csi_v4l2_driver);
+		kfree(g_cam);
+		g_cam = NULL;
+		return err;
+	}
+
+	/* register v4l video device */
+	if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
+	    == -1) {
+		platform_device_unregister(&csi_v4l2_devices);
+		platform_driver_unregister(&csi_v4l2_driver);
+		kfree(g_cam);
+		g_cam = NULL;
+		pr_err("ERROR: v4l2 capture: video_register_device failed\n");
+		return -1;
+	}
+	pr_debug("   Video device registered: %s #%d\n",
+		 g_cam->video_dev->name, g_cam->video_dev->minor);
+
+	return err;
+}
+
+/*!
+ * Exit and cleanup for the V4L2
+ */
+static void __exit camera_exit(void)
+{
+	pr_debug("In MVC: %s\n", __func__);
+
+	if (g_cam->open_count) {
+		pr_err("ERROR: v4l2 capture:camera open "
+		       "-- setting ops to NULL\n");
+	} else {
+		pr_info("V4L2 freeing image input device\n");
+		v4l2_int_device_unregister(&csi_v4l2_int_device);
+		csi_stop_callback(g_cam);
+		video_unregister_device(g_cam->video_dev);
+		platform_driver_unregister(&csi_v4l2_driver);
+		platform_device_unregister(&csi_v4l2_devices);
+
+		kfree(g_cam);
+		g_cam = NULL;
+	}
+}
+
+module_init(camera_init);
+module_exit(camera_exit);
+
+module_param(video_nr, int, 0444);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2 capture driver for Mx25 based cameras");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/capture/emma_mt9v111.c b/drivers/media/video/mxc/capture/emma_mt9v111.c
new file mode 100644
index 000000000000..73e9bba36d1e
--- /dev/null
+++ b/drivers/media/video/mxc/capture/emma_mt9v111.c
@@ -0,0 +1,679 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mt9v111.c
+ *
+ * @brief mt9v111 camera driver functions
+ *
+ * @ingroup Camera
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include "mxc_v4l2_capture.h"
+#include "mt9v111.h"
+
+#ifdef MT9V111_DEBUG
+static u16 testpattern;
+#endif
+
+static sensor_interface *interface_param;
+static mt9v111_conf mt9v111_device;
+static int reset_frame_rate = 30;
+
+#define MT9V111_FRAME_RATE_NUM    20
+
+static mt9v111_image_format format[2] = {
+	{
+	 .index = 0,
+	 .width = 640,
+	 .height = 480,
+	 },
+	{
+	 .index = 1,
+	 .width = 352,
+	 .height = 288,
+	 },
+};
+
+static int mt9v111_attach(struct i2c_adapter *adapter);
+static int mt9v111_detach(struct i2c_client *client);
+
+static struct i2c_driver mt9v111_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "MT9V111 Client",
+		   },
+	.attach_adapter = mt9v111_attach,
+	.detach_client = mt9v111_detach,
+};
+
+static struct i2c_client mt9v111_i2c_client = {
+	.name = "mt9v111 I2C dev",
+	.addr = MT9V111_I2C_ADDRESS,
+	.driver = &mt9v111_i2c_driver,
+};
+
+/*
+ * Function definitions
+ */
+
+#ifdef MT9V111_DEBUG
+static inline int mt9v111_read_reg(u8 reg)
+{
+	int val = i2c_smbus_read_word_data(&mt9v111_i2c_client, reg);
+	if (val != -1)
+		val = cpu_to_be16(val);
+	return val;
+}
+#endif
+
+static inline int mt9v111_write_reg(u8 reg, u16 val)
+{
+	pr_debug("write reg %x val %x.\n", reg, val);
+	return i2c_smbus_write_word_data(&mt9v111_i2c_client, reg,
+					 cpu_to_be16(val));
+}
+
+/*!
+ * Initialize mt9v111_sensor_lib
+ * Libarary for Sensor configuration through I2C
+ *
+ * @param       coreReg       Core Registers
+ * @param       ifpReg        IFP Register
+ *
+ * @return status
+ */
+static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
+{
+	u8 reg;
+	u16 data;
+	u8 error = 0;
+
+	/*
+	 * setup to IFP registers
+	 */
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_MODE_CONTROL;
+	data = ifpReg->modeControl;
+	mt9v111_write_reg(reg, data);
+
+	/* Output format */
+	reg = MT9V111I_FORMAT_CONTROL;
+	data = ifpReg->formatControl;	/* Set bit 12 */
+	mt9v111_write_reg(reg, data);
+
+	/* AE limit 4 */
+	reg = MT9V111I_SHUTTER_WIDTH_LIMIT_AE;
+	data = ifpReg->gainLimitAE;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_OUTPUT_FORMAT_CTRL2;
+	data = ifpReg->outputFormatCtrl2;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_AE_SPEED;
+	data = ifpReg->AESpeed;
+	mt9v111_write_reg(reg, data);
+
+	/* output image size */
+	reg = MT9V111i_H_PAN;
+	data = 0x8000 | ifpReg->HPan;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_ZOOM;
+	data = 0x8000 | ifpReg->HZoom;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_SIZE;
+	data = 0x8000 | ifpReg->HSize;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_PAN;
+	data = 0x8000 | ifpReg->VPan;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_ZOOM;
+	data = 0x8000 | ifpReg->VZoom;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_SIZE;
+	data = 0x8000 | ifpReg->VSize;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_PAN;
+	data = ~0x8000 & ifpReg->HPan;
+	mt9v111_write_reg(reg, data);
+#if 0
+	reg = MT9V111I_UPPER_SHUTTER_DELAY_LIM;
+	data = ifpReg->upperShutterDelayLi;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_SHUTTER_60;
+	data = ifpReg->shutter_width_60;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_SEARCH_FLICK_60;
+	data = ifpReg->search_flicker_60;
+	mt9v111_write_reg(reg, data);
+#endif
+
+	/*
+	 * setup to sensor core registers
+	 */
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = coreReg->addressSelect;
+	mt9v111_write_reg(reg, data);
+
+	/* enable changes and put the Sync bit on */
+	reg = MT9V111S_OUTPUT_CTRL;
+	data = MT9V111S_OUTCTRL_SYNC | MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
+	mt9v111_write_reg(reg, data);
+
+	/* min PIXCLK - Default */
+	reg = MT9V111S_PIXEL_CLOCK_SPEED;
+	data = coreReg->pixelClockSpeed;
+	mt9v111_write_reg(reg, data);
+
+	/* Setup image flipping / Dark rows / row/column skip */
+	reg = MT9V111S_READ_MODE;
+	data = coreReg->readMode;
+	mt9v111_write_reg(reg, data);
+
+	/*zoom 0 */
+	reg = MT9V111S_DIGITAL_ZOOM;
+	data = coreReg->digitalZoom;
+	mt9v111_write_reg(reg, data);
+
+	/* min H-blank */
+	reg = MT9V111S_HOR_BLANKING;
+	data = coreReg->horizontalBlanking;
+	mt9v111_write_reg(reg, data);
+
+	/* min V-blank */
+	reg = MT9V111S_VER_BLANKING;
+	data = coreReg->verticalBlanking;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111S_SHUTTER_WIDTH;
+	data = coreReg->shutterWidth;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111S_SHUTTER_DELAY;
+	data = ifpReg->upperShutterDelayLi;
+	mt9v111_write_reg(reg, data);
+
+	/* changes become effective */
+	reg = MT9V111S_OUTPUT_CTRL;
+	data = MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
+	mt9v111_write_reg(reg, data);
+
+	return error;
+}
+
+/*!
+ * mt9v111 sensor interface Initialization
+ * @param param            sensor_interface *
+ * @param width            u32
+ * @param height           u32
+ * @return  None
+ */
+static void mt9v111_interface(sensor_interface *param, u32 width, u32 height)
+{
+	param->Vsync_pol = 0x0;
+	param->clk_mode = 0x0;	/*gated */
+	param->pixclk_pol = 0x0;
+	param->data_width = 0x1;
+	param->data_pol = 0x0;
+	param->ext_vsync = 0x0;
+	param->Vsync_pol = 0x0;
+	param->Hsync_pol = 0x0;
+	param->width = width - 1;
+	param->height = height - 1;
+	param->active_width = width;
+	param->active_height = height;
+	param->pixel_fmt = IPU_PIX_FMT_UYVY;
+	param->mclk = 27000000;
+}
+
+/*!
+ * MT9V111 frame rate calculate
+ *
+ * @param frame_rate       int *
+ * @param mclk             int
+ * @return  None
+ */
+static void mt9v111_rate_cal(int *frame_rate, int mclk)
+{
+	int num_clock_per_row;
+	int max_rate = 0;
+
+	mt9v111_device.coreReg->horizontalBlanking = MT9V111_HORZBLANK_MIN;
+
+	num_clock_per_row = (format[0].width + 114 + MT9V111_HORZBLANK_MIN) * 2;
+	max_rate = mclk / (num_clock_per_row *
+			   (format[0].height + MT9V111_VERTBLANK_DEFAULT));
+
+	if ((*frame_rate > max_rate) || (*frame_rate == 0)) {
+		*frame_rate = max_rate;
+	}
+
+	mt9v111_device.coreReg->verticalBlanking
+	    = mclk / (*frame_rate * num_clock_per_row) - format[0].height;
+
+	reset_frame_rate = *frame_rate;
+}
+
+/*!
+ * MT9V111 sensor configuration
+ *
+ * @param frame_rate       int *
+ * @param high_quality     int
+ * @return  sensor_interface *
+ */
+sensor_interface *mt9v111_config(int *frame_rate, int high_quality)
+{
+	u32 out_width, out_height;
+
+	if (interface_param == NULL)
+		return NULL;
+
+	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
+	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
+
+	mt9v111_device.coreReg->windowHeight = MT9V111_WINHEIGHT;
+	mt9v111_device.coreReg->windowWidth = MT9V111_WINWIDTH;
+	mt9v111_device.coreReg->zoomColStart = 0;
+	mt9v111_device.coreReg->zomRowStart = 0;
+	mt9v111_device.coreReg->digitalZoom = 0x0;
+
+	mt9v111_device.coreReg->verticalBlanking = MT9V111_VERTBLANK_DEFAULT;
+	mt9v111_device.coreReg->horizontalBlanking = MT9V111_HORZBLANK_MIN;
+	mt9v111_device.coreReg->pixelClockSpeed = 0;
+	mt9v111_device.coreReg->readMode = 0xd0a1;
+
+	mt9v111_device.ifpReg->outputFormatCtrl2 = 0;
+	mt9v111_device.ifpReg->gainLimitAE = 0x300;
+	mt9v111_device.ifpReg->AESpeed = 0x80;
+
+	/* here is the default value */
+	mt9v111_device.ifpReg->formatControl = 0xc800;
+	mt9v111_device.ifpReg->modeControl = 0x708e;
+	mt9v111_device.ifpReg->awbSpeed = 0x4514;
+	mt9v111_device.coreReg->shutterWidth = 0xf8;
+
+	out_width = 640;
+	out_height = 480;
+
+	/*output size */
+	mt9v111_device.ifpReg->HPan = 0;
+	mt9v111_device.ifpReg->HZoom = 640;
+	mt9v111_device.ifpReg->HSize = out_width;
+	mt9v111_device.ifpReg->VPan = 0;
+	mt9v111_device.ifpReg->VZoom = 480;
+	mt9v111_device.ifpReg->VSize = out_height;
+
+	mt9v111_interface(interface_param, out_width, out_height);
+	set_mclk_rate(&interface_param->mclk);
+	mt9v111_rate_cal(frame_rate, interface_param->mclk);
+	mt9v111_sensor_lib(mt9v111_device.coreReg, mt9v111_device.ifpReg);
+
+	return interface_param;
+}
+
+/*!
+ * mt9v111 sensor set color configuration
+ *
+ * @param bright       int
+ * @param saturation   int
+ * @param red          int
+ * @param green        int
+ * @param blue         int
+ * @return  None
+ */
+static void
+mt9v111_set_color(int bright, int saturation, int red, int green, int blue)
+{
+	u8 reg;
+	u16 data;
+
+	switch (saturation) {
+	case 100:
+		mt9v111_device.ifpReg->awbSpeed = 0x4514;
+		break;
+	case 150:
+		mt9v111_device.ifpReg->awbSpeed = 0x6D14;
+		break;
+	case 75:
+		mt9v111_device.ifpReg->awbSpeed = 0x4D14;
+		break;
+	case 50:
+		mt9v111_device.ifpReg->awbSpeed = 0x5514;
+		break;
+	case 37:
+		mt9v111_device.ifpReg->awbSpeed = 0x5D14;
+		break;
+	case 25:
+		mt9v111_device.ifpReg->awbSpeed = 0x6514;
+		break;
+	default:
+		mt9v111_device.ifpReg->awbSpeed = 0x4514;
+		break;
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_AWB_SPEED;
+	data = mt9v111_device.ifpReg->awbSpeed;
+	mt9v111_write_reg(reg, data);
+}
+
+/*!
+ * mt9v111 sensor get color configuration
+ *
+ * @param bright       int *
+ * @param saturation   int *
+ * @param red          int *
+ * @param green        int *
+ * @param blue         int *
+ * @return  None
+ */
+static void
+mt9v111_get_color(int *bright, int *saturation, int *red, int *green, int *blue)
+{
+	*saturation = (mt9v111_device.ifpReg->awbSpeed & 0x3800) >> 11;
+	switch (*saturation) {
+	case 0:
+		*saturation = 100;
+		break;
+	case 1:
+		*saturation = 75;
+		break;
+	case 2:
+		*saturation = 50;
+		break;
+	case 3:
+		*saturation = 37;
+		break;
+	case 4:
+		*saturation = 25;
+		break;
+	case 5:
+		*saturation = 150;
+		break;
+	case 6:
+		*saturation = 0;
+		break;
+	default:
+		*saturation = 0;
+		break;
+	}
+}
+
+/*!
+ * mt9v111 sensor set AE measurement window mode configuration
+ *
+ * @param ae_mode      int
+ * @return  None
+ */
+static void mt9v111_set_ae_mode(int ae_mode)
+{
+	u8 reg;
+	u16 data;
+
+	mt9v111_device.ifpReg->modeControl &= 0xfff3;
+	mt9v111_device.ifpReg->modeControl |= (ae_mode & 0x03) << 2;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_MODE_CONTROL;
+	data = mt9v111_device.ifpReg->modeControl;
+	mt9v111_write_reg(reg, data);
+}
+
+/*!
+ * mt9v111 sensor get AE measurement window mode configuration
+ *
+ * @param ae_mode      int *
+ * @return  None
+ */
+static void mt9v111_get_ae_mode(int *ae_mode)
+{
+	if (ae_mode != NULL) {
+		*ae_mode = (mt9v111_device.ifpReg->modeControl & 0xc) >> 2;
+	}
+}
+
+/*!
+ * mt9v111 Reset function
+ *
+ * @return  None
+ */
+static sensor_interface *mt9v111_reset(void)
+{
+	return mt9v111_config(&reset_frame_rate, 0);
+}
+
+struct camera_sensor camera_sensor_if = {
+	.set_color = mt9v111_set_color,
+	.get_color = mt9v111_get_color,
+	.set_ae_mode = mt9v111_set_ae_mode,
+	.get_ae_mode = mt9v111_get_ae_mode,
+	.config = mt9v111_config,
+	.reset = mt9v111_reset,
+};
+
+#ifdef MT9V111_DEBUG
+/*!
+ * Set sensor to test mode, which will generate test pattern.
+ *
+ * @return none
+ */
+static void mt9v111_test_pattern(bool flag)
+{
+	u16 data;
+
+	/* switch to sensor registers */
+	mt9v111_write_reg(MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
+
+	if (flag == true) {
+		testpattern = MT9V111S_OUTCTRL_TEST_MODE;
+
+		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) & 0xBF;
+		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+
+		mt9v111_write_reg(MT9V111S_TEST_DATA, 0);
+
+		/* changes take effect */
+		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
+		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+	} else {
+		testpattern = 0;
+
+		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) | 0x40;
+		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+
+		/* changes take effect */
+		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
+		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+	}
+}
+#endif
+
+/*!
+ * mt9v111 I2C detect_client function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @param address            int
+ * @param kind               int
+ *
+ * @return  Error code indicating success or failure
+ */
+static int mt9v111_detect_client(struct i2c_adapter *adapter, int address,
+				 int kind)
+{
+	mt9v111_i2c_client.adapter = adapter;
+	if (i2c_attach_client(&mt9v111_i2c_client)) {
+		mt9v111_i2c_client.adapter = NULL;
+		printk(KERN_ERR "mt9v111_attach: i2c_attach_client failed\n");
+		return -1;
+	}
+
+	interface_param = (sensor_interface *)
+	    kmalloc(sizeof(sensor_interface), GFP_KERNEL);
+	if (!interface_param) {
+		printk(KERN_ERR "mt9v111_attach: kmalloc failed \n");
+		return -1;
+	}
+
+	printk(KERN_INFO "MT9V111 Detected\n");
+
+	return 0;
+}
+
+static unsigned short normal_i2c[] = { MT9V111_I2C_ADDRESS, I2C_CLIENT_END };
+
+/* Magic definition of all other variables and things */
+I2C_CLIENT_INSMOD;
+
+/*!
+ * mt9v111 I2C attach function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int mt9v111_attach(struct i2c_adapter *adap)
+{
+	uint32_t mclk = 27000000;
+	struct clk *clk;
+	int err;
+
+	clk = clk_get(NULL, "csi_clk");
+	clk_enable(clk);
+	set_mclk_rate(&mclk);
+
+	err = i2c_probe(adap, &addr_data, &mt9v111_detect_client);
+
+	clk_disable(clk);
+	clk_put(clk);
+
+	return err;
+}
+
+/*!
+ * mt9v111 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int mt9v111_detach(struct i2c_client *client)
+{
+	int err;
+
+	if (!mt9v111_i2c_client.adapter)
+		return -1;
+
+	err = i2c_detach_client(&mt9v111_i2c_client);
+	mt9v111_i2c_client.adapter = NULL;
+
+	if (interface_param)
+		kfree(interface_param);
+	interface_param = NULL;
+
+	return err;
+}
+
+extern void gpio_sensor_active(void);
+
+/*!
+ * MT9V111 init function
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int mt9v111_init(void)
+{
+	u8 err;
+
+	gpio_sensor_active();
+
+	mt9v111_device.coreReg = (mt9v111_coreReg *)
+	    kmalloc(sizeof(mt9v111_coreReg), GFP_KERNEL);
+	if (!mt9v111_device.coreReg)
+		return -1;
+
+	memset(mt9v111_device.coreReg, 0, sizeof(mt9v111_coreReg));
+
+	mt9v111_device.ifpReg = (mt9v111_IFPReg *)
+	    kmalloc(sizeof(mt9v111_IFPReg), GFP_KERNEL);
+	if (!mt9v111_device.ifpReg) {
+		kfree(mt9v111_device.coreReg);
+		mt9v111_device.coreReg = NULL;
+		return -1;
+	}
+
+	memset(mt9v111_device.ifpReg, 0, sizeof(mt9v111_IFPReg));
+
+	err = i2c_add_driver(&mt9v111_i2c_driver);
+
+	return err;
+}
+
+extern void gpio_sensor_inactive(void);
+/*!
+ * MT9V111 cleanup function
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit mt9v111_clean(void)
+{
+	if (mt9v111_device.coreReg) {
+		kfree(mt9v111_device.coreReg);
+		mt9v111_device.coreReg = NULL;
+	}
+
+	if (mt9v111_device.ifpReg) {
+		kfree(mt9v111_device.ifpReg);
+		mt9v111_device.ifpReg = NULL;
+	}
+
+	i2c_del_driver(&mt9v111_i2c_driver);
+
+	gpio_sensor_inactive();
+}
+
+module_init(mt9v111_init);
+module_exit(mt9v111_clean);
+
+/* Exported symbols for modules. */
+EXPORT_SYMBOL(camera_sensor_if);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Mt9v111 Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/emma_ov2640.c b/drivers/media/video/mxc/capture/emma_ov2640.c
new file mode 100644
index 000000000000..ceffea4d52a9
--- /dev/null
+++ b/drivers/media/video/mxc/capture/emma_ov2640.c
@@ -0,0 +1,444 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/regulator/consumer.h>
+
+#include "mxc_v4l2_capture.h"
+
+enum ov2640_mode {
+	ov2640_mode_1600_1120,
+	ov2640_mode_800_600
+};
+
+struct reg_value {
+	u8 reg;
+	u8 value;
+	int delay_ms;
+};
+
+static struct reg_value ov2640_setting_1600_1120[] = {
+	{0xff, 0x1, 0}, {0x12, 0x80, 1}, {0xff, 0, 0}, {0x2c, 0xff, 0},
+	{0x2e, 0xdf, 0}, {0xff, 0x1, 0}, {0x3c, 0x32, 0}, {0x11, 0x01, 0},
+	{0x09, 0x00, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0}, {0x14, 0x48, 0},
+	{0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0}, {0x3b, 0xfb, 0},
+	{0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0}, {0x39, 0x82, 0},
+	{0x35, 0x88, 0}, {0x22, 0x0a, 0}, {0x37, 0x40, 0}, {0x23, 0x00, 0},
+	{0x34, 0xa0, 0}, {0x36, 0x1a, 0}, {0x06, 0x02, 0}, {0x07, 0xc0, 0},
+	{0x0d, 0xb7, 0}, {0x0e, 0x01, 0}, {0x4c, 0x00, 0}, {0x4a, 0x81, 0},
+	{0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0}, {0x26, 0x82, 0},
+	{0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x3f, 0}, {0x0c, 0x3c, 0},
+	{0x5d, 0x55, 0}, {0x5e, 0x7d, 0}, {0x5f, 0x7d, 0}, {0x60, 0x55, 0},
+	{0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0}, {0x20, 0x80, 0},
+	{0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0}, {0x6e, 00, 0},
+	{0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0}, {0x3d, 0x34, 0},
+	{0x5a, 0x57, 0}, {0x4f, 0xbb, 0}, {0x50, 0x9c, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0x44, 0x06, 0},
+	{0xe0, 0x14, 0}, {0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0},
+	{0x43, 0x18, 0}, {0x4c, 0x00, 0}, {0x87, 0xd0, 0}, {0xd7, 0x03, 0},
+	{0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0}, {0xc9, 0x80, 0},
+	{0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0}, {0x7d, 0x48, 0},
+	{0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0}, {0x7d, 0x10, 0},
+	{0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0}, {0x91, 0x1a, 0},
+	{0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0}, {0x91, 0x75, 0},
+	{0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0}, {0x91, 0x96, 0},
+	{0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0}, {0x91, 0xd7, 0},
+	{0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0}, {0x93, 0x06, 0},
+	{0x93, 0xe3, 0}, {0x93, 0x03, 0}, {0x93, 0x03, 0}, {0x93, 0x00, 0},
+	{0x93, 0x02, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0}, {0x97, 0x02, 0},
+	{0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0}, {0x97, 0x28, 0},
+	{0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0}, {0x97, 0x80, 0},
+	{0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xa4, 0x00, 0}, {0xa8, 0x00, 0},
+	{0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0}, {0xc7, 0x10, 0},
+	{0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0}, {0xb9, 0x7c, 0},
+	{0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0}, {0xb0, 0xc5, 0},
+	{0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0}, {0xa6, 0x00, 0},
+	{0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x1b, 0}, {0xa7, 0x31, 0},
+	{0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0},
+	{0xa7, 0x19, 0}, {0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0},
+	{0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x19, 0}, {0xa7, 0x31, 0},
+	{0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xc0, 0xc8, 0}, {0xc1, 0x96, 0},
+	{0x86, 0x3d, 0}, {0x50, 0x00, 0}, {0x51, 0x90, 0}, {0x52, 0x18, 0},
+	{0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x88, 0}, {0x57, 0x00, 0},
+	{0x5a, 0x90, 0}, {0x5b, 0x18, 0}, {0x5c, 0x05, 0}, {0xc3, 0xef, 0},
+	{0x7f, 0x00, 0}, {0xda, 0x01, 0}, {0xe5, 0x1f, 0}, {0xe1, 0x67, 0},
+	{0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0}
+};
+
+static struct reg_value ov2640_setting_800_600[] = {
+	{0xff, 0, 0}, {0xff, 1, 0}, {0x12, 0x80, 1}, {0xff, 00, 0},
+	{0x2c, 0xff, 0}, {0x2e, 0xdf, 0}, {0xff, 0x1, 0}, {0x3c, 0x32, 0},
+	{0x11, 0x01, 0}, {0x09, 0x00, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0},
+	{0x14, 0x48, 0}, {0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0},
+	{0x3b, 0xfb, 0}, {0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0},
+	{0x39, 0x92, 0}, {0x35, 0xda, 0}, {0x22, 0x1a, 0}, {0x37, 0xc3, 0},
+	{0x23, 0x00, 0}, {0x34, 0xc0, 0}, {0x36, 0x1a, 0}, {0x06, 0x88, 0},
+	{0x07, 0xc0, 0}, {0x0d, 0x87, 0}, {0x0e, 0x41, 0}, {0x4c, 0x00, 0},
+	{0x4a, 0x81, 0}, {0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0},
+	{0x26, 0x82, 0}, {0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x22, 0},
+	{0x0c, 0x3c, 0}, {0x5d, 0x55, 0}, {0x5e, 0x7d, 0}, {0x5f, 0x7d, 0},
+	{0x60, 0x55, 0}, {0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0},
+	{0x20, 0x80, 0}, {0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0},
+	{0x6e, 00, 0}, {0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0},
+	{0x12, 0x40, 0}, {0x17, 0x11, 0}, {0x18, 0x43, 0}, {0x19, 0x00, 0},
+	{0x1a, 0x4b, 0}, {0x32, 0x09, 0}, {0x37, 0xc0, 0}, {0x4f, 0xca, 0},
+	{0x50, 0xa8, 0}, {0x6d, 0x00, 0}, {0x3d, 0x38, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0x44, 0x06, 0},
+	{0xe0, 0x14, 0}, {0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0},
+	{0x43, 0x18, 0}, {0x4c, 0x00, 0}, {0x87, 0xd0, 0}, {0x88, 0x3f, 0},
+	{0xd7, 0x03, 0}, {0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0},
+	{0xc9, 0x80, 0}, {0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0},
+	{0x7d, 0x48, 0}, {0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0},
+	{0x7d, 0x10, 0}, {0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0},
+	{0x91, 0x1a, 0}, {0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0},
+	{0x91, 0x75, 0}, {0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0},
+	{0x91, 0x96, 0}, {0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0},
+	{0x91, 0xd7, 0}, {0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0},
+	{0x93, 0x06, 0}, {0x93, 0xe3, 0}, {0x93, 0x03, 0}, {0x93, 0x03, 0},
+	{0x93, 0x00, 0}, {0x93, 0x02, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0},
+	{0x97, 0x02, 0}, {0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0},
+	{0x97, 0x28, 0}, {0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0},
+	{0x97, 0x80, 0}, {0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xa4, 0x00, 0},
+	{0xa8, 0x00, 0}, {0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0},
+	{0xc7, 0x10, 0}, {0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0},
+	{0xb9, 0x7c, 0}, {0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0},
+	{0xb0, 0xc5, 0}, {0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0},
+	{0xa6, 0x00, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x1b, 0},
+	{0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xa7, 0x20, 0},
+	{0xa7, 0xd8, 0}, {0xa7, 0x19, 0}, {0xa7, 0x31, 0}, {0xa7, 0x00, 0},
+	{0xa7, 0x18, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x19, 0},
+	{0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xc0, 0x64, 0},
+	{0xc1, 0x4b, 0}, {0x86, 0x1d, 0}, {0x50, 0x00, 0}, {0x51, 0xc8, 0},
+	{0x52, 0x96, 0}, {0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x00, 0},
+	{0x57, 0x00, 0}, {0x5a, 0xc8, 0}, {0x5b, 0x96, 0}, {0x5c, 0x00, 0},
+	{0xc3, 0xef, 0}, {0x7f, 0x00, 0}, {0xda, 0x01, 0}, {0xe5, 0x1f, 0},
+	{0xe1, 0x67, 0}, {0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0}
+};
+
+static struct regulator *io_regulator;
+static struct regulator *core_regulator;
+static struct regulator *analog_regulator;
+static struct regulator *gpo_regulator;
+u32 mclk = 24000000;
+
+struct i2c_client *ov2640_i2c_client;
+
+static sensor_interface *interface_param;
+static int reset_frame_rate = 30;
+static int ov2640_probe(struct i2c_client *adapter,
+			const struct i2c_device_id *id);
+static int ov2640_remove(struct i2c_client *client);
+
+static const struct i2c_device_id ov2640_id[] = {
+	{"ov2640", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov2640_id);
+
+static struct i2c_driver ov2640_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "ov2640",
+		   },
+	.probe = ov2640_probe,
+	.remove = ov2640_remove,
+	.id_table = ov2640_id,
+};
+
+/*!
+ * ov2640 I2C attach function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int ov2640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct mxc_camera_platform_data *plat_data = client->dev.platform_data;
+
+	ov2640_i2c_client = client;
+	mclk = plat_data->mclk;
+
+	io_regulator = regulator_get(&client->dev, plat_data->io_regulator);
+	core_regulator = regulator_get(&client->dev, plat_data->core_regulator);
+	analog_regulator =
+	    regulator_get(&client->dev, plat_data->analog_regulator);
+	gpo_regulator = regulator_get(&client->dev, plat_data->gpo_regulator);
+
+	interface_param = (sensor_interface *)
+	    kmalloc(sizeof(sensor_interface), GFP_KERNEL);
+	if (!interface_param) {
+		dev_dbg(&ov2640_i2c_client->dev,
+			"ov2640_probe: kmalloc failed \n");
+		return -1;
+	}
+
+	return 0;
+}
+
+/*!
+ * ov2640 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int ov2640_remove(struct i2c_client *client)
+{
+	kfree(interface_param);
+	interface_param = NULL;
+
+	if (!IS_ERR_VALUE((unsigned long)io_regulator)) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+
+	if (!IS_ERR_VALUE((unsigned long)core_regulator)) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+
+	if (!IS_ERR_VALUE((unsigned long)gpo_regulator)) {
+		regulator_disable(gpo_regulator);
+		regulator_put(gpo_regulator);
+	}
+
+	if (!IS_ERR_VALUE((unsigned long)analog_regulator)) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+
+	return 0;
+}
+
+static int ov2640_write_reg(u8 reg, u8 val)
+{
+	if (i2c_smbus_write_byte_data(ov2640_i2c_client, reg, val) < 0) {
+		dev_dbg(&ov2640_i2c_client->dev,
+			"%s:write reg errorr:reg=%x,val=%x\n", __func__, reg,
+			val);
+		return -1;
+	}
+	return 0;
+}
+
+static int ov2640_init_mode(enum ov2640_mode mode)
+{
+	struct reg_value *setting;
+	int i, num;
+
+	switch (mode) {
+	case ov2640_mode_1600_1120:
+		setting = ov2640_setting_1600_1120;
+		num = ARRAY_SIZE(ov2640_setting_1600_1120);
+		break;
+	case ov2640_mode_800_600:
+		setting = ov2640_setting_800_600;
+		num = ARRAY_SIZE(ov2640_setting_800_600);
+		break;
+	default:
+		return 0;
+	}
+
+	for (i = 0; i < num; i++) {
+		ov2640_write_reg(setting[i].reg, setting[i].value);
+		if (setting[i].delay_ms > 0)
+			msleep(setting[i].delay_ms);
+	}
+
+	return 0;
+}
+
+/*!
+ * ov2640 sensor interface Initialization
+ * @param param            sensor_interface *
+ * @param width            u32
+ * @param height           u32
+ * @return  None
+ */
+static void ov2640_interface(sensor_interface *param, u32 width, u32 height)
+{
+	param->Vsync_pol = 0x0;
+	param->clk_mode = 0x0;	/*gated */
+	param->pixclk_pol = 0x0;
+	param->data_width = 0x1;
+	param->data_pol = 0x0;
+	param->ext_vsync = 0x0;
+	param->Vsync_pol = 0x0;
+	param->Hsync_pol = 0x0;
+	param->width = width - 1;
+	param->height = height - 1;
+	param->active_width = width;
+	param->active_height = height;
+	param->pixel_fmt = IPU_PIX_FMT_UYVY;
+	param->mclk = mclk;
+}
+
+static void ov2640_set_color(int bright, int saturation, int red, int green,
+			     int blue)
+{
+
+}
+
+static void ov2640_get_color(int *bright, int *saturation, int *red, int *green,
+			     int *blue)
+{
+
+}
+static void ov2640_set_ae_mode(int ae_mode)
+{
+
+}
+static void ov2640_get_ae_mode(int *ae_mode)
+{
+
+}
+
+extern void gpio_sensor_active(void);
+
+static sensor_interface *ov2640_config(int *frame_rate, int high_quality)
+{
+
+	u32 out_width, out_height;
+
+	/*set io votage */
+	if (!IS_ERR_VALUE((unsigned long)io_regulator)) {
+		regulator_set_voltage(io_regulator, 2800000, 2800000);
+		if (regulator_enable(io_regulator) != 0) {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:io set voltage error\n", __func__);
+			return NULL;
+		} else {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:io set voltage ok\n", __func__);
+		}
+	}
+
+	/*core votage */
+	if (!IS_ERR_VALUE((unsigned long)core_regulator)) {
+		regulator_set_voltage(core_regulator, 1300000, 1300000);
+		if (regulator_enable(core_regulator) != 0) {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:core set voltage error\n", __func__);
+			return NULL;
+		} else {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:core set voltage ok\n", __func__);
+		}
+	}
+
+	/*GPO 3 */
+	if (!IS_ERR_VALUE((unsigned long)gpo_regulator)) {
+		if (regulator_enable(gpo_regulator) != 0) {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:gpo3 enable error\n", __func__);
+			return NULL;
+		} else {
+			dev_dbg(&ov2640_i2c_client->dev, "%s:gpo3 enable ok\n",
+				__func__);
+		}
+	}
+
+	if (!IS_ERR_VALUE((unsigned long)analog_regulator)) {
+		regulator_set_voltage(analog_regulator, 2000000, 2000000);
+		if (regulator_enable(analog_regulator) != 0) {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:analog set voltage error\n", __func__);
+			return NULL;
+		} else {
+			dev_dbg(&ov2640_i2c_client->dev,
+				"%s:analog set voltage ok\n", __func__);
+		}
+	}
+
+	gpio_sensor_active();
+
+	if (high_quality) {
+		out_width = 1600;
+		out_height = 1120;
+	} else {
+		out_width = 800;
+		out_height = 600;
+	}
+	ov2640_interface(interface_param, out_width, out_height);
+	set_mclk_rate(&interface_param->mclk);
+
+	if (high_quality)
+		ov2640_init_mode(ov2640_mode_1600_1120);
+	else
+		ov2640_init_mode(ov2640_mode_800_600);
+
+	msleep(300);
+
+	return interface_param;
+}
+
+static sensor_interface *ov2640_reset(void)
+{
+	return ov2640_config(&reset_frame_rate, 0);
+}
+
+struct camera_sensor camera_sensor_if = {
+	.set_color = ov2640_set_color,
+	.get_color = ov2640_get_color,
+	.set_ae_mode = ov2640_set_ae_mode,
+	.get_ae_mode = ov2640_get_ae_mode,
+	.config = ov2640_config,
+	.reset = ov2640_reset,
+};
+
+EXPORT_SYMBOL(camera_sensor_if);
+
+/*!
+ * ov2640 init function
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int ov2640_init(void)
+{
+	u8 err;
+
+	err = i2c_add_driver(&ov2640_i2c_driver);
+
+	return err;
+}
+
+extern void gpio_sensor_inactive(void);
+/*!
+ * OV2640 cleanup function
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit ov2640_clean(void)
+{
+	i2c_del_driver(&ov2640_i2c_driver);
+
+	gpio_sensor_inactive();
+}
+
+module_init(ov2640_init);
+module_exit(ov2640_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("OV2640 Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/emma_v4l2_capture.c b/drivers/media/video/mxc/capture/emma_v4l2_capture.c
new file mode 100644
index 000000000000..9cb08b26f1cd
--- /dev/null
+++ b/drivers/media/video/mxc/capture/emma_v4l2_capture.c
@@ -0,0 +1,2074 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_v4l2_capture.c
+ *
+ * @brief MX27 Video For Linux 2 driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/pagemap.h>
+#include <linux/vmalloc.h>
+#include <linux/types.h>
+#include <linux/fb.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/semaphore.h>
+#include <linux/version.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+
+#include "mxc_v4l2_capture.h"
+#include "mx27_prp.h"
+#include "mx27_csi.h"
+
+static int csi_mclk_flag_backup;
+static int video_nr = -1;
+static cam_data *g_cam;
+
+/*!
+ * Free frame buffers
+ *
+ * @param cam      Structure cam_data *
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_frame_buf(cam_data *cam)
+{
+	int i;
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		if (cam->frame[i].vaddress != 0) {
+			dma_free_coherent(0,
+					  cam->frame[i].buffer.length,
+					  cam->frame[i].vaddress,
+					  cam->frame[i].paddress);
+			cam->frame[i].vaddress = 0;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * Allocate frame buffers
+ *
+ * @param cam      Structure cam_data *
+ *
+ * @param count    int number of buffer need to allocated
+ *
+ * @return status  -0 Successfully allocated a buffer, -ENOBUFS	failed.
+ */
+static int mxc_allocate_frame_buf(cam_data *cam, int count)
+{
+	int i;
+
+	for (i = 0; i < count; i++) {
+		cam->frame[i].vaddress =
+			dma_alloc_coherent(0,
+					PAGE_ALIGN(cam->v2f. fmt.pix.sizeimage),
+					   &cam->frame[i].paddress,
+					   GFP_DMA | GFP_KERNEL);
+		if (cam->frame[i].vaddress == 0) {
+			pr_debug("mxc_allocate_frame_buf failed.\n");
+			mxc_free_frame_buf(cam);
+			return -ENOBUFS;
+		}
+		cam->frame[i].buffer.index = i;
+		cam->frame[i].buffer.flags = V4L2_BUF_FLAG_MAPPED;
+		cam->frame[i].buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cam->frame[i].buffer.length =
+		    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
+		cam->frame[i].buffer.memory = V4L2_MEMORY_MMAP;
+		cam->frame[i].buffer.m.offset = cam->frame[i].paddress;
+		cam->frame[i].index = i;
+	}
+
+	return 0;
+}
+
+/*!
+ * Free frame buffers status
+ *
+ * @param cam    Structure cam_data *
+ *
+ * @return none
+ */
+static void mxc_free_frames(cam_data *cam)
+{
+	int i;
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		cam->frame[i].buffer.flags = V4L2_BUF_FLAG_MAPPED;
+	}
+
+	cam->enc_counter = 0;
+	cam->skip_frame = 0;
+	INIT_LIST_HEAD(&cam->ready_q);
+	INIT_LIST_HEAD(&cam->working_q);
+	INIT_LIST_HEAD(&cam->done_q);
+}
+
+/*!
+ * Return the buffer status
+ *
+ * @param cam 	   Structure cam_data *
+ * @param buf      Structure v4l2_buffer *
+ *
+ * @return status  0 success, EINVAL failed.
+ */
+static int mxc_v4l2_buffer_status(cam_data *cam, struct v4l2_buffer *buf)
+{
+	/* check range */
+	if (buf->index < 0 || buf->index >= FRAME_NUM) {
+		pr_debug("mxc_v4l2_buffer_status buffers not allocated\n");
+		return -EINVAL;
+	}
+
+	memcpy(buf, &(cam->frame[buf->index].buffer), sizeof(*buf));
+	return 0;
+}
+
+/*!
+ * start the encoder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_streamon(cam_data *cam)
+{
+	struct mxc_v4l_frame *frame;
+	int err = 0;
+
+	if (!cam)
+		return -EIO;
+
+	if (list_empty(&cam->ready_q)) {
+		printk(KERN_ERR "mxc_streamon buffer not been queued yet\n");
+		return -EINVAL;
+	}
+
+	cam->capture_pid = current->pid;
+
+	if (cam->enc_enable) {
+		err = cam->enc_enable(cam);
+		if (err != 0) {
+			return err;
+		}
+	}
+
+	cam->ping_pong_csi = 0;
+	if (cam->enc_update_eba) {
+		frame =
+		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+		list_del(cam->ready_q.next);
+		list_add_tail(&frame->queue, &cam->working_q);
+		err = cam->enc_update_eba(frame->paddress, &cam->ping_pong_csi);
+
+		frame =
+		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+		list_del(cam->ready_q.next);
+		list_add_tail(&frame->queue, &cam->working_q);
+		err |=
+		    cam->enc_update_eba(frame->paddress, &cam->ping_pong_csi);
+	} else {
+		return -EINVAL;
+	}
+
+	return err;
+}
+
+/*!
+ * Shut down the encoder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_streamoff(cam_data *cam)
+{
+	int err = 0;
+
+	if (!cam)
+		return -EIO;
+
+	if (cam->enc_disable) {
+		err = cam->enc_disable(cam);
+	}
+	mxc_free_frames(cam);
+	return err;
+}
+
+/*!
+ * Valid whether the palette is supported
+ *
+ * @param palette pixel format
+ *
+ * @return 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	/*
+	 * MX27 PrP channel 2 supports YUV444, but YUV444 is not
+	 * defined by V4L2 :(
+	 */
+	return ((palette == V4L2_PIX_FMT_YUYV) ||
+		(palette == V4L2_PIX_FMT_YUV420));
+}
+
+/*!
+ * Valid and adjust the overlay window size, position
+ *
+ * @param cam      structure cam_data *
+ * @param win      struct v4l2_window  *
+ *
+ * @return 0
+ */
+static int verify_preview(cam_data *cam, struct v4l2_window *win)
+{
+	if (cam->output >= num_registered_fb) {
+		pr_debug("verify_preview No matched.\n");
+		return -1;
+	}
+	cam->overlay_fb = (struct fb_info *)registered_fb[cam->output];
+
+	/* TODO: suppose 16bpp, 4 bytes alignment */
+	win->w.left &= ~0x1;
+
+	if (win->w.width + win->w.left > cam->overlay_fb->var.xres)
+		win->w.width = cam->overlay_fb->var.xres - win->w.left;
+	if (win->w.height + win->w.top > cam->overlay_fb->var.yres)
+		win->w.height = cam->overlay_fb->var.yres - win->w.top;
+
+	/*
+	 * TODO: suppose 16bpp. Rounded down to a multiple of 2 pixels for
+	 * width according to PrP limitations.
+	 */
+	if ((cam->rotation == V4L2_MXC_ROTATE_90_RIGHT)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_VFLIP)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_HFLIP)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_LEFT))
+		win->w.height &= ~0x1;
+	else
+		win->w.width &= ~0x1;
+
+	return 0;
+}
+
+/*!
+ * start the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int start_preview(cam_data *cam)
+{
+	int err = 0;
+
+	err = prp_vf_select(cam);
+	if (err != 0)
+		return err;
+
+	cam->overlay_pid = current->pid;
+	err = cam->vf_start_sdc(cam);
+
+	return err;
+}
+
+/*!
+ * shut down the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int stop_preview(cam_data *cam)
+{
+	int err = 0;
+
+	err = prp_vf_deselect(cam);
+	return err;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_g_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_g_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		f->fmt.pix.width = cam->v2f.fmt.pix.width;
+		f->fmt.pix.height = cam->v2f.fmt.pix.height;
+		f->fmt.pix.sizeimage = cam->v2f.fmt.pix.sizeimage;
+		f->fmt.pix.pixelformat = cam->v2f.fmt.pix.pixelformat;
+		f->fmt.pix.bytesperline = cam->v2f.fmt.pix.bytesperline;
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
+		retval = 0;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		f->fmt.win = cam->win;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+	int size = 0;
+	int bytesperline = 0;
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		if (!valid_mode(f->fmt.pix.pixelformat)) {
+			pr_debug("mxc_v4l2_s_fmt: format not supported\n");
+			retval = -EINVAL;
+		}
+
+		if (cam->rotation != V4L2_MXC_ROTATE_NONE)
+			pr_debug("mxc_v4l2_s_fmt: capture rotation ignored\n");
+
+		switch (f->fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_YUYV:
+			f->fmt.pix.width &= ~0x1;	/* Multiple of 2 */
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_YUV420:
+			f->fmt.pix.width &= ~0x7;	/* Multiple of 8 */
+			f->fmt.pix.height &= ~0x1;	/* Multiple of 2 */
+			size = f->fmt.pix.width * f->fmt.pix.height * 3 / 2;
+			bytesperline = f->fmt.pix.width * 3 / 2;
+			break;
+		default:
+			/* Suppose it's YUV444 or 32bpp */
+			size = f->fmt.pix.width * f->fmt.pix.height * 4;
+			bytesperline = f->fmt.pix.width * 4;
+			pr_info("mxc_v4l2_s_fmt: default assume"
+				" to be YUV444 interleaved.\n");
+			break;
+		}
+
+		if (f->fmt.pix.bytesperline < bytesperline) {
+			f->fmt.pix.bytesperline = bytesperline;
+		} else {
+			bytesperline = f->fmt.pix.bytesperline;
+		}
+
+		if (f->fmt.pix.sizeimage > size) {
+			pr_debug("mxc_v4l2_s_fmt: sizeimage bigger than"
+				 " needed.\n");
+			size = f->fmt.pix.sizeimage;
+		}
+		f->fmt.pix.sizeimage = size;
+
+		cam->v2f.fmt.pix.sizeimage = size;
+		cam->v2f.fmt.pix.bytesperline = bytesperline;
+		cam->v2f.fmt.pix.width = f->fmt.pix.width;
+		cam->v2f.fmt.pix.height = f->fmt.pix.height;
+		cam->v2f.fmt.pix.pixelformat = f->fmt.pix.pixelformat;
+		retval = 0;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		retval = verify_preview(cam, &f->fmt.win);
+		cam->win = f->fmt.win;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+	return retval;
+}
+
+/*!
+ * get control param
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_get_v42l_control(cam_data *cam, struct v4l2_control *c)
+{
+	int status = 0;
+
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		c->value = cam->rotation;
+		break;
+	case V4L2_CID_VFLIP:
+		c->value = cam->rotation;
+		break;
+	case V4L2_CID_MXC_ROT:
+		c->value = cam->rotation;
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		c->value = cam->bright;
+		break;
+	case V4L2_CID_HUE:
+		c->value = cam->hue;
+		break;
+	case V4L2_CID_CONTRAST:
+		c->value = cam->contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		c->value = cam->saturation;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		c->value = cam->red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		c->value = cam->blue;
+		break;
+	case V4L2_CID_BLACK_LEVEL:
+		c->value = cam->ae_mode;
+		break;
+	default:
+		status = -EINVAL;
+	}
+	return status;
+}
+
+/*!
+ * V4L2 - set_control function
+ * V4L2_CID_MXC_ROT is the extention for rotation/mirroring.
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_set_v42l_control(cam_data *cam, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		if (c->value == 1) {
+			if ((cam->rotation != V4L2_MXC_ROTATE_VERT_FLIP) &&
+			    (cam->rotation != V4L2_MXC_ROTATE_180))
+				cam->rotation = V4L2_MXC_ROTATE_HORIZ_FLIP;
+			else
+				cam->rotation = V4L2_MXC_ROTATE_180;
+		} else {
+			if (cam->rotation == V4L2_MXC_ROTATE_HORIZ_FLIP)
+				cam->rotation = V4L2_MXC_ROTATE_NONE;
+			else if (cam->rotation == V4L2_MXC_ROTATE_180)
+				cam->rotation = V4L2_MXC_ROTATE_VERT_FLIP;
+		}
+		break;
+	case V4L2_CID_VFLIP:
+		if (c->value == 1) {
+			if ((cam->rotation != V4L2_MXC_ROTATE_HORIZ_FLIP) &&
+			    (cam->rotation != V4L2_MXC_ROTATE_180))
+				cam->rotation = V4L2_MXC_ROTATE_VERT_FLIP;
+			else
+				cam->rotation = V4L2_MXC_ROTATE_180;
+		} else {
+			if (cam->rotation == V4L2_MXC_ROTATE_VERT_FLIP)
+				cam->rotation = V4L2_MXC_ROTATE_NONE;
+			if (cam->rotation == V4L2_MXC_ROTATE_180)
+				cam->rotation = V4L2_MXC_ROTATE_HORIZ_FLIP;
+		}
+		break;
+	case V4L2_CID_MXC_ROT:
+		switch (c->value) {
+		case V4L2_MXC_ROTATE_NONE:
+		case V4L2_MXC_ROTATE_VERT_FLIP:
+		case V4L2_MXC_ROTATE_HORIZ_FLIP:
+		case V4L2_MXC_ROTATE_180:
+		case V4L2_MXC_ROTATE_90_RIGHT:
+		case V4L2_MXC_ROTATE_90_RIGHT_VFLIP:
+		case V4L2_MXC_ROTATE_90_RIGHT_HFLIP:
+		case V4L2_MXC_ROTATE_90_LEFT:
+			cam->rotation = c->value;
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case V4L2_CID_HUE:
+		cam->hue = c->value;
+		break;
+	case V4L2_CID_CONTRAST:
+		cam->contrast = c->value;
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		cam->bright = c->value;
+	case V4L2_CID_SATURATION:
+		cam->saturation = c->value;
+	case V4L2_CID_RED_BALANCE:
+		cam->red = c->value;
+	case V4L2_CID_BLUE_BALANCE:
+		cam->blue = c->value;
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		cam->cam_sensor->set_color(cam->bright, cam->saturation,
+					   cam->red, cam->green, cam->blue);
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+		break;
+	case V4L2_CID_BLACK_LEVEL:
+		cam->ae_mode = c->value & 0x03;
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		if (cam->cam_sensor->set_ae_mode)
+			cam->cam_sensor->set_ae_mode(cam->ae_mode);
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+		break;
+	case V4L2_CID_MXC_FLASH:
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_param function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param parm        structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
+{
+	sensor_interface *param;
+	csi_signal_cfg_t csi_param;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pr_debug("mxc_v4l2_s_param invalid type\n");
+		return -EINVAL;
+	}
+
+	if (parm->parm.capture.timeperframe.denominator >
+	    cam->standard.frameperiod.denominator) {
+		pr_debug("mxc_v4l2_s_param frame rate %d larger "
+			 "than standard supported %d\n",
+			 parm->parm.capture.timeperframe.denominator,
+			 cam->standard.frameperiod.denominator);
+		return -EINVAL;
+	}
+
+	cam->streamparm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+
+	csi_enable_mclk(CSI_MCLK_I2C, true, true);
+	param = cam->cam_sensor->config
+	    (&parm->parm.capture.timeperframe.denominator,
+	     parm->parm.capture.capturemode);
+	csi_enable_mclk(CSI_MCLK_I2C, false, false);
+
+	cam->streamparm.parm.capture.timeperframe =
+	    parm->parm.capture.timeperframe;
+
+	if ((parm->parm.capture.capturemode != 0) &&
+	    (parm->parm.capture.capturemode != V4L2_MODE_HIGHQUALITY)) {
+		pr_debug("mxc_v4l2_s_param frame un-supported capture mode\n");
+		return -EINVAL;
+	}
+
+	if (parm->parm.capture.capturemode ==
+	    cam->streamparm.parm.capture.capturemode) {
+		return 0;
+	}
+
+	/* resolution changed, so need to re-program the CSI */
+	csi_param.sens_clksrc = 0;
+	csi_param.clk_mode = param->clk_mode;
+	csi_param.pixclk_pol = param->pixclk_pol;
+	csi_param.data_width = param->data_width;
+	csi_param.data_pol = param->data_pol;
+	csi_param.ext_vsync = param->ext_vsync;
+	csi_param.Vsync_pol = param->Vsync_pol;
+	csi_param.Hsync_pol = param->Hsync_pol;
+	csi_init_interface(param->width, param->height, param->pixel_fmt,
+			   csi_param);
+
+	if (parm->parm.capture.capturemode != V4L2_MODE_HIGHQUALITY) {
+		cam->streamparm.parm.capture.capturemode = 0;
+	} else {
+		cam->streamparm.parm.capture.capturemode =
+		    V4L2_MODE_HIGHQUALITY;
+		cam->streamparm.parm.capture.extendedmode =
+		    parm->parm.capture.extendedmode;
+		cam->streamparm.parm.capture.readbuffers = 1;
+	}
+	return 0;
+}
+
+/*!
+ * Dequeue one V4L capture buffer
+ *
+ * @param cam         structure cam_data *
+ * @param buf         structure v4l2_buffer *
+ *
+ * @return  status    0 success, EINVAL invalid frame number,
+ *                    ETIME timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l_dqueue(cam_data *cam, struct v4l2_buffer *buf)
+{
+	int retval = 0;
+	struct mxc_v4l_frame *frame;
+
+	if (!wait_event_interruptible_timeout(cam->enc_queue,
+					      cam->enc_counter != 0, 10 * HZ)) {
+		printk(KERN_ERR "mxc_v4l_dqueue timeout enc_counter %x\n",
+		       cam->enc_counter);
+		return -ETIME;
+	} else if (signal_pending(current)) {
+		printk(KERN_ERR "mxc_v4l_dqueue() interrupt received\n");
+		return -ERESTARTSYS;
+	}
+
+	cam->enc_counter--;
+
+	frame = list_entry(cam->done_q.next, struct mxc_v4l_frame, queue);
+	list_del(cam->done_q.next);
+	if (frame->buffer.flags & V4L2_BUF_FLAG_DONE) {
+		frame->buffer.flags &= ~V4L2_BUF_FLAG_DONE;
+	} else if (frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
+		printk(KERN_ERR "VIDIOC_DQBUF: Buffer not filled.\n");
+		frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
+		retval = -EINVAL;
+	} else if ((frame->buffer.flags & 0x7) == V4L2_BUF_FLAG_MAPPED) {
+		printk(KERN_ERR "VIDIOC_DQBUF: Buffer not queued.\n");
+		retval = -EINVAL;
+	}
+
+	buf->bytesused = cam->v2f.fmt.pix.sizeimage;
+	buf->index = frame->index;
+	buf->flags = frame->buffer.flags;
+
+	return retval;
+}
+
+/*!
+ * V4L interface - open function
+ *
+ * @param inode        structure inode *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l_open(struct inode *inode, struct file *file)
+{
+	sensor_interface *param;
+	csi_signal_cfg_t csi_param;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int err = 0;
+
+	if (!cam) {
+		pr_info("Internal error, cam_data not found!\n");
+		return -ENODEV;
+	}
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	if (signal_pending(current))
+		goto oops;
+
+	if (cam->open_count++ == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+
+		err = prp_enc_select(cam);
+
+		cam->enc_counter = 0;
+		cam->skip_frame = 0;
+		INIT_LIST_HEAD(&cam->ready_q);
+		INIT_LIST_HEAD(&cam->working_q);
+		INIT_LIST_HEAD(&cam->done_q);
+
+		csi_enable_mclk(CSI_MCLK_I2C, true, true);
+		param = cam->cam_sensor->reset();
+		if (param == NULL) {
+			cam->open_count--;
+			csi_enable_mclk(CSI_MCLK_I2C, false, false);
+			err = -ENODEV;
+			goto oops;
+		}
+		csi_param.sens_clksrc = 0;
+		csi_param.clk_mode = param->clk_mode;
+		csi_param.pixclk_pol = param->pixclk_pol;
+		csi_param.data_width = param->data_width;
+		csi_param.data_pol = param->data_pol;
+		csi_param.ext_vsync = param->ext_vsync;
+		csi_param.Vsync_pol = param->Vsync_pol;
+		csi_param.Hsync_pol = param->Hsync_pol;
+		csi_init_interface(param->width, param->height,
+				   param->pixel_fmt, csi_param);
+		cam->cam_sensor->get_color(&cam->bright, &cam->saturation,
+					   &cam->red, &cam->green, &cam->blue);
+		if (cam->cam_sensor->get_ae_mode)
+			cam->cam_sensor->get_ae_mode(&cam->ae_mode);
+		csi_enable_mclk(CSI_MCLK_I2C, false, false);
+		prp_init(cam);
+
+	}
+
+	file->private_data = dev;
+      oops:
+	up(&cam->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L interface - close function
+ *
+ * @param inode    struct inode *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	int err = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	/* for the case somebody hit the ctrl C */
+	if (cam->overlay_pid == current->pid) {
+		err = stop_preview(cam);
+		cam->overlay_on = false;
+	}
+	if (cam->capture_pid == current->pid) {
+		err |= mxc_streamoff(cam);
+		cam->capture_on = false;
+		wake_up_interruptible(&cam->enc_queue);
+	}
+
+	if (--cam->open_count == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+		pr_debug("mxc_v4l_close: release resource\n");
+
+		err |= prp_enc_deselect(cam);
+
+		mxc_free_frame_buf(cam);
+		file->private_data = NULL;
+
+		/* capture off */
+		wake_up_interruptible(&cam->enc_queue);
+		mxc_free_frames(cam);
+		cam->enc_counter++;
+		prp_exit(cam);
+	}
+
+	return err;
+}
+
+#ifdef CONFIG_VIDEO_MXC_CSI_DMA
+#include <mach/dma.h>
+
+#define CSI_DMA_STATUS_IDLE	0	/* DMA is not started */
+#define CSI_DMA_STATUS_WORKING	1	/* DMA is transfering the data */
+#define CSI_DMA_STATUS_DONE	2	/* One frame completes successfully */
+#define CSI_DMA_STATUS_ERROR	3	/* Error occurs during the DMA */
+
+/*
+ * Sometimes the start of the DMA is not synchronized with the CSI
+ * SOF (Start of Frame) interrupt which will lead to incorrect
+ * captured image. In this case the driver will re-try capturing
+ * another frame. The following macro defines the maximum re-try
+ * times.
+ */
+#define CSI_DMA_RETRY		8
+
+/*
+ * Size of the physical contiguous memory area used to hold image data
+ * transfered by DMA. It can be less than the size of the image data.
+ */
+#define CSI_MEM_SIZE		(1024 * 600)
+
+/* Number of bytes for one DMA transfer */
+#define CSI_DMA_LENGTH		(1024 * 200)
+
+static int g_dma_channel;
+static int g_dma_status = CSI_DMA_STATUS_DONE;
+static volatile int g_dma_completed;	/* number of completed DMA transfers */
+static volatile int g_dma_copied;	/* number of copied DMA transfers */
+static struct tasklet_struct g_dma_tasklet;
+static char *g_user_buf;	/* represents the buf passed by read() */
+static int g_user_count;	/* represents the count passed by read() */
+
+/*!
+ * @brief setup the DMA to transfer data
+ *	  There may be more than one DMA to transfer the whole image. Those
+ *	  DMAs work like chain. This function is used to setup the DMA in
+ *	  case there is enough space to hold the data.
+ * @param	data	pointer to the cam structure
+ */
+static void mxc_csi_dma_chaining(void *data)
+{
+	cam_data *cam = (cam_data *) data;
+	int count, chained = 0;
+	int max_dma = CSI_MEM_SIZE / CSI_DMA_LENGTH;
+	mxc_dma_requestbuf_t dma_request;
+
+	while (chained * CSI_DMA_LENGTH < g_user_count) {
+		/*
+		 * Calculate how many bytes the DMA should transfer. It may
+		 * be less than CSI_DMA_LENGTH if the DMA is the last one.
+		 */
+		if ((chained + 1) * CSI_DMA_LENGTH > g_user_count)
+			count = g_user_count - chained * CSI_DMA_LENGTH;
+		else
+			count = CSI_DMA_LENGTH;
+		pr_debug("%s() DMA chained count = %d\n", __FUNCTION__, count);
+
+		/* Config DMA */
+		memset(&dma_request, 0, sizeof(mxc_dma_requestbuf_t));
+		dma_request.dst_addr = cam->still_buf
+		    + (chained % max_dma) * CSI_DMA_LENGTH;
+		dma_request.src_addr = (dma_addr_t) CSI_CSIRXFIFO_PHYADDR;
+		dma_request.num_of_bytes = count;
+		mxc_dma_config(g_dma_channel, &dma_request, 1,
+			       MXC_DMA_MODE_READ);
+
+		chained++;
+	}
+}
+
+/*!
+ * @brief Copy image data from physical contiguous memory to user space buffer
+ *	  Once the data are copied, there will be more spare space in the
+ *	  physical contiguous memory to receive data from DMA.
+ * @param	data	pointer to the cam structure
+ */
+static void mxc_csi_dma_task(unsigned long data)
+{
+	cam_data *cam = (cam_data *) data;
+	int count;
+	int max_dma = CSI_MEM_SIZE / CSI_DMA_LENGTH;
+
+	while (g_dma_copied < g_dma_completed) {
+		/*
+		 * Calculate how many bytes the DMA has transfered. It may
+		 * be less than CSI_DMA_LENGTH if the DMA is the last one.
+		 */
+		if ((g_dma_copied + 1) * CSI_DMA_LENGTH > g_user_count)
+			count = g_user_count - g_dma_copied * CSI_DMA_LENGTH;
+		else
+			count = CSI_DMA_LENGTH;
+		if (copy_to_user(g_user_buf + g_dma_copied * CSI_DMA_LENGTH,
+				 cam->still_buf_vaddr + (g_dma_copied % max_dma)
+				 * CSI_DMA_LENGTH, count))
+			pr_debug("Warning: some bytes not copied\n");
+
+		g_dma_copied++;
+	}
+
+	/* If the whole image has been captured */
+	if (g_dma_copied * CSI_DMA_LENGTH >= g_user_count) {
+		cam->still_counter++;
+		wake_up_interruptible(&cam->still_queue);
+	}
+
+	pr_debug("%s() DMA completed = %d copied = %d\n",
+		 __FUNCTION__, g_dma_completed, g_dma_copied);
+}
+
+/*!
+ * @brief DMA interrupt callback function
+ * @param	data	pointer to the cam structure
+ * @param	error	DMA error flag
+ * @param	count	number of bytes transfered by the DMA
+ */
+static void mxc_csi_dma_callback(void *data, int error, unsigned int count)
+{
+	cam_data *cam = (cam_data *) data;
+	int max_dma = CSI_MEM_SIZE / CSI_DMA_LENGTH;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&cam->int_lock, lock_flags);
+
+	g_dma_completed++;
+
+	if (error != MXC_DMA_DONE) {
+		g_dma_status = CSI_DMA_STATUS_ERROR;
+		pr_debug("%s() DMA error\n", __FUNCTION__);
+	}
+
+	/* If the whole image has been captured */
+	if ((g_dma_status != CSI_DMA_STATUS_ERROR)
+	    && (g_dma_completed * CSI_DMA_LENGTH >= g_user_count))
+		g_dma_status = CSI_DMA_STATUS_DONE;
+
+	if ((g_dma_status == CSI_DMA_STATUS_WORKING) &&
+	    (g_dma_completed >= g_dma_copied + max_dma)) {
+		g_dma_status = CSI_DMA_STATUS_ERROR;
+		pr_debug("%s() Previous buffer over written\n", __FUNCTION__);
+	}
+
+	/* Schedule the tasklet */
+	tasklet_schedule(&g_dma_tasklet);
+
+	spin_unlock_irqrestore(&cam->int_lock, lock_flags);
+
+	pr_debug("%s() count = %d bytes\n", __FUNCTION__, count);
+}
+
+/*!
+ * @brief CSI interrupt callback function
+ * @param	data	pointer to the cam structure
+ * @param	status	CSI interrupt status
+ */
+static void mxc_csi_irq_callback(void *data, unsigned long status)
+{
+	cam_data *cam = (cam_data *) data;
+	unsigned long lock_flags;
+
+	spin_lock_irqsave(&cam->int_lock, lock_flags);
+
+	/* Wait for SOF (Start of Frame) interrupt to sync the image */
+	if (status & BIT_SOF_INT) {
+		if (g_dma_status == CSI_DMA_STATUS_IDLE) {
+			/* Start DMA transfer to capture image */
+			mxc_dma_enable(g_dma_channel);
+			g_dma_status = CSI_DMA_STATUS_WORKING;
+			pr_debug("%s() DMA started.\n", __FUNCTION__);
+		} else if (g_dma_status == CSI_DMA_STATUS_WORKING) {
+			/*
+			 * Another SOF occurs during DMA transfer. In this
+			 * case the image is not synchronized so need to
+			 * report error and probably try again.
+			 */
+			g_dma_status = CSI_DMA_STATUS_ERROR;
+			pr_debug("%s() Image is not synchronized with DMA - "
+				 "SOF before DMA completes\n", __FUNCTION__);
+		}
+	}
+
+	spin_unlock_irqrestore(&cam->int_lock, lock_flags);
+
+	pr_debug("%s() g_dma_status = %d\n", __FUNCTION__, g_dma_status);
+}
+
+/*!
+ * V4L interface - read function
+ *
+ * @param file       struct file *
+ * @param read buf   char *
+ * @param count      size_t
+ * @param ppos       structure loff_t *
+ *
+ * @return           bytes read
+ */
+static ssize_t
+mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
+{
+	int err = 0;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int retry = CSI_DMA_RETRY;
+
+	g_user_buf = buf;
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	/* Video capture and still image capture are exclusive */
+	if (cam->capture_on == true) {
+		err = -EBUSY;
+		goto exit0;
+	}
+
+	/* The CSI-DMA can not do CSC */
+	if (cam->v2f.fmt.pix.pixelformat != V4L2_PIX_FMT_YUYV) {
+		pr_info("mxc_v4l_read support YUYV pixel format only\n");
+		err = -EINVAL;
+		goto exit0;
+	}
+
+	/* The CSI-DMA can not do resize or crop */
+	if ((cam->v2f.fmt.pix.width != cam->crop_bounds.width)
+	    || (cam->v2f.fmt.pix.height != cam->crop_bounds.height)) {
+		pr_info("mxc_v4l_read resize is not supported\n");
+		pr_info("supported image size width = %d height = %d\n",
+			cam->crop_bounds.width, cam->crop_bounds.height);
+		err = -EINVAL;
+		goto exit0;
+	}
+	if ((cam->crop_current.left != cam->crop_bounds.left)
+	    || (cam->crop_current.width != cam->crop_bounds.width)
+	    || (cam->crop_current.top != cam->crop_bounds.top)
+	    || (cam->crop_current.height != cam->crop_bounds.height)) {
+		pr_info("mxc_v4l_read cropping is not supported\n");
+		err = -EINVAL;
+		goto exit0;
+	}
+
+	cam->still_buf_vaddr = dma_alloc_coherent(0,
+						  PAGE_ALIGN(CSI_MEM_SIZE),
+						  &cam->still_buf,
+						  GFP_DMA | GFP_KERNEL);
+
+	if (!cam->still_buf_vaddr) {
+		pr_info("mxc_v4l_read failed at allocate still_buf\n");
+		err = -ENOBUFS;
+		goto exit0;
+	}
+
+	/* Initialize DMA */
+	g_dma_channel = mxc_dma_request(MXC_DMA_CSI_RX, "CSI RX DMA");
+	if (g_dma_channel < 0) {
+		pr_debug("mxc_v4l_read failed to request DMA channel\n");
+		err = -EIO;
+		goto exit1;
+	}
+
+	err = mxc_dma_callback_set(g_dma_channel,
+				   (mxc_dma_callback_t) mxc_csi_dma_callback,
+				   (void *)cam);
+	if (err != 0) {
+		pr_debug("mxc_v4l_read failed to set DMA callback\n");
+		err = -EIO;
+		goto exit2;
+	}
+
+	g_user_buf = buf;
+	if (cam->v2f.fmt.pix.sizeimage < count)
+		g_user_count = cam->v2f.fmt.pix.sizeimage;
+	else
+		g_user_count = count & ~0x3;
+
+	tasklet_init(&g_dma_tasklet, mxc_csi_dma_task, (unsigned long)cam);
+	g_dma_status = CSI_DMA_STATUS_DONE;
+	csi_set_callback(mxc_csi_irq_callback, cam);
+	csi_enable_prpif(0);
+
+	/* clear current SOF first */
+	csi_clear_status(BIT_SOF_INT);
+	csi_enable_mclk(CSI_MCLK_RAW, true, true);
+
+	do {
+		g_dma_completed = g_dma_copied = 0;
+		mxc_csi_dma_chaining(cam);
+		cam->still_counter = 0;
+		g_dma_status = CSI_DMA_STATUS_IDLE;
+
+		if (!wait_event_interruptible_timeout(cam->still_queue,
+						      cam->still_counter != 0,
+						      10 * HZ)) {
+			pr_info("mxc_v4l_read timeout counter %x\n",
+				cam->still_counter);
+			err = -ETIME;
+			goto exit3;
+		}
+
+		if (g_dma_status == CSI_DMA_STATUS_DONE)
+			break;
+
+		if (retry-- == 0)
+			break;
+
+		pr_debug("Now retry image capture\n");
+	} while (1);
+
+	if (g_dma_status != CSI_DMA_STATUS_DONE)
+		err = -EIO;
+
+      exit3:
+	csi_enable_prpif(1);
+	g_dma_status = CSI_DMA_STATUS_DONE;
+	csi_set_callback(0, 0);
+	csi_enable_mclk(CSI_MCLK_RAW, false, false);
+	tasklet_kill(&g_dma_tasklet);
+
+      exit2:
+	mxc_dma_free(g_dma_channel);
+
+      exit1:
+	dma_free_coherent(0, PAGE_ALIGN(CSI_MEM_SIZE),
+			  cam->still_buf_vaddr, cam->still_buf);
+	cam->still_buf = 0;
+
+      exit0:
+	up(&cam->busy_lock);
+	if (err < 0)
+		return err;
+	else
+		return g_user_count;
+}
+#else
+/*!
+ * V4L interface - read function
+ *
+ * @param file       struct file *
+ * @param read buf   char *
+ * @param count      size_t
+ * @param ppos       structure loff_t *
+ *
+ * @return           bytes read
+ */
+static ssize_t
+mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
+{
+	int err = 0;
+	u8 *v_address;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	/* Video capture and still image capture are exclusive */
+	if (cam->capture_on == true) {
+		err = -EBUSY;
+		goto exit0;
+	}
+
+	v_address = dma_alloc_coherent(0,
+				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				       &cam->still_buf, GFP_DMA | GFP_KERNEL);
+
+	if (!v_address) {
+		pr_info("mxc_v4l_read failed at allocate still_buf\n");
+		err = -ENOBUFS;
+		goto exit0;
+	}
+
+	if (prp_still_select(cam)) {
+		err = -EIO;
+		goto exit1;
+	}
+
+	cam->still_counter = 0;
+	if (cam->csi_start(cam)) {
+		err = -EIO;
+		goto exit2;
+	}
+
+	if (!wait_event_interruptible_timeout(cam->still_queue,
+					      cam->still_counter != 0,
+					      10 * HZ)) {
+		pr_info("mxc_v4l_read timeout counter %x\n",
+			cam->still_counter);
+		err = -ETIME;
+		goto exit2;
+	}
+	err = copy_to_user(buf, v_address, cam->v2f.fmt.pix.sizeimage);
+
+      exit2:
+	prp_still_deselect(cam);
+
+      exit1:
+	dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address,
+			  cam->still_buf);
+	cam->still_buf = 0;
+
+      exit0:
+	up(&cam->busy_lock);
+	if (err < 0)
+		return err;
+	else
+		return (cam->v2f.fmt.pix.sizeimage - err);
+}
+#endif				/* CONFIG_VIDEO_MXC_CSI_DMA */
+
+/*!
+ * V4L interface - ioctl function
+ *
+ * @param inode      struct inode *
+ *
+ * @param file       struct file *
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void *
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int
+mxc_v4l_do_ioctl(struct inode *inode, struct file *file,
+		 unsigned int ioctlnr, void *arg)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int retval = 0;
+	unsigned long lock_flags;
+
+	if (!cam)
+		return -EBADF;
+
+	wait_event_interruptible(cam->power_queue, cam->low_power == false);
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+		/*!
+		 * V4l2 VIDIOC_QUERYCAP ioctl
+		 */
+	case VIDIOC_QUERYCAP:{
+			struct v4l2_capability *cap = arg;
+			strcpy(cap->driver, "mxc_v4l2");
+			cap->version = KERNEL_VERSION(0, 1, 11);
+			cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+			    V4L2_CAP_VIDEO_OVERLAY | V4L2_CAP_STREAMING
+			    | V4L2_CAP_READWRITE;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			retval = 0;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_FMT ioctl
+		 */
+	case VIDIOC_G_FMT:{
+			struct v4l2_format *gf = arg;
+			retval = mxc_v4l2_g_fmt(cam, gf);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_FMT ioctl
+		 */
+	case VIDIOC_S_FMT:{
+			struct v4l2_format *sf = arg;
+			retval = mxc_v4l2_s_fmt(cam, sf);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_REQBUFS ioctl
+		 */
+	case VIDIOC_REQBUFS:{
+			struct v4l2_requestbuffers *req = arg;
+			if (req->count > FRAME_NUM) {
+				pr_info("VIDIOC_REQBUFS: not enough buffer\n");
+				req->count = FRAME_NUM;
+			}
+
+			if ((req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+			    (req->memory != V4L2_MEMORY_MMAP)) {
+				pr_debug("VIDIOC_REQBUFS: wrong buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			mxc_streamoff(cam);
+			mxc_free_frame_buf(cam);
+
+			retval = mxc_allocate_frame_buf(cam, req->count);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_QUERYBUF ioctl
+		 */
+	case VIDIOC_QUERYBUF:{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+
+			if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+				pr_debug
+				    ("VIDIOC_QUERYBUFS: wrong buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			memset(buf, 0, sizeof(buf));
+			buf->index = index;
+
+			down(&cam->param_lock);
+			retval = mxc_v4l2_buffer_status(cam, buf);
+			up(&cam->param_lock);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_QBUF ioctl
+		 */
+	case VIDIOC_QBUF:{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+
+			pr_debug("VIDIOC_QBUF: %d\n", buf->index);
+
+			spin_lock_irqsave(&cam->int_lock, lock_flags);
+			if ((cam->frame[index].buffer.flags & 0x7) ==
+			    V4L2_BUF_FLAG_MAPPED) {
+				cam->frame[index].buffer.flags |=
+				    V4L2_BUF_FLAG_QUEUED;
+				if (cam->skip_frame > 0) {
+					list_add_tail(&cam->frame[index].queue,
+						      &cam->working_q);
+					retval =
+					    cam->enc_update_eba(cam->
+								frame[index].
+								paddress,
+								&cam->
+								ping_pong_csi);
+					cam->skip_frame = 0;
+				} else {
+					list_add_tail(&cam->frame[index].queue,
+						      &cam->ready_q);
+				}
+			} else if (cam->frame[index].buffer.flags &
+				   V4L2_BUF_FLAG_QUEUED) {
+				pr_debug
+				    ("VIDIOC_QBUF: buffer already queued\n");
+			} else if (cam->frame[index].buffer.
+				   flags & V4L2_BUF_FLAG_DONE) {
+				pr_debug
+				    ("VIDIOC_QBUF: overwrite done buffer.\n");
+				cam->frame[index].buffer.flags &=
+				    ~V4L2_BUF_FLAG_DONE;
+				cam->frame[index].buffer.flags |=
+				    V4L2_BUF_FLAG_QUEUED;
+			}
+			buf->flags = cam->frame[index].buffer.flags;
+			spin_unlock_irqrestore(&cam->int_lock, lock_flags);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_DQBUF ioctl
+		 */
+	case VIDIOC_DQBUF:{
+			struct v4l2_buffer *buf = arg;
+
+			retval = mxc_v4l_dqueue(cam, buf);
+
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_STREAMON ioctl
+		 */
+	case VIDIOC_STREAMON:{
+			cam->capture_on = true;
+			retval = mxc_streamon(cam);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_STREAMOFF ioctl
+		 */
+	case VIDIOC_STREAMOFF:{
+			retval = mxc_streamoff(cam);
+			cam->capture_on = false;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_CTRL ioctl
+		 */
+	case VIDIOC_G_CTRL:{
+			retval = mxc_get_v42l_control(cam, arg);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_CTRL ioctl
+		 */
+	case VIDIOC_S_CTRL:{
+			retval = mxc_set_v42l_control(cam, arg);
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_CROPCAP ioctl
+		 */
+	case VIDIOC_CROPCAP:{
+			struct v4l2_cropcap *cap = arg;
+
+			if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+			    cap->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+				retval = -EINVAL;
+				break;
+			}
+			cap->bounds = cam->crop_bounds;
+			cap->defrect = cam->crop_defrect;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_CROP ioctl
+		 */
+	case VIDIOC_G_CROP:{
+			struct v4l2_crop *crop = arg;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+			    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+				retval = -EINVAL;
+				break;
+			}
+			crop->c = cam->crop_current;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_CROP ioctl
+		 */
+	case VIDIOC_S_CROP:{
+			struct v4l2_crop *crop = arg;
+			struct v4l2_rect *b = &cam->crop_bounds;
+			int i;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+			    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+				retval = -EINVAL;
+				break;
+			}
+
+			crop->c.top = (crop->c.top < b->top) ? b->top
+			    : crop->c.top;
+			if (crop->c.top > b->top + b->height)
+				crop->c.top = b->top + b->height - 1;
+			if (crop->c.height > b->top + b->height - crop->c.top)
+				crop->c.height =
+				    b->top + b->height - crop->c.top;
+
+			crop->c.left = (crop->c.left < b->left) ? b->left
+			    : crop->c.left;
+			if (crop->c.left > b->left + b->width)
+				crop->c.left = b->left + b->width - 1;
+			if (crop->c.width > b->left - crop->c.left + b->width)
+				crop->c.width =
+				    b->left - crop->c.left + b->width;
+
+			crop->c.width &= ~0x1;
+
+			/*
+			 * MX27 PrP limitation:
+			 * The right spare space (CSI_FRAME_X_SIZE
+			 *  - SOURCE_LINE_STRIDE - PICTURE_X_SIZE)) must be
+			 * multiple of 32.
+			 * So we tune the crop->c.left value to the closest
+			 * desired cropping value and meet the PrP requirement.
+			 */
+			i = ((b->left + b->width)
+			     - (crop->c.left + crop->c.width)) % 32;
+			if (i <= 16) {
+				if (crop->c.left + crop->c.width + i
+				    <= b->left + b->width)
+					crop->c.left += i;
+				else if (crop->c.left - (32 - i) >= b->left)
+					crop->c.left -= 32 - i;
+				else {
+					retval = -EINVAL;
+					break;
+				}
+			} else {
+				if (crop->c.left - (32 - i) >= b->left)
+					crop->c.left -= 32 - i;
+				else if (crop->c.left + crop->c.width + i
+					 <= b->left + b->width)
+					crop->c.left += i;
+				else {
+					retval = -EINVAL;
+					break;
+				}
+			}
+
+			cam->crop_current = crop->c;
+
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_OVERLAY ioctl
+		 */
+	case VIDIOC_OVERLAY:{
+			int *on = arg;
+			if (*on) {
+				cam->overlay_on = true;
+				retval = start_preview(cam);
+			}
+			if (!*on) {
+				retval = stop_preview(cam);
+				cam->overlay_on = false;
+			}
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_G_FBUF ioctl
+		 */
+	case VIDIOC_G_FBUF:{
+			struct v4l2_framebuffer *fb = arg;
+			struct fb_var_screeninfo *var;
+
+			if (cam->output >= num_registered_fb) {
+				retval = -EINVAL;
+				break;
+			}
+
+			var = &registered_fb[cam->output]->var;
+			cam->v4l2_fb.fmt.width = var->xres;
+			cam->v4l2_fb.fmt.height = var->yres;
+			cam->v4l2_fb.fmt.bytesperline =
+			    var->xres_virtual * var->bits_per_pixel;
+			cam->v4l2_fb.fmt.colorspace = V4L2_COLORSPACE_SRGB;
+			*fb = cam->v4l2_fb;
+			break;
+		}
+
+		/*!
+		 * V4l2 VIDIOC_S_FBUF ioctl
+		 */
+	case VIDIOC_S_FBUF:{
+			struct v4l2_framebuffer *fb = arg;
+			cam->v4l2_fb.flags = fb->flags;
+			cam->v4l2_fb.fmt.pixelformat = fb->fmt.pixelformat;
+			break;
+		}
+
+	case VIDIOC_G_PARM:{
+			struct v4l2_streamparm *parm = arg;
+			if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+				pr_debug("VIDIOC_G_PARM invalid type\n");
+				retval = -EINVAL;
+				break;
+			}
+			parm->parm.capture = cam->streamparm.parm.capture;
+			break;
+		}
+	case VIDIOC_S_PARM:{
+			struct v4l2_streamparm *parm = arg;
+			retval = mxc_v4l2_s_param(cam, parm);
+			break;
+		}
+
+		/* linux v4l2 bug, kernel c0485619 user c0405619 */
+	case VIDIOC_ENUMSTD:{
+			struct v4l2_standard *e = arg;
+			*e = cam->standard;
+			pr_debug("VIDIOC_ENUMSTD call\n");
+			retval = 0;
+			break;
+		}
+
+	case VIDIOC_G_STD:{
+			v4l2_std_id *e = arg;
+			*e = cam->standard.id;
+			break;
+		}
+
+	case VIDIOC_S_STD:{
+			break;
+		}
+
+	case VIDIOC_ENUMOUTPUT:
+		{
+			struct v4l2_output *output = arg;
+
+			if (output->index >= num_registered_fb) {
+				retval = -EINVAL;
+				break;
+			}
+
+			strncpy(output->name,
+				registered_fb[output->index]->fix.id, 31);
+			output->type = V4L2_OUTPUT_TYPE_ANALOG;
+			output->audioset = 0;
+			output->modulator = 0;
+			output->std = V4L2_STD_UNKNOWN;
+
+			break;
+		}
+	case VIDIOC_G_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			*p_output_num = cam->output;
+			break;
+		}
+	case VIDIOC_S_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			if (*p_output_num >= num_registered_fb) {
+				retval = -EINVAL;
+				break;
+			}
+
+			cam->output = *p_output_num;
+			break;
+		}
+
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_ENUMINPUT:
+	case VIDIOC_G_INPUT:
+	case VIDIOC_S_INPUT:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&cam->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L interface - ioctl function
+ *
+ * @return  None
+ */
+static int
+mxc_v4l_ioctl(struct inode *inode, struct file *file,
+	      unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l_do_ioctl);
+}
+
+/*!
+ * V4L interface - mmap function
+ *
+ * @param file        structure file *
+ *
+ * @param vma         structure vm_area_struct *
+ *
+ * @return status     0 Success, EINTR busy lock error, ENOBUFS remap_page error
+ */
+static int mxc_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = video_devdata(file);
+	unsigned long size;
+	int res = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("pgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		 vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	size = vma->vm_end - vma->vm_start;
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		pr_debug("mxc_mmap: remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+      mxc_mmap_exit:
+	up(&cam->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_poll(struct file *file, poll_table * wait)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	queue = &cam->enc_queue;
+	poll_wait(file, queue, wait);
+
+	up(&cam->busy_lock);
+	return res;
+}
+
+static struct
+file_operations mxc_v4l_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l_open,
+	.release = mxc_v4l_close,
+	.read = mxc_v4l_read,
+	.ioctl = mxc_v4l_ioctl,
+	.mmap = mxc_mmap,
+	.poll = mxc_poll,
+};
+
+static struct video_device mxc_v4l_template = {
+	.name = "Mxc Camera",
+	.vfl_type = VID_TYPE_CAPTURE,
+	.fops = &mxc_v4l_fops,
+	.release = video_device_release,
+};
+
+static void camera_platform_release(struct device *device)
+{
+}
+
+/*! Device Definition for Mt9v111 devices */
+static struct platform_device mxc_v4l2_devices = {
+	.name = "mxc_v4l2",
+	.dev = {
+		.release = camera_platform_release,
+		},
+	.id = 0,
+};
+
+extern struct camera_sensor camera_sensor_if;
+
+/*!
+* Camera V4l2 callback function.
+*
+* @return status
+*/
+static void camera_callback(u32 mask, void *dev)
+{
+	struct mxc_v4l_frame *done_frame;
+	struct mxc_v4l_frame *ready_frame;
+
+	cam_data *cam = (cam_data *) dev;
+	if (cam == NULL)
+		return;
+
+	if (list_empty(&cam->working_q)) {
+		printk(KERN_ERR "camera_callback: working queue empty\n");
+		return;
+	}
+
+	done_frame =
+	    list_entry(cam->working_q.next, struct mxc_v4l_frame, queue);
+	if (done_frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
+		done_frame->buffer.flags |= V4L2_BUF_FLAG_DONE;
+		done_frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
+
+		if (list_empty(&cam->ready_q)) {
+			cam->skip_frame++;
+		} else {
+			ready_frame =
+			    list_entry(cam->ready_q.next, struct mxc_v4l_frame,
+				       queue);
+			list_del(cam->ready_q.next);
+			list_add_tail(&ready_frame->queue, &cam->working_q);
+			cam->enc_update_eba(ready_frame->paddress,
+					    &cam->ping_pong_csi);
+		}
+
+		/* Added to the done queue */
+		list_del(cam->working_q.next);
+		list_add_tail(&done_frame->queue, &cam->done_q);
+
+		/* Wake up the queue */
+		cam->enc_counter++;
+		wake_up_interruptible(&cam->enc_queue);
+	} else {
+		printk(KERN_ERR "camera_callback :buffer not queued\n");
+	}
+}
+
+/*!
+ * initialize cam_data structure
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static void init_camera_struct(cam_data *cam)
+{
+	int i;
+
+	/* Default everything to 0 */
+	memset(cam, 0, sizeof(cam_data));
+
+	init_MUTEX(&cam->param_lock);
+	init_MUTEX(&cam->busy_lock);
+
+	cam->video_dev = video_device_alloc();
+	if (cam->video_dev == NULL)
+		return;
+
+	*(cam->video_dev) = mxc_v4l_template;
+
+	video_set_drvdata(cam->video_dev, cam);
+	dev_set_drvdata(&mxc_v4l2_devices.dev, (void *)cam);
+	cam->video_dev->minor = -1;
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		cam->frame[i].width = 0;
+		cam->frame[i].height = 0;
+		cam->frame[i].paddress = 0;
+	}
+
+	init_waitqueue_head(&cam->enc_queue);
+	init_waitqueue_head(&cam->still_queue);
+
+	/* setup cropping */
+	cam->crop_bounds.left = 0;
+	cam->crop_bounds.width = 640;
+	cam->crop_bounds.top = 0;
+	cam->crop_bounds.height = 480;
+	cam->crop_current = cam->crop_defrect = cam->crop_bounds;
+	cam->streamparm.parm.capture.capturemode = 0;
+
+	cam->standard.index = 0;
+	cam->standard.id = V4L2_STD_UNKNOWN;
+	cam->standard.frameperiod.denominator = 30;
+	cam->standard.frameperiod.numerator = 1;
+	cam->standard.framelines = 480;
+	cam->streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	cam->streamparm.parm.capture.timeperframe = cam->standard.frameperiod;
+	cam->streamparm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	cam->overlay_on = false;
+	cam->capture_on = false;
+	cam->skip_frame = 0;
+	cam->v4l2_fb.capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+	cam->v4l2_fb.flags = V4L2_FBUF_FLAG_PRIMARY;
+
+	cam->v2f.fmt.pix.sizeimage = 352 * 288 * 3 / 2;
+	cam->v2f.fmt.pix.bytesperline = 288 * 3 / 2;
+	cam->v2f.fmt.pix.width = 288;
+	cam->v2f.fmt.pix.height = 352;
+	cam->v2f.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
+	cam->win.w.width = 160;
+	cam->win.w.height = 160;
+	cam->win.w.left = 0;
+	cam->win.w.top = 0;
+
+	cam->cam_sensor = &camera_sensor_if;
+	cam->enc_callback = camera_callback;
+
+	init_waitqueue_head(&cam->power_queue);
+	cam->int_lock = __SPIN_LOCK_UNLOCKED(cam->int_lock);
+	spin_lock_init(&cam->int_lock);
+}
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+/*!
+ * camera_power function
+ *    Turn Sensor power On/Off
+ *
+ * @param       cameraOn      true to turn camera on, otherwise shut down
+ *
+ * @return status
+ */
+static u8 camera_power(bool cameraOn)
+{
+	if (cameraOn == true) {
+		gpio_sensor_active();
+		csi_enable_mclk(csi_mclk_flag_backup, true, true);
+	} else {
+		csi_mclk_flag_backup = csi_read_mclk_flag();
+		csi_enable_mclk(csi_mclk_flag_backup, false, false);
+		gpio_sensor_inactive();
+	}
+	return 0;
+}
+
+/*!
+ * This function is called to put the sensor in a low power state. Refer to the
+ * document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure used to give information on which I2C
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure.
+ */
+static int mxc_v4l2_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	if (cam == NULL) {
+		return -1;
+	}
+
+	cam->low_power = true;
+
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+	if ((cam->capture_on == true) && cam->enc_disable) {
+		cam->enc_disable(cam);
+	}
+	camera_power(false);
+
+	return 0;
+}
+
+/*!
+ * This function is called to bring the sensor back from a low power state.Refer
+ * to the document driver-model/driver.txt in the kernel source tree for more
+ * information.
+ *
+ * @param   pdev  the device structure
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int mxc_v4l2_resume(struct platform_device *pdev)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	if (cam == NULL) {
+		return -1;
+	}
+
+	cam->low_power = false;
+	wake_up_interruptible(&cam->power_queue);
+
+	if (cam->overlay_on == true)
+		start_preview(cam);
+	if (cam->capture_on == true)
+		mxc_streamon(cam);
+	camera_power(true);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2_driver = {
+	.driver = {
+		   .name = "mxc_v4l2",
+		   .owner = THIS_MODULE,
+		   .bus = &platform_bus_type,
+		   },
+	.probe = NULL,
+	.remove = NULL,
+	.suspend = mxc_v4l2_suspend,
+	.resume = mxc_v4l2_resume,
+	.shutdown = NULL,
+};
+
+/*!
+ * Entry point for the V4L2
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int camera_init(void)
+{
+	u8 err = 0;
+	cam_data *cam;
+
+	g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL);
+	if (g_cam == NULL) {
+		pr_debug("failed to mxc_v4l_register_camera\n");
+		return -1;
+	}
+
+	cam = g_cam;
+	init_camera_struct(cam);
+
+	/* Register the I2C device */
+	err = platform_device_register(&mxc_v4l2_devices);
+	if (err != 0) {
+		pr_debug("camera_init: platform_device_register failed.\n");
+		video_device_release(cam->video_dev);
+		kfree(cam);
+		g_cam = NULL;
+	}
+
+	/* Register the device driver structure. */
+	err = platform_driver_register(&mxc_v4l2_driver);
+	if (err != 0) {
+		platform_device_unregister(&mxc_v4l2_devices);
+		pr_debug("camera_init: driver_register failed.\n");
+		video_device_release(cam->video_dev);
+		kfree(cam);
+		g_cam = NULL;
+		return err;
+	}
+
+	/* register v4l device */
+	if (video_register_device(cam->video_dev, VFL_TYPE_GRABBER, video_nr)
+	    == -1) {
+		platform_driver_unregister(&mxc_v4l2_driver);
+		platform_device_unregister(&mxc_v4l2_devices);
+		video_device_release(cam->video_dev);
+		kfree(cam);
+		g_cam = NULL;
+		pr_debug("video_register_device failed\n");
+		return -1;
+	}
+
+	return err;
+}
+
+/*!
+ * Exit and cleanup for the V4L2
+ *
+ */
+static void __exit camera_exit(void)
+{
+	pr_debug("unregistering video\n");
+
+	video_unregister_device(g_cam->video_dev);
+
+	platform_driver_unregister(&mxc_v4l2_driver);
+	platform_device_unregister(&mxc_v4l2_devices);
+
+	if (g_cam->open_count) {
+		pr_debug("camera open -- setting ops to NULL\n");
+	} else {
+		pr_debug("freeing camera\n");
+		mxc_free_frame_buf(g_cam);
+		kfree(g_cam);
+		g_cam = NULL;
+	}
+}
+
+module_init(camera_init);
+module_exit(camera_exit);
+
+module_param(video_nr, int, 0444);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2 capture driver for Mxc based cameras");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/capture/fsl_csi.c b/drivers/media/video/mxc/capture/fsl_csi.c
new file mode 100644
index 000000000000..4f14f26eede1
--- /dev/null
+++ b/drivers/media/video/mxc/capture/fsl_csi.c
@@ -0,0 +1,276 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_csi.c, this file is derived from mx27_csi.c
+ *
+ * @brief mx25 CMOS Sensor interface functions
+ *
+ * @ingroup CSI
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <mach/clock.h>
+#include <mach/hardware.h>
+
+#include "mxc_v4l2_capture.h"
+#include "fsl_csi.h"
+
+static bool g_csi_mclk_on;
+static csi_irq_callback_t g_callback;
+static void *g_callback_data;
+static struct clk csi_mclk;
+
+static irqreturn_t csi_irq_handler(int irq, void *data)
+{
+	cam_data *cam = (cam_data *) data;
+	unsigned long status = __raw_readl(CSI_CSISR);
+	unsigned long cr3 = __raw_readl(CSI_CSICR3);
+	unsigned int frame_count = (cr3 >> 16) & 0xFFFF;
+
+	__raw_writel(status, CSI_CSISR);
+
+	if (status & BIT_SOF_INT) {
+		/* reflash the embeded DMA controller */
+		if (frame_count % 2 == 1)
+			__raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+	}
+
+	if (status & BIT_DMA_TSF_DONE_FB2) {
+		if (frame_count == 2) {
+			cam->still_counter++;
+			wake_up_interruptible(&cam->still_queue);
+		}
+	}
+
+	if (g_callback)
+		g_callback(g_callback_data, status);
+
+	pr_debug("CSI status = 0x%08lX\n", status);
+
+	return IRQ_HANDLED;
+}
+
+static void csihw_reset_frame_count(void)
+{
+	__raw_writel(__raw_readl(CSI_CSICR3) | BIT_FRMCNT_RST, CSI_CSICR3);
+}
+
+static void csihw_reset(void)
+{
+	csihw_reset_frame_count();
+	__raw_writel(CSICR1_RESET_VAL, CSI_CSICR1);
+	__raw_writel(CSICR2_RESET_VAL, CSI_CSICR2);
+	__raw_writel(CSICR3_RESET_VAL, CSI_CSICR3);
+}
+
+/*!
+ * csi_init_interface
+ *    Init csi interface
+ */
+void csi_init_interface(void)
+{
+	unsigned int val = 0;
+	unsigned int imag_para;
+
+	val |= BIT_SOF_POL;
+	val |= BIT_REDGE;
+	val |= BIT_GCLK_MODE;
+	val |= BIT_HSYNC_POL;
+	val |= BIT_PACK_DIR;
+	val |= BIT_FCC;
+	val |= BIT_SWAP16_EN;
+	val |= 1 << SHIFT_MCLKDIV;
+	__raw_writel(val, CSI_CSICR1);
+
+	imag_para = (640 << 16) | 960;
+	__raw_writel(imag_para, CSI_CSIIMAG_PARA);
+
+	val = 0x1010;
+	val |= BIT_DMA_REFLASH_RFF;
+	__raw_writel(val, CSI_CSICR3);
+}
+EXPORT_SYMBOL(csi_init_interface);
+
+/*!
+ * csi_enable_mclk
+ *
+ * @param       src         enum define which source to control the clk
+ *                          CSI_MCLK_VF CSI_MCLK_ENC CSI_MCLK_RAW CSI_MCLK_I2C
+ * @param       flag        true to enable mclk, false to disable mclk
+ * @param       wait        true to wait 100ms make clock stable, false not wait
+ *
+ * @return      0 for success
+ */
+int32_t csi_enable_mclk(int src, bool flag, bool wait)
+{
+	if (flag == true) {
+		csi_mclk_enable();
+		if (wait == true)
+			msleep(10);
+		pr_debug("Enable csi clock from source %d\n", src);
+		g_csi_mclk_on = true;
+	} else {
+		csi_mclk_disable();
+		pr_debug("Disable csi clock from source %d\n", src);
+		g_csi_mclk_on = false;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(csi_enable_mclk);
+
+/*!
+ * csi_read_mclk_flag
+ *
+ * @return  gcsi_mclk_source
+ */
+int csi_read_mclk_flag(void)
+{
+	return 0;
+}
+EXPORT_SYMBOL(csi_read_mclk_flag);
+
+void csi_start_callback(void *data)
+{
+	cam_data *cam = (cam_data *) data;
+
+	if (request_irq(MXC_INT_CSI, csi_irq_handler, 0, "csi", cam) < 0)
+		pr_debug("CSI error: irq request fail\n");
+
+}
+EXPORT_SYMBOL(csi_start_callback);
+
+void csi_stop_callback(void *data)
+{
+	cam_data *cam = (cam_data *) data;
+
+	free_irq(MXC_INT_CSI, cam);
+}
+EXPORT_SYMBOL(csi_stop_callback);
+
+void csi_enable_int(int arg)
+{
+	unsigned long cr1 = __raw_readl(CSI_CSICR1);
+
+	cr1 |= BIT_SOF_INTEN;
+	if (arg == 1) {
+		/* still capture needs DMA intterrupt */
+		cr1 |= BIT_FB1_DMA_DONE_INTEN;
+		cr1 |= BIT_FB2_DMA_DONE_INTEN;
+	}
+	__raw_writel(cr1, CSI_CSICR1);
+}
+EXPORT_SYMBOL(csi_enable_int);
+
+void csi_disable_int(void)
+{
+	unsigned long cr1 = __raw_readl(CSI_CSICR1);
+
+	cr1 &= ~BIT_SOF_INTEN;
+	cr1 &= ~BIT_FB1_DMA_DONE_INTEN;
+	cr1 &= ~BIT_FB2_DMA_DONE_INTEN;
+	__raw_writel(cr1, CSI_CSICR1);
+}
+EXPORT_SYMBOL(csi_disable_int);
+
+void csi_set_16bit_imagpara(int width, int height)
+{
+	int imag_para = 0;
+	unsigned long cr3 = __raw_readl(CSI_CSICR3);
+
+	imag_para = (width << 16) | (height * 2);
+	__raw_writel(imag_para, CSI_CSIIMAG_PARA);
+
+	/* reflash the embeded DMA controller */
+	__raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+}
+EXPORT_SYMBOL(csi_set_16bit_imagpara);
+
+void csi_set_12bit_imagpara(int width, int height)
+{
+	int imag_para = 0;
+	unsigned long cr3 = __raw_readl(CSI_CSICR3);
+
+	imag_para = (width << 16) | (height * 3 / 2);
+	__raw_writel(imag_para, CSI_CSIIMAG_PARA);
+
+	/* reflash the embeded DMA controller */
+	__raw_writel(cr3 | BIT_DMA_REFLASH_RFF, CSI_CSICR3);
+}
+EXPORT_SYMBOL(csi_set_12bit_imagpara);
+
+static void csi_mclk_recalc(struct clk *clk)
+{
+	u32 div;
+
+	div = (__raw_readl(CSI_CSICR1) & BIT_MCLKDIV) >> SHIFT_MCLKDIV;
+	if (div == 0)
+		div = 1;
+	else
+		div = div * 2;
+
+	clk->rate = clk->parent->rate / div;
+}
+
+void csi_mclk_enable(void)
+{
+	__raw_writel(__raw_readl(CSI_CSICR1) | BIT_MCLKEN, CSI_CSICR1);
+}
+
+void csi_mclk_disable(void)
+{
+	__raw_writel(__raw_readl(CSI_CSICR1) & ~BIT_MCLKEN, CSI_CSICR1);
+}
+
+int32_t __init csi_init_module(void)
+{
+	int ret = 0;
+	struct clk *per_clk;
+
+	csihw_reset();
+	csi_init_interface();
+
+	per_clk = clk_get(NULL, "csi_clk");
+	if (IS_ERR(per_clk))
+		return PTR_ERR(per_clk);
+
+	clk_put(per_clk);
+	csi_mclk.name = "csi_mclk";
+	csi_mclk.parent = per_clk;
+	clk_register(&csi_mclk);
+	clk_enable(per_clk);
+	csi_mclk_recalc(&csi_mclk);
+
+	return ret;
+}
+
+void __exit csi_cleanup_module(void)
+{
+	clk_disable(&csi_mclk);
+	clk_unregister(&csi_mclk);
+}
+
+module_init(csi_init_module);
+module_exit(csi_cleanup_module);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("fsl CSI driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/fsl_csi.h b/drivers/media/video/mxc/capture/fsl_csi.h
new file mode 100644
index 000000000000..41bfff03f07e
--- /dev/null
+++ b/drivers/media/video/mxc/capture/fsl_csi.h
@@ -0,0 +1,197 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file fsl_csi.h
+ *
+ * @brief mx25 CMOS Sensor interface functions
+ *
+ * @ingroup CSI
+ */
+
+#ifndef MX25_CSI_H
+#define MX25_CSI_H
+
+#include <linux/io.h>
+
+/* reset values */
+#define CSICR1_RESET_VAL	0x40000800
+#define CSICR2_RESET_VAL	0x0
+#define CSICR3_RESET_VAL	0x0
+
+/* csi control reg 1 */
+#define BIT_SWAP16_EN		(0x1 << 31)
+#define BIT_EXT_VSYNC		(0x1 << 30)
+#define BIT_EOF_INT_EN		(0x1 << 29)
+#define BIT_PRP_IF_EN		(0x1 << 28)
+#define BIT_CCIR_MODE		(0x1 << 27)
+#define BIT_COF_INT_EN		(0x1 << 26)
+#define BIT_SF_OR_INTEN		(0x1 << 25)
+#define BIT_RF_OR_INTEN		(0x1 << 24)
+#define BIT_SFF_DMA_DONE_INTEN  (0x1 << 22)
+#define BIT_STATFF_INTEN	(0x1 << 21)
+#define BIT_FB2_DMA_DONE_INTEN  (0x1 << 20)
+#define BIT_FB1_DMA_DONE_INTEN  (0x1 << 19)
+#define BIT_RXFF_INTEN		(0x1 << 18)
+#define BIT_SOF_POL		(0x1 << 17)
+#define BIT_SOF_INTEN		(0x1 << 16)
+#define BIT_MCLKDIV		(0xF << 12)
+#define BIT_HSYNC_POL		(0x1 << 11)
+#define BIT_CCIR_EN		(0x1 << 10)
+#define BIT_MCLKEN		(0x1 << 9)
+#define BIT_FCC			(0x1 << 8)
+#define BIT_PACK_DIR		(0x1 << 7)
+#define BIT_CLR_STATFIFO	(0x1 << 6)
+#define BIT_CLR_RXFIFO		(0x1 << 5)
+#define BIT_GCLK_MODE		(0x1 << 4)
+#define BIT_INV_DATA		(0x1 << 3)
+#define BIT_INV_PCLK		(0x1 << 2)
+#define BIT_REDGE		(0x1 << 1)
+#define BIT_PIXEL_BIT		(0x1 << 0)
+
+#define SHIFT_MCLKDIV		12
+
+/* control reg 3 */
+#define BIT_FRMCNT		(0xFFFF << 16)
+#define BIT_FRMCNT_RST		(0x1 << 15)
+#define BIT_DMA_REFLASH_RFF	(0x1 << 14)
+#define BIT_DMA_REFLASH_SFF	(0x1 << 13)
+#define BIT_DMA_REQ_EN_RFF	(0x1 << 12)
+#define BIT_DMA_REQ_EN_SFF	(0x1 << 11)
+#define BIT_STATFF_LEVEL	(0x7 << 8)
+#define BIT_HRESP_ERR_EN	(0x1 << 7)
+#define BIT_RXFF_LEVEL		(0x7 << 4)
+#define BIT_TWO_8BIT_SENSOR	(0x1 << 3)
+#define BIT_ZERO_PACK_EN	(0x1 << 2)
+#define BIT_ECC_INT_EN		(0x1 << 1)
+#define BIT_ECC_AUTO_EN		(0x1 << 0)
+
+#define SHIFT_FRMCNT		16
+
+/* csi status reg */
+#define BIT_SFF_OR_INT		(0x1 << 25)
+#define BIT_RFF_OR_INT		(0x1 << 24)
+#define BIT_DMA_TSF_DONE_SFF	(0x1 << 22)
+#define BIT_STATFF_INT		(0x1 << 21)
+#define BIT_DMA_TSF_DONE_FB2	(0x1 << 20)
+#define BIT_DMA_TSF_DONE_FB1	(0x1 << 19)
+#define BIT_RXFF_INT		(0x1 << 18)
+#define BIT_EOF_INT		(0x1 << 17)
+#define BIT_SOF_INT		(0x1 << 16)
+#define BIT_F2_INT		(0x1 << 15)
+#define BIT_F1_INT		(0x1 << 14)
+#define BIT_COF_INT		(0x1 << 13)
+#define BIT_HRESP_ERR_INT	(0x1 << 7)
+#define BIT_ECC_INT		(0x1 << 1)
+#define BIT_DRDY		(0x1 << 0)
+
+#define CSI_MCLK_VF		1
+#define CSI_MCLK_ENC		2
+#define CSI_MCLK_RAW		4
+#define CSI_MCLK_I2C		8
+#endif
+
+#define CSI_CSICR1		(IO_ADDRESS(CSI_BASE_ADDR))
+#define CSI_CSICR2		(IO_ADDRESS(CSI_BASE_ADDR + 0x4))
+#define CSI_CSICR3		(IO_ADDRESS(CSI_BASE_ADDR + 0x8))
+#define CSI_STATFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0xC))
+#define CSI_CSIRXFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0x10))
+#define CSI_CSIRXCNT		(IO_ADDRESS(CSI_BASE_ADDR + 0x14))
+#define CSI_CSISR		(IO_ADDRESS(CSI_BASE_ADDR + 0x18))
+
+#define CSI_CSIDBG		(IO_ADDRESS(CSI_BASE_ADDR + 0x1C))
+#define CSI_CSIDMASA_STATFIFO	(IO_ADDRESS(CSI_BASE_ADDR + 0x20))
+#define CSI_CSIDMATS_STATFIFO	(IO_ADDRESS(CSI_BASE_ADDR + 0x24))
+#define CSI_CSIDMASA_FB1	(IO_ADDRESS(CSI_BASE_ADDR + 0x28))
+#define CSI_CSIDMASA_FB2	(IO_ADDRESS(CSI_BASE_ADDR + 0x2C))
+#define CSI_CSIFBUF_PARA	(IO_ADDRESS(CSI_BASE_ADDR + 0x30))
+#define CSI_CSIIMAG_PARA	(IO_ADDRESS(CSI_BASE_ADDR + 0x34))
+
+#define CSI_CSIRXFIFO_PHYADDR	(CSI_BASE_ADDR + 0x10)
+
+static inline void csi_clear_status(unsigned long status)
+{
+	__raw_writel(status, CSI_CSISR);
+}
+
+struct csi_signal_cfg_t {
+	unsigned data_width:3;
+	unsigned clk_mode:2;
+	unsigned ext_vsync:1;
+	unsigned Vsync_pol:1;
+	unsigned Hsync_pol:1;
+	unsigned pixclk_pol:1;
+	unsigned data_pol:1;
+	unsigned sens_clksrc:1;
+};
+
+struct csi_config_t {
+	/* control reg 1 */
+	unsigned int swap16_en:1;
+	unsigned int ext_vsync:1;
+	unsigned int eof_int_en:1;
+	unsigned int prp_if_en:1;
+	unsigned int ccir_mode:1;
+	unsigned int cof_int_en:1;
+	unsigned int sf_or_inten:1;
+	unsigned int rf_or_inten:1;
+	unsigned int sff_dma_done_inten:1;
+	unsigned int statff_inten:1;
+	unsigned int fb2_dma_done_inten:1;
+	unsigned int fb1_dma_done_inten:1;
+	unsigned int rxff_inten:1;
+	unsigned int sof_pol:1;
+	unsigned int sof_inten:1;
+	unsigned int mclkdiv:4;
+	unsigned int hsync_pol:1;
+	unsigned int ccir_en:1;
+	unsigned int mclken:1;
+	unsigned int fcc:1;
+	unsigned int pack_dir:1;
+	unsigned int gclk_mode:1;
+	unsigned int inv_data:1;
+	unsigned int inv_pclk:1;
+	unsigned int redge:1;
+	unsigned int pixel_bit:1;
+
+	/* control reg 3 */
+	unsigned int frmcnt:16;
+	unsigned int frame_reset:1;
+	unsigned int dma_reflash_rff:1;
+	unsigned int dma_reflash_sff:1;
+	unsigned int dma_req_en_rff:1;
+	unsigned int dma_req_en_sff:1;
+	unsigned int statff_level:3;
+	unsigned int hresp_err_en:1;
+	unsigned int rxff_level:3;
+	unsigned int two_8bit_sensor:1;
+	unsigned int zero_pack_en:1;
+	unsigned int ecc_int_en:1;
+	unsigned int ecc_auto_en:1;
+	/* fifo counter */
+	unsigned int rxcnt;
+};
+
+typedef void (*csi_irq_callback_t) (void *data, unsigned long status);
+
+int32_t csi_enable_mclk(int src, bool flag, bool wait);
+void csi_init_interface(void);
+void csi_set_16bit_imagpara(int width, int height);
+void csi_set_12bit_imagpara(int width, int height);
+int csi_read_mclk_flag(void);
+void csi_start_callback(void *data);
+void csi_stop_callback(void *data);
+void csi_enable_int(int arg);
+void csi_disable_int(void);
+void csi_mclk_enable(void);
+void csi_mclk_disable(void);
diff --git a/drivers/media/video/mxc/capture/ipu_csi_enc.c b/drivers/media/video/mxc/capture/ipu_csi_enc.c
new file mode 100644
index 000000000000..fb3d0d7d5ec6
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_csi_enc.c
@@ -0,0 +1,277 @@
+/*
+ * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file csi_enc.c
+ *
+ * @brief CSI Use case for video capture
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/ipu.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+#ifdef CAMERA_DBG
+	#define CAMERA_TRACE(x) (printk)x
+#else
+	#define CAMERA_TRACE(x)
+#endif
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * csi ENC callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t csi_enc_callback(int irq, void *dev_id)
+{
+	cam_data *cam = (cam_data *) dev_id;
+
+	if (cam->enc_callback == NULL)
+		return IRQ_HANDLED;
+
+	cam->enc_callback(irq, dev_id);
+	return IRQ_HANDLED;
+}
+
+/*!
+ * CSI ENC enable channel setup function
+ *
+ * @param cam       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int csi_enc_setup(cam_data *cam)
+{
+	ipu_channel_params_t params;
+	u32 pixel_fmt;
+	int err = 0;
+	dma_addr_t dummy = 0xdeadbeaf;
+
+	CAMERA_TRACE("In csi_enc_setup\n");
+	if (!cam) {
+		printk(KERN_ERR "cam private is NULL\n");
+		return -ENXIO;
+	}
+
+	memset(&params, 0, sizeof(ipu_channel_params_t));
+	params.csi_mem.csi = cam->csi;
+
+	if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
+		pixel_fmt = IPU_PIX_FMT_YUV420P;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P)
+		pixel_fmt = IPU_PIX_FMT_YUV422P;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY)
+		pixel_fmt = IPU_PIX_FMT_UYVY;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_NV12)
+		pixel_fmt = IPU_PIX_FMT_NV12;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR24)
+		pixel_fmt = IPU_PIX_FMT_BGR24;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24)
+		pixel_fmt = IPU_PIX_FMT_RGB24;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB565)
+		pixel_fmt = IPU_PIX_FMT_RGB565;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR32)
+		pixel_fmt = IPU_PIX_FMT_BGR32;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB32)
+		pixel_fmt = IPU_PIX_FMT_RGB32;
+	else {
+		printk(KERN_ERR "format not supported\n");
+		return -EINVAL;
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, true, true);
+
+	err = ipu_init_channel(CSI_MEM, &params);
+	if (err != 0) {
+		printk(KERN_ERR "ipu_init_channel %d\n", err);
+		return err;
+	}
+
+	err = ipu_init_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+				      pixel_fmt, cam->v2f.fmt.pix.width,
+				      cam->v2f.fmt.pix.height,
+				      cam->v2f.fmt.pix.width, IPU_ROTATE_NONE,
+				      dummy, dummy,
+				      cam->offset.u_offset,
+				      cam->offset.v_offset);
+	if (err != 0) {
+		printk(KERN_ERR "CSI_MEM output buffer\n");
+		return err;
+	}
+	err = ipu_enable_channel(CSI_MEM);
+	if (err < 0) {
+		printk(KERN_ERR "ipu_enable_channel CSI_MEM\n");
+		return err;
+	}
+
+	return err;
+}
+
+/*!
+ * function to update physical buffer address for encorder IDMA channel
+ *
+ * @param eba         physical buffer address for encorder IDMA channel
+ * @param buffer_num  int buffer 0 or buffer 1
+ *
+ * @return  status
+ */
+static int csi_enc_eba_update(dma_addr_t eba, int *buffer_num)
+{
+	int err = 0;
+
+	pr_debug("eba %x\n", eba);
+	err = ipu_update_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+					*buffer_num, eba);
+	if (err != 0) {
+		printk(KERN_ERR "err %d buffer_num %d\n", err, *buffer_num);
+		return err;
+	}
+
+	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, *buffer_num);
+
+	*buffer_num = (*buffer_num == 0) ? 1 : 0;
+
+	return 0;
+}
+
+/*!
+ * Enable encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int csi_enc_enabling_tasks(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+	CAMERA_TRACE("IPU:In csi_enc_enabling_tasks\n");
+
+	err = ipu_request_irq(IPU_IRQ_CSI0_OUT_EOF,
+			      csi_enc_callback, 0, "Mxc Camera", cam);
+	if (err != 0) {
+		printk(KERN_ERR "Error registering rot irq\n");
+		return err;
+	}
+
+	err = csi_enc_setup(cam);
+	if (err != 0) {
+		printk(KERN_ERR "csi_enc_setup %d\n", err);
+		return err;
+	}
+
+	return err;
+}
+
+/*!
+ * Disable encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+static int csi_enc_disabling_tasks(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	ipu_free_irq(IPU_IRQ_CSI0_OUT_EOF, cam);
+
+	err = ipu_disable_channel(CSI_MEM, true);
+
+	ipu_uninit_channel(CSI_MEM);
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, false, false);
+
+	return err;
+}
+
+/*!
+ * function to select CSI ENC as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+int csi_enc_select(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam) {
+		cam->enc_update_eba = csi_enc_eba_update;
+		cam->enc_enable = csi_enc_enabling_tasks;
+		cam->enc_disable = csi_enc_disabling_tasks;
+	} else {
+		err = -EIO;
+	}
+
+	return err;
+}
+
+/*!
+ * function to de-select CSI ENC as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+int csi_enc_deselect(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam) {
+		cam->enc_update_eba = NULL;
+		cam->enc_enable = NULL;
+		cam->enc_disable = NULL;
+	}
+
+	return err;
+}
+
+/*!
+ * Init the Encorder channels
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int csi_enc_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit the Encorder channels
+ *
+ */
+void __exit csi_enc_exit(void)
+{
+}
+
+module_init(csi_enc_init);
+module_exit(csi_enc_exit);
+
+EXPORT_SYMBOL(csi_enc_select);
+EXPORT_SYMBOL(csi_enc_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("CSI ENC Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_prp_enc.c b/drivers/media/video/mxc/capture/ipu_prp_enc.c
new file mode 100644
index 000000000000..79eb8a615ee8
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_enc.c
@@ -0,0 +1,455 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_enc.c
+ *
+ * @brief IPU Use case for PRP-ENC
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/ipu.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+#ifdef CAMERA_DBG
+	#define CAMERA_TRACE(x) (printk)x
+#else
+	#define CAMERA_TRACE(x)
+#endif
+
+static ipu_rotate_mode_t grotation = IPU_ROTATE_NONE;
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * IPU ENC callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prp_enc_callback(int irq, void *dev_id)
+{
+	cam_data *cam = (cam_data *) dev_id;
+
+	if (cam->enc_callback == NULL)
+		return IRQ_HANDLED;
+
+	cam->enc_callback(irq, dev_id);
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * PrpENC enable channel setup function
+ *
+ * @param cam       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int prp_enc_setup(cam_data * cam)
+{
+	ipu_channel_params_t enc;
+	int err = 0;
+	dma_addr_t dummy = 0xdeadbeaf;
+
+	CAMERA_TRACE("In prp_enc_setup\n");
+	if (!cam) {
+		printk(KERN_ERR "cam private is NULL\n");
+		return -ENXIO;
+	}
+	memset(&enc, 0, sizeof(ipu_channel_params_t));
+
+	ipu_csi_get_window_size(&enc.csi_prp_enc_mem.in_width,
+				&enc.csi_prp_enc_mem.in_height, cam->csi);
+
+	enc.csi_prp_enc_mem.in_pixel_fmt = IPU_PIX_FMT_UYVY;
+	enc.csi_prp_enc_mem.out_width = cam->v2f.fmt.pix.width;
+	enc.csi_prp_enc_mem.out_height = cam->v2f.fmt.pix.height;
+	enc.csi_prp_enc_mem.csi = cam->csi;
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		enc.csi_prp_enc_mem.out_width = cam->v2f.fmt.pix.height;
+		enc.csi_prp_enc_mem.out_height = cam->v2f.fmt.pix.width;
+	}
+
+	if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_YUV420P;
+		pr_info("YUV420\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_YUV422P;
+		pr_info("YUV422P\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_NV12) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_NV12;
+		pr_info("NV12\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR24) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_BGR24;
+		pr_info("BGR24\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_RGB24;
+		pr_info("RGB24\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB565) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_RGB565;
+		pr_info("RGB565\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR32) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_BGR32;
+		pr_info("BGR32\n");
+	} else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB32) {
+		enc.csi_prp_enc_mem.out_pixel_fmt = IPU_PIX_FMT_RGB32;
+		pr_info("RGB32\n");
+	} else {
+		printk(KERN_ERR "format not supported\n");
+		return -EINVAL;
+	}
+
+	err = ipu_init_channel(CSI_PRP_ENC_MEM, &enc);
+	if (err != 0) {
+		printk(KERN_ERR "ipu_init_channel %d\n", err);
+		return err;
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, true, true);
+
+	grotation = cam->rotation;
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		if (cam->rot_enc_bufs_vaddr[0]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[0],
+					  cam->rot_enc_bufs_vaddr[0],
+					  cam->rot_enc_bufs[0]);
+		}
+		if (cam->rot_enc_bufs_vaddr[1]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[1],
+					  cam->rot_enc_bufs_vaddr[1],
+					  cam->rot_enc_bufs[1]);
+		}
+		cam->rot_enc_buf_size[0] =
+		    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
+		cam->rot_enc_bufs_vaddr[0] =
+		    (void *)dma_alloc_coherent(0, cam->rot_enc_buf_size[0],
+					       &cam->rot_enc_bufs[0],
+					       GFP_DMA | GFP_KERNEL);
+		if (!cam->rot_enc_bufs_vaddr[0]) {
+			printk(KERN_ERR "alloc enc_bufs0\n");
+			return -ENOMEM;
+		}
+		cam->rot_enc_buf_size[1] =
+		    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
+		cam->rot_enc_bufs_vaddr[1] =
+		    (void *)dma_alloc_coherent(0, cam->rot_enc_buf_size[1],
+					       &cam->rot_enc_bufs[1],
+					       GFP_DMA | GFP_KERNEL);
+		if (!cam->rot_enc_bufs_vaddr[1]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[0],
+					  cam->rot_enc_bufs_vaddr[0],
+					  cam->rot_enc_bufs[0]);
+			cam->rot_enc_bufs_vaddr[0] = NULL;
+			cam->rot_enc_bufs[0] = 0;
+			printk(KERN_ERR "alloc enc_bufs1\n");
+			return -ENOMEM;
+		}
+
+		err = ipu_init_channel_buffer(CSI_PRP_ENC_MEM,
+					      IPU_OUTPUT_BUFFER,
+					      enc.csi_prp_enc_mem.out_pixel_fmt,
+					      enc.csi_prp_enc_mem.out_width,
+					      enc.csi_prp_enc_mem.out_height,
+					      enc.csi_prp_enc_mem.out_width,
+					      IPU_ROTATE_NONE,
+					      cam->rot_enc_bufs[0],
+					      cam->rot_enc_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "CSI_PRP_ENC_MEM err\n");
+			return err;
+		}
+
+		err = ipu_init_channel(MEM_ROT_ENC_MEM, NULL);
+		if (err != 0) {
+			printk(KERN_ERR "MEM_ROT_ENC_MEM channel err\n");
+			return err;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_ENC_MEM, IPU_INPUT_BUFFER,
+					      enc.csi_prp_enc_mem.out_pixel_fmt,
+					      enc.csi_prp_enc_mem.out_width,
+					      enc.csi_prp_enc_mem.out_height,
+					      enc.csi_prp_enc_mem.out_width,
+					      cam->rotation,
+					      cam->rot_enc_bufs[0],
+					      cam->rot_enc_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "MEM_ROT_ENC_MEM input buffer\n");
+			return err;
+		}
+
+		err =
+		    ipu_init_channel_buffer(MEM_ROT_ENC_MEM, IPU_OUTPUT_BUFFER,
+					    enc.csi_prp_enc_mem.out_pixel_fmt,
+					    enc.csi_prp_enc_mem.out_height,
+					    enc.csi_prp_enc_mem.out_width,
+					    cam->v2f.fmt.pix.bytesperline /
+					    bytes_per_pixel(enc.csi_prp_enc_mem.
+							    out_pixel_fmt),
+					    IPU_ROTATE_NONE, dummy, dummy,
+					    cam->offset.u_offset,
+					    cam->offset.v_offset);
+		if (err != 0) {
+			printk(KERN_ERR "MEM_ROT_ENC_MEM output buffer\n");
+			return err;
+		}
+
+		err = ipu_link_channels(CSI_PRP_ENC_MEM, MEM_ROT_ENC_MEM);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "link CSI_PRP_ENC_MEM-MEM_ROT_ENC_MEM\n");
+			return err;
+		}
+
+		err = ipu_enable_channel(CSI_PRP_ENC_MEM);
+		if (err < 0) {
+			printk(KERN_ERR "ipu_enable_channel CSI_PRP_ENC_MEM\n");
+			return err;
+		}
+		err = ipu_enable_channel(MEM_ROT_ENC_MEM);
+		if (err < 0) {
+			printk(KERN_ERR "ipu_enable_channel MEM_ROT_ENC_MEM\n");
+			return err;
+		}
+
+		ipu_select_buffer(CSI_PRP_ENC_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_ENC_MEM, IPU_OUTPUT_BUFFER, 1);
+	} else {
+		err =
+		    ipu_init_channel_buffer(CSI_PRP_ENC_MEM, IPU_OUTPUT_BUFFER,
+					    enc.csi_prp_enc_mem.out_pixel_fmt,
+					    enc.csi_prp_enc_mem.out_width,
+					    enc.csi_prp_enc_mem.out_height,
+					    cam->v2f.fmt.pix.bytesperline /
+					    bytes_per_pixel(enc.csi_prp_enc_mem.
+							    out_pixel_fmt),
+					    cam->rotation, dummy, dummy,
+					    cam->offset.u_offset,
+					    cam->offset.v_offset);
+		if (err != 0) {
+			printk(KERN_ERR "CSI_PRP_ENC_MEM output buffer\n");
+			return err;
+		}
+		err = ipu_enable_channel(CSI_PRP_ENC_MEM);
+		if (err < 0) {
+			printk(KERN_ERR "ipu_enable_channel CSI_PRP_ENC_MEM\n");
+			return err;
+		}
+	}
+
+	return err;
+}
+
+/*!
+ * function to update physical buffer address for encorder IDMA channel
+ *
+ * @param eba         physical buffer address for encorder IDMA channel
+ * @param buffer_num  int buffer 0 or buffer 1
+ *
+ * @return  status
+ */
+static int prp_enc_eba_update(dma_addr_t eba, int *buffer_num)
+{
+	int err = 0;
+
+	pr_debug("eba %x\n", eba);
+	if (grotation >= IPU_ROTATE_90_RIGHT) {
+		err = ipu_update_channel_buffer(MEM_ROT_ENC_MEM,
+						IPU_OUTPUT_BUFFER, *buffer_num,
+						eba);
+	} else {
+		err = ipu_update_channel_buffer(CSI_PRP_ENC_MEM,
+						IPU_OUTPUT_BUFFER, *buffer_num,
+						eba);
+	}
+	if (err != 0) {
+		printk(KERN_ERR "err %d buffer_num %d\n", err, *buffer_num);
+		return err;
+	}
+
+	if (grotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_select_buffer(MEM_ROT_ENC_MEM, IPU_OUTPUT_BUFFER,
+				  *buffer_num);
+	} else {
+		ipu_select_buffer(CSI_PRP_ENC_MEM, IPU_OUTPUT_BUFFER,
+				  *buffer_num);
+	}
+
+	*buffer_num = (*buffer_num == 0) ? 1 : 0;
+	return 0;
+}
+
+/*!
+ * Enable encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int prp_enc_enabling_tasks(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+	CAMERA_TRACE("IPU:In prp_enc_enabling_tasks\n");
+
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		err = ipu_request_irq(IPU_IRQ_PRP_ENC_ROT_OUT_EOF,
+				      prp_enc_callback, 0, "Mxc Camera", cam);
+	} else {
+		err = ipu_request_irq(IPU_IRQ_PRP_ENC_OUT_EOF,
+				      prp_enc_callback, 0, "Mxc Camera", cam);
+	}
+	if (err != 0) {
+		printk(KERN_ERR "Error registering rot irq\n");
+		return err;
+	}
+
+	err = prp_enc_setup(cam);
+	if (err != 0) {
+		printk(KERN_ERR "prp_enc_setup %d\n", err);
+		return err;
+	}
+
+	return err;
+}
+
+/*!
+ * Disable encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+static int prp_enc_disabling_tasks(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_free_irq(IPU_IRQ_PRP_ENC_ROT_OUT_EOF, cam);
+	} else {
+		ipu_free_irq(IPU_IRQ_PRP_ENC_OUT_EOF, cam);
+	}
+
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_unlink_channels(CSI_PRP_ENC_MEM, MEM_ROT_ENC_MEM);
+	}
+
+	err = ipu_disable_channel(CSI_PRP_ENC_MEM, true);
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		err |= ipu_disable_channel(MEM_ROT_ENC_MEM, true);
+	}
+
+	ipu_uninit_channel(CSI_PRP_ENC_MEM);
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_uninit_channel(MEM_ROT_ENC_MEM);
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, false, false);
+
+	return err;
+}
+
+/*!
+ * function to select PRP-ENC as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+int prp_enc_select(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam) {
+		cam->enc_update_eba = prp_enc_eba_update;
+		cam->enc_enable = prp_enc_enabling_tasks;
+		cam->enc_disable = prp_enc_disabling_tasks;
+	} else {
+		err = -EIO;
+	}
+
+	return err;
+}
+
+/*!
+ * function to de-select PRP-ENC as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  int
+ */
+int prp_enc_deselect(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	//err = prp_enc_disabling_tasks(cam);
+
+	if (cam) {
+		cam->enc_update_eba = NULL;
+		cam->enc_enable = NULL;
+		cam->enc_disable = NULL;
+		if (cam->rot_enc_bufs_vaddr[0]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[0],
+					  cam->rot_enc_bufs_vaddr[0],
+					  cam->rot_enc_bufs[0]);
+			cam->rot_enc_bufs_vaddr[0] = NULL;
+			cam->rot_enc_bufs[0] = 0;
+		}
+		if (cam->rot_enc_bufs_vaddr[1]) {
+			dma_free_coherent(0, cam->rot_enc_buf_size[1],
+					  cam->rot_enc_bufs_vaddr[1],
+					  cam->rot_enc_bufs[1]);
+			cam->rot_enc_bufs_vaddr[1] = NULL;
+			cam->rot_enc_bufs[1] = 0;
+		}
+	}
+
+	return err;
+}
+
+/*!
+ * Init the Encorder channels
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_enc_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit the Encorder channels
+ *
+ */
+void __exit prp_enc_exit(void)
+{
+}
+
+module_init(prp_enc_init);
+module_exit(prp_enc_exit);
+
+EXPORT_SYMBOL(prp_enc_select);
+EXPORT_SYMBOL(prp_enc_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP ENC Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_prp_sw.h b/drivers/media/video/mxc/capture/ipu_prp_sw.h
new file mode 100644
index 000000000000..0d51e2ae8670
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_sw.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_sw.h
+ *
+ * @brief This file contains the IPU PRP use case driver header.
+ *
+ * @ingroup IPU
+ */
+
+#ifndef _INCLUDE_IPU__PRP_SW_H_
+#define _INCLUDE_IPU__PRP_SW_H_
+
+int csi_enc_select(void *private);
+int csi_enc_deselect(void *private);
+int prp_enc_select(void *private);
+int prp_enc_deselect(void *private);
+int prp_vf_adc_select(void *private);
+int prp_vf_sdc_select(void *private);
+int prp_vf_sdc_select_bg(void *private);
+int prp_vf_adc_deselect(void *private);
+int prp_vf_sdc_deselect(void *private);
+int prp_vf_sdc_deselect_bg(void *private);
+int prp_still_select(void *private);
+int prp_still_deselect(void *private);
+
+#endif
diff --git a/drivers/media/video/mxc/capture/ipu_prp_vf_adc.c b/drivers/media/video/mxc/capture/ipu_prp_vf_adc.c
new file mode 100644
index 000000000000..a7ac09ae87d4
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_vf_adc.c
@@ -0,0 +1,601 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_vf_adc.c
+ *
+ * @brief IPU Use case for PRP-VF
+ *
+ * @ingroup IPU
+ */
+
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+#include <mach/mxcfb.h>
+#include <mach/ipu.h>
+#include <linux/dma-mapping.h>
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * prpvf_start - start the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	ipu_channel_params_t vf;
+	ipu_channel_params_t params;
+	u32 format = IPU_PIX_FMT_RGB565;
+	u32 size = 2;
+	int err = 0;
+
+	if (!cam) {
+		printk(KERN_ERR "prpvf_start private is NULL\n");
+		return -ENXIO;
+	}
+
+	if (cam->overlay_active == true) {
+		printk(KERN_ERR "prpvf_start already start.\n");
+		return 0;
+	}
+
+	mxcfb_set_refresh_mode(cam->overlay_fb, MXCFB_REFRESH_OFF, 0);
+
+	memset(&vf, 0, sizeof(ipu_channel_params_t));
+	ipu_csi_get_window_size(&vf.csi_prp_vf_adc.in_width,
+				&vf.csi_prp_vf_adc.in_height);
+	vf.csi_prp_vf_adc.in_pixel_fmt = IPU_PIX_FMT_UYVY;
+	vf.csi_prp_vf_adc.out_width = cam->win.w.width;
+	vf.csi_prp_vf_adc.out_height = cam->win.w.height;
+	vf.csi_prp_vf_adc.graphics_combine_en = 0;
+	vf.csi_prp_vf_adc.out_left = cam->win.w.left;
+
+	/* hope to be removed when those offset taken cared by adc driver. */
+#ifdef CONFIG_FB_MXC_EPSON_QVGA_PANEL
+	vf.csi_prp_vf_adc.out_left += 12;
+#endif
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+	vf.csi_prp_vf_adc.out_left += 2;
+#endif
+
+	vf.csi_prp_vf_adc.out_top = cam->win.w.top;
+
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		vf.csi_prp_vf_adc.out_width = cam->win.w.height;
+		vf.csi_prp_vf_adc.out_height = cam->win.w.width;
+
+		size = cam->win.w.width * cam->win.w.height * size;
+		vf.csi_prp_vf_adc.out_pixel_fmt = format;
+		err = ipu_init_channel(CSI_PRP_VF_MEM, &vf);
+		if (err != 0)
+			return err;
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+
+		if (cam->vf_bufs_vaddr[0]) {
+			dma_free_coherent(0, cam->vf_bufs_size[0],
+					  cam->vf_bufs_vaddr[0],
+					  cam->vf_bufs[0]);
+		}
+		if (cam->vf_bufs_vaddr[1]) {
+			dma_free_coherent(0, cam->vf_bufs_size[1],
+					  cam->vf_bufs_vaddr[1],
+					  cam->vf_bufs[1]);
+		}
+		cam->vf_bufs_size[0] = size;
+		cam->vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+								   cam->
+								   vf_bufs_size
+								   [0],
+								   &cam->
+								   vf_bufs[0],
+								   GFP_DMA |
+								   GFP_KERNEL);
+		if (cam->vf_bufs_vaddr[0] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate vf buffer\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		cam->vf_bufs_size[1] = size;
+		cam->vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+								   cam->
+								   vf_bufs_size
+								   [1],
+								   &cam->
+								   vf_bufs[1],
+								   GFP_DMA |
+								   GFP_KERNEL);
+		if (cam->vf_bufs_vaddr[1] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate vf buffer\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+
+		err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      IPU_ROTATE_NONE,
+					      cam->vf_bufs[0], cam->vf_bufs[1],
+					      0, 0);
+		if (err != 0)
+			goto out_3;
+
+		if (cam->rot_vf_bufs[0]) {
+			dma_free_coherent(0, cam->rot_vf_buf_size[0],
+					  cam->rot_vf_bufs_vaddr[0],
+					  cam->rot_vf_bufs[0]);
+		}
+		if (cam->rot_vf_bufs[1]) {
+			dma_free_coherent(0, cam->rot_vf_buf_size[1],
+					  cam->rot_vf_bufs_vaddr[1],
+					  cam->rot_vf_bufs[1]);
+		}
+		cam->rot_vf_buf_size[0] = PAGE_ALIGN(size);
+		cam->rot_vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+								       cam->
+								       rot_vf_buf_size
+								       [0],
+								       &cam->
+								       rot_vf_bufs
+								       [0],
+								       GFP_DMA |
+								       GFP_KERNEL);
+		if (cam->rot_vf_bufs_vaddr[0] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate rot_vf_bufs\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		cam->rot_vf_buf_size[1] = PAGE_ALIGN(size);
+		cam->rot_vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+								       cam->
+								       rot_vf_buf_size
+								       [1],
+								       &cam->
+								       rot_vf_bufs
+								       [1],
+								       GFP_DMA |
+								       GFP_KERNEL);
+		if (cam->rot_vf_bufs_vaddr[1] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate rot_vf_bufs\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		err = ipu_init_channel(MEM_ROT_VF_MEM, NULL);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start :Error "
+			       "MEM_ROT_VF_MEM channel\n");
+			goto out_3;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_INPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      cam->vf_rotation, cam->vf_bufs[0],
+					      cam->vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "MEM_ROT_VF_MEM input buffer\n");
+			goto out_2;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      IPU_ROTATE_NONE,
+					      cam->rot_vf_bufs[0],
+					      cam->rot_vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "MEM_ROT_VF_MEM output buffer\n");
+			goto out_2;
+		}
+
+		err = ipu_link_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+		if (err < 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "linking CSI_PRP_VF_MEM-MEM_ROT_VF_MEM\n");
+			goto out_2;
+		}
+
+		ipu_disable_channel(ADC_SYS2, false);
+		ipu_uninit_channel(ADC_SYS2);
+
+		params.adc_sys2.disp = DISP0;
+		params.adc_sys2.ch_mode = WriteTemplateNonSeq;
+		params.adc_sys2.out_left = cam->win.w.left;
+		/* going to be removed when those offset taken cared by adc driver. */
+#ifdef CONFIG_FB_MXC_EPSON_QVGA_PANEL
+		params.adc_sys2.out_left += 12;
+#endif
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+		params.adc_sys2.out_left += 2;
+#endif
+		params.adc_sys2.out_top = cam->win.w.top;
+		err = ipu_init_channel(ADC_SYS2, &params);
+		if (err != 0) {
+			printk(KERN_ERR
+			       "prpvf_start: Error initializing ADC SYS1\n");
+			goto out_2;
+		}
+
+		err = ipu_init_channel_buffer(ADC_SYS2, IPU_INPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      IPU_ROTATE_NONE,
+					      cam->rot_vf_bufs[0],
+					      cam->rot_vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "Error initializing ADC SYS1 buffer\n");
+			goto out_1;
+		}
+
+		err = ipu_link_channels(MEM_ROT_VF_MEM, ADC_SYS2);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "Error linking MEM_ROT_VF_MEM-ADC_SYS2\n");
+			goto out_1;
+		}
+
+		ipu_enable_channel(CSI_PRP_VF_MEM);
+		ipu_enable_channel(MEM_ROT_VF_MEM);
+		ipu_enable_channel(ADC_SYS2);
+
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+	}
+#ifndef CONFIG_MXC_IPU_PRP_VF_SDC
+	else if (cam->vf_rotation == IPU_ROTATE_NONE) {
+		vf.csi_prp_vf_adc.out_pixel_fmt = IPU_PIX_FMT_BGR32;
+		err = ipu_init_channel(CSI_PRP_VF_ADC, &vf);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing CSI_PRP_VF_ADC\n");
+			return err;
+		}
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+		err = ipu_init_channel_buffer(CSI_PRP_VF_ADC, IPU_OUTPUT_BUFFER,
+					      format, cam->win.w.width,
+					      cam->win.w.height,
+					      cam->win.w.width, IPU_ROTATE_NONE,
+					      0, 0, 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing CSI_PRP_VF_MEM\n");
+			return err;
+		}
+		ipu_enable_channel(CSI_PRP_VF_ADC);
+	}
+#endif
+	else {
+		size = cam->win.w.width * cam->win.w.height * size;
+		vf.csi_prp_vf_adc.out_pixel_fmt = format;
+		err = ipu_init_channel(CSI_PRP_VF_MEM, &vf);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing CSI_PRP_VF_MEM\n");
+			return err;
+		}
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+
+		if (cam->vf_bufs[0]) {
+			dma_free_coherent(0, cam->vf_bufs_size[0],
+					  cam->vf_bufs_vaddr[0],
+					  cam->vf_bufs[0]);
+		}
+		if (cam->vf_bufs[1]) {
+			dma_free_coherent(0, cam->vf_bufs_size[1],
+					  cam->vf_bufs_vaddr[1],
+					  cam->vf_bufs[1]);
+		}
+		cam->vf_bufs_size[0] = PAGE_ALIGN(size);
+		cam->vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+								   cam->
+								   vf_bufs_size
+								   [0],
+								   &cam->
+								   vf_bufs[0],
+								   GFP_DMA |
+								   GFP_KERNEL);
+		if (cam->vf_bufs_vaddr[0] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate vf_bufs\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		cam->vf_bufs_size[1] = PAGE_ALIGN(size);
+		cam->vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+								   cam->
+								   vf_bufs_size
+								   [1],
+								   &cam->
+								   vf_bufs[1],
+								   GFP_DMA |
+								   GFP_KERNEL);
+		if (cam->vf_bufs_vaddr[1] == NULL) {
+			printk(KERN_ERR
+			       "prpvf_start: Error to allocate vf_bufs\n");
+			err = -ENOMEM;
+			goto out_3;
+		}
+		err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      cam->vf_rotation,
+					      cam->vf_bufs[0], cam->vf_bufs[1],
+					      0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing CSI_PRP_VF_MEM\n");
+			goto out_3;
+		}
+
+		ipu_disable_channel(ADC_SYS2, false);
+		ipu_uninit_channel(ADC_SYS2);
+
+		params.adc_sys2.disp = DISP0;
+		params.adc_sys2.ch_mode = WriteTemplateNonSeq;
+		params.adc_sys2.out_left = cam->win.w.left;
+		// going to be removed when those offset taken cared by adc driver.
+#ifdef CONFIG_FB_MXC_EPSON_QVGA_PANEL
+		params.adc_sys2.out_left += 12;
+#endif
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+		params.adc_sys2.out_left += 2;
+#endif
+		params.adc_sys2.out_top = cam->win.w.top;
+		err = ipu_init_channel(ADC_SYS2, &params);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing ADC_SYS2\n");
+			goto out_3;
+		}
+
+		err = ipu_init_channel_buffer(ADC_SYS2, IPU_INPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      IPU_ROTATE_NONE, cam->vf_bufs[0],
+					      cam->vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "initializing ADC SYS1 buffer\n");
+			goto out_1;
+		}
+
+		err = ipu_link_channels(CSI_PRP_VF_MEM, ADC_SYS2);
+		if (err < 0) {
+			printk(KERN_ERR "prpvf_start: Error "
+			       "linking MEM_ROT_VF_MEM-ADC_SYS2\n");
+			goto out_1;
+		}
+
+		ipu_enable_channel(CSI_PRP_VF_MEM);
+		ipu_enable_channel(ADC_SYS2);
+
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+	}
+
+	cam->overlay_active = true;
+	return err;
+
+      out_1:
+	ipu_uninit_channel(ADC_SYS2);
+      out_2:
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_uninit_channel(MEM_ROT_VF_MEM);
+	}
+      out_3:
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+	if (cam->rot_vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[0],
+				  cam->rot_vf_bufs_vaddr[0],
+				  cam->rot_vf_bufs[0]);
+		cam->rot_vf_bufs_vaddr[0] = NULL;
+		cam->rot_vf_bufs[0] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[1],
+				  cam->rot_vf_bufs_vaddr[1],
+				  cam->rot_vf_bufs[1]);
+		cam->rot_vf_bufs_vaddr[1] = NULL;
+		cam->rot_vf_bufs[1] = 0;
+	}
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+	return err;
+}
+
+/*!
+ * prpvf_stop - stop the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	if (cam->overlay_active == false)
+		return 0;
+
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		ipu_unlink_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+		ipu_unlink_channels(MEM_ROT_VF_MEM, ADC_SYS2);
+
+		ipu_disable_channel(CSI_PRP_VF_MEM, true);
+		ipu_disable_channel(MEM_ROT_VF_MEM, true);
+		ipu_disable_channel(ADC_SYS2, true);
+
+		ipu_uninit_channel(CSI_PRP_VF_MEM);
+		ipu_uninit_channel(MEM_ROT_VF_MEM);
+		ipu_uninit_channel(ADC_SYS2);
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+	}
+#ifndef CONFIG_MXC_IPU_PRP_VF_SDC
+	else if (cam->vf_rotation == IPU_ROTATE_NONE) {
+		ipu_disable_channel(CSI_PRP_VF_ADC, false);
+		ipu_uninit_channel(CSI_PRP_VF_ADC);
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+	}
+#endif
+	else {
+		ipu_unlink_channels(CSI_PRP_VF_MEM, ADC_SYS2);
+
+		ipu_disable_channel(CSI_PRP_VF_MEM, true);
+		ipu_disable_channel(ADC_SYS2, true);
+
+		ipu_uninit_channel(CSI_PRP_VF_MEM);
+		ipu_uninit_channel(ADC_SYS2);
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+	}
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[0],
+				  cam->rot_vf_bufs_vaddr[0],
+				  cam->rot_vf_bufs[0]);
+		cam->rot_vf_bufs_vaddr[0] = NULL;
+		cam->rot_vf_bufs[0] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[1],
+				  cam->rot_vf_bufs_vaddr[1],
+				  cam->rot_vf_bufs[1]);
+		cam->rot_vf_bufs_vaddr[1] = NULL;
+		cam->rot_vf_bufs[1] = 0;
+	}
+
+	cam->overlay_active = false;
+
+	mxcfb_set_refresh_mode(cam->overlay_fb, MXCFB_REFRESH_PARTIAL, 0);
+	return err;
+}
+
+/*!
+ * function to select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_adc_select(void *private)
+{
+	cam_data *cam;
+	if (private) {
+		cam = (cam_data *) private;
+		cam->vf_start_adc = prpvf_start;
+		cam->vf_stop_adc = prpvf_stop;
+		cam->overlay_active = false;
+	} else {
+		return -EIO;
+	}
+	return 0;
+}
+
+/*!
+ * function to de-select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_adc_deselect(void *private)
+{
+	cam_data *cam;
+	int err = 0;
+	err = prpvf_stop(private);
+
+	if (private) {
+		cam = (cam_data *) private;
+		cam->vf_start_adc = NULL;
+		cam->vf_stop_adc = NULL;
+	}
+	return err;
+}
+
+/*!
+ * Init viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_vf_adc_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+void __exit prp_vf_adc_exit(void)
+{
+}
+
+module_init(prp_vf_adc_init);
+module_exit(prp_vf_adc_exit);
+
+EXPORT_SYMBOL(prp_vf_adc_select);
+EXPORT_SYMBOL(prp_vf_adc_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP VF ADC Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_prp_vf_sdc.c b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc.c
new file mode 100644
index 000000000000..369facdf29c2
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc.c
@@ -0,0 +1,411 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_vf_sdc.c
+ *
+ * @brief IPU Use case for PRP-VF
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/console.h>
+#include <linux/ipu.h>
+#include <linux/mxcfb.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * prpvf_start - start the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_start(void *private)
+{
+	struct fb_var_screeninfo fbvar;
+	struct fb_info *fbi = NULL;
+	cam_data *cam = (cam_data *) private;
+	ipu_channel_params_t vf;
+	u32 format = IPU_PIX_FMT_RGB565;
+	u32 size = 2, temp = 0;
+	int err = 0, i = 0;
+
+	if (!cam) {
+		printk(KERN_ERR "private is NULL\n");
+		return -EIO;
+	}
+
+	if (cam->overlay_active == true) {
+		pr_debug("already started.\n");
+		return 0;
+	}
+
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strcmp(idstr, "DISP3 FG") == 0)
+			fbi = registered_fb[i];
+	}
+
+	if (fbi == NULL) {
+		printk(KERN_ERR "DISP3 FG fb not found\n");
+		return -EPERM;
+	}
+
+	fbvar = fbi->var;
+	fbvar.bits_per_pixel = 16;
+	fbvar.nonstd = 0;
+	fbvar.xres = fbvar.xres_virtual = cam->win.w.width;
+	fbvar.yres = cam->win.w.height;
+	fbvar.yres_virtual = cam->win.w.height * 2;
+	fbvar.activate |= FB_ACTIVATE_FORCE;
+	fb_set_var(fbi, &fbvar);
+
+	ipu_disp_set_window_pos(MEM_FG_SYNC, cam->win.w.left,
+			cam->win.w.top);
+
+	memset(&vf, 0, sizeof(ipu_channel_params_t));
+	ipu_csi_get_window_size(&vf.csi_prp_vf_mem.in_width,
+				&vf.csi_prp_vf_mem.in_height, cam->csi);
+	vf.csi_prp_vf_mem.in_pixel_fmt = IPU_PIX_FMT_UYVY;
+	vf.csi_prp_vf_mem.out_width = cam->win.w.width;
+	vf.csi_prp_vf_mem.out_height = cam->win.w.height;
+	vf.csi_prp_vf_mem.csi = cam->csi;
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		vf.csi_prp_vf_mem.out_width = cam->win.w.height;
+		vf.csi_prp_vf_mem.out_height = cam->win.w.width;
+	}
+	vf.csi_prp_vf_mem.out_pixel_fmt = format;
+	size = cam->win.w.width * cam->win.w.height * size;
+
+	err = ipu_init_channel(CSI_PRP_VF_MEM, &vf);
+	if (err != 0)
+		goto out_5;
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0],
+				  (dma_addr_t) cam->vf_bufs[0]);
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1],
+				  (dma_addr_t) cam->vf_bufs[1]);
+	}
+	cam->vf_bufs_size[0] = PAGE_ALIGN(size);
+	cam->vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+							   cam->vf_bufs_size[0],
+							   (dma_addr_t *) &
+							   cam->vf_bufs[0],
+							   GFP_DMA |
+							   GFP_KERNEL);
+	if (cam->vf_bufs_vaddr[0] == NULL) {
+		printk(KERN_ERR "Error to allocate vf buffer\n");
+		err = -ENOMEM;
+		goto out_4;
+	}
+	cam->vf_bufs_size[1] = PAGE_ALIGN(size);
+	cam->vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+							   cam->vf_bufs_size[1],
+							   (dma_addr_t *) &
+							   cam->vf_bufs[1],
+							   GFP_DMA |
+							   GFP_KERNEL);
+	if (cam->vf_bufs_vaddr[1] == NULL) {
+		printk(KERN_ERR "Error to allocate vf buffer\n");
+		err = -ENOMEM;
+		goto out_3;
+	}
+	pr_debug("vf_bufs %x %x\n", cam->vf_bufs[0], cam->vf_bufs[1]);
+
+	if (cam->vf_rotation >= IPU_ROTATE_VERT_FLIP) {
+		err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      IPU_ROTATE_NONE, cam->vf_bufs[0],
+					      cam->vf_bufs[1], 0, 0);
+		if (err != 0) {
+			goto out_3;
+		}
+
+		err = ipu_init_channel(MEM_ROT_VF_MEM, NULL);
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM channel\n");
+			goto out_3;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_INPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      cam->vf_rotation, cam->vf_bufs[0],
+					      cam->vf_bufs[1], 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM input buffer\n");
+			goto out_2;
+		}
+
+		if (cam->vf_rotation < IPU_ROTATE_90_RIGHT) {
+			temp = vf.csi_prp_vf_mem.out_width;
+			vf.csi_prp_vf_mem.out_width =
+						vf.csi_prp_vf_mem.out_height;
+			vf.csi_prp_vf_mem.out_height = temp;
+		}
+
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      IPU_ROTATE_NONE,
+					      fbi->fix.smem_start +
+					      (fbi->fix.line_length *
+					       fbi->var.yres),
+					      fbi->fix.smem_start, 0, 0);
+
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM output buffer\n");
+			goto out_2;
+		}
+
+		err = ipu_link_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "Error link CSI_PRP_VF_MEM-MEM_ROT_VF_MEM\n");
+			goto out_2;
+		}
+
+		err = ipu_link_channels(MEM_ROT_VF_MEM, MEM_FG_SYNC);
+		if (err < 0) {
+			printk(KERN_ERR
+			       "Error link MEM_ROT_VF_MEM-MEM_FG_SYNC\n");
+			goto out_1;
+		}
+
+		ipu_enable_channel(CSI_PRP_VF_MEM);
+		ipu_enable_channel(MEM_ROT_VF_MEM);
+
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+	} else {
+		err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format, cam->win.w.width,
+					      cam->win.w.height,
+					      cam->win.w.width,
+					      cam->vf_rotation,
+					      fbi->fix.smem_start +
+					      (fbi->fix.line_length *
+					       fbi->var.yres),
+					      fbi->fix.smem_start, 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "Error initializing CSI_PRP_VF_MEM\n");
+			goto out_4;
+		}
+
+		err = ipu_link_channels(CSI_PRP_VF_MEM, MEM_FG_SYNC);
+		if (err < 0) {
+			printk(KERN_ERR "Error linking ipu channels\n");
+			goto out_4;
+		}
+
+		ipu_enable_channel(CSI_PRP_VF_MEM);
+
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+	}
+
+	acquire_console_sem();
+	fb_blank(fbi, FB_BLANK_UNBLANK);
+	release_console_sem();
+
+	cam->overlay_active = true;
+	return err;
+
+out_1:
+	ipu_unlink_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+out_2:
+	if (cam->vf_rotation >= IPU_ROTATE_VERT_FLIP) {
+		ipu_uninit_channel(MEM_ROT_VF_MEM);
+	}
+out_3:
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0],
+				  (dma_addr_t) cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1],
+				  (dma_addr_t) cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+out_4:
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+out_5:
+	return err;
+}
+
+/*!
+ * prpvf_stop - stop the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0, i = 0;
+	struct fb_info *fbi = NULL;
+
+	if (cam->overlay_active == false)
+		return 0;
+
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strcmp(idstr, "DISP3 FG") == 0)
+			fbi = registered_fb[i];
+	}
+
+	if (fbi == NULL) {
+		printk(KERN_ERR "DISP3 FG fb not found\n");
+		return -EPERM;
+	}
+
+	ipu_disp_set_window_pos(MEM_FG_SYNC, 0, 0);
+
+	if (cam->vf_rotation >= IPU_ROTATE_VERT_FLIP) {
+		ipu_unlink_channels(CSI_PRP_VF_MEM, MEM_ROT_VF_MEM);
+		ipu_unlink_channels(MEM_ROT_VF_MEM, MEM_FG_SYNC);
+	} else {
+		ipu_unlink_channels(CSI_PRP_VF_MEM, MEM_FG_SYNC);
+	}
+
+	ipu_disable_channel(CSI_PRP_VF_MEM, true);
+
+	if (cam->vf_rotation >= IPU_ROTATE_VERT_FLIP) {
+		ipu_disable_channel(MEM_ROT_VF_MEM, true);
+		ipu_uninit_channel(MEM_ROT_VF_MEM);
+	}
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+
+	acquire_console_sem();
+	fb_blank(fbi, FB_BLANK_POWERDOWN);
+	release_console_sem();
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0],
+				  (dma_addr_t) cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1],
+				  (dma_addr_t) cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+
+	cam->overlay_active = false;
+	return err;
+}
+
+/*!
+ * function to select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_sdc_select(void *private)
+{
+	cam_data *cam;
+	int err = 0;
+	if (private) {
+		cam = (cam_data *) private;
+		cam->vf_start_sdc = prpvf_start;
+		cam->vf_stop_sdc = prpvf_stop;
+		cam->overlay_active = false;
+	} else
+		err = -EIO;
+
+	return err;
+}
+
+/*!
+ * function to de-select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  int
+ */
+int prp_vf_sdc_deselect(void *private)
+{
+	cam_data *cam;
+	int err = 0;
+	err = prpvf_stop(private);
+
+	if (private) {
+		cam = (cam_data *) private;
+		cam->vf_start_sdc = NULL;
+		cam->vf_stop_sdc = NULL;
+	}
+	return err;
+}
+
+/*!
+ * Init viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_vf_sdc_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+void __exit prp_vf_sdc_exit(void)
+{
+}
+
+module_init(prp_vf_sdc_init);
+module_exit(prp_vf_sdc_exit);
+
+EXPORT_SYMBOL(prp_vf_sdc_select);
+EXPORT_SYMBOL(prp_vf_sdc_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP VF SDC Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
new file mode 100644
index 000000000000..4035c7664022
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
@@ -0,0 +1,413 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_prp_vf_sdc_bg.c
+ *
+ * @brief IPU Use case for PRP-VF back-ground
+ *
+ * @ingroup IPU
+ */
+#include <linux/dma-mapping.h>
+#include <linux/fb.h>
+#include <linux/ipu.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+static int buffer_num = 0;
+static int buffer_ready = 0;
+
+/*
+ * Function definitions
+ */
+
+/*!
+ * SDC V-Sync callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prpvf_sdc_vsync_callback(int irq, void *dev_id)
+{
+	pr_debug("buffer_ready %d buffer_num %d\n", buffer_ready, buffer_num);
+	if (buffer_ready > 0) {
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		buffer_ready--;
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * VF EOF callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prpvf_vf_eof_callback(int irq, void *dev_id)
+{
+	pr_debug("buffer_ready %d buffer_num %d\n", buffer_ready, buffer_num);
+
+	ipu_select_buffer(MEM_ROT_VF_MEM, IPU_INPUT_BUFFER, buffer_num);
+
+	buffer_num = (buffer_num == 0) ? 1 : 0;
+
+	ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, buffer_num);
+	buffer_ready++;
+	return IRQ_HANDLED;
+}
+
+/*!
+ * prpvf_start - start the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	ipu_channel_params_t vf;
+	u32 format;
+	u32 offset;
+	u32 bpp, size = 3;
+	int err = 0;
+
+	if (!cam) {
+		printk(KERN_ERR "private is NULL\n");
+		return -EIO;
+	}
+
+	if (cam->overlay_active == true) {
+		pr_debug("already start.\n");
+		return 0;
+	}
+
+	format = cam->v4l2_fb.fmt.pixelformat;
+	if (cam->v4l2_fb.fmt.pixelformat == IPU_PIX_FMT_BGR24) {
+		bpp = 3, size = 3;
+		pr_info("BGR24\n");
+	} else if (cam->v4l2_fb.fmt.pixelformat == IPU_PIX_FMT_RGB565) {
+		bpp = 2, size = 2;
+		pr_info("RGB565\n");
+	} else if (cam->v4l2_fb.fmt.pixelformat == IPU_PIX_FMT_BGR32) {
+		bpp = 4, size = 4;
+		pr_info("BGR32\n");
+	} else {
+		printk(KERN_ERR
+		       "unsupported fix format from the framebuffer.\n");
+		return -EINVAL;
+	}
+
+	offset = cam->v4l2_fb.fmt.bytesperline * cam->win.w.top +
+	    size * cam->win.w.left;
+
+	if (cam->v4l2_fb.base == 0) {
+		printk(KERN_ERR "invalid frame buffer address.\n");
+	} else {
+		offset += (u32) cam->v4l2_fb.base;
+	}
+
+	memset(&vf, 0, sizeof(ipu_channel_params_t));
+	ipu_csi_get_window_size(&vf.csi_prp_vf_mem.in_width,
+				&vf.csi_prp_vf_mem.in_height, cam->csi);
+	vf.csi_prp_vf_mem.in_pixel_fmt = IPU_PIX_FMT_UYVY;
+	vf.csi_prp_vf_mem.out_width = cam->win.w.width;
+	vf.csi_prp_vf_mem.out_height = cam->win.w.height;
+	vf.csi_prp_vf_mem.csi = cam->csi;
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		vf.csi_prp_vf_mem.out_width = cam->win.w.height;
+		vf.csi_prp_vf_mem.out_height = cam->win.w.width;
+	}
+	vf.csi_prp_vf_mem.out_pixel_fmt = format;
+	size = cam->win.w.width * cam->win.w.height * size;
+
+	err = ipu_init_channel(CSI_PRP_VF_MEM, &vf);
+	if (err != 0)
+		goto out_4;
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, true, true);
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+	}
+	cam->vf_bufs_size[0] = PAGE_ALIGN(size);
+	cam->vf_bufs_vaddr[0] = (void *)dma_alloc_coherent(0,
+							   cam->vf_bufs_size[0],
+							   &cam->vf_bufs[0],
+							   GFP_DMA |
+							   GFP_KERNEL);
+	if (cam->vf_bufs_vaddr[0] == NULL) {
+		printk(KERN_ERR "Error to allocate vf buffer\n");
+		err = -ENOMEM;
+		goto out_3;
+	}
+	cam->vf_bufs_size[1] = PAGE_ALIGN(size);
+	cam->vf_bufs_vaddr[1] = (void *)dma_alloc_coherent(0,
+							   cam->vf_bufs_size[1],
+							   &cam->vf_bufs[1],
+							   GFP_DMA |
+							   GFP_KERNEL);
+	if (cam->vf_bufs_vaddr[1] == NULL) {
+		printk(KERN_ERR "Error to allocate vf buffer\n");
+		err = -ENOMEM;
+		goto out_3;
+	}
+
+	err = ipu_init_channel_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER,
+				      format, vf.csi_prp_vf_mem.out_width,
+				      vf.csi_prp_vf_mem.out_height,
+				      vf.csi_prp_vf_mem.out_width,
+				      IPU_ROTATE_NONE, cam->vf_bufs[0],
+				      cam->vf_bufs[1], 0, 0);
+	if (err != 0) {
+		printk(KERN_ERR "Error initializing CSI_PRP_VF_MEM\n");
+		goto out_3;
+	}
+	err = ipu_init_channel(MEM_ROT_VF_MEM, NULL);
+	if (err != 0) {
+		printk(KERN_ERR "Error MEM_ROT_VF_MEM channel\n");
+		goto out_3;
+	}
+
+	err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_INPUT_BUFFER,
+				      format, vf.csi_prp_vf_mem.out_width,
+				      vf.csi_prp_vf_mem.out_height,
+				      vf.csi_prp_vf_mem.out_width,
+				      cam->vf_rotation, cam->vf_bufs[0],
+				      cam->vf_bufs[1], 0, 0);
+	if (err != 0) {
+		printk(KERN_ERR "Error MEM_ROT_VF_MEM input buffer\n");
+		goto out_2;
+	}
+
+	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_height,
+					      vf.csi_prp_vf_mem.out_width,
+					      cam->overlay_fb->var.xres * bpp,
+					      IPU_ROTATE_NONE, offset, 0, 0, 0);
+
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM output buffer\n");
+			goto out_2;
+		}
+	} else {
+		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
+					      format,
+					      vf.csi_prp_vf_mem.out_width,
+					      vf.csi_prp_vf_mem.out_height,
+					      cam->overlay_fb->var.xres * bpp,
+					      IPU_ROTATE_NONE, offset, 0, 0, 0);
+		if (err != 0) {
+			printk(KERN_ERR "Error MEM_ROT_VF_MEM output buffer\n");
+			goto out_2;
+		}
+	}
+
+	ipu_clear_irq(IPU_IRQ_PRP_VF_OUT_EOF);
+	err = ipu_request_irq(IPU_IRQ_PRP_VF_OUT_EOF, prpvf_vf_eof_callback,
+			      0, "Mxc Camera", cam);
+	if (err != 0) {
+		printk(KERN_ERR
+		       "Error registering IPU_IRQ_PRP_VF_OUT_EOF irq.\n");
+		goto out_2;
+	}
+
+	ipu_clear_irq(IPU_IRQ_BG_SF_END);
+	err = ipu_request_irq(IPU_IRQ_BG_SF_END, prpvf_sdc_vsync_callback,
+			      0, "Mxc Camera", NULL);
+	if (err != 0) {
+		printk(KERN_ERR "Error registering IPU_IRQ_BG_SF_END irq.\n");
+		goto out_1;
+	}
+
+	ipu_enable_channel(CSI_PRP_VF_MEM);
+	ipu_enable_channel(MEM_ROT_VF_MEM);
+
+	buffer_num = 0;
+	buffer_ready = 0;
+	ipu_select_buffer(CSI_PRP_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+
+	cam->overlay_active = true;
+	return err;
+
+      out_1:
+	ipu_free_irq(IPU_IRQ_PRP_VF_OUT_EOF, NULL);
+      out_2:
+	ipu_uninit_channel(MEM_ROT_VF_MEM);
+      out_3:
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+      out_4:
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[0],
+				  cam->rot_vf_bufs_vaddr[0],
+				  cam->rot_vf_bufs[0]);
+		cam->rot_vf_bufs_vaddr[0] = NULL;
+		cam->rot_vf_bufs[0] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[1],
+				  cam->rot_vf_bufs_vaddr[1],
+				  cam->rot_vf_bufs[1]);
+		cam->rot_vf_bufs_vaddr[1] = NULL;
+		cam->rot_vf_bufs[1] = 0;
+	}
+	return err;
+}
+
+/*!
+ * prpvf_stop - stop the vf task
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ */
+static int prpvf_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam->overlay_active == false)
+		return 0;
+
+	ipu_free_irq(IPU_IRQ_BG_SF_END, NULL);
+
+	ipu_free_irq(IPU_IRQ_PRP_VF_OUT_EOF, cam);
+
+	ipu_disable_channel(CSI_PRP_VF_MEM, true);
+	ipu_disable_channel(MEM_ROT_VF_MEM, true);
+	ipu_uninit_channel(CSI_PRP_VF_MEM);
+	ipu_uninit_channel(MEM_ROT_VF_MEM);
+	ipu_csi_enable_mclk_if(CSI_MCLK_VF, cam->csi, false, false);
+
+	if (cam->vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->vf_bufs_size[0],
+				  cam->vf_bufs_vaddr[0], cam->vf_bufs[0]);
+		cam->vf_bufs_vaddr[0] = NULL;
+		cam->vf_bufs[0] = 0;
+	}
+	if (cam->vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->vf_bufs_size[1],
+				  cam->vf_bufs_vaddr[1], cam->vf_bufs[1]);
+		cam->vf_bufs_vaddr[1] = NULL;
+		cam->vf_bufs[1] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[0]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[0],
+				  cam->rot_vf_bufs_vaddr[0],
+				  cam->rot_vf_bufs[0]);
+		cam->rot_vf_bufs_vaddr[0] = NULL;
+		cam->rot_vf_bufs[0] = 0;
+	}
+	if (cam->rot_vf_bufs_vaddr[1]) {
+		dma_free_coherent(0, cam->rot_vf_buf_size[1],
+				  cam->rot_vf_bufs_vaddr[1],
+				  cam->rot_vf_bufs[1]);
+		cam->rot_vf_bufs_vaddr[1] = NULL;
+		cam->rot_vf_bufs[1] = 0;
+	}
+
+	buffer_num = 0;
+	buffer_ready = 0;
+	cam->overlay_active = false;
+	return 0;
+}
+
+/*!
+ * function to select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_sdc_select_bg(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->vf_start_sdc = prpvf_start;
+		cam->vf_stop_sdc = prpvf_stop;
+		cam->overlay_active = false;
+	}
+
+	return 0;
+}
+
+/*!
+ * function to de-select PRP-VF as the working path
+ *
+ * @param private    cam_data * mxc v4l2 main structure
+ *
+ * @return  status
+ */
+int prp_vf_sdc_deselect_bg(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+	err = prpvf_stop(private);
+
+	if (cam) {
+		cam->vf_start_sdc = NULL;
+		cam->vf_stop_sdc = NULL;
+	}
+	return err;
+}
+
+/*!
+ * Init viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_vf_sdc_init_bg(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit viewfinder task.
+ *
+ * @return  Error code indicating success or failure
+ */
+void __exit prp_vf_sdc_exit_bg(void)
+{
+}
+
+module_init(prp_vf_sdc_init_bg);
+module_exit(prp_vf_sdc_exit_bg);
+
+EXPORT_SYMBOL(prp_vf_sdc_select_bg);
+EXPORT_SYMBOL(prp_vf_sdc_deselect_bg);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP VF SDC Backgroud Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ipu_still.c b/drivers/media/video/mxc/capture/ipu_still.c
new file mode 100644
index 000000000000..34cea8609c95
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ipu_still.c
@@ -0,0 +1,252 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ipu_still.c
+ *
+ * @brief IPU Use case for still image capture
+ *
+ * @ingroup IPU
+ */
+
+#include <linux/ipu.h>
+#include <linux/semaphore.h>
+#include <linux/ipu.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+static int callback_eof_flag;
+
+#ifdef CONFIG_MXC_IPU_V1
+static int callback_flag;
+/*
+ * Function definitions
+ */
+/*!
+ * CSI EOF callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prp_csi_eof_callback(int irq, void *dev_id)
+{
+	if (callback_flag == 2) {
+		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_enable_channel(CSI_MEM);
+	}
+
+	callback_flag++;
+	return IRQ_HANDLED;
+}
+#endif
+
+/*!
+ * CSI callback function.
+ *
+ * @param irq       int irq line
+ * @param dev_id    void * device id
+ *
+ * @return status   IRQ_HANDLED for handled
+ */
+static irqreturn_t prp_still_callback(int irq, void *dev_id)
+{
+	cam_data *cam = (cam_data *) dev_id;
+
+	callback_eof_flag++;
+	if (callback_eof_flag < 5)
+		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
+	else {
+		cam->still_counter++;
+		wake_up_interruptible(&cam->still_queue);
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * start csi->mem task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int prp_still_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	u32 pixel_fmt;
+	int err;
+	ipu_channel_params_t params;
+
+	if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
+		pixel_fmt = IPU_PIX_FMT_YUV420P;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P)
+		pixel_fmt = IPU_PIX_FMT_YUV422P;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY)
+		pixel_fmt = IPU_PIX_FMT_UYVY;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR24)
+		pixel_fmt = IPU_PIX_FMT_BGR24;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24)
+		pixel_fmt = IPU_PIX_FMT_RGB24;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB565)
+		pixel_fmt = IPU_PIX_FMT_RGB565;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR32)
+		pixel_fmt = IPU_PIX_FMT_BGR32;
+	else if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_RGB32)
+		pixel_fmt = IPU_PIX_FMT_RGB32;
+	else {
+		printk(KERN_ERR "format not supported\n");
+		return -EINVAL;
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_RAW, cam->csi, true, true);
+
+	memset(&params, 0, sizeof(params));
+	err = ipu_init_channel(CSI_MEM, &params);
+	if (err != 0)
+		return err;
+
+	err = ipu_init_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+				      pixel_fmt, cam->v2f.fmt.pix.width,
+				      cam->v2f.fmt.pix.height,
+				      cam->v2f.fmt.pix.width, IPU_ROTATE_NONE,
+				      cam->still_buf, 0, 0, 0);
+	if (err != 0)
+		return err;
+
+#ifdef CONFIG_MXC_IPU_V1
+	err = ipu_request_irq(IPU_IRQ_SENSOR_OUT_EOF, prp_still_callback,
+			      0, "Mxc Camera", cam);
+	if (err != 0) {
+		printk(KERN_ERR "Error registering irq.\n");
+		return err;
+	}
+	callback_flag = 0;
+	callback_eof_flag = 0;
+	err = ipu_request_irq(IPU_IRQ_SENSOR_EOF, prp_csi_eof_callback,
+			      0, "Mxc Camera", NULL);
+	if (err != 0) {
+		printk(KERN_ERR "Error IPU_IRQ_SENSOR_EOF \n");
+		return err;
+	}
+#else
+	ipu_clear_irq(IPU_IRQ_CSI0_OUT_EOF);
+	err = ipu_request_irq(IPU_IRQ_CSI0_OUT_EOF, prp_still_callback,
+			      0, "Mxc Camera", cam);
+	if (err != 0) {
+		printk(KERN_ERR "Error registering irq.\n");
+		return err;
+	}
+
+	callback_eof_flag = 0;
+
+	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
+	ipu_enable_channel(CSI_MEM);
+#endif
+
+	return err;
+}
+
+/*!
+ * stop csi->mem encoder task
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+static int prp_still_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+#ifdef CONFIG_MXC_IPU_V1
+	ipu_free_irq(IPU_IRQ_SENSOR_EOF, NULL);
+	ipu_free_irq(IPU_IRQ_SENSOR_OUT_EOF, cam);
+#else
+	ipu_free_irq(IPU_IRQ_CSI0_OUT_EOF, cam);
+#endif
+
+	ipu_disable_channel(CSI_MEM, true);
+	ipu_uninit_channel(CSI_MEM);
+	ipu_csi_enable_mclk_if(CSI_MCLK_RAW, cam->csi, false, false);
+
+	return err;
+}
+
+/*!
+ * function to select CSI_MEM as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+int prp_still_select(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->csi_start = prp_still_start;
+		cam->csi_stop = prp_still_stop;
+	}
+
+	return 0;
+}
+
+/*!
+ * function to de-select CSI_MEM as the working path
+ *
+ * @param private       struct cam_data * mxc capture instance
+ *
+ * @return  status
+ */
+int prp_still_deselect(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	err = prp_still_stop(cam);
+
+	if (cam) {
+		cam->csi_start = NULL;
+		cam->csi_stop = NULL;
+	}
+
+	return err;
+}
+
+/*!
+ * Init the Encorder channels
+ *
+ * @return  Error code indicating success or failure
+ */
+__init int prp_still_init(void)
+{
+	return 0;
+}
+
+/*!
+ * Deinit the Encorder channels
+ *
+ */
+void __exit prp_still_exit(void)
+{
+}
+
+module_init(prp_still_init);
+module_exit(prp_still_exit);
+
+EXPORT_SYMBOL(prp_still_select);
+EXPORT_SYMBOL(prp_still_deselect);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("IPU PRP STILL IMAGE Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/mc521da.c b/drivers/media/video/mxc/capture/mc521da.c
new file mode 100644
index 000000000000..a8ff84fb4c84
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mc521da.c
@@ -0,0 +1,648 @@
+/*
+ * Copyright 2006-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mc521da.c
+ *
+ * @brief MC521DA camera driver functions
+ *
+ * @ingroup Camera
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include "mxc_v4l2_capture.h"
+
+#define MC521DA_I2C_ADDRESS	0x22
+#define MC521DA_TERM		0xFF
+
+typedef struct {
+	u16 width;
+	u16 height;
+} mc521da_image_format;
+
+struct mc521da_reg {
+	u8 reg;
+	u8 val;
+};
+
+static sensor_interface *interface_param = NULL;
+
+static mc521da_image_format format[2] = {
+	{
+	 .width = 1600,
+	 .height = 1200,
+	 },
+	{
+	 .width = 640,
+	 .height = 480,
+	 },
+};
+
+const static struct mc521da_reg mc521da_initial[] = {
+	/*----------------------------------------------------------
+	 * Sensor Setting Start
+	 *----------------------------------------------------------
+	 */
+	{0xff, 0x01},		/* Sensor setting start */
+	{0x01, 0x10},		/* Wavetable script, generated by waveman */
+	{0x10, 0x64},
+	{0x03, 0x00}, {0x04, 0x06}, {0x05, 0x30}, {0x06, 0x02}, {0x08, 0x00},
+	{0x03, 0x01}, {0x04, 0x41}, {0x05, 0x70}, {0x06, 0x03}, {0x08, 0x00},
+	{0x03, 0x02}, {0x04, 0x55}, {0x05, 0x30}, {0x06, 0x03}, {0x08, 0x00},
+	{0x03, 0x03}, {0x04, 0x5A}, {0x05, 0x30}, {0x06, 0x02}, {0x08, 0x00},
+	{0x03, 0x04}, {0x04, 0x7A}, {0x05, 0x30}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x05}, {0x04, 0x9C}, {0x05, 0x30}, {0x06, 0x0F}, {0x08, 0x00},
+	{0x03, 0x06}, {0x04, 0x73}, {0x05, 0x31}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x07}, {0x04, 0x2D}, {0x05, 0x3B}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x08}, {0x04, 0x32}, {0x05, 0x33}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x09}, {0x04, 0x67}, {0x05, 0x63}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x0a}, {0x04, 0x6C}, {0x05, 0x23}, {0x06, 0x0E}, {0x08, 0x00},
+	{0x03, 0x0b}, {0x04, 0x71}, {0x05, 0x23}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x0c}, {0x04, 0x30}, {0x05, 0x2F}, {0x06, 0x06}, {0x08, 0x00},
+	{0x03, 0x0d}, {0x04, 0x00}, {0x05, 0x00}, {0x06, 0x06}, {0x08, 0x00},
+	{0x07, 0x0e},
+
+	/* Start Address */
+	{0x10, 0x64}, {0x14, 0x10}, {0x15, 0x00},
+
+	/* SYNC */
+	{0x18, 0x40}, {0x19, 0x00}, {0x1A, 0x03}, {0x1B, 0x00},
+
+	/* X-Y Mirror */
+	{0x11, 0x00}, {0xda, 0x00},	/* X mirror OFF, Y Mirror OFF */
+
+	/* Frame height */
+	{0x1c, 0x13}, {0x1d, 0x04}, {0x0e, 0x4b}, {0x0f, 0x05},
+	{0x9e, 0x04}, {0x9d, 0xc6}, {0xcc, 0x14}, {0xcd, 0x05},
+
+	/* Frame width */
+	{0x0c, 0x35}, {0x0d, 0x07}, {0x9b, 0x10}, {0x9c, 0x07},
+	{0x93, 0x21},
+
+	{0x01, 0x01}, {0x40, 0x00}, {0x41, 0x00}, {0x42, 0xf0},
+	{0x43, 0x03}, {0x44, 0x0a}, {0x45, 0x00}, {0x3b, 0x40},
+	{0x38, 0x18}, {0x3c, 0x00}, {0x20, 0x00}, {0x21, 0x01},
+	{0x22, 0x00}, {0x23, 0x01}, {0x24, 0x00}, {0x25, 0x01},
+	{0x26, 0x00}, {0x27, 0x01}, {0xb9, 0x04}, {0xb8, 0xc3},
+	{0xbb, 0x04}, {0xba, 0xc3}, {0xbf, 0x04}, {0xbe, 0xc3},
+
+	/* Ramp */
+	{0x57, 0x07}, {0x56, 0xd6}, {0x55, 0x03}, {0x54, 0x74},
+	{0x9f, 0x99}, {0x94, 0x80}, {0x91, 0x78}, {0x92, 0x8b},
+
+	/* Output Mode */
+	{0x52, 0x10}, {0x51, 0x00},
+
+	/* Analog Gain and Output driver */
+	{0x28, 0x00}, {0xdd, 0x82}, {0xdb, 0x00}, {0xdc, 0x00},
+
+	/* Update */
+	{0x00, 0x84},
+
+	/* PLL  ADC clock = 75 MHz */
+	{0xb5, 0x60}, {0xb4, 0x02}, {0xb5, 0x20},
+
+	/*----------------------------------------------*/
+	/*      ISP Setting Start                       */
+	/*----------------------------------------------*/
+	{0xff, 0x02},
+	{0x01, 0xbd}, {0x02, 0xf8}, {0x03, 0x3a}, {0x04, 0x00}, {0x0e, 0x00},
+
+	/* Output mode */
+	{0x88, 0x00}, {0x87, 0x11},
+
+	/* Threshold */
+	{0xb6, 0x1b}, {0x0d, 0xc0}, {0x24, 0x00}, {0x25, 0x00}, {0x26, 0x00},
+
+	/* Image Effect */
+	{0x3f, 0x80}, {0x40, 0x00}, {0x41, 0x00}, {0x42, 0x80}, {0x43, 0x00},
+	{0x44, 0x00}, {0x45, 0x00}, {0x46, 0x00}, {0x56, 0x80}, {0x57, 0x20},
+	{0x58, 0x20}, {0x59, 0x02}, {0x5a, 0x00}, {0x5b, 0x78}, {0x5c, 0x7c},
+	{0x5d, 0x84}, {0x5e, 0x85}, {0x5f, 0x78}, {0x60, 0x7e}, {0x61, 0x82},
+	{0x62, 0x85}, {0x63, 0x00}, {0x64, 0x80}, {0x65, 0x00}, {0x66, 0x80},
+	{0x67, 0x80}, {0x68, 0x80},
+
+	/* Auto Focus */
+	{0x6e, 0x02}, {0x6f, 0xe5}, {0x70, 0x08}, {0x71, 0x01}, {0x72, 0x00},
+
+	/* Decimator */
+	{0x78, 0xff}, {0x79, 0xff}, {0x7a, 0x70}, {0x7b, 0x00}, {0x7c, 0x00},
+	{0x7d, 0x00}, {0x7e, 0xc8}, {0x7f, 0xc8}, {0x80, 0x96}, {0x81, 0x96},
+	{0x82, 0x00}, {0x83, 0x00}, {0x84, 0x00}, {0x85, 0x00}, {0x86, 0x00},
+
+	/* Luminance Info */
+	{0xf9, 0x20}, {0xb7, 0x7f}, {0xb8, 0x28}, {0xb9, 0x08},
+	{0xf9, 0xa0}, {0xb7, 0x10}, {0xb9, 0x00},
+	{0xf9, 0x40}, {0xb7, 0x7f}, {0xb8, 0x28}, {0xb9, 0x08},
+	{0xf9, 0xc0}, {0xb7, 0x08}, {0xb9, 0x00},
+	{0xf9, 0x60}, {0xb7, 0x7f}, {0xb8, 0x28}, {0xb9, 0x08},
+	{0xf9, 0xe0}, {0xb7, 0x05}, {0xb9, 0x00},
+	{0xf9, 0x00}, {0xb7, 0x03}, {0xb8, 0x2d}, {0xb9, 0xcd},
+	{0xf9, 0x80}, {0xb7, 0x02}, {0xb9, 0x00},
+
+	/* AE */
+	{0x8a, 0x00}, {0x89, 0xc0}, {0x8c, 0x32}, {0x8d, 0x96}, {0x8e, 0x25},
+	{0x8f, 0x70}, {0x90, 0x12}, {0x91, 0x41}, {0x9e, 0x2e}, {0x9f, 0x2e},
+	{0xa0, 0x0b}, {0xa1, 0x71}, {0xa2, 0xb0}, {0xa3, 0x09}, {0xa4, 0x89},
+	{0xa5, 0x68}, {0xa6, 0x1a}, {0xa7, 0xb3}, {0xa8, 0xf0}, {0xa9, 0x19},
+	{0xaa, 0x6a}, {0xab, 0x6b}, {0xac, 0x01}, {0xad, 0xe8}, {0xae, 0x48},
+	{0xaf, 0x01}, {0xb0, 0x96}, {0xb1, 0xe6}, {0xb2, 0x03}, {0xb3, 0x00},
+	{0xb4, 0x10}, {0xb5, 0x00}, {0xb6, 0x04}, {0xba, 0x44}, {0xbb, 0x3a},
+	{0xbc, 0x01}, {0xbd, 0x08}, {0xbe, 0xa0}, {0xbf, 0x01}, {0xc0, 0x82},
+	{0x8a, 0xe1}, {0x8b, 0x8c},
+
+	/* AWB */
+	{0xc8, 0x00}, {0xc9, 0x00}, {0xca, 0x40}, {0xcb, 0xB0}, {0xcc, 0x40},
+	{0xcd, 0xff}, {0xce, 0x19}, {0xcf, 0x40}, {0xd0, 0x01}, {0xd1, 0x43},
+	{0xd2, 0x80}, {0xd3, 0x80}, {0xd4, 0xf1}, {0xdf, 0x00}, {0xe0, 0x8f},
+	{0xe1, 0x8f}, {0xe2, 0x53}, {0xe3, 0x97}, {0xe4, 0x1f}, {0xe5, 0x3b},
+	{0xe6, 0x9c}, {0xe7, 0x2e}, {0xe8, 0x03}, {0xe9, 0x02},
+
+	/* Neutral CCM */
+	{0xfa, 0x00}, {0xd5, 0x3f}, {0xd6, 0x8c}, {0xd7, 0x43}, {0xd8, 0x08},
+	{0xd9, 0x27}, {0xda, 0x7e}, {0xdb, 0x17}, {0xdc, 0x1a}, {0xdd, 0x47},
+	{0xde, 0xa1},
+
+	/* Blue CCM */
+	{0xfa, 0x01}, {0xd5, 0x3f}, {0xd6, 0x77}, {0xd7, 0x34}, {0xd8, 0x03},
+	{0xd9, 0x18}, {0xda, 0x6e}, {0xdb, 0x16}, {0xdc, 0x0f}, {0xdd, 0x29},
+	{0xde, 0x77},
+
+	/* Red CCM */
+	{0xfa, 0x02}, {0xd5, 0x3f}, {0xd6, 0x7d}, {0xd7, 0x2f}, {0xd8, 0x0e},
+	{0xd9, 0x1e}, {0xda, 0x76}, {0xdb, 0x18}, {0xdc, 0x29}, {0xdd, 0x51},
+	{0xde, 0xba},
+
+	/* AWB */
+	{0xea, 0x00}, {0xeb, 0x1a}, {0xc8, 0x33}, {0xc9, 0xc2},
+
+	{0xed, 0x02}, {0xee, 0x02},
+
+	/* AFD */
+	{0xf0, 0x11}, {0xf1, 0x03}, {0xf2, 0x05}, {0xf5, 0x05}, {0xf6, 0x32},
+	{0xf7, 0x32},
+
+	/* Lens Shading */
+	{0xf9, 0x00}, {0x05, 0x04}, {0x06, 0xff}, {0x07, 0xf2}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xf2}, {0x0b, 0xff}, {0x0c, 0xff},
+	{0xf9, 0x01}, {0x05, 0x04}, {0x06, 0xff}, {0x07, 0x8b}, {0x08, 0x16},
+	{0x09, 0x16}, {0x0a, 0x8b}, {0x0b, 0xff}, {0x0c, 0xe0},
+	{0xf9, 0x02}, {0x05, 0x04}, {0x06, 0xff}, {0x07, 0x8b}, {0x08, 0x16},
+	{0x09, 0x16}, {0x0a, 0x8b}, {0x0b, 0xff}, {0x0c, 0xe0},
+	{0xf9, 0x03}, {0x05, 0x04}, {0x06, 0xff}, {0x07, 0x7c}, {0x08, 0x26},
+	{0x09, 0x26}, {0x0a, 0x7c}, {0x0b, 0xd0}, {0x0c, 0xe0},
+	{0xf9, 0x04}, {0x05, 0x0d}, {0x06, 0x40}, {0x07, 0xa0}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xa0}, {0x0b, 0x40}, {0x0c, 0xe0},
+	{0xf9, 0x05}, {0x05, 0x0d}, {0x06, 0x40}, {0x07, 0xa0}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xa0}, {0x0b, 0x40}, {0x0c, 0xa0},
+	{0xf9, 0x06}, {0x05, 0x0d}, {0x06, 0x40}, {0x07, 0xa0}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xa0}, {0x0b, 0x40}, {0x0c, 0xa0},
+	{0xf9, 0x07}, {0x05, 0x0d}, {0x06, 0x40}, {0x07, 0xa0}, {0x08, 0x00},
+	{0x09, 0x00}, {0x0a, 0xa0}, {0x0b, 0x40}, {0x0c, 0xa0},
+
+	/* Edge setting */
+	{0x73, 0x68}, {0x74, 0x40}, {0x75, 0x00}, {0x76, 0xff}, {0x77, 0x80},
+	{0x4f, 0x80}, {0x50, 0x82}, {0x51, 0x82}, {0x52, 0x08},
+
+	/* Interpolation Setting */
+	{0x23, 0x7f}, {0x22, 0x08}, {0x18, 0xff}, {0x19, 0x00},
+	{0x40, 0x00}, {0x53, 0xff}, {0x54, 0x0a}, {0x55, 0xc2},
+	{0x1b, 0x18},
+
+	{0xfa, 0x00}, {0x15, 0x0c}, {0x22, 0x00}, {0x0e, 0xef}, {0x1f, 0x1d},
+	{0x20, 0x2d}, {0x1c, 0x01}, {0x1d, 0x02}, {0x1e, 0x03}, {0x0e, 0xee},
+	{0x12, 0x10}, {0x16, 0x10}, {0x17, 0x02}, {0x1a, 0x01},
+	{0xfa, 0x04}, {0x0e, 0xef}, {0x1c, 0x01}, {0x1d, 0x02}, {0x1e, 0x03},
+	{0x1f, 0x11}, {0x20, 0x11}, {0x0e, 0xee}, {0x12, 0x03}, {0x16, 0x10},
+	{0x17, 0x02}, {0x1a, 0xee},
+	{0xfa, 0x08}, {0x0e, 0xef}, {0x1c, 0x01}, {0x1d, 0x02}, {0x1e, 0x03},
+	{0x1f, 0x00}, {0x20, 0x00}, {0x0e, 0xee}, {0x12, 0x03}, {0x16, 0x10},
+	{0x17, 0x02}, {0x1a, 0x22},
+
+	/* Gamma Correction */
+	{0x27, 0x62}, {0x28, 0x00}, {0x27, 0x62}, {0x28, 0x00}, {0x29, 0x00},
+	{0x2a, 0x00}, {0x2f, 0x03}, {0x30, 0x10}, {0x31, 0x2b}, {0x32, 0x50},
+	{0x33, 0x70}, {0x34, 0x90}, {0x35, 0xB0}, {0x36, 0xD0}, {0x37, 0x00},
+	{0x38, 0x18}, {0x39, 0x57}, {0x3a, 0x89}, {0x3b, 0xac}, {0x3c, 0xc9},
+	{0x3d, 0xde}, {0x3e, 0xef}, {0x2b, 0x00}, {0x2c, 0x00}, {0x2d, 0x40},
+	{0x2e, 0xab},
+
+	/* Contrast */
+	{0x47, 0x10}, {0x48, 0x1f}, {0x49, 0xe3}, {0x4a, 0xf0}, {0x4b, 0x08},
+	{0x4c, 0x14}, {0x4d, 0xe9}, {0x4e, 0xf5}, {0x98, 0x8a},
+
+	{0xfa, 0x00},
+	{MC521DA_TERM, MC521DA_TERM}
+};
+
+static int mc521da_attach(struct i2c_adapter *adapter);
+static int mc521da_detach(struct i2c_client *client);
+
+static struct i2c_driver mc521da_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "MC521DA Client",
+		   },
+	.attach_adapter = mc521da_attach,
+	.detach_client = mc521da_detach,
+};
+
+static struct i2c_client mc521da_i2c_client = {
+	.name = "MC521DA I2C dev",
+	.addr = MC521DA_I2C_ADDRESS,
+	.driver = &mc521da_i2c_driver,
+};
+
+static inline int mc521da_read_reg(u8 reg)
+{
+	return i2c_smbus_read_byte_data(&mc521da_i2c_client, reg);
+}
+
+static inline int mc521da_write_reg(u8 reg, u8 val)
+{
+	return i2c_smbus_write_byte_data(&mc521da_i2c_client, reg, val);
+}
+
+static int mc521da_write_regs(const struct mc521da_reg reglist[])
+{
+	int err;
+	const struct mc521da_reg *next = reglist;
+
+	while (!((next->reg == MC521DA_TERM) && (next->val == MC521DA_TERM))) {
+		err = mc521da_write_reg(next->reg, next->val);
+		if (err) {
+			return err;
+		}
+		next++;
+	}
+	return 0;
+}
+
+/*!
+ * mc521da sensor downscale function
+ * @param downscale            bool
+ * @return  Error code indicating success or failure
+ */
+static u8 mc521da_sensor_downscale(bool downscale)
+{
+	u8 data;
+	u32 i = 0;
+
+	if (downscale == true) {
+		// VGA
+		mc521da_write_reg(0xff, 0x01);
+
+		mc521da_write_reg(0x52, 0x30);
+		mc521da_write_reg(0x51, 0x00);
+
+		mc521da_write_reg(0xda, 0x01);
+		mc521da_write_reg(0x00, 0x8C);
+
+		/* Wait for changes to take effect */
+		while (i < 256) {
+			i++;
+			data = mc521da_read_reg(0x00);
+			if ((data & 0x80) == 0)
+				break;
+			msleep(5);
+		}
+
+		/* ISP */
+		mc521da_write_reg(0xff, 0x02);
+
+		mc521da_write_reg(0x03, 0x3b);	/* Enable Decimator */
+
+		mc521da_write_reg(0x7a, 0x74);
+		mc521da_write_reg(0x7b, 0x01);
+		mc521da_write_reg(0x7e, 0x50);
+		mc521da_write_reg(0x7f, 0x50);
+		mc521da_write_reg(0x80, 0x3c);
+		mc521da_write_reg(0x81, 0x3c);
+	} else {
+		//UXGA
+		mc521da_write_reg(0xff, 0x01);
+		mc521da_write_reg(0x52, 0x10);
+		mc521da_write_reg(0x51, 0x00);
+		mc521da_write_reg(0xda, 0x00);
+
+		/* update */
+		mc521da_write_reg(0x00, 0x84);
+
+		/* Wait for changes to take effect */
+		while (i < 256) {
+			i++;
+			data = mc521da_read_reg(0x00);
+			if ((data & 0x80) == 0)
+				break;
+			msleep(5);
+		}
+
+		/* ISP */
+		mc521da_write_reg(0xff, 0x02);
+
+		mc521da_write_reg(0x03, 0x3a);
+
+		mc521da_write_reg(0x7a, 0x70);
+		mc521da_write_reg(0x7b, 0x00);
+		mc521da_write_reg(0x7e, 0xc8);
+		mc521da_write_reg(0x7f, 0xc8);
+		mc521da_write_reg(0x80, 0x96);
+		mc521da_write_reg(0x81, 0x96);
+	}
+
+	return 0;
+}
+
+/*!
+ * mc521da sensor interface Initialization
+ * @param param            sensor_interface *
+ * @param width            u32
+ * @param height           u32
+ * @return  None
+ */
+static void mc521da_interface(sensor_interface * param, u32 width, u32 height)
+{
+	param->clk_mode = 0x0;	//gated
+	param->pixclk_pol = 0x0;
+	param->data_width = 0x1;
+	param->data_pol = 0x0;
+	param->ext_vsync = 0x0;
+	param->Vsync_pol = 0x0;
+	param->Hsync_pol = 0x0;
+	param->width = width - 1;
+	param->height = height - 1;
+	param->active_width = width;
+	param->active_height = height;
+	param->pixel_fmt = IPU_PIX_FMT_UYVY;
+}
+
+extern void gpio_sensor_reset(bool flag);
+
+/*!
+ * mc521da Reset function
+ *
+ * @return  None
+ */
+static sensor_interface *mc521da_reset(void)
+{
+	if (interface_param == NULL)
+		return NULL;
+
+	mc521da_interface(interface_param, format[1].width, format[1].height);
+	set_mclk_rate(&interface_param->mclk);
+
+	gpio_sensor_reset(true);
+	msleep(10);
+	gpio_sensor_reset(false);
+	msleep(50);
+
+	return interface_param;
+}
+
+/*!
+ * mc521da sensor configuration
+ *
+ * @param frame_rate       int 	*
+ * @param high_quality     int
+ * @return  sensor_interface *
+ */
+static sensor_interface *mc521da_config(int *frame_rate, int high_quality)
+{
+	int num_clock_per_row, err;
+	int max_rate = 0;
+	int index = 1;
+	u16 frame_height;
+
+	if (high_quality == 1)
+		index = 0;
+
+	err = mc521da_write_regs(mc521da_initial);
+	if (err) {
+		/* Reduce the MCLK */
+		interface_param->mclk = 20000000;
+		mc521da_reset();
+
+		printk(KERN_INFO "mc521da: mclk reduced\n");
+		mc521da_write_regs(mc521da_initial);
+	}
+
+	mc521da_interface(interface_param, format[index].width,
+			  format[index].height);
+
+	if (index == 0) {
+		mc521da_sensor_downscale(false);
+	} else {
+		mc521da_sensor_downscale(true);
+	}
+
+	num_clock_per_row = 1845;
+	max_rate = interface_param->mclk * 3 * (index + 1)
+	    / (2 * num_clock_per_row * 1300);
+
+	if ((*frame_rate > max_rate) || (*frame_rate == 0)) {
+		*frame_rate = max_rate;
+	}
+
+	frame_height = 1300 * max_rate / (*frame_rate);
+
+	*frame_rate = interface_param->mclk * 3 * (index + 1)
+	    / (2 * num_clock_per_row * frame_height);
+
+	mc521da_write_reg(0xff, 0x01);
+	mc521da_write_reg(0xE, frame_height & 0xFF);
+	mc521da_write_reg(0xF, (frame_height & 0xFF00) >> 8);
+	mc521da_write_reg(0xCC, frame_height & 0xFF);
+	mc521da_write_reg(0xCD, (frame_height & 0xFF00) >> 8);
+
+	return interface_param;
+}
+
+/*!
+ * mc521da sensor set color configuration
+ *
+ * @param bright       int
+ * @param saturation   int
+ * @param red          int
+ * @param green        int
+ * @param blue         int
+ * @return  None
+ */
+static void
+mc521da_set_color(int bright, int saturation, int red, int green, int blue)
+{
+	/* Select ISP */
+	mc521da_write_reg(0xff, 0x02);
+
+	mc521da_write_reg(0x41, bright);
+	mc521da_write_reg(0xca, red);
+	mc521da_write_reg(0xcb, green);
+	mc521da_write_reg(0xcc, blue);
+}
+
+/*!
+ * mc521da sensor get color configuration
+ *
+ * @param bright       int *
+ * @param saturation   int *
+ * @param red          int *
+ * @param green        int *
+ * @param blue         int *
+ * @return  None
+ */
+static void
+mc521da_get_color(int *bright, int *saturation, int *red, int *green, int *blue)
+{
+	*saturation = 0;
+
+	/* Select ISP */
+	mc521da_write_reg(0xff, 0x02);
+
+	*bright = mc521da_read_reg(0x41);
+	*red = mc521da_read_reg(0xCA);
+	*green = mc521da_read_reg(0xCB);
+	*blue = mc521da_read_reg(0xCC);
+}
+
+struct camera_sensor camera_sensor_if = {
+      set_color:mc521da_set_color,
+      get_color:mc521da_get_color,
+      config:mc521da_config,
+      reset:mc521da_reset,
+};
+
+/*!
+ * mc521da I2C detect_client function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @param address            int
+ * @param kind               int
+ *
+ * @return  Error code indicating success or failure
+ */
+static int mc521da_detect_client(struct i2c_adapter *adapter, int address,
+				 int kind)
+{
+	mc521da_i2c_client.adapter = adapter;
+	if (i2c_attach_client(&mc521da_i2c_client)) {
+		mc521da_i2c_client.adapter = NULL;
+		printk(KERN_ERR "mc521da_attach: i2c_attach_client failed\n");
+		return -1;
+	}
+
+	interface_param = (sensor_interface *)
+	    kmalloc(sizeof(sensor_interface), GFP_KERNEL);
+	if (!interface_param) {
+		printk(KERN_ERR "mc521da_attach: kmalloc failed \n");
+		return -1;
+	}
+
+	interface_param->mclk = 25000000;
+
+	printk(KERN_INFO "mc521da Detected\n");
+
+	return 0;
+}
+
+static unsigned short normal_i2c[] = { MC521DA_I2C_ADDRESS, I2C_CLIENT_END };
+
+/* Magic definition of all other variables and things */
+I2C_CLIENT_INSMOD;
+
+static int mc521da_attach(struct i2c_adapter *adap)
+{
+	uint32_t mclk = 25000000;
+	struct clk *clk;
+	int err;
+
+	clk = clk_get(NULL, "csi_clk");
+	clk_enable(clk);
+	set_mclk_rate(&mclk);
+
+	gpio_sensor_reset(true);
+	msleep(10);
+	gpio_sensor_reset(false);
+	msleep(100);
+
+	err = i2c_probe(adap, &addr_data, &mc521da_detect_client);
+
+	clk_disable(clk);
+	clk_put(clk);
+
+	return err;
+}
+
+/*!
+ * mc521da I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int mc521da_detach(struct i2c_client *client)
+{
+	int err;
+
+	if (!mc521da_i2c_client.adapter)
+		return -1;
+
+	err = i2c_detach_client(&mc521da_i2c_client);
+	mc521da_i2c_client.adapter = NULL;
+
+	if (interface_param)
+		kfree(interface_param);
+	interface_param = NULL;
+
+	return err;
+}
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+/*!
+ * mc521da init function
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int mc521da_init(void)
+{
+	gpio_sensor_active();
+	return i2c_add_driver(&mc521da_i2c_driver);
+}
+
+/*!
+ * mc521da cleanup function
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit mc521da_clean(void)
+{
+	i2c_del_driver(&mc521da_i2c_driver);
+	gpio_sensor_inactive();
+}
+
+module_init(mc521da_init);
+module_exit(mc521da_clean);
+
+/* Exported symbols for modules. */
+EXPORT_SYMBOL(camera_sensor_if);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MC521DA Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/mt9v111.c b/drivers/media/video/mxc/capture/mt9v111.c
new file mode 100644
index 000000000000..c95a20683924
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mt9v111.c
@@ -0,0 +1,1076 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mt9v111.c
+ *
+ * @brief mt9v111 camera driver functions
+ *
+ * @ingroup Camera
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/clk.h>
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+#include "mt9v111.h"
+
+#ifdef MT9V111_DEBUG
+static u16 testpattern = 0;
+#endif
+
+static mt9v111_conf mt9v111_device;
+
+/*!
+ * Holds the current frame rate.
+ */
+static int reset_frame_rate = MT9V111_FRAME_RATE;
+
+struct sensor {
+	const struct mt9v111_platform_data *platform_data;
+	struct v4l2_int_device *v4l2_int_device;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	struct v4l2_captureparm streamcap;
+	bool on;
+
+	/* control settings */
+	int brightness;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+} mt9v111_data;
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+static int mt9v111_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id);
+static int mt9v111_remove(struct i2c_client *client);
+
+static const struct i2c_device_id mt9v111_id[] = {
+	{"mt9v111", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, mt9v111_id);
+
+static struct i2c_driver mt9v111_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "mt9v111",
+		   },
+	.probe = mt9v111_probe,
+	.remove = mt9v111_remove,
+	.id_table = mt9v111_id,
+/* To add power management add .suspend and .resume functions */
+};
+
+/*
+ * Function definitions
+ */
+
+#ifdef MT9V111_DEBUG
+static inline int mt9v111_read_reg(u8 reg)
+{
+	int val = i2c_smbus_read_word_data(mt9v111_data.i2c_client, reg);
+	if (val != -1)
+		val = cpu_to_be16(val);
+	return val;
+}
+#endif
+
+/*!
+ * Writes to the register via I2C.
+ */
+static inline int mt9v111_write_reg(u8 reg, u16 val)
+{
+	pr_debug("In mt9v111_write_reg (0x%x, 0x%x)\n", reg, val);
+	pr_debug("   write reg %x val %x.\n", reg, val);
+
+	return i2c_smbus_write_word_data(mt9v111_data.i2c_client,
+					 reg, cpu_to_be16(val));
+}
+
+/*!
+ * Initialize mt9v111_sensor_lib
+ * Libarary for Sensor configuration through I2C
+ *
+ * @param       coreReg       Core Registers
+ * @param       ifpReg        IFP Register
+ *
+ * @return status
+ */
+static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
+{
+	u8 reg;
+	u16 data;
+	u8 error = 0;
+
+	pr_debug("In mt9v111_sensor_lib\n");
+
+	/*
+	 * setup to IFP registers
+	 */
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_MODE_CONTROL;
+	data = ifpReg->modeControl;
+	mt9v111_write_reg(reg, data);
+
+	/* Output format */
+	reg = MT9V111I_FORMAT_CONTROL;
+	data = ifpReg->formatControl;	/* Set bit 12 */
+	mt9v111_write_reg(reg, data);
+
+	/* AE limit 4 */
+	reg = MT9V111I_SHUTTER_WIDTH_LIMIT_AE;
+	data = ifpReg->gainLimitAE;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_OUTPUT_FORMAT_CTRL2;
+	data = ifpReg->outputFormatCtrl2;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_AE_SPEED;
+	data = ifpReg->AESpeed;
+	mt9v111_write_reg(reg, data);
+
+	/* output image size */
+	reg = MT9V111i_H_PAN;
+	data = 0x8000 | ifpReg->HPan;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_ZOOM;
+	data = 0x8000 | ifpReg->HZoom;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_SIZE;
+	data = 0x8000 | ifpReg->HSize;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_PAN;
+	data = 0x8000 | ifpReg->VPan;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_ZOOM;
+	data = 0x8000 | ifpReg->VZoom;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_V_SIZE;
+	data = 0x8000 | ifpReg->VSize;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111i_H_PAN;
+	data = ~0x8000 & ifpReg->HPan;
+	mt9v111_write_reg(reg, data);
+#if 0
+	reg = MT9V111I_UPPER_SHUTTER_DELAY_LIM;
+	data = ifpReg->upperShutterDelayLi;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_SHUTTER_60;
+	data = ifpReg->shutter_width_60;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_SEARCH_FLICK_60;
+	data = ifpReg->search_flicker_60;
+	mt9v111_write_reg(reg, data);
+#endif
+
+	/*
+	 * setup to sensor core registers
+	 */
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = coreReg->addressSelect;
+	mt9v111_write_reg(reg, data);
+
+	/* enable changes and put the Sync bit on */
+	reg = MT9V111S_OUTPUT_CTRL;
+	data = MT9V111S_OUTCTRL_SYNC | MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
+	mt9v111_write_reg(reg, data);
+
+	/* min PIXCLK - Default */
+	reg = MT9V111S_PIXEL_CLOCK_SPEED;
+	data = coreReg->pixelClockSpeed;
+	mt9v111_write_reg(reg, data);
+
+	/* Setup image flipping / Dark rows / row/column skip */
+	reg = MT9V111S_READ_MODE;
+	data = coreReg->readMode;
+	mt9v111_write_reg(reg, data);
+
+	/* zoom 0 */
+	reg = MT9V111S_DIGITAL_ZOOM;
+	data = coreReg->digitalZoom;
+	mt9v111_write_reg(reg, data);
+
+	/* min H-blank */
+	reg = MT9V111S_HOR_BLANKING;
+	data = coreReg->horizontalBlanking;
+	mt9v111_write_reg(reg, data);
+
+	/* min V-blank */
+	reg = MT9V111S_VER_BLANKING;
+	data = coreReg->verticalBlanking;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111S_SHUTTER_WIDTH;
+	data = coreReg->shutterWidth;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111S_SHUTTER_DELAY;
+	data = ifpReg->upperShutterDelayLi;
+	mt9v111_write_reg(reg, data);
+
+	/* changes become effective */
+	reg = MT9V111S_OUTPUT_CTRL;
+	data = MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
+	mt9v111_write_reg(reg, data);
+
+	return error;
+}
+
+/*!
+ * MT9V111 frame rate calculate
+ *
+ * @param frame_rate       int *
+ * @param mclk             int
+ * @return  None
+ */
+static void mt9v111_rate_cal(int *frame_rate, int mclk)
+{
+	int num_clock_per_row;
+	int max_rate = 0;
+
+	pr_debug("In mt9v111_rate_cal\n");
+
+	num_clock_per_row = (MT9V111_MAX_WIDTH + 114 + MT9V111_HORZBLANK_MIN)
+			* 2;
+	max_rate = mclk / (num_clock_per_row *
+			   (MT9V111_MAX_HEIGHT + MT9V111_VERTBLANK_DEFAULT));
+
+	if ((*frame_rate > max_rate) || (*frame_rate == 0)) {
+		*frame_rate = max_rate;
+	}
+
+	mt9v111_device.coreReg->verticalBlanking
+	    = mclk / (*frame_rate * num_clock_per_row) - MT9V111_MAX_HEIGHT;
+
+	reset_frame_rate = *frame_rate;
+}
+
+/*!
+ * MT9V111 sensor configuration
+ */
+void mt9v111_config(void)
+{
+	pr_debug("In mt9v111_config\n");
+
+	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
+	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
+
+	mt9v111_device.coreReg->windowHeight = MT9V111_WINHEIGHT;
+	mt9v111_device.coreReg->windowWidth = MT9V111_WINWIDTH;
+	mt9v111_device.coreReg->zoomColStart = 0;
+	mt9v111_device.coreReg->zomRowStart = 0;
+	mt9v111_device.coreReg->digitalZoom = 0x0;
+
+	mt9v111_device.coreReg->verticalBlanking = MT9V111_VERTBLANK_DEFAULT;
+	mt9v111_device.coreReg->horizontalBlanking = MT9V111_HORZBLANK_MIN;
+	mt9v111_device.coreReg->pixelClockSpeed = 0;
+	mt9v111_device.coreReg->readMode = 0xd0a1;
+
+	mt9v111_device.ifpReg->outputFormatCtrl2 = 0;
+	mt9v111_device.ifpReg->gainLimitAE = 0x300;
+	mt9v111_device.ifpReg->AESpeed = 0x80;
+
+	/* here is the default value */
+	mt9v111_device.ifpReg->formatControl = 0xc800;
+	mt9v111_device.ifpReg->modeControl = 0x708e;
+	mt9v111_device.ifpReg->awbSpeed = 0x4514;
+	mt9v111_device.coreReg->shutterWidth = 0xf8;
+
+	/* output size */
+	mt9v111_device.ifpReg->HPan = 0;
+	mt9v111_device.ifpReg->HZoom = MT9V111_MAX_WIDTH;
+	mt9v111_device.ifpReg->HSize = MT9V111_MAX_WIDTH;
+	mt9v111_device.ifpReg->VPan = 0;
+	mt9v111_device.ifpReg->VZoom = MT9V111_MAX_HEIGHT;
+	mt9v111_device.ifpReg->VSize = MT9V111_MAX_HEIGHT;
+}
+
+/*!
+ * mt9v111 sensor set saturtionn
+ *
+ * @param saturation   int
+
+ * @return  Error code of 0.
+ */
+static int mt9v111_set_saturation(int saturation)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_saturation(%d)\n",
+		saturation);
+
+	switch (saturation) {
+	case 150:
+		mt9v111_device.ifpReg->awbSpeed = 0x6D14;
+		break;
+	case 100:
+		mt9v111_device.ifpReg->awbSpeed = 0x4514;
+		break;
+	case 75:
+		mt9v111_device.ifpReg->awbSpeed = 0x4D14;
+		break;
+	case 50:
+		mt9v111_device.ifpReg->awbSpeed = 0x5514;
+		break;
+	case 37:
+		mt9v111_device.ifpReg->awbSpeed = 0x5D14;
+		break;
+	case 25:
+		mt9v111_device.ifpReg->awbSpeed = 0x6514;
+		break;
+	default:
+		mt9v111_device.ifpReg->awbSpeed = 0x4514;
+		break;
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_AWB_SPEED;
+	data = mt9v111_device.ifpReg->awbSpeed;
+	mt9v111_write_reg(reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set Auto Exposure measurement window mode configuration
+ *
+ * @param ae_mode      int
+ * @return  Error code of 0 (no Error)
+ */
+static int mt9v111_set_ae_mode(int ae_mode)
+{
+	u8 reg;
+	u16 data;
+
+	pr_debug("In mt9v111_set_ae_mode(%d)\n",
+		ae_mode);
+
+	/* Currently this driver only supports auto and manual exposure
+	 * modes. */
+	if ((ae_mode > 1) || (ae_mode << 0))
+		return -EPERM;
+
+	/*
+	 * The auto exposure is set in bit 14.
+	 * Other values are set for:
+	 *  -on the fly defect correction is on (bit 13).
+	 *  -aperature correction knee enabled (bit 12).
+	 *  -ITU_R BT656 synchronization codes are embedded in the image (bit 7)
+	 *  -AE measurement window is weighted sum of large and center windows
+	 *     (bits 2-3).
+	 *  -auto white balance is on (bit 1).
+	 *  -normal color processing (bit 4 = 0).
+	 */
+	/* V4L2_EXPOSURE_AUTO = 0; needs register setting of 0x708E */
+	/* V4L2_EXPOSURE_MANUAL = 1 needs register setting of 0x308E */
+	mt9v111_device.ifpReg->modeControl &= 0x3fff;
+	mt9v111_device.ifpReg->modeControl |= (ae_mode & 0x03) << 14;
+	mt9v111_data.ae_mode = ae_mode;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(reg, data);
+
+	reg = MT9V111I_MODE_CONTROL;
+	data = mt9v111_device.ifpReg->modeControl;
+	mt9v111_write_reg(reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor get AE measurement window mode configuration
+ *
+ * @param ae_mode      int *
+ * @return  None
+ */
+static void mt9v111_get_ae_mode(int *ae_mode)
+{
+	pr_debug("In mt9v111_get_ae_mode(%d)\n", *ae_mode);
+
+	if (ae_mode != NULL) {
+		*ae_mode = (mt9v111_device.ifpReg->modeControl & 0xc) >> 2;
+	}
+}
+
+#ifdef MT9V111_DEBUG
+/*!
+ * Set sensor to test mode, which will generate test pattern.
+ *
+ * @return none
+ */
+static void mt9v111_test_pattern(bool flag)
+{
+	u16 data;
+
+	/* switch to sensor registers */
+	mt9v111_write_reg(MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
+
+	if (flag == true) {
+		testpattern = MT9V111S_OUTCTRL_TEST_MODE;
+
+		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) & 0xBF;
+		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+
+		mt9v111_write_reg(MT9V111S_TEST_DATA, 0);
+
+		/* changes take effect */
+		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
+		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+	} else {
+		testpattern = 0;
+
+		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) | 0x40;
+		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+
+		/* changes take effect */
+		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
+		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+	}
+}
+#endif
+
+
+/* --------------- IOCTL functions from v4l2_int_ioctl_desc --------------- */
+
+/*!
+ * ioctl_g_ifparm - V4L2 sensor interface handler for vidioc_int_g_ifparm_num
+ * s: pointer to standard V4L2 device structure
+ * p: pointer to standard V4L2 vidioc_int_g_ifparm_num ioctl structure
+ *
+ * Gets slave interface parameters.
+ * Calculates the required xclk value to support the requested
+ * clock parameters in p.  This value is returned in the p
+ * parameter.
+ *
+ * vidioc_int_g_ifparm returns platform-specific information about the
+ * interface settings used by the sensor.
+ *
+ * Given the image capture format in pix, the nominal frame period in
+ * timeperframe, calculate the required xclk frequency.
+ *
+ * Called on open.
+ */
+static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
+{
+	pr_debug("In mt9v111:ioctl_g_ifparm\n");
+
+	if (s == NULL) {
+		pr_err("   ERROR!! no slave device set!\n");
+		return -1;
+	}
+
+	memset(p, 0, sizeof(*p));
+	p->u.bt656.clock_curr = MT9V111_MCLK;
+	p->if_type = V4L2_IF_TYPE_BT656;
+	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.clock_min = MT9V111_CLK_MIN;
+	p->u.bt656.clock_max = MT9V111_CLK_MAX;
+
+	return 0;
+}
+
+/*!
+ * Sets the camera power.
+ *
+ * s  pointer to the camera device
+ * on if 1, power is to be turned on.  0 means power is to be turned off
+ *
+ * ioctl_s_power - V4L2 sensor interface handler for vidioc_int_s_power_num
+ * @s: pointer to standard V4L2 device structure
+ * @on: power state to which device is to be set
+ *
+ * Sets devices power state to requrested state, if possible.
+ * This is called on suspend and resume.
+ */
+static int ioctl_s_power(struct v4l2_int_device *s, int on)
+{
+	struct sensor *sensor = s->priv;
+
+	pr_debug("In mt9v111:ioctl_s_power\n");
+
+	sensor->on = on;
+
+	if (on)
+		gpio_sensor_active();
+	else
+		gpio_sensor_inactive();
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_parm - V4L2 sensor interface handler for VIDIOC_G_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the sensor's video CAPTURE parameters.
+ */
+static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	int ret = 0;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	/* s->priv points to mt9v111_data */
+
+	pr_debug("In mt9v111:ioctl_g_parm\n");
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = mt9v111_data.streamcap.capability;
+		cparm->timeperframe =
+				mt9v111_data.streamcap.timeperframe;
+		cparm->capturemode = mt9v111_data.streamcap.capturemode;
+		ret = 0;
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_err("   type is not V4L2_BUF_TYPE_VIDEO_CAPTURE " \
+			"but %d\n", a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_err("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ */
+static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	int ret = 0;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	/* s->priv points to mt9v111_data */
+
+	pr_debug("In mt9v111:ioctl_s_parm\n");
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+
+		/* Check that the new frame rate is allowed.
+		 * Changing the frame rate is not allowed on this
+		 *camera. */
+		if (cparm->timeperframe.denominator !=
+		    mt9v111_data.streamcap.timeperframe.denominator) {
+			pr_err("ERROR: mt9v111: ioctl_s_parm: " \
+			       "This camera does not allow frame rate "
+			       "changes.\n");
+			ret = -EINVAL;
+		} else {
+			mt9v111_data.streamcap.timeperframe =
+						cparm->timeperframe;
+		      /* Call any camera functions to match settings. */
+		}
+
+		/* Check that new capture mode is supported. */
+		if ((cparm->capturemode != 0) &&
+		    !(cparm->capturemode & V4L2_MODE_HIGHQUALITY)) {
+			pr_err("ERROR: mt9v111: ioctl_s_parm: " \
+				"unsupported capture mode\n");
+			ret  = -EINVAL;
+		} else {
+			mt9v111_data.streamcap.capturemode =
+						cparm->capturemode;
+		      /* Call any camera functions to match settings. */
+		      /* Right now this camera only supports 1 mode. */
+		}
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_err("   type is not V4L2_BUF_TYPE_VIDEO_CAPTURE " \
+			"but %d\n", a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_err("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_fmt_cap - V4L2 sensor interface handler for ioctl_g_fmt_cap
+ * @s: pointer to standard V4L2 device structure
+ * @f: pointer to standard V4L2 v4l2_format structure
+ *
+ * Returns the sensor's current pixel format in the v4l2_format
+ * parameter.
+ */
+static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+	/* s->priv points to mt9v111_data */
+
+	pr_debug("In mt9v111:ioctl_g_fmt_cap.\n");
+	pr_debug("   Returning size of %dx%d\n",
+		sensor->pix.width, sensor->pix.height);
+
+	f->fmt.pix = sensor->pix;
+
+	return 0;
+}
+
+/*!
+ * ioctl_queryctrl - V4L2 sensor interface handler for VIDIOC_QUERYCTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @qc: standard V4L2 VIDIOC_QUERYCTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control information
+ * from the video_control[] array.  Otherwise, returns -EINVAL if the
+ * control is not supported.
+ */
+static int ioctl_queryctrl(struct v4l2_int_device *s, struct v4l2_queryctrl *qc)
+{
+	pr_debug("In mt9v111:ioctl_queryctrl\n");
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_G_CTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control's current
+ * value from the video_control[] array.  Otherwise, returns -EINVAL
+ * if the control is not supported.
+ */
+static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	pr_debug("In mt9v111:ioctl_g_ctrl\n");
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		pr_debug("   V4L2_CID_BRIGHTNESS\n");
+		vc->value = mt9v111_data.brightness;
+		break;
+	case V4L2_CID_CONTRAST:
+		pr_debug("   V4L2_CID_CONTRAST\n");
+		vc->value = mt9v111_data.contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		pr_debug("   V4L2_CID_SATURATION\n");
+		vc->value = mt9v111_data.saturation;
+		break;
+	case V4L2_CID_HUE:
+		pr_debug("   V4L2_CID_HUE\n");
+		vc->value = mt9v111_data.hue;
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		pr_debug("   V4L2_CID_RED_BALANCE\n");
+		vc->value = mt9v111_data.red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
+		vc->value = mt9v111_data.blue;
+		break;
+	case V4L2_CID_GAMMA:
+		pr_debug("   V4L2_CID_GAMMA\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_EXPOSURE:
+		pr_debug("   V4L2_CID_EXPOSURE\n");
+		vc->value = mt9v111_data.ae_mode;
+		break;
+	case V4L2_CID_AUTOGAIN:
+		pr_debug("   V4L2_CID_AUTOGAIN\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_GAIN:
+		pr_debug("   V4L2_CID_GAIN\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_HFLIP:
+		pr_debug("   V4L2_CID_HFLIP\n");
+		vc->value = 0;
+		break;
+	case V4L2_CID_VFLIP:
+		pr_debug("   V4L2_CID_VFLIP\n");
+		vc->value = 0;
+		break;
+	default:
+		pr_debug("   Default case\n");
+		return -EPERM;
+		break;
+	}
+
+	return 0;
+}
+
+/*!
+ * ioctl_s_ctrl - V4L2 sensor interface handler for VIDIOC_S_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_S_CTRL ioctl structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW (and updates the video_control[] array).  Otherwise,
+ * returns -EINVAL if the control is not supported.
+ */
+static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int retval = 0;
+
+	pr_debug("In mt9v111:ioctl_s_ctrl %d\n",
+		vc->id);
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		pr_debug("   V4L2_CID_BRIGHTNESS\n");
+		break;
+	case V4L2_CID_CONTRAST:
+		pr_debug("   V4L2_CID_CONTRAST\n");
+		break;
+	case V4L2_CID_SATURATION:
+		pr_debug("   V4L2_CID_SATURATION\n");
+		retval = mt9v111_set_saturation(vc->value);
+		break;
+	case V4L2_CID_HUE:
+		pr_debug("   V4L2_CID_HUE\n");
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_RED_BALANCE:
+		pr_debug("   V4L2_CID_RED_BALANCE\n");
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
+		break;
+	case V4L2_CID_GAMMA:
+		pr_debug("   V4L2_CID_GAMMA\n");
+		break;
+	case V4L2_CID_EXPOSURE:
+		pr_debug("   V4L2_CID_EXPOSURE\n");
+		retval = mt9v111_set_ae_mode(vc->value);
+		break;
+	case V4L2_CID_AUTOGAIN:
+		pr_debug("   V4L2_CID_AUTOGAIN\n");
+		break;
+	case V4L2_CID_GAIN:
+		pr_debug("   V4L2_CID_GAIN\n");
+		break;
+	case V4L2_CID_HFLIP:
+		pr_debug("   V4L2_CID_HFLIP\n");
+		break;
+	case V4L2_CID_VFLIP:
+		pr_debug("   V4L2_CID_VFLIP\n");
+		break;
+	default:
+		pr_debug("   Default case\n");
+		retval = -EPERM;
+		break;
+	}
+
+	return retval;
+}
+
+/*!
+ * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
+ * @s: pointer to standard V4L2 device structure
+ */
+static int ioctl_init(struct v4l2_int_device *s)
+{
+	pr_debug("In mt9v111:ioctl_init\n");
+
+	return 0;
+}
+
+/*!
+ * ioctl_dev_init - V4L2 sensor interface handler for vidioc_int_dev_init_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Initialise the device when slave attaches to the master.
+ */
+static int ioctl_dev_init(struct v4l2_int_device *s)
+{
+	uint32_t clock_rate = MT9V111_MCLK;
+
+	pr_debug("In mt9v111:ioctl_dev_init\n");
+
+	gpio_sensor_active();
+
+	set_mclk_rate(&clock_rate);
+	mt9v111_rate_cal(&reset_frame_rate, clock_rate);
+	mt9v111_sensor_lib(mt9v111_device.coreReg, mt9v111_device.ifpReg);
+
+	return 0;
+}
+
+/*!
+ * This structure defines all the ioctls for this module and links them to the
+ * enumeration.
+ */
+static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
+
+	{vidioc_int_dev_init_num, (v4l2_int_ioctl_func *)ioctl_dev_init},
+
+	/*!
+	 * Delinitialise the dev. at slave detach.
+	 * The complement of ioctl_dev_init.
+	 */
+/*	{vidioc_int_dev_exit_num, (v4l2_int_ioctl_func *) ioctl_dev_exit}, */
+
+	{vidioc_int_s_power_num, (v4l2_int_ioctl_func *) ioctl_s_power},
+	{vidioc_int_g_ifparm_num, (v4l2_int_ioctl_func *) ioctl_g_ifparm},
+/*	{vidioc_int_g_needs_reset_num,
+				(v4l2_int_ioctl_func *) ioctl_g_needs_reset}, */
+/*	{vidioc_int_reset_num, (v4l2_int_ioctl_func *) ioctl_reset}, */
+	{vidioc_int_init_num, (v4l2_int_ioctl_func *) ioctl_init},
+
+	/*!
+	 * VIDIOC_ENUM_FMT ioctl for the CAPTURE buffer type.
+	 */
+/*	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *) ioctl_enum_fmt_cap}, */
+
+	/*!
+	 * VIDIOC_TRY_FMT ioctl for the CAPTURE buffer type.
+	 * This ioctl is used to negotiate the image capture size and
+	 * pixel format without actually making it take effect.
+	 */
+/*	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *) ioctl_try_fmt_cap}, */
+
+	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *) ioctl_g_fmt_cap},
+
+	/*!
+	 * If the requested format is supported, configures the HW to use that
+	 * format, returns error code if format not supported or HW can't be
+	 * correctly configured.
+	 */
+/*	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap}, */
+
+	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *) ioctl_g_parm},
+	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *) ioctl_s_parm},
+/*	{vidioc_int_queryctrl_num, (v4l2_int_ioctl_func *) ioctl_queryctrl}, */
+	{vidioc_int_g_ctrl_num, (v4l2_int_ioctl_func *) ioctl_g_ctrl},
+	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *) ioctl_s_ctrl},
+};
+
+static struct v4l2_int_slave mt9v111_slave = {
+	.ioctls = mt9v111_ioctl_desc,
+	.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+};
+
+static struct v4l2_int_device mt9v111_int_device = {
+	.module = THIS_MODULE,
+	.name = "mt9v111",
+	.type = v4l2_int_type_slave,
+	.u = {
+		.slave = &mt9v111_slave,
+		},
+};
+
+/*!
+ * mt9v111 I2C probe function
+ * Function set in i2c_driver struct.
+ * Called by insmod mt9v111_camera.ko.
+ *
+ * @return  Error code indicating success or failure
+ */
+static int mt9v111_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	int retval;
+
+	pr_debug("In mt9v111_probe  device id is %s\n", id->name);
+
+	/* Set initial values for the sensor struct. */
+	memset(&mt9v111_data, 0, sizeof(mt9v111_data));
+	mt9v111_data.i2c_client = client;
+	pr_debug("   client name is %s\n", client->name);
+	mt9v111_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	mt9v111_data.pix.width = MT9V111_MAX_WIDTH;
+	mt9v111_data.pix.height = MT9V111_MAX_HEIGHT;
+	mt9v111_data.streamcap.capability = 0; /* No higher resolution or frame
+						* frame rate changes supported.
+						*/
+	mt9v111_data.streamcap.timeperframe.denominator = MT9V111_FRAME_RATE;
+	mt9v111_data.streamcap.timeperframe.numerator = 1;
+
+	mt9v111_int_device.priv = &mt9v111_data;
+
+	pr_debug("   type is %d (expect %d)\n",
+		mt9v111_int_device.type, v4l2_int_type_slave);
+	pr_debug("   num ioctls is %d\n",
+		mt9v111_int_device.u.slave->num_ioctls);
+
+	/* This function attaches this structure to the /dev/video0 device.
+	 * The pointer in priv points to the mt9v111_data structure here.*/
+	retval = v4l2_int_device_register(&mt9v111_int_device);
+
+	return retval;
+}
+
+/*!
+ * Function set in i2c_driver struct.
+ * Called on rmmod mt9v111_camera.ko
+ */
+static int mt9v111_remove(struct i2c_client *client)
+{
+	pr_debug("In mt9v111_remove\n");
+
+	v4l2_int_device_unregister(&mt9v111_int_device);
+	return 0;
+}
+
+/*!
+ * MT9V111 init function.
+ * Called by insmod mt9v111_camera.ko.
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int mt9v111_init(void)
+{
+	u8 err;
+
+	pr_debug("In mt9v111_init\n");
+
+	/* Allocate memory for state structures. */
+	mt9v111_device.coreReg = (mt9v111_coreReg *)
+				kmalloc(sizeof(mt9v111_coreReg), GFP_KERNEL);
+	if (!mt9v111_device.coreReg)
+		return -1;
+	memset(mt9v111_device.coreReg, 0, sizeof(mt9v111_coreReg));
+
+	mt9v111_device.ifpReg = (mt9v111_IFPReg *)
+				kmalloc(sizeof(mt9v111_IFPReg), GFP_KERNEL);
+	if (!mt9v111_device.ifpReg) {
+		kfree(mt9v111_device.coreReg);
+		mt9v111_device.coreReg = NULL;
+		return -1;
+	}
+	memset(mt9v111_device.ifpReg, 0, sizeof(mt9v111_IFPReg));
+
+	/* Set contents of the just created structures. */
+	mt9v111_config();
+
+	/* Tells the i2c driver what functions to call for this driver. */
+	err = i2c_add_driver(&mt9v111_i2c_driver);
+	if (err != 0)
+		pr_err("%s:driver registration failed, error=%d \n",
+		       __func__, err);
+
+	return err;
+}
+
+/*!
+ * MT9V111 cleanup function.
+ * Called on rmmod mt9v111_camera.ko
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit mt9v111_clean(void)
+{
+	pr_debug("In mt9v111_clean()\n");
+
+	i2c_del_driver(&mt9v111_i2c_driver);
+	gpio_sensor_inactive();
+
+	if (mt9v111_device.coreReg) {
+		kfree(mt9v111_device.coreReg);
+		mt9v111_device.coreReg = NULL;
+	}
+
+	if (mt9v111_device.ifpReg) {
+		kfree(mt9v111_device.ifpReg);
+		mt9v111_device.ifpReg = NULL;
+	}
+}
+
+module_init(mt9v111_init);
+module_exit(mt9v111_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Mt9v111 Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/mt9v111.h b/drivers/media/video/mxc/capture/mt9v111.h
new file mode 100644
index 000000000000..cf38cec4757c
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mt9v111.h
@@ -0,0 +1,431 @@
+/*
+ * Copyright 2004-2008 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup Camera Sensor Drivers
+ */
+
+/*!
+ * @file mt9v111.h
+ *
+ * @brief MT9V111 Camera Header file
+ *
+ * This header file contains defines and structures for the iMagic mi8012
+ * aka the Micron mt9v111 camera.
+ *
+ * @ingroup Camera
+ */
+
+#ifndef MT9V111_H_
+#define MT9V111_H_
+
+/*!
+ * Basic camera values
+ */
+#define MT9V111_FRAME_RATE        30
+#define MT9V111_MCLK              27000000 /* Desired clock rate */
+#define MT9V111_CLK_MIN           12000000 /* This clock rate yields 15 fps */
+#define MT9V111_CLK_MAX           27000000
+#define MT9V111_MAX_WIDTH         640      /* Max width for this camera */
+#define MT9V111_MAX_HEIGHT        480      /* Max height for this camera */
+
+/*!
+ * mt9v111 IFP REGISTER BANK MAP
+ */
+#define MT9V111I_ADDR_SPACE_SEL           0x1
+#define MT9V111I_BASE_MAXTRIX_SIGN        0x2
+#define MT9V111I_BASE_MAXTRIX_SCALE15     0x3
+#define MT9V111I_BASE_MAXTRIX_SCALE69     0x4
+#define MT9V111I_APERTURE_GAIN            0x5
+#define MT9V111I_MODE_CONTROL             0x6
+#define MT9V111I_SOFT_RESET               0x7
+#define MT9V111I_FORMAT_CONTROL           0x8
+#define MT9V111I_BASE_MATRIX_CFK1         0x9
+#define MT9V111I_BASE_MATRIX_CFK2         0xa
+#define MT9V111I_BASE_MATRIX_CFK3         0xb
+#define MT9V111I_BASE_MATRIX_CFK4         0xc
+#define MT9V111I_BASE_MATRIX_CFK5         0xd
+#define MT9V111I_BASE_MATRIX_CFK6         0xe
+#define MT9V111I_BASE_MATRIX_CFK7         0xf
+#define MT9V111I_BASE_MATRIX_CFK8         0x10
+#define MT9V111I_BASE_MATRIX_CFK9         0x11
+#define MT9V111I_AWB_POSITION             0x12
+#define MT9V111I_AWB_RED_GAIN             0x13
+#define MT9V111I_AWB_BLUE_GAIN            0x14
+#define MT9V111I_DELTA_MATRIX_CF_SIGN     0x15
+#define MT9V111I_DELTA_MATRIX_CF_D1       0x16
+#define MT9V111I_DELTA_MATRIX_CF_D2       0x17
+#define MT9V111I_DELTA_MATRIX_CF_D3       0x18
+#define MT9V111I_DELTA_MATRIX_CF_D4       0x19
+#define MT9V111I_DELTA_MATRIX_CF_D5       0x1a
+#define MT9V111I_DELTA_MATRIX_CF_D6       0x1b
+#define MT9V111I_DELTA_MATRIX_CF_D7       0x1c
+#define MT9V111I_DELTA_MATRIX_CF_D8       0x1d
+#define MT9V111I_DELTA_MATRIX_CF_D9       0x1e
+#define MT9V111I_LUMINANCE_LIMIT_WB       0x20
+#define MT9V111I_RBG_MANUUAL_WB           0x21
+#define MT9V111I_AWB_RED_LIMIT            0x22
+#define MT9V111I_AWB_BLUE_LIMIT           0x23
+#define MT9V111I_MATRIX_ADJUST_LIMIT      0x24
+#define MT9V111I_AWB_SPEED                0x25
+#define MT9V111I_H_BOUND_AE               0x26
+#define MT9V111I_V_BOUND_AE               0x27
+#define MT9V111I_H_BOUND_AE_CEN_WIN       0x2b
+#define MT9V111I_V_BOUND_AE_CEN_WIN       0x2c
+#define MT9V111I_BOUND_AWB_WIN            0x2d
+#define MT9V111I_AE_PRECISION_TARGET      0x2e
+#define MT9V111I_AE_SPEED                 0x2f
+#define MT9V111I_RED_AWB_MEASURE          0x30
+#define MT9V111I_LUMA_AWB_MEASURE         0x31
+#define MT9V111I_BLUE_AWB_MEASURE         0x32
+#define MT9V111I_LIMIT_SHARP_SATU_CTRL    0x33
+#define MT9V111I_LUMA_OFFSET              0x34
+#define MT9V111I_CLIP_LIMIT_OUTPUT_LUMI   0x35
+#define MT9V111I_GAIN_LIMIT_AE            0x36
+#define MT9V111I_SHUTTER_WIDTH_LIMIT_AE   0x37
+#define MT9V111I_UPPER_SHUTTER_DELAY_LIM  0x39
+#define MT9V111I_OUTPUT_FORMAT_CTRL2      0x3a
+#define MT9V111I_IPF_BLACK_LEVEL_SUB      0x3b
+#define MT9V111I_IPF_BLACK_LEVEL_ADD      0x3c
+#define MT9V111I_ADC_LIMIT_AE_ADJ         0x3d
+#define MT9V111I_GAIN_THRE_CCAM_ADJ       0x3e
+#define MT9V111I_LINEAR_AE                0x3f
+#define MT9V111I_THRESHOLD_EDGE_DEFECT    0x47
+#define MT9V111I_LUMA_SUM_MEASURE         0x4c
+#define MT9V111I_TIME_ADV_SUM_LUMA        0x4d
+#define MT9V111I_MOTION                   0x52
+#define MT9V111I_GAMMA_KNEE_Y12           0x53
+#define MT9V111I_GAMMA_KNEE_Y34           0x54
+#define MT9V111I_GAMMA_KNEE_Y56           0x55
+#define MT9V111I_GAMMA_KNEE_Y78           0x56
+#define MT9V111I_GAMMA_KNEE_Y90           0x57
+#define MT9V111I_GAMMA_VALUE_Y0           0x58
+#define MT9V111I_SHUTTER_60               0x59
+#define MT9V111I_SEARCH_FLICK_60          0x5c
+#define MT9V111I_RATIO_IMAGE_GAIN_BASE    0x5e
+#define MT9V111I_RATIO_IMAGE_GAIN_DELTA   0x5f
+#define MT9V111I_SIGN_VALUE_REG5F         0x60
+#define MT9V111I_AE_GAIN                  0x62
+#define MT9V111I_MAX_GAIN_AE              0x67
+#define MT9V111I_LENS_CORRECT_CTRL        0x80
+#define MT9V111I_SHADING_PARAMETER1       0x81
+#define MT9V111I_SHADING_PARAMETER2       0x82
+#define MT9V111I_SHADING_PARAMETER3       0x83
+#define MT9V111I_SHADING_PARAMETER4       0x84
+#define MT9V111I_SHADING_PARAMETER5       0x85
+#define MT9V111I_SHADING_PARAMETER6       0x86
+#define MT9V111I_SHADING_PARAMETER7       0x87
+#define MT9V111I_SHADING_PARAMETER8       0x88
+#define MT9V111I_SHADING_PARAMETER9       0x89
+#define MT9V111I_SHADING_PARAMETER10      0x8A
+#define MT9V111I_SHADING_PARAMETER11      0x8B
+#define MT9V111I_SHADING_PARAMETER12      0x8C
+#define MT9V111I_SHADING_PARAMETER13      0x8D
+#define MT9V111I_SHADING_PARAMETER14      0x8E
+#define MT9V111I_SHADING_PARAMETER15      0x8F
+#define MT9V111I_SHADING_PARAMETER16      0x90
+#define MT9V111I_SHADING_PARAMETER17      0x91
+#define MT9V111I_SHADING_PARAMETER18      0x92
+#define MT9V111I_SHADING_PARAMETER19      0x93
+#define MT9V111I_SHADING_PARAMETER20      0x94
+#define MT9V111I_SHADING_PARAMETER21      0x95
+#define MT9V111i_FLASH_CTRL               0x98
+#define MT9V111i_LINE_COUNTER             0x99
+#define MT9V111i_FRAME_COUNTER            0x9A
+#define MT9V111i_H_PAN                    0xA5
+#define MT9V111i_H_ZOOM                   0xA6
+#define MT9V111i_H_SIZE                   0xA7
+#define MT9V111i_V_PAN                    0xA8
+#define MT9V111i_V_ZOOM                   0xA9
+#define MT9V111i_V_SIZE                   0xAA
+
+#define MT9V111I_SEL_IFP                  0x1
+#define MT9V111I_SEL_SCA                  0x4
+#define MT9V111I_FC_RGB_OR_YUV            0x1000
+
+/*!
+ * Mt9v111 SENSOR CORE REGISTER BANK MAP
+ */
+#define MT9V111S_ADDR_SPACE_SEL           0x1
+#define MT9V111S_COLUMN_START             0x2
+#define MT9V111S_WIN_HEIGHT               0x3
+#define MT9V111S_WIN_WIDTH                0x4
+#define MT9V111S_HOR_BLANKING             0x5
+#define MT9V111S_VER_BLANKING             0x6
+#define MT9V111S_OUTPUT_CTRL              0x7
+#define MT9V111S_ROW_START                0x8
+#define MT9V111S_SHUTTER_WIDTH            0x9
+#define MT9V111S_PIXEL_CLOCK_SPEED        0xa
+#define MT9V111S_RESTART                  0xb
+#define MT9V111S_SHUTTER_DELAY            0xc
+#define MT9V111S_RESET                    0xd
+#define MT9V111S_COLUMN_START_IN_ZOOM     0x12
+#define MT9V111S_ROW_START_IN_ZOOM        0x13
+#define MT9V111S_DIGITAL_ZOOM             0x1e
+#define MT9V111S_READ_MODE                0x20
+#define MT9V111S_DAC_CTRL                 0x27
+#define MT9V111S_GREEN1_GAIN              0x2b
+#define MT9V111S_BLUE_GAIN                0x2c
+#define MT9V111S_READ_GAIN                0x2d
+#define MT9V111S_GREEN2_GAIN              0x2e
+#define MT9V111S_ROW_NOISE_CTRL           0x30
+#define MT9V111S_DARK_TARGET_W            0x31
+#define MT9V111S_TEST_DATA                0x32
+#define MT9V111S_GLOBAL_GAIN              0x35
+#define MT9V111S_SENSOR_CORE_VERSION      0x36
+#define MT9V111S_DARK_TARGET_WO           0x37
+#define MT9V111S_VERF_DAC                 0x41
+#define MT9V111S_VCM_VCL                  0x42
+#define MT9V111S_DISABLE_BYPASS           0x58
+#define MT9V111S_CALIB_MEAN_TEST          0x59
+#define MT9V111S_DARK_G1_AVE              0x5B
+#define MT9V111S_DARK_G2_AVE              0x5C
+#define MT9V111S_DARK_R_AVE               0x5D
+#define MT9V111S_DARK_B_AVE               0x5E
+#define MT9V111S_CAL_THRESHOLD            0x5f
+#define MT9V111S_CAL_G1                   0x60
+#define MT9V111S_CAL_G2                   0x61
+#define MT9V111S_CAL_CTRL                 0x62
+#define MT9V111S_CAL_R                    0x63
+#define MT9V111S_CAL_B                    0x64
+#define MT9V111S_CHIP_ENABLE              0xF1
+#define MT9V111S_CHIP_VERSION             0xFF
+
+/* OUTPUT_CTRL */
+#define MT9V111S_OUTCTRL_SYNC             0x1
+#define MT9V111S_OUTCTRL_CHIP_ENABLE      0x2
+#define MT9V111S_OUTCTRL_TEST_MODE        0x40
+
+/* READ_MODE */
+#define MT9V111S_RM_NOBADFRAME            0x1
+#define MT9V111S_RM_NODESTRUCT            0x2
+#define MT9V111S_RM_COLUMNSKIP            0x4
+#define MT9V111S_RM_ROWSKIP               0x8
+#define MT9V111S_RM_BOOSTEDRESET          0x1000
+#define MT9V111S_RM_COLUMN_LATE           0x10
+#define MT9V111S_RM_ROW_LATE              0x80
+#define MT9V111S_RM_RIGTH_TO_LEFT         0x4000
+#define MT9V111S_RM_BOTTOM_TO_TOP         0x8000
+
+/*! I2C Slave Address */
+#define MT9V111_I2C_ADDRESS	0x48
+
+/*!
+ * The image resolution enum for the mt9v111 sensor
+ */
+typedef enum {
+	MT9V111_OutputResolution_VGA = 0,	/*!< VGA size */
+	MT9V111_OutputResolution_QVGA,	/*!< QVGA size */
+	MT9V111_OutputResolution_CIF,	/*!< CIF size */
+	MT9V111_OutputResolution_QCIF,	/*!< QCIF size */
+	MT9V111_OutputResolution_QQVGA,	/*!< QQVGA size */
+	MT9V111_OutputResolution_SXGA	/*!< SXGA size */
+} MT9V111_OutputResolution;
+
+enum {
+	MT9V111_WINWIDTH = 0x287,
+	MT9V111_WINWIDTH_DEFAULT = 0x287,
+	MT9V111_WINWIDTH_MIN = 0x9,
+
+	MT9V111_WINHEIGHT = 0x1E7,
+	MT9V111_WINHEIGHT_DEFAULT = 0x1E7,
+
+	MT9V111_HORZBLANK_DEFAULT = 0x26,
+	MT9V111_HORZBLANK_MIN = 0x9,
+	MT9V111_HORZBLANK_MAX = 0x3FF,
+
+	MT9V111_VERTBLANK_DEFAULT = 0x4,
+	MT9V111_VERTBLANK_MIN = 0x3,
+	MT9V111_VERTBLANK_MAX = 0xFFF,
+};
+
+/*!
+ * Mt9v111 Core Register structure.
+ */
+typedef struct {
+	u32 addressSelect;	/*!< select address bank for Core Register 0x4 */
+	u32 columnStart;	/*!< Starting Column */
+	u32 windowHeight;	/*!< Window Height */
+	u32 windowWidth;	/*!< Window Width */
+	u32 horizontalBlanking;	/*!< Horizontal Blank time, in pixels */
+	u32 verticalBlanking;	/*!< Vertical Blank time, in pixels */
+	u32 outputControl;	/*!< Register to control sensor output */
+	u32 rowStart;		/*!< Starting Row */
+	u32 shutterWidth;
+	u32 pixelClockSpeed;	/*!< pixel date rate */
+	u32 restart;		/*!< Abandon the readout of current frame */
+	u32 shutterDelay;
+	u32 reset;		/*!< reset the sensor to the default mode */
+	u32 zoomColStart;	/*!< Column start in the Zoom mode */
+	u32 zomRowStart;	/*!< Row start in the Zoom mode */
+	u32 digitalZoom;	/*!< 1 means zoom by 2 */
+	u32 readMode;		/*!< Readmode: aspects of the readout of the sensor */
+	u32 dACStandbyControl;
+	u32 green1Gain;		/*!< Gain Settings */
+	u32 blueGain;
+	u32 redGain;
+	u32 green2Gain;
+	u32 rowNoiseControl;
+	u32 darkTargetwNC;
+	u32 testData;		/*!< test mode */
+	u32 globalGain;
+	u32 chipVersion;
+	u32 darkTargetwoNC;
+	u32 vREFDACs;
+	u32 vCMandVCL;
+	u32 disableBypass;
+	u32 calibMeanTest;
+	u32 darkG1average;
+	u32 darkG2average;
+	u32 darkRaverage;
+	u32 darkBaverage;
+	u32 calibThreshold;
+	u32 calibGreen1;
+	u32 calibGreen2;
+	u32 calibControl;
+	u32 calibRed;
+	u32 calibBlue;
+	u32 chipEnable;		/*!< Image core Registers written by image flow processor */
+} mt9v111_coreReg;
+
+/*!
+ * Mt9v111 IFP Register structure.
+ */
+typedef struct {
+	u32 addrSpaceSel;	/*!< select address bank for Core Register 0x1 */
+	u32 baseMaxtrixSign;	/*!< sign of coefficient for base color correction matrix */
+	u32 baseMaxtrixScale15;	/*!< scaling of color correction coefficient K1-5 */
+	u32 baseMaxtrixScale69;	/*!< scaling of color correction coefficient K6-9 */
+	u32 apertureGain;	/*!< sharpening */
+	u32 modeControl;	/*!< bit 7 CCIR656 sync codes are embedded in the image */
+	u32 softReset;		/*!< Image processing mode: 1 reset mode, 0 operational mode */
+	u32 formatControl;	/*!< bit12 1 for RGB565, 0 for YcrCb */
+	u32 baseMatrixCfk1;	/*!< K1 Color correction coefficient */
+	u32 baseMatrixCfk2;	/*!< K2 Color correction coefficient */
+	u32 baseMatrixCfk3;	/*!< K3 Color correction coefficient */
+	u32 baseMatrixCfk4;	/*!< K4 Color correction coefficient */
+	u32 baseMatrixCfk5;	/*!< K5 Color correction coefficient */
+	u32 baseMatrixCfk6;	/*!< K6 Color correction coefficient */
+	u32 baseMatrixCfk7;	/*!< K7 Color correction coefficient */
+	u32 baseMatrixCfk8;	/*!< K8 Color correction coefficient */
+	u32 baseMatrixCfk9;	/*!< K9 Color correction coefficient */
+	u32 awbPosition;	/*!< Current position of AWB color correction matrix */
+	u32 awbRedGain;		/*!< Current value of AWB red channel gain */
+	u32 awbBlueGain;	/*!< Current value of AWB blue channel gain */
+	u32 deltaMatrixCFSign;	/*!< Sign of coefficients of delta color correction matrix register */
+	u32 deltaMatrixCFD1;	/*!< D1 Delta coefficient */
+	u32 deltaMatrixCFD2;	/*!< D2 Delta coefficient */
+	u32 deltaMatrixCFD3;	/*!< D3 Delta coefficient */
+	u32 deltaMatrixCFD4;	/*!< D4 Delta coefficient */
+	u32 deltaMatrixCFD5;	/*!< D5 Delta coefficient */
+	u32 deltaMatrixCFD6;	/*!< D6 Delta coefficient */
+	u32 deltaMatrixCFD7;	/*!< D7 Delta coefficient */
+	u32 deltaMatrixCFD8;	/*!< D8 Delta coefficient */
+	u32 deltaMatrixCFD9;	/*!< D9 Delta coefficient */
+	u32 lumLimitWB;		/*!< Luminance range of pixels considered in WB statistics */
+	u32 RBGManualWB;	/*!< Red and Blue color channel gains for manual white balance */
+	u32 awbRedLimit;	/*!< Limits on Red channel gain adjustment through AWB */
+	u32 awbBlueLimit;	/*!< Limits on Blue channel gain adjustment through AWB */
+	u32 matrixAdjLimit;	/*!< Limits on color correction matrix adjustment through AWB */
+	u32 awbSpeed;		/*!< AWB speed and color saturation control */
+	u32 HBoundAE;		/*!< Horizontal boundaries of AWB measurement window */
+	u32 VBoundAE;		/*!< Vertical boundaries of AWB measurement window */
+	u32 HBoundAECenWin;	/*!< Horizontal boundaries of AE measurement window for backlight compensation */
+	u32 VBoundAECenWin;	/*!< Vertical boundaries of AE measurement window for backlight compensation */
+	u32 boundAwbWin;	/*!< Boundaries of AWB measurement window */
+	u32 AEPrecisionTarget;	/*!< Auto exposure target and precision control */
+	u32 AESpeed;		/*!< AE speed and sensitivity control register */
+	u32 redAWBMeasure;	/*!< Measure of the red channel value used by AWB */
+	u32 lumaAWBMeasure;	/*!< Measure of the luminance channel value used by AWB */
+	u32 blueAWBMeasure;	/*!< Measure of the blue channel value used by AWB */
+	u32 limitSharpSatuCtrl;	/*!< Automatic control of sharpness and color saturation */
+	u32 lumaOffset;		/*!< Luminance offset control (brightness control) */
+	u32 clipLimitOutputLumi;	/*!< Clipping limits for output luminance */
+	u32 gainLimitAE;	/*!< Imager gain limits for AE adjustment */
+	u32 shutterWidthLimitAE;	/*!< Shutter width (exposure time) limits for AE adjustment */
+	u32 upperShutterDelayLi;	/*!< Upper Shutter Delay Limit */
+	u32 outputFormatCtrl2;	/*!< Output Format Control 2
+				   00 = 16-bit RGB565.
+				   01 = 15-bit RGB555.
+				   10 = 12-bit RGB444x.
+				   11 = 12-bit RGBx444.       */
+	u32 ipfBlackLevelSub;	/*!< IFP black level subtraction */
+	u32 ipfBlackLevelAdd;	/*!< IFP black level addition */
+	u32 adcLimitAEAdj;	/*!< ADC limits for AE adjustment */
+	u32 agimnThreCamAdj;	/*!< Gain threshold for CCM adjustment */
+	u32 linearAE;
+	u32 thresholdEdgeDefect;	/*!< Edge threshold for interpolation and defect correction */
+	u32 lumaSumMeasure;	/*!< Luma measured by AE engine */
+	u32 timeAdvSumLuma;	/*!< Time-averaged luminance value tracked by auto exposure */
+	u32 motion;		/*!< 1 when motion is detected */
+	u32 gammaKneeY12;	/*!< Gamma knee points Y1 and Y2 */
+	u32 gammaKneeY34;	/*!< Gamma knee points Y3 and Y4 */
+	u32 gammaKneeY56;	/*!< Gamma knee points Y5 and Y6 */
+	u32 gammaKneeY78;	/*!< Gamma knee points Y7 and Y8 */
+	u32 gammaKneeY90;	/*!< Gamma knee points Y9 and Y10 */
+	u32 gammaKneeY0;	/*!< Gamma knee point Y0 */
+	u32 shutter_width_60;
+	u32 search_flicker_60;
+	u32 ratioImageGainBase;
+	u32 ratioImageGainDelta;
+	u32 signValueReg5F;
+	u32 aeGain;
+	u32 maxGainAE;
+	u32 lensCorrectCtrl;
+	u32 shadingParameter1;	/*!< Shade Parameters */
+	u32 shadingParameter2;
+	u32 shadingParameter3;
+	u32 shadingParameter4;
+	u32 shadingParameter5;
+	u32 shadingParameter6;
+	u32 shadingParameter7;
+	u32 shadingParameter8;
+	u32 shadingParameter9;
+	u32 shadingParameter10;
+	u32 shadingParameter11;
+	u32 shadingParameter12;
+	u32 shadingParameter13;
+	u32 shadingParameter14;
+	u32 shadingParameter15;
+	u32 shadingParameter16;
+	u32 shadingParameter17;
+	u32 shadingParameter18;
+	u32 shadingParameter19;
+	u32 shadingParameter20;
+	u32 shadingParameter21;
+	u32 flashCtrl;		/*!< Flash control */
+	u32 lineCounter;	/*!< Line counter */
+	u32 frameCounter;	/*!< Frame counter */
+	u32 HPan;		/*!< Horizontal pan in decimation */
+	u32 HZoom;		/*!< Horizontal zoom in decimation */
+	u32 HSize;		/*!< Horizontal output size iIn decimation */
+	u32 VPan;		/*!< Vertical pan in decimation */
+	u32 VZoom;		/*!< Vertical zoom in decimation */
+	u32 VSize;		/*!< Vertical output size in decimation */
+} mt9v111_IFPReg;
+
+/*!
+ * mt9v111 Config structure
+ */
+typedef struct {
+	mt9v111_coreReg *coreReg;	/*!< Sensor Core Register Bank */
+	mt9v111_IFPReg *ifpReg;	/*!< IFP Register Bank */
+} mt9v111_conf;
+
+typedef struct {
+	u8 index;
+	u16 width;
+	u16 height;
+} mt9v111_image_format;
+
+#endif				/* MT9V111_H_  */
diff --git a/drivers/media/video/mxc/capture/mx27_csi.c b/drivers/media/video/mxc/capture/mx27_csi.c
new file mode 100644
index 000000000000..24fce05be110
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_csi.c
@@ -0,0 +1,333 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_csi.c
+ *
+ * @brief CMOS Sensor interface functions
+ *
+ * @ingroup CSI
+ */
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <mach/clock.h>
+#include <mach/hardware.h>
+
+#include "mx27_csi.h"
+
+static csi_config_t g_csi_cfg;	/* csi hardware configuration */
+static bool gcsi_mclk_on = false;
+static csi_irq_callback_t g_callback = 0;
+static void *g_callback_data = 0;
+static struct clk csi_mclk;
+
+static irqreturn_t csi_irq_handler(int irq, void *data)
+{
+	unsigned long status = __raw_readl(CSI_CSISR);
+
+	__raw_writel(status, CSI_CSISR);
+	if (g_callback)
+		g_callback(g_callback_data, status);
+
+	pr_debug("CSI status = 0x%08lX\n", status);
+
+	return IRQ_HANDLED;
+}
+
+static void csihw_set_config(csi_config_t * cfg)
+{
+	unsigned val = 0;
+
+	/* control reg 1 */
+	val |= cfg->swap16_en ? BIT_SWAP16_EN : 0;
+	val |= cfg->ext_vsync ? BIT_EXT_VSYNC : 0;
+	val |= cfg->eof_int_en ? BIT_EOF_INT_EN : 0;
+	val |= cfg->prp_if_en ? BIT_PRP_IF_EN : 0;
+	val |= cfg->ccir_mode ? BIT_CCIR_MODE : 0;
+	val |= cfg->cof_int_en ? BIT_COF_INT_EN : 0;
+	val |= cfg->sf_or_inten ? BIT_SF_OR_INTEN : 0;
+	val |= cfg->rf_or_inten ? BIT_RF_OR_INTEN : 0;
+	val |= cfg->statff_level << SHIFT_STATFF_LEVEL;
+	val |= cfg->staff_inten ? BIT_STATFF_INTEN : 0;
+	val |= cfg->rxff_level << SHIFT_RXFF_LEVEL;
+	val |= cfg->rxff_inten ? BIT_RXFF_INTEN : 0;
+	val |= cfg->sof_pol ? BIT_SOF_POL : 0;
+	val |= cfg->sof_inten ? BIT_SOF_INTEN : 0;
+	val |= cfg->mclkdiv << SHIFT_MCLKDIV;
+	val |= cfg->hsync_pol ? BIT_HSYNC_POL : 0;
+	val |= cfg->ccir_en ? BIT_CCIR_EN : 0;
+	val |= cfg->mclken ? BIT_MCLKEN : 0;
+	val |= cfg->fcc ? BIT_FCC : 0;
+	val |= cfg->pack_dir ? BIT_PACK_DIR : 0;
+	val |= cfg->gclk_mode ? BIT_GCLK_MODE : 0;
+	val |= cfg->inv_data ? BIT_INV_DATA : 0;
+	val |= cfg->inv_pclk ? BIT_INV_PCLK : 0;
+	val |= cfg->redge ? BIT_REDGE : 0;
+
+	__raw_writel(val, CSI_CSICR1);
+
+	/* control reg 3 */
+	val = 0x0;
+	val |= cfg->csi_sup ? BIT_CSI_SUP : 0;
+	val |= cfg->zero_pack_en ? BIT_ZERO_PACK_EN : 0;
+	val |= cfg->ecc_int_en ? BIT_ECC_INT_EN : 0;
+	val |= cfg->ecc_auto_en ? BIT_ECC_AUTO_EN : 0;
+
+	__raw_writel(val, CSI_CSICR3);
+
+	/* rxfifo counter */
+	__raw_writel(cfg->rxcnt, CSI_CSIRXCNT);
+
+	/* update global config */
+	memcpy(&g_csi_cfg, cfg, sizeof(csi_config_t));
+}
+
+static void csihw_reset_frame_count(void)
+{
+	__raw_writel(__raw_readl(CSI_CSICR3) | BIT_FRMCNT_RST, CSI_CSICR3);
+}
+
+static void csihw_reset(void)
+{
+	csihw_reset_frame_count();
+	__raw_writel(CSICR1_RESET_VAL, CSI_CSICR1);
+	__raw_writel(CSICR2_RESET_VAL, CSI_CSICR2);
+	__raw_writel(CSICR3_RESET_VAL, CSI_CSICR3);
+}
+
+/*!
+ * csi_init_interface
+ *    Sets initial values for the CSI registers.
+ *    The width and height of the sensor and the actual frame size will be
+ *    set to the same values.
+ * @param       width        Sensor width
+ * @param       height       Sensor height
+ * @param       pixel_fmt    pixel format
+ * @param       sig          csi_signal_cfg_t
+ *
+ * @return      0 for success, -EINVAL for error
+ */
+int32_t csi_init_interface(uint16_t width, uint16_t height,
+			   uint32_t pixel_fmt, csi_signal_cfg_t sig)
+{
+	csi_config_t cfg;
+
+	/* Set the CSI_SENS_CONF register remaining fields */
+	cfg.swap16_en = 1;
+	cfg.ext_vsync = sig.ext_vsync;
+	cfg.eof_int_en = 0;
+	cfg.prp_if_en = 1;
+	cfg.ccir_mode = 0;
+	cfg.cof_int_en = 0;
+	cfg.sf_or_inten = 0;
+	cfg.rf_or_inten = 0;
+	cfg.statff_level = 0;
+	cfg.staff_inten = 0;
+	cfg.rxff_level = 2;
+	cfg.rxff_inten = 0;
+	cfg.sof_pol = 1;
+	cfg.sof_inten = 0;
+	cfg.mclkdiv = 0;
+	cfg.hsync_pol = 1;
+	cfg.ccir_en = 0;
+	cfg.mclken = gcsi_mclk_on ? 1 : 0;
+	cfg.fcc = 1;
+	cfg.pack_dir = 0;
+	cfg.gclk_mode = 1;
+	cfg.inv_data = sig.data_pol;
+	cfg.inv_pclk = sig.pixclk_pol;
+	cfg.redge = 1;
+	cfg.csicnt1_rsv = 0;
+
+	/* control reg 3 */
+	cfg.frmcnt = 0;
+	cfg.frame_reset = 0;
+	cfg.csi_sup = 0;
+	cfg.zero_pack_en = 0;
+	cfg.ecc_int_en = 0;
+	cfg.ecc_auto_en = 0;
+
+	csihw_set_config(&cfg);
+
+	return 0;
+}
+
+/*!
+ * csi_enable_prpif
+ *    Enable or disable CSI-PrP interface
+ * @param       enable        Non-zero to enable, zero to disable
+ */
+void csi_enable_prpif(uint32_t enable)
+{
+	if (enable) {
+		g_csi_cfg.prp_if_en = 1;
+		g_csi_cfg.sof_inten = 0;
+		g_csi_cfg.pack_dir = 0;
+	} else {
+		g_csi_cfg.prp_if_en = 0;
+		g_csi_cfg.sof_inten = 1;
+		g_csi_cfg.pack_dir = 1;
+	}
+
+	csihw_set_config(&g_csi_cfg);
+}
+
+/*!
+ * csi_enable_mclk
+ *
+ * @param       src         enum define which source to control the clk
+ *                          CSI_MCLK_VF CSI_MCLK_ENC CSI_MCLK_RAW CSI_MCLK_I2C
+ * @param       flag        true to enable mclk, false to disable mclk
+ * @param       wait        true to wait 100ms make clock stable, false not wait
+ *
+ * @return      0 for success
+ */
+int32_t csi_enable_mclk(int src, bool flag, bool wait)
+{
+	if (flag == true) {
+		clk_enable(&csi_mclk);
+		if (wait == true)
+			msleep(10);
+		pr_debug("Enable csi clock from source %d\n", src);
+		gcsi_mclk_on = true;
+	} else {
+		clk_disable(&csi_mclk);
+		pr_debug("Disable csi clock from source %d\n", src);
+		gcsi_mclk_on = false;
+	}
+
+	return 0;
+}
+
+/*!
+ * csi_read_mclk_flag
+ *
+ * @return  gcsi_mclk_source
+ */
+int csi_read_mclk_flag(void)
+{
+	return 0;
+}
+
+void csi_set_callback(csi_irq_callback_t callback, void *data)
+{
+	g_callback = callback;
+	g_callback_data = data;
+}
+
+static void _mclk_recalc(struct clk *clk)
+{
+	u32 div;
+
+	div = (__raw_readl(CSI_CSICR1) & BIT_MCLKDIV) >> SHIFT_MCLKDIV;
+	div = (div + 1) * 2;
+
+	clk->rate = clk->parent->rate / div;
+}
+
+static unsigned long _mclk_round_rate(struct clk *clk, unsigned long rate)
+{
+	/* Keep CSI divider and change parent clock */
+	if (clk->parent->round_rate) {
+		return clk->parent->round_rate(clk->parent, rate * 2);
+	}
+	return 0;
+}
+
+static int _mclk_set_rate(struct clk *clk, unsigned long rate)
+{
+	int ret = -EINVAL;
+
+	/* Keep CSI divider and change parent clock */
+	if (clk->parent->set_rate) {
+		ret = clk->parent->set_rate(clk->parent, rate * 2);
+		if (ret == 0) {
+			clk->rate = clk->parent->rate / 2;
+		}
+	}
+
+	return ret;
+}
+
+static int _mclk_enable(struct clk *clk)
+{
+	__raw_writel(__raw_readl(CSI_CSICR1) | BIT_MCLKEN, CSI_CSICR1);
+	return 0;
+}
+
+static void _mclk_disable(struct clk *clk)
+{
+	__raw_writel(__raw_readl(CSI_CSICR1) & ~BIT_MCLKEN, CSI_CSICR1);
+}
+
+static struct clk csi_mclk = {
+	.name = "csi_clk",
+	.recalc = _mclk_recalc,
+	.round_rate = _mclk_round_rate,
+	.set_rate = _mclk_set_rate,
+	.enable = _mclk_enable,
+	.disable = _mclk_disable,
+};
+
+int32_t __init csi_init_module(void)
+{
+	int ret = 0;
+	struct clk *per_clk;
+
+	per_clk = clk_get(NULL, "csi_perclk");
+	if (IS_ERR(per_clk))
+		return PTR_ERR(per_clk);
+	clk_put(per_clk);
+	csi_mclk.parent = per_clk;
+	clk_register(&csi_mclk);
+	clk_enable(per_clk);
+	csi_mclk.recalc(&csi_mclk);
+
+	csihw_reset();
+
+	/* interrupt enable */
+	ret = request_irq(MXC_INT_CSI, csi_irq_handler, 0, "csi", 0);
+	if (ret)
+		pr_debug("CSI error: irq request fail\n");
+
+	return ret;
+}
+
+void __exit csi_cleanup_module(void)
+{
+	/* free irq */
+	free_irq(MXC_INT_CSI, 0);
+
+	clk_disable(&csi_mclk);
+}
+
+module_init(csi_init_module);
+module_exit(csi_cleanup_module);
+
+EXPORT_SYMBOL(csi_init_interface);
+EXPORT_SYMBOL(csi_enable_mclk);
+EXPORT_SYMBOL(csi_read_mclk_flag);
+EXPORT_SYMBOL(csi_set_callback);
+EXPORT_SYMBOL(csi_enable_prpif);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MX27 CSI driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/mx27_csi.h b/drivers/media/video/mxc/capture/mx27_csi.h
new file mode 100644
index 000000000000..9bd99781e626
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_csi.h
@@ -0,0 +1,167 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_csi.h
+ *
+ * @brief CMOS Sensor interface functions
+ *
+ * @ingroup CSI
+ */
+
+#ifndef MX27_CSI_H
+#define MX27_CSI_H
+
+#include <linux/io.h>
+
+/* reset values */
+#define CSICR1_RESET_VAL	0x40000800
+#define CSICR2_RESET_VAL	0x0
+#define CSICR3_RESET_VAL	0x0
+
+/* csi control reg 1 */
+#define BIT_SWAP16_EN		(0x1 << 31)
+#define BIT_EXT_VSYNC		(0x1 << 30)
+#define BIT_EOF_INT_EN		(0x1 << 29)
+#define BIT_PRP_IF_EN		(0x1 << 28)
+#define BIT_CCIR_MODE		(0x1 << 27)
+#define BIT_COF_INT_EN		(0x1 << 26)
+#define BIT_SF_OR_INTEN		(0x1 << 25)
+#define BIT_RF_OR_INTEN		(0x1 << 24)
+#define BIT_STATFF_LEVEL	(0x3 << 22)
+#define BIT_STATFF_INTEN	(0x1 << 21)
+#define BIT_RXFF_LEVEL		(0x3 << 19)
+#define BIT_RXFF_INTEN		(0x1 << 18)
+#define BIT_SOF_POL		(0x1 << 17)
+#define BIT_SOF_INTEN		(0x1 << 16)
+#define BIT_MCLKDIV		(0xF << 12)
+#define BIT_HSYNC_POL		(0x1 << 11)
+#define BIT_CCIR_EN		(0x1 << 10)
+#define BIT_MCLKEN		(0x1 << 9)
+#define BIT_FCC			(0x1 << 8)
+#define BIT_PACK_DIR		(0x1 << 7)
+#define BIT_CLR_STATFIFO	(0x1 << 6)
+#define BIT_CLR_RXFIFO		(0x1 << 5)
+#define BIT_GCLK_MODE		(0x1 << 4)
+#define BIT_INV_DATA		(0x1 << 3)
+#define BIT_INV_PCLK		(0x1 << 2)
+#define BIT_REDGE		(0x1 << 1)
+
+#define SHIFT_STATFF_LEVEL	22
+#define SHIFT_RXFF_LEVEL	19
+#define SHIFT_MCLKDIV		12
+
+/* control reg 3 */
+#define BIT_FRMCNT		(0xFFFF << 16)
+#define BIT_FRMCNT_RST		(0x1 << 15)
+#define BIT_CSI_SUP		(0x1 << 3)
+#define BIT_ZERO_PACK_EN	(0x1 << 2)
+#define BIT_ECC_INT_EN		(0x1 << 1)
+#define BIT_ECC_AUTO_EN		(0x1)
+
+#define SHIFT_FRMCNT		16
+
+/* csi status reg */
+#define BIT_SFF_OR_INT		(0x1 << 25)
+#define BIT_RFF_OR_INT		(0x1 << 24)
+#define BIT_STATFF_INT		(0x1 << 21)
+#define BIT_RXFF_INT		(0x1 << 18)
+#define BIT_EOF_INT		(0x1 << 17)
+#define BIT_SOF_INT		(0x1 << 16)
+#define BIT_F2_INT		(0x1 << 15)
+#define BIT_F1_INT		(0x1 << 14)
+#define BIT_COF_INT		(0x1 << 13)
+#define BIT_ECC_INT		(0x1 << 1)
+#define BIT_DRDY		(0x1 << 0)
+
+#define CSI_MCLK_VF		1
+#define CSI_MCLK_ENC		2
+#define CSI_MCLK_RAW		4
+#define CSI_MCLK_I2C		8
+
+#define CSI_CSICR1		(IO_ADDRESS(CSI_BASE_ADDR))
+#define CSI_CSICR2		(IO_ADDRESS(CSI_BASE_ADDR + 0x4))
+#define CSI_CSISR		(IO_ADDRESS(CSI_BASE_ADDR + 0x8))
+#define CSI_STATFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0xC))
+#define CSI_CSIRXFIFO		(IO_ADDRESS(CSI_BASE_ADDR + 0x10))
+#define CSI_CSIRXCNT		(IO_ADDRESS(CSI_BASE_ADDR + 0x14))
+#define CSI_CSICR3		(IO_ADDRESS(CSI_BASE_ADDR + 0x1C))
+
+#define CSI_CSIRXFIFO_PHYADDR	(CSI_BASE_ADDR + 0x10)
+
+static __inline void csi_clear_status(unsigned long status)
+{
+	__raw_writel(status, CSI_CSISR);
+}
+
+typedef struct {
+	unsigned data_width:3;
+	unsigned clk_mode:2;
+	unsigned ext_vsync:1;
+	unsigned Vsync_pol:1;
+	unsigned Hsync_pol:1;
+	unsigned pixclk_pol:1;
+	unsigned data_pol:1;
+	unsigned sens_clksrc:1;
+} csi_signal_cfg_t;
+
+typedef struct {
+	/* control reg 1 */
+	unsigned int swap16_en:1;
+	unsigned int ext_vsync:1;
+	unsigned int eof_int_en:1;
+	unsigned int prp_if_en:1;
+	unsigned int ccir_mode:1;
+	unsigned int cof_int_en:1;
+	unsigned int sf_or_inten:1;
+	unsigned int rf_or_inten:1;
+	unsigned int statff_level:2;
+	unsigned int staff_inten:1;
+	unsigned int rxff_level:2;
+	unsigned int rxff_inten:1;
+	unsigned int sof_pol:1;
+	unsigned int sof_inten:1;
+	unsigned int mclkdiv:4;
+	unsigned int hsync_pol:1;
+	unsigned int ccir_en:1;
+	unsigned int mclken:1;
+	unsigned int fcc:1;
+	unsigned int pack_dir:1;
+	unsigned int gclk_mode:1;
+	unsigned int inv_data:1;
+	unsigned int inv_pclk:1;
+	unsigned int redge:1;
+	unsigned int csicnt1_rsv:1;
+
+	/* control reg 3 */
+	unsigned int frmcnt:16;
+	unsigned int frame_reset:1;
+	unsigned int csi_sup:1;
+	unsigned int zero_pack_en:1;
+	unsigned int ecc_int_en:1;
+	unsigned int ecc_auto_en:1;
+
+	/* fifo counter */
+	unsigned int rxcnt;
+} csi_config_t;
+
+typedef void (*csi_irq_callback_t) (void *data, unsigned long status);
+
+int32_t csi_enable_mclk(int src, bool flag, bool wait);
+int32_t csi_init_interface(uint16_t width, uint16_t height,
+			   uint32_t pixel_fmt, csi_signal_cfg_t sig);
+int csi_read_mclk_flag(void);
+void csi_set_callback(csi_irq_callback_t callback, void *data);
+void csi_enable_prpif(uint32_t enable);
+
+#endif
diff --git a/drivers/media/video/mxc/capture/mx27_prp.h b/drivers/media/video/mxc/capture/mx27_prp.h
new file mode 100644
index 000000000000..e32e9029daff
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_prp.h
@@ -0,0 +1,310 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_prp.h
+ *
+ * @brief Header file for MX27 V4L2 capture driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#ifndef __MX27_PRP_H__
+#define __MX27_PRP_H__
+
+#define PRP_REG(ofs)	(IO_ADDRESS(EMMA_BASE_ADDR) + ofs)
+
+/* Register definitions of PrP */
+#define PRP_CNTL			PRP_REG(0x00)
+#define PRP_INTRCNTL	 		PRP_REG(0x04)
+#define PRP_INTRSTATUS			PRP_REG(0x08)
+#define PRP_SOURCE_Y_PTR		PRP_REG(0x0C)
+#define PRP_SOURCE_CB_PTR		PRP_REG(0x10)
+#define PRP_SOURCE_CR_PTR		PRP_REG(0x14)
+#define PRP_DEST_RGB1_PTR		PRP_REG(0x18)
+#define PRP_DEST_RGB2_PTR		PRP_REG(0x1C)
+#define PRP_DEST_Y_PTR			PRP_REG(0x20)
+#define PRP_DEST_CB_PTR			PRP_REG(0x24)
+#define PRP_DEST_CR_PTR			PRP_REG(0x28)
+#define PRP_SOURCE_FRAME_SIZE  		PRP_REG(0x2C)
+#define PRP_CH1_LINE_STRIDE		PRP_REG(0x30)
+#define PRP_SRC_PIXEL_FORMAT_CNTL	PRP_REG(0x34)
+#define PRP_CH1_PIXEL_FORMAT_CNTL	PRP_REG(0x38)
+#define PRP_CH1_OUT_IMAGE_SIZE		PRP_REG(0x3C)
+#define PRP_CH2_OUT_IMAGE_SIZE		PRP_REG(0x40)
+#define PRP_SOURCE_LINE_STRIDE		PRP_REG(0x44)
+#define PRP_CSC_COEF_012		PRP_REG(0x48)
+#define PRP_CSC_COEF_345		PRP_REG(0x4C)
+#define PRP_CSC_COEF_678		PRP_REG(0x50)
+#define PRP_CH1_RZ_HORI_COEF1		PRP_REG(0x54)
+#define PRP_CH1_RZ_HORI_COEF2		PRP_REG(0x58)
+#define PRP_CH1_RZ_HORI_VALID		PRP_REG(0x5C)
+#define PRP_CH1_RZ_VERT_COEF1		PRP_REG(0x60)
+#define PRP_CH1_RZ_VERT_COEF2		PRP_REG(0x64)
+#define PRP_CH1_RZ_VERT_VALID		PRP_REG(0x68)
+#define PRP_CH2_RZ_HORI_COEF1		PRP_REG(0x6C)
+#define PRP_CH2_RZ_HORI_COEF2		PRP_REG(0x70)
+#define PRP_CH2_RZ_HORI_VALID		PRP_REG(0x74)
+#define PRP_CH2_RZ_VERT_COEF1		PRP_REG(0x78)
+#define PRP_CH2_RZ_VERT_COEF2		PRP_REG(0x7C)
+#define PRP_CH2_RZ_VERT_VALID		PRP_REG(0x80)
+
+#define B_SET(b)			(1 << (b))
+
+/* Bit definitions for PrP control register */
+#define PRP_CNTL_RSTVAL			0x28
+#define PRP_CNTL_CH1EN			B_SET(0)
+#define PRP_CNTL_CH2EN			B_SET(1)
+#define PRP_CNTL_CSI			B_SET(2)
+#define PRP_CNTL_IN_32			B_SET(3)
+#define PRP_CNTL_IN_RGB			B_SET(4)
+#define PRP_CNTL_IN_YUV420		0
+#define PRP_CNTL_IN_YUV422		PRP_CNTL_IN_32
+#define PRP_CNTL_IN_RGB16		PRP_CNTL_IN_RGB
+#define PRP_CNTL_IN_RGB32		(PRP_CNTL_IN_RGB | PRP_CNTL_IN_32)
+#define PRP_CNTL_CH1_RGB8		0
+#define PRP_CNTL_CH1_RGB16		B_SET(5)
+#define PRP_CNTL_CH1_RGB32		B_SET(6)
+#define PRP_CNTL_CH1_YUV422		(B_SET(5) | B_SET(6))
+#define PRP_CNTL_CH2_YUV420		0
+#define PRP_CNTL_CH2_YUV422		B_SET(7)
+#define PRP_CNTL_CH2_YUV444		B_SET(8)
+#define PRP_CNTL_CH1_LOOP		B_SET(9)
+#define PRP_CNTL_CH2_LOOP		B_SET(10)
+#define PRP_CNTL_AUTODROP		B_SET(11)
+#define PRP_CNTL_RST			B_SET(12)
+#define PRP_CNTL_CNTREN			B_SET(13)
+#define PRP_CNTL_WINEN			B_SET(14)
+#define PRP_CNTL_UNCHAIN		B_SET(15)
+#define PRP_CNTL_IN_SKIP_NONE		0
+#define PRP_CNTL_IN_SKIP_1_2		B_SET(16)
+#define PRP_CNTL_IN_SKIP_1_3		B_SET(17)
+#define PRP_CNTL_IN_SKIP_2_3		(B_SET(16) | B_SET(17))
+#define PRP_CNTL_IN_SKIP_1_4		B_SET(18)
+#define PRP_CNTL_IN_SKIP_3_4		(B_SET(16) | B_SET(18))
+#define PRP_CNTL_IN_SKIP_2_5		(B_SET(17) | B_SET(18))
+#define PRP_CNTL_IN_SKIP_3_5		(B_SET(16) | B_SET(17) | B_SET(18))
+#define PRP_CNTL_CH1_SKIP_NONE		0
+#define PRP_CNTL_CH1_SKIP_1_2		B_SET(19)
+#define PRP_CNTL_CH1_SKIP_1_3		B_SET(20)
+#define PRP_CNTL_CH1_SKIP_2_3		(B_SET(19) | B_SET(20))
+#define PRP_CNTL_CH1_SKIP_1_4		B_SET(21)
+#define PRP_CNTL_CH1_SKIP_3_4		(B_SET(19) | B_SET(21))
+#define PRP_CNTL_CH1_SKIP_2_5		(B_SET(20) | B_SET(21))
+#define PRP_CNTL_CH1_SKIP_3_5		(B_SET(19) | B_SET(20) | B_SET(21))
+#define PRP_CNTL_CH2_SKIP_NONE		0
+#define PRP_CNTL_CH2_SKIP_1_2		B_SET(22)
+#define PRP_CNTL_CH2_SKIP_1_3		B_SET(23)
+#define PRP_CNTL_CH2_SKIP_2_3		(B_SET(22) | B_SET(23))
+#define PRP_CNTL_CH2_SKIP_1_4		B_SET(24)
+#define PRP_CNTL_CH2_SKIP_3_4		(B_SET(22) | B_SET(24))
+#define PRP_CNTL_CH2_SKIP_2_5		(B_SET(23) | B_SET(24))
+#define PRP_CNTL_CH2_SKIP_3_5		(B_SET(22) | B_SET(23) | B_SET(24))
+#define PRP_CNTL_FIFO_I128		0
+#define PRP_CNTL_FIFO_I96		B_SET(25)
+#define PRP_CNTL_FIFO_I64		B_SET(26)
+#define PRP_CNTL_FIFO_I32		(B_SET(25) | B_SET(26))
+#define PRP_CNTL_FIFO_O64		0
+#define PRP_CNTL_FIFO_O48		B_SET(27)
+#define PRP_CNTL_FIFO_O32		B_SET(28)
+#define PRP_CNTL_FIFO_O16		(B_SET(27) | B_SET(28))
+#define PRP_CNTL_CH2B1			B_SET(29)
+#define PRP_CNTL_CH2B2			B_SET(30)
+#define PRP_CNTL_CH2_FLOWEN		B_SET(31)
+
+/* Bit definitions for PrP interrupt control register */
+#define PRP_INTRCNTL_RDERR		B_SET(0)
+#define PRP_INTRCNTL_CH1WERR		B_SET(1)
+#define PRP_INTRCNTL_CH2WERR		B_SET(2)
+#define PRP_INTRCNTL_CH1FC		B_SET(3)
+#define PRP_INTRCNTL_CH2FC		B_SET(5)
+#define PRP_INTRCNTL_LBOVF		B_SET(7)
+#define PRP_INTRCNTL_CH2OVF		B_SET(8)
+
+/* Bit definitions for PrP interrupt status register */
+#define PRP_INTRSTAT_RDERR		B_SET(0)
+#define PRP_INTRSTAT_CH1WERR		B_SET(1)
+#define PRP_INTRSTAT_CH2WERR		B_SET(2)
+#define PRP_INTRSTAT_CH2BUF2		B_SET(3)
+#define PRP_INTRSTAT_CH2BUF1		B_SET(4)
+#define PRP_INTRSTAT_CH1BUF2		B_SET(5)
+#define PRP_INTRSTAT_CH1BUF1		B_SET(6)
+#define PRP_INTRSTAT_LBOVF		B_SET(7)
+#define PRP_INTRSTAT_CH2OVF		B_SET(8)
+
+#define PRP_CHANNEL_1		0x1
+#define PRP_CHANNEL_2		0x2
+
+/* PRP-CSI config */
+#define PRP_CSI_EN		0x80
+#define PRP_CSI_LOOP		(0x40 | PRP_CSI_EN)
+#define PRP_CSI_IRQ_FRM		(0x08 | PRP_CSI_LOOP)
+#define PRP_CSI_IRQ_CH1ERR	(0x10 | PRP_CSI_LOOP)
+#define PRP_CSI_IRQ_CH2ERR	(0x20 | PRP_CSI_LOOP)
+#define PRP_CSI_IRQ_ALL		(0x38 | PRP_CSI_LOOP)
+#define PRP_CSI_SKIP_NONE	0
+#define PRP_CSI_SKIP_1OF2	1
+#define PRP_CSI_SKIP_1OF3	2
+#define PRP_CSI_SKIP_2OF3	3
+#define PRP_CSI_SKIP_1OF4	4
+#define PRP_CSI_SKIP_3OF4	5
+#define PRP_CSI_SKIP_2OF5	6
+#define PRP_CSI_SKIP_4OF5	7
+
+#define PRP_PIXIN_RGB565	0x2CA00565
+#define PRP_PIXIN_RGB888	0x41000888
+#define PRP_PIXIN_YUV420	0
+#define PRP_PIXIN_YUYV		0x22000888
+#define PRP_PIXIN_YVYU		0x20100888
+#define PRP_PIXIN_UYVY		0x03080888
+#define PRP_PIXIN_VYUY		0x01180888
+#define PRP_PIXIN_YUV422	0x62080888
+
+#define PRP_PIX1_RGB332		0x14400322
+#define PRP_PIX1_RGB565		0x2CA00565
+#define PRP_PIX1_RGB888		0x41000888
+#define PRP_PIX1_YUYV		0x62000888
+#define PRP_PIX1_YVYU		0x60100888
+#define PRP_PIX1_UYVY		0x43080888
+#define PRP_PIX1_VYUY		0x41180888
+#define PRP_PIX1_UNUSED		0
+
+#define PRP_PIX2_YUV420		0
+#define PRP_PIX2_YUV422		1
+#define PRP_PIX2_YUV444		4
+#define PRP_PIX2_UNUSED		8
+
+#define PRP_ALGO_WIDTH_ANY	0
+#define PRP_ALGO_HEIGHT_ANY	0
+#define PRP_ALGO_WIDTH_BIL	1
+#define PRP_ALGO_WIDTH_AVG	2
+#define PRP_ALGO_HEIGHT_BIL	4
+#define PRP_ALGO_HEIGHT_AVG	8
+#define PRP_ALGO_BYPASS		0x10
+
+typedef struct _emma_prp_ratio {
+	unsigned short num;
+	unsigned short den;
+} emma_prp_ratio;
+
+/*
+ * The following definitions are for resizing. Definition values must not
+ * be changed otherwise decision logic will be wrong.
+ */
+#define SCALE_RETRY	16	/* retry times if ratio is not supported */
+
+#define BC_COEF		3
+#define MAX_TBL		20
+#define SZ_COEF		(1 << BC_COEF)
+
+#define ALGO_AUTO	0
+#define ALGO_BIL	1
+#define ALGO_AVG	2
+
+typedef struct {
+	char tbl[20];		/* table entries */
+	char len;		/* table length used */
+	char algo;		/* ALGO_xxx */
+	char ratio[20];		/* ratios used */
+} scale_t;
+
+/*
+ * structure for prp scaling.
+ * algorithm - bilinear or averaging for each axis
+ * PRP_ALGO_WIDTH_x | PRP_ALGO_HEIGHT_x | PRP_ALGO_BYPASS
+ * PRP_ALGO_BYPASS - Ch1 will not use Ch2 scaling with this flag
+ */
+typedef struct _emma_prp_scale {
+	unsigned char algo;
+	emma_prp_ratio width;
+	emma_prp_ratio height;
+} emma_prp_scale;
+
+typedef struct emma_prp_cfg {
+	unsigned int in_pix;	/* PRP_PIXIN_xxx */
+	unsigned short in_width;	/* image width, 32 - 2044 */
+	unsigned short in_height;	/* image height, 32 - 2044 */
+	unsigned char in_csi;	/* PRP_CSI_SKIP_x | PRP_CSI_LOOP */
+	unsigned short in_line_stride;	/* in_line_stride and in_line_skip */
+	unsigned short in_line_skip;	/* allow cropping from CSI */
+	unsigned int in_ptr;	/* bus address */
+	/*
+	 * in_csc[9] = 1 -> Y-16
+	 * if in_csc[1..9] == 0
+	 *      in_csc[0] represents YUV range 0-3 = A0,A1,B0,B1;
+	 * else
+	 *      in_csc[0..9] represents either format
+	 */
+	unsigned short in_csc[10];
+
+	unsigned char ch2_pix;	/* PRP_PIX2_xxx */
+	emma_prp_scale ch2_scale;	/* resizing paramters */
+	unsigned short ch2_width;	/* 4-2044, 0 = scaled */
+	unsigned short ch2_height;	/* 4-2044, 0 = scaled */
+	unsigned int ch2_ptr;	/* bus addr */
+	unsigned int ch2_ptr2;	/* bus addr for 2nd buf (loop mode) */
+	unsigned char ch2_csi;	/* PRP_CSI_SKIP_x | PRP_CSI_LOOP */
+
+	unsigned int ch1_pix;	/* PRP_PIX1_xxx */
+	emma_prp_scale ch1_scale;	/* resizing parameters */
+	unsigned short ch1_width;	/* 4-2044, 0 = scaled */
+	unsigned short ch1_height;	/* 4-2044, 0 = scaled */
+	unsigned short ch1_stride;	/* 4-4088, 0 = ch1_width */
+	unsigned int ch1_ptr;	/* bus addr */
+	unsigned int ch1_ptr2;	/* bus addr for 2nd buf (loop mode) */
+	unsigned char ch1_csi;	/* PRP_CSI_SKIP_x | PRP_CSI_LOOP */
+
+	/*
+	 * channel resizing coefficients
+	 * scale[0] for channel 1 width
+	 * scale[1] for channel 1 height
+	 * scale[2] for channel 2 width
+	 * scale[3] for channel 2 height
+	 */
+	scale_t scale[4];
+} emma_prp_cfg;
+
+int prphw_reset(void);
+int prphw_enable(int channel);
+int prphw_disable(int channel);
+int prphw_inptr(emma_prp_cfg *);
+int prphw_ch1ptr(emma_prp_cfg *);
+int prphw_ch1ptr2(emma_prp_cfg *);
+int prphw_ch2ptr(emma_prp_cfg *);
+int prphw_ch2ptr2(emma_prp_cfg *);
+int prphw_cfg(emma_prp_cfg *);
+int prphw_isr(void);
+void prphw_init(void);
+void prphw_exit(void);
+
+/*
+ * scale	out	coefficient table
+ * din		in	scale numerator
+ * dout		in	scale denominator
+ * inv		in	pre-scale dimension
+ * vout		in/out	post-scale output dimension
+ * pout		out	post-scale internal dimension [opt]
+ * retry	in	retry times (round the output length) when need
+ */
+int prp_scale(scale_t * pscale, int din, int dout, int inv,
+	      unsigned short *vout, unsigned short *pout, int retry);
+
+int prp_init(void *dev_id);
+void prp_exit(void *dev_id);
+int prp_enc_select(void *data);
+int prp_enc_deselect(void *data);
+int prp_vf_select(void *data);
+int prp_vf_deselect(void *data);
+int prp_still_select(void *data);
+int prp_still_deselect(void *data);
+
+#endif				/* __MX27_PRP_H__ */
diff --git a/drivers/media/video/mxc/capture/mx27_prphw.c b/drivers/media/video/mxc/capture/mx27_prphw.c
new file mode 100644
index 000000000000..c56a6df1716e
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_prphw.c
@@ -0,0 +1,1099 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_prphw.c
+ *
+ * @brief MX27 Video For Linux 2 capture driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/clk.h>
+#include <asm/io.h>
+#include <linux/delay.h>
+
+#include "mx27_prp.h"
+
+#define PRP_MIN_IN_WIDTH	32
+#define PRP_MAX_IN_WIDTH	2044
+#define PRP_MIN_IN_HEIGHT	32
+#define PRP_MAX_IN_HEIGHT	2044
+
+typedef struct _coeff_t {
+	unsigned long coeff[2];
+	unsigned long cntl;
+} coeff_t[2][2];
+
+static coeff_t *PRP_RSZ_COEFF = (coeff_t *) PRP_CH1_RZ_HORI_COEF1;
+
+static unsigned char scale_get(scale_t * t,
+			       unsigned char *i, unsigned char *out);
+static int gcd(int x, int y);
+static int ratio(int x, int y, int *den);
+static int prp_scale_bilinear(scale_t * t, int coeff, int base, int nxt);
+static int prp_scale_ave(scale_t * t, unsigned char base);
+static int ave_scale(scale_t * t, int inv, int outv);
+static int scale(scale_t * t, int inv, int outv);
+
+/*!
+ * @param t	table
+ * @param i	table index
+ * @param out	bilinear	# input pixels to advance
+ *		average		whether result is ready for output
+ * @return	coefficient
+*/
+static unsigned char scale_get(scale_t * t, unsigned char *i,
+			       unsigned char *out)
+{
+	unsigned char c;
+
+	c = t->tbl[*i];
+	(*i)++;
+	*i %= t->len;
+
+	if (out) {
+		if (t->algo == ALGO_BIL) {
+			for ((*out) = 1;
+			     (*i) && ((*i) < t->len) && !t->tbl[(*i)]; (*i)++) {
+				(*out)++;
+			}
+			if ((*i) == t->len)
+				(*i) = 0;
+		} else
+			*out = c >> BC_COEF;
+	}
+
+	c &= SZ_COEF - 1;
+
+	if (c == SZ_COEF - 1)
+		c = SZ_COEF;
+
+	return c;
+}
+
+/*!
+ * @brief Get maximum common divisor.
+ * @param x	First input value
+ * @param y	Second input value
+ * @return	Maximum common divisor of x and y
+ */
+static int gcd(int x, int y)
+{
+	int k;
+
+	if (x < y) {
+		k = x;
+		x = y;
+		y = k;
+	}
+
+	while ((k = x % y)) {
+		x = y;
+		y = k;
+	}
+
+	return y;
+}
+
+/*!
+ * @brief Get ratio.
+ * @param x	First input value
+ * @param y	Second input value
+ * @param den	Denominator of the ratio (corresponding to y)
+ * @return	Numerator of the ratio (corresponding to x)
+ */
+static int ratio(int x, int y, int *den)
+{
+	int g;
+
+	if (!x || !y)
+		return 0;
+
+	g = gcd(x, y);
+	*den = y / g;
+
+	return x / g;
+}
+
+/*!
+ * @brief Build PrP coefficient entry based on bilinear algorithm
+ *
+ * @param t	The pointer to scale_t structure
+ * @param coeff	The weighting coefficient
+ * @param base	The base of the coefficient
+ * @param nxt	Number of pixels to be read
+ *
+ * @return	The length of current coefficient table on success
+ *		-1 on failure
+ */
+static int prp_scale_bilinear(scale_t * t, int coeff, int base, int nxt)
+{
+	int i;
+
+	if (t->len >= sizeof(t->tbl))
+		return -1;
+
+	coeff = ((coeff << BC_COEF) + (base >> 1)) / base;
+	if (coeff >= SZ_COEF - 1)
+		coeff--;
+
+	coeff |= SZ_COEF;
+	t->tbl[(int)t->len++] = (unsigned char)coeff;
+
+	for (i = 1; i < nxt; i++) {
+		if (t->len >= MAX_TBL)
+			return -1;
+
+		t->tbl[(int)t->len++] = 0;
+	}
+
+	return t->len;
+}
+
+#define _bary(name)	static const unsigned char name[]
+
+_bary(c1) = {
+7};
+
+_bary(c2) = {
+4, 4};
+
+_bary(c3) = {
+2, 4, 2};
+
+_bary(c4) = {
+2, 2, 2, 2};
+
+_bary(c5) = {
+1, 2, 2, 2, 1};
+
+_bary(c6) = {
+1, 1, 2, 2, 1, 1};
+
+_bary(c7) = {
+1, 1, 1, 2, 1, 1, 1};
+
+_bary(c8) = {
+1, 1, 1, 1, 1, 1, 1, 1};
+
+_bary(c9) = {
+1, 1, 1, 1, 1, 1, 1, 1, 0};
+
+_bary(c10) = {
+0, 1, 1, 1, 1, 1, 1, 1, 1, 0};
+
+_bary(c11) = {
+0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0};
+
+_bary(c12) = {
+0, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 0};
+
+_bary(c13) = {
+0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0};
+
+_bary(c14) = {
+0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 0, 1, 0};
+
+_bary(c15) = {
+0, 1, 0, 1, 0, 1, 1, 0, 1, 1, 0, 1, 0, 1, 0};
+
+_bary(c16) = {
+1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0};
+
+_bary(c17) = {
+0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0};
+
+_bary(c18) = {
+0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0};
+
+_bary(c19) = {
+0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0};
+
+_bary(c20) = {
+0, 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0};
+
+static const unsigned char *ave_coeff[] = {
+	c1, c2, c3, c4, c5, c6, c7, c8, c9, c10,
+	c11, c12, c13, c14, c15, c16, c17, c18, c19, c20
+};
+
+/*!
+ * @brief Build PrP coefficient table based on average algorithm
+ *
+ * @param t	The pointer to scale_t structure
+ * @param base	The base of the coefficient
+ *
+ * @return	The length of current coefficient table on success
+ *		-1 on failure
+ */
+static int prp_scale_ave(scale_t * t, unsigned char base)
+{
+	if (t->len + base > sizeof(t->tbl))
+		return -1;
+
+	memcpy(&t->tbl[(int)t->len], ave_coeff[(int)base - 1], base);
+	t->len = (unsigned char)(t->len + base);
+	t->tbl[t->len - 1] |= SZ_COEF;
+
+	return t->len;
+}
+
+/*!
+ * @brief Build PrP coefficient table based on average algorithm
+ *
+ * @param t	The pointer to scale_t structure
+ * @param inv	Input resolution
+ * @param outv	Output resolution
+ *
+ * @return	The length of current coefficient table on success
+ *		-1 on failure
+ */
+static int ave_scale(scale_t * t, int inv, int outv)
+{
+	int ratio_count;
+
+	ratio_count = 0;
+	if (outv != 1) {
+		unsigned char a[20];
+		int v;
+
+		/* split n:m into multiple n[i]:1 */
+		for (v = 0; v < outv; v++)
+			a[v] = (unsigned char)(inv / outv);
+
+		inv %= outv;
+		if (inv) {
+			/* find start of next layer */
+			v = (outv - inv) >> 1;
+			inv += v;
+			for (; v < inv; v++)
+				a[v]++;
+		}
+
+		for (v = 0; v < outv; v++) {
+			if (prp_scale_ave(t, a[v]) < 0)
+				return -1;
+
+			t->ratio[ratio_count] = a[v];
+			ratio_count++;
+		}
+	} else if (prp_scale_ave(t, inv) < 0) {
+		return -1;
+	} else {
+		t->ratio[ratio_count++] = (char)inv;
+		ratio_count++;
+	}
+
+	return t->len;
+}
+
+/*!
+ * @brief Build PrP coefficient table
+ *
+ * @param t	The pointer to scale_t structure
+ * @param inv	input resolution reduced ratio
+ * @param outv	output resolution reduced ratio
+ *
+ * @return	The length of current coefficient table on success
+ *		-1 on failure
+ */
+static int scale(scale_t * t, int inv, int outv)
+{
+	int v;			/* overflow counter */
+	int coeff, nxt;		/* table output */
+
+	t->len = 0;
+	if (t->algo == ALGO_AUTO) {
+		/* automatic choice - bilinear for shrinking less than 2:1 */
+		t->algo = ((outv != inv) && ((2 * outv) > inv)) ?
+		    ALGO_BIL : ALGO_AVG;
+	}
+
+	/* 1:1 resize must use averaging, bilinear will hang */
+	if ((inv == outv) && (t->algo == ALGO_BIL)) {
+		pr_debug("Warning: 1:1 resize must use averaging algo\n");
+		t->algo = ALGO_AVG;
+	}
+
+	memset(t->tbl, 0, sizeof(t->tbl));
+	if (t->algo == ALGO_BIL) {
+		t->ratio[0] = (char)inv;
+		t->ratio[1] = (char)outv;
+	} else
+		memset(t->ratio, 0, sizeof(t->ratio));
+
+	if (inv == outv) {
+		/* force scaling */
+		t->ratio[0] = 1;
+		if (t->algo == ALGO_BIL)
+			t->ratio[1] = 1;
+
+		return prp_scale_ave(t, 1);
+	}
+
+	if (inv < outv) {
+		pr_debug("Upscaling not supported %d:%d\n", inv, outv);
+		return -1;
+	}
+
+	if (t->algo != ALGO_BIL)
+		return ave_scale(t, inv, outv);
+
+	v = 0;
+	if (inv >= 2 * outv) {
+		/* downscale: >=2:1 bilinear approximation */
+		coeff = inv - 2 * outv;
+		v = 0;
+		nxt = 0;
+		do {
+			v += coeff;
+			nxt = 2;
+			while (v >= outv) {
+				v -= outv;
+				nxt++;
+			}
+
+			if (prp_scale_bilinear(t, 1, 2, nxt) < 0)
+				return -1;
+		} while (v);
+	} else {
+		/* downscale: bilinear */
+		int in_pos_inc = 2 * outv;
+		int out_pos = inv;
+		int out_pos_inc = 2 * inv;
+		int init_carry = inv - outv;
+		int carry = init_carry;
+
+		v = outv + in_pos_inc;
+		do {
+			coeff = v - out_pos;
+			out_pos += out_pos_inc;
+			carry += out_pos_inc;
+			for (nxt = 0; v < out_pos; nxt++) {
+				v += in_pos_inc;
+				carry -= in_pos_inc;
+			}
+			if (prp_scale_bilinear(t, coeff, in_pos_inc, nxt) < 0)
+				return -1;
+		} while (carry != init_carry);
+	}
+	return t->len;
+}
+
+/*!
+ * @brief Build PrP coefficient table
+ *
+ * @param pscale	The pointer to scale_t structure which holdes
+ * 			coefficient tables
+ * @param din		Scale ratio numerator
+ * @param dout		Scale ratio denominator
+ * @param inv		Input resolution
+ * @param vout		Output resolution
+ * @param pout		Internal output resolution
+ * @param retry		Retry times (round the output length) when need
+ *
+ * @return		Zero on success, others on failure
+ */
+int prp_scale(scale_t * pscale, int din, int dout, int inv,
+	      unsigned short *vout, unsigned short *pout, int retry)
+{
+	int num;
+	int den;
+	unsigned short outv;
+
+	/* auto-generation of values */
+	if (!(dout && din)) {
+		if (!*vout)
+			dout = din = 1;
+		else {
+			din = inv;
+			dout = *vout;
+		}
+	}
+
+	if (din < dout) {
+		pr_debug("Scale err, unsupported ratio %d : %d\n", din, dout);
+		return -1;
+	}
+
+      lp_retry:
+	num = ratio(din, dout, &den);
+	if (!num) {
+		pr_debug("Scale err, unsupported ratio %d : %d\n", din, dout);
+		return -1;
+	}
+
+	if (num > MAX_TBL || scale(pscale, num, den) < 0) {
+		dout++;
+		if (retry--)
+			goto lp_retry;
+
+		pr_debug("Scale err, unsupported ratio %d : %d\n", num, den);
+		return -1;
+	}
+
+	if (pscale->algo == ALGO_BIL) {
+		unsigned char i, j, k;
+
+		outv =
+		    (unsigned short)(inv / pscale->ratio[0] * pscale->ratio[1]);
+		inv %= pscale->ratio[0];
+		for (i = j = 0; inv > 0; j++) {
+			unsigned char nxt;
+
+			k = scale_get(pscale, &i, &nxt);
+			if (inv == 1 && k < SZ_COEF) {
+				/* needs 2 pixels for this output */
+				break;
+			}
+			inv -= nxt;
+		}
+		outv = outv + j;
+	} else {
+		unsigned char i, tot;
+
+		for (tot = i = 0; pscale->ratio[i]; i++)
+			tot = tot + pscale->ratio[i];
+
+		outv = (unsigned short)(inv / tot) * i;
+		inv %= tot;
+		for (i = 0; inv > 0; i++, outv++)
+			inv -= pscale->ratio[i];
+	}
+
+	if (!(*vout) || ((*vout) > outv))
+		*vout = outv;
+
+	if (pout)
+		*pout = outv;
+
+	return 0;
+}
+
+/*!
+ * @brief Reset PrP block
+ */
+int prphw_reset(void)
+{
+	unsigned long val;
+	unsigned long flag;
+	int i;
+
+	flag = PRP_CNTL_RST;
+	val = PRP_CNTL_RSTVAL;
+
+	__raw_writel(flag, PRP_CNTL);
+
+	/* timeout */
+	for (i = 0; i < 1000; i++) {
+		if (!(__raw_readl(PRP_CNTL) & flag)) {
+			pr_debug("PrP reset over\n");
+			break;
+		}
+		msleep(1);
+	}
+
+	/* verify reset value */
+	if (__raw_readl(PRP_CNTL) != val) {
+		pr_info("PrP reset err, val = 0x%08X\n", __raw_readl(PRP_CNTL));
+		return -1;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Enable PrP channel.
+ * @param channel	Channel number to be enabled
+ * @return		Zero on success, others on failure
+ */
+int prphw_enable(int channel)
+{
+	unsigned long val;
+
+	val = __raw_readl(PRP_CNTL);
+	if (channel & PRP_CHANNEL_1)
+		val |= PRP_CNTL_CH1EN;
+	if (channel & PRP_CHANNEL_2)
+		val |= (PRP_CNTL_CH2EN | PRP_CNTL_CH2_FLOWEN);
+
+	__raw_writel(val, PRP_CNTL);
+
+	return 0;
+}
+
+/*!
+ * @brief Disable PrP channel.
+ * @param channel	Channel number to be disable
+ * @return		Zero on success, others on failure
+ */
+int prphw_disable(int channel)
+{
+	unsigned long val;
+
+	val = __raw_readl(PRP_CNTL);
+	if (channel & PRP_CHANNEL_1)
+		val &= ~PRP_CNTL_CH1EN;
+	if (channel & PRP_CHANNEL_2)
+		val &= ~(PRP_CNTL_CH2EN | PRP_CNTL_CH2_FLOWEN);
+
+	__raw_writel(val, PRP_CNTL);
+
+	return 0;
+}
+
+/*!
+ * @brief Set PrP input buffer address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_inptr(emma_prp_cfg * cfg)
+{
+	if (cfg->in_csi & PRP_CSI_EN)
+		return -1;
+
+	__raw_writel(cfg->in_ptr, PRP_SOURCE_Y_PTR);
+	if (cfg->in_pix == PRP_PIXIN_YUV420) {
+		u32 size;
+
+		size = cfg->in_line_stride * cfg->in_height;
+		__raw_writel(cfg->in_ptr + size, PRP_SOURCE_CB_PTR);
+		__raw_writel(cfg->in_ptr + size + (size >> 2),
+			     PRP_SOURCE_CR_PTR);
+	}
+	return 0;
+}
+
+/*!
+ * @brief Set PrP channel 1 output buffer 1 address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_ch1ptr(emma_prp_cfg * cfg)
+{
+	if (cfg->ch1_pix == PRP_PIX1_UNUSED)
+		return -1;
+
+	__raw_writel(cfg->ch1_ptr, PRP_DEST_RGB1_PTR);
+
+	/* support double buffer in loop mode only */
+	if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+		if (cfg->ch1_ptr2)
+			__raw_writel(cfg->ch1_ptr2, PRP_DEST_RGB2_PTR);
+		else
+			__raw_writel(cfg->ch1_ptr, PRP_DEST_RGB2_PTR);
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Set PrP channel 1 output buffer 2 address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_ch1ptr2(emma_prp_cfg * cfg)
+{
+	if (cfg->ch1_pix == PRP_PIX1_UNUSED ||
+	    (cfg->in_csi & PRP_CSI_LOOP) != PRP_CSI_LOOP)
+		return -1;
+
+	if (cfg->ch1_ptr2)
+		__raw_writel(cfg->ch1_ptr2, PRP_DEST_RGB2_PTR);
+	else
+		return -1;
+
+	return 0;
+}
+
+/*!
+ * @brief Set PrP channel 2 output buffer 1 address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_ch2ptr(emma_prp_cfg * cfg)
+{
+	u32 size;
+
+	if (cfg->ch2_pix == PRP_PIX2_UNUSED)
+		return -1;
+
+	__raw_writel(cfg->ch2_ptr, PRP_DEST_Y_PTR);
+
+	if (cfg->ch2_pix == PRP_PIX2_YUV420) {
+		size = cfg->ch2_width * cfg->ch2_height;
+		__raw_writel(cfg->ch2_ptr + size, PRP_DEST_CB_PTR);
+		__raw_writel(cfg->ch2_ptr + size + (size >> 2),
+			     PRP_DEST_CR_PTR);
+	}
+
+	__raw_writel(__raw_readl(PRP_CNTL) | PRP_CNTL_CH2B1, PRP_CNTL);
+	return 0;
+}
+
+/*!
+ * @brief Set PrP channel 2 output buffer 2 address.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_ch2ptr2(emma_prp_cfg * cfg)
+{
+	u32 size;
+
+	if (cfg->ch2_pix == PRP_PIX2_UNUSED ||
+	    (cfg->in_csi & PRP_CSI_LOOP) != PRP_CSI_LOOP)
+		return -1;
+
+	__raw_writel(cfg->ch2_ptr2, PRP_SOURCE_Y_PTR);
+	if (cfg->ch2_pix == PRP_PIX2_YUV420) {
+		size = cfg->ch2_width * cfg->ch2_height;
+		__raw_writel(cfg->ch2_ptr2 + size, PRP_SOURCE_CB_PTR);
+		__raw_writel(cfg->ch2_ptr2 + size + (size >> 2),
+			     PRP_SOURCE_CR_PTR);
+	}
+
+	__raw_writel(__raw_readl(PRP_CNTL) | PRP_CNTL_CH2B2, PRP_CNTL);
+	return 0;
+}
+
+/*!
+ * @brief Build CSC table
+ * @param csc	CSC table
+ *		in	csc[0]=index		0..3 : A.1 A.0 B.1 B.0
+ *			csc[1]=direction	0 : YUV2RGB  1 : RGB2YUV
+ *		out	csc[0..4] are coefficients c[9] is offset
+ *			csc[0..8] are coefficients c[9] is offset
+ */
+void csc_tbl(short csc[10])
+{
+	static const unsigned short _r2y[][9] = {
+		{0x4D, 0x4B, 0x3A, 0x57, 0x55, 0x40, 0x40, 0x6B, 0x29},
+		{0x42, 0x41, 0x32, 0x4C, 0x4A, 0x38, 0x38, 0x5E, 0x24},
+		{0x36, 0x5C, 0x25, 0x3B, 0x63, 0x40, 0x40, 0x74, 0x18},
+		{0x2F, 0x4F, 0x20, 0x34, 0x57, 0x38, 0x38, 0x66, 0x15},
+	};
+	static const unsigned short _y2r[][5] = {
+		{0x80, 0xb4, 0x2c, 0x5b, 0x0e4},
+		{0x95, 0xcc, 0x32, 0x68, 0x104},
+		{0x80, 0xca, 0x18, 0x3c, 0x0ec},
+		{0x95, 0xe5, 0x1b, 0x44, 0x1e0},
+	};
+	unsigned short *_csc;
+	int _csclen;
+
+	csc[9] = csc[0] & 1;
+	_csclen = csc[0] & 3;
+
+	if (csc[1]) {
+		_csc = (unsigned short *)_r2y[_csclen];
+		_csclen = sizeof(_r2y[0]);
+	} else {
+		_csc = (unsigned short *)_y2r[_csclen];
+		_csclen = sizeof(_y2r[0]);
+		memset(csc + 5, 0, sizeof(short) * 4);
+	}
+	memcpy(csc, _csc, _csclen);
+}
+
+/*!
+ * @brief Setup PrP resize coefficient registers
+ *
+ * @param ch	PrP channel number
+ * @param dir	Direction, 0 - horizontal, 1 - vertical
+ * @param scale	The pointer to scale_t structure
+ */
+static void prp_set_scaler(int ch, int dir, scale_t * scale)
+{
+	int i;
+	unsigned int coeff[2];
+	unsigned int valid;
+
+	for (coeff[0] = coeff[1] = valid = 0, i = 19; i >= 0; i--) {
+		int j;
+
+		j = i > 9 ? 1 : 0;
+		coeff[j] = (coeff[j] << BC_COEF) |
+		    (scale->tbl[i] & (SZ_COEF - 1));
+
+		if (i == 5 || i == 15)
+			coeff[j] <<= 1;
+
+		valid = (valid << 1) | (scale->tbl[i] >> BC_COEF);
+	}
+
+	valid |= (scale->len << 24) | ((2 - scale->algo) << 31);
+
+	for (i = 0; i < 2; i++)
+		(*PRP_RSZ_COEFF)[1 - ch][dir].coeff[i] = coeff[i];
+
+	(*PRP_RSZ_COEFF)[1 - ch][dir].cntl = valid;
+}
+
+/*!
+ * @brief Setup PrP registers relevant to input.
+ * @param cfg		Pointer to PrP configuration parameter
+ * @param prp_cntl	Holds the value for PrP control register
+ * @return		Zero on success, others on failure
+ */
+static int prphw_input_cfg(emma_prp_cfg * cfg, unsigned long *prp_cntl)
+{
+	unsigned long mask;
+
+	switch (cfg->in_pix) {
+	case PRP_PIXIN_YUV420:
+		*prp_cntl |= PRP_CNTL_IN_YUV420;
+		mask = 0x7;
+		break;
+	case PRP_PIXIN_YUYV:
+	case PRP_PIXIN_YVYU:
+	case PRP_PIXIN_UYVY:
+	case PRP_PIXIN_VYUY:
+		*prp_cntl |= PRP_CNTL_IN_YUV422;
+		mask = 0x1;
+		break;
+	case PRP_PIXIN_RGB565:
+		*prp_cntl |= PRP_CNTL_IN_RGB16;
+		mask = 0x1;
+		break;
+	case PRP_PIXIN_RGB888:
+		*prp_cntl |= PRP_CNTL_IN_RGB32;
+		mask = 0;
+		break;
+	default:
+		pr_debug("Unsupported input pix format 0x%08X\n", cfg->in_pix);
+		return -1;
+	}
+
+	/* align the input image width */
+	if (cfg->in_width & mask) {
+		pr_debug("in_width misaligned. in_width=%d\n", cfg->in_width);
+		return -1;
+	}
+
+	if ((cfg->in_width < PRP_MIN_IN_WIDTH)
+	    || (cfg->in_width > PRP_MAX_IN_WIDTH)) {
+		pr_debug("Unsupported input width %d\n", cfg->in_width);
+		return -1;
+	}
+
+	cfg->in_height &= ~1;	/* truncate to make even */
+
+	if ((cfg->in_height < PRP_MIN_IN_HEIGHT)
+	    || (cfg->in_height > PRP_MAX_IN_HEIGHT)) {
+		pr_debug("Unsupported input height %d\n", cfg->in_height);
+		return -1;
+	}
+
+	if (!(cfg->in_csi & PRP_CSI_EN))
+		if (!cfg->in_line_stride)
+			cfg->in_line_stride = cfg->in_width;
+
+	__raw_writel(cfg->in_pix, PRP_SRC_PIXEL_FORMAT_CNTL);
+	__raw_writel((cfg->in_width << 16) | cfg->in_height,
+		     PRP_SOURCE_FRAME_SIZE);
+	__raw_writel((cfg->in_line_skip << 16) | cfg->in_line_stride,
+		     PRP_SOURCE_LINE_STRIDE);
+
+	if (!(cfg->in_csi & PRP_CSI_EN)) {
+		__raw_writel(cfg->in_ptr, PRP_SOURCE_Y_PTR);
+		if (cfg->in_pix == PRP_PIXIN_YUV420) {
+			unsigned int size;
+
+			size = cfg->in_line_stride * cfg->in_height;
+			__raw_writel(cfg->in_ptr + size, PRP_SOURCE_CB_PTR);
+			__raw_writel(cfg->in_ptr + size + (size >> 2),
+				     PRP_SOURCE_CR_PTR);
+		}
+	}
+
+	/* always cropping */
+	*prp_cntl |= PRP_CNTL_WINEN;
+
+	/* color space conversion */
+	if (!cfg->in_csc[1]) {
+		if (cfg->in_csc[0] > 3) {
+			pr_debug("in_csc invalid 0x%X\n", cfg->in_csc[0]);
+			return -1;
+		}
+		if ((cfg->in_pix == PRP_PIXIN_RGB565)
+		    || (cfg->in_pix == PRP_PIXIN_RGB888))
+			cfg->in_csc[1] = 1;
+		else
+			cfg->in_csc[0] = 0;
+		csc_tbl(cfg->in_csc);
+	}
+
+	__raw_writel((cfg->in_csc[0] << 21) | (cfg->in_csc[1] << 11)
+		     | cfg->in_csc[2], PRP_CSC_COEF_012);
+	__raw_writel((cfg->in_csc[3] << 21) | (cfg->in_csc[4] << 11)
+		     | cfg->in_csc[5], PRP_CSC_COEF_345);
+	__raw_writel((cfg->in_csc[6] << 21) | (cfg->in_csc[7] << 11)
+		     | cfg->in_csc[8] | (cfg->in_csc[9] << 31),
+		     PRP_CSC_COEF_678);
+
+	if (cfg->in_csi & PRP_CSI_EN) {
+		*prp_cntl |= PRP_CNTL_CSI;
+
+		/* loop mode enable, ch1 ch2 together */
+		if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP)
+			*prp_cntl |= (PRP_CNTL_CH1_LOOP | PRP_CNTL_CH2_LOOP);
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Setup PrP registers relevant to channel 2.
+ * @param cfg		Pointer to PrP configuration parameter
+ * @param prp_cntl	Holds the value for PrP control register
+ * @return		Zero on success, others on failure
+ */
+static int prphw_ch2_cfg(emma_prp_cfg * cfg, unsigned long *prp_cntl)
+{
+	switch (cfg->ch2_pix) {
+	case PRP_PIX2_YUV420:
+		*prp_cntl |= PRP_CNTL_CH2_YUV420;
+		break;
+	case PRP_PIX2_YUV422:
+		*prp_cntl |= PRP_CNTL_CH2_YUV422;
+		break;
+	case PRP_PIX2_YUV444:
+		*prp_cntl |= PRP_CNTL_CH2_YUV444;
+		break;
+	case PRP_PIX2_UNUSED:
+		return 0;
+	default:
+		pr_debug("Unsupported channel 2 pix format 0x%08X\n",
+			 cfg->ch2_pix);
+		return -1;
+	}
+
+	if (cfg->ch2_pix == PRP_PIX2_YUV420) {
+		cfg->ch2_height &= ~1;	/* ensure U/V presence */
+		cfg->ch2_width &= ~7;	/* ensure U/V word aligned */
+	} else if (cfg->ch2_pix == PRP_PIX2_YUV422) {
+		cfg->ch2_width &= ~1;	/* word aligned */
+	}
+
+	__raw_writel((cfg->ch2_width << 16) | cfg->ch2_height,
+		     PRP_CH2_OUT_IMAGE_SIZE);
+
+	if (cfg->ch2_pix == PRP_PIX2_YUV420) {
+		u32 size;
+
+		/* Luminanance band start address */
+		__raw_writel(cfg->ch2_ptr, PRP_DEST_Y_PTR);
+
+		if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+			if (!cfg->ch2_ptr2)
+				__raw_writel(cfg->ch2_ptr, PRP_SOURCE_Y_PTR);
+			else
+				__raw_writel(cfg->ch2_ptr2, PRP_SOURCE_Y_PTR);
+		}
+
+		/* Cb and Cr band start address */
+		size = cfg->ch2_width * cfg->ch2_height;
+		__raw_writel(cfg->ch2_ptr + size, PRP_DEST_CB_PTR);
+		__raw_writel(cfg->ch2_ptr + size + (size >> 2),
+			     PRP_DEST_CR_PTR);
+
+		if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+			if (!cfg->ch2_ptr2) {
+				__raw_writel(cfg->ch2_ptr + size,
+					     PRP_SOURCE_CB_PTR);
+				__raw_writel(cfg->ch2_ptr + size + (size >> 2),
+					     PRP_SOURCE_CR_PTR);
+			} else {
+				__raw_writel(cfg->ch2_ptr2 + size,
+					     PRP_SOURCE_CB_PTR);
+				__raw_writel(cfg->ch2_ptr2 + size + (size >> 2),
+					     PRP_SOURCE_CR_PTR);
+			}
+		}
+	} else {		/* Pixel interleaved YUV422 or YUV444 */
+		__raw_writel(cfg->ch2_ptr, PRP_DEST_Y_PTR);
+
+		if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+			if (!cfg->ch2_ptr2)
+				__raw_writel(cfg->ch2_ptr, PRP_SOURCE_Y_PTR);
+			else
+				__raw_writel(cfg->ch2_ptr2, PRP_SOURCE_Y_PTR);
+		}
+	}
+	*prp_cntl |= PRP_CNTL_CH2B1 | PRP_CNTL_CH2B2;
+
+	return 0;
+}
+
+/*!
+ * @brief Setup PrP registers relevant to channel 1.
+ * @param cfg		Pointer to PrP configuration parameter
+ * @param prp_cntl	Holds the value for PrP control register
+ * @return		Zero on success, others on failure
+ */
+static int prphw_ch1_cfg(emma_prp_cfg * cfg, unsigned long *prp_cntl)
+{
+	int ch1_bpp = 0;
+
+	switch (cfg->ch1_pix) {
+	case PRP_PIX1_RGB332:
+		*prp_cntl |= PRP_CNTL_CH1_RGB8;
+		ch1_bpp = 1;
+		break;
+	case PRP_PIX1_RGB565:
+		*prp_cntl |= PRP_CNTL_CH1_RGB16;
+		ch1_bpp = 2;
+		break;
+	case PRP_PIX1_RGB888:
+		*prp_cntl |= PRP_CNTL_CH1_RGB32;
+		ch1_bpp = 4;
+		break;
+	case PRP_PIX1_YUYV:
+	case PRP_PIX1_YVYU:
+	case PRP_PIX1_UYVY:
+	case PRP_PIX1_VYUY:
+		*prp_cntl |= PRP_CNTL_CH1_YUV422;
+		ch1_bpp = 2;
+		break;
+	case PRP_PIX1_UNUSED:
+		return 0;
+	default:
+		pr_debug("Unsupported channel 1 pix format 0x%08X\n",
+			 cfg->ch1_pix);
+		return -1;
+	}
+
+	/* parallel or cascade resize */
+	if (cfg->ch1_scale.algo & PRP_ALGO_BYPASS)
+		*prp_cntl |= PRP_CNTL_UNCHAIN;
+
+	/* word align */
+	if (ch1_bpp == 2)
+		cfg->ch1_width &= ~1;
+	else if (ch1_bpp == 1)
+		cfg->ch1_width &= ~3;
+
+	if (!cfg->ch1_stride)
+		cfg->ch1_stride = cfg->ch1_width;
+
+	__raw_writel(cfg->ch1_pix, PRP_CH1_PIXEL_FORMAT_CNTL);
+	__raw_writel((cfg->ch1_width << 16) | cfg->ch1_height,
+		     PRP_CH1_OUT_IMAGE_SIZE);
+	__raw_writel(cfg->ch1_stride * ch1_bpp, PRP_CH1_LINE_STRIDE);
+	__raw_writel(cfg->ch1_ptr, PRP_DEST_RGB1_PTR);
+
+	/* double buffer for loop mode */
+	if ((cfg->in_csi & PRP_CSI_LOOP) == PRP_CSI_LOOP) {
+		if (cfg->ch1_ptr2)
+			__raw_writel(cfg->ch1_ptr2, PRP_DEST_RGB2_PTR);
+		else
+			__raw_writel(cfg->ch1_ptr, PRP_DEST_RGB2_PTR);
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Setup PrP registers.
+ * @param cfg	Pointer to PrP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+int prphw_cfg(emma_prp_cfg * cfg)
+{
+	unsigned long prp_cntl = 0;
+	unsigned long val;
+
+	/* input pixel format checking */
+	if (prphw_input_cfg(cfg, &prp_cntl))
+		return -1;
+
+	if (prphw_ch2_cfg(cfg, &prp_cntl))
+		return -1;
+
+	if (prphw_ch1_cfg(cfg, &prp_cntl))
+		return -1;
+
+	/* register setting */
+	__raw_writel(prp_cntl, PRP_CNTL);
+
+	/* interrupt configuration */
+	val = PRP_INTRCNTL_RDERR | PRP_INTRCNTL_LBOVF;
+	if (cfg->ch1_pix != PRP_PIX1_UNUSED)
+		val |= PRP_INTRCNTL_CH1FC | PRP_INTRCNTL_CH1WERR;
+	if (cfg->ch2_pix != PRP_PIX2_UNUSED)
+		val |=
+		    PRP_INTRCNTL_CH2FC | PRP_INTRCNTL_CH2WERR |
+		    PRP_INTRCNTL_CH2OVF;
+	__raw_writel(val, PRP_INTRCNTL);
+
+	prp_set_scaler(1, 0, &cfg->scale[0]);	/* Channel 1 width */
+	prp_set_scaler(1, 1, &cfg->scale[1]);	/* Channel 1 height */
+	prp_set_scaler(0, 0, &cfg->scale[2]);	/* Channel 2 width */
+	prp_set_scaler(0, 1, &cfg->scale[3]);	/* Channel 2 height */
+
+	return 0;
+}
+
+/*!
+ * @brief Check PrP interrupt status.
+ * @return	PrP interrupt status
+ */
+int prphw_isr(void)
+{
+	int status;
+
+	status = __raw_readl(PRP_INTRSTATUS) & 0x1FF;
+
+	if (status & (PRP_INTRSTAT_RDERR | PRP_INTRSTAT_CH1WERR |
+		      PRP_INTRSTAT_CH2WERR))
+		pr_debug("isr bus error. status= 0x%08X\n", status);
+	else if (status & PRP_INTRSTAT_CH2OVF)
+		pr_debug("isr ch 2 buffer overflow. status= 0x%08X\n", status);
+	else if (status & PRP_INTRSTAT_LBOVF)
+		pr_debug("isr line buffer overflow. status= 0x%08X\n", status);
+
+	/* silicon bug?? enable bit does not self clear? */
+	if (!(__raw_readl(PRP_CNTL) & PRP_CNTL_CH1_LOOP))
+		__raw_writel(__raw_readl(PRP_CNTL) & (~PRP_CNTL_CH1EN),
+			     PRP_CNTL);
+	if (!(__raw_readl(PRP_CNTL) & PRP_CNTL_CH2_LOOP))
+		__raw_writel(__raw_readl(PRP_CNTL) & (~PRP_CNTL_CH2EN),
+			     PRP_CNTL);
+
+	__raw_writel(status, PRP_INTRSTATUS);	/* clr irq */
+
+	return status;
+}
+
+static struct clk *emma_clk;
+
+/*!
+ * @brief  PrP module clock enable
+ */
+void prphw_init(void)
+{
+	emma_clk = clk_get(NULL, "emma_clk");
+	clk_enable(emma_clk);
+}
+
+/*!
+ * @brief PrP module clock disable
+ */
+void prphw_exit(void)
+{
+	clk_disable(emma_clk);
+	clk_put(emma_clk);
+}
diff --git a/drivers/media/video/mxc/capture/mx27_prpsw.c b/drivers/media/video/mxc/capture/mx27_prpsw.c
new file mode 100644
index 000000000000..ce7db16913ec
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mx27_prpsw.c
@@ -0,0 +1,1042 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_prpsw.c
+ *
+ * @brief MX27 Video For Linux 2 capture driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/fb.h>
+#include <linux/pci.h>
+#include <asm/cacheflush.h>
+#include <asm/io.h>
+#include <asm/irq.h>
+
+#include "mxc_v4l2_capture.h"
+#include "mx27_prp.h"
+#include "mx27_csi.h"
+#include "../drivers/video/mxc/mx2fb.h"
+#include "../opl/opl.h"
+
+#define MEAN_COEF	(SZ_COEF >> 1)
+
+static char prp_dev[] = "emma_prp";
+static int g_still_on = 0;
+static emma_prp_cfg g_prp_cfg;
+static int g_vfbuf, g_rotbuf;
+static struct tasklet_struct prp_vf_tasklet;
+
+/*
+ * The following variables represents the virtual address for the cacheable
+ * buffers accessed by SW rotation/mirroring. The rotation/mirroring in
+ * cacheable buffers has significant performance improvement than it in
+ * non-cacheable buffers.
+ */
+static char *g_vaddr_vfbuf[2] = { 0, 0 };
+static char *g_vaddr_rotbuf[2] = { 0, 0 };
+static char *g_vaddr_fb = 0;
+
+static int set_ch1_addr(emma_prp_cfg * cfg, cam_data * cam);
+static int prp_v4l2_cfg(emma_prp_cfg * cfg, cam_data * cam);
+static int prp_vf_mem_alloc(cam_data * cam);
+static void prp_vf_mem_free(cam_data * cam);
+static int prp_rot_mem_alloc(cam_data * cam);
+static void prp_rot_mem_free(cam_data * cam);
+static int prp_enc_update_eba(u32 eba, int *buffer_num);
+static int prp_enc_enable(void *private);
+static int prp_enc_disable(void *private);
+static int prp_vf_start(void *private);
+static int prp_vf_stop(void *private);
+static int prp_still_start(void *private);
+static int prp_still_stop(void *private);
+static irqreturn_t prp_isr(int irq, void *dev_id);
+static void rotation(unsigned long private);
+static int prp_resize_check_ch1(emma_prp_cfg * cfg);
+static int prp_resize_check_ch2(emma_prp_cfg * cfg);
+
+#define PRP_DUMP(val)	pr_debug("%s\t = 0x%08X\t%d\n", #val, val, val)
+
+/*!
+ * @brief Dump PrP configuration parameters.
+ * @param cfg	The pointer to PrP configuration parameter
+ */
+static void prp_cfg_dump(emma_prp_cfg * cfg)
+{
+	PRP_DUMP(cfg->in_pix);
+	PRP_DUMP(cfg->in_width);
+	PRP_DUMP(cfg->in_height);
+	PRP_DUMP(cfg->in_csi);
+	PRP_DUMP(cfg->in_line_stride);
+	PRP_DUMP(cfg->in_line_skip);
+	PRP_DUMP(cfg->in_ptr);
+
+	PRP_DUMP(cfg->ch1_pix);
+	PRP_DUMP(cfg->ch1_width);
+	PRP_DUMP(cfg->ch1_height);
+	PRP_DUMP(cfg->ch1_scale.algo);
+	PRP_DUMP(cfg->ch1_scale.width.num);
+	PRP_DUMP(cfg->ch1_scale.width.den);
+	PRP_DUMP(cfg->ch1_scale.height.num);
+	PRP_DUMP(cfg->ch1_scale.height.den);
+	PRP_DUMP(cfg->ch1_stride);
+	PRP_DUMP(cfg->ch1_ptr);
+	PRP_DUMP(cfg->ch1_ptr2);
+	PRP_DUMP(cfg->ch1_csi);
+
+	PRP_DUMP(cfg->ch2_pix);
+	PRP_DUMP(cfg->ch2_width);
+	PRP_DUMP(cfg->ch2_height);
+	PRP_DUMP(cfg->ch2_scale.algo);
+	PRP_DUMP(cfg->ch2_scale.width.num);
+	PRP_DUMP(cfg->ch2_scale.width.den);
+	PRP_DUMP(cfg->ch2_scale.height.num);
+	PRP_DUMP(cfg->ch2_scale.height.den);
+	PRP_DUMP(cfg->ch2_ptr);
+	PRP_DUMP(cfg->ch2_ptr2);
+	PRP_DUMP(cfg->ch2_csi);
+}
+
+/*!
+ * @brief Set PrP channel 1 output address.
+ * @param cfg	Pointer to emma_prp_cfg structure
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static int set_ch1_addr(emma_prp_cfg * cfg, cam_data * cam)
+{
+	if (cam->rotation != V4L2_MXC_ROTATE_NONE) {
+		cfg->ch1_ptr = (unsigned int)cam->rot_vf_bufs[0];
+		cfg->ch1_ptr2 = (unsigned int)cam->rot_vf_bufs[1];
+		if ((cam->rotation == V4L2_MXC_ROTATE_90_RIGHT)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_VFLIP)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_HFLIP)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_LEFT))
+			cfg->ch1_stride = cam->win.w.height;
+		else
+			cfg->ch1_stride = cam->win.w.width;
+
+		if (cam->v4l2_fb.flags != V4L2_FBUF_FLAG_OVERLAY) {
+			struct fb_info *fb = cam->overlay_fb;
+			if (!fb)
+				return -1;
+			if (g_vaddr_fb)
+				iounmap(g_vaddr_fb);
+			g_vaddr_fb = ioremap_cached(fb->fix.smem_start,
+						    fb->fix.smem_len);
+			if (!g_vaddr_fb)
+				return -1;
+		}
+	} else if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		cfg->ch1_ptr = (unsigned int)cam->vf_bufs[0];
+		cfg->ch1_ptr2 = (unsigned int)cam->vf_bufs[1];
+		cfg->ch1_stride = cam->win.w.width;
+	} else {
+		struct fb_info *fb = cam->overlay_fb;
+
+		if (!fb)
+			return -1;
+
+		cfg->ch1_ptr = fb->fix.smem_start;
+		cfg->ch1_ptr += cam->win.w.top * fb->var.xres_virtual
+		    * (fb->var.bits_per_pixel >> 3)
+		    + cam->win.w.left * (fb->var.bits_per_pixel >> 3);
+		cfg->ch1_ptr2 = cfg->ch1_ptr;
+		cfg->ch1_stride = fb->var.xres_virtual;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Setup PrP configuration parameters.
+ * @param cfg	Pointer to emma_prp_cfg structure
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_v4l2_cfg(emma_prp_cfg * cfg, cam_data * cam)
+{
+	cfg->in_pix = PRP_PIXIN_YUYV;
+	cfg->in_width = cam->crop_current.width;
+	cfg->in_height = cam->crop_current.height;
+	cfg->in_line_stride = cam->crop_current.left;
+	cfg->in_line_skip = cam->crop_current.top;
+	cfg->in_ptr = 0;
+	cfg->in_csi = PRP_CSI_LOOP;
+	memset(cfg->in_csc, 0, sizeof(cfg->in_csc));
+
+	if (cam->overlay_on) {
+		/* Convert V4L2 pixel format to PrP pixel format */
+		switch (cam->v4l2_fb.fmt.pixelformat) {
+		case V4L2_PIX_FMT_RGB332:
+			cfg->ch1_pix = PRP_PIX1_RGB332;
+			break;
+		case V4L2_PIX_FMT_RGB32:
+		case V4L2_PIX_FMT_BGR32:
+			cfg->ch1_pix = PRP_PIX1_RGB888;
+			break;
+		case V4L2_PIX_FMT_YUYV:
+			cfg->ch1_pix = PRP_PIX1_YUYV;
+			break;
+		case V4L2_PIX_FMT_UYVY:
+			cfg->ch1_pix = PRP_PIX1_UYVY;
+			break;
+		case V4L2_PIX_FMT_RGB565:
+		default:
+			cfg->ch1_pix = PRP_PIX1_RGB565;
+			break;
+		}
+		if ((cam->rotation == V4L2_MXC_ROTATE_90_RIGHT)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_VFLIP)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_HFLIP)
+		    || (cam->rotation == V4L2_MXC_ROTATE_90_LEFT)) {
+			cfg->ch1_width = cam->win.w.height;
+			cfg->ch1_height = cam->win.w.width;
+		} else {
+			cfg->ch1_width = cam->win.w.width;
+			cfg->ch1_height = cam->win.w.height;
+		}
+
+		if (set_ch1_addr(cfg, cam))
+			return -1;
+	} else {
+		cfg->ch1_pix = PRP_PIX1_UNUSED;
+		cfg->ch1_width = cfg->in_width;
+		cfg->ch1_height = cfg->in_height;
+	}
+	cfg->ch1_scale.algo = 0;
+	cfg->ch1_scale.width.num = cfg->in_width;
+	cfg->ch1_scale.width.den = cfg->ch1_width;
+	cfg->ch1_scale.height.num = cfg->in_height;
+	cfg->ch1_scale.height.den = cfg->ch1_height;
+	cfg->ch1_csi = PRP_CSI_EN;
+
+	if (cam->capture_on || g_still_on) {
+		switch (cam->v2f.fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_YUYV:
+			cfg->ch2_pix = PRP_PIX2_YUV422;
+			break;
+		case V4L2_PIX_FMT_YUV420:
+			cfg->ch2_pix = PRP_PIX2_YUV420;
+			break;
+			/*
+			 * YUV444 is not defined by V4L2.
+			 * We support it in default case.
+			 */
+		default:
+			cfg->ch2_pix = PRP_PIX2_YUV444;
+			break;
+		}
+		cfg->ch2_width = cam->v2f.fmt.pix.width;
+		cfg->ch2_height = cam->v2f.fmt.pix.height;
+	} else {
+		cfg->ch2_pix = PRP_PIX2_UNUSED;
+		cfg->ch2_width = cfg->in_width;
+		cfg->ch2_height = cfg->in_height;
+	}
+	cfg->ch2_scale.algo = 0;
+	cfg->ch2_scale.width.num = cfg->in_width;
+	cfg->ch2_scale.width.den = cfg->ch2_width;
+	cfg->ch2_scale.height.num = cfg->in_height;
+	cfg->ch2_scale.height.den = cfg->ch2_height;
+	cfg->ch2_csi = PRP_CSI_EN;
+
+	memset(cfg->scale, 0, sizeof(cfg->scale));
+	cfg->scale[0].algo = cfg->ch1_scale.algo & 3;
+	cfg->scale[1].algo = (cfg->ch1_scale.algo >> 2) & 3;
+	cfg->scale[2].algo = cfg->ch2_scale.algo & 3;
+	cfg->scale[3].algo = (cfg->ch2_scale.algo >> 2) & 3;
+
+	prp_cfg_dump(cfg);
+
+	if (prp_resize_check_ch2(cfg))
+		return -1;
+
+	if (prp_resize_check_ch1(cfg))
+		return -1;
+
+	return 0;
+}
+
+/*!
+ * @brief PrP interrupt handler
+ */
+static irqreturn_t prp_isr(int irq, void *dev_id)
+{
+	int status;
+	cam_data *cam = (cam_data *) dev_id;
+
+	status = prphw_isr();
+
+	if (g_still_on && (status & PRP_INTRSTAT_CH2BUF1)) {
+		prp_still_stop(cam);
+		cam->still_counter++;
+		wake_up_interruptible(&cam->still_queue);
+		/*
+		 * Still & video capture use the same PrP channel 2.
+		 * They are execlusive.
+		 */
+	} else if (cam->capture_on) {
+		if (status & (PRP_INTRSTAT_CH2BUF1 | PRP_INTRSTAT_CH2BUF2)) {
+			cam->enc_callback(0, cam);
+		}
+	}
+	if (cam->overlay_on
+	    && (status & (PRP_INTRSTAT_CH1BUF1 | PRP_INTRSTAT_CH1BUF2))) {
+		if (cam->rotation != V4L2_MXC_ROTATE_NONE) {
+			g_rotbuf = (status & PRP_INTRSTAT_CH1BUF1) ? 0 : 1;
+			tasklet_schedule(&prp_vf_tasklet);
+		} else if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+			struct fb_gwinfo gwinfo;
+
+			gwinfo.enabled = 1;
+			gwinfo.alpha_value = 255;
+			gwinfo.ck_enabled = 0;
+			gwinfo.xpos = cam->win.w.left;
+			gwinfo.ypos = cam->win.w.top;
+			gwinfo.xres = cam->win.w.width;
+			gwinfo.yres = cam->win.w.height;
+			gwinfo.xres_virtual = cam->win.w.width;
+			gwinfo.vs_reversed = 0;
+			if (status & PRP_INTRSTAT_CH1BUF1)
+				gwinfo.base = (unsigned long)cam->vf_bufs[0];
+			else
+				gwinfo.base = (unsigned long)cam->vf_bufs[1];
+
+			mx2_gw_set(&gwinfo);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * @brief PrP initialization.
+ * @param dev_id	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_init(void *dev_id)
+{
+	enable_irq(MXC_INT_EMMAPRP);
+	if (request_irq(MXC_INT_EMMAPRP, prp_isr, 0, prp_dev, dev_id))
+		return -1;
+	prphw_init();
+
+	return 0;
+}
+
+/*!
+ * @brief PrP initialization.
+ * @param dev_id	Pointer to cam_data structure
+ */
+void prp_exit(void *dev_id)
+{
+	prphw_exit();
+	disable_irq(MXC_INT_EMMAPRP);
+	free_irq(MXC_INT_EMMAPRP, dev_id);
+}
+
+/*!
+ * @brief Update PrP channel 2 output buffer address.
+ * @param eba		Physical address for PrP output buffer
+ * @param buffer_num	The PrP channel 2 buffer number to be updated
+ * @return		Zero on success, others on failure
+ */
+static int prp_enc_update_eba(u32 eba, int *buffer_num)
+{
+	if (*buffer_num) {
+		g_prp_cfg.ch2_ptr2 = eba;
+		prphw_ch2ptr2(&g_prp_cfg);
+		*buffer_num = 0;
+	} else {
+		g_prp_cfg.ch2_ptr = eba;
+		prphw_ch2ptr(&g_prp_cfg);
+		*buffer_num = 1;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Enable PrP for encoding.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_enc_enable(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (prp_v4l2_cfg(&g_prp_cfg, cam))
+		return -1;
+
+	csi_enable_mclk(CSI_MCLK_ENC, true, true);
+	prphw_reset();
+
+	if (prphw_cfg(&g_prp_cfg))
+		return -1;
+
+	prphw_enable(cam->overlay_on ? (PRP_CHANNEL_1 | PRP_CHANNEL_2)
+		     : PRP_CHANNEL_2);
+
+	return 0;
+}
+
+/*!
+ * @brief Disable PrP for encoding.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_enc_disable(void *private)
+{
+	prphw_disable(PRP_CHANNEL_2);
+	csi_enable_mclk(CSI_MCLK_ENC, false, false);
+
+	return 0;
+}
+
+/*!
+ * @brief Setup encoding functions.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_enc_select(void *private)
+{
+	int ret = 0;
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->enc_update_eba = prp_enc_update_eba;
+		cam->enc_enable = prp_enc_enable;
+		cam->enc_disable = prp_enc_disable;
+	} else
+		ret = -EIO;
+
+	return ret;
+}
+
+/*!
+ * @brief Uninstall encoding functions.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_enc_deselect(void *private)
+{
+	int ret = 0;
+	cam_data *cam = (cam_data *) private;
+
+	ret = prp_enc_disable(private);
+
+	if (cam) {
+		cam->enc_update_eba = NULL;
+		cam->enc_enable = NULL;
+		cam->enc_disable = NULL;
+	}
+
+	return ret;
+}
+
+/*!
+ * @brief Allocate memory for overlay.
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_vf_mem_alloc(cam_data * cam)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		cam->vf_bufs_size[i] = cam->win.w.width * cam->win.w.height * 2;
+		cam->vf_bufs_vaddr[i] = dma_alloc_coherent(0,
+							   cam->vf_bufs_size[i],
+							   &cam->vf_bufs[i],
+							   GFP_DMA |
+							   GFP_KERNEL);
+		if (!cam->vf_bufs_vaddr[i]) {
+			pr_debug("Failed to alloc memory for vf.\n");
+			prp_vf_mem_free(cam);
+			return -1;
+		}
+
+		g_vaddr_vfbuf[i] =
+		    ioremap_cached(cam->vf_bufs[i], cam->vf_bufs_size[i]);
+		if (!g_vaddr_vfbuf[i]) {
+			pr_debug("Failed to ioremap_cached() for vf.\n");
+			prp_vf_mem_free(cam);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Free memory for overlay.
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static void prp_vf_mem_free(cam_data * cam)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		if (cam->vf_bufs_vaddr[i]) {
+			dma_free_coherent(0,
+					  cam->vf_bufs_size[i],
+					  cam->vf_bufs_vaddr[i],
+					  cam->vf_bufs[i]);
+		}
+		cam->vf_bufs[i] = 0;
+		cam->vf_bufs_vaddr[i] = 0;
+		cam->vf_bufs_size[i] = 0;
+		if (g_vaddr_vfbuf[i]) {
+			iounmap(g_vaddr_vfbuf[i]);
+			g_vaddr_vfbuf[i] = 0;
+		}
+	}
+}
+
+/*!
+ * @brief Allocate intermediate memory for overlay rotation/mirroring.
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_rot_mem_alloc(cam_data * cam)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		cam->rot_vf_buf_size[i] =
+		    cam->win.w.width * cam->win.w.height * 2;
+		cam->rot_vf_bufs_vaddr[i] =
+		    dma_alloc_coherent(0, cam->rot_vf_buf_size[i],
+				       &cam->rot_vf_bufs[i],
+				       GFP_DMA | GFP_KERNEL);
+		if (!cam->rot_vf_bufs_vaddr[i]) {
+			pr_debug("Failed to alloc memory for vf rotation.\n");
+			prp_rot_mem_free(cam);
+			return -1;
+		}
+
+		g_vaddr_rotbuf[i] =
+		    ioremap_cached(cam->rot_vf_bufs[i],
+				   cam->rot_vf_buf_size[i]);
+		if (!g_vaddr_rotbuf[i]) {
+			pr_debug
+			    ("Failed to ioremap_cached() for rotation buffer.\n");
+			prp_rot_mem_free(cam);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Free intermedaite memory for overlay rotation/mirroring.
+ * @param cam	Pointer to cam_data structure
+ * @return	Zero on success, others on failure
+ */
+static void prp_rot_mem_free(cam_data * cam)
+{
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		if (cam->rot_vf_bufs_vaddr[i]) {
+			dma_free_coherent(0,
+					  cam->rot_vf_buf_size[i],
+					  cam->rot_vf_bufs_vaddr[i],
+					  cam->rot_vf_bufs[i]);
+		}
+		cam->rot_vf_bufs[i] = 0;
+		cam->rot_vf_bufs_vaddr[i] = 0;
+		cam->rot_vf_buf_size[i] = 0;
+		if (g_vaddr_rotbuf[i]) {
+			iounmap(g_vaddr_rotbuf[i]);
+			g_vaddr_rotbuf[i] = 0;
+		}
+	}
+}
+
+/*!
+ * @brief Start overlay (view finder).
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_vf_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		prp_vf_mem_free(cam);
+		if (prp_vf_mem_alloc(cam)) {
+			pr_info("Error to allocate vf buffer\n");
+			return -ENOMEM;
+		}
+	}
+
+	if (cam->rotation != V4L2_MXC_ROTATE_NONE) {
+		prp_rot_mem_free(cam);
+		if (prp_rot_mem_alloc(cam)) {
+			pr_info("Error to allocate rotation buffer\n");
+			prp_vf_mem_free(cam);
+			return -ENOMEM;
+		}
+	}
+
+	if (prp_v4l2_cfg(&g_prp_cfg, cam)) {
+		prp_vf_mem_free(cam);
+		prp_rot_mem_free(cam);
+		return -1;
+	}
+
+	csi_enable_mclk(CSI_MCLK_VF, true, true);
+	prphw_reset();
+
+	if (prphw_cfg(&g_prp_cfg)) {
+		prp_vf_mem_free(cam);
+		prp_rot_mem_free(cam);
+		return -1;
+	}
+	g_vfbuf = g_rotbuf = 0;
+	tasklet_init(&prp_vf_tasklet, rotation, (unsigned long)private);
+
+	prphw_enable(cam->capture_on ? (PRP_CHANNEL_1 | PRP_CHANNEL_2)
+		     : PRP_CHANNEL_1);
+
+	return 0;
+}
+
+/*!
+ * @brief Stop overlay (view finder).
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_vf_stop(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	prphw_disable(PRP_CHANNEL_1);
+
+	csi_enable_mclk(CSI_MCLK_VF, false, false);
+	tasklet_kill(&prp_vf_tasklet);
+
+	if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		struct fb_gwinfo gwinfo;
+
+		/* Disable graphic window */
+		gwinfo.enabled = 0;
+		mx2_gw_set(&gwinfo);
+
+		prp_vf_mem_free(cam);
+	}
+	prp_rot_mem_free(cam);
+	if (g_vaddr_fb) {
+		iounmap(g_vaddr_fb);
+		g_vaddr_fb = 0;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Setup overlay functions.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_vf_select(void *private)
+{
+	int ret = 0;
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->vf_start_sdc = prp_vf_start;
+		cam->vf_stop_sdc = prp_vf_stop;
+		cam->overlay_active = false;
+	} else
+		ret = -EIO;
+
+	return ret;
+}
+
+/*!
+ * @brief Uninstall overlay functions.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_vf_deselect(void *private)
+{
+	int ret = 0;
+	cam_data *cam = (cam_data *) private;
+
+	ret = prp_vf_stop(private);
+
+	if (cam) {
+		cam->vf_start_sdc = NULL;
+		cam->vf_stop_sdc = NULL;
+	}
+
+	return ret;
+}
+
+/*!
+ * @brief Start still picture capture.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_still_start(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	g_still_on = 1;
+	g_prp_cfg.ch2_ptr = (unsigned int)cam->still_buf;
+	g_prp_cfg.ch2_ptr2 = 0;
+
+	if (prp_v4l2_cfg(&g_prp_cfg, cam))
+		return -1;
+
+	csi_enable_mclk(CSI_MCLK_RAW, true, true);
+	prphw_reset();
+
+	if (prphw_cfg(&g_prp_cfg)) {
+		g_still_on = 0;
+		return -1;
+	}
+
+	prphw_enable(cam->overlay_on ? (PRP_CHANNEL_1 | PRP_CHANNEL_2)
+		     : PRP_CHANNEL_2);
+
+	return 0;
+}
+
+/*!
+ * @brief Stop still picture capture.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+static int prp_still_stop(void *private)
+{
+	prphw_disable(PRP_CHANNEL_2);
+
+	csi_enable_mclk(CSI_MCLK_RAW, false, false);
+
+	g_still_on = 0;
+
+	return 0;
+}
+
+/*!
+ * @brief Setup functions for still picture capture.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_still_select(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+
+	if (cam) {
+		cam->csi_start = prp_still_start;
+		cam->csi_stop = prp_still_stop;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Uninstall functions for still picture capture.
+ * @param private	Pointer to cam_data structure
+ * @return		Zero on success, others on failure
+ */
+int prp_still_deselect(void *private)
+{
+	cam_data *cam = (cam_data *) private;
+	int err = 0;
+
+	err = prp_still_stop(cam);
+
+	if (cam) {
+		cam->csi_start = NULL;
+		cam->csi_stop = NULL;
+	}
+
+	return err;
+}
+
+/*!
+ * @brief Perform software rotation or mirroring
+ * @param private	Argument passed to the tasklet
+ */
+static void rotation(unsigned long private)
+{
+	char *src, *dst;
+	int width, height, s_stride, d_stride;
+	int size;
+	cam_data *cam = (cam_data *) private;
+
+	src = g_vaddr_rotbuf[g_rotbuf];
+	size = cam->rot_vf_buf_size[g_rotbuf];
+
+	if ((cam->rotation == V4L2_MXC_ROTATE_90_RIGHT)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_VFLIP)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_RIGHT_HFLIP)
+	    || (cam->rotation == V4L2_MXC_ROTATE_90_LEFT)) {
+		width = cam->win.w.height;
+		height = cam->win.w.width;
+		s_stride = cam->win.w.height << 1;
+	} else {
+		width = cam->win.w.width;
+		height = cam->win.w.height;
+		s_stride = cam->win.w.width << 1;
+	}
+
+	if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		dst = g_vaddr_vfbuf[g_vfbuf];
+		d_stride = cam->win.w.width << 1;
+	} else {		/* The destination is the framebuffer */
+		struct fb_info *fb = cam->overlay_fb;
+		if (!fb)
+			return;
+		dst = g_vaddr_fb;
+		dst += cam->win.w.top * fb->var.xres_virtual
+		    * (fb->var.bits_per_pixel >> 3)
+		    + cam->win.w.left * (fb->var.bits_per_pixel >> 3);
+		d_stride = fb->var.xres_virtual << 1;
+	}
+
+	/*
+	 * Invalidate the data in cache before performing the SW rotaion
+	 * or mirroring in case the image size is less than QVGA. For image
+	 * larger than QVGA it is not invalidated becase the invalidation
+	 * will consume much time while we don't see any artifacts on the
+	 * output if we don't perform invalidation for them.
+	 * Similarly we don't flush the data after SW rotation/mirroring.
+	 */
+	if (size < 320 * 240 * 2)
+		dmac_inv_range(src, src + size);
+	switch (cam->rotation) {
+	case V4L2_MXC_ROTATE_VERT_FLIP:
+		opl_vmirror_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	case V4L2_MXC_ROTATE_HORIZ_FLIP:
+		opl_hmirror_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	case V4L2_MXC_ROTATE_180:
+		opl_rotate180_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	case V4L2_MXC_ROTATE_90_RIGHT:
+		opl_rotate90_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	case V4L2_MXC_ROTATE_90_RIGHT_VFLIP:
+		opl_rotate90_vmirror_u16(src, s_stride, width, height, dst,
+					 d_stride);
+		break;
+	case V4L2_MXC_ROTATE_90_RIGHT_HFLIP:
+		/* ROTATE_90_RIGHT_HFLIP = ROTATE_270_RIGHT_VFLIP */
+		opl_rotate270_vmirror_u16(src, s_stride, width, height, dst,
+					  d_stride);
+		break;
+	case V4L2_MXC_ROTATE_90_LEFT:
+		opl_rotate270_u16(src, s_stride, width, height, dst, d_stride);
+		break;
+	default:
+		return;
+	}
+
+	/* Config and display the graphic window */
+	if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		struct fb_gwinfo gwinfo;
+
+		gwinfo.enabled = 1;
+		gwinfo.alpha_value = 255;
+		gwinfo.ck_enabled = 0;
+		gwinfo.xpos = cam->win.w.left;
+		gwinfo.ypos = cam->win.w.top;
+		gwinfo.xres = cam->win.w.width;
+		gwinfo.yres = cam->win.w.height;
+		gwinfo.xres_virtual = cam->win.w.width;
+		gwinfo.vs_reversed = 0;
+		gwinfo.base = (unsigned long)cam->vf_bufs[g_vfbuf];
+		mx2_gw_set(&gwinfo);
+
+		g_vfbuf = g_vfbuf ? 0 : 1;
+	}
+}
+
+/*
+ * @brief Check if the resize ratio is supported based on the input and output
+ *        dimension
+ * @param	input	input dimension
+ * @param	output	output dimension
+ * @return	output dimension (should equal the parameter *output*)
+ * 		-1 on failure
+ */
+static int check_simple(scale_t * scale, int input, int output)
+{
+	unsigned short int_out;	/* PrP internel width or height */
+	unsigned short orig_out = output;
+
+	if (prp_scale(scale, input, output, input, &orig_out, &int_out, 0))
+		return -1;	/* resize failed */
+	else
+		return int_out;
+}
+
+/*
+ * @brief Check if the resize ratio is supported based on the input and output
+ *        dimension
+ * @param	input	input dimension
+ * @param	output	output dimension
+ * @return	output dimension, may be rounded.
+ * 		-1 on failure
+ */
+static int check_simple_retry(scale_t * scale, int input, int output)
+{
+	unsigned short int_out;	/* PrP internel width or height */
+	unsigned short orig_out = output;
+
+	if (prp_scale(scale, input, output, input, &orig_out, &int_out,
+		      SCALE_RETRY))
+		return -1;	/* resize failed */
+	else
+		return int_out;
+}
+
+/*!
+ * @brief Check if the resize ratio is supported by PrP channel 1
+ * @param cfg	Pointer to emma_prp_cfg structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_resize_check_ch1(emma_prp_cfg * cfg)
+{
+	int in_w, in_h, ch1_w, ch1_h, ch2_w, ch2_h, w, h;
+	scale_t *pscale = &cfg->scale[0];	/* Ch1 width resize coeff */
+
+	if (cfg->ch1_pix == PRP_PIX1_UNUSED)
+		return 0;
+
+	in_w = cfg->in_width;
+	in_h = cfg->in_height;
+	ch1_w = cfg->ch1_width;
+	ch1_h = cfg->ch1_height;
+	ch2_w = cfg->ch2_width;
+	ch2_h = cfg->ch2_height;
+
+	/*
+	 * For channel 1, try parallel resize first. If the resize
+	 * ratio is not exactly supported, try cascade resize. If it
+	 * still fails, use parallel resize but with rounded value.
+	 */
+	w = check_simple(pscale, in_w, ch1_w);
+	h = check_simple(pscale + 1, in_h, ch1_h);
+	if ((w == ch1_w) && (h == ch1_h))
+		goto exit_parallel;
+
+	if (cfg->ch2_pix != PRP_PIX2_UNUSED) {
+		/*
+		 * Channel 2 is already used. The pscale is still pointing
+		 * to ch1 resize coeff for temporary use.
+		 */
+		w = check_simple(pscale, in_w, ch2_w);
+		h = check_simple(pscale + 1, in_h, ch2_h);
+		if ((w == ch2_w) && (h == ch2_h)) {
+			/* Try cascade resize now */
+			w = check_simple(pscale, ch2_w, ch1_w);
+			h = check_simple(pscale + 1, ch2_h, ch1_h);
+			if ((w == ch1_w) && (h == ch1_h))
+				goto exit_cascade;
+		}
+	} else {
+		/*
+		 * Try cascade resize for width, width is multiple of 2.
+		 * Channel 2 is not used. So we have more values to pick
+		 * for channel 2 resize.
+		 */
+		for (w = in_w - 2; w > ch1_w; w -= 2) {
+			/* Ch2 width resize */
+			if (check_simple(pscale + 2, in_w, w) != w)
+				continue;
+			/* Ch1 width resize */
+			if (check_simple(pscale, w, ch1_w) != ch1_w)
+				continue;
+			break;
+		}
+		if ((ch2_w = w) > ch1_w) {
+			/* try cascade resize for height */
+			for (h = in_h - 1; h > ch1_h; h--) {
+				/* Ch2 height resize */
+				if (check_simple(pscale + 3, in_h, h) != h)
+					continue;
+				/* Ch1 height resize */
+				if (check_simple(pscale + 1, h, ch1_h) != ch1_h)
+					continue;
+				break;
+			}
+			if ((ch2_h = h) > ch1_h)
+				goto exit_cascade;
+		}
+	}
+
+	/* Have to try parallel resize again and round the dimensions */
+	w = check_simple_retry(pscale, in_w, ch1_w);
+	h = check_simple_retry(pscale + 1, in_h, ch1_h);
+	if ((w != -1) && (h != -1))
+		goto exit_parallel;
+
+	pr_debug("Ch1 resize error.\n");
+	return -1;
+
+      exit_parallel:
+	cfg->ch1_scale.algo |= PRP_ALGO_BYPASS;
+	pr_debug("ch1 parallel resize.\n");
+	pr_debug("original width = %d internel width = %d\n", ch1_w, w);
+	pr_debug("original height = %d internel height = %d\n", ch1_h, h);
+	return 0;
+
+      exit_cascade:
+	cfg->ch1_scale.algo &= ~PRP_ALGO_BYPASS;
+	pr_debug("ch1 cascade resize.\n");
+	pr_debug("[width] in : ch2 : ch1=%d : %d : %d\n", in_w, ch2_w, ch1_w);
+	pr_debug("[height] in : ch2 : ch1=%d : %d : %d\n", in_h, ch2_h, ch1_h);
+	return 0;
+}
+
+/*!
+ * @brief Check if the resize ratio is supported by PrP channel 2
+ * @param cfg	Pointer to emma_prp_cfg structure
+ * @return	Zero on success, others on failure
+ */
+static int prp_resize_check_ch2(emma_prp_cfg * cfg)
+{
+	int w, h;
+	scale_t *pscale = &cfg->scale[2];	/* Ch2 width resize coeff */
+
+	if (cfg->ch2_pix == PRP_PIX2_UNUSED)
+		return 0;
+
+	w = check_simple_retry(pscale, cfg->in_width, cfg->ch2_width);
+	h = check_simple_retry(pscale + 1, cfg->in_height, cfg->ch2_height);
+	if ((w != -1) && (h != -1)) {
+		pr_debug("Ch2 resize.\n");
+		pr_debug("Original width = %d internel width = %d\n",
+			 cfg->ch2_width, w);
+		pr_debug("Original height = %d internel height = %d\n",
+			 cfg->ch2_height, h);
+		return 0;
+	} else {
+		pr_debug("Ch2 resize error.\n");
+		return -1;
+	}
+}
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
new file mode 100644
index 000000000000..1852d702e505
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
@@ -0,0 +1,2593 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/media/video/mxc/capture/mxc_v4l2_capture.c
+ *
+ * @brief Mxc Video For Linux 2 driver
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/io.h>
+#include <linux/semaphore.h>
+#include <linux/pagemap.h>
+#include <linux/vmalloc.h>
+#include <linux/types.h>
+#include <linux/fb.h>
+#include <linux/dma-mapping.h>
+#include <linux/mxcfb.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+#include "ipu_prp_sw.h"
+
+static int video_nr = -1;
+static cam_data *g_cam;
+
+/*! This data is used for the output to the display. */
+#define MXC_V4L2_CAPTURE_NUM_OUTPUTS	3
+#define MXC_V4L2_CAPTURE_NUM_INPUTS	2
+static struct v4l2_output mxc_capture_outputs[MXC_V4L2_CAPTURE_NUM_OUTPUTS] = {
+	{
+	 .index = 0,
+	 .name = "DISP3 BG",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 },
+	{
+	 .index = 1,
+	 .name = "DISP0",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 },
+	{
+	 .index = 2,
+	 .name = "DISP3 FG",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 },
+};
+
+static struct v4l2_input mxc_capture_inputs[MXC_V4L2_CAPTURE_NUM_INPUTS] = {
+	{
+	 .index = 0,
+	 .name = "CSI IC MEM",
+	 .type = V4L2_INPUT_TYPE_CAMERA,
+	 .audioset = 0,
+	 .tuner = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 .status = 0,
+	 },
+	{
+	 .index = 1,
+	 .name = "CSI MEM",
+	 .type = V4L2_INPUT_TYPE_CAMERA,
+	 .audioset = 0,
+	 .tuner = 0,
+	 .std = V4L2_STD_UNKNOWN,
+	 .status = V4L2_IN_ST_NO_POWER,
+	 },
+};
+
+/*! List of TV input video formats supported. The video formats is corresponding
+ * to the v4l2_id in video_fmt_t.
+ * Currently, only PAL and NTSC is supported. Needs to be expanded in the
+ * future.
+ */
+typedef enum {
+	TV_NTSC = 0,		/*!< Locked on (M) NTSC video signal. */
+	TV_PAL,			/*!< (B, G, H, I, N)PAL video signal. */
+	TV_NOT_LOCKED,		/*!< Not locked on a signal. */
+} video_fmt_idx;
+
+/*! Number of video standards supported (including 'not locked' signal). */
+#define TV_STD_MAX		(TV_NOT_LOCKED + 1)
+
+/*! Video format structure. */
+typedef struct {
+	int v4l2_id;		/*!< Video for linux ID. */
+	char name[16];		/*!< Name (e.g., "NTSC", "PAL", etc.) */
+	u16 raw_width;		/*!< Raw width. */
+	u16 raw_height;		/*!< Raw height. */
+	u16 active_width;	/*!< Active width. */
+	u16 active_height;	/*!< Active height. */
+	u16 active_top;		/*!< Active top. */
+	u16 active_left;	/*!< Active left. */
+} video_fmt_t;
+
+/*!
+ * Description of video formats supported.
+ *
+ *  PAL: raw=720x625, active=720x576.
+ *  NTSC: raw=720x525, active=720x480.
+ */
+static video_fmt_t video_fmts[] = {
+	{			/*! NTSC */
+	 .v4l2_id = V4L2_STD_NTSC,
+	 .name = "NTSC",
+	 .raw_width = 720 - 1,		/* SENS_FRM_WIDTH */
+	 .raw_height = 288 - 1,		/* SENS_FRM_HEIGHT */
+	 .active_width = 720,		/* ACT_FRM_WIDTH plus 1 */
+	 .active_height = (480 / 2),	/* ACT_FRM_HEIGHT plus 1 */
+	 .active_top = 12,
+	 .active_left = 0,
+	 },
+	{			/*! (B, G, H, I, N) PAL */
+	 .v4l2_id = V4L2_STD_PAL,
+	 .name = "PAL",
+	 .raw_width = 720 - 1,
+	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .active_width = 720,
+	 .active_height = (576 / 2),
+	 .active_top = 0,
+	 .active_left = 0,
+	 },
+	{			/*! Unlocked standard */
+	 .v4l2_id = V4L2_STD_ALL,
+	 .name = "Autodetect",
+	 .raw_width = 720 - 1,
+	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .active_width = 720,
+	 .active_height = (576 / 2),
+	 .active_top = 0,
+	 .active_left = 0,
+	 },
+};
+
+/*!* Standard index of TV. */
+static video_fmt_idx video_index = TV_NOT_LOCKED;
+
+static int mxc_v4l2_master_attach(struct v4l2_int_device *slave);
+static void mxc_v4l2_master_detach(struct v4l2_int_device *slave);
+static u8 camera_power(cam_data *cam, bool cameraOn);
+
+/*! Information about this driver. */
+static struct v4l2_int_master mxc_v4l2_master = {
+	.attach = mxc_v4l2_master_attach,
+	.detach = mxc_v4l2_master_detach,
+};
+
+static struct v4l2_int_device mxc_v4l2_int_device = {
+	.module = THIS_MODULE,
+	.name = "mxc_v4l2_cap",
+	.type = v4l2_int_type_master,
+	.u = {
+		.master = &mxc_v4l2_master,
+		},
+};
+
+/***************************************************************************
+ * Functions for handling Frame buffers.
+ **************************************************************************/
+
+/*!
+ * Free frame buffers
+ *
+ * @param cam      Structure cam_data *
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_frame_buf(cam_data *cam)
+{
+	int i;
+
+	pr_debug("MVC: In mxc_free_frame_buf\n");
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		if (cam->frame[i].vaddress != 0) {
+			dma_free_coherent(0, cam->frame[i].buffer.length,
+					  cam->frame[i].vaddress,
+					  cam->frame[i].paddress);
+			cam->frame[i].vaddress = 0;
+		}
+	}
+
+	return 0;
+}
+
+/*!
+ * Allocate frame buffers
+ *
+ * @param cam      Structure cam_data*
+ * @param count    int number of buffer need to allocated
+ *
+ * @return status  -0 Successfully allocated a buffer, -ENOBUFS	failed.
+ */
+static int mxc_allocate_frame_buf(cam_data *cam, int count)
+{
+	int i;
+
+	pr_debug("In MVC:mxc_allocate_frame_buf - size=%d\n",
+		cam->v2f.fmt.pix.sizeimage);
+
+	for (i = 0; i < count; i++) {
+		cam->frame[i].vaddress =
+		    dma_alloc_coherent(0,
+				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				       &cam->frame[i].paddress,
+				       GFP_DMA | GFP_KERNEL);
+		if (cam->frame[i].vaddress == 0) {
+			pr_err("ERROR: v4l2 capture: "
+				"mxc_allocate_frame_buf failed.\n");
+			mxc_free_frame_buf(cam);
+			return -ENOBUFS;
+		}
+		cam->frame[i].buffer.index = i;
+		cam->frame[i].buffer.flags = V4L2_BUF_FLAG_MAPPED;
+		cam->frame[i].buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cam->frame[i].buffer.length =
+		    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
+		cam->frame[i].buffer.memory = V4L2_MEMORY_MMAP;
+		cam->frame[i].buffer.m.offset = cam->frame[i].paddress;
+		cam->frame[i].index = i;
+	}
+
+	return 0;
+}
+
+/*!
+ * Free frame buffers status
+ *
+ * @param cam    Structure cam_data *
+ *
+ * @return none
+ */
+static void mxc_free_frames(cam_data *cam)
+{
+	int i;
+
+	pr_debug("In MVC:mxc_free_frames\n");
+
+	for (i = 0; i < FRAME_NUM; i++) {
+		cam->frame[i].buffer.flags = V4L2_BUF_FLAG_MAPPED;
+	}
+
+	cam->enc_counter = 0;
+	cam->skip_frame = 0;
+	INIT_LIST_HEAD(&cam->ready_q);
+	INIT_LIST_HEAD(&cam->working_q);
+	INIT_LIST_HEAD(&cam->done_q);
+}
+
+/*!
+ * Return the buffer status
+ *
+ * @param cam 	   Structure cam_data *
+ * @param buf      Structure v4l2_buffer *
+ *
+ * @return status  0 success, EINVAL failed.
+ */
+static int mxc_v4l2_buffer_status(cam_data *cam, struct v4l2_buffer *buf)
+{
+	pr_debug("In MVC:mxc_v4l2_buffer_status\n");
+
+	if (buf->index < 0 || buf->index >= FRAME_NUM) {
+		pr_err("ERROR: v4l2 capture: mxc_v4l2_buffer_status buffers "
+		       "not allocated\n");
+		return -EINVAL;
+	}
+
+	memcpy(buf, &(cam->frame[buf->index].buffer), sizeof(*buf));
+	return 0;
+}
+
+/***************************************************************************
+ * Functions for handling the video stream.
+ **************************************************************************/
+
+/*!
+ * Indicates whether the palette is supported.
+ *
+ * @param palette V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return ((palette == V4L2_PIX_FMT_RGB565) ||
+		(palette == V4L2_PIX_FMT_BGR24) ||
+		(palette == V4L2_PIX_FMT_RGB24) ||
+		(palette == V4L2_PIX_FMT_BGR32) ||
+		(palette == V4L2_PIX_FMT_RGB32) ||
+		(palette == V4L2_PIX_FMT_YUV422P) ||
+		(palette == V4L2_PIX_FMT_UYVY) ||
+		(palette == V4L2_PIX_FMT_YUV420) ||
+		(palette == V4L2_PIX_FMT_NV12));
+}
+
+/*!
+ * Start the encoder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_streamon(cam_data *cam)
+{
+	struct mxc_v4l_frame *frame;
+	int err = 0;
+
+	pr_debug("In MVC:mxc_streamon\n");
+
+	if (NULL == cam) {
+		pr_err("ERROR! cam parameter is NULL\n");
+		return -1;
+	}
+
+	if (list_empty(&cam->ready_q)) {
+		pr_err("ERROR: v4l2 capture: mxc_streamon buffer has not been "
+			"queued yet\n");
+		return -EINVAL;
+	}
+
+	cam->capture_pid = current->pid;
+
+	if (cam->enc_enable) {
+		err = cam->enc_enable(cam);
+		if (err != 0) {
+			return err;
+		}
+	}
+
+	cam->ping_pong_csi = 0;
+	if (cam->enc_update_eba) {
+		frame =
+		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+		list_del(cam->ready_q.next);
+		list_add_tail(&frame->queue, &cam->working_q);
+		err = cam->enc_update_eba(frame->buffer.m.offset,
+					  &cam->ping_pong_csi);
+
+		frame =
+		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+		list_del(cam->ready_q.next);
+		list_add_tail(&frame->queue, &cam->working_q);
+		err |= cam->enc_update_eba(frame->buffer.m.offset,
+					   &cam->ping_pong_csi);
+	} else {
+		return -EINVAL;
+	}
+
+	cam->capture_on = true;
+
+	return err;
+}
+
+/*!
+ * Shut down the encoder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_streamoff(cam_data *cam)
+{
+	int err = 0;
+
+	pr_debug("In MVC:mxc_streamoff\n");
+
+	if (cam->capture_on == false)
+		return 0;
+
+	if (cam->enc_disable) {
+		err = cam->enc_disable(cam);
+	}
+	mxc_free_frames(cam);
+	mxc_capture_inputs[cam->current_input].status |= V4L2_IN_ST_NO_POWER;
+	cam->capture_on = false;
+	return err;
+}
+
+/*!
+ * Valid and adjust the overlay window size, position
+ *
+ * @param cam      structure cam_data *
+ * @param win      struct v4l2_window  *
+ *
+ * @return 0
+ */
+static int verify_preview(cam_data *cam, struct v4l2_window *win)
+{
+	int i = 0, width_bound = 0, height_bound = 0;
+	int *width, *height;
+	struct fb_info *bg_fbi = NULL;
+	bool foregound_fb;
+
+	pr_debug("In MVC: verify_preview\n");
+
+	do {
+		cam->overlay_fb = (struct fb_info *)registered_fb[i];
+		if (cam->overlay_fb == NULL) {
+			pr_err("ERROR: verify_preview frame buffer NULL.\n");
+			return -1;
+		}
+		if (strcmp(cam->overlay_fb->fix.id, "DISP3 BG") == 0)
+			bg_fbi = cam->overlay_fb;
+		if (strcmp(cam->overlay_fb->fix.id,
+			    mxc_capture_outputs[cam->output].name) == 0) {
+			if (strcmp(cam->overlay_fb->fix.id, "DISP3 FG") == 0)
+				foregound_fb = true;
+			break;
+		}
+	} while (++i < FB_MAX);
+
+	if (foregound_fb) {
+		width_bound = bg_fbi->var.xres;
+		height_bound = bg_fbi->var.yres;
+
+		if (win->w.width + win->w.left > bg_fbi->var.xres ||
+		    win->w.height + win->w.top > bg_fbi->var.yres) {
+			pr_err("ERROR: FG window position exceeds.\n");
+			return -1;
+		}
+	} else {
+		/* 4 bytes alignment for BG */
+		width_bound = cam->overlay_fb->var.xres;
+		height_bound = cam->overlay_fb->var.yres;
+
+		if (cam->overlay_fb->var.bits_per_pixel == 24) {
+			win->w.left -= win->w.left % 4;
+		} else if (cam->overlay_fb->var.bits_per_pixel == 16) {
+			win->w.left -= win->w.left % 2;
+		}
+
+		if (win->w.width + win->w.left > cam->overlay_fb->var.xres)
+			win->w.width = cam->overlay_fb->var.xres - win->w.left;
+		if (win->w.height + win->w.top > cam->overlay_fb->var.yres)
+			win->w.height = cam->overlay_fb->var.yres - win->w.top;
+	}
+
+	/* stride line limitation */
+	win->w.height -= win->w.height % 8;
+	win->w.width -= win->w.width % 8;
+
+	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+		height = &win->w.width;
+		width = &win->w.height;
+	} else {
+		width = &win->w.width;
+		height = &win->w.height;
+	}
+
+	if ((cam->crop_bounds.width / *width > 8) ||
+	    ((cam->crop_bounds.width / *width == 8) &&
+	     (cam->crop_bounds.width % *width))) {
+		*width = cam->crop_bounds.width / 8;
+		if (*width % 8)
+			*width += 8 - *width % 8;
+		if (*width + win->w.left > width_bound) {
+			pr_err("ERROR: v4l2 capture: width exceeds "
+				"resize limit.\n");
+			return -1;
+		}
+		pr_err("ERROR: v4l2 capture: width exceeds limit. "
+			"Resize to %d.\n",
+			*width);
+	}
+
+	if ((cam->crop_bounds.height / *height > 8) ||
+	    ((cam->crop_bounds.height / *height == 8) &&
+	     (cam->crop_bounds.height % *height))) {
+		*height = cam->crop_bounds.height / 8;
+		if (*height % 8)
+			*height += 8 - *height % 8;
+		if (*height + win->w.top > height_bound) {
+			pr_err("ERROR: v4l2 capture: height exceeds "
+				"resize limit.\n");
+			return -1;
+		}
+		pr_err("ERROR: v4l2 capture: height exceeds limit "
+			"resize to %d.\n",
+			*height);
+	}
+
+	return 0;
+}
+
+/*!
+ * start the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int start_preview(cam_data *cam)
+{
+	int err = 0;
+
+	pr_debug("MVC: start_preview\n");
+
+#if defined(CONFIG_MXC_IPU_PRP_VF_SDC) || defined(CONFIG_MXC_IPU_PRP_VF_SDC_MODULE)
+	pr_debug("   This is an SDC display\n");
+	if (cam->output == 0 || cam->output == 2) {
+		if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY)
+			err = prp_vf_sdc_select(cam);
+		else if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_PRIMARY)
+			err = prp_vf_sdc_select_bg(cam);
+		if (err != 0)
+			return err;
+
+		err = cam->vf_start_sdc(cam);
+	}
+#endif
+
+#if defined(CONFIG_MXC_IPU_PRP_VF_ADC) || defined(CONFIG_MXC_IPU_PRP_VF_ADC_MODULE)
+	pr_debug("   This is an ADC display\n");
+	if (cam->output == 1) {
+		err = prp_vf_adc_select(cam);
+		if (err != 0)
+			return err;
+
+		err = cam->vf_start_adc(cam);
+	}
+#endif
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__,
+		 cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+	pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_bounds.width, cam->crop_bounds.height);
+	pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_defrect.width, cam->crop_defrect.height);
+	pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_current.width, cam->crop_current.height);
+
+	return err;
+}
+
+/*!
+ * shut down the viewfinder job
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static int stop_preview(cam_data *cam)
+{
+	int err = 0;
+
+	pr_debug("MVC: stop preview\n");
+
+#if defined(CONFIG_MXC_IPU_PRP_VF_ADC) || defined(CONFIG_MXC_IPU_PRP_VF_ADC_MODULE)
+	if (cam->output == 1) {
+		err = prp_vf_adc_deselect(cam);
+	}
+#endif
+
+#if defined(CONFIG_MXC_IPU_PRP_VF_SDC) || defined(CONFIG_MXC_IPU_PRP_VF_SDC_MODULE)
+	if (cam->output == 0 || cam->output == 2) {
+		if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY)
+			err = prp_vf_sdc_deselect(cam);
+		else if (cam->v4l2_fb.flags == V4L2_FBUF_FLAG_PRIMARY)
+			err = prp_vf_sdc_deselect_bg(cam);
+	}
+#endif
+
+	return err;
+}
+
+/***************************************************************************
+ * VIDIOC Functions.
+ **************************************************************************/
+
+/*!
+ * V4L2 - mxc_v4l2_g_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_g_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+
+	pr_debug("In MVC: mxc_v4l2_g_fmt type=%d\n", f->type);
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		f->fmt.pix = cam->v2f.fmt.pix;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
+		f->fmt.win = cam->win;
+		break;
+	default:
+		pr_debug("   type is invalid\n");
+		retval = -EINVAL;
+	}
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__,
+		 cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+	pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_bounds.width, cam->crop_bounds.height);
+	pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_defrect.width, cam->crop_defrect.height);
+	pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_current.width, cam->crop_current.height);
+
+	return retval;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
+{
+	int retval = 0;
+	int size = 0;
+	int bytesperline = 0;
+	int *width, *height;
+
+	pr_debug("In MVC: mxc_v4l2_s_fmt\n");
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		if (!valid_mode(f->fmt.pix.pixelformat)) {
+			pr_err("ERROR: v4l2 capture: mxc_v4l2_s_fmt: format "
+			       "not supported\n");
+			return -EINVAL;
+		}
+
+		/*
+		 * Force the capture window resolution to be crop bounds
+		 * for CSI MEM input mode.
+		 */
+		if (strcmp(mxc_capture_inputs[cam->current_input].name,
+			   "CSI MEM") == 0) {
+			f->fmt.pix.width = cam->crop_bounds.width;
+			f->fmt.pix.height = cam->crop_bounds.height;
+		}
+
+		/* Handle case where size requested is larger than cuurent
+		 * camera setting. */
+		if ((f->fmt.pix.width > cam->crop_bounds.width)
+			|| (f->fmt.pix.height > cam->crop_bounds.height)) {
+			/* Need the logic here, calling vidioc_s_param if
+			 * camera can change. */
+			/* For the moment, just return an error. */
+			return -EINVAL;
+		}
+
+		if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
+			height = &f->fmt.pix.width;
+			width = &f->fmt.pix.height;
+		} else {
+			width = &f->fmt.pix.width;
+			height = &f->fmt.pix.height;
+		}
+
+		/* stride line limitation */
+		*width -= *width % 8;
+		*height -= *height % 8;
+
+		if ((cam->crop_bounds.width / *width > 8) ||
+		    ((cam->crop_bounds.width / *width == 8) &&
+		     (cam->crop_bounds.width % *width))) {
+			*width = cam->crop_bounds.width / 8;
+			if (*width % 8)
+				*width += 8 - *width % 8;
+			pr_err("ERROR: v4l2 capture: width exceeds limit "
+				"resize to %d.\n",
+			       *width);
+		}
+
+		if ((cam->crop_bounds.height / *height > 8) ||
+		    ((cam->crop_bounds.height / *height == 8) &&
+		     (cam->crop_bounds.height % *height))) {
+			*height = cam->crop_bounds.height / 8;
+			if (*height % 8)
+				*height += 8 - *height % 8;
+			pr_err("ERROR: v4l2 capture: height exceeds limit "
+			       "resize to %d.\n",
+			       *height);
+		}
+
+		switch (f->fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_RGB565:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_BGR24:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3;
+			bytesperline = f->fmt.pix.width * 3;
+			break;
+		case V4L2_PIX_FMT_RGB24:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3;
+			bytesperline = f->fmt.pix.width * 3;
+			break;
+		case V4L2_PIX_FMT_BGR32:
+			size = f->fmt.pix.width * f->fmt.pix.height * 4;
+			bytesperline = f->fmt.pix.width * 4;
+			break;
+		case V4L2_PIX_FMT_RGB32:
+			size = f->fmt.pix.width * f->fmt.pix.height * 4;
+			bytesperline = f->fmt.pix.width * 4;
+			break;
+		case V4L2_PIX_FMT_YUV422P:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			bytesperline = f->fmt.pix.width;
+			break;
+		case V4L2_PIX_FMT_UYVY:
+			size = f->fmt.pix.width * f->fmt.pix.height * 2;
+			bytesperline = f->fmt.pix.width * 2;
+			break;
+		case V4L2_PIX_FMT_YUV420:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3 / 2;
+			bytesperline = f->fmt.pix.width;
+			break;
+		case V4L2_PIX_FMT_NV12:
+			size = f->fmt.pix.width * f->fmt.pix.height * 3 / 2;
+			bytesperline = f->fmt.pix.width;
+			break;
+		default:
+			break;
+		}
+
+		if (f->fmt.pix.bytesperline < bytesperline) {
+			f->fmt.pix.bytesperline = bytesperline;
+		} else {
+			bytesperline = f->fmt.pix.bytesperline;
+		}
+
+		if (f->fmt.pix.sizeimage < size) {
+			f->fmt.pix.sizeimage = size;
+		} else {
+			size = f->fmt.pix.sizeimage;
+		}
+
+		cam->v2f.fmt.pix = f->fmt.pix;
+
+		if (cam->v2f.fmt.pix.priv != 0) {
+			if (copy_from_user(&cam->offset,
+					   (void *)cam->v2f.fmt.pix.priv,
+					   sizeof(cam->offset))) {
+				retval = -EFAULT;
+				break;
+			}
+		}
+		break;
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
+		retval = verify_preview(cam, &f->fmt.win);
+		cam->win = f->fmt.win;
+		break;
+	default:
+		retval = -EINVAL;
+	}
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__,
+		 cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+	pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_bounds.width, cam->crop_bounds.height);
+	pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_defrect.width, cam->crop_defrect.height);
+	pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_current.width, cam->crop_current.height);
+
+	return retval;
+}
+
+/*!
+ * get control param
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_g_ctrl(cam_data *cam, struct v4l2_control *c)
+{
+	int status = 0;
+
+	pr_debug("In MVC:mxc_v4l2_g_ctrl\n");
+
+	/* probably don't need to store the values that can be retrieved,
+	 * locally, but they are for now. */
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		/* This is handled in the ipu. */
+		if (cam->rotation == IPU_ROTATE_HORIZ_FLIP)
+			c->value = 1;
+		break;
+	case V4L2_CID_VFLIP:
+		/* This is handled in the ipu. */
+		if (cam->rotation == IPU_ROTATE_VERT_FLIP)
+			c->value = 1;
+		break;
+	case V4L2_CID_MXC_ROT:
+		/* This is handled in the ipu. */
+		c->value = cam->rotation;
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		c->value = cam->bright;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->bright = c->value;
+		break;
+	case V4L2_CID_HUE:
+		c->value = cam->hue;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->hue = c->value;
+		break;
+	case V4L2_CID_CONTRAST:
+		c->value = cam->contrast;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->contrast = c->value;
+		break;
+	case V4L2_CID_SATURATION:
+		c->value = cam->saturation;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->saturation = c->value;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		c->value = cam->red;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->red = c->value;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		c->value = cam->blue;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->blue = c->value;
+		break;
+	case V4L2_CID_BLACK_LEVEL:
+		c->value = cam->ae_mode;
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+		cam->ae_mode = c->value;
+		break;
+	default:
+		status = vidioc_int_g_ctrl(cam->sensor, c);
+	}
+
+	return status;
+}
+
+/*!
+ * V4L2 - set_control function
+ *          V4L2_CID_PRIVATE_BASE is the extention for IPU preprocessing.
+ *          0 for normal operation
+ *          1 for vertical flip
+ *          2 for horizontal flip
+ *          3 for horizontal and vertical flip
+ *          4 for 90 degree rotation
+ * @param cam         structure cam_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_ctrl(cam_data *cam, struct v4l2_control *c)
+{
+	int ret = 0;
+	int tmp_rotation = IPU_ROTATE_NONE;
+
+	pr_debug("In MVC:mxc_v4l2_s_ctrl\n");
+
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		/* This is done by the IPU */
+		if (c->value == 1) {
+			if ((cam->rotation != IPU_ROTATE_VERT_FLIP) &&
+			    (cam->rotation != IPU_ROTATE_180))
+				cam->rotation = IPU_ROTATE_HORIZ_FLIP;
+			else
+				cam->rotation = IPU_ROTATE_180;
+		} else {
+			if (cam->rotation == IPU_ROTATE_HORIZ_FLIP)
+				cam->rotation = IPU_ROTATE_NONE;
+			if (cam->rotation == IPU_ROTATE_180)
+				cam->rotation = IPU_ROTATE_VERT_FLIP;
+		}
+		break;
+	case V4L2_CID_VFLIP:
+		/* This is done by the IPU */
+		if (c->value == 1) {
+			if ((cam->rotation != IPU_ROTATE_HORIZ_FLIP) &&
+			    (cam->rotation != IPU_ROTATE_180))
+				cam->rotation = IPU_ROTATE_VERT_FLIP;
+			else
+				cam->rotation = IPU_ROTATE_180;
+		} else {
+			if (cam->rotation == IPU_ROTATE_VERT_FLIP)
+				cam->rotation = IPU_ROTATE_NONE;
+			if (cam->rotation == IPU_ROTATE_180)
+				cam->rotation = IPU_ROTATE_HORIZ_FLIP;
+		}
+		break;
+	case V4L2_CID_MXC_ROT:
+	case V4L2_CID_MXC_VF_ROT:
+		/* This is done by the IPU */
+		switch (c->value) {
+		case V4L2_MXC_ROTATE_NONE:
+			tmp_rotation = IPU_ROTATE_NONE;
+			break;
+		case V4L2_MXC_ROTATE_VERT_FLIP:
+			tmp_rotation = IPU_ROTATE_VERT_FLIP;
+			break;
+		case V4L2_MXC_ROTATE_HORIZ_FLIP:
+			tmp_rotation = IPU_ROTATE_HORIZ_FLIP;
+			break;
+		case V4L2_MXC_ROTATE_180:
+			tmp_rotation = IPU_ROTATE_180;
+			break;
+		case V4L2_MXC_ROTATE_90_RIGHT:
+			tmp_rotation = IPU_ROTATE_90_RIGHT;
+			break;
+		case V4L2_MXC_ROTATE_90_RIGHT_VFLIP:
+			tmp_rotation = IPU_ROTATE_90_RIGHT_VFLIP;
+			break;
+		case V4L2_MXC_ROTATE_90_RIGHT_HFLIP:
+			tmp_rotation = IPU_ROTATE_90_RIGHT_HFLIP;
+			break;
+		case V4L2_MXC_ROTATE_90_LEFT:
+			tmp_rotation = IPU_ROTATE_90_LEFT;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+
+		if (c->id == V4L2_CID_MXC_VF_ROT)
+			cam->vf_rotation = tmp_rotation;
+		else
+			cam->rotation = tmp_rotation;
+
+		break;
+	case V4L2_CID_HUE:
+		cam->hue = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_CONTRAST:
+		cam->contrast = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_BRIGHTNESS:
+		cam->bright = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_SATURATION:
+		cam->saturation = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		cam->red = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		cam->blue = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_EXPOSURE:
+		cam->ae_mode = c->value;
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		ret = vidioc_int_s_ctrl(cam->sensor, c);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		break;
+	case V4L2_CID_MXC_FLASH:
+#ifdef CONFIG_MXC_IPU_V1
+		ipu_csi_flash_strobe(true);
+#endif
+		break;
+	default:
+		pr_debug("   default case\n");
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_param function
+ * Allows setting of capturemode and frame rate.
+ *
+ * @param cam         structure cam_data *
+ * @param parm        structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
+{
+	struct v4l2_ifparm ifparm;
+	struct v4l2_format cam_fmt;
+	struct v4l2_streamparm currentparm;
+	ipu_csi_signal_cfg_t csi_param;
+	int err = 0;
+
+	pr_debug("In mxc_v4l2_s_param\n");
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pr_err(KERN_ERR "mxc_v4l2_s_param invalid type\n");
+		return -EINVAL;
+	}
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true) {
+		stop_preview(cam);
+	}
+
+	currentparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* First check that this device can support the changes requested. */
+	err = vidioc_int_g_parm(cam->sensor, &currentparm);
+	if (err) {
+		pr_err("%s: vidioc_int_g_parm returned an error %d\n",
+			__func__, err);
+		goto exit;
+	}
+
+	pr_debug("   Current capabilities are %x\n",
+			currentparm.parm.capture.capability);
+	pr_debug("   Current capturemode is %d  change to %d\n",
+			currentparm.parm.capture.capturemode,
+			parm->parm.capture.capturemode);
+	pr_debug("   Current framerate is %d  change to %d\n",
+			currentparm.parm.capture.timeperframe.denominator,
+			parm->parm.capture.timeperframe.denominator);
+
+	/* This will change any camera settings needed. */
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	err = vidioc_int_s_parm(cam->sensor, parm);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+	if (err) {
+		pr_err("%s: vidioc_int_s_parm returned an error %d\n",
+			__func__, err);
+		goto exit;
+	}
+
+	/* If resolution changed, need to re-program the CSI */
+	/* Get new values. */
+	vidioc_int_g_ifparm(cam->sensor, &ifparm);
+
+	csi_param.data_width = 0;
+	csi_param.clk_mode = 0;
+	csi_param.ext_vsync = 0;
+	csi_param.Vsync_pol = 0;
+	csi_param.Hsync_pol = 0;
+	csi_param.pixclk_pol = 0;
+	csi_param.data_pol = 0;
+	csi_param.sens_clksrc = 0;
+	csi_param.pack_tight = 0;
+	csi_param.force_eof = 0;
+	csi_param.data_en_pol = 0;
+	csi_param.data_fmt = 0;
+	csi_param.csi = 0;
+	csi_param.mclk = 0;
+
+	/* This may not work on other platforms. Check when adding a new one.*/
+	pr_debug("   clock_curr=mclk=%d\n", ifparm.u.bt656.clock_curr);
+	if (ifparm.u.bt656.clock_curr == 0) {
+		csi_param.clk_mode = IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE;
+	} else {
+		csi_param.clk_mode = IPU_CSI_CLK_MODE_GATED_CLK;
+	}
+
+	csi_param.pixclk_pol = ifparm.u.bt656.latch_clk_inv;
+
+	if (ifparm.u.bt656.mode == V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT) {
+		csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
+	} else if (ifparm.u.bt656.mode
+				== V4L2_IF_TYPE_BT656_MODE_NOBT_10BIT) {
+		csi_param.data_width = IPU_CSI_DATA_WIDTH_10;
+	} else {
+		csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
+	}
+
+	csi_param.Vsync_pol = ifparm.u.bt656.nobt_vs_inv;
+	csi_param.Hsync_pol = ifparm.u.bt656.nobt_hs_inv;
+	csi_param.ext_vsync = ifparm.u.bt656.bt_sync_correct;
+
+	/* if the capturemode changed, the size bounds will have changed. */
+	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vidioc_int_g_fmt_cap(cam->sensor, &cam_fmt);
+	pr_debug("   g_fmt_cap returns widthxheight of input as %d x %d\n",
+			cam_fmt.fmt.pix.width, cam_fmt.fmt.pix.height);
+
+	csi_param.data_fmt = cam_fmt.fmt.pix.pixelformat;
+
+	cam->crop_bounds.top = cam->crop_bounds.left = 0;
+	cam->crop_bounds.width = cam_fmt.fmt.pix.width;
+	cam->crop_bounds.height = cam_fmt.fmt.pix.height;
+
+	/* This essentially loses the data at the left and bottom of the image
+	 * giving a digital zoom image, if crop_current is less than the full
+	 * size of the image. */
+	ipu_csi_set_window_size(cam->crop_current.width,
+				cam->crop_current.height, cam->csi);
+	ipu_csi_set_window_pos(cam->crop_current.left,
+			       cam->crop_current.top,
+			       cam->csi);
+	ipu_csi_init_interface(cam->crop_bounds.width,
+			       cam->crop_bounds.height,
+			       cam_fmt.fmt.pix.pixelformat, csi_param);
+
+
+exit:
+	if (cam->overlay_on == true)
+		start_preview(cam);
+
+	return err;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_s_std function
+ *
+ * Sets the TV standard to be used.
+ *
+ * @param cam	      structure cam_data *
+ * @param parm	      structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_std(cam_data *cam, v4l2_std_id e)
+{
+	bool change = false;
+
+	if (e != cam->standard.id) {
+		change = true;
+	}
+
+	pr_debug("In mxc_v4l2_s_std %Lx\n", e);
+	if (e == V4L2_STD_PAL) {
+		pr_debug("   Setting standard to PAL %Lx\n", V4L2_STD_PAL);
+		cam->standard.id = V4L2_STD_PAL;
+		video_index = TV_PAL;
+		cam->crop_current.top = 0;
+	} else if (e == V4L2_STD_NTSC) {
+		pr_debug("   Setting standard to NTSC %Lx\n",
+				V4L2_STD_NTSC);
+		/* Get rid of the white dot line in NTSC signal input */
+		cam->standard.id = V4L2_STD_NTSC;
+		video_index = TV_NTSC;
+		cam->crop_current.top = 12;
+	} else {
+		cam->standard.id = V4L2_STD_ALL;
+		video_index = TV_NOT_LOCKED;
+		cam->crop_current.top = 0;
+		pr_err("ERROR: unrecognized std! %Lx (PAL=%Lx, NTSC=%Lx\n",
+			e, V4L2_STD_PAL, V4L2_STD_NTSC);
+	}
+
+	cam->standard.index = video_index;
+	strcpy(cam->standard.name, video_fmts[video_index].name);
+	cam->crop_bounds.width = video_fmts[video_index].raw_width;
+	cam->crop_bounds.height = video_fmts[video_index].raw_height;
+	cam->crop_current.width = video_fmts[video_index].active_width;
+	cam->crop_current.height = video_fmts[video_index].active_height;
+	cam->crop_current.left = 0;
+
+	return 0;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_g_std function
+ *
+ * Gets the TV standard from the TV input device.
+ *
+ * @param cam	      structure cam_data *
+ *
+ * @param e	      structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_g_std(cam_data *cam, v4l2_std_id *e)
+{
+	struct v4l2_format tv_fmt;
+
+	pr_debug("In mxc_v4l2_g_std\n");
+
+	if (cam->device_type == 1) {
+		/* Use this function to get what the TV-In device detects the
+		 * format to be. pixelformat is used to return the std value
+		 * since the interface has no vidioc_g_std.*/
+		tv_fmt.type = V4L2_BUF_TYPE_PRIVATE;
+		vidioc_int_g_fmt_cap(cam->sensor, &tv_fmt);
+
+		/* If the TV-in automatically detects the standard, then if it
+		 * changes, the settings need to change. */
+		if (cam->standard_autodetect) {
+			if (cam->standard.id != tv_fmt.fmt.pix.pixelformat) {
+				pr_debug("MVC: mxc_v4l2_g_std: "
+					"Changing standard\n");
+				mxc_v4l2_s_std(cam, tv_fmt.fmt.pix.pixelformat);
+			}
+		}
+
+		*e = tv_fmt.fmt.pix.pixelformat;
+	}
+
+	return 0;
+}
+
+/*!
+ * Dequeue one V4L capture buffer
+ *
+ * @param cam         structure cam_data *
+ * @param buf         structure v4l2_buffer *
+ *
+ * @return  status    0 success, EINVAL invalid frame number,
+ *                    ETIME timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l_dqueue(cam_data *cam, struct v4l2_buffer *buf)
+{
+	int retval = 0;
+	struct mxc_v4l_frame *frame;
+
+	pr_debug("In MVC:mxc_v4l_dqueue\n");
+
+	if (!wait_event_interruptible_timeout(cam->enc_queue,
+					      cam->enc_counter != 0, 10 * HZ)) {
+		pr_err("ERROR: v4l2 capture: mxc_v4l_dqueue timeout "
+			"enc_counter %x\n",
+		       cam->enc_counter);
+		return -ETIME;
+	} else if (signal_pending(current)) {
+		pr_err("ERROR: v4l2 capture: mxc_v4l_dqueue() "
+			"interrupt received\n");
+		return -ERESTARTSYS;
+	}
+
+	cam->enc_counter--;
+
+	frame = list_entry(cam->done_q.next, struct mxc_v4l_frame, queue);
+	list_del(cam->done_q.next);
+	if (frame->buffer.flags & V4L2_BUF_FLAG_DONE) {
+		frame->buffer.flags &= ~V4L2_BUF_FLAG_DONE;
+	} else if (frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
+		pr_err("ERROR: v4l2 capture: VIDIOC_DQBUF: "
+			"Buffer not filled.\n");
+		frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
+		retval = -EINVAL;
+	} else if ((frame->buffer.flags & 0x7) == V4L2_BUF_FLAG_MAPPED) {
+		pr_err("ERROR: v4l2 capture: VIDIOC_DQBUF: "
+			"Buffer not queued.\n");
+		retval = -EINVAL;
+	}
+
+	buf->bytesused = cam->v2f.fmt.pix.sizeimage;
+	buf->index = frame->index;
+	buf->flags = frame->buffer.flags;
+	buf->m = cam->frame[frame->index].buffer.m;
+
+	return retval;
+}
+
+/*!
+ * V4L interface - open function
+ *
+ * @param inode        structure inode *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l_open(struct inode *inode, struct file *file)
+{
+	struct v4l2_ifparm ifparm;
+	struct v4l2_format cam_fmt;
+	ipu_csi_signal_cfg_t csi_param;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int err = 0;
+
+	pr_debug("\nIn MVC: mxc_v4l_open\n");
+	pr_debug("   device name is %s\n", dev->name);
+
+	if (!cam) {
+		pr_err("ERROR: v4l2 capture: Internal error, "
+			"cam_data not found!\n");
+		return -EBADF;
+	}
+
+	down(&cam->busy_lock);
+	err = 0;
+	if (signal_pending(current))
+		goto oops;
+
+	if (cam->open_count++ == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+
+		if (strcmp(mxc_capture_inputs[cam->current_input].name,
+			   "CSI MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_CSI_ENC) || defined(CONFIG_MXC_IPU_CSI_ENC_MODULE)
+			err = csi_enc_select(cam);
+#endif
+		} else if (strcmp(mxc_capture_inputs[cam->current_input].name,
+				  "CSI IC MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_PRP_ENC) || defined(CONFIG_MXC_IPU_PRP_ENC_MODULE)
+			err = prp_enc_select(cam);
+#endif
+		}
+
+		cam->enc_counter = 0;
+		cam->skip_frame = 0;
+		INIT_LIST_HEAD(&cam->ready_q);
+		INIT_LIST_HEAD(&cam->working_q);
+		INIT_LIST_HEAD(&cam->done_q);
+
+		vidioc_int_g_ifparm(cam->sensor, &ifparm);
+
+		csi_param.sens_clksrc = 0;
+
+		csi_param.clk_mode = 0;
+		csi_param.data_pol = 0;
+		csi_param.ext_vsync = 0;
+
+		csi_param.pack_tight = 0;
+		csi_param.force_eof = 0;
+		csi_param.data_en_pol = 0;
+		csi_param.mclk = ifparm.u.bt656.clock_curr;
+
+		csi_param.pixclk_pol = ifparm.u.bt656.latch_clk_inv;
+
+		/* Once we handle multiple inputs this will need to change. */
+		csi_param.csi = 0;
+
+		if (ifparm.u.bt656.mode
+				== V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT)
+			csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
+		else if (ifparm.u.bt656.mode
+				== V4L2_IF_TYPE_BT656_MODE_NOBT_10BIT)
+			csi_param.data_width = IPU_CSI_DATA_WIDTH_10;
+		else
+			csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
+
+
+		csi_param.Vsync_pol = ifparm.u.bt656.nobt_vs_inv;
+		csi_param.Hsync_pol = ifparm.u.bt656.nobt_hs_inv;
+
+		csi_param.csi = cam->csi;
+
+		cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		vidioc_int_g_fmt_cap(cam->sensor, &cam_fmt);
+
+		/* Reset the sizes.  Needed to prevent carryover of last
+		 * operation.*/
+		cam->crop_bounds.top = cam->crop_bounds.left = 0;
+		cam->crop_bounds.width = cam_fmt.fmt.pix.width;
+		cam->crop_bounds.height = cam_fmt.fmt.pix.height;
+
+		/* This also is the max crop size for this device. */
+		cam->crop_defrect.top = cam->crop_defrect.left = 0;
+		cam->crop_defrect.width = cam_fmt.fmt.pix.width;
+		cam->crop_defrect.height = cam_fmt.fmt.pix.height;
+
+		/* At this point, this is also the current image size. */
+		cam->crop_current.top = cam->crop_current.left = 0;
+		cam->crop_current.width = cam_fmt.fmt.pix.width;
+		cam->crop_current.height = cam_fmt.fmt.pix.height;
+
+		pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+			__func__,
+			cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+		pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+			__func__,
+			cam->crop_bounds.width, cam->crop_bounds.height);
+		pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+			__func__,
+			cam->crop_defrect.width, cam->crop_defrect.height);
+		pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+			__func__,
+			cam->crop_current.width, cam->crop_current.height);
+
+		csi_param.data_fmt = cam_fmt.fmt.pix.pixelformat;
+		pr_debug("On Open: Input to ipu size is %d x %d\n",
+				cam_fmt.fmt.pix.width, cam_fmt.fmt.pix.height);
+		ipu_csi_set_window_size(cam->crop_current.width,
+					cam->crop_current.width,
+					cam->csi);
+		ipu_csi_set_window_pos(cam->crop_current.left,
+					cam->crop_current.top,
+					cam->csi);
+		ipu_csi_init_interface(cam->crop_bounds.width,
+					cam->crop_bounds.height,
+					cam_fmt.fmt.pix.pixelformat,
+					csi_param);
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
+				       true, true);
+		vidioc_int_init(cam->sensor);
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
+				       false, false);
+}
+
+	file->private_data = dev;
+
+      oops:
+	up(&cam->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L interface - close function
+ *
+ * @param inode    struct inode *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	int err = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("In MVC:mxc_v4l_close\n");
+
+	if (!cam) {
+		pr_err("ERROR: v4l2 capture: Internal error, "
+			"cam_data not found!\n");
+		return -EBADF;
+	}
+
+	/* for the case somebody hit the ctrl C */
+	if (cam->overlay_pid == current->pid) {
+		err = stop_preview(cam);
+		cam->overlay_on = false;
+	}
+	if (cam->capture_pid == current->pid) {
+		err |= mxc_streamoff(cam);
+		wake_up_interruptible(&cam->enc_queue);
+	}
+
+	if (--cam->open_count == 0) {
+		wait_event_interruptible(cam->power_queue,
+					 cam->low_power == false);
+		pr_info("mxc_v4l_close: release resource\n");
+
+		if (strcmp(mxc_capture_inputs[cam->current_input].name,
+			   "CSI MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_CSI_ENC) || defined(CONFIG_MXC_IPU_CSI_ENC_MODULE)
+			err |= csi_enc_deselect(cam);
+#endif
+		} else if (strcmp(mxc_capture_inputs[cam->current_input].name,
+				  "CSI IC MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_PRP_ENC) || defined(CONFIG_MXC_IPU_PRP_ENC_MODULE)
+			err |= prp_enc_deselect(cam);
+#endif
+		}
+
+		mxc_free_frame_buf(cam);
+		file->private_data = NULL;
+
+		/* capture off */
+		wake_up_interruptible(&cam->enc_queue);
+		mxc_free_frames(cam);
+		cam->enc_counter++;
+	}
+
+	return err;
+}
+
+#if defined(CONFIG_MXC_IPU_PRP_ENC) || defined(CONFIG_MXC_IPU_CSI_ENC) || \
+    defined(CONFIG_MXC_IPU_PRP_ENC_MODULE) || \
+    defined(CONFIG_MXC_IPU_CSI_ENC_MODULE)
+/*
+ * V4L interface - read function
+ *
+ * @param file       struct file *
+ * @param read buf   char *
+ * @param count      size_t
+ * @param ppos       structure loff_t *
+ *
+ * @return           bytes read
+ */
+static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
+			    loff_t *ppos)
+{
+	int err = 0;
+	u8 *v_address;
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+
+	v_address = dma_alloc_coherent(0,
+				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				       &cam->still_buf, GFP_DMA | GFP_KERNEL);
+
+	if (!v_address) {
+		err = -ENOBUFS;
+		goto exit0;
+	}
+
+	err = prp_still_select(cam);
+	if (err != 0) {
+		err = -EIO;
+		goto exit1;
+	}
+
+	cam->still_counter = 0;
+	err = cam->csi_start(cam);
+	if (err != 0) {
+		err = -EIO;
+		goto exit2;
+	}
+
+	if (!wait_event_interruptible_timeout(cam->still_queue,
+					      cam->still_counter != 0,
+					      10 * HZ)) {
+		pr_err("ERROR: v4l2 capture: mxc_v4l_read timeout counter %x\n",
+		       cam->still_counter);
+		err = -ETIME;
+		goto exit2;
+	}
+	err = copy_to_user(buf, v_address, cam->v2f.fmt.pix.sizeimage);
+
+      exit2:
+	prp_still_deselect(cam);
+
+      exit1:
+	dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address,
+			  cam->still_buf);
+	cam->still_buf = 0;
+
+      exit0:
+	if (cam->overlay_on == true) {
+		start_preview(cam);
+	}
+
+	up(&cam->busy_lock);
+	if (err < 0)
+		return err;
+
+	return (cam->v2f.fmt.pix.sizeimage - err);
+}
+#endif
+
+/*!
+ * V4L interface - ioctl function
+ *
+ * @param inode      struct inode*
+ *
+ * @param file       struct file*
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void*
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int mxc_v4l_do_ioctl(struct inode *inode, struct file *file,
+			    unsigned int ioctlnr, void *arg)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	int retval = 0;
+	unsigned long lock_flags;
+
+	pr_debug("In MVC: mxc_v4l_do_ioctl %x\n", ioctlnr);
+	wait_event_interruptible(cam->power_queue, cam->low_power == false);
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+	/*!
+	 * V4l2 VIDIOC_QUERYCAP ioctl
+	 */
+	case VIDIOC_QUERYCAP: {
+		struct v4l2_capability *cap = arg;
+		pr_debug("   case VIDIOC_QUERYCAP\n");
+		strcpy(cap->driver, "mxc_v4l2");
+		cap->version = KERNEL_VERSION(0, 1, 11);
+		cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+				    V4L2_CAP_VIDEO_OVERLAY |
+				    V4L2_CAP_STREAMING |
+				    V4L2_CAP_READWRITE;
+		cap->card[0] = '\0';
+		cap->bus_info[0] = '\0';
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_G_FMT ioctl
+	 */
+	case VIDIOC_G_FMT: {
+		struct v4l2_format *gf = arg;
+		pr_debug("   case VIDIOC_G_FMT\n");
+		retval = mxc_v4l2_g_fmt(cam, gf);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_S_FMT ioctl
+	 */
+	case VIDIOC_S_FMT: {
+		struct v4l2_format *sf = arg;
+		pr_debug("   case VIDIOC_S_FMT\n");
+		retval = mxc_v4l2_s_fmt(cam, sf);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_REQBUFS ioctl
+	 */
+	case VIDIOC_REQBUFS: {
+		struct v4l2_requestbuffers *req = arg;
+		pr_debug("   case VIDIOC_REQBUFS\n");
+
+		if (req->count > FRAME_NUM) {
+			pr_err("ERROR: v4l2 capture: VIDIOC_REQBUFS: "
+			       "not enough buffers\n");
+			req->count = FRAME_NUM;
+		}
+
+		if ((req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+		    (req->memory != V4L2_MEMORY_MMAP)) {
+			pr_err("ERROR: v4l2 capture: VIDIOC_REQBUFS: "
+			       "wrong buffer type\n");
+			retval = -EINVAL;
+			break;
+		}
+
+		mxc_streamoff(cam);
+		mxc_free_frame_buf(cam);
+		cam->enc_counter = 0;
+		cam->skip_frame = 0;
+		INIT_LIST_HEAD(&cam->ready_q);
+		INIT_LIST_HEAD(&cam->working_q);
+		INIT_LIST_HEAD(&cam->done_q);
+
+		retval = mxc_allocate_frame_buf(cam, req->count);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_QUERYBUF ioctl
+	 */
+	case VIDIOC_QUERYBUF: {
+		struct v4l2_buffer *buf = arg;
+		int index = buf->index;
+		pr_debug("   case VIDIOC_QUERYBUF\n");
+
+		if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+			pr_err("ERROR: v4l2 capture: "
+			       "VIDIOC_QUERYBUFS: "
+			       "wrong buffer type\n");
+			retval = -EINVAL;
+			break;
+		}
+
+		memset(buf, 0, sizeof(buf));
+		buf->index = index;
+
+		down(&cam->param_lock);
+		retval = mxc_v4l2_buffer_status(cam, buf);
+		up(&cam->param_lock);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_QBUF ioctl
+	 */
+	case VIDIOC_QBUF: {
+		struct v4l2_buffer *buf = arg;
+		int index = buf->index;
+		pr_debug("   case VIDIOC_QBUF\n");
+
+		spin_lock_irqsave(&cam->int_lock, lock_flags);
+		cam->frame[index].buffer.m.offset = buf->m.offset;
+		if ((cam->frame[index].buffer.flags & 0x7) ==
+		    V4L2_BUF_FLAG_MAPPED) {
+			cam->frame[index].buffer.flags |=
+			    V4L2_BUF_FLAG_QUEUED;
+			if (cam->skip_frame > 0) {
+				list_add_tail(&cam->frame[index].queue,
+					      &cam->working_q);
+				retval =
+				    cam->enc_update_eba(cam->
+							frame[index].
+							buffer.m.offset,
+							&cam->
+							ping_pong_csi);
+				cam->skip_frame = 0;
+			} else {
+				list_add_tail(&cam->frame[index].queue,
+					      &cam->ready_q);
+			}
+		} else if (cam->frame[index].buffer.
+			   flags & V4L2_BUF_FLAG_QUEUED) {
+			pr_err("ERROR: v4l2 capture: VIDIOC_QBUF: "
+			       "buffer already queued\n");
+			retval = -EINVAL;
+		} else if (cam->frame[index].buffer.
+			   flags & V4L2_BUF_FLAG_DONE) {
+			pr_err("ERROR: v4l2 capture: VIDIOC_QBUF: "
+			       "overwrite done buffer.\n");
+			cam->frame[index].buffer.flags &=
+			    ~V4L2_BUF_FLAG_DONE;
+			cam->frame[index].buffer.flags |=
+			    V4L2_BUF_FLAG_QUEUED;
+			retval = -EINVAL;
+		}
+
+		buf->flags = cam->frame[index].buffer.flags;
+		spin_unlock_irqrestore(&cam->int_lock, lock_flags);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_DQBUF ioctl
+	 */
+	case VIDIOC_DQBUF: {
+		struct v4l2_buffer *buf = arg;
+		pr_debug("   case VIDIOC_DQBUF\n");
+
+		if ((cam->enc_counter == 0) &&
+			(file->f_flags & O_NONBLOCK)) {
+			retval = -EAGAIN;
+			break;
+		}
+
+		retval = mxc_v4l_dqueue(cam, buf);
+
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_STREAMON ioctl
+	 */
+	case VIDIOC_STREAMON: {
+		pr_debug("   case VIDIOC_STREAMON\n");
+		retval = mxc_streamon(cam);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_STREAMOFF ioctl
+	 */
+	case VIDIOC_STREAMOFF: {
+		pr_debug("   case VIDIOC_STREAMOFF\n");
+		retval = mxc_streamoff(cam);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_G_CTRL ioctl
+	 */
+	case VIDIOC_G_CTRL: {
+		pr_debug("   case VIDIOC_G_CTRL\n");
+		retval = mxc_v4l2_g_ctrl(cam, arg);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_S_CTRL ioctl
+	 */
+	case VIDIOC_S_CTRL: {
+		pr_debug("   case VIDIOC_S_CTRL\n");
+		retval = mxc_v4l2_s_ctrl(cam, arg);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_CROPCAP ioctl
+	 */
+	case VIDIOC_CROPCAP: {
+		struct v4l2_cropcap *cap = arg;
+		pr_debug("   case VIDIOC_CROPCAP\n");
+		if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+		    cap->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+			retval = -EINVAL;
+			break;
+		}
+		cap->bounds = cam->crop_bounds;
+		cap->defrect = cam->crop_defrect;
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_G_CROP ioctl
+	 */
+	case VIDIOC_G_CROP: {
+		struct v4l2_crop *crop = arg;
+		pr_debug("   case VIDIOC_G_CROP\n");
+
+		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+		    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+			retval = -EINVAL;
+			break;
+		}
+		crop->c = cam->crop_current;
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_S_CROP ioctl
+	 */
+	case VIDIOC_S_CROP: {
+		struct v4l2_crop *crop = arg;
+		struct v4l2_rect *b = &cam->crop_bounds;
+		pr_debug("   case VIDIOC_S_CROP\n");
+
+		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+		    crop->type != V4L2_BUF_TYPE_VIDEO_OVERLAY) {
+			retval = -EINVAL;
+			break;
+		}
+
+		crop->c.top = (crop->c.top < b->top) ? b->top
+			      : crop->c.top;
+		if (crop->c.top > b->top + b->height)
+			crop->c.top = b->top + b->height - 1;
+		if (crop->c.height > b->top + b->height - crop->c.top)
+			crop->c.height =
+				b->top + b->height - crop->c.top;
+
+		crop->c.left = (crop->c.left < b->left) ? b->left
+		    : crop->c.left;
+		if (crop->c.left > b->left + b->width)
+			crop->c.left = b->left + b->width - 1;
+		if (crop->c.width > b->left - crop->c.left + b->width)
+			crop->c.width =
+				b->left - crop->c.left + b->width;
+
+		crop->c.width -= crop->c.width % 8;
+		crop->c.left -= crop->c.left % 4;
+		cam->crop_current = crop->c;
+
+		pr_debug("   Cropping Input to ipu size %d x %d\n",
+				cam->crop_current.width,
+				cam->crop_current.height);
+		ipu_csi_set_window_size(cam->crop_current.width,
+					cam->crop_current.height,
+					cam->csi);
+		ipu_csi_set_window_pos(cam->crop_current.left,
+				       cam->crop_current.top,
+				       cam->csi);
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_OVERLAY ioctl
+	 */
+	case VIDIOC_OVERLAY: {
+		int *on = arg;
+		pr_debug("   VIDIOC_OVERLAY on=%d\n", *on);
+		if (*on) {
+			cam->overlay_on = true;
+			cam->overlay_pid = current->pid;
+			retval = start_preview(cam);
+		}
+		if (!*on) {
+			retval = stop_preview(cam);
+			cam->overlay_on = false;
+		}
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_G_FBUF ioctl
+	 */
+	case VIDIOC_G_FBUF: {
+		struct v4l2_framebuffer *fb = arg;
+		pr_debug("   case VIDIOC_G_FBUF\n");
+		*fb = cam->v4l2_fb;
+		fb->capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+		break;
+	}
+
+	/*!
+	 * V4l2 VIDIOC_S_FBUF ioctl
+	 */
+	case VIDIOC_S_FBUF: {
+		struct v4l2_framebuffer *fb = arg;
+		pr_debug("   case VIDIOC_S_FBUF\n");
+		cam->v4l2_fb = *fb;
+		break;
+	}
+
+	case VIDIOC_G_PARM: {
+		struct v4l2_streamparm *parm = arg;
+		pr_debug("   case VIDIOC_G_PARM\n");
+		vidioc_int_g_parm(cam->sensor, parm);
+		break;
+	}
+
+	case VIDIOC_S_PARM:  {
+		struct v4l2_streamparm *parm = arg;
+		pr_debug("   case VIDIOC_S_PARM\n");
+		retval = mxc_v4l2_s_param(cam, parm);
+		break;
+	}
+
+	/* linux v4l2 bug, kernel c0485619 user c0405619 */
+	case VIDIOC_ENUMSTD: {
+		struct v4l2_standard *e = arg;
+		pr_debug("   case VIDIOC_ENUMSTD\n");
+		*e = cam->standard;
+		break;
+	}
+
+	case VIDIOC_G_STD: {
+		v4l2_std_id *e = arg;
+		pr_debug("   case VIDIOC_G_STD\n");
+		retval = mxc_v4l2_g_std(cam, e);
+		break;
+	}
+
+	case VIDIOC_S_STD: {
+		v4l2_std_id *e = arg;
+		pr_debug("   case VIDIOC_S_STD\n");
+		retval = mxc_v4l2_s_std(cam, *e);
+
+		break;
+	}
+
+	case VIDIOC_ENUMOUTPUT: {
+		struct v4l2_output *output = arg;
+		pr_debug("   case VIDIOC_ENUMOUTPUT\n");
+		if (output->index >= MXC_V4L2_CAPTURE_NUM_OUTPUTS) {
+			retval = -EINVAL;
+			break;
+		}
+		*output = mxc_capture_outputs[output->index];
+
+		break;
+	}
+	case VIDIOC_G_OUTPUT: {
+		int *p_output_num = arg;
+		pr_debug("   case VIDIOC_G_OUTPUT\n");
+		*p_output_num = cam->output;
+		break;
+	}
+
+	case VIDIOC_S_OUTPUT: {
+		int *p_output_num = arg;
+		pr_debug("   case VIDIOC_S_OUTPUT\n");
+		if (*p_output_num >= MXC_V4L2_CAPTURE_NUM_OUTPUTS) {
+			retval = -EINVAL;
+			break;
+		}
+		cam->output = *p_output_num;
+		break;
+	}
+
+	case VIDIOC_ENUMINPUT: {
+		struct v4l2_input *input = arg;
+		pr_debug("   case VIDIOC_ENUMINPUT\n");
+		if (input->index >= MXC_V4L2_CAPTURE_NUM_INPUTS) {
+			retval = -EINVAL;
+			break;
+		}
+		*input = mxc_capture_inputs[input->index];
+		break;
+	}
+
+	case VIDIOC_G_INPUT: {
+		int *index = arg;
+		pr_debug("   case VIDIOC_G_INPUT\n");
+		*index = cam->current_input;
+		break;
+	}
+
+	case VIDIOC_S_INPUT: {
+		int *index = arg;
+		pr_debug("   case VIDIOC_S_INPUT\n");
+		if (*index >= MXC_V4L2_CAPTURE_NUM_INPUTS) {
+			retval = -EINVAL;
+			break;
+		}
+
+		if (*index == cam->current_input)
+			break;
+
+		if ((mxc_capture_inputs[cam->current_input].status &
+		    V4L2_IN_ST_NO_POWER) == 0) {
+			retval = mxc_streamoff(cam);
+			if (retval)
+				break;
+			mxc_capture_inputs[cam->current_input].status |=
+							V4L2_IN_ST_NO_POWER;
+		}
+
+		if (strcmp(mxc_capture_inputs[*index].name, "CSI MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_CSI_ENC) || defined(CONFIG_MXC_IPU_CSI_ENC_MODULE)
+			retval = csi_enc_select(cam);
+			if (retval)
+				break;
+#endif
+		} else if (strcmp(mxc_capture_inputs[*index].name,
+				  "CSI IC MEM") == 0) {
+#if defined(CONFIG_MXC_IPU_PRP_ENC) || defined(CONFIG_MXC_IPU_PRP_ENC_MODULE)
+			retval = prp_enc_select(cam);
+			if (retval)
+				break;
+#endif
+		}
+
+		mxc_capture_inputs[*index].status &= ~V4L2_IN_ST_NO_POWER;
+		cam->current_input = *index;
+		break;
+	}
+
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		pr_debug("   case default or not supported\n");
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&cam->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L interface - ioctl function
+ *
+ * @return  None
+ */
+static int mxc_v4l_ioctl(struct inode *inode, struct file *file,
+			 unsigned int cmd, unsigned long arg)
+{
+	pr_debug("In MVC:mxc_v4l_ioctl\n");
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l_do_ioctl);
+}
+
+/*!
+ * V4L interface - mmap function
+ *
+ * @param file        structure file *
+ *
+ * @param vma         structure vm_area_struct *
+ *
+ * @return status     0 Success, EINTR busy lock error, ENOBUFS remap_page error
+ */
+static int mxc_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = video_devdata(file);
+	unsigned long size;
+	int res = 0;
+	cam_data *cam = video_get_drvdata(dev);
+
+	pr_debug("In MVC:mxc_mmap\n");
+	pr_debug("   pgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		 vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	size = vma->vm_end - vma->vm_start;
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		pr_err("ERROR: v4l2 capture: mxc_mmap: "
+			"remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+      mxc_mmap_exit:
+	up(&cam->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_poll(struct file *file, poll_table *wait)
+{
+	struct video_device *dev = video_devdata(file);
+	cam_data *cam = video_get_drvdata(dev);
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	pr_debug("In MVC:mxc_poll\n");
+
+	if (down_interruptible(&cam->busy_lock))
+		return -EINTR;
+
+	queue = &cam->enc_queue;
+	poll_wait(file, queue, wait);
+
+	up(&cam->busy_lock);
+
+	return res;
+}
+
+/*!
+ * This structure defines the functions to be called in this driver.
+ */
+static struct file_operations mxc_v4l_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l_open,
+	.release = mxc_v4l_close,
+	.read = mxc_v4l_read,
+	.ioctl = mxc_v4l_ioctl,
+	.mmap = mxc_mmap,
+	.poll = mxc_poll,
+};
+
+static struct video_device mxc_v4l_template = {
+	.name = "Mxc Camera",
+	.vfl_type = VID_TYPE_CAPTURE,
+	.fops = &mxc_v4l_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * This function can be used to release any platform data on closing.
+ */
+static void camera_platform_release(struct device *device)
+{
+}
+
+/*! Device Definition for Mt9v111 devices */
+static struct platform_device mxc_v4l2_devices = {
+	.name = "mxc_v4l2",
+	.dev = {
+		.release = camera_platform_release,
+		},
+	.id = 0,
+};
+
+/*!
+ * Camera V4l2 callback function.
+ *
+ * @param mask      u32
+ *
+ * @param dev       void device structure
+ *
+ * @return status
+ */
+static void camera_callback(u32 mask, void *dev)
+{
+	struct mxc_v4l_frame *done_frame;
+	struct mxc_v4l_frame *ready_frame;
+
+	cam_data *cam = (cam_data *) dev;
+	if (cam == NULL)
+		return;
+
+	pr_debug("In MVC:camera_callback\n");
+
+	if (list_empty(&cam->working_q)) {
+		pr_err("ERROR: v4l2 capture: camera_callback: "
+			"working queue empty\n");
+		return;
+	}
+
+	done_frame =
+		list_entry(cam->working_q.next, struct mxc_v4l_frame, queue);
+	if (done_frame->buffer.flags & V4L2_BUF_FLAG_QUEUED) {
+		done_frame->buffer.flags |= V4L2_BUF_FLAG_DONE;
+		done_frame->buffer.flags &= ~V4L2_BUF_FLAG_QUEUED;
+
+		if (list_empty(&cam->ready_q)) {
+			cam->skip_frame++;
+		} else {
+			ready_frame = list_entry(cam->ready_q.next,
+						 struct mxc_v4l_frame,
+						 queue);
+			list_del(cam->ready_q.next);
+			list_add_tail(&ready_frame->queue, &cam->working_q);
+			cam->enc_update_eba(ready_frame->buffer.m.offset,
+					    &cam->ping_pong_csi);
+		}
+
+		/* Added to the done queue */
+		list_del(cam->working_q.next);
+		list_add_tail(&done_frame->queue, &cam->done_q);
+
+		/* Wake up the queue */
+		cam->enc_counter++;
+		wake_up_interruptible(&cam->enc_queue);
+	} else {
+		pr_err("ERROR: v4l2 capture: camera_callback: "
+			"buffer not queued\n");
+	}
+}
+
+/*!
+ * initialize cam_data structure
+ *
+ * @param cam      structure cam_data *
+ *
+ * @return status  0 Success
+ */
+static void init_camera_struct(cam_data *cam)
+{
+	pr_debug("In MVC: init_camera_struct\n");
+
+	/* Default everything to 0 */
+	memset(cam, 0, sizeof(cam_data));
+
+	init_MUTEX(&cam->param_lock);
+	init_MUTEX(&cam->busy_lock);
+
+	cam->video_dev = video_device_alloc();
+	if (cam->video_dev == NULL)
+		return;
+
+	*(cam->video_dev) = mxc_v4l_template;
+
+	video_set_drvdata(cam->video_dev, cam);
+	dev_set_drvdata(&mxc_v4l2_devices.dev, (void *)cam);
+	cam->video_dev->minor = -1;
+
+	init_waitqueue_head(&cam->enc_queue);
+	init_waitqueue_head(&cam->still_queue);
+
+	/* setup cropping */
+	cam->crop_bounds.left = 0;
+	cam->crop_bounds.width = 640;
+	cam->crop_bounds.top = 0;
+	cam->crop_bounds.height = 480;
+	cam->crop_current = cam->crop_defrect = cam->crop_bounds;
+	ipu_csi_set_window_size(cam->crop_current.width,
+				cam->crop_current.height, cam->csi);
+	ipu_csi_set_window_pos(cam->crop_current.left,
+				cam->crop_current.top, cam->csi);
+	cam->streamparm.parm.capture.capturemode = 0;
+
+	cam->standard.index = 0;
+	cam->standard.id = V4L2_STD_UNKNOWN;
+	cam->standard.frameperiod.denominator = 30;
+	cam->standard.frameperiod.numerator = 1;
+	cam->standard.framelines = 480;
+	cam->standard_autodetect = true;
+	cam->streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	cam->streamparm.parm.capture.timeperframe = cam->standard.frameperiod;
+	cam->streamparm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+	cam->overlay_on = false;
+	cam->capture_on = false;
+	cam->skip_frame = 0;
+	cam->v4l2_fb.flags = V4L2_FBUF_FLAG_OVERLAY;
+
+	cam->v2f.fmt.pix.sizeimage = 352 * 288 * 3 / 2;
+	cam->v2f.fmt.pix.bytesperline = 288 * 3 / 2;
+	cam->v2f.fmt.pix.width = 288;
+	cam->v2f.fmt.pix.height = 352;
+	cam->v2f.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
+	cam->win.w.width = 160;
+	cam->win.w.height = 160;
+	cam->win.w.left = 0;
+	cam->win.w.top = 0;
+
+	cam->csi = 0;  /* Need to determine how to set this correctly with
+			* multiple video input devices. */
+
+	cam->enc_callback = camera_callback;
+	init_waitqueue_head(&cam->power_queue);
+	spin_lock_init(&cam->int_lock);
+}
+
+/*!
+ * camera_power function
+ *    Turns Sensor power On/Off
+ *
+ * @param       cam           cam data struct
+ * @param       cameraOn      true to turn camera on, false to turn off power.
+ *
+ * @return status
+ */
+static u8 camera_power(cam_data *cam, bool cameraOn)
+{
+	pr_debug("In MVC:camera_power on=%d\n", cameraOn);
+
+	if (cameraOn == true) {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+		vidioc_int_s_power(cam->sensor, 1);
+	} else {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+		vidioc_int_s_power(cam->sensor, 0);
+	}
+	return 0;
+}
+
+/*!
+ * This function is called to put the sensor in a low power state.
+ * Refer to the document driver-model/driver.txt in the kernel source tree
+ * for more information.
+ *
+ * @param   pdev  the device structure used to give information on which I2C
+ *                to suspend
+ * @param   state the power state the device is entering
+ *
+ * @return  The function returns 0 on success and -1 on failure.
+ */
+static int mxc_v4l2_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	pr_debug("In MVC:mxc_v4l2_suspend\n");
+
+	if (cam == NULL) {
+		return -1;
+	}
+
+	cam->low_power = true;
+
+	if (cam->overlay_on == true)
+		stop_preview(cam);
+	if ((cam->capture_on == true) && cam->enc_disable) {
+		cam->enc_disable(cam);
+	}
+	camera_power(cam, false);
+
+	return 0;
+}
+
+/*!
+ * This function is called to bring the sensor back from a low power state.
+ * Refer to the document driver-model/driver.txt in the kernel source tree
+ * for more information.
+ *
+ * @param   pdev   the device structure
+ *
+ * @return  The function returns 0 on success and -1 on failure
+ */
+static int mxc_v4l2_resume(struct platform_device *pdev)
+{
+	cam_data *cam = platform_get_drvdata(pdev);
+
+	pr_debug("In MVC:mxc_v4l2_resume\n");
+
+	if (cam == NULL) {
+		return -1;
+	}
+
+	cam->low_power = false;
+	wake_up_interruptible(&cam->power_queue);
+	camera_power(cam, true);
+
+	if (cam->overlay_on == true)
+		start_preview(cam);
+	if (cam->capture_on == true)
+		mxc_streamon(cam);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2_driver = {
+	.driver = {
+		   .name = "mxc_v4l2",
+		   },
+	.probe = NULL,
+	.remove = NULL,
+	.suspend = mxc_v4l2_suspend,
+	.resume = mxc_v4l2_resume,
+	.shutdown = NULL,
+};
+
+/*!
+ * Initializes the camera driver.
+ */
+static int mxc_v4l2_master_attach(struct v4l2_int_device *slave)
+{
+	cam_data *cam = slave->u.slave->master->priv;
+	struct v4l2_format cam_fmt;
+
+	pr_debug("In MVC: mxc_v4l2_master_attach\n");
+	pr_debug("   slave.name = %s\n", slave->name);
+	pr_debug("   master.name = %s\n", slave->u.slave->master->name);
+
+	cam->sensor = slave;
+	if (slave == NULL) {
+		pr_err("ERROR: v4l2 capture: slave parameter not valid.\n");
+		return -1;
+	}
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	vidioc_int_dev_init(slave);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	vidioc_int_g_fmt_cap(cam->sensor, &cam_fmt);
+
+	/* Used to detect TV in (type 1) vs. camera (type 0)*/
+	cam->device_type = cam_fmt.fmt.pix.priv;
+
+	/* Set the input size to the ipu for this device */
+	cam->crop_bounds.top = cam->crop_bounds.left = 0;
+	cam->crop_bounds.width = cam_fmt.fmt.pix.width;
+	cam->crop_bounds.height = cam_fmt.fmt.pix.height;
+
+	/* This also is the max crop size for this device. */
+	cam->crop_defrect.top = cam->crop_defrect.left = 0;
+	cam->crop_defrect.width = cam_fmt.fmt.pix.width;
+	cam->crop_defrect.height = cam_fmt.fmt.pix.height;
+
+	/* At this point, this is also the current image size. */
+	cam->crop_current.top = cam->crop_current.left = 0;
+	cam->crop_current.width = cam_fmt.fmt.pix.width;
+	cam->crop_current.height = cam_fmt.fmt.pix.height;
+
+	pr_debug("End of %s: v2f pix widthxheight %d x %d\n",
+		 __func__,
+		 cam->v2f.fmt.pix.width, cam->v2f.fmt.pix.height);
+	pr_debug("End of %s: crop_bounds widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_bounds.width, cam->crop_bounds.height);
+	pr_debug("End of %s: crop_defrect widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_defrect.width, cam->crop_defrect.height);
+	pr_debug("End of %s: crop_current widthxheight %d x %d\n",
+		 __func__,
+		 cam->crop_current.width, cam->crop_current.height);
+
+	return 0;
+}
+
+/*!
+ * Disconnects the camera driver.
+ */
+static void mxc_v4l2_master_detach(struct v4l2_int_device *slave)
+{
+	pr_debug("In MVC:mxc_v4l2_master_detach\n");
+	vidioc_int_dev_exit(slave);
+}
+
+/*!
+ * Entry point for the V4L2
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int camera_init(void)
+{
+	u8 err = 0;
+
+	pr_debug("In MVC:camera_init\n");
+
+	/* Register the device driver structure. */
+	err = platform_driver_register(&mxc_v4l2_driver);
+	if (err != 0) {
+		pr_err("ERROR: v4l2 capture:camera_init: "
+			"platform_driver_register failed.\n");
+		return err;
+	}
+
+	/* Create g_cam and initialize it. */
+	if ((g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL)) == NULL) {
+		pr_err("ERROR: v4l2 capture: failed to register camera\n");
+		platform_driver_unregister(&mxc_v4l2_driver);
+		return -1;
+	}
+	init_camera_struct(g_cam);
+
+	/* Set up the v4l2 device and register it*/
+	mxc_v4l2_int_device.priv = g_cam;
+	/* This function contains a bug that won't let this be rmmod'd. */
+	v4l2_int_device_register(&mxc_v4l2_int_device);
+
+	/* Register the I2C device */
+	err = platform_device_register(&mxc_v4l2_devices);
+	if (err != 0) {
+		pr_err("ERROR: v4l2 capture: camera_init: "
+		       "platform_device_register failed.\n");
+		platform_driver_unregister(&mxc_v4l2_driver);
+		kfree(g_cam);
+		g_cam = NULL;
+		return err;
+	}
+
+	/* register v4l video device */
+	if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
+	    == -1) {
+		platform_device_unregister(&mxc_v4l2_devices);
+		platform_driver_unregister(&mxc_v4l2_driver);
+		kfree(g_cam);
+		g_cam = NULL;
+		pr_err("ERROR: v4l2 capture: video_register_device failed\n");
+		return -1;
+	}
+	pr_debug("   Video device registered: %s #%d\n",
+		 g_cam->video_dev->name, g_cam->video_dev->minor);
+
+	return err;
+}
+
+/*!
+ * Exit and cleanup for the V4L2
+ */
+static void __exit camera_exit(void)
+{
+	pr_debug("In MVC: camera_exit\n");
+
+	pr_info("V4L2 unregistering video\n");
+
+	if (g_cam->open_count) {
+		pr_err("ERROR: v4l2 capture:camera open "
+			"-- setting ops to NULL\n");
+	} else {
+		pr_info("V4L2 freeing image input device\n");
+		v4l2_int_device_unregister(&mxc_v4l2_int_device);
+		video_unregister_device(g_cam->video_dev);
+		platform_driver_unregister(&mxc_v4l2_driver);
+		platform_device_unregister(&mxc_v4l2_devices);
+
+		mxc_free_frame_buf(g_cam);
+		kfree(g_cam);
+		g_cam = NULL;
+	}
+}
+
+module_init(camera_init);
+module_exit(camera_exit);
+
+module_param(video_nr, int, 0444);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2 capture driver for Mxc based cameras");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
new file mode 100644
index 000000000000..a9c0c4d159da
--- /dev/null
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
@@ -0,0 +1,199 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup MXC_V4L2_CAPTURE MXC V4L2 Video Capture Driver
+ */
+/*!
+ * @file mxc_v4l2_capture.h
+ *
+ * @brief mxc V4L2 capture device API  Header file
+ *
+ * It include all the defines for frame operations, also three structure defines
+ * use case ops structure, common v4l2 driver structure and frame structure.
+ *
+ * @ingroup MXC_V4L2_CAPTURE
+ */
+#ifndef __MXC_V4L2_CAPTURE_H__
+#define __MXC_V4L2_CAPTURE_H__
+
+#include <asm/uaccess.h>
+#include <linux/list.h>
+#include <linux/smp_lock.h>
+#include <linux/ipu.h>
+#include <linux/mxc_v4l2.h>
+
+#include <media/v4l2-dev.h>
+
+#define FRAME_NUM 3
+
+/*!
+ * v4l2 frame structure.
+ */
+struct mxc_v4l_frame {
+	u32 paddress;
+	void *vaddress;
+	int count;
+	int width;
+	int height;
+
+	struct v4l2_buffer buffer;
+	struct list_head queue;
+	int index;
+};
+
+/* Only for old version.  Will go away soon. */
+typedef struct {
+	u8 clk_mode;
+	u8 ext_vsync;
+	u8 Vsync_pol;
+	u8 Hsync_pol;
+	u8 pixclk_pol;
+	u8 data_pol;
+	u8 data_width;
+	u8 pack_tight;
+	u8 force_eof;
+	u8 data_en_pol;
+	u16 width;
+	u16 height;
+	u32 pixel_fmt;
+	u32 mclk;
+	u16 active_width;
+	u16 active_height;
+} sensor_interface;
+
+/* Sensor control function */
+/* Only for old version.  Will go away soon. */
+struct camera_sensor {
+	void (*set_color) (int bright, int saturation, int red, int green,
+			   int blue);
+	void (*get_color) (int *bright, int *saturation, int *red, int *green,
+			   int *blue);
+	void (*set_ae_mode) (int ae_mode);
+	void (*get_ae_mode) (int *ae_mode);
+	sensor_interface *(*config) (int *frame_rate, int high_quality);
+	sensor_interface *(*reset) (void);
+	void (*get_std) (v4l2_std_id *std);
+	void (*set_std) (v4l2_std_id std);
+	unsigned int csi;
+};
+
+/*!
+ * common v4l2 driver structure.
+ */
+typedef struct _cam_data {
+	struct video_device *video_dev;
+	int device_type;
+
+	/* semaphore guard against SMP multithreading */
+	struct semaphore busy_lock;
+
+	int open_count;
+
+	/* params lock for this camera */
+	struct semaphore param_lock;
+
+	/* Encoder */
+	struct list_head ready_q;
+	struct list_head done_q;
+	struct list_head working_q;
+	int ping_pong_csi;
+	spinlock_t int_lock;
+	struct mxc_v4l_frame frame[FRAME_NUM];
+	int skip_frame;
+	wait_queue_head_t enc_queue;
+	int enc_counter;
+	dma_addr_t rot_enc_bufs[2];
+	void *rot_enc_bufs_vaddr[2];
+	int rot_enc_buf_size[2];
+	enum v4l2_buf_type type;
+
+	/* still image capture */
+	wait_queue_head_t still_queue;
+	int still_counter;
+	dma_addr_t still_buf;
+	void *still_buf_vaddr;
+
+	/* overlay */
+	struct v4l2_window win;
+	struct v4l2_framebuffer v4l2_fb;
+	dma_addr_t vf_bufs[2];
+	void *vf_bufs_vaddr[2];
+	int vf_bufs_size[2];
+	dma_addr_t rot_vf_bufs[2];
+	void *rot_vf_bufs_vaddr[2];
+	int rot_vf_buf_size[2];
+	bool overlay_active;
+	int output;
+	struct fb_info *overlay_fb;
+
+	/* v4l2 format */
+	struct v4l2_format v2f;
+	int rotation;	/* for IPUv1 and IPUv3, this means encoder rotation */
+	int vf_rotation; /* viewfinder rotation only for IPUv1 and IPUv3 */
+	struct v4l2_mxc_offset offset;
+
+	/* V4l2 control bit */
+	int bright;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+	/* standard */
+	struct v4l2_streamparm streamparm;
+	struct v4l2_standard standard;
+	bool standard_autodetect;
+
+	/* crop */
+	struct v4l2_rect crop_bounds;
+	struct v4l2_rect crop_defrect;
+	struct v4l2_rect crop_current;
+
+	int (*enc_update_eba) (dma_addr_t eba, int *bufferNum);
+	int (*enc_enable) (void *private);
+	int (*enc_disable) (void *private);
+	void (*enc_callback) (u32 mask, void *dev);
+	int (*vf_start_adc) (void *private);
+	int (*vf_stop_adc) (void *private);
+	int (*vf_start_sdc) (void *private);
+	int (*vf_stop_sdc) (void *private);
+	int (*csi_start) (void *private);
+	int (*csi_stop) (void *private);
+
+	/* misc status flag */
+	bool overlay_on;
+	bool capture_on;
+	int overlay_pid;
+	int capture_pid;
+	bool low_power;
+	wait_queue_head_t power_queue;
+	unsigned int csi;
+	int current_input;
+
+	/* camera sensor interface */
+	struct camera_sensor *cam_sensor; 	/* old version */
+	struct v4l2_int_device *sensor;
+} cam_data;
+
+#if defined(CONFIG_MXC_IPU_V1) || defined(CONFIG_VIDEO_MXC_EMMA_CAMERA) \
+			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA_MODULE) \
+			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA)
+void set_mclk_rate(uint32_t *p_mclk_freq);
+#else
+void set_mclk_rate(uint32_t *p_mclk_freq, uint32_t csi);
+#endif
+#endif				/* __MXC_V4L2_CAPTURE_H__ */
diff --git a/drivers/media/video/mxc/capture/ov2640.c b/drivers/media/video/mxc/capture/ov2640.c
new file mode 100644
index 000000000000..a1329b0b431d
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ov2640.c
@@ -0,0 +1,1080 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file ov2640.c
+ *
+ * @brief ov2640 camera driver functions
+ *
+ * @ingroup Camera
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/regulator/consumer.h>
+
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+
+#define MIN_FPS 5
+#define MAX_FPS 30
+#define DEFAULT_FPS 30
+
+#define OV2640_XCLK_MIN 6000000
+#define OV2640_XCLK_MAX 27000000
+
+/*
+enum ov2640_mode {
+	ov2640_mode_1600_1120,
+	ov2640_mode_800_600
+};
+*/
+
+struct reg_value {
+	u8 reg;
+	u8 value;
+	int delay_ms;
+};
+
+static struct reg_value ov2640_setting_1600_1120[] = {
+#ifdef CONFIG_MACH_MX25_3DS
+	{0xff, 0x01, 0}, {0x12, 0x80, 5}, {0xff, 0x00, 0}, {0x2c, 0xff, 0},
+	{0x2e, 0xdf, 0}, {0xff, 0x01, 0}, {0x3c, 0x32, 0}, {0x11, 0x00, 0},
+	{0x09, 0x02, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0}, {0x14, 0x48, 0},
+	{0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0}, {0x3b, 0xfb, 0},
+	{0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0}, {0x39, 0x02, 0},
+	{0x35, 0x58, 0}, {0x22, 0x0a, 0}, {0x37, 0x40, 0}, {0x23, 0x00, 0},
+	{0x34, 0xa0, 0}, {0x36, 0x1a, 0}, {0x06, 0x02, 0}, {0x07, 0xc0, 0},
+	{0x0d, 0xb7, 0}, {0x0e, 0x01, 0}, {0x4c, 0x00, 0}, {0x4a, 0x81, 0},
+	{0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0}, {0x26, 0x82, 0},
+	{0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x3f, 0}, {0x61, 0x70, 0},
+	{0x62, 0x80, 0}, {0x7c, 0x05, 0}, {0x20, 0x80, 0}, {0x28, 0x30, 0},
+	{0x6c, 0x00, 0}, {0x6d, 0x80, 0}, {0x6e, 0x00, 0}, {0x70, 0x02, 0},
+	{0x71, 0x94, 0}, {0x73, 0xc1, 0}, {0x3d, 0x34, 0}, {0x5a, 0x57, 0},
+	{0x4f, 0xbb, 0}, {0x50, 0x9c, 0}, {0xff, 0x00, 0}, {0xe5, 0x7f, 0},
+	{0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0xe0, 0x14, 0}, {0x76, 0xff, 0},
+	{0x33, 0xa0, 0}, {0x42, 0x20, 0}, {0x43, 0x18, 0}, {0x4c, 0x00, 0},
+	{0x87, 0xd0, 0}, {0x88, 0x3f, 0}, {0xd7, 0x01, 0}, {0xd9, 0x10, 0},
+	{0xd3, 0x82, 0}, {0xc8, 0x08, 0}, {0xc9, 0x80, 0}, {0x7c, 0x00, 0},
+	{0x7d, 0x00, 0}, {0x7c, 0x03, 0}, {0x7d, 0x48, 0}, {0x7d, 0x48, 0},
+	{0x7c, 0x08, 0}, {0x7d, 0x20, 0}, {0x7d, 0x10, 0}, {0x7d, 0x0e, 0},
+	{0x90, 0x00, 0}, {0x91, 0x0e, 0}, {0x91, 0x1a, 0}, {0x91, 0x31, 0},
+	{0x91, 0x5a, 0}, {0x91, 0x69, 0}, {0x91, 0x75, 0}, {0x91, 0x7e, 0},
+	{0x91, 0x88, 0}, {0x91, 0x8f, 0}, {0x91, 0x96, 0}, {0x91, 0xa3, 0},
+	{0x91, 0xaf, 0}, {0x91, 0xc4, 0}, {0x91, 0xd7, 0}, {0x91, 0xe8, 0},
+	{0x91, 0x20, 0}, {0x92, 0x00, 0}, {0x93, 0x06, 0}, {0x93, 0xe3, 0},
+	{0x93, 0x05, 0}, {0x93, 0x05, 0}, {0x93, 0x00, 0}, {0x93, 0x04, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x96, 0x00, 0},
+	{0x97, 0x08, 0}, {0x97, 0x19, 0}, {0x97, 0x02, 0}, {0x97, 0x0c, 0},
+	{0x97, 0x24, 0}, {0x97, 0x30, 0}, {0x97, 0x28, 0}, {0x97, 0x26, 0},
+	{0x97, 0x02, 0}, {0x97, 0x98, 0}, {0x97, 0x80, 0}, {0x97, 0x00, 0},
+	{0x97, 0x00, 0}, {0xc3, 0xed, 0}, {0xa4, 0x00, 0}, {0xa8, 0x00, 0},
+	{0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0}, {0xc7, 0x10, 0},
+	{0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0}, {0xb9, 0x7c, 0},
+	{0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0}, {0xb0, 0xc5, 0},
+	{0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0}, {0xc0, 0xc8, 0},
+	{0xc1, 0x96, 0}, {0x86, 0x1d, 0}, {0x50, 0x00, 0}, {0x51, 0x90, 0},
+	{0x52, 0x2c, 0}, {0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x88, 0},
+	{0x57, 0x00, 0}, {0x5a, 0x90, 0}, {0x5b, 0x2c, 0}, {0x5c, 0x05, 0},
+	{0xc3, 0xed, 0}, {0x7f, 0x00, 0}, {0xda, 0x00, 0}, {0xe5, 0x1f, 0},
+	{0xe1, 0x77, 0}, {0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0},
+	{0xff, 0x00, 0}, {0xe0, 0x04, 0}, {0xc0, 0xc8, 0}, {0xc1, 0x96, 0},
+	{0x86, 0x3d, 0}, {0x50, 0x00, 0}, {0x51, 0x90, 0}, {0x52, 0x2c, 0},
+	{0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x88, 0}, {0x57, 0x00, 0},
+	{0x5a, 0x40, 0}, {0x5b, 0xf0, 0}, {0x5c, 0x01, 0}, {0xd3, 0x82, 0},
+	{0xe0, 0x00, 1000}
+#else
+	{0xff, 0x1, 0}, {0x12, 0x80, 1}, {0xff, 0, 0}, {0x2c, 0xff, 0},
+	{0x2e, 0xdf, 0}, {0xff, 0x1, 0}, {0x3c, 0x32, 0}, {0x11, 0x01, 0},
+	{0x09, 0x00, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0}, {0x14, 0x48, 0},
+	{0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0}, {0x3b, 0xfb, 0},
+	{0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0}, {0x39, 0x82, 0},
+	{0x35, 0x88, 0}, {0x22, 0x0a, 0}, {0x37, 0x40, 0}, {0x23, 0x00, 0},
+	{0x34, 0xa0, 0}, {0x36, 0x1a, 0}, {0x06, 0x02, 0}, {0x07, 0xc0, 0},
+	{0x0d, 0xb7, 0}, {0x0e, 0x01, 0}, {0x4c, 0x00, 0}, {0x4a, 0x81, 0},
+	{0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0}, {0x26, 0x82, 0},
+	{0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x3f, 0}, {0x0c, 0x3c, 0},
+	{0x5d, 0x55, 0}, {0x5e, 0x7d, 0}, {0x5f, 0x7d, 0}, {0x60, 0x55, 0},
+	{0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0}, {0x20, 0x80, 0},
+	{0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0}, {0x6e, 00, 0},
+	{0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0}, {0x3d, 0x34, 0},
+	{0x5a, 0x57, 0}, {0x4f, 0xbb, 0}, {0x50, 0x9c, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0x44, 0x06, 0},
+	{0xe0, 0x14, 0}, {0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0},
+	{0x43, 0x18, 0}, {0x4c, 0x00, 0}, {0x87, 0xd0, 0}, {0xd7, 0x03, 0},
+	{0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0}, {0xc9, 0x80, 0},
+	{0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0}, {0x7d, 0x48, 0},
+	{0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0}, {0x7d, 0x10, 0},
+	{0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0}, {0x91, 0x1a, 0},
+	{0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0}, {0x91, 0x75, 0},
+	{0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0}, {0x91, 0x96, 0},
+	{0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0}, {0x91, 0xd7, 0},
+	{0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0}, {0x93, 0x06, 0},
+	{0x93, 0xe3, 0}, {0x93, 0x03, 0}, {0x93, 0x03, 0}, {0x93, 0x00, 0},
+	{0x93, 0x02, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0}, {0x97, 0x02, 0},
+	{0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0}, {0x97, 0x28, 0},
+	{0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0}, {0x97, 0x80, 0},
+	{0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xa4, 0x00, 0}, {0xa8, 0x00, 0},
+	{0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0}, {0xc7, 0x10, 0},
+	{0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0}, {0xb9, 0x7c, 0},
+	{0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0}, {0xb0, 0xc5, 0},
+	{0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0}, {0xa6, 0x00, 0},
+	{0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x1b, 0}, {0xa7, 0x31, 0},
+	{0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0},
+	{0xa7, 0x19, 0}, {0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0},
+	{0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x19, 0}, {0xa7, 0x31, 0},
+	{0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xc0, 0xc8, 0}, {0xc1, 0x96, 0},
+	{0x86, 0x3d, 0}, {0x50, 0x00, 0}, {0x51, 0x90, 0}, {0x52, 0x18, 0},
+	{0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x88, 0}, {0x57, 0x00, 0},
+	{0x5a, 0x90, 0}, {0x5b, 0x18, 0}, {0x5c, 0x05, 0}, {0xc3, 0xef, 0},
+	{0x7f, 0x00, 0}, {0xda, 0x01, 0}, {0xe5, 0x1f, 0}, {0xe1, 0x67, 0},
+	{0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0}
+#endif
+};
+
+static struct reg_value ov2640_setting_800_600[] = {
+#ifdef CONFIG_MACH_MX25_3DS
+	{0xff, 0x01, 0}, {0x12, 0x80, 5}, {0xff, 0x00, 0}, {0x2c, 0xff, 0},
+	{0x2e, 0xdf, 0}, {0xff, 0x01, 0}, {0x3c, 0x32, 0}, {0x11, 0x00, 0},
+	{0x09, 0x02, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0}, {0x14, 0x48, 0},
+	{0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0}, {0x3b, 0xfb, 0},
+	{0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0}, {0x39, 0x92, 0},
+	{0x35, 0xda, 0}, {0x22, 0x1a, 0}, {0x37, 0xc3, 0}, {0x23, 0x00, 0},
+	{0x34, 0xc0, 0}, {0x36, 0x1a, 0}, {0x06, 0x88, 0}, {0x07, 0xc0, 0},
+	{0x0d, 0x87, 0}, {0x0e, 0x41, 0}, {0x4c, 0x00, 0},
+	{0x48, 0x00, 0}, {0x5b, 0x00, 0}, {0x42, 0x03, 0}, {0x4a, 0x81, 0},
+	{0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0}, {0x26, 0x82, 0},
+	{0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x22, 0}, {0x0c, 0x3c, 0},
+	{0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0}, {0x20, 0x80, 0},
+	{0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0}, {0x6e, 0x00, 0},
+	{0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0}, {0x12, 0x40, 0},
+	{0x17, 0x11, 0}, {0x18, 0x43, 0}, {0x19, 0x00, 0}, {0x1a, 0x4b, 0},
+	{0x32, 0x09, 0}, {0x37, 0xc0, 0}, {0x4f, 0xca, 0}, {0x50, 0xa8, 0},
+	{0x5a, 0x23, 0}, {0x6d, 0x00, 0}, {0x3d, 0x38, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0xe0, 0x14, 0},
+	{0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0}, {0x43, 0x18, 0},
+	{0x4c, 0x00, 0}, {0x87, 0xd5, 0}, {0x88, 0x3f, 0}, {0xd7, 0x01, 0},
+	{0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0}, {0xc9, 0x80, 0},
+	{0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0}, {0x7d, 0x48, 0},
+	{0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0}, {0x7d, 0x10, 0},
+	{0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0}, {0x91, 0x1a, 0},
+	{0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0}, {0x91, 0x75, 0},
+	{0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0}, {0x91, 0x96, 0},
+	{0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0}, {0x91, 0xd7, 0},
+	{0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0}, {0x93, 0x06, 0},
+	{0x93, 0xe3, 0}, {0x93, 0x05, 0}, {0x93, 0x05, 0}, {0x93, 0x00, 0},
+	{0x93, 0x04, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0}, {0x97, 0x02, 0},
+	{0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0}, {0x97, 0x28, 0},
+	{0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0}, {0x97, 0x80, 0},
+	{0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xc3, 0xed, 0}, {0xa4, 0x00, 0},
+	{0xa8, 0x00, 0}, {0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0},
+	{0xc7, 0x10, 0}, {0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0},
+	{0xb9, 0x7c, 0}, {0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0},
+	{0xb0, 0xc5, 0}, {0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0},
+	{0xc0, 0x64, 0}, {0xc1, 0x4b, 0}, {0x8c, 0x00, 0}, {0x86, 0x3d, 0},
+	{0x50, 0x00, 0}, {0x51, 0xc8, 0}, {0x52, 0x96, 0}, {0x53, 0x00, 0},
+	{0x54, 0x00, 0}, {0x55, 0x00, 0}, {0x5a, 0xc8, 0}, {0x5b, 0x96, 0},
+	{0x5c, 0x00, 0}, {0xd3, 0x82, 0}, {0xc3, 0xed, 0}, {0x7f, 0x00, 0},
+	{0xda, 0x00, 0}, {0xe5, 0x1f, 0}, {0xe1, 0x67, 0}, {0xe0, 0x00, 0},
+	{0xdd, 0x7f, 0}, {0x05, 0x00, 0}, {0xff, 0x00, 0}, {0xe0, 0x04, 0},
+	{0xc0, 0x64, 0}, {0xc1, 0x4b, 0}, {0x8c, 0x00, 0}, {0x86, 0x3d, 0},
+	{0x50, 0x00, 0}, {0x51, 0xc8, 0}, {0x52, 0x96, 0}, {0x53, 0x00, 0},
+	{0x54, 0x00, 0}, {0x55, 0x00, 0}, {0x5a, 0xa0, 0}, {0x5b, 0x78, 0},
+	{0x5c, 0x00, 0}, {0xd3, 0x82, 0}, {0xe0, 0x00, 1000}
+#else
+	{0xff, 0, 0}, {0xff, 1, 0}, {0x12, 0x80, 1}, {0xff, 00, 0},
+	{0x2c, 0xff, 0}, {0x2e, 0xdf, 0}, {0xff, 0x1, 0}, {0x3c, 0x32, 0},
+	{0x11, 0x01, 0}, {0x09, 0x00, 0}, {0x04, 0x28, 0}, {0x13, 0xe5, 0},
+	{0x14, 0x48, 0}, {0x2c, 0x0c, 0}, {0x33, 0x78, 0}, {0x3a, 0x33, 0},
+	{0x3b, 0xfb, 0}, {0x3e, 0x00, 0}, {0x43, 0x11, 0}, {0x16, 0x10, 0},
+	{0x39, 0x92, 0}, {0x35, 0xda, 0}, {0x22, 0x1a, 0}, {0x37, 0xc3, 0},
+	{0x23, 0x00, 0}, {0x34, 0xc0, 0}, {0x36, 0x1a, 0}, {0x06, 0x88, 0},
+	{0x07, 0xc0, 0}, {0x0d, 0x87, 0}, {0x0e, 0x41, 0}, {0x4c, 0x00, 0},
+	{0x4a, 0x81, 0}, {0x21, 0x99, 0}, {0x24, 0x40, 0}, {0x25, 0x38, 0},
+	{0x26, 0x82, 0}, {0x5c, 0x00, 0}, {0x63, 0x00, 0}, {0x46, 0x22, 0},
+	{0x0c, 0x3c, 0}, {0x5d, 0x55, 0}, {0x5e, 0x7d, 0}, {0x5f, 0x7d, 0},
+	{0x60, 0x55, 0}, {0x61, 0x70, 0}, {0x62, 0x80, 0}, {0x7c, 0x05, 0},
+	{0x20, 0x80, 0}, {0x28, 0x30, 0}, {0x6c, 0x00, 0}, {0x6d, 0x80, 0},
+	{0x6e, 00, 0}, {0x70, 0x02, 0}, {0x71, 0x94, 0}, {0x73, 0xc1, 0},
+	{0x12, 0x40, 0}, {0x17, 0x11, 0}, {0x18, 0x43, 0}, {0x19, 0x00, 0},
+	{0x1a, 0x4b, 0}, {0x32, 0x09, 0}, {0x37, 0xc0, 0}, {0x4f, 0xca, 0},
+	{0x50, 0xa8, 0}, {0x6d, 0x00, 0}, {0x3d, 0x38, 0}, {0xff, 0x00, 0},
+	{0xe5, 0x7f, 0}, {0xf9, 0xc0, 0}, {0x41, 0x24, 0}, {0x44, 0x06, 0},
+	{0xe0, 0x14, 0}, {0x76, 0xff, 0}, {0x33, 0xa0, 0}, {0x42, 0x20, 0},
+	{0x43, 0x18, 0}, {0x4c, 0x00, 0}, {0x87, 0xd0, 0}, {0x88, 0x3f, 0},
+	{0xd7, 0x03, 0}, {0xd9, 0x10, 0}, {0xd3, 0x82, 0}, {0xc8, 0x08, 0},
+	{0xc9, 0x80, 0}, {0x7c, 0x00, 0}, {0x7d, 0x00, 0}, {0x7c, 0x03, 0},
+	{0x7d, 0x48, 0}, {0x7d, 0x48, 0}, {0x7c, 0x08, 0}, {0x7d, 0x20, 0},
+	{0x7d, 0x10, 0}, {0x7d, 0x0e, 0}, {0x90, 0x00, 0}, {0x91, 0x0e, 0},
+	{0x91, 0x1a, 0}, {0x91, 0x31, 0}, {0x91, 0x5a, 0}, {0x91, 0x69, 0},
+	{0x91, 0x75, 0}, {0x91, 0x7e, 0}, {0x91, 0x88, 0}, {0x91, 0x8f, 0},
+	{0x91, 0x96, 0}, {0x91, 0xa3, 0}, {0x91, 0xaf, 0}, {0x91, 0xc4, 0},
+	{0x91, 0xd7, 0}, {0x91, 0xe8, 0}, {0x91, 0x20, 0}, {0x92, 0x00, 0},
+	{0x93, 0x06, 0}, {0x93, 0xe3, 0}, {0x93, 0x03, 0}, {0x93, 0x03, 0},
+	{0x93, 0x00, 0}, {0x93, 0x02, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0}, {0x93, 0x00, 0},
+	{0x93, 0x00, 0}, {0x96, 0x00, 0}, {0x97, 0x08, 0}, {0x97, 0x19, 0},
+	{0x97, 0x02, 0}, {0x97, 0x0c, 0}, {0x97, 0x24, 0}, {0x97, 0x30, 0},
+	{0x97, 0x28, 0}, {0x97, 0x26, 0}, {0x97, 0x02, 0}, {0x97, 0x98, 0},
+	{0x97, 0x80, 0}, {0x97, 0x00, 0}, {0x97, 0x00, 0}, {0xa4, 0x00, 0},
+	{0xa8, 0x00, 0}, {0xc5, 0x11, 0}, {0xc6, 0x51, 0}, {0xbf, 0x80, 0},
+	{0xc7, 0x10, 0}, {0xb6, 0x66, 0}, {0xb8, 0xa5, 0}, {0xb7, 0x64, 0},
+	{0xb9, 0x7c, 0}, {0xb3, 0xaf, 0}, {0xb4, 0x97, 0}, {0xb5, 0xff, 0},
+	{0xb0, 0xc5, 0}, {0xb1, 0x94, 0}, {0xb2, 0x0f, 0}, {0xc4, 0x5c, 0},
+	{0xa6, 0x00, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x1b, 0},
+	{0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xa7, 0x20, 0},
+	{0xa7, 0xd8, 0}, {0xa7, 0x19, 0}, {0xa7, 0x31, 0}, {0xa7, 0x00, 0},
+	{0xa7, 0x18, 0}, {0xa7, 0x20, 0}, {0xa7, 0xd8, 0}, {0xa7, 0x19, 0},
+	{0xa7, 0x31, 0}, {0xa7, 0x00, 0}, {0xa7, 0x18, 0}, {0xc0, 0x64, 0},
+	{0xc1, 0x4b, 0}, {0x86, 0x1d, 0}, {0x50, 0x00, 0}, {0x51, 0xc8, 0},
+	{0x52, 0x96, 0}, {0x53, 0x00, 0}, {0x54, 0x00, 0}, {0x55, 0x00, 0},
+	{0x57, 0x00, 0}, {0x5a, 0xc8, 0}, {0x5b, 0x96, 0}, {0x5c, 0x00, 0},
+	{0xc3, 0xef, 0}, {0x7f, 0x00, 0}, {0xda, 0x01, 0}, {0xe5, 0x1f, 0},
+	{0xe1, 0x67, 0}, {0xe0, 0x00, 0}, {0xdd, 0x7f, 0}, {0x05, 0x00, 0}
+#endif
+};
+
+/*!
+ * Maintains the information on the current state of the sesor.
+ */
+struct sensor {
+	const struct ov2640_platform_data *platform_data;
+	struct v4l2_int_device *v4l2_int_device;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	struct v4l2_captureparm streamcap;
+	bool on;
+
+	/* control settings */
+	int brightness;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+	u32 csi;
+	u32 mclk;
+
+} ov2640_data;
+
+static struct regulator *io_regulator;
+static struct regulator *core_regulator;
+static struct regulator *analog_regulator;
+static struct regulator *gpo_regulator;
+
+extern void gpio_sensor_active(void);
+extern void gpio_sensor_inactive(void);
+
+/* list of image formats supported by this sensor */
+/*
+const static struct v4l2_fmtdesc ov2640_formats[] = {
+	{
+		.description = "YUYV (YUV 4:2:2), packed",
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+	},
+};
+ */
+
+static int ov2640_init_mode(struct sensor *s)
+{
+	int ret = -1;
+	struct reg_value *setting;
+	int i, num;
+
+	pr_debug("In ov2640:ov2640_init_mode capturemode is %d\n",
+		s->streamcap.capturemode);
+
+	if (s->streamcap.capturemode & V4L2_MODE_HIGHQUALITY) {
+		s->pix.width = 1600;
+		s->pix.height = 1120;
+		setting = ov2640_setting_1600_1120;
+		num = ARRAY_SIZE(ov2640_setting_1600_1120);
+	} else {
+		s->pix.width = 800;
+		s->pix.height = 600;
+		setting = ov2640_setting_800_600;
+		num = ARRAY_SIZE(ov2640_setting_800_600);
+	}
+
+	for (i = 0; i < num; i++) {
+		ret = i2c_smbus_write_byte_data(s->i2c_client,
+						setting[i].reg,
+						setting[i].value);
+		if (ret < 0) {
+			pr_err("write reg error: reg=%x, val=%x\n",
+			       setting[i].reg, setting[i].value);
+			return ret;
+		}
+		if (setting[i].delay_ms > 0)
+			msleep(setting[i].delay_ms);
+	}
+
+	return ret;
+}
+
+/* At present only support change to 15fps(only for SVGA mode) */
+static int ov2640_set_fps(struct sensor *s, int fps)
+{
+	int ret = 0;
+
+	if (i2c_smbus_write_byte_data(s->i2c_client, 0xff, 0x01) < 0) {
+		pr_err("in %s,change to sensor addr failed\n", __func__);
+		ret = -EPERM;
+	}
+
+	/* change the camera framerate to 15fps(only for SVGA mode) */
+	if (i2c_smbus_write_byte_data(s->i2c_client, 0x11, 0x01) < 0) {
+		pr_err("change camera to 15fps failed\n");
+		ret = -EPERM;
+	}
+
+	return ret;
+}
+
+static int ov2640_set_format(struct sensor *s, int format)
+{
+	int ret = 0;
+
+	if (i2c_smbus_write_byte_data(s->i2c_client, 0xff, 0x00) < 0)
+		ret = -EPERM;
+
+	if (format == V4L2_PIX_FMT_RGB565) {
+		/* set RGB565 format */
+		if (i2c_smbus_write_byte_data(s->i2c_client, 0xda, 0x08) < 0)
+			ret = -EPERM;
+
+		if (i2c_smbus_write_byte_data(s->i2c_client, 0xd7, 0x03) < 0)
+			ret = -EPERM;
+	} else if (format == V4L2_PIX_FMT_YUV420) {
+		/* set YUV420 format */
+		if (i2c_smbus_write_byte_data(s->i2c_client, 0xda, 0x00) < 0)
+			ret = -EPERM;
+
+		if (i2c_smbus_write_byte_data(s->i2c_client, 0xd7, 0x1b) < 0)
+			ret = -EPERM;
+	} else {
+		pr_debug("format not supported\n");
+	}
+
+	return ret;
+}
+
+/* --------------- IOCTL functions from v4l2_int_ioctl_desc --------------- */
+
+/*!
+ * ioctl_g_ifparm - V4L2 sensor interface handler for vidioc_int_g_ifparm_num
+ * s: pointer to standard V4L2 device structure
+ * p: pointer to standard V4L2 vidioc_int_g_ifparm_num ioctl structure
+ *
+ * Gets slave interface parameters.
+ * Calculates the required xclk value to support the requested
+ * clock parameters in p.  This value is returned in the p
+ * parameter.
+ *
+ * vidioc_int_g_ifparm returns platform-specific information about the
+ * interface settings used by the sensor.
+ *
+ * Given the image capture format in pix, the nominal frame period in
+ * timeperframe, calculate the required xclk frequency.
+ *
+ * Called on open.
+ */
+static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
+{
+	pr_debug("In ov2640:ioctl_g_ifparm\n");
+
+	if (s == NULL) {
+		pr_err("   ERROR!! no slave device set!\n");
+		return -1;
+	}
+
+	memset(p, 0, sizeof(*p));
+	p->u.bt656.clock_curr = ov2640_data.mclk;
+	p->if_type = V4L2_IF_TYPE_BT656;
+	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.clock_min = OV2640_XCLK_MIN;
+	p->u.bt656.clock_max = OV2640_XCLK_MAX;
+
+	return 0;
+}
+
+/*!
+ * Sets the camera power.
+ *
+ * s  pointer to the camera device
+ * on if 1, power is to be turned on.  0 means power is to be turned off
+ *
+ * ioctl_s_power - V4L2 sensor interface handler for vidioc_int_s_power_num
+ * @s: pointer to standard V4L2 device structure
+ * @on: power state to which device is to be set
+ *
+ * Sets devices power state to requrested state, if possible.
+ * This is called on open, close, suspend and resume.
+ */
+static int ioctl_s_power(struct v4l2_int_device *s, int on)
+{
+	struct sensor *sensor = s->priv;
+
+	pr_debug("In ov2640:ioctl_s_power\n");
+
+	if (on && !sensor->on) {
+		gpio_sensor_active();
+		if (io_regulator)
+			if (regulator_enable(io_regulator) != 0)
+				return -EIO;
+		if (core_regulator)
+			if (regulator_enable(core_regulator) != 0)
+				return -EIO;
+		if (gpo_regulator)
+			if (regulator_enable(gpo_regulator) != 0)
+				return -EIO;
+		if (analog_regulator)
+			if (regulator_enable(analog_regulator) != 0)
+				return -EIO;
+	} else if (!on && sensor->on) {
+		if (analog_regulator)
+			regulator_disable(analog_regulator);
+		if (core_regulator)
+			regulator_disable(core_regulator);
+		if (io_regulator)
+			regulator_disable(io_regulator);
+		if (gpo_regulator)
+			regulator_disable(gpo_regulator);
+		gpio_sensor_inactive();
+	}
+
+	sensor->on = on;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_parm - V4L2 sensor interface handler for VIDIOC_G_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the sensor's video CAPTURE parameters.
+ */
+static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	int ret = 0;
+
+	pr_debug("In ov2640:ioctl_g_parm\n");
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = sensor->streamcap.capability;
+		cparm->timeperframe = sensor->streamcap.timeperframe;
+		cparm->capturemode = sensor->streamcap.capturemode;
+		ret = 0;
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_err("   type is not V4L2_BUF_TYPE_VIDEO_CAPTURE " \
+			"but %d\n", a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_err("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ */
+static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_fract *timeperframe = &a->parm.capture.timeperframe;
+	u32 tgt_fps;	/* target frames per secound */
+	int ret = 0;
+
+	pr_debug("In ov2640:ioctl_s_parm\n");
+
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
+
+		/* Check that the new frame rate is allowed. */
+		if ((timeperframe->numerator == 0)
+		    || (timeperframe->denominator == 0)) {
+			timeperframe->denominator = DEFAULT_FPS;
+			timeperframe->numerator = 1;
+		}
+		tgt_fps = timeperframe->denominator
+			  / timeperframe->numerator;
+
+		if (tgt_fps > MAX_FPS) {
+			timeperframe->denominator = MAX_FPS;
+			timeperframe->numerator = 1;
+		} else if (tgt_fps < MIN_FPS) {
+			timeperframe->denominator = MIN_FPS;
+			timeperframe->numerator = 1;
+		}
+		sensor->streamcap.timeperframe = *timeperframe;
+		sensor->streamcap.capturemode =
+				(u32)a->parm.capture.capturemode;
+
+		ret = ov2640_init_mode(sensor);
+		if (tgt_fps == 15)
+			ov2640_set_fps(sensor, tgt_fps);
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_err("   type is not V4L2_BUF_TYPE_VIDEO_CAPTURE " \
+			"but %d\n", a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_err("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_fmt_cap - V4L2 sensor interface handler for ioctl_s_fmt_cap
+ * 		     set camera output format and resolution format
+ *
+ * @s: pointer to standard V4L2 device structure
+ * @arg: pointer to parameter, according this to set camera
+ *
+ * Returns 0 if set succeed, else return -1
+ */
+static int ioctl_s_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+	u32 format = f->fmt.pix.pixelformat;
+	int size = 0, ret = 0;
+
+	size = f->fmt.pix.width * f->fmt.pix.height;
+	switch (format) {
+	case V4L2_PIX_FMT_RGB565:
+		if (size > 640 * 480)
+			sensor->streamcap.capturemode = V4L2_MODE_HIGHQUALITY;
+		else
+			sensor->streamcap.capturemode = 0;
+		ret = ov2640_init_mode(sensor);
+
+		ret = ov2640_set_format(sensor, V4L2_PIX_FMT_RGB565);
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		if (size > 640 * 480)
+			sensor->streamcap.capturemode = V4L2_MODE_HIGHQUALITY;
+		else
+			sensor->streamcap.capturemode = 0;
+		ret = ov2640_init_mode(sensor);
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		if (size > 640 * 480)
+			sensor->streamcap.capturemode = V4L2_MODE_HIGHQUALITY;
+		else
+			sensor->streamcap.capturemode = 0;
+		ret = ov2640_init_mode(sensor);
+
+		/* YUYV: width * 2, YY: width */
+		ret = ov2640_set_format(sensor, V4L2_PIX_FMT_YUV420);
+		break;
+	default:
+		pr_debug("case not supported\n");
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_g_fmt_cap - V4L2 sensor interface handler for ioctl_g_fmt_cap
+ * @s: pointer to standard V4L2 device structure
+ * @f: pointer to standard V4L2 v4l2_format structure
+ *
+ * Returns the sensor's current pixel format in the v4l2_format
+ * parameter.
+ */
+static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+
+	pr_debug("In ov2640:ioctl_g_fmt_cap.\n");
+
+	f->fmt.pix = sensor->pix;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_G_CTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control's current
+ * value from the video_control[] array.  Otherwise, returns -EINVAL
+ * if the control is not supported.
+ */
+static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int ret = 0;
+
+	pr_debug("In ov2640:ioctl_g_ctrl\n");
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		vc->value = ov2640_data.brightness;
+		break;
+	case V4L2_CID_HUE:
+		vc->value = ov2640_data.hue;
+		break;
+	case V4L2_CID_CONTRAST:
+		vc->value = ov2640_data.contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		vc->value = ov2640_data.saturation;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		vc->value = ov2640_data.red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		vc->value = ov2640_data.blue;
+		break;
+	case V4L2_CID_EXPOSURE:
+		vc->value = ov2640_data.ae_mode;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_ctrl - V4L2 sensor interface handler for VIDIOC_S_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_S_CTRL ioctl structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW (and updates the video_control[] array).  Otherwise,
+ * returns -EINVAL if the control is not supported.
+ */
+static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int retval = 0;
+
+	pr_debug("In ov2640:ioctl_s_ctrl %d\n", vc->id);
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		pr_debug("   V4L2_CID_BRIGHTNESS\n");
+		break;
+	case V4L2_CID_CONTRAST:
+		pr_debug("   V4L2_CID_CONTRAST\n");
+		break;
+	case V4L2_CID_SATURATION:
+		pr_debug("   V4L2_CID_SATURATION\n");
+		break;
+	case V4L2_CID_HUE:
+		pr_debug("   V4L2_CID_HUE\n");
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		pr_debug(
+			"   V4L2_CID_DO_WHITE_BALANCE\n");
+		break;
+	case V4L2_CID_RED_BALANCE:
+		pr_debug("   V4L2_CID_RED_BALANCE\n");
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
+		break;
+	case V4L2_CID_GAMMA:
+		pr_debug("   V4L2_CID_GAMMA\n");
+		break;
+	case V4L2_CID_EXPOSURE:
+		pr_debug("   V4L2_CID_EXPOSURE\n");
+		break;
+	case V4L2_CID_AUTOGAIN:
+		pr_debug("   V4L2_CID_AUTOGAIN\n");
+		break;
+	case V4L2_CID_GAIN:
+		pr_debug("   V4L2_CID_GAIN\n");
+		break;
+	case V4L2_CID_HFLIP:
+		pr_debug("   V4L2_CID_HFLIP\n");
+		break;
+	case V4L2_CID_VFLIP:
+		pr_debug("   V4L2_CID_VFLIP\n");
+		break;
+	default:
+		pr_debug("   Default case\n");
+		retval = -EPERM;
+		break;
+	}
+
+	return retval;
+}
+
+/*!
+ * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
+ * @s: pointer to standard V4L2 device structure
+ */
+static int ioctl_init(struct v4l2_int_device *s)
+{
+	pr_debug("In ov2640:ioctl_init\n");
+
+	return 0;
+}
+
+/*!
+ * ioctl_dev_init - V4L2 sensor interface handler for vidioc_int_dev_init_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Initialise the device when slave attaches to the master.
+ */
+static int ioctl_dev_init(struct v4l2_int_device *s)
+{
+	struct sensor *sensor = s->priv;
+	u32 tgt_xclk;	/* target xclk */
+
+	pr_debug("In ov2640:ioctl_dev_init\n");
+
+	gpio_sensor_active();
+	ov2640_data.on = true;
+
+	tgt_xclk = ov2640_data.mclk;
+	tgt_xclk = min(tgt_xclk, (u32)OV2640_XCLK_MAX);
+	tgt_xclk = max(tgt_xclk, (u32)OV2640_XCLK_MIN);
+	ov2640_data.mclk = tgt_xclk;
+
+	pr_debug("   Setting mclk to %d MHz\n",
+		tgt_xclk / 1000000);
+	set_mclk_rate(&ov2640_data.mclk);
+
+	return ov2640_init_mode(sensor);
+}
+
+/*!
+ * ioctl_dev_exit - V4L2 sensor interface handler for vidioc_int_dev_exit_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Delinitialise the device when slave detaches to the master.
+ */
+static int ioctl_dev_exit(struct v4l2_int_device *s)
+{
+	pr_debug("In ov2640:ioctl_dev_exit\n");
+
+	gpio_sensor_inactive();
+
+	return 0;
+}
+
+/*!
+ * This structure defines all the ioctls for this module and links them to the
+ * enumeration.
+ */
+static struct v4l2_int_ioctl_desc ov2640_ioctl_desc[] = {
+	{vidioc_int_dev_init_num, (v4l2_int_ioctl_func *)ioctl_dev_init},
+	{vidioc_int_dev_exit_num, (v4l2_int_ioctl_func*)ioctl_dev_exit},
+	{vidioc_int_s_power_num, (v4l2_int_ioctl_func *)ioctl_s_power},
+	{vidioc_int_g_ifparm_num, (v4l2_int_ioctl_func *)ioctl_g_ifparm},
+/*	{vidioc_int_g_needs_reset_num,
+				(v4l2_int_ioctl_func *)ioctl_g_needs_reset}, */
+/*	{vidioc_int_reset_num, (v4l2_int_ioctl_func *)ioctl_reset}, */
+	{vidioc_int_init_num, (v4l2_int_ioctl_func *)ioctl_init},
+/*	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_enum_fmt_cap}, */
+/*	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_try_fmt_cap}, */
+	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_g_fmt_cap},
+	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func*)ioctl_s_fmt_cap},
+	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *)ioctl_g_parm},
+	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *)ioctl_s_parm},
+/*	{vidioc_int_queryctrl_num, (v4l2_int_ioctl_func *)ioctl_queryctrl}, */
+	{vidioc_int_g_ctrl_num, (v4l2_int_ioctl_func *)ioctl_g_ctrl},
+	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *)ioctl_s_ctrl},
+};
+
+static struct v4l2_int_slave ov2640_slave = {
+	.ioctls = ov2640_ioctl_desc,
+	.num_ioctls = ARRAY_SIZE(ov2640_ioctl_desc),
+};
+
+static struct v4l2_int_device ov2640_int_device = {
+	.module = THIS_MODULE,
+	.name = "ov2640",
+	.type = v4l2_int_type_slave,
+	.u = {
+		.slave = &ov2640_slave,
+		},
+};
+
+/*!
+ * ov2640 I2C attach function
+ * Function set in i2c_driver struct.
+ * Called by insmod ov2640_camera.ko.
+ *
+ * @param client            struct i2c_client*
+ * @return  Error code indicating success or failure
+ */
+static int ov2640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int retval;
+	struct mxc_camera_platform_data *plat_data = client->dev.platform_data;
+
+	pr_debug("In ov2640_probe (RH_BT565)\n");
+
+	/* Set initial values for the sensor struct. */
+	memset(&ov2640_data, 0, sizeof(ov2640_data));
+	ov2640_data.i2c_client = client;
+	ov2640_data.mclk = 24000000;
+	ov2640_data.mclk = plat_data->mclk;
+	ov2640_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	ov2640_data.pix.width = 800;
+	ov2640_data.pix.height = 600;
+	ov2640_data.streamcap.capability = V4L2_MODE_HIGHQUALITY
+					   | V4L2_CAP_TIMEPERFRAME;
+	ov2640_data.streamcap.capturemode = 0;
+	ov2640_data.streamcap.timeperframe.denominator = DEFAULT_FPS;
+	ov2640_data.streamcap.timeperframe.numerator = 1;
+
+	if (plat_data->io_regulator) {
+		io_regulator =
+		    regulator_get(&client->dev, plat_data->io_regulator);
+		if (!IS_ERR(io_regulator)) {
+			regulator_set_voltage(io_regulator, 2800000, 2800000);
+			if (regulator_enable(io_regulator) != 0) {
+				pr_err("%s:io set voltage error\n", __func__);
+				goto err1;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:io set voltage ok\n", __func__);
+			}
+		} else
+			io_regulator = NULL;
+	}
+
+	if (plat_data->core_regulator) {
+		core_regulator =
+		    regulator_get(&client->dev, plat_data->core_regulator);
+		if (!IS_ERR(core_regulator)) {
+			regulator_set_voltage(core_regulator,
+					 1300000, 1300000);
+			if (regulator_enable(core_regulator) != 0) {
+				pr_err("%s:core set voltage error\n", __func__);
+				goto err2;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:core set voltage ok\n", __func__);
+			}
+		} else
+			core_regulator = NULL;
+	}
+
+	if (plat_data->analog_regulator) {
+		analog_regulator =
+		    regulator_get(&client->dev, plat_data->analog_regulator);
+		if (!IS_ERR(analog_regulator)) {
+			regulator_set_voltage(analog_regulator, 2000000, 2000000);
+			if (regulator_enable(analog_regulator) != 0) {
+				pr_err("%s:analog set voltage error\n",
+					 __func__);
+				goto err3;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:analog set voltage ok\n", __func__);
+			}
+		} else
+			analog_regulator = NULL;
+	}
+
+	if (plat_data->gpo_regulator) {
+		gpo_regulator =
+		    regulator_get(&client->dev, plat_data->gpo_regulator);
+		if (!IS_ERR(gpo_regulator)) {
+			if (regulator_enable(gpo_regulator) != 0) {
+				pr_err("%s:gpo3 set voltage error\n", __func__);
+				goto err4;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:gpo3 set voltage ok\n", __func__);
+			}
+		} else
+			gpo_regulator = NULL;
+	}
+
+	/* This function attaches this structure to the /dev/video0 device.
+	 * The pointer in priv points to the ov2640_data structure here.*/
+	ov2640_int_device.priv = &ov2640_data;
+	retval = v4l2_int_device_register(&ov2640_int_device);
+
+	return retval;
+
+err4:
+	if (analog_regulator) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+err3:
+	if (core_regulator) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+err2:
+	if (io_regulator) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+err1:
+	return -1;
+}
+
+/*!
+ * ov2640 I2C detach function
+ * Called on rmmod ov2640_camera.ko
+ *
+ * @param client            struct i2c_client*
+ * @return  Error code indicating success or failure
+ */
+static int ov2640_remove(struct i2c_client *client)
+{
+	pr_debug("In ov2640_remove\n");
+
+	v4l2_int_device_unregister(&ov2640_int_device);
+
+	if (gpo_regulator) {
+		regulator_disable(gpo_regulator);
+		regulator_put(gpo_regulator);
+	}
+
+	if (analog_regulator) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+
+	if (core_regulator) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+
+	if (io_regulator) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id ov2640_id[] = {
+	{"ov2640", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov2640_id);
+
+static struct i2c_driver ov2640_i2c_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "ov2640",
+		  },
+	.probe = ov2640_probe,
+	.remove = ov2640_remove,
+	.id_table = ov2640_id,
+/* To add power management add .suspend and .resume functions */
+};
+
+/*!
+ * ov2640 init function
+ * Called by insmod ov2640_camera.ko.
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int ov2640_init(void)
+{
+	u8 err;
+
+	pr_debug("In ov2640_init\n");
+
+	err = i2c_add_driver(&ov2640_i2c_driver);
+	if (err != 0)
+		pr_err("%s:driver registration failed, error=%d \n",
+			__func__, err);
+
+	return err;
+}
+
+/*!
+ * OV2640 cleanup function
+ * Called on rmmod ov2640_camera.ko
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit ov2640_clean(void)
+{
+	pr_debug("In ov2640_clean\n");
+	i2c_del_driver(&ov2640_i2c_driver);
+}
+
+module_init(ov2640_init);
+module_exit(ov2640_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("OV2640 Camera Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/capture/ov3640.c b/drivers/media/video/mxc/capture/ov3640.c
new file mode 100644
index 000000000000..47f6e8f3e4a5
--- /dev/null
+++ b/drivers/media/video/mxc/capture/ov3640.c
@@ -0,0 +1,1130 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-int-device.h>
+#include "mxc_v4l2_capture.h"
+
+#define CAMERA_DBG
+
+#ifdef CAMERA_DBG
+	#define CAMERA_TRACE(x) (printk)x
+#else
+	#define CAMERA_TRACE(x)
+#endif
+
+#define OV3640_VOLTAGE_ANALOG               2800000
+#define OV3640_VOLTAGE_DIGITAL_CORE         1500000
+#define OV3640_VOLTAGE_DIGITAL_IO           1800000
+
+
+/* Check these values! */
+#define MIN_FPS 15
+#define MAX_FPS 30
+#define DEFAULT_FPS 30
+
+#define OV3640_XCLK_MIN 6000000
+#define OV3640_XCLK_MAX 24000000
+
+enum ov3640_mode {
+	ov3640_mode_MIN = 0,
+	ov3640_mode_VGA_640_480 = 0,
+	ov3640_mode_QVGA_320_240 = 1,
+	ov3640_mode_QXGA_2048_1536 = 2,
+	ov3640_mode_XGA_1024_768 = 3,
+	ov3640_mode_MAX = 3
+};
+
+enum ov3640_frame_rate {
+	ov3640_15_fps,
+	ov3640_30_fps
+};
+
+struct reg_value {
+	u16 u16RegAddr;
+	u8 u8Val;
+	u8 u8Mask;
+	u32 u32Delay_ms;
+};
+
+struct ov3640_mode_info {
+	enum ov3640_mode mode;
+	u32 width;
+	u32 height;
+	struct reg_value *init_data_ptr;
+	u32 init_data_size;
+};
+
+/*!
+ * Maintains the information on the current state of the sesor.
+ */
+struct sensor {
+	const struct ov3640_platform_data *platform_data;
+	struct v4l2_int_device *v4l2_int_device;
+	struct i2c_client *i2c_client;
+	struct v4l2_pix_format pix;
+	struct v4l2_captureparm streamcap;
+	bool on;
+
+	/* control settings */
+	int brightness;
+	int hue;
+	int contrast;
+	int saturation;
+	int red;
+	int green;
+	int blue;
+	int ae_mode;
+
+	u32 mclk;
+	int csi;
+} ov3640_data;
+
+static struct reg_value ov3640_setting_15fps_QXGA_2048_1536[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0}, {0x304c, 0x81, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x04, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0},
+	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
+	{0x3400, 0x00, 0, 0}, {0x3404, 0x02, 0, 0}, {0x3600, 0xc4, 0, 0},
+	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
+	{0x3362, 0x68, 0, 0}, {0x3363, 0x00, 0, 0}, {0x3364, 0x00, 0, 0},
+	{0x3403, 0x00, 0, 0}, {0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0},
+	{0x308a, 0x06, 0, 0}, {0x308b, 0x00, 0, 0}, {0x307c, 0x10, 0, 0},
+	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3012, 0x00, 0, 0},
+	{0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0}, {0x3022, 0x00, 0, 0},
+	{0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0}, {0x3025, 0x18, 0, 0},
+	{0x3026, 0x06, 0, 0}, {0x3027, 0x0c, 0, 0}, {0x302a, 0x06, 0, 0},
+	{0x302b, 0x20, 0, 0}, {0x3075, 0x44, 0, 0}, {0x300d, 0x00, 0, 0},
+	{0x30d7, 0x00, 0, 0}, {0x3069, 0x40, 0, 0}, {0x303e, 0x01, 0, 0},
+	{0x303f, 0x80, 0, 0}, {0x3302, 0x20, 0, 0}, {0x335f, 0x68, 0, 0},
+	{0x3360, 0x18, 0, 0}, {0x3361, 0x0c, 0, 0}, {0x3362, 0x68, 0, 0},
+	{0x3363, 0x08, 0, 0}, {0x3364, 0x04, 0, 0}, {0x3403, 0x42, 0, 0},
+	{0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0}, {0x308a, 0x06, 0, 0},
+	{0x308b, 0x00, 0, 0},
+};
+
+static struct reg_value ov3640_setting_15fps_XGA_1024_768[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0}, {0x304c, 0x81, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x04, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0},
+	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
+	{0x3400, 0x01, 0, 0}, {0x3404, 0x1d, 0, 0}, {0x3600, 0xc4, 0, 0},
+	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
+	{0x3362, 0x34, 0, 0}, {0x3363, 0x00, 0, 0}, {0x3364, 0x00, 0, 0},
+	{0x3403, 0x00, 0, 0}, {0x3088, 0x04, 0, 0}, {0x3089, 0x00, 0, 0},
+	{0x308a, 0x03, 0, 0}, {0x308b, 0x00, 0, 0}, {0x307c, 0x10, 0, 0},
+	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3011, 0x01, 0, 0},
+};
+
+static struct reg_value ov3640_setting_30fps_XGA_1024_768[] = {
+	{0x0, 0x0, 0}
+};
+
+static struct reg_value ov3640_setting_15fps_VGA_640_480[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0}, {0x304c, 0x81, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x04, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0},
+	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
+	{0x3400, 0x00, 0, 0}, {0x3404, 0x42, 0, 0}, {0x3600, 0xc4, 0, 0},
+	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
+	{0x3362, 0x12, 0, 0}, {0x3363, 0x80, 0, 0}, {0x3364, 0xe0, 0, 0},
+	{0x3403, 0x00, 0, 0}, {0x3088, 0x02, 0, 0}, {0x3089, 0x80, 0, 0},
+	{0x308a, 0x01, 0, 0}, {0x308b, 0xe0, 0, 0}, {0x307c, 0x10, 0, 0},
+	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3011, 0x00, 0, 0},
+};
+
+static struct reg_value ov3640_setting_30fps_VGA_640_480[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x01, 0, 0}, {0x304c, 0x82, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x0c, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x3300, 0x13, 0, 0}, {0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0},
+	{0x30ba, 0x04, 0, 0}, {0x30bb, 0x08, 0, 0}, {0x3100, 0x02, 0, 0},
+	{0x3301, 0x10, 0x30, 0}, {0x3304, 0x00, 0x03, 0}, {0x3400, 0x00, 0, 0},
+	{0x3404, 0x02, 0, 0}, {0x3600, 0xc0, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3012, 0x10, 0, 0},
+	{0x3023, 0x06, 0, 0}, {0x3026, 0x03, 0, 0}, {0x3027, 0x04, 0, 0},
+	{0x302a, 0x03, 0, 0}, {0x302b, 0x10, 0, 0}, {0x3075, 0x24, 0, 0},
+	{0x300d, 0x01, 0, 0}, {0x30d7, 0x80, 0x80, 0}, {0x3069, 0x00, 0x40, 0},
+	{0x303e, 0x00, 0, 0}, {0x303f, 0xc0, 0, 0}, {0x3302, 0x20, 0x20, 0},
+	{0x335f, 0x34, 0, 0}, {0x3360, 0x0c, 0, 0}, {0x3361, 0x04, 0, 0},
+	{0x3362, 0x12, 0, 0}, {0x3363, 0x88, 0, 0}, {0x3364, 0xe4, 0, 0},
+	{0x3403, 0x42, 0, 0}, {0x3088, 0x02, 0, 0}, {0x3089, 0x80, 0, 0},
+	{0x308a, 0x01, 0, 0}, {0x308b, 0xe0, 0, 0}, {0x3362, 0x12, 0, 0},
+	{0x3363, 0x88, 0, 0}, {0x3364, 0xe4, 0, 0}, {0x3403, 0x42, 0, 0},
+	{0x3088, 0x02, 0, 0}, {0x3089, 0x80, 0, 0}, {0x308a, 0x01, 0, 0},
+	{0x308b, 0xe0, 0, 0}, {0x300e, 0x37, 0, 0}, {0x300f, 0xe1, 0, 0},
+	{0x3010, 0x22, 0, 0}, {0x3011, 0x01, 0, 0}, {0x304c, 0x84, 0, 0},
+	{0x3014, 0x04, 0, 0}, {0x3015, 0x02, 0, 0}, {0x302e, 0x00, 0, 0},
+	{0x302d, 0x00, 0, 0},
+};
+
+static struct reg_value ov3640_setting_15fps_QVGA_320_240[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0}, {0x304c, 0x81, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x04, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0},
+	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
+	{0x3400, 0x00, 0, 0}, {0x3404, 0x42, 0, 0}, {0x3600, 0xc4, 0, 0},
+	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
+	{0x3362, 0x01, 0, 0}, {0x3363, 0x40, 0, 0}, {0x3364, 0xf0, 0, 0},
+	{0x3403, 0x00, 0, 0}, {0x3088, 0x01, 0, 0}, {0x3089, 0x40, 0, 0},
+	{0x308a, 0x00, 0, 0}, {0x308b, 0xf0, 0, 0}, {0x307c, 0x10, 0, 0},
+	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3011, 0x01, 0, 0},
+};
+
+static struct reg_value ov3640_setting_30fps_QVGA_320_240[] = {
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
+	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
+	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x10, 0, 0}, {0x300e, 0x32, 0, 0}, {0x300f, 0x21, 0, 0},
+	{0x3010, 0x20, 0, 0}, {0x3011, 0x01, 0, 0}, {0x304c, 0x82, 0, 0},
+	{0x30d7, 0x10, 0, 0}, {0x30d9, 0x0d, 0, 0}, {0x30db, 0x08, 0, 0},
+	{0x3016, 0x82, 0, 0}, {0x3018, 0x38, 0, 0}, {0x3019, 0x30, 0, 0},
+	{0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0}, {0x3087, 0x02, 0, 0},
+	{0x3082, 0x20, 0, 0}, {0x3015, 0x12, 0, 0}, {0x3014, 0x0c, 0, 0},
+	{0x3013, 0xf7, 0, 0}, {0x303c, 0x08, 0, 0}, {0x303d, 0x18, 0, 0},
+	{0x303e, 0x06, 0, 0}, {0x303f, 0x0c, 0, 0}, {0x3030, 0x62, 0, 0},
+	{0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0}, {0x3033, 0x6e, 0, 0},
+	{0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0}, {0x3036, 0xa6, 0, 0},
+	{0x3037, 0x6a, 0, 0}, {0x3104, 0x02, 0, 0}, {0x3105, 0xfd, 0, 0},
+	{0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0}, {0x3300, 0x12, 0, 0},
+	{0x3301, 0xde, 0, 0}, {0x3302, 0xcf, 0, 0}, {0x3312, 0x26, 0, 0},
+	{0x3314, 0x42, 0, 0}, {0x3313, 0x2b, 0, 0}, {0x3315, 0x42, 0, 0},
+	{0x3310, 0xd0, 0, 0}, {0x3311, 0xbd, 0, 0}, {0x330c, 0x18, 0, 0},
+	{0x330d, 0x18, 0, 0}, {0x330e, 0x56, 0, 0}, {0x330f, 0x5c, 0, 0},
+	{0x330b, 0x1c, 0, 0}, {0x3306, 0x5c, 0, 0}, {0x3307, 0x11, 0, 0},
+	{0x336a, 0x52, 0, 0}, {0x3370, 0x46, 0, 0}, {0x3376, 0x38, 0, 0},
+	{0x3300, 0x13, 0, 0}, {0x30b8, 0x20, 0, 0}, {0x30b9, 0x17, 0, 0},
+	{0x30ba, 0x04, 0, 0}, {0x30bb, 0x08, 0, 0}, {0x3100, 0x02, 0, 0},
+	{0x3301, 0x10, 0x30, 0}, {0x3304, 0x00, 0x03, 0}, {0x3400, 0x00, 0, 0},
+	{0x3404, 0x02, 0, 0}, {0x3600, 0xc0, 0, 0}, {0x308d, 0x04, 0, 0},
+	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3012, 0x10, 0, 0},
+	{0x3023, 0x06, 0, 0}, {0x3026, 0x03, 0, 0}, {0x3027, 0x04, 0, 0},
+	{0x302a, 0x03, 0, 0}, {0x302b, 0x10, 0, 0}, {0x3075, 0x24, 0, 0},
+	{0x300d, 0x01, 0, 0}, {0x30d7, 0x80, 0x80, 0}, {0x3069, 0x00, 0x40, 0},
+	{0x303e, 0x00, 0, 0}, {0x303f, 0xc0, 0, 0}, {0x3302, 0x20, 0x20, 0},
+	{0x335f, 0x34, 0, 0}, {0x3360, 0x0c, 0, 0}, {0x3361, 0x04, 0, 0},
+	{0x3362, 0x34, 0, 0}, {0x3363, 0x08, 0, 0}, {0x3364, 0x04, 0, 0},
+	{0x3403, 0x42, 0, 0}, {0x3088, 0x04, 0, 0}, {0x3089, 0x00, 0, 0},
+	{0x308a, 0x03, 0, 0}, {0x308b, 0x00, 0, 0}, {0x3362, 0x12, 0, 0},
+	{0x3363, 0x88, 0, 0}, {0x3364, 0xe4, 0, 0}, {0x3403, 0x42, 0, 0},
+	{0x3088, 0x02, 0, 0}, {0x3089, 0x80, 0, 0}, {0x308a, 0x01, 0, 0},
+	{0x308b, 0xe0, 0, 0}, {0x300e, 0x37, 0, 0}, {0x300f, 0xe1, 0, 0},
+	{0x3010, 0x22, 0, 0}, {0x3011, 0x01, 0, 0}, {0x304c, 0x84, 0, 0},
+};
+
+static struct ov3640_mode_info ov3640_mode_info_data[2][ov3640_mode_MAX + 1] = {
+	{
+		{ov3640_mode_VGA_640_480,    640,  480,
+		ov3640_setting_15fps_VGA_640_480,
+		ARRAY_SIZE(ov3640_setting_15fps_VGA_640_480)},
+		{ov3640_mode_QVGA_320_240,   320,  240,
+		ov3640_setting_15fps_QVGA_320_240,
+		ARRAY_SIZE(ov3640_setting_15fps_QVGA_320_240)},
+		{ov3640_mode_XGA_1024_768,   1024, 768,
+		ov3640_setting_15fps_XGA_1024_768,
+		ARRAY_SIZE(ov3640_setting_15fps_XGA_1024_768)},
+		{ov3640_mode_QXGA_2048_1536, 2048, 1536,
+		ov3640_setting_15fps_QXGA_2048_1536,
+		ARRAY_SIZE(ov3640_setting_15fps_QXGA_2048_1536)},
+	},
+	{
+		{ov3640_mode_VGA_640_480,    640,  480,
+		ov3640_setting_30fps_VGA_640_480,
+		ARRAY_SIZE(ov3640_setting_30fps_VGA_640_480)},
+		{ov3640_mode_QVGA_320_240,   320,  240,
+		ov3640_setting_30fps_QVGA_320_240,
+		ARRAY_SIZE(ov3640_setting_30fps_QVGA_320_240)},
+		{ov3640_mode_XGA_1024_768,   1024, 768,
+		ov3640_setting_30fps_XGA_1024_768,
+		ARRAY_SIZE(ov3640_setting_30fps_XGA_1024_768)},
+		{ov3640_mode_QXGA_2048_1536, 0, 0, NULL, 0},
+	},
+};
+
+static struct regulator *io_regulator;
+static struct regulator *core_regulator;
+static struct regulator *analog_regulator;
+static struct regulator *gpo_regulator;
+
+static int ov3640_probe(struct i2c_client *adapter,
+				const struct i2c_device_id *device_id);
+static int ov3640_remove(struct i2c_client *client);
+
+static s32 ov3640_read_reg(u16 reg, u8 *val);
+static s32 ov3640_write_reg(u16 reg, u8 val);
+
+static const struct i2c_device_id ov3640_id[] = {
+	{"ov3640", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(i2c, ov3640_id);
+
+static struct i2c_driver ov3640_i2c_driver = {
+	.driver = {
+		  .owner = THIS_MODULE,
+		  .name  = "ov3640",
+		  },
+	.probe  = ov3640_probe,
+	.remove = ov3640_remove,
+	.id_table = ov3640_id,
+};
+
+extern void gpio_sensor_active(unsigned int csi_index);
+extern void gpio_sensor_inactive(unsigned int csi);
+
+static s32 ov3640_write_reg(u16 reg, u8 val)
+{
+	u8 au8Buf[3] = {0};
+
+	au8Buf[0] = reg >> 8;
+	au8Buf[1] = reg & 0xff;
+	au8Buf[2] = val;
+
+	if (i2c_master_send(ov3640_data.i2c_client, au8Buf, 3) < 0) {
+		pr_err("%s:write reg error:reg=%x,val=%x\n",
+			__func__, reg, val);
+		return -1;
+	}
+
+	return 0;
+}
+
+static s32 ov3640_read_reg(u16 reg, u8 *val)
+{
+	u8 au8RegBuf[2] = {0};
+	u8 u8RdVal = 0;
+
+	au8RegBuf[0] = reg >> 8;
+	au8RegBuf[1] = reg & 0xff;
+
+	if (2 != i2c_master_send(ov3640_data.i2c_client, au8RegBuf, 2)) {
+		pr_err("%s:write reg error:reg=%x\n",
+				__func__, reg);
+		return -1;
+	}
+
+	if (1 != i2c_master_recv(ov3640_data.i2c_client, &u8RdVal, 1)) {
+		pr_err("%s:read reg error:reg=%x,val=%x\n",
+				__func__, reg, u8RdVal);
+		return -1;
+	}
+
+	*val = u8RdVal;
+
+	return u8RdVal;
+}
+
+static int ov3640_init_mode(enum ov3640_frame_rate frame_rate,
+			    enum ov3640_mode mode)
+{
+	struct reg_value *pModeSetting = NULL;
+	s32 i = 0;
+	s32 iModeSettingArySize = 0;
+	register u32 Delay_ms = 0;
+	register u16 RegAddr = 0;
+	register u8 Mask = 0;
+	register u8 Val = 0;
+	u8 RegVal = 0;
+	int retval = 0;
+
+	CAMERA_TRACE(("CAMERA_DBG Entry: ov3640_init_mode\n"));
+
+	if (mode > ov3640_mode_MAX || mode < ov3640_mode_MIN) {
+		pr_err("Wrong ov3640 mode detected!\n");
+		return -1;
+	}
+
+	pModeSetting = ov3640_mode_info_data[frame_rate][mode].init_data_ptr;
+	iModeSettingArySize =
+		ov3640_mode_info_data[frame_rate][mode].init_data_size;
+
+	ov3640_data.pix.width = ov3640_mode_info_data[frame_rate][mode].width;
+	ov3640_data.pix.height = ov3640_mode_info_data[frame_rate][mode].height;
+
+	for (i = 0; i < iModeSettingArySize; ++i, ++pModeSetting) {
+		Delay_ms = pModeSetting->u32Delay_ms;
+		RegAddr = pModeSetting->u16RegAddr;
+		Val = pModeSetting->u8Val;
+		Mask = pModeSetting->u8Mask;
+
+		if (Mask) {
+			retval = ov3640_read_reg(RegAddr, &RegVal);
+			if (retval < 0)
+				goto err;
+
+			RegVal &= ~(u8)Mask;
+			Val &= Mask;
+			Val |= RegVal;
+		}
+
+		retval = ov3640_write_reg(RegAddr, Val);
+		if (retval < 0)
+			goto err;
+
+		if (Delay_ms)
+			msleep(Delay_ms);
+	}
+err:
+	CAMERA_TRACE(("CAMERA_DBG Exit: ov3640_init_mode\n"));
+
+	return retval;
+}
+
+/* --------------- IOCTL functions from v4l2_int_ioctl_desc --------------- */
+
+static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
+{
+	CAMERA_TRACE(("In ov3640:ioctl_g_ifparm\n"));
+	if (s == NULL) {
+		pr_err("   ERROR!! no slave device set!\n");
+		return -1;
+	}
+
+	memset(p, 0, sizeof(*p));
+	p->u.bt656.clock_curr = ov3640_data.mclk;
+	pr_debug("   clock_curr=mclk=%d\n", ov3640_data.mclk);
+	p->if_type = V4L2_IF_TYPE_BT656;
+	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.clock_min = OV3640_XCLK_MIN;
+	p->u.bt656.clock_max = OV3640_XCLK_MAX;
+	p->u.bt656.bt_sync_correct = 1;  /* Indicate external vsync */
+
+	return 0;
+}
+
+/*!
+ * ioctl_s_power - V4L2 sensor interface handler for VIDIOC_S_POWER ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @on: indicates power mode (on or off)
+ *
+ * Turns the power on or off, depending on the value of on and returns the
+ * appropriate error code.
+ */
+static int ioctl_s_power(struct v4l2_int_device *s, int on)
+{
+	struct sensor *sensor = s->priv;
+
+	CAMERA_TRACE(("In ov3640:ioctl_s_power\n"));
+
+	if (on && !sensor->on) {
+		gpio_sensor_active(ov3640_data.csi);
+		if (io_regulator)
+			if (regulator_enable(io_regulator) != 0)
+				return -EIO;
+		if (core_regulator)
+			if (regulator_enable(core_regulator) != 0)
+				return -EIO;
+		if (gpo_regulator)
+			if (regulator_enable(gpo_regulator) != 0)
+				return -EIO;
+		if (analog_regulator)
+			if (regulator_enable(analog_regulator) != 0)
+				return -EIO;
+	} else if (!on && sensor->on) {
+		if (analog_regulator)
+			regulator_disable(analog_regulator);
+		if (core_regulator)
+			regulator_disable(core_regulator);
+		if (io_regulator)
+			regulator_disable(io_regulator);
+		if (gpo_regulator)
+			regulator_disable(gpo_regulator);
+		gpio_sensor_inactive(ov3640_data.csi);
+	}
+
+	sensor->on = on;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_parm - V4L2 sensor interface handler for VIDIOC_G_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
+ *
+ * Returns the sensor's video CAPTURE parameters.
+ */
+static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_captureparm *cparm = &a->parm.capture;
+	int ret = 0;
+
+	CAMERA_TRACE(("In ov3640:ioctl_g_parm\n"));
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		CAMERA_TRACE(("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n"));
+		memset(a, 0, sizeof(*a));
+		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		cparm->capability = sensor->streamcap.capability;
+		cparm->timeperframe = sensor->streamcap.timeperframe;
+		cparm->capturemode = sensor->streamcap.capturemode;
+		ret = 0;
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		CAMERA_TRACE(("   type is not " \
+			"V4L2_BUF_TYPE_VIDEO_CAPTURE but %d\n",
+			a->type));
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_debug("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_parm - V4L2 sensor interface handler for VIDIOC_S_PARM ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @a: pointer to standard V4L2 VIDIOC_S_PARM ioctl structure
+ *
+ * Configures the sensor to use the input parameters, if possible.  If
+ * not possible, reverts to the old parameters and returns the
+ * appropriate error code.
+ */
+static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
+{
+	struct sensor *sensor = s->priv;
+	struct v4l2_fract *timeperframe = &a->parm.capture.timeperframe;
+	u32 tgt_fps;	/* target frames per secound */
+	enum ov3640_frame_rate frame_rate;
+	int ret = 0;
+
+	CAMERA_TRACE(("In ov3640:ioctl_s_parm\n"));
+	switch (a->type) {
+	/* This is the only case currently handled. */
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		CAMERA_TRACE(("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n"));
+
+		/* Check that the new frame rate is allowed. */
+		if ((timeperframe->numerator == 0) ||
+		    (timeperframe->denominator == 0)) {
+			timeperframe->denominator = DEFAULT_FPS;
+			timeperframe->numerator = 1;
+		}
+
+		tgt_fps = timeperframe->denominator /
+			  timeperframe->numerator;
+
+		if (tgt_fps > MAX_FPS) {
+			timeperframe->denominator = MAX_FPS;
+			timeperframe->numerator = 1;
+		} else if (tgt_fps < MIN_FPS) {
+			timeperframe->denominator = MIN_FPS;
+			timeperframe->numerator = 1;
+		}
+
+		/* Actual frame rate we use */
+		tgt_fps = timeperframe->denominator /
+			  timeperframe->numerator;
+
+		if (tgt_fps == 15)
+			frame_rate = ov3640_15_fps;
+		else if (tgt_fps == 30)
+			frame_rate = ov3640_30_fps;
+		else {
+			pr_err(" The camera frame rate is not supported!\n");
+			return -EINVAL;
+		}
+
+		sensor->streamcap.timeperframe = *timeperframe;
+		sensor->streamcap.capturemode =
+				(u32)a->parm.capture.capturemode;
+
+		ret = ov3640_init_mode(frame_rate,
+				       sensor->streamcap.capturemode);
+		break;
+
+	/* These are all the possible cases. */
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		pr_debug("   type is not " \
+			"V4L2_BUF_TYPE_VIDEO_CAPTURE but %d\n",
+			a->type);
+		ret = -EINVAL;
+		break;
+
+	default:
+		pr_debug("   type is unknown - %d\n", a->type);
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_g_fmt_cap - V4L2 sensor interface handler for ioctl_g_fmt_cap
+ * @s: pointer to standard V4L2 device structure
+ * @f: pointer to standard V4L2 v4l2_format structure
+ *
+ * Returns the sensor's current pixel format in the v4l2_format
+ * parameter.
+ */
+static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	struct sensor *sensor = s->priv;
+
+	CAMERA_TRACE(("In ov3640:ioctl_g_fmt_cap.\n"));
+
+	f->fmt.pix = sensor->pix;
+
+	return 0;
+}
+
+/*!
+ * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_G_CTRL ioctl structure
+ *
+ * If the requested control is supported, returns the control's current
+ * value from the video_control[] array.  Otherwise, returns -EINVAL
+ * if the control is not supported.
+ */
+static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int ret = 0;
+
+	CAMERA_TRACE(("In ov3640:ioctl_g_ctrl\n"));
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		vc->value = ov3640_data.brightness;
+		break;
+	case V4L2_CID_HUE:
+		vc->value = ov3640_data.hue;
+		break;
+	case V4L2_CID_CONTRAST:
+		vc->value = ov3640_data.contrast;
+		break;
+	case V4L2_CID_SATURATION:
+		vc->value = ov3640_data.saturation;
+		break;
+	case V4L2_CID_RED_BALANCE:
+		vc->value = ov3640_data.red;
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		vc->value = ov3640_data.blue;
+		break;
+	case V4L2_CID_EXPOSURE:
+		vc->value = ov3640_data.ae_mode;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+/*!
+ * ioctl_s_ctrl - V4L2 sensor interface handler for VIDIOC_S_CTRL ioctl
+ * @s: pointer to standard V4L2 device structure
+ * @vc: standard V4L2 VIDIOC_S_CTRL ioctl structure
+ *
+ * If the requested control is supported, sets the control's current
+ * value in HW (and updates the video_control[] array).  Otherwise,
+ * returns -EINVAL if the control is not supported.
+ */
+static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
+{
+	int retval = 0;
+
+	pr_debug("In ov3640:ioctl_s_ctrl %d\n",
+		 vc->id);
+
+	switch (vc->id) {
+	case V4L2_CID_BRIGHTNESS:
+		CAMERA_TRACE(("   V4L2_CID_BRIGHTNESS\n"));
+		break;
+	case V4L2_CID_CONTRAST:
+		CAMERA_TRACE(("   V4L2_CID_CONTRAST\n"));
+		break;
+	case V4L2_CID_SATURATION:
+		CAMERA_TRACE(("   V4L2_CID_SATURATION\n"));
+		break;
+	case V4L2_CID_HUE:
+		CAMERA_TRACE(("   V4L2_CID_HUE\n"));
+		break;
+	case V4L2_CID_AUTO_WHITE_BALANCE:
+		CAMERA_TRACE(("   V4L2_CID_AUTO_WHITE_BALANCE\n"));
+		break;
+	case V4L2_CID_DO_WHITE_BALANCE:
+		CAMERA_TRACE(("   V4L2_CID_DO_WHITE_BALANCE\n"));
+		break;
+	case V4L2_CID_RED_BALANCE:
+		CAMERA_TRACE(("   V4L2_CID_RED_BALANCE\n"));
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		CAMERA_TRACE(("   V4L2_CID_BLUE_BALANCE\n"));
+		break;
+	case V4L2_CID_GAMMA:
+		CAMERA_TRACE(("   V4L2_CID_GAMMA\n"));
+		break;
+	case V4L2_CID_EXPOSURE:
+		CAMERA_TRACE(("   V4L2_CID_EXPOSURE\n"));
+		break;
+	case V4L2_CID_AUTOGAIN:
+		CAMERA_TRACE(("   V4L2_CID_AUTOGAIN\n"));
+		break;
+	case V4L2_CID_GAIN:
+		CAMERA_TRACE(("   V4L2_CID_GAIN\n"));
+		break;
+	case V4L2_CID_HFLIP:
+		CAMERA_TRACE(("   V4L2_CID_HFLIP\n"));
+		break;
+	case V4L2_CID_VFLIP:
+		CAMERA_TRACE(("   V4L2_CID_VFLIP\n"));
+		break;
+	default:
+		CAMERA_TRACE(("   Default case\n"));
+		retval = -EPERM;
+		break;
+	}
+
+	return retval;
+}
+
+/*!
+ * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
+ * @s: pointer to standard V4L2 device structure
+ */
+static int ioctl_init(struct v4l2_int_device *s)
+{
+	CAMERA_TRACE(("In ov3640:ioctl_init\n"));
+
+	return 0;
+}
+
+/*!
+ * ioctl_dev_init - V4L2 sensor interface handler for vidioc_int_dev_init_num
+ * @s: pointer to standard V4L2 device structure
+ *
+ * Initialise the device when slave attaches to the master.
+ */
+static int ioctl_dev_init(struct v4l2_int_device *s)
+{
+	struct sensor *sensor = s->priv;
+	u32 tgt_xclk;	/* target xclk */
+	u32 tgt_fps;	/* target frames per secound */
+	enum ov3640_frame_rate frame_rate;
+
+	CAMERA_TRACE(("In ov3640:ioctl_dev_init\n"));
+
+	gpio_sensor_active(ov3640_data.csi);
+	ov3640_data.on = true;
+
+	/* mclk */
+	tgt_xclk = ov3640_data.mclk;
+	tgt_xclk = min(tgt_xclk, (u32)OV3640_XCLK_MAX);
+	tgt_xclk = max(tgt_xclk, (u32)OV3640_XCLK_MIN);
+	ov3640_data.mclk = tgt_xclk;
+
+	pr_debug("   Setting mclk to %d MHz\n", tgt_xclk / 1000000);
+	set_mclk_rate(&ov3640_data.mclk, ov3640_data.csi);
+
+	/* Default camera frame rate is set in probe */
+	tgt_fps = sensor->streamcap.timeperframe.denominator /
+		  sensor->streamcap.timeperframe.numerator;
+
+	if (tgt_fps == 15)
+		frame_rate = ov3640_15_fps;
+	else if (tgt_fps == 30)
+		frame_rate = ov3640_30_fps;
+	else
+		return -EINVAL; /* Only support 15fps or 30fps now. */
+
+	return ov3640_init_mode(frame_rate,
+				sensor->streamcap.capturemode);
+}
+
+/*!
+ * This structure defines all the ioctls for this module and links them to the
+ * enumeration.
+ */
+static struct v4l2_int_ioctl_desc ov3640_ioctl_desc[] = {
+	{vidioc_int_dev_init_num, (v4l2_int_ioctl_func *)ioctl_dev_init},
+/*	{vidioc_int_dev_exit_num, (v4l2_int_ioctl_func *)ioctl_dev_exit}, */
+	{vidioc_int_s_power_num, (v4l2_int_ioctl_func *)ioctl_s_power},
+	{vidioc_int_g_ifparm_num, (v4l2_int_ioctl_func *)ioctl_g_ifparm},
+/*	{vidioc_int_g_needs_reset_num,
+				(v4l2_int_ioctl_func *)ioctl_g_needs_reset}, */
+/*	{vidioc_int_reset_num, (v4l2_int_ioctl_func *)ioctl_reset}, */
+	{vidioc_int_init_num, (v4l2_int_ioctl_func *)ioctl_init},
+/*	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_enum_fmt_cap}, */
+/*	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *)ioctl_try_fmt_cap}, */
+	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_g_fmt_cap},
+/*	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap}, */
+	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *)ioctl_g_parm},
+	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *)ioctl_s_parm},
+/*	{vidioc_int_queryctrl_num, (v4l2_int_ioctl_func *)ioctl_queryctrl}, */
+	{vidioc_int_g_ctrl_num, (v4l2_int_ioctl_func *)ioctl_g_ctrl},
+	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *)ioctl_s_ctrl},
+};
+
+static struct v4l2_int_slave ov3640_slave = {
+	.ioctls = ov3640_ioctl_desc,
+	.num_ioctls = ARRAY_SIZE(ov3640_ioctl_desc),
+};
+
+static struct v4l2_int_device ov3640_int_device = {
+	.module = THIS_MODULE,
+	.name = "ov3640",
+	.type = v4l2_int_type_slave,
+	.u = {
+		.slave = &ov3640_slave,
+	},
+};
+
+/*!
+ * ov3640 I2C probe function
+ *
+ * @param adapter            struct i2c_adapter *
+ * @return  Error code indicating success or failure
+ */
+static int ov3640_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int retval;
+	struct mxc_camera_platform_data *plat_data = client->dev.platform_data;
+
+	CAMERA_TRACE(("CAMERA_DBG Entry: ov3640_probe\n"));
+
+	/* Set initial values for the sensor struct. */
+	memset(&ov3640_data, 0, sizeof(ov3640_data));
+	ov3640_data.mclk = 24000000; /* 6 - 54 MHz, typical 24MHz */
+	ov3640_data.mclk = plat_data->mclk;
+	ov3640_data.csi = plat_data->csi;
+
+	ov3640_data.i2c_client = client;
+	ov3640_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	ov3640_data.pix.width = 640;
+	ov3640_data.pix.height = 480;
+	ov3640_data.streamcap.capability = V4L2_MODE_HIGHQUALITY |
+					   V4L2_CAP_TIMEPERFRAME;
+	ov3640_data.streamcap.capturemode = 0;
+	ov3640_data.streamcap.timeperframe.denominator = DEFAULT_FPS;
+	ov3640_data.streamcap.timeperframe.numerator = 1;
+
+	if (plat_data->io_regulator) {
+		io_regulator = regulator_get(&client->dev,
+					     plat_data->io_regulator);
+		if (!IS_ERR(io_regulator)) {
+			regulator_set_voltage(io_regulator,
+					      OV3640_VOLTAGE_DIGITAL_IO,
+					      OV3640_VOLTAGE_DIGITAL_IO);
+			if (regulator_enable(io_regulator) != 0) {
+				pr_err("%s:io set voltage error\n", __func__);
+				goto err1;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:io set voltage ok\n", __func__);
+			}
+		} else
+			io_regulator = NULL;
+	}
+
+	if (plat_data->core_regulator) {
+		core_regulator = regulator_get(&client->dev,
+					       plat_data->core_regulator);
+		if (!IS_ERR(core_regulator)) {
+			regulator_set_voltage(core_regulator,
+					      OV3640_VOLTAGE_DIGITAL_CORE,
+					      OV3640_VOLTAGE_DIGITAL_CORE);
+			if (regulator_enable(core_regulator) != 0) {
+				pr_err("%s:core set voltage error\n", __func__);
+				goto err2;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:core set voltage ok\n", __func__);
+			}
+		} else
+			core_regulator = NULL;
+	}
+
+	if (plat_data->analog_regulator) {
+		analog_regulator = regulator_get(&client->dev,
+						 plat_data->analog_regulator);
+		if (!IS_ERR(analog_regulator)) {
+			regulator_set_voltage(analog_regulator,
+					      OV3640_VOLTAGE_ANALOG,
+					      OV3640_VOLTAGE_ANALOG);
+			if (regulator_enable(analog_regulator) != 0) {
+				pr_err("%s:analog set voltage error\n",
+					__func__);
+				goto err3;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:analog set voltage ok\n", __func__);
+			}
+		} else
+			analog_regulator = NULL;
+	}
+
+	if (plat_data->gpo_regulator) {
+		gpo_regulator = regulator_get(&client->dev,
+					      plat_data->gpo_regulator);
+		if (!IS_ERR(gpo_regulator)) {
+			if (regulator_enable(gpo_regulator) != 0) {
+				pr_err("%s:gpo3 enable error\n", __func__);
+				goto err4;
+			} else {
+				dev_dbg(&client->dev,
+					"%s:gpo3 enable ok\n", __func__);
+			}
+		} else
+			gpo_regulator = NULL;
+	}
+
+	ov3640_int_device.priv = &ov3640_data;
+	retval = v4l2_int_device_register(&ov3640_int_device);
+
+	return retval;
+
+err4:
+	if (analog_regulator) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+err3:
+	if (core_regulator) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+err2:
+	if (io_regulator) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+err1:
+	return -1;
+}
+
+/*!
+ * ov3640 I2C detach function
+ *
+ * @param client            struct i2c_client *
+ * @return  Error code indicating success or failure
+ */
+static int ov3640_remove(struct i2c_client *client)
+{
+	CAMERA_TRACE(("In ov3640_remove\n"));
+
+	v4l2_int_device_unregister(&ov3640_int_device);
+
+	if (gpo_regulator) {
+		regulator_disable(gpo_regulator);
+		regulator_put(gpo_regulator);
+	}
+
+	if (analog_regulator) {
+		regulator_disable(analog_regulator);
+		regulator_put(analog_regulator);
+	}
+
+	if (core_regulator) {
+		regulator_disable(core_regulator);
+		regulator_put(core_regulator);
+	}
+
+	if (io_regulator) {
+		regulator_disable(io_regulator);
+		regulator_put(io_regulator);
+	}
+
+	return 0;
+}
+
+/*!
+ * ov3640 init function
+ * Called by insmod ov3640_camera.ko.
+ *
+ * @return  Error code indicating success or failure
+ */
+static __init int ov3640_init(void)
+{
+	u8 err;
+
+	CAMERA_TRACE(("CAMERA_DBG Entry: ov3640_init\n"));
+
+	err = i2c_add_driver(&ov3640_i2c_driver);
+	if (err != 0)
+		pr_err("%s:driver registration failed, error=%d \n",
+			__func__, err);
+
+	CAMERA_TRACE(("CAMERA_DBG Exit: ov3640_init\n"));
+
+	return err;
+}
+
+/*!
+ * OV3640 cleanup function
+ * Called on rmmod ov3640_camera.ko
+ *
+ * @return  Error code indicating success or failure
+ */
+static void __exit ov3640_clean(void)
+{
+	CAMERA_TRACE(("CAMERA_DBG Entry: ov3640_clean\n"));
+
+	i2c_del_driver(&ov3640_i2c_driver);
+	gpio_sensor_inactive(ov3640_data.csi);
+
+	CAMERA_TRACE(("CAMERA_DBG Exit: ov3640_clean\n"));
+}
+
+module_init(ov3640_init);
+module_exit(ov3640_clean);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("OV3640 Camera Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
+MODULE_ALIAS("CSI");
diff --git a/drivers/media/video/mxc/capture/sensor_clock.c b/drivers/media/video/mxc/capture/sensor_clock.c
new file mode 100644
index 000000000000..c15cf27c57cd
--- /dev/null
+++ b/drivers/media/video/mxc/capture/sensor_clock.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file sensor_clock.c
+ *
+ * @brief camera clock function
+ *
+ * @ingroup Camera
+ */
+#include <linux/init.h>
+#include <linux/ctype.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/clk.h>
+
+#if defined(CONFIG_MXC_IPU_V1) || defined(CONFIG_VIDEO_MXC_EMMA_CAMERA) \
+			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA_MODULE) \
+			       || defined(CONFIG_VIDEO_MXC_CSI_CAMERA)
+/*
+ * set_mclk_rate
+ *
+ * @param       p_mclk_freq  mclk frequence
+ *
+ */
+void set_mclk_rate(uint32_t * p_mclk_freq)
+{
+	struct clk *clk;
+	uint32_t freq = 0;
+
+	clk = clk_get(NULL, "csi_clk");
+
+	freq = clk_round_rate(clk, *p_mclk_freq);
+	clk_set_rate(clk, freq);
+
+	*p_mclk_freq = freq;
+
+	clk_put(clk);
+	pr_debug("mclk frequency = %d\n", *p_mclk_freq);
+}
+#else
+/*
+ * set_mclk_rate
+ *
+ * @param       p_mclk_freq  mclk frequence
+ * @param	csi         csi 0 or csi 1
+ *
+ */
+void set_mclk_rate(uint32_t *p_mclk_freq, uint32_t csi)
+{
+	struct clk *clk;
+	uint32_t freq = 0;
+	char *mclk;
+
+	if (csi == 0) {
+		mclk = "csi_mclk1";
+	} else if (csi == 1) {
+		mclk = "csi_mclk2";
+	} else {
+		pr_debug("invalid csi num %d\n", csi);
+		return;
+	}
+
+	clk = clk_get(NULL, mclk);
+
+	freq = clk_round_rate(clk, *p_mclk_freq);
+	clk_set_rate(clk, freq);
+
+	*p_mclk_freq = freq;
+
+	clk_put(clk);
+	pr_debug("%s frequency = %d\n", mclk, *p_mclk_freq);
+}
+#endif
+
+/* Exported symbols for modules. */
+EXPORT_SYMBOL(set_mclk_rate);
diff --git a/drivers/media/video/mxc/opl/Makefile b/drivers/media/video/mxc/opl/Makefile
new file mode 100644
index 000000000000..092a62c5ac4a
--- /dev/null
+++ b/drivers/media/video/mxc/opl/Makefile
@@ -0,0 +1,5 @@
+opl-objs	:= opl_mod.o rotate90_u16.o rotate270_u16.o	\
+		   rotate90_u16_qcif.o rotate270_u16_qcif.o	\
+		   vmirror_u16.o hmirror_rotate180_u16.o
+
+obj-$(CONFIG_VIDEO_MXC_OPL)	+= opl.o
diff --git a/drivers/media/video/mxc/opl/hmirror_rotate180_u16.c b/drivers/media/video/mxc/opl/hmirror_rotate180_u16.c
new file mode 100644
index 000000000000..3119a128c1f2
--- /dev/null
+++ b/drivers/media/video/mxc/opl/hmirror_rotate180_u16.c
@@ -0,0 +1,259 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include "opl.h"
+
+static inline u32 rot_left_u16(u16 x, unsigned int n)
+{
+	return (x << n) | (x >> (16 - n));
+}
+
+static inline u32 rot_left_u32(u32 x, unsigned int n)
+{
+	return (x << n) | (x >> (32 - n));
+}
+
+static inline u32 byte_swap_u32(u32 x)
+{
+	u32 t1, t2, t3;
+
+	t1 = x ^ ((x << 16) | x >> 16);
+	t2 = t1 & 0xff00ffff;
+	t3 = (x >> 8) | (x << 24);
+	return t3 ^ (t2 >> 8);
+}
+
+static int opl_hmirror_u16_by1(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror);
+static int opl_hmirror_u16_by2(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror);
+static int opl_hmirror_u16_by4(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror);
+static int opl_hmirror_u16_by8(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror);
+
+int opl_hmirror_u16(const u8 * src, int src_line_stride, int width, int height,
+		    u8 * dst, int dst_line_stride)
+{
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	if (width % 8 == 0)
+		return opl_hmirror_u16_by8(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 0);
+	else if (width % 4 == 0)
+		return opl_hmirror_u16_by4(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 0);
+	else if (width % 2 == 0)
+		return opl_hmirror_u16_by2(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 0);
+	else			/* (width % 1) */
+		return opl_hmirror_u16_by1(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 0);
+}
+
+int opl_rotate180_u16(const u8 * src, int src_line_stride, int width,
+		      int height, u8 * dst, int dst_line_stride)
+{
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	if (width % 8 == 0)
+		return opl_hmirror_u16_by8(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 1);
+	else if (width % 4 == 0)
+		return opl_hmirror_u16_by4(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 1);
+	else if (width % 2 == 0)
+		return opl_hmirror_u16_by2(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 1);
+	else			/* (width % 1) */
+		return opl_hmirror_u16_by1(src, src_line_stride, width, height,
+					   dst, dst_line_stride, 1);
+}
+
+static int opl_hmirror_u16_by1(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror)
+{
+	const u8 *src_row_addr;
+	const u8 *psrc;
+	u8 *dst_row_addr, *pdst;
+	int i, j;
+	u16 pixel;
+
+	src_row_addr = src;
+	if (vmirror) {
+		dst_row_addr = dst + dst_line_stride * (height - 1);
+		dst_line_stride = -dst_line_stride;
+	} else
+		dst_row_addr = dst;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* Loop over each pixel */
+		psrc = src_row_addr;
+		pdst = dst_row_addr + (width - 1) * BYTES_PER_PIXEL
+		    - (BYTES_PER_PIXEL - BYTES_PER_PIXEL);
+		for (j = 0; j < width; j++) {
+			pixel = *(u16 *) psrc;
+			*(u16 *) pdst = pixel;
+			psrc += BYTES_PER_PIXEL;
+			pdst -= BYTES_PER_PIXEL;
+		}
+		src_row_addr += src_line_stride;
+		dst_row_addr += dst_line_stride;
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+static int opl_hmirror_u16_by2(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror)
+{
+	const u8 *src_row_addr;
+	const u8 *psrc;
+	u8 *dst_row_addr, *pdst;
+	int i, j;
+	u32 pixelsin, pixelsout;
+
+	src_row_addr = src;
+	if (vmirror) {
+		dst_row_addr = dst + dst_line_stride * (height - 1);
+		dst_line_stride = -dst_line_stride;
+	} else
+		dst_row_addr = dst;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* Loop over each pixel */
+		psrc = src_row_addr;
+		pdst = dst_row_addr + (width - 2) * BYTES_PER_PIXEL;
+		for (j = 0; j < (width >> 1); j++) {
+			pixelsin = *(u32 *) psrc;
+			pixelsout = rot_left_u32(pixelsin, 16);
+			*(u32 *) pdst = pixelsout;
+			psrc += BYTES_PER_2PIXEL;
+			pdst -= BYTES_PER_2PIXEL;
+		}
+		src_row_addr += src_line_stride;
+		dst_row_addr += dst_line_stride;
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+static int opl_hmirror_u16_by4(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror)
+{
+	const u8 *src_row_addr;
+	const u8 *psrc;
+	u8 *dst_row_addr, *pdst;
+	int i, j;
+
+	union doubleword {
+		u64 dw;
+		u32 w[2];
+	};
+
+	union doubleword inbuf;
+	union doubleword outbuf;
+
+	src_row_addr = src;
+	if (vmirror) {
+		dst_row_addr = dst + dst_line_stride * (height - 1);
+		dst_line_stride = -dst_line_stride;
+	} else
+		dst_row_addr = dst;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* Loop over each pixel */
+		psrc = src_row_addr;
+		pdst = dst_row_addr + (width - 4) * BYTES_PER_PIXEL;
+		for (j = 0; j < (width >> 2); j++) {
+			inbuf.dw = *(u64 *) psrc;
+			outbuf.w[0] = rot_left_u32(inbuf.w[1], 16);
+			outbuf.w[1] = rot_left_u32(inbuf.w[0], 16);
+			*(u64 *) pdst = outbuf.dw;
+			psrc += BYTES_PER_4PIXEL;
+			pdst -= BYTES_PER_4PIXEL;
+		}
+		src_row_addr += src_line_stride;
+		dst_row_addr += dst_line_stride;
+	}
+	return OPLERR_SUCCESS;
+}
+
+static int opl_hmirror_u16_by8(const u8 * src, int src_line_stride, int width,
+			       int height, u8 * dst, int dst_line_stride,
+			       int vmirror)
+{
+	const u8 *src_row_addr;
+	const u8 *psrc;
+	u8 *dst_row_addr, *pdst;
+	int i, j;
+
+	src_row_addr = src;
+	if (vmirror) {
+		dst_row_addr = dst + dst_line_stride * (height - 1);
+		dst_line_stride = -dst_line_stride;
+	} else
+		dst_row_addr = dst;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* Loop over each pixel */
+		psrc = src_row_addr;
+		pdst = dst_row_addr + (width - 1) * BYTES_PER_PIXEL - 2;
+		for (j = (width >> 3); j > 0; j--) {
+			__asm__ volatile (
+				"ldmia	%0!,{r2-r5}\n\t"
+				"mov	r6, r2\n\t"
+				"mov	r7, r3\n\t"
+				"mov	r2, r5, ROR #16\n\t"
+				"mov	r3, r4, ROR #16\n\t"
+				"mov	r4, r7, ROR #16\n\t"
+				"mov	r5, r6, ROR #16\n\t"
+				"stmda	%1!,{r2-r5}\n\t"
+
+				:"+r"(psrc), "+r"(pdst)
+				:"0"(psrc), "1"(pdst)
+				:"r2", "r3", "r4", "r5", "r6", "r7",
+				"memory"
+			);
+		}
+		src_row_addr += src_line_stride;
+		dst_row_addr += dst_line_stride;
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+EXPORT_SYMBOL(opl_hmirror_u16);
+EXPORT_SYMBOL(opl_rotate180_u16);
diff --git a/drivers/media/video/mxc/opl/opl.h b/drivers/media/video/mxc/opl/opl.h
new file mode 100644
index 000000000000..24644c8e78fa
--- /dev/null
+++ b/drivers/media/video/mxc/opl/opl.h
@@ -0,0 +1,162 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup OPLIP OPL Image Processing
+ */
+/*!
+ * @file opl.h
+ *
+ * @brief The OPL (Open Primitives Library) Image Processing library defines
+ * efficient functions for rotation and mirroring.
+ *
+ * It includes ARM9-optimized rotation and mirroring functions. It is derived
+ * from the original OPL project which is found at sourceforge.freescale.net.
+ *
+ * @ingroup OPLIP
+ */
+#ifndef __OPL_H__
+#define __OPL_H__
+
+#include <linux/types.h>
+
+#define BYTES_PER_PIXEL			2
+#define CACHE_LINE_WORDS		8
+#define BYTES_PER_WORD			4
+
+#define BYTES_PER_2PIXEL		(BYTES_PER_PIXEL * 2)
+#define BYTES_PER_4PIXEL		(BYTES_PER_PIXEL * 4)
+#define BYTES_PER_8PIXEL		(BYTES_PER_PIXEL * 8)
+
+#define QCIF_Y_WIDTH			176
+#define QCIF_Y_HEIGHT			144
+
+/*! Enumerations of opl error code */
+enum opl_error {
+	OPLERR_SUCCESS = 0,
+	OPLERR_NULL_PTR,
+	OPLERR_BAD_ARG,
+	OPLERR_DIV_BY_ZERO,
+	OPLERR_OVER_FLOW,
+	OPLERR_UNDER_FLOW,
+	OPLERR_MISALIGNED,
+};
+
+/*!
+ * @brief Rotate a 16bbp buffer 90 degrees clockwise.
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate90_u16(const u8 * src, int src_line_stride, int width, int height,
+		     u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Rotate a 16bbp buffer 180 degrees clockwise.
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate180_u16(const u8 * src, int src_line_stride, int width,
+		      int height, u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Rotate a 16bbp buffer 270 degrees clockwise
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate270_u16(const u8 * src, int src_line_stride, int width,
+		      int height, u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Mirror a 16bpp buffer horizontally
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_hmirror_u16(const u8 * src, int src_line_stride, int width, int height,
+		    u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Mirror a 16bpp buffer vertically
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_vmirror_u16(const u8 * src, int src_line_stride, int width, int height,
+		    u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Rotate a 16bbp buffer 90 degrees clockwise and mirror vertically
+ *	  It is equivalent to rotate 270 degree and mirror horizontally
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate90_vmirror_u16(const u8 * src, int src_line_stride, int width,
+			     int height, u8 * dst, int dst_line_stride);
+
+/*!
+ * @brief Rotate a 16bbp buffer 270 degrees clockwise and mirror vertically
+ *	  It is equivalent to rotate 90 degree and mirror horizontally
+ *
+ * @param src             Pointer to the input buffer
+ * @param src_line_stride Length in bytes of a raster line of the input buffer
+ * @param width           Width in pixels of the region in the input buffer
+ * @param height          Height in pixels of the region in the input buffer
+ * @param dst             Pointer to the output buffer
+ * @param dst_line_stride Length in bytes of a raster line of the output buffer
+ *
+ * @return Standard OPL error code. See enumeration for possible result codes.
+ */
+int opl_rotate270_vmirror_u16(const u8 * src, int src_line_stride, int width,
+			      int height, u8 * dst, int dst_line_stride);
+
+#endif				/* __OPL_H__ */
diff --git a/drivers/media/video/mxc/opl/opl_mod.c b/drivers/media/video/mxc/opl/opl_mod.c
new file mode 100644
index 000000000000..a581aadda252
--- /dev/null
+++ b/drivers/media/video/mxc/opl/opl_mod.c
@@ -0,0 +1,30 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+
+static __init int opl_init(void)
+{
+	return 0;
+}
+
+static void __exit opl_exit(void)
+{
+}
+
+module_init(opl_init);
+module_exit(opl_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("OPL Software Rotation/Mirroring");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/mxc/opl/rotate270_u16.c b/drivers/media/video/mxc/opl/rotate270_u16.c
new file mode 100644
index 000000000000..add87f1a9e44
--- /dev/null
+++ b/drivers/media/video/mxc/opl/rotate270_u16.c
@@ -0,0 +1,285 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include "opl.h"
+
+static int opl_rotate270_u16_by16(const u8 * src, int src_line_stride,
+				  int width, int height, u8 * dst,
+				  int dst_line_stride, int vmirror);
+static int opl_rotate270_u16_by4(const u8 * src, int src_line_stride, int width,
+				 int height, u8 * dst, int dst_line_stride,
+				 int vmirror);
+static int opl_rotate270_vmirror_u16_both(const u8 * src, int src_line_stride,
+					  int width, int height, u8 * dst,
+					  int dst_line_stride, int vmirror);
+int opl_rotate270_u16_qcif(const u8 * src, u8 * dst);
+
+int opl_rotate270_u16(const u8 * src, int src_line_stride, int width,
+		      int height, u8 * dst, int dst_line_stride)
+{
+	return opl_rotate270_vmirror_u16_both(src, src_line_stride, width,
+					      height, dst, dst_line_stride, 0);
+}
+
+int opl_rotate270_vmirror_u16(const u8 * src, int src_line_stride, int width,
+			      int height, u8 * dst, int dst_line_stride)
+{
+	return opl_rotate270_vmirror_u16_both(src, src_line_stride, width,
+					      height, dst, dst_line_stride, 1);
+}
+
+static int opl_rotate270_vmirror_u16_both(const u8 * src, int src_line_stride,
+					  int width, int height, u8 * dst,
+					  int dst_line_stride, int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL;
+	const int BLOCK_SIZE_PIXELS_BY4 = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL / 4;
+
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	/* The QCIF algorithm doesn't support vertical mirroring */
+	if (vmirror == 0 && width == QCIF_Y_WIDTH && height == QCIF_Y_HEIGHT
+	    && src_line_stride == QCIF_Y_WIDTH * 2
+	    && src_line_stride == QCIF_Y_HEIGHT * 2)
+		return opl_rotate270_u16_qcif(src, dst);
+	else if (width % BLOCK_SIZE_PIXELS == 0
+		 && height % BLOCK_SIZE_PIXELS == 0)
+		return opl_rotate270_u16_by16(src, src_line_stride, width,
+					      height, dst, dst_line_stride,
+					      vmirror);
+	else if (width % BLOCK_SIZE_PIXELS_BY4 == 0
+		 && height % BLOCK_SIZE_PIXELS_BY4 == 0)
+		return opl_rotate270_u16_by4(src, src_line_stride, width,
+					     height, dst, dst_line_stride,
+					     vmirror);
+	else
+		return OPLERR_BAD_ARG;
+}
+
+/*
+ * Rotate Counter Clockwise, divide RGB component into 16 row strips, read
+ * non sequentially and write sequentially. This is done in 16 line strips
+ * so that the cache is used better. Cachelines are 8 words = 32 bytes. Pixels
+ * are 2 bytes. The 16 reads will be cache misses, but the next 240 should
+ * be from cache. The writes to the output buffer will be sequential for 16
+ * writes.
+ *
+ * Example:
+ * Input data matrix:	    output matrix
+ *
+ * 0 | 1 | 2 | 3 | 4 |      4 | 0 | 0 | 3 |
+ * 4 | 3 | 2 | 1 | 0 |      3 | 1 | 9 | 6 |
+ * 6 | 7 | 8 | 9 | 0 |      2 | 2 | 8 | 2 |
+ * 5 | 3 | 2 | 6 | 3 |      1 | 3 | 7 | 3 |
+ * ^			    0 | 4 | 6 | 5 | < Write the input data sequentially
+ * Read first column
+ * Start at the bottom
+ * Move to next column and repeat
+ *
+ * Loop over k decreasing (blocks)
+ * in_block_ptr = src + (((RGB_HEIGHT_PIXELS / BLOCK_SIZE_PIXELS) - k)
+ *                * BLOCK_SIZE_PIXELS) * (RGB_WIDTH_BYTES)
+ * out_block_ptr = dst + (((RGB_HEIGHT_PIXELS / BLOCK_SIZE_PIXELS) - k)
+ *                * BLOCK_SIZE_BYTES) + (RGB_WIDTH_PIXELS - 1)
+ *                * RGB_HEIGHT_PIXELS * BYTES_PER_PIXEL
+ *
+ * Loop over i decreasing (width)
+ * Each pix:
+ * in_block_ptr += RGB_WIDTH_BYTES
+ * out_block_ptr += 4
+ *
+ * Each row of block:
+ * in_block_ptr -= RGB_WIDTH_BYTES * BLOCK_SIZE_PIXELS - 2
+ * out_block_ptr -= RGB_HEIGHT_PIXELS * BYTES_PER_PIXEL + 2 * BLOCK_SIZE_PIXELS;
+ *
+ * It may perform vertical mirroring too depending on the vmirror flag.
+ */
+static int opl_rotate270_u16_by16(const u8 * src, int src_line_stride,
+				  int width, int height, u8 * dst,
+				  int dst_line_stride, int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL;
+	const int IN_INDEX = src_line_stride * BLOCK_SIZE_PIXELS
+	    - BYTES_PER_PIXEL;
+	const int OUT_INDEX = vmirror ?
+	    -dst_line_stride + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS
+	    : dst_line_stride + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS;
+	const u8 *in_block_ptr;
+	u8 *out_block_ptr;
+	int i, k;
+
+	for (k = height / BLOCK_SIZE_PIXELS; k > 0; k--) {
+		in_block_ptr = src + (((height / BLOCK_SIZE_PIXELS) - k)
+				      * BLOCK_SIZE_PIXELS) * src_line_stride;
+		out_block_ptr = dst + (((height / BLOCK_SIZE_PIXELS) - k)
+				       * BLOCK_SIZE_PIXELS * BYTES_PER_PIXEL) +
+		    (width - 1) * dst_line_stride;
+
+		/*
+		* For vertical mirroring the writing starts from the
+		* first line
+		*/
+		if (vmirror)
+			out_block_ptr -= dst_line_stride * (width - 1);
+
+		for (i = width; i > 0; i--) {
+			__asm__ volatile (
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				:"+r" (in_block_ptr), "+r"(out_block_ptr)	/* output */
+				:"0"(in_block_ptr), "1"(out_block_ptr), "r"(src_line_stride)	/* input  */
+				:"r2", "r3", "r4", "r5", "memory"	/* modify */
+			);
+			in_block_ptr -= IN_INDEX;
+			out_block_ptr -= OUT_INDEX;
+		}
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+/*
+ * Rotate Counter Clockwise, divide RGB component into 4 row strips, read
+ * non sequentially and write sequentially. This is done in 4 line strips
+ * so that the cache is used better. Cachelines are 8 words = 32 bytes. Pixels
+ * are 2 bytes. The 4 reads will be cache misses, but the next 60 should
+ * be from cache. The writes to the output buffer will be sequential for 4
+ * writes.
+ *
+ * Example:
+ * Input data matrix:	    output matrix
+ *
+ * 0 | 1 | 2 | 3 | 4 |      4 | 0 | 0 | 3 |
+ * 4 | 3 | 2 | 1 | 0 |      3 | 1 | 9 | 6 |
+ * 6 | 7 | 8 | 9 | 0 |      2 | 2 | 8 | 2 |
+ * 5 | 3 | 2 | 6 | 3 |      1 | 3 | 7 | 3 |
+ * ^			    0 | 4 | 6 | 5 | < Write the input data sequentially
+ * Read first column
+ * Start at the bottom
+ * Move to next column and repeat
+ *
+ * Loop over k decreasing (blocks)
+ * in_block_ptr = src + (((RGB_HEIGHT_PIXELS / BLOCK_SIZE_PIXELS) - k)
+ *                * BLOCK_SIZE_PIXELS) * (RGB_WIDTH_BYTES)
+ * out_block_ptr = dst + (((RGB_HEIGHT_PIXELS / BLOCK_SIZE_PIXELS) - k)
+ *                * BLOCK_SIZE_BYTES) + (RGB_WIDTH_PIXELS - 1)
+ *                * RGB_HEIGHT_PIXELS * BYTES_PER_PIXEL
+ *
+ * Loop over i decreasing (width)
+ * Each pix:
+ * in_block_ptr += RGB_WIDTH_BYTES
+ * out_block_ptr += 4
+ *
+ * Each row of block:
+ * in_block_ptr -= RGB_WIDTH_BYTES * BLOCK_SIZE_PIXELS - 2
+ * out_block_ptr -= RGB_HEIGHT_PIXELS * BYTES_PER_PIXEL + 2 * BLOCK_SIZE_PIXELS;
+ *
+ * It may perform vertical mirroring too depending on the vmirror flag.
+ */
+static int opl_rotate270_u16_by4(const u8 * src, int src_line_stride, int width,
+				 int height, u8 * dst, int dst_line_stride,
+				 int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL / 4;
+	const int IN_INDEX = src_line_stride * BLOCK_SIZE_PIXELS
+	    - BYTES_PER_PIXEL;
+	const int OUT_INDEX = vmirror ?
+	    -dst_line_stride + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS
+	    : dst_line_stride + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS;
+	const u8 *in_block_ptr;
+	u8 *out_block_ptr;
+	int i, k;
+
+	for (k = height / BLOCK_SIZE_PIXELS; k > 0; k--) {
+		in_block_ptr = src + (((height / BLOCK_SIZE_PIXELS) - k)
+				      * BLOCK_SIZE_PIXELS) * src_line_stride;
+		out_block_ptr = dst + (((height / BLOCK_SIZE_PIXELS) - k)
+				       * BLOCK_SIZE_PIXELS * BYTES_PER_PIXEL)
+		    + (width - 1) * dst_line_stride;
+
+		/*
+		* For vertical mirroring the writing starts from the
+		* first line
+		*/
+		if (vmirror)
+			out_block_ptr -= dst_line_stride * (width - 1);
+
+		for (i = width; i > 0; i--) {
+			__asm__ volatile (
+				"ldrh	r2, [%0], %4\n\t"
+				"ldrh	r3, [%0], %4\n\t"
+				"ldrh	r4, [%0], %4\n\t"
+				"ldrh	r5, [%0], %4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				:"+r" (in_block_ptr), "+r"(out_block_ptr)	/* output */
+				:"0"(in_block_ptr), "1"(out_block_ptr), "r"(src_line_stride)	/* input  */
+				:"r2", "r3", "r4", "r5", "memory"	/* modify */
+			);
+			in_block_ptr -= IN_INDEX;
+			out_block_ptr -= OUT_INDEX;
+		}
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+EXPORT_SYMBOL(opl_rotate270_u16);
+EXPORT_SYMBOL(opl_rotate270_vmirror_u16);
diff --git a/drivers/media/video/mxc/opl/rotate270_u16_qcif.S b/drivers/media/video/mxc/opl/rotate270_u16_qcif.S
new file mode 100644
index 000000000000..4101eaac4554
--- /dev/null
+++ b/drivers/media/video/mxc/opl/rotate270_u16_qcif.S
@@ -0,0 +1,70 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/linkage.h>
+
+	.text
+	.align	2
+ENTRY(opl_rotate270_u16_qcif)
+	STMFD    sp!,{r4-r10}
+        MOV      r12,#0x160
+        MOV      r10,#0x90
+        MOV      r3,r10,LSR #4
+.L1.16:
+        RSB      r2,r3,r10,LSR #4
+        MOV      r5,r2,LSL #5
+        MOV      r4,r12,LSR #1
+        SMULBB   r4,r5,r4
+        ADD      r2,r1,r2,LSL #5
+        ADD      r5,r2,#0xc000
+        ADD      r5,r5,#0x4e0
+        MOV      r2,r12,LSR #1
+        ADD      r4,r0,r4
+.L1.52:
+        LDRH     r6,[r4],r12
+        LDRH     r7,[r4],r12
+        LDRH     r8,[r4],r12
+        LDRH     r9,[r4],r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        SUBS     r2,r2,#1
+        LDRH     r6,[r4],r12
+        LDRH     r7,[r4],r12
+        LDRH     r8,[r4],r12
+        LDRH     r9,[r4],r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        LDRH     r6,[r4],r12
+        LDRH     r7,[r4],r12
+        LDRH     r8,[r4],r12
+        LDRH     r9,[r4],r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        LDRH     r6,[r4],r12
+        LDRH     r7,[r4],r12
+        LDRH     r8,[r4],r12
+        LDRH     r9,[r4],r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        SUB      r4,r4,#0x1500
+        STMIA    r5,{r6,r7}
+        SUB      r5,r5,#0x138
+        SUB      r4,r4,#0xfe
+        BGT      .L1.52
+        SUBS     r3,r3,#1
+        BGT      .L1.16
+        LDMFD    sp!,{r4-r10}
+        BX       lr
+	.size	opl_rotate270_u16_qcif, . - opl_rotate270_u16_qcif
diff --git a/drivers/media/video/mxc/opl/rotate90_u16.c b/drivers/media/video/mxc/opl/rotate90_u16.c
new file mode 100644
index 000000000000..dd7d445aa952
--- /dev/null
+++ b/drivers/media/video/mxc/opl/rotate90_u16.c
@@ -0,0 +1,220 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include "opl.h"
+
+static int opl_rotate90_u16_by16(const u8 * src, int src_line_stride, int width,
+				 int height, u8 * dst, int dst_line_stride,
+				 int vmirror);
+static int opl_rotate90_u16_by4(const u8 * src, int src_line_stride, int width,
+				int height, u8 * dst, int dst_line_stride,
+				int vmirror);
+static int opl_rotate90_vmirror_u16_both(const u8 * src, int src_line_stride,
+					 int width, int height, u8 * dst,
+					 int dst_line_stride, int vmirror);
+int opl_rotate90_u16_qcif(const u8 * src, u8 * dst);
+
+int opl_rotate90_u16(const u8 * src, int src_line_stride, int width, int height,
+		     u8 * dst, int dst_line_stride)
+{
+	return opl_rotate90_vmirror_u16_both(src, src_line_stride, width,
+					    height, dst, dst_line_stride, 0);
+}
+
+int opl_rotate90_vmirror_u16(const u8 * src, int src_line_stride, int width,
+			    int height, u8 * dst, int dst_line_stride)
+{
+	return opl_rotate90_vmirror_u16_both(src, src_line_stride, width,
+					    height, dst, dst_line_stride, 1);
+}
+
+static int opl_rotate90_vmirror_u16_both(const u8 * src, int src_line_stride,
+					 int width, int height, u8 * dst,
+					 int dst_line_stride, int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL;
+	const int BLOCK_SIZE_PIXELS_BY4 = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL / 4;
+
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	/* The QCIF algorithm doesn't support vertical mirroring */
+	if (vmirror == 0 && width == QCIF_Y_WIDTH && height == QCIF_Y_HEIGHT
+	    && src_line_stride == QCIF_Y_WIDTH * 2
+	    && src_line_stride == QCIF_Y_HEIGHT * 2)
+		return opl_rotate90_u16_qcif(src, dst);
+	else if (width % BLOCK_SIZE_PIXELS == 0
+		 && height % BLOCK_SIZE_PIXELS == 0)
+		return opl_rotate90_u16_by16(src, src_line_stride, width,
+					     height, dst, dst_line_stride,
+					     vmirror);
+	else if (width % BLOCK_SIZE_PIXELS_BY4 == 0
+		 && height % BLOCK_SIZE_PIXELS_BY4 == 0)
+		return opl_rotate90_u16_by4(src, src_line_stride, width, height,
+					    dst, dst_line_stride, vmirror);
+	else
+		return OPLERR_BAD_ARG;
+}
+
+/*
+ * Performs clockwise rotation (and possibly vertical mirroring depending
+ * on the vmirror flag) using block sizes of 16x16
+ * The algorithm is similar to 270 degree clockwise rotation algorithm
+ */
+static int opl_rotate90_u16_by16(const u8 * src, int src_line_stride, int width,
+				 int height, u8 * dst, int dst_line_stride,
+				 int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL;
+	const int BLOCK_SIZE_BYTES = BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS;
+	const int IN_INDEX = src_line_stride * BLOCK_SIZE_PIXELS
+	    + BYTES_PER_PIXEL;
+	const int OUT_INDEX = vmirror ?
+	    -dst_line_stride - BLOCK_SIZE_BYTES
+	    : dst_line_stride - BLOCK_SIZE_BYTES;
+	const u8 *in_block_ptr;
+	u8 *out_block_ptr;
+	int i, k;
+
+	for (k = height / BLOCK_SIZE_PIXELS; k > 0; k--) {
+		in_block_ptr = src + src_line_stride * (height - 1)
+		    - (src_line_stride * BLOCK_SIZE_PIXELS *
+		       (height / BLOCK_SIZE_PIXELS - k));
+		out_block_ptr = dst + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS *
+		    ((height / BLOCK_SIZE_PIXELS) - k);
+
+		/*
+		 * For vertical mirroring the writing starts from the
+		 * bottom line
+		 */
+		if (vmirror)
+			out_block_ptr += dst_line_stride * (width - 1);
+
+		for (i = width; i > 0; i--) {
+			__asm__ volatile (
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				:"+r" (in_block_ptr), "+r"(out_block_ptr)	/* output */
+				:"0"(in_block_ptr), "1"(out_block_ptr), "r"(src_line_stride)	/* input  */
+				:"r2", "r3", "r4", "r5", "memory"	/* modify */
+			);
+			in_block_ptr += IN_INDEX;
+			out_block_ptr += OUT_INDEX;
+		}
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+/*
+ * Performs clockwise rotation (and possibly vertical mirroring depending
+ * on the vmirror flag) using block sizes of 4x4
+ * The algorithm is similar to 270 degree clockwise rotation algorithm
+ */
+static int opl_rotate90_u16_by4(const u8 * src, int src_line_stride, int width,
+				int height, u8 * dst, int dst_line_stride,
+				int vmirror)
+{
+	const int BLOCK_SIZE_PIXELS = CACHE_LINE_WORDS * BYTES_PER_WORD
+	    / BYTES_PER_PIXEL / 4;
+	const int BLOCK_SIZE_BYTES = BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS;
+	const int IN_INDEX = src_line_stride * BLOCK_SIZE_PIXELS
+	    + BYTES_PER_PIXEL;
+	const int OUT_INDEX = vmirror ?
+	    -dst_line_stride - BLOCK_SIZE_BYTES
+	    : dst_line_stride - BLOCK_SIZE_BYTES;
+	const u8 *in_block_ptr;
+	u8 *out_block_ptr;
+	int i, k;
+
+	for (k = height / BLOCK_SIZE_PIXELS; k > 0; k--) {
+		in_block_ptr = src + src_line_stride * (height - 1)
+		    - (src_line_stride * BLOCK_SIZE_PIXELS *
+		       (height / BLOCK_SIZE_PIXELS - k));
+		out_block_ptr = dst + BYTES_PER_PIXEL * BLOCK_SIZE_PIXELS
+		    * ((height / BLOCK_SIZE_PIXELS) - k);
+
+		/*
+		 * For horizontal mirroring the writing starts from the
+		 * bottom line
+		 */
+		if (vmirror)
+			out_block_ptr += dst_line_stride * (width - 1);
+
+		for (i = width; i > 0; i--) {
+			__asm__ volatile (
+				"ldrh	r2, [%0], -%4\n\t"
+				"ldrh	r3, [%0], -%4\n\t"
+				"ldrh	r4, [%0], -%4\n\t"
+				"ldrh	r5, [%0], -%4\n\t"
+				"orr	r2, r2, r3, lsl #16\n\t"
+				"orr	r4, r4, r5, lsl #16\n\t"
+				"str	r2, [%1], #4\n\t"
+				"str	r4, [%1], #4\n\t"
+
+				:"+r" (in_block_ptr), "+r"(out_block_ptr)	/* output */
+				:"0"(in_block_ptr), "1"(out_block_ptr), "r"(src_line_stride)	/* input  */
+				:"r2", "r3", "r4", "r5", "memory"	/* modify */
+			);
+			in_block_ptr += IN_INDEX;
+			out_block_ptr += OUT_INDEX;
+		}
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+EXPORT_SYMBOL(opl_rotate90_u16);
+EXPORT_SYMBOL(opl_rotate90_vmirror_u16);
diff --git a/drivers/media/video/mxc/opl/rotate90_u16_qcif.S b/drivers/media/video/mxc/opl/rotate90_u16_qcif.S
new file mode 100644
index 000000000000..8568a9e629e5
--- /dev/null
+++ b/drivers/media/video/mxc/opl/rotate90_u16_qcif.S
@@ -0,0 +1,71 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/linkage.h>
+
+	.text
+	.align	2
+ENTRY(opl_rotate90_u16_qcif)
+        STMFD    sp!,{r4-r10}
+        MOV      r12,#0x160
+        MOV      r10,#0x90
+        MOV      r3,r10,LSR #4
+.L1.216:
+        RSB      r2,r3,r10,LSR #4
+        MOV      r4,#0x20
+        SMULBB   r5,r4,r2
+        MOV      r4,#0x1600
+        SMULBB   r2,r4,r2
+        ADD      r4,r0,#0xc000
+        ADD      r4,r4,#0x4a0
+        SUB      r4,r4,r2
+        MOV      r2,r12,LSR #1
+        ADD      r5,r1,r5
+.L1.256:
+        LDRH     r6,[r4],-r12
+        LDRH     r7,[r4],-r12
+        LDRH     r8,[r4],-r12
+        LDRH     r9,[r4],-r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        SUBS     r2,r2,#1
+        LDRH     r6,[r4],-r12
+        LDRH     r7,[r4],-r12
+        LDRH     r8,[r4],-r12
+        LDRH     r9,[r4],-r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        LDRH     r6,[r4],-r12
+        LDRH     r7,[r4],-r12
+        LDRH     r8,[r4],-r12
+        LDRH     r9,[r4],-r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        STMIA    r5!,{r6,r7}
+        LDRH     r6,[r4],-r12
+        LDRH     r7,[r4],-r12
+        LDRH     r8,[r4],-r12
+        LDRH     r9,[r4],-r12
+        ORR      r6,r6,r7,LSL #16
+        ORR      r7,r8,r9,LSL #16
+        ADD      r4,r4,#0x1600
+        STMIA    r5!,{r6,r7}
+        ADD      r5,r5,#0x100
+        ADD      r4,r4,#2
+        BGT      .L1.256
+        SUBS     r3,r3,#1
+        BGT      .L1.216
+        LDMFD    sp!,{r4-r10}
+        BX       lr
+	.size	opl_rotate90_u16_qcif, . - opl_rotate90_u16_qcif
diff --git a/drivers/media/video/mxc/opl/vmirror_u16.c b/drivers/media/video/mxc/opl/vmirror_u16.c
new file mode 100644
index 000000000000..57f805c08a81
--- /dev/null
+++ b/drivers/media/video/mxc/opl/vmirror_u16.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/module.h>
+#include <linux/string.h>
+#include "opl.h"
+
+int opl_vmirror_u16(const u8 * src, int src_line_stride, int width, int height,
+		    u8 * dst, int dst_line_stride)
+{
+	const u8 *src_row_addr;
+	u8 *dst_row_addr;
+	int i;
+
+	if (!src || !dst)
+		return OPLERR_NULL_PTR;
+
+	if (width == 0 || height == 0 || src_line_stride == 0
+	    || dst_line_stride == 0)
+		return OPLERR_BAD_ARG;
+
+	src_row_addr = src;
+	dst_row_addr = dst + (height - 1) * dst_line_stride;
+
+	/* Loop over all rows */
+	for (i = 0; i < height; i++) {
+		/* memcpy each row */
+		memcpy(dst_row_addr, src_row_addr, BYTES_PER_PIXEL * width);
+		src_row_addr += src_line_stride;
+		dst_row_addr -= dst_line_stride;
+	}
+
+	return OPLERR_SUCCESS;
+}
+
+EXPORT_SYMBOL(opl_vmirror_u16);
diff --git a/drivers/media/video/mxc/output/Kconfig b/drivers/media/video/mxc/output/Kconfig
new file mode 100644
index 000000000000..2153ad248907
--- /dev/null
+++ b/drivers/media/video/mxc/output/Kconfig
@@ -0,0 +1,28 @@
+config VIDEO_MXC_IPU_OUTPUT
+	bool "IPU v4l2 support"
+	depends on VIDEO_MXC_OUTPUT && MXC_IPU
+	default y
+	---help---
+	  This is the video4linux2 driver for IPU post processing video output.
+
+config VIDEO_MXC_IPUV1_WVGA_OUTPUT
+	bool "IPUv1 WVGA v4l2 display support"
+	depends on VIDEO_MXC_OUTPUT && MXC_IPU
+	default n
+	---help---
+	  This is the video4linux2 driver for IPUv1 WVGA post processing video output.
+
+config VIDEO_MXC_EMMA_OUTPUT
+	bool
+	depends on VIDEO_MXC_OUTPUT && MXC_EMMA && FB_MXC_SYNC_PANEL
+	default y
+	---help---
+	  This is the video4linux2 driver for EMMA post processing video output.
+
+config VIDEO_MXC_OUTPUT_FBSYNC
+	bool "Synchronize the output with LCDC refresh"
+	depends on VIDEO_MXC_EMMA_OUTPUT
+	default y
+	---help---
+	  Synchronize the post-processing with LCDC EOF (End of Frame) to
+	  prevent tearing issue. If unsure, say Y.
diff --git a/drivers/media/video/mxc/output/Makefile b/drivers/media/video/mxc/output/Makefile
new file mode 100644
index 000000000000..1713fa3bf3ab
--- /dev/null
+++ b/drivers/media/video/mxc/output/Makefile
@@ -0,0 +1,11 @@
+ifeq ($(CONFIG_VIDEO_MXC_EMMA_OUTPUT),y)
+	mx27_output-objs := mx27_v4l2_output.o mx27_pp.o
+	obj-$(CONFIG_VIDEO_MXC_OUTPUT) += mx27_output.o
+endif
+
+ifeq ($(CONFIG_VIDEO_MXC_IPU_OUTPUT),y)
+	obj-$(CONFIG_VIDEO_MXC_OUTPUT)	+= mxc_v4l2_output.o
+endif
+ifeq ($(CONFIG_VIDEO_MXC_IPUV1_WVGA_OUTPUT),y)
+	obj-$(CONFIG_VIDEO_MXC_OUTPUT)	+= mx31_v4l2_wvga_output.o
+endif
diff --git a/drivers/media/video/mxc/output/mx27_pp.c b/drivers/media/video/mxc/output/mx27_pp.c
new file mode 100644
index 000000000000..a82328015fe2
--- /dev/null
+++ b/drivers/media/video/mxc/output/mx27_pp.c
@@ -0,0 +1,904 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_pp.c
+ *
+ * @brief MX27 V4L2 Video Output Driver
+ *
+ * Video4Linux2 Output Device using MX27 eMMA Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/module.h>
+#include <linux/fb.h>
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <asm/io.h>
+
+#include "mx27_pp.h"
+#include "mxc_v4l2_output.h"
+
+#define SCALE_RETRY	32	/* to be more relax, less precise */
+#define PP_SKIP		1
+#define PP_TBL_MAX	40
+
+static unsigned short scale_tbl[PP_TBL_MAX];
+static int g_hlen, g_vlen;
+
+static emma_pp_cfg g_pp_cfg;
+static int g_disp_num = 0;
+static char pp_dev[] = "emma_pp";
+
+/*!
+ * @brief PP resizing routines
+ */
+static int gcd(int x, int y);
+static int ratio(int x, int y, int *den);
+static int scale_0d(int k, int coeff, int base, int nxt);
+static int scale_1d(int inv, int outv, int k);
+static int scale_1d_smart(int *inv, int *outv, int index);
+static int scale_2d(emma_pp_scale * sz);
+
+static irqreturn_t pp_isr(int irq, void *dev_id);
+static int set_output_addr(emma_pp_cfg * cfg, vout_data * vout);
+static int pphw_reset(void);
+static int pphw_enable(int flag);
+static int pphw_ptr(emma_pp_cfg * cfg);
+static int pphw_outptr(emma_pp_cfg * cfg);
+static int pphw_cfg(emma_pp_cfg * cfg);
+static int pphw_isr(void);
+static void pphw_init(void);
+static void pphw_exit(void);
+
+#define PP_DUMP(reg)	pr_debug("%s\t = 0x%08X\n", #reg, __raw_readl(reg))
+void pp_dump(void)
+{
+	PP_DUMP(PP_CNTL);
+	PP_DUMP(PP_INTRCNTL);
+	PP_DUMP(PP_INTRSTATUS);
+	PP_DUMP(PP_SOURCE_Y_PTR);
+	PP_DUMP(PP_SOURCE_CB_PTR);
+	PP_DUMP(PP_SOURCE_CR_PTR);
+	PP_DUMP(PP_DEST_RGB_PTR);
+	PP_DUMP(PP_QUANTIZER_PTR);
+	PP_DUMP(PP_PROCESS_FRAME_PARA);
+	PP_DUMP(PP_SOURCE_FRAME_WIDTH);
+	PP_DUMP(PP_DEST_DISPLAY_WIDTH);
+	PP_DUMP(PP_DEST_IMAGE_SIZE);
+	PP_DUMP(PP_DEST_FRAME_FMT_CNTL);
+	PP_DUMP(PP_RESIZE_INDEX);
+	PP_DUMP(PP_CSC_COEF_0123);
+	PP_DUMP(PP_CSC_COEF_4);
+}
+
+/*!
+ * @brief Set PP input address.
+ * @param ptr	The pointer to the Y value of input
+ * @return	Zero on success, others on failure
+ */
+int pp_ptr(unsigned long ptr)
+{
+	g_pp_cfg.ptr.y = ptr;
+	g_pp_cfg.ptr.u = g_pp_cfg.ptr.v = g_pp_cfg.ptr.qp = 0;
+
+	return pphw_ptr(&g_pp_cfg);
+}
+
+/*!
+ * @brief Enable or disable PP.
+ * @param flag	Zero to disable PP, others to enable PP
+ * @return	Zero on success, others on failure
+ */
+int pp_enable(int flag)
+{
+	return pphw_enable(flag);
+}
+
+/*!
+ * @brief Get the display No. of last completed PP frame.
+ * @return	The display No. of last completed PP frame.
+ */
+int pp_num_last(void)
+{
+	return (g_disp_num ? 0 : 1);
+}
+
+/*!
+ * @brief Initialize PP.
+ * @param vout	Pointer to _vout_data structure
+ * @return	Zero on success, others on failure
+ */
+int pp_init(vout_data * vout)
+{
+	pphw_init();
+	pphw_enable(0);
+	enable_irq(MXC_INT_EMMAPP);
+	return request_irq(MXC_INT_EMMAPP, pp_isr, 0, pp_dev, vout);
+}
+
+/*!
+ * @brief Deinitialize PP.
+ * @param vout	Pointer to _vout_data structure
+ */
+void pp_exit(vout_data * vout)
+{
+	disable_irq(MXC_INT_EMMAPP);
+	free_irq(MXC_INT_EMMAPP, vout);
+	pphw_enable(0);
+	pphw_exit();
+}
+
+/*!
+ * @brief Configure PP.
+ * @param vout	Pointer to _vout_data structure
+ * @return	Zero on success, others on failure
+ */
+int pp_cfg(vout_data * vout)
+{
+	if (!vout)
+		return -1;
+
+	/* PP accepts YUV420 input only */
+	if (vout->v2f.fmt.pix.pixelformat != V4L2_PIX_FMT_YUV420) {
+		pr_debug("unsupported pixel format.\n");
+		return -1;
+	}
+
+	g_pp_cfg.operation = 0;
+
+	memset(g_pp_cfg.csc_table, 0, sizeof(g_pp_cfg.csc_table));
+
+	/* Convert output pixel format to PP required format */
+	switch (vout->v4l2_fb.fmt.pixelformat) {
+	case V4L2_PIX_FMT_BGR32:
+		g_pp_cfg.red_width = 8;
+		g_pp_cfg.green_width = 8;
+		g_pp_cfg.blue_width = 8;
+		g_pp_cfg.red_offset = 8;
+		g_pp_cfg.green_offset = 16;
+		g_pp_cfg.blue_offset = 24;
+		g_pp_cfg.rgb_resolution = 32;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+		g_pp_cfg.red_width = 8;
+		g_pp_cfg.green_width = 8;
+		g_pp_cfg.blue_width = 8;
+		g_pp_cfg.red_offset = 24;
+		g_pp_cfg.green_offset = 16;
+		g_pp_cfg.blue_offset = 8;
+		g_pp_cfg.rgb_resolution = 32;
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		g_pp_cfg.red_width = 0;
+		g_pp_cfg.green_width = 0;
+		g_pp_cfg.blue_width = 0;
+		g_pp_cfg.red_offset = 0;
+		g_pp_cfg.green_offset = 0;
+		g_pp_cfg.blue_offset = PP_PIX_YUYV;
+		g_pp_cfg.rgb_resolution = 16;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		g_pp_cfg.red_width = 0;
+		g_pp_cfg.green_width = 0;
+		g_pp_cfg.blue_width = 0;
+		g_pp_cfg.red_offset = 0;
+		g_pp_cfg.green_offset = 0;
+		g_pp_cfg.blue_offset = PP_PIX_UYVY;
+		g_pp_cfg.rgb_resolution = 16;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+	default:
+		g_pp_cfg.red_width = 5;
+		g_pp_cfg.green_width = 6;
+		g_pp_cfg.blue_width = 5;
+		g_pp_cfg.red_offset = 11;
+		g_pp_cfg.green_offset = 5;
+		g_pp_cfg.blue_offset = 0;
+		g_pp_cfg.rgb_resolution = 16;
+		break;
+	}
+
+	if (vout->ipu_buf[0] != -1)
+		g_pp_cfg.ptr.y =
+		    (unsigned int)vout->queue_buf_paddr[vout->ipu_buf[0]];
+	else
+		g_pp_cfg.ptr.y = 0;
+
+	g_pp_cfg.ptr.u = g_pp_cfg.ptr.v = g_pp_cfg.ptr.qp = 0;
+
+	g_pp_cfg.dim.in.width = vout->v2f.fmt.pix.width;
+	g_pp_cfg.dim.in.height = vout->v2f.fmt.pix.height;
+	g_pp_cfg.dim.out.width = vout->crop_current.width;
+	g_pp_cfg.dim.out.height = vout->crop_current.height;
+	g_pp_cfg.dim.num.width = 0;
+	g_pp_cfg.dim.num.height = 0;
+	g_pp_cfg.dim.den.width = 0;
+	g_pp_cfg.dim.den.height = 0;
+
+	if (scale_2d(&g_pp_cfg.dim)) {
+		pr_debug("unsupported resize ratio.\n");
+		return -1;
+	}
+
+	g_pp_cfg.dim.out.width = vout->crop_current.width;
+	g_pp_cfg.dim.out.height = vout->crop_current.height;
+
+	g_pp_cfg.in_y_stride = 0;
+	if (set_output_addr(&g_pp_cfg, vout)) {
+		pr_debug("failed to set pp output address.\n");
+		return -1;
+	}
+
+	return pphw_cfg(&g_pp_cfg);
+}
+
+irqreturn_t mxc_v4l2out_pp_in_irq_handler(int irq, void *dev_id);
+
+/*!
+ * @brief PP IRQ handler.
+ */
+static irqreturn_t pp_isr(int irq, void *dev_id)
+{
+	int status;
+	vout_data *vout = dev_id;
+
+	status = pphw_isr();
+	if ((status & 0x1) == 0) {	/* Not frame complete interrupt */
+		pr_debug("not pp frame complete interrupt\n");
+		return IRQ_HANDLED;
+	}
+
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		g_disp_num = g_disp_num ? 0 : 1;
+		g_pp_cfg.outptr = (unsigned int)vout->display_bufs[g_disp_num];
+		pphw_outptr(&g_pp_cfg);
+	}
+
+	return mxc_v4l2out_pp_in_irq_handler(irq, dev_id);
+}
+
+/*!
+ * @brief Set PP output address.
+ * @param cfg	Pointer to emma_pp_cfg structure
+ * @param vout	Pointer to _vout_data structure
+ * @return	Zero on success, others on failure
+ */
+static int set_output_addr(emma_pp_cfg * cfg, vout_data * vout)
+{
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		g_disp_num = 0;
+		cfg->outptr = (unsigned int)vout->display_bufs[g_disp_num];
+		cfg->out_stride = vout->crop_current.width;
+		return 0;
+	} else {
+		struct fb_info *fb;
+
+		fb = registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+		if (!fb)
+			return -1;
+
+		cfg->outptr = fb->fix.smem_start;
+		cfg->outptr += vout->crop_current.top * fb->var.xres_virtual
+		    * (fb->var.bits_per_pixel >> 3)
+		    + vout->crop_current.left * (fb->var.bits_per_pixel >> 3);
+		cfg->out_stride = fb->var.xres_virtual;
+
+		return 0;
+	}
+}
+
+/*!
+ * @brief Get maximum common divisor.
+ * @param x	First input value
+ * @param y	Second input value
+ * @return	Maximum common divisor of x and y
+ */
+static int gcd(int x, int y)
+{
+	int k;
+
+	if (x < y) {
+		k = x;
+		x = y;
+		y = k;
+	}
+
+	while ((k = x % y)) {
+		x = y;
+		y = k;
+	}
+
+	return y;
+}
+
+/*!
+ * @brief Get ratio.
+ * @param x	First input value
+ * @param y	Second input value
+ * @param den	Denominator of the ratio (corresponding to y)
+ * @return	Numerator of the ratio (corresponding to x)
+ */
+static int ratio(int x, int y, int *den)
+{
+	int g;
+
+	if (!x || !y)
+		return 0;
+
+	g = gcd(x, y);
+	*den = y / g;
+
+	return x / g;
+}
+
+/*!
+ * @brief Build PP coefficient entry
+ * Build one or more coefficient entries for PP coefficient table based
+ * on given coefficient.
+ *
+ * @param k	The index of the coefficient in coefficient table
+ * @param coeff	The weighting coefficient
+ * @param base	The base of the coefficient
+ * @param nxt	Number of pixels to be read
+ *
+ * @return	The index of the next coefficient entry on success
+ *		-1 on failure
+ */
+static int scale_0d(int k, int coeff, int base, int nxt)
+{
+	if (k >= PP_TBL_MAX) {
+		/* no more space in table */
+		pr_debug("no space in scale table, k = %d\n", k);
+		return -1;
+	}
+
+	coeff = ((coeff << BC_COEF) + (base >> 1)) / base;
+
+	/*
+	 * Valid values for weighting coefficient are 0, 2 to 30, and 31.
+	 * A value of 31 is treated as 32 and therefore 31 is an
+	 * invalid co-efficient.
+	 */
+	if (coeff >= SZ_COEF - 1)
+		coeff--;
+	else if (coeff == 1)
+		coeff++;
+	coeff = coeff << BC_NXT;
+
+	if (nxt < SZ_NXT) {
+		coeff |= nxt;
+		coeff <<= 1;
+		coeff |= 1;
+	} else {
+		/*
+		 * src inc field is 2 bit wide, for 4+, use special
+		 * code 0:0:1 to prevent dest inc
+		 */
+		coeff |= PP_SKIP;
+		coeff <<= 1;
+		coeff |= 1;
+		nxt -= PP_SKIP;
+		do {
+			pr_debug("tbl = %03X\n", coeff);
+			scale_tbl[k++] = coeff;
+			coeff = (nxt > PP_SKIP) ? PP_SKIP : nxt;
+			coeff <<= 1;
+		} while ((nxt -= PP_SKIP) > 0);
+	}
+	pr_debug("tbl = %03X\n", coeff);
+	scale_tbl[k++] = coeff;
+
+	return k;
+}
+
+/*
+ * @brief Build PP coefficient table
+ * Build PP coefficient table for one dimension (width or height)
+ * based on given input and output resolution
+ *
+ * @param inv	input resolution
+ * @param outv	output resolution
+ * @param k	index of free table entry
+ *
+ * @return	The index of the next free coefficient entry on success
+ *		-1 on failure
+ */
+static int scale_1d(int inv, int outv, int k)
+{
+	int v;			/* overflow counter */
+	int coeff, nxt;		/* table output */
+
+	if (inv == outv)
+		return scale_0d(k, 1, 1, 1);	/* force scaling */
+
+	if (inv * 4 < outv) {
+		pr_debug("upscale err: ratio should be in range 1:1 to 1:4\n");
+		return -1;
+	}
+
+	v = 0;
+	if (inv < outv) {
+		/* upscale: mix <= 2 input pixels per output pixel */
+		do {
+			coeff = outv - v;
+			v += inv;
+			if (v >= outv) {
+				v -= outv;
+				nxt = 1;
+			} else
+				nxt = 0;
+			pr_debug("upscale: coeff = %d/%d nxt = %d\n", coeff,
+				 outv, nxt);
+			k = scale_0d(k, coeff, outv, nxt);
+			if (k < 0)
+				return -1;
+		} while (v);
+	} else if (inv >= 2 * outv) {
+		/* PP doesn't support resize ratio > 2:1 except 4:1. */
+		if ((inv != 2 * outv) && (inv != 4 * outv))
+			return -1;
+		/* downscale: >=2:1 bilinear approximation */
+		coeff = inv - 2 * outv;
+		v = 0;
+		nxt = 0;
+		do {
+			v += coeff;
+			nxt = 2;
+			while (v >= outv) {
+				v -= outv;
+				nxt++;
+			}
+			pr_debug("downscale: coeff = 1/2 nxt = %d\n", nxt);
+			k = scale_0d(k, 1, 2, nxt);
+			if (k < 0)
+				return -1;
+		} while (v);
+	} else {
+		/* downscale: bilinear */
+		int in_pos_inc = 2 * outv;
+		int out_pos = inv;
+		int out_pos_inc = 2 * inv;
+		int init_carry = inv - outv;
+		int carry = init_carry;
+
+		v = outv + in_pos_inc;
+		do {
+			coeff = v - out_pos;
+			out_pos += out_pos_inc;
+			carry += out_pos_inc;
+			for (nxt = 0; v < out_pos; nxt++) {
+				v += in_pos_inc;
+				carry -= in_pos_inc;
+			}
+			pr_debug("downscale: coeff = %d/%d nxt = %d\n", coeff,
+				 in_pos_inc, nxt);
+			k = scale_0d(k, coeff, in_pos_inc, nxt);
+			if (k < 0)
+				return -1;
+		} while (carry != init_carry);
+	}
+	return k;
+}
+
+/*
+ * @brief Build PP coefficient table
+ * Build PP coefficient table for one dimension (width or height)
+ * based on given input and output resolution. The given input
+ * and output resolution might be not supported due to hardware
+ * limits. In this case this functin rounds the input and output
+ * to closest possible values and return them to caller.
+ *
+ * @param inv	input resolution, might be modified after the call
+ * @param outv	output resolution, might be modified after the call
+ * @param k	index of free table entry
+ *
+ * @return	The index of the next free coefficient entry on success
+ *		-1 on failure
+ */
+static int scale_1d_smart(int *inv, int *outv, int index)
+{
+	int len, num, den, retry;
+	static int num1, den1;
+
+	if (!inv || !outv)
+		return -1;
+
+	/* Both should be non-zero */
+	if (!(*inv) || !(*outv))
+		return -1;
+
+	retry = SCALE_RETRY;
+
+	do {
+		num = ratio(*inv, *outv, &den);
+		pr_debug("num = %d, den = %d\n", num, den);
+		if (!num)
+			continue;
+
+		if (index != 0) {
+			/*
+			 * We are now resizing height. Check to see if the
+			 * resize ratio for width can be reused by height
+			 */
+			if ((num == num1) && (den == den1))
+				return index;
+		}
+
+		if ((len = scale_1d(num, den, index)) < 0)
+			/* increase output dimension to try another ratio */
+			(*outv)++;
+		else {
+			if (index == 0) {
+				/*
+				 * We are now resizing width. The same resize
+				 * ratio may be reused by height, so save the
+				 * ratio.
+				 */
+				num1 = num;
+				den1 = den;
+			}
+			return len;
+		}
+	} while (retry--);
+
+	pr_debug("pp scale err\n");
+	return -1;
+}
+
+/*
+ * @brief Build PP coefficient table for both width and height
+ * Build PP coefficient table for both width and height based on
+ * given resizing ratios.
+ *
+ * @param sz	Structure contains resizing ratio informations
+ *
+ * @return	0 on success, others on failure
+ */
+static int scale_2d(emma_pp_scale * sz)
+{
+	int inv, outv;
+
+	/* horizontal resizing. parameter check - must provide in size */
+	if (!sz->in.width)
+		return -1;
+
+	/* Resizing based on num:den */
+	inv = sz->num.width;
+	outv = sz->den.width;
+
+	if ((g_hlen = scale_1d_smart(&inv, &outv, 0)) > 0) {
+		/* Resizing succeeded */
+		sz->den.width = outv;
+		sz->out.width = (sz->in.width * outv) / inv;
+	} else {
+		/* Resizing based on in:out */
+		inv = sz->in.width;
+		outv = sz->out.width;
+
+		if ((g_hlen = scale_1d_smart(&inv, &outv, 0)) > 0) {
+			/* Resizing succeeded */
+			sz->out.width = outv;
+			sz->num.width = ratio(sz->in.width, sz->out.width,
+					      &sz->den.width);
+		} else
+			return -1;
+	}
+
+	sz->out.width &= ~1;
+
+	/* vertical resizing. parameter check - must provide in size */
+	if (!sz->in.height)
+		return -1;
+
+	/* Resizing based on num:den */
+	inv = sz->num.height;
+	outv = sz->den.height;
+
+	if ((g_vlen = scale_1d_smart(&inv, &outv, g_hlen)) > 0) {
+		/* Resizing succeeded */
+		sz->den.height = outv;
+		sz->out.height = (sz->in.height * outv) / inv;
+	} else {
+		/* Resizing based on in:out */
+		inv = sz->in.height;
+		outv = sz->out.height;
+
+		if ((g_vlen = scale_1d_smart(&inv, &outv, g_hlen)) > 0) {
+			/* Resizing succeeded */
+			sz->out.height = outv;
+			sz->num.height = ratio(sz->in.height, sz->out.height,
+					       &sz->den.height);
+		} else
+			return -1;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Set PP resizing registers.
+ * @param sz	Pointer to pp scaling structure
+ * @return	Zero on success, others on failure
+ */
+static int pphw_scale(emma_pp_scale * sz)
+{
+	__raw_writel((sz->out.width << 16) | sz->out.height,
+		     PP_DEST_IMAGE_SIZE);
+	__raw_writel(((g_hlen - 1) << 16) | (g_vlen ==
+					     g_hlen ? 0 : (g_hlen << 8)) |
+		     (g_vlen - 1), PP_RESIZE_INDEX);
+	for (g_hlen = 0; g_hlen < g_vlen; g_hlen++)
+		__raw_writel(scale_tbl[g_hlen],
+			     PP_RESIZE_COEF_TBL + g_hlen * 4);
+
+	return 0;
+}
+
+/*!
+ * @brief Reset PP.
+ * @return	Zero on success, others on failure
+ */
+static int pphw_reset(void)
+{
+	int i;
+
+	__raw_writel(0x100, PP_CNTL);
+
+	/* timeout */
+	for (i = 0; i < 1000; i++) {
+		if (!(__raw_readl(PP_CNTL) & 0x100)) {
+			pr_debug("pp reset over\n");
+			break;
+		}
+	}
+
+	/* check reset value */
+	if (__raw_readl(PP_CNTL) != 0x876) {
+		pr_debug("pp reset value err = 0x%08X\n", __raw_readl(PP_CNTL));
+		return -1;
+	}
+
+	return 0;
+}
+
+/*!
+ * @brief Enable or disable PP.
+ * @param flag	Zero to disable PP, others to enable PP
+ * @return	Zero on success, others on failure
+ */
+static int pphw_enable(int flag)
+{
+	int ret = 0;
+
+	if (flag)
+		__raw_writel(__raw_readl(PP_CNTL) | 1, PP_CNTL);
+	else
+		ret = pphw_reset();
+
+	return ret;
+}
+
+/*!
+ * @brief Set PP input address.
+ * @param cfg	The pointer to PP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+static int pphw_ptr(emma_pp_cfg * cfg)
+{
+	if (!cfg->ptr.u) {
+		int size;
+
+		/* yuv - packed */
+		size = PP_CALC_Y_SIZE(cfg);
+		cfg->ptr.u = cfg->ptr.y + size;
+		cfg->ptr.v = cfg->ptr.u + (size >> 2);
+
+		/* yuv packed with qp appended */
+		if (!cfg->ptr.qp)
+			cfg->ptr.qp = cfg->ptr.v + (size >> 2);
+	}
+	__raw_writel(cfg->ptr.y, PP_SOURCE_Y_PTR);
+	__raw_writel(cfg->ptr.u, PP_SOURCE_CB_PTR);
+	__raw_writel(cfg->ptr.v, PP_SOURCE_CR_PTR);
+	__raw_writel(cfg->ptr.qp, PP_QUANTIZER_PTR);
+
+	return 0;
+}
+
+/*!
+ * @brief Set PP output address.
+ * @param cfg	The pointer to PP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+static int pphw_outptr(emma_pp_cfg * cfg)
+{
+	__raw_writel(cfg->outptr, PP_DEST_RGB_PTR);
+	return 0;
+}
+
+/*!
+ * @brief Configuration PP.
+ * @param cfg	The pointer to PP configuration parameter
+ * @return	Zero on success, others on failure
+ */
+static int pphw_cfg(emma_pp_cfg * cfg)
+{
+	int rt;
+	register int r;
+
+	pphw_scale(&cfg->dim);
+
+	if (!cfg->in_y_stride)
+		cfg->in_y_stride = cfg->dim.in.width;
+
+	if (!cfg->out_stride)
+		cfg->out_stride = cfg->dim.out.width;
+
+	r = __raw_readl(PP_CNTL) & ~EN_MASK;
+
+	/* config parms */
+	r |= cfg->operation & EN_MASK;
+	if (cfg->operation & EN_MACROBLOCK) {
+		/* Macroblock Mode */
+		r |= 0x0200;
+		__raw_writel(0x06, PP_INTRCNTL);
+	} else {
+		/* Frame mode */
+		__raw_writel(0x05, PP_INTRCNTL);
+	}
+
+	if (cfg->red_width | cfg->green_width | cfg->blue_width) {
+		/* color conversion to be performed */
+		r |= EN_CSC;
+		if (!(cfg->red_offset | cfg->green_offset)) {
+			/* auto offset B:G:R LSb to Msb */
+			cfg->green_offset = cfg->blue_offset + cfg->blue_width;
+			cfg->red_offset = cfg->green_offset + cfg->green_width;
+		}
+		if (!cfg->rgb_resolution) {
+			/* derive minimum resolution required */
+			int w, w2;
+
+			w = cfg->red_offset + cfg->red_width;
+			w2 = cfg->blue_offset + cfg->blue_width;
+			if (w < w2)
+				w = w2;
+			w2 = cfg->green_offset + cfg->green_width;
+			if (w < w2)
+				w = w2;
+			if (w > 16)
+				w = 24;
+			else if (w > 8)
+				w = 16;
+			else
+				w = 8;
+			cfg->rgb_resolution = w;
+		}
+		/* 00,11 - 32 bpp, 10 - 16 bpp, 01 - 8 bpp */
+		r &= ~0xC00;
+		if (cfg->rgb_resolution < 32)
+			r |= (cfg->rgb_resolution << 7);
+		__raw_writel((cfg->red_offset << 26) |
+			     (cfg->green_offset << 21) |
+			     (cfg->blue_offset << 16) |
+			     (cfg->red_width << 8) |
+			     (cfg->green_width << 4) |
+			     cfg->blue_width, PP_DEST_FRAME_FMT_CNTL);
+	} else {
+		/* add YUV422 formatting */
+		static const unsigned int _422[] = {
+			0x62000888,
+			0x60100888,
+			0x43080888,
+			0x41180888
+		};
+
+		__raw_writel(_422[(cfg->blue_offset >> 3) & 3],
+			     PP_DEST_FRAME_FMT_CNTL);
+		cfg->rgb_resolution = 16;
+		r &= ~0xC00;
+		r |= (cfg->rgb_resolution << 7);
+	}
+
+	/* add csc formatting */
+	if (!cfg->csc_table[1]) {
+		static const unsigned short _csc[][6] = {
+			{0x80, 0xb4, 0x2c, 0x5b, 0x0e4, 0},
+			{0x95, 0xcc, 0x32, 0x68, 0x104, 1},
+			{0x80, 0xca, 0x18, 0x3c, 0x0ec, 0},
+			{0x95, 0xe5, 0x1b, 0x44, 0x10e, 1},
+		};
+		memcpy(cfg->csc_table, _csc[cfg->csc_table[0]],
+		       sizeof(_csc[0]));
+	}
+	__raw_writel((cfg->csc_table[0] << 24) |
+		     (cfg->csc_table[1] << 16) |
+		     (cfg->csc_table[2] << 8) |
+		     cfg->csc_table[3], PP_CSC_COEF_0123);
+	__raw_writel((cfg->csc_table[5] ? (1 << 9) : 0) | cfg->csc_table[4],
+		     PP_CSC_COEF_4);
+
+	__raw_writel(r, PP_CNTL);
+
+	pphw_ptr(cfg);
+	pphw_outptr(cfg);
+
+	/*
+	 * #MB in a row = input_width / 16pix
+	 * 1 byte per QP per MB
+	 * QP must be formatted to be 4-byte aligned
+	 * YUV lines are to be 4-byte aligned as well
+	 * So Y is 8 byte aligned, as U = V = Y/2 for 420
+	 * MPEG MBs are 16x16 anyway
+	 */
+	__raw_writel((cfg->dim.in.width << 16) | cfg->dim.in.height,
+		     PP_PROCESS_FRAME_PARA);
+	__raw_writel(cfg->in_y_stride | (PP_CALC_QP_WIDTH(cfg) << 16),
+		     PP_SOURCE_FRAME_WIDTH);
+
+	/* in bytes */
+	rt = cfg->rgb_resolution >> 3;
+	if (rt == 3)
+		rt = 4;
+	__raw_writel(cfg->out_stride * rt, PP_DEST_DISPLAY_WIDTH);
+
+	pp_dump();
+	return 0;
+}
+
+/*!
+ * @brief Check PP interrupt status.
+ * @return	PP interrupt status
+ */
+static int pphw_isr(void)
+{
+	unsigned long status;
+
+	pr_debug("pp: in isr.\n");
+	status = __raw_readl(PP_INTRSTATUS) & 7;
+	if (!status) {
+		pr_debug("pp: not my isr err.\n");
+		return status;
+	}
+
+	if (status & 4)
+		pr_debug("pp: isr state error.\n");
+
+	/* clear interrupt status */
+	__raw_writel(status, PP_INTRSTATUS);
+
+	return status;
+}
+
+static struct clk *emma_clk;
+
+/*!
+ * @brief PP module clock enable
+ */
+static void pphw_init(void)
+{
+	emma_clk = clk_get(NULL, "emma_clk");
+	clk_enable(emma_clk);
+}
+
+/*!
+ * @brief PP module clock disable
+ */
+static void pphw_exit(void)
+{
+	clk_disable(emma_clk);
+	clk_put(emma_clk);
+}
diff --git a/drivers/media/video/mxc/output/mx27_pp.h b/drivers/media/video/mxc/output/mx27_pp.h
new file mode 100644
index 000000000000..7bc65ddda3a1
--- /dev/null
+++ b/drivers/media/video/mxc/output/mx27_pp.h
@@ -0,0 +1,180 @@
+/*
+ * Copyright 2005-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_pp.h
+ *
+ * @brief Header file for MX27 V4L2 Video Output Driver
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#ifndef __MX27_PP_H__
+#define __MX27_PP_H__
+
+#include "mxc_v4l2_output.h"
+
+/* PP register definitions */
+#define PP_REG(ofs)    (IO_ADDRESS(EMMA_BASE_ADDR) - 0x400 + ofs)
+
+/* Register offsets */
+#define PP_CNTL			PP_REG(0x00)
+#define PP_INTRCNTL	 	PP_REG(0x04)
+#define PP_INTRSTATUS		PP_REG(0x08)
+#define PP_SOURCE_Y_PTR		PP_REG(0x0C)
+#define PP_SOURCE_CB_PTR	PP_REG(0x10)
+#define PP_SOURCE_CR_PTR	PP_REG(0x14)
+#define PP_DEST_RGB_PTR 	PP_REG(0x18)
+#define PP_QUANTIZER_PTR	PP_REG(0x1C)
+#define PP_PROCESS_FRAME_PARA	PP_REG(0x20)
+#define PP_SOURCE_FRAME_WIDTH	PP_REG(0x24)
+#define PP_DEST_DISPLAY_WIDTH	PP_REG(0x28)
+#define PP_DEST_IMAGE_SIZE	PP_REG(0x2C)
+#define PP_DEST_FRAME_FMT_CNTL	PP_REG(0x30)
+#define PP_RESIZE_INDEX		PP_REG(0x34)
+#define	PP_CSC_COEF_0123	PP_REG(0x38)
+#define	PP_CSC_COEF_4		PP_REG(0x3C)
+#define PP_RESIZE_COEF_TBL	PP_REG(0x100)
+
+/* resize table dimensions
+    dest pixel index    left/32    right/32    #src pixels to read
+    0                   [BC_COEF]  [BC_COEF]   [BC_NXT]
+    :
+    pp_tbl_max-1
+*/
+#define BC_NXT		2
+#define BC_COEF		5
+#define SZ_COEF		(1 << BC_COEF)
+#define SZ_NXT		(1 << BC_NXT)
+
+/* PP operations */
+#define EN_DEBLOCK 	0x02
+#define EN_DERING	0x04
+#define EN_CSC		0x10
+#define EN_MACROBLOCK	0x20
+#define EN_DEF		0x16
+#define EN_MASK		0x36
+#define EN_BIGDATA	0x1000
+#define EN_BIGQP	0x2000
+
+/* PP CSC tables */
+#define CSC_TBL_NONE	0x80
+#define CSC_TBL_REUSE	0x81
+#define CSC_TBL_A1	0x00
+#define CSC_TBL_A0	0x20
+#define CSC_TBL_B1	0x40
+#define CSC_TBL_B0	0x60
+/* converts from 4 decimal fixed point to hw setting & vice versa */
+#define PP_CSC_FP4_2_HW(coeff)	((((coeff) << 7) + 5000) / 10000)
+#define PP_CSC_HW_2_FP4(coeff)	((((coeff) * 10000) + 64) >> 7)
+
+#define PP_PIX_YUYV	0
+#define PP_PIX_YVYU	8
+#define PP_PIX_UYVY	16
+#define PP_PIX_VYUY	24
+
+/* PP size & width calculation macros */
+#define PP_CALC_QP_WIDTH(cfg)	\
+	(!((cfg)->operation & (EN_DEBLOCK | EN_DERING)) ? 0 : \
+		(((((cfg)->dim.in.width + 15) >> 4) + 3) & ~3))
+#define PP_CALC_Y_SIZE(cfg)		\
+	((cfg)->in_y_stride * (cfg)->dim.in.height)
+#define PP_CALC_CH_SIZE(cfg)		(PP_CALC_Y_SIZE(cfg) >> 2)
+#define PP_CALC_BPP(cfg) 		\
+	((cfg)->rgb_resolution > 16 ?  4 : ((cfg)->rgb_resolution >> 3))
+#define PP_CALC_YUV_SIZE(cfg)		\
+	((PP_CALC_Y_SIZE(cfg) * 3) >> 1)
+#define PP_CALC_QP_SIZE(cfg)		\
+	(PP_CALC_QP_WIDTH(cfg) * (((cfg)->dim.in.height + 15) >> 4))
+#define PP_CALC_DEST_WIDTH(cfg)	\
+	(((cfg)->out_stride & ~1) * PP_CALC_BPP(cfg))
+#define PP_CALC_DEST_SIZE(cfg)	\
+	((cfg)->dim.out.height * PP_CALC_DEST_WIDTH(cfg))
+
+/*
+ * physical addresses for bus mastering
+ * v=0 -> yuv packed
+ * v=0 & qp=0 -> yuv packed with qp appended
+ */
+typedef struct _emma_pp_ptr {
+	unsigned int y;		/* Y data (line align8) */
+	unsigned int u;		/* U data (line align4) */
+	unsigned int v;		/* V data (line align4) */
+	unsigned int qp;	/* Quantization (line align4) */
+} emma_pp_ptr;
+
+typedef struct _emma_pp_size {
+	int width;
+	int height;
+} emma_pp_size;
+
+/*
+ * if num.width != 0
+ * 	resize ratio = num.width : den.width
+ * else
+ * 	resize ratio = in.width : out.width
+ * same for height
+ */
+typedef struct _emma_pp_scale {
+	emma_pp_size num;
+	emma_pp_size den;
+	emma_pp_size in;	/* clip */
+	emma_pp_size out;	/* 0 -> same as in */
+} emma_pp_scale;
+
+typedef struct _emma_pp_cfg {
+	unsigned char operation;	/* OR of EN_xx defines */
+
+	/*
+	 * input color coeff
+	 * fixed pt 8 bits, steps of 1/128
+	 * csc[5] is 1 or 0 to indicate Y + 16
+	 * csc[0] is matrix id 0-3 while csc[1-5]=0
+	 */
+	unsigned short csc_table[6];
+
+	/*
+	 * Output color (shade width, shade offset, pixel resolution)
+	 * Eg. 16bpp RGB565 resolution, the values could be:
+	 * red_width = 5, green_width = 6, blue_width = 6
+	 * red_offset = 11, green_offset = 5, blue_offset = 0 (defaults)
+	 * rgb_resolution = 16 (default)
+	 * For YUV422: xxx_width=0, blue_offset=PP_PIX_xxx
+	 */
+	unsigned short red_width;
+	unsigned short green_width;
+	unsigned short blue_width;
+	/* if offsets are 0, the offsets are by width LSb to MSb B:G:R */
+	unsigned short red_offset;
+	unsigned short blue_offset;
+	unsigned short green_offset;
+	/* if resolution is 0, the minimum for the sum of widths is chosen */
+	short rgb_resolution;	/* 8,16,24 bpp only */
+
+	emma_pp_ptr ptr;	/* dma buffer pointers */
+	unsigned int outptr;	/* RGB/YUV output */
+	emma_pp_scale dim;	/* in/out dimensions */
+
+	/* pixels between two adjacent input Y rows */
+	unsigned short in_y_stride;	/* 0 = in_width */
+	/* PIXELS between two adjacent output rows */
+	unsigned short out_stride;	/* 0 = out_width */
+} emma_pp_cfg;
+
+int pp_ptr(unsigned long ptr);
+int pp_enable(int flag);
+int pp_cfg(vout_data * vout);
+int pp_init(vout_data * vout);
+int pp_num_last(void);
+void pp_exit(vout_data * vout);
+
+#endif				/* __MX27_PP_H__ */
diff --git a/drivers/media/video/mxc/output/mx27_v4l2_output.c b/drivers/media/video/mxc/output/mx27_v4l2_output.c
new file mode 100644
index 000000000000..a00f92d8e979
--- /dev/null
+++ b/drivers/media/video/mxc/output/mx27_v4l2_output.c
@@ -0,0 +1,1442 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file mx27_v4l2_output.c
+ *
+ * @brief MX27 V4L2 Video Output Driver
+ *
+ * Video4Linux2 Output Device using MX27 eMMA Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/semaphore.h>
+#include <linux/poll.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+
+#include "mxc_v4l2_output.h"
+#include "mx27_pp.h"
+#include "../drivers/video/mxc/mx2fb.h"
+
+#define SDC_FG_FB_FORMAT	V4L2_PIX_FMT_RGB565
+
+struct v4l2_output mxc_outputs[1] = {
+	{
+	 .index = 0,
+	 .name = "DISP0 Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN},
+};
+
+static int video_nr = 16;
+static spinlock_t g_lock = SPIN_LOCK_UNLOCKED;
+vout_data *g_vout;
+
+/* debug counters */
+uint32_t g_irq_cnt;
+uint32_t g_buf_output_cnt;
+uint32_t g_buf_q_cnt;
+uint32_t g_buf_dq_cnt;
+
+#ifdef CONFIG_VIDEO_MXC_OUTPUT_FBSYNC
+static uint32_t g_output_fb = -1;
+static uint32_t g_fb_enabled = 0;
+static uint32_t g_pp_ready = 0;
+
+static int fb_event_notify(struct notifier_block *self,
+			   unsigned long action, void *data)
+{
+	struct fb_event *event = data;
+	struct fb_info *info = event->info;
+	unsigned long lock_flags;
+	int blank, i;
+
+	for (i = 0; i < num_registered_fb; i++)
+		if (registered_fb[i] == info)
+			break;
+
+	/*
+	 * Check if the event is sent by the framebuffer in which
+	 * the video is displayed.
+	 */
+	if (i != g_output_fb)
+		return 0;
+
+	switch (action) {
+	case FB_EVENT_BLANK:
+		blank = *(int *)event->data;
+		spin_lock_irqsave(&g_lock, lock_flags);
+		g_fb_enabled = !blank;
+		if (blank && g_pp_ready) {
+			if (pp_enable(1))
+				pr_debug("unable to enable PP\n");
+			g_pp_ready = 0;
+		}
+		spin_unlock_irqrestore(&g_lock, lock_flags);
+		break;
+	case FB_EVENT_MXC_EOF:
+		spin_lock_irqsave(&g_lock, lock_flags);
+		g_fb_enabled = 1;
+		if (g_pp_ready) {
+			if (pp_enable(1))
+				pr_debug("unable to enable PP\n");
+			g_pp_ready = 0;
+		}
+		spin_unlock_irqrestore(&g_lock, lock_flags);
+		break;
+	}
+
+	return 0;
+}
+
+static struct notifier_block fb_event_notifier = {
+	.notifier_call = fb_event_notify,
+};
+
+static struct notifier_block mx2fb_event_notifier = {
+	.notifier_call = fb_event_notify,
+};
+#endif
+
+#define QUEUE_SIZE (MAX_FRAME_NUM + 1)
+static __inline int queue_size(v4l_queue * q)
+{
+	if (q->tail >= q->head)
+		return (q->tail - q->head);
+	else
+		return ((q->tail + QUEUE_SIZE) - q->head);
+}
+
+static __inline int queue_buf(v4l_queue * q, int idx)
+{
+	if (((q->tail + 1) % QUEUE_SIZE) == q->head)
+		return -1;	/* queue full */
+	q->list[q->tail] = idx;
+	q->tail = (q->tail + 1) % QUEUE_SIZE;
+	return 0;
+}
+
+static __inline int dequeue_buf(v4l_queue * q)
+{
+	int ret;
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	ret = q->list[q->head];
+	q->head = (q->head + 1) % QUEUE_SIZE;
+	return ret;
+}
+
+static __inline int peek_next_buf(v4l_queue * q)
+{
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	return q->list[q->head];
+}
+
+static __inline unsigned long get_jiffies(struct timeval *t)
+{
+	struct timeval cur;
+
+	if (t->tv_usec >= 1000000) {
+		t->tv_sec += t->tv_usec / 1000000;
+		t->tv_usec = t->tv_usec % 1000000;
+	}
+
+	do_gettimeofday(&cur);
+	if ((t->tv_sec < cur.tv_sec)
+	    || ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
+		return jiffies;
+
+	if (t->tv_usec < cur.tv_usec) {
+		cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
+		cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
+	} else {
+		cur.tv_sec = t->tv_sec - cur.tv_sec;
+		cur.tv_usec = t->tv_usec - cur.tv_usec;
+	}
+
+	return jiffies + timeval_to_jiffies(&cur);
+}
+
+/*!
+ * Private function to free buffers
+ *
+ * @param bufs_paddr	Array of physical address of buffers to be freed
+ *
+ * @param bufs_vaddr	Array of virtual address of buffers to be freed
+ *
+ * @param num_buf	Number of buffers to be freed
+ *
+ * @param size		Size for each buffer to be free
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+			    int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		if (bufs_vaddr[i] != 0) {
+			dma_free_coherent(0, size, bufs_vaddr[i],
+					  bufs_paddr[i]);
+			pr_debug("freed @ paddr=0x%08X\n", (u32) bufs_paddr[i]);
+			bufs_paddr[i] = 0;
+			bufs_vaddr[i] = NULL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Private function to allocate buffers
+ *
+ * @param bufs_paddr	Output array of physical address of buffers allocated
+ *
+ * @param bufs_vaddr	Output array of virtual address of buffers allocated
+ *
+ * @param num_buf	Input number of buffers to allocate
+ *
+ * @param size		Input size for each buffer to allocate
+ *
+ * @return status	-0 Successfully allocated a buffer, -ENOBUFS failed.
+ */
+static int mxc_allocate_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+				int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		bufs_vaddr[i] = dma_alloc_coherent(0, size,
+						   &bufs_paddr[i],
+						   GFP_DMA | GFP_KERNEL);
+
+		if (bufs_vaddr[i] == 0) {
+			mxc_free_buffers(bufs_paddr, bufs_vaddr, i, size);
+			pr_debug("dma_alloc_coherent failed.\n");
+			return -ENOBUFS;
+		}
+		pr_debug("allocated @ paddr=0x%08X, size=%d.\n",
+			 (u32) bufs_paddr[i], size);
+	}
+
+	return 0;
+}
+
+static void mxc_v4l2out_timer_handler(unsigned long arg)
+{
+	int index;
+	unsigned long timeout;
+	unsigned long lock_flags;
+	vout_data *vout = (vout_data *) arg;
+
+	pr_debug("timer handler: %lu\n", jiffies);
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	if ((vout->state == STATE_STREAM_OFF)
+	    || (vout->state == STATE_STREAM_STOPPING)) {
+		pr_debug("stream has stopped\n");
+		goto exit0;
+	}
+
+	/*
+	 * If timer occurs before PP h/w is ready, then set the state to
+	 * paused and the timer will be set again when next buffer is queued
+	 * or PP completes.
+	 */
+	if (vout->ipu_buf[0] != -1) {
+		pr_debug("buffer is busy\n");
+		vout->state = STATE_STREAM_PAUSED;
+		goto exit0;
+	}
+
+	/* Dequeue buffer and pass to PP */
+	index = dequeue_buf(&vout->ready_q);
+	if (index == -1) {	/* no buffers ready, should never occur */
+		pr_debug("mxc_v4l2out: timer - no queued buffers ready\n");
+		goto exit0;
+	}
+
+	g_buf_dq_cnt++;
+	vout->frame_count++;
+	vout->ipu_buf[0] = index;
+
+	if (pp_ptr((unsigned int)vout->queue_buf_paddr[index])) {
+		pr_debug("unable to update buffer\n");
+		goto exit0;
+	}
+#ifdef CONFIG_VIDEO_MXC_OUTPUT_FBSYNC
+	if (g_fb_enabled && (vout->v4l2_fb.flags != V4L2_FBUF_FLAG_OVERLAY))
+		g_pp_ready = 1;
+	else if (pp_enable(1)) {
+		pr_debug("unable to enable PP\n");
+		goto exit0;
+	}
+#else
+	if (pp_enable(1)) {
+		pr_debug("unable to enable PP\n");
+		goto exit0;
+	}
+#endif
+	pr_debug("enabled index %d\n", index);
+
+	/* Setup timer for next buffer */
+	index = peek_next_buf(&vout->ready_q);
+	pr_debug("next index %d\n", index);
+	if (index != -1) {
+		/* if timestamp is 0, then default to 30fps */
+		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+			timeout =
+			    vout->start_jiffies + vout->frame_count * HZ / 30;
+		else
+			timeout =
+			    get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+		if (jiffies >= timeout) {
+			pr_debug("warning: timer timeout already expired.\n");
+		}
+
+		if (mod_timer(&vout->output_timer, timeout))
+			pr_debug("warning: timer was already set\n");
+
+		pr_debug("timer handler next schedule: %lu\n", timeout);
+	} else {
+		vout->state = STATE_STREAM_PAUSED;
+		pr_debug("timer handler paused\n");
+	}
+
+      exit0:
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+}
+
+irqreturn_t mxc_v4l2out_pp_in_irq_handler(int irq, void *dev_id)
+{
+	int last_buf;
+	int index;
+	unsigned long timeout;
+	unsigned long lock_flags;
+	vout_data *vout = dev_id;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	g_irq_cnt++;
+
+	if ((vout->state == STATE_STREAM_OFF)
+	    || (vout->state == STATE_STREAM_STOPPING)) {
+		spin_unlock_irqrestore(&g_lock, lock_flags);
+		return IRQ_HANDLED;
+	}
+
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		struct fb_gwinfo gwinfo;
+
+		gwinfo.enabled = 1;
+		gwinfo.alpha_value = 255;
+		gwinfo.ck_enabled = 0;
+		gwinfo.xpos = vout->crop_current.left;
+		gwinfo.ypos = vout->crop_current.top;
+		gwinfo.base = (unsigned long)vout->display_bufs[pp_num_last()];
+		gwinfo.xres = vout->crop_current.width;
+		gwinfo.yres = vout->crop_current.height;
+		gwinfo.xres_virtual = vout->crop_current.width;
+		gwinfo.vs_reversed = 0;
+
+		mx2_gw_set(&gwinfo);
+	}
+
+	/* Process previous buffer */
+	last_buf = vout->ipu_buf[0];
+	pr_debug("last_buf %d g_irq_cnt %d\n", last_buf, g_irq_cnt);
+	if (last_buf != -1) {
+		g_buf_output_cnt++;
+		vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+		queue_buf(&vout->done_q, last_buf);
+		vout->ipu_buf[0] = -1;
+		wake_up_interruptible(&vout->v4l_bufq);
+	}
+
+	/* Setup timer for next buffer, when stream has been paused */
+	if ((vout->state == STATE_STREAM_PAUSED)
+	    && ((index = peek_next_buf(&vout->ready_q)) != -1)) {
+		pr_debug("next index %d\n", index);
+		/* if timestamp is 0, then default to 30fps */
+		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+			timeout =
+			    vout->start_jiffies + vout->frame_count * HZ / 30;
+		else
+			timeout =
+			    get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+		if (jiffies >= timeout) {
+			pr_debug("warning: timer timeout already expired.\n");
+		}
+
+		vout->state = STATE_STREAM_ON;
+
+		if (mod_timer(&vout->output_timer, timeout))
+			pr_debug("warning: timer was already set\n");
+
+		pr_debug("timer handler next schedule: %lu\n", timeout);
+	}
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ * Start the output stream
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamon(vout_data * vout)
+{
+	unsigned long timeout;
+	int index;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state != STATE_STREAM_OFF)
+		return -EBUSY;
+
+	if (queue_size(&vout->ready_q) < 1) {
+		pr_debug("no buffers queued yet!\n");
+		return -EINVAL;
+	}
+
+	vout->ipu_buf[0] = -1;
+
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		/* Free previously allocated buffer */
+		mxc_free_buffers(vout->display_bufs, vout->display_bufs_vaddr,
+				 2, vout->display_buf_size);
+		/* Allocate buffers for foreground */
+		if (mxc_allocate_buffers(vout->display_bufs,
+					 vout->display_bufs_vaddr, 2,
+					 vout->display_buf_size) < 0) {
+			pr_debug("unable to allocate SDC FG buffers\n");
+			return -ENOMEM;
+		}
+	}
+
+	/* Configure PP */
+	if (pp_cfg(vout)) {
+		/* Free previously allocated buffer */
+		mxc_free_buffers(vout->display_bufs, vout->display_bufs_vaddr,
+				 2, vout->display_buf_size);
+		pr_debug("failed to config PP.\n");
+		return -EINVAL;
+	}
+#ifdef CONFIG_VIDEO_MXC_OUTPUT_FBSYNC
+	g_output_fb = vout->output_fb_num[vout->cur_disp_output];
+	g_fb_enabled = 0;
+	g_pp_ready = 0;
+	fb_register_client(&fb_event_notifier);
+	mx2fb_register_client(&mx2fb_event_notifier);
+#endif
+	vout->frame_count = 0;
+	vout->state = STATE_STREAM_ON;
+	index = peek_next_buf(&vout->ready_q);
+
+	/* if timestamp is 0, then default to 30fps */
+	if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+	    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+		timeout = jiffies;
+	else
+		timeout = get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+	if (jiffies >= timeout) {
+		pr_debug("warning: timer timeout already expired.\n");
+	}
+
+	vout->start_jiffies = vout->output_timer.expires = timeout;
+	pr_debug("STREAMON:Add timer %d timeout @ %lu jiffies, current = %lu\n",
+		 index, timeout, jiffies);
+	add_timer(&vout->output_timer);
+
+	return 0;
+}
+
+/*!
+ * Shut down the voutera
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamoff(vout_data * vout)
+{
+	int i, retval = 0;
+	unsigned long lock_flag = 0;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state == STATE_STREAM_OFF) {
+		return 0;
+	}
+
+	spin_lock_irqsave(&g_lock, lock_flag);
+
+	del_timer(&vout->output_timer);
+	pp_enable(0);		/* Disable PP */
+
+	if (vout->state == STATE_STREAM_ON) {
+		vout->state = STATE_STREAM_STOPPING;
+	}
+
+	spin_unlock_irqrestore(&g_lock, lock_flag);
+
+	vout->ready_q.head = vout->ready_q.tail = 0;
+	vout->done_q.head = vout->done_q.tail = 0;
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		vout->v4l2_bufs[i].flags = 0;
+		vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+		vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+	}
+
+	vout->state = STATE_STREAM_OFF;
+
+	if (vout->v4l2_fb.flags == V4L2_FBUF_FLAG_OVERLAY) {
+		struct fb_gwinfo gwinfo;
+
+		/* Disable graphic window */
+		gwinfo.enabled = 0;
+		mx2_gw_set(&gwinfo);
+	}
+#ifdef CONFIG_VIDEO_MXC_OUTPUT_FBSYNC
+	g_output_fb = -1;
+	g_fb_enabled = 0;
+	g_pp_ready = 0;
+	fb_unregister_client(&fb_event_notifier);
+	mx2fb_unregister_client(&mx2fb_event_notifier);
+#endif
+
+	mxc_free_buffers(vout->display_bufs, vout->display_bufs_vaddr,
+			 2, vout->display_buf_size);
+
+	return retval;
+}
+
+/*
+ * Valid whether the palette is supported
+ *
+ * @param palette  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return 1 if supported, 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return (palette == V4L2_PIX_FMT_YUV420);
+}
+
+/*
+ * Returns bits per pixel for given pixel format
+ *
+ * @param pixelformat  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return bits per pixel of pixelformat
+ */
+static u32 fmt_to_bpp(u32 pixelformat)
+{
+	u32 bpp;
+
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_RGB565:
+		bpp = 16;
+		break;
+	case V4L2_PIX_FMT_BGR24:
+	case V4L2_PIX_FMT_RGB24:
+		bpp = 24;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_RGB32:
+		bpp = 32;
+		break;
+	default:
+		bpp = 8;
+		break;
+	}
+	return bpp;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_g_fmt(vout_data * vout, struct v4l2_format *f)
+{
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		return -EINVAL;
+	}
+	*f = vout->v2f;
+	return 0;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_s_fmt(vout_data * vout, struct v4l2_format *f)
+{
+	int retval = 0;
+	u32 size = 0;
+	u32 bytesperline;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		retval = -EINVAL;
+		goto err0;
+	}
+	if (!valid_mode(f->fmt.pix.pixelformat)) {
+		pr_debug("pixel format not supported\n");
+		retval = -EINVAL;
+		goto err0;
+	}
+
+	bytesperline = (f->fmt.pix.width * fmt_to_bpp(f->fmt.pix.pixelformat)) /
+	    8;
+	if (f->fmt.pix.bytesperline < bytesperline) {
+		f->fmt.pix.bytesperline = bytesperline;
+	} else {
+		bytesperline = f->fmt.pix.bytesperline;
+	}
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV422P:
+		/* byteperline for YUV planar formats is for
+		   Y plane only */
+		size = bytesperline * f->fmt.pix.height * 2;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+		size = (bytesperline * f->fmt.pix.height * 3) / 2;
+		break;
+	default:
+		size = bytesperline * f->fmt.pix.height;
+		break;
+	}
+
+	/* Return the actual size of the image to the app */
+	f->fmt.pix.sizeimage = size;
+
+	vout->v2f.fmt.pix.sizeimage = size;
+	vout->v2f.fmt.pix.width = f->fmt.pix.width;
+	vout->v2f.fmt.pix.height = f->fmt.pix.height;
+	vout->v2f.fmt.pix.pixelformat = f->fmt.pix.pixelformat;
+	vout->v2f.fmt.pix.bytesperline = f->fmt.pix.bytesperline;
+
+	retval = 0;
+      err0:
+	return retval;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_get_v42lout_control(vout_data * vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		return (vout->rotate & IPU_ROTATE_HORIZ_FLIP) ? 1 : 0;
+	case V4L2_CID_VFLIP:
+		return (vout->rotate & IPU_ROTATE_VERT_FLIP) ? 1 : 0;
+	case (V4L2_CID_PRIVATE_BASE + 1):
+		return vout->rotate;
+	default:
+		return -EINVAL;
+	}
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_set_v42lout_control(vout_data * vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+	case V4L2_CID_MXC_ROT:
+		return 0;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * V4L2 interface - open function
+ *
+ * @param inode        structure inode *
+ *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l2out_open(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+	int err;
+
+	if (!vout) {
+		pr_info("Internal error, vout_data not found!\n");
+		return -ENODEV;
+	}
+
+	down(&vout->busy_lock);
+
+	err = -EINTR;
+	if (signal_pending(current))
+		goto oops;
+
+	if (vout->open_count++ == 0) {
+		pp_init(vout);
+
+		init_waitqueue_head(&vout->v4l_bufq);
+
+		init_timer(&vout->output_timer);
+		vout->output_timer.function = mxc_v4l2out_timer_handler;
+		vout->output_timer.data = (unsigned long)vout;
+
+		vout->state = STATE_STREAM_OFF;
+		g_irq_cnt = g_buf_output_cnt = g_buf_q_cnt = g_buf_dq_cnt = 0;
+
+	}
+
+	file->private_data = dev;
+	up(&vout->busy_lock);
+	return 0;
+
+      oops:
+	up(&vout->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L2 interface - close function
+ *
+ * @param inode    struct inode *
+ *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l2out_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = file->private_data;
+	vout_data *vout = video_get_drvdata(dev);
+
+	if (--vout->open_count == 0) {
+		pr_debug("release resource\n");
+
+		pp_exit(vout);
+		if (vout->state != STATE_STREAM_OFF)
+			mxc_v4l2out_streamoff(vout);
+
+		file->private_data = NULL;
+
+		mxc_free_buffers(vout->queue_buf_paddr,
+				 vout->queue_buf_vaddr,
+				 vout->buffer_cnt, vout->queue_buf_size);
+		vout->buffer_cnt = 0;
+		mxc_free_buffers(vout->display_bufs,
+				 vout->display_bufs_vaddr,
+				 2, vout->display_buf_size);
+
+		/* capture off */
+		wake_up_interruptible(&vout->v4l_bufq);
+	}
+
+	return 0;
+}
+
+/*!
+ * V4L2 interface - ioctl function
+ *
+ * @param inode      struct inode *
+ *
+ * @param file       struct file *
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void *
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int
+mxc_v4l2out_do_ioctl(struct inode *inode, struct file *file,
+		     unsigned int ioctlnr, void *arg)
+{
+	struct video_device *dev = file->private_data;
+	vout_data *vout = video_get_drvdata(dev);
+	int retval = 0;
+	int i = 0;
+
+	if (!vout)
+		return -EBADF;
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+	case VIDIOC_QUERYCAP:
+		{
+			struct v4l2_capability *cap = arg;
+			strcpy(cap->driver, "mxc_v4l2_output");
+			cap->version = 0;
+			cap->capabilities =
+			    V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_FMT:
+		{
+			struct v4l2_format *gf = arg;
+			retval = mxc_v4l2out_g_fmt(vout, gf);
+			break;
+		}
+	case VIDIOC_S_FMT:
+		{
+			struct v4l2_format *sf = arg;
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+			retval = mxc_v4l2out_s_fmt(vout, sf);
+			break;
+		}
+	case VIDIOC_REQBUFS:
+		{
+			struct v4l2_requestbuffers *req = arg;
+			if ((req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (req->memory != V4L2_MEMORY_MMAP)) {
+				pr_debug
+				    ("VIDIOC_REQBUFS: incorrect buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			if (req->count == 0)
+				mxc_v4l2out_streamoff(vout);
+
+			if (vout->state == STATE_STREAM_OFF) {
+				if (vout->queue_buf_paddr[0] != 0) {
+					mxc_free_buffers(vout->queue_buf_paddr,
+							 vout->queue_buf_vaddr,
+							 vout->buffer_cnt,
+							 vout->queue_buf_size);
+					pr_debug
+					    ("VIDIOC_REQBUFS: freed buffers\n");
+				}
+				vout->buffer_cnt = 0;
+			} else {
+				pr_debug("VIDIOC_REQBUFS: Buffer is in use\n");
+				retval = -EBUSY;
+				break;
+			}
+
+			if (req->count == 0)
+				break;
+
+			if (req->count < MIN_FRAME_NUM) {
+				req->count = MIN_FRAME_NUM;
+			} else if (req->count > MAX_FRAME_NUM) {
+				req->count = MAX_FRAME_NUM;
+			}
+			vout->buffer_cnt = req->count;
+			vout->queue_buf_size =
+			    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+
+			retval = mxc_allocate_buffers(vout->queue_buf_paddr,
+						      vout->queue_buf_vaddr,
+						      vout->buffer_cnt,
+						      vout->queue_buf_size);
+			if (retval < 0)
+				break;
+
+			/* Init buffer queues */
+			vout->done_q.head = 0;
+			vout->done_q.tail = 0;
+			vout->ready_q.head = 0;
+			vout->ready_q.tail = 0;
+
+			for (i = 0; i < vout->buffer_cnt; i++) {
+				memset(&(vout->v4l2_bufs[i]), 0,
+				       sizeof(vout->v4l2_bufs[i]));
+				vout->v4l2_bufs[i].flags = 0;
+				vout->v4l2_bufs[i].memory = V4L2_MEMORY_MMAP;
+				vout->v4l2_bufs[i].index = i;
+				vout->v4l2_bufs[i].type =
+				    V4L2_BUF_TYPE_VIDEO_OUTPUT;
+				vout->v4l2_bufs[i].length =
+				    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+				vout->v4l2_bufs[i].m.offset =
+				    (unsigned long)vout->queue_buf_paddr[i];
+				vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+				vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+			}
+			break;
+		}
+	case VIDIOC_QUERYBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			u32 type = buf->type;
+			int index = buf->index;
+
+			if ((type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				pr_debug
+				    ("VIDIOC_QUERYBUFS: incorrect buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+			down(&vout->param_lock);
+			memcpy(buf, &(vout->v4l2_bufs[index]), sizeof(*buf));
+			up(&vout->param_lock);
+			break;
+		}
+	case VIDIOC_QBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+			unsigned long lock_flags;
+			unsigned long timeout;
+
+			if ((buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt) || (buf->flags != 0)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			pr_debug("VIDIOC_QBUF: %d\n", buf->index);
+
+			spin_lock_irqsave(&g_lock, lock_flags);
+
+			memcpy(&(vout->v4l2_bufs[index]), buf, sizeof(*buf));
+			vout->v4l2_bufs[index].flags |= V4L2_BUF_FLAG_QUEUED;
+
+			g_buf_q_cnt++;
+			queue_buf(&vout->ready_q, index);
+
+			if (vout->state == STATE_STREAM_PAUSED) {
+				index = peek_next_buf(&vout->ready_q);
+
+				/* if timestamp is 0, then default to 30fps */
+				if ((vout->v4l2_bufs[index].timestamp.tv_sec ==
+				     0)
+				    && (vout->v4l2_bufs[index].timestamp.
+					tv_usec == 0))
+					timeout =
+					    vout->start_jiffies +
+					    vout->frame_count * HZ / 30;
+				else
+					timeout =
+					    get_jiffies(&vout->v4l2_bufs[index].
+							timestamp);
+
+				if (jiffies >= timeout) {
+					pr_debug
+					    ("warning: timer timeout already expired.\n");
+				}
+
+				vout->output_timer.expires = timeout;
+				pr_debug
+				    ("QBUF:Add timer %d timeout @ %lu jiffies, "
+				     "current = %lu\n", index, timeout,
+				     jiffies);
+				add_timer(&vout->output_timer);
+				vout->state = STATE_STREAM_ON;
+			}
+
+			spin_unlock_irqrestore(&g_lock, lock_flags);
+			break;
+		}
+	case VIDIOC_DQBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int idx;
+
+			pr_debug("VIDIOC_DQBUF: q size = %d\n",
+				 queue_size(&vout->done_q));
+
+			if ((queue_size(&vout->done_q) == 0) &&
+			    (file->f_flags & O_NONBLOCK)) {
+				retval = -EAGAIN;
+				break;
+			}
+
+			if (!wait_event_interruptible_timeout(vout->v4l_bufq,
+							      queue_size(&vout->
+									 done_q)
+							      != 0, 10 * HZ)) {
+				pr_debug("VIDIOC_DQBUF: timeout\n");
+				retval = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				pr_debug("VIDIOC_DQBUF: interrupt received\n");
+				retval = -ERESTARTSYS;
+				break;
+			}
+			idx = dequeue_buf(&vout->done_q);
+			if (idx == -1) {	/* No frame free */
+				pr_debug
+				    ("VIDIOC_DQBUF: no free buffers, returning\n");
+				retval = -EAGAIN;
+				break;
+			}
+			if ((vout->v4l2_bufs[idx].flags & V4L2_BUF_FLAG_DONE) ==
+			    0)
+				pr_debug
+				    ("VIDIOC_DQBUF: buffer in done q, but not "
+				     "flagged as done\n");
+
+			vout->v4l2_bufs[idx].flags = 0;
+			memcpy(buf, &(vout->v4l2_bufs[idx]), sizeof(*buf));
+			pr_debug("VIDIOC_DQBUF: %d\n", buf->index);
+			break;
+		}
+	case VIDIOC_STREAMON:
+		{
+			retval = mxc_v4l2out_streamon(vout);
+			break;
+		}
+	case VIDIOC_STREAMOFF:
+		{
+			retval = mxc_v4l2out_streamoff(vout);
+			break;
+		}
+	case VIDIOC_G_CTRL:
+		{
+			retval = mxc_get_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_S_CTRL:
+		{
+			retval = mxc_set_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_CROPCAP:
+		{
+			struct v4l2_cropcap *cap = arg;
+
+			if (cap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			cap->bounds = vout->crop_bounds[vout->cur_disp_output];
+			cap->defrect = vout->crop_bounds[vout->cur_disp_output];
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			crop->c = vout->crop_current;
+			break;
+		}
+	case VIDIOC_S_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+			struct v4l2_rect *b =
+			    &(vout->crop_bounds[vout->cur_disp_output]);
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.height < 0) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.width < 0) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (crop->c.top < b->top)
+				crop->c.top = b->top;
+			if (crop->c.top > b->top + b->height)
+				crop->c.top = b->top + b->height;
+			if (crop->c.height > b->top - crop->c.top + b->height)
+				crop->c.height =
+				    b->top - crop->c.top + b->height;
+
+			if (crop->c.left < b->left)
+				crop->c.top = b->left;
+			if (crop->c.left > b->left + b->width)
+				crop->c.top = b->left + b->width;
+			if (crop->c.width > b->left - crop->c.left + b->width)
+				crop->c.width =
+				    b->left - crop->c.left + b->width;
+
+			/* stride line limitation */
+			crop->c.height -= crop->c.height % 8;
+			crop->c.width -= crop->c.width % 8;
+
+			vout->crop_current = crop->c;
+
+			vout->display_buf_size = vout->crop_current.width *
+			    vout->crop_current.height;
+			vout->display_buf_size *=
+			    fmt_to_bpp(SDC_FG_FB_FORMAT) / 8;
+			break;
+		}
+	case VIDIOC_ENUMOUTPUT:
+		{
+			struct v4l2_output *output = arg;
+
+			if ((output->index >= 2) ||
+			    (vout->output_enabled[output->index] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			*output = mxc_outputs[0];
+			output->name[4] = '0' + output->index;
+			break;
+		}
+	case VIDIOC_G_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			*p_output_num = vout->cur_disp_output;
+			break;
+		}
+	case VIDIOC_S_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			if ((*p_output_num >= 2) ||
+			    (vout->output_enabled[*p_output_num] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+
+			vout->cur_disp_output = *p_output_num;
+			break;
+		}
+	case VIDIOC_G_FBUF:
+		{
+			struct v4l2_framebuffer *fb = arg;
+			*fb = vout->v4l2_fb;
+			break;
+		}
+	case VIDIOC_S_FBUF:
+		{
+			struct v4l2_framebuffer *fb = arg;
+			vout->v4l2_fb = *fb;
+			vout->v4l2_fb.capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+			break;
+		}
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_G_PARM:
+	case VIDIOC_ENUMSTD:
+	case VIDIOC_G_STD:
+	case VIDIOC_S_STD:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&vout->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L2 interface - ioctl function
+ *
+ * @return  None
+ */
+static int
+mxc_v4l2out_ioctl(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l2out_do_ioctl);
+}
+
+/*!
+ * V4L2 interface - mmap function
+ *
+ * @param file          structure file *
+ *
+ * @param vma           structure vm_area_struct *
+ *
+ * @return status       0 Success, EINTR busy lock error,
+ *                      ENOBUFS remap_page error
+ */
+static int mxc_v4l2out_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *dev = file->private_data;
+	unsigned long start = vma->vm_start;
+	unsigned long size = vma->vm_end - vma->vm_start;
+	int res = 0;
+	vout_data *vout = video_get_drvdata(dev);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	/* make buffers write-thru cacheable */
+	vma->vm_page_prot = __pgprot(pgprot_val(vma->vm_page_prot) &
+				     ~L_PTE_BUFFERABLE);
+
+	if (remap_pfn_range(vma, start, vma->vm_pgoff, size, vma->vm_page_prot)) {
+		pr_debug("mxc_mmap(V4L)i - remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+      mxc_mmap_exit:
+	up(&vout->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L2 interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_v4l2out_poll(struct file *file, poll_table * wait)
+{
+	struct video_device *dev = file->private_data;
+	vout_data *vout = video_get_drvdata(dev);
+
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	queue = &vout->v4l_bufq;
+	poll_wait(file, queue, wait);
+
+	up(&vout->busy_lock);
+	return res;
+}
+
+static struct file_operations mxc_v4l2out_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l2out_open,
+	.release = mxc_v4l2out_close,
+	.ioctl = mxc_v4l2out_ioctl,
+	.mmap = mxc_v4l2out_mmap,
+	.poll = mxc_v4l2out_poll,
+};
+
+static struct video_device mxc_v4l2out_template = {
+	.name = "MXC Video Output",
+	.vfl_type = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING,
+	.fops = &mxc_v4l2out_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxc_v4l2out_probe(struct platform_device *pdev)
+{
+	int i;
+	vout_data *vout;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	g_vout = vout = kmalloc(sizeof(vout_data), GFP_KERNEL);
+
+	if (!vout)
+		return 0;
+
+	memset(vout, 0, sizeof(vout_data));
+
+	vout->video_dev = video_device_alloc();
+	if (vout->video_dev == NULL)
+		return -1;
+	vout->video_dev->minor = -1;
+
+	*(vout->video_dev) = mxc_v4l2out_template;
+
+	/* register v4l device */
+	if (video_register_device(vout->video_dev,
+				  VFL_TYPE_GRABBER, video_nr) == -1) {
+		pr_debug("video_register_device failed\n");
+		return 0;
+	}
+	pr_debug("mxc_v4l2out: registered device video%d\n",
+		 vout->video_dev->minor & 0x1f);
+
+	video_set_drvdata(vout->video_dev, vout);
+
+	init_MUTEX(&vout->param_lock);
+	init_MUTEX(&vout->busy_lock);
+
+	/* setup outputs and cropping */
+	vout->cur_disp_output = -1;
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strncmp(idstr, "DISP", 4) == 0) {
+			int disp_num = i;
+			vout->crop_bounds[disp_num].left = 0;
+			vout->crop_bounds[disp_num].top = 0;
+			vout->crop_bounds[disp_num].width =
+			    registered_fb[i]->var.xres;
+			vout->crop_bounds[disp_num].height =
+			    registered_fb[i]->var.yres;
+			vout->output_enabled[disp_num] = true;
+			vout->output_fb_num[disp_num] = i;
+			if (vout->cur_disp_output == -1)
+				vout->cur_disp_output = disp_num;
+		}
+
+	}
+	vout->crop_current = vout->crop_bounds[vout->cur_disp_output];
+
+	/* Setup framebuffer parameters */
+	vout->v4l2_fb.capability = V4L2_FBUF_CAP_EXTERNOVERLAY;
+	vout->v4l2_fb.flags = V4L2_FBUF_FLAG_PRIMARY;
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2out_driver = {
+	.driver = {
+		   .name = "MXC Video Output",
+		   .owner = THIS_MODULE,
+		   .bus = &platform_bus_type,
+		   },
+	.probe = mxc_v4l2out_probe,
+	.remove = NULL,
+};
+
+static void camera_platform_release(struct device *device)
+{
+}
+
+static struct platform_device mxc_v4l2out_device = {
+	.name = "MXC Video Output",
+	.dev = {
+		.release = camera_platform_release,
+		},
+	.id = 0,
+};
+
+/*!
+ * mxc v4l2 init function
+ *
+ */
+static int mxc_v4l2out_init(void)
+{
+	u8 err = 0;
+
+	err = platform_driver_register(&mxc_v4l2out_driver);
+	if (err == 0) {
+		platform_device_register(&mxc_v4l2out_device);
+	}
+	return err;
+}
+
+/*!
+ * mxc v4l2 cleanup function
+ *
+ */
+static void mxc_v4l2out_clean(void)
+{
+	pr_debug("unregistering video\n");
+
+	video_unregister_device(g_vout->video_dev);
+
+	platform_driver_unregister(&mxc_v4l2out_driver);
+	platform_device_unregister(&mxc_v4l2out_device);
+	kfree(g_vout);
+	g_vout = NULL;
+}
+
+module_init(mxc_v4l2out_init);
+module_exit(mxc_v4l2out_clean);
+
+module_param(video_nr, int, 0444);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2-driver for MXC video output");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/output/mx31_v4l2_wvga_output.c b/drivers/media/video/mxc/output/mx31_v4l2_wvga_output.c
new file mode 100644
index 000000000000..4a82da2d7d00
--- /dev/null
+++ b/drivers/media/video/mxc/output/mx31_v4l2_wvga_output.c
@@ -0,0 +1,1926 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/media/video/mxc/output/mxc_v4l2_output.c
+ *
+ * @brief MXC V4L2 Video Output Driver
+ *
+ * Video4Linux2 Output Device using MXC IPU Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/console.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <asm/cacheflush.h>
+#include <asm/io.h>
+#include <asm/semaphore.h>
+#include <linux/dma-mapping.h>
+
+#include <mach/mxcfb.h>
+#include <mach/ipu.h>
+
+#include "mxc_v4l2_output.h"
+
+vout_data *g_vout;
+#define SDC_FG_FB_FORMAT        IPU_PIX_FMT_RGB565
+
+struct v4l2_output mxc_outputs[2] = {
+	{
+	 .index = MXC_V4L2_OUT_2_SDC,
+	 .name = "DISP3 Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN},
+	{
+	 .index = MXC_V4L2_OUT_2_ADC,
+	 .name = "DISPx Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN}
+};
+
+static int video_nr = 16;
+static DEFINE_SPINLOCK(g_lock);
+static unsigned int g_pp_out_number;
+static unsigned int g_pp_in_number;
+
+/* debug counters */
+uint32_t g_irq_cnt;
+uint32_t g_buf_output_cnt;
+uint32_t g_buf_q_cnt;
+uint32_t g_buf_dq_cnt;
+
+static inline uint32_t channel_2_dma(ipu_channel_t ch, ipu_buffer_t type)
+{
+	return ((type == IPU_INPUT_BUFFER) ? ((uint32_t) ch & 0xFF) :
+		((type == IPU_OUTPUT_BUFFER) ? (((uint32_t) ch >> 8) & 0xFF)
+		 : (((uint32_t) ch >> 16) & 0xFF)));
+};
+
+static inline uint32_t DMAParamAddr(uint32_t dma_ch)
+{
+	return (0x10000 | (dma_ch << 4));
+};
+
+#define QUEUE_SIZE (MAX_FRAME_NUM + 1)
+static inline int queue_size(v4l_queue * q)
+{
+	if (q->tail >= q->head)
+		return (q->tail - q->head);
+	else
+		return ((q->tail + QUEUE_SIZE) - q->head);
+}
+
+static inline int queue_buf(v4l_queue * q, int idx)
+{
+	if (((q->tail + 1) % QUEUE_SIZE) == q->head)
+		return -1;	/* queue full */
+	q->list[q->tail] = idx;
+	q->tail = (q->tail + 1) % QUEUE_SIZE;
+	return 0;
+}
+
+static inline int dequeue_buf(v4l_queue * q)
+{
+	int ret;
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	ret = q->list[q->head];
+	q->head = (q->head + 1) % QUEUE_SIZE;
+	return ret;
+}
+
+static inline int peek_next_buf(v4l_queue * q)
+{
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	return q->list[q->head];
+}
+
+static inline unsigned long get_jiffies(struct timeval *t)
+{
+	struct timeval cur;
+
+	if (t->tv_usec >= 1000000) {
+		t->tv_sec += t->tv_usec / 1000000;
+		t->tv_usec = t->tv_usec % 1000000;
+	}
+
+	do_gettimeofday(&cur);
+	if ((t->tv_sec < cur.tv_sec)
+	    || ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
+		return jiffies;
+
+	if (t->tv_usec < cur.tv_usec) {
+		cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
+		cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
+	} else {
+		cur.tv_sec = t->tv_sec - cur.tv_sec;
+		cur.tv_usec = t->tv_usec - cur.tv_usec;
+	}
+
+	return jiffies + timeval_to_jiffies(&cur);
+}
+
+/*!
+ * Private function to free buffers
+ *
+ * @param bufs_paddr	Array of physical address of buffers to be freed
+ *
+ * @param bufs_vaddr	Array of virtual address of buffers to be freed
+ *
+ * @param num_buf	Number of buffers to be freed
+ *
+ * @param size		Size for each buffer to be free
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+			    int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		if (bufs_vaddr[i] != 0) {
+			dma_free_coherent(0, size, bufs_vaddr[i],
+					  bufs_paddr[i]);
+			pr_debug("freed @ paddr=0x%08X\n", (u32) bufs_paddr[i]);
+			bufs_paddr[i] = 0;
+			bufs_vaddr[i] = NULL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Private function to allocate buffers
+ *
+ * @param bufs_paddr	Output array of physical address of buffers allocated
+ *
+ * @param bufs_vaddr	Output array of virtual address of buffers allocated
+ *
+ * @param num_buf	Input number of buffers to allocate
+ *
+ * @param size		Input size for each buffer to allocate
+ *
+ * @return status	-0 Successfully allocated a buffer, -ENOBUFS failed.
+ */
+static int mxc_allocate_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+				int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		bufs_vaddr[i] = dma_alloc_coherent(0, size,
+						   &bufs_paddr[i],
+						   GFP_DMA | GFP_KERNEL);
+
+		if (bufs_vaddr[i] == 0) {
+			mxc_free_buffers(bufs_paddr, bufs_vaddr, i, size);
+			printk(KERN_ERR "dma_alloc_coherent failed.\n");
+			return -ENOBUFS;
+		}
+		pr_debug("allocated @ paddr=0x%08X, size=%d.\n",
+			 (u32) bufs_paddr[i], size);
+	}
+
+	return 0;
+}
+
+/*
+ * Returns bits per pixel for given pixel format
+ *
+ * @param pixelformat  V4L2_PIX_FMT_RGB565,
+ * 		       V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return bits per pixel of pixelformat
+ */
+static u32 fmt_to_bpp(u32 pixelformat)
+{
+	u32 bpp;
+
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_RGB565:
+		bpp = 16;
+		break;
+	case V4L2_PIX_FMT_BGR24:
+	case V4L2_PIX_FMT_RGB24:
+		bpp = 24;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_RGB32:
+		bpp = 32;
+		break;
+	default:
+		bpp = 8;
+		break;
+	}
+	return bpp;
+}
+
+static u32 bpp_to_fmt(struct fb_info *fbi)
+{
+	if (fbi->var.nonstd)
+		return fbi->var.nonstd;
+
+	if (fbi->var.bits_per_pixel == 24)
+		return V4L2_PIX_FMT_BGR24;
+	else if (fbi->var.bits_per_pixel == 32)
+		return V4L2_PIX_FMT_BGR32;
+	else if (fbi->var.bits_per_pixel == 16)
+		return V4L2_PIX_FMT_RGB565;
+
+	return 0;
+}
+
+static void mxc_v4l2out_timer_handler(unsigned long arg)
+{
+	int index;
+	unsigned long timeout;
+	unsigned long lock_flags = 0;
+	vout_data *vout = (vout_data *) arg;
+
+	dev_dbg(vout->video_dev->dev, "timer handler: %lu\n", jiffies);
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	if ((vout->state == STATE_STREAM_STOPPING)
+	    || (vout->state == STATE_STREAM_OFF))
+		goto exit0;
+	/*
+	 * If timer occurs before IPU h/w is ready, then set the state to
+	 * paused and the timer will be set again when next buffer is queued
+	 * or PP comletes
+	 */
+	if (vout->ipu_buf[0] != -1) {
+		dev_dbg(vout->video_dev->dev, "IPU buffer busy\n");
+		vout->state = STATE_STREAM_PAUSED;
+		goto exit0;
+	}
+
+	/* One frame buffer should be ready here */
+	if (vout->frame_count % 2 == 1) {
+		/* set BUF0 rdy */
+		if (ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 0) <
+		    0)
+			pr_debug("error selecting display buf 0");
+	} else {
+		if (ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 1) <
+		    0)
+			pr_debug("error selecting display buf 1");
+	}
+
+	/* Dequeue buffer and pass to IPU */
+	index = dequeue_buf(&vout->ready_q);
+	if (index == -1) {	/* no buffers ready, should never occur */
+		dev_err(vout->video_dev->dev,
+			"mxc_v4l2out: timer - no queued buffers ready\n");
+		goto exit0;
+	}
+
+	g_buf_dq_cnt++;
+	vout->frame_count++;
+	vout->ipu_buf[1] = vout->ipu_buf[0] = index;
+
+	if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+				      0,
+				      vout->v4l2_bufs[vout->ipu_buf[0]].m.
+				      offset) < 0)
+		goto exit0;
+
+	if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+				      1,
+				      vout->v4l2_bufs[vout->ipu_buf[0]].m.
+				      offset + vout->v2f.fmt.pix.width / 2) < 0)
+		goto exit0;
+
+	/* All buffer should now ready in IPU out, tranfer to display buf */
+	if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
+				      0,
+				      vout->
+				      display_bufs[(vout->frame_count -
+						    1) % 2]) < 0) {
+		dev_err(vout->video_dev->dev,
+			"unable to update buffer %d address\n",
+			vout->next_rdy_ipu_buf);
+		goto exit0;
+	}
+	if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
+				      1,
+				      vout->
+				      display_bufs[(vout->frame_count -
+						    1) % 2] +
+				      vout->crop_current.width / 2 *
+				      bytes_per_pixel(SDC_FG_FB_FORMAT)) < 0) {
+		dev_err(vout->video_dev->dev,
+			"unable to update buffer %d address\n",
+			vout->next_rdy_ipu_buf);
+		goto exit0;
+	}
+
+	if (ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0) < 0) {
+		dev_err(vout->video_dev->dev,
+			"unable to set IPU buffer ready\n");
+		goto exit0;
+	}
+
+	if (ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0) < 0) {
+		dev_err(vout->video_dev->dev,
+			"unable to set IPU buffer ready\n");
+		goto exit0;
+	}
+
+	/* Setup timer for next buffer */
+	index = peek_next_buf(&vout->ready_q);
+	if (index != -1) {
+		/* if timestamp is 0, then default to 30fps */
+		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+			timeout =
+			    vout->start_jiffies + vout->frame_count * HZ / 30;
+		else
+			timeout =
+			    get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+		if (jiffies >= timeout) {
+			dev_dbg(vout->video_dev->dev,
+				"warning: timer timeout already expired.\n");
+		}
+		if (mod_timer(&vout->output_timer, timeout))
+			dev_dbg(vout->video_dev->dev,
+				"warning: timer was already set\n");
+
+		dev_dbg(vout->video_dev->dev,
+			"timer handler next schedule: %lu\n", timeout);
+	} else {
+		vout->state = STATE_STREAM_PAUSED;
+	}
+
+exit0:
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+}
+
+extern void _ipu_write_param_mem(uint32_t addr, uint32_t *data,
+				 uint32_t numWords);
+
+static irqreturn_t mxc_v4l2out_pp_in_irq_handler(int irq, void *dev_id)
+{
+	unsigned long lock_flags = 0;
+	vout_data *vout = dev_id;
+	uint32_t u_offset;
+	uint32_t v_offset;
+	uint32_t local_params[4];
+	uint32_t width, height;
+	uint32_t dma_chan;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+	g_irq_cnt++;
+
+	dma_chan = channel_2_dma(vout->post_proc_ch, IPU_INPUT_BUFFER);
+	memset(&local_params, 0, sizeof(local_params));
+
+	if (g_pp_in_number % 2 == 1) {
+		u_offset = vout->offset.u_offset - vout->v2f.fmt.pix.width / 4;
+		v_offset = vout->offset.v_offset - vout->v2f.fmt.pix.width / 4;
+		width = vout->v2f.fmt.pix.width / 2;
+		height = vout->v2f.fmt.pix.height;
+		local_params[3] =
+		    (uint32_t) ((width - 1) << 12) | ((uint32_t) (height -
+								  1) << 24);
+		local_params[1] = (1UL << (46 - 32)) | (u_offset << (53 - 32));
+		local_params[2] = u_offset >> (64 - 53);
+		local_params[2] |= v_offset << (79 - 64);
+		local_params[3] |= v_offset >> (96 - 79);
+		_ipu_write_param_mem(DMAParamAddr(dma_chan), local_params, 4);
+
+		if (ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1) <
+		    0) {
+			dev_err(vout->video_dev->dev,
+				"unable to set IPU buffer ready\n");
+		}
+	} else {
+		u_offset = vout->offset.u_offset;
+		v_offset = vout->offset.v_offset;
+		width = vout->v2f.fmt.pix.width / 2;
+		height = vout->v2f.fmt.pix.height;
+		local_params[3] =
+		    (uint32_t) ((width - 1) << 12) | ((uint32_t) (height -
+								  1) << 24);
+		local_params[1] = (1UL << (46 - 32)) | (u_offset << (53 - 32));
+		local_params[2] = u_offset >> (64 - 53);
+		local_params[2] |= v_offset << (79 - 64);
+		local_params[3] |= v_offset >> (96 - 79);
+		_ipu_write_param_mem(DMAParamAddr(dma_chan), local_params, 4);
+	}
+	g_pp_in_number++;
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t mxc_v4l2out_pp_out_irq_handler(int irq, void *dev_id)
+{
+	vout_data *vout = dev_id;
+	int index;
+	unsigned long timeout;
+	u32 lock_flags = 0;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	if (g_pp_out_number % 2 == 1) {
+		if (ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 1)
+		    < 0) {
+			dev_err(vout->video_dev->dev,
+				"unable to set IPU buffer ready\n");
+		}
+	} else {
+		if (vout->ipu_buf[0] != -1) {
+			vout->v4l2_bufs[vout->ipu_buf[0]].flags =
+			    V4L2_BUF_FLAG_DONE;
+			queue_buf(&vout->done_q, vout->ipu_buf[0]);
+			wake_up_interruptible(&vout->v4l_bufq);
+			vout->ipu_buf[0] = -1;
+		}
+		if (vout->state == STATE_STREAM_STOPPING) {
+			if ((vout->ipu_buf[0] == -1)
+			    && (vout->ipu_buf[1] == -1))
+				vout->state = STATE_STREAM_OFF;
+		} else if ((vout->state == STATE_STREAM_PAUSED)
+			   && ((index = peek_next_buf(&vout->ready_q)) != -1)) {
+			/*!
+			 * Setup timer for next buffer,
+			 * when stream has been paused
+			 */
+			pr_debug("next index %d\n", index);
+
+			/* if timestamp is 0, then default to 30fps */
+			if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+			    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+				timeout =
+				    vout->start_jiffies +
+				    vout->frame_count * HZ / 30;
+			else
+				timeout =
+				    get_jiffies(&vout->v4l2_bufs[index].
+						timestamp);
+
+			if (jiffies >= timeout) {
+				pr_debug
+				    ("warning: timer timeout"
+				     "already expired.\n");
+			}
+
+			vout->state = STATE_STREAM_ON;
+
+			if (mod_timer(&vout->output_timer, timeout))
+				pr_debug("warning: timer was already set\n");
+
+			pr_debug("timer handler next schedule: %lu\n", timeout);
+		}
+	}
+	g_pp_out_number++;
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+	return IRQ_HANDLED;
+}
+
+/*!
+ * Start the output stream
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamon(vout_data *vout)
+{
+	struct device *dev = vout->video_dev->dev;
+	ipu_channel_params_t params;
+	struct mxcfb_pos fb_pos;
+	struct fb_var_screeninfo fbvar;
+	struct fb_info *fbi =
+	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	int pp_in_buf[2];
+	u16 out_width;
+	u16 out_height;
+	ipu_channel_t display_input_ch = MEM_PP_MEM;
+	bool use_direct_adc = false;
+	mm_segment_t old_fs;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state != STATE_STREAM_OFF)
+		return -EBUSY;
+
+	if (queue_size(&vout->ready_q) < 2) {
+		dev_err(dev, "2 buffers not been queued yet!\n");
+		return -EINVAL;
+	}
+
+	out_width = vout->crop_current.width;
+	out_height = vout->crop_current.height;
+
+	vout->next_done_ipu_buf = vout->next_rdy_ipu_buf = 0;
+	vout->ipu_buf[0] = pp_in_buf[0] = dequeue_buf(&vout->ready_q);
+	vout->ipu_buf[1] = pp_in_buf[1] = vout->ipu_buf[0];
+	vout->frame_count = 1;
+	g_pp_out_number = 1;
+	g_pp_in_number = 1;
+
+	ipu_enable_irq(IPU_IRQ_PP_IN_EOF);
+	ipu_enable_irq(IPU_IRQ_PP_OUT_EOF);
+
+	/* Init Display Channel */
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL
+	if (vout->cur_disp_output < DISP3) {
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_OFF, 0);
+		fbi = NULL;
+		if (ipu_can_rotate_in_place(vout->rotate)) {
+			dev_dbg(dev, "Using PP direct to ADC channel\n");
+			use_direct_adc = true;
+			vout->display_ch = MEM_PP_ADC;
+			vout->post_proc_ch = MEM_PP_ADC;
+
+			memset(&params, 0, sizeof(params));
+			params.mem_pp_adc.in_width = vout->v2f.fmt.pix.width;
+			params.mem_pp_adc.in_height = vout->v2f.fmt.pix.height;
+			params.mem_pp_adc.in_pixel_fmt =
+			    vout->v2f.fmt.pix.pixelformat;
+			params.mem_pp_adc.out_width = out_width;
+			params.mem_pp_adc.out_height = out_height;
+			params.mem_pp_adc.out_pixel_fmt = SDC_FG_FB_FORMAT;
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+			params.mem_pp_adc.out_left =
+			    2 + vout->crop_current.left;
+#else
+			params.mem_pp_adc.out_left =
+			    12 + vout->crop_current.left;
+#endif
+			params.mem_pp_adc.out_top = vout->crop_current.top;
+			if (ipu_init_channel(
+				vout->post_proc_ch, &params) != 0) {
+				dev_err(dev, "Error initializing PP chan\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_INPUT_BUFFER,
+						    params.mem_pp_adc.
+						    in_pixel_fmt,
+						    params.mem_pp_adc.in_width,
+						    params.mem_pp_adc.in_height,
+						    vout->v2f.fmt.pix.
+						    bytesperline /
+						    bytes_per_pixel(params.
+							mem_pp_adc.
+							in_pixel_fmt),
+						    vout->rotate,
+						    vout->
+						    v4l2_bufs[pp_in_buf[0]].m.
+						    offset,
+						    vout->
+						    v4l2_bufs[pp_in_buf[1]].m.
+						    offset,
+						    vout->offset.u_offset,
+						    vout->offset.v_offset) !=
+			    0) {
+				dev_err(dev, "Error initializing PP in buf\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_adc.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    vout->rotate, 0, 0, 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP"
+					"output buffer\n");
+				return -EINVAL;
+			}
+
+		} else {
+			dev_dbg(dev, "Using ADC SYS2 channel\n");
+			vout->display_ch = ADC_SYS2;
+			vout->post_proc_ch = MEM_PP_MEM;
+
+			if (vout->display_bufs[0]) {
+				mxc_free_buffers(vout->display_bufs,
+						 vout->display_bufs_vaddr,
+						 2, vout->display_buf_size);
+			}
+
+			vout->display_buf_size = vout->crop_current.width *
+			    vout->crop_current.height *
+			    fmt_to_bpp(SDC_FG_FB_FORMAT) / 8;
+			mxc_allocate_buffers(vout->display_bufs,
+					     vout->display_bufs_vaddr,
+					     2, vout->display_buf_size);
+
+			memset(&params, 0, sizeof(params));
+			params.adc_sys2.disp = vout->cur_disp_output;
+			params.adc_sys2.ch_mode = WriteTemplateNonSeq;
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+			params.adc_sys2.out_left = 2 + vout->crop_current.left;
+#else
+			params.adc_sys2.out_left = 12 + vout->crop_current.left;
+#endif
+			params.adc_sys2.out_top = vout->crop_current.top;
+			if (ipu_init_channel(ADC_SYS2, &params) < 0)
+				return -EINVAL;
+
+			if (ipu_init_channel_buffer(vout->display_ch,
+						    IPU_INPUT_BUFFER,
+						    SDC_FG_FB_FORMAT,
+						    out_width, out_height,
+						    out_width, IPU_ROTATE_NONE,
+						    vout->display_bufs[0],
+						    vout->display_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing SDC FG buffer\n");
+				return -EINVAL;
+			}
+		}
+	} else
+#endif
+	{			/* Use SDC */
+		dev_dbg(dev, "Using SDC channel\n");
+
+		fbvar = fbi->var;
+		if (vout->cur_disp_output == 3) {
+			vout->display_ch = MEM_FG_SYNC;
+			fbvar.bits_per_pixel = 16;
+			fbvar.nonstd = IPU_PIX_FMT_UYVY;
+
+			fbvar.xres = fbvar.xres_virtual = out_width;
+			fbvar.yres = out_height;
+			fbvar.yres_virtual = out_height * 2;
+		} else if (vout->cur_disp_output == 5) {
+			vout->display_ch = MEM_DC_SYNC;
+			fbvar.bits_per_pixel = 16;
+			fbvar.nonstd = IPU_PIX_FMT_UYVY;
+
+			fbvar.xres = fbvar.xres_virtual = out_width;
+			fbvar.yres = out_height;
+			fbvar.yres_virtual = out_height * 2;
+		} else {
+			vout->display_ch = MEM_BG_SYNC;
+		}
+
+		fbvar.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbvar);
+
+		fb_pos.x = vout->crop_current.left;
+		fb_pos.y = vout->crop_current.top;
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
+					     (unsigned long)&fb_pos);
+			set_fs(old_fs);
+		}
+
+		vout->display_bufs[1] = fbi->fix.smem_start;
+		vout->display_bufs[0] = fbi->fix.smem_start +
+		    (fbi->fix.line_length * fbi->var.yres);
+		vout->display_buf_size = vout->crop_current.width *
+		    vout->crop_current.height * fbi->var.bits_per_pixel / 8;
+
+		vout->post_proc_ch = MEM_PP_MEM;
+	}
+
+	/* Init PP */
+	if (use_direct_adc == false) {
+		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+			out_width = vout->crop_current.height;
+			out_height = vout->crop_current.width;
+		}
+		memset(&params, 0, sizeof(params));
+		params.mem_pp_mem.in_width = vout->v2f.fmt.pix.width / 2;
+		params.mem_pp_mem.in_height = vout->v2f.fmt.pix.height;
+		params.mem_pp_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
+		params.mem_pp_mem.out_width = out_width / 2;
+		params.mem_pp_mem.out_height = out_height;
+		if (vout->display_ch == ADC_SYS2)
+			params.mem_pp_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
+		else
+			params.mem_pp_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+		if (ipu_init_channel(vout->post_proc_ch, &params) != 0) {
+			dev_err(dev, "Error initializing PP channel\n");
+			return -EINVAL;
+		}
+
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_INPUT_BUFFER,
+					    params.mem_pp_mem.in_pixel_fmt,
+					    params.mem_pp_mem.in_width,
+					    params.mem_pp_mem.in_height,
+					    vout->v2f.fmt.pix.bytesperline /
+					    bytes_per_pixel(params.mem_pp_mem.
+							    in_pixel_fmt),
+					    IPU_ROTATE_NONE,
+					    vout->v4l2_bufs[pp_in_buf[0]].m.
+					    offset,
+					    vout->v4l2_bufs[pp_in_buf[0]].m.
+					    offset + params.mem_pp_mem.in_width,
+					    vout->offset.u_offset,
+					    vout->offset.v_offset) != 0) {
+			dev_err(dev, "Error initializing PP input buffer\n");
+			return -EINVAL;
+		}
+
+		if (!ipu_can_rotate_in_place(vout->rotate)) {
+			if (vout->rot_pp_bufs[0]) {
+				mxc_free_buffers(vout->rot_pp_bufs,
+						 vout->rot_pp_bufs_vaddr, 2,
+						 vout->display_buf_size);
+			}
+			if (mxc_allocate_buffers
+			    (vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+			     vout->display_buf_size) < 0)
+				return -ENOBUFS;
+
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_mem.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    IPU_ROTATE_NONE,
+						    vout->rot_pp_bufs[0],
+						    vout->rot_pp_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing"
+					"PP output buffer\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel(MEM_ROT_PP_MEM, NULL) != 0) {
+				dev_err(dev,
+					"Error initializing PP ROT channel\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel_buffer(MEM_ROT_PP_MEM,
+						    IPU_INPUT_BUFFER,
+						    params.mem_pp_mem.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    vout->rotate,
+						    vout->rot_pp_bufs[0],
+						    vout->rot_pp_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP ROT"
+					"input buffer\n");
+				return -EINVAL;
+			}
+
+			/* swap width and height */
+			if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+				out_width = vout->crop_current.width;
+				out_height = vout->crop_current.height;
+			}
+
+			if (ipu_init_channel_buffer(MEM_ROT_PP_MEM,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_mem.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    IPU_ROTATE_NONE,
+						    vout->display_bufs[0],
+						    vout->display_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP"
+					"output buffer\n");
+				return -EINVAL;
+			}
+
+			if (ipu_link_channels(vout->post_proc_ch,
+					      MEM_ROT_PP_MEM) < 0)
+				return -EINVAL;
+
+			ipu_select_buffer(MEM_ROT_PP_MEM, IPU_OUTPUT_BUFFER, 0);
+			ipu_select_buffer(MEM_ROT_PP_MEM, IPU_OUTPUT_BUFFER, 1);
+
+			ipu_enable_channel(MEM_ROT_PP_MEM);
+
+			display_input_ch = MEM_ROT_PP_MEM;
+		} else {
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_mem.
+						    out_pixel_fmt,
+						    out_width / 2,
+						    out_height,
+						    out_width,
+						    vout->rotate,
+						    vout->display_bufs[0],
+						    vout->display_bufs[0]
+						    +
+						    out_width / 2 *
+						    bytes_per_pixel
+						    (SDC_FG_FB_FORMAT), 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP"
+					"output buffer\n");
+				return -EINVAL;
+			}
+		}
+		if (ipu_unlink_channels(
+			display_input_ch, vout->display_ch) < 0) {
+			dev_err(dev, "Error linking ipu channels\n");
+			return -EINVAL;
+		}
+	}
+
+	vout->state = STATE_STREAM_PAUSED;
+
+	ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+
+	if (use_direct_adc == false) {
+		ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0);
+		ipu_enable_channel(vout->post_proc_ch);
+
+		if (fbi) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_UNBLANK);
+			release_console_sem();
+		} else {
+			ipu_enable_channel(vout->display_ch);
+		}
+	} else {
+		ipu_enable_channel(vout->post_proc_ch);
+	}
+
+	vout->start_jiffies = jiffies;
+	dev_dbg(dev,
+		"streamon: start time = %lu jiffies\n", vout->start_jiffies);
+
+	return 0;
+}
+
+/*!
+ * Shut down the voutera
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamoff(vout_data *vout)
+{
+	struct fb_info *fbi =
+	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	int i, retval = 0;
+	unsigned long lockflag = 0;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state == STATE_STREAM_OFF)
+		return 0;
+
+	spin_lock_irqsave(&g_lock, lockflag);
+
+	del_timer(&vout->output_timer);
+
+	if (vout->state == STATE_STREAM_ON)
+		vout->state = STATE_STREAM_STOPPING;
+
+	ipu_disable_irq(IPU_IRQ_PP_IN_EOF);
+	ipu_disable_irq(IPU_IRQ_PP_OUT_EOF);
+
+	spin_unlock_irqrestore(&g_lock, lockflag);
+
+	if (vout->post_proc_ch == MEM_PP_MEM) {	/* SDC or ADC with Rotation */
+		if (!ipu_can_rotate_in_place(vout->rotate)) {
+			ipu_unlink_channels(MEM_PP_MEM, MEM_ROT_PP_MEM);
+			ipu_unlink_channels(MEM_ROT_PP_MEM, vout->display_ch);
+			ipu_disable_channel(MEM_ROT_PP_MEM, true);
+
+			if (vout->rot_pp_bufs[0]) {
+				mxc_free_buffers(vout->rot_pp_bufs,
+						 vout->rot_pp_bufs_vaddr, 2,
+						 vout->display_buf_size);
+			}
+		} else {
+			ipu_unlink_channels(MEM_PP_MEM, vout->display_ch);
+		}
+		ipu_disable_channel(MEM_PP_MEM, true);
+
+		if (vout->display_ch == ADC_SYS2) {
+			ipu_disable_channel(vout->display_ch, true);
+			ipu_uninit_channel(vout->display_ch);
+		} else {
+			fbi->var.activate |= FB_ACTIVATE_FORCE;
+			fb_set_var(fbi, &fbi->var);
+
+			if (vout->display_ch == MEM_FG_SYNC) {
+				acquire_console_sem();
+				fb_blank(fbi, FB_BLANK_POWERDOWN);
+				release_console_sem();
+			}
+
+			vout->display_bufs[0] = 0;
+			vout->display_bufs[1] = 0;
+		}
+
+		ipu_uninit_channel(MEM_PP_MEM);
+		if (!ipu_can_rotate_in_place(vout->rotate))
+			ipu_uninit_channel(MEM_ROT_PP_MEM);
+	} else {		/* ADC Direct */
+		ipu_disable_channel(MEM_PP_ADC, true);
+		ipu_uninit_channel(MEM_PP_ADC);
+	}
+	vout->ready_q.head = vout->ready_q.tail = 0;
+	vout->done_q.head = vout->done_q.tail = 0;
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		vout->v4l2_bufs[i].flags = 0;
+		vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+		vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+	}
+
+	vout->state = STATE_STREAM_OFF;
+
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL
+	if (vout->cur_disp_output < DISP3) {
+		if (vout->display_bufs[0] != 0) {
+			mxc_free_buffers(vout->display_bufs,
+					 vout->display_bufs_vaddr, 2,
+					 vout->display_buf_size);
+		}
+
+		mxcfb_set_refresh_mode(registered_fb
+				       [vout->
+					output_fb_num[vout->cur_disp_output]],
+				       MXCFB_REFRESH_PARTIAL, 0);
+	}
+#endif
+
+	return retval;
+}
+
+/*
+ * Valid whether the palette is supported
+ *
+ * @param palette  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return 1 if supported, 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return ((palette == V4L2_PIX_FMT_RGB565) ||
+		(palette == V4L2_PIX_FMT_BGR24) ||
+		(palette == V4L2_PIX_FMT_RGB24) ||
+		(palette == V4L2_PIX_FMT_BGR32) ||
+		(palette == V4L2_PIX_FMT_RGB32) ||
+		(palette == V4L2_PIX_FMT_NV12) ||
+		(palette == V4L2_PIX_FMT_YUV422P) ||
+		(palette == V4L2_PIX_FMT_YUV420));
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_g_fmt(vout_data *vout, struct v4l2_format *f)
+{
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+	*f = vout->v2f;
+	return 0;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_s_fmt(vout_data *vout, struct v4l2_format *f)
+{
+	int retval = 0;
+	u32 size = 0;
+	u32 bytesperline;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		retval = -EINVAL;
+		goto err0;
+	}
+	if (!valid_mode(f->fmt.pix.pixelformat)) {
+		dev_err(vout->video_dev->dev, "pixel format not supported\n");
+		retval = -EINVAL;
+		goto err0;
+	}
+
+	bytesperline = (f->fmt.pix.width * fmt_to_bpp(f->fmt.pix.pixelformat)) /
+	    8;
+	if (f->fmt.pix.bytesperline < bytesperline) {
+		f->fmt.pix.bytesperline = bytesperline;
+	} else {
+		bytesperline = f->fmt.pix.bytesperline;
+	}
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV422P:
+		/* byteperline for YUV planar formats is for
+		   Y plane only */
+		size = bytesperline * f->fmt.pix.height * 2;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_NV12:
+		size = (bytesperline * f->fmt.pix.height * 3) / 2;
+		break;
+	default:
+		size = bytesperline * f->fmt.pix.height;
+		break;
+	}
+
+	/* Return the actual size of the image to the app */
+	if (f->fmt.pix.sizeimage < size)
+		f->fmt.pix.sizeimage = size;
+	else
+		size = f->fmt.pix.sizeimage;
+
+	vout->v2f.fmt.pix = f->fmt.pix;
+	if (vout->v2f.fmt.pix.priv != 0) {
+		if (copy_from_user(&vout->offset,
+				   (void *)vout->v2f.fmt.pix.priv,
+				   sizeof(vout->offset))) {
+			retval = -EFAULT;
+			goto err0;
+		}
+	} else {
+		vout->offset.u_offset = 0;
+		vout->offset.v_offset = 0;
+	}
+
+	retval = 0;
+err0:
+	return retval;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_get_v42lout_control(vout_data *vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		return (vout->rotate & IPU_ROTATE_HORIZ_FLIP) ? 1 : 0;
+	case V4L2_CID_VFLIP:
+		return (vout->rotate & IPU_ROTATE_VERT_FLIP) ? 1 : 0;
+	case (V4L2_CID_PRIVATE_BASE + 1):
+		return vout->rotate;
+	default:
+		return -EINVAL;
+	}
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_set_v42lout_control(vout_data *vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		vout->rotate |= c->value ? IPU_ROTATE_HORIZ_FLIP :
+		    IPU_ROTATE_NONE;
+		break;
+	case V4L2_CID_VFLIP:
+		vout->rotate |= c->value ? IPU_ROTATE_VERT_FLIP :
+		    IPU_ROTATE_NONE;
+		break;
+	case V4L2_CID_MXC_ROT:
+		vout->rotate = c->value;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * V4L2 interface - open function
+ *
+ * @param inode        structure inode *
+ *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l2out_open(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+	int err;
+
+	if (!vout)
+		return -ENODEV;
+
+	down(&vout->busy_lock);
+
+	err = -EINTR;
+	if (signal_pending(current))
+		goto oops;
+
+	if (vout->open_count++ == 0) {
+		ipu_request_irq(IPU_IRQ_PP_IN_EOF,
+				mxc_v4l2out_pp_in_irq_handler,
+				0, dev->name, vout);
+		ipu_request_irq(IPU_IRQ_PP_OUT_EOF,
+				mxc_v4l2out_pp_out_irq_handler,
+				0, dev->name, vout);
+
+		init_waitqueue_head(&vout->v4l_bufq);
+
+		init_timer(&vout->output_timer);
+		vout->output_timer.function = mxc_v4l2out_timer_handler;
+		vout->output_timer.data = (unsigned long)vout;
+
+		vout->state = STATE_STREAM_OFF;
+		vout->rotate = IPU_ROTATE_NONE;
+		g_irq_cnt = g_buf_output_cnt = g_buf_q_cnt = g_buf_dq_cnt = 0;
+
+	}
+
+	file->private_data = dev;
+
+	up(&vout->busy_lock);
+
+	return 0;
+
+oops:
+	up(&vout->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L2 interface - close function
+ *
+ * @param inode    struct inode *
+ *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l2out_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+
+	if (--vout->open_count == 0) {
+		if (vout->state != STATE_STREAM_OFF)
+			mxc_v4l2out_streamoff(vout);
+
+		ipu_free_irq(IPU_IRQ_PP_IN_EOF, vout);
+		ipu_free_irq(IPU_IRQ_PP_OUT_EOF, vout);
+
+		file->private_data = NULL;
+
+		mxc_free_buffers(vout->queue_buf_paddr, vout->queue_buf_vaddr,
+				 vout->buffer_cnt, vout->queue_buf_size);
+		vout->buffer_cnt = 0;
+		mxc_free_buffers(vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+				 vout->display_buf_size);
+
+		/* capture off */
+		wake_up_interruptible(&vout->v4l_bufq);
+	}
+
+	return 0;
+}
+
+/*!
+ * V4L2 interface - ioctl function
+ *
+ * @param inode      struct inode *
+ *
+ * @param file       struct file *
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void *
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int
+mxc_v4l2out_do_ioctl(struct inode *inode, struct file *file,
+		     unsigned int ioctlnr, void *arg)
+{
+	struct video_device *vdev = file->private_data;
+	vout_data *vout = video_get_drvdata(vdev);
+	int retval = 0;
+	int i = 0;
+
+	if (!vout)
+		return -EBADF;
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+	case VIDIOC_QUERYCAP:
+		{
+			struct v4l2_capability *cap = arg;
+			strcpy(cap->driver, "mxc_v4l2_output");
+			cap->version = 0;
+			cap->capabilities =
+			    V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_FMT:
+		{
+			struct v4l2_format *gf = arg;
+			retval = mxc_v4l2out_g_fmt(vout, gf);
+			break;
+		}
+	case VIDIOC_S_FMT:
+		{
+			struct v4l2_format *sf = arg;
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+			retval = mxc_v4l2out_s_fmt(vout, sf);
+			break;
+		}
+	case VIDIOC_REQBUFS:
+		{
+			struct v4l2_requestbuffers *req = arg;
+			if ((req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (req->memory != V4L2_MEMORY_MMAP)) {
+				dev_dbg(vdev->dev,
+					"VIDIOC_REQBUFS: incorrect"
+					"buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			if (req->count == 0)
+				mxc_v4l2out_streamoff(vout);
+
+			if (vout->state == STATE_STREAM_OFF) {
+				if (vout->queue_buf_paddr[0] != 0) {
+					mxc_free_buffers(vout->queue_buf_paddr,
+							 vout->queue_buf_vaddr,
+							 vout->buffer_cnt,
+							 vout->queue_buf_size);
+					dev_dbg(vdev->dev,
+						"VIDIOC_REQBUFS:"
+						"freed buffers\n");
+				}
+				vout->buffer_cnt = 0;
+			} else {
+				dev_dbg(vdev->dev,
+					"VIDIOC_REQBUFS: Buffer is in use\n");
+				retval = -EBUSY;
+				break;
+			}
+
+			if (req->count == 0)
+				break;
+
+			if (req->count < MIN_FRAME_NUM)
+				req->count = MIN_FRAME_NUM;
+			else if (req->count > MAX_FRAME_NUM)
+				req->count = MAX_FRAME_NUM;
+			vout->buffer_cnt = req->count;
+			vout->queue_buf_size =
+			    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+
+			retval = mxc_allocate_buffers(vout->queue_buf_paddr,
+						      vout->queue_buf_vaddr,
+						      vout->buffer_cnt,
+						      vout->queue_buf_size);
+			if (retval < 0)
+				break;
+
+			/* Init buffer queues */
+			vout->done_q.head = 0;
+			vout->done_q.tail = 0;
+			vout->ready_q.head = 0;
+			vout->ready_q.tail = 0;
+
+			for (i = 0; i < vout->buffer_cnt; i++) {
+				memset(&(vout->v4l2_bufs[i]), 0,
+				       sizeof(vout->v4l2_bufs[i]));
+				vout->v4l2_bufs[i].flags = 0;
+				vout->v4l2_bufs[i].memory = V4L2_MEMORY_MMAP;
+				vout->v4l2_bufs[i].index = i;
+				vout->v4l2_bufs[i].type =
+				    V4L2_BUF_TYPE_VIDEO_OUTPUT;
+				vout->v4l2_bufs[i].length =
+				    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+				vout->v4l2_bufs[i].m.offset =
+				    (unsigned long)vout->queue_buf_paddr[i];
+				vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+				vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+			}
+			break;
+		}
+	case VIDIOC_QUERYBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			u32 type = buf->type;
+			int index = buf->index;
+
+			if ((type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				dev_dbg(vdev->dev,
+					"VIDIOC_QUERYBUFS: incorrect"
+					"buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+			down(&vout->param_lock);
+			memcpy(buf, &(vout->v4l2_bufs[index]), sizeof(*buf));
+			up(&vout->param_lock);
+			break;
+		}
+	case VIDIOC_QBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+			unsigned long lock_flags;
+
+			if ((buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			dev_dbg(vdev->dev, "VIDIOC_QBUF: %d\n", buf->index);
+
+			/* mmapped buffers are L1 WB cached,
+			 * so we need to clean them */
+			if (buf->flags & V4L2_BUF_FLAG_MAPPED)
+				flush_cache_all();
+
+			spin_lock_irqsave(&g_lock, lock_flags);
+
+			memcpy(&(vout->v4l2_bufs[index]), buf, sizeof(*buf));
+			vout->v4l2_bufs[index].flags |= V4L2_BUF_FLAG_QUEUED;
+
+			g_buf_q_cnt++;
+			queue_buf(&vout->ready_q, index);
+			if (vout->state == STATE_STREAM_PAUSED) {
+				unsigned long timeout;
+
+				index = peek_next_buf(&vout->ready_q);
+
+				/* if timestamp is 0, then default to 30fps */
+				if ((vout->v4l2_bufs[index].timestamp.tv_sec ==
+				     0)
+				    && (vout->v4l2_bufs[index].timestamp.
+					tv_usec == 0))
+					timeout =
+					    vout->start_jiffies +
+					    vout->frame_count * HZ / 30;
+				else
+					timeout =
+					    get_jiffies(&vout->v4l2_bufs[index].
+							timestamp);
+
+				if (jiffies >= timeout) {
+					dev_dbg(vout->video_dev->dev,
+						"warning: timer timeout"
+						"already expired.\n");
+				}
+				vout->output_timer.expires = timeout;
+				dev_dbg(vdev->dev,
+					"QBUF: frame #%u timeout @"
+					" %lu jiffies, current = %lu\n",
+					vout->frame_count, timeout, jiffies);
+				add_timer(&vout->output_timer);
+				vout->state = STATE_STREAM_ON;
+			}
+
+			spin_unlock_irqrestore(&g_lock, lock_flags);
+			break;
+		}
+	case VIDIOC_DQBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int idx;
+
+			if ((queue_size(&vout->done_q) == 0) &&
+			    (file->f_flags & O_NONBLOCK)) {
+				retval = -EAGAIN;
+				break;
+			}
+
+			if (!wait_event_interruptible_timeout(vout->v4l_bufq,
+							      queue_size(&vout->
+									 done_q)
+							      != 0, 10 * HZ)) {
+				dev_dbg(vdev->dev, "VIDIOC_DQBUF: timeout\n");
+				retval = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				dev_dbg(vdev->dev,
+					"VIDIOC_DQBUF: interrupt received\n");
+				retval = -ERESTARTSYS;
+				break;
+			}
+			idx = dequeue_buf(&vout->done_q);
+			if (idx == -1) {	/* No frame free */
+				dev_dbg(vdev->dev,
+					"VIDIOC_DQBUF: no free buffers\n");
+				retval = -EAGAIN;
+				break;
+			}
+			if ((vout->v4l2_bufs[idx].flags & V4L2_BUF_FLAG_DONE) ==
+			    0)
+				dev_dbg(vdev->dev,
+					"VIDIOC_DQBUF: buffer in done q, "
+					"but not flagged as done\n");
+
+			vout->v4l2_bufs[idx].flags = 0;
+			memcpy(buf, &(vout->v4l2_bufs[idx]), sizeof(*buf));
+			dev_dbg(vdev->dev, "VIDIOC_DQBUF: %d\n", buf->index);
+			break;
+		}
+	case VIDIOC_STREAMON:
+		{
+			retval = mxc_v4l2out_streamon(vout);
+			break;
+		}
+	case VIDIOC_STREAMOFF:
+		{
+			retval = mxc_v4l2out_streamoff(vout);
+			break;
+		}
+	case VIDIOC_G_CTRL:
+		{
+			retval = mxc_get_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_S_CTRL:
+		{
+			retval = mxc_set_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_CROPCAP:
+		{
+			struct v4l2_cropcap *cap = arg;
+
+			if (cap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+
+			cap->bounds = vout->crop_bounds[vout->cur_disp_output];
+			cap->defrect = vout->crop_bounds[vout->cur_disp_output];
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			crop->c = vout->crop_current;
+			break;
+		}
+	case VIDIOC_S_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+			struct v4l2_rect *b =
+			    &(vout->crop_bounds[vout->cur_disp_output]);
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.height < 0) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.width < 0) {
+				retval = -EINVAL;
+				break;
+			}
+
+			/* only full screen supported for SDC BG */
+			if (vout->cur_disp_output == 4) {
+				crop->c = vout->crop_current;
+				break;
+			}
+
+			if (crop->c.top < b->top)
+				crop->c.top = b->top;
+			if (crop->c.top >= b->top + b->height)
+				crop->c.top = b->top + b->height - 1;
+			if (crop->c.height > b->top - crop->c.top + b->height)
+				crop->c.height =
+				    b->top - crop->c.top + b->height;
+
+			if (crop->c.left < b->left)
+				crop->c.left = b->left;
+			if (crop->c.left >= b->left + b->width)
+				crop->c.left = b->left + b->width - 1;
+			if (crop->c.width > b->left - crop->c.left + b->width)
+				crop->c.width =
+				    b->left - crop->c.left + b->width;
+
+			/* stride line limitation */
+			crop->c.height -= crop->c.height % 8;
+			crop->c.width -= crop->c.width % 8;
+
+			vout->crop_current = crop->c;
+			break;
+		}
+	case VIDIOC_ENUMOUTPUT:
+		{
+			struct v4l2_output *output = arg;
+
+			if ((output->index >= 5) ||
+			    (vout->output_enabled[output->index] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (output->index < 3) {
+				*output = mxc_outputs[MXC_V4L2_OUT_2_ADC];
+				output->name[4] = '0' + output->index;
+			} else {
+				*output = mxc_outputs[MXC_V4L2_OUT_2_SDC];
+			}
+			break;
+		}
+	case VIDIOC_G_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			*p_output_num = vout->cur_disp_output;
+			break;
+		}
+	case VIDIOC_S_OUTPUT:
+		{
+			int *p_output_num = arg;
+			int fbnum;
+			struct v4l2_rect *b;
+
+			if ((*p_output_num >= MXC_V4L2_OUT_NUM_OUTPUTS) ||
+			    (vout->output_enabled[*p_output_num] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+
+			vout->cur_disp_output = *p_output_num;
+
+			/* Update bounds in case they have changed */
+			b = &vout->crop_bounds[vout->cur_disp_output];
+
+			fbnum = vout->output_fb_num[vout->cur_disp_output];
+			if (vout->cur_disp_output == 3)
+				fbnum = vout->output_fb_num[4];
+
+			b->width = registered_fb[fbnum]->var.xres;
+			b->height = registered_fb[fbnum]->var.yres;
+
+			vout->crop_current = *b;
+			break;
+		}
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_G_PARM:
+	case VIDIOC_ENUMSTD:
+	case VIDIOC_G_STD:
+	case VIDIOC_S_STD:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&vout->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L2 interface - ioctl function
+ *
+ * @return  None
+ */
+static int
+mxc_v4l2out_ioctl(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l2out_do_ioctl);
+}
+
+/*!
+ * V4L2 interface - mmap function
+ *
+ * @param file          structure file *
+ *
+ * @param vma           structure vm_area_struct *
+ *
+ * @return status       0 Success, EINTR busy lock error,
+ *                      ENOBUFS remap_page error
+ */
+static int mxc_v4l2out_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *vdev = video_devdata(file);
+	unsigned long size = vma->vm_end - vma->vm_start;
+	int res = 0;
+	int i;
+	vout_data *vout = video_get_drvdata(vdev);
+
+	dev_dbg(vdev->dev, "pgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		if ((vout->v4l2_bufs[i].m.offset ==
+		     (vma->vm_pgoff << PAGE_SHIFT)) &&
+		    (vout->v4l2_bufs[i].length >= size)) {
+			vout->v4l2_bufs[i].flags |= V4L2_BUF_FLAG_MAPPED;
+			break;
+		}
+	}
+	if (i == vout->buffer_cnt) {
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	/* make buffers inner write-back, outer write-thru cacheable */
+	vma->vm_page_prot = pgprot_outer_wrthru(vma->vm_page_prot);
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		dev_dbg(vdev->dev, "mmap remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+mxc_mmap_exit:
+	up(&vout->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L2 interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_v4l2out_poll(struct file *file, poll_table * wait)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	queue = &vout->v4l_bufq;
+	poll_wait(file, queue, wait);
+
+	up(&vout->busy_lock);
+	return res;
+}
+
+static struct
+file_operations mxc_v4l2out_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l2out_open,
+	.release = mxc_v4l2out_close,
+	.ioctl = mxc_v4l2out_ioctl,
+	.mmap = mxc_v4l2out_mmap,
+	.poll = mxc_v4l2out_poll,
+};
+
+static struct video_device mxc_v4l2out_template = {
+	.owner = THIS_MODULE,
+	.name = "MXC Video Output",
+	.type = 0,
+	.type2 = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING,
+	.fops = &mxc_v4l2out_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxc_v4l2out_probe(struct platform_device *pdev)
+{
+	int i;
+	vout_data *vout;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	g_vout = vout = kmalloc(sizeof(vout_data), GFP_KERNEL);
+
+	if (!vout)
+		return 0;
+
+	memset(vout, 0, sizeof(vout_data));
+
+	vout->video_dev = video_device_alloc();
+	if (vout->video_dev == NULL)
+		return -1;
+	vout->video_dev->dev = &pdev->dev;
+	vout->video_dev->minor = -1;
+
+	*(vout->video_dev) = mxc_v4l2out_template;
+
+	/* register v4l device */
+	if (video_register_device(vout->video_dev,
+				  VFL_TYPE_GRABBER, video_nr) == -1) {
+		dev_dbg(&pdev->dev, "video_register_device failed\n");
+		return 0;
+	}
+	dev_info(&pdev->dev, "Registered device video%d\n",
+		 vout->video_dev->minor & 0x1f);
+	vout->video_dev->dev = &pdev->dev;
+
+	video_set_drvdata(vout->video_dev, vout);
+
+	init_MUTEX(&vout->param_lock);
+	init_MUTEX(&vout->busy_lock);
+
+	/* setup outputs and cropping */
+	vout->cur_disp_output = -1;
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strncmp(idstr, "DISP", 4) == 0) {
+			int disp_num = idstr[4] - '0';
+			if (disp_num == 3) {
+				if (strcmp(idstr, "DISP3 BG - DI1") == 0)
+					disp_num = 5;
+				else if (strncmp(idstr, "DISP3 BG", 8) == 0)
+					disp_num = 4;
+			}
+			vout->crop_bounds[disp_num].left = 0;
+			vout->crop_bounds[disp_num].top = 0;
+			vout->crop_bounds[disp_num].width =
+			    registered_fb[i]->var.xres;
+			vout->crop_bounds[disp_num].height =
+			    registered_fb[i]->var.yres;
+			vout->output_enabled[disp_num] = true;
+			vout->output_fb_num[disp_num] = i;
+			if (vout->cur_disp_output == -1)
+				vout->cur_disp_output = disp_num;
+		}
+
+	}
+	vout->crop_current = vout->crop_bounds[vout->cur_disp_output];
+
+	platform_set_drvdata(pdev, vout);
+
+	return 0;
+}
+
+static int mxc_v4l2out_remove(struct platform_device *pdev)
+{
+	vout_data *vout = platform_get_drvdata(pdev);
+
+	if (vout->video_dev) {
+		if (-1 != vout->video_dev->minor)
+			video_unregister_device(vout->video_dev);
+		else
+			video_device_release(vout->video_dev);
+		vout->video_dev = NULL;
+	}
+
+	platform_set_drvdata(pdev, NULL);
+
+	kfree(vout);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2out_driver = {
+	.driver = {
+		   .name = "MXC Video Output",
+		   },
+	.probe = mxc_v4l2out_probe,
+	.remove = mxc_v4l2out_remove,
+};
+
+static struct platform_device mxc_v4l2out_device = {
+	.name = "MXC Video Output",
+	.id = 0,
+};
+
+/*!
+ * mxc v4l2 init function
+ *
+ */
+static int mxc_v4l2out_init(void)
+{
+	u8 err = 0;
+
+	err = platform_driver_register(&mxc_v4l2out_driver);
+	if (err == 0)
+		platform_device_register(&mxc_v4l2out_device);
+	return err;
+}
+
+/*!
+ * mxc v4l2 cleanup function
+ *
+ */
+static void mxc_v4l2out_clean(void)
+{
+	video_unregister_device(g_vout->video_dev);
+
+	platform_driver_unregister(&mxc_v4l2out_driver);
+	platform_device_unregister(&mxc_v4l2out_device);
+	kfree(g_vout);
+	g_vout = NULL;
+}
+
+module_init(mxc_v4l2out_init);
+module_exit(mxc_v4l2out_clean);
+
+module_param(video_nr, int, 0444);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2-driver for MXC video output");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/output/mxc_v4l2_output.c b/drivers/media/video/mxc/output/mxc_v4l2_output.c
new file mode 100644
index 000000000000..4fa4a641ab62
--- /dev/null
+++ b/drivers/media/video/mxc/output/mxc_v4l2_output.c
@@ -0,0 +1,2405 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @file drivers/media/video/mxc/output/mxc_v4l2_output.c
+ *
+ * @brief MXC V4L2 Video Output Driver
+ *
+ * Video4Linux2 Output Device using MXC IPU Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/semaphore.h>
+#include <linux/console.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/mxcfb.h>
+#include <media/v4l2-ioctl.h>
+#include <asm/cacheflush.h>
+
+#include "mxc_v4l2_output.h"
+
+vout_data *g_vout;
+#define INTERLACED_CONTENT(vout) ((cpu_is_mx51_rev(CHIP_REV_2_0) >= 1) && \
+				  (((vout)->field_fmt == V4L2_FIELD_INTERLACED_TB) || \
+				   ((vout)->field_fmt == V4L2_FIELD_INTERLACED_BT)))
+#define LOAD_3FIELDS(vout) ((INTERLACED_CONTENT(vout)) && \
+			    ((vout)->motion_sel != HIGH_MOTION))
+
+#define SDC_FG_FB_FORMAT        IPU_PIX_FMT_RGB565
+
+struct v4l2_output mxc_outputs[2] = {
+	{
+	 .index = MXC_V4L2_OUT_2_SDC,
+	 .name = "DISP3 Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN},
+	{
+	 .index = MXC_V4L2_OUT_2_ADC,
+	 .name = "DISPx Video Out",
+	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
+						   but no other choice */
+	 .audioset = 0,
+	 .modulator = 0,
+	 .std = V4L2_STD_UNKNOWN}
+};
+
+static int video_nr = 16;
+static int pending_buffer;
+static int pp_eof;
+static spinlock_t g_lock = SPIN_LOCK_UNLOCKED;
+static int last_index_n;
+static int last_index_c;
+
+/* debug counters */
+uint32_t g_irq_cnt;
+uint32_t g_buf_output_cnt;
+uint32_t g_buf_q_cnt;
+uint32_t g_buf_dq_cnt;
+
+#define QUEUE_SIZE (MAX_FRAME_NUM + 1)
+static __inline int queue_size(v4l_queue * q)
+{
+	if (q->tail >= q->head)
+		return (q->tail - q->head);
+	else
+		return ((q->tail + QUEUE_SIZE) - q->head);
+}
+
+static __inline int queue_buf(v4l_queue * q, int idx)
+{
+	if (((q->tail + 1) % QUEUE_SIZE) == q->head)
+		return -1;	/* queue full */
+	q->list[q->tail] = idx;
+	q->tail = (q->tail + 1) % QUEUE_SIZE;
+	return 0;
+}
+
+static __inline int dequeue_buf(v4l_queue * q)
+{
+	int ret;
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	ret = q->list[q->head];
+	q->head = (q->head + 1) % QUEUE_SIZE;
+	return ret;
+}
+
+static __inline int peek_next_buf(v4l_queue * q)
+{
+	if (q->tail == q->head)
+		return -1;	/* queue empty */
+	return q->list[q->head];
+}
+
+static __inline unsigned long get_jiffies(struct timeval *t)
+{
+	struct timeval cur;
+
+	if (t->tv_usec >= 1000000) {
+		t->tv_sec += t->tv_usec / 1000000;
+		t->tv_usec = t->tv_usec % 1000000;
+	}
+
+	do_gettimeofday(&cur);
+	if ((t->tv_sec < cur.tv_sec)
+	    || ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
+		return jiffies;
+
+	if (t->tv_usec < cur.tv_usec) {
+		cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
+		cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
+	} else {
+		cur.tv_sec = t->tv_sec - cur.tv_sec;
+		cur.tv_usec = t->tv_usec - cur.tv_usec;
+	}
+
+	return jiffies + timeval_to_jiffies(&cur);
+}
+
+/*!
+ * Private function to free buffers
+ *
+ * @param bufs_paddr	Array of physical address of buffers to be freed
+ *
+ * @param bufs_vaddr	Array of virtual address of buffers to be freed
+ *
+ * @param num_buf	Number of buffers to be freed
+ *
+ * @param size		Size for each buffer to be free
+ *
+ * @return status  0 success.
+ */
+static int mxc_free_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+			    int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		if (bufs_vaddr[i] != 0) {
+			dma_free_coherent(0, size, bufs_vaddr[i],
+					  bufs_paddr[i]);
+			pr_debug("freed @ paddr=0x%08X\n", (u32) bufs_paddr[i]);
+			bufs_paddr[i] = 0;
+			bufs_vaddr[i] = NULL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Private function to allocate buffers
+ *
+ * @param bufs_paddr	Output array of physical address of buffers allocated
+ *
+ * @param bufs_vaddr	Output array of virtual address of buffers allocated
+ *
+ * @param num_buf	Input number of buffers to allocate
+ *
+ * @param size		Input size for each buffer to allocate
+ *
+ * @return status	-0 Successfully allocated a buffer, -ENOBUFS failed.
+ */
+static int mxc_allocate_buffers(dma_addr_t bufs_paddr[], void *bufs_vaddr[],
+				int num_buf, int size)
+{
+	int i;
+
+	for (i = 0; i < num_buf; i++) {
+		bufs_vaddr[i] = dma_alloc_coherent(0, size,
+						   &bufs_paddr[i],
+						   GFP_DMA | GFP_KERNEL);
+
+		if (bufs_vaddr[i] == 0) {
+			mxc_free_buffers(bufs_paddr, bufs_vaddr, i, size);
+			printk(KERN_ERR "dma_alloc_coherent failed.\n");
+			return -ENOBUFS;
+		}
+		pr_debug("allocated @ paddr=0x%08X, size=%d.\n",
+			 (u32) bufs_paddr[i], size);
+	}
+
+	return 0;
+}
+
+/*
+ * Returns bits per pixel for given pixel format
+ *
+ * @param pixelformat  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return bits per pixel of pixelformat
+ */
+static u32 fmt_to_bpp(u32 pixelformat)
+{
+	u32 bpp;
+
+	bpp = 8*bytes_per_pixel(pixelformat);
+	return bpp;
+}
+
+static bool format_is_yuv(u32 pixelformat)
+{
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YUV422P:
+	case V4L2_PIX_FMT_YVU420:
+	case V4L2_PIX_FMT_NV12:
+		return true;
+		break;
+	}
+	return false;
+}
+
+static u32 bpp_to_fmt(struct fb_info *fbi)
+{
+	if (fbi->var.nonstd)
+		return fbi->var.nonstd;
+
+	if (fbi->var.bits_per_pixel == 24)
+		return V4L2_PIX_FMT_BGR24;
+	else if (fbi->var.bits_per_pixel == 32)
+		return V4L2_PIX_FMT_BGR32;
+	else if (fbi->var.bits_per_pixel == 16)
+		return V4L2_PIX_FMT_RGB565;
+
+	return 0;
+}
+
+static irqreturn_t mxc_v4l2out_disp_refresh_irq_handler(int irq, void *dev_id)
+{
+	vout_data *vout = dev_id;
+	int index, last_buf, ret;
+	unsigned long timeout;
+	unsigned long lock_flags = 0;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	g_irq_cnt++;
+
+	if (vout->ic_bypass && (pending_buffer || vout->frame_count < 3)) {
+		last_buf = vout->ipu_buf[vout->next_done_ipu_buf];
+		if (last_buf != -1) {
+			g_buf_output_cnt++;
+			vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+			queue_buf(&vout->done_q, last_buf);
+			vout->ipu_buf[vout->next_done_ipu_buf] = -1;
+			wake_up_interruptible(&vout->v4l_bufq);
+			vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+		}
+	}
+
+	if ((pending_buffer) && (pp_eof || vout->ic_bypass)) {
+		pp_eof = 0;
+		if (vout->ic_bypass) {
+			ret = ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER,
+					  vout->next_rdy_ipu_buf);
+		} else {
+			if (LOAD_3FIELDS(vout)) {
+				ret = ipu_select_multi_vdi_buffer(vout->next_rdy_ipu_buf);
+			} else {
+				ret = ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+							vout->next_rdy_ipu_buf);
+			}
+		}
+		if (ret < 0) {
+			dev_err(&vout->video_dev->dev,
+				"unable to set IPU buffer ready\n");
+		}
+		vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
+
+		pending_buffer = 0;
+
+		/* Setup timer for next buffer */
+		index = peek_next_buf(&vout->ready_q);
+		if (index != -1) {
+			/* if timestamp is 0, then default to 30fps */
+			if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+				&& (vout->v4l2_bufs[index].timestamp.tv_usec == 0)
+				&& vout->start_jiffies)
+				timeout =
+					vout->start_jiffies + vout->frame_count * HZ / 30;
+			else
+				timeout =
+					get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+			if (jiffies >= timeout) {
+				dev_dbg(&vout->video_dev->dev,
+					"warning: timer timeout already expired.\n");
+			}
+			if (mod_timer(&vout->output_timer, timeout))
+				dev_dbg(&vout->video_dev->dev,
+					"warning: timer was already set\n");
+
+			dev_dbg(&vout->video_dev->dev,
+				"timer handler next schedule: %lu\n", timeout);
+		} else {
+			vout->state = STATE_STREAM_PAUSED;
+		}
+	}
+
+	if (vout->state == STATE_STREAM_STOPPING) {
+		if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
+			vout->state = STATE_STREAM_OFF;
+		}
+	}
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return IRQ_HANDLED;
+}
+
+static int get_display_irq(vout_data *vout)
+{
+
+	int disp_irq = 0;
+
+	switch (vout->display_ch) {
+	case MEM_FG_SYNC:
+	case MEM_BG_SYNC:
+		disp_irq = IPU_IRQ_BG_SF_END;
+		break;
+	case MEM_DC_SYNC:
+		disp_irq = IPU_IRQ_DC_FC_1;
+		break;
+	default:
+		dev_err(&vout->video_dev->dev,
+			"not support display channel\n");
+	}
+
+	return disp_irq;
+}
+
+static void mxc_v4l2out_timer_handler(unsigned long arg)
+{
+	int index, ret;
+	unsigned long lock_flags = 0;
+	vout_data *vout = (vout_data *) arg;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	if ((vout->state == STATE_STREAM_STOPPING)
+	    || (vout->state == STATE_STREAM_OFF))
+		goto exit0;
+	/*
+	 * If timer occurs before IPU h/w is ready, then set the state to
+	 * paused and the timer will be set again when next buffer is queued
+	 * or PP comletes
+	 */
+	if (vout->ipu_buf[vout->next_rdy_ipu_buf] != -1) {
+		dev_dbg(&vout->video_dev->dev, "IPU buffer busy\n");
+		vout->state = STATE_STREAM_PAUSED;
+		goto exit0;
+	}
+
+	/* Dequeue buffer and pass to IPU */
+	unsigned int aid_field_offset, current_field_offset;
+	if (INTERLACED_CONTENT(vout)) {
+		if (((LOAD_3FIELDS(vout)) && (vout->next_rdy_ipu_buf)) ||
+		    ((!LOAD_3FIELDS(vout)) && !(vout->next_rdy_ipu_buf))) {
+			aid_field_offset = vout->bytesperline;
+			current_field_offset = 0;
+			index = last_index_n;
+		} else {
+			aid_field_offset = 0;
+			current_field_offset = vout->bytesperline;
+			index = dequeue_buf(&vout->ready_q);
+			if (index == -1) {	/* no buffers ready, should never occur */
+				dev_err(&vout->video_dev->dev,
+					"mxc_v4l2out: timer - no queued buffers ready\n");
+				goto exit0;
+			}
+			g_buf_dq_cnt++;
+			vout->frame_count++;
+			last_index_n = index;
+		}
+	} else {
+		current_field_offset = 0;
+		index = dequeue_buf(&vout->ready_q);
+		if (index == -1) {	/* no buffers ready, should never occur */
+			dev_err(&vout->video_dev->dev,
+				"mxc_v4l2out: timer - no queued buffers ready\n");
+			goto exit0;
+		}
+		g_buf_dq_cnt++;
+		vout->frame_count++;
+	}
+
+	if (vout->ic_bypass) {
+		vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
+		ret = ipu_update_channel_buffer(vout->display_ch, IPU_INPUT_BUFFER,
+				      vout->next_rdy_ipu_buf,
+				      vout->v4l2_bufs[index].m.offset);
+	} else {
+		if (LOAD_3FIELDS(vout)) {
+			int index_n = index;
+			index = last_index_n;
+			int index_p = last_index_c;
+			vout->ipu_buf_p[vout->next_rdy_ipu_buf] = index_p;
+			vout->ipu_buf[vout->next_rdy_ipu_buf] = last_index_c = index;
+			vout->ipu_buf_n[vout->next_rdy_ipu_buf] = last_index_n = index_n;
+			last_index_n = vout->ipu_buf_n[vout->next_rdy_ipu_buf];
+			last_index_c = vout->ipu_buf[vout->next_rdy_ipu_buf];
+			ret = ipu_update_channel_buffer(vout->post_proc_ch,
+							IPU_INPUT_BUFFER,
+							vout->next_rdy_ipu_buf,
+							vout->v4l2_bufs[index].m.offset+current_field_offset);
+			ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_P,
+							 IPU_INPUT_BUFFER,
+							 vout->next_rdy_ipu_buf,
+							 vout->v4l2_bufs[index_p].m.offset+aid_field_offset);
+			ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_N,
+							 IPU_INPUT_BUFFER,
+							 vout->next_rdy_ipu_buf,
+							 vout->v4l2_bufs[index_n].m.offset+aid_field_offset);
+		} else {
+			vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
+			ret = ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+							vout->next_rdy_ipu_buf,
+							vout->v4l2_bufs[index].m.offset+current_field_offset);
+		}
+	}
+	if (ret < 0) {
+		dev_err(&vout->video_dev->dev,
+			"unable to update buffer %d address rc=%d\n",
+			vout->next_rdy_ipu_buf, ret);
+		goto exit0;
+	}
+
+	pending_buffer = 1;
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return;
+
+      exit0:
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+}
+
+static irqreturn_t mxc_v4l2out_pp_in_irq_handler(int irq, void *dev_id)
+{
+	int last_buf;
+	int index;
+	unsigned long timeout;
+	unsigned long lock_flags = 0;
+	vout_data *vout = dev_id;
+
+	spin_lock_irqsave(&g_lock, lock_flags);
+
+	g_irq_cnt++;
+
+	/* Process previous buffer */
+	if (LOAD_3FIELDS(vout))
+		last_buf = vout->ipu_buf_p[vout->next_done_ipu_buf];
+	else
+		last_buf = vout->ipu_buf[vout->next_done_ipu_buf];
+
+	if (last_buf != -1) {
+		if ((!INTERLACED_CONTENT(vout)) || (vout->next_done_ipu_buf)) {
+			g_buf_output_cnt++;
+			vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+			queue_buf(&vout->done_q, last_buf);
+			wake_up_interruptible(&vout->v4l_bufq);
+		}
+		vout->ipu_buf[vout->next_done_ipu_buf] = -1;
+		if (LOAD_3FIELDS(vout)) {
+			vout->ipu_buf_p[vout->next_done_ipu_buf] = -1;
+			vout->ipu_buf_n[vout->next_done_ipu_buf] = -1;
+		}
+		vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+	}
+	pp_eof = 1;
+
+	if (vout->state == STATE_STREAM_STOPPING) {
+		if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
+			vout->state = STATE_STREAM_OFF;
+		}
+	} else if ((vout->state == STATE_STREAM_PAUSED)
+		   && ((index = peek_next_buf(&vout->ready_q)) != -1)) {
+		/* Setup timer for next buffer, when stream has been paused */
+		pr_debug("next index %d\n", index);
+
+		/* if timestamp is 0, then default to 30fps */
+		if ((vout->v4l2_bufs[index].timestamp.tv_sec == 0)
+		    && (vout->v4l2_bufs[index].timestamp.tv_usec == 0))
+			timeout =
+			    vout->start_jiffies + vout->frame_count * HZ / 30;
+		else
+			timeout =
+			    get_jiffies(&vout->v4l2_bufs[index].timestamp);
+
+		if (jiffies >= timeout) {
+			pr_debug("warning: timer timeout already expired.\n");
+		}
+
+		vout->state = STATE_STREAM_ON;
+
+		if (mod_timer(&vout->output_timer, timeout))
+			pr_debug("warning: timer was already set\n");
+
+		pr_debug("timer handler next schedule: %lu\n", timeout);
+	}
+
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+
+	return IRQ_HANDLED;
+}
+
+/*!
+ *  Initialize VDI channels
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int init_VDI_channel(vout_data *vout, ipu_channel_params_t params)
+{
+	struct device *dev = &vout->video_dev->dev;
+
+	if (ipu_init_channel(MEM_VDI_PRP_VF_MEM, &params) != 0) {
+		dev_dbg(dev, "Error initializing VDI current channel\n");
+		return -EINVAL;
+	}
+	if (LOAD_3FIELDS(vout)) {
+		if (ipu_init_channel(MEM_VDI_PRP_VF_MEM_P, &params) != 0) {
+			dev_err(dev, "Error initializing VDI previous channel\n");
+			return -EINVAL;
+		}
+		if (ipu_init_channel(MEM_VDI_PRP_VF_MEM_N, &params) != 0) {
+			dev_err(dev, "Error initializing VDI next channel\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Initialize VDI channel buffers
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
+				   uint16_t in_width, uint16_t in_height,
+				   uint32_t stride,
+				   dma_addr_t phyaddr_0, dma_addr_t phyaddr_1,
+				   uint32_t u_offset, uint32_t v_offset)
+{
+	struct device *dev = &vout->video_dev->dev;
+
+	if (ipu_init_channel_buffer(MEM_VDI_PRP_VF_MEM, IPU_INPUT_BUFFER,
+				    in_pixel_fmt, in_width, in_height, stride,
+				    IPU_ROTATE_NONE,
+				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset+vout->bytesperline,
+				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
+				    u_offset, v_offset) != 0) {
+		dev_err(dev, "Error initializing VDI current input buffer\n");
+		return -EINVAL;
+	}
+	if (LOAD_3FIELDS(vout)) {
+		if (ipu_init_channel_buffer(MEM_VDI_PRP_VF_MEM_P,
+					    IPU_INPUT_BUFFER,
+					    in_pixel_fmt, in_width, in_height,
+					    stride, IPU_ROTATE_NONE,
+					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset,
+					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset+vout->bytesperline,
+					    u_offset, v_offset) != 0) {
+			dev_err(dev, "Error initializing VDI previous input buffer\n");
+			return -EINVAL;
+		}
+		if (ipu_init_channel_buffer(MEM_VDI_PRP_VF_MEM_N,
+					    IPU_INPUT_BUFFER,
+					    in_pixel_fmt, in_width, in_height,
+					    stride, IPU_ROTATE_NONE,
+					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset,
+					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset+vout->bytesperline,
+					    u_offset, v_offset) != 0) {
+			dev_err(dev, "Error initializing VDI next input buffer\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Initialize VDI path
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int init_VDI(ipu_channel_params_t params, vout_data *vout,
+			struct device *dev, struct fb_info *fbi,
+			ipu_channel_t  *display_input_ch, u16 out_width,
+			u16 out_height)
+{
+	params.mem_prp_vf_mem.in_width = vout->v2f.fmt.pix.width;
+	params.mem_prp_vf_mem.in_height = vout->v2f.fmt.pix.height;
+	params.mem_prp_vf_mem.motion_sel = vout->motion_sel;
+	params.mem_prp_vf_mem.field_fmt = vout->field_fmt;
+	params.mem_prp_vf_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
+	params.mem_prp_vf_mem.out_width = out_width;
+	params.mem_prp_vf_mem.out_height = out_height;
+	if (vout->display_ch == ADC_SYS2)
+		params.mem_prp_vf_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
+	else
+		params.mem_prp_vf_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+
+	if (init_VDI_channel(vout, params) != 0) {
+		dev_err(dev, "Error init_VDI_channel channel\n");
+		return -EINVAL;
+	}
+
+
+	if (init_VDI_in_channel_buffer(vout,
+				       params.mem_prp_vf_mem.in_pixel_fmt,
+				       params.mem_prp_vf_mem.in_width,
+				       params.mem_prp_vf_mem.in_height,
+				       bytes_per_pixel(params.mem_prp_vf_mem.
+						       in_pixel_fmt),
+				       vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
+				       vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
+				       vout->offset.u_offset,
+				       vout->offset.v_offset) != 0) {
+		return -EINVAL;
+	}
+
+	if (!ipu_can_rotate_in_place(vout->rotate)) {
+		if (vout->rot_pp_bufs[0]) {
+			mxc_free_buffers(vout->rot_pp_bufs,
+					 vout->rot_pp_bufs_vaddr, 2,
+					 vout->display_buf_size);
+		}
+		if (mxc_allocate_buffers
+		    (vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+		     vout->display_buf_size) < 0) {
+			return -ENOBUFS;
+		}
+
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_OUTPUT_BUFFER,
+					    params.mem_prp_vf_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    IPU_ROTATE_NONE,
+					    vout->rot_pp_bufs[0],
+					    vout->rot_pp_bufs[1], 0, 0) != 0) {
+			dev_err(dev, "Error initializing PRP output buffer\n");
+			return -EINVAL;
+		}
+
+		if (ipu_init_channel(MEM_ROT_VF_MEM, NULL) != 0) {
+			dev_err(dev, "Error initializing PP ROT channel\n");
+			return -EINVAL;
+		}
+		if (ipu_init_channel_buffer(MEM_ROT_VF_MEM,
+					    IPU_INPUT_BUFFER,
+					    params.mem_prp_vf_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    vout->rotate,
+					    vout->rot_pp_bufs[0],
+					    vout->rot_pp_bufs[1], 0, 0) != 0) {
+			dev_err(dev,
+				"Error initializing PP ROT input buffer\n");
+			return -EINVAL;
+		}
+
+		/* swap width and height */
+		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+			out_width = vout->crop_current.width;
+			out_height = vout->crop_current.height;
+		}
+
+		if (ipu_init_channel_buffer(MEM_ROT_VF_MEM,
+					    IPU_OUTPUT_BUFFER,
+					    params.mem_prp_vf_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    IPU_ROTATE_NONE,
+					    vout->display_bufs[0],
+					    vout->display_bufs[1], 0, 0) != 0) {
+			dev_err(dev,
+				"Error initializing PP-VDI output buffer\n");
+			return -EINVAL;
+		}
+
+		if (ipu_link_channels(vout->post_proc_ch, MEM_ROT_VF_MEM) < 0)
+			return -EINVAL;
+
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER, 1);
+
+		ipu_enable_channel(MEM_ROT_VF_MEM);
+		*display_input_ch = MEM_ROT_VF_MEM;
+
+	} else {
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_OUTPUT_BUFFER,
+					    params.mem_prp_vf_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    vout->rotate,
+					    vout->display_bufs[0],
+					    vout->display_bufs[1], 0, 0) != 0) {
+			dev_err(dev,
+				"Error initializing PP-VDI output buffer\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Initialize PP path
+ *
+ * @param params    structure ipu_channel_params_t
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int init_PP(ipu_channel_params_t params, vout_data *vout,
+		   struct device *dev, struct fb_info *fbi,
+		   ipu_channel_t *display_input_ch, u16 out_width,
+		   u16 out_height)
+{
+	params.mem_pp_mem.in_width = vout->v2f.fmt.pix.width;
+	params.mem_pp_mem.in_height = vout->v2f.fmt.pix.height;
+	params.mem_pp_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
+	params.mem_pp_mem.out_width = out_width;
+	params.mem_pp_mem.out_height = out_height;
+	if (vout->display_ch == ADC_SYS2)
+		params.mem_pp_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
+	else
+		params.mem_pp_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+	if (ipu_init_channel(vout->post_proc_ch, &params) != 0) {
+		dev_err(dev, "Error initializing PP channel\n");
+		return -EINVAL;
+	}
+
+	if (ipu_init_channel_buffer(vout->post_proc_ch,
+				    IPU_INPUT_BUFFER,
+				    params.mem_pp_mem.in_pixel_fmt,
+				    params.mem_pp_mem.in_width,
+				    params.mem_pp_mem.in_height,
+				    vout->v2f.fmt.pix.bytesperline /
+				    bytes_per_pixel(params.mem_pp_mem.
+						    in_pixel_fmt),
+				    IPU_ROTATE_NONE,
+				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
+				    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
+				    vout->offset.u_offset,
+				    vout->offset.v_offset) != 0) {
+		dev_err(dev, "Error initializing PP input buffer\n");
+		return -EINVAL;
+	}
+
+	if (!ipu_can_rotate_in_place(vout->rotate)) {
+		if (vout->rot_pp_bufs[0]) {
+			mxc_free_buffers(vout->rot_pp_bufs,
+					 vout->rot_pp_bufs_vaddr, 2,
+					 vout->display_buf_size);
+		}
+		if (mxc_allocate_buffers
+		    (vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+		     vout->display_buf_size) < 0) {
+			return -ENOBUFS;
+		}
+
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_OUTPUT_BUFFER,
+					    params.mem_pp_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    IPU_ROTATE_NONE,
+					    vout->rot_pp_bufs[0],
+					    vout->rot_pp_bufs[1], 0, 0) != 0) {
+			dev_err(dev, "Error initializing PP output buffer\n");
+			return -EINVAL;
+		}
+
+		if (ipu_init_channel(MEM_ROT_PP_MEM, NULL) != 0) {
+			dev_err(dev, "Error initializing PP ROT channel\n");
+			return -EINVAL;
+		}
+		if (ipu_init_channel_buffer(MEM_ROT_PP_MEM,
+					    IPU_INPUT_BUFFER,
+					    params.mem_pp_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    vout->rotate,
+					    vout->rot_pp_bufs[0],
+					    vout->rot_pp_bufs[1], 0, 0) != 0) {
+			dev_err(dev,
+				"Error initializing PP ROT input buffer\n");
+			return -EINVAL;
+		}
+
+		/* swap width and height */
+		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+			out_width = vout->crop_current.width;
+			out_height = vout->crop_current.height;
+		}
+
+		if (ipu_init_channel_buffer(MEM_ROT_PP_MEM,
+					    IPU_OUTPUT_BUFFER,
+					    params.mem_pp_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    IPU_ROTATE_NONE,
+					    vout->display_bufs[0],
+					    vout->display_bufs[1], 0, 0) != 0) {
+			dev_err(dev, "Error initializing PP output buffer\n");
+			return -EINVAL;
+		}
+
+		if (ipu_link_channels(vout->post_proc_ch, MEM_ROT_PP_MEM) < 0)
+			return -EINVAL;
+
+		ipu_select_buffer(MEM_ROT_PP_MEM, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(MEM_ROT_PP_MEM, IPU_OUTPUT_BUFFER, 1);
+
+		ipu_enable_channel(MEM_ROT_PP_MEM);
+		*display_input_ch = MEM_ROT_PP_MEM;
+
+	} else {
+		if (ipu_init_channel_buffer(vout->post_proc_ch,
+					    IPU_OUTPUT_BUFFER,
+					    params.mem_pp_mem.
+					    out_pixel_fmt, out_width,
+					    out_height, out_width,
+					    vout->rotate,
+					    vout->display_bufs[0],
+					    vout->display_bufs[1], 0, 0) != 0) {
+			dev_err(dev, "Error initializing PP output buffer\n");
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/*!
+ * Start the output stream
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamon(vout_data * vout)
+{
+	struct device *dev = &vout->video_dev->dev;
+	ipu_channel_params_t params;
+	struct mxcfb_pos fb_pos;
+	struct fb_var_screeninfo fbvar;
+	struct fb_info *fbi =
+	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	u16 out_width;
+	u16 out_height;
+	int disp_irq = 0;
+	ipu_channel_t display_input_ch;
+	bool use_direct_adc = false;
+	mm_segment_t old_fs;
+
+	dev_dbg(dev, "mxc_v4l2out_streamon: field format=%d\n",
+		vout->field_fmt);
+	if (INTERLACED_CONTENT(vout)) {
+		ipu_request_irq(IPU_IRQ_PRP_VF_OUT_EOF,
+				mxc_v4l2out_pp_in_irq_handler,
+				0, &vout->video_dev->name, vout);
+		display_input_ch = MEM_VDI_PRP_VF_MEM;
+	} else {
+		ipu_request_irq(IPU_IRQ_PP_IN_EOF,
+				mxc_v4l2out_pp_in_irq_handler,
+				0, &vout->video_dev->name, vout);
+		display_input_ch = MEM_PP_MEM;
+	}
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state != STATE_STREAM_OFF)
+		return -EBUSY;
+
+	if (queue_size(&vout->ready_q) < 2) {
+		dev_err(dev, "2 buffers not been queued yet!\n");
+		return -EINVAL;
+	}
+
+	if ((vout->field_fmt == V4L2_FIELD_BOTTOM) || (vout->field_fmt == V4L2_FIELD_TOP)) {
+		dev_err(dev, "4 queued buffers need, not supported yet!\n");
+		return -EINVAL;
+	}
+
+	pending_buffer = 0;
+
+	out_width = vout->crop_current.width;
+	out_height = vout->crop_current.height;
+	vout->next_done_ipu_buf = 0;
+	vout->next_rdy_ipu_buf = 1;
+
+	if (!INTERLACED_CONTENT(vout)) {
+		vout->next_done_ipu_buf = vout->next_rdy_ipu_buf = 0;
+		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf[1] = dequeue_buf(&vout->ready_q);
+		vout->frame_count = 2;
+	} else if (!LOAD_3FIELDS(vout)) {
+		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf[1] = -1;
+		vout->frame_count = 1;
+		last_index_n = vout->ipu_buf[0];
+	} else {
+		vout->ipu_buf_p[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf[0] = vout->ipu_buf_p[0];
+		vout->ipu_buf_n[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf_p[1] = -1;
+		vout->ipu_buf[1] = -1;
+		vout->ipu_buf_n[1] = -1;
+		last_index_c = vout->ipu_buf[0];
+		last_index_n = vout->ipu_buf_n[0];
+		vout->frame_count = 2;
+	}
+
+	/* Init Display Channel */
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL
+	if (INTERLACED_CONTENT(vout))
+		ipu_enable_irq(IPU_IRQ_PRP_VF_OUT_EOF);
+	else
+		ipu_enable_irq(IPU_IRQ_PP_IN_EOF);
+
+	if (vout->cur_disp_output < DISP3) {
+		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_OFF, 0);
+		fbi = NULL;
+		if (ipu_can_rotate_in_place(vout->rotate)) {
+			dev_dbg(dev, "Using PP direct to ADC channel\n");
+			use_direct_adc = true;
+			vout->display_ch = MEM_PP_ADC;
+			vout->post_proc_ch = MEM_PP_ADC;
+
+			memset(&params, 0, sizeof(params));
+			params.mem_pp_adc.in_width = vout->v2f.fmt.pix.width;
+			params.mem_pp_adc.in_height = vout->v2f.fmt.pix.height;
+			params.mem_pp_adc.in_pixel_fmt =
+			    vout->v2f.fmt.pix.pixelformat;
+			params.mem_pp_adc.out_width = out_width;
+			params.mem_pp_adc.out_height = out_height;
+			params.mem_pp_adc.out_pixel_fmt = SDC_FG_FB_FORMAT;
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+			params.mem_pp_adc.out_left =
+			    2 + vout->crop_current.left;
+#else
+			params.mem_pp_adc.out_left =
+			    12 + vout->crop_current.left;
+#endif
+			params.mem_pp_adc.out_top = vout->crop_current.top;
+			if (ipu_init_channel(vout->post_proc_ch, &params) != 0) {
+				dev_err(dev, "Error initializing PP chan\n");
+				return -EINVAL;
+			}
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_INPUT_BUFFER,
+						    params.mem_pp_adc.
+						    in_pixel_fmt,
+						    params.mem_pp_adc.in_width,
+						    params.mem_pp_adc.in_height,
+						    vout->v2f.fmt.pix.
+						    bytesperline /
+						    bytes_per_pixel(params.
+								    mem_pp_adc.
+								    in_pixel_fmt),
+						    vout->rotate,
+						    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
+						    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
+						    vout->offset.u_offset,
+						    vout->offset.v_offset) !=
+			    0) {
+				dev_err(dev, "Error initializing PP in buf\n");
+				return -EINVAL;
+			}
+
+			if (ipu_init_channel_buffer(vout->post_proc_ch,
+						    IPU_OUTPUT_BUFFER,
+						    params.mem_pp_adc.
+						    out_pixel_fmt, out_width,
+						    out_height, out_width,
+						    vout->rotate, 0, 0, 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing PP output buffer\n");
+				return -EINVAL;
+			}
+
+		} else {
+			dev_dbg(dev, "Using ADC SYS2 channel\n");
+			vout->display_ch = ADC_SYS2;
+			vout->post_proc_ch = MEM_PP_MEM;
+
+			if (vout->display_bufs[0]) {
+				mxc_free_buffers(vout->display_bufs,
+						 vout->display_bufs_vaddr,
+						 2, vout->display_buf_size);
+			}
+
+			vout->display_buf_size = vout->crop_current.width *
+			    vout->crop_current.height *
+			    fmt_to_bpp(SDC_FG_FB_FORMAT) / 8;
+			mxc_allocate_buffers(vout->display_bufs,
+					     vout->display_bufs_vaddr,
+					     2, vout->display_buf_size);
+
+			memset(&params, 0, sizeof(params));
+			params.adc_sys2.disp = vout->cur_disp_output;
+			params.adc_sys2.ch_mode = WriteTemplateNonSeq;
+#ifdef CONFIG_FB_MXC_EPSON_PANEL
+			params.adc_sys2.out_left = 2 + vout->crop_current.left;
+#else
+			params.adc_sys2.out_left = 12 + vout->crop_current.left;
+#endif
+			params.adc_sys2.out_top = vout->crop_current.top;
+			if (ipu_init_channel(ADC_SYS2, &params) < 0)
+				return -EINVAL;
+
+			if (ipu_init_channel_buffer(vout->display_ch,
+						    IPU_INPUT_BUFFER,
+						    SDC_FG_FB_FORMAT,
+						    out_width, out_height,
+						    out_width, IPU_ROTATE_NONE,
+						    vout->display_bufs[0],
+						    vout->display_bufs[1], 0,
+						    0) != 0) {
+				dev_err(dev,
+					"Error initializing SDC FG buffer\n");
+				return -EINVAL;
+			}
+		}
+	} else
+#endif
+	{			/* Use SDC */
+		unsigned int ipu_ch = CHAN_NONE;
+
+		dev_dbg(dev, "Using SDC channel\n");
+
+		/* Bypass IC if resizing and rotation not needed
+		   Always do CSC in DP
+		   Meanwhile, apply IC bypass to SDC only
+		 */
+		if (out_width == vout->v2f.fmt.pix.width &&
+			out_height == vout->v2f.fmt.pix.height &&
+			ipu_can_rotate_in_place(vout->rotate)) {
+			pr_debug("Bypassing IC\n");
+			vout->ic_bypass = 1;
+			ipu_disable_irq(IPU_IRQ_PP_IN_EOF);
+		} else {
+			vout->ic_bypass = 0;
+		}
+
+#ifdef CONFIG_MXC_IPU_V1
+		/* IPUv1 needs IC to do CSC */
+		if (format_is_yuv(vout->v2f.fmt.pix.pixelformat) !=
+		    format_is_yuv(bpp_to_fmt(fbi)))
+			vout->ic_bypass = 0;
+#endif
+
+		fbvar = fbi->var;
+
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_IPU_CHAN,
+					(unsigned long)&ipu_ch);
+			set_fs(old_fs);
+		}
+
+		if (ipu_ch == CHAN_NONE) {
+			dev_err(dev,
+					"Can not get display ipu channel\n");
+			return -EINVAL;
+		}
+
+		vout->display_ch = ipu_ch;
+
+		if (vout->cur_disp_output == 3 || vout->cur_disp_output == 5) {
+			fbvar.bits_per_pixel = 16;
+			if (format_is_yuv(vout->v2f.fmt.pix.pixelformat))
+				fbvar.nonstd = IPU_PIX_FMT_UYVY;
+			else
+				fbvar.nonstd = 0;
+
+			fbvar.xres = fbvar.xres_virtual = out_width;
+			fbvar.yres = out_height;
+			fbvar.yres_virtual = out_height * 2;
+		}
+
+		if (vout->ic_bypass) {
+			fbvar.bits_per_pixel = 8*
+				bytes_per_pixel(vout->v2f.fmt.pix.pixelformat);
+			fbvar.nonstd = vout->v2f.fmt.pix.pixelformat;
+		}
+
+		fbvar.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbvar);
+
+		fb_pos.x = vout->crop_current.left;
+		fb_pos.y = vout->crop_current.top;
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
+					     (unsigned long)&fb_pos);
+			set_fs(old_fs);
+		}
+
+		vout->display_bufs[1] = fbi->fix.smem_start;
+		vout->display_bufs[0] = fbi->fix.smem_start +
+		    (fbi->fix.line_length * fbi->var.yres);
+		vout->display_buf_size = vout->crop_current.width *
+		    vout->crop_current.height * fbi->var.bits_per_pixel / 8;
+		if (INTERLACED_CONTENT(vout))
+			vout->post_proc_ch = MEM_VDI_PRP_VF_MEM;
+		else
+			vout->post_proc_ch = MEM_PP_MEM;
+	}
+
+	/* Init PP */
+	if (use_direct_adc == false && !vout->ic_bypass) {
+		if (INTERLACED_CONTENT(vout)) {
+			vout->post_proc_ch = MEM_VDI_PRP_VF_MEM;
+			ipu_enable_irq(IPU_IRQ_PRP_VF_OUT_EOF);
+		} else {
+			vout->post_proc_ch = MEM_PP_MEM;
+			ipu_enable_irq(IPU_IRQ_PP_IN_EOF);
+		}
+
+		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
+			out_width = vout->crop_current.height;
+			out_height = vout->crop_current.width;
+		}
+		memset(&params, 0, sizeof(params));
+		int rc;
+		if (INTERLACED_CONTENT(vout)) {
+			rc = init_VDI(params, vout, dev, fbi, &display_input_ch,
+				      out_width, out_height);
+		} else {
+			rc = init_PP(params, vout, dev, fbi, &display_input_ch,
+				     out_width, out_height);
+		}
+		if (rc < 0)
+			return rc;
+		if (ipu_link_channels(display_input_ch, vout->display_ch) < 0) {
+			dev_err(dev, "Error linking ipu channels\n");
+			return -EINVAL;
+		}
+	}
+
+	vout->state = STATE_STREAM_PAUSED;
+
+	if (use_direct_adc == false) {
+		if (!vout->ic_bypass) {
+#ifndef CONFIG_MXC_IPU_V1
+			ipu_enable_channel(vout->post_proc_ch);
+#endif
+			if (LOAD_3FIELDS(vout)) {
+				ipu_enable_channel(MEM_VDI_PRP_VF_MEM_P);
+				ipu_enable_channel(MEM_VDI_PRP_VF_MEM_N);
+				ipu_select_multi_vdi_buffer(0);
+			} else if (INTERLACED_CONTENT(vout)) {
+				ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+			} else {
+				ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+				ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1);
+			}
+			ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0);
+			ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 1);
+#ifdef CONFIG_MXC_IPU_V1
+			ipu_enable_channel(vout->post_proc_ch);
+#endif
+		} else {
+			ipu_update_channel_buffer(vout->display_ch,
+				IPU_INPUT_BUFFER,
+				0, vout->v4l2_bufs[vout->ipu_buf[0]].m.offset);
+			ipu_update_channel_buffer(vout->display_ch,
+				IPU_INPUT_BUFFER,
+				1, vout->v4l2_bufs[vout->ipu_buf[1]].m.offset);
+			ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 0);
+			ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 1);
+		}
+		disp_irq = get_display_irq(vout);
+		ipu_request_irq(disp_irq, mxc_v4l2out_disp_refresh_irq_handler,
+				0, NULL, vout);
+
+		if (fbi) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_UNBLANK);
+			release_console_sem();
+		} else {
+			ipu_enable_channel(vout->display_ch);
+		}
+	} else {
+		ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+		ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1);
+		ipu_enable_channel(vout->post_proc_ch);
+	}
+	vout->start_jiffies = jiffies;
+
+	msleep(1);
+
+	dev_dbg(dev,
+		"streamon: start time = %lu jiffies\n", vout->start_jiffies);
+
+	return 0;
+}
+
+/*!
+ * Shut down the voutera
+ *
+ * @param vout      structure vout_data *
+ *
+ * @return status  0 Success
+ */
+static int mxc_v4l2out_streamoff(vout_data * vout)
+{
+	struct fb_info *fbi =
+	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	int i, retval = 0, disp_irq = 0;
+	unsigned long lockflag = 0;
+
+	if (!vout)
+		return -EINVAL;
+
+	if (vout->state == STATE_STREAM_OFF) {
+		return 0;
+	}
+
+	if (INTERLACED_CONTENT(vout))
+		ipu_free_irq(IPU_IRQ_PRP_VF_OUT_EOF, vout);
+	else
+		ipu_free_irq(IPU_IRQ_PP_IN_EOF, vout);
+
+	spin_lock_irqsave(&g_lock, lockflag);
+
+	del_timer(&vout->output_timer);
+
+	if (vout->state == STATE_STREAM_ON) {
+		vout->state = STATE_STREAM_STOPPING;
+	}
+
+	if (!vout->ic_bypass) {
+		if (INTERLACED_CONTENT(vout))
+			ipu_disable_irq(IPU_IRQ_PRP_VF_OUT_EOF);
+		else
+			ipu_disable_irq(IPU_IRQ_PP_IN_EOF);
+	}
+
+	spin_unlock_irqrestore(&g_lock, lockflag);
+
+	pending_buffer = 0;
+	disp_irq = get_display_irq(vout);
+	ipu_free_irq(disp_irq, vout);
+
+	if (vout->display_ch == MEM_FG_SYNC) {
+		struct mxcfb_pos fb_pos;
+		mm_segment_t old_fs;
+
+		fb_pos.x = 0;
+		fb_pos.y = 0;
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
+					(unsigned long)&fb_pos);
+			set_fs(old_fs);
+		}
+	}
+
+	if (vout->post_proc_ch == MEM_PP_MEM ||
+	    vout->post_proc_ch == MEM_PRP_VF_MEM) {
+		/* SDC or ADC with Rotation */
+		if (!ipu_can_rotate_in_place(vout->rotate)) {
+			ipu_unlink_channels(MEM_PP_MEM, MEM_ROT_PP_MEM);
+			ipu_unlink_channels(MEM_ROT_PP_MEM,
+					    vout->display_ch);
+			ipu_disable_channel(MEM_ROT_PP_MEM, true);
+
+			if (vout->rot_pp_bufs[0]) {
+				mxc_free_buffers(vout->rot_pp_bufs,
+						 vout->rot_pp_bufs_vaddr, 2,
+						 vout->display_buf_size);
+			}
+		} else {
+			ipu_unlink_channels(MEM_PP_MEM, vout->display_ch);
+		}
+		ipu_disable_channel(MEM_PP_MEM, true);
+
+		if (vout->display_ch == ADC_SYS2 ||
+			vout->display_ch == MEM_FG_SYNC) {
+			ipu_disable_channel(vout->display_ch, true);
+			ipu_uninit_channel(vout->display_ch);
+		} else {
+			fbi->var.activate |= FB_ACTIVATE_FORCE;
+			fb_set_var(fbi, &fbi->var);
+
+			if (vout->display_ch == MEM_FG_SYNC) {
+				acquire_console_sem();
+				fb_blank(fbi, FB_BLANK_POWERDOWN);
+				release_console_sem();
+			}
+
+			vout->display_bufs[0] = 0;
+			vout->display_bufs[1] = 0;
+		}
+
+		ipu_uninit_channel(MEM_PP_MEM);
+		if (!ipu_can_rotate_in_place(vout->rotate))
+			ipu_uninit_channel(MEM_ROT_PP_MEM);
+	} else if (INTERLACED_CONTENT(vout) && (vout->post_proc_ch == MEM_VDI_PRP_VF_MEM)) {
+		if (!ipu_can_rotate_in_place(vout->rotate)) {
+			ipu_unlink_channels(MEM_VDI_PRP_VF_MEM,
+					    MEM_ROT_VF_MEM);
+			ipu_unlink_channels(MEM_ROT_VF_MEM,
+					    vout->display_ch);
+			ipu_disable_channel(MEM_ROT_VF_MEM, true);
+
+			if (vout->rot_pp_bufs[0]) {
+				mxc_free_buffers(vout->rot_pp_bufs,
+						 vout->rot_pp_bufs_vaddr, 2,
+						 vout->display_buf_size);
+			}
+		} else {
+			ipu_unlink_channels(MEM_VDI_PRP_VF_MEM,
+					    vout->display_ch);
+		}
+
+		ipu_disable_channel(MEM_VDI_PRP_VF_MEM, true);
+
+		if (vout->display_ch == ADC_SYS2 ||
+			vout->display_ch == MEM_FG_SYNC) {
+			ipu_disable_channel(vout->display_ch, true);
+			ipu_uninit_channel(vout->display_ch);
+		} else {
+			fbi->var.activate |= FB_ACTIVATE_FORCE;
+			fb_set_var(fbi, &fbi->var);
+
+			if (vout->display_ch == MEM_FG_SYNC) {
+				acquire_console_sem();
+				fb_blank(fbi, FB_BLANK_POWERDOWN);
+				release_console_sem();
+			}
+
+			vout->display_bufs[0] = 0;
+			vout->display_bufs[1] = 0;
+		}
+
+		ipu_uninit_channel(MEM_VDI_PRP_VF_MEM);
+		if (!ipu_can_rotate_in_place(vout->rotate))
+			ipu_uninit_channel(MEM_ROT_VF_MEM);
+	} else {		/* ADC Direct */
+		ipu_disable_channel(MEM_PP_ADC, true);
+		ipu_uninit_channel(MEM_PP_ADC);
+	}
+	vout->ready_q.head = vout->ready_q.tail = 0;
+	vout->done_q.head = vout->done_q.tail = 0;
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		vout->v4l2_bufs[i].flags = 0;
+		vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+		vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+	}
+
+	vout->state = STATE_STREAM_OFF;
+
+#ifdef CONFIG_FB_MXC_ASYNC_PANEL
+	if (vout->cur_disp_output < DISP3) {
+		if (vout->display_bufs[0] != 0) {
+			mxc_free_buffers(vout->display_bufs,
+					 vout->display_bufs_vaddr, 2,
+					 vout->display_buf_size);
+		}
+
+		mxcfb_set_refresh_mode(registered_fb
+				       [vout->
+					output_fb_num[vout->cur_disp_output]],
+				       MXCFB_REFRESH_PARTIAL, 0);
+	}
+#endif
+
+	return retval;
+}
+
+/*
+ * Valid whether the palette is supported
+ *
+ * @param palette  V4L2_PIX_FMT_RGB565, V4L2_PIX_FMT_BGR24 or V4L2_PIX_FMT_BGR32
+ *
+ * @return 1 if supported, 0 if failed
+ */
+static inline int valid_mode(u32 palette)
+{
+	return ((palette == V4L2_PIX_FMT_RGB565) ||
+		(palette == V4L2_PIX_FMT_BGR24) ||
+		(palette == V4L2_PIX_FMT_RGB24) ||
+		(palette == V4L2_PIX_FMT_BGR32) ||
+		(palette == V4L2_PIX_FMT_RGB32) ||
+		(palette == V4L2_PIX_FMT_NV12) ||
+		(palette == V4L2_PIX_FMT_YUV422P) ||
+		(palette == V4L2_PIX_FMT_YUV420));
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_g_fmt(vout_data * vout, struct v4l2_format *f)
+{
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		return -EINVAL;
+	}
+	*f = vout->v2f;
+	return 0;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_FMT Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param v4l2_format structure v4l2_format *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2out_s_fmt(vout_data * vout, struct v4l2_format *f)
+{
+	int retval = 0;
+	u32 size = 0;
+	u32 bytesperline;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		retval = -EINVAL;
+		goto err0;
+	}
+	if (!valid_mode(f->fmt.pix.pixelformat)) {
+		dev_err(&vout->video_dev->dev, "pixel format not supported\n");
+		retval = -EINVAL;
+		goto err0;
+	}
+
+	bytesperline = (f->fmt.pix.width * fmt_to_bpp(f->fmt.pix.pixelformat)) /
+	    8;
+	if (f->fmt.pix.bytesperline < bytesperline) {
+		f->fmt.pix.bytesperline = bytesperline;
+	} else {
+		bytesperline = f->fmt.pix.bytesperline;
+	}
+	vout->bytesperline = bytesperline;
+
+	/* Based on http://v4l2spec.bytesex.org/spec/x6386.htm#V4L2-FIELD */
+	vout->field_fmt = f->fmt.pix.field;
+	switch (vout->field_fmt) {
+		/* Images are in progressive format, not interlaced */
+	case V4L2_FIELD_NONE:
+		break;
+		/* The two fields of a frame are passed in separate buffers,
+		   in temporal order, i. e. the older one first. */
+	case V4L2_FIELD_ALTERNATE:
+		dev_err(&vout->video_dev->dev,
+			"V4L2_FIELD_ALTERNATE field format not supported yet!\n");
+		break;
+	case V4L2_FIELD_INTERLACED_TB:
+		if (cpu_is_mx51())
+			break;
+	case V4L2_FIELD_INTERLACED_BT:
+		dev_err(&vout->video_dev->dev,
+			"V4L2_FIELD_INTERLACED_BT field format not supported yet!\n");
+	default:
+		vout->field_fmt = V4L2_FIELD_NONE;
+		break;
+	}
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV422P:
+		/* byteperline for YUV planar formats is for
+		   Y plane only */
+		size = bytesperline * f->fmt.pix.height * 2;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_NV12:
+		size = (bytesperline * f->fmt.pix.height * 3) / 2;
+		break;
+	default:
+		size = bytesperline * f->fmt.pix.height;
+		break;
+	}
+
+	/* Return the actual size of the image to the app */
+	if (f->fmt.pix.sizeimage < size) {
+		f->fmt.pix.sizeimage = size;
+	} else {
+		size = f->fmt.pix.sizeimage;
+	}
+
+	vout->v2f.fmt.pix = f->fmt.pix;
+	if (vout->v2f.fmt.pix.priv != 0) {
+		if (copy_from_user(&vout->offset,
+				   (void *)vout->v2f.fmt.pix.priv,
+				   sizeof(vout->offset))) {
+			retval = -EFAULT;
+			goto err0;
+		}
+	} else {
+		vout->offset.u_offset = 0;
+		vout->offset.v_offset = 0;
+	}
+
+	retval = 0;
+      err0:
+	return retval;
+}
+
+/*
+ * V4L2 - Handles VIDIOC_G_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_get_v42lout_control(vout_data * vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		return (vout->rotate & IPU_ROTATE_HORIZ_FLIP) ? 1 : 0;
+	case V4L2_CID_VFLIP:
+		return (vout->rotate & IPU_ROTATE_VERT_FLIP) ? 1 : 0;
+	case (V4L2_CID_PRIVATE_BASE + 1):
+		return vout->rotate;
+	default:
+		return -EINVAL;
+	}
+}
+
+/*
+ * V4L2 - Handles VIDIOC_S_CTRL Ioctl
+ *
+ * @param vout         structure vout_data *
+ *
+ * @param c           structure v4l2_control *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_set_v42lout_control(vout_data * vout, struct v4l2_control *c)
+{
+	switch (c->id) {
+	case V4L2_CID_HFLIP:
+		vout->rotate |= c->value ? IPU_ROTATE_HORIZ_FLIP :
+		    IPU_ROTATE_NONE;
+		break;
+	case V4L2_CID_VFLIP:
+		vout->rotate |= c->value ? IPU_ROTATE_VERT_FLIP :
+		    IPU_ROTATE_NONE;
+		break;
+	case V4L2_CID_MXC_ROT:
+		vout->rotate = c->value;
+		break;
+	case V4L2_CID_MXC_MOTION:
+		vout->motion_sel = c->value;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*!
+ * V4L2 interface - open function
+ *
+ * @param inode        structure inode *
+ *
+ * @param file         structure file *
+ *
+ * @return  status    0 success, ENODEV invalid device instance,
+ *                    ENODEV timeout, ERESTARTSYS interrupted by user
+ */
+static int mxc_v4l2out_open(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+	int err;
+
+	if (!vout) {
+		return -ENODEV;
+	}
+
+	down(&vout->busy_lock);
+
+	err = -EINTR;
+	if (signal_pending(current))
+		goto oops;
+
+
+	if (vout->open_count++ == 0) {
+		init_waitqueue_head(&vout->v4l_bufq);
+
+		init_timer(&vout->output_timer);
+		vout->output_timer.function = mxc_v4l2out_timer_handler;
+		vout->output_timer.data = (unsigned long)vout;
+
+		vout->state = STATE_STREAM_OFF;
+		vout->rotate = IPU_ROTATE_NONE;
+		g_irq_cnt = g_buf_output_cnt = g_buf_q_cnt = g_buf_dq_cnt = 0;
+
+	}
+
+	file->private_data = dev;
+
+	up(&vout->busy_lock);
+
+	return 0;
+
+      oops:
+	up(&vout->busy_lock);
+	return err;
+}
+
+/*!
+ * V4L2 interface - close function
+ *
+ * @param inode    struct inode *
+ *
+ * @param file     struct file *
+ *
+ * @return         0 success
+ */
+static int mxc_v4l2out_close(struct inode *inode, struct file *file)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+
+	if (--vout->open_count == 0) {
+		if (vout->state != STATE_STREAM_OFF)
+			mxc_v4l2out_streamoff(vout);
+
+		file->private_data = NULL;
+
+		mxc_free_buffers(vout->queue_buf_paddr, vout->queue_buf_vaddr,
+				 vout->buffer_cnt, vout->queue_buf_size);
+		vout->buffer_cnt = 0;
+		mxc_free_buffers(vout->rot_pp_bufs, vout->rot_pp_bufs_vaddr, 2,
+				 vout->display_buf_size);
+
+		/* capture off */
+		wake_up_interruptible(&vout->v4l_bufq);
+
+	}
+
+	return 0;
+}
+
+/*!
+ * V4L2 interface - ioctl function
+ *
+ * @param inode      struct inode *
+ *
+ * @param file       struct file *
+ *
+ * @param ioctlnr    unsigned int
+ *
+ * @param arg        void *
+ *
+ * @return           0 success, ENODEV for invalid device instance,
+ *                   -1 for other errors.
+ */
+static int
+mxc_v4l2out_do_ioctl(struct inode *inode, struct file *file,
+		     unsigned int ioctlnr, void *arg)
+{
+	struct video_device *vdev = file->private_data;
+	vout_data *vout = video_get_drvdata(vdev);
+	int retval = 0;
+	int i = 0;
+
+	if (!vout)
+		return -EBADF;
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EBUSY;
+
+	switch (ioctlnr) {
+	case VIDIOC_QUERYCAP:
+		{
+			struct v4l2_capability *cap = arg;
+			strcpy(cap->driver, "mxc_v4l2_output");
+			cap->version = 0;
+			cap->capabilities =
+			    V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+			cap->card[0] = '\0';
+			cap->bus_info[0] = '\0';
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_FMT:
+		{
+			struct v4l2_format *gf = arg;
+			retval = mxc_v4l2out_g_fmt(vout, gf);
+			break;
+		}
+	case VIDIOC_S_FMT:
+		{
+			struct v4l2_format *sf = arg;
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+			retval = mxc_v4l2out_s_fmt(vout, sf);
+			break;
+		}
+	case VIDIOC_REQBUFS:
+		{
+			struct v4l2_requestbuffers *req = arg;
+			if ((req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (req->memory != V4L2_MEMORY_MMAP)) {
+				dev_dbg(&vdev->dev,
+					"VIDIOC_REQBUFS: incorrect buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+
+			if (req->count == 0)
+				mxc_v4l2out_streamoff(vout);
+
+			if (vout->state == STATE_STREAM_OFF) {
+				if (vout->queue_buf_paddr[0] != 0) {
+					mxc_free_buffers(vout->queue_buf_paddr,
+							 vout->queue_buf_vaddr,
+							 vout->buffer_cnt,
+							 vout->queue_buf_size);
+					dev_dbg(&vdev->dev,
+						"VIDIOC_REQBUFS: freed buffers\n");
+				}
+				vout->buffer_cnt = 0;
+			} else {
+				dev_dbg(&vdev->dev,
+					"VIDIOC_REQBUFS: Buffer is in use\n");
+				retval = -EBUSY;
+				break;
+			}
+
+			if (req->count == 0)
+				break;
+
+			if (req->count < MIN_FRAME_NUM) {
+				req->count = MIN_FRAME_NUM;
+			} else if (req->count > MAX_FRAME_NUM) {
+				req->count = MAX_FRAME_NUM;
+			}
+			vout->buffer_cnt = req->count;
+			vout->queue_buf_size =
+			    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+
+			retval = mxc_allocate_buffers(vout->queue_buf_paddr,
+						      vout->queue_buf_vaddr,
+						      vout->buffer_cnt,
+						      vout->queue_buf_size);
+			if (retval < 0)
+				break;
+
+			/* Init buffer queues */
+			vout->done_q.head = 0;
+			vout->done_q.tail = 0;
+			vout->ready_q.head = 0;
+			vout->ready_q.tail = 0;
+
+			for (i = 0; i < vout->buffer_cnt; i++) {
+				memset(&(vout->v4l2_bufs[i]), 0,
+				       sizeof(vout->v4l2_bufs[i]));
+				vout->v4l2_bufs[i].flags = 0;
+				vout->v4l2_bufs[i].memory = V4L2_MEMORY_MMAP;
+				vout->v4l2_bufs[i].index = i;
+				vout->v4l2_bufs[i].type =
+				    V4L2_BUF_TYPE_VIDEO_OUTPUT;
+				vout->v4l2_bufs[i].length =
+				    PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
+				vout->v4l2_bufs[i].m.offset =
+				    (unsigned long)vout->queue_buf_paddr[i];
+				vout->v4l2_bufs[i].timestamp.tv_sec = 0;
+				vout->v4l2_bufs[i].timestamp.tv_usec = 0;
+			}
+			break;
+		}
+	case VIDIOC_QUERYBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			u32 type = buf->type;
+			int index = buf->index;
+
+			if ((type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				dev_dbg(&vdev->dev,
+					"VIDIOC_QUERYBUFS: incorrect buffer type\n");
+				retval = -EINVAL;
+				break;
+			}
+			down(&vout->param_lock);
+			memcpy(buf, &(vout->v4l2_bufs[index]), sizeof(*buf));
+			up(&vout->param_lock);
+			break;
+		}
+	case VIDIOC_QBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int index = buf->index;
+			unsigned long lock_flags;
+			int param[5][3];
+
+			if ((buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
+			    (index >= vout->buffer_cnt)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			dev_dbg(&vdev->dev, "VIDIOC_QBUF: %d field = %d\n", buf->index, buf->field);
+
+			/* mmapped buffers are L1 WB cached,
+			 * so we need to clean them */
+			if (buf->memory & V4L2_MEMORY_MMAP) {
+				flush_cache_all();
+			}
+
+			spin_lock_irqsave(&g_lock, lock_flags);
+
+			memcpy(&(vout->v4l2_bufs[index]), buf, sizeof(*buf));
+			vout->v4l2_bufs[index].flags |= V4L2_BUF_FLAG_QUEUED;
+
+			g_buf_q_cnt++;
+			if (vout->v4l2_bufs[index].reserved)
+				if (!copy_from_user(&param[0][0],
+						    (void *)vout->
+						    v4l2_bufs[index]
+						    .reserved, sizeof(param)))
+					ipu_set_csc_coefficients(vout->
+								 display_ch,
+								 param);
+			queue_buf(&vout->ready_q, index);
+			if (vout->state == STATE_STREAM_PAUSED) {
+				unsigned long timeout;
+
+				index = peek_next_buf(&vout->ready_q);
+
+				/* if timestamp is 0, then default to 30fps */
+				if ((vout->v4l2_bufs[index].timestamp.tv_sec ==
+				     0)
+				    && (vout->v4l2_bufs[index].timestamp.
+					tv_usec == 0))
+					timeout =
+					    vout->start_jiffies +
+					    vout->frame_count * HZ / 30;
+				else
+					timeout =
+					    get_jiffies(&vout->v4l2_bufs[index].
+							timestamp);
+
+				if (jiffies >= timeout) {
+					dev_dbg(&vout->video_dev->dev,
+						"warning: timer timeout already expired.\n");
+				}
+				vout->output_timer.expires = timeout;
+				dev_dbg(&vdev->dev,
+					"QBUF: frame #%u timeout @ %lu jiffies, current = %lu\n",
+					vout->frame_count, timeout, jiffies);
+				add_timer(&vout->output_timer);
+				vout->state = STATE_STREAM_ON;
+			}
+
+			spin_unlock_irqrestore(&g_lock, lock_flags);
+			break;
+		}
+	case VIDIOC_DQBUF:
+		{
+			struct v4l2_buffer *buf = arg;
+			int idx;
+
+			if ((queue_size(&vout->done_q) == 0) &&
+			    (file->f_flags & O_NONBLOCK)) {
+				retval = -EAGAIN;
+				break;
+			}
+
+			if (!wait_event_interruptible_timeout(vout->v4l_bufq,
+							      queue_size(&vout->
+									 done_q)
+							      != 0, 10 * HZ)) {
+				dev_dbg(&vdev->dev, "VIDIOC_DQBUF: timeout\n");
+				retval = -ETIME;
+				break;
+			} else if (signal_pending(current)) {
+				dev_dbg(&vdev->dev,
+					"VIDIOC_DQBUF: interrupt received\n");
+				retval = -ERESTARTSYS;
+				break;
+			}
+			idx = dequeue_buf(&vout->done_q);
+			if (idx == -1) {	/* No frame free */
+				dev_dbg(&vdev->dev,
+					"VIDIOC_DQBUF: no free buffers, returning\n");
+				retval = -EAGAIN;
+				break;
+			}
+			if ((vout->v4l2_bufs[idx].flags & V4L2_BUF_FLAG_DONE) ==
+			    0)
+				dev_dbg(&vdev->dev,
+					"VIDIOC_DQBUF: buffer in done q, but not "
+					"flagged as done\n");
+
+			vout->v4l2_bufs[idx].flags = 0;
+			memcpy(buf, &(vout->v4l2_bufs[idx]), sizeof(*buf));
+			dev_dbg(&vdev->dev, "VIDIOC_DQBUF: %d\n", buf->index);
+			break;
+		}
+	case VIDIOC_STREAMON:
+		{
+			retval = mxc_v4l2out_streamon(vout);
+			break;
+		}
+	case VIDIOC_STREAMOFF:
+		{
+			retval = mxc_v4l2out_streamoff(vout);
+			break;
+		}
+	case VIDIOC_G_CTRL:
+		{
+			retval = mxc_get_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_S_CTRL:
+		{
+			retval = mxc_set_v42lout_control(vout, arg);
+			break;
+		}
+	case VIDIOC_CROPCAP:
+		{
+			struct v4l2_cropcap *cap = arg;
+
+			if (cap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+
+			cap->bounds = vout->crop_bounds[vout->cur_disp_output];
+			cap->defrect = vout->crop_bounds[vout->cur_disp_output];
+			retval = 0;
+			break;
+		}
+	case VIDIOC_G_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			crop->c = vout->crop_current;
+			break;
+		}
+	case VIDIOC_S_CROP:
+		{
+			struct v4l2_crop *crop = arg;
+			struct v4l2_rect *b =
+			    &(vout->crop_bounds[vout->cur_disp_output]);
+
+			if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.height < 0) {
+				retval = -EINVAL;
+				break;
+			}
+			if (crop->c.width < 0) {
+				retval = -EINVAL;
+				break;
+			}
+
+			/* only full screen supported for SDC BG and SDC DC */
+			if (vout->cur_disp_output == 4 ||
+			    vout->cur_disp_output == 5) {
+				crop->c = vout->crop_current;
+				break;
+			}
+
+			if (crop->c.top < b->top)
+				crop->c.top = b->top;
+			if (crop->c.top >= b->top + b->height)
+				crop->c.top = b->top + b->height - 1;
+			if (crop->c.height > b->top - crop->c.top + b->height)
+				crop->c.height =
+				    b->top - crop->c.top + b->height;
+
+			if (crop->c.left < b->left)
+				crop->c.left = b->left;
+			if (crop->c.left >= b->left + b->width)
+				crop->c.left = b->left + b->width - 1;
+			if (crop->c.width > b->left - crop->c.left + b->width)
+				crop->c.width =
+				    b->left - crop->c.left + b->width;
+
+			/* stride line limitation */
+			crop->c.height -= crop->c.height % 8;
+			crop->c.width -= crop->c.width % 8;
+
+			vout->crop_current = crop->c;
+			break;
+		}
+	case VIDIOC_ENUMOUTPUT:
+		{
+			struct v4l2_output *output = arg;
+
+			if ((output->index >= 5) ||
+			    (vout->output_enabled[output->index] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (output->index < 3) {
+				*output = mxc_outputs[MXC_V4L2_OUT_2_ADC];
+				output->name[4] = '0' + output->index;
+			} else {
+				*output = mxc_outputs[MXC_V4L2_OUT_2_SDC];
+			}
+			break;
+		}
+	case VIDIOC_G_OUTPUT:
+		{
+			int *p_output_num = arg;
+
+			*p_output_num = vout->cur_disp_output;
+			break;
+		}
+	case VIDIOC_S_OUTPUT:
+		{
+			int *p_output_num = arg;
+			int fbnum;
+			struct v4l2_rect *b;
+
+			if ((*p_output_num >= MXC_V4L2_OUT_NUM_OUTPUTS) ||
+			    (vout->output_enabled[*p_output_num] == false)) {
+				retval = -EINVAL;
+				break;
+			}
+
+			if (vout->state != STATE_STREAM_OFF) {
+				retval = -EBUSY;
+				break;
+			}
+
+			vout->cur_disp_output = *p_output_num;
+
+			/* Update bounds in case they have changed */
+			b = &vout->crop_bounds[vout->cur_disp_output];
+
+			fbnum = vout->output_fb_num[vout->cur_disp_output];
+
+			/*
+			 * For FG overlay, it uses BG window parameter as
+			 * limitation reference; and BG must be enabled to
+			 * support FG.
+			 */
+			if (vout->cur_disp_output == 3) {
+				unsigned int i, ipu_ch = CHAN_NONE;
+				struct fb_info *fbi;
+				mm_segment_t old_fs;
+
+				for (i = 0; i < num_registered_fb; i++) {
+					fbi = registered_fb[i];
+					if (fbi->fbops->fb_ioctl) {
+						old_fs = get_fs();
+						set_fs(KERNEL_DS);
+						fbi->fbops->fb_ioctl(fbi,
+							MXCFB_GET_FB_IPU_CHAN,
+							(unsigned long)&ipu_ch);
+						set_fs(old_fs);
+					}
+					if (ipu_ch == CHAN_NONE) {
+						dev_err(&vdev->dev,
+						"Can't get disp ipu channel\n");
+						retval = -EINVAL;
+						break;
+					}
+
+					if (ipu_ch == MEM_BG_SYNC) {
+						fbnum = i;
+						break;
+					}
+				}
+			}
+
+			b->width = registered_fb[fbnum]->var.xres;
+			b->height = registered_fb[fbnum]->var.yres;
+
+			vout->crop_current = *b;
+			break;
+		}
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_TRY_FMT:
+	case VIDIOC_QUERYCTRL:
+	case VIDIOC_G_PARM:
+	case VIDIOC_ENUMSTD:
+	case VIDIOC_G_STD:
+	case VIDIOC_S_STD:
+	case VIDIOC_G_TUNER:
+	case VIDIOC_S_TUNER:
+	case VIDIOC_G_FREQUENCY:
+	case VIDIOC_S_FREQUENCY:
+	default:
+		retval = -EINVAL;
+		break;
+	}
+
+	up(&vout->busy_lock);
+	return retval;
+}
+
+/*
+ * V4L2 interface - ioctl function
+ *
+ * @return  None
+ */
+static int
+mxc_v4l2out_ioctl(struct inode *inode, struct file *file,
+		  unsigned int cmd, unsigned long arg)
+{
+	return video_usercopy(inode, file, cmd, arg, mxc_v4l2out_do_ioctl);
+}
+
+/*!
+ * V4L2 interface - mmap function
+ *
+ * @param file          structure file *
+ *
+ * @param vma           structure vm_area_struct *
+ *
+ * @return status       0 Success, EINTR busy lock error,
+ *                      ENOBUFS remap_page error
+ */
+static int mxc_v4l2out_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct video_device *vdev = video_devdata(file);
+	unsigned long size = vma->vm_end - vma->vm_start;
+	int res = 0;
+	int i;
+	vout_data *vout = video_get_drvdata(vdev);
+
+	dev_dbg(&vdev->dev, "pgoff=0x%lx, start=0x%lx, end=0x%lx\n",
+		vma->vm_pgoff, vma->vm_start, vma->vm_end);
+
+	/* make this _really_ smp-safe */
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	for (i = 0; i < vout->buffer_cnt; i++) {
+		if ((vout->v4l2_bufs[i].m.offset ==
+		     (vma->vm_pgoff << PAGE_SHIFT)) &&
+		    (vout->v4l2_bufs[i].length >= size)) {
+			vout->v4l2_bufs[i].flags |= V4L2_BUF_FLAG_MAPPED;
+			break;
+		}
+	}
+	if (i == vout->buffer_cnt) {
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	/* make buffers inner write-back, outer write-thru cacheable */
+	/* vma->vm_page_prot = pgprot_outer_wrthru(vma->vm_page_prot);*/
+
+	if (remap_pfn_range(vma, vma->vm_start,
+			    vma->vm_pgoff, size, vma->vm_page_prot)) {
+		dev_dbg(&vdev->dev, "mmap remap_pfn_range failed\n");
+		res = -ENOBUFS;
+		goto mxc_mmap_exit;
+	}
+
+	vma->vm_flags &= ~VM_IO;	/* using shared anonymous pages */
+
+      mxc_mmap_exit:
+	up(&vout->busy_lock);
+	return res;
+}
+
+/*!
+ * V4L2 interface - poll function
+ *
+ * @param file       structure file *
+ *
+ * @param wait       structure poll_table *
+ *
+ * @return  status   POLLIN | POLLRDNORM
+ */
+static unsigned int mxc_v4l2out_poll(struct file *file, poll_table * wait)
+{
+	struct video_device *dev = video_devdata(file);
+	vout_data *vout = video_get_drvdata(dev);
+
+	wait_queue_head_t *queue = NULL;
+	int res = POLLIN | POLLRDNORM;
+
+	if (down_interruptible(&vout->busy_lock))
+		return -EINTR;
+
+	queue = &vout->v4l_bufq;
+	poll_wait(file, queue, wait);
+
+	up(&vout->busy_lock);
+	return res;
+}
+
+static struct
+file_operations mxc_v4l2out_fops = {
+	.owner = THIS_MODULE,
+	.open = mxc_v4l2out_open,
+	.release = mxc_v4l2out_close,
+	.ioctl = mxc_v4l2out_ioctl,
+	.mmap = mxc_v4l2out_mmap,
+	.poll = mxc_v4l2out_poll,
+};
+
+static struct video_device mxc_v4l2out_template = {
+	.name = "MXC Video Output",
+	.vfl_type = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING,
+	.fops = &mxc_v4l2out_fops,
+	.release = video_device_release,
+};
+
+/*!
+ * Probe routine for the framebuffer driver. It is called during the
+ * driver binding process.      The following functions are performed in
+ * this routine: Framebuffer initialization, Memory allocation and
+ * mapping, Framebuffer registration, IPU initialization.
+ *
+ * @return      Appropriate error code to the kernel common code
+ */
+static int mxc_v4l2out_probe(struct platform_device *pdev)
+{
+	int i;
+	vout_data *vout;
+
+	/*
+	 * Allocate sufficient memory for the fb structure
+	 */
+	g_vout = vout = kmalloc(sizeof(vout_data), GFP_KERNEL);
+
+	if (!vout)
+		return 0;
+
+	memset(vout, 0, sizeof(vout_data));
+
+	vout->video_dev = video_device_alloc();
+	if (vout->video_dev == NULL)
+		return -1;
+	vout->video_dev->minor = -1;
+
+	*(vout->video_dev) = mxc_v4l2out_template;
+
+	/* register v4l device */
+	if (video_register_device(vout->video_dev,
+				  VFL_TYPE_GRABBER, video_nr) == -1) {
+		dev_dbg(&pdev->dev, "video_register_device failed\n");
+		return 0;
+	}
+	dev_info(&pdev->dev, "Registered device video%d\n",
+		 vout->video_dev->minor & 0x1f);
+	/*vout->video_dev->dev = &pdev->dev;*/
+
+	video_set_drvdata(vout->video_dev, vout);
+
+	init_MUTEX(&vout->param_lock);
+	init_MUTEX(&vout->busy_lock);
+
+	/* setup outputs and cropping */
+	vout->cur_disp_output = -1;
+	for (i = 0; i < num_registered_fb; i++) {
+		char *idstr = registered_fb[i]->fix.id;
+		if (strncmp(idstr, "DISP", 4) == 0) {
+			int disp_num = idstr[4] - '0';
+			if (disp_num == 3) {
+				if (strcmp(idstr, "DISP3 BG - DI1") == 0)
+					disp_num = 5;
+				else if (strncmp(idstr, "DISP3 BG", 8) == 0)
+					disp_num = 4;
+			}
+			vout->crop_bounds[disp_num].left = 0;
+			vout->crop_bounds[disp_num].top = 0;
+			vout->crop_bounds[disp_num].width =
+			    registered_fb[i]->var.xres;
+			vout->crop_bounds[disp_num].height =
+			    registered_fb[i]->var.yres;
+			vout->output_enabled[disp_num] = true;
+			vout->output_fb_num[disp_num] = i;
+			if (vout->cur_disp_output == -1) {
+				vout->cur_disp_output = disp_num;
+			}
+		}
+
+	}
+	vout->crop_current = vout->crop_bounds[vout->cur_disp_output];
+
+	platform_set_drvdata(pdev, vout);
+
+	return 0;
+}
+
+static int mxc_v4l2out_remove(struct platform_device *pdev)
+{
+	vout_data *vout = platform_get_drvdata(pdev);
+
+	if (vout->video_dev) {
+		if (-1 != vout->video_dev->minor)
+			video_unregister_device(vout->video_dev);
+		else
+			video_device_release(vout->video_dev);
+		vout->video_dev = NULL;
+	}
+
+	platform_set_drvdata(pdev, NULL);
+
+	kfree(vout);
+
+	return 0;
+}
+
+/*!
+ * This structure contains pointers to the power management callback functions.
+ */
+static struct platform_driver mxc_v4l2out_driver = {
+	.driver = {
+		   .name = "MXC Video Output",
+		   },
+	.probe = mxc_v4l2out_probe,
+	.remove = mxc_v4l2out_remove,
+};
+
+static struct platform_device mxc_v4l2out_device = {
+	.name = "MXC Video Output",
+	.id = 0,
+};
+
+/*!
+ * mxc v4l2 init function
+ *
+ */
+static int mxc_v4l2out_init(void)
+{
+	u8 err = 0;
+
+	err = platform_driver_register(&mxc_v4l2out_driver);
+	if (err == 0) {
+		platform_device_register(&mxc_v4l2out_device);
+	}
+	return err;
+}
+
+/*!
+ * mxc v4l2 cleanup function
+ *
+ */
+static void mxc_v4l2out_clean(void)
+{
+	video_unregister_device(g_vout->video_dev);
+
+	platform_driver_unregister(&mxc_v4l2out_driver);
+	platform_device_unregister(&mxc_v4l2out_device);
+	kfree(g_vout);
+	g_vout = NULL;
+}
+
+module_init(mxc_v4l2out_init);
+module_exit(mxc_v4l2out_clean);
+
+module_param(video_nr, int, 0444);
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("V4L2-driver for MXC video output");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("video");
diff --git a/drivers/media/video/mxc/output/mxc_v4l2_output.h b/drivers/media/video/mxc/output/mxc_v4l2_output.h
new file mode 100644
index 000000000000..069edde1e850
--- /dev/null
+++ b/drivers/media/video/mxc/output/mxc_v4l2_output.h
@@ -0,0 +1,138 @@
+/*
+ * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+/*!
+ * @defgroup MXC_V4L2_OUTPUT MXC V4L2 Video Output Driver
+ */
+/*!
+ * @file mxc_v4l2_output.h
+ *
+ * @brief MXC V4L2 Video Output Driver Header file
+ *
+ * Video4Linux2 Output Device using MXC IPU Post-processing functionality.
+ *
+ * @ingroup MXC_V4L2_OUTPUT
+ */
+#ifndef __MXC_V4L2_OUTPUT_H__
+#define __MXC_V4L2_OUTPUT_H__
+
+#include <media/v4l2-dev.h>
+
+#ifdef __KERNEL__
+
+#include <linux/ipu.h>
+#include <linux/mxc_v4l2.h>
+#include <linux/videodev2.h>
+
+#define MIN_FRAME_NUM 2
+#define MAX_FRAME_NUM 30
+
+#define MXC_V4L2_OUT_NUM_OUTPUTS        6
+#define MXC_V4L2_OUT_2_SDC              0
+#define MXC_V4L2_OUT_2_ADC              1
+
+
+typedef struct {
+	int list[MAX_FRAME_NUM + 1];
+	int head;
+	int tail;
+} v4l_queue;
+
+/*!
+ * States for the video stream
+ */
+typedef enum {
+	STATE_STREAM_OFF,
+	STATE_STREAM_ON,
+	STATE_STREAM_PAUSED,
+	STATE_STREAM_STOPPING,
+} v4lout_state;
+
+/*!
+ * common v4l2 driver structure.
+ */
+typedef struct _vout_data {
+	struct video_device *video_dev;
+	/*!
+	 * semaphore guard against SMP multithreading
+	 */
+	struct semaphore busy_lock;
+
+	/*!
+	 * number of process that have device open
+	 */
+	int open_count;
+
+	/*!
+	 * params lock for this camera
+	 */
+	struct semaphore param_lock;
+
+	struct timer_list output_timer;
+	unsigned long start_jiffies;
+	u32 frame_count;
+
+	v4l_queue ready_q;
+	v4l_queue done_q;
+
+	s8 next_rdy_ipu_buf;
+	s8 next_done_ipu_buf;
+	s8 ipu_buf[2];
+	s8 ipu_buf_p[2];
+	s8 ipu_buf_n[2];
+	volatile v4lout_state state;
+
+	int cur_disp_output;
+	int output_fb_num[MXC_V4L2_OUT_NUM_OUTPUTS];
+	int output_enabled[MXC_V4L2_OUT_NUM_OUTPUTS];
+	struct v4l2_framebuffer v4l2_fb;
+	int ic_bypass;
+	ipu_channel_t display_ch;
+	ipu_channel_t post_proc_ch;
+
+	/*!
+	 * FRAME_NUM-buffering, so we need a array
+	 */
+	int buffer_cnt;
+	dma_addr_t queue_buf_paddr[MAX_FRAME_NUM];
+	void *queue_buf_vaddr[MAX_FRAME_NUM];
+	u32 queue_buf_size;
+	struct v4l2_buffer v4l2_bufs[MAX_FRAME_NUM];
+	u32 display_buf_size;
+	dma_addr_t display_bufs[2];
+	void *display_bufs_vaddr[2];
+	dma_addr_t rot_pp_bufs[2];
+	void *rot_pp_bufs_vaddr[2];
+
+	/*!
+	 * Poll wait queue
+	 */
+	wait_queue_head_t v4l_bufq;
+
+	/*!
+	 * v4l2 format
+	 */
+	struct v4l2_format v2f;
+	struct v4l2_mxc_offset offset;
+	ipu_rotate_mode_t rotate;
+
+	/* crop */
+	struct v4l2_rect crop_bounds[MXC_V4L2_OUT_NUM_OUTPUTS];
+	struct v4l2_rect crop_current;
+	u32 bytesperline;
+	enum v4l2_field field_fmt;
+	ipu_motion_sel motion_sel;
+} vout_data;
+
+#endif
+#endif				/* __MXC_V4L2_OUTPUT_H__ */
diff --git a/drivers/media/video/pxp.c b/drivers/media/video/pxp.c
new file mode 100644
index 000000000000..0c9d858630be
--- /dev/null
+++ b/drivers/media/video/pxp.c
@@ -0,0 +1,1231 @@
+/*
+ * Freescale STMP378X PxP driver
+ *
+ * Author: Matt Porter <mporter@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/vmalloc.h>
+#include <linux/videodev.h>
+
+#include <media/videobuf-dma-contig.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ioctl.h>
+
+#include <mach/platform.h>
+#include <mach/regs-pxp.h>
+
+#include "pxp.h"
+
+#define PXP_DRIVER_NAME			"stmp3xxx-pxp"
+#define PXP_DRIVER_MAJOR		1
+#define PXP_DRIVER_MINOR		0
+
+#define PXP_DEF_BUFS			2
+#define PXP_MIN_PIX			8
+
+#define V4L2_OUTPUT_TYPE_INTERNAL	4
+
+static struct pxp_data_format pxp_s0_formats[] = {
+	{
+		.name = "24-bit RGB",
+		.bpp = 4,
+		.fourcc = V4L2_PIX_FMT_RGB24,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__RGB888,
+	}, {
+		.name = "16-bit RGB 5:6:5",
+		.bpp = 2,
+		.fourcc = V4L2_PIX_FMT_RGB565,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__RGB565,
+	}, {
+		.name = "16-bit RGB 5:5:5",
+		.bpp = 2,
+		.fourcc = V4L2_PIX_FMT_RGB555,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__RGB555,
+	}, {
+		.name = "YUV 4:2:0 Planar",
+		.bpp = 2,
+		.fourcc = V4L2_PIX_FMT_YUV420,
+		.colorspace = V4L2_COLORSPACE_JPEG,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__YUV420,
+	}, {
+		.name = "YUV 4:2:2 Planar",
+		.bpp = 2,
+		.fourcc = V4L2_PIX_FMT_YUV422P,
+		.colorspace = V4L2_COLORSPACE_JPEG,
+		.ctrl_s0_fmt = BV_PXP_CTRL_S0_FORMAT__YUV422,
+	},
+};
+
+struct v4l2_queryctrl pxp_controls[] = {
+	{
+		.id 		= V4L2_CID_HFLIP,
+		.type 		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name 		= "Horizontal Flip",
+		.minimum 	= 0,
+		.maximum 	= 1,
+		.step 		= 1,
+		.default_value	= 0,
+		.flags		= 0,
+	}, {
+		.id		= V4L2_CID_VFLIP,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+		.name		= "Vertical Flip",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 0,
+		.flags		= 0,
+	}, {
+		.id		= V4L2_CID_PRIVATE_BASE,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+		.name		= "Rotation",
+		.minimum	= 0,
+		.maximum	= 270,
+		.step		= 90,
+		.default_value	= 0,
+		.flags		= 0,
+	}, {
+		.id		= V4L2_CID_PRIVATE_BASE + 1,
+		.name		= "Background Color",
+		.minimum	= 0,
+		.maximum	= 0xFFFFFF,
+		.step		= 1,
+		.default_value	= 0,
+		.flags		= 0,
+		.type		= V4L2_CTRL_TYPE_INTEGER,
+	}, {
+		.id		= V4L2_CID_PRIVATE_BASE + 2,
+		.name		= "YUV Colorspace",
+		.minimum	= 0,
+		.maximum	= 1,
+		.step		= 1,
+		.default_value	= 0,
+		.flags		= 0,
+		.type		= V4L2_CTRL_TYPE_BOOLEAN,
+	},
+};
+
+static void pxp_set_ctrl(struct pxps *pxp)
+{
+	u32 ctrl;
+
+	ctrl = BF(pxp->s0_fmt->ctrl_s0_fmt, PXP_CTRL_S0_FORMAT);
+	ctrl |=
+	BF(BV_PXP_CTRL_OUTPUT_RGB_FORMAT__RGB888, PXP_CTRL_OUTPUT_RGB_FORMAT);
+	ctrl |= BM_PXP_CTRL_CROP;
+
+	if (pxp->scaling)
+		ctrl |= BM_PXP_CTRL_SCALE;
+	if (pxp->vflip)
+		ctrl |= BM_PXP_CTRL_VFLIP;
+	if (pxp->hflip)
+		ctrl |= BM_PXP_CTRL_HFLIP;
+	if (pxp->rotate)
+		ctrl |= BF(pxp->rotate/90, PXP_CTRL_ROTATE);
+
+	ctrl |= BM_PXP_CTRL_IRQ_ENABLE;
+	if (pxp->active)
+		ctrl |= BM_PXP_CTRL_ENABLE;
+
+	__raw_writel(ctrl, REGS_PXP_BASE + HW_PXP_CTRL);
+}
+
+static void pxp_set_rgbbuf(struct pxps *pxp)
+{
+	__raw_writel(pxp->outb_phys, REGS_PXP_BASE + HW_PXP_RGBBUF);
+	/* Always equal to the FB size */
+	__raw_writel(BF(pxp->fb.fmt.width, PXP_RGBSIZE_WIDTH) |
+			  BF(pxp->fb.fmt.height, PXP_RGBSIZE_HEIGHT),
+			  REGS_PXP_BASE + HW_PXP_RGBSIZE);
+}
+
+static void pxp_set_colorkey(struct pxps *pxp)
+{
+	/* Low and high are set equal. V4L does not allow a chromakey range */
+	__raw_writel(pxp->chromakey, REGS_PXP_BASE + HW_PXP_S0COLORKEYLOW);
+	__raw_writel(pxp->chromakey, REGS_PXP_BASE + HW_PXP_S0COLORKEYHIGH);
+}
+
+static void pxp_set_oln(struct pxps *pxp)
+{
+	__raw_writel((u32)pxp->fb.base, REGS_PXP_BASE + HW_PXP_OL0);
+	__raw_writel(BF(pxp->fb.fmt.width >> 3, PXP_OLnSIZE_WIDTH) |
+				BF(pxp->fb.fmt.height >> 3, PXP_OLnSIZE_HEIGHT),
+				REGS_PXP_BASE + HW_PXP_OL0SIZE);
+}
+
+static void pxp_set_olparam(struct pxps *pxp)
+{
+	u32 olparam;
+	struct v4l2_pix_format *fmt = &pxp->fb.fmt;
+
+	olparam = BF(pxp->global_alpha, PXP_OLnPARAM_ALPHA);
+	if (fmt->pixelformat == V4L2_PIX_FMT_RGB24)
+		olparam |=
+		BF(BV_PXP_OLnPARAM_FORMAT__RGB888, PXP_OLnPARAM_FORMAT);
+	else
+		olparam |=
+		BF(BV_PXP_OLnPARAM_FORMAT__RGB565, PXP_OLnPARAM_FORMAT);
+	if (pxp->global_alpha_state)
+		olparam |= BF(BV_PXP_OLnPARAM_ALPHA_CNTL__Override,
+					PXP_OLnPARAM_ALPHA_CNTL);
+	if (pxp->chromakey_state)
+		olparam |= BM_PXP_OLnPARAM_ENABLE_COLORKEY;
+	if (pxp->overlay_state)
+		olparam |= BM_PXP_OLnPARAM_ENABLE;
+	__raw_writel(olparam, REGS_PXP_BASE + HW_PXP_OL0PARAM);
+}
+
+static void pxp_set_s0param(struct pxps *pxp)
+{
+	u32 s0param;
+
+	s0param = BF(pxp->drect.left >> 3, PXP_S0PARAM_XBASE);
+	s0param |= BF(pxp->drect.top >> 3, PXP_S0PARAM_YBASE);
+	s0param |= BF(pxp->s0_width >> 3, PXP_S0PARAM_WIDTH);
+	s0param |= BF(pxp->s0_height >> 3, PXP_S0PARAM_HEIGHT);
+	__raw_writel(s0param, REGS_PXP_BASE + HW_PXP_S0PARAM);
+}
+
+static void pxp_set_s0crop(struct pxps *pxp)
+{
+	u32 s0crop;
+
+	s0crop = BF(pxp->srect.left >> 3, PXP_S0CROP_XBASE);
+	s0crop |= BF(pxp->srect.top >> 3, PXP_S0CROP_YBASE);
+	s0crop |= BF(pxp->drect.width >> 3, PXP_S0CROP_WIDTH);
+	s0crop |= BF(pxp->drect.height >> 3, PXP_S0CROP_HEIGHT);
+	__raw_writel(s0crop, REGS_PXP_BASE + HW_PXP_S0CROP);
+}
+
+static int pxp_set_scaling(struct pxps *pxp)
+{
+	int ret = 0;
+	u32 xscale, yscale, s0scale;
+
+	if ((pxp->s0_fmt->fourcc != V4L2_PIX_FMT_YUV420) &&
+		(pxp->s0_fmt->fourcc != V4L2_PIX_FMT_YUV422P)) {
+		pxp->scaling = 0;
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if ((pxp->srect.width == pxp->drect.width) &&
+		(pxp->srect.height == pxp->drect.height)) {
+		pxp->scaling = 0;
+		goto out;
+	}
+
+	pxp->scaling = 1;
+	xscale = pxp->srect.width * 0x1000 / pxp->drect.width;
+	yscale = pxp->srect.height * 0x1000 / pxp->drect.height;
+	s0scale = BF(yscale, PXP_S0SCALE_YSCALE) |
+		  BF(xscale, PXP_S0SCALE_XSCALE);
+	__raw_writel(s0scale, REGS_PXP_BASE + HW_PXP_S0SCALE);
+
+out:
+	pxp_set_ctrl(pxp);
+
+	return ret;
+}
+
+static int pxp_set_fbinfo(struct pxps *pxp)
+{
+	struct fb_var_screeninfo var;
+	struct fb_fix_screeninfo fix;
+	struct v4l2_framebuffer *fb = &pxp->fb;
+	int err;
+
+	err = stmp3xxxfb_get_info(&var, &fix);
+
+	fb->fmt.width = var.xres;
+	fb->fmt.height = var.yres;
+	if (var.bits_per_pixel == 16)
+		fb->fmt.pixelformat = V4L2_PIX_FMT_RGB565;
+	else
+		fb->fmt.pixelformat = V4L2_PIX_FMT_RGB24;
+	fb->base = (void *)fix.smem_start;
+	return err;
+}
+
+static void pxp_set_s0bg(struct pxps *pxp)
+{
+	__raw_writel(pxp->s0_bgcolor, REGS_PXP_BASE + HW_PXP_S0BACKGROUND);
+}
+
+static void pxp_set_csc(struct pxps *pxp)
+{
+	if (pxp->yuv) {
+		/* YUV colorspace */
+		__raw_writel(0x04030000, REGS_PXP_BASE + HW_PXP_CSCCOEFF0);
+		__raw_writel(0x01230208, REGS_PXP_BASE + HW_PXP_CSCCOEFF1);
+		__raw_writel(0x076b079c, REGS_PXP_BASE + HW_PXP_CSCCOEFF2);
+	} else {
+		/* YCrCb colorspace */
+		__raw_writel(0x84ab01f0, REGS_PXP_BASE + HW_PXP_CSCCOEFF0);
+		__raw_writel(0x01230204, REGS_PXP_BASE + HW_PXP_CSCCOEFF1);
+		__raw_writel(0x0730079c, REGS_PXP_BASE + HW_PXP_CSCCOEFF2);
+	}
+}
+
+static int pxp_set_cstate(struct pxps *pxp, struct v4l2_control *vc)
+{
+
+	if (vc->id == V4L2_CID_HFLIP)
+		pxp->hflip = vc->value;
+	else if (vc->id == V4L2_CID_VFLIP)
+		pxp->vflip = vc->value;
+	else if (vc->id == V4L2_CID_PRIVATE_BASE) {
+		if (vc->value % 90)
+			return -ERANGE;
+		pxp->rotate = vc->value;
+	} else if (vc->id == V4L2_CID_PRIVATE_BASE + 1) {
+		pxp->s0_bgcolor = vc->value;
+		pxp_set_s0bg(pxp);
+	} else if (vc->id == V4L2_CID_PRIVATE_BASE + 2) {
+		pxp->yuv = vc->value;
+		pxp_set_csc(pxp);
+	}
+
+	pxp_set_ctrl(pxp);
+
+	return 0;
+}
+
+static int pxp_get_cstate(struct pxps *pxp, struct v4l2_control *vc)
+{
+	if (vc->id == V4L2_CID_HFLIP)
+		vc->value = pxp->hflip;
+	else if (vc->id == V4L2_CID_VFLIP)
+		vc->value = pxp->vflip;
+	else if (vc->id == V4L2_CID_PRIVATE_BASE)
+		vc->value = pxp->rotate;
+	else if (vc->id == V4L2_CID_PRIVATE_BASE + 1)
+		vc->value = pxp->s0_bgcolor;
+	else if (vc->id == V4L2_CID_PRIVATE_BASE + 2)
+		vc->value = pxp->yuv;
+
+	return 0;
+}
+
+static int pxp_enumoutput(struct file *file, void *fh,
+			struct v4l2_output *o)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	if ((o->index < 0) || (o->index > 1))
+		return -EINVAL;
+
+	memset(o, 0, sizeof(struct v4l2_output));
+	if (o->index == 0) {
+		strcpy(o->name, "PxP Display Output");
+		pxp->output = 0;
+	} else {
+		strcpy(o->name, "PxP Virtual Output");
+		pxp->output = 1;
+	}
+	o->type = V4L2_OUTPUT_TYPE_INTERNAL;
+	o->std = 0;
+	o->reserved[0] = pxp->outb_phys;
+
+	return 0;
+}
+
+static int pxp_g_output(struct file *file, void *fh,
+			unsigned int *i)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	*i = pxp->output;
+
+	return 0;
+}
+
+static int pxp_s_output(struct file *file, void *fh,
+			unsigned int i)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_pix_format *fmt = &pxp->fb.fmt;
+	int bpp;
+
+	if ((i < 0) || (i > 1))
+		return -EINVAL;
+
+	if (pxp->outb)
+		goto out;
+
+	/* Output buffer is same format as fbdev */
+	if (fmt->pixelformat == V4L2_PIX_FMT_RGB24)
+		bpp = 4;
+	else
+		bpp = 2;
+
+	pxp->outb = kmalloc(fmt->width * fmt->height * bpp, GFP_KERNEL);
+	pxp->outb_phys = virt_to_phys(pxp->outb);
+	dma_map_single(NULL, pxp->outb,
+			fmt->width * fmt->height * bpp, DMA_TO_DEVICE);
+
+out:
+	pxp_set_rgbbuf(pxp);
+
+	return 0;
+}
+
+static int pxp_enum_fmt_video_output(struct file *file, void *fh,
+				struct v4l2_fmtdesc *fmt)
+{
+	enum v4l2_buf_type type = fmt->type;
+	int index = fmt->index;
+
+	if ((fmt->index < 0) || (fmt->index >= ARRAY_SIZE(pxp_s0_formats)))
+		return -EINVAL;
+
+	memset(fmt, 0, sizeof(struct v4l2_fmtdesc));
+	fmt->index = index;
+	fmt->type = type;
+	fmt->pixelformat = pxp_s0_formats[index].fourcc;
+	strcpy(fmt->description, pxp_s0_formats[index].name);
+
+	return 0;
+}
+
+static int pxp_g_fmt_video_output(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct v4l2_pix_format *pf = &f->fmt.pix;
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct pxp_data_format *fmt = pxp->s0_fmt;
+
+	pf->width = pxp->s0_width;
+	pf->height = pxp->s0_height;
+	pf->pixelformat = fmt->fourcc;
+	pf->field = V4L2_FIELD_NONE;
+	pf->bytesperline = fmt->bpp * pf->width;
+	pf->sizeimage = pf->bytesperline * pf->height;
+	pf->colorspace = fmt->colorspace;
+	pf->priv = 0;
+
+	return 0;
+}
+
+static struct pxp_data_format *pxp_get_format(struct v4l2_format *f)
+{
+	struct pxp_data_format *fmt;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pxp_s0_formats); i++) {
+		fmt = &pxp_s0_formats[i];
+		if (fmt->fourcc == f->fmt.pix.pixelformat)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(pxp_s0_formats))
+		return NULL;
+
+	return &pxp_s0_formats[i];
+}
+
+static int pxp_try_fmt_video_output(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	int w = f->fmt.pix.width;
+	int h = f->fmt.pix.height;
+	struct pxp_data_format *fmt = pxp_get_format(f);
+
+	if (!fmt)
+		return -EINVAL;
+
+	w = min(w, 2040);
+	w = max(w, 8);
+	h = min(h, 2040);
+	h = max(h, 8);
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.width = w;
+	f->fmt.pix.height = h;
+	f->fmt.pix.pixelformat = fmt->fourcc;
+
+	return 0;
+}
+
+static int pxp_s_fmt_video_output(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_pix_format *pf = &f->fmt.pix;
+	int ret = pxp_try_fmt_video_output(file, fh, f);
+
+	if (ret == 0) {
+		pxp->s0_fmt = pxp_get_format(f);
+		pxp->s0_width = pf->width;
+		pxp->s0_height = pf->height;
+		pxp_set_ctrl(pxp);
+		pxp_set_s0param(pxp);
+	}
+
+	return ret;
+}
+
+static int pxp_g_fmt_output_overlay(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_window *wf = &f->fmt.win;
+
+	memset(wf, 0, sizeof(struct v4l2_window));
+	wf->chromakey = pxp->chromakey;
+	wf->global_alpha = pxp->global_alpha;
+	wf->field = V4L2_FIELD_NONE;
+	wf->clips = NULL;
+	wf->clipcount = 0;
+	wf->bitmap = NULL;
+	wf->w.left = pxp->srect.left;
+	wf->w.top = pxp->srect.top;
+	wf->w.width = pxp->srect.width;
+	wf->w.height = pxp->srect.height;
+
+	return 0;
+}
+
+static int pxp_try_fmt_output_overlay(struct file *file, void *fh,
+				struct v4l2_format *f)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_window *wf = &f->fmt.win;
+	struct v4l2_rect srect;
+	u32 chromakey = wf->chromakey;
+	u8 global_alpha = wf->global_alpha;
+
+	memcpy(&srect, &(wf->w), sizeof(struct v4l2_rect));
+
+	pxp_g_fmt_output_overlay(file, fh, f);
+
+	wf->chromakey = chromakey;
+	wf->global_alpha = global_alpha;
+
+	/* Constrain parameters to the input buffer */
+	wf->w.left = srect.left;
+	wf->w.top = srect.top;
+	wf->w.width = min(srect.width, ((__s32)pxp->s0_width - wf->w.left));
+	wf->w.height = min(srect.height, ((__s32)pxp->s0_height - wf->w.top));
+
+	return 0;
+}
+
+static int pxp_s_fmt_output_overlay(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	struct v4l2_window *wf = &f->fmt.win;
+	int ret = pxp_try_fmt_output_overlay(file, fh, f);
+
+	if (ret == 0) {
+		pxp->srect.left = wf->w.left;
+		pxp->srect.top = wf->w.top;
+		pxp->srect.width = wf->w.width;
+		pxp->srect.height = wf->w.height;
+		pxp->global_alpha = wf->global_alpha;
+		pxp->chromakey = wf->chromakey;
+		pxp_set_s0param(pxp);
+		pxp_set_s0crop(pxp);
+		pxp_set_scaling(pxp);
+		pxp_set_olparam(pxp);
+		pxp_set_colorkey(pxp);
+	}
+
+	return ret;
+}
+
+static int pxp_reqbufs(struct file *file, void *priv,
+			struct v4l2_requestbuffers *r)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	return videobuf_reqbufs(&pxp->s0_vbq, r);
+}
+
+static int pxp_querybuf(struct file *file, void *priv,
+			struct v4l2_buffer *b)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	return videobuf_querybuf(&pxp->s0_vbq, b);
+}
+
+static int pxp_qbuf(struct file *file, void *priv,
+			struct v4l2_buffer *b)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	return videobuf_qbuf(&pxp->s0_vbq, b);
+}
+
+static int pxp_dqbuf(struct file *file, void *priv,
+			struct v4l2_buffer *b)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	return videobuf_dqbuf(&pxp->s0_vbq, b, file->f_flags & O_NONBLOCK);
+}
+
+static int pxp_streamon(struct file *file, void *priv,
+			enum v4l2_buf_type t)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int ret = 0;
+
+	if ((t != V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	ret = videobuf_streamon(&pxp->s0_vbq);
+
+	if (!ret && (pxp->output == 0))
+		stmp3xxxfb_cfg_pxp(1, pxp->outb_phys);
+
+	return ret;
+}
+
+static int pxp_streamoff(struct file *file, void *priv,
+			enum v4l2_buf_type t)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int ret = 0;
+
+	if ((t != V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	ret = videobuf_streamoff(&pxp->s0_vbq);
+
+	if (!ret)
+		stmp3xxxfb_cfg_pxp(0, 0);
+
+	return ret;
+}
+
+static int pxp_buf_setup(struct videobuf_queue *q,
+			unsigned int *count, unsigned *size)
+{
+	struct pxps *pxp = q->priv_data;
+
+	*size = pxp->s0_width * pxp->s0_height * pxp->s0_fmt->bpp;
+
+	if (0 == *count)
+		*count = PXP_DEF_BUFS;
+
+	return 0;
+}
+
+static void pxp_buf_free(struct videobuf_queue *q, struct videobuf_buffer *vb)
+{
+	if (in_interrupt())
+		BUG();
+
+	videobuf_dma_contig_free(q, vb);
+
+	vb->state = VIDEOBUF_NEEDS_INIT;
+}
+
+static int pxp_buf_prepare(struct videobuf_queue *q,
+			struct videobuf_buffer *vb,
+			enum v4l2_field field)
+{
+	struct pxps *pxp = q->priv_data;
+	int ret = 0;
+
+	vb->width = pxp->s0_width;
+	vb->height = pxp->s0_height;
+	vb->size = vb->width * vb->height * pxp->s0_fmt->bpp;
+	vb->field = V4L2_FIELD_NONE;
+	vb->state = VIDEOBUF_NEEDS_INIT;
+
+
+	ret = videobuf_iolock(q, vb, NULL);
+	if (ret)
+		goto fail;
+	vb->state = VIDEOBUF_PREPARED;
+
+	return 0;
+
+fail:
+	pxp_buf_free(q, vb);
+	return ret;
+}
+
+static void pxp_buf_output(struct pxps *pxp)
+{
+	dma_addr_t Y, U, V;
+
+	if (pxp->active) {
+		pxp->active->state = VIDEOBUF_ACTIVE;
+		Y = videobuf_to_dma_contig(pxp->active);
+		__raw_writel(Y, REGS_PXP_BASE + HW_PXP_S0BUF);
+		if ((pxp->s0_fmt->fourcc == V4L2_PIX_FMT_YUV420) ||
+		    (pxp->s0_fmt->fourcc == V4L2_PIX_FMT_YUV422P)) {
+			int s = 1;	/* default to YUV 4:2:2 */
+			if (pxp->s0_fmt->fourcc == V4L2_PIX_FMT_YUV420)
+				s = 2;
+			U = Y + (pxp->s0_width * pxp->s0_height);
+			V = U + ((pxp->s0_width * pxp->s0_height) >> s);
+			__raw_writel(U, REGS_PXP_BASE + HW_PXP_S0UBUF);
+			__raw_writel(V, REGS_PXP_BASE + HW_PXP_S0VBUF);
+		}
+		stmp3xxx_setl(BM_PXP_CTRL_ENABLE, REGS_PXP_BASE + HW_PXP_CTRL);
+	}
+}
+
+static void pxp_buf_queue(struct videobuf_queue *q,
+			struct videobuf_buffer *vb)
+{
+	struct pxps *pxp = q->priv_data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pxp->lock, flags);
+
+	list_add_tail(&vb->queue, &pxp->outq);
+	vb->state = VIDEOBUF_QUEUED;
+
+	if (!pxp->active) {
+		pxp->active = vb;
+		pxp_buf_output(pxp);
+	}
+
+	spin_unlock_irqrestore(&pxp->lock, flags);
+}
+
+static void pxp_buf_release(struct videobuf_queue *q,
+			struct videobuf_buffer *vb)
+{
+	pxp_buf_free(q, vb);
+}
+
+static struct videobuf_queue_ops pxp_vbq_ops = {
+	.buf_setup	= pxp_buf_setup,
+	.buf_prepare	= pxp_buf_prepare,
+	.buf_queue	= pxp_buf_queue,
+	.buf_release	= pxp_buf_release,
+};
+
+static int pxp_querycap(struct file *file, void *fh,
+			struct v4l2_capability *cap)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	memset(cap, 0, sizeof(*cap));
+	strcpy(cap->driver, "pxp");
+	strcpy(cap->card, "pxp");
+	strlcpy(cap->bus_info, dev_name(&pxp->pdev->dev), sizeof(cap->bus_info));
+
+	cap->version = (PXP_DRIVER_MAJOR << 8) + PXP_DRIVER_MINOR;
+
+	cap->capabilities = V4L2_CAP_VIDEO_OUTPUT |
+				V4L2_CAP_VIDEO_OUTPUT_OVERLAY |
+				V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+static int pxp_g_fbuf(struct file *file, void *priv,
+			struct v4l2_framebuffer *fb)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	memset(fb, 0, sizeof(*fb));
+
+	fb->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
+			 V4L2_FBUF_CAP_CHROMAKEY |
+			 V4L2_FBUF_CAP_LOCAL_ALPHA |
+			 V4L2_FBUF_CAP_GLOBAL_ALPHA;
+
+	if (pxp->global_alpha_state)
+		fb->flags |= V4L2_FBUF_FLAG_GLOBAL_ALPHA;
+	if (pxp->local_alpha_state)
+		fb->flags |= V4L2_FBUF_FLAG_LOCAL_ALPHA;
+	if (pxp->chromakey_state)
+		fb->flags |= V4L2_FBUF_FLAG_CHROMAKEY;
+
+	return 0;
+}
+
+static int pxp_s_fbuf(struct file *file, void *priv,
+			struct v4l2_framebuffer *fb)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	pxp->overlay_state =
+		(fb->flags & V4L2_FBUF_FLAG_OVERLAY) != 0;
+	pxp->global_alpha_state =
+		(fb->flags & V4L2_FBUF_FLAG_GLOBAL_ALPHA) != 0;
+	pxp->local_alpha_state =
+		(fb->flags & V4L2_FBUF_FLAG_LOCAL_ALPHA) != 0;
+	/* Global alpha overrides local alpha if both are requested */
+	if (pxp->global_alpha_state && pxp->local_alpha_state)
+		pxp->local_alpha_state = 0;
+	pxp->chromakey_state =
+		(fb->flags & V4L2_FBUF_FLAG_CHROMAKEY) != 0;
+
+	pxp_set_olparam(pxp);
+	pxp_set_s0crop(pxp);
+	pxp_set_scaling(pxp);
+
+	return 0;
+}
+
+static int pxp_g_crop(struct file *file, void *fh,
+			struct v4l2_crop *c)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	if (c->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY)
+		return -EINVAL;
+
+	c->c.left = pxp->drect.left;
+	c->c.top = pxp->drect.top;
+	c->c.width = pxp->drect.width;
+	c->c.height = pxp->drect.height;
+
+	return 0;
+}
+
+static int pxp_s_crop(struct file *file, void *fh,
+			struct v4l2_crop *c)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int l = c->c.left;
+	int t = c->c.top;
+	int w = c->c.width;
+	int h = c->c.height;
+	int fbw = pxp->fb.fmt.width;
+	int fbh = pxp->fb.fmt.height;
+
+	if (c->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY)
+		return -EINVAL;
+
+	/* Constrain parameters to FB limits */
+	w = min(w, fbw);
+	w = max(w, PXP_MIN_PIX);
+	h = min(h, fbh);
+	h = max(h, PXP_MIN_PIX);
+	if ((l + w) > fbw)
+		l = 0;
+	if ((t + h) > fbh)
+		t = 0;
+
+	/* Round up values to PxP pixel block */
+	l = roundup(l, PXP_MIN_PIX);
+	t = roundup(t, PXP_MIN_PIX);
+	w = roundup(w, PXP_MIN_PIX);
+	h = roundup(h, PXP_MIN_PIX);
+
+	pxp->drect.left = l;
+	pxp->drect.top = t;
+	pxp->drect.width = w;
+	pxp->drect.height = h;
+
+	pxp_set_s0param(pxp);
+	pxp_set_s0crop(pxp);
+	pxp_set_scaling(pxp);
+
+	return 0;
+}
+
+static int pxp_queryctrl(struct file *file, void *priv,
+			 struct v4l2_queryctrl *qc)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pxp_controls); i++)
+		if (qc->id && qc->id == pxp_controls[i].id) {
+			memcpy(qc, &(pxp_controls[i]), sizeof(*qc));
+			return 0;
+		}
+
+	return -EINVAL;
+}
+
+static int pxp_g_ctrl(struct file *file, void *priv,
+			 struct v4l2_control *vc)
+{
+	int i;
+
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	for (i = 0; i < ARRAY_SIZE(pxp_controls); i++)
+		if (vc->id == pxp_controls[i].id)
+			return pxp_get_cstate(pxp, vc);
+
+	return -EINVAL;
+}
+
+static int pxp_s_ctrl(struct file *file, void *priv,
+			 struct v4l2_control *vc)
+{
+	int i;
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	for (i = 0; i < ARRAY_SIZE(pxp_controls); i++)
+		if (vc->id == pxp_controls[i].id) {
+			if (vc->value < pxp_controls[i].minimum ||
+			    vc->value > pxp_controls[i].maximum)
+				return -ERANGE;
+			return pxp_set_cstate(pxp, vc);
+		}
+
+	return -EINVAL;
+}
+
+void pxp_release(struct video_device *vfd)
+{
+	struct pxps *pxp = video_get_drvdata(vfd);
+
+	spin_lock(&pxp->lock);
+	video_device_release(vfd);
+	spin_unlock(&pxp->lock);
+}
+
+static int pxp_hw_init(struct pxps *pxp)
+{
+	struct fb_var_screeninfo var;
+	struct fb_fix_screeninfo fix;
+	int err;
+
+	err = stmp3xxxfb_get_info(&var, &fix);
+	if (err)
+		return err;
+
+	/* Pull PxP out of reset */
+	__raw_writel(0, REGS_PXP_BASE + HW_PXP_CTRL);
+
+	/* Config defaults */
+	pxp->active = NULL;
+
+	pxp->s0_fmt = &pxp_s0_formats[0];
+	pxp->drect.left = pxp->srect.left = 0;
+	pxp->drect.top = pxp->srect.top = 0;
+	pxp->drect.width = pxp->srect.width = pxp->s0_width = var.xres;
+	pxp->drect.height = pxp->srect.height = pxp->s0_height = var.yres;
+	pxp->s0_bgcolor = 0;
+
+	pxp->output = 0;
+	err = pxp_set_fbinfo(pxp);
+	if (err)
+		return err;
+
+	pxp->scaling = 0;
+	pxp->hflip = 0;
+	pxp->vflip = 0;
+	pxp->rotate = 0;
+	pxp->yuv = 0;
+
+	pxp->overlay_state = 0;
+	pxp->global_alpha_state = 0;
+	pxp->global_alpha = 0;
+	pxp->local_alpha_state = 0;
+	pxp->chromakey_state = 0;
+	pxp->chromakey = 0;
+
+	/* Write default h/w config */
+	pxp_set_ctrl(pxp);
+	pxp_set_s0param(pxp);
+	pxp_set_s0crop(pxp);
+	pxp_set_oln(pxp);
+	pxp_set_olparam(pxp);
+	pxp_set_colorkey(pxp);
+	pxp_set_csc(pxp);
+
+	return 0;
+}
+
+static int pxp_open(struct file *file)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int ret = 0;
+
+	mutex_lock(&pxp->mutex);
+	pxp->users++;
+
+	if (pxp->users > 1) {
+		pxp->users--;
+		ret = -EBUSY;
+		goto out;
+	}
+
+out:
+	mutex_unlock(&pxp->mutex);
+	if (ret)
+		return ret;
+
+	videobuf_queue_dma_contig_init(&pxp->s0_vbq,
+				&pxp_vbq_ops,
+				&pxp->pdev->dev,
+				&pxp->lock,
+				V4L2_BUF_TYPE_VIDEO_OUTPUT,
+				V4L2_FIELD_NONE,
+				sizeof(struct videobuf_buffer),
+				pxp);
+
+	return 0;
+}
+
+static int pxp_close(struct file *file)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+
+	videobuf_stop(&pxp->s0_vbq);
+	videobuf_mmap_free(&pxp->s0_vbq);
+
+	mutex_lock(&pxp->mutex);
+	pxp->users--;
+	mutex_unlock(&pxp->mutex);
+
+	return 0;
+}
+
+static int pxp_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	struct pxps *pxp = video_get_drvdata(video_devdata(file));
+	int ret;
+
+	ret = videobuf_mmap_mapper(&pxp->s0_vbq, vma);
+
+	return ret;
+}
+
+static const struct v4l2_file_operations pxp_fops = {
+	.owner		= THIS_MODULE,
+	.open		= pxp_open,
+	.release	= pxp_close,
+	.ioctl		= video_ioctl2,
+	.mmap		= pxp_mmap,
+};
+
+static const struct v4l2_ioctl_ops pxp_ioctl_ops = {
+	.vidioc_querycap		= pxp_querycap,
+
+	.vidioc_reqbufs			= pxp_reqbufs,
+	.vidioc_querybuf		= pxp_querybuf,
+	.vidioc_qbuf			= pxp_qbuf,
+	.vidioc_dqbuf			= pxp_dqbuf,
+
+	.vidioc_streamon		= pxp_streamon,
+	.vidioc_streamoff		= pxp_streamoff,
+
+	.vidioc_enum_output		= pxp_enumoutput,
+	.vidioc_g_output		= pxp_g_output,
+	.vidioc_s_output		= pxp_s_output,
+
+	.vidioc_enum_fmt_vid_out	= pxp_enum_fmt_video_output,
+	.vidioc_try_fmt_vid_out		= pxp_try_fmt_video_output,
+	.vidioc_g_fmt_vid_out		= pxp_g_fmt_video_output,
+	.vidioc_s_fmt_vid_out		= pxp_s_fmt_video_output,
+
+	.vidioc_try_fmt_vid_out_overlay	= pxp_try_fmt_output_overlay,
+	.vidioc_g_fmt_vid_out_overlay	= pxp_g_fmt_output_overlay,
+	.vidioc_s_fmt_vid_out_overlay	= pxp_s_fmt_output_overlay,
+
+	.vidioc_g_fbuf			= pxp_g_fbuf,
+	.vidioc_s_fbuf			= pxp_s_fbuf,
+
+	.vidioc_g_crop			= pxp_g_crop,
+	.vidioc_s_crop			= pxp_s_crop,
+
+	.vidioc_queryctrl		= pxp_queryctrl,
+	.vidioc_g_ctrl			= pxp_g_ctrl,
+	.vidioc_s_ctrl			= pxp_s_ctrl,
+};
+
+static const struct video_device pxp_template = {
+	.name				= "PxP",
+	.vfl_type			= VID_TYPE_OVERLAY |
+					  VID_TYPE_CLIPPING |
+					  VID_TYPE_SCALES,
+	.fops				= &pxp_fops,
+	.release			= pxp_release,
+	.minor				= -1,
+	.ioctl_ops			= &pxp_ioctl_ops,
+};
+
+static irqreturn_t pxp_irq(int irq, void *dev_id)
+{
+	struct pxps *pxp = (struct pxps *)dev_id;
+	struct videobuf_buffer *vb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&pxp->lock, flags);
+
+	stmp3xxx_clearl(BM_PXP_STAT_IRQ, REGS_PXP_BASE + HW_PXP_STAT);
+
+	vb = pxp->active;
+	vb->state = VIDEOBUF_DONE;
+	do_gettimeofday(&vb->ts);
+	vb->field_count++;
+
+	list_del_init(&vb->queue);
+
+	if (list_empty(&pxp->outq)) {
+		pxp->active = NULL;
+		goto out;
+	}
+
+	pxp->active = list_entry(pxp->outq.next,
+				struct videobuf_buffer,
+				queue);
+
+	pxp_buf_output(pxp);
+
+out:
+	wake_up(&vb->done);
+
+	spin_unlock_irqrestore(&pxp->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static int pxp_probe(struct platform_device *pdev)
+{
+	struct pxps *pxp;
+	struct resource *res;
+	int irq;
+	int err = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	irq = platform_get_irq(pdev, 0);
+	if (!res || irq < 0) {
+		err = -ENODEV;
+		goto exit;
+	}
+
+	pxp = kzalloc(sizeof(*pxp), GFP_KERNEL);
+	if (!pxp) {
+		dev_err(&pdev->dev, "failed to allocate control object\n");
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	dev_set_drvdata(&pdev->dev, pxp);
+	pxp->res = res;
+	pxp->irq = irq;
+
+	INIT_LIST_HEAD(&pxp->outq);
+	spin_lock_init(&pxp->lock);
+	mutex_init(&pxp->mutex);
+
+	if (!request_mem_region(res->start, res->end - res->start + 1,
+				PXP_DRIVER_NAME)) {
+		err = -EBUSY;
+		goto freepxp;
+	}
+
+	pxp->regs = (void __iomem *)res->start; /* it is already ioremapped */
+	pxp->pdev = pdev;
+
+	err = request_irq(pxp->irq, pxp_irq, 0, PXP_DRIVER_NAME, pxp);
+
+	if (err) {
+		dev_err(&pdev->dev, "interrupt register failed\n");
+		goto release;
+	}
+
+	pxp->vdev = video_device_alloc();
+	if (!pxp->vdev) {
+		dev_err(&pdev->dev, "video_device_alloc() failed\n");
+		err = -ENOMEM;
+		goto freeirq;
+	}
+
+	memcpy(pxp->vdev, &pxp_template, sizeof(pxp_template));
+	video_set_drvdata(pxp->vdev, pxp);
+
+	err = video_register_device(pxp->vdev, VFL_TYPE_GRABBER, 0);
+	if (err) {
+		dev_err(&pdev->dev, "failed to register video device\n");
+		goto freevdev;
+	}
+
+	err = pxp_hw_init(pxp);
+	if (err) {
+		dev_err(&pdev->dev, "failed to initialize hardware\n");
+		goto freevdev;
+	}
+
+	dev_info(&pdev->dev, "initialized\n");
+
+exit:
+	return err;
+
+freevdev:
+	video_device_release(pxp->vdev);
+
+freeirq:
+	free_irq(pxp->irq, pxp);
+
+release:
+	release_mem_region(res->start, res->end - res->start + 1);
+
+freepxp:
+	kfree(pxp);
+
+	return err;
+}
+
+static int __devexit pxp_remove(struct platform_device *pdev)
+{
+	struct pxps *pxp = platform_get_drvdata(pdev);
+
+	video_unregister_device(pxp->vdev);
+	video_device_release(pxp->vdev);
+
+	kfree(pxp->outb);
+	kfree(pxp);
+
+	return 0;
+}
+
+static struct platform_driver pxp_driver = {
+	.driver 	= {
+		.name	= PXP_DRIVER_NAME,
+	},
+	.probe		= pxp_probe,
+	.remove		= __exit_p(pxp_remove),
+};
+
+
+static int __devinit pxp_init(void)
+{
+	return platform_driver_register(&pxp_driver);
+}
+
+static void __exit pxp_exit(void)
+{
+	platform_driver_unregister(&pxp_driver);
+}
+
+module_init(pxp_init);
+module_exit(pxp_exit);
+
+MODULE_DESCRIPTION("STMP37xx PxP driver");
+MODULE_AUTHOR("Matt Porter <mporter@embeddedalley.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/video/pxp.h b/drivers/media/video/pxp.h
new file mode 100644
index 000000000000..0e2cd62969ca
--- /dev/null
+++ b/drivers/media/video/pxp.h
@@ -0,0 +1,75 @@
+/*
+ * Freescale STMP378X PxP driver
+ *
+ * Author: Matt Porter <mporter@embeddedalley.com>
+ *
+ * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2009 Embedded Alley Solutions, Inc All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+struct pxps {
+	struct platform_device *pdev;
+	struct resource *res;
+	int irq;
+	void __iomem *regs;
+
+	spinlock_t lock;
+	struct mutex mutex;
+	int users;
+
+	struct video_device *vdev;
+
+	struct videobuf_queue s0_vbq;
+	struct videobuf_buffer *active;
+	struct list_head outq;
+
+	int output;
+	u32 *outb;
+	dma_addr_t outb_phys;
+
+	/* Current S0 configuration */
+	struct pxp_data_format *s0_fmt;
+	u32 s0_width;
+	u32 s0_height;
+	u32 s0_bgcolor;
+
+	struct v4l2_framebuffer fb;
+	struct v4l2_rect drect;
+	struct v4l2_rect srect;
+
+	/* Transformation support */
+	int scaling;
+	int hflip;
+	int vflip;
+	int rotate;
+	int yuv;
+
+	/* Output overlay support */
+	int overlay_state;
+	int global_alpha_state;
+	u8  global_alpha;
+	int local_alpha_state;
+	int chromakey_state;
+	u32 chromakey;
+};
+
+struct pxp_data_format {
+	char *name;
+	unsigned int bpp;
+	u32 fourcc;
+	enum v4l2_colorspace colorspace;
+	u32 ctrl_s0_fmt;
+};
+
+extern int stmp3xxxfb_get_info(struct fb_var_screeninfo *var,
+				struct fb_fix_screeninfo *fix);
+extern void stmp3xxxfb_cfg_pxp(int enable, dma_addr_t pxp_phys);